/////////////////////////////////////////////////////////////
//                                                         //
// 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 "PolygonWithLines2D.h"

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

using std::rand;

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

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

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


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

PolygonWithLines2D::PolygonWithLines2D()
{}

/*!
  Construct regular n-sided polygon

  \param centre the center postition of the polygon
  \param rad the distance of the corners fron the center, i.e. the radius of the circle into which the polygon is incribed
  \param nsides nr. of sides
  \param add_bounding_lines add fitting lines to sides of polygon (yes/no)
*/
PolygonWithLines2D::PolygonWithLines2D(const Vector3& centre,double rad,int nsides,bool add_bounding_lines)
  : m_centre(centre),m_radius(rad),m_nsides(nsides)
{
  Vector3 pmin, pmax;
  int i;
  double px,py,pz,theta,theta0;
  Line2D l;

  pmin[0]=m_centre[0]-m_radius;
  pmin[1]=m_centre[1]-m_radius;
  pmin[2]=m_centre[2];
  pmax[0]=m_centre[0]+m_radius;
  pmax[1]=m_centre[1]+m_radius;
  pmax[2]=m_centre[2];

  m_pmin=(pmin);
  m_pmax=(pmax);

  for (i=0;i<m_nsides;i++) {
     theta = 2*3.141591654*(double)(i)/(double)(m_nsides);
     theta0 = 0.5*3.141591654/(double)(m_nsides);
     px = m_centre[0] + m_radius * cos(theta-theta0);
     py = m_centre[1] + m_radius * sin(theta-theta0);
     pz = m_centre[2];
     m_vertices[i] = Vector3(px,py,pz);
  }
  if (add_bounding_lines == true) {
     for (i=0;i<m_nsides-1;i++) {
        l = Line2D(m_vertices[i],m_vertices[i+1]);
        addLine(l);
     }
     l = Line2D(m_vertices[m_nsides-1],m_vertices[0]);
     addLine(l);
  }

}

/*!
  Construct polygon from list of corner points
  
  \param corners the list of corner points as Vector3 
*/
PolygonWithLines2D::PolygonWithLines2D(boost::python::list corners)
{
  // convert python list to STL-list
  boost::python::stl_input_iterator<Vector3> begin(corners), end;
  list<Vector3> vertex_list=list<Vector3>(begin, end);

  
  int idx=0;
  //go though list of corner points
  for(list<Vector3>::iterator iter=vertex_list.begin();
      iter!=vertex_list.end();
      iter++){
    m_vertices[idx]=*iter;
    idx++;
    // update bounding box
    if(idx==1){
      m_pmin=*iter;
      m_pmax=*iter;
    }
    m_pmin=comp_min(m_pmin,*iter);
    m_pmax=comp_max(m_pmax,*iter);
  }
  //  m_vertices[idx]=m_vertices[0]; // closure
  std::cout << m_pmin << " - " <<  m_pmax << std::endl;
  m_nsides=idx;
  m_centre=0.5*(m_pmin+m_pmax);
  for(int i=0;i<m_nsides;i++){
    std::cout << m_vertices[i]  << std::endl;
  }
}

/*!
  add a line

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

/*!
  get bounding box
*/
pair<Vector3,Vector3> PolygonWithLines2D::getBoundingBox()
{
  return make_pair(m_pmin,m_pmax);
}
  
/*!
  get point inside the box. The Argument is ignored
*/
Vector3 PolygonWithLines2D::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 PolygonWithLines2D::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*> PolygonWithLines2D::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 PolygonWithLines2D::isIn(const Vector3& p) const
{
   bool inside,inbox;
   double dp,dv;
   Vector3 cp,cv;
   int i;

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

   if (inbox) {
      inside = true;

      for (i=0;i<m_nsides-1;i++) {
         cp = cross(m_vertices[i+1]-m_vertices[i],p-m_vertices[i]);
         cv = cross(m_vertices[i+1]-m_vertices[i],m_centre-m_vertices[i]);
         dp = cp.z();
         dv = cv.z(); 
         if (dp*dv<0.0) inside=false;
      }
      cp = cross(m_vertices[0]-m_vertices[m_nsides-1],p-m_vertices[m_nsides-1]);
      cv = cross(m_vertices[0]-m_vertices[m_nsides-1],m_centre-m_vertices[m_nsides-1]);
      dp = cp.z();
      dv = cv.z(); 
      if (dp*dv<0.0) inside=false;
   }

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

   return (inside && inbox);

}

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

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

//    std::cout << "PolygonWithLines2D 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 PolygonWithLines2D& L)
{
  ost << " centre: " << L.m_centre << " radius: " << L.m_radius << " number of sides: " << L.m_nsides;

  return ost;
}

