// AddFitCyl_Phi_Phi.cpp

#include <iostream>
#include "AddFitCyl_Phi_Phi.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfutil/trfstream.h"
#include "trfutil/TRFMath.h"
#include "trfutil/TupleManager.h"
#include "trfutil/EventID.h"
#include "HitCylPhi.h"
#include "trfbase/PropStat.h"
#include "trffit/HTrack.h"
#include "PropCyl.h"

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

using namespace trf;

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "AddFitCyl_Phi_Phi" );
  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 << "AddFitCyl_Phi_Phi: can't get bfield parameter." << endl;
    abort();
  }
  double ptmin = data.get_double("ptmin");
  const AddFitter* pfit;
  data.get_bare_pointer("fitter",pfit);
  PropagatorPtr pprop;
  data.get_share_pointer("propagator",pprop);
  return ObjPtr( new AddFitCyl_Phi_Phi(bfield,*pfit,ptmin,pprop) );
}

}

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

// Assign track parameter indices.
enum {
  IPHI = SurfCylinder::IPHI,
  IZ   = SurfCylinder::IZ,
  IALF = SurfCylinder::IALF,
  ITLM = SurfCylinder::ITLM,
  IQPT = SurfCylinder::IQPT
};

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

bool AddFitCyl_Phi_Phi::_ltuple = false;

char* AddFitCyl_Phi_Phi::_tuple_name = "AddFitCyl_Phi_Phi";

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

// ouput stream

void AddFitCyl_Phi_Phi::ostr(ostream& stream) const {
  stream << begin_object;
  stream << "Add fitter for two cylinder phi measurements." << new_line;
  stream << "B-field is " << (*_bfield)() << " Tesla." << new_line;
  stream << "pT threshold is " << 1.0/_qpt_max << " GeV/c." << new_line;
  stream << "Internal fitter is " << _fit << new_line;
  if ( _pprop ) stream << "Internal propagator is " << *_pprop;
  else stream << "No internal propagator defined.";
  stream << end_object;
}

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

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

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

// Define the ntuple.

void AddFitCyl_Phi_Phi::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

AddFitCyl_Phi_Phi::AddFitCyl_Phi_Phi(ObjDoublePtr bfield,
                                     const AddFitter& fit, double ptmin,
                                     const PropagatorPtr& pprop)
: _fit(fit), _pprop(pprop), _bfield(bfield), _qpt_max(1.0/ptmin) {
  if ( _ltuple ) define_tuple();
}

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

// destructor

AddFitCyl_Phi_Phi::~AddFitCyl_Phi_Phi() { }

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

// Return the creator.

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

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

// Write the object data.

ObjData AddFitCyl_Phi_Phi::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "bfield", get_bfield() );
  data.add_double( "ptmin", get_ptmin() );
  data.add_bare_pointer( "fitter", &get_fitter() );
  data.add_share_pointer( "propagator", get_propagator() );
  return data;
}

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

// add the hit

int AddFitCyl_Phi_Phi::add_hit(HTrack& trh, const HitPtr& phit) const {

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

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

  // 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 HitPtr phit1 = trh.get_hits().front();
  if ( phit1->get_type() != HitCylPhi::get_static_type() )
    return fill_tuple(WRONG_TYPE_EXISTING);

  // Fetch the track vector and error.
  ETrack tre = trh.get_track();
  TrackVector vec = tre.get_vector();
  const TrackError& err = tre.get_error();

  // Fetch r, phi and phi-error for the first hit.
  const Surface& srf1 = phit1->get_surface();
  double r1 = srf1.get_parameter(0);
  double phi1 = phit1->measured_vector()(0);
  double ephi1 = phit1->measured_error()(0,0);

  // Fetch r, phi and phi-error for the second (new) hit.
  const Surface& srf2 = phit->get_surface();
  double r2 = srf2.get_parameter(0);
  double phi2 = phit->measured_vector()(0);
  double ephi2 = phit->measured_error()(0,0);

  // Calculate cos(lambda).
  double tlam = vec(ITLM);
  double clam = 1.0/sqrt(1.0+tlam*tlam);

  // check radii
  if ( r1 == r2 ) return fill_tuple(EQUAL_RADII);

  // Calculate track parameters.
  // We assume track from origin in uniform field
  // with alpha < pi/2; i.e. the 1st half revolution.
  // We must solve the transcendental equation
  // phi2 - phi1 = asin(r2*C/2) - asin(r1*C/2) .

  // Calculate the maximum allowed curvature and dphi for a track
  // from the origin.
  // For r2 > r1, C_max=2/r2, dphi_max = pi/2 - asin(r1/r2).
  double cmax = r1<r2 ? 1.0/r1 : 1.0/r2;
  double dphi_max = 1.0 - (r1<r2 ? r1/r2 : r2/r1);

  // If dphi is greater than the above value, exit with error.
  if ( abs(fmod2(phi2-phi1,TWOPI)) > dphi_max ) {
    return fill_tuple(DPHI_TOO_LARGE);
  }

  // Solve the equation.
  double sindphi = sin(phi2-phi1);
  double cosdphi = cos(phi2-phi1);
  double crv = (r1>r2 ? -2. : 2.) * sindphi / 
    sqrt((r1-r2)*(r1-r2) + 2.*r1*r2*sindphi*sindphi/(1.+cosdphi));
  if ( fabs(crv) > cmax ) return fill_tuple(ROOT_FIND_FAILED);
  double qpt = -crv/(BFAC*(*_bfield)());
  double alf2 = asin(0.5*r2*crv);
  double alf1 = asin(0.5*r1*crv);

  // fill tuple
  if ( tuple_is_active() ) {
    _ptup->capture("qpt",float(qpt));
  }

  if ( fabs(qpt) > _qpt_max ) return fill_tuple(PT_TOO_SMALL);

  // Check that alpha was in range for both points.
  if ( fabs(alf1) > PI2 ) return fill_tuple(ALPHA1_OUT_OF_RANGE);
  if ( fabs(alf2) > PI2 ) return fill_tuple(ALPHA2_OUT_OF_RANGE);

  // If propagator is defined, set parameters at the first surface.
  // This requires propagating back and forth.
  if ( _pprop ) {

    // Propagate back to the 1st hit.
    PropStat pstat = _pprop->err_prop( tre, srf1 );
    assert( pstat.success() );
    if ( ! pstat.success() ) return fill_tuple(UNEXPECTED_ERROR);

    // Fill track vector.
    vec = tre.get_vector();
    vec(IPHI) = phi1;
    vec(IALF) = alf1;
    vec(IQPT) = qpt;
    tre.set_vector(vec);

    // Propagate back to the new hit.
    pstat = _pprop->err_prop( tre, srf2 );
    assert( pstat.success() );
    if ( ! pstat.success() ) return fill_tuple(UNEXPECTED_ERROR);

    // Check the values.
    vec = tre.get_vector();
    double small2 = 1.e-8;
    assert( fabs(vec(IPHI) - phi2) < small2 );
    assert( fabs(vec(IALF) - alf2) < small2 );
    assert( fabs(vec(IQPT) - qpt ) < small2 );
    if ( fabs(vec(IPHI) - phi2) > small2 )
      return fill_tuple(UNEXPECTED_ERROR);
    if ( fabs(vec(IALF) - alf2) > small2 )
      return fill_tuple(UNEXPECTED_ERROR);
    if ( fabs(vec(IQPT) - qpt ) > small2 )
      return fill_tuple(UNEXPECTED_ERROR);

  // Or just set parameters at the new (second) surface.
  } else {

    // Set parameters at new surface.
    vec(IPHI) = phi2;
    vec(IALF) = alf2;
    vec(IQPT) = qpt;
    tre.set_vector(vec);

  }

  // Set the new fit.
  trh.set_fit(tre,0.0);

  // Use enclosed fitter to update error.
  int stat = _fit.add_hit(trh,phit);
  if ( stat ) return fill_tuple(ENCLOSED_FITTER_ERROR);
  return fill_tuple(OK);

}

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