#ifndef TPCALIGNMENTMANAGER_H
#define TPCALIGNMENTMANAGER_H

//ROOT headers
#include "TGeoMatrix.h"
#include "TVector3.h"
#include "TLorentzVector.h"
#include "TMatrixD.h"
//STL headers
#include <string>
#include <map>
#include <iostream>
#include <vector>

/*
  Euler convention 
  This represent the composition of: first a rotation about Z axis with
  angle phi, then a rotation with theta about the rotated X axis, and
  finally a rotation with psi about the new Z axis.
  
  calculation taken from mathematica
   zrot2.xrot.zrot (c,b,a)
   cosphi*cospsi - costhe*sinphi*sinpsi    cospsi*sinphi + cosphi*costhe*sinpsi    sinthe*sinpsi
  -cosphi*sinpsi - costhe*cospsi*sinphi   -sinphi*sinpsi + cosphi*costhe*cospsi    cospsi*sinthe
   sinphi*sinthe                          -cosphi*sinthe                           costhe

   //phi,the, psi (from TGeoRotation source )

   fRotationMatrix[0] =  cospsi*cosphi - costhe*sinphi*sinpsi;
   fRotationMatrix[1] = -sinpsi*cosphi - costhe*sinphi*cospsi;
   fRotationMatrix[2] =  sinthe*sinphi;
   fRotationMatrix[3] =  cospsi*sinphi + costhe*cosphi*sinpsi;
   fRotationMatrix[4] = -sinpsi*sinphi + costhe*cosphi*cospsi;
   fRotationMatrix[5] = -sinthe*cosphi;
   fRotationMatrix[6] =  sinpsi*sinthe;
   fRotationMatrix[7] =  cospsi*sinthe;
   fRotationMatrix[8] =  costhe

   [0]  [3]  [6]
   [1]  [4]  [7]
   [2]  [5]  [8]
   //inverse matrix
)
 */

//TODO:
// Make AlignmentObject containing all relevant information to be saved in a tree


class TpcAlignmentManager{

 public:

  static bool init(TString filename){
    if(inst==NULL){
      std::cerr<<"########## TpcAlignmentMangarer::init("<<filename<<") "<<std::endl;
      inst=new TpcAlignmentManager(filename);
    }else {
      std::cerr<<"########## TpcAlignmentMangarer::init("<<filename<<") called twice, this should never happen!!!"<<std::endl;
      inst->read(filename);
    }
    return true;
  }


  //TODO, need to make identity-transform
  static TpcAlignmentManager* getInstance(){
    if(inst==NULL){
      inst=new TpcAlignmentManager();
      std::cerr<<"WARNING AlignmentManager called without init, does no transformations!!"<<std::endl;
      std::cerr<<"Getters will always return 0"<<std::endl;
    } 
    return inst;
  }
  TGeoCombiTrans* getTransformation(TString detName);
  void getEulerAngles(TString detName, 
		      double& phi, 
		      double& theta, 
		      double& psi);
  void getEulerAnglesZYX(TString detName, 
			 double& phi, 
			 double& theta, 
			 double& psi);
  double getPhi(TString detName);
  double getPhiXYZ(TString detName);
  double getTheta(TString detName);
  double getThetaXYZ(TString detName);
  double getPsi(TString detName);
  double getPsiXYZ(TString detName);
  
  void setEulerAngles(TString detName,
                      double phi,
                      double theta,
                      double psi, int mode=1);
  TVector3 getTranslation(TString detName);
  void setTranslation(TString detName, TVector3 newTrans);
  void getPlaneDef(TString detName, 
		   TVector3 &o, 
		   TVector3 &u, 
		   TVector3 &v);
  //Add/update alignment information for detector by hand
  void setConv(TString detName, 
	       TGeoTranslation* trans, 
	       std::vector<double> rot,
	       int angleMode );
  TVector3 localToMaster(TString detName, TVector3 uvw);
  TMatrixD localToMaster(TString detName, TMatrixD covUVW);
  TVector3 localToMasterVect(TString detName, TVector3 uvw);
  TVector3 masterToLocal(TString detName, TVector3 xyz);
  TMatrixD masterToLocal(TString detName, TMatrixD covXYZ);
  TVector3 masterToLocalVect(TString detName, TVector3 xyz);

  void localToMaster(TString detName, TVector3 uvw,TVector3 &xyz);
  void localToMaster(TString detName, TMatrixD covUVW, TMatrixD &ovXYZ);
  void localToMasterVect(TString detName, TVector3 uvw,TVector3 &xyz);
  void masterToLocal(TString detName, TVector3 xyz, TVector3 &uvw);
  void masterToLocal(TString detName, TMatrixD covXYZ, TMatrixD &ovUVW);
  void masterToLocalVect(TString detName, TVector3 xyz, TVector3 &uvw);
  //needs to be 9 elements
  void setRotationMatrix(TString detName, std::vector<double> matrix);
  void setAngleMode(TString detName,int mode);
  TString getFileName(){return transformationFileName;}
  void Print(){ 
    write(std::cout); 
  } 
  std::vector<TString> getDetNames(){return detNames;}
  void read(TString filename);
  void write(TString filename);
  
  ~TpcAlignmentManager();

  //TestBench specific methods
  
 private:
  bool initialized;
  //Mode 1 = ZXZ-convention, Mode 2 = ZYX convention.  
  // 1: Phi=z, Theta=x , psi=Z'
  // 2: Phi=z, Theta=Y , psi=X'
  // in alignmentFile, euler0 or euler1
  // if only euler is specified, euler0 convention is used
  // this is to keep backward compatibility

  std::map<TString, int> fAngleMode;
  std::map<TString, TGeoCombiTrans* > fTransformations;
  std::map<TString, std::vector<double> > fAngles;
  std::vector<TString> detNames;
  TString transformationFileName;
  TString fClusterBranchName;
  static TpcAlignmentManager *inst;
  TpcAlignmentManager(TString filename){
    //  std::cout<<"creator called"<<std::endl;
    read(filename);
    initialized=true;
  }
  TpcAlignmentManager(){
    initialized=false;
  }
  void clear();
  void readDet(TString detName, std::ifstream &input);
  void quit(TString detName){
    std::cerr << "TpcAlignmentManager: info for detName " << detName
	      << " requested, which is not known ->abort" << std::endl;
    throw;
  }
  std::ostream&  write(std::ostream& stream);
 public:
  static std::vector<double> tVector3ToStlVector(TVector3 v){
    std::vector<double> r;
    r.push_back(v[0]);
    r.push_back(v[1]);
    r.push_back(v[2]);
    return r;
  }
  ClassDef(TpcAlignmentManager,1); 
};

#endif