//-----------------------------------------------------------
// 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 <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 "FairField.h"
#include "FairRunAna.h"

#include "PndFieldAdaptor.h"

#include "TpcAlignmentSimple.h"
#include "TpcRefGFTrkResCalc.h"
#include "TpcRefGFTrkTpcClusterResCalc.h"
#include "TpcCdcFit2DResCalc_Alignment.h"

#include "RKTrackRep.h"
#include "GFException.h"

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

#include "CdcHit.h"
#include "CdcTrack.h"

#include <stdlib.h>
#include <omp.h>
#include <sstream>
#include <fstream>
#include <iomanip>
//##############################################
//# initialization of static variables
//##############################################
TpcAlignmentSimple *TpcAlignmentSimple::lol = NULL;
TStopwatch TpcAlignmentSimple::clock;
int TpcAlignmentSimple::nCallsFcn = 0;
int TpcAlignmentSimple::fVerbose = 0;
bool TpcAlignmentSimple::fuseShiftedRotation(false);
double TpcAlignmentSimple::fshift(50);
bool TpcAlignmentSimple::fUseTrackLinearization(false);
bool TpcAlignmentSimple::fUse3DResidual(false);
bool TpcAlignmentSimple::fUse2DResidual(false);
std::string TpcAlignmentSimple::detectorName = "tpc";
std::string TpcAlignmentSimple::alignmentFile =
        (TString(getenv("VMCWORKDIR")) +
         TString("tpc/FOPI/par/june11_alignment_gem0.txt")).Data();
std::vector<TpcResidual *> TpcAlignmentSimple::fResiduals;
std::vector<TpcSimpleResidual *> TpcAlignmentSimple::fSimpleResiduals;
TClonesArray TpcAlignmentSimple::fTCResiduals("TpcResidual");
double TpcAlignmentSimple::meanZ = 0.;
double TpcAlignmentSimple::phi_ = 0.;
double TpcAlignmentSimple::theta_ = 0.;
double TpcAlignmentSimple::psi_ = 0.;
double TpcAlignmentSimple::dphi_ = 0.;
double TpcAlignmentSimple::dtheta_ = 0.;
double TpcAlignmentSimple::dpsi_ = 0.;
double TpcAlignmentSimple::firstChiSq = 0;
TVector3 TpcAlignmentSimple::tpcpos_(0, 0, 0);
TVector3 TpcAlignmentSimple::dtpcpos_(0, 0, 0);
TGeoCombiTrans *TpcAlignmentSimple::lastTransP1P2 = new TGeoCombiTrans();
bool TpcAlignmentSimple::printChi2(false);


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

double getPhiInSector(double phi) {
    double phii = phi - 11.25;
    if (phii < 0) {
        phii += 360;
    }
    return (fmod(phii, 22.5) - 11.25);
}

TpcAlignmentSimple::TpcAlignmentSimple()
  :       oldtpcxyz(""),
	  inititalTpcPos(0, 0, 0),
	  initialPhi(0),
	  initialTheta(0),
	  initialPsi(0),
	  xyzPhi(0),
	  xyzTheta(0),
	  xyzPsi(0),
	  fBatSimAn(true),
	  fUseBat(false),
	  fBatItMax(5000),
	  fBatItMin(5000),
	  fBatItUpdate(2000),
	  fBatNChains(4),
	  fAutoRotationShift(false),
	  maxRes(40),
	  maxResXY(3.5),
	  maxResZ(8),
	  rangeXY(15),
	  rangeZ(40),
	  zSlices(10),
	  zStart(-25),
	  zEnd(30),
	  cdcTrackBranchName("CdcTrack"),
	  cdcGFTrackBranchName("CdcTrackPostFit"),	  
	  cdcGFHitBranchName("CdcSpacepoint"),
	  cdcHitBranchName("CdcHit"),
	  tpcTrackBranchName("TpcTrackPostFit"),
	  tpcClusterBranchName("TpcCluster"),
	  tpcShieldPointBranchName("TpcShieldPoint"),
	  fopiTupleBranchName("FopiTrackTuples"),
	  fopiTrackBranchName("FopiTrackPostFit"),
	  tpcEventHeaderBranchName("TpcEventIdentifier"),
	  cdcTracks(NULL),
	  cdcGFTracks(NULL),
	  cdcGFHits(NULL),
	  cdcHits(NULL),
	  tpcClusters(NULL),
	  tpcSPHits(NULL),
	  tpcTracks(NULL),
	  shieldPoints(NULL),
	  mcTracks(NULL),
	  trackTuples(NULL),
	  fopiTracks(NULL),
	  tpcEventHeaders(NULL),
	  alMan(NULL),
	  inputChain(NULL),
	  residualCalculator(NULL),
	  resCalcName(""),
	  maxEvents(0),
	  storeResiduals(true),
	  residualFile(""),
	  chargeFiltering(0) ,
	  
	  fHistoFile(NULL),
	  fHistoFileName("AlignmentHists.root"),
	  testRun(false)










      {
    // residualCalculator = new TpcRefGFTrkResCalc();
    if (lol != NULL) {
        throw;
    }
    lol = this;
}

bool TpcAlignmentSimple::init() {
    if (residualCalculator == NULL) {
        std::cerr << "TpcAlignmentSimple::init residual calculator missing"
                  << std::endl;
        return false;
    }
    if (inputChain == NULL) {
        std::cerr << "TpcAlignmentSimple::init() \n "
                  << "Input chain missing, please execuite setInputChain(chain) "
                     "before running init" << std::endl;
        return false;
    }
    cdcTracks = new TClonesArray("CdcTrack");
    cdcHits= new TClonesArray("CdcHit");
    cdcGFTracks = new TClonesArray("GFTrack");
    cdcGFHits = new TClonesArray("CdcSpacepoint");
    tpcClusters = new TClonesArray("TpcCluster");
    tpcTracks = new TClonesArray("GFTrack");
    tpcSPHits = new TClonesArray("TpcSPHit");
    shieldPoints = new TClonesArray("TpcShieldPoint");
    mcTracks = new TClonesArray("PndMCTrack");
    trackTuples = new TClonesArray("MatchingTuple");
    fopiTracks = new TClonesArray("GFTrack");
    tpcEventHeaders = new TClonesArray("TpcEventIdentifier");
    std::cout
            << "#################################################################"
            << std::endl;
    std::cout << " Adding input branches to branchMap " << std::endl;
    std::cout
            << "#################################################################"
            << std::endl;
    
    int test =
            inputChain->SetBranchAddress(fopiTupleBranchName.c_str(), &trackTuples);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init FopiTuple-array not found!"
                  << test << std::endl;
    } else {
        fBranches[fopiTupleBranchName] = trackTuples;
    }
    
    test = inputChain->SetBranchAddress(tpcEventHeaderBranchName.c_str(),
                                        &tpcEventHeaders);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init tpcEventIdentifier not found!"
                  << test << std::endl;
    } else {
        fBranches[tpcEventHeaderBranchName] = tpcEventHeaders;
    }
    
    test = inputChain->SetBranchAddress(fopiTrackBranchName.c_str(), &fopiTracks);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init fopitracks not found!" << test
                  << std::endl;
    } else {
        fBranches[fopiTrackBranchName] = fopiTracks;
    }
    
    test = inputChain->SetBranchAddress(cdcTrackBranchName.c_str(), &cdcTracks);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init CDC Track-array not found!"
                  << test << std::endl;
    } else {
        fBranches[cdcTrackBranchName] = cdcTracks;
    }
    test = inputChain->SetBranchAddress(cdcHitBranchName.c_str(), &cdcHits);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init CDC Hit-array not found!"
                  << test << std::endl;
    } else {
        fBranches[cdcHitBranchName] = cdcHits;
    }
    test =
            inputChain->SetBranchAddress(cdcGFTrackBranchName.c_str(), &cdcGFTracks);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init CDC GF Track-array not found!"
                  << test << std::endl;
    } else {
        fBranches[cdcGFTrackBranchName] = cdcGFTracks;
    }
    test = inputChain->SetBranchAddress(cdcGFHitBranchName.c_str(), &cdcGFHits);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init CDC GF HIT-array not found!"
                  << test << std::endl;
    } else {
        fBranches[cdcGFHitBranchName] = cdcGFHits;
    }
    
    test =
            inputChain->SetBranchAddress(tpcClusterBranchName.c_str(), &tpcClusters);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::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 << "TpcAlignmentSimple::Init() Init TpcTrack-array not found!"
                  << test << std::endl;
    } else {
        fBranches[tpcTrackBranchName] = tpcTracks;
    }
    test = inputChain->SetBranchAddress("TpcSPHit", &tpcSPHits);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init TpcSPHit-array not found!"
                  << test << std::endl;
    } else {
        fBranches["TpcSPHit"] = tpcSPHits;
    }
    test = inputChain->SetBranchAddress("TpcShieldPointAl", &shieldPoints);
    if (test < 0) {
        std::cerr
                << "TpcAlignmentSimple::Init() Init TpcShieldPoint-array not found!"
                << test << std::endl;
    } else {
        std::cout << "TpcShieldPointsAl found" << std::endl;
        fBranches["TpcShieldPointAl"] = shieldPoints;
    }
    
    test = inputChain->SetBranchAddress("TpcShieldPoint", &shieldPoints);
    if (test < 0) {
        std::cerr
                << "TpcAlignmentSimple::Init() Init TpcShieldPoint-array not found!"
                << test << std::endl;
    } else {
        std::cout << "TpcShieldPoints found" << std::endl;
        fBranches["TpcShieldPoint"] = shieldPoints;
    }
    
    test = inputChain->SetBranchAddress("MCTrack", &mcTracks);
    if (test < 0) {
        std::cerr << "TpcAlignmentSimple::Init() Init MCTrack-array not found!"
                  << test << std::endl;
    } else {
        std::cout << "MCTrack found" << std::endl;
        fBranches["MCTrack"] = mcTracks;
    }
    
    inputChain->SetCacheSize(100e6);
    TTreeCache::SetLearnEntries(10);
    // std::cout << "ALMAN..." << std::endl;
    TpcAlignmentManager::init(alignmentFile.c_str());
    alMan = TpcAlignmentManager::getInstance();
    if (alMan == 0) {
        std::cerr << "TpcAlignmentSimple::init() Init TpcAlignmentManager:"
                  << alignmentFile.c_str() << " not found!" << std::endl;
        return false;
    }
    
    alMan->getEulerAngles(detectorName, initialPhi, initialTheta, initialPsi);
    inititalTpcPos = alMan->getTranslation(detectorName);
    
    TpcAlignmentHelper a;
    if (alMan->getTransformation(detectorName)->GetRotation() != 0) {
        a.getAnglesZYX(alMan->getTransformation(detectorName)->GetRotation(),
                       xyzPhi, xyzTheta, xyzPsi);
    } else {
        xyzPhi = 0;
        xyzTheta = 0;
        xyzPsi = 0;
    }
    std::cout << "Intput AlignmentFile: " << alignmentFile.c_str() << std::endl;
    std::cout << detectorName << " TPC xyz(" << inititalTpcPos.X() << ", "
              << inititalTpcPos.Y() << ", " << inititalTpcPos.Z() << ")."
              << std::endl;
    std::cout << "Rot. angles_ROOT(" << initialPhi << ", " << initialTheta << ", "
              << initialPsi << ")" << std::endl;
    std::cout << "Rot. angles_XYZ(" << xyzPhi << ", " << xyzTheta << ", "
              << xyzPsi << ")" << std::endl;
    
    oldtpcxyz.Form("OLD TPC xyz(%f, %f, %f). Rot. angles(%f, %f, %f)",
                   inititalTpcPos.X(), inititalTpcPos.Y(), inititalTpcPos.Z(),
                   initialPhi, initialTheta, initialPsi);
    
    fHistoFile = new TFile(fHistoFileName, "RECREATE");
    fHistoFile->cd();
    //################################################################################################
    // Init QA histograms
    //################################################################################################
    initHistos();
    //################################################################################################
    
    std::cout << "adding branches to residual calculator branch-map" << std::endl;
    // Adding Branches to the residual calculator
    std::map<TString, TClonesArray *> *branches =
            residualCalculator->getBranchMap();
    std::map<TString, TClonesArray *>::iterator it;
    std::cout << "looking for " << branches->size() << " branches" << std::endl;
    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("TpcAlignmentSimple::Init()", s.c_str());
        }
        it->second = br;
    }
    // Initializing, searching for set branches
    std::cout << "TpcAlignmentSimple:init() initializing residual calculator: "
              << std::endl;
    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;
    return false;
}

bool TpcAlignmentSimple::execute() {
    std::cout << "TpcAlignmentSimple::execute()" << std::endl;
    
    fResiduals.clear();  // TODO: should I delete?
    unsigned int nEvents = inputChain->GetEntries();
    
    if (maxEvents != 0 && (int)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);
    }
    clock.Start();
    
    std::cout << "TpcAlignmentSimple::execute() nEvents:" << nEvents << std::endl;
    for (unsigned int iEv = 0; iEv < nEvents; ++iEv) {
        if (iEv % 1000 == 0) {
            std::cout << " " << iEv << " ";
        }
        if (iEv % 10000 == 0) {
            std::cout << "###########################################################"
                         "######### " << iEv << " / " << nEvents << " ("
                      << ((double)iEv / (double)nEvents) * 100 << " %)"
                      << " ##########################################################"
                         "###########" << std::endl;
            clock.Print();
            clock.Continue();
        }
        
        tpcTracks->Delete();
        cdcGFTracks->Delete();
        fopiTracks->Delete();
        // cdcTracks->Delete();
        // tpcClusters->Delete();
        // tpcSPHits->Delete();
        // shieldPoints->Delete();
        // get the event
        inputChain->GetEvent(iEv);
        if (storeResiduals && testRun) {
            fTCResiduals.Delete();
        }
        // if(tpcEventHeaders->GetEntriesFast()>0){
        //   TpcEventIdentifier* evId=(TpcEventIdentifier*)tpcEventHeaders->At(0);
        //   int evInSpill=evId->getEventInSpill();
        //   if(evInSpill>2){
        // 	continue;
        //   }
        // }
        residualCalculator->calc();
        
        std::vector<TpcRefResidualCollection *> *resCols =
                residualCalculator->getResiduals();
        int nRes = resCols->size();
        for (int ires = 0; ires < nRes; ++ires) {
            TpcRefResidualCollection *resCol = (*resCols)[ires];
            TClonesArray* refArray=fBranches[resCol->getRefBranch()];
            TVector3 cdcPos20, cdcMom20;
            std::string type(refArray->GetClass()->GetName());
            if(type.find("CdcTrack") <type.size()){
                CdcTrack* tr=(CdcTrack*)refArray->At(resCol->getRefIndex());
                TpcCdcFit2DResCalc_Alignment::getTrackCylinderIntersect(tr,20,cdcPos20, cdcMom20);
            }else if(type.find("GFTrack")<type.size()){
                GFTrack* refTrack=(GFTrack*) refArray->At(resCol->getRefIndex());refArray->At(resCol->getRefIndex());
                RKTrackRep* refRep=(RKTrackRep*)refTrack->getCardinalRep();
                
                try{
                    refRep->extrapolateToCylinder(20,cdcPos20,cdcMom20);
                }catch(GFException exp){
                    std::cout<<"TpcAlignmentSimple:: Error extrapolating cdc track to cyl r=20\n";
                    std::cout<<exp.what()<<"\n";
                    continue;
                }    
            }else{
                std::cerr<<"No ref track!?: "<<refArray->GetClass()->GetName()<<std::endl;
                throw;
            }
            
            
            for (int ii = 0; ii < resCol->getSize(); ii++) {
                const TpcResidual *miaou;
                TpcSimpleResidual *miaouSimple;
                if (testRun) {
                    if (storeResiduals) {
                        Int_t pos = fTCResiduals.GetEntries();
                        miaou =
                                new (fTCResiduals[pos]) TpcResidual(*resCol->getResidual(ii));
                    } else {
                        miaou = resCol->getResidual(ii);
                    }
                } else {
                    miaou = resCol->getResidual(ii);
                    int charge = miaou->getCharge();
                    if (chargeFiltering == 0 or charge * chargeFiltering > 0) {
                        TpcAlignmentSimple::fSimpleResiduals.push_back(
                                    new TpcSimpleResidual(*miaou));
                        miaouSimple = fSimpleResiduals.back();
                    } else {
                        continue;
                    }
                }
                
                // from mathematica
                //((-2*a*b - 2*e*f - Sqrt(Power(2*a*b + 2*e*f,2) - 4*(Power(b,2) +
                // Power(f,2))*(Power(a,2) + Power(e,2) - Power(r,2))))/(2.*(Power(b,2)
                // +
                // Power(f,2))))
                //(p_x+d*dir_x)^2 + (p_y+d*dir_y)^2 = r ^2
                //
                
                // recalculating residual
                TVector3 uvw = alMan->masterToLocal("tpc", miaou->getHitPos());
                miaouSimple->fHitU = uvw.x();
                miaouSimple->fHitV = uvw.y();
                miaouSimple->fHitW = uvw.z();
                fillHistos(miaou, cdcPos20,cdcMom20);
                
            }  // loop over residuals in this collection
        }    // loop over residual collections for this event
        if (storeResiduals && testRun) {
            std::cout << fTCResiduals.GetEntries() << std::endl;
            outfile->cd();
            outTree->Fill();
        }
    }  // Event loop end
    if (fAutoRotationShift) {
        double w = 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;
    }
    writeHistos();
    fHistoFile->Save();
    if (testRun) {
        if (storeResiduals) {
            outTree->Write();
            outfile->Close();
        }
        return true;
    }
    std::cout << "TpcAlignmentSimple::exec() " << fSimpleResiduals.size()
              << " TpcSimpleResiduals"
              << "Starting alignment using ";
    double x, y, xerr, yerr, ntheta, nthetaerr, nphi, nphierr, npsi, npsierr, ndr,
            ndrerr, z, zerr;
    std::vector<double> pars;
    std::vector<double> simAnPars;
    std::vector<std::string> parNames;
    if (fUseBat) {
        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->MCMCSetNIterationsEfficiencyCheck(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);
        if (!fBatSimAn) {
            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();
            mod->PrintAllMarginalized("bat.pdf");
            mod->PrintCorrelationPlot("bat_correlations.pdf");
	    //            mod->PrintResults("bat_results.txt");
	    //            pars = mod->GetBestFitParameters();
	    pars = mod->GetGlobalMode();
            errs = mod->GetBestFitParameterErrors();
        } else {
            // 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 test[2];
            test[0] = 10000;
            test[1] = 0.1;
            mod->SetMinuitArgList(test);
            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) {
                BCParameter par = mod->GetParameter(haha);
                parNames.push_back(par.GetName());
                std::cout << "par " << haha << " : " << par.GetName() << ": "
                          << 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 << "par " << haha << " : "
                          << (mod->GetParameter(haha)).GetName() << ": " << pars[haha]
                             << std::endl;
            }
        }
        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 = errs[0];
        yerr = errs[1];
        nphierr = errs[2];
        nthetaerr = errs[3];
        // npsierr=pars[2]*-1+pars[4];
        npsierr = errs[4];
        ndrerr = errs[5];
        zerr = errs[6];
    } else {
        std::cout << "TMinuit" << std::endl;
        meanZ = 0;
        TMinuit *fMinuit;
        fMinuit = new TMinuit(7);
        fMinuit->SetFCN(&this->fcn);
        double fErr = 0.01;
        fMinuit->DefineParameter(0, "x", 0., fErr, -3., 3.);
        fMinuit->DefineParameter(1, "y", 0., fErr, -3., 3.);
        
        // angles defined in degrees
        fMinuit->DefineParameter(2, "phi", 0., fErr, -10, 10);  //-3.
        fMinuit->DefineParameter(3, "theta", 0., fErr, -10, 10);
        fMinuit->DefineParameter(4, "psi", 0., fErr, -10, 10);
        // alternative
        
        fMinuit->DefineParameter(5, "drift", 1., fErr, 0.7, 1.3);
        fMinuit->DefineParameter(6, "z", 0., fErr, -5., 5.);
        // 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(0);
        // gMinuit->FixParameter(1);
        //gMinuit->FixParameter(2);
        //        gMinuit->FixParameter(3);
        //        gMinuit->FixParameter(4);
        gMinuit->FixParameter(5);
        // gMinuit->FixParameter(6);
        
        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::vector<double> tmp_pars;
    tmp_pars.push_back(x);
    tmp_pars.push_back(y);
    tmp_pars.push_back(nphi);
    tmp_pars.push_back(ntheta);
    tmp_pars.push_back(npsi);
    tmp_pars.push_back(ndr);
    tmp_pars.push_back(z);
    // Calculate Chi2 for final parameters
    double chi2Final = chisquare(tmp_pars.size(), tmp_pars.data());
    
    std::cout << "Intital angles (phi,theta,psi) (" << initialPhi << ", "
              << initialTheta << ", " << initialPsi << ")" << std::endl;
    TpcAlignmentHelper alHelper;
    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;
    std::cout << "initialPos ";
    inititalTpcPos.Print();
    // calculating new position
    TVector3 newTpcPos = inititalTpcPos + TVector3(x, y, z);  // t1+dt
    TVector3 newTpcPos_shifted =
            tpcpos_ + TVector3(x, y, z) + 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*
        alMan->setRotationMatrix(
                    detectorName, alHelper.getRotationMatrixZYX(
                        xyzPhi + nphi, xyzTheta + ntheta, xyzPsi + npsi));
        TVector3 r2_t0 =
                alMan->localToMasterVect(detectorName, TVector3(0, 0, fshift));
        
        alMan->setRotationMatrix(
                    detectorName, alHelper.getRotationMatrixZYX(xyzPhi, xyzTheta, xyzPsi));
        TVector3 r1_t0 =
                alMan->localToMasterVect(detectorName, 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 << "Alman->print before" << std::endl;
    alMan->Print();
    std::cout << "Alman->write(" << outfilealman << ")" << std::endl << std::endl;
    alMan->write(outfilealman);
    
    double oldPhi, oldTheta, oldPsi;
    alMan->getEulerAngles(detectorName, oldPhi, oldTheta, oldPsi);
    std::cout << "setNew Rot Matrix" << std::endl;
    alMan->setRotationMatrix(
                detectorName, alHelper.getRotationMatrixZYX(
                    xyzPhi + nphi, xyzTheta + ntheta, xyzPsi + npsi));
    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 << std::endl << "Alman->print after" << std::endl;
    alMan->Print();
    
    std::ofstream txtoutfile("Result.txt");
    txtoutfile << "Alignment results using ";
    if (fUseBat) {
        txtoutfile << " BAT ";
    } else {
        txtoutfile << " Minuit ";
    }
    TVector3 difpos = newTpcPos - inititalTpcPos;
    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: " << firstChiSq << std::endl;
    if (fUseBat) {
        txtoutfile << "fBatSimAn: " << fBatSimAn << 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 << "residual calc: " << resCalcName << std::endl;
    txtoutfile << "charge filtering (+:pos,-:neg,0:all): " << chargeFiltering
               << std::endl;
    txtoutfile << "initial pos(xyz):\t" << inititalTpcPos.x() << ", "
               << inititalTpcPos.y() << ", " << inititalTpcPos.z() << std::endl;
    txtoutfile << "new pos(xyz):     \t" << newTpcPos.x() << " " << newTpcPos.y()
               << " " << newTpcPos.z() << std::endl;
    txtoutfile << "initial angles (phi,theta,psi)_xyz: \t" << phi_ << " "
               << theta_ << " " << psi_ << "" << std::endl;
    txtoutfile << "new angles (phi,theta,psi)_xyz: \t" << xyzPhi + nphi << " "
               << xyzTheta + ntheta << " " << xyzPsi + 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() << " (" << xerr << "), ";
    txtoutfile << "dy: " << difpos.y() << " (" << yerr << "), ";
    txtoutfile << "dz: " << difpos.z() << " (" << zerr << "), " << std::endl;
    txtoutfile << "dphi: " << nphi << " (" << nphierr << "), ";
    txtoutfile << "dtheta: " << ntheta << " (" << nthetaerr << "), ";
    txtoutfile << "dpsi: " << npsi << " (" << npsierr << "), " << std::endl;
    if (fUseBat && fBatSimAn) {
        txtoutfile << "sim an pars\n";
        for (unsigned int i = 0; i < simAnPars.size(); ++i) {
            txtoutfile << parNames[i] << ": " << simAnPars[i] << ", ";
        }
        txtoutfile << "\n minuit pars: \n";
        for (unsigned int i = 0; i < pars.size(); ++i) {
            txtoutfile << parNames[i] << ": " << pars[i] << " ";
        }
        txtoutfile << "\n";
    }
    outfilealman = "alignmentAfter.txt";
    alMan->write(outfilealman);
    fHistoFile->Close();
    std::cout << "file closed " << fHistoFileName << std::endl;
    writeChiSqHists();
    
    return false;
}
// For TMinuit
void TpcAlignmentSimple::fcn(Int_t &npar, Double_t *gin, Double_t &f,
                             Double_t *par, Int_t iflag) {
    f = lol->chisquare(npar, par);
}

double TpcAlignmentSimple::chisquare(int npar, double *par) {
    if (nCallsFcn % 100 == 0) {
        std::cout << "." << std::flush;
    }
    std::ofstream *txtoutfile2;
    if (printChi2) {
        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();
    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]);
    
    TpcAlignmentHelper alHelper;
    // Initialization
    if (nCallsFcn == 0) {
        if (printChi2) {
            (*txtoutfile2) << "First call to function nRes: " << nbins << "\n";
        }
        alHelper.getAnglesZYX(
                    sAlman->getTransformation(detectorName)->GetRotation(), phi_, theta_,
                    psi_);
        
        // tpcpos_.SetXYZ(t1.x(), t1.y(), t1.z());
        // t1
        tpcpos_ = sAlman->getTranslation(detectorName);
        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 = sAlman->localToMasterVect(
                        detectorName,
                        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 = (sAlman->getTransformation(detectorName))->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);
    // create vectors of transformations
    //  std::vector<TGeoCombiTrans> transf;TVector3 dir = miaou->getTrackDir();
    //  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(alHelper.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));
    }
    TH1 *wHist = hists["hitsW"];
    //    TAxis *wAxis = wHist->GetXaxis();
    //TH1 *rlHist = hists["hitsRl"];
    //TAxis *rlAxis = rlHist->GetXaxis();
#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);
        // TVector3 hit(resi->fHitX,resi->fHitY,resi->fHitZ);
        // TMatrixD cov(3,3,resi->fHitCov.data());
        //    std::vector<double>
        //    xyz_=TpcAlignmentManager::tVector3ToStlVector(hit);
        
        std::vector<double> xyz_;
        xyz_.push_back(resi->fHitX);
        xyz_.push_back(resi->fHitY);
        xyz_.push_back(resi->fHitZ);
        // double uvw[3]; //legacy compatability to be able to to back/forth
        // transformation
        // 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);
        // transfBack[omp_get_thread_num()].LocalToMaster(uvw,xyz_newBackForth);
        
        TVector3 newxyz(xyz_new);
        if (false) {
            TVector3 origXYZ(xyz_.data());
            TVector3 shift = newxyz - origXYZ;
            chi2Hists["hshiftX"]->Fill(shift.x());
            chi2Hists["hshiftY"]->Fill(shift.y());
            chi2Hists["hshiftZ"]->Fill(shift.z());
            chi2Hists["hhifts"]->Fill(shift.Mag());
            chi2Hists["hshift2D"]->Fill(shift.Perp());
            
            chi2Hists["hshiftXVsRefX"]->Fill(ref.x(), shift.x());
            chi2Hists["hshiftYVsRefX"]->Fill(ref.x(), shift.y());
            chi2Hists["hshiftZVsRefX"]->Fill(ref.x(), shift.z());
            chi2Hists["hhiftsVsRefX"]->Fill(ref.x(), shift.Mag());
            chi2Hists["hshift2DVsRefX"]->Fill(ref.x(), shift.Perp());
            
            chi2Hists["hshiftXVsRefY"]->Fill(ref.y(), shift.x());
            chi2Hists["hshiftYVsRefY"]->Fill(ref.y(), shift.y());
            chi2Hists["hshiftZVsRefY"]->Fill(ref.y(), shift.z());
            chi2Hists["hhiftsVsRefY"]->Fill(ref.y(), shift.Mag());
            chi2Hists["hshift2DVsRefY"]->Fill(ref.y(), shift.Perp());
            
            chi2Hists["hshiftXVsRefZ"]->Fill(ref.z(), shift.x());
            chi2Hists["hshiftYVsRefZ"]->Fill(ref.z(), shift.y());
            chi2Hists["hshiftZVsRefZ"]->Fill(ref.z
(), shift.z());
            chi2Hists["hhiftsVsRefZ"]->Fill(ref.z(), shift.Mag());
            chi2Hists["hshift2DVsRefZ"]->Fill(ref.z(), shift.Perp());
            
            chi2Hists["hshiftYvX"]->Fill(shift.x(), shift.y());
            chi2Hists["hshiftXvZ"]->Fill(shift.z(), shift.x());
            chi2Hists["hshiftYvZ"]->Fill(shift.z(), shift.y());
            ((TH3 *)chi2Hists["hshiftXvYvZ"])->Fill(shift.x(), shift.y(), shift.z());
            
            chi2Hists["hpardX"]->Fill(vadd.x());
            chi2Hists["hpardY"]->Fill(vadd.y());
            chi2Hists["hpardZ"]->Fill(vadd.z());
            chi2Hists["hpardPhi"]->Fill(phipar);
            chi2Hists["hpardTheta"]->Fill(thetapar);
            chi2Hists["hpardWritePsi"]->Fill(psipar);
        }
        //*********************************
        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 = 10000;//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+resPerp.Z()*resPerp.Z()/100;
            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);
            }
        }
    }
    // meanZ = sum/(1.*nbins);
    double ret = 0;
    int nNan = 0;
    for (unsigned int test = 0; test < nThreads; ++test) {
        ret += chisq[test];
        nNan += numberOfNan[test];
    }
    
    // std::cout << "number of nans " << nNan << " number of hits:" << nbins <<
    // "\n";
    //   //transf12[omp_get_thread_num()].Print();
    //   // vadd.Print();
    // std::cout << phipar << ", " << thetapar << ", " << psipar << "\n";
    // std::cout << "nCallsFcn: " << nCallsFcn << std::setprecision(15)
    //          << " chi2: " << ret << "\n";
    //   //throw;
    // }
    // throw;
    //  dtpcpos_.SetXYZ(par[0], par[1], par[6]);
    if (nCallsFcn == 0) {
        firstChiSq = ret;
        std::cout << "First call!" << firstChiSq<<std::cout;
        vadd.Print();
        ctrans12->Print();
    }
    
    ++nCallsFcn;
    if (printChi2) {
        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;
    /// eturn chisq[0]+chisq[1]+chisq[2];
}

void TpcAlignmentSimple::writeHistos() {
    std::cout << "writing hists to file: " << fHistoFileName << std::endl;
    fHistoFile->cd();
    std::map<TString, TH1 *>::iterator it;
    for (it = hists.begin(); it != hists.end(); ++it) {
        std::cout << "." << std::flush;
        fHistoFile->cd();
        fHistoFile->cd(histDirName[(*it).first]);
        (*it).second->Write();
        if (it->second->InheritsFrom("TH2D")) {
            TString name = it->second->GetName();
            if (name.Contains("phi") or name.Contains("Phi") or
                    name.Contains("vPerp")) {
                TProfile *temp = ((TH2D *)it->second)->ProfileX();
                temp->Write();
            }
        }
    }
}

// void TpcAlignmentSimple::getRealParameters(const std::vector<double> pars,
//                                           std::vector<double>& realpars){
void TpcAlignmentSimple::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] = xyzPhi + pars[2];
    realpars[3] = xyzTheta + pars[3];
    realpars[4] = xyzPsi + pars[4];
    // inititalTpcPos
    TpcAlignmentHelper alHelper;
    TVector3 newTpcPos = inititalTpcPos + TVector3(pars[0], pars[1], pars[6]);
    if (fuseShiftedRotation) {
        alMan->setRotationMatrix(
                    detectorName,
                    alHelper.getRotationMatrixZYX(realpars[2], realpars[3], realpars[4]));
        TVector3 r2_t0 =
                alMan->localToMasterVect(detectorName, TVector3(0, 0, fshift));
        
        alMan->setRotationMatrix(
                    detectorName, alHelper.getRotationMatrixZYX(xyzPhi, xyzTheta, xyzPsi));
        TVector3 r1_t0 =
                alMan->localToMasterVect(detectorName, 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();
}
void TpcAlignmentSimple::initHistos() {
    fHistoFile->mkdir("1d-histograms");
    fHistoFile->cd("1d-histograms");
    TH1D *hresx =
            new TH1D("hresX", "residual X; dx (cm)", 8000, -maxResXY, maxResXY);
    hists["hresx"] = hresx;
    histDirName["hresx"] = "1d-histograms";
    TH1D *hresy =
            new TH1D("hresY", "residual Y; dy (cm)", 8000, -maxResXY, maxResXY);
    hists["hresy"] = hresy;
    histDirName["hresy"] = "1d-histograms";
    TH1D *hresz =
            new TH1D("hresZ", "residual Z; dz (cm)", 8000, -maxResZ, maxResZ);
    hists["hresz"] = hresz;
    histDirName["hresz"] = "1d-histograms";
    TH1D *hresPerp = new TH1D("hresPerp", "Resolution on Perp plane;; dperp (cm)",
                              8000, 0, maxResXY);
    hists["hresPerp"] = hresPerp;
    histDirName["hresPerp"] = "1d-histograms";
    TH1D *hresMag = new TH1D("hresMag", "residual; dMag (cm)", 8000, 0, 10);
    hists["hresMag"] = hresMag;
    histDirName["hresMag"] = "1d-histograms";
    TH1D *hresPhi =
            new TH1D("hresPhi", "#phi residual; dPhi (deg)", 8000, -10, 10);
    hists["hresPhi"] = hresPhi;
    histDirName["hresPhi"] = "1d-histograms";
    TH1D *hresPhiPlus =
            new TH1D("hresPhiPlus", "#phi residual, plus; dPhi (deg)", 8000, -10, 10);
    hists["hresPhiPlus"] = hresPhiPlus;
    histDirName["hresPhiPlus"] = "1d-histograms";
    TH1D *hresPhiMinus = new TH1D(
                "hresPhiMinus", "#phi residual, minus; dPhi (deg)", 8000, -10, 10);
    hists["hresPhiMinus"] = hresPhiMinus;
    histDirName["hresPhiMinus"] = "1d-histograms";
    TH1D *hres_trDist =
            new TH1D("hres_trDist", "extrapolation length; cm", 8000, -100, 100);
    hists["hres_trDist"] = hres_trDist;
    histDirName["hres_trDist"] = "1d-histograms";
    TH1D *hresSignedPerp =
            new TH1D("hresSignedPerp", "Resolution on Perp plane; dperp (cm)", 8000,
                     -maxResXY, maxResXY);
    hists["hresSignedPerp"] = hresSignedPerp;
    histDirName["hresSignedPerp"] = "1d-histograms";
    TH1D *hresSignedPerpPlus = new TH1D(
                "hresSignedPerpPlus", "Resolution on Perp plane, plus; dperp (cm)", 8000,
                -maxResXY, maxResXY);
    hists["hresSignedPerpPlus"] = hresSignedPerpPlus;
    histDirName["hresSignedPerpPlus"] = "1d-histograms";
    TH1D *hresSignedPerpMinus = new TH1D(
                "hresSignedPerpMinus", "Resolution on Perp plane, minus; dperp (cm)",
                8000, -maxResXY, maxResXY);
    hists["hresSignedPerpMinus"] = hresSignedPerpMinus;
    histDirName["hresSignedPerpMinus"] = "1d-histograms";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("x_residual");
    fHistoFile->cd("x_residual");
    // X-residuals versus X,Y,Z, perp
    TH2D *hresXvX = new TH2D("hresXvX", "residual X v X;x (cm);dx (cm)", 500,
                             -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresXvX"] = hresXvX;
    histDirName["hresXvX"] = "x_residual";
    TH2D *hresXvY = new TH2D("hresXvY", "residual X v Y;y (cm);dx (cm)", 500,
                             -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresXvY"] = hresXvY;
    histDirName["hresXvY"] = "x_residual";
    TH2D *hresXvZ = new TH2D("hresXvZ", "residual X v Z;z (cm);dx (cm)", 500, -40,
                             40, 500, -maxResXY, maxResXY);
    hists["hresXvZ"] = hresXvZ;
    histDirName["hresXvZ"] = "x_residual";
    TH2D *hresXvPerp =
            new TH2D("hresXvPerp", "residual X v Perp;perp (cm);dx(cm)", 500, -0,
                     rangeXY, 500, -maxResXY, maxResXY);
    hists["hresXvPerp"] = hresXvPerp;
    histDirName["hresXvPerp"] = "x_residual";
    TH2D *hresXvPhi = new TH2D(
                "hresXvPhi", "residual X v #phi_{track};#phi_{track} (deg);dx(cm)", 500,
                -180, 180, 500, -maxResXY, maxResXY);
    hists["hresXvPhi"] = hresXvPhi;
    histDirName["hresXvPhi"] = "x_residual";
    TH2D *hresXvPhiPos = new TH2D(
                "hresXvPhiPos", "residual X v #phi_{pos};#phi_{pos} (deg);dx(cm)", 500,
                -180, 180, 500, -maxResXY, maxResXY);
    hists["hresXvPhiPos"] = hresXvPhiPos;
    histDirName["hresXvPhiPos"] = "x_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("y_residual");
    fHistoFile->cd("y_residual");
    // Y-residuals versus X, Y, Z, perp
    TH2D *hresYvX = new TH2D("hresYvX", "residual Y v X;x (cm);dy (cm)", 500,
                             -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresYvX"] = hresYvX;
    histDirName["hresYvX"] = "y_residual";
    TH2D *hresYvY = new TH2D("hresYvY", "residual Y v Y;y (cm);dy (cm)", 500,
                             -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresYvY"] = hresYvY;
    histDirName["hresYvY"] = "y_residual";
    TH2D *hresYvZ = new TH2D("hresYvZ", "residual Y v Z;z (cm);dy (cm)", 500, -40,
                             40, 500, -maxResXY, maxResXY);
    hists["hresYvZ"] = hresYvZ;
    histDirName["hresYvZ"] = "y_residual";
    TH2D *hresYvPerp =
            new TH2D("hresYvPerp", "residual Y v Perp;perp (cm);dy (cm)", 500, 0,
                     rangeXY, 500, -maxResXY, maxResXY);
    hists["hresYvPerp"] = hresYvPerp;
    histDirName["hresYvPerp"] = "y_residual";
    TH2D *hresYvPhi = new TH2D(
                "hresYvPhi", "residual Y v #phi_{track};#phi_{track} (deg);dy (cm)", 500,
                -180, 180, 500, -maxResXY, maxResXY);
    hists["hresYvPhi"] = hresYvPhi;
    histDirName["hresYvPhi"] = "y_residual";
    TH2D *hresYvPhiPos = new TH2D(
                "hresYvPhiPos", "residual Y v #phi_{pos};#phi_{pos} (deg);dy (cm)", 500,
                -180, 180, 500, -maxResXY, maxResXY);
    hists["hresYvPhiPos"] = hresYvPhiPos;
    histDirName["hresYvPhiPos"] = "y_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("z_residual");
    fHistoFile->cd("z_residual");
    // Z-residual versus X, Y, Z, perp
    TH2D *hresZvX = new TH2D("hresZvX", "residual Z v X;x (cm);dz (cm)", 500,
                             -rangeXY, rangeXY, 500, -maxResZ, maxResZ);
    hists["hresZvX"] = hresZvX;
    histDirName["hresZvX"] = "z_residual";
    TH2D *hresZvY = new TH2D("hresZvY", "residual Z v Y;y (cm);dz (cm)", 500,
                             -rangeXY, rangeXY, 500, -maxResZ, maxResZ);
    hists["hresZvY"] = hresZvY;
    histDirName["hresZvY"] = "z_residual";
    TH2D *hresZvZ = new TH2D("hresZvZ", "residual Z v Z;z(cm);dz (cm)", 500, -40,
                             40, 500, -maxResZ, maxResZ);
    hists["hresZvZ"] = hresZvZ;
    histDirName["hresZvZ"] = "z_residual";
    TH2D *hresZvPerp =
            new TH2D("hresZvPerp", "residual Z v Perp; perp(cm);dz (cm)", 500, 0,
                     rangeXY, 500, -maxResZ, maxResZ);
    hists["hresZvPerp"] = hresZvPerp;
    histDirName["hresZvPerp"] = "z_residual";
    TH2D *hresZvPhi = new TH2D(
                "hresZvPhi", "residual Z v #phi_{track}; #phi_{track} (deg);dz (cm)", 500,
                -180, 180, 500, -maxResZ, maxResZ);
    hists["hresZvPhi"] = hresZvPhi;
    histDirName["hresZvPhi"] = "z_residual";
    TH2D *hresZvPhiPos = new TH2D(
                "hresZvPhiPos", "residual Z v #phi_{pos}; #phi_{pos} (deg);dz (cm)", 500,
                -180, 180, 500, -maxResZ, maxResZ);
    hists["hresZvPhiPos"] = hresZvPhiPos;
    histDirName["hresZvPhiPos"] = "z_residual";
    TH2D *hresZvPhiCdc = new TH2D(
                "hresZvPhiCdc", "residual Z v #phi_{cdc}; #phi_{cdc} (deg);dz (cm)", 500,
                -180, 180, 500, -maxResZ, maxResZ);
    hists["hresZvPhiCdc"] = hresZvPhiCdc;
    histDirName["hresZvPhiCdc"] = "z_residual";
    TH2D *hresZvPhiCdcMinus = new TH2D(
                "hresZvPhiCdcMinus", "residual Z v #phi_{cdc} minus; #phi_{cdc} (deg);dz (cm)", 500,
                -180, 180, 500, -maxResZ, maxResZ);
    hists["hresZvPhiCdcMinus"] = hresZvPhiCdcMinus;
    histDirName["hresZvPhiCdcMinus"] = "z_residual";
    TH2D *hresZvPhiCdcPlus = new TH2D(
                "hresZvPhiCdcPlus", "residual Z v #phi_{cdc} Plus; #phi_{cdc} (deg);dz (cm)", 500,
                -180, 180, 500, -maxResZ, maxResZ);
    hists["hresZvPhiCdcPlus"] = hresZvPhiCdcPlus;
    histDirName["hresZvPhiCdcPlus"] = "z_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("perp_residual");
    fHistoFile->cd("perp_residual");
    // Perp-residual versus X, Y, Z, perp
    TH2D *hresPerpvX =
            new TH2D("hresPerpvX", "residual Perp v X;x (cm);dPerp (cm)", 500,
                     -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPerpvX"] = hresPerpvX;
    histDirName["hresPerpvX"] = "perp_residual";
    TH2D *hresPerpvY =
            new TH2D("hresPerpvY", "residual Perp v Y;y (cm);dPerp (cm)", 500,
                     -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPerpvY"] = hresPerpvY;
    histDirName["hresPerpvY"] = "perp_residual";
    TH2D *hresPerpvZ =
            new TH2D("hresPerpvZ", "residual Perp v Z;z (cm);dPerp (cm)", 500, -40,
                     40, 500, -maxResXY, maxResXY);
    hists["hresPerpvZ"] = hresPerpvZ;
    histDirName["hresPerpvZ"] = "perp_residual";
    TH2D *hresPerpvPerp =
            new TH2D("hresPerpvPerp", "residual PerpvPerp;perp (cm);dPerp (cm)", 500,
                     0, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPerpvPerp"] = hresPerpvPerp;
    histDirName["hresPerpvPerp"] = "perp_residual";
    TH2D *hresPerpvPhi =
            new TH2D("hresPerpvPhi",
                     "residual Perpv #phi_{track};#phi_{track} (deg);dPerp (cm)", 500,
                     -180, 180, 500, -maxResXY, maxResXY);
    hists["hresPerpvPhi"] = hresPerpvPhi;
    histDirName["hresPerpvPhi"] = "perp_residual";
    TH2D *hresPerpvPhiPos =
            new TH2D("hresPerpvPhiPos",
                     "residual Perpv #phi_{pos};#phi_{pos} (deg);dPerp (cm)", 500,
                     -180, 180, 500, -maxResXY, maxResXY);
    hists["hresPerpvPhiPos"] = hresPerpvPhiPos;
    histDirName["hresPerpvPhiPos"] = "perp_residual";
    TH2D *hresPerpvDist =
            new TH2D("hresPerpvDist", "Residual_{xy} vs extrap length;cm;dperp (cm)",
                     500, -100, 100, 500, 0, 100);
    hists["hresPerpvDist"] = hresPerpvDist;
    histDirName["hresPerpvDist"] = "perp_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("signedPerp_residual");
    fHistoFile->cd("signedPerp_residual");
    
    TH2D *hresSignedPerpvX = new TH2D(
                "hresSignedPerpvX", "residual SignedPerp v X;x (cm);dSignedPerp (cm)",
                500, -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvX"] = hresSignedPerpvX;
    histDirName["hresSignedPerpvX"] = "signedPerp_residual";
    TH2D *hresSignedPerpvY = new TH2D(
                "hresSignedPerpvY", "residual SignedPerp v Y;y (cm);dSignedPerp (cm)",
                500, -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvY"] = hresSignedPerpvY;
    histDirName["hresSignedPerpvY"] = "signedPerp_residual";
    TH2D *hresSignedPerpvZ = new TH2D(
                "hresSignedPerpvZ", "residual SignedPerp v Z;z (cm);dSignedPerp (cm)",
                500, -rangeZ, rangeZ, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvZ"] = hresSignedPerpvZ;
    histDirName["hresSignedPerpvZ"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPerp =
            new TH2D("hresSignedPerpvPerp",
                     "residual SignedPerpvPerp;Perp (cm);dSignedPerp (cm)", 500, 0,
                     rangeXY, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPerp"] = hresSignedPerpvPerp;
    histDirName["hresSignedPerpvPerp"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPerpPlus =
            new TH2D("hresSignedPerpvPerpPlus",
                     "residual SignedPerpvPerp Plus;Perp (cm);dSignedPerp (cm)", 500,
                     0, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPerpPlus"] = hresSignedPerpvPerpPlus;
    histDirName["hresSignedPerpvPerpPlus"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPerpMinus =
            new TH2D("hresSignedPerpvPerpMinus",
                     "residual SignedPerpvPerp Minus;Perp (cm);dSignedPerp (cm)", 500,
                     0, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPerpMinus"] = hresSignedPerpvPerpMinus;
    histDirName["hresSignedPerpvPerpMinus"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPhi = new TH2D(
                "hresSignedPerpvPhi",
                "residual SignedPerpv #phi_{track};#phi_{track} (deg);dSignedPerp (cm)",
                500, -180, 180, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhi"] = hresSignedPerpvPhi;
    histDirName["hresSignedPerpvPhi"] = "signedPerp_residual";
    
    TH2D *hresSignedPerpvPhiPos = new TH2D(
                "hresSignedPerpvPhiPos",
                "residual SignedPerpv #phi_{pos};#phi_{pos} (deg);dSignedPerp (cm)", 1000,
                -180, 180, 1000, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiPos"] = hresSignedPerpvPhiPos;
    histDirName["hresSignedPerpvPhiPos"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPhiPosPlus = new TH2D(
                "hresSignedPerpvPhiPosPlus",
                "residual SignedPerpv #phi_{pos} Plus;#phi_{pos} (deg);dSignedPerp (cm)",
                1000, -180, 180, 1000, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiPosPlus"] = hresSignedPerpvPhiPosPlus;
    histDirName["hresSignedPerpvPhiPosPlus"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPhiPosMinus = new TH2D(
                "hresSignedPerpvPhiPosMinus",
                "residual SignedPerpv #phi_{pos} Minus;#phi_{pos} (deg);dSignedPerp (cm)",
                1000, -180, 180, 1000, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiPosMinus"] = hresSignedPerpvPhiPosMinus;
    histDirName["hresSignedPerpvPhiPosMinus"] = "signedPerp_residual";
    
    TH2D *hresSignedPerpvPhiCdc = new TH2D(
                "hresSignedPerpvPhiCdc",
                "residual SignedPerpv #phi_{cdc};#phi_{cdc} (deg);dSignedPerp (cm)", 360,
                -180, 180, 1000, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiCdc"] = hresSignedPerpvPhiCdc;
    histDirName["hresSignedPerpvPhiCdc"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPhiCdcPlus = new TH2D(
                "hresSignedPerpvPhiCdcPlus",
                "residual SignedPerpv #phi_{cdc} Plus;#phi_{cdc} (deg);dSignedPerp (cm)",
                360, -180, 180, 1000, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiCdcPlus"] = hresSignedPerpvPhiCdcPlus;
    histDirName["hresSignedPerpvPhiCdcPlus"] = "signedPerp_residual";
    TH2D *hresSignedPerpvPhiCdcMinus = new TH2D(
                "hresSignedPerpvPhiCdcMinus",
                "residual SignedPerpv #phi_{cdc} Minus;#phi_{cdc} (deg);dSignedPerp (cm)",
                360, -180, 180, 1000, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiCdcMinus"] = hresSignedPerpvPhiCdcMinus;
    histDirName["hresSignedPerpvPhiCdcMinus"] = "signedPerp_residual";
    
    
    TH2D *hresSignedPerpvDafWeight =
            new TH2D("hresSignedPerpvDafWeight",
                     "residual SignedPerp v daf weight;daf weightdSignedPerp (cm)",
                     500, 0, 1, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvDafWeight"] = hresSignedPerpvDafWeight;
    histDirName["hresSignedPerpvDafWeight"] = "signedPerp_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("3D_residual");
    fHistoFile->cd("3D_residual");
    // Residual versus X, Y, Z, perp
    TH2D *hresMagvX = new TH2D("hresMagvX", "residual v X;x (cm);dMag (cm)", 500,
                               -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresMagvX"] = hresMagvX;
    histDirName["hresMagvX"] = "3D_residual";
    TH2D *hresMagvY = new TH2D("hresMagvY", "residual v Y;y (cm);dMag (cm)", 500,
                               -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresMagvY"] = hresMagvY;
    histDirName["hresMagvY"] = "3D_residual";
    TH2D *hresMagvZ = new TH2D("hresMagvZ", "residual v Z;z (cm);dMag (cm)", 500,
                               -40, 40, 500, -maxResXY, maxResXY);
    hists["hresMagvZ"] = hresMagvZ;
    histDirName["hresMagvZ"] = "3D_residual";
    TH2D *hresMagvPerp =
            new TH2D("hresMagvPerp", "residual vPerp;perp (cm);dMag (cm)", 500, 0,
                     rangeXY, 500, -maxResXY, maxResXY);
    hists["hresMagvPerp"] = hresMagvPerp;
    histDirName["hresMagvPerp"] = "3D_residual";
    TH2D *hresMagvPhi = new TH2D(
                "hresMagvPhi", "residual v #phi_{track};#phi_{track} (cm);dMag (cm)", 500,
                -180, 180, 500, -maxResXY, maxResXY);
    hists["hresMagvPhi"] = hresMagvPhi;
    histDirName["hresMagvPhi"] = "3D_residual";
    TH2D *hresMagvPhiPos = new TH2D(
                "hresMagvPhiPos", "residual v #phi_{pos};#phi_{pos} (cm);dMag (cm)", 500,
                -180, 180, 500, -maxResXY, maxResXY);
    hists["hresMagvPhiPos"] = hresMagvPhiPos;
    histDirName["hresMagvPhiPos"] = "3D_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("phi_residual");
    fHistoFile->cd("phi_residual");
    // Phi-residual versus X, Y, Z, perp
    TH2D *hresPhivX = new TH2D("hresPhivX", "#phi residual v X;x (cm);dPhi (deg)",
                               500, -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPhivX"] = hresPhivX;
    histDirName["hresPhivX"] = "phi_residual";
    TH2D *hresPhivY = new TH2D("hresPhivY", "#phi residual v Y;y (cm);dPhi (deg)",
                               500, -rangeXY, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPhivY"] = hresPhivY;
    histDirName["hresPhivY"] = "phi_residual";
    TH2D *hresPhivZ = new TH2D("hresPhivZ", "#phi residual v Z;z (cm);dPhi (deg)",
                               500, -40, 40, 500, -maxResXY, maxResXY);
    hists["hresPhivZ"] = hresPhivZ;
    histDirName["hresPhivZ"] = "phi_residual";
    TH2D *hresPhivPerp =
            new TH2D("hresPhivPerp", "#phi residual vPerp;perp (cm);dPhi (deg)", 500,
                     0, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPhivPerp"] = hresPhivPerp;
    histDirName["hresPhivPerp"] = "phi_residual";
    
    TH2D *hresPhivPerpPlus = new TH2D(
                "hresPhivPerpPlus", "#phi residual vPerpPlus;perp (cm);dPhi (deg)", 500,
                0, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPhivPerpPlus"] = hresPhivPerpPlus;
    histDirName["hresPhivPerpPlus"] = "phi_residual";
    TH2D *hresPhivPerpMinus = new TH2D(
                "hresPhivPerpMinus", "#phi residual vPerpMinus;perp (cm);dPhi (deg)", 500,
                0, rangeXY, 500, -maxResXY, maxResXY);
    hists["hresPhivPerpMinus"] = hresPhivPerpMinus;
    histDirName["hresPhivPerpMinus"] = "phi_residual";
    
    TH2D *hresPhivPhi =
            new TH2D("hresPhivPhi",
                     "#phi residual v #phi_{track};#phi_{track} (cm);dPhi (deg)", 500,
                     -180, 180, 500, -maxResXY, maxResXY);
    hists["hresPhivPhi"] = hresPhivPhi;
    histDirName["hresPhivPhi"] = "phi_residual";
    TH2D *hresPhivPhiPos = new TH2D(
                "hresPhivPhiPos", "#phi residual v #phi_{pos};#phi_{pos} (cm);dPhi (deg)",
                500, -180, 180, 500, -maxResXY, maxResXY);
    hists["hresPhivPhiPos"] = hresPhivPhiPos;
    histDirName["hresPhivPhiPos"] = "phi_residual";
    TH2D *hresPhivPhiPosPlus =
            new TH2D("hresPhivPhiPosPlus",
                     "#phi residual v #phi_{pos} Plus;#phi_{pos} (cm);dPhi (deg)",
                     500, -180, 180, 500, -maxResXY, maxResXY);
    hists["hresPhivPhiPosPlus"] = hresPhivPhiPosPlus;
    histDirName["hresPhivPhiPosPlus"] = "phi_residual";
    TH2D *hresPhivPhiPosMinus =
            new TH2D("hresPhivPhiPosMinus",
                     "#phi residual v #phi_{pos} Minus;#phi_{pos} (cm);dPhi (deg)",
                     500, -180, 180, 500, -maxResXY, maxResXY);
    hists["hresPhivPhiPosMinus"] = hresPhivPhiPosMinus;
    histDirName["hresPhivPhiPosMinus"] = "phi_residual";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("occupancy");
    fHistoFile->cd("occupancy");
    TH1D *hitsX = new TH1D("hitsX", "hitsX;x (cm)", 800, -20, 20);
    hists["hitsX"] = hitsX;
    histDirName["hitsX"] = "occupancy";
    TH1D *hitsY = new TH1D("hitsY", "hitsY;y (cm)", 800, -20, 20);
    hists["hitsY"] = hitsY;
    histDirName["hitsY"] = "occupancy";
    TH1D *hitsZ = new TH1D("hitsZ", "hitsZ;z (cm)", 1600, -200, 200);
    hists["hitsZ"] = hitsZ;
    histDirName["hitsZ"] = "occupancy";
    
    TH1D *hitsW = new TH1D("hitsW", "hitsW;W (cm)", 1600, -200, 200);
    hists["hitsW"] = hitsW;
    histDirName["hitsW"] = "occupancy";
    TH1D *hitsRl =
            new TH1D("hitsRl", "hitsR_{local};R_{local} (cm)", 1600, 0, 15);
    hists["hitsRl"] = hitsRl;
    histDirName["hitsRl"] = "occupancy";
    TH2D *hitsPerpl = new TH2D("hitsPerpl", "hitsPerp_{local};u (cm);v (cm)", 800,
                               -rangeXY, rangeXY, 800, -rangeXY, rangeXY);
    hists["hitsPerpl"] = hitsPerpl;
    histDirName["hitsPerpl"] = "occupancy";
    
    TH1D *hitsPhi = new TH1D("hitsPhi", "hits #phi; #phi (deg)", 1600, -180, 180);
    hists["hitsPhi"] = hitsPhi;
    histDirName["hitsPhi"] = "occupancy";
    TH2D *hitsPerp = new TH2D("hitsPerp", "hitsPerp;x (cm);y (cm)", 800, -rangeXY,
                              rangeXY, 800, -rangeXY, rangeXY);
    hists["hitsPerp"] = hitsPerp;
    histDirName["hitsPerp"] = "occupancy";
    TH2D *hitsZR = new TH2D("hitsZR", "hitsZR;z (cm);r (cm)", 1600, -200, 200,
                            400, 0, rangeXY);
    hists["hitsZR"] = hitsZR;
    histDirName["hitsZR"] = "occupancy";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("covariance");
    fHistoFile->cd("covariance");
    TH1D *hresCovX =
            new TH1D("hresCovX", "hit covariance x projection", 1000, 0, 1.05);
    hists["hresCovX"] = hresCovX;
    histDirName["hresCovX"] = "covariance";
    TH1D *hresCovY =
            new TH1D("hresCovY", "hit covariance Y projection", 1000, 0, 1.05);
    hists["hresCovY"] = hresCovY;
    histDirName["hresCovY"] = "covariance";
    TH1D *hresCovZ =
            new TH1D("hresCovZ", "hit covariance Z projection", 1000, 0, 1.05);
    hists["hresCovZ"] = hresCovZ;
    histDirName["hresCovZ"] = "covariance";
    TH1D *hresCov =
            new TH1D("hresCov", "Covariance residual projection", 1000, 0, 1.05);
    hists["hresCov"] = hresCov;
    histDirName["hresCov"] = "covariance";
    TH1D *hresCovXY =
            new TH1D("hresCovXY", "Covariance residualXY projection", 1000, 0, 1.05);
    hists["hresCovXY"] = hresCovXY;
    histDirName["hresCovXY"] = "covariance";
    TH1D *hresMagdCov = new TH1D(
                "hresMagdCov", "res.Mag weighted with hit covariance", 5000, 0, 50000);
    hists["hresMagdCov"] = hresMagdCov;
    histDirName["hresMagdCov"] = "covariance";
    TH1D *hresSignedPerpdCov = new TH1D(
                "hresSignedPerpdCov", "signed res.Perp weighted with hit covariance",
                5000, -50000, 50000);
    hists["hresSignedPerpdCov"] = hresSignedPerpdCov;
    histDirName["hresSignedPerpdCov"] = "covariance";
    
    TH2D *hresMagVCov = new TH2D("hresMagVCov", "residual v covariance", 500, 0,
                                 1.05, 500, 0, 10);
    hists["hresMagVCov"] = hresMagVCov;
    histDirName["hresMagVCov"] = "covariance";
    TH2D *hresXVCov = new TH2D("hresXVCov", "residual X v covarianceX", 500, 0,
                               1.05, 500, -3, 3);
    hists["hresXVCov"] = hresXVCov;
    histDirName["hresXVCov"] = "covariance";
    TH2D *hresYVCov = new TH2D("hresYVCov", "residual y v covarianceY", 500, 0,
                               1.05, 500, -3, 3);
    hists["hresYVCov"] = hresYVCov;
    histDirName["hresYVCov"] = "covariance";
    TH2D *hresZVCov = new TH2D("hresZVCov", "residual z v covarianceZ", 500, 0,
                               1.05, 500, -10, 10);
    hists["hresZVCov"] = hresZVCov;
    histDirName["hresZVCov"] = "covariance";
    TH2D *hresSignedPerpVCov =
            new TH2D("hresSignedPerpVCov", "signed residual xy v covarianceXY", 500,
                     0, 1.05, 500, -10, 10);
    hists["hresSignedPerpVCov"] = hresSignedPerpVCov;
    histDirName["hresSignedPerpVCov"] = "covariance";
    
    TH2D *hresCovVPerp =
            new TH2D("hresCovVPerp", "hit cov vs hit_xy", 500, 0, 15, 500, 0, 0.4);
    hists["hresCovVPerp"] = hresCovVPerp;
    histDirName["hresCovVPerp"] = "covariance";
    TH2D *hresCovPerpVPerp = new TH2D("hresCovPerpVPerp", "hit cov_xy vs hit_xy",
                                      500, 0, 15, 500, 0, 0.4);
    hists["hresCovPerpVPerp"] = hresCovPerpVPerp;
    histDirName["hresCovPerpVPerp"] = "covariance";
    
    fHistoFile->cd("");
    fHistoFile->mkdir("covarianceTrack");
    fHistoFile->cd("covarianceTrack");
    TH1D *hresCovTrackX =
            new TH1D("hresCovTrackX", "track covariance x projection", 1000, 0, 1.);
    hists["hresCovTrackX"] = hresCovTrackX;
    histDirName["hresCovTrackX"] = "covarianceTrack";
    TH1D *hresCovTrackY =
            new TH1D("hresCovTrackY", "track covariance Y projection", 1000, 0, 1.);
    hists["hresCovTrackY"] = hresCovTrackY;
    histDirName["hresCovTrackY"] = "covarianceTrack";
    TH1D *hresCovTrackZ =
            new TH1D("hresCovTrackZ", "track covariance Z projection", 1000, 0, 20.);
    hists["hresCovTrackZ"] = hresCovTrackZ;
    histDirName["hresCovTrackZ"] = "covarianceTrack";
    TH1D *hresCovTrack = new TH1D(
                "hresCovTrack", "Track covariance residual projection", 1000, 0, 1.);
    hists["hresCovTrack"] = hresCovTrack;
    histDirName["hresCovTrack"] = "covarianceTrack";
    TH1D *hresCovTrackXY = new TH1D(
                "hresCovTrackXY", "Track Covariance residualXY projection", 1000, 0, .05);
    hists["hresCovTrackXY"] = hresCovTrackXY;
    histDirName["hresCovTrackXY"] = "covarianceTrack";
    TH1D *hresMagdCovTrack =
            new TH1D("hresMagdCovTrack", "res.Mag weighted with track covariance",
                     5000, 0, 50000);
    hists["hresMagdCovTrack"] = hresMagdCovTrack;
    histDirName["hresMagdCovTrack"] = "covarianceTrack";
    TH1D *hresSignedPerpdCovTrack = new TH1D(
                "hresSignedPerpdCovTrack",
                "signed res.Perp weighted with track covariance", 5000, -5000, 5000);
    hists["hresSignedPerpdCovTrack"] = hresSignedPerpdCovTrack;
    histDirName["hresSignedPerpdCovTrack"] = "covarianceTrack";
    
    TH2D *hresMagVCovTrack =
            new TH2D("hresMagVCovTrack", "residual v track covariance", 500, 0, 20.0,
                     500, 0, 10);
    hists["hresMagVCovTrack"] = hresMagVCovTrack;
    histDirName["hresMagVCovTrack"] = "covarianceTrack";
    TH2D *hresXVCovTrack =
            new TH2D("hresXVCovTrack", "residual X v track covarianceX", 500, 0, 0.2,
                     500, -3, 3);
    hists["hresXVCovTrack"] = hresXVCovTrack;
    histDirName["hresXVCovTrack"] = "covarianceTrack";
    TH2D *hresYVCovTrack =
            new TH2D("hresYVCovTrack", "residual y v track covarianceY", 500, 0, 0.2,
                     500, -3, 3);
    hists["hresYVCovTrack"] = hresYVCovTrack;
    histDirName["hresYVCovTrack"] = "covarianceTrack";
    TH2D *hresZVCovTrack =
            new TH2D("hresZVCovTrack", "residual z v track covarianceZ", 500, 0, 30.,
                     500, -30, 30);
    hists["hresZVCovTrack"] = hresZVCovTrack;
    histDirName["hresZVCovTrack"] = "covarianceTrack";
    TH2D *hresSignedPerpVCovTrack = new TH2D(
                "hresSignedPerpVCovTrack", "signed residual xy v track covarianceXY", 500,
                0, 0.2, 500, -3, 3);
    hists["hresSignedPerpVCovTrack"] = hresSignedPerpVCovTrack;
    histDirName["hresSignedPerpVCovTrack"] = "covarianceTrack";
    
    TH2D *hresCovTrackVPerp = new TH2D("hresCovTrackVPerp", "track cov vs hit_xy",
                                       500, 0, 15, 500, 0, 0);
    hists["hresCovTrackVPerp"] = hresCovTrackVPerp;
    histDirName["hresCovTrackVPerp"] = "covarianceTrack";
    TH2D *hresCovTrackPerpVPerp =
            new TH2D("hresCovTrackPerpVPerp", "track cov_xy vs hit_xy", 500, 0, 15,
                     500, 0, 0.05);
    hists["hresCovTrackPerpVPerp"] = hresCovTrackPerpVPerp;
    histDirName["hresCovTrackPerpVPerp"] = "covarianceTrack";
    // zSliced hists
    double zRange = zEnd - zStart;
    double zBin = zRange / ((double)zSlices);
    double zBinStart = zStart;
    double zBinEnd = zBinStart;
    fHistoFile->cd("");
    fHistoFile->mkdir("hresPhivPerp_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresPhivPhi_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresPhivPhiPos_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresPhivPhiPosPlus_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresPhivPhiPosMinus_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresSignedPerpvPhiPos_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresSignedPerpvPhiPosPlus_zslice");
    fHistoFile->cd("");
    fHistoFile->mkdir("hresSignedPerpvPhiPosMinus_zslice");
    for (int i = 0; i < zSlices; ++i) {
        zBinEnd += zBin;
        TString histPostFix = TString::Format("_zSlice_%i", i);
        TString histPrettyPostFix =
                TString::Format(" zSlice %f - %f", zBinStart, zBinEnd);
        
        TH2D *hresPhivPerpTemp = new TH2D(
                    "hresPhivPerp" + histPostFix,
                    "#phi residual vPerp" + histPrettyPostFix + ";perp (cm);dPhi (deg)",
                    500, 0, rangeXY, 500, -maxResXY, maxResXY);
        hists["hresPhivPerp" + histPostFix] = hresPhivPerpTemp;
        histDirName["hresPhivPerp" + histPostFix] = "hresPhivPerp_zslice";
        
        TH2D *hresPhivPhiTemp =
                new TH2D("hresPhivPhi" + histPostFix,
                         "#phi residual v #phi_{track}" + histPrettyPostFix +
                         ";#phi_{track} (cm);dPhi (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresPhivPhiTemp" + histPostFix] = hresPhivPhiTemp;
        histDirName["hresPhivPhiTemp" + histPostFix] = "hresPhivPhi_zslice";
        
        TH2D *hresPhivPhiPosTemp =
                new TH2D("hresPhivPhiPos" + histPostFix,
                         "#phi residual v #phi_{pos}" + histPrettyPostFix +
                         ";#phi_{pos} (cm);dPhi (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresPhivPhiPos" + histPostFix] = hresPhivPhiPosTemp;
        histDirName["hresPhivPhiPos" + histPostFix] = "hresPhivPhiPos_zslice";
        
        TH2D *hresPhivPhiPosPlusTemp =
                new TH2D("hresPhivPhiPosPlus" + histPostFix,
                         "plus #phi residual v #phi_{pos}" + histPrettyPostFix +
                         ";#phi_{pos} (cm);dPhi (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresPhivPhiPosPlus" + histPostFix] = hresPhivPhiPosPlusTemp;
        histDirName["hresPhivPhiPosPlus" + histPostFix] =
                "hresPhivPhiPosPlus_zslice";
        
        TH2D *hresPhivPhiPosMinusTemp =
                new TH2D("hresPhivPhiPosMinus" + histPostFix,
                         "Minus #phi residual v #phi_{pos}" + histPrettyPostFix +
                         ";#phi_{pos} (cm);dPhi (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresPhivPhiPosMinus" + histPostFix] = hresPhivPhiPosMinusTemp;
        histDirName["hresPhivPhiPosMinus" + histPostFix] =
                "hresPhivPhiPosMinus_zslice";
        
        // PERP!
        TH2D *hresSignedPerpvPhiPosTemp =
                new TH2D("hresSignedPerpvPhiPos" + histPostFix,
                         "SignedPerp residual v #phi_{pos}" + histPrettyPostFix +
                         ";#phi_{pos} (cm);dSignedPerp (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresSignedPerpvPhiPos" + histPostFix] = hresSignedPerpvPhiPosTemp;
        histDirName["hresSignedPerpvPhiPos" + histPostFix] =
                "hresSignedPerpvPhiPos_zslice";
        
        TH2D *hresSignedPerpvPhiPosPlusTemp =
                new TH2D("hresSignedPerpvPhiPosPlus" + histPostFix,
                         "plus SignedPerp residual v #phi_{pos}" + histPrettyPostFix +
                         ";#phi_{pos} (cm);dSignedPerp (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresSignedPerpvPhiPosPlus" + histPostFix] =
                hresSignedPerpvPhiPosPlusTemp;
        histDirName["hresSignedPerpvPhiPosPlus" + histPostFix] =
                "hresSignedPerpvPhiPosPlus_zslice";
        
        TH2D *hresSignedPerpvPhiPosMinusTemp =
                new TH2D("hresSignedPerpvPhiPosMinus" + histPostFix,
                         "Minus SignedPerp residual v #phi_{pos}" + histPrettyPostFix +
                         ";#phi_{pos} (cm);dSignedPerp (deg)",
                         500, -180, 180, 500, -maxResXY, maxResXY);
        hists["hresSignedPerpvPhiPosMinus" + histPostFix] =
                hresSignedPerpvPhiPosMinusTemp;
        histDirName["hresSignedPerpvPhiPosMinus" + histPostFix] =
                "hresSignedPerpvPhiPosMinus_zslice";
        
        zBinStart = zBinEnd;
    }
    
    double phiRange = 22.5;
    int phiSlices = 20;
    double phiBinStart = -11.25;
    double phiBin = phiRange / phiSlices;
    double phiBinEnd = phiBinStart;
    fHistoFile->cd("");
    fHistoFile->mkdir("hresSignedPerpVSignedPerp_phiSlice");
    fHistoFile->cd("hresSignedPerpVSignedPerp_phiSlice");
    for (int i = 0; i < phiSlices; ++i) {
        phiBinEnd += phiBin;
        TString histPostFix = TString::Format("_phiSlice_%i", i);
        TString histPrettyPostFix =
                TString::Format("#phi-slice %f - %f", phiBinStart, phiBinEnd);
        TH2D *hresSignedPerpvPerpTmp =
                new TH2D("hresSignedPerpvSignedPerp" + histPostFix,
                         "residual SignedPerpvPerp" + histPrettyPostFix +
                         ";Perp (cm);dSignedPerp (cm)",
                         500, 0, rangeXY, 500, -maxResXY, maxResXY);
        TH2D *hresSignedPerpvPerpPlusTmp =
                new TH2D("hresSignedPerpvSignedPerpPlus" + histPostFix,
                         "residual SignedPerpvPerp" + histPrettyPostFix +
                         ", plus;Perp (cm);dSignedPerp (cm)",
                         500, 0, rangeXY, 500, -maxResXY, maxResXY);
        TH2D *hresSignedPerpvPerpMinusTmp =
                new TH2D("hresSignedPerpvSignedPerpMinus" + histPostFix,
                         "residual SignedPerpvPerp" + histPrettyPostFix +
                         ", minus;Perp (cm);dSignedPerp (cm)",
                         500, 0, rangeXY, 500, -maxResXY, maxResXY);
        hists["hresSignedPerpvPerp" + histPostFix] = hresSignedPerpvPerpTmp;
        hists["hresSignedPerpvPerpPlus" + histPostFix] = hresSignedPerpvPerpPlusTmp;
        hists["hresSignedPerpvPerpMinus" + histPostFix] =
                hresSignedPerpvPerpMinusTmp;
        histDirName["hresSignedPerpvPerp" + histPostFix] =
                "hresSignedPerpVSignedPerp_phiSlice";
        histDirName["hresSignedPerpvPerpPlus" + histPostFix] =
                "hresSignedPerpVSignedPerp_phiSlice";
        histDirName["hresSignedPerpvPerpMinus" + histPostFix] =
                "hresSignedPerpVSignedPerp_phiSlice";
        phiBinStart = phiBinEnd;
    }
    
    // Hists modulo 22.5
    fHistoFile->cd();
    fHistoFile->mkdir("modulo");
    
    TH2D *hresPhivPhiPosModulo = new TH2D(
                "hresPhivPhiPos", "#phi residual v $phi modulo 22.5;#phi(cm);d#Phi (deg)",
                500, -11.25, 11.25, 500, -maxResXY, maxResXY);
    hists["hresPhivPhiPosModulo"] = hresPhivPhiPosModulo;
    histDirName["hresPhivPhiPosModulo"] = "modulo";
    TH2D *hresPhivPhiPosPlusModulo =
            new TH2D("hresPhivPhiPosPlus",
                     "#phi residual v $phi modulo 22.5 plus;#phi(cm);d#Phi (deg)",
                     500, -11.25, 11.25, 500, -maxResXY, maxResXY);
    hists["hresPhivPhiPosPlusModulo"] = hresPhivPhiPosPlusModulo;
    histDirName["hresPhivPhiPosPlusModulo"] = "modulo";
    TH2D *hresPhivPhiPosMinusModulo =
            new TH2D("hresPhivPhiPosMinus",
                     "#phi residual v $phi modulo 22.5 minus;#phi(cm);d#Phi (deg)",
                     500, -11.25, 11.25, 500, -maxResXY, maxResXY);
    hists["hresPhivPhiPosMinusModulo"] = hresPhivPhiPosMinusModulo;
    histDirName["hresPhivPhiPosMinusModulo"] = "modulo";
    
    TH2D *hresSignedPerpvPhiPosModulo =
            new TH2D("hresSignedPerpvPhiPos",
                     "signed perp residual v $phi modulo 22.5;#phi(cm);d#Phi (deg)",
                     500, -11.25, 11.25, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiPosModulo"] = hresSignedPerpvPhiPosModulo;
    histDirName["hresSignedPerpvPhiPosModulo"] = "modulo";
    TH2D *hresSignedPerpvPhiPosPlusModulo = new TH2D(
                "hresSignedPerpvPhiPosPlus",
                "signed perp residual v $phi modulo 22.5 plus;#phi(cm);d#Phi (deg)", 500,
                -11.25, 11.25, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiPosPlusModulo"] = hresSignedPerpvPhiPosPlusModulo;
    histDirName["hresSignedPerpvPhiPosPlusModulo"] = "modulo";
    TH2D *hresSignedPerpvPhiPosMinusModulo =
            new TH2D("hresSignedPerpvPhiPosMinus",
                     "signed residual v $phi modulo 22.5 minus;#phi(cm);d#Phi (deg)",
                     500, -11.25, 11.25, 500, -maxResXY, maxResXY);
    hists["hresSignedPerpvPhiPosMinusModulo"] = hresSignedPerpvPhiPosMinusModulo;
    histDirName["hresSignedPerpvPhiPosMinusModulo"] = "modulo";
    
    fHistoFile->cd();
    fHistoFile->mkdir("TrackDirs");
    
    TH1D *hresdTrackPhi =
            new TH1D("hresdTrackPhi", "d#phi_{track};#dphi", 1000, -5, 5);
    hists["hresdTrackPhi"] = hresdTrackPhi;
    histDirName["hresdTrackPhi"] = "TrackDirs";
    
    TH2D *hresdTrackPhiVPhiPos =
            new TH2D("hresdTrackPhiVPhisPos",
                     "d#phi_{track} vs #phi_{pos} (deg);#phi_{pos} (deg);#dphi", 1000,
                     -180, 180, 1000, -5, 5);
    hists["hresdTrackPhiVPhiPos"] = hresdTrackPhiVPhiPos;
    histDirName["hresdTrackPhiVPhiPos"] = "TrackDirs";
    
    TH2D *hresdTrackPhiVPerp = new TH2D(
                "hresdTrackPhiVPerp", "d#phi_{track} vs perp;perp (cm);#dphi (deg)", 1000,
                0, 15, 1000, -5, 5);
    hists["hresdTrackPhiVPerp"] = hresdTrackPhiVPerp;
    histDirName["hresdTrackPhiVPerp"] = "TrackDirs";
    
    // plus
    TH1D *hresdTrackPhiPlus =
            new TH1D("hresdTrackPhiPlus", "d#phi_{track} Plus;#dphi", 1000, -5, 5);
    hists["hresdTrackPhiPlus"] = hresdTrackPhiPlus;
    histDirName["hresdTrackPhiPlus"] = "TrackDirs";
    
    TH2D *hresdTrackPhiVPhiPosPlus =
            new TH2D("hresdTrackPhiVPhisPosPlus",
                     "d#phi_{track} vs #phi_{pos} (deg) Plus;#phi_{pos} (deg);#dphi",
                     1000, -180, 180, 1000, -5, 5);
    hists["hresdTrackPhiVPhiPosPlus"] = hresdTrackPhiVPhiPosPlus;
    histDirName["hresdTrackPhiVPhiPosPlus"] = "TrackDirs";
    
    TH2D *hresdTrackPhiVPerpPlus =
            new TH2D("hresdTrackPhiVPerpPlus",
                     "d#phi_{track} vs perp Plus;perp (cm);#dphi (deg)", 1000, 0, 15,
                     1000, -5, 5);
    hists["hresdTrackPhiVPerpPlus"] = hresdTrackPhiVPerpPlus;
    histDirName["hresdTrackPhiVPerpPlus"] = "TrackDirs";
    // minus
    TH1D *hresdTrackPhiMinus =
            new TH1D("hresdTrackPhiMinus", "d#phi_{track} Minus;#dphi", 1000, -5, 5);
    hists["hresdTrackPhiMinus"] = hresdTrackPhiMinus;
    histDirName["hresdTrackPhiMinus"] = "TrackDirs";
    
    TH2D *hresdTrackPhiVPhiPosMinus =
            new TH2D("hresdTrackPhiVPhisPosMinus",
                     "d#phi_{track} vs #phi_{pos} (deg) Minus;#phi_{pos} (deg);#dphi",
                     1000, -180, 180, 1000, -5, 5);
    hists["hresdTrackPhiVPhiPosMinus"] = hresdTrackPhiVPhiPosMinus;
    histDirName["hresdTrackPhiVPhiPosMinus"] = "TrackDirs";
    
    TH2D *hresdTrackPhiVPerpMinus =
            new TH2D("hresdTrackPhiVPerpMinus",
                     "d#phi_{track} vs perp Minus;perp (cm);#dphi (deg)", 1000, 0, 15,
                     1000, -5, 5);
    hists["hresdTrackPhiVPerpMinus"] = hresdTrackPhiVPerpMinus;
    histDirName["hresdTrackPhiVPerpMinus"] = "TrackDirs";
    //##########################################################
    //##########################################################
    //##########################################################
    // # Additional separate histograms for chi2 method:
    //##########################################################
    // 1D
    fchi2HistFile = new TFile("test123.root", "RECREATE");
    fchi2HistFile->cd();
    TH1D *hshiftX = new TH1D("hshiftX", "X ;shift_{x} (cm)", 5000, -3, 3);
    TH1D *hshiftY = new TH1D("hshiftY", "Y ;shift_{y} (cm)", 5000, -3, 3);
    TH1D *hshiftZ = new TH1D("hshiftZ", "Z ;shift_{z} (cm)", 5000, -3, 3);
    TH1D *hhifts = new TH1D("hhifts", ";shift (cm)", 5000, 0, 10);
    TH1D *hshift2D = new TH1D("hshift2D", "2d ;shift_{2D} (cm)", 5000, 0, 10);
    chi2Hists["hshiftX"] = hshiftX;
    chi2Hists["hshiftY"] = hshiftY;
    chi2Hists["hshiftZ"] = hshiftZ;
    chi2Hists["hhifts"] = hhifts;
    chi2Hists["hshift2D"] = hshift2D;
    
    // 2d hists:
    TH2D *hshiftYvX = new TH2D("hshiftYvX", "YvX ;shift_{x} (cm);shift_{y} (cm)",
                               1000, -3, 3, 1000, -3, 3);
    TH2D *hshiftYvZ = new TH2D("hshiftYvZ", "YvZ ;shift_{z} (cm);shift_{y} (cm)",
                               1000, -3, 3, 1000, -3, 3);
    TH2D *hshiftXvZ = new TH2D("hshiftXvZ", "XvZ ;shift_{z} (cm);shift_{x} (cm)",
                               1000, -3, 3, 1000, -3, 3);
    chi2Hists["hshiftYvX"] = hshiftYvX;
    chi2Hists["hshiftXvZ"] = hshiftXvZ;
    chi2Hists["hshiftYvZ"] = hshiftYvZ;
    // 3d hists:
    TH3D *hshiftXvYvZ = new TH3D(
                "hshiftXvYvZ", "XvYvZ ;shift_{x} (cm);shift_{y} (cm);shift_{z} (cm)", 100,
                -3, 3, 100, -3, 3, 100, -3, 3);
    chi2Hists["hshiftXvYvZ"] = hshiftXvYvZ;
    chi2Hists["hshiftYvX"] = hshiftYvX;
    chi2Hists["hshiftXvZ"] = hshiftXvZ;
    chi2Hists["hshiftYvZ"] = hshiftYvZ;
    // 2D hists, vs ref pos z
    TH2D *hshiftXVsRefZ = new TH2D("hshiftXVsRefZ", "X ;shift_{x} (cm)", 1000, -3,
                                   3, 1000, -20, 30);
    TH2D *hshiftYVsRefZ = new TH2D("hshiftYVsRefZ", "Y ;shift_{y} (cm)", 1000, -3,
                                   3, 1000, -20, 30);
    TH2D *hshiftZVsRefZ = new TH2D("hshiftZVsRefZ", "Z ;shift_{z} (cm)", 1000, -3,
                                   3, 1000, -20, 30);
    TH2D *hhiftsVsRefZ =
            new TH2D("hhiftsVsRefZ", ";shift (cm)", 1000, 0, 10, 1000, -20, 30);
    TH2D *hshift2DVsRefZ = new TH2D("hshift2DVsRefZ", "2d ;shift_{2D} (cm)", 1000,
                                    0, 10, 1000, -20, 30);
    chi2Hists["hshiftXVsRefZ"] = hshiftXVsRefZ;
    chi2Hists["hshiftYVsRefZ"] = hshiftYVsRefZ;
    chi2Hists["hshiftZVsRefZ"] = hshiftZVsRefZ;
    chi2Hists["hhiftsVsRefZ"] = hhiftsVsRefZ;
    chi2Hists["hshift2DVsRefZ"] = hshift2DVsRefZ;
    
    // 2D hists, vs ref pos x
    TH2D *hshiftXVsRefX = new TH2D("hshiftXVsRefX", "X ;shift_{x} (cm)", 1000, -3,
                                   3, 1000, -15, 15);
    TH2D *hshiftYVsRefX = new TH2D("hshiftYVsRefX", "Y ;shift_{y} (cm)", 1000, -3,
                                   3, 1000, -15, 15);
    TH2D *hshiftZVsRefX = new TH2D("hshiftZVsRefX", "Z ;shift_{z} (cm)", 1000, -3,
                                   3, 1000, -15, 15);
    TH2D *hhiftsVsRefX =
            new TH2D("hhiftsVsRefX", ";shift (cm)", 1000, 0, 10, 1000, -15, 15);
    TH2D *hshift2DVsRefX = new TH2D("hshift2DVsRefX", "2d ;shift_{2D} (cm)", 1000,
                                    0, 10, 1000, -15, 15);
    chi2Hists["hshiftXVsRefX"] = hshiftXVsRefX;
    chi2Hists["hshiftYVsRefX"] = hshiftYVsRefX;
    chi2Hists["hshiftZVsRefX"] = hshiftZVsRefX;
    chi2Hists["hhiftsVsRefX"] = hhiftsVsRefX;
    chi2Hists["hshift2DVsRefX"] = hshift2DVsRefX;
    
    // 2D hists, vs ref pos y
    TH2D *hshiftXVsRefY = new TH2D("hshiftXVsRefY", "X ;shift_{x} (cm)", 1000, -3,
                                   3, 1000, -15, 15);
    TH2D *hshiftYVsRefY = new TH2D("hshiftYVsRefY", "Y ;shift_{y} (cm)", 1000, -3,
                                   3, 1000, -15, 15);
    TH2D *hshiftZVsRefY = new TH2D("hshiftZVsRefY", "Z ;shift_{z} (cm)", 1000, -3,
                                   3, 1000, -15, 15);
    TH2D *hhiftsVsRefY =
            new TH2D("hhiftsVsRefY", ";shift (cm)", 1000, 0, 10, 1000, -15, 15);
    TH2D *hshift2DVsRefY = new TH2D("hshift2DVsRefY", "2d ;shift_{2D} (cm)", 1000,
                                    0, 10, 1000, -15, 15);
    chi2Hists["hshiftXVsRefY"] = hshiftXVsRefY;
    chi2Hists["hshiftYVsRefY"] = hshiftYVsRefY;
    chi2Hists["hshiftZVsRefY"] = hshiftZVsRefY;
    chi2Hists["hhiftsVsRefY"] = hhiftsVsRefY;
    chi2Hists["hshift2DVsRefY"] = hshift2DVsRefY;
    
    // Parameter hists
    TH1D *hpardX = new TH1D("hpardX", "dX ;dx (cm)", 5000, -3, 3);
    TH1D *hpardY = new TH1D("hpardY", "dY ;dy (cm)", 5000, -3, 3);
    TH1D *hpardZ = new TH1D("hpardZ", "dZ ;dz (cm)", 5000, -3, 3);
    TH1D *hpardPhi = new TH1D("hpardPhi", ";dPhi (deg)", 5000, -3, 3);
    TH1D *hpardTheta =
            new TH1D("hpardTheta", "dTheta ;dTheta (deg)", 5000, -3, 3);
    TH1D *hpardPsi = new TH1D("hpardPsi", "2d ;dPsi (deg)", 5000, -3, 3);
    chi2Hists["hpardX"] = hpardX;
    chi2Hists["hpardY"] = hpardY;
    chi2Hists["hpardZ"] = hpardZ;
    chi2Hists["hpardPhi"] = hpardPhi;
    chi2Hists["hpardTheta"] = hpardTheta;
    chi2Hists["hpardPsi"] = hpardPsi;
}

void TpcAlignmentSimple::fillHistos(const TpcResidual *theRes, const TVector3 pos20, const TVector3 mom20) {
    TVector3 dir = theRes->getTrackDir();
    TVector3 ref = theRes->getRefPos();
    
    TVector3 pos = theRes->getHitPos();
    
    TVector3 res = theRes->getResXYZ();
    
    TVector3 dirLocal = theRes->getLocalTrackDir();
    TVector3 uvw = alMan->masterToLocal("tpc", theRes->getHitPos());
    //TVector3 uvw = alMan->masterToLocal("tpc", theRes->getHitPos());
    
    double refPhi=ref.Phi()* TMath::RadToDeg();
    double phiPos = uvw.Phi() * TMath::RadToDeg();
    double phiPosInSector = getPhiInSector(phiPos);
    double phiDir = dir.Phi() * TMath::RadToDeg();
    double phiLocalDir = dirLocal.Phi() * TMath::RadToDeg();
    
    double phiOuter=pos20.Phi()*TMath::RadToDeg();
    
    double charge = theRes->getCharge();
    
    double resMag = res.Mag();
    double resPerp = res.Perp();
    double posPerp = uvw.Perp();
    
    double dPhiHist = pos.Phi() - ref.Phi();
    dPhiHist = dPhiHist * 180 / TMath::Pi();
    if (dPhiHist > 180) {
        dPhiHist -= 360;
    } else if (dPhiHist < -180) {
        dPhiHist += 360;
    }
    
    double dPhiTrackDir = phiLocalDir - phiDir;
    if (dPhiTrackDir > 180) {
        dPhiTrackDir -= 360;
    } else if (dPhiTrackDir < -180) {
        dPhiTrackDir += 360;
    }
    double perpDir = dPhiHist / fabs(dPhiHist);
    hists["hresx"]->Fill(res.X());
    hists["hresXvX"]->Fill(pos.X(), res.X());
    hists["hresXvY"]->Fill(pos.Y(), res.X());
    hists["hresXvZ"]->Fill(pos.Z(), res.X());
    hists["hresXvPerp"]->Fill(posPerp, res.X());
    hists["hresXvPhi"]->Fill(phiDir, res.X());
    hists["hresXvPhiPos"]->Fill(phiOuter, res.X());
    
    hists["hresy"]->Fill(res.Y());
    hists["hresYvX"]->Fill(pos.X(), res.Y());
    hists["hresYvY"]->Fill(pos.Y(), res.Y());
    hists["hresYvZ"]->Fill(pos.Z(), res.Y());
    hists["hresYvPerp"]->Fill(posPerp, res.Y());
    hists["hresYvPhi"]->Fill(phiPos, res.Y());
    hists["hresYvPhiPos"]->Fill(phiOuter, res.Y());
    
    hists["hresz"]->Fill(res.Z());
    hists["hresZvX"]->Fill(pos.X(), res.Z());
    hists["hresZvY"]->Fill(pos.Y(), res.Z());
    hists["hresZvZ"]->Fill(pos.Z(), res.Z());
    hists["hresZvPerp"]->Fill(posPerp, res.Z());
    hists["hresZvPhi"]->Fill(phiDir, res.Z());
    hists["hresZvPhiPos"]->Fill(phiOuter, res.Z());
    hists["hresZvPhiCdc"]->Fill(phiOuter, res.Z());
    if (charge > 0) {
        hists["hresZvPhiCdcPlus"]->Fill(phiOuter, res.Z());
    }else{
        hists["hresZvPhiCdcMinus"]->Fill(phiOuter, res.Z());
    }
    hists["hresPerp"]->Fill(resPerp);
    hists["hresPerpvX"]->Fill(pos.X(), resPerp);
    hists["hresPerpvY"]->Fill(pos.Y(), resPerp);
    hists["hresPerpvZ"]->Fill(pos.Z(), resPerp);
    hists["hresPerpvPerp"]->Fill(posPerp, resPerp);
    hists["hresPerpvPhi"]->Fill(phiPos, resPerp);
    hists["hresPerpvPhiPos"]->Fill(phiOuter, resPerp);
    
    
    hists["hresSignedPerp"]->Fill(resPerp * perpDir);
    hists["hresSignedPerpvX"]->Fill(pos.X(), resPerp * perpDir);
    hists["hresSignedPerpvY"]->Fill(pos.Y(), resPerp * perpDir);
    hists["hresSignedPerpvZ"]->Fill(pos.Z(), resPerp * perpDir);
    hists["hresSignedPerpvPerp"]->Fill(posPerp, resPerp * perpDir);
    hists["hresSignedPerpvPhi"]->Fill(phiPos,
                                      resPerp * perpDir);
    hists["hresSignedPerpvPhiPos"]->Fill(phiPos,
                                         resPerp * perpDir);
    hists["hresSignedPerpvPhiCdc"]->Fill(phiOuter,
                                         resPerp * perpDir);
    hists["hresSignedPerpvDafWeight"]->Fill(theRes->getTrackLength(),
                                            resPerp * perpDir);
    if (charge > 0) {
        hists["hresSignedPerpPlus"]->Fill(resPerp * perpDir);
        hists["hresSignedPerpvPhiPosPlus"]->Fill(phiPos,
                                                 resPerp * perpDir);
        hists["hresSignedPerpvPhiCdcPlus"]->Fill(phiOuter,
                                                 resPerp * perpDir);
        hists["hresSignedPerpvPerpPlus"]->Fill(posPerp, resPerp * perpDir);
    } else {
        hists["hresSignedPerpMinus"]->Fill(resPerp * perpDir);
        hists["hresSignedPerpvPhiPosMinus"]->Fill(phiPos,
                                                  resPerp * perpDir);
        hists["hresSignedPerpvPhiCdcMinus"]->Fill(phiOuter,
                                                  resPerp * perpDir);
        hists["hresSignedPerpvPerpMinus"]->Fill(posPerp, resPerp * perpDir);
    }
    
    hists["hresMag"]->Fill(resMag);
    hists["hresMagvX"]->Fill(pos.X(), resMag);
    hists["hresMagvY"]->Fill(pos.Y(), resMag);
    hists["hresMagvZ"]->Fill(pos.Z(), resMag);
    hists["hresMagvPerp"]->Fill(posPerp, resMag);
    hists["hresMagvPhi"]->Fill(phiPos, resMag);
    hists["hresMagvPhiPos"]->Fill(phiOuter, resMag);
    
    hists["hresPhi"]->Fill(dPhiHist);
    hists["hresPhivX"]->Fill(pos.X(), dPhiHist);
    hists["hresPhivY"]->Fill(pos.Y(), dPhiHist);
    hists["hresPhivZ"]->Fill(pos.Z(), dPhiHist);
    hists["hresPhivPerp"]->Fill(posPerp, dPhiHist);
    hists["hresPhivPhi"]->Fill(phiPos, dPhiHist);
    hists["hresPhivPhiPos"]->Fill(phiOuter, dPhiHist);
    if (charge > 0) {
        hists["hresPhiPlus"]->Fill(dPhiHist);
        hists["hresPhivPhiPosPlus"]->Fill(phiOuter, dPhiHist);
        hists["hresPhivPerpPlus"]->Fill(posPerp, dPhiHist);
    } else {
        hists["hresPhiMinus"]->Fill(dPhiHist);
        hists["hresPhivPhiPosMinus"]->Fill(phiOuter, dPhiHist);
        hists["hresPhivPerpMinus"]->Fill(posPerp, dPhiHist);
    }
    
    hists["hres_trDist"]->Fill(theRes->getTrackLength());
    hists["hresPerpvDist"]->Fill(theRes->getTrackLength(), resPerp);
    hists["hitsX"]->Fill(pos.X());
    hists["hitsY"]->Fill(pos.Y());
    hists["hitsZ"]->Fill(pos.Z());
    hists["hitsPhi"]->Fill(phiPos);
    
    hists["hitsW"]->Fill(uvw.Z());
    hists["hitsRl"]->Fill(uvw.Perp());
    hists["hitsPerpl"]->Fill(uvw.x(), uvw.y());
    hists["hitsPerp"]->Fill(pos.X(), pos.Y());
    hists["hitsZR"]->Fill(pos.Z(), posPerp);
    
    hists["hresPhivPhiPosModulo"]->Fill(phiPosInSector, dPhiHist);
    if (charge > 0) {
        hists["hresPhivPhiPosPlusModulo"]->Fill(phiPosInSector, dPhiHist);
        hists["hresSignedPerpvPhiPosPlusModulo"]->Fill(phiPosInSector,
                                                       resPerp * perpDir);
    } else {
        hists["hresPhivPhiPosMinusModulo"]->Fill(phiPosInSector, dPhiHist);
        hists["hresSignedPerpvPhiPosMinusModulo"]->Fill(phiPosInSector,
                                                        resPerp * perpDir);
    }
    hists["hresSignedPerpvPhiPosModulo"]->Fill(phiPosInSector, resPerp * perpDir);
    
    double zRange = zEnd - zStart;
    double zBin = zRange / ((double)zSlices);
    double zBinStart = zStart;
    double zBinEnd = zBinStart;
    for (int i = 0; i < zSlices; ++i) {
        zBinEnd += zBin;
        if (pos.Z() < zBinEnd) {
            TString histPostFix = TString::Format("_zSlice_%i", i);
            hists["hresPhivPerp" + histPostFix]->Fill(posPerp, dPhiHist);
            hists["hresPhivPhiTemp" + histPostFix]->Fill(
                        phiOuter, dPhiHist);
            hists["hresPhivPhiPos" + histPostFix]->Fill(phiOuter,
                                                        dPhiHist);
            hists["hresSignedPerpvPhiPos" + histPostFix]->Fill(
                        phiOuter , resPerp * perpDir);
            
            if (charge > 0) {
                hists["hresPhivPhiPosPlus" + histPostFix]->Fill(
                            phiOuter, dPhiHist);
                hists["hresSignedPerpvPhiPosPlus" + histPostFix]->Fill(
                            phiOuter, resPerp * perpDir);
            } else {
                hists["hresPhivPhiPosMinus" + histPostFix]->Fill(
                            phiOuter, dPhiHist);
                hists["hresSignedPerpvPhiPosMinus" + histPostFix]->Fill(
                            phiOuter , resPerp * perpDir);
            }
            break;
        }
    }
    double phiRange = 22.5;
    int phiSlices = 20;
    double phiBinStart = -11.25;
    double phiBin = phiRange / phiSlices;
    double phiBinEnd = phiBinStart;
    
    for (int i = 0; i < phiSlices; ++i) {
        phiBinEnd += phiBin;
        
        if (phiPosInSector < phiBinEnd) {
            TString histPostFix = TString::Format("_phiSlice_%i", i);
            hists["hresSignedPerpvPerp" + histPostFix]->Fill(posPerp,
                                                             resPerp * perpDir);
            if (charge > 0) {
                hists["hresSignedPerpvPerpPlus" + histPostFix]->Fill(posPerp,
                                                                     resPerp * perpDir);
            } else {
                hists["hresSignedPerpvPerpMinus" + histPostFix]->Fill(
                            posPerp, resPerp * perpDir);
            }
            break;
        }
        phiBinStart = phiBinEnd;
    }
    
    TVectorD normResD(3);
    TVectorD normRes2D(3);
    TVector3 normRes = res.Unit();
    if(theRes->getHitCov()[0][0]!=0){
        TMatrixDSym covHitSym(3, theRes->getHitCov().GetMatrixArray());
        TMatrixDSym covHitSymInv = covHitSym.Invert();
        //    std::cout<<"getting track cov matrix
        //    #########################################################\n";
        TMatrixDSym covTrackSym(5, theRes->getTrackCov().GetMatrixArray());
        //    std::cout<<"trackCov print: \n"<<std::flush;
        //    std::cout<<covTrackSym[0][0]<<"\t"<<covTrackSym[0][1]<<"\t"<<covTrackSym[0][2]<<"\n";
        //    std::cout<<covTrackSym[1][0]<<"\t"<<covTrackSym[1][1]<<"\t"<<covTrackSym[1][2]<<"\n";
        //    std::cout<<covTrackSym[2][0]<<"\t"<<covTrackSym[2][1]<<"\t"<<covTrackSym[2][2]<<"\n";
        //    std::cout<<covTrackSym.Determinant()<<"\n";
        // TMatrixDSym covTrackSymInv(TMatrixDSym::kInverted,covTrackSym);
        //    std::cout<<"trackCov invert: \n"<<std::flush;
        //    std::cout<<std::setprecision(9)<<covTrackSymInv[0][0]<<"\t"<<covTrackSymInv[0][1]<<"\t"<<covTrackSymInv[0][2]<<"\n";
        //    std::cout<<covTrackSymInv[1][0]<<"\t"<<covTrackSymInv[1][1]<<"\t"<<covTrackSymInv[1][2]<<"\n";
        //    std::cout<<covTrackSymInv[2][0]<<"\t"<<covTrackSymInv[2][1]<<"\t"<<covTrackSymInv[2][2]<<"\n";
        //    std::cout<<covTrackSymInv.Determinant()<<"\n";
        normResD[0] = normRes.X();
        normResD[1] = normRes.Y();
        normResD[2] = normRes.Z();
        normRes2D[0] = res.XYvector().Unit().X();
        normRes2D[1] = res.XYvector().Unit().Y();
        normRes2D[2] = 0;
        TVectorD xD(3);
        xD[0] = 1;
        xD[1] = 0;
        xD[2] = 0;
        TVectorD yD(3);
        yD[0] = 0;
        yD[1] = 1;
        yD[2] = 0;
        TVectorD zD(3);
        zD[0] = 0;
        zD[1] = 0;
        zD[2] = 1;
        
        // lambda2= res*cov*res // projection of cov
        //##########################################################
        //##########################################################
        //##########################################################
        // # Additional separate histograms for chi2 method:
        //##########################################################matrix onto
        //residual vector
        // 1/lambda1=resT * inverse(cov) * residual //cut of cov-matrix onto residual
        // vector
        
        double covRes = covHitSym.Similarity(normResD);
        double covX = covHitSym.Similarity(xD);
        double covY = covHitSym.Similarity(yD);
        double covZ = covHitSym.Similarity(zD);
        double cov2D = covHitSym.Similarity(normRes2D);
        // Trackerr
        //  double covTrackRes = covTrackSym.Similarity(normResD);
        //  double covTrackX = covTrackSym.Similarity(xD);
        //  double covTrackY = covTrackSym.Similarity(yD);
        //  double covTrackZ = covTrackSym.Similarity(zD);
        //  double covTrack2D = covTrackSym.Similarity(normRes2D);
        
        double covTrackRes = 0;
        double covTrackX = 0;
        double covTrackY = 0;
        double covTrackZ = sqrt(covTrackSym[3][3]);
        ;
        double covTrack2D = sqrt(covTrackSym[4][4]);
        
        //    double covRes = 1/covHitSymInv.Similarity(normResD);
        //    double covX =  1/covHitSymInv.Similarity(xD);
        //    double covY =  1/covHitSymInv.Similarity(yD);
        //    double covZ =  1/covHitSymInv.Similarity(zD);
        //    double cov2D = 1/covHitSymInv.Similarity(normRes2D);
        //    // Trackerr
        //    double covTrackRes = 1/covTrackSymInv.Similarity(normResD);
        //    double covTrackX = 1/covTrackSymInv.Similarity(xD);
        //    double covTrackY = 1/covTrackSymInv.Similarity(yD);
        //    double covTrackZ = 1/covTrackSymInv.Similarity(zD);
        //    double covTrack2D = 1/covTrackSymInv.Similarity(normRes2D);
        
        // std::cout<<covTrackRes<<", "<<covTrack2D<<"\n";
        
        hists["hresCovX"]->Fill(covX);
        hists["hresCovY"]->Fill(covY);
        hists["hresCovZ"]->Fill(covZ);
        hists["hresCov"]->Fill(covRes);
        hists["hresCovXY"]->Fill(cov2D);
        hists["hresMagdCov"]->Fill(resMag / covRes);
        hists["hresSignedPerpdCov"]->Fill(resPerp * perpDir / cov2D);
        hists["hresMagVCov"]->Fill(covRes, resMag);
        hists["hresXVCov"]->Fill(covX, res.x());
        hists["hresYVCov"]->Fill(covY, res.y());
        hists["hresZVCov"]->Fill(covZ, res.z());
        hists["hresSignedPerpVCov"]->Fill(cov2D, resPerp * perpDir);
        hists["hresCovVPerp"]->Fill(posPerp, covRes);
        hists["hresCovPerpVPerp"]->Fill(posPerp, cov2D);
        
        hists["hresCovTrackX"]->Fill(covTrackX);
        hists["hresCovTrackY"]->Fill(covTrackY);
        hists["hresCovTrackZ"]->Fill(covTrackZ);
        hists["hresCovTrack"]->Fill(covTrackRes);
        hists["hresCovTrackXY"]->Fill(covTrack2D);
        hists["hresMagdCovTrack"]->Fill(resMag / covTrackRes);
        hists["hresSignedPerpdCovTrack"]->Fill(resPerp * perpDir / covTrack2D);
        hists["hresMagVCovTrack"]->Fill(covTrackRes, resMag);
        hists["hresXVCovTrack"]->Fill(covTrackX, res.x());
        hists["hresYVCovTrack"]->Fill(covTrackY, res.y());
        hists["hresZVCovTrack"]->Fill(covTrackZ, res.z());
        hists["hresSignedPerpVCovTrack"]->Fill(covTrack2D, resPerp * perpDir);
        hists["hresCovTrackVPerp"]->Fill(posPerp, covTrackRes);
        hists["hresCovTrackPerpVPerp"]->Fill(posPerp, covTrack2D);
    }
    hists["hresdTrackPhi"]->Fill(dPhiTrackDir);
    hists["hresdTrackPhiVPhiPos"]->Fill(phiOuter, dPhiTrackDir);
    hists["hresdTrackPhiVPerp"]->Fill(posPerp, dPhiTrackDir);
    if (charge > 0) {
        hists["hresdTrackPhiPlus"]->Fill(dPhiTrackDir);
        hists["hresdTrackPhiVPhiPosPlus"]->Fill(phiOuter, dPhiTrackDir);
        hists["hresdTrackPhiVPerpPlus"]->Fill(posPerp, dPhiTrackDir);
    } else {
        hists["hresdTrackPhiMinus"]->Fill(dPhiTrackDir);
        hists["hresdTrackPhiVPhiPosMinus"]->Fill(phiOuter, dPhiTrackDir);
        hists["hresdTrackPhiVPerpMinus"]->Fill(posPerp, dPhiTrackDir);
    }
}

void TpcAlignmentSimple::writeChiSqHists(TString filename) {
    fchi2HistFile->cd();
    std::map<TString, TH1 *>::iterator it;
    for (it = chi2Hists.begin(); it != chi2Hists.end(); ++it) {
        std::cout << "." << std::flush;
        std::cout << (*it).first << std::endl;
        (*it).second->Write();
    }
}