#include <IApplyAlign.hxx>
#include <TGeoManager.h>
#include <TGeoOverlap.h>
#include <IOARuntimeParameters.hxx>
#include <TFile.h>
IApplyAlign::IApplyAlign()
{
  ApplyTrackerSurvey((bool)COMET::IOARuntimeParameters::Get().GetParameterI("oaApplyAlign.ApplyTrackerSurvey"));
  ApplyFGDInternal((bool)COMET::IOARuntimeParameters::Get().GetParameterI("oaApplyAlign.ApplyFGDIntAlign"));
  ApplyMMSurvey((bool)COMET::IOARuntimeParameters::Get().GetParameterI("oaApplyAlign.ApplyMMSurvey"));
  
  surveyTransConst["TPC1"] = TVector3(-2.253, -4.665, -0.270);
  surveyTransConst["TPC2"] = TVector3(-1.276, -3.401, -2.362);
  surveyTransConst["TPC3"] = TVector3(-3.458, -3.142, -0.028);
  surveyTransConst["FGD1"] = TVector3(-0.954, -3.822, 0.954);
  surveyTransConst["FGD2"] = TVector3(-1.260, -3.397, 1.087);
  surveyTransConst["DSECal"] = TVector3(-3.095, -0.579, 0);
  
  surveyRotConst["TPC1"].first = TVector3(-0.793845, 0.597900, 0.111017);
  surveyRotConst["TPC1"].second = 1.718;
  surveyRotConst["TPC2"].first = TVector3(0.808855, 0.574514, 0.125252);
  surveyRotConst["TPC2"].second = -1.313;
  surveyRotConst["TPC3"].first = TVector3(-0.675385, 0.223278, 0.702853);
  surveyRotConst["TPC3"].second = 0.610;
  surveyRotConst["FGD1"].first = TVector3(-0.987514, 0.118678, 0.103597);
  surveyRotConst["FGD1"].second = 0; 
  surveyRotConst["FGD2"].first = TVector3(-0.160236, 0.980503, 0.113749);
  surveyRotConst["FGD2"].second = 0;
  surveyRotConst["DSECal"].first = TVector3(-0.270950, -0.806958, 0.524790);
  surveyRotConst["DSECal"].second = 1.203;
  
  fgdIntTransConst[0] = -0.379351;
  fgdIntTransConst[1] = -0.115179;
  fgdIntTransConst[2] = 0.210465;
  fgdIntTransConst[3] = 0.161571;
  fgdIntTransConst[4] = -0.0505728;
  fgdIntTransConst[5] = 0.0535972; 
  fgdIntTransConst[6] = 0.0305484; 
  fgdIntTransConst[7] = -0.199944;
  fgdIntTransConst[8] = -0.0697529;
  fgdIntTransConst[9] = -0.270023;
  fgdIntTransConst[10] = -0.129371; 
  fgdIntTransConst[11] = 0.0471496; 
  fgdIntTransConst[12] = 0.255003; 
  fgdIntTransConst[13] = -0.13411; 
  fgdIntTransConst[14] = 0.214838; 
  fgdIntTransConst[15] = 0.0558979; 
  fgdIntTransConst[16] = 0.266153; 
  fgdIntTransConst[17] = 0.555704; 
  fgdIntTransConst[18] = 0.0547172; 
  fgdIntTransConst[19] = 0.351819; 
  fgdIntTransConst[20] = 0.00473694; 
  fgdIntTransConst[21] = 0.786871; 
  fgdIntTransConst[22] = 0.0380896; 
  fgdIntTransConst[23] = -0.401259; 
  fgdIntTransConst[24] = -0.245828;
  fgdIntTransConst[25] = -0.414316; 
  fgdIntTransConst[26] = -0.124361; 
  fgdIntTransConst[27] = -0.0840803; 
  fgdIntTransConst[28] = -0.0219941;
  fgdIntTransConst[29] = -0.251145; 
  fgdIntTransConst[30] = -0.0640535; 
  fgdIntTransConst[31] = -0.0783846; 
  fgdIntTransConst[32] = -0.143142; 
  fgdIntTransConst[33] = 0.408897;
  fgdIntTransConst[34] = 0.221593; 
  fgdIntTransConst[35] = -0.477095;
  fgdIntTransConst[36] = 0.20223; 
  fgdIntTransConst[37] = 0.235765;
  fgdIntTransConst[38] = -0.382516; 
  fgdIntTransConst[39] = 0.0894779; 
  fgdIntTransConst[40] = 0.401933; 
  fgdIntTransConst[41] = -0.134181;
  fgdIntTransConst[42] = -0.235573; 
  fgdIntTransConst[43] = 0.0246083; 

  tpcMmTransConst[0][0][0] = TVector3(0., 0.488, -0.428);
  tpcMmTransConst[0][0][1] = TVector3(0., 0.291, -0.363);
  tpcMmTransConst[0][0][2] = TVector3(0., 0.346, -0.258);
  tpcMmTransConst[0][0][3] = TVector3(0., 0.304, -0.149); 
  tpcMmTransConst[0][0][4] = TVector3(0., 0.318, -0.083); 
  tpcMmTransConst[0][0][5] = TVector3(0., 0.113, -0.074); 
  tpcMmTransConst[0][0][6] = TVector3(0., 0.135, -0.212); 
  tpcMmTransConst[0][0][7] = TVector3(0., 0.050, -0.171);
  tpcMmTransConst[0][0][8] = TVector3(0., 0.249, -0.123); 
  tpcMmTransConst[0][0][9] = TVector3(0., 0.002, -0.061);
  tpcMmTransConst[0][0][10] = TVector3(0., 0.018, -0.072); 
  tpcMmTransConst[0][0][11] = TVector3(0., -0.063, -0.120);
  tpcMmTransConst[0][1][0] = TVector3(0., 0.060, 0.108);
  tpcMmTransConst[0][1][1] = TVector3(0., 0.142, 0.176); 
  tpcMmTransConst[0][1][2] = TVector3(0., 0.115, 0.010);
  tpcMmTransConst[0][1][3] = TVector3(0., -0.022, 0.053); 
  tpcMmTransConst[0][1][4] = TVector3(0., 0.052, 0.075); 
  tpcMmTransConst[0][1][5] = TVector3(0., 0.121, 0.045); 
  tpcMmTransConst[0][1][6] = TVector3(0., 0.027, -0.007); 
  tpcMmTransConst[0][1][7] = TVector3(0., -0.002, -0.164); 
  tpcMmTransConst[0][1][8] = TVector3(0., 0.029, -0.083); 
  tpcMmTransConst[0][1][9] = TVector3(0., -0.012, -0.127); 
  tpcMmTransConst[0][1][10] = TVector3(0., -0.019, -0.067); 
  tpcMmTransConst[0][1][11] = TVector3(0., -0.131, 0.053); 
  tpcMmTransConst[1][0][0] = TVector3(0., -0.091, 0.510);
  tpcMmTransConst[1][0][1] = TVector3(0., -0.082, 0.149); 
  tpcMmTransConst[1][0][2] = TVector3(0., -0.520, 0.541); 
  tpcMmTransConst[1][0][3] = TVector3(0., -0.452, 0.525); 
  tpcMmTransConst[1][0][4] = TVector3(0., -0.299, 0.542);
  tpcMmTransConst[1][0][5] = TVector3(0., -0.358, 0.389); 
  tpcMmTransConst[1][0][6] = TVector3(0., -0.464, 0.333); 
  tpcMmTransConst[1][0][7] = TVector3(0., -0.593, 0.281); 
  tpcMmTransConst[1][0][8] = TVector3(0., -0.575, 0.291);
  tpcMmTransConst[1][0][9] = TVector3(0., -0.697, 0.380); 
  tpcMmTransConst[1][0][10] = TVector3(0., -0.952, 0.624); 
  tpcMmTransConst[1][0][11] = TVector3(0., -0.648, 0.283); 
  tpcMmTransConst[1][1][0] = TVector3(0., 0.274, -0.156); 
  tpcMmTransConst[1][1][1] = TVector3(0., 0.386, -0.054);
  tpcMmTransConst[1][1][2] = TVector3(0., 0.213, 0.198); 
  tpcMmTransConst[1][1][3] = TVector3(0., 0.257, 0.014);
  tpcMmTransConst[1][1][4] = TVector3(0., 0.169, 0.001);
  tpcMmTransConst[1][1][5] = TVector3(0., 0.250, 0.010); 
  tpcMmTransConst[1][1][6] = TVector3(0., -0.138, 0.003);
  tpcMmTransConst[1][1][7] = TVector3(0., -0.292, 0.048); 
  tpcMmTransConst[1][1][8] = TVector3(0., -0.109, 0.113); 
  tpcMmTransConst[1][1][9] = TVector3(0., -0.344, -0.156); 
  tpcMmTransConst[1][1][10] = TVector3(0., -0.171, -0.081);
  tpcMmTransConst[1][1][11] = TVector3(0., -0.310, -0.141); 
  tpcMmTransConst[2][0][0] = TVector3(0., -0.005, -0.407);
  tpcMmTransConst[2][0][1]= TVector3(0., 0.334, -0.280); 
  tpcMmTransConst[2][0][2] = TVector3(0., 0.068, -0.169); 
  tpcMmTransConst[2][0][3] = TVector3(0., -0.016, 0.001);
  tpcMmTransConst[2][0][4] = TVector3(0., 0.059, 0.230);
  tpcMmTransConst[2][0][5] = TVector3(0., 0.006, 0.456);
  tpcMmTransConst[2][0][6] = TVector3(0., -0.113, -0.480); 
  tpcMmTransConst[2][0][7] = TVector3(0., -0.159, -0.229); 
  tpcMmTransConst[2][0][8] = TVector3(0., -0.172, 0.078); 
  tpcMmTransConst[2][0][9] = TVector3(0., -0.207, 0.236); 
  tpcMmTransConst[2][0][10] = TVector3(0., -0.034, 0.448); 
  tpcMmTransConst[2][0][11] = TVector3(0., -0.287, 0.558);
  tpcMmTransConst[2][1][0] = TVector3(0., 0.297, 0.058); 
  tpcMmTransConst[2][1][1] = TVector3(0., 0.052, 0.110); 
  tpcMmTransConst[2][1][2] = TVector3(0., 0.082, 0.191); 
  tpcMmTransConst[2][1][3] = TVector3(0., 0.208, 0.093); 
  tpcMmTransConst[2][1][4] = TVector3(0., 0.090, 0.105); 
  tpcMmTransConst[2][1][5] = TVector3(0., 0.159, -0.063); 
  tpcMmTransConst[2][1][6] = TVector3(0., 0.026, -0.100); 
  tpcMmTransConst[2][1][7] = TVector3(0., -0.074, -0.154); 
  tpcMmTransConst[2][1][8] = TVector3(0., -0.106, -0.021); 
  tpcMmTransConst[2][1][9] = TVector3(0., -0.138, 0.165); 
  tpcMmTransConst[2][1][10] = TVector3(0., -0.256, -0.174);
  tpcMmTransConst[2][1][11] = TVector3(0., -0.140, -0.100); 
  
  tpcMmRotConst[0][0][0] = 0.123;
  tpcMmRotConst[0][0][1] = -0.124;
  tpcMmRotConst[0][0][2] = 0.213;
  tpcMmRotConst[0][0][3] = 0.347;
  tpcMmRotConst[0][0][4] = 0.419;
  tpcMmRotConst[0][0][5] = -0.295;
  tpcMmRotConst[0][0][6] = -0.326;
  tpcMmRotConst[0][0][7] = -0.481;
  tpcMmRotConst[0][0][8] = -0.086;
  tpcMmRotConst[0][0][9] = -0.537;
  tpcMmRotConst[0][0][10] = 0.041;
  tpcMmRotConst[0][0][11] = -0.459;
  tpcMmRotConst[0][1][0] = -0.731;
  tpcMmRotConst[0][1][1] = -0.427;
  tpcMmRotConst[0][1][2] = -0.787;
  tpcMmRotConst[0][1][3] = -0.389;
  tpcMmRotConst[0][1][4] = -0.616;
  tpcMmRotConst[0][1][5] = 0.621;
  tpcMmRotConst[0][1][6] = 0.374;
  tpcMmRotConst[0][1][7] = 0.578;
  tpcMmRotConst[0][1][8] = 0.944;
  tpcMmRotConst[0][1][9] = 0.468;
  tpcMmRotConst[0][1][10] = 0.013;
  tpcMmRotConst[0][1][11] = 0.636;
  tpcMmRotConst[1][0][0] = 0.317;
  tpcMmRotConst[1][0][1] = 0.385;
  tpcMmRotConst[1][0][2] = 1.458;
  tpcMmRotConst[1][0][3] = 0.933;
  tpcMmRotConst[1][0][4] = 0.575;
  tpcMmRotConst[1][0][5] = 0.650;
  tpcMmRotConst[1][0][6] = 0.263;
  tpcMmRotConst[1][0][7] = -0.679;
  tpcMmRotConst[1][0][8] = -0.606;
  tpcMmRotConst[1][0][9] = -1.311;
  tpcMmRotConst[1][0][10] = -0.658;
  tpcMmRotConst[1][0][11] = -0.290;
  tpcMmRotConst[1][1][0] = -0.894;
  tpcMmRotConst[1][1][1] = -0.726;
  tpcMmRotConst[1][1][2] = 0.085;
  tpcMmRotConst[1][1][3] = -0.439;
  tpcMmRotConst[1][1][4] = -0.373;
  tpcMmRotConst[1][1][5] = -0.567;
  tpcMmRotConst[1][1][6] = 0.258;
  tpcMmRotConst[1][1][7] = 0.800;
  tpcMmRotConst[1][1][8] = 0.034;
  tpcMmRotConst[1][1][9] = 0.916;
  tpcMmRotConst[1][1][10] = -0.396;
  tpcMmRotConst[1][1][11] = -0.092;
  tpcMmRotConst[2][0][0] = -0.406;
  tpcMmRotConst[2][0][1] = -0.343;
  tpcMmRotConst[2][0][2] = -0.120;
  tpcMmRotConst[2][0][3] = -0.092;
  tpcMmRotConst[2][0][4] = -0.633;
  tpcMmRotConst[2][0][5] = -0.212;
  tpcMmRotConst[2][0][6] = -0.608;
  tpcMmRotConst[2][0][7] = -1.372;
  tpcMmRotConst[2][0][8] = -0.229;
  tpcMmRotConst[2][0][9] = -1.011;
  tpcMmRotConst[2][0][10] = -1.513;
  tpcMmRotConst[2][0][11] = -1.083;
  tpcMmRotConst[2][1][0] = -0.799;
  tpcMmRotConst[2][1][1] = -0.915;
  tpcMmRotConst[2][1][2] = -0.369;
  tpcMmRotConst[2][1][3] = -0.134;
  tpcMmRotConst[2][1][4] = -0.183;
  tpcMmRotConst[2][1][5] = 0.421;
  tpcMmRotConst[2][1][6] = 0.344;
  tpcMmRotConst[2][1][7] = 0.843;
  tpcMmRotConst[2][1][8] = 0.581;
  tpcMmRotConst[2][1][9] = 0.320;
  tpcMmRotConst[2][1][10] = 0.599;
  tpcMmRotConst[2][1][11] = 0.175;	
}

IApplyAlign::~IApplyAlign()
{
  //delete std::map<4> surveyTransConst;
  //delete std::map<4> surveyRotConst;
  //delete fgdIntTransConst;
  //delete tpcMmTransConst;
  //delete tpcMmRotConst;
}

/// Takes in FGD internal alignment constants and translates FGD layer positions.
/// Assume x-y layers to be glued.		
void IApplyAlign::ApplyFGDInternalAlign()
{
  /*
  // FGD internal alignment parameters
  int fgdLayerNum;
  double fgdIntAlignParam;
  double fgdIntAlignParamErr;
  double RMS;
  double fgdLayerAlign[44];
  
  alignLookupFGD.open(inputFGDIntAlignTxt);
  if (alignLookupFGD.is_open())
  {
  
  std::cout<<"FGD internal alignment text detected: "<<inputFGDIntAlignTxt<<std::endl;
  std::cout<<"Applying FGD internal geometry modifications..."<<std::endl;
  while (alignLookupFGD >> fgdLayerNum >> fgdIntAlignParam >> fgdIntAlignParamErr >> RMS)
  {
  fgdLayerAlign[fgdLayerNum] = fgdIntAlignParam;	
  if (fgdLayerNum < 30 && fgdLayerNum%2 == 1)
  std::cout<<"Translation on FGD1 module " <<(fgdLayerNum-1)/2<<": ("<<fgdLayerAlign[fgdLayerNum-1]<<", "<<fgdLayerAlign[fgdLayerNum]<<")"<<std::endl;
  else if (fgdLayerNum%2 == 1)
  std::cout<<"Translation on FGD2 module " <<(fgdLayerNum-31)/2<<": ("<<fgdLayerAlign[fgdLayerNum-1]<<", "<<fgdLayerAlign[fgdLayerNum]<<")"<<std::endl;
  }
  for (int i = 0; i < 44; i+=2)
  {
  fAligTable.AddTranslation(fgdLayerId[i],fgdLayerAlign[i],fgdLayerAlign[i+1],0);
  fAligTable.AddTranslation(fgdLayerId[i+1],fgdLayerAlign[i],fgdLayerAlign[i+1],0);
  
  }
  alignLookupFGD.close();
  }
  else
  std::cout<<"FGD internal alignment text "<<inputFGDIntAlignTxt<<" not found, alignment not applied"<<std::endl;
  */
  for (int i = 0; i < 44; i+=2)
    {
      if (i < 30 && i%2 == 0)
  std::cout<<"Translation on FGD1 module " <<(i)/2<<": ("<<fgdIntTransConst[i]<<", "<<fgdIntTransConst[i+1]<<")"<<std::endl;
      else if (i%2 == 0)
  std::cout<<"Translation on FGD2 module " <<(i-30)/2<<": ("<<fgdIntTransConst[i]<<", "<<fgdIntTransConst[i+1]<<")"<<std::endl;
      fAligTable.AddTranslation(fgdLayerId[i], fgdIntTransConst[i], fgdIntTransConst[i+1], 0);
      fAligTable.AddTranslation(fgdLayerId[i+1], fgdIntTransConst[i], fgdIntTransConst[i+1], 0);
    }
}	

/// Reads in TPC micromegas survey alignment constants and modifies geometry.
/// Uses TGeoManager::MasterToLocalVect(double master, double local) to resolve global-local coordinate system difference.
void IApplyAlign::ApplyMMSurveyAlign()
{
  /*
  // The current structure of the text file is:
  // TPC, RP, MM, dx, dy, dz, thetaMRad
  
  int tpcNum;
  int rpNum;
  int mmNum;
  
  TVector3 mmAxis;
  
  alignLookupMM.open(inputMMSurveyAlignTxt);
  if (alignLookupMM.is_open())
  {
  std::cout<<"Micromegas survey alignment text detected: "<<inputMMSurveyAlignTxt<<std::endl;
  
  while (alignLookupMM >> tpcNum >> rpNum >> mmNum >> transX >> transY >> transZ >> thetaMRad)
  {
  // Different coordinate system for every 6 MM's
  if (rpNum == 0 && mmNum >= 0 && mmNum < 6)
  {
  mmAxis.SetXYZ(0, 0, 1);
  fAligTable.AddTranslationRotation(tpcMMId[(tpcNum-1)][rpNum][mmNum], -transZ, transY, transX, mmAxis, thetaMRad);
  }
  else if (rpNum == 0 && mmNum >= 6 && mmNum < 12)
  {
  mmAxis.SetXYZ(0, 0, 1);
  fAligTable.AddTranslationRotation(tpcMMId[(tpcNum-1)][rpNum][mmNum], transZ, -transY, transX, mmAxis, thetaMRad);	
  }	
  else if (rpNum == 1 && mmNum >= 0 && mmNum < 6)
  {
  mmAxis.SetXYZ(0, 0, -1);
  fAligTable.AddTranslationRotation(tpcMMId[(tpcNum-1)][rpNum][mmNum], transZ, transY, -transX, mmAxis, thetaMRad);
  }
  else
  {
  mmAxis.SetXYZ(0, 0, -1);
  fAligTable.AddTranslationRotation(tpcMMId[(tpcNum-1)][rpNum][mmNum], -transZ, -transY, -transX, mmAxis, thetaMRad);
  }	
  std::cout<<"Alignment constants on TPC "<< tpcNum <<", RP "<<rpNum<<", MM " <<mmNum<<": ("<<
  transX<<", "<<
  transY<<", "<<
  transZ<<", " <<
  thetaMRad<<" mrad)"<<std::endl;
  
  }
  alignLookupMM.close();
  }
  else
  std::cout<<"Micromegas survey alignment text " <<inputMMSurveyAlignTxt<<" not found, alignment not applied"<<std::endl;	
  */
 
  Double_t localAxis[3], globalAxis[3];
  Double_t localTrans[3], globalTrans[3];
  globalAxis[0] = 1.;
  globalAxis[1] = 0.;
  globalAxis[2] = 0.;
  for (int i = 0; i < 3; i++)
    {
      for (int j = 0; j < 2; j++)
  {
    for (int k = 0; k < 12; k++)
      {
        
        globalTrans[0] = tpcMmTransConst[i][j][k].X();
        globalTrans[1] = tpcMmTransConst[i][j][k].Y();
        globalTrans[2] = tpcMmTransConst[i][j][k].Z();

        gGeoManager->cd(tpcMMId[i][j][k].GetName().c_str());
        gGeoManager->MasterToLocalVect(globalAxis, localAxis);
        gGeoManager->MasterToLocalVect(globalTrans, localTrans);
        TVector3 mmAxis(localAxis[0], localAxis[1], localAxis[2]);
        TVector3 mmTrans(localTrans[0], localTrans[1], localTrans[2]);

        fAligTable.AddTranslationRotation(tpcMMId[i][j][k],
            mmTrans,
            mmAxis, 
            tpcMmRotConst[i][j][k]);
        
        std::cout<<"Alignment constants on TPC "<< i+1 <<", RP "<<j<<", MM " <<k<<": ("<<
          tpcMmTransConst[i][j][k].X()<<", "<<
          tpcMmTransConst[i][j][k].Y()<<", "<<
          tpcMmTransConst[i][j][k].Z()<<", " <<
          tpcMmRotConst[i][j][k]<<" mrad)"<<std::endl;	
      }
  }
    }
}
/// This is Kendall's method to align TPC micromegas modules. 
/// This method requires a change in IOADatabase.cxx in oaEvent/src, which resolves the local-global coordinate system difference.
/*	
void IApplyAlign::ApplyMMSurveyAlignGlobal()
{
  int tpcNum;
  int rpNum;
  int mmNum;
  
  // No need to define rotation axis on case-by-case basis
  TVector3 mmAxis(1, 0, 0);
  
  alignLookupMM.open(inputMMSurveyAlignTxt);
  if (alignLookupMM.is_open())
    {
      std::cout<<"Micromegas survey alignment text detected: "<<inputMMSurveyAlignTxt<<std::endl;
      while (alignLookupMM >>  tpcNum >> rpNum >> mmNum >> transX >> transY >> transZ >> thetaMRad)
  {
    
    fAligTable.AddTranslationRotation(tpcMMId[(tpcNum-1)][rpNum][mmNum], transX, transY, transZ, mmAxis, thetaMRad);
    std::cout<<"Alignment constants on TPC "<< tpcNum <<", RP "<<rpNum<<", MM " <<mmNum<<": ("<<
      transX<<", "<<
      transY<<", "<<
      transZ<<", " <<
      thetaMRad<<" mrad)"<<std::endl;
    
  }
      alignLookupMM.close();
    }
  else
    std::cout<<"Micromegas survey alignment text " <<inputMMSurveyAlignTxt<<" not found, alignment not applied"<<std::endl;	
  
}	
*/	
/// Reads in COMET survey alignment constants determined from 
/// http://www.t2k.org/nd280/calib/Meetings/Jan10Workshop/survey/
/// and modifies geometry.
/// If using a text file, the format of the text file requires a special structure:
///
/// First column is the sub-detector name (TPC1 or FGD2, for example).
/// Second, third and fourth column are the translation constants (dx, dy, dz).
/// Fifth, sixth and seventh column are the normalized components of axis of rotation (ux, uy, uz).
/// Eighth column is the angle of rotation given in milliradians.
void IApplyAlign::ApplyTrackerSurveyAlign()
{
  // Reading from survey text file and applying translations and rotations to each sub-detector.
  // For trackers (TPC & FGD): rotate on the envelope volume's center and translate to simulate rotation on the active volume's center
  /*
    alignLookupTracker.open(inputSurveyAlignTxt);
    if (alignLookupTracker.is_open())
    {
    std::cout<<"Tracker survey alignment text detected: "<<inputSurveyAlignTxt<<std::endl;
    while (alignLookupTracker >> subDetName >> transX >> transY >> transZ >> axisX >> axisY >> axisZ >> thetaMRad)
    {
    // Rotation axis from survey is already normalized, but make sure it is normalized for offset calculations.
    double mag = sqrt(axisX*axisX + axisY*axisY + axisZ*axisZ);
    axisX/=mag;
    axisY/=mag;
    axisZ/=mag;
    
    std::cout<<"Modifying "<<subDetName<<" geometry..."<<std::endl;
    std::cout<<"Translation: ("<<transX<<", "<<transY<<", "<<transZ<<")"<<std::endl;
    std::cout<<"Rotation: ("<<axisX<<", "<<axisY<<", "<<axisZ<<", "<<thetaMRad<<" mrad)"<<std::endl;
    TVector3 axis;
    axis.SetXYZ(axisX, axisY, axisZ);
    // DSECal doesn't have Y-difference; unsure about P0D
    if (subDetName != "DSECal" && subDetName != "P0D")
    {
    // Dean's rotation matrix
    trackerRotMatrix[0] = 2*pow(sin(thetaMRad/2000),2)*pow(axis[0],2) + cos(thetaMRad/1000);
    trackerRotMatrix[1] = 2*pow(sin(thetaMRad/2000),2)*axis[0]*axis[1] - sin(thetaMRad/1000)*axis[2];
    trackerRotMatrix[2] = 2*pow(sin(thetaMRad/2000),2)*axis[0]*axis[2] + sin(thetaMRad/1000)*axis[1];
    trackerRotMatrix[3] = 2*pow(sin(thetaMRad/2000),2)*axis[0]*axis[1] + sin(thetaMRad/1000)*axis[2];
    trackerRotMatrix[4] = 2*pow(sin(thetaMRad/2000),2)*pow(axis[1],2) + cos(thetaMRad/1000);
    trackerRotMatrix[5] = 2*pow(sin(thetaMRad/2000),2)*axis[1]*axis[2] - sin(thetaMRad/1000)*axis[0];
    trackerRotMatrix[6] = 2*pow(sin(thetaMRad/2000),2)*axis[0]*axis[2] - sin(thetaMRad/1000)*axis[1];
    trackerRotMatrix[7] = 2*pow(sin(thetaMRad/2000),2)*axis[1]*axis[2] + sin(thetaMRad/1000)*axis[0];
    trackerRotMatrix[8] = 2*pow(sin(thetaMRad/2000),2)*pow(axis[2],2) + cos(thetaMRad/1000);
    for (int i = 0; i < 9; i+=3)
    {
    std::cout<<"Rotation matrix elements: [" 
    <<trackerRotMatrix[i] << " " 
    <<trackerRotMatrix[i+1] << " " 
    <<trackerRotMatrix[i+2] << "]" <<std::endl;
    }
    
    // Quantities to offset the 25mm shift in Y for trackers
    offsetX = trackerRotMatrix[1]*-25;
    offsetY = (trackerRotMatrix[4]-1)*-25;
    offsetZ = trackerRotMatrix[7]*-25;
    
    // Inform the offsets
    std::cout<<"On " <<name->first<<", offsets are: ("
    <<offsetX<<", "
    <<offsetY<<", "
    <<offsetZ<<")"<<std::endl;
    
    fAligTable.AddTranslationRotation(
    subDetId[subDetName],
    offsetX + transX, 
    offsetY + transY, 
    offsetZ + transZ, 
    axis, 
    thetaMRad);
    }
    else 
    {
    fAligTable.AddTranslationRotation(
    subDetId[subDetName],
    transX, 
    transY, 
    transZ, 
    axis, 
    thetaMRad);
    }
    }	
    alignLookupTracker.close();
    }
    else
    std::cout<<"Survey alignment text " <<inputSurveyAlignTxt<<" not found, alignment not applied"<<std::endl;	
  */
  for(std::map<std::string, COMET::IGeometryId>::iterator name = subDetId.begin(); name != subDetId.end(); name++)
    {
      TVector3 rotAxis = surveyRotConst[name->first].first;
      TVector3 trans = surveyTransConst[name->first];
      double thetaMRad = surveyRotConst[name->first].second/1000;
      double mag = rotAxis.Mag();
      rotAxis = TVector3(rotAxis.X()/mag,
       rotAxis.Y()/mag,
       rotAxis.Z()/mag);
      
      std::cout<<"Modifying "<<name->first<<" geometry..."<<std::endl;
      std::cout<<"Translation: ("<<trans.X()<<", "<<trans.Y()<<", "<<trans.Z()<<")"<<std::endl;
      std::cout<<"Rotation: ("<<rotAxis.X()<<", "<<rotAxis.Y()<<", "<<rotAxis.Z()<<", "<<surveyRotConst[name->first].second<<" mrad)"<<std::endl;
      
      if (name->first != "DSECal" && name->first != "P0D")
  {
    // Dean's rotation matrix
    trackerRotMatrix[0] = 2*pow(sin(thetaMRad/2),2)*pow(rotAxis.X(),2) + cos(thetaMRad);
    trackerRotMatrix[1] = 2*pow(sin(thetaMRad/2),2)*rotAxis.X()*rotAxis.Y() - sin(thetaMRad)*rotAxis.Z();
    trackerRotMatrix[2] = 2*pow(sin(thetaMRad/2),2)*rotAxis.X()*rotAxis.Z() + sin(thetaMRad)*rotAxis.Y();
    trackerRotMatrix[3] = 2*pow(sin(thetaMRad/2),2)*rotAxis.X()*rotAxis.Y() + sin(thetaMRad)*rotAxis.Z();
    trackerRotMatrix[4] = 2*pow(sin(thetaMRad/2),2)*pow(rotAxis.Y(),2) + cos(thetaMRad);
    trackerRotMatrix[5] = 2*pow(sin(thetaMRad/2),2)*rotAxis.Y()*rotAxis.Z() - sin(thetaMRad)*rotAxis.X();
    trackerRotMatrix[6] = 2*pow(sin(thetaMRad/2),2)*rotAxis.X()*rotAxis.Z() - sin(thetaMRad)*rotAxis.Y();
    trackerRotMatrix[7] = 2*pow(sin(thetaMRad/2),2)*rotAxis.Y()*rotAxis.Z() + sin(thetaMRad)*rotAxis.X();
    trackerRotMatrix[8] = 2*pow(sin(thetaMRad/2),2)*pow(rotAxis.Z(),2) + cos(thetaMRad);
    for (int i = 0; i < 9; i+=3)
      {
        std::cout<<"Rotation matrix elements: [" 
           <<trackerRotMatrix[i] << " " 
           <<trackerRotMatrix[i+1] << " " 
           <<trackerRotMatrix[i+2] << "]" <<std::endl;
      }
    
    // Quantities to offset the 25mm shift in Y for trackers
    TVector3 offset(trackerRotMatrix[1]*-25, (trackerRotMatrix[4]-1)*-25, trackerRotMatrix[7]*-25);
    
    // Inform the offsets
    std::cout<<"On " <<name->first<<", offsets are: ("
       <<offset.X()<<", "
       <<offset.Y()<<", "
       <<offset.Z()<<")"<<std::endl;
    
    fAligTable.AddTranslationRotation(name->second,
              offset + trans, 
              rotAxis, 
              surveyRotConst[name->first].second);
  }
      else 
  {
    fAligTable.AddTranslationRotation(name->second,
              trans, 
              rotAxis, 
              surveyRotConst[name->first].second);
  }
    }
}


/// Main part of the program. 
/// Declares geometry ID's for each sub-detector and modifies geometry.
/// It has 3 switches to control the alignment: survey, FGD internal, and TPC mm survey.
bool IApplyAlign::AlignGeometry()
{
  /////////////////////////////////////////////////////////////////////////////
  // Specify input alignment text files here
  //inputFGDIntAlignTxt = "Itr0.txt";
  //inputMMSurveyAlignTxt = "TPCMM.txt";
  //inputSurveyAlignTxt = "Survey.txt";
  /////////////////////////////////////////////////////////////////////////////
  
  // Get the fresh geometry from file
  //COMET::IOADatabase oldBase = COMET::IOADatabase::Get();
  COMET::IOADatabase::Get().UpdateGeometry();
  /* This has to be updated for COMET sub-detectors
  // Declaring sub-detectors
  subDetId["TPC1"] = COMET::GeomId::TPC::TPC1();
  subDetId["TPC2"] = COMET::GeomId::TPC::TPC2();
  subDetId["TPC3"] = COMET::GeomId::TPC::TPC3();
  subDetId["FGD1"] = COMET::GeomId::FGD::FGD1();
  subDetId["FGD2"] = COMET::GeomId::FGD::FGD2();
  subDetId["DSECal"] = COMET::GeomId::DSECal::Detector();
  subDetId["P0D"] = COMET::GeomId::P0D::Detector();
  
  // Declaring P0D components - not useful right now
  for (int i = 0; i < 4; i++)
    {
      superP0Dule[i] = COMET::GeomId::P0D::SuperP0Dule(i);
    }
  for (int i = 0; i < 40; i++)
    {
      p0dule[i] = COMET::GeomId::P0D::P0Dule(i);
    }
  
  // Declaring DSECal layers
  for (int i = 0; i < 34; i++)
    {
      dsEcalLayer[i] = COMET::GeomId::DSECal::Layer(i);
    }
  
  // Declaring FGD layers
  for (int i = 0; i < 44; i++)
    {
      if (i < 30 && i%2 == 0)
  fgdLayerId[i] = COMET::GeomId::FGD::Layer(0, i/2, 0);
      else if (i < 30)
  fgdLayerId[i] = COMET::GeomId::FGD::Layer(0, (i-1)/2, 1);
      else if (i%2 == 0)
  fgdLayerId[i] = COMET::GeomId::FGD::Layer(1, (i-30)/2, 0);
      else
  fgdLayerId[i] = COMET::GeomId::FGD::Layer(1, (i-31)/2, 1);
    }	
  
  // Declaring TPC MM's
  for (int i = 0; i < 3; i++)
    {
      for (int j = 0; j < 2; j++)
  {
    for (int k = 0; k < 12; k++)
      {	      
        tpcMMId[i][j][k] = COMET::GeomId::TPC::MicroMega(i, j, k);
      }
  }
    }
  
  // Give original geometry information
  std::cout<<"*******************************************"<<std::endl;
  std::cout<<"Preparing to apply alignment..."<<std::endl;
  
  
  // Check conditions and apply alignment constants. 
  // Do survey alignment first before applying internal alignment;
  
  if (applyTrackerSurvey)
    {
      std::cout<<"Applying tracker survey alignment..."<<std::endl;
      ApplyTrackerSurveyAlign(); 
    }
  else
    std::cout<<"Tracker survey alignment not applied"<<std::endl;
  
  if (applyMMSurvey)
    {
      std::cout<<"Applying Micromegas survey alignment..."<<std::endl;
      
      // Use my revised method
      ApplyMMSurveyAlign();
      // ApplyMMSurveyAlignGlobal();
    }	
  else
    std::cout<<"Micromegas survey alignment not applied"<<std::endl;
  
  if (applyFGDInternal)
    {
      std::cout<<"Applying FGD internal alignment..."<<std::endl;
      ApplyFGDInternalAlign();
    }	
  else
    std::cout<<"FGD Internal alignment not applied"<<std::endl;

  // Registers this alignment table to modify geometry
 
  COMET::IOADatabase::Get().RegisterAlignmentLookup(&fAligTable);
  COMET::IOADatabase::Get().AlignGeometry(NULL);
  
  // Do an overlap check
  std::cout<<"Checking for overlaps in modified geometry..."<<std::endl;
  
  gGeoManager->CheckOverlaps(0.1);
  TIter next(gGeoManager->GetListOfOverlaps());
  TGeoOverlap* overlap;
   int count = 0;
   while ( (overlap = (TGeoOverlap*)next()) ) 
     {
      count++;
    }
  
  if (count==0)
    {
      std::cout<<"No overlaps found, preserving modified geometry"<<std::endl;
      return true;
    }
  else
    {
      std::cout<<"Overlap found, reverting to original geometry"<<std::endl;
      // This does the trick.
      //oldBase.Geometry();
      return false;
    }
  */
}