#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 "TVector3.h"
#include "TRotation.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 "PndDrcSurfPolySphere.h"
#include "PndDrcSurfPolyPara.h"
#include "PndDrcOptMatAbs.h"
#include "PndDrcOptDev.h"


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

  return *this;
}
//----------------------------------------------------------------------
XYZVector PndDrcSurfPolyPara::normal(const XYZPoint& point) const
{
  if (!m_checked) m_checked = check();// check dimensions

  // transform point back, where paraboloid was defined.
  XYZPoint p1(m_transInv*point);

  // unit vector from focal point to point
  XYZVector v1 = (p1 - XYZPoint(0,0,m_radius/2)).Unit();
  // unit vector parallel to z-axis hitting point
  XYZVector v2 = XYZVector(0,0,fabs(p1.Z())).Unit();

  XYZVector pn(v1+v2);

  return (m_trans*pn).Unit();
}
//----------------------------------------------------------------------
bool PndDrcSurfPolyPara::surfaceHit(PndDrcPhoton& ph, 
				 XYZPoint&  pos_new, 
				 double&    path_length) const
{
  //cout<<" PndDrcSurfPolyPara::surfaceHit"<<endl;//###

  if (!m_checked) m_checked = check();// check dimensions

  // transform back in sphere definitions space
  XYZPoint  pos(m_transInv*ph.position());
  XYZVector dir(m_transInv*ph.direction());


  // compute hit position
  //
  // line : x = pos + lambda*dir
  //
  // paraboloid: z = p - 1/2p * (x^2+y^2)
  //
  // z = pz + lam*dz = p - 1/2p *  
  // (px2 + lam2 dx2 + 2 px lam dx  + py2 + lam2 dy2 + 2 py lam dy)
  //
  // -2p* (pz + lam*dz -p) =   
  // (px2 + lam2 dx2 + 2 px lam dx  + py2 + lam2 dy2 + 2 py lam dy)
  //
  // 2p* (p - pz - lam*dz) =   
  // (px2 + lam2 dx2 + 2 px lam dx  + py2 + lam2 dy2 + 2 py lam dy)
  //
  // 0 = lam2 (dx2+dy2) + lam (2px dx + 2 py dy + 2p dz) + (px2 + py2 +2p pz - 2pp)

  double p = m_radius;
  double dx2dy2 = dir.X()*dir.X() + dir.Y()*dir.Y();
  double lambda;
  const double eps = 0.001; 

  if (fabs(dx2dy2)<1.0e-12)// otherwise the too small dx2dy2 will cause trouble in
    //                        the next clause
    {
      // 0 = lam2 (dx2+dy2) + lam (2px dx + 2 py dy + 2p dz) + (px2 + py2 +2p pz - 2pp)
      // 0 = lam (2px dx + 2 py dy + 2p dz) + (px2 + py2 +2p pz - 2pp)
      double P = (2*pos.X()*dir.X()+2*pos.Y()*dir.Y()+2*p*dir.Z());
      double Q =  (pos.X()*pos.X() + pos.Y()*pos.Y() + 2*p*pos.Z() - 2*p*p);
      lambda = -Q/P;
      if (lambda<eps) 
	{
	  return false;
	}
    }
  else
    {
      double P = (2*pos.X()*dir.X()+2*pos.Y()*dir.Y()+2*p*dir.Z())/dx2dy2;
      double Q =  (pos.X()*pos.X() + pos.Y()*pos.Y() + 2*p*pos.Z() - 2*p*p)/dx2dy2;
      
      long double root2 = P*P/4 - Q;
      if (root2<0) 
	{
	  return false; // photon does not hit parabola 
	  // eg. photon behind parabola perpendicular to z-axis.
	  cerr<<" *** PndDrcSurfPolyPara::surfaceHit: root<0 : "<<root2<<endl;
	  //exit(EXIT_FAILURE);
	}
      double lambda1 = -P/2 + sqrt(root2);
      double lambda2 = -P/2 - sqrt(root2);

      if (lambda1>eps && lambda2>eps)
	{ // check both solutions
	  XYZPoint pos_new1     = pos + lambda1*dir;
	  XYZPoint pos_check(pos_new);
	  pos_check.SetZ(0);
	  bool hit1 = withinSurface(pos_check);  
	  XYZPoint pos_new2     = pos + lambda2*dir;
	  pos_check = pos_new2;
	  pos_check.SetZ(0);
	  bool hit2 = withinSurface(pos_check);
	  if (hit1 && hit2)
	    {
	      // take closest
	      lambda = (lambda1<lambda2) ? lambda1 : lambda2;
	      pos_new = pos + lambda*dir;
	      path_length = lambda;
	      pos_new = m_trans * pos_new;
	      if (m_pixel) ph.setFate(Drc::PhotMeasured);
	      return true;
	    }
	  else if (hit1)
	    {
	      pos_new = pos + lambda1*dir;
	      path_length = lambda1;
	      pos_new = m_trans * pos_new;
	      if (m_pixel) ph.setFate(Drc::PhotMeasured);
	      return true;
	    }
	  else if (hit2)
	    {
	      pos_new = pos + lambda2*dir;
	      path_length = lambda2;
	      pos_new = m_trans * pos_new;
	      if (m_pixel) ph.setFate(Drc::PhotMeasured);
	      return true;
	    }
	  else
	    {
	      return false;
	    }
	}
      else if (lambda1>eps && lambda2<=eps)
	{
	  lambda = lambda1;
	}
      else if (lambda1<=eps && lambda2>eps)
	{
	  lambda = lambda2;
	}
      else
	{
	  //cout<<" both lambdas <0"<<endl;//###
	  return false;
	}


    }

  path_length = lambda;
  pos_new     = pos + path_length*dir;
  // check if pos_new is within polynom
  XYZPoint pos_check(pos_new);
  pos_check.SetZ(0);
  if (withinSurface(pos_check)) 
    {
      pos_new = m_trans * pos_new;
      if (m_pixel) ph.setFate(Drc::PhotMeasured);
      return true;
    }
  return false;
} 
//----------------------------------------------------------------------
bool PndDrcSurfPolyPara::check() const
{
  if (m_radius<=0)
    {
      cerr<<" ***  PndDrcSurfPolyPara::check: "
	  <<m_name<<": wrong or not set radius="
	  <<m_radius<<endl;
      exit(EXIT_FAILURE);
    }

 for (unsigned int i=0; i<m_p.size(); i++)
    {
      if (m_p[i].Z() != 0)
	{
	  cerr<<" ***  PndDrcSurfPolyPara::check: "
	      <<m_name<<": z component of point != 0 "
	      <<" m_p["<<i<<"]="
	      <<m_p[i].X()<<" "
	      <<m_p[i].Y()<<" "
	      <<m_p[i].Z()<<endl;
	  return false;
	}
    }

  return true;
}
//----------------------------------------------------------------------
void PndDrcSurfPolyPara::addTransform(const Transform3D& trans)
{
  if (verbosity()>=3) cout<<"    PndDrcSurfPolyPara::addTransform() name="<<m_name<<endl;
  m_trans    = trans * m_trans;
  m_transInv  = m_trans.Inverse();
}
//----------------------------------------------------------------------
XYZPoint PndDrcSurfPolyPara::limitingPoint(unsigned int i)
{
  if (i>(m_p.size()-1) || i<0)
    {
      cerr<<" *** PndDrcSurfPolyPara::limitingPoint: index out of range, i="<<i<<endl;
      exit(EXIT_FAILURE);
    }
  XYZPoint p1(m_p[i]);
  
  double z = m_radius 
    - p1.Mag2()/(2*m_radius); // z comp = 0
  p1.SetZ(z);

  return m_trans*p1;
  
  
}
//----------------------------------------------------------------------
XYZPoint PndDrcSurfPolyPara::centerPoint()
{
  return m_trans*XYZPoint(0,0,m_radius);
}
//----------------------------------------------------------------------
void PndDrcSurfPolyPara::print(fstream& stream) const
{
  if (!m_checked) m_checked = check();// check dimensions

  // print points
  unsigned int isize = m_p.size();

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



  for (double frac=1.0; frac>0.19; frac-=0.2)
    {// several (5) lines

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

      XYZPoint m_p1;
      int icnt=0;

      for (unsigned int i=0; i<isize; i++)
	{
	  for (unsigned int j=0; j<20; j++)
	    {
	      unsigned int next = i+1;
	      if (next==isize) next=0;


	      m_p1 = m_p[i]*frac + j/20.0*(m_p[next]-m_p[i])*frac;
	      double z = m_radius 
		- m_p1.Mag2()/(2*m_radius); // z comp = 0
	      m_p1.SetZ(z);
	  
	      m_p1 = m_trans * m_p1;

	      stream<<" l->SetPoint("<<(icnt++)<<","
		    <<m_p1.X()<<","
		    <<m_p1.Y()<<","
		    <<m_p1.Z()<<");"<<endl;
	    }
	}
      m_p1 = m_p[0]*frac;
      double z = m_radius - m_p1.Mag2()/(2*m_radius); // z comp = 0
      m_p1.SetZ(z);

      m_p1 = m_trans * m_p1;
	      
      stream<<" l->SetPoint("<<(icnt++)<<","
	    <<m_p1.X()<<","
	    <<m_p1.Y()<<","
	    <<m_p1.Z()<<");"<<endl;
      stream<<" l->SetLineColor("<<m_printColor<<");"<<endl;
      stream<<" l->Draw();"<<endl;

  
    }
  

}