//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//      Implementation of class TpcSPHit
//      see TpcSPHit.hh for details
//
// Environment:
//      Software developed for the PANDA Detector at FAIR.
//
// Author List:
//      Sebastian Neubert    TUM            (original author)
//
//
//-----------------------------------------------------------

// Panda Headers ----------------------

// This Class' Header ------------------
#include "TpcSPHit.h"

// C/C++ Headers ----------------------
#include <assert.h>
#include <iostream>
#include <string>
#include <stdio.h>


// Collaborating Class Headers --------
#include "FairMCPoint.h"
#include "TpcCluster.h"
//#include "GeaneTrackRep.h"
#include "TpcAlignmentManager.h"
#include "TMatrixTSym.h"

//#include "LSLTrackRep.h"
#include "RKTrackRep.h"
#include "GFDetPlane.h"
#include "GFException.h"
#include "RKTrackRep.h"
#include "TpcClusterErrorScaler.h"

// Class Member definitions -----------

ClassImp(TpcSPHit)

static TMatrixD HMatrix(2,5);

TpcSPHit::~TpcSPHit()
{}

TpcSPHit::TpcSPHit()
: GFAbsSpacepointHit(3),
  _amp(0.),
  _clusterID(-1),
  _clusterBranchID(-1),
  _useParam(false)

{}

TpcSPHit::TpcSPHit(double x, double y, double z,
		double sigx, double sigy, double sigz)
: GFAbsSpacepointHit(3), _amp(0.), _clusterID(-1),
  _clusterBranchID(-5),_useParam(false)
{

	fHitCoord[0] = x;
	fHitCoord[1] = y;
	fHitCoord[2] = z;

	fHitCov[0][0] = sigx*sigx;
	fHitCov[1][1] = sigy*sigy;
	fHitCov[2][2] = sigz*sigz;

	fHitCovBackup = TMatrixD(fHitCov);
}

TpcSPHit::TpcSPHit(const TVector3& pos,
		const TVector3& sig)
: GFAbsSpacepointHit(3), _amp(0.), _clusterID(-1),
  _clusterBranchID(-4),_useParam(false)
{

	fHitCoord[0] = pos.X();
	fHitCoord[1] = pos.Y();
	fHitCoord[2] = pos.Z();

	fHitCov[0][0] = sig.X()*sig.X();
	fHitCov[1][1] = sig.Y()*sig.Y();
	fHitCov[2][2] = sig.Z()*sig.Z();

	fHitCovBackup = TMatrixD(fHitCov);
}

//for testing purposes
TpcSPHit::TpcSPHit(FairMCPoint* point)
: GFAbsSpacepointHit(3), _amp(0.), _clusterID(-1),
  _clusterBranchID(-3),_useParam(false)
{
	fHitCoord[0] = point->GetX();
	fHitCoord[1] = point->GetY();
	fHitCoord[2] = point->GetZ();
	// fixed errors on the monte carlo points
	fHitCov[0][0] = 0.5;
	fHitCov[1][1] = 0.5;
	fHitCov[2][2] = 0.5;

	fHitCovBackup = TMatrixD(fHitCov);
}

TpcSPHit::TpcSPHit(const TpcCluster* cluster, bool weightedPlaneConstruction, bool cutCov)
: GFAbsSpacepointHit(3,weightedPlaneConstruction,cutCov),
  _clusterID(cluster->index()),
  _clusterBranchID(-2),_useParam(false)
{

	TVector3 clusterpos = cluster->pos();
	//clusterpos= TpcAlignmentManager::getInstance()->localToMaster("tpc",clusterpos);
	fHitCoord[0] = clusterpos.X();
	fHitCoord[1] = clusterpos.Y();
	fHitCoord[2] = clusterpos.Z();

	if (cluster->hasCov())
	{
		fHitCov=TMatrixDSym(3,cluster->cov().GetMatrixArray());
	}
	else
	{
		TVector3 sig = cluster->sig();
		fHitCov[0][0] = sig.X()*sig.X();
		fHitCov[1][1] = sig.Y()*sig.Y();
		fHitCov[2][2] = sig.Z()*sig.Z();
	}

	fHitCovBackup = TMatrixD(fHitCov);
	//fHitCov= TpcAlignmentManager::getInstance()->localToMaster("tpc",fHitCov);
	_amp = cluster->amp();
}

TVector3 TpcSPHit::getPos(){
	return TVector3( fHitCoord[0],  fHitCoord[1], fHitCoord[2]);
}

//pseudo copy constructor to make this work with the GFRecoHitFactory
//very hackish. Can't call normal copy constructor inside constructor.
TpcSPHit::TpcSPHit(const TpcSPHit* hit) 
: GFAbsSpacepointHit(3,hit->weightedPlaneConstruction_,hit->cutCov_)
//    _amp(hit->amp()),
//    _clusterID(hit->getClusterID()),
//    _clusterBranchID(hit->getClusterBranchID())
{
	assert(hit!=NULL);
	assert(sizeof(*hit) == sizeof(TpcSPHit));  //prevent derived objects from entering
	//TODO: use typeinfo
	memcpy(this,hit,sizeof(*hit));
} 

GFAbsRecoHit* 
TpcSPHit::clone(){
	return new TpcSPHit(*this);
}

void TpcSPHit::setHitCoord(const TVectorD& coord) {
	if(coord.GetNoElements() == 3 && coord.GetNrows() == 3)
		fHitCoord = coord;
	else {
		std::string err("Invalid format of raw hit matrix");
		GFException ex(err,__LINE__,__FILE__);
		throw ex;
	}
}

void TpcSPHit::setHitCov(const TMatrixDSym& cov) {
	if(cov.GetNrows() == 3 && cov.GetNcols() == 3) {
		fHitCov = cov;
		fHitCovBackup = TMatrixD(fHitCov);
	}
	else {
		std::string err("Invalid format of covariance matrix");
		GFException ex(err,__LINE__,__FILE__);
		throw ex;
	}
}

const TMatrixD&
TpcSPHit::getHMatrix(const GFAbsTrackRep* stateVector) 
{
	assert(stateVector!=NULL);

	if (dynamic_cast<const RKTrackRep*>(stateVector) != NULL) {
		// Uses TrackParP (q/p,v',w',v,w)
		// coordinates are defined by detplane!
		//TMatrixT<double> HMatrix(2,5);

		HMatrix[0][0] = 0.;
		HMatrix[0][1] = 0.;
		HMatrix[0][2] = 0.;
		HMatrix[0][3] = 1.;
		HMatrix[0][4] = 0.;

		HMatrix[1][0] = 0.;
		HMatrix[1][1] = 0.;
		HMatrix[1][2] = 0.;
		HMatrix[1][3] = 0.;
		HMatrix[1][4] = 1.;

		return HMatrix;
	}

	// else if (dynamic_cast<const GeaneTrackRep*>(stateVector) != NULL) {
	//   // Uses TrackParP (q/p,v',w',v,w)
	//   // coordinates are defined by detplane!
	//   TMatrixT<double> HMatrix(2,5);

	//   HMatrix[0][0] = 0.;
	//   HMatrix[0][1] = 0.;
	//   HMatrix[0][2] = 0.;
	//   HMatrix[0][3] = 1.;
	//   HMatrix[0][4] = 0.;

	//   HMatrix[1][0] = 0.;
	//   HMatrix[1][1] = 0.;
	//   HMatrix[1][2] = 0.;
	//   HMatrix[1][3] = 0.;
	//   HMatrix[1][4] = 1.;
	//   return HMatrix;
	// }

	// else if (dynamic_cast<const LSLTrackRep*>(stateVector) != NULL) {
	//   // Uses TrackParP (u,v,u',v',q/p)
	//   // coordinates are defined by detplane!
	//   TMatrixT<double> HMatrix(2,5);
	//   HMatrix[0][0] = 1.;
	//   HMatrix[0][1] = 0.;
	//   HMatrix[0][2] = 0.;
	//   HMatrix[0][3] = 0.;
	//   HMatrix[0][4] = 0.;

	//   HMatrix[1][0] = 0.;
	//   HMatrix[1][1] = 1.;
	//   HMatrix[1][2] = 0.;
	//   HMatrix[1][3] = 0.;
	//   HMatrix[1][4] = 0.;
	//   return HMatrix;
	// }
	else {
		std::cerr << "TpcSPHit can only handle state"
				<< " vectors of type GeaneTrackRep or LSLTrackRep-> abort"
				<< std::endl;
		throw;
	}

}

void TpcSPHit::getMeasurement(const GFAbsTrackRep* rep,
		const GFDetPlane& pl,
		const TVectorD& statePred,
		const TMatrixDSym& covPred,
		TVectorD& m,
		TMatrixDSym& V)
{

	if (not _useParam)
	{
		GFAbsSpacepointHit::getMeasurement(rep,pl,statePred,covPred,m,V);
		return;
	}

	static_cast<void>(rep);
	static_cast<void>(statePred);
	static_cast<void>(covPred);

	const TVector3& o(pl.getO());
	const TVector3& u(pl.getU());
	const TVector3& v(pl.getV());

	// m
	m.ResizeTo(2);


	m(0) = (fHitCoord(0)-o.X()) * u.X() +
		   (fHitCoord(1)-o.Y()) * u.Y() +
		   (fHitCoord(2)-o.Z()) * u.Z();

	m(1) = (fHitCoord(0)-o.X()) * v.X() +
		   (fHitCoord(1)-o.Y()) * v.Y() +
		   (fHitCoord(2)-o.Z()) * v.Z();

	TMatrixD jac(3,2);

	// jac = dF_i/dx_j = s_unitvec * t_untivec, with s=u,v and t=x,y,z
	jac(0,0) = u.X();
	jac(1,0) = u.Y();
	jac(2,0) = u.Z();
	jac(0,1) = v.X();
	jac(1,1) = v.Y();
	jac(2,1) = v.Z();

	V.ResizeTo(2,2);
	V=TpcClusterErrorScaler::getInstance()->getErrorCovOnPlane(_clusterID,jac);

}