// AddFitZPlane_Z2.cpp

#include "AddFitZPlane_Z2.h"
#include "trffit/HTrack.h"
#include "HitZPlane2.h"
#include "trfutil/trfstream.h"
#include "objstream/ObjData.hpp"
#include "objstream/ObjTable.hpp"

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

// Assign track parameter indices.
enum {
  IX    = SurfZPlane::IX,
  IY    = SurfZPlane::IY,
  IDXDZ = SurfZPlane::IDXDZ,
  IDYDZ = SurfZPlane::IDYDZ,
  IQP   = SurfZPlane::IQP
};

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

namespace {

// Creator.

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

  ObjDoublePtr ptmin;
  ObjData::Type 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 << "AddFitZPlane_Z2: can't get ptmin parameter." << endl;
    abort();
  }

  return ObjPtr( new AddFitZPlane_Z2((*ptmin)()) );
}

}


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

// ouput stream

void AddFitZPlane_Z2::ostr(ostream& stream) const {
  stream << begin_object ;
  stream << "Add fitter for a single zplane (x,y) measurement."<<new_line;
  stream << " with minimum pt at "<<get_ptmin()<< end_object;
}

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

// constructor

AddFitZPlane_Z2::AddFitZPlane_Z2(double ptmin)  {
  assert( ptmin > 0. );
  _qptmax2 = 1./ptmin/ptmin;
}

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

// destructor

AddFitZPlane_Z2::~AddFitZPlane_Z2() { }

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

// add the hit

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

  const HitZPlane2* hit = dynamic_cast<const HitZPlane2*>(&*phit);

  // check type
  if ( !hit ) return 1;

  const ClusZPlane2& clu = hit->get_full_cluster();

  // Fetch track vector
  ETrack tre = trh.get_track();

  // check that new hit and track on the same surface
  if ( !hit->get_surface().pure_equal(*tre.get_surface()) )
    return 2;

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

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

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

  // Fetch the track vector and error.

  TrackVector vec = tre.get_vector();
  TrackError err = tre.get_error();

  // Update vector and error with meaurement.
  // We ignore the starting (x,y) and their errors.
  double x=clu.get_x();
  double y=clu.get_y();
  double r=std::sqrt(x*x+y*y);

  double ptpz = 0.01;

  vec(IX) = x;
  vec(IY) = y;
  vec(IQP) = 0.001;

  err(IX,IX) = clu.get_dx2();
  err(IX,IY) = clu.get_dxdy();
  err(IY,IY) = clu.get_dy2();

  err(IQP,IQP)   = _qptmax2;  

  err(IX,IDXDZ) = 0.;
  err(IX,IDYDZ) = 0.;
  err(IX,IQP)   = 0.;  

  err(IY,IDXDZ) = 0.;
  err(IY,IDYDZ) = 0.;
  err(IY,IQP)   = 0.;  

  // Update track parameters.

  // find out initial direction. Abort if direction is undefined
  int flag_forward = 0;
  if(tre.is_forward()) flag_forward = 1;
  else if(tre.is_backward()) flag_forward = -1;
  else assert ( flag_forward != 0);

  int x_sign=1;
  int y_sign=1;
  if(vec(IX)<0.) x_sign=-1;
  if(vec(IY)<0.) y_sign=-1;
  vec(IDXDZ)= x/r*ptpz*flag_forward;
  vec(IDYDZ)= y/r*ptpz*flag_forward;

  tre.set_vector(vec);
  tre.set_error(err);
  (flag_forward==1)?tre.set_forward():tre.set_backward();
  trh.set_fit(tre,0.0);

  // fit is successful
  return 0;
}

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

// Return the creator.

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

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

// Write the object data.

ObjData AddFitZPlane_Z2::write_data() const {
  ObjData data( get_type_name() );
  data.add_double("ptmin",get_ptmin());
  return data;
}
//**********************************************************************