/*
 * PndSpinObsTools.cxx
 *
 *  Created on: Sep 29, 2016
 *      Author: walan603
 */

#include "PndSpinObsTools.h"

/*
 * Rotates a TLorentzvector p in the coordinate system initframe to the finalframe
 */
TLorentzVector TransformCoords(std::vector<TVector3> initframe, std::vector<TVector3> finalframe, TLorentzVector p){
	TLorentzVector result;
	TVector3 p3 = p.Vect();

	p.SetX(finalframe.at(0).Dot(initframe.at(0)) * p3.X() +
			finalframe.at(0).Dot(initframe.at(1)) * p3.Y() +
			finalframe.at(0).Dot(initframe.at(2)) * p3.Z()
			);
	p.SetY(finalframe.at(1).Dot(initframe.at(0)) * p3.X() +
			finalframe.at(1).Dot(initframe.at(1)) * p3.Y() +
			finalframe.at(1).Dot(initframe.at(2)) * p3.Z()
			);
	p.SetZ(finalframe.at(2).Dot(initframe.at(0)) * p3.X() +
			finalframe.at(2).Dot(initframe.at(1)) * p3.Y() +
			finalframe.at(2).Dot(initframe.at(2)) * p3.Z()
			);

	return p;
}

Double_t SpinWeight(std::vector<TVector3> pdecayframe, std::vector<TVector3> pbardecayframe, TLorentzVector p,
		TLorentzVector pbar, Double_t Py, Double_t Pybar, Double_t Cxx, Double_t Cyy, Double_t Czz, Double_t Cxz, Double_t Czx){

	Double_t result = 0.;

	return result;
}

Double_t SpinWeightGeneral(std::vector<TVector3> pdecayframe, std::vector<TVector3> pbardecayframe, TLorentzVector p,
			TLorentzVector pbar, Double_t Py, Double_t Pybar, TMatrixD Cij, Double_t alpha, Double_t alphabar){

	Double_t result = 1 + alphabar*Pybar*TMath::Cos(pbardecayframe.at(1).Angle(pbar.Vect())) +
						alpha*Py*TMath::Cos(pdecayframe.at(1).Angle(p.Vect()));

	for (int i = 0; i < 3; i++) {
		for (int j = 0; j < 3; j++) {
			//std::cout<<"cxx: "<<Cij(1,2)<<std::endl;
			result += alpha*alphabar*Cij(i,j) *
					TMath::Cos(pbardecayframe.at(i).Angle(pbar.Vect())) *
					TMath::Cos(pdecayframe.at(j).Angle(p.Vect()));
		}
	}

	result = result*(1/(16.*pow(TMath::Pi(),2)));

	return result;
}

TMatrixD GenerateSpinCorrTrigFuncs(Double_t cmAngle) {
	TMatrixD result(3,3);
	Double_t sin =  TMath::Sin(cmAngle);

	result(0,0) = sin;
	result(1,0) = 0;
	result(2,0) = sin;
	result(0,1) = 0;
	result(1,1) = sin;
	result(2,1) = 0;
	result(0,2) = sin;
	result(1,2) = 0;
	result(2,2) = sin;

	return result;
}

//Cij fitted to Cij = A0 + A1*cos(Theta) + A2*cos(Theta)^2 + A3*cos(Theta)^3 + A4*cos(Theta)^4 + A5*cos(Theta)^5
//at p_beam = 1.637 GeV/c
TMatrixD GenerateSpinCorrData(Double_t cmAngle) {
	TMatrixD result(3,3);

	result(0,0) = 0.43484 + 1.0498*TMath::Cos(cmAngle) - 2.532*pow(TMath::Cos(cmAngle),2.) -
			1.7598*pow(TMath::Cos(cmAngle),3.) + 2.5511*pow(TMath::Cos(cmAngle),4.);
	result(1,0) = 0;
	result(2,0) = 0.56455 + 0.63164*TMath::Cos(cmAngle) - 0.9645*pow(TMath::Cos(cmAngle),2.) -
			0.64014*pow(TMath::Cos(cmAngle),3.);
	result(0,1) = 0;
	result(1,1) = 0.51782 + 0.54426*TMath::Cos(cmAngle) - 0.47872*pow(TMath::Cos(cmAngle),2.) -
			0.48097*pow(TMath::Cos(cmAngle),3.);
	result(2,1) = 0;
	result(0,2) = 0.56455 + 0.63164*TMath::Cos(cmAngle) - 0.9645*pow(TMath::Cos(cmAngle),2.) -
			0.64014*pow(TMath::Cos(cmAngle),3.);
	result(1,2) = 0;
	result(2,2) = - 0.7148 - 1.2273*TMath::Cos(cmAngle) + 0.86644*pow(TMath::Cos(cmAngle),2.) +
			4.5709*pow(TMath::Cos(cmAngle),3.) - 0.68308*pow(TMath::Cos(cmAngle),4.) -
			3.6507*pow(TMath::Cos(cmAngle),4.);

	return result;
}

Double_t GenerateSpinCorrTrigSingle(Double_t cmAngle) {
	Double_t result = TMath::Sin(cmAngle);

	return result;
}

Double_t GenerateSpinCorrDataCxx(Double_t cmAngle){
	Double_t result = 0.43484 + 1.0498*TMath::Cos(cmAngle) - 2.532*pow(TMath::Cos(cmAngle),2.) -
			1.7598*pow(TMath::Cos(cmAngle),3.) + 2.5511*pow(TMath::Cos(cmAngle),4.);

	return result;
}

Double_t GenerateSpinCorrDataCyy(Double_t cmAngle){
	Double_t result = 0.51782 + 0.54426*TMath::Cos(cmAngle) - 0.47872*pow(TMath::Cos(cmAngle),2.) -
			0.48097*pow(TMath::Cos(cmAngle),3.);

	return result;
}

Double_t GenerateSpinCorrDataCzz(Double_t cmAngle){
	Double_t result = 0. - 0.7148 - 1.2273*TMath::Cos(cmAngle) + 0.86644*pow(TMath::Cos(cmAngle),2.) +
			4.5709*pow(TMath::Cos(cmAngle),3.) - 0.68308*pow(TMath::Cos(cmAngle),4.) -
			3.6507*pow(TMath::Cos(cmAngle),4.);

	return result;
}

Double_t GenerateSpinCorrDataCxz(Double_t cmAngle){
	Double_t result = 0.56455 + 0.63164*TMath::Cos(cmAngle) - 0.9645*pow(TMath::Cos(cmAngle),2.) -
			0.64014*pow(TMath::Cos(cmAngle),3.);

	return result;
}

std::vector<TVector3> GenerateUnitVectors(TLorentzVector p_beam, TLorentzVector p_out, TLorentzVector p_ref) {
	TVector3 DecayFrameAxisZ = p_out.Vect();
	Double_t AxisZMag = DecayFrameAxisZ.Mag();
	DecayFrameAxisZ.SetX(DecayFrameAxisZ.X()/AxisZMag);
	DecayFrameAxisZ.SetY(DecayFrameAxisZ.Y()/AxisZMag);
	DecayFrameAxisZ.SetZ(DecayFrameAxisZ.Z()/AxisZMag);

	TVector3 DecayFrameAxisY  = p_beam.Vect().Cross(p_ref.Vect());//Note: Using lambar here
	Double_t AxisYMag = DecayFrameAxisY.Mag();
	DecayFrameAxisY.SetX(DecayFrameAxisY.X()/AxisYMag);
	DecayFrameAxisY.SetY(DecayFrameAxisY.Y()/AxisYMag);
	DecayFrameAxisY.SetZ(DecayFrameAxisY.Z()/AxisYMag);

	TVector3 DecayFrameAxisX  = DecayFrameAxisY.Cross(DecayFrameAxisZ);
	std::vector <TVector3> final;
	final.push_back(DecayFrameAxisX);
	final.push_back(DecayFrameAxisY);
	final.push_back(DecayFrameAxisZ);

	return final;
}