#include "WCPData/SlimMergeGeomCell.h"
#include "WCPData/Singleton.h"
#include "WCPData/TPCParams.h"

#include "TVector3.h"

using namespace WCP;

WCP::SlimMergeGeomCell::SlimMergeGeomCell(int ident)
  : time_slice(-1)
  , uq(0)
  , udq(0)
  , vq(0)
  , vdq(0)
  , wq(0)
  , wdq(0)
  , q(0)
{
    _ident = ident;
}

WCP::SlimMergeGeomCell::~SlimMergeGeomCell()
{
    uwires.clear();
    vwires.clear();
    wwires.clear();
    sample_points.clear();
    sample_points_wires.clear();
    bad_planes.clear();
    wirechargemap.clear();
    wirechargeerrmap.clear();
}

// Point SlimMergeGeomCell::get_sampling_points_center(){

// }

Point SlimMergeGeomCell::center() const
{
    if (flag_center == 0) {
        ret.x = 0;
        ret.y = 0;
        ret.z = 0;
        const size_t npoints = sample_points.size();
        for (size_t ipoint = 0; ipoint < npoints; ++ipoint) {
            const Point& point = sample_points[ipoint];
            ret.x += point.x;
            ret.y += point.y;
            ret.z += point.z;
            // std::cout << "qx1 " << point.y << " " << ret.y << std::endl;
        }

        ret.x /= npoints;
        ret.y /= npoints;
        ret.z /= npoints;
        flag_center = 1;
    }

    return ret;
}

bool SlimMergeGeomCell::IsSame(SlimMergeGeomCell* mcell1)
{
    GeomWireSelection& mcell1_uwires = mcell1->get_uwires();
    GeomWireSelection& mcell1_vwires = mcell1->get_vwires();
    GeomWireSelection& mcell1_wwires = mcell1->get_wwires();

    if (uwires.size() != mcell1_uwires.size() || vwires.size() != mcell1_vwires.size() ||
        wwires.size() != mcell1_wwires.size()) {
        return false;
    }

    for (size_t i = 0; i != uwires.size(); i++) {
        if (uwires.at(i) != mcell1_uwires.at(i)) return false;
    }
    for (size_t i = 0; i != vwires.size(); i++) {
        if (vwires.at(i) != mcell1_vwires.at(i)) return false;
    }
    for (size_t i = 0; i != wwires.size(); i++) {
        if (wwires.at(i) != mcell1_wwires.at(i)) return false;
    }

    return true;
}

float WCP::SlimMergeGeomCell::Estimate_minimum_charge()
{
    float u_charge = 0;
    float v_charge = 0;
    float w_charge = 0;

    float min_charge = 1e9;

    bool flag_u, flag_v, flag_w;

    if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(0)) == bad_planes.end()) {
        for (auto it = uwires.begin(); it != uwires.end(); it++) {
            u_charge += wirechargemap[*it];
        }
        flag_u = true;
    }
    else {
        flag_u = false;
    }
    if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(1)) == bad_planes.end()) {
        for (auto it = vwires.begin(); it != vwires.end(); it++) {
            v_charge += wirechargemap[*it];
        }
        flag_v = true;
    }
    else {
        flag_v = false;
    }
    if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(2)) == bad_planes.end()) {
        for (auto it = wwires.begin(); it != wwires.end(); it++) {
            w_charge += wirechargemap[*it];
        }
        flag_w = true;
    }
    else {
        flag_w = false;
    }
    if (flag_u && u_charge < min_charge) min_charge = u_charge;
    if (flag_v && v_charge < min_charge) min_charge = v_charge;
    if (flag_w && w_charge < min_charge) min_charge = w_charge;

    return min_charge;
}

float WCP::SlimMergeGeomCell::Estimate_total_charge()
{
    float total_charge = 0;
    float count = 0;
    if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(0)) == bad_planes.end()) {
        for (auto it = uwires.begin(); it != uwires.end(); it++) {
            total_charge += wirechargemap[*it];
        }
        count++;
    }
    if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(1)) == bad_planes.end()) {
        for (auto it = vwires.begin(); it != vwires.end(); it++) {
            total_charge += wirechargemap[*it];
        }
        count++;
    }
    if (find(bad_planes.begin(), bad_planes.end(), WirePlaneType_t(2)) == bad_planes.end()) {
        for (auto it = wwires.begin(); it != wwires.end(); it++) {
            total_charge += wirechargemap[*it];
        }
        count++;
    }

    total_charge /= count;

    return total_charge;
}

float WCP::SlimMergeGeomCell::Get_Wire_Charge(const GeomWire* wire)
{
    if (wirechargemap.find(wire) != wirechargemap.end()) {
        return wirechargemap[wire];
    }
    else {
        return 0;
    }
}

float WCP::SlimMergeGeomCell::Get_Wire_Charge_Err(const GeomWire* wire)
{
    if (wirechargeerrmap.find(wire) != wirechargeerrmap.end()) {
        return wirechargeerrmap[wire];
    }
    else {
        return 0;
    }
}

void WCP::SlimMergeGeomCell::add_bad_planes(WirePlaneType_t plane)
{
    if (find(bad_planes.begin(), bad_planes.end(), plane) != bad_planes.end()) {
    }
    else {
        bad_planes.push_back(plane);
    }
}

void WCP::SlimMergeGeomCell::AddBoundary(const PointVector& boundary)
{
    _boundary = boundary;
    flag_center = 0;
    order_boundary();
}

void WCP::SlimMergeGeomCell::AddSamplingPoints(const PointVector& sampling_points) { sample_points = sampling_points; }

void WCP::SlimMergeGeomCell::AddSamplingPointsWires(std::vector<std::tuple<int, int, int>>& sampling_points_wires)
{
    sample_points_wires = sampling_points_wires;
}

void WCP::SlimMergeGeomCell::SetMaxWireInterval(WirePlaneType_t type, int value)
{
    max_wire_interval = value;
    max_wire_type = type;
}

void WCP::SlimMergeGeomCell::SetMinWireInterval(WirePlaneType_t type, int value)
{
    min_wire_interval = value;
    min_wire_type = type;
}

void WCP::SlimMergeGeomCell::AddWire(const GeomWire* wire, WirePlaneType_t plane, float charge, float charge_err)
{
    if (plane == WirePlaneType_t(0)) {
        if (find(uwires.begin(), uwires.end(), wire) == uwires.end()) {
            uwires.push_back(wire);
            wirechargemap[wire] = charge;
            wirechargeerrmap[wire] = charge_err;
            map_index_uwire[wire->index()] = wire;
        }
    }
    else if (plane == WirePlaneType_t(1)) {
        if (find(vwires.begin(), vwires.end(), wire) == vwires.end()) {
            vwires.push_back(wire);
            wirechargemap[wire] = charge;
            wirechargeerrmap[wire] = charge_err;
            map_index_vwire[wire->index()] = wire;
        }
    }
    else if (plane == WirePlaneType_t(2)) {
        if (find(wwires.begin(), wwires.end(), wire) == wwires.end()) {
            wwires.push_back(wire);
            wirechargemap[wire] = charge;
            wirechargeerrmap[wire] = charge_err;
            map_index_wwire[wire->index()] = wire;
        }
    }
}

void WCP::SlimMergeGeomCell::OrderWires()
{
    WCP::sort_by_ident(uwires);
    WCP::sort_by_ident(vwires);
    WCP::sort_by_ident(wwires);
}

bool WCP::SlimMergeGeomCell::Overlap(WCP::SlimMergeGeomCell* cell, float num)
{
    double cut_limit = 1.2;

    int flag_u = 0;

    TVector3 dir_x(1, 0, 0);
    TVector3 dir_u(uwires.at(0)->point1().x - uwires.at(0)->point2().x,
                   uwires.at(0)->point1().y - uwires.at(0)->point2().y,
                   uwires.at(0)->point1().z - uwires.at(0)->point2().z);
    TVector3 dir_v(vwires.at(0)->point1().x - vwires.at(0)->point2().x,
                   vwires.at(0)->point1().y - vwires.at(0)->point2().y,
                   vwires.at(0)->point1().z - vwires.at(0)->point2().z);
    TVector3 dir_w(wwires.at(0)->point1().x - wwires.at(0)->point2().x,
                   wwires.at(0)->point1().y - wwires.at(0)->point2().y,
                   wwires.at(0)->point1().z - wwires.at(0)->point2().z);

    TVector3 dir_up = dir_x.Cross(dir_u);
    TVector3 dir_vp = dir_x.Cross(dir_v);
    TVector3 dir_wp = dir_x.Cross(dir_w);

    dir_up *= 1. / dir_up.Mag();
    dir_vp *= 1. / dir_vp.Mag();
    dir_wp *= 1. / dir_wp.Mag();

    for (int i = 0; i != cell->get_uwires().size(); i++) {
        //	auto it = find(uwires.begin(),uwires.end(),cell.get_uwires().at(i));
        // if (it != uwires.end()){
        //}
        for (int j = 0; j != uwires.size(); j++) {
            TVector3 dir(cell->get_uwires().at(i)->point1().x - uwires.at(j)->point1().x,
                         cell->get_uwires().at(i)->point1().y - uwires.at(j)->point1().y,
                         cell->get_uwires().at(i)->point1().z - uwires.at(j)->point1().z);
            float dis = fabs(dir.Dot(dir_up));
            if (dis < Singleton<TPCParams>::Instance().get_pitch() * cut_limit) {
                flag_u = 1;
                break;
            }
        }
        if (flag_u == 1) break;
    }
    if (flag_u == 0) return false;

    int flag_v = 0;
    for (int i = 0; i != cell->get_vwires().size(); i++) {
        // auto it = find(vwires.begin(),vwires.end(),cell.get_vwires().at(i));
        // if (it != vwires.end()){
        //   }
        for (int j = 0; j != vwires.size(); j++) {
            TVector3 dir(cell->get_vwires().at(i)->point1().x - vwires.at(j)->point1().x,
                         cell->get_vwires().at(i)->point1().y - vwires.at(j)->point1().y,
                         cell->get_vwires().at(i)->point1().z - vwires.at(j)->point1().z);
            float dis = fabs(dir.Dot(dir_vp));
            if (dis < Singleton<TPCParams>::Instance().get_pitch() * cut_limit) {
                flag_v = 1;
                break;
            }
        }
        if (flag_v == 1) break;
    }
    if (flag_v == 0) return false;

    int flag_w = 0;
    for (int i = 0; i != cell->get_wwires().size(); i++) {
        // auto it = find(wwires.begin(),wwires.end(),cell.get_wwires().at(i));
        // if (it != wwires.end()){
        //   }
        for (int j = 0; j != wwires.size(); j++) {
            TVector3 dir(cell->get_wwires().at(i)->point1().x - wwires.at(j)->point1().x,
                         cell->get_wwires().at(i)->point1().y - wwires.at(j)->point1().y,
                         cell->get_wwires().at(i)->point1().z - wwires.at(j)->point1().z);
            float dis = fabs(dir.Dot(dir_wp));
            if (dis < Singleton<TPCParams>::Instance().get_pitch() * cut_limit) {
                flag_w = 1;
                break;
            }
        }
        if (flag_w == 1) break;
    }
    if (flag_w == 0) return false;

    // if (flag_u == 1 && flag_v == 1 && flag_w == 1){
    if (flag_u + flag_v + flag_w == 3) {
        return true;
    }

    return false;
}

bool WCP::SlimMergeGeomCell::Overlap_fast(WCP::SlimMergeGeomCell* cell, int offset)
{
    // std::cout << uwires.size() << " " << vwires.size() << " " << wwires.size() << " "
    // 	    << cell->get_uwires().size() << " " << cell->get_vwires().size() << " " << cell->get_wwires().size() <<
    // std::endl;

    int u_low_index = uwires.front()->index();
    int u_high_index = uwires.back()->index();

    int v_low_index = vwires.front()->index();
    int v_high_index = vwires.back()->index();

    int w_low_index = wwires.front()->index();
    int w_high_index = wwires.back()->index();

    int u1_low_index = cell->get_uwires().front()->index();
    int u1_high_index = cell->get_uwires().back()->index();

    int v1_low_index = cell->get_vwires().front()->index();
    int v1_high_index = cell->get_vwires().back()->index();

    int w1_low_index = cell->get_wwires().front()->index();
    int w1_high_index = cell->get_wwires().back()->index();

    // std::cout << u_low_index << " " << u_high_index << " " << u1_low_index << " " << u1_high_index << " " <<
    // v_low_index << " " << v_high_index << " " << v1_low_index << " " << v1_high_index << " " << w_low_index << " " <<
    // w_high_index << " " << w1_low_index << " " << w1_high_index << " " << std::endl;

    if (u_low_index > u1_high_index + offset || u1_low_index > u_high_index + offset) return false;
    if (v_low_index > v1_high_index + offset || v1_low_index > v_high_index + offset) return false;
    if (w_low_index > w1_high_index + offset || w1_low_index > w_high_index + offset) return false;

    return true;
}

bool WCP::SlimMergeGeomCell::IsPointGood(int index_u, int index_v, int index_w, int ncut)
{
    float charge_u = 0;
    float charge_v = 0;
    float charge_w = 0;
    if (map_index_uwire.find(index_u) != map_index_uwire.end()) {
        charge_u = wirechargemap[map_index_uwire[index_u]];
    }
    if (map_index_vwire.find(index_v) != map_index_vwire.end()) {
        charge_v = wirechargemap[map_index_vwire[index_v]];
    }
    if (map_index_wwire.find(index_w) != map_index_wwire.end()) {
        charge_w = wirechargemap[map_index_wwire[index_w]];
    }
    int ncount = 0;
    if (charge_u > 10) ncount++;
    if (charge_v > 10) ncount++;
    if (charge_w > 10) ncount++;

    if (ncount >= ncut) return true;
    return false;
}

bool WCP::SlimMergeGeomCell::Adjacent(WCP::SlimMergeGeomCell* cell)
{
    int u_low_index = uwires.front()->index();
    int u_high_index = uwires.back()->index();

    int v_low_index = vwires.front()->index();
    int v_high_index = vwires.back()->index();

    int w_low_index = wwires.front()->index();
    int w_high_index = wwires.back()->index();

    int u1_low_index = cell->get_uwires().front()->index();
    int u1_high_index = cell->get_uwires().back()->index();

    int v1_low_index = cell->get_vwires().front()->index();
    int v1_high_index = cell->get_vwires().back()->index();

    int w1_low_index = cell->get_wwires().front()->index();
    int w1_high_index = cell->get_wwires().back()->index();

    int u_score = 0;
    int v_score = 0;
    int w_score = 0;

    if (u_low_index == u1_high_index + 1 || u1_low_index == u_high_index + 1) {
        u_score = 1;
    }
    else if (u_low_index <= u1_high_index && u1_low_index <= u_high_index) {
        u_score = 2;
    }

    if (u_score == 0) return false;

    if (v_low_index == v1_high_index + 1 || v1_low_index == v_high_index + 1) {
        v_score = 1;
    }
    else if (v_low_index <= v1_high_index && v1_low_index <= v_high_index) {
        v_score = 2;
    }

    if (v_score == 0) return false;

    if (w_low_index == w1_high_index + 1 || w1_low_index == w_high_index + 1) {
        w_score = 1;
    }
    else if (w_low_index <= w1_high_index && w1_low_index <= w_high_index) {
        w_score = 2;
    }

    if (w_score == 0) return false;

    if (u_score + v_score + w_score >= 5) {
        return true;
    }
    else {
        return false;
    }
}
