#include "IGenericAlignmentLookup.hxx"
#include "TRotation.h"
#include "TMath.h"

//*****************************************************
COMET::IAlignmentId IGenericAlignmentLookup::StartAlignment(const COMET::ICOMETEvent* const event) {
//*****************************************************
  fCalls = 0;

  return COMET::IAlignmentId();
}

//*****************************************************
TGeoMatrix* IGenericAlignmentLookup::Align(const COMET::ICOMETEvent* const event,
				       COMET::IGeometryId& geomId) {
//*****************************************************

  TGeoMatrix* result = NULL;
  if (fCalls<fCorrections.size()) {

    geomId = fCorrections[fCalls].first;
    result = fCorrections[fCalls].second;

  }

  ++fCalls;
  return result;

}

//*****************************************************
void  IGenericAlignmentLookup::AddTranslation(const COMET::IGeometryId& geomId, double dx, double dy, double dz) {
//*****************************************************

  TGeoMatrix* trans = new TGeoTranslation(dx,dy,dz);
  fCorrections.push_back(std::pair<COMET::IGeometryId,TGeoMatrix*>(geomId,trans));

}

//*****************************************************
void  IGenericAlignmentLookup::AddRotation(const COMET::IGeometryId& geomId, double a, double b, double c) {
//*****************************************************

// Euler angles in degrees
  TGeoMatrix* rot = new TGeoRotation("align",a,b,c);
  fCorrections.push_back(std::pair<COMET::IGeometryId,TGeoMatrix*>(geomId,rot));


}

//*****************************************************
void  IGenericAlignmentLookup::AddRotation(const COMET::IGeometryId& geomId, TVector3 axis, double angle) {
//*****************************************************

  // angle is in milliradians.
  //  TGeoMatrix* rot = new TGeoRotation("align",angle,0,0);
  TGeoRotation* rot = new TGeoRotation("align");

  double elem[9];

  TRotation rot2;
  rot2.Rotate(angle/1000.,axis);
  elem[0] = rot2.XX();
  elem[1] = rot2.XY();
  elem[2] = rot2.XZ();
  elem[3] = rot2.YX();
  elem[4] = rot2.YY();
  elem[5] = rot2.YZ();
  elem[6] = rot2.ZX();
  elem[7] = rot2.ZY();
  elem[8] = rot2.ZZ();


  rot->SetMatrix(elem);

  fCorrections.push_back(std::pair<COMET::IGeometryId,TGeoMatrix*>(geomId,rot));


}

//*****************************************************
void  IGenericAlignmentLookup::AddTranslationRotation(const COMET::IGeometryId& geomId, double dx, double dy, double dz, double a, double b, double c) {
//*****************************************************


// Euler angles in degrees
  TGeoTranslation trans(dx,dy,dz);
  TGeoRotation rot("align",a,b,c);
  TGeoMatrix* corr = new  TGeoCombiTrans(trans,rot);
  fCorrections.push_back(std::pair<COMET::IGeometryId,TGeoMatrix*>(geomId,corr));

}

//*****************************************************
void  IGenericAlignmentLookup::AddTranslationRotation(const COMET::IGeometryId& geomId, double dx, double dy, double dz, TVector3 axis, double angle) {
//*****************************************************


// angle is in milliradians.
  TGeoTranslation trans(dx,dy,dz);

  TGeoRotation rot;

  double elem[9];

  TRotation rot2;
  rot2.Rotate(angle/1000.,axis);
  elem[0] = rot2.XX();
  elem[1] = rot2.XY();
  elem[2] = rot2.XZ();
  elem[3] = rot2.YX();
  elem[4] = rot2.YY();
  elem[5] = rot2.YZ();
  elem[6] = rot2.ZX();
  elem[7] = rot2.ZY();
  elem[8] = rot2.ZZ();


  rot.SetMatrix(elem);

  TGeoMatrix* corr = new  TGeoCombiTrans(trans,rot);
  fCorrections.push_back(std::pair<COMET::IGeometryId,TGeoMatrix*>(geomId,corr));

}

//*****************************************************
void  IGenericAlignmentLookup::AddTranslationRotation(const COMET::IGeometryId& geomId, TVector3 trans, TVector3 axis, double angle) {
//*****************************************************


// angle is in milliradians.
  TGeoTranslation transVec(trans.X(),trans.Y(),trans.Z());

  TGeoRotation rot;

  double elem[9];

  TRotation rot2;
  rot2.Rotate(angle/1000.,axis);
  elem[0] = rot2.XX();
  elem[1] = rot2.XY();
  elem[2] = rot2.XZ();
  elem[3] = rot2.YX();
  elem[4] = rot2.YY();
  elem[5] = rot2.YZ();
  elem[6] = rot2.ZX();
  elem[7] = rot2.ZY();
  elem[8] = rot2.ZZ();


  rot.SetMatrix(elem);

  TGeoMatrix* corr = new  TGeoCombiTrans(transVec,rot);
  fCorrections.push_back(std::pair<COMET::IGeometryId,TGeoMatrix*>(geomId,corr));

}