#include "WCP2dToy/ToyCrawler.h"
#include "TVector3.h"
#include "WCP2dToy/ToyWalking.h"

using namespace WCP;

WCP2dToy::ToyCrawler::ToyCrawler(MergeSpaceCellSelection &mcells, int flag, int flag1)
{
    CreateClusterTrack(mcells);
    FormGraph();

    // std::cout << all_clustertrack.size() << std::endl;

    std::cout << "Crawler: Merge Clusters " << std::endl;
    // Merge Cluster ...
    MergeCTrack(flag1);

    std::cout << "Crawler: Further Extend " << std::endl;
    // Further merge trying to extend into other tracks ...
    FurtherExtendCTrack();
    PurgeMergeCTrack();

    std::cout << "Crawler: CleanUp " << std::endl;
    // this is not a good assumption
    CleanUpCTTrack(flag);

    // for (int i=0;i!=mcells.size();i++){
    //   std::cout << mcells.at(i) << " " << mcells.at(i)->Get_Center().x << " "
    // 	      << mcells.at(i)->Get_Center().y << " "
    // 	      << mcells.at(i)->Get_Center().z << " "
    // 	      << std::endl;
    // }

    std::cout << "Crawler: Prepare Tracking " << std::endl;
    PrepareTracking();  // Get the order correct ... not well tuned yet ...
    UpdateMap();

    std::cout << "Crawler: CleanUp " << std::endl;
    PurgeMergeCTrack();
    CleanUpCTTrack(flag);

    // for (int i=0;i!=all_mergeclustertrack.size();i++){
    //   MergeClusterTrack *mct = all_mergeclustertrack.at(i);
    //   std::cout << i << " " << mct->Get_allmcells().size() << std::endl;
    //   if (i==0){
    //     for (int j=0;j!=mct->Get_allmcells().size();j++){
    // 	std::cout << i << " " << j << " " << mct->Get_allmcells().at(j)->Get_Center().x/units::cm << " " <<
    // mct->Get_allmcells().at(j)->Get_Center().y/units::cm << " " <<
    // mct->Get_allmcells().at(j)->Get_Center().z/units::cm << std::endl;
    //     }
    //   }
    //  }
}

void WCP2dToy::ToyCrawler::PrepareTracking()
{
    MergeClusterTrackSelection to_be_removed;
    MergeClusterTrackSelection to_be_added;
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        // if (i%1000) std::cout << i << " " << all_mergeclustertrack.at(i)->Get_ctracks().size() << " " <<
        // all_mergeclustertrack.size() << std::endl;
        int n_same = 0;
        int n_diff = 0;
        int first = -1;
        for (int j = 0; j < all_mergeclustertrack.at(i)->Get_ctracks().size(); j++) {
            ClusterTrack *track1 = all_mergeclustertrack.at(i)->Get_ctracks().at(j);
            TVector3 v2(track1->Get_LastMSCell()->Get_Center().x - track1->Get_FirstMSCell()->Get_Center().x,
                        track1->Get_LastMSCell()->Get_Center().y - track1->Get_FirstMSCell()->Get_Center().y,
                        track1->Get_LastMSCell()->Get_Center().z - track1->Get_FirstMSCell()->Get_Center().z);

            TVector3 v1(all_mergeclustertrack.at(i)->Get_LastMSCell()->Get_Center().x -
                            all_mergeclustertrack.at(i)->Get_FirstMSCell()->Get_Center().x,
                        all_mergeclustertrack.at(i)->Get_LastMSCell()->Get_Center().y -
                            all_mergeclustertrack.at(i)->Get_FirstMSCell()->Get_Center().y,
                        all_mergeclustertrack.at(i)->Get_LastMSCell()->Get_Center().z -
                            all_mergeclustertrack.at(i)->Get_FirstMSCell()->Get_Center().z);
            float a = v1.Dot(v2) / v1.Mag() / v2.Mag();

            // first is the one contain the maximum one
            if (first < 0) {
                auto it = find(track1->Get_allmcells().begin(), track1->Get_allmcells().end(),
                               all_mergeclustertrack.at(i)->Get_LastMSCell());
                if (it != track1->Get_allmcells().end()) first = j;
            }

            if (a < -0.5) {
                n_diff++;
            }
            else if (a > 0.5) {
                n_same++;
            }

            // std::cout << i << " " << j << " " << a << " " << all_mergeclustertrack.at(i)->Get_ctracks().size()
            // 		<< " " << all_mergeclustertrack.at(i)->Get_allmcells().size() << " "
            // 		<< track1->Get_FirstMSCell()->Get_Center().x/units::cm << " "
            // 		<< track1->Get_LastMSCell()->Get_Center().x/units::cm << std::endl;
        }

        if (n_same >= 2 && n_diff >= 2 && n_same > 0.1 * all_mergeclustertrack.at(i)->Get_ctracks().size() &&
            n_diff > 0.1 * all_mergeclustertrack.at(i)->Get_ctracks().size()) {
            //	&& first >=2 && first <all_mergeclustertrack.at(i)->Get_ctracks().size()-1){

            MergeSpaceCell *min_cell = 0, *max_cell = 0;
            float min_x, max_x;
            for (int qx = 0; qx != all_mergeclustertrack.at(i)->Get_allmcells().size(); qx++) {
                MergeSpaceCell *mcell = all_mergeclustertrack.at(i)->Get_allmcells().at(qx);
                if (min_cell == 0) {
                    min_x = mcell->Get_Center().x;
                    min_cell = mcell;
                }
                if (max_cell == 0) {
                    max_x = mcell->Get_Center().x;
                    max_cell = mcell;
                }

                if (mcell->Get_Center().x < min_x) {
                    min_x = mcell->Get_Center().x;
                    min_cell = mcell;
                }

                if (mcell->Get_Center().x > max_x) {
                    max_x = mcell->Get_Center().x;
                    max_cell = mcell;
                }
            }

            // a small value than a time slice ...  at most 0.8 mm

            MergeSpaceCell *first_cell = 0, *second_cell = 0, *third_cell = 0;
            if (fabs(all_mergeclustertrack.at(i)->Get_FirstMSCell()->Get_Center().x - min_x) < 0.5 * units::mm) {
                // std::cout << " a1 " << std::endl;
                first_cell = all_mergeclustertrack.at(i)->Get_FirstMSCell();
                second_cell = max_cell;
                third_cell = all_mergeclustertrack.at(i)->Get_LastMSCell();
            }
            else if (fabs(all_mergeclustertrack.at(i)->Get_FirstMSCell()->Get_Center().x - max_x) < 0.5 * units::mm) {
                // std::cout << " a2 " << std::endl;
                first_cell = min_cell;
                second_cell = all_mergeclustertrack.at(i)->Get_FirstMSCell();
                // third_cell = all_mergeclustertrack.at(i)->Get_LastMSCell();
            }
            else if (fabs(all_mergeclustertrack.at(i)->Get_LastMSCell()->Get_Center().x - min_x) < 0.5 * units::mm) {
                // std::cout << " a3 " << std::endl;
                first_cell = all_mergeclustertrack.at(i)->Get_LastMSCell();
                second_cell = max_cell;
                // third_cell = all_mergeclustertrack.at(i)->Get_FirstMSCell();
            }
            else if (fabs(all_mergeclustertrack.at(i)->Get_LastMSCell()->Get_Center().x - max_x) < 0.5 * units::mm) {
                // std::cout << " a4 " << std::endl;
                first_cell = min_cell;
                second_cell = all_mergeclustertrack.at(i)->Get_LastMSCell();
                // third_cell = all_mergeclustertrack.at(i)->Get_FirstMSCell();
            }
            else {
                first_cell = all_mergeclustertrack.at(i)->Get_FirstMSCell();
                second_cell = max_cell;
                // third_cell = all_mergeclustertrack.at(i)->Get_LastMSCell();
            }

            MergeSpaceCellSelection &must_cells = all_mergeclustertrack.at(i)->Get_allmcells();

            // Create two new MergedClusterTrack // just handle two ...

            // std::cout << "Walk 1" << std::endl;

            MergeSpaceCellSelection qx_mcells;
            MergeClusterTrack *track1 = 0;

            if (first_cell != 0 && second_cell != 0) {
                ToyWalking walking(first_cell, first_cell->Get_Center(), second_cell, second_cell->Get_Center(),
                                   mcells_map, must_cells);
                // std::cout << walking.get_dist() << " " << walking.get_cells().size() << std::endl;
                qx_mcells = walking.get_cells();
                track1 = new MergeClusterTrack(qx_mcells);

                // std::cout << track1->Get_allmcells().size() << " " << track1->Get_ctracks().size() << std::endl;
                MergeSpaceCell *next_cell1 = 0;
                float max_dist1 = 0;
                MergeSpaceCell *next_cell2 = 0;
                float max_dist2 = 0;
                for (int qx = 0; qx != all_mergeclustertrack.at(i)->Get_allmcells().size(); qx++) {
                    MergeSpaceCell *mcell = all_mergeclustertrack.at(i)->Get_allmcells().at(qx);
                    auto it = find(qx_mcells.begin(), qx_mcells.end(), mcell);
                    if (it == qx_mcells.end()) {
                        if (fabs(mcell->Get_Center().x - first_cell->Get_Center().x) > max_dist1) {
                            max_dist1 = fabs(mcell->Get_Center().x - first_cell->Get_Center().x);
                            next_cell1 = mcell;
                        }

                        if (fabs(mcell->Get_Center().x - second_cell->Get_Center().x) > max_dist2) {
                            max_dist2 = fabs(mcell->Get_Center().x - second_cell->Get_Center().x);
                            next_cell2 = mcell;
                        }
                    }
                }

                if (max_dist1 < max_dist2) {
                    // if (next_cell->Get_Center().x < third_cell->Get_Center().x){
                    //   first_cell = next_cell;
                    //   second_cell = third_cell;
                    // }else{
                    //   first_cell = third_cell;
                    //   second_cell = next_cell;
                    // }
                    second_cell = next_cell1;
                }
                else {
                    first_cell = next_cell2;
                }

                // std::cout << first_cell->Get_Center().x/units::cm << " "
                // 	<< first_cell->Get_Center().y/units::cm << " "
                // 	<< first_cell->Get_Center().z/units::cm << " "
                // 	<< second_cell->Get_Center().x/units::cm << " "
                // 	<< second_cell->Get_Center().y/units::cm << " "
                // 	<< second_cell->Get_Center().z/units::cm << " "
                // 	<< min_x/units::cm  << " " << max_x/units::cm  << " "
                // 	<< all_mergeclustertrack.at(i)->Get_FirstMSCell()->Get_Center().x /units::cm << " "
                // 	<< all_mergeclustertrack.at(i)->Get_LastMSCell()->Get_Center().x /units::cm << " "
                // 		<< max_dist1 << " " << max_dist2 << " "  << qx_mcells.size() << " "
                // 	<< std::endl;
                // std::cout << "Walk 2" << std::endl;
                // std::cout << first_cell << " " << second_cell << std::endl;

                MergeSpaceCellSelection qx_mcells1;
                MergeClusterTrack *track2 = 0;

                if (first_cell != 0 && second_cell != 0) {
                    ToyWalking walking1(first_cell, first_cell->Get_Center(), second_cell, second_cell->Get_Center(),
                                        mcells_map, must_cells);
                    qx_mcells1 = walking1.get_cells();
                    track2 = new MergeClusterTrack(qx_mcells1);
                }

                if (qx_mcells.size() != 0 && qx_mcells1.size() != 0) {
                    to_be_removed.push_back(all_mergeclustertrack.at(i));
                    to_be_added.push_back(track1);
                    to_be_added.push_back(track2);
                }
            }
            // MergeClusterTrack *track2 = new MergeClusterTrack( all_mergeclustertrack.at(i)->Get_ctracks().back());
            // for (int j=all_mergeclustertrack.at(i)->Get_ctracks().size()-1;j>=first;j--){
            // 	track2->Add(all_mergeclustertrack.at(i)->Get_ctracks().at(j),all_mergeclustertrack.at(i)->Get_ctracks().at(j)->Get_FirstMSCell());
            // }
            // to_be_added.push_back(track2);

            // std::cout << i << " " << n_same << " " << n_diff << " " << first << " " << std::endl;
        }
    }

    for (int i = 0; i != to_be_removed.size(); i++) {
        auto it = find(all_mergeclustertrack.begin(), all_mergeclustertrack.end(), to_be_removed.at(i));
        if (it != all_mergeclustertrack.end()) {
            delete to_be_removed.at(i);
            all_mergeclustertrack.erase(it);
        }
    }

    for (int i = 0; i != to_be_added.size(); i++) {
        all_mergeclustertrack.push_back(to_be_added.at(i));
    }
}

MergeSpaceCell *WCP2dToy::ToyCrawler::GetClosestMSC(Point p, WCP::MergeSpaceCellSelection &cells2)
{
    // loop all mergeclustertrack
    // find the right time slice and search for mergespace cell (or the closest)
    int time = WCP2dToy::MergeToyTiling::Dis2Time(p.x);
    // std::cout << p.x/units::cm << " " << time << std::endl;
    MergeSpaceCell *cell = 0;

    double min_dis = 1e9;

    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        int ntime = mct->Get_TimeLength();
        for (int j = 0; j != ntime; j++) {
            if (mct->Get_Time(j) == time) {
                // examine all the merge cells
                MergeSpaceCellSelection &cells1 = mct->Get_MSCS(j);
                for (int k = 0; k != cells1.size(); k++) {
                    auto it = find(cells2.begin(), cells2.end(), cells1.at(k));
                    if (it != cells2.end()) {
                        double dis = cells1.at(k)->ClosestDis(p);
                        if (dis < min_dis) {
                            cell = cells1.at(k);
                            min_dis = dis;
                        }
                    }
                }
            }
        }
    }

    if (cell == 0) {
        for (int i = 0; i != all_mergeclustertrack.size(); i++) {
            MergeClusterTrack *mct = all_mergeclustertrack.at(i);
            MergeSpaceCellSelection cells1 = mct->Get_allmcells();
            for (int j = 0; j != cells1.size(); j++) {
                double dis = cells1.at(j)->ClosestDis(p);
                if (dis < min_dis) {
                    cell = cells1.at(j);
                    min_dis = dis;
                }
            }
        }
    }

    return cell;
}

void WCP2dToy::ToyCrawler::CleanUpCTTrack(int flag)
{
    // Sort the track first
    MergeClusterTrackSet MCT_set;
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        MCT_set.insert(mct);
    }
    all_mergeclustertrack.clear();
    for (auto it = MCT_set.begin(); it != MCT_set.end(); it++) {
        MergeClusterTrack *mct = *it;
        all_mergeclustertrack.push_back(mct);
        // std::cout << mct->Get_allmcells().size() << std::endl;
    }

    MergeClusterTrackSelection to_be_removed;
    MergeClusterTrackSelection examined;

    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        examined.push_back(mct);

        for (int j = 0; j != mct->Get_allmcells().size(); j++) {
            MergeSpaceCell *mcell = mct->Get_allmcells().at(j);
            for (int k = 0; k != mcells_mct_map[mcell].size(); k++) {
                MergeClusterTrack *mct1 = mcells_mct_map[mcell].at(k);
                auto it1 = find(to_be_removed.begin(), to_be_removed.end(), mct1);
                auto it2 = find(examined.begin(), examined.end(), mct1);
                if (it1 == to_be_removed.end() && it2 == examined.end()) {
                    int n_common = 0;
                    int n_diff = 0;
                    float ncells_common = 0;
                    float ncells_diff = 0;

                    for (int i1 = 0; i1 != mct1->Get_allmcells().size(); i1++) {
                        MergeSpaceCell *mcell1 = mct1->Get_allmcells().at(i1);
                        auto it3 = find(mct->Get_allmcells().begin(), mct->Get_allmcells().end(), mcell1);
                        if (it3 == mct->Get_allmcells().end()) {
                            n_diff++;
                            if (flag == 1) {
                                ncells_diff += mcell1->Get_all_spacecell().size();
                            }
                            else {
                                ncells_diff += mcell1->Get_Charge();
                            }
                        }
                        else {
                            n_common++;
                            if (flag == 1) {
                                ncells_common += mcell1->Get_all_spacecell().size();
                            }
                            else {
                                ncells_common += mcell1->Get_Charge();
                            }
                        }
                    }

                    if ((flag == 1) && ((n_common > n_diff && n_diff < 2 && ncells_common >= 20 * ncells_diff))) {
                        // merge
                        to_be_removed.push_back(mct1);
                        mct->AddTrack(mct1);
                    }
                    else if ((flag == 2) && (n_common >= n_diff && n_diff < 2 && ncells_common >= 20 * ncells_diff)) {
                        // merge
                        to_be_removed.push_back(mct1);
                        mct->AddTrack(mct1);
                    }
                    else {
                        // not merge
                        examined.push_back(mct1);
                    }
                }
            }
        }
    }

    for (int i = 0; i != to_be_removed.size(); i++) {
        auto it = find(all_mergeclustertrack.begin(), all_mergeclustertrack.end(), to_be_removed.at(i));
        all_mergeclustertrack.erase(it);
    }

    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        mct->Organize();
    }

    UpdateMap();
}

void WCP2dToy::ToyCrawler::UpdateMap()
{
    // Update map again ...
    mcells_mct_map.clear();
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        for (int j = 0; j != mct->Get_allmcells().size(); j++) {
            MergeSpaceCell *mcell = mct->Get_allmcells().at(j);

            if (mcells_mct_map.find(mcell) == mcells_mct_map.end()) {
                MergeClusterTrackSelection temp1;
                temp1.push_back(mct);
                mcells_mct_map[mcell] = temp1;
            }
            else {
                mcells_mct_map[mcell].push_back(mct);
            }
        }
    }
}

void WCP2dToy::ToyCrawler::PurgeMergeCTrack()
{
    // remove merged cluster in which all the merge space cell are accounted for ... s
    MergeClusterTrackSelection temp1;
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        int flag = 0;
        for (int j = 0; j != mct->Get_allmcells().size(); j++) {
            MergeSpaceCell *mcell = mct->Get_allmcells().at(j);

            if (mcells_mct_map[mcell].size() == 1) {
                flag = 1;
                break;
            }
        }
        if (flag == 0) {
            temp1.push_back(mct);
        }
    }

    for (int i = 0; i != temp1.size(); i++) {
        auto it = find(all_mergeclustertrack.begin(), all_mergeclustertrack.end(), temp1.at(i));
        all_mergeclustertrack.erase(it);
    }
    temp1.clear();

    // Update map again ...
    mcells_mct_map.clear();
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        for (int j = 0; j != mct->Get_allmcells().size(); j++) {
            MergeSpaceCell *mcell = mct->Get_allmcells().at(j);

            if (mcells_mct_map.find(mcell) == mcells_mct_map.end()) {
                MergeClusterTrackSelection temp1;
                temp1.push_back(mct);
                mcells_mct_map[mcell] = temp1;
            }
            else {
                mcells_mct_map[mcell].push_back(mct);
            }
        }
    }
}

void WCP2dToy::ToyCrawler::FurtherExtendCTrack(int flag_qx)
{
    MergeClusterTrackSelection temp1;

    int flag3 = 1;
    flag3 = 0;
    float cut_angle = 15;

    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        MergeSpaceCellSelection mcells = mct->Get_allmcells();

        if (mcells.size() >= 2) {
            MergeSpaceCell *fvertex = mcells.front();
            MergeSpaceCell *fvertex_next = mcells.at(1);
            // determine direction
            int flag1 = 0;
            if (fvertex->Get_Center().x - fvertex_next->Get_Center().x > 0) {
                flag1 = 1;
            }
            else if (fvertex->Get_Center().x - fvertex_next->Get_Center().x < 0) {
                flag1 = -1;
            }

            if (flag1 != 0) {
                mct->SC_Hough(fvertex->Get_Center());
                double theta = mct->Get_Theta();
                double phi = mct->Get_Phi();

                mct->SC_Hough(fvertex->Get_Center(), -1, 2);
                double theta_m = mct->Get_Theta();
                double phi_m = mct->Get_Phi();

                // save the one that satisfy the requirement
                MergeSpaceCellSelection temp;
                MergeSpaceCell *cur_msc = fvertex;
                MergeSpaceCellSelection cur_cells = mcells_map[cur_msc];

                int flag2 = 1;
                while (flag2 == 1) {
                    // insert temp
                    flag2 = 0;
                    for (int j = 0; j != cur_cells.size(); j++) {
                        MergeSpaceCell *next_msc = cur_cells.at(j);
                        if ((next_msc->Get_Center().x - cur_msc->Get_Center().x) * flag1 > 0) {
                            // judge overlap?
                            if (next_msc->CrossCell(fvertex->Get_Center(), theta_m, phi_m)) {
                                temp.push_back(next_msc);
                                cur_msc = next_msc;
                                cur_cells = mcells_map[cur_msc];
                                flag2 = 1;
                                break;
                            }
                        }
                    }
                }

                // Now temp contains all the cells to be merged

                // add one cell in, and then figure out whether to add its track
                flag2 = 1;
                while (flag2) {
                    flag2 = 0;
                    for (int j = 0; j != temp.size(); j++) {
                        MergeSpaceCell *mcell = temp.at(j);
                        auto it = find(mct->Get_allmcells().begin(), mct->Get_allmcells().end(), mcell);
                        if (it == mct->Get_allmcells().end()) {
                            MergeClusterTrackSelection MCTS = mcells_mct_map[mcell];
                            // insert it in
                            int insert_flag = 1;
                            // for (int k=0;k!=MCTS.size();k++){
                            // 	if ( MCTS.at(k)->CheckCell(mcell)){
                            // 	  insert_flag = 1;
                            // 	  break;
                            // 	}
                            // }
                            // std::cout << "abc: " << insert_flag << std::endl;

                            if (insert_flag == 1) {
                                mct->Add(mcell, -1);

                                // Now find the associated clusterTrack
                                // judge whether to put in temp1 to be deleted
                                for (int k = 0; k != MCTS.size(); k++) {
                                    MCTS.at(k)->SC_Hough(mcell->Get_Center());
                                    float theta1 = MCTS.at(k)->Get_Theta();
                                    float phi1 = MCTS.at(k)->Get_Phi();

                                    MCTS.at(k)->SC_Hough(mcell->Get_Center(), -1, 2);
                                    float theta1_m = MCTS.at(k)->Get_Theta();
                                    float phi1_m = MCTS.at(k)->Get_Phi();

                                    if (MCTS.at(k)->CheckCell(mcell)) {
                                        if ((fabs(theta1 + theta - 3.1415926) <
                                                 cut_angle / 180. * 3.1415926  // 5 degrees
                                             && fabs(fabs(phi1 - phi) - 3.1415926) < cut_angle / 180. * 3.1415926) ||
                                            (fabs(theta1_m + theta_m - 3.1415926) <
                                                 cut_angle / 180. * 3.1415926  // 5 degrees
                                             &&
                                             fabs(fabs(phi1_m - phi_m) - 3.1415926) < cut_angle / 180. * 3.1415926)) {
                                            // add this Merged Track
                                            mct->Add(MCTS.at(k), mcell, -1);
                                            temp1.push_back(MCTS.at(k));
                                            flag2 = 1;
                                        }
                                    }
                                }
                            }
                        }
                    }
                }  // while(flag2)
                   // std::cout << i << " " << temp.size() << " " << temp1.size() << std::endl;
                if (temp1.size() > 0) {
                    flag3 = 1;
                    goto abc;
                }
            }

            // deal with the front
            MergeSpaceCell *bvertex = mcells.back();
            MergeSpaceCell *bvertex_next = mcells.at(mcells.size() - 2);

            flag1 = 0;
            if (bvertex->Get_Center().x - bvertex_next->Get_Center().x > 0) {
                flag1 = 1;
            }
            else if (bvertex->Get_Center().x - bvertex_next->Get_Center().x < 0) {
                flag1 = -1;
            }

            if (flag1 != 0) {
                mct->SC_Hough(bvertex->Get_Center());
                double theta = mct->Get_Theta();
                double phi = mct->Get_Phi();
                mct->SC_Hough(bvertex->Get_Center(), -1, 2);
                double theta_m = mct->Get_Theta();
                double phi_m = mct->Get_Phi();

                // save the one that satisfy the requirement
                MergeSpaceCellSelection temp;
                MergeSpaceCell *cur_msc = bvertex;
                MergeSpaceCellSelection cur_cells = mcells_map[cur_msc];

                int flag2 = 1;
                while (flag2 == 1) {
                    // insert temp
                    flag2 = 0;
                    for (int j = 0; j != cur_cells.size(); j++) {
                        MergeSpaceCell *next_msc = cur_cells.at(j);
                        if ((next_msc->Get_Center().x - cur_msc->Get_Center().x) * flag1 > 0) {
                            // judge overlap?
                            if (next_msc->CrossCell(bvertex->Get_Center(), theta, phi)) {
                                temp.push_back(next_msc);
                                cur_msc = next_msc;
                                cur_cells = mcells_map[cur_msc];
                                flag2 = 1;
                                break;
                            }
                        }
                    }
                }

                // Now temp contains all the cells to be merged

                // add one cell in, and then figure out whether to add its track
                flag2 = 1;
                while (flag2) {
                    flag2 = 0;
                    for (int j = 0; j != temp.size(); j++) {
                        MergeSpaceCell *mcell = temp.at(j);
                        auto it = find(mct->Get_allmcells().begin(), mct->Get_allmcells().end(), mcell);
                        if (it == mct->Get_allmcells().end()) {
                            MergeClusterTrackSelection MCTS = mcells_mct_map[mcell];
                            int insert_flag = 1;

                            // for (int k=0;k!=MCTS.size();k++){
                            // 	if ( MCTS.at(k)->CheckCell(mcell) ){
                            // 	  insert_flag = 1;
                            // 	  break;
                            // 	}
                            // }

                            if (insert_flag == 1) {
                                // insert it in
                                mct->Add(mcell, 1);

                                // Now find the associated clusterTrack
                                // judge whether to put in temp1 to be deleted
                                for (int k = 0; k != MCTS.size(); k++) {
                                    MCTS.at(k)->SC_Hough(mcell->Get_Center());
                                    float theta1 = MCTS.at(k)->Get_Theta();
                                    float phi1 = MCTS.at(k)->Get_Phi();
                                    MCTS.at(k)->SC_Hough(mcell->Get_Center(), -1, 2);
                                    float theta1_m = MCTS.at(k)->Get_Theta();
                                    float phi1_m = MCTS.at(k)->Get_Phi();

                                    if (MCTS.at(k)->CheckCell(mcell)) {
                                        if ((fabs(theta1 + theta - 3.1415926) <
                                                 cut_angle / 180. * 3.1415926  // 5 degrees
                                             && fabs(fabs(phi1 - phi) - 3.1415926) < cut_angle / 180. * 3.1415926) ||
                                            (fabs(theta1_m + theta_m - 3.1415926) <
                                                 cut_angle / 180. * 3.1415926  // 5 degrees
                                             &&
                                             fabs(fabs(phi1_m - phi_m) - 3.1415926) < cut_angle / 180. * 3.1415926)) {
                                            // std::cout << theta << " " << theta1 << " " << phi << " " << phi1 <<
                                            // std::endl; add this Merged Track
                                            mct->Add(MCTS.at(k), mcell, 1);
                                            temp1.push_back(MCTS.at(k));
                                            flag2 = 1;
                                        }
                                    }
                                }
                            }
                        }
                    }
                }  // while(flag2)
                   // std::cout << i << " " << temp.size() << " " << temp1.size() << std::endl;

                if (temp1.size() > 0) {
                    flag3 = 1;
                    goto abc;
                }
            }
        }
    }  // remove the merge cluster after this one

abc:

    // std::cout << all_mergeclustertrack.size() << std::endl;
    for (int i = 0; i != temp1.size(); i++) {
        auto it = find(all_mergeclustertrack.begin(), all_mergeclustertrack.end(), temp1.at(i));
        all_mergeclustertrack.erase(it);
    }
    temp1.clear();
    // std::cout << all_mergeclustertrack.size() << std::endl;
    // }

    // Update Map

    mcells_mct_map.clear();
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        for (int j = 0; j != mct->Get_allmcells().size(); j++) {
            MergeSpaceCell *mcell = mct->Get_allmcells().at(j);

            if (mcells_mct_map.find(mcell) == mcells_mct_map.end()) {
                MergeClusterTrackSelection temp1;
                temp1.push_back(mct);
                mcells_mct_map[mcell] = temp1;
            }
            else {
                mcells_mct_map[mcell].push_back(mct);
            }
        }
    }
}

void WCP2dToy::ToyCrawler::MergeCTrack(int flag1)
{
    WCP::ClusterTrackSelection used_clustertrack;  // hold the used tracks

    for (int i = 0; i != all_clustertrack.size(); i++) {
        auto it = find(used_clustertrack.begin(), used_clustertrack.end(), all_clustertrack.at(i));
        if (it == used_clustertrack.end()) {
            MergeClusterTrack *mct = new MergeClusterTrack(all_clustertrack.at(i));

            MergeSpaceCellSelection vertices;  // save existing vertices

            MergeSpaceCellSelection used_vertices;  // save used vertices

            all_mergeclustertrack.push_back(mct);
            used_clustertrack.push_back(all_clustertrack.at(i));
            vertices.push_back(all_clustertrack.at(i)->Get_FirstMSCell());
            vertices.push_back(all_clustertrack.at(i)->Get_LastMSCell());

            // start to add stuff ...
            while (vertices.size() != 0) {
                MergeSpaceCell *vertex = vertices.back();
                vertices.pop_back();
                used_vertices.push_back(vertex);

                // find the track inside MergeClusterTrack which contain this vertex
                // ClusterTrack* old_cct = mct->GetClusterTrack(vertex);
                if (flag1 == 1) {
                    mct->SC_Hough(vertex->Get_Center());
                }
                else {
                    mct->SC_Hough(vertex->Get_Center(), -1, 3);
                }
                //

                float theta1 = mct->Get_Theta();
                float phi1 = mct->Get_Phi();

                mct->SC_Hough(vertex->Get_Center(), -1, 2);
                float theta1_m = mct->Get_Theta();
                float phi1_m = mct->Get_Phi();

                ClusterTrackSelection cts = ms_ct_map[vertex];

                if (cts.size() > 50) continue;

                // std::cout << i << " " << vertices.size() << " " << cts.size() << " " <<
                // vertex->Get_Center().x/units::cm << " " <<
                //   vertex->Get_Center().y/units::cm << " " <<  vertex->Get_Center().z/units::cm << " " << std::endl;
                // std::cout << theta1 << " " << phi1 << " " << cts.size() << std::endl;

                int flag_special = 0;
                for (int j = 0; j != cts.size(); j++) {
                    if (cts.at(j)->Get_allmcells().size() > 3) flag_special++;
                }

                for (int j = 0; j != cts.size(); j++) {
                    ClusterTrack *cct = cts.at(j);
                    auto it1 = find(used_clustertrack.begin(), used_clustertrack.end(), cct);
                    if (it1 == used_clustertrack.end()) {
                        // Judge angle matching ...
                        int flag = 0;
                        // write the main alg. ...

                        float cut_angle;
                        if (flag_special > 2) {
                            cut_angle = 10;
                        }
                        else {
                            cut_angle = 25;
                        }
                        float cut_angle1 = 15;

                        float shift_angle = 100;

                        cct->SC_Hough(vertex->Get_Center(), -1, 2);
                        float theta2_m = cct->Get_Theta();
                        float phi2_m = cct->Get_Phi();

                        if ((fabs(theta1_m + theta2_m - 3.1415926) < cut_angle1 / 180. * 3.1415926  // 5 degrees
                             && fabs(fabs(phi1_m - phi2_m) - 3.1415926) < cut_angle1 / 180. * 3.1415926)) {
                            flag = 1;
                        }

                        if (flag == 0) {
                            //
                            if (flag1 == 1) {
                                cct->SC_Hough(vertex->Get_Center());
                            }
                            else {
                                cct->SC_Hough(vertex->Get_Center(), -1, 3);
                            }
                            float theta2 = cct->Get_Theta();
                            float phi2 = cct->Get_Phi();

                            if ((fabs(theta1 + theta2 - 3.1415926) < cut_angle / 180. * 3.1415926  // 5 degrees
                                 && fabs(fabs(phi1 - phi2) - 3.1415926) < cut_angle / 180. * 3.1415926)) {
                                flag = 1;
                            }
                        }

                        // std::cout << theta1_m/3.1415926*180. << " " << theta2_m/3.1415926*180.
                        // 	      << " " << phi1_m/3.1415926*180. << " " << phi2_m/3.1415926*180.
                        // 	      << std::endl;

                        if (flag == 0) {
                            // int cross_num = cct->CrossNum(vertex->Get_Center(), theta1_m,phi1_m);
                            int cross_num = cct->CrossNum(vertex, theta1_m, phi1_m);
                            if (cross_num == cct->Get_allmcells().size()) {
                                flag = 1;
                            }
                        }

                        // if (flag==0){
                        //   theta2 = cct->Get_CTheta(vertex->Get_Center());
                        //   phi2 = cct->Get_CPhi(vertex->Get_Center());
                        //   if ((fabs(theta1+theta2-3.1415926)<cut_angle/180.*3.1415926 // 5 degrees
                        // 	   && fabs(fabs(phi1-phi2)-3.1415926)<cut_angle/180.*3.1415926)
                        // 	  ||(fabs(theta1-theta2-shift_angle)<cut_angle1/180.*3.1415926 // 5 degrees
                        // 	     && fabs(phi1-phi2)<cut_angle1/180.*3.1415926)
                        // 	  ){
                        // 	flag = 1;
                        //   }
                        // }

                        // if (flag == 0 ){
                        //   mct->SC_Hough(vertex->Get_Center(), 3 * units::cm);
                        //   cct->SC_Hough(vertex->Get_Center(), 3 * units::cm);

                        //   theta1 = mct->Get_Theta();
                        //   phi1 = mct->Get_Phi();

                        //   theta2 = cct->Get_Theta();
                        //   phi2 = cct->Get_Phi();

                        //   if ((fabs(theta1+theta2-3.1415926)<cut_angle/180.*3.1415926 // 5 degrees
                        // 	   && fabs(fabs(phi1-phi2)-3.1415926)<cut_angle/180.*3.1415926)
                        // 	  ||(fabs(theta1-theta2-shift_angle)<cut_angle1/180.*3.1415926 // 5 degrees
                        // 	     && fabs(phi1-phi2)<cut_angle1/180.*3.1415926)
                        // 	  ){
                        // 	flag = 1;
                        //   }
                        // }

                        // std::cout <<  i << " " <<
                        // find(all_clustertrack.begin(),all_clustertrack.end(),old_cct)-all_clustertrack.begin()
                        // 	      << " " <<
                        // find(all_clustertrack.begin(),all_clustertrack.end(),cct)-all_clustertrack.begin() << " "
                        // 	      << cts.size() << " "
                        // 	      << flag << " " << theta1/3.1415926*180. << " " << phi1/3.1415926*180. << " "
                        // 	      << theta2/3.1415926*180. << " "
                        // 	      <<  phi2/3.1415926*180. << " "
                        // 	      << cct->Get_CTheta(vertex->Get_Center())/3.1415926*180. << " "
                        // 	      << cct->Get_CPhi(vertex->Get_Center())/3.1415926*180. << " "
                        // 	      << old_cct->Get_CTheta(vertex->Get_Center())/3.1415926*180. << " "
                        // 	      << old_cct->Get_CPhi(vertex->Get_Center())/3.1415926*180. << " "
                        // 	      << vertex->Get_Center().x/units::cm << " "
                        // 	      << vertex->Get_Center().y/units::cm << " "
                        // 	      << vertex->Get_Center().z/units::cm << " "
                        // 	      << std::endl;

                        // flag = 0;

                        // if good, save it
                        if (flag == 1) {
                            mct->Add(cct, vertex);
                            used_clustertrack.push_back(cct);
                            if (cct->Get_FirstMSCell() != vertex) {
                                auto it_qx1 = find(used_vertices.begin(), used_vertices.end(), cct->Get_FirstMSCell());
                                if (it_qx1 == used_vertices.end()) vertices.push_back(cct->Get_FirstMSCell());
                            }
                            if (cct->Get_LastMSCell() != vertex) {
                                auto it_qx1 = find(used_vertices.begin(), used_vertices.end(), cct->Get_LastMSCell());
                                if (it_qx1 == used_vertices.end()) vertices.push_back(cct->Get_LastMSCell());
                            }
                        }
                    }
                }  // end for loop
            }      // end while
        }
    }

    // Form map ...

    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        MergeClusterTrack *mct = all_mergeclustertrack.at(i);
        for (int j = 0; j != mct->Get_allmcells().size(); j++) {
            MergeSpaceCell *mcell = mct->Get_allmcells().at(j);

            if (mcells_mct_map.find(mcell) == mcells_mct_map.end()) {
                MergeClusterTrackSelection temp1;
                temp1.push_back(mct);
                mcells_mct_map[mcell] = temp1;
            }
            else {
                mcells_mct_map[mcell].push_back(mct);
            }
        }
    }
}

void WCP2dToy::ToyCrawler::CreateClusterTrack(MergeSpaceCellSelection &mcells)
{
    for (int i = 0; i != mcells.size(); i++) {
        MergeSpaceCell *mcell1 = mcells.at(i);
        MergeSpaceCellSelection mcell1_sel;
        mcells_map[mcell1] = mcell1_sel;  // create basic ...
        MergeSpaceCellSelection mcell2_sel;
        mcells_save[mcell1] = mcell2_sel;
    }

    //  std::cout << mcells.size() << std::endl;
    // form associations ...
    for (int i = 0; i != mcells.size(); i++) {
        //    std::cout << i << " " << " "   << mcells.size() << std::endl;

        MergeSpaceCell *mcell1 = mcells.at(i);
        Point m1center = mcell1->Get_Center();
        double thickness = mcell1->thickness();

        // MergeSpaceCellSelection mcell1_sel;
        for (int j = 0; j != mcells.size(); j++) {
            MergeSpaceCell *mcell2 = mcells.at(j);
            Point m2center = mcell2->Get_Center();

            if (fabs(fabs(m1center.x - m2center.x) - thickness) < 0.1 * thickness) {
                auto it = find(mcells_map[mcell1].begin(), mcells_map[mcell1].end(), mcell2);
                if (it != mcells_map[mcell1].end()) continue;

                if (mcell1->Overlap(*mcell2)) {
                    // mcell1_sel.push_back(mcell2);
                    mcells_map[mcell1].push_back(mcell2);
                    mcells_map[mcell2].push_back(mcell1);
                }
            }
        }

        // mcells_map[mcell1] = mcell1_sel; // form structure   adjacent ones ...
        //    mcells_counter[mcell1] = 0;  // initiliaization
    }

    // std::cout << mcells_map.size() << " " << mcells_counter.size() << std::endl;

    // for (int qx = 0; qx!=14;qx++){
    //
    while (used_mcells.size() != mcells_map.size()) {
        // std::cout << "Xin: "  << " " << used_mcells.size() << " " << mcells_map.size() << std::endl;
        // start to construct ClusterTrack ... first one
        ClusterTrack *ctrack;
        // find the start point
        int flag1 = 0;
        for (int i = 0; i != mcells.size(); i++) {
            MergeSpaceCell *mcell = mcells.at(i);
            auto it1 = find(used_mcells.begin(), used_mcells.end(), mcell);
            auto it2 = find(end_mcells.begin(), end_mcells.end(), mcell);

            if (it1 == used_mcells.end()                                     // not insided the used ones
                || (it2 != end_mcells.end()                                  // inside the end ones
                    && mcells_map[mcell].size() > mcells_save[mcell].size()  // not used all
                    )) {
                // std::cout << it1 - used_mcells.end() << " " <<
                // it2 - end_mcells.end() << " " <<
                // mcells_map[mcell].size() << " " << mcells_save[mcell].size()
                // 	      <<std::endl;

                ctrack = new ClusterTrack(mcell);
                if (it2 == end_mcells.end()) end_mcells.push_back(mcell);
                if (it1 == used_mcells.end()) used_mcells.push_back(mcell);
                // mcells_counter[mcell] ++;
                flag1 = 1;
                break;
            }
        }

        if (flag1 == 1) {
            int flag = 0;
            while (flag == 0) {
                MergeSpaceCell *cur_cell = ctrack->Get_LastMSCell();
                int num_daughter = mcells_map[cur_cell].size();
                //      std::cout << "nd: " << num_daughter << std::endl;
                if (num_daughter == 0) {  // no daugher, end
                    flag = 1;
                }
                else if (num_daughter == 1) {
                    MergeSpaceCell *dcell = mcells_map[cur_cell].at(0);
                    auto it1 = find(ctrack->Get_allmcells().begin(), ctrack->Get_allmcells().end(), dcell);
                    if (it1 != ctrack->Get_allmcells().end()) {
                        flag = 1;  // already existed ... end it
                    }
                    else {
                        auto it2 = find(used_mcells.begin(), used_mcells.end(), dcell);
                        if (it2 == used_mcells.end()) {
                            // not used yet? add it

                            // Now need to judge angle ...

                            if (ctrack->AddMSCell(dcell)) {
                                used_mcells.push_back(dcell);  // continue
                                flag = 0;
                            }
                            else {
                                flag = 1;
                            }
                        }
                        else {
                            auto it3 = find(end_mcells.begin(), end_mcells.end(), dcell);
                            flag = 1;
                            if (it3 != end_mcells.end()) {
                                // already used, add it, and end it
                                ctrack->AddMSCell(dcell);
                            }
                        }
                    }
                }  // # of daughter  == 1
                else if (num_daughter == 2) {
                    // two cases,    just one element
                    if (ctrack->Get_allmcells().size() == 1) {
                        for (int i = 0; i != num_daughter; i++) {
                            MergeSpaceCell *dcell = mcells_map[cur_cell].at(i);
                            auto it1 = find(ctrack->Get_allmcells().begin(), ctrack->Get_allmcells().end(), dcell);
                            // std::cout << it1 - ctrack->Get_allmcells().end() << std::endl;
                            if (it1 == ctrack->Get_allmcells().end()) {
                                // not used yet
                                auto it2 = find(used_mcells.begin(), used_mcells.end(), dcell);
                                if (it2 == used_mcells.end()) {
                                    // not used yet? add it
                                    if (ctrack->AddMSCell(dcell)) {
                                        used_mcells.push_back(dcell);  // continue
                                        flag = 0;
                                        break;
                                    }
                                    else {
                                        flag = 1;
                                    }
                                }
                                else {
                                    auto it3 = find(end_mcells.begin(), end_mcells.end(), dcell);
                                    flag = 1;
                                    if (it3 != end_mcells.end()) {
                                        // already used, add it, and end it
                                        auto it4 =
                                            find(mcells_save[cur_cell].begin(), mcells_save[cur_cell].end(), dcell);
                                        if (it4 == mcells_save[cur_cell].end()) {
                                            if (ctrack->AddMSCell(dcell)) {
                                                break;
                                            }
                                        }
                                    }
                                }
                            }
                            else {
                                flag = 1;
                            }
                        }
                    }
                    else {
                        // more than one element
                        MergeSpaceCell *dcell1 = mcells_map[cur_cell].at(0);
                        MergeSpaceCell *dcell2 = mcells_map[cur_cell].at(1);
                        auto it1 = find(ctrack->Get_allmcells().begin(), ctrack->Get_allmcells().end(), dcell1);
                        auto it2 = find(ctrack->Get_allmcells().begin(), ctrack->Get_allmcells().end(), dcell2);

                        if (it1 == ctrack->Get_allmcells().end() && it2 == ctrack->Get_allmcells().end()) {
                            flag = 1;  // end it
                        }
                        else if (it1 == ctrack->Get_allmcells().end() && it2 != ctrack->Get_allmcells().end()) {
                            // look at it1
                            auto it3 = find(used_mcells.begin(), used_mcells.end(), dcell1);
                            if (it3 == used_mcells.end()) {
                                // not used yet? add it
                                if (ctrack->AddMSCell(dcell1)) {
                                    used_mcells.push_back(dcell1);  // continue
                                    flag = 0;
                                }
                                else {
                                    flag = 1;
                                }
                            }
                            else {
                                auto it4 = find(end_mcells.begin(), end_mcells.end(), dcell1);
                                flag = 1;
                                if (it4 != end_mcells.end()) {
                                    // already used, add it, and end it
                                    // if (mcells_counter[dcell1]+1 < mcells_map[dcell1].size()){
                                    ctrack->AddMSCell(dcell1);
                                    //}
                                }
                            }
                        }
                        else if (it1 != ctrack->Get_allmcells().end() && it2 == ctrack->Get_allmcells().end()) {
                            // look at it2
                            auto it3 = find(used_mcells.begin(), used_mcells.end(), dcell2);
                            if (it3 == used_mcells.end()) {
                                // not used yet? add it
                                if (ctrack->AddMSCell(dcell2)) {
                                    used_mcells.push_back(dcell2);  // continue
                                    flag = 0;
                                }
                                else {
                                    flag = 1;
                                }
                            }
                            else {
                                auto it4 = find(end_mcells.begin(), end_mcells.end(), dcell2);
                                flag = 1;
                                if (it4 != end_mcells.end()) {
                                    // already used, add it, and end it
                                    // if (mcells_counter[dcell2]+1 < mcells_map[dcell2].size()){
                                    ctrack->AddMSCell(dcell2);
                                    //}
                                }
                            }
                        }
                        else if (it1 != ctrack->Get_allmcells().end() && it2 != ctrack->Get_allmcells().end()) {
                            flag = 1;
                        }
                    }
                }

                if (num_daughter > 2) {
                    // two cases,    just one element
                    if (ctrack->Get_allmcells().size() == 1) {
                        for (int i = 0; i != num_daughter; i++) {
                            MergeSpaceCell *dcell = mcells_map[cur_cell].at(i);
                            auto it1 = find(ctrack->Get_allmcells().begin(), ctrack->Get_allmcells().end(), dcell);

                            // std::cout << "Xin2: " <<it1 - ctrack->Get_allmcells().end() << std::endl;

                            if (it1 == ctrack->Get_allmcells().end()) {
                                // not used yet
                                auto it2 = find(used_mcells.begin(), used_mcells.end(), dcell);
                                // std::cout << "Xin3: " << it2 - used_mcells.end() << std::endl;
                                if (it2 == used_mcells.end()) {
                                    // not used yet? add it
                                    if (ctrack->AddMSCell(dcell)) {
                                        used_mcells.push_back(dcell);  // continue
                                        flag = 0;
                                        break;
                                    }
                                    else {
                                        flag = 1;
                                    }
                                }
                                else {
                                    auto it3 = find(end_mcells.begin(), end_mcells.end(), dcell);
                                    flag = 1;
                                    if (it3 != end_mcells.end()) {
                                        // already used, add it, and end it
                                        //		  std::cout << mcells_counter[dcell] << " " <<
                                        //mcells_map[dcell].size() << std::endl;
                                        auto it4 =
                                            find(mcells_save[cur_cell].begin(), mcells_save[cur_cell].end(), dcell);
                                        if (it4 == mcells_save[cur_cell].end()) {
                                            // if (mcells_counter[dcell]+1 < mcells_map[dcell].size()){
                                            if (ctrack->AddMSCell(dcell)) {
                                                break;
                                            }
                                        }
                                    }
                                }
                            }
                            else {
                                flag = 1;
                            }

                            // std::cout << i << " " << flag << std::endl;
                        }
                    }
                    else {
                        flag = 1;  // end
                    }
                }
            }  // while(flag==0)

            // deal with the last element
            if (ctrack->Get_allmcells().size() > 1) {
                auto it = find(end_mcells.begin(), end_mcells.end(), ctrack->Get_LastMSCell());
                if (it == end_mcells.end()) end_mcells.push_back(ctrack->Get_LastMSCell());
            }

            if (ctrack->Get_allmcells().size() != 1) {
                MergeSpaceCell *fcell = ctrack->Get_FirstMSCell();
                MergeSpaceCell *fcell_next = ctrack->Get_allmcells().at(1);

                MergeSpaceCell *lcell = ctrack->Get_LastMSCell();
                MergeSpaceCell *lcell_next = ctrack->Get_allmcells().at(ctrack->Get_allmcells().size() - 2);

                mcells_save[fcell].push_back(fcell_next);
                mcells_save[lcell].push_back(lcell_next);
            }
            else {
                MergeSpaceCell *fcell = ctrack->Get_FirstMSCell();
                for (int qq = 0; qq != mcells_map[fcell].size(); qq++) {
                    mcells_save[fcell].push_back(mcells_map[fcell].at(qq));
                }
            }

            all_clustertrack.push_back(ctrack);
        }
    }  // while loop
}

void WCP2dToy::ToyCrawler::FormGraph()
{
    ct_ms_map.clear();
    ms_ct_map.clear();

    for (int i = 0; i != all_clustertrack.size(); i++) {
        ClusterTrack *ctrack = all_clustertrack.at(i);
        MergeSpaceCell *FMSCell = ctrack->Get_FirstMSCell();
        MergeSpaceCell *LMSCell = ctrack->Get_LastMSCell();

        MergeSpaceCellSelection temp;
        temp.push_back(FMSCell);
        temp.push_back(LMSCell);
        ct_ms_map[ctrack] = temp;

        if (ms_ct_map.find(FMSCell) == ms_ct_map.end()) {
            ClusterTrackSelection temp1;
            temp1.push_back(ctrack);
            ms_ct_map[FMSCell] = temp1;
        }
        else {
            ms_ct_map[FMSCell].push_back(ctrack);
        }

        if (ms_ct_map.find(LMSCell) == ms_ct_map.end()) {
            ClusterTrackSelection temp1;
            temp1.push_back(ctrack);
            ms_ct_map[LMSCell] = temp1;
        }
        else {
            ms_ct_map[LMSCell].push_back(ctrack);
        }
    }

    // need to expand the distance cut a little bit ...
}

WCP2dToy::ToyCrawler::~ToyCrawler()
{
    for (int i = 0; i != all_clustertrack.size(); i++) {
        delete all_clustertrack.at(i);
    }
    for (int i = 0; i != all_mergeclustertrack.size(); i++) {
        delete all_mergeclustertrack.at(i);
    }
    all_mergeclustertrack.clear();
    all_clustertrack.clear();
}
