#include <iostream>
#include <vector>
#include <map>
#include <utility>
#include <numeric>
#include <limits>
#include <cmath>
#include <TSystem.h>

#include "filval/filval.hpp"
#include "filval/root/filval.hpp"

#include <boost/range/combine.hpp>
#include <boost/format.hpp>

#include "TrackingNtuple.h"
#include "analysis/obj_types.cpp"

using namespace std;
using namespace fv;
using namespace fv::root;

typedef std::tuple<Seed, PixRecHit, Track, SimHit, SimTrack> MatchedTrack;
typedef std::pair<std::vector<float>,std::vector<float>> pair_vec;

#define HIT_TYPE_PIXEL 0
#define HIT_TYPE_GLUED 1
#define HIT_TYPE_STRIP 2

#define PIXEL_BARREL 1
#define PIXEL_ENDCAP 2
std::map<int,string> subdet_names = {{1, "BPIX"}, {2, "FPIX"}};

template<typename A, typename B>
float displacement(const A& a, const B& b){
    return std::sqrt(pow(a.x-b.x, 2) +
                     pow(a.x-b.x, 2) +
                     pow(a.x-b.x, 2));
}

bool in_det(const MatchedTrack &mt, int &&det, int &&layer){
    auto& hit = std::get<PixRecHit>(mt);
    return hit.det == det && hit.lay == layer;
};

float pseudorapidity(float z, float r){
    float theta = atan2(r, z);
    return -log(tan(theta/2.0));
}
float pseudorapidity(float x, float y, float z){
    float r = sqrt(x*x + y*y);
    return pseudorapidity(z, r);
}
template<typename T>
float pseudorapidity(const T &t){
    return pseudorapidity(t.x, t.y, t.z);
}


vector<string> seedTypes =
    {"initialStepSeeds",
     "highPtTripletStepSeeds",
     "mixedTripletStepSeeds",
     "pixelLessStepSeeds",
     "tripletElectronSeeds",
     "pixelPairElectronSeeds",
     "stripPairElectronSeeds"};

Value<vector<Seed>>* seeds;
Value<vector<PixRecHit>>* pixrec_hits;
Value<vector<Track>>* tracks;
Value<vector<SimHit>>* sim_hits;
Value<vector<SimTrack>>* sim_tracks;

void register_objects(TrackingDataSet& tds){
    seeds = register_seeds(tds);

    pixrec_hits = register_pixrec_hits(tds);
    tracks = register_tracks(tds);

    sim_hits = register_sim_hits(tds);
    sim_tracks = register_sim_tracks(tds);
}

void setup_first_hit_pairs(TrackingDataSet& tds){
    typedef std::tuple<PixRecHit, SimHit> HitPair;

    auto& find_matched_nth_hit_in_layer =
    func<vector<HitPair>(vector<Seed>,
                         vector<PixRecHit>,
                         vector<SimHit>,
                         int, int, int)>("find_matched_nth_hit_in_layer",
        FUNC(([](const vector<Seed>& seeds,
                 const vector<PixRecHit>& pixrec_hits,
                 const vector<SimHit>& sim_hits,
                 const int&& det,
                 const int&& bpix_layer,
                 const int&& hit_number){
            vector<HitPair> matched_hits;
            for(const Seed &seed : seeds){ // loop over all seeds
                if(seed.hitIdx.size() <= hit_number) continue;
                if(seed.algoOriginal < 0 || seed.algoOriginal >= seedTypes.size()) continue;
                if(seed.hitType[hit_number] != HIT_TYPE_PIXEL) continue; // take only pixel hits for now
                const PixRecHit &rec_hit = pixrec_hits[seed.hitIdx[hit_number]];
                if(rec_hit.det == det && rec_hit.lay == bpix_layer){
                    if(rec_hit.simHitIdx.size() > 0){
                        // take first matched simhit (should be the closest one)
                        matched_hits.push_back({rec_hit, sim_hits[rec_hit.simHitIdx[0]]});
                    }
                }
            }
            return matched_hits;
        })));

    auto& select_even_odd_ladder_blade_hit_pairs =
    func<vector<HitPair>(vector<HitPair>, bool)>("select_even_odd_ladder_blade_hit_pairs",
        FUNC(([](const vector<HitPair>& hit_pairs,
                 const bool &&odd){
            vector<HitPair> even_pairs;
            for(const HitPair &hit_pair : hit_pairs){ // loop over all seeds
                if(std::get<PixRecHit>(hit_pair).ladder_blade % 2 == odd){
                    even_pairs.push_back(hit_pair);
                }
            }
            return even_pairs;
        })));

    auto barrel_val = constant("PIXEL_BARREL", PIXEL_BARREL);
    auto endcap_val = constant("PIXEL_ENDCAP", PIXEL_ENDCAP);
    auto first_hit  = constant("1st", 0);
    auto second_hit = constant("2nd", 1);

    // First hits on inner three bpix layers
    auto first_hits_in_B1 = fv::apply(find_matched_nth_hit_in_layer,
            fv::tuple(seeds, pixrec_hits, sim_hits, barrel_val, constant("L1", 1), first_hit), "first_hits_in_B1");
    auto first_hits_in_B2 = fv::apply(find_matched_nth_hit_in_layer,
            fv::tuple(seeds, pixrec_hits, sim_hits, barrel_val, constant("L2", 2), first_hit), "first_hits_in_B2");
    auto first_hits_in_B3 = fv::apply(find_matched_nth_hit_in_layer,
            fv::tuple(seeds, pixrec_hits, sim_hits, barrel_val, constant("L3", 3), first_hit), "first_hits_in_B3");

    // Second hits on outer three bpix layers
    auto second_hits_in_B2 = fv::apply(find_matched_nth_hit_in_layer,
            fv::tuple(seeds, pixrec_hits, sim_hits, barrel_val, constant("L2", 2), second_hit), "second_hits_in_B2");
    auto second_hits_in_B3 = fv::apply(find_matched_nth_hit_in_layer,
            fv::tuple(seeds, pixrec_hits, sim_hits, barrel_val, constant("L3", 3), second_hit), "second_hits_in_B3");
    auto second_hits_in_B4 = fv::apply(find_matched_nth_hit_in_layer,
            fv::tuple(seeds, pixrec_hits, sim_hits, barrel_val, constant("L4", 4), second_hit), "second_hits_in_B4");

    /* auto first_hits_in_F1 = fv::apply(find_matched_nth_hit_in_layer, */
    /*         fv::tuple(seeds, pixrec_hits, sim_hits, endcap_val, constant("1", 1)), "first_hits_in_F1"); */
    /* auto first_hits_in_F2 = fv::apply(find_matched_nth_hit_in_layer, */
    /*         fv::tuple(seeds, pixrec_hits, sim_hits, endcap_val, constant("2", 2)), "first_hits_in_F2"); */

    // Even vs Odd Ladders
    auto even = constant("even", false);
    auto odd = constant("odd", true);

    auto first_hits_in_B1_even_ladder = fv::apply(select_even_odd_ladder_blade_hit_pairs,
            fv::tuple(first_hits_in_B1, even), "first_hits_in_B1_even_ladder");
    auto first_hits_in_B1_odd_ladder = fv::apply(select_even_odd_ladder_blade_hit_pairs,
            fv::tuple(first_hits_in_B1, odd), "first_hits_in_B1_odd_ladder");

    auto first_hits_in_B2_even_ladder = fv::apply(select_even_odd_ladder_blade_hit_pairs,
            fv::tuple(first_hits_in_B2, even), "first_hits_in_B2_even_ladder");
    auto first_hits_in_B2_odd_ladder = fv::apply(select_even_odd_ladder_blade_hit_pairs,
            fv::tuple(first_hits_in_B2, odd), "first_hits_in_B2_odd_ladder");

    auto first_hits_in_B3_even_ladder = fv::apply(select_even_odd_ladder_blade_hit_pairs,
            fv::tuple(first_hits_in_B3, even), "first_hits_in_B3_even_ladder");
    auto first_hits_in_B3_odd_ladder = fv::apply(select_even_odd_ladder_blade_hit_pairs,
            fv::tuple(first_hits_in_B3, odd), "first_hits_in_B3_odd_ladder");

    //Plots for dPhi of collections defined above
    auto& calc_dphi_v_eta = func<pair_vec(vector<HitPair>)>("calc_dphi_v_eta",
        FUNC(([](const vector<HitPair>& hit_pairs){
            vector<float> dphis;
            vector<float> etas;
            for(auto hit_pair : hit_pairs){
                auto& pixrec_hit = std::get<PixRecHit>(hit_pair);
                auto& sim_hit = std::get<SimHit>(hit_pair);
                dphis.push_back(atan2(sim_hit.x, sim_hit.y) - atan2(pixrec_hit.x, pixrec_hit.y));
                etas.push_back(pseudorapidity(pixrec_hit));
            }
            return std::make_pair(etas, dphis);
        })));
    TH2Params params_dphi = {"$\\eta$",       100, -4,   4,
                             "$\\Delta \\phi$(rad)", 75,  -.0015, .0015};
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B1",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B1)),
                                                    "First Hit in BPIX-L1", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B2",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B2)),
                                                    "First Hit in BPIX-L2", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B3",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B3)),
                                                    "First Hit in BPIX-L3", params_dphi);

    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B1_even_ladder",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B1_even_ladder)),
                                                    "First Hit in BPIX-L1 - Even Ladders", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B1_odd_ladder",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B1_odd_ladder)),
                                                    "First Hit in BPIX-L1 - Odd Ladders", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B2_even_ladder",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B2_even_ladder)),
                                                    "First Hit in BPIX-L2 - Even Ladders", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B2_odd_ladder",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B2_odd_ladder)),
                                                    "First Hit in BPIX-L2 - Odd Ladders", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B3_even_ladder",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B3_even_ladder)),
                                                    "First Hit in BPIX-L3 - Even Ladders", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_first_hits_in_B3_odd_ladder",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(first_hits_in_B3_odd_ladder)),
                                                    "First Hit in BPIX-L3 - Odd Ladders", params_dphi);

    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_second_hits_in_B2",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(second_hits_in_B2)),
                                                    "Second Hit in BPIX-L2", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_second_hits_in_B3",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(second_hits_in_B3)),
                                                    "Second Hit in BPIX-L3", params_dphi);
    tds.register_container<ContainerTH2Many<float>>("dphi_v_eta_second_hits_in_B4",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(second_hits_in_B4)),
                                                    "Second Hit in BPIX-L4", params_dphi);

    //Plots for dz of collections defined above
    auto& calc_dz_v_eta = func<pair_vec(vector<HitPair>)>("calc_dz_v_eta",
        FUNC(([](const vector<HitPair>& hit_pairs){
            vector<float> dzs;
            vector<float> etas;
            for(auto hit_pair : hit_pairs){
                auto& pixrec_hit = std::get<PixRecHit>(hit_pair);
                auto& sim_hit = std::get<SimHit>(hit_pair);
                dzs.push_back(sim_hit.z - pixrec_hit.z);
                etas.push_back(pseudorapidity(pixrec_hit));
            }
            return std::make_pair(etas, dzs);
        })));
    TH2Params params_dz = {"$\\eta$",       100, -4,   4,
                           "$\\Delta z$(rad)", 100,  -.01, .01};
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B1",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B1)),
                                                    "First Hit in BPIX-L1", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B2",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B2)),
                                                    "First Hit in BPIX-L2", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B3",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B3)),
                                                    "First Hit in BPIX-L3", params_dz);

    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B1_even_ladder",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B1_even_ladder)),
                                                    "First Hit in BPIX-L1 - Even Ladders", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B1_odd_ladder",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B1_odd_ladder)),
                                                    "First Hit in BPIX-L1 - Odd Ladders", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B2_even_ladder",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B2_even_ladder)),
                                                    "First Hit in BPIX-L2 - Even Ladders", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B2_odd_ladder",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B2_odd_ladder)),
                                                    "First Hit in BPIX-L2 - Odd Ladders", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B3_even_ladder",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B3_even_ladder)),
                                                    "First Hit in BPIX-L3 - Even Ladders", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_first_hits_in_B3_odd_ladder",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(first_hits_in_B3_odd_ladder)),
                                                    "First Hit in BPIX-L3 - Odd Ladders", params_dz);

    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_second_hits_in_B2",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(second_hits_in_B2)),
                                                    "Second Hit in BPIX-L2", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_second_hits_in_B3",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(second_hits_in_B3)),
                                                    "Second Hit in BPIX-L3", params_dz);
    tds.register_container<ContainerTH2Many<float>>("dz_v_eta_second_hits_in_B4",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(second_hits_in_B4)),
                                                    "Second Hit in BPIX-L4", params_dz);


}

void setup_matched_tracks(TrackingDataSet& tds){
    // Finds sets of (seed, rechit, track, simhit sim_track) where the cycle
    // track->seed->rec_hit->sim_hit->sim_track->track
    // is closed
    auto& find_matched_tracks =
    func<vector<MatchedTrack>(vector<Seed>,
                              vector<PixRecHit>,
                              vector<Track>,
                              vector<SimHit>,
                              vector<SimTrack>)>("find_matched_tracks",
        FUNC(([](const vector<Seed>& seeds,
                 const vector<PixRecHit> pixrec_hits,
                 const vector<Track>& tracks,
                 const vector<SimHit> sim_hits,
                 const vector<SimTrack> sim_tracks){
            INFO("New event");
            INFO(boost::format("Number of tracks is %d, number of seeds is %d, number of hits %d, number of simhits %d")
                    % tracks.size() % seeds.size() % pixrec_hits.size() % sim_hits.size());
            vector<MatchedTrack> matched_tracks;
            for(const Track &track : tracks){
                const Seed& seed = seeds[track.seedIdx];
                if( seed.algoOriginal < 0 || seed.algoOriginal >= seedTypes.size()) continue;
                INFO(boost::format("       seed indx= %d   algorithm= %d   type %s\n")
                        % track.seedIdx % seed.algoOriginal % seedTypes[seed.algoOriginal]);
                for(auto tup : boost::combine(seed.hitIdx, seed.hitType)){ //for hits in track's seed
                    int hitIdx, hitType;
                    boost::tie(hitIdx, hitType) = tup;
                    if(hitType != HIT_TYPE_PIXEL) continue; // take only pixel hits for now
                    const PixRecHit &rec_hit = pixrec_hits[hitIdx];

                    TVector3 recoHitPoint;
                    recoHitPoint.SetXYZ( rec_hit.x, rec_hit.y, rec_hit.z);
                    float Rreco = recoHitPoint.Perp();
                    INFO(boost::format("            hit= %d is in %s-L%d at radius= %f\n")
                                       % hitIdx % subdet_names[rec_hit.det] % rec_hit.lay % Rreco);

                    for(const int& simHitIdx : rec_hit.simHitIdx){ // for sim-hits matching rec-hit
                        const SimHit &sim_hit = sim_hits[simHitIdx];
                        const SimTrack &sim_track = sim_tracks[sim_hit.simTrkIdx];
                        for(const int &trkIdx : sim_track.trkIdx){ // for tracks matched to this sim_track
                            if(trkIdx == track.idx){  // sim_track matches with our original track
                                matched_tracks.push_back({seed, rec_hit, track, sim_hit, sim_track});
                                break;
                            }
                        }
                    }
                }
            }
            return matched_tracks;
        })));

    fv::apply(find_matched_tracks, fv::tuple(seeds, pixrec_hits, tracks, sim_hits, sim_tracks), "matched_tracks");
}

void setup_residuals(TrackingDataSet &tds){

    auto matched_tracks = lookup<vector<MatchedTrack>>("matched_tracks");

    // Break matched_tracks into catagories based on Barrel/Endcap and layer
    auto matched_tracks_B1 = filter(func<bool(MatchedTrack)>("matched_tracks_B1",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_BARREL, 1);
        }))), matched_tracks);
    auto matched_tracks_B2 = filter(func<bool(MatchedTrack)>("matched_tracks_B2",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_BARREL, 2);
        }))), matched_tracks);
    auto matched_tracks_B3 = filter(func<bool(MatchedTrack)>("matched_tracks_B3",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_BARREL, 3);
        }))), matched_tracks);
    auto matched_tracks_B4 = filter(func<bool(MatchedTrack)>("matched_tracks_B4",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_BARREL, 4);
        }))), matched_tracks);

    auto matched_tracks_F1 = filter(func<bool(MatchedTrack)>("matched_tracks_F1",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_ENDCAP, 1);
        }))), matched_tracks);
    auto matched_tracks_F2 = filter(func<bool(MatchedTrack)>("matched_tracks_F2",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_ENDCAP, 2);
        }))), matched_tracks);
    auto matched_tracks_F3 = filter(func<bool(MatchedTrack)>("matched_tracks_F3",
        FUNC(([](const MatchedTrack& matched_track){
            return in_det(matched_track, PIXEL_ENDCAP, 3);
        }))), matched_tracks);


    TH2Params params = {"$\\eta$",       100, -4,   4,
                        "Residuals(cm)", 100,  0, .01};
    auto& calc_residuals_v_eta = func<pair_vec(vector<MatchedTrack>)>("matched_track_hit_residuals",
        FUNC(([](const vector<MatchedTrack>& matched_tracks){
            vector<float> residuals;
            vector<float> etas;
            for(auto matched_track : matched_tracks){
                auto& pixrec_hit = std::get<PixRecHit>(matched_track);
                auto& sim_hit = std::get<SimHit>(matched_track);
                auto& sim_track = std::get<SimTrack>(matched_track);
                residuals.push_back(displacement(pixrec_hit, sim_hit));
                etas.push_back(sim_track.eta);
            }
            return std::make_pair(etas, residuals);
        })));
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_all",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks)),
                                                    "Matched Track Residuals - All", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_B1",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_B1)),
                                                    "Matched Track Residuals - B1", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_B2",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_B2)),
                                                    "Matched Track Residuals - B2", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_B3",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_B3)),
                                                    "Matched Track Residuals - B3", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_B4",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_B4)),
                                                    "Matched Track Residuals - B4", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_F1",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_F1)),
                                                    "Matched Track Residuals - F1", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_F2",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_F2)),
                                                    "Matched Track Residuals - F2", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_residuals_v_eta_F3",
                                                    fv::apply(calc_residuals_v_eta, fv::tuple(matched_tracks_F3)),
                                                    "Matched Track Residuals - F3", params);


    params = {"$\\eta$",        100,    -4,    4,
              "$\\Delta \\phi$(rad)", 100, -.002, .002};
    auto& calc_dphi_v_eta = func<pair_vec(vector<MatchedTrack>)>("matched_track_hit_dphis",
        FUNC(([](const vector<MatchedTrack>& matched_tracks){
            vector<float> dphis;
            vector<float> etas;
            for(auto matched_track : matched_tracks){
                auto& pixrec_hit = std::get<PixRecHit>(matched_track);
                auto& sim_hit = std::get<SimHit>(matched_track);
                auto& sim_track = std::get<SimTrack>(matched_track);
                dphis.push_back(atan2(sim_hit.x, sim_hit.y) - atan2(pixrec_hit.x, pixrec_hit.y));
                etas.push_back(sim_track.eta);
            }
            return std::make_pair(etas, dphis);
        })));

    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_all",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks)),
                                                    "Matched Track $\\Delta \\phi$ - All", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_B1",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_B1)),
                                                    "Matched Track $\\Delta \\phi$ - B1", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_B2",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_B2)),
                                                    "Matched Track $\\Delta \\phi$ - B2", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_B3",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_B3)),
                                                    "Matched Track $\\Delta \\phi$ - B3", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_B4",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_B4)),
                                                    "Matched Track $\\Delta \\phi$ - B4", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_F1",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_F1)),
                                                    "Matched Track $\\Delta \\phi$ - F1", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_F2",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_F2)),
                                                    "Matched Track $\\Delta \\phi$ - F2", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dphis_v_eta_F3",
                                                    fv::apply(calc_dphi_v_eta, fv::tuple(matched_tracks_F3)),
                                                    "Matched Track $\\Delta \\phi$ - F3", params);


    auto& calc_dz_v_eta = func<pair_vec(vector<MatchedTrack>)>("matched_track_hit_dphis",
        FUNC(([](const vector<MatchedTrack>& matched_tracks){
            vector<float> dzs;
            vector<float> etas;
            for(auto matched_track : matched_tracks){
                auto& pixrec_hit = std::get<PixRecHit>(matched_track);
                auto& sim_hit = std::get<SimHit>(matched_track);
                auto& sim_track = std::get<SimTrack>(matched_track);
                dzs.push_back(sim_hit.z - pixrec_hit.z);
                etas.push_back(sim_track.eta);
            }
            return std::make_pair(etas, dzs);
        })));
    params = {"$\\eta$",         100,    -4,    4,
              "$\\Delta z$(cm)", 100, -.002, .002};

    tds.register_container<ContainerTH2Many<float>>("matched_track_dzs_v_eta_all",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(matched_tracks)),
                                                    "Matched Track $\\Delta z$ - All", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dzs_v_eta_B1",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(matched_tracks_B1)),
                                                    "Matched Track $\\Delta z$ - B1", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dzs_v_eta_B2",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(matched_tracks_B2)),
                                                    "Matched Track $\\Delta z$ - B2", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dzs_v_eta_B3",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(matched_tracks_B3)),
                                                    "Matched Track $\\Delta z$ - B3", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_dzs_v_eta_B4",
                                                    fv::apply(calc_dz_v_eta, fv::tuple(matched_tracks_B4)),
                                                    "Matched Track $\\Delta z$ - B4", params);

    auto& calc_dr_v_eta = func<pair_vec(vector<MatchedTrack>)>("matched_track_hit_drs",
        FUNC(([](const vector<MatchedTrack>& matched_tracks){
            vector<float> drs;
            vector<float> etas;
            for(auto matched_track : matched_tracks){
                auto& rec_hit = std::get<PixRecHit>(matched_track);
                auto& sim_hit = std::get<SimHit>(matched_track);
                float sim_hit_r = sqrt(pow(sim_hit.x, 2) + pow(sim_hit.y, 2));
                float rec_hit_r = sqrt(pow(rec_hit.x, 2) + pow(rec_hit.y, 2));
                drs.push_back(sim_hit_r - rec_hit_r);
                etas.push_back(std::get<SimTrack>(matched_track).eta);
            }
            return std::make_pair(etas, drs);
        })));
    params = {"$\\eta$",         100,    -4,    4,
              "$\\Delta r$(cm)", 100, -.002, .002};
    tds.register_container<ContainerTH2Many<float>>("matched_track_drs_v_eta_F1",
                                                    fv::apply(calc_dr_v_eta, fv::tuple(matched_tracks_F1)),
                                                    "Matched Track $\\Delta r$ - F1", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_drs_v_eta_F2",
                                                    fv::apply(calc_dr_v_eta, fv::tuple(matched_tracks_F2)),
                                                    "Matched Track $\\Delta r$ - F2", params);
    tds.register_container<ContainerTH2Many<float>>("matched_track_drs_v_eta_F3",
                                                    fv::apply(calc_dr_v_eta, fv::tuple(matched_tracks_F3)),
                                                    "Matched Track $\\Delta r$ - F3", params);

}

void setup_ladder_vs_phi(TrackingDataSet& tds){
    auto& calc_phi_vs_ladder =
    func<pair_vec(vector<PixRecHit>, int)>("calc_phi_vs_ladder",
        FUNC(([](const vector<PixRecHit>& pixrec_hits, int layer){
            std::vector<float> phis;
            std::vector<float> ladders;
            for(const PixRecHit &pixrec_hit : pixrec_hits){
                if(pixrec_hit.det == PIXEL_BARREL && pixrec_hit.lay == layer){
                    float phi = pixrec_hit.phi();
                    unsigned short ladder = pixrec_hit.ladder_blade;
                    phis.push_back(phi);
                    ladders.push_back(ladder);
                    INFO(boost::format("Hit Phi: %7.3f , Hit detID: %8x, Ladder: %d")
                            % phi % pixrec_hit.detId % ladder);
                }
            }
            return std::make_pair(phis, ladders);
        })));

    TH2Params params = {"$\\phi$",         100,    -3.14159,    3.14159,
                        "Ladder Number", 50, 0, 50};
    tds.register_container<ContainerTH2Many<float>>("rechit_phi_vs_ladder_l1",
                                                    fv::apply(calc_phi_vs_ladder, fv::tuple(pixrec_hits, constant<int>("1",1))),
                                                    "phi vs ladder number - Layer 1", params);
    tds.register_container<ContainerTH2Many<float>>("rechit_phi_vs_ladder_l2",
                                                    fv::apply(calc_phi_vs_ladder, fv::tuple(pixrec_hits, constant<int>("2",2))),
                                                    "phi vs ladder number - Layer 2", params);
    tds.register_container<ContainerTH2Many<float>>("rechit_phi_vs_ladder_l3",
                                                    fv::apply(calc_phi_vs_ladder, fv::tuple(pixrec_hits, constant<int>("3",3))),
                                                    "phi vs ladder number - Layer 3", params);
}

void run_analysis(const vector<fv::util::DataFileDescriptor>& dfds, const string output_filename, bool silent){
    gSystem->Load("libfilval.so");
    auto replace_suffix = [](const std::string& input, const std::string& new_suffix){
        return input.substr(0, input.find_last_of(".")) + new_suffix;
    };

    string log_filename = replace_suffix(output_filename, ".log");
    fv::util::Log::init_logger(log_filename, fv::util::LogPriority::kLogInfo);

    TrackingDataSet tds(output_filename, dfds, "trackingNtuple/tree");
    /* tds.set_max_events(10); */
    register_objects(tds);

    /* setup_matched_tracks(tds); */
    /* setup_residuals(tds); */
    setup_first_hit_pairs(tds);

    /* setup_ladder_vs_phi(tds); */

    tds.process(silent);
    tds.save_all();
}

int main(int argc, char * argv[]){
    fv::util::ArgParser args(argc, argv);
    bool silent = args.cmdOptionExists("-s");
    string output_filename = args.cmdOptionExists("-o") ? args.getCmdOption("-o") : "output.root";
    if(args.cmdOptionExists("-F")){
        auto file_list = fv::util::read_input_list(args.getCmdOption("-F"));
        run_analysis(file_list, output_filename, silent);
    }else if(args.cmdOptionExists("-f")){
        string input_filename = args.getCmdOption("-f");
        string data_label = args.cmdOptionExists("-l") ? args.getCmdOption("-l") : "";
        fv::util::DataFileDescriptor dfd(input_filename, data_label);
        run_analysis(std::vector<fv::util::DataFileDescriptor>({dfd}), output_filename, silent);
    }else {
        cout << "Usage: ./tracking_validation (-s) {-o output_filename} -F datafiles.txt" << endl;
        cout << "       ./tracking_validation (-s) {-l DATA_LABEL} {-o output_filename} -f treefile.root" << endl;
    }
}