#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 "PndDrcSurfPolyCyl.h"
#include "PndDrcOptMatAbs.h"
#include "PndDrcOptDev.h"


//----------------------------------------------------------------------
PndDrcSurfPolyCyl::PndDrcSurfPolyCyl()
{
  m_radius  = -999;
  m_checked = false;
}
//----------------------------------------------------------------------
PndDrcSurfPolyCyl* PndDrcSurfPolyCyl::clone() const
{
  return new PndDrcSurfPolyCyl(*this); 
}
//----------------------------------------------------------------------
void PndDrcSurfPolyCyl::copy(const PndDrcSurfPolyCyl& s)
{
  m_radius   = s.m_radius;
  m_checked  = s.m_checked;
  m_trans    = s.m_trans;
  m_transInv = s.m_transInv;
}
//----------------------------------------------------------------------
PndDrcSurfPolyCyl::PndDrcSurfPolyCyl(const PndDrcSurfPolyCyl& s) : PndDrcSurfPolyFlat(s)
{
  if (s.m_verbosity>=1) cout<<"  PndDrcSurfPolyCyl::PndDrcSurfPolyCyl"
			    <<"(const PndDrcSurfPolyCyl&) name,copy: "  
			    <<s.m_name<<" "<<s.m_copyNumber<<endl;
  copy(s);
}
//----------------------------------------------------------------------
PndDrcSurfPolyCyl&  PndDrcSurfPolyCyl::operator=(const PndDrcSurfPolyCyl& s)
{
  if (s.m_verbosity>=1) cout<<"  PndDrcSurfPolyCyl::operator="
			    <<"(const PndDrcSurfPolyCyl&) 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 PndDrcSurfPolyCyl::normal(const XYZPoint& point) const
{
  if (!m_checked) m_checked = check();// check dimensions

  // transform point back, where sphere was defined.
  XYZPoint point0(m_transInv*point);

  // normal vector points from origin (0,0,0) to point of surface.
  XYZVector pn(point0);
  // but y-component is zero
  pn.SetY(0);
  
  // transform back

  return m_trans(pn).Unit();//p1.Unit();
}
//----------------------------------------------------------------------
bool PndDrcSurfPolyCyl::surfaceHit(PndDrcPhoton& ph, 
				XYZPoint&  pos_new, 
				double&    path_length) const
{
  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());

  // 2 positions for photon to have distance=radius from y-axis 
  //
  // (pos+a*dir)^2=R^2
  // pos^2 + 2 a dir pos + a^2 dir^2 = R^2
  // a^2 + a 2*dir*pos/dir2 + (pos2-R2)/dir2 = 0
  //
  // a1/2 = ...
  
  double pos2   = XYZVector(pos).Dot(XYZVector(pos));
  double dir2   = dir.Dot(dir);
  double posdir = dir.Dot(pos);
  
  double p = 2*posdir/dir2;
  double q = (pos2-m_radius*m_radius)/dir2;
  
  double root2 = p*p/4-q;
  if (root2<0) return false; // no hit


  double lambda1 = -posdir + sqrt(root2);
  double lambda2 = -posdir - sqrt(root2);
  const double eps = 0.001;

  if (m_verbosity>=4) cout<<" PndDrcSurfPolyCyl::surfaceHit lambda_1,2="
			  <<lambda1<<" "<<lambda2<<endl;  

  if (lambda1>eps && lambda2>eps)
    { // check both lambdas

      XYZPoint pos_new1     = pos + lambda1*dir;
      XYZPoint pos_check(pos_new1);
      bool hit1 = (pos_new1.Z()>0);
      pos_check.SetZ(0);
      if (hit1) hit1 = withinSurface(pos_check);  
      XYZPoint pos_new2     = pos + lambda2*dir;
      pos_check = pos_new2;
      bool hit2 = (pos_new2.Z()>0);
      pos_check.SetZ(0);
      if (hit2) hit2 = withinSurface(pos_check);
      if (hit1 && hit2)
	{
	      // take closest
	  double 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)
    {
      path_length = lambda1;
    }
  else if (lambda1<=eps && lambda2>eps)
    {
      path_length = lambda2;
    }
  else
    {
     //cout<<" not taken"<<endl;//###
      return false;
      
    }
  pos_new     = pos + path_length*dir;
  if (m_verbosity>=4) cout<<" PndDrcSurfPolyCyl::surfaceHit pos_new_z="
			    <<pos_new.Z()<<endl;  
  if (pos_new.Z()<0) return false; // wrong half cyl.


  // 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;
    }

  if (m_verbosity>=4) cout<<" PndDrcSurfPolyCyl::surfaceHit false"<<endl;  
  



 if (m_verbosity>=4) cout<<" PndDrcSurfPolyCyl::surfaceHit false"<<endl;  

  return false;

} 
//----------------------------------------------------------------------
bool PndDrcSurfPolyCyl::check() const
{
  if (m_radius<=0)
    {
      cerr<<" ***  PndDrcSurfPolyCyl::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()*m_p[i].Z()+m_p[i].X()*m_p[i].X()>m_radius*m_radius)
	{
	  cerr<<" ***  PndDrcSurfPolyCyl::check: "
	      <<m_name<<": point outside sphere projection:"
	      <<" m_p["<<i<<"]="
	      <<m_p[i].X()<<" "
	      <<m_p[i].Y()<<" "
	      <<m_p[i].Z()<<endl;
	  exit(EXIT_FAILURE);
	}
    }
  for (unsigned int i=0; i<m_p.size(); i++)
    {
      if (m_p[i].Z() != 0)
	{
	  cerr<<" ***  PndDrcSurfPolyCyl::check: "
	      <<m_name<<": z component of point != 0 "
	      <<" m_p["<<i<<"]="
	      <<m_p[i].X()<<" "
	      <<m_p[i].Y()<<" "
	      <<m_p[i].Z()<<endl;
	  exit(EXIT_FAILURE);
	}
    }

  return true;
}
//----------------------------------------------------------------------
XYZPoint PndDrcSurfPolyCyl::limitingPoint(unsigned int i)
{
  unsigned int isize=m_p.size();
  if (i>(isize-1) || i<0)
    {
      cerr<<" *** PndDrcSurfPolyCyl::limitingPoint: index out of range, i="<<i<<endl;
      exit(EXIT_FAILURE);
    }
  //TVector3 p1(m_p[i]);

  XYZPoint p1(m_p.at(i));

  p1.SetZ(sqrt(m_radius*m_radius - p1.X()*p1.X())); // m_p1.Z()=0...

  return m_trans*p1;
  
  
}
//----------------------------------------------------------------------
XYZPoint PndDrcSurfPolyCyl::centerPoint()
{
  return m_trans*XYZPoint(0,0,m_radius);
}
//----------------------------------------------------------------------
void PndDrcSurfPolyCyl::addTransform(const Transform3D& trans)
{
  if (verbosity()>=3) cout<<"    PndDrcSurfPolyCyl::addTransform() name="<<m_name<<endl;
  m_trans    = trans * m_trans;
  m_transInv = m_trans.Inverse();
}
//----------------------------------------------------------------------
void PndDrcSurfPolyCyl::print(fstream& stream) const
{
  if (!m_checked) m_checked = check();// check dimensions

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

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



  double frac=1;
  

  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 x2y2 = m_p1.X()*m_p1.X()+m_p1.Y()*m_p1.Y();
	  m_p1.SetZ(sqrt(m_radius*m_radius - x2y2)); // m_p1.Z()=0...
	  
	  m_p1 = m_trans(m_p1);
	  //m_p1.Transform(m_rot);
	  //m_p1 += m_shift;

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

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

}