// PropXYXY.cpp

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

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;
#endif

using namespace trf;

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "PropXYXY" );
  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 << "PropXYXY: can't get bfield parameter." << endl;
    abort();
  }
  return ObjPtr( new PropXYXY(bfield) );
}

PropStat _zero_bfield(VTrack& trv,const SurfXYPlane& sxyp1,const SurfXYPlane& sxyp2,Propagator::PropDir dir,TrackDerivative* pderiv);

}

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

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

// Private function to determine dphi of the propagation
double direction(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;

 if( fabs(cos_dphi)>1. ) 
   {
     double sn= -1.;
     if ( cos_dphi > 0 ) sn=1.;
     cos_dphi= sn*sqrt(1.-sin_dphi*sin_dphi);
   }
 if( fabs(sin_dphi)>1. ) 
   {
     double sn= -1.;
     if ( sin_dphi > 0 ) sn=1.;
     sin_dphi= sn*sqrt(1.-cos_dphi*cos_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.

PropStat 
vec_propagatexyxy_( double B, VTrack& trv, const Surface& srf, 
                    const Propagator::PropDir dir1,
                    TrackDerivative* pderiv =0 ) {

 // construct return status
 PropStat pstat;
 Propagator::PropDir dir(dir1);
 bool move = Propagator::reduce_direction(dir);

 // 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 and no XXX_MOVE is requested we can return now.
 bool same = srf.pure_equal(srf1);
 if ( same && ! move ) {
   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;
 }


 if ( same && dir1 == Propagator::NEAREST_MOVE ) dir = Propagator::FORWARD;

 if( fabs(B) < 1e-7 ) return _zero_bfield(trv,sxyp1,sxyp2,dir1,pderiv);

  // Fetch the u's and phi's of the planes and the starting track vector.
 TrackVector vec = trv.get_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);
 if ( same && move )
   {
     int forw= trv.is_forward()? 1 : -1 ;
     int forw_dir = (dir == Propagator::FORWARD) ? 1 : -1 ;
     int sn =  vec(IDVDU) > 0. ? 1 : -1 ;
     u_0 += 1.e-7 * (double)(forw*forw_dir*sn);
   }

 double b1_0 = vec(IV);                  // v
 double b2_0 = vec(IZ);                  // z
 double b3_0 = vec(IDVDU);               // dv/du
 double b4_0 = vec(IDZDU);               // dz/du
 double b5_0 = vec(IQP);                 // q/p

 double phi_u = phi_0 - phi_n;
 
 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 << "PropXYXY::_vec_propagate: 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*B>0.) sign_dphi =  1;
    if(b5*B<0.) sign_dphi = -1;
    double dphi1=
             direction(flag_forward,sign_dphi,du,norm, cat, sinphi, cosphi);
    // try backward propagation
    flag_forward = -1;
    if(b5*B>0.) sign_dphi = -1;
    if(b5*B<0.) sign_dphi =  1;
    double dphi2=
             direction(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*B>0.) sign_dphi =  1;
   if(b5*B<0.) sign_dphi = -1;
  break;
  case Propagator::BACKWARD:
   flag_forward = -1;
   if(b5*B>0.) sign_dphi = -1;
   if(b5*B<0.) sign_dphi =  1;
  break;
  default:
   cerr << "PropXYXY::_vec_propagate: Unknown direction." << endl;
   abort();
 }

 int sign_cat;
 if(du*sign_dphi*b5*B>0.) sign_cat =  1;
 if(du*sign_dphi*b5*B<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;

 vec(IV)    = b1_n;
 vec(IZ)    = b2_n;
 vec(IDVDU) = b3_n;
 vec(IDZDU) = b4_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();

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

 deriv(IV,IV)       =db1_n_db1_0;
 deriv(IV,IZ)       =db1_n_db2_0;
 deriv(IV,IDVDU)    =db1_n_db3_0;
 deriv(IV,IDZDU)    =db1_n_db4_0;
 deriv(IV,IQP)      =db1_n_db5_0;
 deriv(IZ,IV)       =db2_n_db1_0;
 deriv(IZ,IZ)       =db2_n_db2_0;
 deriv(IZ,IDVDU)    =db2_n_db3_0;
 deriv(IZ,IDZDU)    =db2_n_db4_0;
 deriv(IZ,IQP)      =db2_n_db5_0;
 deriv(IDVDU,IV)    =db3_n_db1_0;
 deriv(IDVDU,IZ)    =db3_n_db2_0;
 deriv(IDVDU,IDVDU) =db3_n_db3_0;
 deriv(IDVDU,IDZDU) =db3_n_db4_0;
 deriv(IDVDU,IQP)   =db3_n_db5_0;
 deriv(IDZDU,IV)    =db4_n_db1_0;
 deriv(IDZDU,IZ)    =db4_n_db2_0;
 deriv(IDZDU,IDVDU) =db4_n_db3_0;
 deriv(IDZDU,IDZDU) =db4_n_db4_0;
 deriv(IDZDU,IQP)   =db4_n_db5_0;
 deriv(IQP,IV)      =db5_n_db1_0;
 deriv(IQP,IZ)      =db5_n_db2_0;
 deriv(IQP,IDVDU)   =db5_n_db3_0;
 deriv(IQP,IDZDU)   =db5_n_db4_0;
 deriv(IQP,IQP)     =db5_n_db5_0;

 return pstat;

}

namespace {

PropStat _zero_bfield( VTrack& trv,const SurfXYPlane& sxyp1,
                       const SurfXYPlane& sxyp2,Propagator::PropDir dir1,
                      TrackDerivative* pderiv) {
  PropStat pstat;
  Propagator::PropDir dir(dir1);
  bool move = Propagator::reduce_direction(dir);
  bool same = sxyp2.pure_equal(sxyp1);

  // There is only one solution. Can't XXX_MOVE
  if ( same && move ) return pstat;

  if ( same ) {
    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;
  }

  TrackVector vec = trv.get_vector();
  double v0 = vec(IV);
  double z0 = vec(IZ);
  double dvdu0 = vec(IDVDU);
  double dzdu0 = vec(IDZDU);

  double du0 =1.;
  if( trv.is_backward() ) du0 = -1.;

  double phi0 = sxyp1.get_parameter(SurfXYPlane::NORMPHI);
  double cphi0 = cos(phi0);
  double sphi0 = sin(phi0);
  double u0 = sxyp1.get_parameter(SurfXYPlane::DISTNORM);

  double phi1 = sxyp2.get_parameter(SurfXYPlane::NORMPHI);
  double cphi1 = cos(phi1);
  double sphi1 = sin(phi1);
  double u1 = sxyp2.get_parameter(SurfXYPlane::DISTNORM);

  double a = (cphi0 - dvdu0*sphi0)*du0;
  double b = (sphi0 + dvdu0*cphi0)*du0;
  double c = dzdu0*du0;

  double x0 = u0*cphi0 - v0*sphi0;
  double y0 = u0*sphi0 + v0*cphi0;

  double ap = u1*cphi1;
  double bp = u1*sphi1;
  double cp = 0;

  double xp = ap;
  double yp = bp;
  double zp = 0.0;

  double denom = a*ap + b*bp + c*cp;

  if( denom == 0. ) return pstat;

  double S = ( (xp-x0)*ap + (yp-y0)*bp + (zp-z0)*cp )/denom;

  double x1 = x0 + S*a;
  double y1 = y0 + S*b;
  double z1 = z0 + S*c;

  bool forward = S > 0. ? true : false;

  if( dir == Propagator::FORWARD && !forward ) return pstat;
  if( dir == Propagator::BACKWARD && forward ) return pstat;

  double v1 = y1*cphi1 - x1*sphi1;

  double v01 = y0*cphi1 - x0*sphi1;
  double u01 = y0*sphi1 + x0*cphi1;
  double z01 = z0;

  if( u01 == u1 ) return pstat;

  double dvdu1 = (v1-v01)/(u1-u01);
  double dzdu1 = (z1-z01)/(u1-u01);

  vec(IV) = v1;
  vec(IZ) = z1;
  vec(IDVDU) = dvdu1;
  vec(IDZDU) = dzdu1;
 
  trv.set_surface(SurfacePtr(sxyp2.new_pure_surface()));
  trv.set_vector(vec);
  if( b*sphi1 + a*cphi1 >0 ) trv.set_forward();
  else trv.set_backward();

  double ds = S*sqrt(a*a+b*b+c*c);

  pstat.set_path_distance(ds);

  if( pderiv == 0 ) return pstat;

  double dx0dv0 = -sphi0;
  double dy0dv0 = cphi0;

  double dadv_du = -sphi0*du0; 
  double dbdv_du = cphi0*du0; 
  double dcdz_du = du0; 

  double ddenomdv_du = dadv_du*ap + dbdv_du*bp;
  double ddenomdz_du = dcdz_du*cp;
  

  double dSdv0 = -(dx0dv0*ap + dy0dv0*bp)/denom;
  double dSdz0 = -cp/denom;
  double dSdv_du = -S/denom*ddenomdv_du;
  double dSdz_du = -S/denom*ddenomdz_du;

  double dy1dv0 = dy0dv0 + dSdv0*b;
  double dx1dv0 = dx0dv0 + dSdv0*a;
  double dx1dv_du = dSdv_du*a + S*dadv_du;
  double dy1dv_du = dSdv_du*b + S*dbdv_du;
  
  double du01dv0 = dy0dv0*sphi1 + dx0dv0*cphi1;
  double dv01dv0 = dy0dv0*cphi1 - dx0dv0*sphi1;

  double dv1dv0 = dy1dv0*cphi1 - dx1dv0*sphi1;
  double dv1dv_du = dy1dv_du*cphi1 - dx1dv_du*sphi1;

  double dz1dz0 = 1 + dSdz0*c;
  double dz1dv0 = dSdv0*c;
  double dz1dv_du = dSdv_du*c;
  double dz1dz_du = dSdz_du*c + S*dcdz_du;

  double dv_du1dv0 = ((dv1dv0-dv01dv0)*(u1-u01) + du01dv0*(v1-v01))/(u1-u01)/(u1-u01);
  double dv_du1dv_du = dv1dv_du/(u1-u01);

  double dz_du1dv0 = (dz1dv0*(u1-u01) + du01dv0*(z1-z01))/(u1-u01)/(u1-u01);
  double dz_du1dz0 = (dz1dz0 - 1.)/(u1-u01);
  double dz_du1dv_du = dz1dv_du/(u1-u01);
  double dz_du1dz_du = dz1dz_du/(u1-u01);

  TrackDerivative& deriv = *pderiv;
  deriv.fill( 0.0 );
  deriv(IV,IV) = dv1dv0;
  deriv(IV,IDVDU) = dv1dv_du;

  deriv(IZ,IV) = dz1dv0;
  deriv(IZ,IZ) = dz1dz0;
  deriv(IZ,IDVDU) = dz1dv_du;
  deriv(IZ,IDZDU) = dz1dz_du;

  deriv(IDVDU,IV) = dv_du1dv0;
  deriv(IDVDU,IDVDU) = dv_du1dv_du;
  
  deriv(IDZDU,IV) = dz_du1dv0;
  deriv(IDZDU,IZ) = dz_du1dz0;
  deriv(IDZDU,IDVDU) = dz_du1dv_du;
  deriv(IDZDU,IDZDU) = dz_du1dz_du; 

  deriv(IQP,IQP) = 1.0;

  return pstat;

 }
}

//************************************************************************
// PropXYXY member functions.
//************************************************************************

// output stream

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

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

// Constructor.

PropXYXY::PropXYXY(ObjDoublePtr bfield) : _bfield(bfield) { 
}

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

// Destructor.

PropXYXY::~PropXYXY() { }

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

// Clone.

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


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

// propagate a track with error in the specified direction
PropStat PropXYXY::vec_dir_prop( VTrack& trv, const Surface& srf,
				 PropDir dir,TrackDerivative* pder) const {
  double bfac = -BFAC * (*_bfield)();
  PropStat pstat = vec_propagatexyxy_( bfac, trv, srf, dir, pder );
  return pstat;
}

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

// return the magnetic field

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

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

// Return the creator.

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

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

// Write the object data.

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

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