// SurfPolygon.cpp

#include "SurfPolygon.h"
#include <cassert>
#include <cmath>
#include <vector>
#include "trfbase/CrossStat.h"
#include "trfbase/ETrack.h"
#include "trfutil/TRFMath.h"
#include "trfutil/trfstream.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"

#ifndef DEFECT_NO_STDLIB_NAMESPACES
using std::sqrt;
using std::asin;
using std::fabs;
using std::vector;
#endif

using namespace trf;

using polygon::PairList;
using polygon::PairPtr;
using polygon::Pair;

//**********************************************************************

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "SurfPolygon" );
  double z = data.get_double("z");

  vector<double> points;
#ifdef ObjData_supports_lists
  data.get_double_list("pairs",points);
#endif
  assert( ((int)(points.size()/2))*2 == points.size());
  PairList pairs;
  for(int i=0;i<points.size();i+=2)
    pairs.push_back(PairPtr(new Pair(points[i],points[i+1])));

  return ObjPtr( new SurfPolygon(z,pairs) );
}

}

//**********************************************************************
int up_or_down(double x,double y,double x1,double y1,double x2,double y2);

bool check_crossing(double x1,double y1,double x2,double y2,
                    double x3,double y3,double x4,double y4);
bool check_xing(double x1,double y1,double x2,double y2,
                    double x3,double y3,double x4,double y4);
bool check_distance(double x, double y, 
                    double x1, double y1, double x2, double y2);
//**********************************************************************

//helpers



//*********************************************************************

int up_or_down(double x,double y,double x1,double y1,double x2,double y2)
{
double a=x2-x1;
double b=y1-y2;
double res=(y-y1)*a+(x-x1)*b;
if(res<0.) return SurfPolygon::DOWN;
if(res>0.) return SurfPolygon::UP;
//assert(res==0.);
return SurfPolygon::ON;
}

//*********************************************************************

// this function work only because I assume that x1==x2 or y1==y2

bool check_xing(double x1,double y1,double x2,double y2,
                    double x3,double y3,double x4,double y4)
{

 if(x1==x2)
   {
     if(x4==x3)
       {
        if(x3!=x1) return false;
        double ymin,ymax;
        if(y1<y2) {ymin=y1;ymax=y2;}
        else {ymin=y2;ymax=y1;}
        if((y3>=ymin&&y3<=ymax)||(y4>=ymin&&y4<=ymax)) return true;
        return false;
       }
     double xmin,xmax;
     if(x3<x4) {xmin=x3;xmax=x4;}
     else {xmin=x4;xmax=x3;}
     if(x1<xmin||x1>xmax) return false;
     double y=(y4-y3)/(x4-x3)*(x1-x3)+y3;
     double ymin,ymax;
     if(y1<y2) {ymin=y1;ymax=y2;}
     else {ymin=y2;ymax=y1;}
     if(y>=ymin&&y<=ymax) return true;
     return false;
   }
 else
   {
    assert(y1==y2); 
    if(y4==y3)
       {
        if(y3!=y1) return false;
        double xmin,xmax;
        if(x1<x2) {xmin=x1;xmax=x2;}
        else {xmin=x2;xmax=x1;}
        if((x3>=xmin&&x3<=xmax)||(x4>=xmin&&x4<=xmax)) return true;
        return false;
       }
     double ymin,ymax;
     if(y3<y4) {ymin=y3;ymax=y4;}
     else {ymin=y4;ymax=y3;}
     if(y1<ymin||y1>ymax) return false;
     double x=(x4-x3)/(y4-y3)*(y1-y3)+x3;
     double xmin,xmax;
     if(x1<x2) {xmin=x1;xmax=x2;}
     else {xmin=x2;xmax=x1;}
     if(x>=xmin&&x<=xmax) return true;
     return false;
   }

}

//*********************************************************************

bool check_crossing(double x1,double y1,double x2,double y2,
                    double x3,double y3,double x4,double y4)
{

double a1=x2-x1;
double b1=y2-y1;
double a2=x4-x3;
double b2=y4-y3;

if( b2<1.e-7 && a2<1.e-7 && b2>-1.e-7 && a2>-1.e-7)
  {
   double chk= (x2-x1)*(y3-y1) - (y2-y1)*(x3-x1);
   return chk<1.e-10&&chk>-1.e-10;
  }
if( b1<1.e-7 && a1<1.e-7 && b1>-1.e-7 && a1>-1.e-7)
  {
   double chk=(x4-x3)*(y1-y3) - (y4-y3)*(x1-x3);
   return chk<1.e-10&&chk>-1.e-10;
  }
    

if( b2*a1-b1*a2 == 0.)
  {
    double chk=a1*(y3-y1)-b1*(x3-x1);
    if( chk>1.e-10 || chk<-1.e-10 ) return false;
    return check_distance(x1,y1,x3,y3,x4,y4) ||
           check_distance(x2,y2,x3,y3,x4,y4);
  }


double det= b2*a1-b1*a2;
assert(det!=0.);

double x= (b2*a1*x3+a1*a2*(y1-y3)-b1*a2*x1)/det;
double y= (b1*b2*(x3-x1)+a1*b2*y1-a2*b1*y3)/det;

return  check_distance(x,y,x3,y3,x4,y4) &&
        check_distance(x,y,x1,y1,x2,y2);

}

//*********************************************************************

bool check_distance(double x, double y, 
                    double x1, double y1, double x2, double y2)
{

double dist= (x2-x1)*(y-y1) - (y2-y1)*(x-x1);

assert( dist< 1.e-7 && dist> -1.e-7 );

double len= (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1) ;

return (x-x1)*(x-x1)+(y-y1)*(y-y1) <= len &&
       (x-x2)*(x-x2)+(y-y2)*(y-y2) <= len ;

}


//**********************************************************************


// output stream
void SurfPolygon::ostr(ostream& stream) const {
  stream << begin_object ;
  SurfZPlane::raw_ostr(stream);
  if(_size!=0) stream<<" With Points"<<new_line;
  else stream<<" With no Points"<<new_line;
  for(int i=0;i<_size;++i)
    stream << *_plist[i]<<' ';
  stream << end_object;
}

//*********************************************************************


SurfPolygon::~SurfPolygon() {
for(int i=0;i<_size;++i)
 delete _plist[i];
}


//**********************************************************************

// bounded equality
bool SurfPolygon::safe_bound_equal(const Surface& srf) const {

  const SurfPolygon& bxyp = (const SurfPolygon&) srf;
  if(!SurfZPlane::safe_pure_equal(srf)) return false;
  if(_size!=bxyp._size) return false;
  if(_size==0) return true;

  // find first point 
   int i;
   for(i=0;i<bxyp._size&&*_plist[0]!=*bxyp._plist[i];i++);
   if(i==bxyp._size) return false;
   for(int j=0;j<_size;j++)
     {
      if(i==bxyp._size) i=0;
      if(*_plist[j]!=*bxyp._plist[i]) return false;
      if(where == bxyp.where) i++;
      else 
	{
         if(i==0) i=bxyp._size;
         i--;
	}
     }
     
    return true;
}

//**********************************************************************

// constructor
SurfPolygon::SurfPolygon(double zpos,const PairList& plist)
: SurfZPlane(zpos){
  where=UNDEFINED;
  _size=0;
  int sz=plist.size();
  for(int i=0;i<sz;++i)
    add_point(plist[i]->_x,plist[i]->_y);
}

//**********************************************************************

void SurfPolygon::add_point(double x,double y) {
  assert(_size<MAXPOINTS);
  _plist[_size++]=new polygon::Pair(x,y);
  check();
}


//**********************************************************************

// constructor private
SurfPolygon::SurfPolygon(double zpos, polygon::Pair* const plist[MAXPOINTS],
                                              const int _where,const int size)
: SurfZPlane(zpos){
  for(int i=0;i<size;++i)
  _plist[i]= new polygon::Pair(*plist[i]);
  where=_where;
  _size=size;
}


//**********************************************************************

// Clone.

Surface* SurfPolygon::new_surface() const {

  double zpos = get_parameter(SurfZPlane::ZPOS);
  return new SurfPolygon( zpos, _plist,where ,_size);
}
//**********************************************************************

// Calculate crossing status for VTrack.

CrossStat SurfPolygon::status(const VTrack& trv) const {
  double prec = CrossStat::get_static_precision();
  double dxtrk = prec;
  double dytrk = prec;
  return status(trv,dxtrk,dytrk);
}

//**********************************************************************

// Calculate crossing status for ETrack.

CrossStat SurfPolygon::status(const ETrack& tre) const {
  double nsigma = CrossStat::get_static_nsigma();
  double prec = CrossStat::get_static_precision();
  double dxtrk = nsigma*
                 sqrt(tre.get_error()(SurfZPlane::IX,SurfZPlane::IX)) + prec; 
  double dytrk = nsigma*
                 sqrt(tre.get_error()(SurfZPlane::IY,SurfZPlane::IY)) + prec;
  return status(tre,dxtrk,dytrk);
}
//**********************************************************************

// calculate crossing status VTrack plus track (x,y)-reach.
CrossStat SurfPolygon::status(const VTrack& trv,
                               double dxtrk, double dytrk) const {
  CrossStat xstat = SurfZPlane::pure_status(trv);
  // If track is not at surface exit with bounds flags undefined.
  if ( !xstat.at() ) return xstat;
  assert( ! xstat.bounds_checked() );
  // Calculate whether x track +/- nsigma is in and/or out of bounds.
  double prec = CrossStat::get_static_precision();
  bool vtrk= dxtrk == prec && dytrk == prec;
  double xtrk = trv.get_vector()(SurfZPlane::IX);
  double xtrkmin = xtrk - dxtrk;
  double xtrkmax = xtrk + dxtrk;
  
  // Calculate whether y track +/- nsigma is in and/or out of bounds.
  double ytrk = trv.get_vector()(SurfZPlane::IY);
  double ytrkmin = ytrk - dytrk;
  double ytrkmax = ytrk + dytrk;

  // create four points of error elips, with the status -- it'll show is this
  // point in or out of bounds

  double x[]={xtrkmax,xtrkmax,xtrkmin,xtrkmin};
  double y[]={ytrkmin,ytrkmax,ytrkmax,ytrkmin};
  double test[]={0,0,0,0};
  int sz=_size;
  assert(sz>=3);
  assert(where!=UNDEFINED);
  int test_err=0;
  int test1_j=0;
  bool cross= false;

  for(int j=0;j<sz;++j)
    {      
      for(int i=0;i<4;++i)
	{
         if( test[i] == 0. )
	 {
         if( up_or_down(x[i],y[i],
             _plist[j]->_x,_plist[j]->_y,
             _plist[(j+1<sz?j+1:0)]->_x,_plist[(j+1<sz?j+1:0)]->_y) == where
	     )  test[i]=1; 
	 }
	}
    }

  // fully in bounds

  if(  test[0] == 0 &&  test[1] == 0 && test[2] == 0 && test[3] == 0 )
    return CrossStat(CrossStat::IN_BOUNDS);

  if( vtrk ) return CrossStat(CrossStat::OUT_OF_BOUNDS);

  for(int j=0;j<sz;++j)
    {      
      for(int i=0;i<4;++i)
	{
         if( test1_j == 0) 
	 {
	 if( up_or_down(_plist[j]->_x,_plist[j]->_y,
             x[i],y[i],x[(i+1<4?i+1:0)],y[(i+1<4?i+1:0)]
             ) == DOWN) test1_j=1;  
	 }
	}
      if(test1_j ==1 ) test_err++;
    }

  // if at least one of the Poligon vertices is inside an error ellips then
  // we are both and out of bounds ( we checked that we aren't completely
  // in bounds 
  // or
  // if one of the error ellips points is in bounds.

  if(test_err<sz || 
      test[0] == 0 ||  test[1] == 0 || test[2] == 0 || test[3] == 0  ) 
  return CrossStat(CrossStat::BOTH_BOUNDS);

  for(int j=0;j<sz;++j)
    {      
      for(int i=0;i<4;++i)
	{
         if( !cross )
	 {
         if( check_xing(
                        x[i],y[i],x[(i+1<4?i+1:0)],y[(i+1<4?i+1:0)],
                       _plist[j]->_x,_plist[j]->_y,
                       _plist[(j+1<sz?j+1:0)]->_x,_plist[(j+1<sz?j+1:0)]->_y
                           ) == true ) cross=true;
	 }
	}
    }

  // fully out of bounds if no error ellips vertices is in the Polygon and no
  // Polygon vertices is in the error ellips and no error ellips side crosses
  // Polygon side.
  if(  !cross   )
    return CrossStat(CrossStat::OUT_OF_BOUNDS); 
  // must be both
  return CrossStat(CrossStat::BOTH_BOUNDS);
}

//**********************************************************************

void SurfPolygon::check() {

int sz=_size;
if(sz<3) return;

//correct if we can first with third and second is DOWN and all after that
// is DOWN or UP and all after that is UP

if(sz==3)
  {
   assert(where== UNDEFINED);
   where=DOWN;
   if( up_or_down(_plist[1]->_x,_plist[1]->_y,
                  _plist[0]->_x,_plist[0]->_y,
                  _plist[2]->_x,_plist[2]->_y) == UP )
   where= UP;
  }
else
   assert( up_or_down(_plist[sz-2]->_x,_plist[sz-2]->_y,
                      _plist[sz-3]->_x,_plist[sz-3]->_y,
                      _plist[sz-1]->_x,_plist[sz-1]->_y) == where );
}


//**********************************************************************

// Return the creator.

ObjCreator SurfPolygon::get_creator() {
  return create;
}

//**********************************************************************

// Write the object data.

ObjData SurfPolygon::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "z", get_z() );

  vector<double> points;
  for(int i=0;i<_size;++i)
    {
      points.push_back( _plist[i]->_x);
      points.push_back( _plist[i]->_y);      
    }
  
#ifdef ObjData_supports_lists
  data.add_double_list( "pairs", points );
#endif

  return data;

}

//**********************************************************************
// Helper

using namespace trf::polygon;

Pair::Pair(double x,double y) : RefCounter() { 
  _x=x;
  _y=y;
}

Pair::Pair (const Pair& pr) : RefCounter() {
  *this=pr;
}


Pair& Pair::operator=(const Pair&pr) {
  _x=pr._x;
  _y=pr._y ; 
  return *this; 
}


bool Pair::operator==(const Pair& pr) const {
  if(_x==pr._x&&_y==pr._y) return true;
  return false;
}

bool Pair::operator!=(const Pair& pr) const {
  return !(*this==pr);
}

ostream& trf::polygon::operator<<(ostream& stream, const Pair& pr) {
  stream<<begin_object<<"Point x= "<<pr._x<<" y= "<<pr._y<<end_object;
  return stream;
}

//***********************************************************

//**********************************************************************
// Added by dla
//**********************************************************************

// constructor

SurfPolygon::SurfPolygon(double zpos,const Points& pts)
: SurfZPlane(zpos) {
  where=UNDEFINED;
  _size=0;
  Points::const_iterator ipt;
  for ( ipt=pts.begin(); ipt!=pts.end(); ++ipt )
    add_point( ipt->x(), ipt->y() );
}

//**********************************************************************

// Return the list of vertices.

SurfPolygon::Points SurfPolygon::get_vertices() const {
  Points pts;
  for ( int i=0; i<_size; ++i )
    pts.push_back( CartesianTwoPoint( _plist[i]->_x, _plist[i]->_y ) );
  return pts;
}

//**********************************************************************
// End added by dla
//**********************************************************************