// Copyright 2006-2015 Tobias Sargeant (tobias.sargeant@gmail.com).
//
// This file is part of the Carve CSG Library (http://carve-csg.com/)
//
// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#pragma once

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

#include <carve/geom3d.hpp>
#include <carve/vector.hpp>

#include <carve/geom.hpp>

#include <vector>

namespace carve {
namespace geom {

template <unsigned ndim>
void aabb<ndim>::empty() {
  pos.setZero();
  extent.setZero();
}

template <unsigned ndim>
bool aabb<ndim>::isEmpty() const {
  return extent.exactlyZero();
}

template <unsigned ndim>
template <typename iter_t, typename value_type>
void aabb<ndim>::_fit(iter_t begin, iter_t end, value_type) {
  if (begin == end) {
    empty();
    return;
  }

  vector_t min, max;
  aabb<ndim> a = get_aabb<ndim, value_type>()(*begin);
  ++begin;
  min = a.min();
  max = a.max();
  while (begin != end) {
    aabb<ndim> a = get_aabb<ndim, value_type>()(*begin);
    ++begin;
    assign_op(min, min, a.min(), carve::util::min_functor());
    assign_op(max, max, a.max(), carve::util::max_functor());
  }

  pos = (min + max) / 2.0;
  assign_op(extent, max - pos, pos - min, carve::util::max_functor());
}

template <unsigned ndim>
template <typename iter_t>
void aabb<ndim>::_fit(iter_t begin, iter_t end, vector_t) {
  if (begin == end) {
    empty();
    return;
  }

  vector_t min, max;
  bounds(begin, end, min, max);
  pos = (min + max) / 2.0;
  assign_op(extent, max - pos, pos - min, carve::util::max_functor());
}

template <unsigned ndim>
template <typename iter_t>
void aabb<ndim>::_fit(iter_t begin, iter_t end, aabb_t) {
  if (begin == end) {
    empty();
    return;
  }

  vector_t min, max;
  aabb<ndim> a = *begin++;
  min = a.min();
  max = a.max();
  while (begin != end) {
    aabb<ndim> a = *begin;
    ++begin;
    assign_op(min, min, a.min(), carve::util::min_functor());
    assign_op(max, max, a.max(), carve::util::max_functor());
  }

  pos = (min + max) / 2.0;
  assign_op(extent, max - pos, pos - min, carve::util::max_functor());
}

template <unsigned ndim>
void aabb<ndim>::fit(const vector_t& v1) {
  pos = v1;
  extent.setZero();
}

template <unsigned ndim>
void aabb<ndim>::fit(const vector_t& v1, const vector_t& v2) {
  vector_t min, max;
  assign_op(min, v1, v2, carve::util::min_functor());
  assign_op(max, v1, v2, carve::util::max_functor());

  pos = (min + max) / 2.0;
  assign_op(extent, max - pos, pos - min, carve::util::max_functor());
}

template <unsigned ndim>
void aabb<ndim>::fit(const vector_t& v1, const vector_t& v2,
                     const vector_t& v3) {
  vector_t min, max;
  min = max = v1;

  assign_op(min, min, v2, carve::util::min_functor());
  assign_op(max, max, v2, carve::util::max_functor());
  assign_op(min, min, v3, carve::util::min_functor());
  assign_op(max, max, v3, carve::util::max_functor());

  pos = (min + max) / 2.0;
  assign_op(extent, max - pos, pos - min, carve::util::max_functor());
}

template <unsigned ndim>
template <typename iter_t, typename adapt_t>
void aabb<ndim>::fit(iter_t begin, iter_t end, adapt_t adapt) {
  vector_t min, max;

  bounds(begin, end, adapt, min, max);
  pos = (min + max) / 2.0;
  assign_op(extent, max - pos, pos - min, carve::util::max_functor());
}

template <unsigned ndim>
template <typename iter_t>
void aabb<ndim>::fit(iter_t begin, iter_t end) {
  _fit(begin, end, typename std::iterator_traits<iter_t>::value_type());
}

template <unsigned ndim>
void aabb<ndim>::expand(double pad) {
  extent += pad;
}

template <unsigned ndim>
void aabb<ndim>::unionAABB(const aabb<ndim>& a) {
  vector_t vmin, vmax;

  assign_op(vmin, min(), a.min(), carve::util::min_functor());
  assign_op(vmax, max(), a.max(), carve::util::max_functor());
  pos = (vmin + vmax) / 2.0;
  assign_op(extent, vmax - pos, pos - vmin, carve::util::max_functor());
}

template <unsigned ndim>
bool aabb<ndim>::completelyContains(const aabb<ndim>& other) const {
  for (unsigned i = 0; i < ndim; ++i) {
    if (fabs(other.pos.v[i] - pos.v[i]) + other.extent.v[i] > extent.v[i]) {
      return false;
    }
  }
  return true;
}

template <unsigned ndim>
bool aabb<ndim>::containsPoint(const vector_t& v) const {
  for (unsigned i = 0; i < ndim; ++i) {
    if (fabs(v.v[i] - pos.v[i]) > extent.v[i]) {
      return false;
    }
  }
  return true;
}

template <unsigned ndim>
double aabb<ndim>::axisSeparation(const aabb<ndim>& other,
                                  unsigned axis) const {
  return fabs(other.pos.v[axis] - pos.v[axis]) - extent.v[axis] -
         other.extent.v[axis];
}

template <unsigned ndim>
double aabb<ndim>::maxAxisSeparation(const aabb<ndim>& other) const {
  double m = axisSeparation(other, 0);
  for (unsigned i = 1; i < ndim; ++i) {
    m = std::max(m, axisSeparation(other, i));
  }
  return m;
}

template <unsigned ndim>
bool aabb<ndim>::intersects(const aabb<ndim>& other) const {
  return maxAxisSeparation(other) <= 0.0;
}

template <unsigned ndim>
bool aabb<ndim>::intersects(const sphere<ndim>& s) const {
  double r = 0.0;
  for (unsigned i = 0; i < ndim; ++i) {
    double t = fabs(s.C[i] - pos[i]) - extent[i];
    if (t > 0.0) {
      r += t * t;
    }
  }
  return r <= s.r * s.r;
}

template <unsigned ndim>
bool aabb<ndim>::intersects(const plane<ndim>& plane) const {
  double d1 = fabs(distance(plane, pos));
  double d2 = dot(abs(plane.N), extent);
  return d1 <= d2;
}

template <unsigned ndim>
bool aabb<ndim>::intersects(const linesegment<ndim>& ls) const {
  return intersectsLineSegment(ls.v1, ls.v2);
}

template <unsigned ndim>
std::pair<double, double> aabb<ndim>::rangeInDirection(
    const carve::geom::vector<ndim>& v) const {
  double d1 = dot(v, pos);
  double d2 = dot(abs(v), extent);

  return std::make_pair(d1 - d2, d1 + d2);
}

template <unsigned ndim>
typename aabb<ndim>::vector_t aabb<ndim>::min() const {
  return pos - extent;
}

template <unsigned ndim>
typename aabb<ndim>::vector_t aabb<ndim>::mid() const {
  return pos;
}

template <unsigned ndim>
typename aabb<ndim>::vector_t aabb<ndim>::max() const {
  return pos + extent;
}

template <unsigned ndim>
double aabb<ndim>::min(unsigned dim) const {
  return pos.v[dim] - extent.v[dim];
}

template <unsigned ndim>
double aabb<ndim>::mid(unsigned dim) const {
  return pos.v[dim];
}

template <unsigned ndim>
double aabb<ndim>::max(unsigned dim) const {
  return pos.v[dim] + extent.v[dim];
}

template <unsigned ndim>
double aabb<ndim>::volume() const {
  double v = 1.0;
  for (size_t dim = 0; dim < ndim; ++dim) {
    v *= 2.0 * extent.v[dim];
  }
  return v;
}

template <unsigned ndim>
int aabb<ndim>::compareAxis(const axis_pos& ap) const {
  double p = ap.pos - pos[ap.axis];
  if (p > extent[ap.axis]) {
    return -1;
  }
  if (p < -extent[ap.axis]) {
    return +1;
  }
  return 0;
}

template <unsigned ndim>
void aabb<ndim>::constrainMax(const axis_pos& ap) {
  if (pos[ap.axis] + extent[ap.axis] > ap.pos) {
    double min = std::min(ap.pos, pos[ap.axis] - extent[ap.axis]);
    pos[ap.axis] = (min + ap.pos) / 2.0;
    extent[ap.axis] = ap.pos - pos[ap.axis];
  }
}

template <unsigned ndim>
void aabb<ndim>::constrainMin(const axis_pos& ap) {
  if (pos[ap.axis] - extent[ap.axis] < ap.pos) {
    double max = std::max(ap.pos, pos[ap.axis] + extent[ap.axis]);
    pos[ap.axis] = (ap.pos + max) / 2.0;
    extent[ap.axis] = pos[ap.axis] - ap.pos;
  }
}

template <unsigned ndim>
aabb<ndim> aabb<ndim>::getAABB() const {
  return *this;
}

template <unsigned ndim>
aabb<ndim>::aabb(const vector_t& _pos, const vector_t& _extent)
    : pos(_pos), extent(_extent) {}

template <unsigned ndim>
template <typename iter_t, typename adapt_t>
aabb<ndim>::aabb(iter_t begin, iter_t end, adapt_t adapt) {
  fit(begin, end, adapt);
}

template <unsigned ndim>
template <typename iter_t>
aabb<ndim>::aabb(iter_t begin, iter_t end) {
  fit(begin, end);
}

template <unsigned ndim>
aabb<ndim>::aabb(const aabb<ndim>& a, const aabb<ndim>& b) {
  fit(a, b);
}

template <unsigned ndim>
bool operator==(const aabb<ndim>& a, const aabb<ndim>& b) {
  return a.pos == b.pos && a.extent == b.extent;
}

template <unsigned ndim>
bool operator!=(const aabb<ndim>& a, const aabb<ndim>& b) {
  return a.pos != b.pos || a.extent != b.extent;
}

template <unsigned ndim>
std::ostream& operator<<(std::ostream& o, const aabb<ndim>& a) {
  o << (a.pos - a.extent) << "--" << (a.pos + a.extent);
  return o;
}

template <unsigned ndim>
double distance2(const aabb<3>& a, const vector<ndim>& v) {
  double d2 = 0.0;
  for (unsigned i = 0; i < ndim; ++i) {
    double d = ::fabs(v.v[i] - a.pos.v[i]) - a.extent.v[i];
    if (d > 0.0) {
      d2 += d * d;
    }
  }
  return d2;
}

template <unsigned ndim>
double distance(const aabb<3>& a, const vector<ndim>& v) {
  return ::sqrt(distance2(a, v));
}

template <>
inline bool aabb<3>::intersects(const ray<3>& ray) const {
  vector<3> t = pos - ray.v;
  double r;

  // l.cross(x-axis)?
  r = extent.y * fabs(ray.D.z) + extent.z * fabs(ray.D.y);
  if (fabs(t.y * ray.D.z - t.z * ray.D.y) > r) {
    return false;
  }

  // ray.D.cross(y-axis)?
  r = extent.x * fabs(ray.D.z) + extent.z * fabs(ray.D.x);
  if (fabs(t.z * ray.D.x - t.x * ray.D.z) > r) {
    return false;
  }

  // ray.D.cross(z-axis)?
  r = extent.x * fabs(ray.D.y) + extent.y * fabs(ray.D.x);
  if (fabs(t.x * ray.D.y - t.y * ray.D.x) > r) {
    return false;
  }

  return true;
}

template <>
inline bool aabb<3>::intersectsLineSegment(const vector<3>& v1,
                                           const vector<3>& v2) const {
  vector<3> half_length = 0.5 * (v2 - v1);
  vector<3> t = pos - half_length - v1;
  double r;

  // do any of the principal axes form a separating axis?
  if (fabs(t.x) > extent.x + fabs(half_length.x)) {
    return false;
  }
  if (fabs(t.y) > extent.y + fabs(half_length.y)) {
    return false;
  }
  if (fabs(t.z) > extent.z + fabs(half_length.z)) {
    return false;
  }

  // NOTE: Since the separating axis is perpendicular to the line in
  // these last four cases, the line does not contribute to the
  // projection.

  // line.cross(x-axis)?
  r = extent.y * fabs(half_length.z) + extent.z * fabs(half_length.y);
  if (fabs(t.y * half_length.z - t.z * half_length.y) > r) {
    return false;
  }

  // half_length.cross(y-axis)?
  r = extent.x * fabs(half_length.z) + extent.z * fabs(half_length.x);
  if (fabs(t.z * half_length.x - t.x * half_length.z) > r) {
    return false;
  }

  // half_length.cross(z-axis)?
  r = extent.x * fabs(half_length.y) + extent.y * fabs(half_length.x);
  if (fabs(t.x * half_length.y - t.y * half_length.x) > r) {
    return false;
  }

  return true;
}

template <int Ax, int Ay, int Az, int c>
static inline bool intersectsTriangle_axisTest_3(const aabb<3>& aabb,
                                                 const tri<3>& tri) {
  const int d = (c + 1) % 3, e = (c + 2) % 3;
  const vector<3> a = cross(VECTOR(Ax, Ay, Az), tri.v[d] - tri.v[c]);
  double p1 = dot(a, tri.v[c]), p2 = dot(a, tri.v[e]);
  if (p1 > p2) {
    std::swap(p1, p2);
  }
  const double r = dot(abs(a), aabb.extent);
  return !(p1 > r || p2 < -r);
}

template <int c>
static inline bool intersectsTriangle_axisTest_2(const aabb<3>& aabb,
                                                 const tri<3>& tri) {
  double vmin = std::min(std::min(tri.v[0][c], tri.v[1][c]), tri.v[2][c]),
         vmax = std::max(std::max(tri.v[0][c], tri.v[1][c]), tri.v[2][c]);
  return !(vmin > aabb.extent[c] || vmax < -aabb.extent[c]);
}

static inline bool intersectsTriangle_axisTest_1(const aabb<3>& aabb,
                                                 const tri<3>& tri) {
  vector<3> n = cross(tri.v[1] - tri.v[0], tri.v[2] - tri.v[0]);
  double d1 = fabs(dot(n, tri.v[0]));
  double d2 = dot(abs(n), aabb.extent);
  return d1 <= d2;
}

template <>
inline bool aabb<3>::intersects(tri<3> tri) const {
  tri.v[0] -= pos;
  tri.v[1] -= pos;
  tri.v[2] -= pos;

  if (!intersectsTriangle_axisTest_2<0>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_2<1>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_2<2>(*this, tri)) {
    return false;
  }

  if (!intersectsTriangle_axisTest_3<1, 0, 0, 0>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_3<1, 0, 0, 1>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_3<1, 0, 0, 2>(*this, tri)) {
    return false;
  }

  if (!intersectsTriangle_axisTest_3<0, 1, 0, 0>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_3<0, 1, 0, 1>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_3<0, 1, 0, 2>(*this, tri)) {
    return false;
  }

  if (!intersectsTriangle_axisTest_3<0, 0, 1, 0>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_3<0, 0, 1, 1>(*this, tri)) {
    return false;
  }
  if (!intersectsTriangle_axisTest_3<0, 0, 1, 2>(*this, tri)) {
    return false;
  }

  if (!intersectsTriangle_axisTest_1(*this, tri)) {
    return false;
  }

  return true;
}
}  // namespace geom
}  // namespace carve
