#include <iostream>
#include <stdio.h>
#include <TObject.h>
#include <TString.h>
#include <TMatrixD.h>
#include <TVector3.h>

#include <OrientationInfo.h>

ClassImp(OrientationInfo)


using namespace std;

OrientationInfo::OrientationInfo() : TObject(){
    a = 360/(2*TMath::Pi());
    Re = 6000000;
}

OrientationInfo::~OrientationInfo(){
}

TMatrixD OrientationInfo::QuatoECI(Float_t q0, Float_t q1, Float_t q2, Float_t q3){
    TMatrixD Pij(3,3);
    Pij(0,0) = pow(q0,2)+pow(q1,2)-pow(q2,2)-pow(q3,2);
    Pij(0,1) = /*2*(q1*q2+q0*q3);/*/ 2*(q1*q2-q0*q3);
    Pij(0,2) = /*2*(q1*q3-q0*q2);/*/ 2*(q1*q3+q0*q2);
    Pij(1,0) = /*2*(q1*q2-q0*q3);/*/ 2*(q1*q2+q0*q3);
    Pij(1,1) = pow(q0,2)-pow(q1,2)+pow(q2,2)-pow(q3,2);
    Pij(1,2) = /*2*(q2*q3+q0*q1);/*/ 2*(q2*q3-q0*q1);
    Pij(2,0) = /*2*(q1*q3+q0*q2);/*/ 2*(q1*q3-q0*q2);
    Pij(2,1) = /*2*(q2*q3-q0*q1);/*/ 2*(q2*q3+q0*q1);
    Pij(2,2) = pow(q0,2)-pow(q1,2)-pow(q2,2)+pow(q3,2);
    return Pij;
}

TMatrixD OrientationInfo::ECItoGreenwich(TMatrixD Aij, UInt_t t){
    TMatrixD Gij(3,3);
    UInt_t t1=t-t%86400;
    UInt_t t2=t1+86400;
    Double_t omg = (7.292115e-5)*a; 			// Earth rotation velosity (Around polar axis);
    Double_t d = (t1-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
    d = d/86400;
    Double_t T = d/36525;				     //Number of Julian centuries;
    Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T;  //18 <-> 6
    Double_t tr = (t1-10957*86400)%86400;
    Double_t Somg1 = (Se+49.077+omg*86400*tr/360.)*360/86400.;

    d = (t2-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
    d = d/86400;
    T = d/36525;				     //Number of Julian centuries;
    Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T;  //18 <-> 6
    tr = (t2-10957*86400)%86400;
    Double_t Somg2 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
    Somg2+=360.0;

    Double_t kk=(Somg2-Somg1)/(t2-t1);
    Double_t bb= Somg1-kk*t1;
    Double_t Somg=kk*t+bb;

    Gij(0,0) = cos(Somg/a);
    Gij(0,1) = -sin(Somg/a);
    Gij(0,2) = 0;
    Gij(1,0) = sin(Somg/a);
    Gij(1,1) = cos(Somg/a);
    Gij(1,2) = 0;
    Gij(2,0) = 0;
    Gij(2,1) = 0;
    Gij(2,2) = 1;
    Gij.Invert();
    return Gij*Aij;
}

TMatrixD OrientationInfo::GreenwichtoGEO(Double_t lat, Double_t lon, TMatrixD Aij){

    TMatrixD Gij(3,3);
    TMatrixD Fij(3,3);

    lon=(-lon)/a; lat=(-lat)/a;   // here has the same result as Gij.Invert() in ECItoGreenwich function

    Gij(0,0) = cos(lon);	// rotation around z-axis:
    Gij(0,1) = -sin(lon);
    Gij(0,2) = 0;		//	| cos(lon) 	-sin(lon)	0|
    Gij(1,0) = sin(lon);	//	| sin(lon) 	cos(lon)	0|
    Gij(1,1) = cos(lon);	//	|   0			0	1|
    Gij(1,2) = 0;
    Gij(2,0) = 0;
    Gij(2,1) = 0;
    Gij(2,2) = 1;
    
    Fij(0,0) = cos(lat);	// rotation around y-axis at angle -lat, cause rotation around y from x to z axis is negative
    Fij(0,1) = 0;		//
    Fij(0,2) = -sin(lat);	//	|cos(-lat)	0	sin(-lat)|		|cos(lat)	0	-sin(lat)|
    Fij(1,0) = 0;		//	|	0	1		0 |	==>	|	0	1		0 |
    Fij(1,1) = 1;		//	|-sin(-lat)	0	cos(-lat)|		|sin(lat)	0	cos(lat) |
    Fij(1,2) = 0;
    Fij(2,0) = sin(lat);
    Fij(2,1) = 0;
    Fij(2,2) = cos(lat);

    return Fij*(Gij*Aij);
}

TMatrixD OrientationInfo::EulertoEci(Double_t x0, Double_t y0, Double_t z0, Double_t Vx0, Double_t Vy0, Double_t Vz0, Double_t Bank, Double_t Yaw, Double_t SPitch){
//cerr.precision(12);
//cerr<<"Position:\t"<<x0<<"\t"<<y0<<"\t"<<z0<<"\tVelocity:\t"<<Vx0<<"\t"<<Vy0<<"\t"<<Vz0<<endl;
    //Sangur to Resurs transition
    TMatrixD Zij(3,3);
    Zij(0,0) = 0.0; Zij(0,1) = 0.0; Zij(0,2) = -1.0;
    Zij(1,0) = -1.0; Zij(1,1) = 0.0; Zij(1,2) = 0.0;
    Zij(2,0) = 0.0; Zij(2,1) = 1.0; Zij(2,2) = 0.0;

    //Spacecraft velosity referenca frame in Eci
    TMatrixD Aij(3,3);
    Double_t C1 = y0*Vz0 - z0*Vy0;
    Double_t C2 = z0*Vx0 - x0*Vz0;
    Double_t C3 = x0*Vy0 - y0*Vx0;
    Double_t C  = sqrt(C1*C1 + C2*C2 + C3*C3);
    Double_t V0 = sqrt(Vx0*Vx0+Vy0*Vy0 + Vz0*Vz0);
    Aij(0,0) = Vx0/V0;	Aij(0,1) = C1/C;	Aij(0,2) = (Vy0*C3-Vz0*C2)/(V0*C);
    Aij(1,0) = Vy0/V0;	Aij(1,1) = C2/C;	Aij(1,2) = (Vz0*C1-Vx0*C3)/(V0*C);
    Aij(2,0) = Vz0/V0;	Aij(2,1) = C3/C;	Aij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);

    //Elements of matrix elements described orientation of spacecraft on velocity reference frame
     Double_t u10 = tan(Bank*TMath::DegToRad())/sqrt(tan(Bank*TMath::DegToRad())*tan(Bank*TMath::DegToRad())+1);
     Double_t u11 = -sqrt((1-u10*u10))/(1+tan(Yaw*TMath::DegToRad())*tan(Yaw*TMath::DegToRad()));
     Double_t u12 = u11*tan(Yaw*TMath::DegToRad());
     Double_t u20 = -sqrt((1-u10*u10)/(1+tan(SPitch*TMath::DegToRad())*tan(SPitch*TMath::DegToRad())));
     Double_t u00 = -u20*tan(SPitch*TMath::DegToRad());

     Double_t ab = 1+(u20*u20/(u00*u00));
     Double_t by = 2*u10*u11*u20/(u00*u00);
     Double_t cy = (1+u10*u10/(u00*u00))*u11*u11-1;
     Double_t bz = 2*u10*u12*u20/(u00*u00);
     Double_t cz = (1+u10*u10/(u00*u00))*u12*u12-1;

    Int_t uj = TMath::Sign(1.,Yaw)*TMath::Sign(1.,SPitch);
     //long double by_l = by;
     Double_t Ds = by*by-4*ab*cy;
     if(Ds<0) Ds = 0.;
     Double_t u21 = (-by+uj*sqrt(Ds))/(2*ab);
     Double_t u21s = -TMath::Sign(1.,Bank)*TMath::Abs(u21);
     Double_t u01 = TMath::Sign(1.,Yaw)*TMath::Abs((u10*u11+u20*u21)/u00);

    Int_t fj=1;
    if(TMath::Sign(1.,SPitch)>0 && TMath::Sign(1.,Yaw)>0) fj=-1;

     Double_t u22 = (-bz+fj*sqrt(bz*bz-4*ab*cz))/(2*ab);
     Double_t u22s = -TMath::Sign(1.,SPitch)*TMath::Abs(u22);
     Double_t u02 = -TMath::Abs((u10*u12+u20*u22)/u00);

    TMatrixD Dij(3,3);
    Dij(0,0) = u00;  Dij(0,1) = u01;  Dij(0,2) = u02;
    Dij(1,0) = u10;  Dij(1,1) = u11;  Dij(1,2) = u12;
    Dij(2,0) = u20;  Dij(2,1) = u21s;  Dij(2,2) = u22s;

    TMatrixD Shij(3,3);
    TMatrixD Usij(3,3);
    Usij = (Aij*Dij);
    Usij.Invert();
    Shij = Zij*Usij;
    Shij.Invert();

    return Shij;
}

TMatrixD OrientationInfo::ECItoGEO(TMatrixD Aij, UInt_t t, Double_t lat, Double_t lon){
    TMatrixD Gij(3,3);
    UInt_t t1=t-t%86400;
    UInt_t t2=t1+86400;
    Double_t omg = (7.292115e-5)*a; 			// Earth rotation velosity (Around polar axis);
    Double_t d = (t1-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
    d = d/86400;
    Double_t T = d/36525;				     //Number of Julian centuries;
    Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T;  //18 <-> 6
    Double_t tr = (t1-10957*86400)%86400;
    Double_t Somg1 = (Se+49.077+omg*86400*tr/360.)*360/86400.;

    d = (t2-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
    d = d/86400;
    T = d/36525;				     //Number of Julian centuries;
    Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T;  //18 <-> 6
    tr = (t2-10957*86400)%86400;
    Double_t Somg2 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
    Somg2+=360.0;

    Double_t kk=(Somg2-Somg1)/(t2-t1);
    Double_t bb= Somg1-kk*t1;
    Double_t Somg=kk*t+bb;

    lon=(-lon)/a; lat=(-lat)/a;

    Gij(0,0)=cos(lat)*cos(lon)*cos(Somg/a)+cos(lat)*sin(lon)*sin(Somg/a);
    Gij(0,1)=cos(lat)*cos(lon)*sin(Somg/a)-cos(lat)*sin(lon)*cos(Somg/a);
    Gij(0,2)=-sin(lat);
    Gij(1,0)=sin(lon)*cos(Somg/a)-cos(lon)*sin(Somg/a);
    Gij(1,1)=sin(lon)*sin(Somg/a)+cos(lon)*cos(Somg/a);
    Gij(1,2)=0;
    Gij(2,0)=sin(lat)*cos(lon)*cos(Somg/a)+sin(lat)*sin(lon)*sin(Somg/a);
    Gij(2,1)=sin(lat)*cos(lon)*sin(Somg/a)-sin(lat)*sin(lon)*cos(Somg/a);
    Gij(2,2)=cos(lat);

    TMatrixD Tij=Gij*Aij;

    return Tij;
}

TMatrixD OrientationInfo::GEOtoECI(TMatrixD Aij, UInt_t t, Double_t lat, Double_t lon){
   TMatrixD Gij(3,3);
    UInt_t t1=t-t%86400;
    UInt_t t2=t1+86400;
    Double_t omg = (7.292115e-5)*a; 			// Earth rotation velosity (Around polar axis);
    Double_t d = (t1-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
    d = d/86400;
    Double_t T = d/36525;				     //Number of Julian centuries;
    Double_t Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T;  //18 <-> 6
    Double_t tr = (t1-10957*86400)%86400;
    Double_t Somg1 = (Se+49.077+omg*86400*tr/360.)*360/86400.;

    d = (t2-10957*86400-43200); 		     //Number of day, passing from 01/01/2000 12:00:00 to t;
    d = d/86400;
    T = d/36525;				     //Number of Julian centuries;
    Se = 6*3600+41*60+236.555367908*d+0.093104*T*T-(6.2e-6)*T*T*T;  //18 <-> 6
    tr = (t2-10957*86400)%86400;
    Double_t Somg2 = (Se+49.077+omg*86400*tr/360.)*360/86400.;
    Somg2+=360.0;

    Double_t kk=(Somg2-Somg1)/(t2-t1);
    Double_t bb= Somg1-kk*t1;
    Double_t Somg=kk*t+bb;

   lon=(-lon)/a; lat=(-lat)/a;

   Gij(0,0)=cos(lat)*cos(lon)*cos(Somg/a)+cos(lat)*sin(lon)*sin(Somg/a);
   Gij(1,0)=cos(lat)*cos(lon)*sin(Somg/a)-cos(lat)*sin(lon)*cos(Somg/a);
   Gij(2,0)=-sin(lat);
   Gij(0,1)=sin(lon)*cos(Somg/a)-cos(lon)*sin(Somg/a);
   Gij(1,1)=sin(lon)*sin(Somg/a)+cos(lon)*cos(Somg/a);
   Gij(2,1)=0;
   Gij(0,2)=sin(lat)*cos(lon)*cos(Somg/a)+sin(lat)*sin(lon)*sin(Somg/a);
   Gij(1,2)=sin(lat)*cos(lon)*sin(Somg/a)-sin(lat)*sin(lon)*cos(Somg/a);
   Gij(2,2)=cos(lat);

   return Gij*Aij;
}


TMatrixD OrientationInfo::GEOtoGeomag(TMatrixD Aij,Double_t Bnorth, Double_t Beast, Double_t Bup){	//Geomagnetic geodetic reference frame
	Double_t alpha = 0;
	if(Beast==0. && Bnorth>0) alpha = 0; else
	if(Beast==0. && Bnorth<0) alpha = 180.; else{
		if(Beast > 0) alpha = TMath::ATan(Bnorth/Beast)*TMath::RadToDeg() - 90.;
		if(Beast < 0) alpha = TMath::ATan(Bnorth/Beast)*TMath::RadToDeg() + 90.;
	}
	alpha = alpha*TMath::DegToRad();
	Double_t beta = TMath::ATan(Bup/sqrt(pow(Bnorth,2)+pow(Beast,2)));
	//if(Bup<0.0) beta = TMath::ATan(TMath::Abs(Bup/sqrt(pow(Bnorth,2)+pow(Beast,2))));
	//if(Bup>0.0) beta = TMath::ATan(TMath::Abs(sqrt(pow(Bnorth,2)+pow(Beast,2))/Bup));
	//cout<<"GEOtomag:alpha = "<<alpha*TMath::RadToDeg()<<"\tbeta = "<<beta*TMath::RadToDeg()<<endl;
	TMatrixD Gij(3,3);
    	TMatrixD Fij(3,3);
	Gij(0,0) = 1;			//rotation around x-axis at angle alpha
	Gij(0,1) = 0;
	Gij(0,2) = 0;			//	|1		0		0	|
	Gij(1,0) = 0;			//	|0	cos(alpha)	-sin(alpha)	|
	Gij(1,1) = cos(alpha);	//	|0	sin(alpha)	cos(alpha)	|
	Gij(1,2) = -sin(alpha);
	Gij(2,0) = 0;
	Gij(2,1) = sin(alpha);
	Gij(2,2) = cos(alpha);
	Gij.Invert();
	Fij(0,0) = cos(beta);	//rotation around y-axis at angle beta
	Fij(0,1) = 0;
    	Fij(0,2) = sin(beta);	//	|cos(beta)	0	sin(beta)|
    	Fij(1,0) = 0;			//	|	0	1		0 |
    	Fij(1,1) = 1;			//	|-sin(beta)	0	cos(beta)|
    	Fij(1,2) = 0;
    	Fij(2,0) = -sin(beta);
    	Fij(2,1) = 0;
    	Fij(2,2) = cos(beta);
	Fij.Invert();
	//Int_t tri;
	//cin >> tri;
	return Fij*(Gij*Aij);
}

TMatrixD OrientationInfo::PamelatoGEO(TMatrixD Aij, Double_t B1, Double_t B2, Double_t B3){
    //TMatrixD Gij(3,3);
    TMatrixD Hij(3,1);
    TMatrixD Bij(3,1);
    Bij(0,0) = B1;
    Bij(1,0) = B2;
    Bij(2,0) = B3;
    Hij=Aij*Bij;
    return Hij;
}

TMatrixD OrientationInfo::ColPermutation(TMatrixD Aij){
    TMatrixD Gij(3,3);
    Gij(0,0) = 1; Gij(0,1) = 0; Gij(0,2) = 0;
    Gij(1,0) = 0; Gij(1,1) = 0; Gij(1,2) = 1;
    Gij(2,0) = 0; Gij(2,1) = -1; Gij(2,2) = 0;
    return Aij*Gij;
}

TVector3 OrientationInfo::GetSunPosition(UInt_t atime){
  TVector3 sunout;
  Float_t JD=atime/86400.+2440587.5;
//SAV
//  cout << "JD = " << JD <<endl;
//SAV
 //test June 1997 JD=2451545.0-877.047;
  Float_t Tm = (JD - 2451545.0)/36525.;
  Float_t Mo = (357.52910+35999.05030*Tm-0.0001559*Tm*Tm-0.00000048*Tm*Tm*Tm);
//SAV
//  cout<<"Tm = " << Tm << "Mo = " << Mo <<endl;
//SAV
  Mo=Mo*TMath::DegToRad();
 
  Float_t Co = ((1.914600 - 0.004817*Tm - 0.00014*Tm*Tm)*sin(Mo) + (0.019993 - 0.000101*Tm)* sin(2.*Mo) + 0.000290* sin(3.*Mo));
  Co=Co* TMath::DegToRad();
  
  Float_t Lo = (280.46645 + 36000.76983*Tm +0.0003032*Tm*Tm);
  Lo=Lo*TMath::DegToRad();
  
  Float_t theta = (Lo + Co); // * TMath::DegToRad();
   
  Float_t eps = (23.+26./60.+21.448/3600. - 46.8150/3600.*Tm - 0.00059/3600.*Tm*Tm + 0.001813*Tm*Tm*Tm)*TMath::DegToRad();

//SAV
//  cout << "Co = " << Co*TMath::RadToDeg() << "\tLo = " << Lo*TMath::RadToDeg() << "\ttheta = " << theta << "\teps = " << eps << endl;
//SAV

  Float_t YY=cos(eps)*sin(theta);
  Float_t XX=cos(theta);
//SAV
//  cout << "XX = " << XX << "\tYY" << YY << endl;
//SAV
  Float_t RASun=atan(YY/XX);
  if(XX<0. ) RASun=RASun+TMath::Pi();
  if(XX >0. && YY <0.) RASun=RASun+2*TMath::Pi();
  Float_t DESun = asin(sin(eps)*sin(theta));
//SAV
//  cout << "DE = " << DESun << "\t" << RASun << endl;
//SAV
  sunout.SetMagThetaPhi(1.0,TMath::Pi()/2.-DESun,RASun);
  return sunout;
}

Float_t OrientationInfo::Larmor(Float_t Ek,Float_t Bm,Int_t iZ,Float_t xA){  //Ek in MeV, Bm in nT, Pitch-angle, rad
  Float_t mp = 938.272029;//	Float_t amu = 931.494043e0;
    Float_t cc = 299792458.;
    Float_t ee = 1.60217653e-19;
    Float_t kg = 1.7826619e-30;
    Float_t gam = (Ek+mp)/mp;
    Float_t mm = mp*kg;
    Float_t omega = iZ*ee*Bm*1e-9/(gam*mm);
    Float_t larmor = 1e-3*sqrt(1e0-1e0/pow(gam,2))*cc/omega;
    larmor = 1e-3*Ek*cc/omega; //Ek here is p or for onecharged particle R; larmor in m
    return larmor;
}

TMatrixD OrientationInfo::GetDirectiontoGirocenter(Float_t R, Float_t Px, Float_t Py){
    TMatrixD GirDir(3,1);
    if(R>0){
	GirDir(0,0) = Py;
	GirDir(1,0) = -Px;
    }else{
	GirDir(0,0) = -Py;
	GirDir(1,0) = Px;
    }
    GirDir(2,0) = 0.;
    return GirDir;
}

Double_t OrientationInfo::GetPitchAngle(Double_t x1, Double_t y1, Double_t z1, Double_t x2, Double_t y2, Double_t z2){
    return TMath::ACos((x1*x2 + y1*y2 + z1*z2)/(sqrt(pow(x1,2)+pow(y1,2)+pow(z1,2))*sqrt(pow(x2,2)+pow(y2,2)+pow(z2,2)))) * TMath::RadToDeg();
}
