// AddFitCyl_PhiZ.cpp

#include "AddFitCyl_PhiZ.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfutil/trfstream.h"
#include "trfutil/TRFMath.h"
#include "trfutil/TupleManager.h"
#include "trffit/HTrack.h"
#include "HitCylPhi.h"
#include "HitCylPhiZ.h"

using namespace trf;

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

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

bool AddFitCyl_PhiZ::_ltuple = false;

char* AddFitCyl_PhiZ::_tuple_name = "AddFitCyl_Phiz";

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "AddFitCyl_PhiZ" );
  double zmin = data.get_double("zmin");
  double zmax = data.get_double("zmax");
  return ObjPtr( new AddFitCyl_PhiZ(zmin,zmax) );
}

}

//**********************************************************************
// private methods
//**********************************************************************

// ouput stream

void AddFitCyl_PhiZ::ostr(ostream& stream) const {
  stream << begin_object;
  stream << "Add fitter for first single cylinder phi-z measurement.";
  stream << end_object;
}

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

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

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

// Define the ntuple.

void AddFitCyl_PhiZ::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("z0",xdefault);
    _ptup->columnDirect("z",xdefault);
    _ptup->columnDirect("return",-999);
  }
}

//**********************************************************************
// public methods
//**********************************************************************

// constructor

AddFitCyl_PhiZ::AddFitCyl_PhiZ(double zmin, double zmax)
: _zmin(zmin), _zmax(zmax), _zavg(0.5*(_zmin+_zmax)), _ptup(0) {
  if ( _ltuple ) define_tuple();
}

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

// destructor

AddFitCyl_PhiZ::~AddFitCyl_PhiZ() { }

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

// Return the creator.

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

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

// Write the object data.

ObjData AddFitCyl_PhiZ::write_data() const {
  ObjData data( get_type_name() );
  data.add_double( "zmin", get_zmin() );
  data.add_double( "zmax", get_zmax() );
  return data;
}

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

// add the hit

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

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

  // Check that any existing hits are of type HitCylPhi.
  const HitList hits = trh.get_hits();
  HitList::const_iterator ihit;
  for ( ihit=hits.begin(); ihit!=hits.end(); ++ihit )
    if ( (*ihit)->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();
  TrackError err = tre.get_error();

  // Fetch the stereo parameter and measurement and error.
  double stereo = phit->dhit_dtrack()(0,1);
  if ( stereo == 0.0 ) return fill_tuple(ZERO_STEREO);
  double phiz = phit->measured_vector()(0);
  double e_pz_pz = phit->measured_error()(0,0);

  // Fetch the track phi and phi error.
  double phi = vec(IPHI);
  double e_p_p = err(IPHI,IPHI);

  // Calculate z, the error in z and the phi-z error correlation.
  // phiz = phi +stereo*z
  // E_phiz_phiz = e_phi_phi + 2*stereo*E_phi_z + stereo*stereo*E_z_z
  double z0 = (phiz-phi)/stereo;
  if ( tuple_is_active() ) _ptup->capture("z0",float(z0));
  double e_z_z = e_pz_pz/(stereo*stereo);
  double e_p_z = (e_pz_pz - e_p_p - stereo*stereo*e_z_z) *0.5 /stereo;

  // Since phiz is cyclic, there are multiple solutions for z.
  // Choose the first one above _zmin.
  double z = fmod2( z0-_zavg, TWOPI/stereo ) + _zavg;
  if ( tuple_is_active() ) _ptup->capture("z",float(z));

  // If z is out of range; exit.
  if ( z < _zmin || z > _zmax ) return fill_tuple(OUT_OF_RANGE);

  // add the hit
  trh.add_hit(phit);

  // Update vector and error with meaurement.
  // We ignore the starting phi and phi-errors.
  vec(IZ) = z;
  err(IZ,IPHI) = e_p_z;
  err(IZ,IZ)   = e_z_z;
  err(IZ,IALF) = 0.0;
  err(IZ,ITLM) = 0.0;
  err(IZ,IQPT) = 0.0;

  // Update track parameters.
  tre.set_vector(vec);
  tre.set_error(err);
  trh.set_fit(tre,0.0);

  // fit is successful
  return fill_tuple(OK);

}

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