// Begin License:
// Copyright (C) 2006-2008 Tobias Sargeant (tobias.sargeant@gmail.com).
// All rights reserved.
//
// This file is part of the Carve CSG Library (http://carve-csg.com/)
//
// This file may be used under the terms of the GNU General Public
// License version 2.0 as published by the Free Software Foundation
// and appearing in the file LICENSE.GPL2 included in the packaging of
// this file.
//
// This file is provided "AS IS" with NO WARRANTY OF ANY KIND,
// INCLUDING THE WARRANTIES OF DESIGN, MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE.
// End:


#pragma once

#include <carve/carve.hpp>
#include <carve/geom.hpp>

#include <math.h>
#include <carve/math_constants.hpp>

#include <vector>
#include <list>
#include <map>

#if defined(CARVE_DEBUG)
#  include <iostream>
#endif

namespace carve {
  namespace geom3d {

    typedef carve::geom::plane<3> Plane;
    typedef carve::geom::ray<3> Ray;
    typedef carve::geom::linesegment<3> LineSegment;
    typedef carve::geom::vector<3> Vector;

    template<typename iter_t, typename adapt_t>
    bool fitPlane(iter_t begin, iter_t end, adapt_t adapt, Plane &plane) {
      Vector centroid;
      carve::geom::centroid(begin, end, adapt, centroid);
      iter_t i;

      Vector n = Vector::ZERO();
      Vector v, z;
      Vector p1, p2, p3, c1, c2;
      if (begin == end) return false;

      i = begin;
      p1 = c1 = adapt(*i++); if (i == end) return false;
      p2 = c2 = adapt(*i++); if (i == end) return false;

#if defined(CARVE_DEBUG)
      size_t N = 2;
#endif
      while (i != end) {
        p3 = adapt(*i++);
        v = cross(p3 - p2, p1 - p2);
        if (v.v[largestAxis(v)]) v.negate();
        n += v;
        p1 = p2; p2 = p3;
#if defined(CARVE_DEBUG)
        ++N;
#endif
      }

      p1 = p2; p2 = p3; p3 = c1;
      v = cross(p3 - p2, p1 - p2);
      if (v.v[largestAxis(v)]) v.negate();
      n += v;

      p1 = p2; p2 = p3; p3 = c2;
      v = cross(p3 - p2, p1 - p2);
      if (v.v[largestAxis(v)]) v.negate();
      n += v;

      n.normalize();
      plane.N = n;
      plane.d = -dot(n, centroid);
#if defined(CARVE_DEBUG)
      if (N > 3) {
        std::cerr << "N = " << N << " fitted distance:";
        for (i = begin; i != end; ++i) {
          Vector p = adapt(*i);
          std::cerr << " {" << p << "} " << distance(plane, p);
        }
        std::cerr << std::endl;
      }
#endif
      return true;
    }

    bool planeIntersection(const Plane &a, const Plane &b, Ray &r);

    IntersectionClass rayPlaneIntersection(const Plane &p,
                                           const Vector &v1,
                                           const Vector &v2,
                                           Vector &v,
                                           double &t);

    IntersectionClass lineSegmentPlaneIntersection(const Plane &p,
                                                   const LineSegment &line,
                                                   Vector &v);

    RayIntersectionClass rayRayIntersection(const Ray &r1,
                                            const Ray &r2,
                                            Vector &v1,
                                            Vector &v2,
                                            double &mu1,
                                            double &mu2);



    // test whether point d is above, below or on the plane formed by the triangle a,b,c.
    // return: +ve = d is below a,b,c
    //         -ve = d is above a,b,c
    //           0 = d is on a,b,c
    static inline double orient3d(const carve::geom3d::Vector &a,
        const carve::geom3d::Vector &b,
        const carve::geom3d::Vector &c,
        const carve::geom3d::Vector &d) {
      return dotcross((a - d), (b - d), (c - d));
    }



    // Volume of a tetrahedron described by 4 points. Will be
    // positive if the anticlockwise normal of a,b,c is oriented out
    // of the tetrahedron.
    //
    // see: http://mathworld.wolfram.com/Tetrahedron.html
    inline double tetrahedronVolume(const Vector &a,
                                    const Vector &b,
                                    const Vector &c,
                                    const Vector &d) {
      return dotcross((a - d), (b - d), (c - d)) / 6.0;
    }

    /** 
     * \brief Determine whether p is internal to the wedge defined by
     *        the area between the planes defined by a,b,c and a,b,d
     *        angle abc, where ab is the apex of the angle.
     *
     * @param[in] a 
     * @param[in] b 
     * @param[in] c
     * @param[in] d
     * @param[in] p 
     * 
     * @return true, if p is contained in the wedge defined by the
     *               area between the planes defined by a,b,c and
     *               a,b,d. If the wedge is reflex, p is considered to
     *               be contained if it lies on either plane. Acute
     *               wdges do not contain p if p lies on either
     *               plane. This is so that internalToWedge(a,b,c,d,p) =
     *               !internalToWedge(a,b,d,c,p)
     */
    inline bool internalToWedge(const Vector &a,
                                const Vector &b,
                                const Vector &c,
                                const Vector &d,
                                const Vector &p) {
      bool reflex = (c < d) ?
        orient3d(a, b, c, d) >= 0.0 :
        orient3d(a, b, d, c) < 0.0;

      double d1 = orient3d(a, b, c, p);
      double d2 = orient3d(a, b, d, p);

      if (reflex) {
        // above a,b,c or below a,b,d (or coplanar with either)
        return d1 <= 0.0 || d2 >= 0.0;
      } else {
        // above a,b,c and below a,b,d
        return d1 < 0.0 && d2 > 0.0;
      }
    }

    /** 
     * \brief Determine the ordering relationship of a and b, when
     *        rotating around direction, starting from base.
     *
     * @param[in] adirection
     * @param[in] base
     * @param[in] a
     * @param[in] b
     * 
     * @return 
     *         * -1, if a is ordered before b around, rotating about direction.
     *         * 0, if a and b are equal in angle.
     *         * +1, if a is ordered after b around, rotating about direction.
     */
    inline int compareAngles(const Vector &direction, const Vector &base, const Vector &a, const Vector &b) {
      double d1 = carve::geom3d::orient3d(carve::geom::VECTOR(0,0,0), direction, a, b);
      double d2 = carve::geom3d::orient3d(carve::geom::VECTOR(0,0,0), direction, base, a);
      double d3 = carve::geom3d::orient3d(carve::geom::VECTOR(0,0,0), direction, base, b);

      // CASE: a and b are coplanar wrt. direction.
      if (d1 == 0.0) {
        // a and b point in the same direction.
        if (dot(a, b) > 0.0) {
          // Neither is less than the other.
          return 0;
        }

        // a and b point in opposite directions.
        double d2 = carve::geom3d::orient3d(carve::geom::VECTOR(0,0,0), direction, base, a);
        // * if d2 < 0.0, a is above plane(direction, base) and is less
        //   than b.
        // * if d2 == 0.0 a is coplanar with plane(direction, base) and is
        //   less than b if it points in the same direction as base.
        // * if d2 > 0.0, a is below plane(direction, base) and is greater
        //   than b.

        if (d2 == 0.0) { return dot(a, base) > 0.0 ? -1 : +1; }
        if (d3 == 0.0) { return dot(b, base) > 0.0 ? +1 : -1; }
        if (d2 < 0.0 && d3 > 0.0) return -1;
        if (d2 > 0.0 && d3 < 0.0) return +1;

        // both a and b are to one side of plane(direction, base) -
        // rounding error (if a and b are truly coplanar with
        // direction, one should be above, and one should be below any
        // other plane that is not itself coplanar with
        // plane(direction, a|b) - which would imply d2 and d3 == 0.0).

        // If both are below plane(direction, base) then the one that
        // points in the same direction as base is greater.
        // If both are above plane(direction, base) then the one that
        // points in the same direction as base is lesser.
        if (d2 > 0.0) { return dot(a, base) > 0.0 ? +1 : -1; }
        else          { return dot(a, base) > 0.0 ? -1 : +1; }
      }

      // CASE: a and b are not coplanar wrt. direction

      if (d2 < 0.0) {
        // if a is above plane(direction,base), then a is less than b if
        // b is below plane(direction,base) or b is above plane(direction,a)
        return (d3 > 0.0 || d1 < 0.0) ? -1 : +1;
      } else if (d2 == 0.0) {
        // if a is on plane(direction,base) then a is less than b if a
        // points in the same direction as base, or b is below
        // plane(direction,base)
        return (dot(a, base) > 0.0 || d3 > 0.0) ? -1 : +1;
      } else {
        // if a is below plane(direction,base), then a is less than b if b
        // is below plane(direction,base) and b is above plane(direction,a)
        return (d3 > 0.0 && d1 < 0.0) ? -1 : +1;
      }
    }

    // The anticlockwise angle from vector "from" to vector "to", oriented around the vector "orient".
    static inline double antiClockwiseAngle(const Vector &from, const Vector &to, const Vector &orient) {
      double dp = dot(from, to);
      Vector cp = cross(from, to);
      if (cp.isZero()) {
        if (dp < 0) {
          return M_PI;
        } else {
          return 0.0;
        }
      } else {
        if (dot(cp, orient) > 0.0) {
          return acos(dp);
        } else {
          return M_TWOPI - acos(dp);
        }
      }
    }



    static inline double antiClockwiseOrdering(const Vector &from, const Vector &to, const Vector &orient) {
      double dp = dot(from, to);
      Vector cp = cross(from, to);
      if (cp.isZero()) {
        if (dp < 0) {
          return 2.0;
        } else {
          return 0.0;
        }
      } else {
        if (dot(cp, orient) > 0.0) {
          // 1..-1 -> 0..2
          return 1.0 - dp;
        } else {
          // -1..1 -> 2..4
          return dp + 1.0;
        }
      }
    }



  }
}