// point.h (point class copied from libCoal, subsequently modified)
//
//  The WorldForge Project
//  Copyright (C) 2000, 2001, 2002  The WorldForge Project
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//
//  For information about WorldForge and its authors, please contact
//  the Worldforge Web Site at http://www.worldforge.org.
//

// Author: Ron Steinke

#ifndef WFMATH_POINT_H
#define WFMATH_POINT_H

#include <wfmath/const.h>

#include <memory>
#include <iosfwd>

#include <cmath>

namespace WFMath {

template<int dim>
Point<dim>& operator+=(Point<dim>& p, const Vector<dim>& v);

template<int dim>
Point<dim>& operator-=(Point<dim>& p, const Vector<dim>& v);

template<int dim>
Vector<dim> operator-(const Point<dim>& c1, const Point<dim>& c2);

template<int dim>
Point<dim> operator+(const Point<dim>& c, const Vector<dim>& v);

template<int dim>
Point<dim> operator+(const Vector<dim>& v, const Point<dim>& c);

template<int dim>
Point<dim> operator-(const Point<dim>& c, const Vector<dim>& v);

template<int dim>
CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2);

template<int dim>
CoordType Distance(const Point<dim>& p1, const Point<dim>& p2) { return std::sqrt(SquaredDistance(p1, p2)); }

template<int dim>
CoordType SloppyDistance(const Point<dim>& p1, const Point<dim>& p2) { return (p1 - p2).sloppyMag(); }

/// Find the center of a set of points, all weighted equally
template<int dim, template<class, class> class container>
Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c);
/// Find the center of a set of points with the given weights
/**
 * If the number of points and the number of weights are not equal,
 * the excess of either is ignored. The weights (or that subset
 * which is used, if there are more weights than points), must not
 * sum to zero.
 **/
template<int dim, template<class, class> class container,
		template<class, class> class container2>
Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c,
					  const container2<CoordType, std::allocator<CoordType> >& weights);

// This is used a couple of places in the library
template<int dim>
Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2,
					CoordType dist = 0.5);

template<int dim>
std::ostream& operator<<(std::ostream& os, const Point<dim>& m);

template<int dim>
std::istream& operator>>(std::istream& is, Point<dim>& m);

template<typename Shape>
class ZeroPrimitive;

/// A dim dimensional point
/**
 * This class implements the full shape interface, as described in
 * the fake class Shape.
 **/
template<int dim = 3>
class Point {
	friend class ZeroPrimitive<Point<dim> >;

public:
	/// Construct an uninitialized point
	Point() : m_elem{}, m_valid(false) {};

	/// Construct a copy of a point
	Point(const Point&) = default;

	/// Construct a point from an object passed by Atlas
	explicit Point(const AtlasInType& a);

	/// Construct a point from a vector.
	explicit Point(const Vector<dim>& vector);

	/**
	 * @brief Provides a global instance preset to zero.
	 */
	static const Point<dim>& ZERO();

	friend std::ostream& operator<<<dim>(std::ostream& os, const Point& p);

	friend std::istream& operator>><dim>(std::istream& is, Point& p);

	/// Create an Atlas object from the point
	AtlasOutType toAtlas() const;

	/// Set the point's value to that given by an Atlas object
	void fromAtlas(const AtlasInType& a);

	Point& operator=(const Point& rhs) = default;

	bool isEqualTo(const Point& p, CoordType epsilon = numeric_constants<CoordType>::epsilon()) const;

	bool operator==(const Point& rhs) const { return isEqualTo(rhs); }

	bool operator!=(const Point& rhs) const { return !isEqualTo(rhs); }

	bool isValid() const { return m_valid; }

	/// make isValid() return true if you've initialized the point by hand
	void setValid(bool valid = true) { m_valid = valid; }

	/// Set point to (0,0,...,0)
	Point& setToOrigin();

	// Operators

	// Documented in vector.h
	friend Vector<dim> operator-<dim>(const Point& c1, const Point& c2);

	friend Point operator+<dim>(const Point& c, const Vector<dim>& v);

	friend Point operator-<dim>(const Point& c, const Vector<dim>& v);

	friend Point operator+<dim>(const Vector<dim>& v, const Point& c);

	friend Point& operator+=<dim>(Point& p, const Vector<dim>& rhs);

	friend Point& operator-=<dim>(Point& p, const Vector<dim>& rhs);

	/// Rotate about point p
	Point& rotate(const RotMatrix<dim>& m, const Point& p) { return (*this = p + Prod(*this - p, m)); }

	// Functions so that Point<> has the generic shape interface

	size_t numCorners() const { return 1; }

	Point<dim> getCorner(size_t) const { return *this; }

	Point<dim> getCenter() const { return *this; }

	Point shift(const Vector<dim>& v) { return *this += v; }

	Point moveCornerTo(const Point& p, size_t) { return operator=(p); }

	Point moveCenterTo(const Point& p) { return operator=(p); }

	Point& rotateCorner(const RotMatrix<dim>&, size_t) { return *this; }

	Point& rotateCenter(const RotMatrix<dim>&) { return *this; }

	Point& rotatePoint(const RotMatrix<dim>& m, const Point& p) { return rotate(m, p); }

	// 3D rotation functions
	Point& rotate(const Quaternion& q, const Point& p);

	Point& rotateCorner(const Quaternion&, size_t) { return *this; }

	Point& rotateCenter(const Quaternion&) { return *this; }

	Point& rotatePoint(const Quaternion& q, const Point& p);

	// The implementations of these lie in axisbox_funcs.h and
	// ball_funcs.h, to reduce include dependencies
	AxisBox<dim> boundingBox() const;

	Ball<dim> boundingSphere() const;

	Ball<dim> boundingSphereSloppy() const;

	Point toParentCoords(const Point& origin,
						 const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const { return origin + (*this - Point().setToOrigin()) * rotation; }

	Point toParentCoords(const AxisBox<dim>& coords) const;

	Point toParentCoords(const RotBox<dim>& coords) const;

	// toLocal is just like toParent, expect we reverse the order of
	// translation and rotation and use the opposite sense of the rotation
	// matrix

	Point toLocalCoords(const Point& origin,
						const RotMatrix<dim>& rotation = RotMatrix<dim>().identity()) const { return Point().setToOrigin() + rotation * (*this - origin); }

	Point toLocalCoords(const AxisBox<dim>& coords) const;

	Point toLocalCoords(const RotBox<dim>& coords) const;

	// 3D only
	Point toParentCoords(const Point& origin, const Quaternion& rotation) const;

	Point toLocalCoords(const Point& origin, const Quaternion& rotation) const;

	// Member access

	/// Access the i'th coordinate of the point
	CoordType operator[](const int i) const { return m_elem[i]; }

	/// Access the i'th coordinate of the point
	CoordType& operator[](const int i) { return m_elem[i]; }

	/// Get the square of the distance from p1 to p2
	friend CoordType SquaredDistance<dim>(const Point& p1, const Point& p2);

// FIXME instatiation problem when declared as friend
//  template<template<class> class container>
//  friend Point Barycenter(const container<Point>& c);

	/// Find a point on the line containing p1 and p2, by default the midpoint
	/**
	 * The default value of 0.5 for dist gives the midpoint. A value of 0 gives
	 * p1, and 1 gives p2. Values of dist outside the [0, 1] range are allowed,
	 * and give points on the line which are not on the segment bounded by
	 * p1 and p2.
	 **/
	friend Point<dim> Midpoint<dim>(const Point& p1, const Point& p2, CoordType dist);

	// 2D/3D stuff

	/// 2D only: construct a point from its (x, y) coordinates
	Point(CoordType x, CoordType y); // 2D only
	/// 3D only: construct a point from its (x, y, z) coordinates
	Point(CoordType x, CoordType y, CoordType z); // 3D only

	// Label the first three components of the vector as (x,y,z) for
	// 2D/3D convienience

	/// access the first component of a point
	CoordType x() const { return m_elem[0]; }

	/// access the first component of a point
	CoordType& x() { return m_elem[0]; }

	/// access the second component of a point
	CoordType y() const { return m_elem[1]; }

	/// access the second component of a point
	CoordType& y() { return m_elem[1]; }

	/// access the third component of a point
	CoordType z() const;

	/// access the third component of a point
	CoordType& z();

	/// 2D only: construct a vector from polar coordinates
	Point& polar(CoordType r, CoordType theta);

	/// 2D only: convert a vector to polar coordinates
	void asPolar(CoordType& r, CoordType& theta) const;

	/// 3D only: construct a vector from polar coordinates
	Point& polar(CoordType r, CoordType theta, CoordType z);

	/// 3D only: convert a vector to polar coordinates
	void asPolar(CoordType& r, CoordType& theta, CoordType& z) const;

	/// 3D only: construct a vector from spherical coordinates
	Point& spherical(CoordType r, CoordType theta, CoordType phi);

	/// 3D only: convert a vector to spherical coordinates
	void asSpherical(CoordType& r, CoordType& theta, CoordType& phi) const;

	const CoordType* elements() const { return m_elem; }

private:
	CoordType m_elem[dim];
	bool m_valid;
};

template<>
inline CoordType Point<3>::z() const {
	return m_elem[2];
}

template<>
inline CoordType& Point<3>::z() {
	return m_elem[2];
}

template<int dim>
inline Point<dim> operator+(const Point<dim>& c, const Vector<dim>& v) {
	Point<dim> out(c);

	out += v;

	return out;
}

template<int dim>
inline Point<dim> operator+(const Vector<dim>& v, const Point<dim>& c) {
	Point<dim> out(c);

	out += v;

	return out;
}

template<int dim>
inline Point<dim> operator-(const Point<dim>& c, const Vector<dim>& v) {
	Point<dim> out(c);

	out -= v;

	return out;
}

template<>
inline Point<2>::Point(CoordType x, CoordType y) : m_valid(true) {
	m_elem[0] = x;
	m_elem[1] = y;
}

template<>
inline Point<3>::Point(CoordType x, CoordType y, CoordType z) : m_valid(true) {
	m_elem[0] = x;
	m_elem[1] = y;
	m_elem[2] = z;
}

} // namespace WFMath

#endif  // WFMATH_POINT_H
