#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 "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 "DrcSurfCyl.h"
#include "DrcOptMatAbs.h"
#include "DrcOptDev.h"


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

  return *this;
}
//----------------------------------------------------------------------
void DrcSurfCyl::set(XYZPoint r1, XYZPoint r2, double radius)
{
  m_p1 = r1;
  m_p2 = r2;
  m_radius = radius;



}
//----------------------------------------------------------------------
XYZVector DrcSurfCyl::normal(const XYZPoint& point) const
{
  // normal vector is perpendicular to vector m_p1-m_p2

  XYZVector diff(m_p2-m_p1);
  XYZVector cross1 = (point-m_p1).Cross(diff);

  return (diff.Cross(cross1)).Unit();
}
//----------------------------------------------------------------------
bool DrcSurfCyl::surfaceHit(DrcPhoton& ph, 
			    XYZPoint&  pos_new, 
			    double&    path_length) const
{	
  //static const double eps = 1.0e-9;

  // for the methode look up
  // http://www.matheraum.de/read?t=77538&v=t
  // but watch out: the results are wrong.

  XYZPoint  a(ph.position());
  XYZVector u((ph.direction()).Unit()); //  photon: a + mu*u;
  XYZPoint  b(m_p1);
  XYZVector n(m_p2-m_p1);

  XYZVector c = a-b;
  XYZVector p = u.Dot(n)/n.Mag2()*n;
  XYZVector m = n.Dot(c)/n.Mag2()*n;

  // mu2 * s + mu * p + q = 0;

  double s = u.Dot(u) - 2*u.Dot(p) + p.Dot(p);
  double P = 2*(c.Dot(u) - c.Dot(p) - u.Dot(m) + p.Dot(m));
  double Q = c.Dot(c) - 2*c.Dot(m) + m.Dot(m) - m_radius*m_radius;


  //cout<<" s P Q "<<s<<" "<<P<<" "<<Q<<endl;//###
  if (s != 0)
    { 
      P /= s;
      Q /= s;
    }
  else
    {
      return false;
    }

  double root2 = P*P/4 - Q;
  //cout<<" root2="<<root2<<endl;//###


  if (root2<0) return false;

  double mu1 = -P/2 + sqrt(root2);
  double mu2 = -P/2 - sqrt(root2);

  //cout<<" mu1/2 = "<<mu1<<" "<<mu2<<endl;//###
  double mu_low,mu_high;
  if (mu1<mu2)
    {
      mu_low  = mu1;
      mu_high = mu2;
    }
  else
    {
      mu_low  = mu2;
      mu_high = mu1;
    }

  double mu=-1;
  if (mu_low>0) 
    {
      mu = mu_low;
    }
  else if (mu_high>0)
    {
      mu = mu_high;
    }
  else
    {
      return false;
    }

  if (mu>0)
    {
      double lambda = (n.Dot(c)+mu*n.Dot(u))/n.Mag2();
      //cout<<" lambda = "<<lambda<<endl;//###
      if (lambda>0 && lambda<1) 
	{
	  pos_new = a + mu*u;
	  path_length = mu;
	  if (m_pixel) ph.setFate(Drc::PhotMeasured);
	  return true;
	} 

    }
    
  return false;

} 
//----------------------------------------------------------------------
void DrcSurfCyl::print(fstream& stream) const
{
  const double pi=3.1415926535;

  TVector3 diff(m_p2.X()-m_p1.X(), m_p2.Y()-m_p1.Y(), m_p2.Z()-m_p1.Z());
  TVector3 norm = m_radius * (diff.Orthogonal()).Unit();
  stream<<" TPolyLine3D *l1 = new TPolyLine3D(49);"<<endl;
  stream<<" TPolyLine3D *l2 = new TPolyLine3D(49);"<<endl;
  TVector3 r1_null;
  TVector3 r2_null;
  TVector3 r1;
  TVector3 r2;

  for (int i=0; i<48; i++)
    {
      norm.Rotate((2*pi/360)*360/48, diff);
      stream<<" TPolyLine3D *l = new TPolyLine3D(2);"<<endl;
      r1 = TVector3(m_p1.X()+norm.X(), m_p1.Y()+norm.Y(), m_p1.Z()+norm.Z());
      stream<<" l->SetPoint("<<0<<","
	<<r1.X()<<","
	<<r1.Y()<<","
	<<r1.Z()<<");"<<endl;
      r2 = TVector3(m_p2.X()+norm.X(), m_p2.Y()+norm.Y(), m_p2.Z()+norm.Z());
      stream<<" l->SetPoint("<<1<<","
	<<r2.X()<<","
	<<r2.Y()<<","
	<<r2.Z()<<");"<<endl;
      stream<<" l->SetLineColor("<<m_printColor<<");"<<endl;
      stream<<" l->Draw();"<<endl;

      if (i==0)
	{
	  r1_null = r1;
	  r2_null = r2;
	}
      stream<<" l1->SetPoint("<<i<<","
	<<r1.X()<<","
	<<r1.Y()<<","
	<<r1.Z()<<");"<<endl;

      stream<<" l2->SetPoint("<<i<<","
	<<r2.X()<<","
	<<r2.Y()<<","
	<<r2.Z()<<");"<<endl;
    }
      stream<<" l1->SetPoint("<<48<<","
	<<r1_null.X()<<","
	<<r1_null.Y()<<","
	<<r1_null.Z()<<");"<<endl;

      stream<<" l2->SetPoint("<<48<<","
	<<r2_null.X()<<","
	<<r2_null.Y()<<","
	<<r2_null.Z()<<");"<<endl;

      stream<<" l1->SetLineColor("<<m_printColor<<");"<<endl;
      stream<<" l1->Draw();"<<endl;
      stream<<" l2->SetLineColor("<<m_printColor<<");"<<endl;
      stream<<" l2->Draw();"<<endl;


}
//----------------------------------------------------------------------
void DrcSurfCyl::addTransform(const Transform3D& trans)
{
  if (verbosity()>=3) cout<<"    DrcSurfAbs::addTransform() name="<<m_name<<endl;
  //m_trans    *= trans;
  //m_transInv  = m_trans.Inverse();

  m_p1 = trans * m_p1;
  m_p2 = trans * m_p2;

}