// AddFitXYPlane_Z2_XY2.cpp

#include <iostream>
#include "AddFitXYPlane_Z2_XY2.h"
#include "trffit/HTrack.h"
#include "trfxyp/HitXYPlane2.h"
#include "trfzp/HitZPlane2.h"
#include "trfutil/TRFMath.h"
#include "trfutil/TupleManager.h"
#include "trfutil/EventID.h"
#include "trfbase/Propagator.h"
#include "trfbase/PropStat.h"
#include "trfutil/trfstream.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfutil/RootFindLinear.h"
#include "trfutil/Circle.h"

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

using namespace trfutil;
using namespace trf;
using std::endl;
using std::cerr;

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

//**********************************************************************
// Static data.
//**********************************************************************

bool AddFitXYPlane_Z2_XY2::_ltuple = false;

char* AddFitXYPlane_Z2_XY2::_tuple_name = "AddFitXYPlane_Z2_XY2";

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "AddFitXYPlane_Z2_XY2" );

  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 << "AddFitXYPlane_Z2_XY2: can't get bfield parameter." << endl;
    abort();
  }
  ObjDoublePtr ptmin;
  dtype = data.get_type("ptmin");
  if(dtype == ObjData::DOUBLE)
    ptmin = new ObjDouble(data.get_double("ptmin"));
  else if(dtype == ObjData::SHARE_PTR)
    ptmin.assign_with_dynamic_cast(data.get_share_pointer("ptmin"));
  else {
    cerr << "AddFitXYPlane_Z2_XY2: can't get ptmin parameter." << endl;
    abort();
  }
  ObjDoublePtr vx(new ObjDouble(0.));
  if( data.has("vx") ) {
    dtype = data.get_type("vx");
    if(dtype == ObjData::DOUBLE)
      vx = new ObjDouble(data.get_double("vx"));
    else if(dtype == ObjData::SHARE_PTR)
      vx.assign_with_dynamic_cast(data.get_share_pointer("vx"));
    else {
      cerr << "ClusterFilterDCA: can't get vx parameter." << endl;
      abort();
    }
  }

  ObjDoublePtr vy(new ObjDouble(0.));
  if( data.has("vy") ) {
    dtype = data.get_type("vy");
    if(dtype == ObjData::DOUBLE)
      vy = new ObjDouble(data.get_double("vy"));
    else if(dtype == ObjData::SHARE_PTR)
      vy.assign_with_dynamic_cast(data.get_share_pointer("vy"));
    else {
      cerr << "ClusterFilterDCA: can't get vy parameter." << endl;
      abort();
    }
  }

  PropagatorPtr prop;
  data.get_share_pointer("propagator",prop);
  SurfacePtr psrf;
  data.get_share_pointer("surface",psrf);
  AddFitterPtr pfit;
  data.get_share_pointer("fitter",pfit);
  std::vector<double> error;
  data.get_double_list("error",error);
  assert(error.size()==15);
  TrackError err;
  int n=0;
  for(int i=0;i<5;++i)
    for(int j=0;j<=i;++j)
      err(i,j)=error[n++];
  return ObjPtr( new AddFitXYPlane_Z2_XY2(bfield,pfit,prop,psrf,err,(*ptmin)(),(*vx)(),(*vy)()) );
}

}

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

namespace {
class RootFindCurv : public RootFindLinear {
private:
  double _c1;
  double _c2;
  double _dphi;
public:
  RootFindCurv(double c1, double c2, double dphi)
  : _c1(c1), _c2(c2), _dphi(dphi) { };
  StatusDouble evaluate(double x) const {
    double xfail = 9999.99;
    double arg1 = _c1*x;
    double arg2 = _c2*x;
    if ( abs(arg1) > 1.0 ) return StatusDouble(1,xfail);
    if ( abs(arg2) > 1.0 ) return StatusDouble(2,xfail);
    double val = asin(arg1) - asin(arg2) - _dphi;
    return StatusDouble(0,val);
  };
};
}

//**********************************************************************
// Methods.
//**********************************************************************

// ouput stream

void AddFitXYPlane_Z2_XY2::ostr(std::ostream& stream) const {
  stream << begin_object ;
  stream << "Add fitter for one ZPlane(x,y) and XYPlane(v,z) measurements." 
  << new_line;
  stream << "B-field is " << get_bfield() << " Tesla." << new_line;
  stream << "Internal fitter is " << *_fit << new_line;
  stream << "Internal propagator is " << *_prop<<new_line;
  stream << "Starting surface is " << *_srf<<new_line;
  stream << "Starting error matrix is " << _err << new_line;
  stream << "Minimum pt is " << get_ptmin() << new_line;
  stream << end_object;
}

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

// fill tuple and exit
int AddFitXYPlane_Z2_XY2::fill_tuple(ReturnStatus status) const {
  if ( tuple_is_active() ) {
    _ptup->capture("return",status);
    _ptup->storeCapturedData();
    _ptup->clearData();
  }
  return status;
}

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

// Define the ntuple.

void AddFitXYPlane_Z2_XY2::define_tuple() {
  assert( _ptup == 0 );
  bool defined = TupleManager::is_tuple(_tuple_name);
  _ptup = &TupleManager::tuple(_tuple_name);
  if ( ! defined ) {
    float xdefault = -9999.99;
    _ptup->columnDirect("evid",-1);
    _ptup->columnDirect("qpt",xdefault);
    _ptup->columnDirect("return",-999);
  }
}

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

// constructor

AddFitXYPlane_Z2_XY2::AddFitXYPlane_Z2_XY2(const ObjDoublePtr& bfield,
const AddFitterPtr& fit, const PropagatorPtr& prop, const SurfacePtr& srf,
                                         const TrackError& err,double ptmin,
					 double xv,double yv)
: _bfield(bfield),_fit(fit),_prop(prop),_srf(srf),_err(err),_qpt_max(1.0/ptmin),_xv(xv),_yv(yv) {
  if ( _ltuple ) define_tuple();
}

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

// destructor

AddFitXYPlane_Z2_XY2::~AddFitXYPlane_Z2_XY2() { }

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

// add the hit

int AddFitXYPlane_Z2_XY2::add_hit(HTrack& trh, const HitPtr& phit2) const {

  // Add event number to tuple.
  if ( tuple_is_active() ) {
    _ptup->capture("evid",EventID::get_global_event_number());
  }

  // check type
  if ( phit2->get_type() != HitXYPlane2::get_static_type() )
    return fill_tuple(WRONG_TYPE);

  // Fetch track vector

  ETrack trv = trh.get_track();

  // check that new hit and track on the same surface
  if ( !phit2->get_surface().pure_equal(*trv.get_surface()) )
    return fill_tuple(DIFFERENT_SURFACES);

  // check hit count
  if ( trh.get_hits().size() != 1 ) return fill_tuple(WRONG_HIT_COUNT);

  // check chi-square
  //assert( trh.get_chi_square() == 0.0 );

  // check the type of the first hit

  const HitZPlane2* hit1 = dynamic_cast<const HitZPlane2*>(&*(trh.get_hits().front()));
  const HitXYPlane2* hit2 = dynamic_cast<const HitXYPlane2*>(&*phit2);
  if ( ! hit1 || ! hit2 )  return fill_tuple(WRONG_TYPE_EXISTING);

  bool zero_bfield = false;
  if( fabs(get_bfield()) < 1e-7 ) 
    zero_bfield = true;

  // Fetch the track vector and error.
  TrackVector vec = trv.get_vector();

  // define ZPlane indices

  int izpos  = SurfZPlane::ZPOS;

  // define XYPlane indices

  int iphi  = SurfXYPlane::NORMPHI;
  int idist = SurfXYPlane::DISTNORM;
 
  // Fetch x,y,z for the first hit.
  const Surface& srf1 = hit1->get_surface();
  const ClusZPlane2& clu1 = hit1->get_full_cluster();
  double z1 = srf1.get_parameter(izpos);
  double y1 = clu1.get_y();
  double x1 = clu1.get_x();

  // Fetch u,v,z and plane phi2  for the second (new) hit.
  const Surface& srf2 = hit2->get_surface();
  const ClusXYPlane2& clu2 = hit2->get_full_cluster();
  double phi2 = srf2.get_parameter(iphi);
  double u2   = srf2.get_parameter(idist);
  double v2   = clu2.get_v();
  double z2   = clu2.get_z();
  double r2=u2;
  double x2 =  u2*cos(phi2) - v2*sin(phi2);
  double y2 =  u2*sin(phi2) + v2*cos(phi2);

  // Calculate vector (x1,y1,z1) in coordinates of the XYPlane
  
  double cos_phi2 = cos(phi2);
  double sin_phi2 = sin(phi2);

  double u1   = x1*cos_phi2 + y1*sin_phi2;
  double v1   = y1*cos_phi2 - x1*sin_phi2;
 
  // check that u2-u1 doesn't equal to 0
  double du = u2 - u1;
  if ( du == 0. ) return fill_tuple(DU_IS_ZERO);

  // Calculate track parameters.
  // We assume track is a straight line
  // from (u1,v1,z1) to (u2,v2,z2)
  
  double dv = v2 - v1;
  double dvdu2 = dv/du;
  double dzdu2 = (z2 - z1)/du;

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

  if( !zero_bfield ) {
    // check that curvature is small enough
    /*
    double r_min = 0.5*sqrt(dv*dv + du*du);
    double pt_p = sqrt( (1.+dvdu2*dvdu2)/(1.+dvdu2*dvdu2+dzdu2*dzdu2) );
    double b5_max = fabs(pt_p/(r_min*bfac));
    if ( fabs(vec(IQP)) > b5_max ) return fill_tuple(LARGE_CURVATURE);
    */
    // check curvature and dhpi

    Circle cl(x1,y1,x2,y2,_xv,_yv);
    if( !cl.is_dphi_ok() ) return fill_tuple(DPHI_TOO_LARGE);
    if(fabs(cl.get_curvature()/bfac)>_qpt_max) return fill_tuple(DPHI_TOO_LARGE);
  }

  // Set parameters at new surface.
  vec(IV)    = v2;
  vec(IZ)    = z2;
  vec(IDVDU) = dvdu2;
  vec(IDZDU) = dzdu2;
  // vec(IQP) stays the same

  trv.set_vector(vec);
  
  // set new direction of the track assuming backward in time propagation
  (du<0.)?trv.set_forward():trv.set_backward();

  //propagate it to a starting surface
  
  PropStat pstat = _prop->vec_dir_prop(trv,*_srf,Propagator::FORWARD);
  if ( ! pstat.success() ) return fill_tuple(PROP_FAILED);

  // construct and fit a new track
  ETrack tre( trv, _err );
  trv.is_forward() ?  tre.set_forward() : tre.set_backward() ;

  double chsq = 0.0;
 
  // add hit to a list of hits

  HitList::const_iterator ihit;
  HitList hits = trh.get_hits();
  hits.push_back(phit2);

  // Loop over hits and add each in turn to the track.
  
  for ( ihit=hits.begin(); ihit!=hits.end(); ++ihit ) {
    const Surface& srf = (*ihit)->get_surface();
    PropStat pstat =  _prop->err_dir_prop(tre,srf,Propagator::BACKWARD);
    if ( ! pstat.success() ) return fill_tuple(PROP_FAILED);
    int stat = _fit->add_hit_fit(tre,chsq,*ihit);
    if ( stat )  return fill_tuple(ENCLOSED_FITTER_ERROR);
  }

  // Fit was successful: update fit and add new hit.
  chsq=0.;
  trh.add_hit(phit2);
  trh.set_fit(tre,chsq);
  return fill_tuple(OK);
}
//**********************************************************************

// Return the creator.

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

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

// Write the object data.

ObjData AddFitXYPlane_Z2_XY2::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "bfield", get_bfield());
  data.add_double( "ptmin", get_ptmin());
  data.add_double( "vx",get_vx());
  data.add_double( "vy",get_vy());
  data.add_share_pointer( "propagator", _prop );
  data.add_share_pointer( "fitter", _fit );
  data.add_share_pointer( "surface", _srf );
  std::vector<double> error;
  for(int i=0;i<5;++i)
    for(int j=0;j<=i;++j)
      error.push_back(_err(i,j));
  data.add_double_list("error",error);
  return data;
}

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