//-----------------------------------------------------------
// File and Version Information:
// $Id$
//
// Description:
//     Class to align tpc with respect to external tracks
//
//
// Environment:
//      Software NOT developed for the PANDA Detector at FAIR.
//
// Author List:
//      Alexander Schmah      TUM            (original author)
//      Maxence Vandenbroucke TUM            (author)
//      Francesco Cusanno     TUM            (author)
//      Sverre Doerheim       TUM            (author, rewrite from macro to compiled object)
//
//
//-----------------------------------------------------------
#include <TH1D.h>
#include <TH2D.h>
#include <TMinuit.h>
#include <TString.h>
#include <TFile.h>
#include <TTree.h>

#include "GFTrack.h"
#include "GFFieldManager.h"

#include "FairField.h"
#include "FairRunAna.h"

#include "PndFieldAdaptor.h"

#include "TpcAlignment.h"
#include "TpcRefGFTrkResCalc.h"
#include "TpcRefGFTrkTpcClusterResCalc.h"
#include "TpcCdcFit2DResCalc.h"

#include "BatAlignmentModel.h"
#include "BAT/BCEngineMCMC.h"
#include "BAT/BCLog.h"
#include "BAT/BCModelOutput.h"
#include "TpcShieldPoint.h"
#include "TpcAlignmentHelper.h"

#include <stdlib.h>
#include <omp.h>
#include <sstream>
#include <fstream>

//##############################################
//# initialization of static variables
//##############################################
double TpcAlignment::meanZ=0.;
std::string TpcAlignment::alignmentFile=(TString(getenv("VMCWORKDIR"))+TString("tpc/FOPI/par/june11_alignment_gem0.txt")).Data();
std::vector<TpcResidual*> TpcAlignment::fResiduals;
std::vector<TpcSimpleResidual*> TpcAlignment::fSimpleResiduals;
TClonesArray TpcAlignment::fTCResiduals("TpcResidual");
std::string TpcAlignment::detectorName="tpc";
int TpcAlignment::nCallsFcn=0;
bool TpcAlignment::fcdc3Dcalculation(false);
TVector3 TpcAlignment::tpcpos_(0,0,0);
TVector3 TpcAlignment::dtpcpos_(0,0,0);
double TpcAlignment::dphi_=0.;
double TpcAlignment::dtheta_=0.;
double TpcAlignment::dpsi_=0.;
double TpcAlignment::phi_=0.;
double TpcAlignment::theta_=0.;
double TpcAlignment::psi_=0.;
TGeoCombiTrans* TpcAlignment::lastTransP1P2=new TGeoCombiTrans();

//##############################################



TpcAlignment::TpcAlignment():fUseBat(false), maxRes(40),maxResXY(3.5),maxResZ(8),
    tpcTrackBranchName("TpcTrackPostFit"),
    cdcTrackBranchName("CdcTrack"),
    cdcGFTrackBranchName("CdcTrackPostFit"),
    tpcClusterBranchName("TpcCluster"),
    tpcShieldPointBranchName("TpcShieldPoint"),
    alMan(NULL),
    inititalTpcPos(0,0,0),initialPhi(0),initialTheta(0),initialPsi(0),inputChain(NULL),
    oldtpcxyz(""),residualCalculator(NULL),maxEvents(0),tpcClusters(NULL),cdcTracks(NULL),residualCalculatorType(""),
    testRun(false),storeResiduals(true)
{
    // residualCalculator = new TpcRefGFTrkResCalc();

}

bool TpcAlignment::init(){

    if(residualCalculator==NULL) {
        std::cerr<<"TpcAlignment::init residual calculator missing"<<std::endl;
        return false;
    }
    if(inputChain==NULL){
        std::cerr<<"TpcAlignment::init() \n "
                <<"Input chain missing, please execuite setInputChain(chain) before running init"<<std::endl;
        return false;
    }
    cdcTracks=new TClonesArray("CdcTrack");
    cdcGFTracks=new TClonesArray("GFTrack");
    tpcClusters=new TClonesArray("TpcCluster");
    tpcTracks=new TClonesArray("GFTrack");
    tpcSPHits=new TClonesArray("TpcSPHit");
    shieldPoints=new TClonesArray("TpcShieldPoint");

    int test=inputChain->SetBranchAddress(cdcTrackBranchName.c_str(),&cdcTracks);
    if(test<0) {
        std::cerr<< "TpcAlignment::Init() Init CDC Track-array not found!"<< test<<std::endl;
    }else{
        fBranches[cdcTrackBranchName] = cdcTracks;
    }


    test=inputChain->SetBranchAddress(cdcGFTrackBranchName.c_str(),&cdcGFTracks);
    if(test<0) {
        std::cerr<< "TpcAlignment::Init() Init CDC GF Track-array not found!"<< test<<std::endl;
    }else{
        fBranches[cdcGFTrackBranchName] = cdcGFTracks;
    }

    test=inputChain->SetBranchAddress(tpcClusterBranchName.c_str(),&tpcClusters);
    if(test<0) {
        std::cerr<< "TpcAlignment::Init() Init TpcCluster-array not found!"<< test<<std::endl;
    }else{
        fBranches[tpcClusterBranchName] = tpcClusters;
    }

    test=inputChain->SetBranchAddress(tpcTrackBranchName.c_str(),&tpcTracks);
    if(test<0) {
        std::cerr<< "TpcAlignment::Init() Init TpcTrack-array not found!"<< test<<std::endl;
    }else{
        fBranches[tpcTrackBranchName] = tpcTracks;
    }

    test=inputChain->SetBranchAddress("TpcSPHit",&tpcSPHits);
    if(test<0) {
        std::cerr<< "TpcAlignment::Init() Init TpcSPHit-array not found!"<< test<<std::endl;
    }else{
        fBranches["TpcSPHit"] = tpcSPHits;
    }


    test=inputChain->SetBranchAddress("TpcShieldPointAl",&shieldPoints);
    if(test<0) {
        std::cerr<< "TpcAlignment::Init() Init TpcShieldPoint-array not found!"<< test<<std::endl;
    }else{
        std::cout<<"TpcShieldPoints found"<<std::endl;
        fBranches["TpcShieldPointAl"] = shieldPoints;
    }

    //std::cout << "ALMAN..." << std::endl;
    TpcAlignmentManager::init(alignmentFile.c_str());
    alMan = TpcAlignmentManager::getInstance();
    if(alMan==0) {
        std::cerr<< "TpcAlignment::init() Init TpcAlignmentManager:"<< alignmentFile.c_str()<<" not found!"<< std::endl;
        return false;
    }


    //double phi,theta,psi;
    std::cout << "EULER..." << std::endl;
    alMan->getEulerAngles(detectorName, initialPhi, initialTheta, initialPsi);
    std::cout << "TRANS..." << std::endl;
    inititalTpcPos=  alMan->getTranslation(detectorName);
    std::cout << "FILE : " << alignmentFile.c_str() << std::endl;
    std::cout << detectorName<< " TPC xyz("<<inititalTpcPos.X()<< ", "
              <<inititalTpcPos.Y()<< ", "<<inititalTpcPos.Z()<< "). Rot. angles("
             << initialPhi<<", " << initialTheta << ", " << initialPsi<< ")"<<std::endl;

    oldtpcxyz.Form("OLD TPC xyz(%f, %f, %f). Rot. angles(%f, %f, %f)", inititalTpcPos.X(),inititalTpcPos.Y(),inititalTpcPos.Z(),initialPhi,initialTheta,initialPsi);
    //Quality assurance histograms
    TH1D * hresx = new TH1D("hresx","hresx",8000,-maxResXY,maxResXY);
    hresx->SetLineColor(kRed);
    hists["hresx"]=hresx;
    TH1D * hresy = new TH1D("hresy","hresy",8000,-maxResXY,maxResXY);
    hists["hresy"]=hresy;
    hresy->SetLineColor(kRed);
    TH1D * hresPerp = new TH1D("hresPerp","Resolution on Perp plane",8000,0,maxResXY);
    hists["hresPerp"]=hresPerp;
    hresPerp->SetLineColor(kRed);
    TH2D * hresPerpvPerp = new TH2D("hresPerpvPerp","Resolution on XY plane vs Perp",2000,0,40,2000,0,maxResXY);
    hists["hresPerpvPerp"]=hresPerpvPerp;
    TH1D * hresz = new TH1D("hresz","hresz",8000,-maxResZ,maxResZ);
    hists["hresz"]=hresz;
    hresz->SetLineColor(kRed);
    TH1D * hres_trDist = new TH1D("hres_trDist","hres_trDist",80000,-1000,100);
    hists["hres_trDist"]=hres_trDist;
    hres_trDist->SetLineColor(kBlue);
    TH2D* hresPerpvDist = new TH2D("hresPerpvDist","hresPerpvDist",2000,-100,100,2000,0,100);
    hists["hresPerpvDist"]=hresPerpvDist;
    hresPerpvDist->SetLineColor(kBlue);
    TH2D* hresXvX = new TH2D("hresXvX","hresXvX",1000,-20,20,1000,-maxResXY,maxResXY);
    hists["hresXvX"]=hresXvX;
    hresXvX->SetLineColor(kBlue);
    TH2D* hresYvY = new TH2D("hresYvY","hresYvY",1000,-20,20,1000,-maxResXY,maxResXY);
    hists["hresYvY"]=hresYvY;
    hresYvY->SetLineColor(kBlue);
    TH2D* hresZvZ = new TH2D("hresZvZ","hresZvZ",1000,-100,100,1000,-maxResZ,maxResZ);
    hists["hresZvZ"]=hresZvZ;
    hresZvZ->SetLineColor(kBlue);

    TH1D * hnesx = new TH1D("hnesx","hnesx",8000,-maxResXY,maxResXY);
    hists["hnesx"]=hnesx;
    hnesx->SetLineColor(kBlue);
    TH1D * hnesy = new TH1D("hnesy","hnesy",8000,-maxResXY,maxResXY);
    hists["hnesy"]=hnesy;
    hnesy->SetLineColor(kBlue);
    TH1D * hnesPerp = new TH1D("hnesPerp","Resolution on XY plane",4000,0,maxResXY);
    hists["hnesPerp"]=hnesPerp;
    hnesPerp->SetLineColor(kBlue);
    TH1D * hnesz = new TH1D("hnesz","hnesz",8000,-maxResZ,maxResZ);
    hists["hnesz"]=hnesz;
    hnesz->SetLineColor(kBlue);
    TH1D * hnes_trDist = new TH1D("hnes_trDist","hnes_trDist",80000,0,10000);
    hists["hnes_trDist"]=hnes_trDist;
    hnes_trDist->SetLineColor(kBlue);

    TH1D * hitsX = new TH1D("hitsX","hitsX",800,-20,20);
    hists["hitsX"]=hitsX;
    TH1D * hitsY = new TH1D("hitsY","hitsY",800,-20,20);
    hists["hitsY"]=hitsY;
    TH1D * hitsZ = new TH1D("hitsZ","hitsZ",1600,-200,200);
    hists["hitsZ"]=hitsZ;

    TH2D * hitsPerp = new TH2D("hitsPerp","hitsPerp",800,-20,20,800,-20,20);
    hists["hitsPerp"]=hitsPerp;
    TH2D * hnesPerpvPerp = new TH2D("hnesPerpvPerp","Resolution on XY plane vs Perp",2000,0,40,2000,0,maxResXY);
    hists["hnesPerpvPerp"]=hnesPerpvPerp;
    TH2D * hitsZR = new TH2D("hitsZR","hitsZR",1600,-200,200,400,0,20);
    hists["hitsZR"]=hitsZR;

    TH1D * hresXcov = new TH1D("hresXcov","hresXcov",8000,0,0.05);
    hists["hresXcov"]=hresXcov;
    TH1D * hresYcov = new TH1D("hresYcov","hresYcov",8000,0,0.05);
    hists["hresYcov"]=hresYcov;
    TH1D * hresZcov = new TH1D("hresZcov","hresZcov",8000,0,0.05);
    hists["hresZcov"]=hresZcov;
    TH1D * hresRescov = new TH1D("hresRescov","hresRescov",8000,0,0.05);
    hists["hresRescov"]=hresRescov;

    TH1D* hresMagdCov = new TH1D("hresMagdCov","hresMagdCov",5000,0,50000);
    hists["hresMagdCov"]=hresMagdCov;
    TH2D* hresMagVCov = new TH2D("hresMagVCov","hresMagVCov",1000,0,0.05,1000,0,3);
    hists["hresMagVCov"]=hresMagVCov;
    TH2D* hresXVCov = new TH2D("hresXVCov","hresXVCov",1000,0,0.05,1000,-3,3);
    hists["hresXVCov"]=hresXVCov;
    TH2D* hresYVCov = new TH2D("hresYVCov","hresYVCov",1000,0,0.05,1000,-3,3);
    hists["hresYVCov"]=hresYVCov;
    TH2D* hresZVCov = new TH2D("hresZVCov","hresZVCov",1000,0,0.05,1000,-3,3);
    hists["hresZVCov"]=hresZVCov;
    TH1D* hresMag = new TH1D("hresMag","hresMag",8000,0,10);
    hists["hresMag"]=hresMag;

    TH1D* hnesMagdCov = new TH1D("hnesMagdCov","hnesMagdCov",5000,0,50000);
    hists["hnesMagdCov"]=hnesMagdCov;
    TH2D* hnesMagVCov = new TH2D("hnesMagVCov","hnesMagVCov",1000,0,0.05,1000,0,3);
    hists["hnesMagVCov"]=hnesMagVCov;
    TH2D* hnesXVCov = new TH2D("hnesXVCov","hnesXVCov",1000,0,0.05,1000,-3,3);
    hists["hnesXVCov"]=hnesXVCov;
    TH2D* hnesYVCov = new TH2D("hnesYVCov","hnesYVCov",1000,0,0.05,1000,-3,3);
    hists["hnesYVCov"]=hnesYVCov;
    TH2D* hnesZVCov = new TH2D("hnesZVCov","hnesZVCov",1000,0,0.05,1000,-3,3);
    hists["hnesZVCov"]=hnesZVCov;
    TH1D* hnesMag = new TH1D("hnesMag","hnesMag",8000,0,10);
    hists["hnesMag"]=hnesMag;


    std::cout << "TpcAlignment:init() initializing residual calculator: " << std::endl;
    //Initializing the RefTrackResCalc

    //
    //residualCalculator->setTrackRepId(0);

    //Adding Branches to the residual calculator
    std::map<TString, TClonesArray*>* branches = residualCalculator->getBranchMap();
    std::map<TString, TClonesArray*>::iterator it;
    for(it=branches->begin(); it!=branches->end(); ++it) {
        TClonesArray* br = fBranches[it->first];
        std::cout<<it->first<<std::endl;
        if(br==NULL) {
            std::string s("Branch ");
            s.append(it->first.Data());
            s.append(" requested by residual calculator, but not found in tree!");
            Fatal("TpcAlignment::Init()",s.c_str());
        }
        it->second = br;
    }
    //Initializing, searching for set branches
    std::cout << "Self awareness ... " << std::endl;
    if(residualCalculator->init()){
        throw;
    }
    //

    unsigned int nEvents =inputChain->GetEntries();
    std::cout << "Number of events in the input file: " << nEvents<<std::endl;

}

bool TpcAlignment::execute(){
    fResiduals.clear(); //TODO: should I delete?
    unsigned int nEvents =inputChain->GetEntries();
    //gather all residuals
    if(maxEvents!=0&&nEvents>maxEvents){
        nEvents=maxEvents;
    }
    TFile* outfile;
    TTree* outTree;
    if(storeResiduals&&testRun){
        outfile=new TFile("testOut.root","recreate");
        outfile->cd();
        outTree= new TTree("test","test");
        outTree->Branch("residual","TClonesArray",&fTCResiduals);
    }



    std::cout<<"TpcAlignment::execute() nEvents:"<<nEvents<<std::endl;
    for(unsigned int iEv=0;  iEv<nEvents; ++iEv)   {
        if(iEv%100==0){
            std::cout << "." << std::flush;
        }
        if(iEv%2000==0){
            std::cout<<"#################################################################### "<<iEv<<" "<<std::endl;
        }
        //get the event
        //cdcTracks->Delete();
        //tpcClusters->Delete();
        inputChain->GetEvent(iEv);
        if(storeResiduals&&testRun){
            fTCResiduals.Delete();
        }
        //    std::cout<<"testCalc"<<std::endl;
        residualCalculator->calc();
        //    std::cout<<"testAfter Calc"<<std::endl;
        std::vector<TpcRefResidualCollection*>* resCols = residualCalculator->getResiduals() ;
        int nRes = resCols->size();
        //int ntpccl = tpcArr->GetEntries();
        //int ntpccl = tpcArr->GetEntries();
        for(int ires=0; ires<nRes; ++ires) {
            TpcRefResidualCollection * resCol = (*resCols)[ires];
            // this returns the index of the TpcCluster?
            int TestIndex = resCol->getRefIndex();
            // This returns the branch name of the used hits, that is TpcCluster
            std::string testname = resCol->getRefBranch();
            // THIS SHOULD BE THE TRACK INDEX, THEN THE COVARIANCE MATRIX HAS TO BE CALCULATED ACCORDINGLY
            //std::cout << "TRACK INDEX: " << TestIndex << std::endl;
            //std::cout<<" nRes: "<<resCol->getSize()<<" ";
            for (int ii = 0 ; ii < resCol->getSize(); ii++)    {
                //TpcResidual * miaou = new TpcResidual(*resCol->getResidual(ii));
                //TpcResidual * miaou = new(fTCResiduals[ii]) TpcResidual(*resCol->getResidual(ii));
                TpcResidual *miaou ;
                if(testRun){
                    if(storeResiduals){
                        Int_t pos =fTCResiduals.GetEntries();
                        miaou = new(fTCResiduals[pos]) TpcResidual(*resCol->getResidual(ii));
                    }else{
                        miaou= new TpcResidual(*resCol->getResidual(ii));
                    }
                }else{
                    Int_t pos =fTCResiduals.GetEntries();
                    miaou = new(fTCResiduals[pos]) TpcResidual(*resCol->getResidual(ii));
                    // TpcSimpleResidual* res = new TpcSimpleResidual(*miaou);
                    //fSimpleResiduals.push_back(res);
                }
                //TpcCluster* cl = (TpcCluster*)tpcArr->At(miaou->getHitIndex());
                //if(miaou->getResXYZ().Mag()>maxRes) continue; //CUT ON RESIDUALS
                //if(miaou->getResXYZ().Perp()>maxResXY) continue; //CUT ON RESIDUALS
                //if(fabs(miaou->getResXYZ().Z())>maxResZ) continue; //CUT ON RESIDUALS
                //fResiduals.push_back(miaou);
                //    fResiduals.push_back(miaou);

                //        if(residualCalculatorType=="TpcRefGFTrkResCalc"){
                //          if(miaou->getTrackLength()<20||miaou->getTrackLength()>40){
                //            continue;
                //          }
                //        }
                hists["hresx"]->Fill(miaou->getResXYZ().X());
                hists["hresXvX"]->Fill(miaou->getHitPos().X(),miaou->getResXYZ().X());
                hists["hresy"]->Fill(miaou->getResXYZ().Y());
                hists["hresYvY"]->Fill(miaou->getHitPos().Y(),miaou->getResXYZ().Y());
                hists["hresPerp"]->Fill(miaou->getResXYZ().Perp());
                hists["hresPerpvPerp"]->Fill(miaou->getHitPos().Perp(), miaou->getResXYZ().Perp());
                hists["hresz"]->Fill(miaou->getResXYZ().Z());
                hists["hresZvZ"]->Fill(miaou->getHitPos().Z(),miaou->getResXYZ().Z());
                hists["hres_trDist"]->Fill(miaou->getTrackLength());
                hists["hresPerpvDist"]->Fill(miaou->getTrackLength(),miaou->getResXYZ().Perp());
                hists["hitsX"]->Fill(miaou->getHitPos().X());
                hists["hitsY"]->Fill(miaou->getHitPos().Y());
                hists["hitsZ"]->Fill(miaou->getHitPos().Z());
                hists["hitsPerp"]->Fill(miaou->getHitPos().X(),miaou->getHitPos().Y());
                hists["hitsZR"]->Fill(miaou->getHitPos().Z(),miaou->getHitPos().Perp());
                //std::cout<<"test0"<<std::endl;
                TVectorD res(3);
                //std::cout<<"test1"<<std::endl;
                TVector3 normRes=miaou->getResXYZ();
                normRes=normRes*(1/normRes.Mag());

                //res[0]=miaou->getResXYZ().X();
                res[0]=normRes.X();
                //std::cout<<"test2"<<std::endl;
                //res[1]=miaou->getResXYZ().Y();
                res[1]=normRes.Y();
                //std::cout<<"test3"<<std::endl;
                //res[2]=miaou->getResXYZ().Z();
                res[2]=normRes.Z();
                //std::cout<<"test4"<<std::endl;
                TVectorD x(3);
                x[0]=1;        x[1]=0;        x[2]=0;
                TVectorD y(3);
                y[0]=1;        y[1]=0;        y[2]=0;
                TVectorD z(3);
                z[0]=1;        z[1]=0;        z[2]=0;
                //std::cout<<"test5"<<std::endl;
                //TVectorD resErr=miaou->getHitCov()*res;
                double resErr=res*(miaou->getHitCov()*res);
                //std::cout<<"test6"<<std::endl;
                //TVectorD resX=miaou->getHitCov()*x;
                double resX=x*(miaou->getHitCov()*x);
                //std::cout<<"test7"<<std::endl;
                double resY=y*(miaou->getHitCov()*y);
                //std::cout<<"test8"<<std::endl;
                double resZ=z*(miaou->getHitCov()*z);
                //std::cout<<"test9"<<std::endl;
                hists["hresXcov"]->Fill(sqrt(resX));
                //std::cout<<"test10"<<std::endl;
                hists["hresYcov"]->Fill(sqrt(resY));
                //std::cout<<"test11"<<std::endl;
                hists["hresZcov"]->Fill(sqrt(resZ));
                //std::cout<<"test12"<<std::endl;
                hists["hresRescov"]->Fill(sqrt(resErr));
                //std::cout<<"test13"<<std::endl;
                hists["hresMagdCov"]->Fill(miaou->getResXYZ().Mag()/sqrt(resErr));
                hists["hresMagVCov"]->Fill(sqrt(resErr),miaou->getResXYZ().Mag());
                hists["hresXVCov"]->Fill(sqrt(resX),miaou->getResXYZ().X());
                hists["hresYVCov"]->Fill(sqrt(resY),miaou->getResXYZ().Y());
                hists["hresZVCov"]->Fill(sqrt(resZ),miaou->getResXYZ().Z());
                hists["hresMag"]->Fill(miaou->getResXYZ().Mag());

                //std::cout<<"test14"<<std::endl;

                //        if(testRun){
                //          delete miaou;
                //        }
            }
        }
        if(storeResiduals&&testRun){
            //std::cout<<fTCResiduals.GetEntries()<<std::endl;
            outfile->cd();
            outTree->Fill();
        }
    }//Event loop end

    if(testRun){
        if(storeResiduals){
            outTree->Write();
            outfile->Close();
        }
        if (residualCalculatorType=="TpcRefGFTrkResCalc"){
            ((TpcRefGFTrkResCalc*)residualCalculator)->writeHists();
        }else if (residualCalculatorType=="TpcRefGFTrkTpcClusterResCalc"){
            ((TpcRefGFTrkTpcClusterResCalc*)residualCalculator)->writeHists();
        }
        return true;
    }
    std::cout <<"TpcAlignment::exec() "<< fTCResiduals.GetEntries()<<" TpcResiduals, Starting sattelites alignment..." << std::endl;
    double x,y,xerr,yerr,ntheta,nthetaerr,nphi,nphierr, npsi, npsierr, ndr, ndrerr, z, zerr;
    if(fUseBat){
        BCLog::OpenLog("bat_log.txt",BCLog::detail,BCLog::detail);
        BatAlignmentModel *mod= new BatAlignmentModel(this);
        mod->MCMCSetPrecision(BCEngineMCMC::kHigh);
        mod->MCMCSetNChains(5);
        mod->MCMCSetNIterationsMax(160000);
        mod->MCMCSetNIterationsUpdateMax(4000);
        mod->MCMCSetNIterationsRun(80000);
        //BCModelOutput * output = new BCModelOutput(mod,"MCChains.root");
        // output->WriteMarkovChain();
        mod->MarginalizeAll(BCIntegrate::kMargMetropolis);
        mod->PrintAllMarginalized("bat.pdf");
        //output->Close();
        std::vector<double> pars=mod->GetBestFitParameters();
        std::vector<double> errs=mod->GetBestFitParameterErrors();
        //simulated annealing
        //    std::vector<double> guess(7,0);
        //    guess[6]=1;
        //    std::vector<double> pars=mod->FindMode(BCIntegrate::kOptSimAnn,guess);
        x=pars[0];
        y=pars[1];
        nphi=pars[2];
        ntheta=pars[3];
        //npsi=pars[2]*-1+pars[4];
        npsi=pars[4];
        ndr=pars[5];
        z=pars[6];

        xerr=pars[0];
        yerr=pars[1];
        nphierr=pars[2];
        nthetaerr=pars[3];
        //npsierr=pars[2]*-1+pars[4];
        npsierr=pars[4];
        ndrerr=pars[5];
        zerr=pars[6];
    }else{
        meanZ=0;
        TMinuit *fMinuit;
        fMinuit = new TMinuit(7);
        fMinuit->SetFCN(&this->fcn);
        fMinuit->DefineParameter(0, "x", 0., 0.00001, -5.,5.);
        fMinuit->DefineParameter(1, "y", 0., 0.00001, -5.,5.);

        //angles defined in degrees
        fMinuit->DefineParameter(2, "phi", 0., 0.000001, -30,30);//-3.
        fMinuit->DefineParameter(3, "theta", 0., 0.00001, -30,30);
        fMinuit->DefineParameter(4, "psi", 0., 0.00001, -30,30);
        //alternative

        fMinuit->DefineParameter(5, "drift", 1., 0.00001, 0.7, 1.3);
        fMinuit->DefineParameter(6, "z", 0., 0.00001, -30.,30.);
        //First everything w/o Z + drift
        //

        std::cout <<"##############################################################\n"
                 <<"------------------ First XY-phi,theta,psi Alignment ------------------------" << std::endl;
        std::cout<<"starting migrad minimization, one .= 5 calls to fcn"<<std::endl;
        std::cout <<"##############################################################\n";
        gMinuit->FixParameter(5);
        gMinuit->FixParameter(6);
        //gMinuit->FixParameter(0);
        //gMinuit->FixParameter(1);
        // Now ready for minimization step
        //return true;


        gMinuit->Migrad();
        std::cout<<"migrad minimization, done"<<std::endl;



        gMinuit->GetParameter(0,x,xerr);
        gMinuit->GetParameter(1,y,yerr);
        gMinuit->GetParameter(2,nphi,nphierr);
        gMinuit->GetParameter(3,ntheta,nthetaerr);
        gMinuit->GetParameter(4,npsi,npsierr);
        gMinuit->GetParameter(5,ndr,ndrerr);
        gMinuit->GetParameter(6,z,zerr);

    }
    std::cout<<"\n\n\n";
    std::cout<<"dX: "<<x<<std::endl;
    std::cout<<"dY: "<<y<<std::endl;
    std::cout<<"dZ: "<<z<<std::endl;
    std::cout<<"dPhi: "<<nphi<<std::endl;
    std::cout<<"dTheta: "<<ntheta<<std::endl;
    std::cout<<"dPsi: "<<npsi<<std::endl;
    std::cout<<"\n\n\n";

    std::cout<<"initialPos ";
    inititalTpcPos.Print();

    TVector3 newTpcPos = inititalTpcPos + TVector3(x,y,z);
    std::cout<<"newPos     ";
    newTpcPos.Print();

    std::cout<<"Intital angles (phi,theta,psi) ("<<initialPhi<<", "<<initialTheta<<", " <<initialPsi<<")"<<std::endl;
    TpcAlignmentHelper a;
    double xyzPhi, xyzTheta, xyzPsi;
    if(alMan->getTransformation(detectorName)->GetRotation()!=0){
        a.getAnglesZYX(alMan->getTransformation(detectorName)->GetRotation(),xyzPhi,xyzTheta,xyzPsi);
    }else{
        xyzPhi=0;
        xyzTheta=0;
        xyzPsi=0;
    }
    std::cout<<"Initial angles xyz-conv(phi,theta,psi) ("<<xyzPhi<<", "<<xyzTheta<<", "<<xyzPsi<<")"<<std::endl;
    std::cout<<"New angles xyz-conv(phi,theta,psi) ("<<xyzPhi+nphi<<", "<<xyzTheta+ntheta<<", "<<xyzPsi+npsi<<")"<<std::endl;

    TString outfilealman="alignmentBefore.txt";
    if(residualCalculatorType=="TpcCdcFit2DResCalc"){
        outfilealman.Form("alignmentBefore_%s_.txt","Cdc2DFit");
    }else if (residualCalculatorType=="TpcRefGFTrkResCalc"){
        outfilealman.Form("alignmentBefore_%s_.txt","CdcGFFit");
    }else if (residualCalculatorType=="TpcRefGFTrkTpcClusterResCalc"){
        outfilealman.Form("alignmentBefore_%s_.txt","CdcGFFitSimple");
    }
    std::cout<<"\n\n\nAlman->print before"<<std::endl;
    alMan->Print();
    //  alman->rint();
    std::cout<<"\n\n\nAlman->write("<<outfilealman<<")"<<std::endl;
    alMan->write(outfilealman);

    //
    std::ofstream txtoutfile("Result.txt");
    txtoutfile<<"Alignment results using ";
    if(fUseBat){
        txtoutfile<<" BAT "<<std::endl;
    }else{
        txtoutfile<<" Minuit "<<std::endl;
    }

    txtoutfile<<"initial pos(xyz): ("<<inititalTpcPos.x()<<", "<<inititalTpcPos.y()<<", "<<inititalTpcPos.z()<<")"<<std::endl;
    txtoutfile<<"initial angles (phi,theta,psi)_xyz: "<<phi_<<" "<<theta_<<" "<<psi_<<""<<std::endl;
    txtoutfile<<"new pos(xyz): "<<newTpcPos.x()<<" "<<newTpcPos.y()<<" "<<newTpcPos.z()<<""<<std::endl;
    txtoutfile<<"new angles (phi,theta,psi)_xyz: "<<xyzPhi+nphi<<" "<<xyzTheta+ntheta<<" "<<xyzPsi+npsi<<""<<std::endl;
    txtoutfile<<"alignment variables: "<<std::endl;
    txtoutfile<<"dx: "<<x<<" ("<<xerr<<"), ";
    txtoutfile<<"dy: "<<y<<" ("<<yerr<<"), ";
    txtoutfile<<"dz: "<<z<<" ("<<zerr<<"), "<<std::endl;
    txtoutfile<<"dphi: "<<nphi<<" ("<<nphierr<<"), ";
    txtoutfile<<"dtheta: "<<ntheta<<" ("<<nthetaerr<<"), ";
    txtoutfile<<"dpsi: "<<npsi<<" ("<<npsierr<<"), "<<std::endl;





    std::cout<<"setNew Rot Matrix"<<std::endl;
    alMan->setRotationMatrix(detectorName,a.getRotationMatrixZYX(xyzPhi+nphi,xyzTheta+ntheta,xyzPsi+npsi));
    //alMan->setEulerAngles(detectorName, nphi+initialPhi,ntheta+initialTheta,npsi+initialPsi);  //we rotate
    double newPhi,newTheta,newPsi;
    alMan->getEulerAngles(detectorName,newPhi,newTheta,newPsi);
    std::cout<<"new angles (phi,theta,psi) ("<<newPhi<<", "<<newTheta<<", " <<newPsi<<")"<<std::endl;
    alMan->setTranslation(detectorName, newTpcPos);        //then translate
    std::cout<<"\n\n\nAlman->print after"<<std::endl;
    alMan->Print();
    //  alMan->getEulerAngles(alignmentFile, phi, theta, psi);
    //tpcpos = alMan->getTranslation(alignmentFile);
    //Modify all TpcCluster with the new alignmen
    outfilealman="alignmentAfter.txt";
    if(residualCalculatorType=="TpcCdcFit2DResCalc"){
        outfilealman.Form("alignmentAfter_%s_.txt","Cdc2DFit");
    }else if (residualCalculatorType=="TpcRefGFTrkResCalc"){
        outfilealman.Form("alignmentAfter_%s_.txt","CdcGFFit");
    }else if (residualCalculatorType=="TpcRefGFTrkTpcClusterResCalc"){
        outfilealman.Form("alignmentAfter_%s_.txt","CdcGFFitSimple");
    }
    alMan->write(outfilealman);
    return false;
    for(unsigned int iEv=0;  iEv<nEvents; ++iEv)     {
        if(iEv%100==0){
            std::cout << "." << std::flush;
        }
        if(iEv%2000==0){
            std::cout<<"#################################################################### "<<iEv<<" "<<std::endl;
        }
        inputChain->GetEvent(iEv);
        residualCalculator->calc();
        std::vector<TpcRefResidualCollection*>* blka =  residualCalculator->getResiduals() ;
        int nRes = blka->size();
        for(int ires=0; ires<nRes; ires++){
            TpcRefResidualCollection * resCol = (*blka)[ires];
            for (int ii = 0 ; ii < resCol->getSize(); ii++){
                const TpcResidual  miaou = *resCol->getResidual(ii);
                //TpcCluster* cl = (TpcCluster*)tpcArr->At(miaou->getHitIndex());
                //if(miaou->getResXYZ().Mag()>maxRes) continue; //cut on residuals
                //if(miaou->getResXYZ().Perp()>maxResXY) continue; //cut on residuals
                //if(fabs(miaou->getResXYZ().Z())>maxResZ) continue; //cut on residuals
                hists["hnesx"]->Fill(miaou.getResXYZ().X());
                hists["hnesy"]->Fill(miaou.getResXYZ().Y());
                hists["hnesPerp"]->Fill(miaou.getResXYZ().Perp());
                hists["hnesz"]->Fill(miaou.getResXYZ().Z());
                hists["hnes_trDist"]->Fill(miaou.getTrackLength());
                hists["hnesMag"]->Fill(miaou.getResXYZ().Mag());
            }
        }
    }//end event loop

}

void TpcAlignment::fcn(Int_t &npar, Double_t *gin, Double_t &f, Double_t *par, Int_t iflag){



    f = chisquare(npar,par);
}
double TpcAlignment::chisquare(int npar,double * par){
    if(nCallsFcn%50==0){
        std::cout<<"."<<std::flush;
        //std::cout<<nCallsFcn<<std::endl;
    }

    if(nCallsFcn%4000==0){
        std::cout<<nCallsFcn<<std::endl;
        std::cout<<"\n"<<std::flush;
    }


    TpcAlignmentManager* sAlman=TpcAlignmentManager::getInstance();
    double phipar = par[2];
    double thetapar = par[3];
    double psipar= par[4];
    //double corrdrift = par[5];
    TVector3 vadd(par[0], par[1], par[6]);


    //changing convention to xyz over zxz
    //sAlman->getEulerAngles(detectorName, phi, theta, psi);
    //double phi,theta,psi;
    TpcAlignmentHelper a;
    if(nCallsFcn==0){
        a.getAnglesZYX(sAlman->getTransformation(detectorName)->GetRotation(),phi_,theta_,psi_);
        TVector3 t1=sAlman->getTranslation(detectorName);
        tpcpos_.SetXYZ(t1.x(),t1.y(),t1.z());
        std::cout<<"##################################################################################"<<std::endl;
        std::cout<<"First call to chi2-function, getting original phi/theta/psi(zyx)"<<std::endl;
        std::cout<<"Values are: (phi,theta,psi): ("<<phi_<<", "<<theta_<<", "<<psi_<<")"<<std::endl;
        std::cout<<"##################################################################################"<<std::endl;
        //lastTransP1P2=new TGeoCombiTrans();
    }

    /*
    1. :  p1=R1*p0+t1
    =>
    2. :  p0=R1T(p1-t1)

    3. :  p2=R2*p0+t2
    3.+2. =>
    3:    p2=(R2*R1T(p1-t1) +t2
          p2=R2*R1T*p1-R2*R1T*t1 +t2
          p2=R2'*p1 + t2'
          R2'=R2*R1T
          t2'=t2-R2't1
    4: new rot operator R2*R1T has to be calculated, but only once per chisquare

    */

    TGeoCombiTrans* ctrans12=lastTransP1P2;
    const double* lastTrans_=ctrans12->GetTranslation();

    if(phipar==dphi_&&thetapar==dtheta_&&psipar==dpsi_){
        //TGeoRotation* rot1=;
        //no extra rotation, only need to add shift
        //shift is equal to the newShift-lastShift
        const double* lastTrans=ctrans12->GetTranslation();
        ctrans12->SetTranslation(lastTrans[0]+vadd.x()-dtpcpos_.x(),
                lastTrans[1]+vadd.y()-dtpcpos_.y(),
                lastTrans[2]+vadd.z()-dtpcpos_.z());
        //std::cout<<"only change in translation"<<std::endl;
    }else{
        //Rotation included, all translations change
        //need to recalculate relative transformation
        //    std::cout<<"change in rot "
        //        <<par[0]<<", "<<par[1]<<", "
        //        <<par[2]<<", "<<par[3]<<", "<<par[3]<<", "
        //        <<par[4]<<", "<<par[5]<<", "<<par[6]<<std::endl;

        TVector3 t2 = tpcpos_+vadd; //configurable
        TGeoRotation* rot=(sAlman->getTransformation(detectorName))->GetRotation();
        TGeoRotation rot1t;
        if(rot!=0){
            rot1t.SetRotation(rot->Inverse());
        }
        TGeoRotation rot2_;
        rot2_.SetMatrix(a.getRotationMatrixZYX(phi_+phipar,theta_+thetapar,psi_+psipar).data());
        TGeoRotation rot2prime(rot1t);
        rot2prime.MultiplyBy(&rot2_,false);//rot2_*rot1t
        std::vector<double> t1_=TpcAlignmentManager::tVector3ToStlVector(tpcpos_);
        double t1__[3];
        rot2prime.LocalToMaster(t1_.data(),t1__);
        TVector3 t2prime=t2-TVector3(t1__);
        TGeoTranslation trans12(t2prime.X(),t2prime.Y(),t2prime.Y());
        ctrans12->SetTranslation(trans12);
        ctrans12->SetRotation(rot2prime);
        dphi_=phipar;
        dtheta_=thetapar;
        dpsi_=phipar;
    }
    //std::cout<<nCallsFcn<<" lastTrans "<<lastTrans_[0]<<", "<<lastTrans_[1]<<", "<<lastTrans_[2]<<std::endl;
    const double* newTrans=ctrans12->GetTranslation();
    //std::cout<<nCallsFcn<<" newTrans "<<newTrans[0]<<", "<<newTrans[1]<<", "<<newTrans[2]<<std::endl;

    //####################################################################################3

    Int_t nbins=fTCResiduals.GetEntries();
    //std::cerr<<"test1-duplicating"<<std::endl;

    //to be able to parallelize we get the transformations here
    unsigned int nThreads =7;
    std::vector<double> chisq(nThreads,0);
    std::vector<TGeoCombiTrans> transf;
    std::vector<TGeoCombiTrans> transfBack;
    std::vector<TGeoCombiTrans> transf12;
    //sAlman->getTransformation(detectorName)->R
    for(unsigned int test=0;test<nThreads;++test){
        //std::cerr<<"test thread "<<test<<std::endl;
        transf.push_back(TGeoCombiTrans(*(sAlman->getTransformation(detectorName))));
        transfBack.push_back(TGeoCombiTrans(*(sAlman->getTransformation(detectorName))));
        TGeoRotation rot2;
        rot2.SetMatrix(a.getRotationMatrixZYX(phi_+phipar,theta_+thetapar,psi_+psipar).data());
        transfBack.back().SetRotation(rot2);
        TVector3 newpos=tpcpos_+vadd;
        transfBack.back().SetTranslation(newpos.X(),newpos.Y(),newpos.Z());

        transf12.push_back(TGeoCombiTrans(*ctrans12));

    }


#pragma omp parallel for num_threads(nThreads)
    for (int i=0;i<nbins; ++i) {
        //std::cout<<"test2 "<<omp_get_thread_num()<<std::endl;
        TpcResidual* resi = (TpcResidual*) fTCResiduals.UncheckedAt(i);
        //TpcSimpleResidual* sresi = (TpcSimpleResidual*) fSimpleResiduals.at(i);
        //std::cout<<"test4 "<<omp_get_thread_num()<<std::endl;
        //    TVector3 ref =fResiduals[i]->getRefPos();
        //    TVector3 hit = fResiduals[i]->getHitPos();
        TVector3 ref =resi->getRefPos();
        TVector3 hit = resi->getHitPos();
        TVector3 dir = resi->getTrackDir();
        TMatrixD cov = resi->getHitCov();
        //std::cout<<"testing "<<(dir*(ref-hit))<<std::endl;
        //ref.Print();
        //
        // IT MUST BE REPLACED WITH THE COVARIANCE MATRIX OF THE FITTED REFERENCE TRACK AT THE CONSIDERED POINT
        // TMatrixT<double> CovMatr = vres[i]->getHitCov();
        //

        //*************************************
        // openmp
        //*************************************
        std::vector<double> xyz_=TpcAlignmentManager::tVector3ToStlVector(hit);
        double uvw[3];
        transf[omp_get_thread_num()].MasterToLocal(&xyz_[0],uvw);

        double xyz_new[3];
        //   transf12[omp_get_thread_num()].LocalToMaster(xyz_.data(),xyz_new);
        transfBack[omp_get_thread_num()].LocalToMaster(uvw,xyz_new);
        TVector3 newxyz(xyz_new);

        //*************************************

        //*********************************
        //*********************************
        //*********************************
        // OLD
        //*********************************
        //    TVector3 hituvw = sAlman->masterToLocal(detectorName,hit);
        //    //hituvw.SetZ((hituvw.Z()-meanZ)*corrdrift+meanZ); // adjust Z coordinate
        //    sAlman->setEulerAngles(detectorName, phi+phipar,theta+thetapar,psi+psipar);  //and we rotate
        //    sAlman->setTranslation(detectorName, tpcpos+vadd);                 //then translate
        //    TVector3 newxyz = sAlman->localToMaster(detectorName,hituvw);
        //    sAlman->setTranslation(detectorName, tpcpos);      //back translation
        //    sAlman->setEulerAngles(detectorName, phi,theta,psi);  //and we rotate it back
        //*********************************
        TVector3 res = ref-newxyz;
        //    double angle=TMath::ACos((res*dir)/res.Mag());
        //    double resPerp=res.Mag()*TMath::Sin(angle);
        TVector3 resPerp;
        if(fcdc3Dcalculation){
            TVector3 resPar=(res*dir)*dir;
            resPerp=res-resPar;
        }else{
            resPerp=res;

            //res.SetZ(0);
        }
        //  double resErr=resPerp*(cov*resPerp);

        //
        //chisq += res.Mag()*res.Mag();
        //THE FOLLOWING DOES NOT WORK: FINAL RESIDUALS ARE WORSE THAN INITIAL VALUES!
        //chisq += pow(res.X()/CovMatr[0][0],2)+pow(res.Y()/CovMatr[1][1],2)+pow(res.Z()/CovMatr[2][2],2);
        //
        //chisq += pow(res.Z(),2);
        //chisq += pow(res.Perp(),2);
        //chisq += res.Mag2();
        //std::cout<<"test3 "<<omp_get_thread_num()<<std::endl;
        //chisq[omp_get_thread_num()] += resPerp.Mag2();//*TMath::Power(1/resErr,2);
        chisq[omp_get_thread_num()] += resPerp.Perp2();//*TMath::Power(1/resErr,2);
        //sum+=hituvw.Z();
    }
    //meanZ = sum/(1.*nbins);
    double ret=0;
    for(unsigned int test=0;test<nThreads;++test){
        ret+=chisq[test];
    }

    dtpcpos_.SetXYZ(par[0], par[1], par[6]);
    ++nCallsFcn;
    return ret;
    ///eturn chisq[0]+chisq[1]+chisq[2];
}

void TpcAlignment::writeHistos(std::string outfile){
    TFile* outFile= new TFile(outfile.c_str(),"RECREATE");
    std::map<TString,TH1*>::iterator it;
    for(it=hists.begin();it!=hists.end();++it){
        (*it).second->Write();
    }

}