// point_funcs.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_FUNCS_H
#define WFMATH_POINT_FUNCS_H

#include <wfmath/point.h>

#include <wfmath/vector.h>
#include <wfmath/zero.h>

#include <cmath>

namespace WFMath {

template<int dim>
inline Point<dim>::Point(const Vector<dim>& v) : m_valid(v.isValid()) {
	for (int i = 0; i < dim; ++i) {
		m_elem[i] = v.elements()[i];
	}
}

template<int dim>
const Point<dim>& Point<dim>::ZERO() {
	static ZeroPrimitive<Point<dim> > zeroPoint(dim);
	return zeroPoint.getShape();
}


template<int dim>
inline Point<dim>& Point<dim>::setToOrigin() {
	for (int i = 0; i < dim; ++i) {
		m_elem[i] = 0;
	}

	m_valid = true;

	return *this;
}

template<int dim>
inline bool Point<dim>::isEqualTo(const Point<dim>& p, CoordType epsilon) const {
	//If anyone is invalid they are never equal
	if (!p.m_valid || !m_valid) {
		return false;
	}

	CoordType delta = _ScaleEpsilon(m_elem, p.m_elem, dim, epsilon);
	for (int i = 0; i < dim; ++i) {
		if (std::fabs(m_elem[i] - p.m_elem[i]) > delta) {
			return false;
		}
	}

	return true;
}

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

	for (int i = 0; i < dim; ++i) {
		out.m_elem[i] = c1.m_elem[i] - c2.m_elem[i];
	}

	out.m_valid = c1.m_valid && c2.m_valid;

	return out;
}

template<int dim>
inline Point<dim>& operator+=(Point<dim>& p, const Vector<dim>& rhs) {
	for (int i = 0; i < dim; ++i) {
		p.m_elem[i] += rhs.m_elem[i];
	}

	p.m_valid = p.m_valid && rhs.m_valid;

	return p;
}

template<int dim>
inline Point<dim>& operator-=(Point<dim>& p, const Vector<dim>& rhs) {
	for (int i = 0; i < dim; ++i) {
		p.m_elem[i] -= rhs.m_elem[i];
	}

	p.m_valid = p.m_valid && rhs.m_valid;

	return p;
}

template<int dim>
inline CoordType SquaredDistance(const Point<dim>& p1, const Point<dim>& p2) {
	CoordType ans = 0;

	for (int i = 0; i < dim; ++i) {
		CoordType diff = p1.m_elem[i] - p2.m_elem[i];
		ans += diff * diff;
	}

	return (std::fabs(ans) >= _ScaleEpsilon(p1.m_elem, p2.m_elem, dim)) ? ans : 0;
}

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) {
	// FIXME become friend

	typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator c_i = c.begin(), c_end = c.end();
	typename container2<CoordType, std::allocator<CoordType> >::const_iterator w_i = weights.begin(),
			w_end = weights.end();

	Point<dim> out;

	if (c_i == c_end || w_i == w_end) {
		return out;
	}

	bool valid = c_i->isValid();

	CoordType tot_weight = *w_i, max_weight = std::fabs(*w_i);
	for (int j = 0; j < dim; ++j) {
		out[j] = (*c_i)[j] * *w_i;
	}

	while (++c_i != c_end && ++w_i != w_end) {
		tot_weight += *w_i;
		CoordType val = std::fabs(*w_i);
		if (val > max_weight)
			max_weight = val;
		if (!c_i->isValid())
			valid = false;
		for (int j = 0; j < dim; ++j)
			out[j] += (*c_i)[j] * *w_i;
	}

	// Make sure the weights don't add up to zero
	if (max_weight <= 0 || std::fabs(tot_weight) <= max_weight * numeric_constants<CoordType>::epsilon()) {
		return out;
	}

	for (int j = 0; j < dim; ++j) {
		out[j] /= tot_weight;
	}

	out.setValid(valid);

	return out;
}

template<int dim, template<class, class> class container>
Point<dim> Barycenter(const container<Point<dim>, std::allocator<Point<dim> > >& c) {
	// FIXME become friend

	typename container<Point<dim>, std::allocator<Point<dim> > >::const_iterator i = c.begin(), end = c.end();

	if (i == end) {
		return Point<dim>();
	}

	Point<dim> out = *i;
	CoordType num_points = 1;

	bool valid = i->isValid();

	while (++i != end) {
		++num_points;
		if (!i->isValid())
			valid = false;
		for (int j = 0; j < dim; ++j)
			out[j] += (*i)[j];
	}

	for (int j = 0; j < dim; ++j) {
		out[j] /= num_points;
	}

	out.setValid(valid);

	return out;
}

template<int dim>
inline Point<dim> Midpoint(const Point<dim>& p1, const Point<dim>& p2, CoordType dist) {
	Point<dim> out;
	CoordType conj_dist = 1 - dist;

	for (int i = 0; i < dim; ++i) {
		out.m_elem[i] = p1.m_elem[i] * conj_dist + p2.m_elem[i] * dist;
	}

	out.m_valid = p1.m_valid && p2.m_valid;

	return out;
}

template<>
Point<2>& Point<2>::polar(CoordType r, CoordType theta);

template<>
void Point<2>::asPolar(CoordType& r, CoordType& theta) const;

template<>
Point<3>& Point<3>::polar(CoordType r, CoordType theta,
						  CoordType z);

template<>
void Point<3>::asPolar(CoordType& r, CoordType& theta,
					   CoordType& z) const;

template<>
Point<3>& Point<3>::spherical(CoordType r, CoordType theta,
							  CoordType phi);

template<>
void Point<3>::asSpherical(CoordType& r, CoordType& theta,
						   CoordType& phi) const;

} // namespace WFMath

#endif  // WFMATH_POINT_FUNCS_H
