#include "TpcAlignmentHelper.h"
#include <cmath>
#include  <iostream>
#include <TMath.h>
TpcAlignmentHelper::TpcAlignmentHelper():deg2rad(TMath::Pi()/180)
{

}


//rotation phi around z, then  theta around y, then psi around x
std::vector<double> TpcAlignmentHelper::getRotationMatrixZYX(double phi, double theta, double psi,bool print){
  //Matematica
  //d=xrot.yrot.zrot
  //d//MatrixForm
//  (cos[a]*cos[b]                       cos[b]*sin[a]                       -sin[b]
//  -cos[c]*sin[a]+cos[a]*sin[b]*sin[c]  cos[a]*cos[c]+sin[a]*sin[b]*sin[c]   cos[b]*sin[c]
//   cos[a]*cos[c]*sin[b]+sin[a]*sin[c]  cos[c]*sin[a]*sin[b]-cos[a]*sin[c]   cos[b]*cos[c]
//  )
  //std::cerr<<"getRotMat"<<std::endl;
  phi*=deg2rad;
  theta*=deg2rad;
  psi*=deg2rad;
  double ca=cos(phi);
  double cb=cos(theta);
  double cc=cos(psi);
  double sa=sin(phi);
  double sb=sin(theta);
  double sc=sin(psi);
  std::vector<double> ret;
  //std::cerr<<"test1"<<std::endl;
//using the inverse matrix, as with TGeoRotation
  TMatrixD fRotMatrix(3,3);
  //std::cerr<<"test2"<<std::endl;
  fRotMatrix[0][0] = ca*cb;          //0
  //std::cerr<<"test3"<<std::endl;
  fRotMatrix[0][1] = ca*sb*sc-sa*cc; //1
  //std::cerr<<"test4"<<std::endl;
  fRotMatrix[0][2] = ca*sb*cc+sa*sc; //2
  fRotMatrix[1][0] = cb*sa;          //3
  fRotMatrix[1][1] = sa*sb*sc+ca*cc; //4
  fRotMatrix[1][2] = sa*sb*cc-ca*sc; //5
  fRotMatrix[2][0] = -sb;            //6
  fRotMatrix[2][1] = cb*sc;          //7
  fRotMatrix[2][2] = cb*cc;          //8
//  if (print){
//    fRotMatrix.Print();
//  }
  //std::cerr<<"test5"<<std::endl;
  ret.push_back(fRotMatrix[0][0]);//0
  //std::cerr<<"test6"<<std::endl;
  ret.push_back(fRotMatrix[0][1]);//1
  ret.push_back(fRotMatrix[0][2]);//2
  ret.push_back(fRotMatrix[1][0]);//3
  ret.push_back(fRotMatrix[1][1]);//4
  ret.push_back(fRotMatrix[1][2]);//5
  ret.push_back(fRotMatrix[2][0]);//6
  ret.push_back(fRotMatrix[2][1]);//7
  ret.push_back(fRotMatrix[2][2]);//8

  return ret;
}

void TpcAlignmentHelper::getAnglesZYX(TGeoRotation *rot, double &phi, double &theta, double &psi){
  //std::cout<<rot<<std::endl;
  //rot->Print();
  ////std::cerrr<<"test0"<<std::endl;


  //std::cerr<<"test1 "<<rot->IsIdentity()<<std::endl;

  if(rot==NULL||rot==0||rot->IsIdentity()){
    //std::cout<<"ret"<<std::endl;
    phi=0;
    theta=0;
    psi=0;
    return;
  }
  const Double_t* matrix=rot->GetRotationMatrix();
  if(matrix==NULL){
    //std::cout<<"ret2"<<std::endl;
    phi=0;
    theta=0;
    psi=0;
    return;
  }
  //What checks do I need?
  //std::cerrr<<"test1"<<std::endl;
  //std::cerrr<<matrix[3]<<" "<<matrix[0]<<std::endl;
  phi=atan2(matrix[3],matrix[0]);
  //std::cerrr<<"test2"<<std::endl;
  // the other program used atan2 here as well, why?
  theta=asin(-matrix[6]);
  //std::cerrr<<"test2"<<std::endl;
  psi=atan2(matrix[7],matrix[8]);
  //std::cerrr<<"test1"<<std::endl;
  phi/=deg2rad;
  theta/=deg2rad;
  psi/=deg2rad;

}
void TpcAlignmentHelper::getAnglesZYX(std::vector<double> matrix, double &phi, double &theta, double &psi){

  //What checks do I need?

  phi=atan2(matrix[3],matrix[0]);
  // the other program used atan2 here as well, why?
  //theta=asin(-matrix[6]);
  theta=atan2(-matrix[6],sqrt(matrix[7]*matrix[7]+matrix[8]*matrix[8]));
  psi=atan2(matrix[7],matrix[8]);
  phi/=deg2rad;
  theta/=deg2rad;
  psi/=deg2rad;

}

std::vector<double> TpcAlignmentHelper::getAnglesZYX(std::vector<double> mat){
    double phi;
    double theta;
    double psi;
    getAnglesZYX(mat,phi,theta,psi);
    std::vector<double> ret;
    ret.push_back(phi);
    ret.push_back(theta);
    ret.push_back(psi);
    return ret;
}
std::vector<double> TpcAlignmentHelper::getAnglesZYX(TGeoRotation* rot){
    double phi;
    double theta;
    double psi;
    getAnglesZYX(rot,phi,theta,psi);
    std::vector<double> ret;
    ret.push_back(phi);
    ret.push_back(theta);
    ret.push_back(psi);
    return ret;
}