#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);

    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::pair<int, int>, ContainerTH2<float>*> BPIX_residuals_dRz;
    std::map<std::pair<int, int>, ContainerTH2<float>*> BPIX_residuals_dPhi;
    std::map<std::pair<int, int>, ContainerTH2<float>*> FPIX_residuals_dRz;
    std::map<std::pair<int, int>, ContainerTH2<float>*> FPIX_residuals_dPhi;
    stringstream name;
    THParams hist_params;
    for (int layer=1; layer<=4; layer++) {
        for (int hit=1; hit<=3; hit++) {
            hist_params = (hit==1) ? THParams::lookup("dRz_v_Et") : THParams::lookup("dRz_v_Et_outer_hits");
            name.str("");
            name << "dRz_BPIX_L" << layer << "_H" << hit << "_v_Et";
            BPIX_residuals_dRz[{layer,hit}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
            name.str("");
            name << "dRz_FPIX_L" << layer << "_H" << hit << "_v_Et";
            FPIX_residuals_dRz[{layer,hit}] = 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_BPIX_L" << layer << "_H" << hit << "_v_Et";
            BPIX_residuals_dPhi[{layer,hit}] = tds.register_container<ContainerTH2<float>>(name.str(), hist_params);
            name.str("");
            name << "dPhi_FPIX_L" << layer << "_H" << hit << "_v_Et";
            FPIX_residuals_dPhi[{layer,hit}] = 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& gsf_track : gsf_tracks) {
//            cout << "GSF Track " << gsf_track.idx << endl;
//            long n_matched_sim = gsf_track.simTrkIdx().size();
//            for (int idx=0; idx<n_matched_sim; idx++) {
//                int sim_trk_idx = gsf_track.simTrkIdx()[idx];
//                int sim_trk_share = gsf_track.shareFrac()[idx];
//                cout << "  " <<
//                cout << "  " << sim_tracks[sim_trk_idx].
//            }
//        }

//        ecal_energy_resolution.fill(0.0);
        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;
//                cout << "trk_idx: " << sim_track.idx << " seed_idx: " << seed_idx << " err: " << reco_energy_rel_err(sim_track, seed) << endl;
                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;
                }
            }
            if (!is_sim_matched) continue;
            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]);


                if (isBarrel) {
                    BPIX_residuals_dRz[{layerOrDiskNr, hit_idx+1}]->fill(dRz, seed.Et());
                    BPIX_residuals_dPhi[{layerOrDiskNr, hit_idx+1}]->fill(dPhi, seed.Et());
                } else {
                    FPIX_residuals_dRz[{layerOrDiskNr, hit_idx+1}]->fill(dRz, seed.Et());
                    FPIX_residuals_dPhi[{layerOrDiskNr, hit_idx+1}]->fill(dPhi, seed.Et());
                }
                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));
            }
        }
    }

    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;
}