#include <iostream>
using std::cout;
using std::cerr;
using std::cin;
using std::endl;

#include <valarray>
using std::valarray;

#include <fstream>
using std::fstream;

#include <string>
using std::string;

#include <list>
using std::list;

#include <vector>
using std::vector;

#include "TObject.h"
#include "TVector3.h"
#include "TRandom.h"
#include "TMatrixD.h"
#include "TVectorD.h"
#include "TDecompSVD.h"
#include "Math/Vector3D.h"
using ROOT::Math::XYZVector;

#include "Math/Point3D.h"
using ROOT::Math::XYZPoint;

#include "Math/Transform3D.h"
using ROOT::Math::Transform3D;

#include "Math/RotationX.h"
using ROOT::Math::RotationX;
#include "Math/RotationY.h"
using ROOT::Math::RotationY;
#include "Math/RotationZ.h"
using ROOT::Math::RotationZ;
#include "Math/Rotation3D.h"
using ROOT::Math::Rotation3D;

#include "PndDrcPhoton.h"
#include "PndDrcOptReflAbs.h"
#include "PndDrcSurfAbs.h"
#include "PndDrcSurfPolyFlat.h"
#include "PndDrcOptMatAbs.h"
#include "PndDrcOptDev.h"


//----------------------------------------------------------------------
PndDrcSurfPolyFlat::PndDrcSurfPolyFlat()
{
}
//----------------------------------------------------------------------
PndDrcSurfPolyFlat* PndDrcSurfPolyFlat::clone() const
{
  return new PndDrcSurfPolyFlat(*this); 
}
//----------------------------------------------------------------------
void PndDrcSurfPolyFlat::copy(const PndDrcSurfPolyFlat& s)
{
  m_p           = s.m_p;
  m_normal      = s.m_normal;
}
//----------------------------------------------------------------------
PndDrcSurfPolyFlat::PndDrcSurfPolyFlat(const PndDrcSurfPolyFlat& s) : PndDrcSurfAbs(s)
{
  if (s.m_verbosity>=1) cout<<"  PndDrcSurfPolyFlat::PndDrcSurfPolyFlat"
			    <<"(const PndDrcSurfPolyFlat&) name,copy: "  
			    <<s.m_name<<" "<<s.m_copyNumber<<endl;
  copy(s);
}
//----------------------------------------------------------------------
PndDrcSurfPolyFlat&  PndDrcSurfPolyFlat::operator=(const PndDrcSurfPolyFlat& s)
{
  if (s.m_verbosity>=1) cout<<"  PndDrcSurfPolyFlat::operator="
			    <<"(const PndDrcSurfPolyFlat&) name,copy: "  
			    <<s.m_name<<" "<<s.m_copyNumber<<endl;
  if (&s != this)
    {
      static_cast<PndDrcSurfAbs&>((*this)) = s; // assignment of base class part.
      copy(s);
    }

  return *this;
}
//----------------------------------------------------------------------
void PndDrcSurfPolyFlat::addPoint(XYZPoint point)
{
  m_p.push_back(point);
  int isize = m_p.size();


  // search normal vector from point differences with largest absolute
  // cross product.
  if (isize > 2)
    {
      double length2 = 0;
      for (int i=2; i<isize; i++)
	{
	  XYZVector normal = ((m_p[i-1]-m_p[i-2]).Cross(m_p[i]-m_p[i-1]));
	      
	  if (normal.Mag2() > length2)
	    {
	      length2 = normal.Mag2();
	      m_normal = normal.Unit();
	    }
	}
    }

  


  if (verbosity()>=5)
    {
      cout<<"      PndDrcSurfPolyFlat::addPoint: vector size = "<<m_p.size()<<endl;
      for (int i=0; i<isize; i++)
	{
	  cout<<" i="<<i<<" x y z = "
	      <<m_p[i].X()<<" "
	      <<m_p[i].Y()<<" "
	      <<m_p[i].Z()<<endl;
	}
    }
}
//----------------------------------------------------------------------
XYZVector PndDrcSurfPolyFlat::normal(const XYZPoint& point) const
{
  // normal vector is a constant.
  return m_normal;
}
//----------------------------------------------------------------------
bool PndDrcSurfPolyFlat::lineCross(const XYZPoint& pa, const XYZPoint& pb,
				const XYZPoint& qa, const XYZPoint& qb) const
{
  const double eps = 1.0e-9;
 
  TVector3 p((pb-pa).x(), (pb-pa).y(), (pb-pa).z());
  TVector3 q((qb-qa).x(), (qb-qa).y(), (qb-qa).z());
 


  if (fabs(p.Dot(q)/(p.Mag2()*q.Mag2())) > 1.0L-eps) return false; // directions parallel

  // calculate closest distance
  TVector3 n0   = (p.Cross(q)).Unit();
  double   dist = fabs((pb-qb).Dot(n0));   
  if (dist>eps) return false;

  TVector3 P(pa.x(),pa.y(),pa.z());
  TVector3 Q(qa.x(),qa.y(),qa.z());
  // solve
  // P + p*s = Q + q*t
  // 
  // d**2 = ((P+ps) - (Q+qt))**2
  // 
  // d/ds d**2 = d/dt d**2 => (P-Q) + (sp-tq) = 0
  //
  // This linear equation is for the minimum distance between both lines
  // We prefer minimum distance over equality of both lines due to numerical
  // uncertainties.
  //
  // Solve linear equation
  //
  //    px  -qx   s     Qx-Px
  //    py  -qy   t  =  Qy-Py   
  //    pz  -qz         Qz-Pz
  //
  TMatrixD A(3,2);

  A(0,0) =  p.X();
  A(1,0) =  p.Y();
  A(2,0) =  p.Z();
  A(0,1) = -q.X();
  A(1,1) = -q.Y();
  A(2,1) = -q.Z();

  TVectorD B(3);

  B(0) = Q.X()-P.X();
  B(1) = Q.Y()-P.Y();
  B(2) = Q.Z()-P.Z();

  TDecompSVD svd(A);

  if (!svd.Solve(B)) return false;
  //cout<<" crossing "<<B[0]<<" "<<B[1]<<endl;//###
  
  if (B[0]>-eps && B[0]<1+eps && B[1]>-eps && B[1]<1+eps) return true;
  return false;

}
//----------------------------------------------------------------------
bool PndDrcSurfPolyFlat::surfaceHit(PndDrcPhoton&       ph, 
				 XYZPoint&        pos_new, 
				 double&          path_length) const
{
  if (verbosity()>=4) cout<<"      PndDrcSurfPolyFlat::surfaceHit: name="<<name()<<endl;
 	
  static const double eps = 1.0e-9;

  unsigned int isize = m_p.size();

  //for (int i=0; i<isize; i++)
  //{
  //cout<<m_p[i].X()<<" "<<m_p[i].Y()<<" "<<m_p[i].Z()<<" "
  //<<name()<<" "<<copyNumber()<<endl;//###
  //}

  if (isize < 3)
    {
      cerr<<" *** PndDrcSurfPolyFlat::surfaceHit: surface has only "<<m_p.size()
	  <<" points. Aborting. Name="<<name()<<endl;
      exit(EXIT_FAILURE);
    }


  XYZPoint  point = ph.position();
  XYZVector dir   = ph.direction().Unit();

  // calculate hits with area
  // (point+lambda*dir - m_p[0]) * m_normal = 0
  //
  // point*m_normal + lambda*dir*m_normal - m_p[0]*m_normal = 0
  //
  // lamda = (m_p[0]-point)*m_normal  / dir*m_normal
  //
  // dir*m_normal > 0 , see condition above

  double h = dir.Dot(m_normal);
  if (h==0) 
    {
      if (verbosity()>=4) cout<<"      PndDrcSurfPolyFlat::surfaceHit: dir perp norm\n";
     return false;
    }
  double   lambda = (m_p[0]-point).Dot(m_normal) / dir.Dot(m_normal);
  pos_new = point + lambda*dir;


  if (lambda<-eps) // flying in wrong direction.
    {
      if (verbosity()>=4) cout<<"      PndDrcSurfPolyFlat::surfaceHit: wrong dir "
			  <<" lambda par ="<<lambda<<endl;
      return false; 
    }

  path_length = fabs(lambda);

  if (withinSurface(pos_new)) 
    {
      if (m_pixel) 
	{
	  ph.setFate(Drc::PhotMeasured);
	}
      return true;
    }

  return false;
} 
//----------------------------------------------------------------------
bool PndDrcSurfPolyFlat::withinSurface(XYZPoint& point) const
{
  // check if pos_new is within area.
  //
  // 
  //
  //         p3-------------------------p2
  //         /                          /
  //        /                         /
  //       /                        /
  //      /                a------/--------- b
  //     /                      /
  //    /                     /
  //   p4-------------------p1         normal vector to top
  //
  // if the line ab intersects the borders an equal number the point is inside
  // for odd numbers it is outside. 

  unsigned int isize = m_p.size();

  // find a point outside.
  vector<XYZVector> diff(isize);

  diff[isize-1] = (m_p[0] - m_p[isize-1]);


  XYZPoint p_out(diff[isize-1]);
  for (unsigned int i=0; i<isize-1; i++)
    {
      diff[i] = m_p[i+1] - m_p[i];
      if (diff[i].Dot(diff[isize-1])<0) diff[i] *= -1;
      p_out += diff[i];
    }
  p_out += XYZVector(m_p[0]); // = p0 + differences

  //cout<<" p_out="<<p_out.X()<<" "<<p_out.Y()<<" "<<p_out.Z()<<endl;//###

  // determine number of intersections.
  int intersections = 0;
  if (lineCross(p_out,point,m_p[isize-1],m_p[0])) intersections++;
  //cout<<" 1st "<<intersections<<endl;
  for (unsigned int i=0; i<isize-1; i++)
    {
      if (lineCross(p_out,point,m_p[i],m_p[i+1])) intersections++;
      /*     
	     cout<<" nst "<<i<<" "<<intersections<<" "
	     <<m_p[i].X()<<" "
	     <<m_p[i].Y()<<" "
	     <<m_p[i].Z()<<" "
	     <<m_p[i+1].X()<<" "
	     <<m_p[i+1].Y()<<" "
	     <<m_p[i+1].Z()<<endl;
      */
    }

  if (intersections%2 == 0) 
    {
      if (verbosity()>=4) 
	{
	  cout<<"      PndDrcSurfPolyFlat::withinSurface:"<<name()<<" no hit "
	      <<"intersections="<<intersections<<endl;
	}
      return false; 
    }
  if (verbosity()>=4) 
    {
      cout<<"      PndDrcSurfPolyFlat::withinSurface:"<<name()<<" hit intersections="
	  <<intersections<<endl;
    }
  return true;
}
//----------------------------------------------------------------------
void PndDrcSurfPolyFlat::print(fstream& stream) const
{
  int isize = m_p.size();


  if (isize < 3)
    {
      cerr<<" *** PndDrcSurfPolyFlat::surfaceHit: surface has only "<<m_p.size()
	  <<" points. Aborting. Name="<<name()<<endl;
      exit(EXIT_FAILURE);
    }

  stream<<" TPolyLine3D *l = new TPolyLine3D("<<isize+1<<");"<<endl;

  for (int i=0; i<isize; i++)
    {
      stream<<" l->SetPoint("<<i<<","
	<<m_p[i].X()<<","
	<<m_p[i].Y()<<","
	<<m_p[i].Z()<<");"<<endl;
    }
  stream<<" l->SetPoint("<<isize<<","
	<<m_p[0].X()<<","
	<<m_p[0].Y()<<","
	<<m_p[0].Z()<<");"<<endl;
  stream<<" l->SetLineColor("<<m_printColor<<");"<<endl;
  stream<<" l->Draw();"<<endl;
}
//----------------------------------------------------------------------
void PndDrcSurfPolyFlat::addTransform(const Transform3D& trans)
{
  if (verbosity()>=3) cout<<"    PndDrcSurfPolyFlat::addTransform() name="<<m_name<<endl;
  //m_trans    *= trans;
  //m_transInv  = m_trans.Inverse();

  for (unsigned int i=0; i<m_p.size(); i++)
    {
      m_p[i] = trans*m_p[i];
    }
  m_normal = trans * m_normal;

}