//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcPad
//      see TpcPad.h for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//      Cristoforo Simonetto TUM
//
//
//-----------------------------------------------------------

// This Class' Header ------------------
#include "TpcPad.h"

// C/C++ Headers ----------------------
#include <cmath>
#include <iostream>
#include <algorithm>    // std::sort

// Collaborating Class Headers --------
#include "TpcAbsPadShape.h"
#include "TError.h"

//for sorting of vertices
struct phiCompare
{
	phiCompare(const TVector3 ref) : m_ref(ref){}
	const TVector3& m_ref;
	bool operator()(TVector3 vec1, TVector3 vec2)
	{
		//if (vec1==m_ref)
		//	return true;
		return((vec1-m_ref).Phi()<(vec2-m_ref).Phi());
	}
};

// Class Member definitions -----------

TpcPad::TpcPad()
  : fx(0),fy(0),fangle(0),fwidth(0),fheight(0),fshape(0),fsectorId(0),fid(0)
{;}

TpcPad::TpcPad(const double X,
	       const double Y,
	       const double Angle,
	       TpcAbsPadShape* const Shape,
	       const unsigned int sectorID,
	       const unsigned int ID)
  : fx(X),fy(Y),fangle(Angle),fshape(Shape),fsectorId(sectorID),fid(ID)
{
  EvalBoundingRect();
}

bool
TpcPad::Contains(const double X, const double Y) const
{
  if (fshape == 0)
    Fatal("TpcPad::Contains","No AbsPadShapePolygon assigned.");
  double xSh = X;
  double ySh = Y;
  ToShapeCoord(xSh, ySh);
  return(fshape->Contains(xSh, ySh));
}

bool
TpcPad::CircleIntersection(const double X, const double Y,
			   const double r) const
{
  double dx=std::fabs(fx-X); double dy=std::fabs(fy-Y);
  double hw = 0.5*fwidth; double hh = 0.5*fheight;
  //most Pads are far away
  if (dx > hw+r || dy > hh+r )
    return(false);
  return ( (dx < hw+r && dy < hh) || (dx < hw && dy < hh+r) ||
	   (dx-hw)*(dx-hw)+(dy-hh)*(dy-hh) < r*r );
}

double
TpcPad::GetValue(const double X, const double Y) const
{
  if (fshape == 0)
    Fatal("TpcPad::GetValue","No AbsPadShape assigned.");
  double xSh = X;
  double ySh = Y;
  ToShapeCoord(xSh, ySh);
  return(fshape->GetValue(xSh, ySh));
}

double
TpcPad::GetLTValue(const int X, const int Y) const
{
  if (fshape == 0)
    Fatal("TpcPad::GetValue","No AbsPadShape assigned.");
  return(fshape->GetLTValue(X,Y));
}

double TpcPad::GetWeight(const double amp, const double dirX, const double dirY)
{
  if (fshape==0)
    Fatal("TpcPad::GetWeight","No AbsPadShape assigned.");
  double xSh = dirX;
  double ySh = dirY;
  RotToShapeCoord(xSh, ySh);
  return(fshape->GetWeight(amp,xSh,ySh));
  //return(fshape->GetWeight(amp,dirX,dirY));
}

int TpcPad::GetNBoundaryPoints() const
{
  if (fshape == 0)
    Fatal("TpcPad::GetNBoundaryPoints","No AbsPadShape assigned.");
  return(fshape->GetNBoundaryPoints());
}

void TpcPad::GetBoundaryPoint(const int index, double& X, double& Y) const
{
  if (fshape == 0)
    Fatal("TpcPad::GetBoundaryPoint","No AbsPadShape assigned.");
   fshape->GetBoundaryPoint(index, X, Y);
   ToPlaneCoord(X, Y);
}

std::vector<double> TpcPad::GetBoundaryPoint(const int index) const
{
  if (fshape == 0)
    Fatal("TpcPad::GetBoundaryPoint","No AbsPadShape assigned.");
  double X, Y;
  fshape->GetBoundaryPoint(index, X, Y);
  ToPlaneCoord(X, Y);
  std::vector<double> ret;
  ret.push_back(X);
  ret.push_back(Y);
  return ret;
}

bool operator== (const TpcPad& lhs, const TpcPad& rhs)
{
  //true if same position, angle and shape
  double dx=lhs.fx-rhs.fx;
  double dy=lhs.fy-rhs.fy;
  double da=lhs.fangle-rhs.fangle;
  return lhs.fshape==rhs.fshape &&
    (dx*dx<1E-10) && (dy*dy<1E-10) && (da*da<1E-10);
}

bool operator< (const TpcPad& lhs, const TpcPad& rhs){
  //true if lhs below (y) or left (x) of rhs
  double dy=lhs.fy-rhs.fy;
  double dx=lhs.fx-rhs.fx;
  bool flag=(dy*dy<1E-10) && (dx<-1E-5);
    return (dy<-1E-5) || flag;
}

std::ostream& operator<< (std::ostream& s, const TpcPad& me){
  return s << "TpcPad\n"
           << "sectorID="<<me.fsectorId<<"\n"
	   << "ID="<<me.fid<<"\n"
	   << "x="<<me.fx<<"    y="<<me.fy<<"    angle="<<me.fangle<<"\n"
           << "shape="<<me.fshape<<"\n";
}

void 
TpcPad::ToShapeCoord(double& xP, double& yP) const
{
  double xori=xP-fx;
  double yori=yP-fy;
  xP = xori*std::cos(fangle) + yori*std::sin(fangle);
  yP = -xori*std::sin(fangle) + yori*std::cos(fangle);
}

void 
TpcPad::RotToShapeCoord(double& xP, double& yP) const
{
  double xori=xP;
  double yori=yP;
  xP = xori*std::cos(fangle) + yori*std::sin(fangle);
  yP = -xori*std::sin(fangle) + yori*std::cos(fangle);

}

void 
TpcPad::ToPlaneCoord(double& xP, double& yP) const
{
  double xori = xP*std::cos(-fangle) + yP*std::sin(-fangle);
  yP = -xP*std::sin(-fangle) + yP*std::cos(-fangle)+fy;
  xP = xori+fx;
}

void
TpcPad::EvalBoundingRect()
{
  if (fshape == 0)
  {
    fwidth = 0;
    fheight =0;
    return;
  }
  fshape->EvalBoundingRect(fwidth, fheight, fangle);
}

void
TpcPad::Draw(int c) const
{
  fshape->Draw(fx,fy,fangle, c);
}

bool TpcPad::removeNeighbour(unsigned int n) {
  std::vector<unsigned int>::iterator it;
  for(it=fneighbourIds.begin(); it!=fneighbourIds.end(); it++) 
    if(*it==n) {
      fneighbourIds.erase(it);
      return false; //no error
    }
  return true; //not found
}

double TpcPad::GetArea()
{
	return fshape->GetArea();
}

double TpcPad::GetAreaFractionBelowPlane(TVector3 origin, TVector3 normal)
{
	TVector3 int0,int1;
	if(IntersectWithPlane(origin,normal,int0,int1)==0)
	{
		//no intersection
		TVector3 pad_origin(fx,fy,0);
		if( (pad_origin-origin) * normal > 0)
			return 1.;	//pad is fully above plane
		else
			return 0.;  //pad is fully below plane
	}

	std::vector<TVector3> vertices;
	//collect all vertices which are below the plane
	vertices.push_back(int0);
	vertices.push_back(int1);
	for(int i=0;i<GetNBoundaryPoints()-1;i++)
	{
		TVector3 vertex(GetBoundaryPoint(i)[0],GetBoundaryPoint(i)[1],0);
		if(normal* (vertex-int0) >= 0 )
		{
			vertices.push_back(vertex);
			//std::cout<<"vertex "<<i<<" is above ("<<vertex.X()<<","<<vertex.Y()<<")\n";
		}
	}

	std::sort(vertices.begin(),vertices.end(),phiCompare(TVector3(fx,fy,0)));
	//for(int i=0;i<vertices.size();i++)
	//	std::cout<<"vertex "<<i<<" ("<<vertices[i].X()<<","<<vertices[i].Y()<<")\n";
	//now build triangles, calculate their area and sum up
	double area=0;
	for(int i=1;i<vertices.size()-1;i++)
	{
		//std::cout<<"triangle: ("<<vertices[0].X()<<","<<vertices[0].Y()<<") ("<<vertices[i].X()<<","<<vertices[i].Y()<<") ("<<vertices[i+1].X()<<","<<vertices[i+1].Y()<<")\n";
		area+=fshape->GetAreaTriangle(vertices[0].X(),vertices[0].Y(),vertices[i].X(),vertices[i].Y(),vertices[i+1].X(),vertices[i+1].Y());
	}
	return area/fshape->GetArea();
}

int TpcPad::FindIntersectionLine(TVector3 origin, TVector3 normal, TVector3& point, TVector3& direction)
{
	//first find intersection  line of plane with padplane
	TVector3 normal_pad(0,0,1);
	TVector3 origin_pad(fx,fy,0);

	direction=normal.Cross(normal_pad);

	//check if planes are parallel
	if (direction.Mag()<10e-10)
	{
		TVector3 v(origin_pad - origin);
		if( (v*normal) == 0) //planes coincide
			return 1;
		else
			return 0;       //parallel planes
	}
	//padplane and plane intersect obviously. so find intersection line

	//find maximal coordinate (for numerical stability)
	int maxc;
	if(fabs(direction.X()) > fabs(direction.Y()) )
	{
		if(fabs(direction.X()) > fabs(direction.Z()) )
			maxc=0;
		else
			maxc=2;
	}
	else
	{
		if(fabs(direction.Y())>fabs(direction.Z()) )
			maxc=1;
		else
			maxc=2;
	}

	// next, to get a point on the intersect line
	// zero the max coord, and solve for the other two
	double d1,d2;
	d1=-normal*origin;
	d2=-normal_pad*origin_pad;

	switch(maxc)
	{
	case 0:
		point[0] =0;
		point[1] = (d2*normal.Z()     - d1*normal_pad.Z()) /  direction.X();
        point[2] = (d1*normal_pad.Y() - d2*normal.Y())     /  direction.X();
        break;
	case 1:
		point[0] = (d1*normal_pad.Z() - d2*normal.Z()) /  direction.Y();
		point[1] = 0;
		point[2] = (d2*normal.X()     - d1*normal_pad.X()) /  direction.Y();
		break;
	case 2:                     // intersect with z=0
		point[0] = (d2*normal.Y() - d1*normal_pad.Y()) /  direction.Z();
		point[1] = (d1*normal_pad.X() - d2*normal.X()) /  direction.Z();
		point[2] = 0;
		break;
	}
	return 2;
}

int TpcPad::IntersectWithPlane(TVector3 origin, TVector3 normal, TVector3& point0, TVector3& point1)
{
	//first find intersection of plane on padplane:
	TVector3 pointOnLine0;
	TVector3 pointOnLine1;
	TVector3 direction;
	TVector3 padpos(fx,fy,0);
	FindIntersectionLine(origin,normal,pointOnLine0,direction);
	//std::cout<<"intersection line:\n";
	//pointOnLine0.Print();
	//direction.Print();
	direction.SetMag(1);

	//find poca of pad origin to line. if poca is more than longest pad bounding box side away, line is not intersecting.
	TVector3 poca=pointOnLine0 + direction * ( direction * (padpos - pointOnLine0) );
	double maxdist=sqrt( (fwidth/2. * fwidth/2.) + (fheight/2. * fheight/2.) );
	if ((poca-TVector3(fx,fy,0)).Mag() > maxdist)
	{
		//std::cout<<"poca to far away\n";
		return 0;
	}
	//now move pointOnLine to make sure to be outside of pad
	pointOnLine0=-2*maxdist*direction+poca;
	pointOnLine1= 2*maxdist*direction+poca;
	direction=pointOnLine1-pointOnLine0;

	double lambda,lambda0=0,lambda1=1; //line parameters
	double N,D;
	TVector3 e,ne; //edge vector, outward normal of edge vector
	TVector3 edge;
	TVector3 next_edge;
	for(int i=0;i<GetNBoundaryPoints();i++)
	{
		edge.SetXYZ(GetBoundaryPoint(i)[0],GetBoundaryPoint(i)[1],0);
		next_edge.SetXYZ(GetBoundaryPoint(i+1)[0],GetBoundaryPoint(i+1)[1],0);
		e=next_edge-edge;
		ne.SetXYZ(-e.Y(),e.X(),0);
		N=(edge - pointOnLine0) * ne;
		D=direction*ne;
		if (fabs(D)<1e-10)
		{
			if (N < 0)              // P0 is outside this edge, so
			{
				//std::cout<<"line outside polygon\n";
				return 0;           // line is outside the polygon
			}
			else                    // line cannot cross this edge, so
				continue;           // ignore this edge
		}
		lambda=N/D;
		if (D < 0) {                     // line is entering across this edge
			if (lambda > lambda0) {      // new max lambda0
				lambda0 = lambda;
				if (lambda0 > lambda1)   // line enters after leaving polygon
				{
					//std::cout<<"line enters after leaving\n";
					return 0;
				}
			}
		}
		else {                            // line is leaving across this edge
			if (lambda < lambda1) {       // new min lambda1
				lambda1 = lambda;
				if (lambda1 < lambda0)    // line leaves before entering polygon
				{
					//std::cout<<"line leaves before entering\n";
					return 0;
				}
			}
		}
	}
	point0 = pointOnLine0 + lambda0*direction;
	point1 = pointOnLine0 + lambda1*direction;
	return 1;
}
ClassImp(TpcPad)