////////////////////////////////////////////////////////////////////////
//                                                                    //
//                 FOPI Forward Hit class                             //
//                 =================================                  //
//                                                                    //
// Author: paul.buehler@oeaw.ac.at                                    //
//                                                                    //
//                                                                    //
//                                                                    //
////////////////////////////////////////////////////////////////////////

#include <iostream>

#include "FopiForwardHit.h"

ClassImp(FopiForwardHit)

// ----------------------------------------------------------------------------
FopiForwardHit::FopiForwardHit() {

  // initialize local point
  fRad = 3.1415926/180.;
  fType = 0;
  fLocalPos = TVector3(0., 0., 0.);
  
  Double_t loccov[9] = {0.,0.,0.,0.,0.,0.,0.,0.,0.};
  fLocalCov = new TMatrixD(3,3);
  fLocalCov->SetMatrixArray(loccov);
  Double_t glocov[9] = {0.,0.,0.,0.,0.,0.,0.,0.,0.};
  fGlobalCov = new TMatrixD(3,3);
  fGlobalCov->SetMatrixArray(glocov);
  
  // initialise shifts and rotations
  fHrsec  = 0;
  fTaroff = 0;
  fHoff   = TVector3(0., 0., 0.);
  fPhi0   = 11.25;
  
  fHrot   = TVector3(0., 0., 0.);
  ComputeRotMatrix();
  
  // set the wire direction
  ComputeWireDirection();
  
  // compute global point
  Convert();
  
}

// ----------------------------------------------------------------------------
FopiForwardHit::FopiForwardHit(Int_t type,
  Float_t hrsec, Float_t taroff, TVector3 hoff, TVector3 hrot) {

  // initialize type
  fRad = 3.1415926/180.;
	fType = type;

  Double_t loccov[9] = {0.,0.,0.,0.,0.,0.,0.,0.,0.};
  fLocalCov = new TMatrixD(3,3);
  fLocalCov->SetMatrixArray(loccov);
  Double_t glocov[9] = {0.,0.,0.,0.,0.,0.,0.,0.,0.};
  fGlobalCov = new TMatrixD(3,3);
  fGlobalCov->SetMatrixArray(glocov);
  
  // initialize shifts and rotations
  fHrsec  = hrsec;
  fTaroff = taroff;
  fHoff   = hoff;
  fPhi0   = 11.25;
  
  fHrot   = hrot;
  ComputeRotMatrix();
  
  // set the wire direction
  ComputeWireDirection();
	
  // initialize points according to type
  if (type == 0) {
    fLocalPos = TVector3(0.,0.,0.);
  } else {
	  fGlobalPos = TVector3(0.,0.,0.);
  }
  Convert();

}

// ----------------------------------------------------------------------------
void FopiForwardHit::SetPos (Int_t type, Float_t x, Float_t y, Float_t z) {

  // set point and translate to other coordinate system
  if (type == 0) {					// local
    
    fType = 0;
    fLocalPos = TVector3(x,y,z);
    
  } else {									// global
    
    fType = 1;
    fGlobalPos = TVector3(x,y,z);
    
  }
  Convert();

}

// ----------------------------------------------------------------------------
void FopiForwardHit::SetPos (Float_t x, Float_t y, Float_t z) {

  // call SetPos
  SetPos(fType, x, y, z);

}

// ----------------------------------------------------------------------------
void FopiForwardHit::SetPos (Int_t type, TVector3 pos) {

  // call SetPos
  SetPos(type, pos.X(), pos.Y(), pos.Z());

}

// ----------------------------------------------------------------------------
void FopiForwardHit::SetPos (TVector3 pos) {

  // call SetPos
  SetPos(fType, pos.X(), pos.Y(), pos.Z());

}

// ----------------------------------------------------------------------------
void FopiForwardHit::Convert () {
  
  Float_t xtmp, ytmp, ztmp;
  Float_t pr, phi;
  if (fType == 0) {					// local -> global
    
    // correct for shift and rotation of helitron
    TVector3 pos2rot = TVector3(fLocalPos.X(),fLocalPos.Y(),fLocalPos.Z());
    TVector3 postmp = fSecMatrix*pos2rot;
    
    postmp *= fRotMatrix;
    xtmp = postmp.X() + fHoff.X();
    ytmp = postmp.Y() + fHoff.Y();
    
    // correct for shift of target
    // ztmp = postmp.Z() + fTaroff;
    ztmp = postmp.Z();
    
    // set the global coordinates
    fGlobalPos = TVector3(xtmp,ytmp,ztmp);
    
    // error matrix
    Double_t secmatvals[9] = {
      fSecMatrix.XX(),fSecMatrix.XY(),fSecMatrix.XZ(),
      fSecMatrix.YX(),fSecMatrix.YY(),fSecMatrix.YZ(),
      fSecMatrix.ZX(),fSecMatrix.ZY(),fSecMatrix.ZZ()};
    TMatrixD secrotmat = TMatrixD(3,3);
    secrotmat.SetMatrixArray(secmatvals,"F");
    
    Double_t *loccov = fLocalCov->GetMatrixArray();
    Double_t covvals[9] = {
      loccov[0],loccov[1],loccov[2],
      loccov[3],loccov[4],loccov[5],
      loccov[6],loccov[7],loccov[8]};
    TMatrixD cov2rot = TMatrixD(3,3);
    cov2rot.SetMatrixArray(covvals,"F");
    //fprintf(stderr,"Local Covariance:\n");
    //cov2rot.Print();
    
    //fprintf(stderr,"Rotation Matrix:\n");
    //secrotmat.Print();
    
    Double_t *glocov = (secrotmat.T()*(cov2rot*secrotmat)).GetMatrixArray();
    fGlobalCov->SetMatrixArray(glocov,"F");
    //fprintf(stderr,"Global Covariance:\n");
    //fGlobalCov->Print();
    
  } else {									// global -> local
	
    // correct for shift of target
    ztmp = fGlobalPos.Z();
    
    // correct for shift and rotation of Helitron
    xtmp = fGlobalPos.X() - fHoff.X();
    ytmp = fGlobalPos.Y() - fHoff.Y();
    TVector3 postmp = TVector3(xtmp, ytmp, ztmp);
    postmp *= fRotMatrix.Invert();
    
    postmp *= fSecMatrix.Invert();
    xtmp = postmp.X();
    ytmp = postmp.Y();
    ztmp = postmp.Z();
    
    // set the global coordinates
    fLocalPos = TVector3(xtmp,ytmp,ztmp);
    
    // error matrix
    Double_t secmatvals[9] = {
      fSecMatrix.XX(),fSecMatrix.XY(),fSecMatrix.XZ(),
      fSecMatrix.YX(),fSecMatrix.YY(),fSecMatrix.YZ(),
      fSecMatrix.ZX(),fSecMatrix.ZY(),fSecMatrix.ZZ()};
    TMatrixD secrotmat = TMatrixD(3,3);
    secrotmat.SetMatrixArray(secmatvals,"F");
    //TMatrixD secrotmattrans = secrotmat;
    //secrotmattrans.T();
    
    Double_t *loccov = (secrotmat.T().Invert()*
      ((TMatrixD)(*fGlobalCov)*secrotmat.Invert())).GetMatrixArray();
    Double_t covvals[9] = {
      loccov[0],loccov[1],loccov[2],
      loccov[3],loccov[4],loccov[5],
      loccov[6],loccov[7],loccov[8]};
    fLocalCov->SetMatrixArray(covvals,"F");
    
	}

}

// ----------------------------------------------------------------------------
void FopiForwardHit::ComputeSecMatrix () {

  // sector specific rotation angle
	Float_t alpha = fRad*( fPhi0 + (fHrsec-1)*15. - 90.);
  
  fSecMatrix.SetYEulerAngles(alpha,0., 0.);

}

// ----------------------------------------------------------------------------
void FopiForwardHit::ComputeRotMatrix () {

  fRotMatrix.SetYEulerAngles(fHrot[0], fHrot[1], fHrot[2]);
  //fprintf(stderr,"fRotMatrix= %f %f %f\n",
  //  fRotMatrix.XX(),fRotMatrix.YY(),fRotMatrix.ZZ());

}

// ----------------------------------------------------------------------------
void FopiForwardHit::ComputeWireDirection () {

  fWireDirection.SetMagThetaPhi(1.,fRad*90.,fRad*(fPhi0 + (fHrsec-1)*15.));

}

// ----------------------------------------------------------------------------