// 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 <vector>

namespace carve {
  namespace geom {

    // ========================================================================
    struct _uninitialized { };

    template<unsigned ndim>
    struct base { double v[ndim]; };

    template<> struct base<2> {union { double v[2]; struct { double x, y; }; }; };
    template<> struct base<3> {union { double v[3]; struct { double x, y, z; }; }; };
    template<> struct base<4> {union { double v[4]; struct { double x, y, z, w; }; }; };

    template<unsigned ndim>
    struct vector : public base<ndim> {
      static vector ZERO() { vector<ndim> r; r.setZero(); return r; }
      double length2() const;
      double length() const;
      vector<ndim> &normalize();
      vector<ndim> normalized() const;
      bool exactlyZero() const;
      bool isZero(double epsilon = EPSILON) const;
      void setZero();
      void fill(double val);
      vector<ndim> &scaleBy(double d);
      vector<ndim> &invscaleBy(double d);
      vector<ndim> scaled(double d) const;
      vector<ndim> invscaled(double d) const;
      vector<ndim> &negate();
      vector<ndim> negated() const;
      double &operator[](unsigned i);
      const double &operator[](unsigned i) const;
      template<typename assign_t>
      vector<ndim> &operator=(const assign_t &t);
      std::string asStr() const;
      vector() { setZero(); }
      vector(noinit_t) { }
    };

    static inline vector<2> VECTOR(double x, double y) { vector<2> r; r.x = x; r.y = y; return r; }
    static inline vector<3> VECTOR(double x, double y, double z) { vector<3> r; r.x = x; r.y = y; r.z = z; return r; }
    static inline vector<4> VECTOR(double x, double y, double z, double w) { vector<4> r; r.x = x; r.y = y; r.z = z; r.w = w; return r; }

    template<unsigned ndim, typename val_t>
    double dot(const vector<ndim> &a, const val_t &b) {
      double r = 0.0;
      for (unsigned i = 0; i < ndim; ++i) r += a[i] * b[i];
      return r;
    }

    template<unsigned ndim>
    vector<ndim> operator-(const vector<ndim> &a) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = -a[i];
      return c;
    }

    template<unsigned ndim>
    vector<ndim> &operator*=(vector<ndim> &a, double s) {
      for (unsigned i = 0; i < ndim; ++i) a[i] *= s;
      return a;
    }

    template<unsigned ndim>
    vector<ndim> &operator/=(vector<ndim> &a, double s) {
      for (unsigned i = 0; i < ndim; ++i) a[i] /= s;
      return a;
    }

    template<unsigned ndim>
    vector<ndim> operator*(const vector<ndim> &a, double s) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] * s;
      return c;
    }

    template<unsigned ndim>
    vector<ndim> operator*(double s, const vector<ndim> &a) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] * s;
      return c;
    }

    template<unsigned ndim>
    vector<ndim> operator/(const vector<ndim> &a, double s) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] / s;
      return c;
    }

    template<unsigned ndim>
    vector<ndim> &operator+=(vector<ndim> &a, const vector<ndim> &b) {
      for (unsigned i = 0; i < ndim; ++i) a[i] += b[i];
      return a;
    }

    template<unsigned ndim, typename val_t>
    vector<ndim> &operator+=(vector<ndim> &a, const val_t &b) {
      for (unsigned i = 0; i < ndim; ++i) a[i] += b[i];
      return a;
    }

    template<unsigned ndim>
    vector<ndim> &operator+=(vector<ndim> &a, double b) {
      for (unsigned i = 0; i < ndim; ++i) a[i] += b;
      return a;
    }

    template<unsigned ndim>
    vector<ndim> operator+(const vector<ndim> &a, const vector<ndim> &b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b[i];
      return c;
    }

    template<unsigned ndim, typename val_t>
    vector<ndim> operator+(const val_t &a, const vector<ndim> &b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b[i];
      return c;
    }

    template<unsigned ndim, typename val_t>
    vector<ndim> operator+(const vector<ndim> &a, const val_t &b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b[i];
      return c;
    }

    template<unsigned ndim>
    vector<ndim> operator+(const vector<ndim> &a, double b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] + b;
      return c;
    }

    template<unsigned ndim>
    vector<ndim> &operator-=(vector<ndim> &a, const vector<ndim> &b) {
      for (unsigned i = 0; i < ndim; ++i) a[i] -= b[i];
      return a;
    }

    template<unsigned ndim, typename val_t>
    vector<ndim> &operator-=(vector<ndim> &a, const val_t &b) {
      for (unsigned i = 0; i < ndim; ++i) a[i] -= b[i];
      return a;
    }

    template<unsigned ndim>
    vector<ndim> &operator-=(vector<ndim> &a, double b) {
      for (unsigned i = 0; i < ndim; ++i) a[i] -= b;
      return a;
    }

    template<unsigned ndim>
    vector<ndim> operator-(const vector<ndim> &a, const vector<ndim> &b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b[i];
      return c;
    }

    template<unsigned ndim, typename val_t>
    vector<ndim> operator-(const vector<ndim> &a, const val_t &b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b[i];
      return c;
    }

    template<unsigned ndim, typename val_t>
    vector<ndim> operator-(const val_t &a, const vector<ndim> &b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b[i];
      return c;
    }

    template<unsigned ndim>
    vector<ndim> operator-(const vector<ndim> &a, double b) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = a[i] - b;
      return c;
    }

    template<unsigned ndim>
    vector<ndim> abs(const vector<ndim> &a) {
      vector<ndim> c(NOINIT);
      for (unsigned i = 0; i < ndim; ++i) c[i] = fabs(a[i]);
      return c;
    }

    template<unsigned ndim, typename assign_t, typename oper_t>
    vector<ndim> &assign_op(vector<ndim> &a, const assign_t &t, oper_t op) {
      for (unsigned i = 0; i < ndim; ++i) a[i] = op(t[i]);
      return a;
    }

    template<unsigned ndim, typename assign1_t, typename assign2_t, typename oper_t>
    vector<ndim> &assign_op(vector<ndim> &a, const assign1_t &t1, const assign2_t &t2, oper_t op) {
      for (unsigned i = 0; i < ndim; ++i) a[i] = op(t1[i], t2[i]);
      return a;
    }

    template<unsigned ndim, typename iter_t, typename adapt_t>
    void bounds(iter_t begin, iter_t end, adapt_t adapt, vector<ndim> &min, vector<ndim> &max) {
      if (begin == end) {
        min.setZero();
        max.setZero();
      } else {
        min = max = adapt(*begin);
        while (++begin != end) {
          vector<ndim> v = adapt(*begin);
          assign_op(min, min, v, carve::util::min_functor());
          assign_op(max, max, v, carve::util::max_functor());
        }
      }
    }

    template<unsigned ndim, typename iter_t>
    void bounds(iter_t begin, iter_t end, vector<ndim> &min, vector<ndim> &max) {
      if (begin == end) {
        min.setZero();
        max.setZero();
      } else {
        min = max = *begin;
        while (++begin != end) {
          vector<ndim> v = *begin;
          assign_op(min, min, v, carve::util::min_functor());
          assign_op(max, max, v, carve::util::max_functor());
        }
      }
    }

    template<unsigned ndim>
    bool operator==(const vector<ndim> &a, const vector<ndim> &b) {
      for (unsigned i = 0; i < ndim; ++i) { if (a[i] != b[i]) return false; }
      return true;
    }

    template<unsigned ndim>
    bool operator!=(const vector<ndim> &a, const vector<ndim> &b) {
      return !(a == b);
    }

    template<unsigned ndim>
    bool operator<(const vector<ndim> &a, const vector<ndim> &b) {
      for (unsigned i = 0; i < ndim; ++i) { if (a[i] < b[i]) return true; if (a[i] > b[i]) return false; }
      return false;
    }

    template<unsigned ndim>
    bool operator<=(const vector<ndim> &a, const vector<ndim> &b) {
      return !(b < a);
    }

    template<unsigned ndim>
    bool operator>(const vector<ndim> &a, const vector<ndim> &b) {
      return b < a;
    }

    template<unsigned ndim>
    bool operator>=(const vector<ndim> &a, const vector<ndim> &b) {
      return !(a < b);
    }

    template<unsigned ndim>
    double distance2(const vector<ndim> &a, const vector<ndim> &b) {
      return (b - a).length2();
    }

    template<unsigned ndim>
    double distance(const vector<ndim> &a, const vector<ndim> &b) {
      return (b - a).length();
    }

    template<unsigned ndim>
    bool equal(const vector<ndim> &a, const vector<ndim> &b) {
      return (b - a).isZero();
    }

    template<unsigned ndim>
    int smallestAxis(const vector<ndim> &a) {
      int x = 0;
      double y = fabs(a[0]);
      for (unsigned i = 1; i < ndim; ++i) {
        double z = fabs(a[i]);
        if (z <= y) { y = z; x = i; }
      }
      return x;
    }

    template<unsigned ndim>
    int largestAxis(const vector<ndim> &a) {
      int x = 0;
      double y = fabs(a[0]);
      for (unsigned i = 1; i < ndim; ++i) {
        double z = fabs(a[i]);
        if (z > y) { y = z; x = i; }
      }
      return x;
    }

    template<unsigned ndim>
    double vector<ndim>::length2() const { return dot(*this, *this); }
    template<unsigned ndim>
    double vector<ndim>::length() const { return sqrt(dot(*this, *this)); }

    template<unsigned ndim>
    vector<ndim> &vector<ndim>::normalize() { *this /= length(); return *this; }
    template<unsigned ndim>
    vector<ndim> vector<ndim>::normalized() const { return *this / length(); }

    template<unsigned ndim>
    bool vector<ndim>::exactlyZero() const {
      for (unsigned i = 0; i < ndim; ++i) if (this->v[i]) return false;
      return true;
    }
    template<unsigned ndim>
    bool vector<ndim>::isZero(double epsilon) const {
      return length2() < epsilon * epsilon;
    }

    template<unsigned ndim>
    void vector<ndim>::setZero() { for (size_t i = 0; i < ndim; ++i) this->v[i] = 0.0; }

    template<unsigned ndim>
    void vector<ndim>::fill(double val)  { for (size_t i = 0; i < ndim; ++i) this->v[i] = val; }

    template<unsigned ndim>
    vector<ndim> &vector<ndim>::scaleBy(double d) { for (unsigned i = 0; i < ndim; ++i) this->v[i] *= d; return *this; }
    template<unsigned ndim>
    vector<ndim> &vector<ndim>::invscaleBy(double d) { for (unsigned i = 0; i < ndim; ++i) this->v[i] /= d; return *this; }

    template<unsigned ndim>
    vector<ndim> vector<ndim>::scaled(double d) const { return *this * d; }
    template<unsigned ndim>
    vector<ndim> vector<ndim>::invscaled(double d) const { return *this / d; }

    template<unsigned ndim>
    vector<ndim> &vector<ndim>::negate() { for (unsigned i = 0; i < ndim; ++i) this->v[i] = -this->v[i]; return *this; }
    template<unsigned ndim>
    vector<ndim> vector<ndim>::negated() const { return -*this; }

    template<unsigned ndim>
    double &vector<ndim>::operator[](unsigned i) { return this->v[i]; }
    template<unsigned ndim>
    const double &vector<ndim>::operator[](unsigned i) const { return this->v[i]; }

    template<unsigned ndim>
    template<typename assign_t>
    vector<ndim> &vector<ndim>::operator=(const assign_t &t) {
      for (unsigned i = 0; i < ndim; ++i) this->v[i] = t[i];
      return *this;
    }

    template<unsigned ndim>
    std::string vector<ndim>::asStr() const {
      std::ostringstream out;
      out << '<';
      out << std::setprecision(24);
      for (unsigned i = 0; i < ndim; ++i) { if (i) out << ','; out << this->v[i]; }
      out << '>';
      return out.str();
    }

    template<unsigned ndim, typename iter_t, typename adapt_t>
    void centroid(iter_t begin, iter_t end, adapt_t adapt, vector<ndim> &c) {
      c.setZero();
      int n = 0;
      while (begin != end) { c += adapt(*begin++); ++n; }
      c /= double(n);
    }

    template<unsigned ndim>
    vector<2> select(const vector<ndim> &a, int a1, int a2) {
      vector<2> r(NOINIT);
      r.v[0] = a.v[a1]; r.v[1] = a.v[a2];
      return r;
    }

    template<unsigned ndim>
    vector<3> select(const vector<ndim> &a, int a1, int a2, int a3) {
      vector<3> r(NOINIT);
      r.v[0] = a.v[a1]; r.v[1] = a.v[a2]; r.v[2] = a.v[a3];
      return r;
    }

    static inline vector<3> cross(const vector<3> &a, const vector<3> &b) {
      // Compute a x b
      return VECTOR(+(a.y * b.z - a.z * b.y),
                    -(a.x * b.z - a.z * b.x),
                    +(a.x * b.y - a.y * b.x));
    }

    static inline double cross(const vector<2> &a, const vector<2> &b) {
      // Compute a x b
      return a.x * b.y - b.x * a.y;
    }

    static inline double dotcross(const vector<3> &a, const vector<3> &b, const vector<3> &c) {
      // Compute a . (b x c)
      return
        (a.x * b.y * c.z + a.y * b.z * c.x + a.z * b.x * c.y) -
        (a.x * b.z * c.y + a.y * b.x * c.z + a.z * b.y * c.x);
    }


    // ========================================================================
    struct axis_pos {
      int axis;
      double pos;

      axis_pos(int _axis, double _pos) : axis(_axis), pos(_pos) {
      }
    };

    template<unsigned ndim>
    double distance(const axis_pos &a, const vector<ndim> &b) {
      return fabs(b[a.axis] - a.pos);
    }

    template<unsigned ndim>
    double distance2(const axis_pos &a, const vector<ndim> &b) {
      double r = fabs(b[a.axis] - a.pos);
      return r * r;
    }

    template<unsigned ndim> bool operator<(const axis_pos &a, const vector<ndim> &b) { return a.pos < b[a.axis]; }
    template<unsigned ndim> bool operator<(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] < b.pos; }

    template<unsigned ndim> bool operator<=(const axis_pos &a, const vector<ndim> &b) { return a.pos <= b[a.axis]; }
    template<unsigned ndim> bool operator<=(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] <= b.pos; }

    template<unsigned ndim> bool operator>(const axis_pos &a, const vector<ndim> &b) { return a.pos > b[a.axis]; }
    template<unsigned ndim> bool operator>(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] > b.pos; }

    template<unsigned ndim> bool operator>=(const axis_pos &a, const vector<ndim> &b) { return a.pos >= b[a.axis]; }
    template<unsigned ndim> bool operator>=(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] >= b.pos; }

    template<unsigned ndim> bool operator==(const axis_pos &a, const vector<ndim> &b) { return a.pos == b[a.axis]; }
    template<unsigned ndim> bool operator==(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] == b.pos; }

    template<unsigned ndim> bool operator!=(const axis_pos &a, const vector<ndim> &b) { return a.pos != b[a.axis]; }
    template<unsigned ndim> bool operator!=(const vector<ndim> &a, const axis_pos &b) { return a[b.axis] != b.pos; }


    // ========================================================================
    template<unsigned ndim>
    struct ray {
      typedef vector<ndim> vector_t;

      vector_t D, v;

      ray() { }
      ray(vector_t _D, vector_t _v) : D(_D), v(_v) { }
      bool OK() const { return !D.isZero(); }
    };

    static inline double distance2(const ray<3> &r, const vector<3> &v) {
      return cross(r.D, v - r.v).length2() / r.D.length2();
    }

    static inline double distance(const ray<3> &r, const vector<3> &v) {
      return sqrt(distance2(r, v));
    }

    inline double distance2(const ray<2> &r, const vector<2> &v) {
      double t = cross(r.D, v - r.v);
      return (t * t) / r.D.length2();
    }

    inline double distance(const ray<2> &r, const vector<2> &v) {
      return sqrt(distance2(r, v));
    }
    template<unsigned ndim>
    ray<ndim> rayThrough(const vector<ndim> &a, const vector<ndim> &b) {
      return ray<ndim>(b - a, a);
    }



    // ========================================================================
    template<unsigned ndim>
    struct linesegment {
      typedef vector<ndim> vector_t;

      vector_t v1;
      vector_t v2;
      vector_t midpoint;
      vector_t half_length;

      linesegment(const vector_t &_v1, const vector_t &_v2) : v1(_v1), v2(_v2) {
        update();
      }

      void update() {
        midpoint = (v2 + v1) / 2.0;
        half_length = (v2 - v1) / 2.0;
      }
      bool OK() const {
        return !half_length.isZero();
      }
      void flip() {
        std::swap(v1, v2);
        half_length = (v2 - v1) / 2.0;
      }
    };

    template<unsigned ndim>
    double distance2(const linesegment<ndim> &l, const vector<ndim> &v) {
      vector<ndim> D = l.v2 - l.v1;
      double t = dot(v - l.v1, D) / dot(D, D);
      if (t <= 0.0) return (v - l.v1).length2();
      if (t >= 1.0) return (v - l.v2).length2();
      vector<ndim> vc = D * t + l.v1;
      return (v - vc).length2();
    }

    template<unsigned ndim>
    double distance(const linesegment<ndim> &l, const vector<ndim> &v) {
      return sqrt(distance2(l, v));
    }



    // ========================================================================
    template<unsigned ndim>
    struct plane {
      typedef vector<ndim> vector_t;

      vector_t N;
      double d;

      // plane() { }
      plane() { N.setZero(); N[0] = 1.0; d= 0.0; }
      plane(const vector_t &_N, vector_t _p) : N(_N), d(-dot(_p, _N)) { }
      plane(const vector_t &_N, double _d) : N(_N), d(_d) { }
      void negate() { N.negate(); d = -d; }
    };

    template<unsigned ndim>
    inline plane<ndim> operator-(const plane<ndim> &p) {
      return plane<ndim>(-p.N, -p.d);
    }

    template<unsigned ndim, typename val_t>
    double distance(const plane<ndim> &plane, const val_t &point) {
      return dot(plane.N, point) + plane.d;
    }

    template<unsigned ndim>
    static inline vector<ndim> closestPoint(const plane<ndim> &p, const vector<ndim> &v) {
      return v - p.N * (p.d + dot(p.N, v)) / dot(p.N, p.N);
    }



    // ========================================================================
    template<unsigned ndim>
    struct sphere {
      typedef vector<ndim> vector_t;

      vector_t C;
      double r;

      // sphere() { }
      sphere() { C.setZero(); r = 1.0; }
      sphere(const vector_t &_C, double _r) : C(_C), r(_r) { }
    };

    template<unsigned ndim, typename val_t>
    double distance(const sphere<ndim> &sphere, const val_t &point) {
      return std::max(0.0, distance(sphere.C, point) - sphere.r);
    }

    template<unsigned ndim>
    static inline vector<ndim> closestPoint(const sphere<ndim> &sphere, const vector<ndim> &point) {
      return (point - sphere.C).normalized() * sphere.r;
    }


    // ========================================================================
    template<unsigned ndim>
    struct tri {
      typedef vector<ndim> vector_t;

      vector_t v[3];

      tri(vector_t _v[3]) {
        std::copy(v, v+3, _v);
      }
      tri(const vector_t &a, const vector_t &b, const vector_t &c) {
        v[0] = a; v[1] = b; v[2] = c;
      }
    };

    template<unsigned ndim>
    inline std::ostream &operator<<(std::ostream &o, const vector<ndim> &v) {
      o << v.asStr();
      return o;
    }

    template<unsigned ndim>
    inline std::ostream &operator<<(std::ostream &o, const carve::geom::plane<ndim> &p) {
      o << p.N << ";" << p.d;
      return o;
    }
    
    template<unsigned ndim>
    std::ostream &operator<<(std::ostream &o, const carve::geom::sphere<ndim> &sphere) {
      o << "{sphere " << sphere.C << ";" << sphere.r << "}";
      return o;
    }
    
    template<unsigned ndim>
    std::ostream &operator<<(std::ostream &o, const carve::geom::tri<ndim> &tri) {
      o << "{tri " << tri.v[0] << ";" << tri.v[1] << ";" << tri.v[2] << "}";
      return o;
    }

  }
}


