// AddFitCyl_Phi_Phi_Phi.cpp

#include "AddFitCyl_Phi_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 "trfbase/PropStat.h"
#include "trffit/HTrack.h"
#include "PropCyl.h"
#include "HitCylPhi.h"

using std::vector;

using namespace trf;

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "AddFitCyl_Phi_Phi_Phi" );
  double nsigma = data.get_double("nsigma");
  const AddFitter* pfit;
  data.get_bare_pointer("fitter",pfit);

  AddFitCyl_Phi_Phi_Phi::FitOrder fit_order = AddFitCyl_Phi_Phi_Phi::UNCHANGED;

  if ( data.has("fit_order") ) {
    string fitorder;
    fitorder = data.get_string("fit_order");
    if( fitorder == "UNCHANGED" ) fit_order=AddFitCyl_Phi_Phi_Phi::UNCHANGED;
    else if ( fitorder == "INWARD" ) fit_order=AddFitCyl_Phi_Phi_Phi::INWARD;
    else if ( fitorder == "OUTWARD" ) fit_order=AddFitCyl_Phi_Phi_Phi::OUTWARD;
    else assert(false);
  }

  if ( fit_order != AddFitCyl_Phi_Phi_Phi::UNCHANGED ) {
    const Propagator* pprop;
    data.get_bare_pointer("propagator",pprop);
    SurfacePtr psrf;
    data.get_share_pointer("surface",psrf);
    TrackError err;
    #ifdef ObjData_supports_lists
    vector<double> vals;
    data.get_double_list("error_matrix",vals);
    assert( vals.size() == 15 );
    int ival = 0;
    for ( int j=0; j<5; ++j ) {
      for ( int i=0; i<=j; ++i ) {
	err(i,j) = vals[ival++];
      }
    }
   #endif
    return ObjPtr( new AddFitCyl_Phi_Phi_Phi(nsigma,*pfit,pprop,psrf,err,fit_order) );
  }
  

  return ObjPtr( new AddFitCyl_Phi_Phi_Phi(nsigma,*pfit) );
}

}

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

bool AddFitCyl_Phi_Phi_Phi::_ltuple = false;

char* AddFitCyl_Phi_Phi_Phi::_tuple_name = "AddFitCyl_Phi_Phi_Phi";

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

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

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

// ouput stream

void AddFitCyl_Phi_Phi_Phi::ostr(ostream& stream) const {
  stream << begin_object;
  stream << "Add fitter for three cylinder phi measurements." << new_line;
  stream << "Cutoff is " << _nsigma << " sigma." << new_line;
  stream << "Internal fitter is " << _fit;
  stream << end_object;
}

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

// fill tuple and exit
int AddFitCyl_Phi_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_Phi::define_tuple() {
  assert( _ptup == 0 );
  bool defined = TupleManager::is_tuple(_tuple_name);
  _ptup = &TupleManager::tuple(_tuple_name);
  if ( ! defined ) {
    _ptup->columnDirect("evid",-1);
    _ptup->columnDirect("chsqdiff",float(-99.99));
    _ptup->columnDirect("return",-999);
  }
}

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

// constructor

AddFitCyl_Phi_Phi_Phi::
AddFitCyl_Phi_Phi_Phi(double nsigma, const AddFitter& fit)
: _fit(fit), _nsigma(nsigma) , _psrf(0), _pprop(0) , _fit_order(UNCHANGED) {
  if ( _ltuple ) define_tuple();
}


AddFitCyl_Phi_Phi_Phi::AddFitCyl_Phi_Phi_Phi(double nsigma, const AddFitter& fit,const Propagator* prop, const SurfacePtr& psrf,const TrackError& err,int fit_order) : _fit(fit), _nsigma(nsigma) , _psrf(psrf), _pprop(prop) , _fit_order(fit_order),_err(err) {
  if ( _ltuple ) define_tuple();
  assert( _fit_order == UNCHANGED || _fit_order == INWARD || _fit_order == OUTWARD);
}

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

// destructor

AddFitCyl_Phi_Phi_Phi::~AddFitCyl_Phi_Phi_Phi() { }

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

// Return the creator.

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

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

// Write the object data.

ObjData AddFitCyl_Phi_Phi_Phi::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "nsigma", get_nsigma() );
  data.add_bare_pointer( "fitter", &get_fitter() );
  
  if( _fit_order != UNCHANGED ) {
    string fitorder="OUTWARD";
    if ( _fit_order == INWARD ) fitorder="INWARD";
 
    data.add_string("fit_order",fitorder);
    data.add_bare_pointer( "propagator", get_propagator() );
    data.add_share_pointer( "surface", get_surface() );
#ifdef ObjData_supports_lists
    const TrackError& err = get_error();
    vector<double> vals;
    for ( int j=0; j<5; ++j ) {
      for ( int i=0; i<=j; ++i ) {
	vals.push_back( err(i,j) );
      }
    }
    data.add_double_list( "error_matrix", vals );
#endif
  }

  return data;
}

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

// add the hit

int AddFitCyl_Phi_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() != 2 ) return fill_tuple(WRONG_HIT_COUNT);

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

  // check the type of the first hits
  const HitPtr phit1 = trh.get_hits().front();
  if ( phit1->get_type() != HitCylPhi::get_static_type() )
    return fill_tuple(WRONG_TYPE_FIRST);
  const HitPtr phit2 = trh.get_hits().back();
  if ( phit2->get_type() != HitCylPhi::get_static_type() )
    return fill_tuple(WRONG_TYPE_SECOND);

  // extract the radii
  double r1 = phit1->get_surface().get_parameter(IR);
  double r2 = phit2->get_surface().get_parameter(IR);
  double r3 = phit->get_surface().get_parameter(IR);

  // extract the errors in the phi-measurements
  double ephi1 = phit1->measured_error()(0,0);
  double ephi2 = phit2->measured_error()(0,0);
  double ephi3 = phit->measured_error()(0,0);

  // Fetch the difference between prediction and measurement.
  double diff = phit->difference_vector()(0);

  // Estimate the predicted error using the approximate expression
  // phi3 = r13/r12*phi2 - r23/r12*phi1
  // where rij = ri - rj
  double fac1 = (r3-r2)/(r1-r2);
  double fac2 = (r1-r3)/(r1-r2);
  double epred = fac1*fac1*ephi1 + fac2*fac2*ephi2;

  // Calculate the chi-square difference.
  double chsq_diff = diff*diff/(ephi3+epred);

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

  // If the difference is too large, exit.
  if ( chsq_diff > _nsigma*_nsigma ) return fill_tuple(CHSQ_DIFF_TOO_LARGE); 

  // Use enclosed fitter to update.
  int stat = _fit.add_hit(trh,phit);
  if ( stat ) return fill_tuple(ENCLOSED_FITTER_ERROR);
  if( _fit_order == UNCHANGED )  return fill_tuple(OK);
  
  assert( _pprop != 0 && _psrf != 0 );

  // construct and fit a new track
  const Propagator& _prop = *_pprop;
  const Surface& _srf = *_psrf;

  VTrack trv = trh.get_track();
  PropStat pstat = _prop.vec_prop(trv,_srf);
  if ( ! pstat.success() ) return 9;

  ETrack tre( SurfacePtr(_srf.new_surface()), trv.get_vector(), _err );
  tre.set_forward();
  if ( trv.is_backward() ) tre.set_backward();
  double chsq = 0.0;

  // Loop over hits and add each in turn to the track.
  int count = 0;
  HitList hits;
  HitList::const_iterator ihit;

  r1=phit1->get_surface().get_parameter(SurfCylinder::RADIUS);
  r2=phit2->get_surface().get_parameter(SurfCylinder::RADIUS);
  r3=phit->get_surface().get_parameter(SurfCylinder::RADIUS);

  double rarray[3] = {r1,r2,r3};
  HitPtr harray[3] = {phit1,phit2,phit};
  
  for(int i=0;i<3;++i)
    for(int j=i+1;j<3;++j)
      {
	if( rarray[j] < rarray[i] ) {
	  HitPtr tmp = harray[j];
	  double rtmp = rarray[j];
	  harray[j] = harray[i];
	  rarray[j] = rarray[i];
	  harray[i] = tmp;
	  rarray[i] = rtmp;
	}
      }

  if( _fit_order == OUTWARD ) {
    hits.push_back(harray[0]);
    hits.push_back(harray[1]);
    hits.push_back(harray[2]);
  }
  else {
    assert (_fit_order == INWARD );
    hits.push_back(harray[2]);
    hits.push_back(harray[1]);
    hits.push_back(harray[0]);
  }

  for ( ihit=hits.begin(); ihit!=hits.end(); ++ihit ) {
    ++count;
    const Surface& srf = (*ihit)->get_surface();
    PropStat pstat =  _prop.err_prop(tre,srf);
    if ( ! pstat.success() ) return 10+count;
    int stat = _fit.add_hit_fit(tre,chsq,*ihit);
    if ( stat ) return 100*count + stat;
  }

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

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