#include "AddFitCyl_Phi.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"
#include "trfutil/trfstream.h"
#include "trffit/HTrack.h"
#include "HitCylPhi.h"

using namespace trf;

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

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

namespace {

// Creator.

ObjPtr create(const ObjData& data) {
  assert( data.get_object_type() == "AddFitCyl_Phi" );
  return ObjPtr( new AddFitCyl_Phi );
}

}

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

// ouput stream

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

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

// constructor

AddFitCyl_Phi::AddFitCyl_Phi() { }

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

// destructor

AddFitCyl_Phi::~AddFitCyl_Phi() { }

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

// Return the creator.

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

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

// Write the object data.

ObjData AddFitCyl_Phi::write_data() const {
  return ObjData( get_type_name() );
}

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

// add the hit

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

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

  // check hit count
  if ( trh.get_hits().size() != 0 ) return 2;

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

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

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

  // Update vector and error with meaurement.
  // We ignore the starting phi and phi-errors.
  vec(IPHI) = phit->measured_vector()(0);
  err(IPHI,IPHI) = phit->measured_error()(0,0);
  err(IPHI,IZ)   = 0.0;
  err(IPHI,IALF) = 0.0;
  err(IPHI,ITLM) = 0.0;
  err(IPHI,IQPT) = 0.0;

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

  // fit is successful
  return 0;

}

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