#include "TpcAlignmentManager.h"
#include "TpcAlignmentHelper.h"

#include <sstream>
#include <fstream>
#include <iostream>
#include <iomanip>
#include <time.h>
//#include <boost/algorithm/string/trim.hpp>

#include "TLorentzVector.h"
using namespace std;

TpcAlignmentManager* TpcAlignmentManager::inst=NULL;

TpcAlignmentManager::~TpcAlignmentManager(){
    std::map<TString,TGeoCombiTrans*>::iterator itT=fTransformations.begin();
    while(itT!=fTransformations.end()){
        delete itT->second;
        ++itT;
    }
    fTransformations.clear();
}

void TpcAlignmentManager::read(TString filename){
    clear();
    
    transformationFileName=filename;
    ifstream input(filename.Data());
    if(!input){
        cerr << "unable to open input file: "
             << filename << " --bailing out! \n";
        throw;
    }
    string line;
    while(getline(input,line)){
        istringstream lineStream(line);
        string word;
        lineStream>>word;
        //  boost::algorithm::trim(word);
        if(word=="#"||word==""){
            continue;
        }else if(word=="detName"){
            string id;
            lineStream>>id;
            readDet(id,input);
        }
    }
    if(detNames.size()==0){
        cerr << "empty alignment, terminating" <<endl;
        throw;
    }
    
    std::cout<<"TpcAlignmentManager read file: "<<filename<<" with "<<detNames.size()<<" entries"<<endl;
    
}

void TpcAlignmentManager::readDet(TString detName, ifstream &input){
    //TODO check for missing information 
    string line;
    int angleMode=1;
    TGeoTranslation* trans=NULL;
    std::vector<double> rot;
    
    while(getline(input,line)){
        istringstream lineStream(line);
        string word;
        lineStream>>word;
        //    boost::algorithm::trim(word);
        if(word=="#"||word==""){
            continue;
        }
        if(word=="detName"){
            setConv(detName, trans,rot,angleMode);
            string name;
            lineStream>>name;
            readDet(name,input);
            return;
        }
        if(word=="translation"){
            double x,y,z;      
            lineStream>>x>>y>>z;
            if(!(lineStream.good() or lineStream.eof())){
                std::cerr<<"error reading translation for det:"<<detName<<std::endl;
                throw;
            }
            trans=new TGeoTranslation(x,y,z);
        }
        if(word=="euler" or word=="euler1" or word=="euler2"){
            double phi,theta,psi;
            if(word=="euler2"){
                std::cout<<"anglemode 2"<<std::endl;
                angleMode=2;
            }
            lineStream>>phi>>theta>>psi;
            if(!(lineStream.good() or lineStream.eof())){
                std::cerr<<"error reading rotation for det:"<<detName<<std::endl;
                throw;
            }
            rot.push_back(phi);
            rot.push_back(theta);
            rot.push_back(psi);
        }
    }
    if(rot.size()!=3 or trans==NULL){
        std::cerr<<"incomplete detector definition"<<std::endl;
        throw;
    }
    setConv(detName, trans,rot,angleMode);
    return;
}
void TpcAlignmentManager::clear(){
    std::map<TString,TGeoCombiTrans*>::iterator itT=fTransformations.begin();
    while(itT!=fTransformations.end()){
        delete itT->second;
        ++itT;
    }
    fTransformations.clear();
    fAngleMode.clear();
    fAngles.clear();
    detNames.clear();
}


void TpcAlignmentManager::getPlaneDef(TString detName, 
                                      TVector3 &o,
                                      TVector3 &u, 
                                      TVector3 &v){
    
    if(fTransformations.count(detName)){
        o=TVector3(fTransformations[detName]->GetTranslation());
        TVector3 u_l(1,0,0);
        TVector3 v_l(0,1,0);
        u=localToMasterVect(detName,u_l);
        v=localToMasterVect(detName,v_l);
        return;
    }else{
        quit(detName);
    } 
}
TGeoCombiTrans* TpcAlignmentManager::getTransformation(TString detName){
    if(!initialized){
        
        return new TGeoCombiTrans(*(new TGeoTranslation(0,0,0)),*(new TGeoRotation()));
    }
    if(fTransformations.count(detName)){
        return fTransformations[detName];
    }else{
        quit(detName);
    }
}
void TpcAlignmentManager::getEulerAngles(TString detName, double &phi, double &theta, double &psi){
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            fTransformations[detName]->GetRotation()->GetAngles(phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
}
void TpcAlignmentManager::getEulerAnglesZYX(TString detName, double &phi, double &theta, double &psi){
    TpcAlignmentHelper alHelper;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            alHelper.getAnglesZYX(fTransformations[detName]->GetRotation(),phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
}

double TpcAlignmentManager::getPhi(TString detName)
{
     double phi,theta,psi;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            fTransformations[detName]->GetRotation()->GetAngles(phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
    return phi;
}

double TpcAlignmentManager::getPhiXYZ(TString detName)
{
    double phi,theta,psi;
    TpcAlignmentHelper alHelper;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            alHelper.getAnglesZYX(fTransformations[detName]->GetRotation(),phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
    return phi;
}

double TpcAlignmentManager::getTheta(TString detName)
{
     double phi,theta,psi;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            fTransformations[detName]->GetRotation()->GetAngles(phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
    return theta;
}

double TpcAlignmentManager::getThetaXYZ(TString detName)
{
    double phi,theta,psi;
    TpcAlignmentHelper alHelper;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            alHelper.getAnglesZYX(fTransformations[detName]->GetRotation(),phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
    return theta;
}

double TpcAlignmentManager::getPsi(TString detName)
{
     double phi,theta,psi;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            fTransformations[detName]->GetRotation()->GetAngles(phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
    return psi;
}

double TpcAlignmentManager::getPsiXYZ(TString detName)
{
    double phi,theta,psi;
    TpcAlignmentHelper alHelper;
    if(!initialized){
        phi=0;psi=0;theta=0;
    }
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            phi=0;psi=0;theta=0;
        }else{
            alHelper.getAnglesZYX(fTransformations[detName]->GetRotation(),phi,theta,psi);
        }
    }else if(!initialized){
        phi=0;psi=0;theta=0;
    }else {
        quit(detName);
    }
    return psi;
}

//
TVector3 TpcAlignmentManager::getTranslation(TString detName)
{
    if(fTransformations.count(detName)) {
        TGeoCombiTrans *tr=fTransformations[detName];
        TVector3 trans(tr->GetTranslation()[0],tr->GetTranslation()[1],tr->GetTranslation()[2]);
        return trans;
    }else if(!initialized){
        return TVector3(0,0,0);
    }  else quit(detName);
}

void TpcAlignmentManager::setEulerAngles(TString detName, double phi, double theta, double psi, int mode)
{
    ostringstream name;
    name<<"rotation_"<<detName;
    if(fTransformations.count(detName)){
        TGeoRotation *grot=new TGeoRotation(name.str().c_str(),phi,theta,psi);
        if(mode ==2 ){
            TpcAlignmentHelper alHelp;
            std::vector<double> rotMatrix=alHelp.getRotationMatrixZYX(phi,theta,psi);
            grot->SetMatrix(rotMatrix.data());
            fAngleMode[detName]=2;
        }
        fTransformations[detName]->SetRotation(grot);
        
        if(fAngles.count(detName)){
            if(fAngles[detName].size()==3){
                fAngles[detName][0]=phi;
                fAngles[detName][1]=theta;
                fAngles[detName][2]=psi;
            }
        }
        delete grot;
    }
    else{
        std::cerr << "TpcAlignmentManager::setEulerAngles called for not existing detector:" <<detName << std::endl;
        std::cerr<< "Too add a new detector use the method TpcAlignmentManager::setConv"<<std::endl;
    }
}

void TpcAlignmentManager::setTranslation(TString detName, TVector3 newTrans)
{
    if(fTransformations.count(detName)){
        fTransformations[detName]->SetTranslation(newTrans.X(),newTrans.Y(),newTrans.Z());
    }else {
        std::cerr << "TpcAlignmentManager::setSetTranslation called for not existing detector:" <<detName << std::endl;
        std::cerr<< "Too add a new detector use the method TpcAlignmentManager::setConv"<<std::endl;
        quit(detName);
    }
}
void TpcAlignmentManager::setAngleMode(TString detName, int mode){
    if(fTransformations.count(detName)){
        if(fAngleMode[detName]!=mode){
            double phi, theta, psi;
            if(mode==2){
                getEulerAnglesZYX(detName,phi,theta,psi);
                fAngles[detName][0]=phi;
                fAngles[detName][1]=theta;
                fAngles[detName][2]=psi;
            }else{
                getEulerAngles(detName,phi,theta,psi);
                fAngles[detName][0]=phi;
                fAngles[detName][1]=theta;
                fAngles[detName][2]=psi;
            }
            fAngleMode[detName]=mode;
        }
        
    }else {
        std::cerr << "TpcAlignmentManager::setSetTranslation called for not existing detector:" <<detName << std::endl;
        std::cerr<< "Too add a new detector use the method TpcAlignmentManager::setConv"<<std::endl;
        quit(detName);
    }
}
//
void TpcAlignmentManager::setConv(TString detName,
                                  TGeoTranslation* trans,
                                  std::vector<double> rot,
                                  int angleMode){
    if(fTransformations.count(detName)>0){
        std::cerr<<"WARNING TpcAlignmentManager::setConv for detName "<<detName
                <<" wich is already existing, overwriting convention!!"<<std::endl;
    }else{
        detNames.push_back(detName);
    }
    initialized=true;
    
    ostringstream name;
    name<<"rotation_detName_"<<detName;
    if(rot.size()!=3){
        quit(detName);
    }
    TGeoRotation *grot=new TGeoRotation(name.str().c_str(),rot[0],rot[1],rot[2]);
    if(angleMode==2){
        TpcAlignmentHelper alHelp;
        std::vector<double> rotMatrix=alHelp.getRotationMatrixZYX(rot[0],rot[1],rot[2]);
        grot->SetMatrix(rotMatrix.data());
    }
    fAngleMode[detName]=angleMode;
    fTransformations[detName]=new TGeoCombiTrans(*trans,*grot);
    fAngles[detName]=rot;
}

TVector3 TpcAlignmentManager::localToMaster(TString detName, TVector3 uvw){
    if(fTransformations.count(detName)){
        vector<double> uvw_=tVector3ToStlVector(uvw);
        double xyz[3];
        fTransformations[detName]->LocalToMaster(&uvw_[0],xyz);
        return TVector3(xyz);
    }else if(!initialized){
        return uvw;
    }else{
        quit(detName);
    }
}
TMatrixD TpcAlignmentManager::localToMaster(TString detName, TMatrixD covUVW){
    if(covUVW.GetNcols()!=3||covUVW.GetNrows()!=3){
        std::cerr << "TpcAlignmentManager: trying to transform covariance of invalid dimension "
                  <<covUVW.GetNcols()<<" x "<<covUVW.GetNrows()<<" -> abort"<<std::endl;
    }
    if(fTransformations.count(detName)){
        TMatrixD rot(3,3,fTransformations[detName]->GetRotationMatrix());
        //    TMatrixD ret(3,3);
        TMatrixD rotT(3,3);
        rotT.Transpose(rot);
        TMatrixD ret=(rot*covUVW)*rotT;
        return ret;
    }else if(!initialized){
        return covUVW;
    }else{
        quit(detName);
    }
}
void TpcAlignmentManager::localToMaster(TString detName, TVector3 uvw,TVector3 &xyz){
    xyz=localToMaster(detName, uvw);
}
void TpcAlignmentManager::localToMaster(TString detName, TMatrixD covUVW, TMatrixD &covXYZ){
    covXYZ=localToMaster(detName, covUVW);
}
TVector3 TpcAlignmentManager::localToMasterVect(TString detName, TVector3 uvw){
    if(fTransformations.count(detName)){
        vector<double> uvw_=tVector3ToStlVector(uvw);
        double xyz[3];
        fTransformations[detName]->LocalToMasterVect(&uvw_[0],xyz);
        return TVector3(xyz);
    }else if(!initialized){
        return uvw;
    }else{
        quit(detName);
    }
}
void TpcAlignmentManager::localToMasterVect(TString detName, TVector3 uvw,TVector3 &xyz){
    xyz=localToMasterVect(detName, uvw);
}
TVector3 TpcAlignmentManager::masterToLocal(TString detName, TVector3 xyz){
    if(fTransformations.count(detName)){
        vector<double> xyz_=tVector3ToStlVector(xyz);
        double uvw[3];
        fTransformations[detName]->MasterToLocal(&xyz_[0],uvw);
        return TVector3(uvw);
    }else if(!initialized){
        return xyz;
    }else{
        quit(detName);
    }
}
TMatrixD TpcAlignmentManager::masterToLocal(TString detName, TMatrixD covXYZ){
    if(covXYZ.GetNcols()!=3||covXYZ.GetNrows()!=3){
        std::cerr << "TpcAlignmentManager: trying to transform covariance of invalid dimension "
                  <<covXYZ.GetNcols()<<" x "<<covXYZ.GetNrows()<<" -> abort"<<std::endl;
    }
    if(fTransformations.count(detName)){
        TMatrixD rot(3,3,fTransformations[detName]->GetRotationMatrix());
        TMatrixD ret(3,3);
        TMatrixD rotT(3,3);
        rotT.Transpose(rot);
        ret=rotT*covXYZ*rot;
        return ret;
    }else if(!initialized){
        return covXYZ;
    }else{
        quit(detName);
    }
}


void TpcAlignmentManager::masterToLocal(TString detName, TVector3 xyz,TVector3 &uvw){
    uvw=localToMaster(detName, xyz);
}
void TpcAlignmentManager::masterToLocal(TString detName, TMatrixD covXYZ, TMatrixD &covUVW){
    covUVW=localToMaster(detName, covXYZ);
}
TVector3 TpcAlignmentManager::masterToLocalVect(TString detName, TVector3 xyz){
    if(fTransformations.count(detName)){
        vector<double> xyz_=tVector3ToStlVector(xyz);
        double uvw[3];
        fTransformations[detName]->MasterToLocalVect(&xyz_[0],uvw);
        return TVector3(uvw);
    }else if(!initialized){
        return xyz;
    }else{
        quit(detName);
    }
}
void TpcAlignmentManager::masterToLocalVect(TString detName, TVector3 xyz,TVector3 &uvw){
    uvw=localToMasterVect(detName, xyz);
}
void TpcAlignmentManager::write(TString outfilename){
    if(initialized){
        ofstream outfile(outfilename.Data());
        time_t rawtime;
        struct tm * timeinfo;
        time(&rawtime);
        timeinfo=localtime(&rawtime);
        outfile<<"#alignment file written at "<<asctime(timeinfo)<<endl;
        outfile.unsetf ( std::ios::floatfield );
        for(unsigned int i=0;i<detNames.size();++i){
            outfile<<"##########################################################"<<endl;
            outfile<<"detName "<<detNames.at(i)<<endl;
            if(fTransformations.count(detNames.at(i))){
                TGeoCombiTrans *tr=fTransformations[detNames.at(i)];
                outfile<<"translation "
                      <<std::showpos <<std::setprecision(17)<<scientific<<tr->GetTranslation()[0]<<"\t"
                     <<std::showpos <<std::setprecision(17)<<scientific<<tr->GetTranslation()[1]<<"\t"
                    <<std::showpos <<std::setprecision(17)<<scientific<<tr->GetTranslation()[2]<<"    #";
                outfile.unsetf ( std::ios::floatfield);
                outfile<<std::showpos <<std::setprecision(4)<<tr->GetTranslation()[0]<<"\t"
                      <<std::showpos <<std::setprecision(4)<<tr->GetTranslation()[1]<<"\t"
                     <<std::showpos <<std::setprecision(4)<<tr->GetTranslation()[2]<<std::endl;
                double phi,theta,psi;
                if(fAngleMode[detNames.at(i)]==1){
                    //	  getEulerAngles(detNames.at(i),phi,theta,psi);
                    outfile<<"euler1      ";
                }else if (fAngleMode[detNames.at(i)]==2){
                    //	  getEulerAnglesZYX(detNames.at(i),phi,theta,psi);
                    outfile<<"euler2      ";
                }
                outfile<<std::showpos <<std::setprecision(17)<<scientific<<fAngles[detNames.at(i)][0]<<"\t"
                                                                                                    <<std::showpos <<std::setprecision(17) <<scientific<<fAngles[detNames.at(i)][1]<<"\t"
                                                                                                                                                                                  <<std::showpos <<std::setprecision(17)<<scientific<<fAngles[detNames.at(i)][2]<<"    #";
                outfile.unsetf ( std::ios::floatfield);
                outfile<<std::showpos <<std::setprecision(4)<<fAngles[detNames.at(i)][0]<<"\t"
                                                                                       <<std::showpos <<std::setprecision(4) <<fAngles[detNames.at(i)][1]<<"\t"
                                                                                                                                                        <<std::showpos <<std::setprecision(4)<<fAngles[detNames.at(i)][2]<<endl;
                
            }
            
            
        }
    }
}


std::ostream& TpcAlignmentManager::write(std::ostream& stream){
    time_t rawtime;
    struct tm * timeinfo;
    time(&rawtime);
    timeinfo=localtime(&rawtime);
    stream<<"#alignment file written at "<<asctime(timeinfo)<<endl;
    for(unsigned int i=0;i<detNames.size();++i){
        stream.unsetf ( std::ios::floatfield );   
        stream<<"##########################################################"<<endl;
        stream<<"detName "<<detNames.at(i)<<endl;
        if(fTransformations.count(detNames.at(i))){
            TGeoCombiTrans *tr=fTransformations[detNames.at(i)];
            stream<<"translation "
                 <<std::setprecision(17)<<tr->GetTranslation()[0]<<"\t "
                <<std::setprecision(17)<<tr->GetTranslation()[1]<<"\t "
               <<std::setprecision(17)<<tr->GetTranslation()[2]<<endl;
            double phi,theta,psi;
            //      getEulerAngles(detNames.at(i),phi,theta,psi);
            if(fAngleMode[detNames.at(i)]==1){
                //	getEulerAngles(detNames.at(i),phi,theta,psi);
                stream<<"euler1       ";
            }else if (fAngleMode[detNames.at(i)]==2){
                //	getEulerAnglesZYX(detNames.at(i),phi,theta,psi);
                stream<<"euler2       ";
            }
            stream<<std::setprecision(17)<<fAngles[detNames.at(i)][0]<<"\t "
                                                                    <<std::setprecision(17)<<fAngles[detNames.at(i)][1]<<"\t "
                                                                                                                      <<std::setprecision(17)<<fAngles[detNames.at(i)][2]<<endl;
        }
        
        
    }
    return stream;
}

void TpcAlignmentManager::setRotationMatrix(TString detName, std::vector<double>matrix){
    if(fTransformations.count(detName)){
        if(fTransformations[detName]->GetRotation()==0){
            TGeoRotation* rot = new TGeoRotation();
            rot->SetMatrix(matrix.data());
            fTransformations[detName]->SetRotation(rot);
        }else{
            fTransformations[detName]->GetRotation()->SetMatrix(matrix.data());
        }
        double a=0;
        double b=0;
        double c=0;
        if(fTransformations[detName]->GetRotation()==0){
            return;
        }
        if(fTransformations[detName]->GetRotation()->IsRotation()){
            fTransformations[detName]->GetRotation()->GetAngles(a,b,c);
            if(fAngleMode[detName]==2){
                getEulerAnglesZYX(detName,a,b,c);
            }
        }
        fAngles[detName][0]=a;
        fAngles[detName][1]=b;
        fAngles[detName][2]=c;
    }else{
        std::cerr << "TpcAlignmentManager::setRotationMatrixcalled for not existing detector:" <<detName << std::endl;
        std::cerr<< "Too add a new detector use the method TpcAlignmentManager::setConv"<<std::endl;
        quit(detName);
    }
}