/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include "TriWithLines2D.h"

//-- system includes --
#include <cstdlib>

using std::rand;

// --- STL includes ---
#include <utility>
#include <map>

using std::make_pair;
using std::map;

//--- IO includes ---
#include <iostream>


/*!
  helper function, return random value between min & max
*/
double TriWithLines2D::m_random(double imin,double imax) const
{
  return imin+((imax-imin)*((double)(rand())/(double)(RAND_MAX)));
}

TriWithLines2D::TriWithLines2D()
{}

/*!
  Construct Triangle 

  \param p0 first corner of triangle box
  \param p1 second corner of triangle box
  \param p2 third corner of triangle box
*/
TriWithLines2D::TriWithLines2D(const Vector3& p0,const Vector3& p1,const Vector3& p2)
  : m_p0(p0),m_p1(p1),m_p2(p2)
{
  Vector3 pmin, pmax;
  double minV,maxV;
  Line2D l;

  int i;

  for (i=0;i<3;i++) {
     minV=p0[i];
     maxV=p0[i];
     if (p1[i] < minV) minV = p1[i];
     if (p1[i] > maxV) maxV = p1[i];
     if (p2[i] < minV) minV = p2[i];
     if (p2[i] > maxV) maxV = p2[i];
     pmin[i] = minV;
     pmax[i] = maxV;
  }
  m_pmin=(pmin);
  m_pmax=(pmax);

  m_a0=((p1[1] - p0[1])/(p1[0]-p0[0]));
  m_b0=(1.0);
  m_c0=(p0[1] - m_a0*p0[0]);

  m_a1=((p2[1] - p1[1])/(p2[0]-p1[0]));
  m_b1=(1.0);
  m_c1=(p1[1] - m_a1*p1[0]);

  m_a2=((p0[1] - p2[1])/(p0[0]-p2[0]));
  m_b2=(1.0);
  m_c2=(p2[1] - m_a2*p2[0]);

  l = Line2D(p0,p1);
  addLine(l);
  l = Line2D(p1,p2);
  addLine(l);
  l = Line2D(p2,p0);
  addLine(l);

}

/*!
  add a line

  \param l the line
*/
void TriWithLines2D::addLine(const Line2D& l)
{
  m_lines.push_back(l);
}

/*!
  get bounding box
*/
pair<Vector3,Vector3> TriWithLines2D::getBoundingBox()
{
  return make_pair(m_pmin,m_pmax);
}
  
/*!
  get point inside the box. The Argument is ignored
*/
Vector3 TriWithLines2D::getAPoint(int) const
{
  double px,py;
  bool found_a_point;
  Vector3 aPoint;
  
  found_a_point = false;
  do {
     px=m_random(m_pmin.x(),m_pmax.x());
     py=m_random(m_pmin.y(),m_pmax.y());
     aPoint = Vector3(px,py,0.0);
     found_a_point = isIn(aPoint);
  } while (found_a_point==false);

  return Vector3(px,py,0.0);  
}

/*!
  Return closest plane to a given point. Assumes that the list of Planes/Lines is not empty.
  
  \param p the point
  \warning check non-empty list of lines (via hasPlane()) before invoking
*/
Line2D TriWithLines2D::getClosestPlane(const Vector3& p)
{
  std::cout << "getClosestPlane : " << p << std::endl;

  vector<Line2D>::iterator PL=m_lines.begin();
  double dist=PL->getDist(p);

  for(vector<Line2D>::iterator iter=m_lines.begin();
      iter!=m_lines.end();
      iter++){
    double ndist=iter->getDist(p);
    std::cout << "Line: " << *iter << " Distance: " << ndist << std::endl;
    if(ndist<dist){
      PL=iter;
      dist=ndist;
    }
  }
  std::cout << "closest line: " << *PL <<  " Distance: " << dist << std::endl;

  return (*PL);
}

const map<double, const Line2D*> TriWithLines2D::getClosestPlanes(const Vector3& p,int nmax) const
{
  map<double,const Line2D*> res;

  for(vector<Line2D>::const_iterator iter=m_lines.begin();
      iter!=m_lines.end();
      iter++){
    double ndist=iter->getDist(p);
    const Line2D *l=&(*iter);
    res.insert(make_pair(ndist,l));
  }

  return res;
}


/*!
  check if point is inside

  \param p the point
*/
bool TriWithLines2D::isIn(const Vector3& p) const
{
   bool inside0,inside1,inside2,inbox;
   double dp,dv;
   Vector3 cp,cv;

   inside0 = false;
   inside1 = false;
   inside2 = false;

//   dp = m_a0*p[0] + m_b0*p[1] + m_c0;
//   dv = m_a0*m_p2[0] + m_b0*m_p2[1] + m_c0;

   cp = cross(m_p1-m_p0,p-m_p0);
   cv = cross(m_p1-m_p0,m_p2-m_p0);
   dp = cp.z();
   dv = cv.z(); 
   if (dp*dv>0.0) inside0=true;

//   dp = m_a1*p[0] + m_b1*p[1] + m_c1;
//   dv = m_a1*m_p0[0] + m_b1*m_p0[1] + m_c1;

   cp = cross(m_p2-m_p1,p-m_p1);
   cv = cross(m_p2-m_p1,m_p0-m_p1);
   dp = cp.z();
   dv = cv.z(); 
   if (dp*dv>0.0) inside1=true;

//   dp = m_a2*p[0] + m_b2*p[1] + m_c2;
//   dv = m_a2*m_p1[0] + m_b2*m_p1[1] + m_c2;

   cp = cross(m_p0-m_p2,p-m_p2);
   cv = cross(m_p0-m_p2,m_p1-m_p2);
   dp = cp.z();
   dv = cv.z(); 
   if (dp*dv>0.0) inside2=true;

   inbox = ((p.x()>m_pmin.x()) && (p.x()<m_pmax.x()) && (p.y()>m_pmin.y()) && (p.y()<m_pmax.y()));

//    if ((inside0) && (inside1) && (inside2) && (inbox)) std::cout << "inside " << p << std::endl;
   

   return ((inside0) && (inside1) && (inside2) && inbox);

//  return ((p.x()>m_pmin.x()) && (p.x()<m_pmax.x()) && (p.y()>m_pmin.y()) && (p.y()<m_pmax.y()));
}

/*!
  check if a Sphere is inside and doesn't intersect any lines

  \param S the Sphere
*/
bool TriWithLines2D::isIn(const Sphere& S)
{
  double r=S.Radius();
  Vector3 p=S.Center();

//    std::cout << "TriWithLines2D rad: " << r << "  pos: " << p << std::endl;

//  bool inside=(p.X()>m_pmin.X()+r) && (p.X()<m_pmax.X()-r) && (p.Y()>m_pmin.Y()+r) && (p.Y()<m_pmax.Y()-r);
  bool inside = isIn(p);

  vector<Line2D>::iterator iter=m_lines.begin();

  double dist=2*r;
  while(iter!=m_lines.end() && dist>r){
    dist=iter->getDist(p);
    iter++;
  }

  return inside && (dist>r);  
}

ostream& operator<< (ostream& ost, const TriWithLines2D& L)
{
  ost << L.m_p0 << " to " << L.m_p1 << " to " << L.m_p2;

  return ost;
}

