// PropJoinCyl.cpp

#include "PropJoinCyl.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfutil/trfstream.h"
#include "trfbase/PropStat.h"
#include "SurfCylinder.h"
#include "SurfCylinderParameters.h"
#include "trfbase/VTrack.h"
#include "trfbase/ETrack.h"
#include "spacegeom/SpacePoint.h"
#include <cassert>

using trf::TrackDerivative;
using trf::Surface;
using trf::VTrack;
using trf::ETrack;
using trf::PropStat;
using trf::Propagator;
using trf::PropagatorPtr;
using trf::SurfCylinder;
using trf::PropJoinCyl;

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "PropJoinCyl" );
  double rmin = data.get_double("rmin");
  double rfac = data.get_double("rfac");
  PropagatorPtr pprop1;
  data.get_share_pointer("prop1", pprop1);
  PropagatorPtr pprop2;
  data.get_share_pointer("prop2", pprop2);
  return ObjPtr( new PropJoinCyl(rmin,rfac,pprop1,pprop2) );
}

}

//************************************************************************
// Member functions.
//************************************************************************

// output stream

void PropJoinCyl::ostr( ostream& stream ) const { 
  stream << begin_object;
  stream << "Propagator using intermediate cylinder." << new_line;
  stream << "rmin = " << _rmin << "; rfac = " << _rfac << new_line;
  stream << "First propagator: " << *_pprop1 << new_line;;
  stream << "Second propagator: " << *_pprop2;
  stream << end_object;
}

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

// Constructor.

PropJoinCyl::
PropJoinCyl(double rmin, double rfac, const PropagatorPtr& pprop1,
                                      const PropagatorPtr& pprop2)
: _rmin(rmin), _rfac(rfac),_pprop1(pprop1), _pprop2(pprop2) { }

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

// Destructor.

PropJoinCyl::~PropJoinCyl() { }

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

// Clone.

Propagator* PropJoinCyl::new_propagator() const {
  return new PropJoinCyl(_rmin,_rfac,_pprop1, _pprop2);
}

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

// Return the creator.

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

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

// Write the object data.

ObjData PropJoinCyl::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "rmin", get_rmin() );
  data.add_double( "rfac", get_rfac() );
  data.add_share_pointer( "prop1", get_prop1() );
  data.add_share_pointer( "prop2", get_prop2() );
  return data;
}

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

// Propagate a track without error in the specified direction.

PropStat PropJoinCyl::
vec_dir_prop(VTrack& trv, const Surface& srf, PropDir dir,
             TrackDerivative* pder) const {

  // Construct intermediate derivatives.
  TrackDerivative tmpder1;
  TrackDerivative tmpder2;
  TrackDerivative* ptmpder1 = 0;
  TrackDerivative* ptmpder2 = 0;
  if ( pder != 0 ) {
    ptmpder1 = &tmpder1;
    ptmpder2 = &tmpder2;
  }

  // Propagate from starting surface to cylinder.
  double r = _rfac*trv.space_point().rxy();
  if ( r < _rmin ) r = _rmin;
  PropStat pstat = _pprop1->vec_dir_prop(trv, SurfCylinder(r),
                                         Propagator::NEAREST, ptmpder1);
  if ( ! pstat.success() ) {
    return pstat;
  }

  // Propagate from cylinder to final surface.
  pstat = _pprop2->vec_dir_prop(trv, srf, dir, ptmpder2);
  if ( ! pstat.success() ) {
    return pstat;
  }

  // Calculate the overall derivative matrix.
  if ( pder != 0 ) {
    *pder = TrackDerivative(tmpder2*tmpder1);
  }

  // Return the final status.
  return pstat;

}

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

// Propagate a track with error in the specified direction.

PropStat PropJoinCyl::
err_dir_prop(ETrack& trv, const Surface& srf, PropDir dir,
             TrackDerivative* pder) const {

  // Construct intermediate derivatives.
  TrackDerivative tmpder1;
  TrackDerivative tmpder2;
  TrackDerivative* ptmpder1 = 0;
  TrackDerivative* ptmpder2 = 0;
  if ( pder != 0 ) {
    ptmpder1 = &tmpder1;
    ptmpder2 = &tmpder2;
  }

  // Propagate from starting surface to cylinder.
  double r = _rfac*trv.space_point().rxy();
  if ( r < _rmin ) r = _rmin;
  PropStat pstat = _pprop1->err_dir_prop(trv, SurfCylinder(r),
                                         Propagator::NEAREST, ptmpder1 );
  if ( ! pstat.success() ) {
    return pstat;
  }

  // Propagate from cylinder to final surface.
  pstat = _pprop2->err_dir_prop(trv, srf, dir, ptmpder2);
  if ( ! pstat.success() ) {
    return pstat;
  }


  // Calculate the overall derivative matrix.
  if ( pder != 0 ) {
    *pder = TrackDerivative(tmpder2*tmpder1);
  }

  // Return the final status.
  return pstat;

}

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