/** @ myAlg.cpp 
 *
 *  The main alignment code
 *  
 *  Interface with Millepede is made by MilleTool
 *
 *  @author M.Needham
 */

#include <iostream>
#include <iomanip>

// For ROOT

#include "TFile.h"
#include "TH1F.h"
#include "TH2F.h"
#include "TF1.h"

// Knossos includes

#include "LMDAlg.h"
#include "MilleTool.h"
//#include "Track.h"
//#include "TrackStore.h"
//#include "constantsLMD.h"
#include "constantsTelescope.h"// description of test set-up in March 2015


using namespace std;

LMDAlg::LMDAlg(){
 //  :m_rootFile(0),m_mis_aft(0),m_pullHistos(0)
  // constructer
  // m_mis_aft = new TObjArray;
  // m_pullHistos = new TObjArray;
}

LMDAlg::~LMDAlg()
{
 // // destructer
 // delete m_GaussGenerator;
  delete m_align;
  // delete m_rootFile;
}

/*
  INITIALIZE:  Initialization of the ROOTfile and the detector geometry
*/

bool LMDAlg::initialize(const jobParams& tparams)
{
  m_params = tparams;
  output.open(params().outputFile().c_str());

  //  The tool to access millepede
  m_align  = new MilleTool(); 
  return true;	  
}


/*
  EXECUTE:  
*/

bool LMDAlg::execute()
{
  const int totnumsensors = nStation;
  //  const int totnumsensors = nStation*nSides;
  //cout<<"m_params.nRun() = "<<m_params.nRun()<<endl;
  //  for (unsigned int iRun = 0;  iRun < m_params.nRun(); ++iRun)
  for (unsigned int iRun = 0;  iRun <1; ++iRun) 
  {
    //
    // Millepede tool is initialized
    //
    vector<int> uSens;
    for(int ik=0; ik<totnumsensors;ik++)
      uSens.push_back(Stations[ik]);
    m_align->initialize(totnumsensors,m_params.inputFile(),uSens);
    //  int iteration = 0; 
    bool readtrks=true;
    int nTrks_in = 0;
    for(int idtrk=0;idtrk<m_params.nEvent();idtrk++){
      if(!readtrks) break;
      readtrks = m_align->addTrack();
      nTrks_in++;
    }

    //
    // ...finally make the alignment and print the results
    //
    m_align->globalFit(); 
  
    const std::vector<double>& par    = m_align->parameters();
    const std::vector<double>& errors = m_align->errors(); 
    const std::vector<double>& pulls  = m_align->pulls(); 

    int nPlane = totnumsensors;


    cout<<"Alignment based on "<<nTrks_in<<" input tracks"<<endl;
    std::cout << "" << std::endl;
    std::cout << "---------------------------------------------" << std::endl;
    std::cout << "|              FINAL RESULT                 |" << std::endl;
    std::cout << "---------------------------------------------" << std::endl;
    std::cout << "|  Recon.  |  Error   |   Cov.   |   Pull   |" << std::endl;
    //  std::cout << "|  Gener.  |  Recon.  |   Diff.  |   Error  |    Cov.  |" << std::endl;
    std::cout << "-----------------------------------" << std::endl;

    for (unsigned int iPlane = 0; iPlane < totnumsensors ; ++iPlane)
    { 
      std::cout << "| " << std::setw(8) << par[iPlane]
  		<< " | " << std::setw(8) << errors[iPlane]
  		<< " | " << std::setw(8) << sqrt(fabs(errors[iPlane])) 
		<< " | " << std::setw(8) << pulls[iPlane] << " |" << std::endl;

      std::cout << "| " << std::setw(8) <<  par[iPlane+nPlane] 
      		<< " | " << std::setw(8) << errors[iPlane+nPlane]
      		<< " | " << std::setw(8) << sqrt(fabs(errors[iPlane+nPlane]))
      		<< " | " << std::setw(8) << pulls[iPlane+nPlane] << " |" << std::endl;

      std::cout << "| " << std::setw(8) << par[iPlane+2*nPlane] 
      		<< " | " << std::setw(8) << errors[iPlane+2*nPlane]
      		<< " | " << std::setw(8) << sqrt(fabs(errors[iPlane+2*nPlane]))
      		<< " | " << std::setw(8) << pulls[iPlane+2*nPlane] << " |" << std::endl;

      std::cout << "| " << std::setw(8) << par[iPlane+3*nPlane] 
      		<< " | " << std::setw(8) << errors[iPlane+3*nPlane]
      		<< " | " << std::setw(8) << sqrt(fabs(errors[iPlane+3*nPlane]))
      		<< " | " << std::setw(8) << pulls[iPlane+3*nPlane] << " |" << std::endl;

      std::cout << "| " << std::setw(8) << par[iPlane+4*nPlane] 
      		<< " | " << std::setw(8) << errors[iPlane+4*nPlane]
      		<< " | " << std::setw(8) << sqrt(fabs(errors[iPlane+4*nPlane]))
      		<< " | " << std::setw(8) << pulls[iPlane+4*nPlane] << " |" << std::endl;

      std::cout << "| " << std::setw(8) << par[iPlane+5*nPlane] 
      		<< " | " << std::setw(8) << errors[iPlane+5*nPlane]
      		<< " | " << std::setw(8) << sqrt(fabs(errors[iPlane+5*nPlane]))
      		<< " | " << std::setw(8) << pulls[iPlane+5*nPlane] << " |" << std::endl;
      std::cout << "---------------------------------------------" << std::endl;
      
    } //iPlan
    }

  //save results in txt file
  const std::vector<double>& par    = m_align->parameters();
  const std::vector<double>& errors = m_align->errors(); 
  const std::vector<double>& pulls  = m_align->pulls(); 
  int nPlane = totnumsensors;
  //  const int nTrks = m_align->GetNTracks();
//FOR test with Telescope, March 2015
  output<<" Software alignment sugests following correction for translation [cm] and rotation [rad]:" << std::endl;
  for (unsigned int iPlane = 0; iPlane < totnumsensors ; ++iPlane)
    { 
      // //FOR normal LMD/PANDAROOT
      // output<<par[iPlane]<<" "<<errors[iPlane]<<" "<< sqrt(fabs(errors[iPlane])) <<" "<< pulls[iPlane]<< std::endl;
      // output<<par[iPlane+nPlane]<<" "<< errors[iPlane+nPlane]<<" "<< sqrt(fabs(errors[iPlane+nPlane])) <<" "<< pulls[iPlane+nPlane]<< std::endl;
      // output<< par[iPlane+2*nPlane]<<" "<< errors[iPlane+2*nPlane]<<" "<< sqrt(fabs(errors[iPlane+2*nPlane])) <<" "<< pulls[iPlane+2*nPlane]<< std::endl;
      // output<< par[iPlane+3*nPlane]<<" "<< errors[iPlane+3*nPlane]<<" "<< sqrt(fabs(errors[iPlane+3*nPlane])) <<" "<< pulls[iPlane+3*nPlane]<< std::endl;
      // output<< par[iPlane+4*nPlane]<<" "<< errors[iPlane+4*nPlane]<<" "<< sqrt(fabs(errors[iPlane+4*nPlane])) <<" "<< pulls[iPlane+4*nPlane]<< std::endl;
      // output<< par[iPlane+5*nPlane]<<" "<< errors[iPlane+5*nPlane]<<" "<< sqrt(fabs(errors[iPlane+5*nPlane])) <<" "<< pulls[iPlane+5*nPlane]<< std::endl;
      // output<<" "<<std::endl;

 //FOR test with Telescope, March 2015
      output<<"Plane #"<<iPlane<< std::endl;
      output<<" positionX = "<<par[iPlane]<<" +/- "<< errors[iPlane] <<""<< std::endl;
      output<<" positionY = "<<par[iPlane+nPlane]<<" +/- "<< errors[iPlane+nPlane] <<""<< std::endl;
      output<<" positionZ = "<< par[iPlane+2*nPlane]<<" +/- "<< errors[iPlane+2*nPlane] <<""<< std::endl;
      output<<" rotationX = "<< par[iPlane+3*nPlane]<<" +/- "<< errors[iPlane+3*nPlane] <<""<< std::endl;
      output<<" rotationY = "<< par[iPlane+4*nPlane]<<" +/- "<< errors[iPlane+4*nPlane] <<""<< std::endl;
      output<<" rotationZ = "<< par[iPlane+5*nPlane]<<" +/- "<< errors[iPlane+5*nPlane] <<""<< std::endl;
      output<<" "<<std::endl;
    } //iPlan
  // output<<"used "<<nTrks<<" tracks"<<endl;
  output.close();
  m_align->reset(); 
  return true;
}


/*
  FINALIZE:  
*/

bool LMDAlg::finalize()
{
  // m_rootFile->Write();	 
  std::cout << "*** Thats all folks ***" <<std::endl;	 
  return true;
}