// PropXYXYBV.cpp

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

using std::cerr;
using std::endl;

#ifndef DEFECT_NO_STDLIB_NAMESPACES
using std::sqrt;
using std::cos;
using std::sin;
using std::atan2;
using std::asin;
using std::abs;
#endif

using namespace trf;

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

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "PropXYXYBV" );
  double bfield = data.get_double("bfield");
  return ObjPtr( new PropXYXYBV(bfield) );
}

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

// Private function to determine dphi of the propagation
double direction_bv(int flag_forward, int sign_dphi,
                 double du,
                 double norm, double cat,
                 double sinphi, double cosphi)
{
  int sign_cat;
  if(du*flag_forward>0.) sign_cat =  1;
  if(du*flag_forward<0.) sign_cat = -1;
  if(du == 0.){
    if(sinphi >=0. ) sign_cat =  1;
    if(sinphi < 0. ) sign_cat = -1;
  }
  
  double sin_dphi =  norm*sinphi + cat*cosphi*sign_cat;
  double cos_dphi = -norm*cosphi + cat*sinphi*sign_cat;
  
  int sign_sindphi;
  if(sin_dphi> 0.) sign_sindphi =  1;
  if(sin_dphi< 0.) sign_sindphi = -1;
  if(sin_dphi == 0.) sign_sindphi = sign_dphi;
  
  double dphi = PI*(sign_dphi - sign_sindphi) + sign_sindphi*acos(cos_dphi);
  return dphi;
}
//**********************************************************************

// Private function to propagate a track without error
// The corresponding track parameters are:
// 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.

// We use the code from PropXYXY written for the case of B^ || z^.
// First, we rotate the track to the c.f. where B^ || z^, propagate the
// track and rotate the propagated track back to the original frame.
// We require the two planes to be parallel to avoid the corner regions
// where the approximation B^ || v^ is not valid.

PropStat 
vec_propagatexyxy_bv_( double B, VTrack& trv, const Surface& srf, 
                    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 XYPlane.
  assert( srf1.get_pure_type() == SurfXYPlane::get_static_type() );
  if ( srf1.get_pure_type( ) != SurfXYPlane::get_static_type() )
    return pstat;
  const SurfXYPlane& sxyp1 = (const SurfXYPlane&) srf1;
  
  // 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;
  
  // If surfaces are the same, we can return now.
  if ( srf.pure_equal(srf1) ) {
    if(pderiv)
      {
	TrackDerivative& deriv = *pderiv;
	deriv.fill( 0.0 );
	deriv(0,0) = 1.0;
	deriv(1,1) = 1.0;
	deriv(2,2) = 1.0;
	deriv(3,3) = 1.0;
	deriv(4,4) = 1.0;
      }
    pstat.set_same();
    return pstat;
  }
  
  // Fetch the u's and phi's of the planes and the starting track vector.
  int iphi  = SurfXYPlane::NORMPHI;
  int idist = SurfXYPlane::DISTNORM;
  double phi_0 = sxyp1.get_parameter(iphi);
  double   u_0 = sxyp1.get_parameter(idist);
  double phi_n = sxyp2.get_parameter(iphi);
  double   u_n = sxyp2.get_parameter(idist);
  double phi_u = phi_0 - phi_n;

  // check here that first planes are parallel and second that 
  //planes have phi==0||PI , otherwise this propagator doesn't work.

  if (abs(phi_0)!=0. && abs(phi_0)!=PI ) return pstat;
  if ( abs(phi_u)!=0. && abs(phi_u)!=PI && abs(phi_u)!=TWOPI ) return pstat;

  //cout<<" PropXYXYBV "<<endl;
  //cout<<trv<<endl;
  TrackVector vec = trv.get_vector();

  // b_0 is rotated by 90 deg around the u axis wrt to the input track vector
  // (v',z') = (-z,v)
  double b1_0 = -vec(IZ);                  
  double b2_0 = vec(IV);                  
  double b3_0 = -vec(IDZDU);               
  double b4_0 = vec(IDVDU);               
  double b5_0 = vec(IQP);                 
  
  
  double cosphi_u = cos(phi_u);
  double sinphi_u = sin(phi_u);
  
  // check if du == 0 ( that is track moves parallel to the destination plane )
  double du_du_0 = cosphi_u - b3_0*sinphi_u;
  if(du_du_0==0.) return pstat;
  
  double a_hat_u = 1./du_du_0; 
  double a_hat_u2 = a_hat_u*a_hat_u;
  
  double u  = u_0*cosphi_u - b1_0*sinphi_u;
  double b1 = b1_0*cosphi_u + u_0*sinphi_u;
  double b2 = b2_0;
  double b3 = (b3_0*cosphi_u + sinphi_u)*a_hat_u;
  double b4 = b4_0*a_hat_u;
  double b5 = b5_0;
  
  int sign_du0 = 0;
  if(trv.is_forward())  sign_du0 =  1;
  if(trv.is_backward()) sign_du0 = -1;
  if(sign_du0 == 0){
    cerr << "PropXYXYBV::_vec_prop: Unknown direction of a track "<< endl;
    abort();
  }
  int sign_du;
  if(du_du_0*sign_du0 > 0) sign_du =  1;
  if(du_du_0*sign_du0 < 0) sign_du = -1;
  
  // check that q/p != 0
  assert(b5 !=0. );
  
  // 1/curv of the track is r 
  double r = 1/(b5*B)*sqrt(1 + b3_0*b3_0)/sqrt(1 + b3_0*b3_0 + b4_0*b4_0);
  double b3_hat = sqrt(1 + b3*b3);
  double b34_hat = sqrt(1 + b3*b3 + b4*b4);
  double b3_hat2 = b3_hat*b3_hat;
  double b34_hat2 = b34_hat*b34_hat;
  double cosphi =         -b3*sign_du/b3_hat;
  double sinphi =             sign_du/b3_hat;
  double rsinphi =  1./(b5*B)*sign_du/b34_hat;
  double rcosphi = -b3/(b5*B)*sign_du/b34_hat;
  
  double du = u_n - u;
  double norm = du/r - cosphi;
  
  // xy-xy propagation failed : noway to the new plane
  if(abs(norm)>1.) return pstat;
  
  double cat = sqrt(1.-norm*norm);
  int flag_forward;
  int sign_dphi;
  switch (dir) {
  case Propagator::NEAREST:
    {
      // try forward propagation
      flag_forward = 1;
      if(b5>0.) sign_dphi =  1;
      if(b5<0.) sign_dphi = -1;
      double dphi1=
	direction_bv(flag_forward,sign_dphi,du,norm, cat, sinphi, cosphi);
      // try backward propagation
      flag_forward = -1;
      if(b5>0.) sign_dphi = -1;
      if(b5<0.) sign_dphi =  1;
      double dphi2=
	direction_bv(flag_forward,sign_dphi,du,norm, cat, sinphi, cosphi);
      if( abs(dphi2)>abs(dphi1) )
	{
	  flag_forward = -flag_forward;
	  sign_dphi = - sign_dphi;
	}
    }
    break;
  case Propagator::FORWARD:
    flag_forward = 1;
    if(b5>0.) sign_dphi =  1;
    if(b5<0.) sign_dphi = -1;
    break;
  case Propagator::BACKWARD:
    flag_forward = -1;
    if(b5>0.) sign_dphi = -1;
    if(b5<0.) sign_dphi =  1;
    break;
  default:
    cerr << "PropXYXY::_vec_prop: Unknown direction." << endl;
    abort();
  }
  
  int sign_cat;
  if(du*sign_dphi*b5>0.) sign_cat =  1;
  if(du*sign_dphi*b5<0.) sign_cat = -1;
  if(du == 0.){
    if(sinphi >=0. ) sign_cat =  1;
    if(sinphi < 0. ) sign_cat = -1;
  }
  
  double sin_dphi =  norm*sinphi + cat*cosphi*sign_cat;
  double cos_dphi = -norm*cosphi + cat*sinphi*sign_cat;
  if(cos_dphi>1.) cos_dphi=1.;
  if(cos_dphi<-1.) cos_dphi= -1.;
  
  int sign_sindphi;
  if(sin_dphi> 0.) sign_sindphi =  1;
  if(sin_dphi< 0.) sign_sindphi = -1;
  if(sin_dphi == 0.) sign_sindphi = sign_dphi;
  
  double dphi = PI*(sign_dphi - sign_sindphi) + sign_sindphi*acos(cos_dphi);
  
  // check that I didn't make any mistakes
  
  assert(abs(sin(dphi) -sin_dphi)<1.e-5);
  
  // check if dun == 0 ( that is track moves parallel to the destination plane)
  double du_n_du = cos_dphi - b3*sin_dphi;
  if(du_n_du==0.) return pstat;
  
  double a_hat_dphi = 1./du_n_du;
  double a_hat_dphi2 = a_hat_dphi*a_hat_dphi;
  double c_hat_dphi =     sin_dphi + b3*cos_dphi ;
  
  double b1_n = b1 + rsinphi*(1-cos_dphi) - rcosphi*sin_dphi;
  double b2_n = b2 + b4/(b5*B)*sign_du/b34_hat*dphi;
  double b3_n = c_hat_dphi*a_hat_dphi;
  double b4_n =         b4*a_hat_dphi;
  double b5_n = b5;
  
  // double u_n_0 = u_n*cosphi_u + b1_n*sinphi_u;
  
  // check if track crossed original plane during the propagation
  // switch (dir) {
  //  case Propagator::FORWARD:
  //   if((u_n_0 - u_0)*sign_du0<0) return pstat;
  //  break;
  //  case Propagator::BACKWARD:
  //   if((u_n_0 - u_0)*sign_du0>0) return pstat;
  //  break;
  // }
  
  int sign_dun;
  if(du_n_du*sign_du > 0) sign_dun =  1;
  if(du_n_du*sign_du < 0) sign_dun = -1;
  
  // rotate the propagated track  back by -90 deg around the u axis: 
  // (v,z) = (z',-v').
  vec(IV)    = b2_n;
  vec(IZ)    = -b1_n;
  vec(IDVDU) = b4_n;
  vec(IDZDU) = -b3_n;
  vec(IQP)   = b5_n;
  
  // Update trv
  trv.set_surface(SurfacePtr(srf.new_pure_surface()));
  trv.set_vector(vec);
  
  // set new direction of the track
  if(sign_dun ==  1) trv.set_forward();
  if(sign_dun == -1) trv.set_backward();
  //cout<<trv<<endl; 
  // Calculate sT.
  double crv = B*b5;
  double dv = b1_n - b1;
  double dxy = sqrt( du*du + dv*dv );
  double arg = 0.5*crv*dxy;
  double dst = dxy*asinrat(arg);
  
  // Calculate s.
  double dz = b2_n - b2;
  assert( flag_forward==1 || flag_forward==-1 );
  double ds = double(flag_forward)*sqrt(dst*dst+dz*dz);
  
  // Set the return status.
  pstat.set_path_distance(ds);
  //(flag_forward==1)?pstat.set_forward():pstat.set_backward();
  
  // exit now if user did not ask for error matrix.
  if ( ! pderiv ) return pstat;
  
  // du_d0
  
  double du_db1_0 = - sinphi_u;
  
  // db1_d0
  
  double db1_db1_0 = cosphi_u;
  
  // db3_d0
  
  double db3_db3_0 = a_hat_u2;
  
  // db4_d0
  
  double db4_db3_0 = b4_0*sinphi_u*a_hat_u2;
  double db4_db4_0 = a_hat_u;
  
  // dr_d
  
  double dr_db3 = r*b3*b4*b4/(b3_hat2*b34_hat2);
  double dr_db4 = -r*b4/b34_hat2;
  double dr_db5 = -r/b5;
  
  // dcosphi_d
  
  double dcosphi_db3 = - sign_du/b3_hat - cosphi*b3/b3_hat2;
  
  // dsinphi_d
  
  double dsinphi_db3 = - sinphi*b3/b3_hat2;
  
  // dcat_d
  
  double dcat_db3 = norm/cat*(du/(r*r)*dr_db3 + dcosphi_db3 );
  double dcat_db4 = norm/cat* du/(r*r)*dr_db4;
  double dcat_db5 = norm/cat* du/(r*r)*dr_db5;
  double dcat_du  = norm/(cat*r);
  
  // dnorm_d
  
  double dnorm_db3 = - du/(r*r)*dr_db3 - dcosphi_db3;
  double dnorm_db4 = - du/(r*r)*dr_db4;
  double dnorm_db5 = - du/(r*r)*dr_db5;
  double dnorm_du  = - 1./r;
  
  // dcos_dphi_d
  
  double dcos_dphi_db3 = - cosphi*dnorm_db3 - norm*dcosphi_db3 +
    sign_cat*(sinphi*dcat_db3 + cat*dsinphi_db3);
  double dcos_dphi_db4 = - cosphi*dnorm_db4 + sign_cat*sinphi*dcat_db4;
  double dcos_dphi_db5 = - cosphi*dnorm_db5 + sign_cat*sinphi*dcat_db5;
  double dcos_dphi_du  = - cosphi*dnorm_du  + sign_cat*sinphi*dcat_du;
  
  // dsin_dphi_d
  
  double dsin_dphi_db3 = sinphi*dnorm_db3 + norm*dsinphi_db3 +
    sign_cat*(cosphi*dcat_db3 + cat*dcosphi_db3);
  double dsin_dphi_db4 = sinphi*dnorm_db4 + sign_cat*cosphi*dcat_db4; 
  double dsin_dphi_db5 = sinphi*dnorm_db5 + sign_cat*cosphi*dcat_db5;
  double dsin_dphi_du  = sinphi*dnorm_du  + sign_cat*cosphi*dcat_du;
  
  // ddphi_d
  
  double ddphi_db3;
  double ddphi_db4;
  double ddphi_db5;
  double ddphi_du;
  if(abs(sin_dphi)>0.5)
    {
      ddphi_db3 = - dcos_dphi_db3/sin_dphi;
      ddphi_db4 = - dcos_dphi_db4/sin_dphi;
      ddphi_db5 = - dcos_dphi_db5/sin_dphi;
      ddphi_du  = - dcos_dphi_du /sin_dphi;
    }
  else
    {
      ddphi_db3 = dsin_dphi_db3/cos_dphi;
      ddphi_db4 = dsin_dphi_db4/cos_dphi;
      ddphi_db5 = dsin_dphi_db5/cos_dphi;
      ddphi_du  = dsin_dphi_du /cos_dphi;
    }
  
  // da_hat_dphi_d
  
  double da_hat_dphi_db3 = - a_hat_dphi2*
    (dcos_dphi_db3 - sin_dphi - b3*dsin_dphi_db3); 
  double da_hat_dphi_db4 = - a_hat_dphi2*(dcos_dphi_db4 - b3*dsin_dphi_db4);
  double da_hat_dphi_db5 = - a_hat_dphi2*(dcos_dphi_db5 - b3*dsin_dphi_db5);
  double da_hat_dphi_du  = - a_hat_dphi2*(dcos_dphi_du  - b3*dsin_dphi_du );
  
  // dc_hat_dphi_d
  
  double dc_hat_dphi_db3 = b3*dcos_dphi_db3 + dsin_dphi_db3 + cos_dphi;
  double dc_hat_dphi_db4 = b3*dcos_dphi_db4 + dsin_dphi_db4;
  double dc_hat_dphi_db5 = b3*dcos_dphi_db5 + dsin_dphi_db5;
  double dc_hat_dphi_du  = b3*dcos_dphi_du  + dsin_dphi_du ;
  
  // db1_n_d
  
  double db1_n_db1 = 1;
  double db1_n_db3 = (dr_db3*sinphi+r*dsinphi_db3)*(1-cos_dphi) 
    - rsinphi*dcos_dphi_db3
    - dr_db3*cosphi*sin_dphi - r*dcosphi_db3*sin_dphi
    - rcosphi*dsin_dphi_db3;
  double db1_n_db4 = dr_db4*sinphi*(1-cos_dphi) - rsinphi*dcos_dphi_db4 
    - dr_db4*cosphi*sin_dphi - rcosphi*dsin_dphi_db4;
  double db1_n_db5 = dr_db5*sinphi*(1-cos_dphi) - rsinphi*dcos_dphi_db5 
    - dr_db5*cosphi*sin_dphi - rcosphi*dsin_dphi_db5;
  double db1_n_du  = - rsinphi*dcos_dphi_du - rcosphi*dsin_dphi_du;
  
  // db2_n_d
  
  double db2_n_db2 = 1.;
  double db2_n_db3 = 1./(b5*B)*b4*sign_du/b34_hat*
                    ( - dphi*b3/b34_hat2 + ddphi_db3);
  double db2_n_db4 = 1./(b5*B)*sign_du/b34_hat*
    ( - dphi*b4*b4/b34_hat2 + b4*ddphi_db4 + dphi );
  double db2_n_db5 = 1./(b5*B)*b4*sign_du/b34_hat*( ddphi_db5 - dphi/b5);
  double db2_n_du  = 1./(b5*B)*b4*sign_du/b34_hat*ddphi_du;

  // db3_n_d
  
  double db3_n_db3 = a_hat_dphi*dc_hat_dphi_db3 + da_hat_dphi_db3*c_hat_dphi;
  double db3_n_db4 = a_hat_dphi*dc_hat_dphi_db4 + da_hat_dphi_db4*c_hat_dphi;
  double db3_n_db5 = a_hat_dphi*dc_hat_dphi_db5 + da_hat_dphi_db5*c_hat_dphi;
  double db3_n_du  = a_hat_dphi*dc_hat_dphi_du  + da_hat_dphi_du *c_hat_dphi;
  
  // db4_n_d
  
  double db4_n_db3 = b4*da_hat_dphi_db3;
  double db4_n_db4 = b4*da_hat_dphi_db4 + a_hat_dphi;
  double db4_n_db5 = b4*da_hat_dphi_db5;
  double db4_n_du  = b4*da_hat_dphi_du;
  
  // db5_n_d
  
  // db1_n_d0
  
  double db1_n_db1_0 = db1_n_du * du_db1_0 + db1_n_db1*db1_db1_0;
  double db1_n_db2_0 = 0.;
  double db1_n_db3_0 = db1_n_db3*db3_db3_0 + db1_n_db4*db4_db3_0;
  double db1_n_db4_0 = db1_n_db4*db4_db4_0;
  double db1_n_db5_0 = db1_n_db5;
  
  // db2_n_d0
  
  double db2_n_db1_0 = db2_n_du *du_db1_0;
  double db2_n_db2_0 = db2_n_db2;
  double db2_n_db3_0 = db2_n_db3*db3_db3_0 + db2_n_db4*db4_db3_0;
  double db2_n_db4_0 = db2_n_db4*db4_db4_0;
  double db2_n_db5_0 = db2_n_db5;
  
  // db3_n_d0
  
  double db3_n_db1_0 = db3_n_du*du_db1_0;
  double db3_n_db2_0 = 0.;
  double db3_n_db3_0 = db3_n_db3*db3_db3_0 + db3_n_db4*db4_db3_0;
  double db3_n_db4_0 = db3_n_db4*db4_db4_0;
  double db3_n_db5_0 = db3_n_db5;
  
 // db4_n_d0
  
  double db4_n_db1_0 = db4_n_du*du_db1_0;
  double db4_n_db2_0 = 0.;
  double db4_n_db3_0 = db4_n_db3*db3_db3_0 + db4_n_db4*db4_db3_0;
  double db4_n_db4_0 = db4_n_db4*db4_db4_0;
  double db4_n_db5_0 = db4_n_db5;
  
  // db5_n_d0

  double db5_n_db1_0 = 0.;
  double db5_n_db2_0 = 0.;
  double db5_n_db3_0 = 0.;
  double db5_n_db4_0 = 0.;
  double db5_n_db5_0 = 1.;
  
  TrackDerivative& deriv = *pderiv;
  
  // apply the rotation to the matrix db to return to the original c.f.
  deriv(IV,IV)       =db2_n_db2_0;
  deriv(IV,IZ)       =-db2_n_db1_0;
  deriv(IV,IDVDU)    =db2_n_db4_0;
  deriv(IV,IDZDU)    =-db2_n_db3_0;
  deriv(IV,IQP)      =db2_n_db5_0;
  deriv(IZ,IV)       =-db1_n_db2_0;
  deriv(IZ,IZ)       =db1_n_db1_0;
  deriv(IZ,IDVDU)    =-db1_n_db4_0;
  deriv(IZ,IDZDU)    =db1_n_db3_0;
  deriv(IZ,IQP)      = -db1_n_db5_0; 
  deriv(IDVDU,IV)    =db4_n_db2_0;
  deriv(IDVDU,IZ)    =-db4_n_db1_0;
  deriv(IDVDU,IDVDU) =db4_n_db4_0;
  deriv(IDVDU,IDZDU) =-db4_n_db3_0;
  deriv(IDVDU,IQP)   =db4_n_db5_0;
  deriv(IDZDU,IV)    =-db3_n_db2_0;
  deriv(IDZDU,IZ)    =db3_n_db1_0;
  deriv(IDZDU,IDVDU) =-db3_n_db4_0;
  deriv(IDZDU,IDZDU) =db3_n_db3_0;
  deriv(IDZDU,IQP)   =-db3_n_db5_0;
  deriv(IQP,IV)      =db5_n_db2_0;
  deriv(IQP,IZ)      =-db5_n_db1_0;
  deriv(IQP,IDVDU)   =db5_n_db4_0;
  deriv(IQP,IDZDU)   = -db5_n_db3_0; 
  deriv(IQP,IQP)     =db5_n_db5_0;
  
  return pstat;

}

//************************************************************************
// PropXYXYBV member functions.
//************************************************************************

// output stream

void PropXYXYBV::ostr( ostream& stream ) const
{ 
  stream << begin_object ;
  stream << "XYPlane-XYPlane propagation with constant " << get_bfield()
	 << " Tesla field normal to z and parallel to the planes"; 
  stream << end_object;
}

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

// Constructor.

PropXYXYBV::PropXYXYBV(double bfield) : _bfac(BFAC*bfield) { }

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

// Destructor.

PropXYXYBV::~PropXYXYBV() { }

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

// Clone.

Propagator* PropXYXYBV::new_propagator() const {
  return new PropXYXYBV( get_bfield() );
}


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

// propagate a track with error in the specified direction
PropStat PropXYXYBV::vec_dir_prop( VTrack& trv, const Surface& srf,
				   PropDir dir,TrackDerivative* pder ) const {
  PropStat pstat = vec_propagatexyxy_bv_( _bfac, trv, srf, dir, pder );
  return pstat;
}

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

// return the magnetic field

double PropXYXYBV::get_bfield() const {
  return _bfac/BFAC;
}

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

// Return the creator.

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

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

// Write the object data.

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

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