/*
 * PndTrackFromCircle.cxx
 *
 *  Created on: 10.10.2014
 *      Author: Stockmanns
 */

#include "PndTrackFromCircle.h"

#include "PndSttHit.h"
#include "FairRootManager.h"

#include "TClonesArray.h"
#include "TH2D.h"
#include "TVector2.h"
#include "TMath.h"

ClassImp(PndTrackFromCircle);

PndTrackFromCircle::PndTrackFromCircle() {
	// TODO Auto-generated constructor stub
}

PndTrackFromCircle::~PndTrackFromCircle() {
	// TODO Auto-generated destructor stub
}

PndTrack PndTrackFromCircle::CalcPndTrack(PndTrackCand cand, TVector2 circle, Double_t radius, Double_t bField) {
	FairLink first = cand.GetSortedHit(0);
	FairLink last = cand.GetSortedHit(cand.GetNHits()-1);
	FairHit* firstHit = (FairHit*)FairRootManager::Instance()->GetCloneOfLinkData(first);
	FairHit* lastHit =  (FairHit*)FairRootManager::Instance()->GetCloneOfLinkData(last);

	FairTrackParP firstPar = CalcTrackParP(*firstHit, circle, radius, bField);
	FairTrackParP lastPar  = CalcTrackParP(*lastHit, circle, radius, bField);
	PndTrack result(firstPar, lastPar, cand);
	return result;
}

FairTrackParP PndTrackFromCircle::CalcTrackParP(FairHit hit, TVector2 circleCenter, Double_t radius, Double_t bField)
{
	TVector3 hitPos;
	TVector3 hitPosError(0.1, 0.1, 0.1);
	TVector3 momError(2, 2, 2);
	TVector3 dj(1,0,0);
	TVector3 dk(0,1,0);
	TVector3 origin(0, 0, 1);

	hitPos = CalcHitPosInTrack(hit, circleCenter, radius);
	Double_t pt = CalcPt(circleCenter, radius, bField);
	TVector2 hit2D(hit.GetX(), hit.GetY());
	TVector2 ptDir = CalcPtDir(hit2D, circleCenter);
	TVector2 ptVec2D = pt * ptDir;
	TVector3 ptVec(ptVec2D.X(), ptVec2D.Y(), 0);
	Double_t q = 0;
//	if (fClockwise)
//		q = 1;
//	else
//		q = -1;
	FairTrackParP result(hitPos, ptVec, hitPosError, momError, q, origin, dj, dk);
	return result;
}

Double_t PndTrackFromCircle::CalcPt(TVector2 circleCenter, Double_t radius, Double_t bField) {
	double result = 0.3 * bField * radius * 0.01;
	std::cout << "PndTrackFromCircle::CalcPt " << result << std::endl;
	return result;
}

TVector2 PndTrackFromCircle::CalcPtDir(TVector2 hit, TVector2 circleCenter) {
	TVector2 hitDirUnit = (hit - circleCenter).Unit();
	TVector2 result;
	result = hitDirUnit.Rotate(90.0 * TMath::DegToRad());
//	if (fClockwise)
//		result = result.Rotate(180.0 * TMath::DegToRad());

	return result;
}

TVector3 PndTrackFromCircle::CalcHitPosInTrack(FairHit hit, TVector2 circleCenter, Double_t radius)
{
	Double_t s = CalcArcLength(hit, circleCenter, radius);
	TVector2 negOrigin = (-1)*circleCenter;

	Double_t startPhi = negOrigin.Phi();
	TVector2 radVec(radius, 0);
	TVector2 rotVec = radVec.Rotate(startPhi + s);
	rotVec += circleCenter;
	TVector3 result(rotVec.X(), rotVec.Y(), hit.GetZ());

	std::cout << "PndTrackFromCircle::CalcHitPosInTrack s: " << s * TMath::RadToDeg() << " startPhi: " << startPhi * TMath::RadToDeg()
		<< " RadVec: " << radVec.X() << "/" << radVec.Y() << " RotVec: " << rotVec.X() << "/" << rotVec.Y() << std::endl;

	std::cout << "PndTrackFromCircle::CalcHitPosInTrack Hit " << hit << " Circle: " << circleCenter.X() << "/" << circleCenter.Y()
			<< " s " << s << " startPhi: " << startPhi << " Result " << result.X() << "/" << result.Y() << "/" << result.Z() << std::endl;
	return result;
}

Double_t PndTrackFromCircle::CalcArcLength(FairHit hit, TVector2 circleCenter, Double_t radius){
	TVector2 hitVec(hit.GetX(), hit.GetY());
	TVector2 opCircleCenter = (-1)*circleCenter;
	TVector2 dirVec = hitVec - circleCenter;

	std::cout << "PndTrackFromCircle::CalcArcLength: Hit: " << hitVec.X() << "/" << hitVec.Y()
			<< " circleCenter " << circleCenter.X() << "/" << circleCenter.Y()
			<< " arcLength: " << TVector2::Phi_0_2pi(dirVec.DeltaPhi(opCircleCenter)) * TMath::RadToDeg() << std::endl;
	return radius * TVector2::Phi_0_2pi(dirVec.DeltaPhi(opCircleCenter));

}

Bool_t PndTrackFromCircle::Clockwise(FairHit firstHit, FairHit lastHit, FairHit intermediateHit, TVector2 circleCenter){

	Bool_t clockwise = kFALSE;  //check the orientation of your circle. Standard is counterclockwise.

	FairHit* first;
	FairHit* last;
	TVector3 firstVec, lastVec;
	firstHit.Position(firstVec);
	lastHit.Position(lastVec);
	if (firstVec.Mag() < lastVec.Mag()){
		first = &firstHit;
		last = &lastHit;
	} else {
		first = &lastHit;
		last = &firstHit;
	}

	Double_t lastArcLength = CalcArcLength(*last, circleCenter, 1);
	Double_t firstArcLength = CalcArcLength(*first, circleCenter, 1);
	Double_t intermediateLength = CalcArcLength(intermediateHit, circleCenter, 1);

	std::cout << "PndTrackFromCircle::Clockwise  First: " << firstArcLength << " Intermediate: " << intermediateLength << " last " << lastArcLength << std::endl;

	Double_t deltaLast = lastArcLength - firstArcLength;
	Double_t deltaInter = intermediateLength - firstArcLength;

	if (deltaLast < 0) {
		deltaLast += (2 * TMath::Pi());
		deltaInter += (2 * TMath::Pi());
	}
	if (deltaInter < deltaLast){
		clockwise = kTRUE;
	} else {
		clockwise = kFALSE;
	}

	return clockwise;
}