//-----------------------------------------------------------
// 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 <stdlib.h>
#include <omp.h>
#include <sstream>
#include <fstream>
#include <iomanip>

#include <TH1D.h>
#include <TH2D.h>
#include <TH3D.h>
#include <TProfile.h>
#include <TMinuit.h>
#include <TString.h>
#include <TFile.h>
#include <TTree.h>
#include <TStopwatch.h>
#include <TTreeCache.h>

#include "GFTrack.h"
#include "GFFieldManager.h"
#include "RKTrackRep.h"
#include "GFException.h"

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

#include "PndFieldAdaptor.h"



#include "BatAlignmentModel.h"
#include "BAT/BCEngineMCMC.h"
#include "BAT/BCLog.h"
#include "BAT/BCParameter.h"

#include "TpcAlignmentHelper.h"
#include "TpcEventIdentifier.h"

#include "TpcAligner.h"



TpcAligner::TpcAligner()
    : fAlMan(NULL),
      nCallsFcn(0),
      fVerbose(0),
      fuseShiftedRotation(false),
      fshift(0),
      fUseTrackLinearization(true),
      fUse3DResidual(false),
      fUse2DResidual(false),
      fDetectorName("tpc"),
      fAlignmentFile("alinput.txt"),
      fTCResiduals(NULL),
      
      fAutoRotationShift(false),
      fMeanZ(0),
      fInititalTpcPos(0, 0, 0),
      fInitialPhi(0),
      fInitialTheta(0),
      fInitialPsi(0),
      fFirstChiSq(0),
      fPrintChi2(false),
      fAlignmentMode(fMinuit),
      fBatItMax(500000),
      fBatItMin(10000),
      fBatItUpdate(2000),
      fBatNChains(4),
      
      fInputChain(NULL),
      fMaxEvents(0)
    
    
    
    
{
    
    
}

bool TpcAligner::init() {
    if (fInputChain == NULL) {
        std::cerr << "TpcAligner::init() \n "
                  << "Input chain missing, please execuite setInputChain(chain) "
                     "before running init" << std::endl;
        return false;
    }
    int test =
            fInputChain->SetBranchAddress("residual", &fTCResiduals);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init FopiTuple-array not found!"
                  << test << std::endl;
        return 1;
    }
    fInputChain->SetCacheSize(100e6);
    TTreeCache::SetLearnEntries(10);
    
    
    // std::cout << "fAlMan..." << std::endl;
    TpcAlignmentManager::init(fAlignmentFile.c_str());
    fAlMan = TpcAlignmentManager::getInstance();
    if (fAlMan == 0) {
        std::cerr << "TpcAligner::init() Init TpcAlignmentManager:"
                  << fAlignmentFile.c_str() << " not found!" << std::endl;
        return false;
    }
    
    fAlMan->getEulerAngles(fDetectorName, fInitialPhi, fInitialTheta, fInitialPsi);
    fInititalTpcPos = fAlMan->getTranslation(fDetectorName);
    
    TpcAlignmentHelper a;
    if (fAlMan->getTransformation(fDetectorName)->GetRotation() != 0) {
        a.getAnglesZYX(fAlMan->getTransformation(fDetectorName)->GetRotation(),
                       fInitialPhiXYZ, fInitialThetaXZY, fInitialPsiXYZ);
    } else {
        fInitialPhiXYZ = 0;
        fInitialThetaXZY= 0;
        fInitialPsiXYZ= 0;
    }
    std::cout << "Intput AlignmentFile: " << fAlignmentFile.c_str() << std::endl;
    std::cout << fDetectorName << " TPC xyz(" << fInititalTpcPos.X() << ", "
              << fInititalTpcPos.Y() << ", " << fInititalTpcPos.Z() << ")."
              << std::endl;
    std::cout << "Rot. angles_ROOT(" << fInitialPhi << ", " << fInitialTheta << ", "
              << fInitialPsi << ")" << std::endl;
    std::cout << "Rot. angles_XYZ(" << fInitialPhiXYZ<< ", " << fInitialThetaXZY<< ", "
              << fInitialPsiXYZ << ")" << std::endl;
    
    
    // Initializing, searching for set branches
    std::cout << "TpcAligner:init() initializing residual calculator: "
              << std::endl;
    std::cout << "Self awareness ... " << std::endl;
    
    unsigned int nEvents = fInputChain->GetEntries();
    std::cout << "Number of events in the input file: " << nEvents << std::endl;
    return false;
}

bool TpcAligner::execute() {
    std::cout << "TpcAligner::execute()" << std::endl;
    int nEvents = fInputChain->GetEntries();
    TpcAlignmentHelper alHelper;
    if (fMaxEvents != 0 && nEvents > fMaxEvents) {
        nEvents = fMaxEvents;
    }
    
    clock.Start();
    
    std::cout << "TpcAligner::execute() nEvents:" << nEvents << std::endl;
    double sumW=0;
    double nW=0;
    for (int iEv = 0; iEv < nEvents; ++iEv) {
        if (iEv % 10000 == 0) {
            std::cout << "###########################################################"
                         "######### " << iEv << " / " << nEvents << " ("
                      << ((double)iEv / (double)nEvents) * 100 << " %)"
                      << " ##########################################################"
                         "###########" << std::endl;
            clock.Print();
            clock.Continue();
        }
        
        
        // get the event
        fInputChain->GetEvent(iEv);
        int nRes=fTCResiduals->GetEntriesFast();
        for(int iRes=0;iRes<nRes;++iRes){
            TpcResidual* res= (TpcResidual*)fTCResiduals->UncheckedAt(iRes);
            if(fAutoRotationShift){
                TVector3 xyz=res->getHitPos();
                TVector3 uvw=fAlMan->masterToLocal("tpc",xyz);
                sumW+=uvw.Z();
                nW++;
            }
            
            fSimpleResiduals.push_back(
                        new TpcSimpleResidual(*res));
            //res->getResXYZ().Print();
        }
        //std::cout<<nRes<<std::endl;
        
    }  // Event loop endfMCMC
    if (fAutoRotationShift) {
        double w = sumW/nW;//hists["hitsW"]->GetMean();
        std::cout << "#############################################################"
                     "###########################" << std::endl;
        std::cout << "#############################################################"
                     "###########################" << std::endl;
        std::cout << "# WARNING overriding the rotation shift using the "
                     "w-distribution of clusters: <w>=" << w << std::endl;
        std::cout << "#############################################################"
                     "###########################" << std::endl;
        std::cout << "#############################################################"
                     "###########################" << std::endl;
        fshift = w;
    }
    
    std::cout << "TpcAligner::exec() " << fSimpleResiduals.size()
              << " TpcSimpleResiduals"
              << "Starting alignment using ";
    //    double x, y, xerr, yerr, ntheta, nthetaerr, nphi, nphierr, npsi, npsierr, ndr,
    //            ndrerr, z, zerr, ndrerr, z, zerr;
    std::vector<double> pars;
    std::vector<double> simAnPars;
    std::vector<std::string> parNames;
    
    BCLog::OpenLog("bat_log.txt", BCLog::debug, BCLog::debug);
    BatAlignmentModel *mod = new BatAlignmentModel(this);
    mod->MCMCSetPrecision(BCEngineMCMC::kHigh);
    mod->MCMCSetNChains(fBatNChains);
    //    mod->MCMCSetNIterationsEfficiencyCheck(1000);
    mod->MCMCSetNIterationsPreRunCheck(1000);
    //    mod->MCMCSetNIterationsConvergenceCheck(fBatItUpdate);
    mod->MCMCSetNIterationsClearConvergenceStats(fBatItMax / 2);
    mod->MCMCSetNIterationsPreRunMax(fBatItMax);
    // new stuff!
    mod->MCMCSetMultivariateProposalFunction(true);
    mod->MCMCSetMinimumEfficiency(0.15);
    mod->MCMCSetMaximumEfficiency(0.35);                    // default 0.65
    mod->MCMCSetMultivariateProposalFunctionEpsilon(1e-3);  // defaul: 1e-3
    // epsilon/nudge for cholesky decomposition of covariance matrix
    // epsilon is added to diagonals, if matrix singular
    // if that does not work
    mod->MCMCSetNIterationsRun(fBatItMin);
    std::vector<double> scales = std::vector<double>(7, 0.4);
    std::vector<double> start = std::vector<double>(7, 0);
    start[5] = 1;
    scales[0] = 0.001;
    scales[1] = 0.001;
    scales[2] = 0.001;
    scales[3] = 0.001;
    scales[4] = 0.001;
    scales[5] = 1;
    scales[6] = 0.001;
    mod->MCMCSetTrialFunctionScaleFactor(scales);
    // F    mod->MCMCSetInitialPositions(start);
    std::vector<double> errs(7, 0);
    switch (fAlignmentMode) {
    case fMCMC:
    {
        std::cout << "Bat with Markov Chains" << std::endl;
        mod->WriteMarkovChain("MCChains.root", "RECREATE");
        TFile test("testBat.root", "recreate");
        mod->MarginalizeAll(BCIntegrate::kMargMetropolis);
        mod->Remarginalize(true);
        mod->MCMCCloseOutputFile();
        test.Write();
        
        
        
        //        pars = mod->GetBestFitParameters();
        pars = mod->GetGlobalMode();
        std::cout << "Pars from MCMC" << std::endl;
        for (unsigned int haha = 0; haha < pars.size(); ++haha) {
            std::cout << haha<<": "<<pars[haha] << std::endl;
        }
        pars = mod->FindMode(BCIntegrate::kOptMinuit, pars);
        errs = mod->GetBestFitParameterErrors();
        std::cout << "Pars from minuit" << std::endl;
        for (unsigned int haha = 0; haha < pars.size(); ++haha) {
            std::cout << haha<<": "<<pars[haha] << std::endl;
        }
        mod->PrintAllMarginalized("bat.pdf");
        mod->PrintCorrelationPlot("bat_correlations.pdf");
        //        mod->PrintResults("bat_results.txt");
    }
        break;
    case fSimulatedAnnealing:
    {// simulated annealing
        std::cout << "Bat with Simulated anhealing and Minuit" << std::endl;
        std::vector<double> guess(7, 0);
        guess[5] = 1;
        mod->SetSAT0(50);
        double test2[2];
        test2[0] = 10000;
        test2[1] = 0.1;
        mod->SetMinuitArgList(test2);
        std::cout<<"pars before"<<std::endl;
        for (unsigned int haha = 0; haha < pars.size(); ++haha) {
            std::cout << haha<<": "<<pars[haha] << std::endl;
        }
        pars = mod->FindMode(BCIntegrate::kOptSimAnn, guess);
        simAnPars = pars;
        std::cout << "Pars from simulated annhealing" << std::endl;
        for (unsigned int haha = 0; haha < pars.size(); ++haha) {
            
            std::cout << haha<<": "<<pars[haha] << std::endl;
        }
        pars = mod->FindMode(BCIntegrate::kOptMinuit, pars);
        // pars=mod->FindModeMinuit(pars,errs,pars);
        errs = mod->GetBestFitParameterErrors();
        std::cout << "Pars from minuit" << std::endl;
        for (unsigned int haha = 0; haha < pars.size(); ++haha) {
            std::cout << haha<<": "<<pars[haha] << std::endl;
        }
    }
        break;
    case fMinuit:
    {
        std::vector<double> guess2(7, 0);
        pars = mod->FindMode(BCIntegrate::kOptMinuit, guess2);
        errs = mod->GetBestFitParameterErrors();
        std::cout << "Pars from minuit" << std::endl;
        for (unsigned int haha = 0; haha < pars.size(); ++haha) {
            std::cout << haha<<": "<<pars[haha] << std::endl;
        }
    }
        break;
    default:
        break;
    }
    
    double nx = pars[0];
    double ny = pars[1];
    double nphi = pars[2];
    double ntheta = pars[3];
    double npsi = pars[4];
    //double ndr = pars[5];
    double nz = pars[6];
    
    double nxerr = errs[0];
    double nyerr = errs[1];
    double nphierr = errs[2];
    double nthetaerr = errs[3];
    double npsierr = errs[4];
    //double ndrerr = errs[5];
    double nzerr = errs[6];
    
    std::cout << "\n\n\n";
    std::cout << "dX: " << nx << std::endl;
    std::cout << "dY: " << ny << std::endl;
    std::cout << "dZ: " << nz << 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";
    
    // Calculate Chi2 for final parameters
    double chi2Final = chisquare(7, pars.data());
    
    std::cout << "Intital angles (phi,theta,psi) (" << fInitialPhi << ", "
              << fInitialTheta << ", " << fInitialPsi << ")" << std::endl;
    
    std::cout << "Initial angles xyz-conv(phi,theta,psi) (" << fInitialPhiXYZ << ", "
              << fInitialThetaXZY << ", " << fInitialPsiXYZ << ")" << std::endl;
    std::cout << "New angles xyz-conv(phi,theta,psi) (" << fInitialPhi+ nphi << ", "
              << fInitialThetaXZY + ntheta << ", " << fInitialPsiXYZ+ npsi << ")" << std::endl;
    std::cout << "initialPos ";
    fInititalTpcPos.Print();
    // calculating new position
    TVector3 newTpcPos = fInititalTpcPos + TVector3(nx, ny, nz);  // t1+dt
    TVector3 newTpcPos_shifted =
            tpcpos_ + TVector3(nx, ny, nz) + TVector3(0, 0, fshift);  // t1*+dt=t2*
    // t_par*
    if (fuseShiftedRotation) {
        // t2=t1+dt+R1*t0-R2*t0
        // skipping the steps with t2* and t1*
        fAlMan->setRotationMatrix(
                    fDetectorName, alHelper.getRotationMatrixZYX(
                        fInitialPhiXYZ+ nphi, fInitialThetaXZY + ntheta, fInitialPsiXYZ+ npsi));
        TVector3 r2_t0 =
                fAlMan->localToMasterVect(fDetectorName, TVector3(0, 0, fshift));
        
        fAlMan->setRotationMatrix(
                    fDetectorName, alHelper.getRotationMatrixZYX(fInitialPhiXYZ, fInitialThetaXZY, fInitialPsiXYZ));
        TVector3 r1_t0 =
                fAlMan->localToMasterVect(fDetectorName, TVector3(0, 0, fshift));
        
        std::cout << "r1_t0: (" << r1_t0.X() << ", " << r1_t0.Y() << ", "
                  << r1_t0.Z() << ") r2_t0: (" << r2_t0.X() << ", " << r2_t0.Y()
                  << ", " << r2_t0.Z() << ") " << std::endl;
        newTpcPos = newTpcPos + r1_t0 - r2_t0;
    }
    
    std::cout << "newPos     ";
    newTpcPos.Print();
    
    TString outfilealman = "alignmentBefore.txt";
    std::cout << std::endl << "fAlMan->print before" << std::endl;
    fAlMan->Print();
    std::cout << "fAlMan->write(" << outfilealman << ")" << std::endl << std::endl;
    fAlMan->write(outfilealman);
    
    double oldPhi, oldTheta, oldPsi;
    fAlMan->getEulerAngles(fDetectorName, oldPhi, oldTheta, oldPsi);
    std::cout << "setNew Rot Matrix" << std::endl;
    fAlMan->setRotationMatrix(
                fDetectorName, alHelper.getRotationMatrixZYX(
                    fInitialPhiXYZ + nphi, fInitialThetaXZY + ntheta, fInitialPsiXYZ + npsi));
    double newPhi, newTheta, newPsi;
    fAlMan->getEulerAngles(fDetectorName, newPhi, newTheta, newPsi);
    std::cout << "new angles (phi,theta,psi) (" << newPhi << ", " << newTheta
              << ", " << newPsi << ")" << std::endl;
    fAlMan->setTranslation(fDetectorName, newTpcPos);  // then translate
    std::cout << std::endl << "fAlMan->print after" << std::endl;
    fAlMan->Print();
    
    std::ofstream txtoutfile("Result.txt");
    txtoutfile << "Alignment results using ";
    txtoutfile << fAlignmentMode;
    
    TVector3 difpos = newTpcPos - fInititalTpcPos;
    txtoutfile << "using #" << nCallsFcn
               << " calls to the chi2-function, resulting of a Chi^2 of: "
               << std::setprecision(9) << chi2Final << " with # "
               << fSimpleResiduals.size() << " residuals" << std::endl;
    txtoutfile << "Original chi2: " << fFirstChiSq << std::endl;
    
    txtoutfile << "fBatItMax: " << fBatItMax << std::endl;
    txtoutfile << "fBatItMin: " << fBatItMin << std::endl;
    txtoutfile << "fBatItUpdate: " << fBatItUpdate << std::endl;
    txtoutfile << "fBatNChains: " << fBatNChains << std::endl;
    
    txtoutfile << "fuseShiftedRotation: " << fuseShiftedRotation << std::endl;
    if (fuseShiftedRotation) {
        txtoutfile << "fshift: " << fshift << std::endl;
        txtoutfile << "fshifted translation (t2*+t0): " << newTpcPos_shifted.X()
                   << ", " << newTpcPos_shifted.Y() << ", " << newTpcPos_shifted.X()
                   << std::endl;
    }
    txtoutfile << "fUseTrackLinearization: " << fUseTrackLinearization
               << std::endl;
    txtoutfile << "fUse3DResidual: " << fUse3DResidual << std::endl;
    
    txtoutfile << "initial pos(xyz):\t" << fInititalTpcPos.x() << ", "
               << fInititalTpcPos.y() << ", " << fInititalTpcPos.z() << std::endl;
    txtoutfile << "new pos(xyz):     \t" << newTpcPos.x() << " " << newTpcPos.y()
               << " " << newTpcPos.z() << std::endl;
    txtoutfile << "initial angles (phi,theta,psi)_xyz: \t" << fInitialPhiXYZ << " "
               << fInitialThetaXZY << " " << fInitialPsiXYZ << "" << std::endl;
    txtoutfile << "new angles (phi,theta,psi)_xyz: \t" << fInitialPhiXYZ + nphi << " "
               << fInitialThetaXZY + ntheta << " " << fInitialPsiXYZ + npsi << "" << std::endl;
    txtoutfile << "initial angles (phi,theta,psi)_zxz: \t" << oldPhi << " "
               << oldTheta << " " << oldPsi << "" << std::endl;
    txtoutfile << "new angles (phi,theta,psi)_zxz: \t" << newPhi << " "
               << newTheta << " " << newPsi << "" << std::endl;
    txtoutfile << "alignment variables: " << std::endl;
    txtoutfile << "dx: " << difpos.x() << " (" << nxerr << "), ";
    txtoutfile << "dy: " << difpos.y() << " (" << nyerr << "), ";
    txtoutfile << "dz: " << difpos.z() << " (" << nzerr << "), " << std::endl;
    txtoutfile << "dphi: " << nphi << " (" << nphierr << "), ";
    txtoutfile << "dtheta: " << ntheta << " (" << nthetaerr << "), ";
    txtoutfile << "dpsi: " << npsi << " (" << npsierr << "), " << std::endl;
    
    outfilealman = "alignmentAfter.txt";
    fAlMan->write(outfilealman);
    
    
    return false;
}

void TpcAligner::getRealParameters(const std::vector<double> pars,
                                   double *realpars) {
    //    if(pars.size()!=realpars.size()){
    //        std::cerr<<__FILE__<<"
    //        "<<__LINE__<<"pars.size!=realpars.size"<<std::endl;
    //        std::cerr<<"pars.size() = "<<pars.size()<<" realpars.size() =
    //        "<<realpars.size()<<std::endl;
    //        throw;
    //    }
    // Angles, simple addition
    realpars[2] = fInitialPhiXYZ + pars[2];
    realpars[3] = fInitialThetaXZY + pars[3];
    realpars[4] = fInitialPsiXYZ+ pars[4];
    // inititalTpcPos
    TpcAlignmentHelper alHelper;
    TVector3 newTpcPos = fInititalTpcPos + TVector3(pars[0], pars[1], pars[6]);
    if (fuseShiftedRotation) {
        //if(!(realpars[2]==0 and realpars[3]==0 and realpars[5]==0)){
        fAlMan->setRotationMatrix(
                    fDetectorName,
                    alHelper.getRotationMatrixZYX(realpars[2], realpars[3], realpars[4]));
        //}
        TVector3 r2_t0 =
                fAlMan->localToMasterVect(fDetectorName, TVector3(0, 0, fshift));
        //if(!(fInitialPhiXYZ==0 and fInitialTheta==0 and fInitialPsi==0)){
        fAlMan->setRotationMatrix(
                    fDetectorName, alHelper.getRotationMatrixZYX(fInitialPhiXYZ,fInitialThetaXZY , fInitialPsiXYZ));
        //}
        TVector3 r1_t0 =
                fAlMan->localToMasterVect(fDetectorName, TVector3(0, 0, fshift));
        
        // std::cout<<"r1_t0: ("<<r1_t0.X()<<", "<<r1_t0.Y()<<", "<<r1_t0.Z()<<")
        // r2_t0: ("<<r2_t0.X()<<", "<<r2_t0.Y()<<", "<<r2_t0.Z()<<") "<<std::endl;
        newTpcPos = newTpcPos + r1_t0 - r2_t0;
    }
    realpars[0] = newTpcPos.X();
    realpars[1] = newTpcPos.Y();
    realpars[6] = newTpcPos.Z();
}

double TpcAligner::chisquare(int npar, double *par) {
    if (nCallsFcn % 100 == 0) {
        std::cout << "." << std::flush;
    }
    std::ofstream *txtoutfile2;
    if (fPrintChi2) {
        txtoutfile2 = new std::ofstream("chisq.txt", std::ofstream::app);
    }
    if (nCallsFcn % 10000 == 0) {
        std::cout << nCallsFcn << std::endl;
        std::cout << "\n" << std::flush;
        clock.Print();
        clock.Continue();
    }
    Int_t nbins = fSimpleResiduals.size();
    double phipar = par[2];
    double thetapar = par[3];
    double psipar = par[4];
    // double corrdrift = par[5];
    TVector3 vadd(par[0], par[1], par[6]);
    //double phi_,theta_, psi_;
    TpcAlignmentHelper alHelper;
    // Initialization
    if (nCallsFcn == 0) {
        if (fPrintChi2) {
            (*txtoutfile2) << "First call to function nRes: " << nbins << "\n";
        }
        alHelper.getAnglesZYX(
                    fAlMan->getTransformation(fDetectorName)->GetRotation(), phi_, theta_,
                    psi_);
        
        // tpcpos_.SetXYZ(t1.x(), t1.y(), t1.z());
        // t1
        tpcpos_ = fAlMan->getTranslation(fDetectorName);
        std::cout << "#############################################################"
                     "#####################" << std::endl;
        std::cout << "nRes: " << nbins << 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;
        tpcpos_.Print();
        std::cout << "#############################################################"
                     "#####################" << std::endl;
        
        // tpcpos_ = t1
        if (fuseShiftedRotation) {
            // tpcpos_ = t1*
            // t1*=t1-t0+r1*t0
            TVector3 shift(0, 0, fshift);
            tpcpos_ -= shift;
            shift = fAlMan->localToMasterVect(
                        fDetectorName,
                        shift);  // TODO need to check that R1 and not R1T is used
            tpcpos_ += shift;
        }
        // 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 = new TGeoCombiTrans();
    TVector3 t2 = tpcpos_ + vadd;  // if shifted: t2*
    TGeoRotation *rot1 = (fAlMan->getTransformation(fDetectorName))->GetRotation();
    TGeoRotation rot1t;
    if (rot1 != 0) {
        rot1t.SetRotation(rot1->Inverse());
    }
    TGeoRotation rot2;
    rot2.SetMatrix(alHelper.getRotationMatrixZYX(phi_ + phipar, theta_ + thetapar,
                                                 psi_ + psipar).data());
    
    TGeoRotation rot12(rot1t);
    rot12.MultiplyBy(&rot2, false);  // rot2*rot1t
    
    std::vector<double> t1_ = TpcAlignmentManager::tVector3ToStlVector(
                tpcpos_);  // t1* in case of shifted rot
    double rot12_t1[3];
    
    rot12.LocalToMaster(t1_.data(), rot12_t1);  // rot2_*rot1t*t1
    TVector3 t12 =
            t2 - TVector3(rot12_t1);  // t12=t2-R12*t1, shifted: t12=t2*-r12 . t1*
    if (fuseShiftedRotation) {
        TVector3 t0(0, 0, fshift);  // t0
        std::vector<double> t0_ = TpcAlignmentManager::tVector3ToStlVector(t0);
        double rot12_t0[3];
        rot12.LocalToMaster(t0_.data(), rot12_t0);
        t12 = t12 - TVector3(rot12_t0) + t0;
    }
    TGeoTranslation trans12(t12.X(), t12.Y(), t12.Z());
    ctrans12->SetTranslation(trans12);
    ctrans12->SetRotation(rot12);
    //####################################################################################3
    // to be able to parallelize we get duplicate the transformations here
    unsigned int nThreads = 6;  // configurable on build
    std::vector<double> chisq(nThreads, 0);
    std::vector<int> numberOfNan(nThreads, 0);
    
    std::vector<TGeoCombiTrans> transf12;
    for (unsigned int test = 0; test < nThreads; ++test) {
        transf12.push_back(TGeoCombiTrans(*ctrans12));
    }
#pragma omp parallel for num_threads(nThreads)
    for (int i = 0; i < nbins; ++i) {
        TpcSimpleResidual *resi = (TpcSimpleResidual *)fSimpleResiduals[i];
        TVector3 ref(resi->fRefX, resi->fRefY, resi->fRefZ);
        
        std::vector<double> xyz_;
        xyz_.push_back(resi->fHitX);
        xyz_.push_back(resi->fHitY);
        xyz_.push_back(resi->fHitZ);
        
        double xyz_new[3];
        
        transf12[omp_get_thread_num()].LocalToMaster(xyz_.data(), xyz_new);
        
        TVector3 newxyz(xyz_new);
        
        TVector3 res;
        TVector3 resPerp;
        if (fUseTrackLinearization) {
            if (fUse3DResidual) {
                res = ref - newxyz;
                TVector3 dir(resi->fDirX, resi->fDirY, resi->fDirZ);
                TVector3 resPar = (res * dir) * dir;
                resPerp = res - resPar;
            } else if (true) {
                //2d extrapolation
                res = ref - newxyz;
                TVector3 dir(resi->fDirX, resi->fDirY, 0);
                TVector3 resPar = (res * dir) * dir;
                resPerp = res - resPar;
                //                std::cout<<"2D extrap: "<<std::flush;resPerp.Print();
                //                TVector3 dir3(resi->fDirX, resi->fDirY, resi->fDirZ);
                //                resPar = (res * dir3) * dir3;
                //                resPerp = res - resPar;
                //                std::cout<<"3D extrap: "<<std::flush;resPerp.Print();
            } else {
                TVector3 dir(resi->fDirX, resi->fDirY, resi->fDirZ);
                double d_x = resi->fDirX;
                double d_y = resi->fDirY;
                double p_x = resi->fRefX;
                double p_y = resi->fRefY;
                double r = newxyz.Perp();
                double a_ = dir.Perp2();  // pow(d_x,2)+pow(d_y,2);
                double b = 2 * (p_x * d_x + p_y * d_y);
                double c = pow(p_x, 2) + pow(p_y, 2) - pow(r, 2);
                double solution1 = (-b + sqrt(pow(b, 2) - 4 * a_ * c)) / (2 * a_);
                double solution2 = (-b - sqrt(pow(b, 2) - 4 * a_ * c)) / (2 * a_);
                double d = (fabs(solution1) <= fabs(solution2) ? solution1 : solution2);
                TVector3 newRef = ref + d * dir;
                resPerp = newRef - newxyz;
            }
        } else {
            resPerp = res;
            // res.SetZ(0);
        }
        //    TVectorD r(3);
        //    r[0]=resPerp.X();
        //    r[1]=resPerp.YWrite();
        //    r[2]=resPerp.Z();
        //    double resErr=r*(cov*r);
        double resErr = 1;//resi->fTrackErrPhi;  // 0.0270932;//0.1646*0.1646;
        double resZ = 500;//resi->fTrackErrZ;
        double weight = 1;
        //        int binW=wAxis->FindBin(resi->fHitW);
        //        int binRl=rlAxis->FindBin(hypot(resi->fHitU,resi->fHitV));
        //        double
        //        weight=1/(wHist->GetBinContent(binW))+1/(rlHist->GetBinContent(binRl));
        
        if (fUse3DResidual) {
            chisq[omp_get_thread_num()] +=
                    resPerp.Mag2() / resErr;  //*TMath::Power(1/resErr,2);
        } else {
            double increment = resPerp.Perp2() / resErr + pow(resPerp.Z(), 2) / resZ;
            if (increment != increment) {
                std::cout<<"chi2Increment"<<increment<<", resPerp.Perp2(): "
                        <<resPerp.Perp2()<<", resPerp.Z(): "
                       <<resPerp.Z() <<"newxyz ";newxyz.Print();
                res = ref - newxyz;
                TVector3 dir(resi->fDirX, resi->fDirY, resi->fDirZ);
                TVector3 resPar = (res * dir) * dir;
                resPerp = res - resPar;
                chisq[omp_get_thread_num()] +=
                        (resPerp.Perp2() / resErr) *
                        weight;  // + resPerp.Z() * resPerp.Z() / 16);//*resi->fTrackLength;
                numberOfNan[omp_get_thread_num()]++;
            } else {
                chisq[omp_get_thread_num()] +=
                        increment *
                        weight;  //*resi->fTrackLength; //*TMath::Power(1/resErr,2);
            }
        }
    }
    double ret = 0;
    int nNan = 0;
    for (unsigned int test = 0; test < nThreads; ++test) {
        ret += chisq[test];
        nNan += numberOfNan[test];
    }
    
    if (nCallsFcn == 0) {
        fFirstChiSq = ret;
        std::cout << "First call!" << fFirstChiSq<<std::cout;
        vadd.Print();
        ctrans12->Print();
    }
    
    ++nCallsFcn;
    if (fPrintChi2) {
        std::cout << " (Chi2: " << std::setprecision(15) << ret
                  << " ) NRES: " << nbins << " Chi2/NRES: " << ret / ((double)nbins)
                  << " (pos: (" << par[0] << ", " << par[1] << ", " << par[6]
                  << ") "
                  << "Angles (" << par[2] << ", " << par[3] << ", " << par[4]
                  << ") " << std::endl;
        (*txtoutfile2) << " (Chi2: " << std::setprecision(15) << ret
                       << " ) NRES: " << nbins
                       << " Chi2/NRES: " << ret / ((double)nbins) << " (pos: ("
                       << par[0] << ", " << par[1] << ", " << par[6] << ") "
                       << "Angles (" << par[2] << ", " << par[3] << ", " << par[4]
                       << ") " << std::endl;
        delete txtoutfile2;
    }
    if (ret != ret) {
        std::cout << ret << std::endl;
        throw;
    }
    
    return ret;
}
bool TpcAligner::getPrintChi2() const
{
    return fPrintChi2;
}

void TpcAligner::setPrintChi2(bool value)
{
    fPrintChi2 = value;
}