#include "WCP2dToy/ToyMatrixIterate_SingleWire.h"

using namespace WCP;

WCP2dToy::ToyMatrixIterate_SingleWire::ToyMatrixIterate_SingleWire(
    WCP2dToy::ToyMatrix *toybefore, WCP2dToy::ToyMatrix *toycur, WCP2dToy::ToyMatrix *toyafter,
    WCP2dToy::MergeToyTiling *mergebefore, WCP2dToy::MergeToyTiling *mergecur, WCP2dToy::MergeToyTiling *mergeafter,
    int recon_t, float limit, double penalty, double penalty_ncpt)
  : toymatrix(*toycur)
  , mergetiling(mergecur)
  , limit(limit)
  , penalty_ncpt(penalty_ncpt)
{
    int recon_threshold = recon_t;

    // find the penalties for the current set ...
    GeomCellSelection allmcell_p;
    if (mergebefore != 0) {
        allmcell_p = mergebefore->get_allcell();
    }
    GeomCellSelection allmcell_c = mergecur->get_allcell();
    GeomCellSelection allmcell_n;
    if (mergeafter != 0) {
        allmcell_n = mergeafter->get_allcell();
    }

    for (int i = 0; i != allmcell_c.size(); i++) {
        MergeGeomCell *mcell_c = (MergeGeomCell *) allmcell_c[i];
        int index_c = toycur->Get_mcindex(mcell_c);
        cell_penal[index_c] = 0;

        int flag_before = 0;
        if (toybefore != 0) {
            if (toybefore->Get_Solve_Flag() != 0) {
                for (int j = 0; j != allmcell_p.size(); j++) {
                    MergeGeomCell *mcell_p = (MergeGeomCell *) allmcell_p[j];
                    double charge = toybefore->Get_Cell_Charge(mcell_p, 1);
                    if (charge > recon_threshold && mcell_c->Overlap(*mcell_p)) {
                        flag_before = 1;
                        break;
                    }
                }
            }
        }
        if (flag_before == 1) {
            cell_penal[index_c] += penalty;
        }

        int flag_after = 0;
        if (toyafter != 0) {
            if (toyafter->Get_Solve_Flag() != 0) {
                for (int j = 0; j != allmcell_n.size(); j++) {
                    MergeGeomCell *mcell_n = (MergeGeomCell *) allmcell_n[j];
                    double charge = toyafter->Get_Cell_Charge(mcell_n, 1);
                    if (charge > recon_threshold && mcell_c->Overlap(*mcell_n)) {
                        flag_after = 1;
                        break;
                    }
                }
            }
        }
        if (flag_after == 1) {
            cell_penal[index_c] += penalty;
        }
    }

    // fill in the cell_connect map;
    GeomCellCellsMap &cells_map = mergecur->get_not_compatible_cells_map();
    GeomCellSelection cells = mergecur->get_allcell();
    for (int i = 0; i != cells.size(); i++) {
        MergeGeomCell *mcell = (MergeGeomCell *) cells.at(i);
        int index = toycur->Get_mcindex(mcell);
        if (cells_map[mcell].size() > 0) {
            std::vector<int> cells_indices;
            for (int j = 0; j != cells_map[mcell].size(); j++) {
                int index_c = toycur->Get_mcindex(cells_map[mcell].at(j));
                cells_indices.push_back(index_c);
            }
            cells_ncpt[index] = cells_indices;
        }
    }

    // std::cout << "Check " << cells_ncpt.size() << std::endl;

    // figure out single-wire cell, and no need to remove these
    WCP::GeomCellSelection all_cells = mergetiling->get_allcell();
    WCP::GeomCellSelection single_wire_cells = mergetiling->get_single_wire_cells();

    wirechargemap = mergetiling->wcmap();

    cells.clear();
    for (int i = 0; i != all_cells.size(); i++) {
        auto it = find(single_wire_cells.begin(), single_wire_cells.end(), all_cells.at(i));
        if (it == single_wire_cells.end()) cells.push_back(all_cells.at(i));
    }
    GeomCellMap cellmap = mergetiling->cmap();
    GeomWireMap wiremap = mergetiling->wmap();

    //  std::cout << "Test: " << cells.size() << std::endl;
    ncount = 0;
    nlevel = 0;
    if (cells.size() != 0) {
        GeomCellSelection tried_cell;
        Iterate(cells, single_wire_cells, tried_cell, cellmap, wiremap);
    }
    else {
        std::vector<int> already_removed;
        std::vector<int> no_need_remove;

        for (int i = 0; i != mergetiling->get_allcell().size(); i++) {
            MergeGeomCell *mcell = (MergeGeomCell *) mergetiling->get_allcell().at(i);
            int index = toymatrix.Get_mcindex(mcell);
            no_need_remove.push_back(index);
        }

        double chi2_p = 0;
        for (int j = 0; j != already_removed.size(); j++) {
            chi2_p += cell_penal[already_removed.at(j)];
        }

        if (penalty_ncpt > 0) {
            // add the penalty due to not compatible cells (ncpt)
            // int flag_test = 0;
            for (auto it = cells_ncpt.begin(); it != cells_ncpt.end(); it++) {
                int index1 = it->first;
                if (find(already_removed.begin(), already_removed.end(), index1) != already_removed.end()) continue;
                std::vector<int> indices = it->second;

                for (int j = 0; j != indices.size(); j++) {
                    int index2 = indices.at(j);
                    if (find(already_removed.begin(), already_removed.end(), index2) == already_removed.end()) {
                        chi2_p += penalty_ncpt;
                        // flag_test = 1;
                        // break;
                    }
                }
                // if (flag_test ==1) break;
            }
            // if (flag_test == 1){
            //   chi2_p += penalty_ncpt;
            // }
        }

        WCP2dToy::ToyMatrixKalman toykalman(already_removed, no_need_remove, toymatrix, 0, 0, chi2_p);
        // std::cout << "Test: " << cellmap.size() << " " << wiremap.size() << " " << toykalman.Get_numz() << " " <<
        // ncount << std::endl;
    }
}

WCP2dToy::ToyMatrixIterate_SingleWire::ToyMatrixIterate_SingleWire(WCP2dToy::ToyMatrix &toymatrix,
                                                                   WCP2dToy::MergeToyTiling *mergetiling)
  : toymatrix(toymatrix)
  , mergetiling(mergetiling)
  , penalty_ncpt(0)
{
    for (int i = 0; i != toymatrix.Get_mcindex(); i++) {
        cell_penal[i] = 0;
    }
    limit = 1e5;

    // figure out single-wire cell, and no need to remove these
    WCP::GeomCellSelection all_cells = mergetiling->get_allcell();
    WCP::GeomCellSelection single_wire_cells = mergetiling->get_single_wire_cells();

    wirechargemap = mergetiling->wcmap();

    GeomCellSelection cells;
    for (int i = 0; i != all_cells.size(); i++) {
        auto it = find(single_wire_cells.begin(), single_wire_cells.end(), all_cells.at(i));
        if (it == single_wire_cells.end()) cells.push_back(all_cells.at(i));
    }
    GeomCellMap cellmap = mergetiling->cmap();
    GeomWireMap wiremap = mergetiling->wmap();

    //  std::cout << "Test: " << cells.size() << std::endl;
    ncount = 0;
    nlevel = 0;
    if (cells.size() != 0) {
        GeomCellSelection tried_cell;
        Iterate(cells, single_wire_cells, tried_cell, cellmap, wiremap);
    }
    else {
        std::vector<int> already_removed;
        std::vector<int> no_need_remove;

        for (int i = 0; i != mergetiling->get_allcell().size(); i++) {
            MergeGeomCell *mcell = (MergeGeomCell *) mergetiling->get_allcell().at(i);
            int index = toymatrix.Get_mcindex(mcell);
            no_need_remove.push_back(index);
        }

        WCP2dToy::ToyMatrixKalman toykalman(already_removed, no_need_remove, toymatrix, 0, 0);
        // std::cout << "Test: " << cellmap.size() << " " << wiremap.size() << " " << toykalman.Get_numz() << " " <<
        // ncount << std::endl;
    }
}

void WCP2dToy::ToyMatrixIterate_SingleWire::Iterate(WCP::GeomCellSelection cells, WCP::GeomCellSelection single_cells,
                                                    WCP::GeomCellSelection tried_cell, WCP::GeomCellMap cellmap,
                                                    WCP::GeomWireMap wiremap)
{
    if (ncount > limit) return;

    nlevel++;
    for (int i1 = 0; i1 != cells.size(); i1++) {
        // stuff that will get passed ...
        GeomCellSelection cells_1 = cells;
        GeomCellMap cellmap_1 = cellmap;
        GeomWireMap wiremap_1 = wiremap;
        GeomCellSelection single_wire_cells = single_cells;
        GeomCellSelection tried_cells = tried_cell;
        auto it = find(tried_cells.begin(), tried_cells.end(), cells.at(i1));
        if (it == tried_cells.end()) {
            // std::cout << "Start: " << nlevel << " " << tried_cells.size() << " " <<
            // 	cells_1.size() << " " << single_wire_cells.size() << " " <<
            // 	cellmap_1.size() << " " << wiremap_1.size() << std::endl;

            // remove the cell
            tried_cells.push_back(cells.at(i1));
            auto it1 = find(cells_1.begin(), cells_1.end(), cells.at(i1));
            cells_1.erase(it1);

            // Update the maps
            cellmap_1.erase(cells.at(i1));
            for (auto it2 = wiremap_1.begin(); it2 != wiremap_1.end(); it2++) {
                auto it3 = find(it2->second.begin(), it2->second.end(), cells.at(i1));
                if (it3 != it2->second.end()) it2->second.erase(it3);
            }

            int flag1 = 1;
            while (flag1 == 1) {
                flag1 = 0;
                GeomCellSelection to_be_removed_cells;

                for (int i = 0; i != cells_1.size(); i++) {
                    MergeGeomCell *mcell = (MergeGeomCell *) cells_1.at(i);
                    GeomWireSelection wires = cellmap_1[mcell];
                    int flag = 0;
                    for (int j = 0; j != wires.size(); j++) {
                        MergeGeomWire *mwire = (MergeGeomWire *) wires.at(j);
                        if (wirechargemap[mwire] > 10) {
                            if (wiremap_1[mwire].size() == 1) {
                                flag = 1;
                                break;
                            }
                        }
                    }
                    if (flag == 1) {
                        auto it4 = find(single_wire_cells.begin(), single_wire_cells.end(), mcell);
                        if (it4 == single_wire_cells.end()) single_wire_cells.push_back(mcell);
                    }
                }

                for (int i = 0; i != single_wire_cells.size(); i++) {
                    MergeGeomCell *mcell = (MergeGeomCell *) single_wire_cells.at(i);

                    auto it6 = find(cells_1.begin(), cells_1.end(), mcell);
                    if (it6 != cells_1.end()) cells_1.erase(it6);

                    GeomWireSelection wires = cellmap_1[mcell];
                    for (int j = 0; j != wires.size(); j++) {
                        MergeGeomWire *mwire = (MergeGeomWire *) wires.at(j);
                        GeomCellSelection tmp_cells = wiremap_1[mwire];
                        for (int k = 0; k != tmp_cells.size(); k++) {
                            auto it4 = find(single_wire_cells.begin(), single_wire_cells.end(), tmp_cells.at(k));
                            if (it4 == single_wire_cells.end()) {
                                auto it5 =
                                    find(to_be_removed_cells.begin(), to_be_removed_cells.end(), tmp_cells.at(k));

                                if (it5 == to_be_removed_cells.end()) to_be_removed_cells.push_back(tmp_cells.at(k));
                            }
                        }
                    }
                }

                if (to_be_removed_cells.size() != 0) flag1 = 1;

                // Now remove these cells, and the update the two maps
                for (int i = 0; i != to_be_removed_cells.size(); i++) {
                    MergeGeomCell *mcell = (MergeGeomCell *) to_be_removed_cells.at(i);
                    // // put into tried pool
                    // auto it7 = find(tried_cells.begin(),tried_cells.end(),mcell);
                    // if (it7 == tried_cells.end())
                    //   tried_cells.push_back(mcell);
                    // remove it
                    auto it4 = find(cells_1.begin(), cells_1.end(), mcell);
                    cells_1.erase(it4);
                    // Upate map
                    cellmap_1.erase(mcell);
                    for (auto it5 = wiremap_1.begin(); it5 != wiremap_1.end(); it5++) {
                        auto it6 = find(it5->second.begin(), it5->second.end(), mcell);
                        if (it6 != it5->second.end()) it5->second.erase(it6);
                    }
                }

                // std::cout << "Middle: " << nlevel << " " << tried_cells.size() << " " <<
                //   cells_1.size() << " " << single_wire_cells.size() << " " <<
                //   cellmap_1.size() << " " << wiremap_1.size() << std::endl;
            }
            // go to next level
            if (cells_1.size() > 0) {
                // std::cout << "End: " << nlevel << " " << tried_cells.size() << " " <<
                //   cells_1.size() << " " << single_wire_cells.size() << " " <<
                //   cellmap_1.size() << " " << wiremap_1.size() << std::endl;

                Iterate(cells_1, single_wire_cells, tried_cells, cellmap_1, wiremap_1);
            }
            else {
                std::vector<int> already_removed;
                std::vector<int> no_need_remove;

                for (int i = 0; i != mergetiling->get_allcell().size(); i++) {
                    MergeGeomCell *mcell = (MergeGeomCell *) mergetiling->get_allcell().at(i);
                    int index = toymatrix.Get_mcindex(mcell);
                    if (cellmap_1.find(mcell) == cellmap_1.end()) {
                        already_removed.push_back(index);
                    }
                    else {
                        no_need_remove.push_back(index);
                    }
                }

                double chi2_p = 0;
                for (int j = 0; j != already_removed.size(); j++) {
                    chi2_p += cell_penal[already_removed.at(j)];
                }

                if (penalty_ncpt > 0) {
                    // add the penalty due to not compatible cells (ncpt)
                    // int flag_test = 0;
                    for (auto it = cells_ncpt.begin(); it != cells_ncpt.end(); it++) {
                        int index1 = it->first;
                        if (find(already_removed.begin(), already_removed.end(), index1) != already_removed.end())
                            continue;
                        std::vector<int> indices = it->second;

                        for (int j = 0; j != indices.size(); j++) {
                            int index2 = indices.at(j);
                            if (find(already_removed.begin(), already_removed.end(), index2) == already_removed.end()) {
                                chi2_p += penalty_ncpt;
                                // flag_test = 1;
                                // break;
                            }
                        }
                        // if (flag_test ==1) break;
                    }
                    // if (flag_test == 1){
                    //   chi2_p += penalty_ncpt;
                    // }
                }

                WCP2dToy::ToyMatrixKalman toykalman(already_removed, no_need_remove, toymatrix, 0, 0, chi2_p);
                if (ncount > limit) return;
                ncount++;

                // std::cout << "Test: " << cellmap.size() << " " << wiremap.size() << " " << toykalman.Get_numz() << "
                // " << ncount << std::endl;
            }
        }
    }

    nlevel--;
}

WCP2dToy::ToyMatrixIterate_SingleWire::~ToyMatrixIterate_SingleWire() {}
