// PropCylXY.cpp

#include <iostream>
#include "PropCylXY.h"
#include "trfxyp/PropXYXY.h"
#include "trfbase/PropStat.h"
#include "trfutil/TRFMath.h"
#include "trfxyp/SurfXYPlane.h"
#include "trfcyl/SurfCylinder.h"
#include "trfbase/VTrack.h"
#include "trfbase/ETrack.h"
#include "trfutil/trfstream.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include <cassert>

#ifndef DEFECT_NO_STDLIB_NAMESPACES
using std::cerr;
using std::endl;
using std::sqrt;
using std::cos;
using std::sin;
using std::atan2;
using std::asin;
using namespace trf;
#endif

//**********************************************************************
// Free functions.
//**********************************************************************

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "PropCylXY" );
  ObjDoublePtr bfield;
  ObjData::Type dtype = data.get_type("bfield");
  if(dtype == ObjData::DOUBLE)
    bfield = new ObjDouble(data.get_double("bfield"));
  else if(dtype == ObjData::SHARE_PTR)
    bfield.assign_with_dynamic_cast(data.get_share_pointer("bfield"));
  else {
    cerr << "PropCylXY: can't get bfield parameter." << endl;
    abort();
  }
  return ObjPtr( new PropCylXY(bfield) );
}

}
//**********************************************************************

// Assign track parameter indices.
enum {
  IV     = SurfXYPlane::IV,
  IZ     = SurfXYPlane::IZ,
  IDVDU  = SurfXYPlane::IDVDU,
  IDZDU  = SurfXYPlane::IDZDU,
  IQP_XY = SurfXYPlane::IQP,

  IPHI   = SurfCylinder::IPHI, 
  IZ_CYL = SurfCylinder::IZ, 
  IALF   = SurfCylinder::IALF, 
  ITLM   = SurfCylinder::ITLM, 
  IQPT   = SurfCylinder::IQPT
};

//**********************************************************************

// Private function to propagate a track without error
// The corresponding track parameters are:
// On Cylinder:
// r (cm) is fixed
// 0 - phi
// 1 - z (cm)
// 2 - alpha = phi_dir - phi; tan(alpha) = r*dphi/dr
// 3 - sin(lambda) = dz/ds; tan(lambda) = dz/dsT
// 4 - q/pT (1/GeV/c) (pT is component of p parallel to cylinder)
// On XYPlane:
// u (cm) is fixed 
// 0 - v (cm)
// 1 - z (cm)
// 2 - dv/du 
// 3 - dz/du 
// 4 - q/p   p is momentum of a track, q is its charge
// If pderiv is nonzero, return the derivative matrix there.

PropStat 
vec_transformcylxy_( double B, VTrack& trv, double phi_n,
                    const Propagator::PropDir dir,
                    TrackDerivative* pderiv =0 ) {

 // construct return status
 PropStat pstat;

 // fetch the originating surface and vector
 const Surface& srf1 = *trv.get_surface();
 // TrackVector vec1 = trv.get_vector();

 // Check origin is a Cylinder.
 assert( srf1.get_pure_type() == SurfCylinder::get_static_type() );
 if ( srf1.get_pure_type( ) != SurfCylinder::get_static_type() )
   return pstat;
 const SurfCylinder& scy1 = (const SurfCylinder&) srf1;

 // Fetch the R of the cylinder and the starting track vector.
 int ir  = SurfCylinder::RADIUS;
 double Rcyl = scy1.get_parameter(ir);
 
 TrackVector vec = trv.get_vector();
 double c1 = vec(IPHI);                 // phi
 double c2 = vec(IZ);                   // z
 double c3 = vec(IALF);                 // alpha
 double c4 = vec(ITLM);                 // tan(lambda)
 double c5 = vec(IQPT);                 // q/pt

 // rotate coordinate system on phi_n

 c1 -= phi_n;
 if(c1 < 0.) c1 += TWOPI;

 double cos_c1 = cos(c1);

 double u = Rcyl*cos_c1;
 if( u < 0. )
 {
  u = - u;
  cos_c1 = - cos_c1;
  c1 -= PI;
  if(c1 < 0.) c1 += TWOPI;
  phi_n = phi_n + PI;
  if( phi_n >= TWOPI) phi_n -= TWOPI;
 } 

 double sin_c1  = sin(c1);
 double cos_dir = cos(c1+c3);
 double sin_dir = sin(c1+c3);
 double c4_hat2 = 1+c4*c4;
 double c4_hat  = sqrt(c4_hat2);

 // check if du == 0 ( that is track moves parallel to the destination plane )
 // du = pt*cos_dir 
 if(cos_dir == 0.) return pstat;

 double tan_dir = sin_dir/cos_dir;

 double b1 = Rcyl*sin_c1;
 double b2 = c2;
 double b3 = tan_dir;
 double b4 = c4/cos_dir;
 double b5 = c5/c4_hat;
 
 int sign_du;
 if(cos_dir > 0) sign_du =  1;
 if(cos_dir < 0) sign_du = -1;

 vec(IV)     = b1;
 vec(IZ)     = b2;
 vec(IDVDU)  = b3;
 vec(IDZDU)  = b4;
 vec(IQP_XY) = b5;

 // Update trv
 SurfXYPlane sxyp(u,phi_n);
 trv.set_surface(SurfacePtr(sxyp.new_pure_surface()));
 trv.set_vector(vec);

 // set new direction of the track
 if(sign_du ==  1) trv.set_forward();
 if(sign_du == -1) trv.set_backward();

 // Set the return status.
 pstat.set_same();

 // exit now if user did not ask for error matrix.
 if ( ! pderiv ) return pstat;
 
 // check that q/p != 0
 //assert(b5 !=0. );

 double b34_hat = sqrt(1 + b3*b3 + b4*b4);
 double invert_rsinphi =  (b5*B)*sign_du*b34_hat;

 // du_dc

 double du_dc1 = -Rcyl*sin_c1;

 // db1_dc

 double db1_dc1 = Rcyl*cos_c1;

 // db2_dc

 double db2_dc2 = 1.;

 // db3_dc

 double db3_dc1 = 1./(cos_dir*cos_dir);
 double db3_dc3 = 1./(cos_dir*cos_dir);

 // db4_dc

 double db4_dc1 = b4*tan_dir;
 double db4_dc3 = b4*tan_dir;
 double db4_dc4 = 1/cos_dir;

 // db5_dc

 double db5_dc4 = -c4*c5/(c4_hat*c4_hat2);
 double db5_dc5 = 1./c4_hat;

 // db3_n_db

 double db3_n_du  = -(1. + b3*b3)*invert_rsinphi;

 // db4_n_db

 double db4_n_du  = -b4*b3*invert_rsinphi;


 // db1_n_dc

 double db1_n_dc1 = db1_dc1 - b3 * du_dc1;
 double db1_n_dc2 = 0.;
 double db1_n_dc3 = 0.;
 double db1_n_dc4 = 0.;
 double db1_n_dc5 = 0.;

 // db2_n_dc

 double db2_n_dc1 = -b4 * du_dc1;
 double db2_n_dc2 = db2_dc2;
 double db2_n_dc3 = 0.;
 double db2_n_dc4 = 0.;
 double db2_n_dc5 = 0.;

 // db3_n_dc

 double db3_n_dc1 = db3_dc1 + db3_n_du * du_dc1;
 double db3_n_dc2 = 0.;
 double db3_n_dc3 = db3_dc3;
 double db3_n_dc4 = 0.;
 double db3_n_dc5 = 0.;

 // db4_n_dc

 double db4_n_dc1 = db4_dc1 + db4_n_du * du_dc1;
 double db4_n_dc2 = 0.;
 double db4_n_dc3 = db4_dc3;
 double db4_n_dc4 = db4_dc4;
 double db4_n_dc5 = 0.;

 // db5_n_dc

 double db5_n_dc1 = 0.;
 double db5_n_dc2 = 0.;
 double db5_n_dc3 = 0.;
 double db5_n_dc4 = db5_dc4;
 double db5_n_dc5 = db5_dc5;

 TrackDerivative& deriv = *pderiv;

 deriv(IV,IPHI)       =db1_n_dc1;
 deriv(IV,IZ_CYL)     =db1_n_dc2;
 deriv(IV,IALF)       =db1_n_dc3;
 deriv(IV,ITLM)       =db1_n_dc4;
 deriv(IV,IQPT)       =db1_n_dc5;
 deriv(IZ,IPHI)       =db2_n_dc1;
 deriv(IZ,IZ_CYL)     =db2_n_dc2;
 deriv(IZ,IALF)       =db2_n_dc3;
 deriv(IZ,ITLM)       =db2_n_dc4;
 deriv(IZ,IQPT)       =db2_n_dc5;
 deriv(IDVDU,IPHI)    =db3_n_dc1;
 deriv(IDVDU,IZ_CYL)  =db3_n_dc2;
 deriv(IDVDU,IALF)    =db3_n_dc3;
 deriv(IDVDU,ITLM)    =db3_n_dc4;
 deriv(IDVDU,IQPT)    =db3_n_dc5;
 deriv(IDZDU,IPHI)    =db4_n_dc1;
 deriv(IDZDU,IZ_CYL)  =db4_n_dc2;
 deriv(IDZDU,IALF)    =db4_n_dc3;
 deriv(IDZDU,ITLM)    =db4_n_dc4;
 deriv(IDZDU,IQPT)    =db4_n_dc5;
 deriv(IQP_XY,IPHI)   =db5_n_dc1;
 deriv(IQP_XY,IZ_CYL) =db5_n_dc2;
 deriv(IQP_XY,IALF)   =db5_n_dc3;
 deriv(IQP_XY,ITLM)   =db5_n_dc4;
 deriv(IQP_XY,IQPT)   =db5_n_dc5;
 /*
 if( fabs(B) < 1e-11 ) {
   deriv(IQP_XY,IPHI)   =0.0;
   deriv(IQP_XY,IZ_CYL) =0.0;
   deriv(IQP_XY,IALF)   =0.0;
   deriv(IQP_XY,ITLM)   =0.0;
   deriv(IQP_XY,IQPT)   =1.0;
 }
 */
 return pstat;
}
//************************************************************************
// PropCylXY member functions.
//************************************************************************

// output stream

void PropCylXY::ostr( std::ostream& stream ) const
{ 
  stream << begin_object ;
  stream << "Cylinder-XYPlane propagation with constant "
           << get_bfield() << " Tesla field"; 
  stream << end_object;
}

//************************************************************************

// Constructor.

PropCylXY::PropCylXY(ObjDoublePtr bfield) : _bfield(bfield),
                                  _propxyxy(bfield){

}

//************************************************************************

// Destructor.

PropCylXY::~PropCylXY() { }

//************************************************************************

// Clone.

Propagator* PropCylXY::new_propagator() const {
  return new PropCylXY( _bfield );
}

//************************************************************************

// propagate a track without error in the specified direction
PropStat PropCylXY::vec_dir_prop( VTrack& trv, const Surface& srf,
				  PropDir dir,TrackDerivative* pder ) const {
 PropStat pstat; 
 Propagator::reduce_direction(dir);

 // Check destination is a XYPlane.
 assert( srf.get_pure_type() == SurfXYPlane::get_static_type() );
 if ( srf.get_pure_type( ) != SurfXYPlane::get_static_type() )
   return pstat;
 const SurfXYPlane& sxyp2 = (const SurfXYPlane&) srf;

   // Fetch phi of the destination plane
 
 int iphi  = SurfXYPlane::NORMPHI;
 double phi_n = sxyp2.get_parameter(iphi);
 VTrack trv0 = trv;
 TrackDerivative deriv1 , deriv2;

 double bfac = -BFAC * (*_bfield)();

 if ( pder != 0 ) pstat = vec_transformcylxy_( bfac, trv0, phi_n, dir,&deriv1 );
 else pstat = vec_transformcylxy_( bfac, trv0, phi_n, dir );

 if ( ! pstat.success() ) return pstat; 

 if ( pder != 0 ) pstat = _propxyxy.vec_dir_prop(trv0,srf,dir,&deriv2);
 else  pstat = _propxyxy.vec_dir_prop(trv0,srf,dir);

 if ( pstat.success() ) 
   {
     trv = trv0;
     if ( pder != 0 )  *pder = TrackDerivative(deriv2*deriv1);
   }

 return pstat;
}

//**********************************************************************

// return the magnetic field

double PropCylXY::get_bfield() const {
  return (*_bfield)();
}

//**********************************************************************

// Return the creator.

ObjCreator PropCylXY::get_creator() {
  return create;
}

//**********************************************************************

// Write the object data.

ObjData PropCylXY::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "bfield", get_bfield() );
  return data;
}

//************************************************************************