#include<boost/tokenizer.hpp>
#include <algorithm>    // std::shuffle
#include <random>      // std::default_random_engine
#include <chrono>       // std::chrono::system_clock
#include"Instance.h"

void Instance::initMap(SharedEnvironment* simulate_env)
{
    env = simulate_env;
    existing_path.resize(env->num_of_agents);
}

void Instance::prepareDummy()
{
    //prepare dummy goals
    vector<list<int>> loc_degrees;
    loc_degrees.resize(8); 

    for (int i = 0; i < env->map.size(); i++)
    {
        if (env->map[i] == 1)
            continue;
        int d = getDegreeAdvanced(i);
        if (d == 0)
            continue;
        if (getAllpairDistance(env->curr_states[0].location, i) == MAX_TIMESTEP) //do not belongs to the current connectted components
            continue;
        loc_degrees[d-1].push_back(i);
    }

    unordered_set<int> candidates;
    for (int j = 7; j >= 0; j--)
    {
        for (auto loc: loc_degrees[j])
        {
            candidates.insert(loc);
        }
        if (candidates.size() >= env->num_of_agents)
        {
            break;
        }
    }

    dummy_goals.resize(candidates.size());
    int index = 0;
    for (auto loc: candidates)
    {
        dummy_goals[index] = loc;
        index++;
    }
    std::random_shuffle(dummy_goals.begin(), dummy_goals.end());
    dummy_goals.resize(env->num_of_agents);
}


void Instance::printMap() const
{
	for (int i = 0; i< env->rows; i++)
	{
		for (int j = 0; j < env->cols;j++)
		{
			if (this->env->map[linearizeCoordinate(i, j)] == 1)
				cout << '@';
			else
				cout << '.';
		}
		cout << endl;
	}
}


void Instance::saveMap() const
{
	ofstream myfile;
	myfile.open(map_fname);
	if (!myfile.is_open())
	{
		cout << "Fail to save the map to " << map_fname << endl;
		return;
	}
	myfile << env->rows << "," << env->cols << endl;
	for (int i = 0; i < env->rows; i++)
	{
		for (int j = 0; j < env->cols; j++)
		{
			if (env->map[linearizeCoordinate(i, j)] == 1)
				myfile << "@";
			else
				myfile << ".";
		}
		myfile << endl;
	}
	myfile.close();
}


list<int> Instance::getNeighbors(int curr) const
{
	list<int> neighbors;
	int candidates[4] = {curr + 1, curr - 1, curr + env->cols, curr - env->cols};
	for (int next : candidates)
	{
		if (validMove(curr, next))
			neighbors.emplace_back(next);
	}
	return neighbors;
}

void Instance::savePaths(const string & file_name, const vector<Path*>& paths) const
{
    std::ofstream output;
    output.open(file_name);

    for (auto i = 0; i < paths.size(); i++)
    {
        output << "Agent " << i << ":";
        for (const auto &state : (*paths[i]))
        {
            if (nathan_benchmark)
                output << "(" << getColCoordinate(state.location) << "," << getRowCoordinate(state.location) << ")->";
            else
                output << "(" << getRowCoordinate(state.location) << "," << getColCoordinate(state.location) << ")->";
        }
        output << endl;
    }
    output.close();
}

void Instance::computeAllPair()
{
    cout<<"computing all pair"<<endl;
    heuristic.resize(env->map.size());

    struct Node
	{
		int location;
		int value;

		Node() = default;
		Node(int location, int value) : location(location), value(value) {}
		// the following is used to compare nodes in the OPEN list
		struct compare_node
		{
			// returns true if n1 > n2 (note -- this gives us *min*-heap).
			bool operator()(const Node& n1, const Node& n2) const
			{
				return n1.value >= n2.value;
			}
		};  // used by OPEN (heap) to compare nodes (top of the heap has min f-val, and then highest g-val)
	};

    for (int i = 0; i < heuristic.size(); i++)
    {
        if (env->map[i] == 1)
            continue;
        //heuristic[i] = std::vector<int>(heuristic.size()-i, MAX_TIMESTEP);
        heuristic[i] = std::vector<int>(heuristic.size(), MAX_TIMESTEP);
    }
    for (int i = 0; i < heuristic.size(); i++)
    {
        if (env->map[i] == 1)
            continue;
        // generate a heap that can save nodes (and a open_handle)
        boost::heap::pairing_heap< Node, boost::heap::compare<Node::compare_node> > heap;

        Node root(i, 0); //compute every node to i
        heuristic[i][i] = 0;

        heap.push(root);  // add root to heap
        while (!heap.empty())
        {
            Node curr = heap.top();
            heap.pop();
            for (int next_location : getNeighbors(curr.location))
            {
                if (heuristic[i][next_location] > curr.value + 1)
                {
                    heuristic[i][next_location] = curr.value + 1;
                    Node next(next_location, curr.value + 1);
                    heap.push(next);
                }
            }
        }
    }
}

bool Instance::hasCollision(const Path& p1, const Path& p2) const
{
    int t = 1;
    for (; t < (int) min(p1.size(), p2.size()); t++)
    {
        if (p1[t].location == p2[t].location) // vertex conflict
        {
            return true;
        }
        else if (p1[t].location == p2[t-1].location and p1[t-1].location == p2[t].location) // edge conflict
        {
            return true;
        }
    }
    return false;
}
