/*
  This code is based on code from the Geometric Tools library,
  which is licensed under a boost license.
  Such usage is permitted by the boost license; for details, 
  please see the boost license below.
*/

// Geometric Tools, LLC
// Copyright (c) 1998-2014
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt

/*************************************************************************
 *                                                                       *
 * We release our improvements to the wildMagic code under our standard  *
 * Vega FEM license, as follows:                                         *
 *                                                                       *
 * Vega FEM Simulation Library Version 3.1                               *
 *                                                                       *
 * "improvements to the wildMagic library" , Copyright (C) 2016 USC      *
 * All rights reserved.                                                  *
 *                                                                       *
 * Code author: Yijing Li                                                *
 * http://www.jernejbarbic.com/code                                      *
 *                                                                       *
 * Funding: National Science Foundation                                  *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of the BSD-style license that is            *
 * included with this library in the file LICENSE.txt                    *
 *                                                                       *
 * This library 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 file     *
 * LICENSE.TXT for more details.                                         *
 *                                                                       *
 *************************************************************************/

#include "verticesQuery.h"
#include <cassert>
#include <cstring>
#include <iostream>
#include <cmath>
using namespace std;

VerticesQuery::VerticesQuery (int numVertices, const Vec3d* v)
{
  assert(numVertices > 0 && v);
  vertices.resize(numVertices);
  memcpy(vertices.data(), v, sizeof(Vec3d) * numVertices);
}


VerticesQuery::~VerticesQuery ()
{
}

void VerticesQuery::addVertex(const Vec3d & v) 
{
  vertices.push_back(v);
}

int VerticesQuery::toPlane (int i, int v0, int v1, int v2) const
{
  return toPlane(vertices[i], v0, v1, v2);
}

int VerticesQuery::toPlane (const Vec3d& test, int v0, int v1, int v2) const
{
  bool positive = sort(v0, v1, v2);

  const Vec3d & vec0 = vertices[v0];
  const Vec3d & vec1 = vertices[v1];
  const Vec3d & vec2 = vertices[v2];

  double x0 = test[0] - vec0[0];
  double y0 = test[1] - vec0[1];
  double z0 = test[2] - vec0[2];
  double x1 = vec1[0] - vec0[0];
  double y1 = vec1[1] - vec0[1];
  double z1 = vec1[2] - vec0[2];
  double x2 = vec2[0] - vec0[0];
  double y2 = vec2[1] - vec0[1];
  double z2 = vec2[2] - vec0[2];

  double det = det3(x0,y0,z0,x1,y1,z1,x2,y2,z2);
  if (!positive)
    det = -det;

  return (det > (double)0 ? +1 : (det < (double)0 ? -1 : 0));
}
//----------------------------------------------------------------------------

int VerticesQuery::toTetrahedron (int i, int v0, int v1, int v2,
    int v3) const
{
  return toTetrahedron(vertices[i], v0, v1, v2, v3);
}
//----------------------------------------------------------------------------

int VerticesQuery::toTetrahedron (const Vec3d& test, int v0, int v1, int v2, int v3) const
{
  int sign0 = toPlane(test, v1, v2, v3);
  if (sign0 > 0)
    return +1;

  int sign1 = toPlane(test, v0, v2, v3);
  if (sign1 < 0)
    return +1;

  int sign2 = toPlane(test, v0, v1, v3);
  if (sign2 > 0)
    return +1;

  int sign3 = toPlane(test, v0, v1, v2);
  if (sign3 < 0)
    return +1;

  return ((sign0 && sign1 && sign2 && sign3) ? -1 : 0);
}

int VerticesQuery::toCircumsphere (int i, int v0, int v1, int v2, int v3) const
{
  return toCircumsphere(vertices[i], v0, v1, v2, v3);
}

int VerticesQuery::toCircumsphere (const Vec3d& test, int v0, int v1, int v2, int v3) const
{
  bool positive = sort(v0, v1, v2, v3);

  const Vec3d& vec0 = vertices[v0];
  const Vec3d& vec1 = vertices[v1];
  const Vec3d& vec2 = vertices[v2];
  const Vec3d& vec3 = vertices[v3];

  double s0x = vec0[0] + test[0];
  double d0x = vec0[0] - test[0];
  double s0y = vec0[1] + test[1];
  double d0y = vec0[1] - test[1];
  double s0z = vec0[2] + test[2];
  double d0z = vec0[2] - test[2];
  double s1x = vec1[0] + test[0];
  double d1x = vec1[0] - test[0];
  double s1y = vec1[1] + test[1];
  double d1y = vec1[1] - test[1];
  double s1z = vec1[2] + test[2];
  double d1z = vec1[2] - test[2];
  double s2x = vec2[0] + test[0];
  double d2x = vec2[0] - test[0];
  double s2y = vec2[1] + test[1];
  double d2y = vec2[1] - test[1];
  double s2z = vec2[2] + test[2];
  double d2z = vec2[2] - test[2];
  double s3x = vec3[0] + test[0];
  double d3x = vec3[0] - test[0];
  double s3y = vec3[1] + test[1];
  double d3y = vec3[1] - test[1];
  double s3z = vec3[2] + test[2];
  double d3z = vec3[2] - test[2];
  double w0 = s0x*d0x + s0y*d0y + s0z*d0z;
  double w1 = s1x*d1x + s1y*d1y + s1z*d1z;
  double w2 = s2x*d2x + s2y*d2y + s2z*d2z;
  double w3 = s3x*d3x + s3y*d3y + s3z*d3z;

  double det = det4(d0x, d0y, d0z, w0, d1x, d1y, d1z, w1, d2x, d2y, d2z, w2,
      d3x,d3y,d3z,w3);
  if (!positive)
    det = -det;
  return (det > 0 ? 1 : (det < 0 ? -1 : 0));
}


int VerticesQuery::toCircumsphere (int i, int v0, int v1, int v2) const 
{
  return toCircumsphere(vertices[i], v0,v1,v2);
}

// https://en.wikipedia.org/wiki/Circumscribed_circle
// circumcenter of triangle(p0,p1,p2):
// O = (|p2-p1|^2 dot(p0-p1,p0-p2) p0 + |p0-p2|^2 dot(p1-p0,p1-p2) p1 + |p1-p0|^2 dot(p2-p0,p2-p1) p2 ) / (2 |cross(p1-p0, p2-p1)|^2)
// radius = |p0-p1| |p1-p2| |p2-p0| / (2 |cross(p1-p0, p2-p1) )
// p1-p0 = d0, p2-p1 = d1, p0-p2 = d2

// So, S = cross(d0, d1)
// O = - (|d1|^2 dot(d0,d2) p0 + |d2|^2 dot(d0,d1) p1 + |d0|^2 dot(d2, d1) p2 ) / ( 2 |S|^2)
// radius = sqrt( d0^2 d1^2 d2^2 / 4 S^2 )
int VerticesQuery::toCircumsphere (const Vec3d & test, int v0, int v1, int v2) const 
{
  const Vec3d & p0 = vertices[v0];
  const Vec3d & p1 = vertices[v1];
  const Vec3d & p2 = vertices[v2];
  sort(v0,v1,v2);
  Vec3d d0 = p1 - p0, d1 = p2 - p1, d2 = p0 - p2;
  Vec3d S = cross(d0, d1);
  double S2 = len2(S);
  double d02 = len2(d0), d12 = len2(d1), d22 = len2(d2);
  Vec3d center( (d12 * ::dot(d0,d2)) * p0 + (d22 * ::dot(d0,d1)) * p1 + (d02 * ::dot(d2,d1)) * p2 );
  center /= -(2 * S2);
  double r2 = d02 * d12 * d22 / (4*S2);
  //assert(abs(len2(center - p0) - r2) < 1e-6);
  //assert(abs(len2(center - p1) - r2) < 1e-6);
  //assert(abs(len2(center - p2) - r2) < 1e-6);

  double dist2 =  len2(test - center);
  return dist2 > r2 ? +1 : (dist2 < r2 ? -1 : 0);
}

double VerticesQuery::dot (double x0, double y0, double z0, double x1, double y1,
    double z1)
{
  return x0*x1 + y0*y1 + z0*z1;
}
//----------------------------------------------------------------------------

double VerticesQuery::det3 (double x0, double y0, double z0, double x1, double y1,
    double z1, double x2, double y2, double z2)
{
  double c00 = y1*z2 - y2*z1;
  double c01 = y2*z0 - y0*z2;
  double c02 = y0*z1 - y1*z0;
  return x0*c00 + x1*c01 + x2*c02;
}
//----------------------------------------------------------------------------

double VerticesQuery::det4 (double x0, double y0, double z0, double w0, double x1,
    double y1, double z1, double w1, double x2, double y2, double z2, double w2,
    double x3, double y3, double z3, double w3)
{
  double a0 = x0*y1 - x1*y0;
  double a1 = x0*y2 - x2*y0;
  double a2 = x0*y3 - x3*y0;
  double a3 = x1*y2 - x2*y1;
  double a4 = x1*y3 - x3*y1;
  double a5 = x2*y3 - x3*y2;
  double b0 = z0*w1 - z1*w0;
  double b1 = z0*w2 - z2*w0;
  double b2 = z0*w3 - z3*w0;
  double b3 = z1*w2 - z2*w1;
  double b4 = z1*w3 - z3*w1;
  double b5 = z2*w3 - z3*w2;
  return a0*b5 - a1*b4 + a2*b3 + a3*b2 - a4*b1 + a5*b0;
}


bool VerticesQuery::sort (int& v0, int& v1)
{
  int j0, j1;
  bool positive;

  if (v0 < v1)
  {
    j0 = 0; j1 = 1; positive = true;
  }
  else
  {
    j0 = 1; j1 = 0; positive = false;
  }

  int value[2] = { v0, v1 };
  v0 = value[j0];
  v1 = value[j1];
  return positive;
}
//----------------------------------------------------------------------------
bool VerticesQuery::sort (int& v0, int& v1, int& v2)
{
  int j0, j1, j2;
  bool positive;

  if (v0 < v1)
  {
    if (v2 < v0)
    {
      j0 = 2; j1 = 0; j2 = 1; positive = true;
    }
    else if (v2 < v1)
    {
      j0 = 0; j1 = 2; j2 = 1; positive = false;
    }
    else
    {
      j0 = 0; j1 = 1; j2 = 2; positive = true;
    }
  }
  else
  {
    if (v2 < v1)
    {
      j0 = 2; j1 = 1; j2 = 0; positive = false;
    }
    else if (v2 < v0)
    {
      j0 = 1; j1 = 2; j2 = 0; positive = true;
    }
    else
    {
      j0 = 1; j1 = 0; j2 = 2; positive = false;
    }
  }

  int value[3] = { v0, v1, v2 };
  v0 = value[j0];
  v1 = value[j1];
  v2 = value[j2];
  return positive;
}
//----------------------------------------------------------------------------
bool VerticesQuery::sort (int& v0, int& v1, int& v2, int& v3)
{
  int j0, j1, j2, j3;
  bool positive;

  if (v0 < v1)
  {
    if (v2 < v3)
    {
      if (v1 < v2)
      {
        j0 = 0; j1 = 1; j2 = 2; j3 = 3; positive = true;
      }
      else if (v3 < v0)
      {
        j0 = 2; j1 = 3; j2 = 0; j3 = 1; positive = true;
      }
      else if (v2 < v0)
      {
        if (v3 < v1)
        {
          j0 = 2; j1 = 0; j2 = 3; j3 = 1; positive = false;
        }
        else
        {
          j0 = 2; j1 = 0; j2 = 1; j3 = 3; positive = true;
        }
      }
      else
      {
        if (v3 < v1)
        {
          j0 = 0; j1 = 2; j2 = 3; j3 = 1; positive = true;
        }
        else
        {
          j0 = 0; j1 = 2; j2 = 1; j3 = 3; positive = false;
        }
      }
    }
    else
    {
      if (v1 < v3)
      {
        j0 = 0; j1 = 1; j2 = 3; j3 = 2; positive = false;
      }
      else if (v2 < v0)
      {
        j0 = 3; j1 = 2; j2 = 0; j3 = 1; positive = false;
      }
      else if (v3 < v0)
      {
        if (v2 < v1)
        {
          j0 = 3; j1 = 0; j2 = 2; j3 = 1; positive = true;
        }
        else
        {
          j0 = 3; j1 = 0; j2 = 1; j3 = 2; positive = false;
        }
      }
      else
      {
        if (v2 < v1)
        {
          j0 = 0; j1 = 3; j2 = 2; j3 = 1; positive = false;
        }
        else
        {
          j0 = 0; j1 = 3; j2 = 1; j3 = 2; positive = true;
        }
      }
    }
  }
  else
  {
    if (v2 < v3)
    {
      if (v0 < v2)
      {
        j0 = 1; j1 = 0; j2 = 2; j3 = 3; positive = false;
      }
      else if (v3 < v1)
      {
        j0 = 2; j1 = 3; j2 = 1; j3 = 0; positive = false;
      }
      else if (v2 < v1)
      {
        if (v3 < v0)
        {
          j0 = 2; j1 = 1; j2 = 3; j3 = 0; positive = true;
        }
        else
        {
          j0 = 2; j1 = 1; j2 = 0; j3 = 3; positive = false;
        }
      }
      else
      {
        if (v3 < v0)
        {
          j0 = 1; j1 = 2; j2 = 3; j3 = 0; positive = false;
        }
        else
        {
          j0 = 1; j1 = 2; j2 = 0; j3 = 3; positive = true;
        }
      }
    }
    else
    {
      if (v0 < v3)
      {
        j0 = 1; j1 = 0; j2 = 3; j3 = 2; positive = true;
      }
      else if (v2 < v1)
      {
        j0 = 3; j1 = 2; j2 = 1; j3 = 0; positive = true;
      }
      else if (v3 < v1)
      {
        if (v2 < v0)
        {
          j0 = 3; j1 = 1; j2 = 2; j3 = 0; positive = false;
        }
        else
        {
          j0 = 3; j1 = 1; j2 = 0; j3 = 2; positive = true;
        }
      }
      else
      {
        if (v2 < v0)
        {
          j0 = 1; j1 = 3; j2 = 2; j3 = 0; positive = true;
        }
        else
        {
          j0 = 1; j1 = 3; j2 = 0; j3 = 2; positive = false;
        }
      }
    }
  }

  int value[4] = { v0, v1, v2, v3 };
  v0 = value[j0];
  v1 = value[j1];
  v2 = value[j2];
  v3 = value[j3];
  return positive;
}


void VerticesQuery::getInformation(double epsilon, Information & info) 
{
  assert(epsilon >= 0 && vertices.size() > 0);
  info.extremeCCW = false;

  // Compute the axis-aligned bounding box for the vertices.  Keep track
  // of the indices in the vertices for the current min and max.
  int indexMin[3] = {0,0,0}, indexMax[3] = {0,0,0};
  vertices[0].convertToArray(info.min);
  vertices[0].convertToArray(info.max);

  for(size_t i = 1; i < vertices.size(); ++i) 
  {
    for(int j = 0; j < 3; ++j) 
    {
      if (vertices[i][j] < info.min[j]) 
      {
        info.min[j] = vertices[i][j];
        indexMin[j] = i;
      }
      else if (vertices[i][j] > info.max[j]) 
      {
        info.max[j] = vertices[i][j];
        indexMax[j] = i;
      }
    }
  }

  // Determine the maximum range for the bounding box.
  info.maxRange = info.max[0] - info.min[0];
  info.extreme[0] = indexMin[0];
  info.extreme[1] = indexMax[0];
  double range = info.max[1] - info.min[1];
  if (range > info.maxRange) 
  {
    info.maxRange = range;
    info.extreme[0] = indexMin[1];
    info.extreme[1] = indexMax[1];
  }
  range = info.max[2] - info.min[2];
  if (range > info.maxRange) 
  {
    info.maxRange = range;
    info.extreme[0] = indexMin[2];
    info.extreme[1] = indexMax[2];
  }

  // The origin is either the point of minimum x-value, point of
  // minimum y-value, or point of minimum z-value.
  info.origin = vertices[info.extreme[0]];

  // Test whether the point set is (nearly) a point.
  if (info.maxRange < epsilon) 
  {
    info.dimension = 0;
    for(int j = 0; j < 3; ++j) 
    {
      info.extreme[j + 1] = info.extreme[0];
      info.direction[j] = Vec3d(0.);
    }
    return;
  }

  // Test whether the point set is (nearly) a line segment.
  info.direction[0] = vertices[info.extreme[1]] - info.origin;
  info.direction[0].normalize();
  double maxDistance = 0.;
  info.extreme[2] = info.extreme[0];
  for(size_t i = 0; i < vertices.size(); ++i) 
  {
    Vec3d diff = vertices[i] - info.origin;
    double dotProduct = ::dot(info.direction[0], diff);
    Vec3d proj = diff - dotProduct * info.direction[0];
    double distance = len(proj);
    if (distance > maxDistance) 
    {
      maxDistance = distance;
      info.extreme[2] = i;
    }
  }

  if (maxDistance < epsilon*info.maxRange) 
  {
    info.dimension = 1;
    info.extreme[2] = info.extreme[1];
    info.extreme[3] = info.extreme[1];
    return;
  }

  // Test whether the point set is (nearly) a planar polygon.
  info.direction[1] = vertices[info.extreme[2]] - info.origin;
  info.direction[1] -= ::dot(info.direction[0], info.direction[1]) * info.direction[0];
  info.direction[1].normalize();
  info.direction[2] = cross(info.direction[0], info.direction[1]);
  maxDistance = 0;
  double maxSign = 0;
  info.extreme[3] = info.extreme[0];
  for(size_t i = 0; i < vertices.size(); ++i) 
  {
    Vec3d diff = vertices[i] - info.origin;
    double distance = ::dot(info.direction[2],diff);
    int sign = distance >= 0 ? 1 : -1;
    distance = fabs(distance);
    if (distance > maxDistance) 
    {
      maxDistance = distance;
      maxSign = sign;
      info.extreme[3] = i;
    }
  }

  if (maxDistance < epsilon*info.maxRange) 
  {
    info.dimension = 2;
    info.extreme[3] = info.extreme[2];
    return;
  }

  info.dimension = 3;
  assert(epsilon == 0 || maxSign != 0);
  info.extremeCCW = (maxSign >= 0);
}


