#include <iostream>
#include <vector>
#include <cmath>
#include <TSystem.h>

#include "filval.hpp"
#include "root_filval.hpp"

#include "analysis/TrackingNtupleObjs.hpp"

SeedCollection seeds;
SimTrackCollection sim_tracks;
SimVertexCollection sim_vertices;
TrackCollection gsf_tracks;

void register_objects(TrackingDataSet& tds){
    seeds.init(tds);
    sim_tracks.init(tds);
    sim_vertices.init(tds);
    gsf_tracks.init(tds);
}

struct {
    Value<float>* x;
    Value<float>* y;
    Value<float>* z;
    Value<float>* sigma_x;
    Value<float>* sigma_y;
    Value<float>* sigma_z;
} bs;

bool in_lum_region(const SimVertex& vertex) {
    /* Here approximate the luminous region as a cylinder with length 5*\sigma_z and radius
     * sqrt(\simga_x^2 + \sigma_y^2) centered as beamspot position.
     */
    float dx = vertex.x() - bs.x->get();
    float dy = vertex.y() - bs.y->get();
    float dz = vertex.z() - bs.z->get();
    auto radius = static_cast<float>(5 * sqrt(pow(bs.sigma_x->get(), 2) + pow(bs.sigma_y->get(), 2)));
    float half_len = 5*bs.sigma_z->get();
    return sqrt(dx*dx + dy*dy) < radius and abs(dz) < half_len;
};

bool is_good_sim(const SimTrack& sim_track) {
    const auto& vertex = sim_vertices[sim_track.parentVtxIdx()];
    return abs(sim_track.pdgId()) == 11 and in_lum_region(vertex);
};

float reco_energy_rel_err(const SimTrack& sim_track, const Seed& seed) {
    return (sim_track.pt() - seed.Et()) / sim_track.pt() ;
}

bool reco_energy_consistent(const SimTrack& sim_track, const Seed& seed, float consistency_cut=0.1) {
    return fabs(reco_energy_rel_err(sim_track, seed)) < consistency_cut;
}

void run(bool silent){
    using namespace std;
    using namespace fv;
    using namespace fv_root;
    auto file_list = the_config->get_source_files();
    string output_filename = the_config->get_output_filename();
    TrackingDataSet tds(output_filename, file_list, "trackingNtuple/tree");
    register_objects(tds);

    int x = 12;
    int y = 13;

    for (auto z : vector<pair<int,int>>{{x, x}, {y, y}}) {
        cout << z.first << endl;
    }

    bs = {
        tds.track_branch<float>("bsp_x"),
        tds.track_branch<float>("bsp_y"),
        tds.track_branch<float>("bsp_z"),
        tds.track_branch<float>("bsp_sigmax"),
        tds.track_branch<float>("bsp_sigmay"),
        tds.track_branch<float>("bsp_sigmaz")
    };

    std::map<std::tuple<int, int, bool>, ContainerTH2<float>*> BPIX_residuals_dRz;
    std::map<std::tuple<int, int, bool>, ContainerTH2<float>*> BPIX_residuals_dPhi;
    std::map<std::tuple<int, int, bool>, ContainerTH2<float>*> FPIX_residuals_dRz;
    std::map<std::tuple<int, int, bool>, ContainerTH2<float>*> FPIX_residuals_dPhi;

    int layer, hit, isFake;
    stringstream name;
    auto set_name = [&name, &layer, &hit, &isFake](const std::string& var, const std::string& region) {
        name.str("");
        name << var << "_" << region << "_L" << layer << "_H" << hit << "_v_Et";
        if (isFake)
            name << "_fake";

    };

    THParams hist_params;
    for (layer=1; layer<=4; layer++) {
        for (hit=1; hit<=3; hit++) {
            for (isFake=0; isFake<=1; isFake++) {
                hist_params = (hit == 1) ? THParams::lookup("dRz_v_Et") : THParams::lookup("dRz_v_Et_outer_hits");
                set_name("dRz", "BPIX");
                BPIX_residuals_dRz[{layer, hit, isFake}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
                set_name("dRz", "FPIX");
                FPIX_residuals_dRz[{layer, hit, isFake}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);

                hist_params = (hit == 1) ? THParams::lookup("dPhi_v_Et") : THParams::lookup("dPhi_v_Et_outer_hits");
                set_name("dPhi", "BPIX");
                BPIX_residuals_dPhi[{layer, hit, isFake}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
                set_name("dPhi", "FPIX");
                FPIX_residuals_dPhi[{layer, hit, isFake}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
            }
        }
    }

    std::map<int,vector<float>> eta_regions = {{1, {0.0, 1.1, 1.8, 3.0}},
                                               {2, {0.0, 1.4, 2.3, 3.0}},
                                               {3, {0.0, 1.0, 2.0, 3.0}}};

    auto get_region = [&eta_regions](int hit, float eta) {
        auto regions = eta_regions[hit];
        if (regions.size() <= 1) return 1;
        float abseta = abs(eta);
        for (int r_idx=1; r_idx < regions.size(); r_idx++) {
            if (abseta > regions[r_idx-1] and abseta < regions[r_idx]) {
                return r_idx;
            }
        }
        return static_cast<int>(regions.size());
    };
    std::map<int, ContainerTH2<float>*> dPhi_residuals_v_eta;
    std::map<int, ContainerTH2<float>*> dRz_residuals_v_eta;

    std::map<std::pair<int, int>, ContainerTH2<float>*> dPhi_residuals_v_region;
    std::map<std::pair<int, int>, ContainerTH2<float>*> dRz_residuals_v_region;
    for (int hit=1; hit<=3; hit++) {
        name.str("");
        name << "dPhi_residuals_v_eta_H" << hit;
        hist_params = (hit==1) ? THParams::lookup("dPhi_v_eta") : THParams::lookup("dPhi_v_eta_outer_hits");
        dPhi_residuals_v_eta[hit] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);

        name.str("");
        name << "dRz_residuals_v_eta_H" << hit;
        hist_params = (hit==1) ? THParams::lookup("dRz_v_eta") : THParams::lookup("dRz_v_eta_outer_hits");
        dRz_residuals_v_eta[hit] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
        for (int region=1; region<=eta_regions[hit].size()-1; region++){
            hist_params = (hit==1) ? THParams::lookup("dRz_v_Et") : THParams::lookup("dRz_v_Et_outer_hits");
            name.str("");
            name << "dRz_residuals_H"  << hit << "_R" << region;
            dRz_residuals_v_region[{hit, region}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);

            hist_params = (hit==1) ? THParams::lookup("dPhi_v_Et") : THParams::lookup("dPhi_v_Et_outer_hits");
            name.str("");
            name << "dPhi_residuals_H"  << hit << "_R" << region;
            dPhi_residuals_v_region[{hit, region}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
        }
    }

    auto& seed_eff_v_pt  = *tds.register_container<EfficiencyContainer<float>>("seed_eff_v_pt",  THParams::lookup("eff_v_pt"));
    auto& seed_eff_v_eta = *tds.register_container<EfficiencyContainer<float>>("seed_eff_v_eta", THParams::lookup("eff_v_eta"));
    auto& seed_eff_v_phi = *tds.register_container<EfficiencyContainer<float>>("seed_eff_v_phi", THParams::lookup("eff_v_phi"));
    auto& seed_eff_v_eta_pt  = *tds.register_container<EfficiencyContainer2D<float>>("seed_eff_v_eta_pt",  THParams::lookup("eff_v_eta_pt"));

    auto& tracking_eff_v_pt  = *tds.register_container<EfficiencyContainer<float>>("tracking_eff_v_pt",  THParams::lookup("eff_v_pt"));
    auto& tracking_eff_v_eta = *tds.register_container<EfficiencyContainer<float>>("tracking_eff_v_eta", THParams::lookup("eff_v_eta"));
    auto& tracking_eff_v_phi = *tds.register_container<EfficiencyContainer<float>>("tracking_eff_v_phi", THParams::lookup("eff_v_phi"));
    auto& tracking_eff_v_eta_pt  = *tds.register_container<EfficiencyContainer2D<float>>("tracking_eff_v_eta_pt",  THParams::lookup("eff_v_eta_pt"));

    auto& tracking_eff_v_pt2  = *tds.register_container<EfficiencyContainer<float>>("tracking_eff_v_pt2",  THParams::lookup("eff_v_pt"));
    auto& tracking_eff_v_eta2 = *tds.register_container<EfficiencyContainer<float>>("tracking_eff_v_eta2", THParams::lookup("eff_v_eta"));
    auto& tracking_eff_v_phi2 = *tds.register_container<EfficiencyContainer<float>>("tracking_eff_v_phi2", THParams::lookup("eff_v_phi"));

    auto& seed_pur_v_pt  = *tds.register_container<EfficiencyContainer<float>>("seed_pur_v_pt",  THParams::lookup("pur_v_pt"));
    auto& seed_pur_v_eta = *tds.register_container<EfficiencyContainer<float>>("seed_pur_v_eta", THParams::lookup("pur_v_eta"));
    auto& seed_pur_v_phi = *tds.register_container<EfficiencyContainer<float>>("seed_pur_v_phi", THParams::lookup("pur_v_phi"));

    auto& tracking_pur_v_pt  = *tds.register_container<EfficiencyContainer<float>>("tracking_pur_v_pt",  THParams::lookup("pur_v_pt"));
    auto& tracking_pur_v_eta = *tds.register_container<EfficiencyContainer<float>>("tracking_pur_v_eta", THParams::lookup("pur_v_eta"));
    auto& tracking_pur_v_phi = *tds.register_container<EfficiencyContainer<float>>("tracking_pur_v_phi", THParams::lookup("pur_v_phi"));

    auto& tracking_pur_v_pt2  = *tds.register_container<EfficiencyContainer<float>>("tracking_pur_v_pt2",  THParams::lookup("pur_v_pt"));
    auto& tracking_pur_v_eta2 = *tds.register_container<EfficiencyContainer<float>>("tracking_pur_v_eta2", THParams::lookup("pur_v_eta"));
    auto& tracking_pur_v_phi2 = *tds.register_container<EfficiencyContainer<float>>("tracking_pur_v_phi2", THParams::lookup("pur_v_phi"));



    auto& hit_vs_layer_barrel = *tds.register_container<ContainerTH2<int>>("hit_vs_layer_barrel", THParams::lookup("hit_vs_layer"));
    auto& hit_vs_layer_forward = *tds.register_container<ContainerTH2<int>>("hit_vs_layer_forward", THParams::lookup("hit_vs_layer"));

    auto& gsf_tracks_nmatch_sim_tracks = *tds.register_container<ContainerTH1<size_t>>("gsf_tracks_nmatch_sim_tracks ", THParams::lookup("gsf_tracks_nmatch_sim_tracks"));


    auto& ecal_energy_resolution = *tds.register_container<ContainerTH1<float>>("ecal_energy_resolution ", THParams::lookup("ecal_energy_resolution"));

    while (tds.next(!silent)) {

        for (const auto& sim_track : sim_tracks) {
            if (!is_good_sim(sim_track)) continue;
            if (seeds.size() == 0) continue;
            for (const auto &seed_idx : sim_track.seedIdx()) {
                const Seed& seed = seeds[seed_idx];
                if (not seed.isECALDriven()) continue;
                ecal_energy_resolution.fill(reco_energy_rel_err(sim_track, seed));
            }
        }

        // Seeding Efficiency
        for (const auto& sim_track : sim_tracks) {
            if (!is_good_sim(sim_track)) continue;
            if (seeds.size() == 0) continue;
            bool matched = false;
            for (const auto& seed_idx : sim_track.seedIdx()) {
                const Seed& seed = seeds[seed_idx];
                if (seed.isECALDriven()) {
                    matched=true;
                    break;
                }
            }
            seed_eff_v_eta_pt.fill(sim_track.eta(), sim_track.pt(), matched);
            if (abs(sim_track.eta()) < 2.4)
                seed_eff_v_pt.fill(sim_track.pt(), matched);
            if (sim_track.pt() > 20.0)
                seed_eff_v_eta.fill(sim_track.eta(), matched);
            if (abs(sim_track.eta()) < 2.4 and sim_track.pt() > 20)
                seed_eff_v_phi.fill(sim_track.phi(), matched);
        }

        // Tracking Efficiency
        for (const auto& sim_track : sim_tracks) {
            if (!is_good_sim(sim_track)) continue;
            if (gsf_tracks.size() == 0) continue;
            bool matched = false;
            for (const auto& track_idx : sim_track.trkIdx()) {
                const Seed& seed = seeds[gsf_tracks[track_idx].seedIdx()];
                if (seed.isECALDriven()) {
                    matched=true;
                    break;
                }
            }
            tracking_eff_v_eta_pt.fill(sim_track.eta(), sim_track.pt(), matched);
            if (abs(sim_track.eta()) < 2.4)
                tracking_eff_v_pt.fill(sim_track.pt(), matched);
            if (sim_track.pt() > 20.0)
                tracking_eff_v_eta.fill(sim_track.eta(), matched);
            if (abs(sim_track.eta()) < 2.4 and sim_track.pt() > 20)
                tracking_eff_v_phi.fill(sim_track.phi(), matched);



            matched = false;
            for (const auto& seed_idx : sim_track.seedIdx()) {
                const Seed& seed = seeds[seed_idx];
                if (seed.isECALDriven() and seed.trkIdx()>=0 and reco_energy_consistent(sim_track, seed)) {
                    matched=true;
                    break;
                }
            }
            if (abs(sim_track.eta()) < 2.4)
                tracking_eff_v_pt2.fill(sim_track.pt(), matched);
            if (sim_track.pt() > 20.0)
                tracking_eff_v_eta2.fill(sim_track.eta(), matched);
            if (abs(sim_track.eta()) < 2.4 and sim_track.pt() > 20)
                tracking_eff_v_phi2.fill(sim_track.phi(), matched);
        }

        // Seeding Purity
        for (const auto& seed : seeds) {
            if (!seed.isECALDriven()) continue;
            bool match = false;
            for (const auto& sim_track_idx : seed.simTrkIdx()) {
                if(is_good_sim(sim_tracks[sim_track_idx])) {
                    match = true;
                    break;
                }
            }
            if (abs(seed.eta()) < 2.4)
                seed_pur_v_pt.fill(seed.pt(), match);
            if (seed.pt() > 20)
                seed_pur_v_eta.fill(seed.eta(), match);
            if (abs(seed.eta()) < 2.4 and seed.pt() > 20)
                seed_pur_v_phi.fill(seed.phi(), match);
        }

        // Tracking Purity
        for (const auto& gsf_track : gsf_tracks) {
            gsf_tracks_nmatch_sim_tracks.fill(gsf_track.simTrkIdx().size());
            bool match = false;
            for (const auto& sim_track_idx : gsf_track.simTrkIdx()) {
                if (is_good_sim(sim_tracks[sim_track_idx])) {
                    match = true;
                    break;
                }
            }
            if (abs(gsf_track.eta()) < 2.4)
                tracking_pur_v_pt.fill(gsf_track.pt(), match);
            if (gsf_track.pt() > 20)
                tracking_pur_v_eta.fill(gsf_track.eta(), match);
            if (abs(gsf_track.eta()) < 2.4 and gsf_track.pt() > 20)
                tracking_pur_v_phi.fill(gsf_track.phi(), match);

            match = false;
            for (const auto& sim_track_idx : seeds[gsf_track.seedIdx()].simTrkIdx()) {
                if (is_good_sim(sim_tracks[sim_track_idx])) {
                    match = true;
                    break;
                }
            }
            if (abs(gsf_track.eta()) < 2.4)
                tracking_pur_v_pt2.fill(gsf_track.pt(), match);
            if (gsf_track.pt() > 20)
                tracking_pur_v_eta2.fill(gsf_track.eta(), match);
            if (abs(gsf_track.eta()) < 2.4 and gsf_track.pt() > 20)
                tracking_pur_v_phi2.fill(gsf_track.phi(), match);
        }

        // Hit Residuals
        for (const auto& seed : seeds) {
            if (seed.trkIdx() < 0) continue;  // require that seed produced gsf-track
            if (!seed.isECALDriven()) continue;
            bool is_sim_matched = false;
            for (const auto& sim_track_idx : seed.simTrkIdx()) {
                const SimTrack& sim_track = sim_tracks[sim_track_idx];
                if (is_good_sim(sim_track) and reco_energy_consistent(sim_track, seed)) {
                    is_sim_matched = true;
                    break;
//                    std::cout << "Matched to non-electron: " << sim_tracks[sim_track_idx].pdgId()
//                              << " (" << seed.simTrkIdx().size() << ")" << std::endl;
                }
            }
            const auto the_track = gsf_tracks[seed.trkIdx()];
            vector<int>   hits_valid;
            vector<float> hits_dRz;
            vector<float> hits_dPhi;
            if (the_track.q() == 1) {
                hits_valid = seed.isValidPos();
                hits_dRz   = seed.dRZPos();
                hits_dPhi  = seed.dPhiPos();
            } else {
                hits_valid = seed.isValidNeg();
                hits_dRz   = seed.dRZNeg();
                hits_dPhi  = seed.dPhiNeg();
            }
            const vector<int>& hits_isBarrel = seed.isBarrel();
            const vector<int>& hits_layerOrDiskNr = seed.layerOrDiskNr();

            size_t nHits = hits_valid.size();
            for (size_t hit_idx=0; hit_idx<nHits; hit_idx++) {
                if (!hits_valid[hit_idx]) continue;
                bool isBarrel = hits_isBarrel[hit_idx] == 1;
                int layerOrDiskNr = hits_layerOrDiskNr[hit_idx];
                float dRz = abs(hits_dRz[hit_idx]);
                float dPhi = abs(hits_dPhi[hit_idx]);
                int eta_region = get_region(static_cast<int>(hit_idx + 1), seed.eta());


                if (is_sim_matched) {
                    dRz_residuals_v_eta[hit_idx + 1]->fill(dRz, abs(seed.eta()));
                    dPhi_residuals_v_eta[hit_idx + 1]->fill(dPhi, abs(seed.eta()));

                    dRz_residuals_v_region[{hit_idx + 1, eta_region}]->fill(dRz, seed.Et());
                    dPhi_residuals_v_region[{hit_idx + 1, eta_region}]->fill(dPhi, seed.Et());

                }

                if (layerOrDiskNr == -1) continue;  // subDet is not set w/ old seeding

                if (isBarrel)
                    hit_vs_layer_barrel.fill(layerOrDiskNr, static_cast<const int &>(hit_idx + 1));
                else
                    hit_vs_layer_forward.fill(layerOrDiskNr, static_cast<const int &>(hit_idx + 1));

                if (isBarrel) {
                    BPIX_residuals_dRz[{layerOrDiskNr, hit_idx+1, !is_sim_matched}]->fill(dRz, seed.Et());
                    BPIX_residuals_dPhi[{layerOrDiskNr, hit_idx+1, !is_sim_matched}]->fill(dPhi, seed.Et());
                } else {
                    FPIX_residuals_dRz[{layerOrDiskNr, hit_idx+1, !is_sim_matched}]->fill(dRz, seed.Et());
                    FPIX_residuals_dPhi[{layerOrDiskNr, hit_idx+1, !is_sim_matched}]->fill(dPhi, seed.Et());
                }
            }
        }
    }

    tds.save_all();
}

int main(int argc, char * argv[]){
    using namespace fv_util;
    ArgParser args(argc, argv);
    bool silent = args.cmd_option_exists("-s");
    if(args.cmd_option_exists("-c")) {
        init_config(args.get_cmd_option("-c"));
        args.update_config();
        init_log(LogPriority::kLogInfo);
//        gSystem->Load("libfilval.so");
        run(silent);
    } else {
        cout << "Usage: ./" << argv[0] << " (-s) -c config_file.yaml" << endl;
    }
    return 0;
}