#include "LNS.h"
#include "ECBS.h"
#include <queue>
#include<boost/tokenizer.hpp>
#include <utility>
#include "lacam2/lacam2.hpp"

LNS::LNS(const Instance& instance, double time_limit, string init_algo_name, string replan_algo_name,
         const string & destory_name, int neighbor_size, int num_of_iterations, bool use_init_lns,
         string init_destory_name, bool use_sipp, bool truncate_initial_paths,
         int screen, PIBTPPS_option pipp_option) :
         BasicLNS(instance, time_limit, neighbor_size, screen),
         init_algo_name(std::move(init_algo_name)),  replan_algo_name(std::move(replan_algo_name)),
         num_of_iterations(num_of_iterations),
         use_init_lns(use_init_lns), init_destory_name(std::move(init_destory_name)),
         truncate_initial_paths(truncate_initial_paths),
         path_table(instance.map_size), pipp_option(pipp_option)
{
    start_time = Time::now();
    //replan_time_limit = time_limit / 100;
    replan_time_limit = time_limit;
    if (destory_name == "Adaptive")
    {
        ALNS = true;
        destroy_weights.assign(DESTORY_COUNT, 1);
        decay_factor = 0.01;
        reaction_factor = 0.01;
    }
    else if (destory_name == "RandomWalk")
        destroy_strategy = RANDOMWALK;
    else if (destory_name == "Intersection")
        destroy_strategy = INTERSECTION;
    else if (destory_name == "Random")
        destroy_strategy = RANDOMAGENTS;
    else
    {
        cerr << "Destroy heuristic " << destory_name << " does not exists. " << endl;
        exit(-1);
    }

    int N = instance.getDefaultNumberOfAgents();
    agents.reserve(N);
    stay_target.resize(N);

    for (int i = 0; i < N; i++)
    {
        agents.emplace_back(instance, i, use_sipp);
        stay_target[i] = 0;
    }
        
    
    preprocessing_time = ((fsec)(Time::now() - start_time)).count();
    if (screen >= 2)
        cout << "Pre-processing time = " << preprocessing_time << " seconds." << endl;
    
}

bool LNS::run()
{
    // only for statistic analysis, and thus is not included in runtime
    sum_of_distances = 0;
    for (const auto & agent : agents)
    {
        sum_of_distances += agent.path_planner->my_heuristic[agent.path_planner->start_location];
    }
    num_ll_search = 0;
    sum_ll_time = 0.0;

    initial_solution_runtime = 0;
    start_time = Time::now();
    bool succ;
    if (has_initial_solution)
        //succ = fixInitialSolution();
        succ = true;
    else
        succ = getInitialSolution();
    bool initial_succ = succ;
    initial_solution_runtime = ((fsec)(Time::now() - start_time)).count();
    // if (!succ && initial_solution_runtime < time_limit)
    // {
    //     if (use_init_lns)
    //     {
    //         init_lns = new InitLNS(instance, agents, time_limit - initial_solution_runtime,
    //                 replan_algo_name,init_destory_name, neighbor_size, screen);
    //         succ = init_lns->run();
    //         if (succ) // accept new paths
    //         {
    //             path_table.reset();
    //             for (const auto & agent : agents)
    //             {
    //                 path_table.insertPath(agent.id, agent.path);
    //             }
    //             init_lns->clear();
    //             initial_sum_of_costs = init_lns->sum_of_costs;
    //             sum_of_costs = initial_sum_of_costs;
    //         }
    //         initial_solution_runtime = ((fsec)(Time::now() - start_time)).count();
    //     }
    //     else // use random restart
    //     {
    //         while (!succ && initial_solution_runtime < time_limit)
    //         {
    //             succ = getInitialSolution();
    //             initial_solution_runtime = ((fsec)(Time::now() - start_time)).count();
    //             restart_times++;
    //         }
    //     }
    // }

    iteration_stats.emplace_back(neighbor.agents.size(),
                                 initial_sum_of_costs, initial_solution_runtime, init_algo_name);
    runtime = initial_solution_runtime;
    if (succ)
    {
        //if (screen >= 1)
            cout << "Initial solution cost = " << initial_sum_of_costs << ", "
                 << "runtime = " << initial_solution_runtime << endl;
    }
    else
    {
        cout << "Failed to find an initial solution in "
             << runtime << " seconds after  " << restart_times << " restarts" << endl;
        return false; // terminate because no initial solution is found
    }

    while (runtime < time_limit && iteration_stats.size() <= num_of_iterations)
    {
        int a = -1;
        int max_delays = -1;
        for (int i = 0; i < agents.size(); i++)
        {
            int delays = agents[i].getNumOfDelays();
            if (max_delays < delays)
            {
                a = i;
                max_delays = delays;
            }
        }
        if (max_delays == 0)
        {
            break; //early termination
        }

        // int settings = 0;
        // if (sum_of_costs <= settings)
        // {
        //     break;
        // }
        runtime =((fsec)(Time::now() - start_time)).count();
        if(screen >= 1)
            validateSolution();
        if (ALNS)
            chooseDestroyHeuristicbyALNS();

        switch (destroy_strategy)
        {
            case RANDOMWALK:
                succ = generateNeighborByRandomWalk();
                break;
            case INTERSECTION:
                succ = generateNeighborByIntersection();
                break;
            case RANDOMAGENTS:
                neighbor.agents.resize(agents.size());
                for (int i = 0; i < (int)agents.size(); i++)
                    neighbor.agents[i] = i;
                if (neighbor.agents.size() > neighbor_size)
                {
                    std::random_shuffle(neighbor.agents.begin(), neighbor.agents.end());
                    neighbor.agents.resize(neighbor_size);
                }
                succ = true;
                break;
            default:
                cerr << "Wrong neighbor generation strategy" << endl;
                exit(-1);
        }
        if(!succ)
            continue;

        runtime =((fsec)(Time::now() - start_time)).count();
        if (runtime >= time_limit)
            break;

        // store the neighbor information
        neighbor.old_paths.resize(neighbor.agents.size());
        neighbor.old_sum_of_costs = 0;
        neighbor.old_sum_of_costs_target_stay = 0;
        for (int i = 0; i < (int)neighbor.agents.size(); i++)
        {
            if (replan_algo_name == "PP")
                neighbor.old_paths[i] = agents[neighbor.agents[i]].path;
            path_table.deletePath(neighbor.agents[i], agents[neighbor.agents[i]].path);
            neighbor.old_sum_of_costs += agents[neighbor.agents[i]].path.size() - 1;
            neighbor.old_sum_of_costs_target_stay += agents[neighbor.agents[i]].path.size() - 1;
            //cout<<stay_target[neighbor.agents[i]]<<" ";
            if (agents[neighbor.agents[i]].path.size() > 1 && stay_target[neighbor.agents[i]] > 0)
            {
                neighbor.old_sum_of_costs_target_stay += stay_target[neighbor.agents[i]];
            }
        }
        //cout<<endl;

        // if (replan_algo_name == "EECBS")
        //     succ = runEECBS();
        // else if (replan_algo_name == "CBS")
        //     succ = runCBS();
        // else if (replan_algo_name == "PP")
        succ = runPP();
        // else
        // {
        //     cerr << "Wrong replanning strategy" << endl;
        //     exit(-1);
        // }

        if (ALNS) // update destroy heuristics
        {
            // if (neighbor.old_sum_of_costs > neighbor.sum_of_costs )
            //     destroy_weights[selected_neighbor] =
            //             reaction_factor * (neighbor.old_sum_of_costs - neighbor.sum_of_costs) / neighbor.agents.size()
            //             + (1 - reaction_factor) * destroy_weights[selected_neighbor];
            if (neighbor.old_sum_of_costs_target_stay > neighbor.sum_of_costs_target_stay )
                destroy_weights[selected_neighbor] =
                        reaction_factor * (neighbor.old_sum_of_costs_target_stay - neighbor.sum_of_costs_target_stay) / neighbor.agents.size()
                        + (1 - reaction_factor) * destroy_weights[selected_neighbor];
            else
                destroy_weights[selected_neighbor] =
                        (1 - decay_factor) * destroy_weights[selected_neighbor];
        }
        runtime = ((fsec)(Time::now() - start_time)).count();
        sum_of_costs += neighbor.sum_of_costs - neighbor.old_sum_of_costs;
        if (screen >= 1)
            cout << "Iteration " << iteration_stats.size() << ", "
                 << "group size = " << neighbor.agents.size() << ", "
                 << "solution cost = " << sum_of_costs << ", "
                 << "remaining time = " << time_limit - runtime << endl;
        iteration_stats.emplace_back(neighbor.agents.size(), sum_of_costs, runtime, replan_algo_name);
    }


    average_group_size = - iteration_stats.front().num_of_agents;
    for (const auto& data : iteration_stats)
        average_group_size += data.num_of_agents;
    if (average_group_size > 0)
        average_group_size /= (double)(iteration_stats.size() - 1);

    cout << getSolverName() << ": "
         << "runtime = " << runtime << ", "
         << "deleted timesteps = " << delete_timesteps << ","
         << "iterations = " << iteration_stats.size() << ", "
         << "solution cost = " << sum_of_costs << ", "
         << "initial solution cost = " << initial_sum_of_costs << ", "
         << "failed iterations = " << num_of_failures << endl;
    cout<<"iterations details"<<endl;
    int time = 0;
    for (auto i: iteration_stats)
    {
        if (i.runtime >= time)
        {
            cout<<i.runtime<<" ";
            time++;
        }
    }
    cout<<endl;
    time = 0;
    for (auto i: iteration_stats)
    {
        if (i.runtime >= time)
        {
            cout<<i.sum_of_costs<<" ";
            time++;
        }
    }
    cout<<endl;
    return initial_succ;
}

void LNS::truncatePaths() // truncate paths to maximize the number of agents at goal locations
{
    int best_t = 0;
    int best_finished = 0;
    int makespan = 0;
    for (int t = 0;; t++)
    {
        int finished = 0;
        bool all_finished = true;
        for (const auto& agent : agents)
        {
            if (!agent.path.empty() and
                agent.path[min(t, (int)agent.path.size() - 1)].location == agent.path_planner->goal_location)
                finished++;
            if ((int)agent.path.size() > t + 1) all_finished = false;
        }
        if (finished > best_finished)
        {
            best_t = t;
            best_finished = finished;
        }
        if (all_finished)
        {
            makespan = t;
            if (screen == 2)
                cout << finished << " finished agents at the LAST timestep of " << t << endl;
            break;
        }
    }
    if (screen == 2)
        cout << best_finished << " finished agents at the BEST timestep of " << best_t << endl;
    if (best_t < makespan)
    {
        delete_timesteps += makespan - best_t;
        for (auto& agent : agents)
        {
            if ((int)agent.path.size() > best_t + 1) agent.path.resize(best_t + 1);
        }
    }
}
void LNS::deleteRepeatedStates() // if there are two timesteps when locations of all agents are the same, delete the subpaths in between
{
    unordered_map<vector<int>, int, container_hash<vector<int>>> states;
    for (int t = 0;; t++)
    {
        bool all_finished = true;
        vector<int> state(agents.size(), -1);
        for (const auto& agent : agents)
        {
            if (!agent.path.empty())
                state[agent.id] =  agent.path[min(t, (int)agent.path.size() - 1)].location;
            if ((int)agent.path.size() > t + 1) all_finished = false;
        }
        if (states.find(state) != states.end())
        {
            auto t1 = states[state];
            if (screen > 1)
                cout << "Delete paths between timesteps " << t1 << " and " << t << endl;
            delete_timesteps += t - t1;
            for (auto& agent : agents)
            {
                if ((int)agent.path.size() <= t1 + 1) continue;
                else if (agent.path.size() <= t) agent.path.resize(t1 + 1);
                else
                {
                    for (int t2 = 1; t2 < agent.path.size() - t; t2++)
                        agent.path[t1+t2] = agent.path[t+t2];
                    agent.path.resize(agent.path.size() - t + t1);
                }
                for (auto i = states.begin(), last = states.end(); i != last; )
                {
                    if (i->second > t1) i = states.erase(i);
                    else ++i;
                }
            }
            t = t1;
        }
        else
            states[state] = t;
        if (all_finished)
        {
            if (screen > 1)
            {
                cout << "Makespan is " << t << endl;
                for (const auto& a1_ : agents)
                {
                    for (int t = 1; t < (int) a1_.path.size(); t++ )
                    {
                        if (!instance.validMove(a1_.path[t - 1].location, a1_.path[t].location))
                        {
                            cerr << "The path of agent " << a1_.id << " jump from "
                                 << a1_.path[t - 1].location << " to " << a1_.path[t].location
                                 << " between timesteps " << t - 1 << " and " << t << endl;
                            exit(-1);
                        }
                    }
                }
            }
            break;
        }
    }
}
bool LNS::fixInitialSolution()
{
    neighbor.agents.clear();
    initial_sum_of_costs = 0;
    list<int> complete_agents; // subsets of agents who have complete and collision-free paths
    if (truncate_initial_paths)
    {
        truncatePaths();
        deleteRepeatedStates();
    }
    int makespan = 0;
    for (auto& agent : agents)
    {
        if (agent.path.empty() or agent.path.back().location != agent.path_planner->goal_location)
        {
            neighbor.agents.emplace_back(agent.id);
            agent.path.clear();
        }
        else
        {
            bool has_collision = false;
            for (auto other : complete_agents)
            {
                has_collision = instance.hasCollision(agent.path, agents[other].path);
                if (has_collision)
                {
                    neighbor.agents.emplace_back(agent.id);
                    break;
                }
            }
            if (!has_collision)
            {
                path_table.insertPath(agent.id, agent.path);
                complete_paths++;
                initial_sum_of_costs += (int)agent.path.size() - 1;
                complete_agents.emplace_back(agent.id);
                makespan = max(makespan, (int)agent.path.size() - 1);
            }

        }
    }
    if (screen == 2)
        cout << complete_agents.size() << " collision-free agents at timestep " << makespan << endl;
    neighbor.old_sum_of_costs = MAX_COST;
    neighbor.sum_of_costs = 0;
    auto succ = runPP();
    if (succ)
    {
        initial_sum_of_costs += neighbor.sum_of_costs;
        sum_of_costs = initial_sum_of_costs;
        return true;
    }
    else
    {
        return false;
    }
}

bool LNS::getInitialSolution()
{
    neighbor.agents.resize(agents.size());
    for (int i = 0; i < (int)agents.size(); i++)
        neighbor.agents[i] = i;
    neighbor.old_sum_of_costs = MAX_COST;
    neighbor.sum_of_costs = 0;
    bool succ = false;
    // if (init_algo_name == "EECBS")
    //     succ = runEECBS();
    // else if (init_algo_name == "PP")
    //     succ = runPP();
    // // else if (init_algo_name == "PIBT")
    // //     succ = runPIBT();
    // // else if (init_algo_name == "PPS")
    // //     succ = runPPS();
    // // else if (init_algo_name == "winPIBT")
    // //     succ = runWinPIBT();
    // else if (init_algo_name == "CBS")
    //     succ = runCBS();
    succ = runLACAM2();
    // else
    // {
    //     cerr <<  "Initial MAPF solver " << init_algo_name << " does not exist!" << endl;
    //     exit(-1);
    // }
    if (succ)
    {
        initial_sum_of_costs = neighbor.sum_of_costs;
        sum_of_costs = neighbor.sum_of_costs;
        return true;
    }
    else
    {
        initial_sum_of_costs = neighbor.sum_of_costs;
        sum_of_costs = neighbor.sum_of_costs;
        return false;
    }
}

bool LNS::runEECBS()
{
    vector<SingleAgentSolver*> search_engines;
    search_engines.reserve(neighbor.agents.size());
    for (int i : neighbor.agents)
    {
        search_engines.push_back(agents[i].path_planner);
    }

    ECBS ecbs(search_engines, screen - 1, &path_table);
    ecbs.setPrioritizeConflicts(true);
    ecbs.setDisjointSplitting(false);
    ecbs.setBypass(true);
    ecbs.setRectangleReasoning(true);
    ecbs.setCorridorReasoning(true);
    ecbs.setHeuristicType(heuristics_type::WDG, heuristics_type::GLOBAL);
    ecbs.setTargetReasoning(true);
    ecbs.setMutexReasoning(false);
    ecbs.setConflictSelectionRule(conflict_selection::EARLIEST);
    ecbs.setNodeSelectionRule(node_selection::NODE_CONFLICTPAIRS);
    ecbs.setSavingStats(false);
    double w;
    if (iteration_stats.empty())
        w = 5; // initial run
    else
        w = 1.1; // replan
    ecbs.setHighLevelSolver(high_level_solver_type::EES, w);
    runtime = ((fsec)(Time::now() - start_time)).count();
    double T = time_limit - runtime;
    if (!iteration_stats.empty()) // replan
        T = min(T, replan_time_limit);
    bool succ = ecbs.solve(T, 0);
    if (succ && ecbs.solution_cost < neighbor.old_sum_of_costs) // accept new paths
    {
        auto id = neighbor.agents.begin();
        for (size_t i = 0; i < neighbor.agents.size(); i++)
        {
            agents[*id].path = *ecbs.paths[i];
            path_table.insertPath(agents[*id].id, agents[*id].path);
            ++id;
        }
        neighbor.sum_of_costs = ecbs.solution_cost;
        if (sum_of_costs_lowerbound < 0)
            sum_of_costs_lowerbound = ecbs.getLowerBound();
    }
    else // stick to old paths
    {
        if (!neighbor.old_paths.empty())
        {
            for (int id : neighbor.agents)
            {
                path_table.insertPath(agents[id].id, agents[id].path);
            }
            neighbor.sum_of_costs = neighbor.old_sum_of_costs;
        }
        if (!succ)
            num_of_failures++;
    }
    return succ;
}
bool LNS::runCBS()
{
    if (screen >= 2)
        cout << "old sum of costs = " << neighbor.old_sum_of_costs << endl;
    vector<SingleAgentSolver*> search_engines;
    search_engines.reserve(neighbor.agents.size());
    for (int i : neighbor.agents)
    {
        search_engines.push_back(agents[i].path_planner);
    }

    CBS cbs(search_engines, screen - 1, &path_table);
    cbs.setPrioritizeConflicts(true);
    cbs.setDisjointSplitting(false);
    cbs.setBypass(true);
    cbs.setRectangleReasoning(true);
    cbs.setCorridorReasoning(true);
    cbs.setHeuristicType(heuristics_type::WDG, heuristics_type::ZERO);
    cbs.setTargetReasoning(true);
    cbs.setMutexReasoning(false);
    cbs.setConflictSelectionRule(conflict_selection::EARLIEST);
    cbs.setNodeSelectionRule(node_selection::NODE_CONFLICTPAIRS);
    cbs.setSavingStats(false);
    cbs.setHighLevelSolver(high_level_solver_type::ASTAR, 1);
    runtime = ((fsec)(Time::now() - start_time)).count();
    double T = time_limit - runtime; // time limit
    if (!iteration_stats.empty()) // replan
        T = min(T, replan_time_limit);
    bool succ = cbs.solve(T, 0);
    if (succ && cbs.solution_cost <= neighbor.old_sum_of_costs) // accept new paths
    {
        auto id = neighbor.agents.begin();
        for (size_t i = 0; i < neighbor.agents.size(); i++)
        {
            agents[*id].path = *cbs.paths[i];
            path_table.insertPath(agents[*id].id, agents[*id].path);
            ++id;
        }
        neighbor.sum_of_costs = cbs.solution_cost;
        if (sum_of_costs_lowerbound < 0)
            sum_of_costs_lowerbound = cbs.getLowerBound();
    }
    else // stick to old paths
    {
        if (!neighbor.old_paths.empty())
        {
            for (int id : neighbor.agents)
            {
                path_table.insertPath(agents[id].id, agents[id].path);
            }
            neighbor.sum_of_costs = neighbor.old_sum_of_costs;

        }
        if (!succ)
            num_of_failures++;
    }
    return succ;
}
bool LNS::runPP()
{
    auto shuffled_agents = neighbor.agents;
    std::random_shuffle(shuffled_agents.begin(), shuffled_agents.end());
    if (screen >= 2) {
        for (auto id : shuffled_agents)
            cout << id << "(" << agents[id].path_planner->my_heuristic[agents[id].path_planner->start_location] <<
                "->" << agents[id].path.size() - 1 << "), ";
        cout << endl;
    }
    int remaining_agents = (int)shuffled_agents.size();
    auto p = shuffled_agents.begin();
    neighbor.sum_of_costs = 0;
    neighbor.sum_of_costs_target_stay = 0;
    runtime = ((fsec)(Time::now() - start_time)).count();
    double T = time_limit - runtime; // time limit
    // if (!iteration_stats.empty()) // replan
    //     T = min(T, replan_time_limit);
    auto time = Time::now();
    ConstraintTable constraint_table(instance.num_of_cols, instance.map_size, &path_table);
    while (p != shuffled_agents.end() && ((fsec)(Time::now() - time)).count() < T)
    {
        int id = *p;
        if (screen >= 3)
            cout << "Remaining agents = " << remaining_agents <<
                 ", remaining time = " << T - ((fsec)(Time::now() - time)).count() << " seconds. " << endl
                 << "Agent " << agents[id].id << endl;
        agents[id].path = agents[id].path_planner->findPath(constraint_table);
        num_ll_search++;
    
        if (agents[id].path.empty()) 
        {
            if (screen >= 3)
                cout<<"pp failed with remaining agents = " << remaining_agents<<endl;
            break;
        }
        neighbor.sum_of_costs += (int)agents[id].path.size() - 1;
        neighbor.sum_of_costs_target_stay  += (int)agents[id].path.size() - 1;
        if ((int)agents[id].path.size() > 1 && stay_target[id] > 0)
        {
            neighbor.sum_of_costs_target_stay  += stay_target[id];
        }
        //if (neighbor.sum_of_costs >= neighbor.old_sum_of_costs)
         if (neighbor.sum_of_costs_target_stay >= neighbor.old_sum_of_costs_target_stay)
        {
            if (screen >= 3)
                cout<<"solution worse"<<endl;
            break;
        }
        remaining_agents--;
        path_table.insertPath(agents[id].id, agents[id].path);
        ++p;
    }
    sum_ll_time += ((fsec)(Time::now() - time)).count();

    //if (remaining_agents == 0 && neighbor.sum_of_costs <= neighbor.old_sum_of_costs) // accept new paths
    if (remaining_agents == 0 && neighbor.sum_of_costs_target_stay <= neighbor.old_sum_of_costs_target_stay) // accept new paths
    {
        return true;
    }
    else // stick to old paths
    {
        if (p != shuffled_agents.end())
            num_of_failures++;
        auto p2 = shuffled_agents.begin();
        while (p2 != p)
        {
            int a = *p2;
            path_table.deletePath(agents[a].id, agents[a].path);
            ++p2;
        }
        if (!neighbor.old_paths.empty())
        {
            p2 = neighbor.agents.begin();
            for (int i = 0; i < (int)neighbor.agents.size(); i++)
            {
                int a = *p2;
                agents[a].path = neighbor.old_paths[i];
                path_table.insertPath(agents[a].id, agents[a].path);
                ++p2;
            }
            neighbor.sum_of_costs = neighbor.old_sum_of_costs;
            neighbor.sum_of_costs_target_stay = neighbor.old_sum_of_costs_target_stay;
        }
        return false;
    }
}
// bool LNS::runPPS(){
//     auto shuffled_agents = neighbor.agents;
//     std::random_shuffle(shuffled_agents.begin(), shuffled_agents.end());

//     MAPF P = preparePIBTProblem(shuffled_agents);
//     P.setTimestepLimit(pipp_option.timestepLimit);

//     // seed for solver
//     auto* MT_S = new std::mt19937(0);
//     PPS solver(&P,MT_S);
//     solver.setTimeLimit(time_limit);
// //    solver.WarshallFloyd();
//     bool result = solver.solve();
//     if (result)
//         updatePIBTResult(P.getA(),shuffled_agents);
//     return result;
// }
// bool LNS::runPIBT(){
//     auto shuffled_agents = neighbor.agents;
//      std::random_shuffle(shuffled_agents.begin(), shuffled_agents.end());

//     MAPF P = preparePIBTProblem(shuffled_agents);

//     // seed for solver
//     auto MT_S = new std::mt19937(0);
//     PIBT solver(&P,MT_S);
//     solver.setTimeLimit(time_limit);
//     bool result = solver.solve();
//     if (result)
//         updatePIBTResult(P.getA(),shuffled_agents);
//     return result;
// }
// bool LNS::runWinPIBT(){
//     auto shuffled_agents = neighbor.agents;
//     std::random_shuffle(shuffled_agents.begin(), shuffled_agents.end());

//     MAPF P = preparePIBTProblem(shuffled_agents);
//     P.setTimestepLimit(pipp_option.timestepLimit);

//     // seed for solver
//     auto MT_S = new std::mt19937(0);
//     winPIBT solver(&P,pipp_option.windowSize,pipp_option.winPIBTSoft,MT_S);
//     solver.setTimeLimit(time_limit);
//     bool result = solver.solve();
//     if (result)
//         updatePIBTResult(P.getA(),shuffled_agents);
//     return result;
// }

// MAPF LNS::preparePIBTProblem(vector<int>& shuffled_agents){

//     // seed for problem and graph
//     auto MT_PG = new std::mt19937(0);

// //    Graph* G = new SimpleGrid(instance);
//     Graph* G = new SimpleGrid(instance.getMapFile());

//     std::vector<Task*> T;
//     PIBT_Agents A;

//     for (int i : shuffled_agents){
//         assert(G->existNode(agents[i].path_planner->start_location));
//         assert(G->existNode(agents[i].path_planner->goal_location));
//         auto a = new PIBT_Agent(G->getNode( agents[i].path_planner->start_location));

// //        PIBT_Agent* a = new PIBT_Agent(G->getNode( agents[i].path_planner.start_location));
//         A.push_back(a);
//         Task* tau = new Task(G->getNode( agents[i].path_planner->goal_location));


//         T.push_back(tau);
//         if(screen>=5){
//             cout<<"Agent "<<i<<" start: " <<a->getNode()->getPos()<<" goal: "<<tau->getG().front()->getPos()<<endl;
//         }
//     }

//     return MAPF(G, A, T, MT_PG);

// }

// void LNS::updatePIBTResult(const PIBT_Agents& A, vector<int>& shuffled_agents){
//     int soc = 0;
//     for (int i=0; i<A.size();i++){
//         int a_id = shuffled_agents[i];

//         agents[a_id].path.resize(A[i]->getHist().size());
//         int last_goal_visit = 0;
//         if(screen>=2)
//             std::cout<<A[i]->logStr()<<std::endl;
//         for (int n_index = 0; n_index < A[i]->getHist().size(); n_index++){
//             auto n = A[i]->getHist()[n_index];
//             agents[a_id].path[n_index] = PathEntry(n->v->getId());

//             //record the last time agent reach the goal from a non-goal vertex.
//             if(agents[a_id].path_planner->goal_location == n->v->getId()
//                 && n_index - 1>=0
//                 && agents[a_id].path_planner->goal_location !=  agents[a_id].path[n_index - 1].location)
//                 last_goal_visit = n_index;

//         }
//         //resize to last goal visit time
//         agents[a_id].path.resize(last_goal_visit + 1);
//         if(screen>=2)
//             std::cout<<" Length: "<< agents[a_id].path.size() <<std::endl;
//         if(screen>=5){
//             cout <<"Agent "<<a_id<<":";
//             for (auto loc : agents[a_id].path){
//                 cout <<loc.location<<",";
//             }
//             cout<<endl;
//         }
//         path_table.insertPath(agents[a_id].id, agents[a_id].path);
//         soc += (int)agents[a_id].path.size()-1;
//     }

//     neighbor.sum_of_costs =soc;
// }


bool LNS::runLACAM2() 
{
    vector<int> shuffled_agents = neighbor.agents;
    std::random_shuffle(shuffled_agents.begin(), shuffled_agents.end());
    
    string map_fname = instance.getMapFile();
    string scen_fname = instance.getInstanceName();
    uint num_of_agents = instance.getDefaultNumberOfAgents();
    //LACAMInstance ins = LACAMInstance(scen_fname, map_fname, num_of_agents);
    LACAMInstance ins = LACAMInstance(map_fname, instance.getStarts(), instance.getGoals());
    string verbose = "1";
    auto MT = std::mt19937(0);
    const auto deadline = Deadline(time_limit * 1000);

    const Objective objective = static_cast<Objective>(1);
    const float restart_rate = 0.01;
    const auto solution = solve(ins, verbose, 0, &deadline, &MT, objective, restart_rate);

    if (solution.empty()) 
        cout<<"empty"<<endl;

    bool solved = true;
    for (int agent = 0; agent < num_of_agents; agent++)
    {
        if (solution.back()[agent]->index != instance.getGoals()[agent])
        {
            solved = false; //find not stay at target
            break;
        }
    }

    int soc = 0;
    for (int agent = 0; agent < num_of_agents; agent++)
    {
        size_t max_time = solution.size()-1;
        for (; max_time > 0; max_time--)
        {
            if (solution[max_time][agent]->index != solution[max_time-1][agent]->index)
                break;
        }
        agents[agent].path.resize(max_time+1);
        for (size_t t = 0; t <= max_time; t++)
        {
            agents[agent].path[t].location = solution[t][agent]->index;
        }
        path_table.insertPath(agents[agent].id, agents[agent].path);
        soc+=agents[agent].path.size()-1;
    }

    neighbor.sum_of_costs =soc;

    if (solved)
    {
        return true;
    }
    else
    {
        return false;
    }
    
}

void LNS::chooseDestroyHeuristicbyALNS()
{
    rouletteWheel();
    switch (selected_neighbor)
    {
        case 0 : destroy_strategy = RANDOMWALK; break;
        case 1 : destroy_strategy = INTERSECTION; break;
        case 2 : destroy_strategy = RANDOMAGENTS; break;
        default : cerr << "ERROR" << endl; exit(-1);
    }
}

bool LNS::generateNeighborByIntersection()
{
    if (intersections.empty())
    {
        for (int i = 0; i < instance.map_size; i++)
        {
            if (!instance.isObstacle(i) && instance.getDegree(i) > 2)
                intersections.push_back(i);
        }
    }

    set<int> neighbors_set;
    auto pt = intersections.begin();
    std::advance(pt, rand() % intersections.size());
    int location = *pt;
    //path_table.get_agents(neighbors_set, neighbor_size, location);
    path_table.get_agents_with_stay_target(neighbors_set, stay_target, neighbor_size, location);
    if (neighbors_set.size() < neighbor_size)
    {
        set<int> closed;
        closed.insert(location);
        std::queue<int> open;
        open.push(location);
        while (!open.empty() && (int) neighbors_set.size() < neighbor_size)
        {
            int curr = open.front();
            open.pop();
            for (auto next : instance.getNeighbors(curr))
            {
                if (closed.count(next) > 0)
                    continue;
                open.push(next);
                closed.insert(next);
                if (instance.getDegree(next) >= 3)
                {
                    //path_table.get_agents(neighbors_set, neighbor_size, next);
                    path_table.get_agents_with_stay_target(neighbors_set, stay_target, neighbor_size, next);
                    if ((int) neighbors_set.size() == neighbor_size)
                        break;
                }
            }
        }
    }
    neighbor.agents.assign(neighbors_set.begin(), neighbors_set.end());
    if (neighbor.agents.size() > neighbor_size)
    {
        std::random_shuffle(neighbor.agents.begin(), neighbor.agents.end());
        neighbor.agents.resize(neighbor_size);
    }
    if (screen >= 2)
        cout << "Generate " << neighbor.agents.size() << " neighbors by intersection " << location << endl;
    return true;
}

bool LNS::generateNeighborByRandomWalk()
{
    if (neighbor_size >= (int)agents.size())
    {
        neighbor.agents.resize(agents.size());
        for (int i = 0; i < (int)agents.size(); i++)
            neighbor.agents[i] = i;
        return true;
    }

    int a = findMostDelayedAgent();
    if (a < 0)
        return false;
    
    set<int> neighbors_set;
    neighbors_set.insert(a);
    randomWalk(a, agents[a].path[0].location, 0, neighbors_set, neighbor_size, (int) agents[a].path.size() - 1);
    int count = 0;
    while (neighbors_set.size() < neighbor_size && count < 10)
    {
        int t = rand() % agents[a].path.size();
        if (target_considered)
            randomWalkwithStayTarget(a, agents[a].path[t].location, t, neighbors_set, neighbor_size, (int) agents[a].path.size() - 1);
        else
            randomWalk(a, agents[a].path[t].location, t, neighbors_set, neighbor_size, (int) agents[a].path.size() - 1);
        count++;
        // select the next agent randomly
        int idx = rand() % neighbors_set.size();
        int i = 0;
        for (auto n : neighbors_set)
        {
            if (i == idx)
            {
                a = i;
                break;
            }
            i++;
        }
    }
    if (neighbors_set.size() < 2)
        return false;
    neighbor.agents.assign(neighbors_set.begin(), neighbors_set.end());
    if (screen >= 2)
        cout << "Generate " << neighbor.agents.size() << " neighbors by random walks of agent " << a
             << "(" << agents[a].path_planner->my_heuristic[agents[a].path_planner->start_location]
             << "->" << agents[a].path.size() - 1 << ")" << endl;

    return true;
}

int LNS::findMostDelayedAgent()
{
    int a = -1;
    int max_delays = -1;
    for (int i = 0; i < agents.size(); i++)
    {
        if (tabu_list.find(i) != tabu_list.end())
            continue;
        int delays = agents[i].getNumOfDelays();
        if (max_delays < delays)
        {
            a = i;
            max_delays = delays;
        }
    }
    if (max_delays == 0)
    {
        tabu_list.clear();
        return -1;
    }
    tabu_list.insert(a);
    if (tabu_list.size() == agents.size())
        tabu_list.clear();
    return a;
}

int LNS::findRandomAgent() const
{
    int a = 0;
    int pt = rand() % (sum_of_costs - sum_of_distances) + 1;
    int sum = 0;
    for (; a < (int) agents.size(); a++)
    {
        sum += agents[a].getNumOfDelays();
        if (sum >= pt)
            break;
    }
    assert(sum >= pt);
    return a;
}

// a random walk with path that is shorter than upperbound and has conflicting with neighbor_size agents
void LNS::randomWalk(int agent_id, int start_location, int start_timestep,
                     set<int>& conflicting_agents, int neighbor_size, int upperbound)
{
    int loc = start_location;
    for (int t = start_timestep; t < upperbound; t++)
    {
        auto next_locs = instance.getNeighbors(loc);
        next_locs.push_back(loc);
        while (!next_locs.empty())
        {
            int step = rand() % next_locs.size();
            auto it = next_locs.begin();
            advance(it, step);
            int next_h_val = agents[agent_id].path_planner->my_heuristic[*it];
            if (t + 1 + next_h_val < upperbound) // move to this location
            {
                path_table.getConflictingAgents(agent_id, conflicting_agents, loc, *it, t + 1);
                loc = *it;
                break;
            }
            next_locs.erase(it);
        }
        if (next_locs.empty() || conflicting_agents.size() >= neighbor_size)
            break;
    }
}

void LNS::randomWalkwithStayTarget(int agent_id, int start_location, int start_timestep,
                     set<int>& conflicting_agents, int neighbor_size, int upperbound)
{
    int loc = start_location;
    for (int t = start_timestep; t < upperbound; t++)
    {
        auto next_locs = instance.getNeighbors(loc);
        next_locs.push_back(loc);
        while (!next_locs.empty())
        {
            int step = rand() % next_locs.size();
            auto it = next_locs.begin();
            advance(it, step);
            int next_h_val = agents[agent_id].path_planner->my_heuristic[*it];
            if (t + 1 + next_h_val < upperbound) // move to this location
            {
                //path_table.getConflictingAgents(agent_id, conflicting_agents, loc, *it, t + 1);
                path_table.getConflictingAgentswithStayTarget(agent_id, conflicting_agents,stay_target, loc, *it, t + 1);
                loc = *it;
                break;
            }
            next_locs.erase(it);
        }
        if (next_locs.empty() || conflicting_agents.size() >= neighbor_size)
            break;
    }
}

void LNS::validateSolution() const
{
    int sum = 0;
    for (const auto& a1_ : agents)
    {
        if (a1_.path.empty())
        {
            cerr << "No solution for agent " << a1_.id << endl;
            exit(-1);
        }
        else if (a1_.path_planner->start_location != a1_.path.front().location)
        {
            cerr << "The path of agent " << a1_.id << " starts from location " << a1_.path.front().location
                << ", which is different from its start location " << a1_.path_planner->start_location << endl;
            exit(-1);
        }
        else if (a1_.path_planner->goal_location != a1_.path.back().location)
        {
            cerr << "The path of agent " << a1_.id << " ends at location " << a1_.path.back().location
                 << ", which is different from its goal location " << a1_.path_planner->goal_location << endl;
            exit(-1);
        }
        for (int t = 1; t < (int) a1_.path.size(); t++ )
        {
            if (!instance.validMove(a1_.path[t - 1].location, a1_.path[t].location))
            {
                cerr << "The path of agent " << a1_.id << " jump from "
                     << a1_.path[t - 1].location << " to " << a1_.path[t].location
                     << " between timesteps " << t - 1 << " and " << t << endl;
                exit(-1);
            }
        }
        sum += (int) a1_.path.size() - 1;
        for (const auto  & a2_: agents)
        {
            if (a1_.id >= a2_.id || a2_.path.empty())
                continue;
            const auto & a1 = a1_.path.size() <= a2_.path.size()? a1_ : a2_;
            const auto & a2 = a1_.path.size() <= a2_.path.size()? a2_ : a1_;
            int t = 1;
            for (; t < (int) a1.path.size(); t++)
            {
                if (a1.path[t].location == a2.path[t].location) // vertex conflict
                {
                    cerr << "Find a vertex conflict between agents " << a1.id << " and " << a2.id <<
                            " at location " << a1.path[t].location << " at timestep " << t << endl;
                    exit(-1);
                }
                else if (a1.path[t].location == a2.path[t - 1].location &&
                        a1.path[t - 1].location == a2.path[t].location) // edge conflict
                {
                    cerr << "Find an edge conflict between agents " << a1.id << " and " << a2.id <<
                         " at edge (" << a1.path[t - 1].location << "," << a1.path[t].location <<
                         ") at timestep " << t << endl;
                    exit(-1);
                }
            }
            int target = a1.path.back().location;
            for (; t < (int) a2.path.size(); t++)
            {
                if (a2.path[t].location == target)  // target conflict
                {
                    cerr << "Find a target conflict where agent " << a2.id << " (of length " << a2.path.size() - 1<<
                         ") traverses agent " << a1.id << " (of length " << a1.path.size() - 1<<
                         ")'s target location " << target << " at timestep " << t << endl;
                    exit(-1);
                }
            }
        }
    }
    if (sum_of_costs != sum)
    {
        cerr << "The computed sum of costs " << sum_of_costs <<
             " is different from the sum of the paths in the solution " << sum << endl;
        exit(-1);
    }
}

void LNS::writeIterStatsToFile(const string & file_name) const
{
    if (init_lns != nullptr)
    {
        init_lns->writeIterStatsToFile(file_name + "-initLNS.csv");
    }
    if (iteration_stats.size() <= 1)
        return;
    string name = file_name;
    if (use_init_lns or num_of_iterations > 0)
        name += "-LNS.csv";
    else
        name += "-" + init_algo_name + ".csv";
    std::ofstream output;
    output.open(name);
    // header
    output << "num of agents," <<
           "sum of costs," <<
           "runtime," <<
           "cost lowerbound," <<
           "sum of distances," <<
           "MAPF algorithm" << endl;

    for (const auto &data : iteration_stats)
    {
        output << data.num_of_agents << "," <<
               data.sum_of_costs << "," <<
               data.runtime << "," <<
               max(sum_of_costs_lowerbound, sum_of_distances) << "," <<
               sum_of_distances << "," <<
               data.algorithm << endl;
    }
    output.close();
}

void LNS::writeResultToFile(const string & file_name) const
{
    if (init_lns != nullptr)
    {
        init_lns->writeResultToFile(file_name + "-initLNS.csv", sum_of_distances, preprocessing_time);
    }
    string name = file_name;
    if (use_init_lns or num_of_iterations > 0)
        name += "-LNS.csv";
    else
        name += "-" + init_algo_name + ".csv";
    std::ifstream infile(name);
    bool exist = infile.good();
    infile.close();
    if (!exist)
    {
        ofstream addHeads(name);
        addHeads << "runtime,solution cost,initial solution cost,lower bound,sum of distance," <<
                 "initial collision-free paths,delete timesteps,iterations," <<
                 "group size," <<
                 "runtime of initial solution,restart times,area under curve," <<
                 "LL expanded nodes,LL generated,LL reopened,LL runs," <<
                 "preprocessing runtime,solver name,instance name" << endl;
        addHeads.close();
    }
    uint64_t num_LL_expanded = 0, num_LL_generated = 0, num_LL_reopened = 0, num_LL_runs = 0;
    for (auto & agent : agents)
    {
        agent.path_planner->reset();
        num_LL_expanded += agent.path_planner->accumulated_num_expanded;
        num_LL_generated += agent.path_planner->accumulated_num_generated;
        num_LL_reopened += agent.path_planner->accumulated_num_reopened;
        num_LL_runs += agent.path_planner->num_runs;
    }
    double auc = 0;
    if (!iteration_stats.empty())
    {
        auto prev = iteration_stats.begin();
        auto curr = prev;
        ++curr;
        while (curr != iteration_stats.end() && curr->runtime < time_limit)
        {
            auc += (prev->sum_of_costs - sum_of_distances) * (curr->runtime - prev->runtime);
            prev = curr;
            ++curr;
        }
        auc += (prev->sum_of_costs - sum_of_distances) * (time_limit - prev->runtime);
    }
    ofstream stats(name, std::ios::app);
    stats << runtime << "," << sum_of_costs << "," << initial_sum_of_costs << "," <<
          max(sum_of_distances, sum_of_costs_lowerbound) << "," << sum_of_distances << "," <<
          complete_paths << "," << delete_timesteps << "," <<
          iteration_stats.size() << "," << average_group_size << "," <<
          initial_solution_runtime << "," << restart_times << "," << auc << "," <<
          num_LL_expanded << "," << num_LL_generated << "," << num_LL_reopened << "," << num_LL_runs << "," <<
          preprocessing_time << "," << getSolverName() << "," << instance.getInstanceName() << endl;
    stats.close();
}

bool LNS::loadPaths(const string & file_name)
{
    using namespace std;

    string line;
    ifstream myfile (file_name.c_str());
    if (!myfile.is_open())
        return false;

    while(getline(myfile, line))
    {
        boost::char_separator<char> sep(":()-> ,");
        boost::tokenizer< boost::char_separator<char> > tok(line, sep);
        auto beg = tok.begin();
        beg++; // skip "Agent"
        int agent_id = atoi((*beg).c_str());
        assert(0 <= agent_id < agents.size());
        beg++;
        while (beg != tok.end())
        {
            int col = atoi((*beg).c_str());
            beg++;
            int row = atoi((*beg).c_str());
            beg++;
            agents[agent_id].path.emplace_back(instance.linearizeCoordinate(row, col));
        }
        if (agents[agent_id].path.front().location != agents[agent_id].path_planner->start_location)
        {
            cerr << "Agent " << agent_id <<"'s path starts at " << agents[agent_id].path.front().location
            << "=(" << instance.getColCoordinate(agents[agent_id].path.front().location)
            << "," << instance.getRowCoordinate(agents[agent_id].path.front().location)
            << "), which is different from its start location " << agents[agent_id].path_planner->start_location << endl
            << "=(" << instance.getColCoordinate(agents[agent_id].path_planner->start_location)
            << "," << instance.getRowCoordinate(agents[agent_id].path_planner->start_location)
            << ")" << endl;
            exit(-1);
        }
    }
    myfile.close();
    has_initial_solution = true;
    return true;
}

bool LNS::loadPaths(vector<list<int>> paths)
{
    for (int agent_id = 0; agent_id < instance.getDefaultNumberOfAgents(); agent_id++)
    {
        for(auto location: paths[agent_id])
        {
            agents[agent_id].path.emplace_back(location);
        }
        if (agents[agent_id].path.front().location != agents[agent_id].path_planner->start_location)
        {
            cerr << "Agent " << agent_id <<"'s path starts at " << agents[agent_id].path.front().location
            << "=(" << instance.getColCoordinate(agents[agent_id].path.front().location)
            << "," << instance.getRowCoordinate(agents[agent_id].path.front().location)
            << "), which is different from its start location " << agents[agent_id].path_planner->start_location << endl
            << "=(" << instance.getColCoordinate(agents[agent_id].path_planner->start_location)
            << "," << instance.getRowCoordinate(agents[agent_id].path_planner->start_location)
            << ")" << endl;
            exit(-1);
        }
    }
    has_initial_solution = true;
    return fixInitialSolution();
}

void LNS::writePathsToFile(const string & file_name) const
{
    std::ofstream output;
    output.open(file_name);
    // header
    // output << agents.size() << endl;

    for (const auto &agent : agents)
    {
        output << "Agent " << agent.id << ":";
        for (const auto &state : agent.path)
            output << "(" << instance.getColCoordinate(state.location) << "," <<
                            instance.getRowCoordinate(state.location) << ")->";
        output << endl;
    }
    output.close();
}

// void LNS::commitPath(int commit_step, vector<list<int>> &commit_path, vector<list<int>> &future_path,bool skip_start,int current_time)
// {
//     int screen = 0;
//     for (const auto &agent : agents)
//     {
//         if (screen == 3)
//             cout<<"Commiting: "<<agent.id<<endl;
//         //agent reach target before, but need to de-tour due to resolving conflict, so we add the time reach target as waiting
//         if (current_time != 0 && agent.path.size() > 1 && commit_path[agent.id].size() <= current_time)
//         {
//             commit_path[agent.id].emplace_back(commit_path[agent.id].back());
//             if (screen == 3)
//                 cout<< "(" << instance.getColCoordinate(commit_path[agent.id].back()) << "," <<
//                                 instance.getRowCoordinate(commit_path[agent.id].back()) << ")->";
//         }
//         if (agent.path.size() > commit_step)
//         {
//             int step = 0;
//             for (const auto &state : agent.path)
//             {
//                 if (step == 0)
//                 {
//                     if (skip_start)
//                     {
//                         step++;
//                         continue;
//                     }
//                 }
//                 if(step <commit_step)
//                 {
//                     commit_path[agent.id].emplace_back(state.location);
//                     if (screen == 3)
//                         cout<< "(" << instance.getColCoordinate(state.location) << "," <<
//                                 instance.getRowCoordinate(state.location) << ")->";
//                 }
//                 else if (step == commit_step)
//                 {
//                     commit_path[agent.id].emplace_back(state.location);
//                     if (screen == 3)
//                     {
//                         cout<< "(" << instance.getColCoordinate(state.location) << "," <<
//                                     instance.getRowCoordinate(state.location) << ")"<<endl;
//                         cout<<"Remaining: "<<endl;
//                     }
                        
//                     future_path[agent.id].emplace_back(state.location);
//                     if (screen == 3)
//                         cout<< "(" << instance.getColCoordinate(state.location) << "," <<
//                                 instance.getRowCoordinate(state.location) << ")->";
//                 }
//                 else
//                 {
//                     future_path[agent.id].emplace_back(state.location);
//                     if (screen == 3)
//                         cout<< "(" << instance.getColCoordinate(state.location) << "," <<
//                                 instance.getRowCoordinate(state.location) << ")->";
//                 }
//                 step++;
//             }

//         }
//         else
//         {
//             int step = 0;
//             for (const auto &state : agent.path)
//             {
//                 if (step == 0)
//                 {
//                     if (skip_start)
//                     {
//                         step++;
//                         continue;
//                     }
//                 }
//                 commit_path[agent.id].emplace_back(state.location);
//                 if (screen == 3)
//                     cout<< "(" << instance.getColCoordinate(state.location) << "," <<
//                             instance.getRowCoordinate(state.location) << ")->";
//             }
//             if (screen == 3)
//             {
//                 cout<<endl;
//                 cout<<"Remaining: "<<endl;
//             }
//             future_path[agent.id].emplace_back(commit_path[agent.id].back());
//             if (screen == 3)
//                 cout<< "(" << instance.getColCoordinate(commit_path[agent.id].back()) << "," <<
//                         instance.getRowCoordinate(commit_path[agent.id].back()) << ")->";
//         }
//         if (screen == 3)
//             cout<<endl;
//     }
// }

void LNS::commitPath(int commit_step, vector<list<int>> &commit_path, vector<list<int>> &future_path,bool skip_start,int current_time)
{
    for (const auto &agent : agents)
    {
        if (screen > 3)
            cout<<"Commiting: "<<agent.id<<endl;
        //agent reach target before, but need to de-tour due to resolving conflict, so we add the time reach target as waiting
        // if (current_time != 0 && agent.path.size() > 1 && commit_path[agent.id].size() <= current_time)
        // {
        //     commit_path[agent.id].emplace_back(commit_path[agent.id].back());
        //     if (screen == 3)
        //         cout<< "(" << instance.getColCoordinate(commit_path[agent.id].back()) << "," <<
        //                         instance.getRowCoordinate(commit_path[agent.id].back()) << ")->";
        // }
        if (agent.path.size() > commit_step)
        {
            if (stay_target[agent.id] != 0)
            {
                stay_target[agent.id] = 0;
            }
            int step = 0;
            for (const auto &state : agent.path)
            {
                if (step == 0)
                {
                    if (skip_start)
                    {
                        step++;
                        continue;
                    }
                }
                if(step <commit_step)
                {
                    commit_path[agent.id].emplace_back(state.location);
                    if (screen > 3)
                        cout<< "(" << instance.getColCoordinate(state.location) << "," <<
                                instance.getRowCoordinate(state.location) << ")->";
                }
                else if (step == commit_step)
                {
                    commit_path[agent.id].emplace_back(state.location);
                    if (screen > 3)
                    {
                        cout<< "(" << instance.getColCoordinate(state.location) << "," <<
                                    instance.getRowCoordinate(state.location) << ")"<<endl;
                        cout<<"Remaining: "<<endl;
                    }
                        
                    future_path[agent.id].emplace_back(state.location);
                    if (screen > 3)
                        cout<< "(" << instance.getColCoordinate(state.location) << "," <<
                                instance.getRowCoordinate(state.location) << ")->";
                }
                else
                {
                    future_path[agent.id].emplace_back(state.location);
                    if (screen > 3)
                        cout<< "(" << instance.getColCoordinate(state.location) << "," <<
                                instance.getRowCoordinate(state.location) << ")->";
                }
                step++;
            }

        }
        else
        {
            int step = 0;
            for (const auto &state : agent.path)
            {
                if (step == 0)
                {
                    if (skip_start)
                    {
                        step++;
                        continue;
                    }
                }
                commit_path[agent.id].emplace_back(state.location);
                if (screen > 3)
                    cout<< "(" << instance.getColCoordinate(state.location) << "," <<
                            instance.getRowCoordinate(state.location) << ")->";
                step++;
            }
            for (;step<=commit_step;step++)
            {
                commit_path[agent.id].emplace_back(commit_path[agent.id].back());
                stay_target[agent.id]++;
            }
            if (screen > 3)
            {
                cout<<endl;
                cout<<"Remaining: "<<endl;
            }
            future_path[agent.id].emplace_back(commit_path[agent.id].back());
            if (screen > 3)
                cout<< "(" << instance.getColCoordinate(commit_path[agent.id].back()) << "," <<
                        instance.getRowCoordinate(commit_path[agent.id].back()) << ")->";
        }
        if (screen > 3)
            cout<<endl;
    }
}

void LNS::clearAll(const string & destory_name)
{
    path_table.reset();
    //tabu_list.clear();
    //intersections.clear();
    int i = 0;
    auto starts = instance.getStarts();
    for (auto& a: agents)
    {
        a.path.clear();
        a.path_planner->start_location = starts[i];
        i++;
    }
    //start_time = Time::now();
    neighbor.agents.clear();
    neighbor.sum_of_costs = 0;
    neighbor.sum_of_costs_target_stay = 0;
    neighbor.old_sum_of_costs = 0;
    neighbor.old_sum_of_costs_target_stay = 0;
    neighbor.colliding_pairs.clear();
    neighbor.old_colliding_pairs.clear();
    neighbor.old_paths.clear();

    num_of_failures = 0;
    iteration_stats.clear();
    average_group_size = -1;
    sum_of_costs = 0;

    // decay_factor = -1;
    // reaction_factor = -1;
    // destroy_weights.clear();
    // selected_neighbor = 0;

    preprocessing_time = 0;
    initial_solution_runtime = 0;
    initial_sum_of_costs = -1;
    sum_of_costs_lowerbound = -1;
    sum_of_distances = -1;
    restart_times = 0;
    complete_paths = 0;
    delete_timesteps = 0;

    num_of_iterations = 0;

    if (destory_name == "Adaptive")
    {
        ALNS = true;
        destroy_weights.clear();
        destroy_weights.assign(DESTORY_COUNT, 1);
        decay_factor = 0.01;
        reaction_factor = 0.01;
    }
    else if (destory_name == "RandomWalk")
        destroy_strategy = RANDOMWALK;
    else if (destory_name == "Intersection")
        destroy_strategy = INTERSECTION;
    else if (destory_name == "Random")
        destroy_strategy = RANDOMAGENTS;
    else
    {
        cerr << "Destroy heuristic " << destory_name << " does not exists. " << endl;
        exit(-1);
    }

    //start_time = Time::now();
    //replan_time_limit = time_limit / 100;
}


void LNS::validateCommitSolution(vector<list<int>> commited_paths) const
{
    cerr << "validation"<<endl;
    vector<Agent> commited_agents;
    int N = instance.getDefaultNumberOfAgents();
    commited_agents.reserve(N);
    for (int i = 0; i < N; i++)
        commited_agents.emplace_back(instance, i, false);
    for (int agent_id = 0; agent_id < instance.getDefaultNumberOfAgents(); agent_id++)
    {
        for(auto location: commited_paths[agent_id])
        {
            commited_agents[agent_id].path.emplace_back(location);
        }
    }
    int sum = 0;
    for (const auto& a1_ : commited_agents)
    {
        for (int t = 1; t < (int) a1_.path.size(); t++ )
        {
            if (!instance.validMove(a1_.path[t - 1].location, a1_.path[t].location))
            {
                cerr << "The path of agent " << a1_.id << " jump from "
                     << a1_.path[t - 1].location << " to " << a1_.path[t].location
                     << " between timesteps " << t - 1 << " and " << t << endl;
                //exit(-1);
            }
        }
        sum += (int) a1_.path.size() - 1;
        for (const auto  & a2_: commited_agents)
        {
            if (a1_.id >= a2_.id || a2_.path.empty())
                continue;
            const auto & a1 = a1_.path.size() <= a2_.path.size()? a1_ : a2_;
            const auto & a2 = a1_.path.size() <= a2_.path.size()? a2_ : a1_;
            int t = 1;
            for (; t < (int) a1.path.size(); t++)
            {
                if (a1.path[t].location == a2.path[t].location) // vertex conflict
                {
                    cerr << "Find a vertex conflict between agents " << a1.id << " and " << a2.id <<
                            " at location " << a1.path[t].location << " at timestep " << t << endl;
                    //exit(-1);
                }
                else if (a1.path[t].location == a2.path[t - 1].location &&
                        a1.path[t - 1].location == a2.path[t].location) // edge conflict
                {
                    cerr << "Find an edge conflict between agents " << a1.id << " and " << a2.id <<
                         " at edge (" << a1.path[t - 1].location << "," << a1.path[t].location <<
                         ") at timestep " << t << endl;
                    //exit(-1);
                }
            }
            int target = a1.path.back().location;
            for (; t < (int) a2.path.size(); t++)
            {
                if (a2.path[t].location == target)  // target conflict
                {
                    cerr << "Find a target conflict where agent " << a2.id << " (of length " << a2.path.size() - 1<<
                         ") traverses agent " << a1.id << " (of length " << a1.path.size() - 1<<
                         ")'s target location " << target << " at timestep " << t << endl;
                    //exit(-1);
                }
            }
        }
    }
    // if (sum_of_costs != sum)
    // {
    //     cerr << "The computed sum of costs " << sum_of_costs <<
    //          " is different from the sum of the paths in the solution " << sum << endl;
    //     exit(-1);
    // }
}


