/***************************************************************************
 *   Copyright (C) 2006 by pamelaprod                                      *
 *   pamelaprod@P1.pamela                                                  *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <InclinationInfo.h>

using namespace std;

// InclinationInfoI()::InclinationInfoI() {
//   //  memset(time,0,6*sizeof(double)); 
//   // memset(quad,0,6*4*sizeof(double));
// };

void InclinationInfoI::fill(TArrayC* data){
  short extIndex = 0;
  short innIndex = 0;
  long tempData = 0;
  for (int i = 0; i < 6; i++){
    extIndex = 20*i;
    time[i] = (((data->At(extIndex) << 24) & 0xFF000000) +
	       ((data->At(extIndex + 1) << 16) & 0x00FF0000) + ((data->At(extIndex + 2) << 8) & 0x0000FF00) +
	       (data->At(extIndex + 3) & 0x000000FF))/128.0;
    for (int j = 0; j < 4; j++){
      innIndex = extIndex + 4*j;
      tempData = ((data->At(innIndex + 4) << 24) & 0xFF000000) + ((data->At(innIndex + 5) << 16) & 0x00FF0000) + ((data->At(innIndex + 6) << 8) & 0x0000FF00) + (data->At(innIndex + 7) & 0x000000FF);
      if (data->At(innIndex + 4) >> 8) {
	quat[i][j] = (~tempData * -1.0)/1073741824.0;
      } else {
	quat[i][j] = tempData / 1073741824.0;
      }
    }
  }
}

void InclinationInfoI::clear() {
   for(UInt_t i = 0; i < 6; i++){
   	time[i]=0;
 	for(UInt_t j = 0; j < 4; j++) quat[i][j]=0;
   }
return ;   
}


Quaternions::Quaternions() 
  : InclinationInfoI()
{
}


Quaternions::~Quaternions()
{
}

InclinationInfo::InclinationInfo()
  : TObject()
{
}

InclinationInfo::~InclinationInfo()
{
}

/*Sine::Sine()
  : TObject()
{
}

Sine::~Sine()
{
}*/

short int Sign_1(double_t a, Int_t b){
  if(a>0){b=1;}
  if(a<0){b=-1;}
  else{b=0;}
  return b; 
}


/******************************************************************************************************************/
/******************************************************************************************************************/
//*********************                             ***************************************************************/
//*********************     COORDINATE SYSTEMS      ***************************************************************/
//*********************                             ***************************************************************/
//*****************************************************************************************************************/
//*****************************************************************************************************************/
//
//                                  ZISK
//                                 +
//                                / \       YOSK      ZOSK (Directed by Radius)
//                                 |       _        _.
//                                 |      |\        /|
//                                 |        \      / 
//                                 |         \    /
//                                 |.__..__   \  /
//                Orbit     _._.***|        **.\/_        XOSK (Directed by velocity)
//                        .*       | (X0,Y0,Z0) **--.___|
//                     _**         |        /     *.    /
//                   .*            |       *        *
//                  *        ..****|***.. /  R       * 
//                         .*      |    .*.
//                        .*       |   /  *.
//                        * EARTH  |  /    *                                    YISK
//                        *        | /_ _  _*_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _|
//                        *        /       *                                   /
//                         *      /      .*
//                          *.   /      .*
//                            **/*******
//                             /
//                            /
//                           /
//                          /
//                         /
//                        /
//                      |/
//                      *--
//                  XISK
//
//****************************************************************************************************/
//****************************************************************************************************/

    
void InclinationInfo::TransAngle(Double_t x0, Double_t y0, Double_t z0, Double_t Vx0, Double_t Vy0, Double_t Vz0, Double_t q0, Double_t q1, Double_t q2, Double_t q3){
    
    cout.precision(12);

    double_t a = 360/(2*TMath::Pi());
    
    TMatrixD Xij(3,3);
    Xij(0,0) = 1; Xij(0,1) = 0; Xij(0,2) = 0;
    Xij(1,0) = 0; Xij(1,1) = 0; Xij(1,2) = 1;
    Xij(2,0) = 0; Xij(2,1) = -1; Xij(2,2) = 0;
    
    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;

    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);

    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(pow(C1,2) + pow(C2,2) + pow(C3,2));
    Double_t V0 = sqrt(pow(Vx0,2)+pow(Vy0,2) + pow(Vz0,2));
    //    Double_t R0 = sqrt(pow(x0,2)+pow(y0,2) + pow(z0,2));
    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);

    TMatrixD Bij(3,3);
    Bij(0,0) = Vx0/V0;
    Bij(1,0) = C1/C;
    Bij(2,0) = (Vy0*C3-Vz0*C2)/(V0*C);
    Bij(0,1) = Vy0/V0;
    Bij(1,1) = C2/C;
    Bij(2,1) = (Vz0*C1-Vx0*C3)/(V0*C);
    Bij(0,2) = Vz0/V0;
    Bij(1,2) = C3/C;
    Bij(2,2) = (Vx0*C2-Vy0*C1)/(V0*C);
/*
    cout<<"Coordinates:"<<endl;
    cout<<x0<<"\t"<<y0<<"\t"<<z0<<"\t"<<Vx0/V0<<"\t"<<Vy0/V0<<"\t"<<Vz0/V0<<endl;

    cout<<"Pij"<<endl;
    cout<<Pij(0,0)<<"\t"<<Pij(0,1)<<"\t"<<Pij(0,2)<<endl;
    cout<<Pij(1,0)<<"\t"<<Pij(1,1)<<"\t"<<Pij(1,2)<<endl;
    cout<<Pij(2,0)<<"\t"<<Pij(2,1)<<"\t"<<Pij(2,2)<<endl;
*/
    //Aij.Invert();
    
    TMatrixD Full_(3,3);
    
    Full_ = Bij*(Pij*Zij);
/*/temprary
    TMatrixD Tmp(3,3);
    Tmp=Pij*Zij;
    
    cout<<"Quaternion matrix (Tmp)"<<endl;
    cout<<Tmp(0,0)<<"\t"<<Tmp(0,1)<<"\t"<<Tmp(0,2)<<endl;
    cout<<Tmp(1,0)<<"\t"<<Tmp(1,1)<<"\t"<<Tmp(1,2)<<endl;
    cout<<Tmp(2,0)<<"\t"<<Tmp(2,1)<<"\t"<<Tmp(2,2)<<endl;

    cout<<"Orientation based on Velocity"<<endl;
    cout<<Aij(0,0)<<"\t"<<Aij(0,1)<<"\t"<<Aij(0,2)<<endl;
    cout<<Aij(1,0)<<"\t"<<Aij(1,1)<<"\t"<<Aij(1,2)<<endl;
    cout<<Aij(2,0)<<"\t"<<Aij(2,1)<<"\t"<<Aij(2,2)<<endl;

    cout<<"Satellite Axis in Velocity Reference frame (Full_):"<<endl;
    cout<<Full_(0,0)<<"\t"<<Full_(0,1)<<"\t"<<Full_(0,2)<<endl;
    cout<<Full_(1,0)<<"\t"<<Full_(1,1)<<"\t"<<Full_(1,2)<<endl;
    cout<<Full_(2,0)<<"\t"<<Full_(2,1)<<"\t"<<Full_(2,2)<<endl;
*/
    Double_t u00 = Full_(0,0);
    Double_t u10 = Full_(1,0);
    Double_t u11 = Full_(1,1);
    Double_t u20 = Full_(2,0);
    Double_t u12 = Full_(1,2);
    
    //Double_t u13 = Full_(0,0);
    //Double_t u23 = -Full_(1,0);
    //Double_t u22 = Full_(1,1);
    //Double_t u33 = Full_(2,0);
    //Double_t u21 = Full_(1,2);
    
    Tangazh = a*atan(-u00/u20);
    Kren = a*atan(u10/sqrt(1 - pow(u10,2)));
    Ryskanie = a*atan(u12/u11);

    //Tangazh = a*atan(-u13/u33);
    //Kren = a*atan(-u23/sqrt(1 - pow(u23,2)));
    //Ryskanie = a*atan(u21/u22);
// end temprary

//10RED CHECK
/*
u10 = tan(Kren*TMath::DegToRad())/sqrt(pow(tan(Kren*TMath::DegToRad()),2)+1);
u11 = -sqrt((1-pow(u10,2))/(1+pow(tan(Ryskanie*TMath::DegToRad()),2)));
u12 = u11*tan(Ryskanie*TMath::DegToRad());
u20 = -sqrt((1-pow(u10,2))/(1+pow(tan(Tangazh*TMath::DegToRad()),2)));
u00 = -u20*tan(Tangazh*TMath::DegToRad());

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

Int_t uj = TMath::Sign(1.,Ryskanie)*TMath::Sign(1.,Tangazh);
Double_t u21 = (-by+uj*sqrt(pow(by,2)-4*aa*cy))/(2*aa);
Double_t u21s = -TMath::Sign(1.,Kren)*TMath::Abs(u21);
Double_t u01 = TMath::Sign(1.,Ryskanie)*TMath::Abs((u10*u11+u20*u21)/u00);

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

Double_t u22 = (-bz+fj*sqrt(pow(bz,2)-4*aa*cz))/(2*aa);
Double_t u22s = -TMath::Sign(1.,Tangazh)*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;

//cout<<"Dij"<<endl;
//cout<<Dij(0,0)<<"\t"<<Dij(0,1)<<"\t"<<Dij(0,2)<<endl;
//cout<<Dij(1,0)<<"\t"<<Dij(1,1)<<"\t"<<Dij(1,2)<<endl;
//cout<<Dij(2,0)<<"\t"<<Dij(2,1)<<"\t"<<Dij(2,2)<<endl;

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

cout<<"Full_ matrix having got from Euler angles"<<endl;
cout<<Shij(0,0)<<"\t"<<Shij(0,1)<<"\t"<<Shij(0,2)<<endl;
cout<<Shij(1,0)<<"\t"<<Shij(1,1)<<"\t"<<Shij(1,2)<<endl;
cout<<Shij(2,0)<<"\t"<<Shij(2,1)<<"\t"<<Shij(2,2)<<endl;

    cout<<"Bank = "<<Kren<<"\tSPitch = "<<Tangazh<<"\tYaw = "<<Ryskanie<<endl;
    if(TMath::Abs(Kren)>10.0){
//    if(Vz0/V0>0.99){
      Int_t Fer;
      cin>>Fer;
    }

    Full_.Delete();
    Aij.Delete();
    Pij.Delete();
    Zij.Delete();
    Xij.Delete();
    Dij.Delete();
    Shij.Delete();
    Usij.Delete();

// END 10RED CHECK
*/
return ;    
}


void InclinationInfo::Clear(Option_t *t){
  //Int_t gyh = 0;
}


//ClassImp(McmdItem)
ClassImp(InclinationInfoI)
ClassImp(Quaternions)
ClassImp(InclinationInfo)
//ClassImp(Sine)