/** @ MilleTool.cpp 
 *
 *  Interface to Millepede
 *
 */

#include "MilleTool.h"
//#include "constants.h"
//#include "constantsLMD.h"
#include "constantsTelescope.h"// description of test set-up in March 2015
#include <iostream>

using namespace std;

bool MilleTool::initialize(int paramnum, std::string infile, std::vector<int> stations){
  ntrk=0;//total number of executed tracks
  // in.open("/data/FAIRsorf/pandaroot/karavdina/myKnossos/TracksForAligment.txt");
  in.open(infile.c_str());
  in.seekg (0, in.beg);//start file from the begining
  // init phase
  pglSize = paramnum;
  const int pSize = pglSize; // Number of elements to align
  cout<<"... MilleTool::initialize ... "<<endl;
  cout<<"Data from file: "<<infile.c_str()<<endl;
  Xmeas.resize(pSize);
  Ymeas.resize(pSize);
  Zmeas.resize(pSize);
  errXmeas.resize(pSize);
  errYmeas.resize(pSize);
  errZmeas.resize(pSize);
  usedSens.resize(stations.size());
  for(int ik=0;ik<stations.size();ik++)
    usedSens[ik] = stations[ik];
  // Global parameters vectors
  //
  // !!! All is dimensionned as if we want to find all the 6DOFs
  // But we will inform Millepede of the DOFs to take into account

  bool   dofs[6];
  double Sigm[6];
  double res_cut, res_cut_init, start_chi_cut;

  res_cut       = m_res_cut; 
  res_cut_init  = m_res_cut_init;
  start_chi_cut = m_start_chi_cut;

  for (int i = 0; i<6; ++i)
  {
    dofs[i] = m_dofs[i];
    Sigm[i] = m_Sigm[i];
  }

  m_par.resize(6*pSize);
  m_error.resize(6*pSize);
  m_pull.resize(6*pSize);

  m_alignTool.InitMille(&dofs[0], &Sigm[0], pSize, 4, 
			start_chi_cut, 3, res_cut, res_cut_init);

  //
  // Constraint equation on the X, Y, and Z translations (prevent global translation and shearing)
  //

  // First of all the Global Translation

  double Const_TX[6*pSize];
  double Const_TY[6*pSize];
  double Const_TZ[6*pSize];
  double Const_RX[6*pSize];
  double Const_RY[6*pSize];
  double Const_RZ[6*pSize];

  //double Const_R1plZ[6*pSize];

  for (int i = 0; i< 6*pSize; ++i)
  {
    Const_TX[i] = 0.0;
    Const_TY[i] = 0.0;
    Const_TZ[i] = 0.0;
    Const_RX[i] = 0.0;
    Const_RY[i] = 0.0;
    Const_RZ[i] = 0.0;
    //  Const_R1plZ[i]=0.0;
  }
  
  for (int i = 0; i< pSize; ++i)
  {
    Const_TX[i]         = 1.0;
    Const_TY[i+pSize]   = 1.0;
    Const_TZ[i+2*pSize] = 1.0;
    Const_RX[i+3*pSize] = 1.0;
    Const_RY[i+4*pSize] = 1.0;
    Const_RZ[i+5*pSize] = 1.0;
    //   if(i==0) Const_R1plZ[i+5*pSize]=1.0;
  }

  // Feed Millepede (only if requested)  
  
  if (dofs[0]) m_alignTool.ConstF(&Const_TX[0],  0.0);   
  if (dofs[1]) m_alignTool.ConstF(&Const_TY[0],  0.0); 
  if (dofs[2]) m_alignTool.ConstF(&Const_TZ[0],  0.0);
  if (dofs[3]) m_alignTool.ConstF(&Const_RX[0],  0.0);
  if (dofs[4]) m_alignTool.ConstF(&Const_RY[0],  0.0);
  if (dofs[5]) m_alignTool.ConstF(&Const_RZ[0],  0.0);

  // m_alignTool.ConstF(&Const_R1plZ[0],  0.0);//fix 1st plane from rotation

  // Then the shearings and scalings 

//  double zplanes[32]={0,0,0,0,0,0,0,0,10,10,10,10,10,10,10,10,20,20,20,20,20,20,20,20,30,30,30,30,30,30,30,30};
  // double zplanes[4]={0,10,20,30};
 double z_moy = 0;
 double sigma_z_moy = 0;

 //take into account shift in z for diff planes
 // z_average  
for (unsigned int jp=0; jp<pSize; ++jp) z_moy += zplanes[jp];
 z_moy /= float(pSize);
 
 // Error on z_moy
for (unsigned int jp=0; jp<pSize; ++jp) sigma_z_moy += (zplanes[jp]-z_moy)*
							 (zplanes[jp]-z_moy);
 sigma_z_moy /= float(pSize);
 // sigma_z_moy /= 4.;
  
  // constraint on the shearing
  double Const_SX[6*pSize];
  double Const_SY[6*pSize];
  double Const_FZ[6*pSize];

  for (int i = 0; i< 6*pSize; ++i)
  {
    Const_SX[i] = 0.0;
    Const_SY[i] = 0.0;
    Const_FZ[i] = 0.0;
  }

  // cout<<"sigma_z_moy = "<<sigma_z_moy<<" z_moy = "<<z_moy<<endl;
  for (int i = 0; i< pSize; ++i){
    //sigma_z_moy = 0.0075;
    //    cout<<"zplanes["<<i<<"]="<<zplanes[i]<<endl;
    Const_SX[i]         = (zplanes[i]-z_moy)/sigma_z_moy;
    Const_SY[i+pSize]   = (zplanes[i]-z_moy)/sigma_z_moy;
    Const_FZ[i+2*pSize] = (zplanes[i]-z_moy)/sigma_z_moy;
    //   cout<<" Const_SX["<<i<<"]="<<Const_SX[i]<<endl;
  }

  // Feed Millepede (only if requested)  
  
  if (dofs[0]) m_alignTool.ConstF(&Const_SX[0],  0.0);   
  if (dofs[1]) m_alignTool.ConstF(&Const_SY[0],  0.0); 
  if (dofs[2]) m_alignTool.ConstF(&Const_FZ[0],  0.0);
  
  // this is it! Millepede is now ready to align...

  return true;
}


// void MilleTool::CloseFile(){
//   in.close();
// }

// add a track to millipede

//bool MilleTool::addTrack(Track trueTrack)
bool MilleTool::addTrack()
{
  const int pSize = pglSize; // Number of elements to align
  //  cout << "Into addtrack" << endl;

  //  const int nStation = pSize; //total number of sensors
  // cout<<"pglSize = "<<pglSize<<endl;
  double m_derGB[6*pSize];
  double m_derNonLin[6*pSize];
  double m_derNonLin_ind[6*pSize];
  double m_derLC[4];

    
  for (int i=0; i<6*pSize; ++i)  m_derGB[i] = 0.;
  for (int i=0; i<6*pSize; ++i)  m_derNonLin[i] = 0.;
  for (int i=0; i<6*pSize; ++i)  m_derNonLin_ind[i] = 0.;
  for (int i=0; i<4; i++)        m_derLC[i] = 0.;

  for (int iStation = 0; iStation < pSize; ++iStation)
  { 
    Xmeas[iStation] = 0.;
    Ymeas[iStation] = 0.;
    Zmeas[iStation] = 0.;
    errXmeas[iStation] = 0.;
    errYmeas[iStation] = 0.;
    errZmeas[iStation] = 0.;
  }

  // Add the track measurements
  Double_t xhit,yhit,zhit,errxhit,erryhit,errzhit,sensorID;
  int trkend=0;
  
  if(!in.eof()){
    while (trkend!=1 && !in.eof()) {
      in>>xhit>>errxhit>>yhit>>erryhit>>zhit>>errzhit>>sensorID>>trkend;
      if(in.eof())
	return false;
      bool rightID = false;
      for(int iid=0;iid<usedSens.size();iid++)
	if(sensorID==usedSens[iid]) rightID = true;
      if(!rightID) continue;
      int senID = sensorID;
      Xmeas[senID] = xhit;
      errXmeas[senID] = errxhit;
      Ymeas[senID] = yhit;
      errYmeas[senID] = erryhit;
      Zmeas[senID] = zhit;
      //Zmeas[senID] = zhit+zplanes[senID];//TEST
      errZmeas[senID] = errzhit;
      //    cout<<"modID = "<<senID<<" X,Y,Z = "<<Xmeas[senID]<<" "<<Ymeas[senID]<<" "<<Zmeas[senID]<<" zpl="<<zplanes[senID]<<endl;
      //
      // Then we add hit of the track to Millepede
      //
      m_alignTool.ZerLoc(&m_derGB[0],&m_derLC[0],&m_derNonLin[0],&m_derNonLin_ind[0]); // Reset the derivatives arrays
      
      // Add the local derivatives for the X part of the track
      
      m_derLC[0]= 1.0;
      m_derLC[1] = Zmeas[senID];
      m_derLC[2]= 0.0;
      m_derLC[3]= 0.0;
      
      // Then add the global one
      //      if(senID>0){
      if (m_dofs[0]) m_derGB[senID]         = -1.0;
      if (m_dofs[1]) m_derGB[senID+pSize]   = 0.0;
      if (m_dofs[2]) m_derGB[senID+2*pSize] = 0.0;
      if (m_dofs[3]) m_derGB[senID+3*pSize] = 0.0;
      if (m_dofs[4]) m_derGB[senID+4*pSize] = 0.0;
      if (m_dofs[5]) m_derGB[senID+5*pSize] = Ymeas[senID];
      //      }
      //
      // Specific treatment for non-linear terms
      //
      
      // First the non-linear global derivatives
      //            if(senID>0){
      if (m_dofs[0]) m_derNonLin[senID]         = 0.0;
      if (m_dofs[1]) m_derNonLin[senID+pSize]   = 0.0;
      if (m_dofs[2]) m_derNonLin[senID+2*pSize] = 1.0;
      if (m_dofs[3]) m_derNonLin[senID+3*pSize] = Ymeas[senID];  
      if (m_dofs[4]) m_derNonLin[senID+4*pSize] = Xmeas[senID];
      if (m_dofs[5]) m_derNonLin[senID+5*pSize] = 0.0;
      //    }
      // Then to which local parameter they are related (here the slope in X)
      //    if(senID>0){
      if (m_dofs[0]) m_derNonLin_ind[senID]         = 0.0;
      if (m_dofs[1]) m_derNonLin_ind[senID+pSize]   = 0.0;
      if (m_dofs[2]) m_derNonLin_ind[senID+2*pSize] = 1.0;
      if (m_dofs[3]) m_derNonLin_ind[senID+3*pSize] = 1.0;  
      if (m_dofs[4]) m_derNonLin_ind[senID+4*pSize] = 1.0;
      if (m_dofs[5]) m_derNonLin_ind[senID+5*pSize] = 0.0;
      //    }
      // And we put the X equation in Millepede
      
      m_alignTool.EquLoc(&m_derGB[0], &m_derLC[0], &m_derNonLin[0], &m_derNonLin_ind[0]
			 , Xmeas[senID], errXmeas[senID]); // Store hits parameters
      
      m_alignTool.ZerLoc(&m_derGB[0],&m_derLC[0],&m_derNonLin[0], &m_derNonLin_ind[0]); // Reset the derivatives arrays
      
      // Add the local derivatives for the Y part of the track
      
      m_derLC[0]= 0.0;
      m_derLC[1]= 0.0;
      m_derLC[2]= 1.0;
      m_derLC[3] = Zmeas[senID];
      
      // Then add the global one
      //            if(senID>0){
      if (m_dofs[0]) m_derGB[senID]            = 0.0;
      if (m_dofs[1]) m_derGB[senID+pSize]   = -1.0;
      if (m_dofs[2]) m_derGB[senID+2*pSize] = 0.0;
      if (m_dofs[3]) m_derGB[senID+3*pSize] = 0.0;
      if (m_dofs[4]) m_derGB[senID+4*pSize] = 0.0;
      if (m_dofs[5]) m_derGB[senID+5*pSize] = -Xmeas[senID];
      //    }
      //
      // Specific treatment for non-linear terms
      //
      
      // First the non-linear global derivatives
      // if(senID>0){
      if (m_dofs[0]) m_derNonLin[senID]            = 0.0;
      if (m_dofs[1]) m_derNonLin[senID+pSize]   = 0.0;
      if (m_dofs[2]) m_derNonLin[senID+2*pSize] = 1.0;
      if (m_dofs[3]) m_derNonLin[senID+3*pSize] = Ymeas[senID];  
      if (m_dofs[4]) m_derNonLin[senID+4*pSize] = Xmeas[senID];
      if (m_dofs[5]) m_derNonLin[senID+5*pSize] = 0.0;
      //	    }
      // Then to which local parameter they are related (here the slope in Y)
      //    if(senID>0){
      if (m_dofs[0]) m_derNonLin_ind[senID]            = 0.0;
      if (m_dofs[1]) m_derNonLin_ind[senID+pSize]   = 0.0;
      if (m_dofs[2]) m_derNonLin_ind[senID+2*pSize] = 3.0;
      if (m_dofs[3]) m_derNonLin_ind[senID+3*pSize] = 3.0;  
      if (m_dofs[4]) m_derNonLin_ind[senID+4*pSize] = 3.0;
      if (m_dofs[5]) m_derNonLin_ind[senID+5*pSize] = 0.0;
      //    }
      // And we put the Y equation in Millepede
      
      m_alignTool.EquLoc(&m_derGB[0], &m_derLC[0], &m_derNonLin[0], &m_derNonLin_ind[0]
			 , Ymeas[senID], errYmeas[senID]); // Store hits parameters 
     


    }//end read hits from one trk
    
    // local fit of the track
    double track_params[10];     // Vector containing the track parameters+errors 
    for (int i=0; i<10; ++i) track_params[i] = 0.;      
    bool sc = m_alignTool.FitLoc(m_alignTool.GetTrackNumber(),&track_params[0],0);
    if (sc) m_alignTool.SetTrackNumber(m_alignTool.GetTrackNumber()+1);
    //    if (debug_mode)   
    //    cout<<"Track #"<<ntrk<<" was read!"<<endl;
    ntrk++;
    return true;
  }
  else{//end read all trks
    cout<<"File has only "<<ntrk<<" tracks!"<<endl;
    return false;
  }
  // m_nTrack = ntrk;
}


void MilleTool::reset() {
  //m_nTrack = 0;
  // in.seekg(0, std::ios::beg); //start file from the begining
  in.close();
  //  in.seekg (0, in.beg);//start file from the begining
}

bool MilleTool::globalFit(){

  // final global fit
  m_alignTool.MakeGlobalFit(&m_par[0],&m_error[0],&m_pull[0]);

  return true;
}

const std::vector<double>& MilleTool::parameters() const{
  return m_par;
}

const std::vector<double>& MilleTool::errors() const{
  return m_error;
}

const std::vector<double>& MilleTool::pulls() const{
  return m_pull;
}