#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 "DrcPhoton.h"
#include "DrcOptReflAbs.h"
#include "DrcSurfAbs.h"
#include "DrcSurfPolyFlat.h"
#include "DrcSurfPolySphere.h"
#include "DrcOptMatAbs.h"
#include "DrcOptDev.h"


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

  return *this;
}
//----------------------------------------------------------------------
XYZVector DrcSurfPolySphere::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);

  // transform back

  return m_trans(pn).Unit();//p1.Unit();
}
//----------------------------------------------------------------------
bool DrcSurfPolySphere::surfaceHit(DrcPhoton& 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());

  //if (/*pos.Z()<0 && */ dir.Z()<0) return false; // wrong direction###

  // check if the photon hits the full sphere
  // double costh = sqrt((dir.X()*dir.X()+dir.Y()*dir.Y())/dir.Mag2());
  // double theta = fabs(acos(costh));
  // if (tan(theta)* -dir.Z() >= m_radius) return false; // angle too steep

  // compute hit position
  //
  // line : x = pos + lambda*dir
  //
  // sphere: x^2 = r^2
  //
  // pos^2 + lambda^2*dir^2 + 2 pos lambda dir = r^2
  //
  //
  // lambda^2 + lambda*2*pos*dir + pos^2-r^2 = 0;
  //
  // lambda12 = -pos*dir + sqrt((pos*dir)^2 - (pos^2-r^2)
  //
  // the other (-) solution is the wrong one.
  //
  // path_length = lambda since dir is normalized

  double posdir = pos.Dot(dir);
  double diff   = pos.Mag2() - m_radius*m_radius;

  double root2 = posdir*posdir - diff;
  if (m_verbosity>=4) cout<<" DrcSurfPolySphere::surfaceHit root2="<<root2<<endl;  
  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<<" DrcSurfPolySphere::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<<" DrcSurfPolySphere::surfaceHit pos_new_z="
			    <<pos_new.Z()<<endl;  
  if (pos_new.Z()<0) return false; // wrong half sphere.


  // 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<<" DrcSurfPolySphere::surfaceHit false"<<endl;  

  return false;

} 
//----------------------------------------------------------------------
bool DrcSurfPolySphere::check() const
{
  if (m_radius<=0)
    {
      cerr<<" ***  DrcSurfPolySphere::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].Mag2()>m_radius*m_radius)
	{
	  cerr<<" ***  DrcSurfPolySphere::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<<" ***  DrcSurfPolySphere::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 DrcSurfPolySphere::limitingPoint(unsigned int i)
{
  unsigned int isize=m_p.size();
  if (i>(isize-1) || i<0)
    {
      cerr<<" *** DrcSurfPolySphere::limitingPoint: index out of range, i="<<i<<endl;
      exit(EXIT_FAILURE);
    }
  //TVector3 p1(m_p[i]);

  XYZPoint p1(m_p.at(i));
  
  double x2y2 = p1.X()*p1.X()+p1.Y()*p1.Y();
  p1.SetZ(sqrt(m_radius*m_radius - x2y2)); // m_p1.Z()=0...

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

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

  if (isize < 3)
    {
      cerr<<" *** DrcSurfPolySphere::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 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;
  
    }

}