#include "PndTpcDXCalculator.h"
#include "TVector3.h"
#include <iostream>
using namespace std;

void PndTpcDXCalculator::SetNextPoint(double x, double y, double z, double de)
{
	TVector3 *p=new TVector3(x,y,z);
	_vpos.push_back(p);
	_vde.push_back(de);
}

void PndTpcDXCalculator::GetDEDX(double &de, double &dx)
{
	if( _vpos.size()>1 )	{
		TVector3 *current_point=_vpos[_vpos.size()-1];
		TVector3 *last_point=_vpos[_vpos.size()-2];
		TVector3 VectorToCurrentPoint=(*current_point)-(*last_point);
		Double_t distance=VectorToCurrentPoint.Mag();
		dx=distance;
		de=_vde[_vde.size()-1];
		RemoveUnusedPoint();
	}
	else	{
		de=0.;
		dx=0.;
	}
}

void PndTpcDXCalculator::GetCenteredDEDX(double &de, double &dx)
{
	if( _vpos.size() > 2 )	{
		dx=CalculateDX();
		de=_vde[_vde.size()-2];	//the energyloss that belongs to the point in the middle
		RemoveUnusedPoint();
	}
	else		{
		de=0.;
		dx=0.;
	}
}

double PndTpcDXCalculator::CalculateDX()
{
	TVector3 *first_point=_vpos[_vpos.size()-3];
	TVector3 *mid_point=_vpos[_vpos.size()-2];
	TVector3 *third_point=_vpos[_vpos.size()-1];
	
	TVector3 VectorToMidPoint=(*mid_point)-(*first_point);
	Double_t distanceToMidPoint=VectorToMidPoint.Mag();
	//cout << "distanceToMidPoint: " << distanceToMidPoint << endl;
	
	TVector3 VectorFromMidPoint=(*third_point)-(*mid_point);
	Double_t distanceFromMidPoint=VectorFromMidPoint.Mag();	
	//cout << "distanceFromMidPoint: " << distanceFromMidPoint << endl;	
	
	double dx=distanceToMidPoint/2.+distanceFromMidPoint/2.;
	//cout << "dx: " << dx << endl; 
	return dx;
}

void PndTpcDXCalculator::RemoveUnusedPoint()
{
	if(_vde.size()>0)	{
		delete _vpos[0];
		_vpos[0]=NULL;
		_vpos.erase(_vpos.begin());
		_vde.erase(_vde.begin());
	}
}

std::ostream& operator<< (std::ostream& s, const PndTpcDXCalculator& me){
	if( me._vpos.size() != me._vde.size()  )	{
		s << "PndTpcDXCalculator: ERROR";
		return s; 
	}
	
	for(int i=0; i<me._vpos.size(); i++)	{
		s << "dedx=("<<me._vde[i] <<","<<(me._vpos[i])->X() <<","
									<<(me._vpos[i])->Y() <<","
									<<(me._vpos[i])->Z() <<")  ";
    }
    return s;
}