#include "AStar.h"
#include <algorithm>
#include <iostream>
#include "mpi.h"
#include "Coordinate.h"
#include "Node.h"
#include "Heuristic.h"
#include "AStarMPICommunication.h"

#define RESULT_TAG 1
#define TASK_TAG 2

using namespace std::placeholders;


struct Para {//pomocnicza struktura do przechowania wynikow
    NodeSet n;
    int G;
    void setNode(NodeSet nod) {
        this->n = nod;
    }
};



AStar::AStar()
{
    setDiagonalMovement(false);
    setHeuristic(&Heuristic::euclidean);

    wallSymbol = '0';
    freeSymbol = '-';
    pathSymbol = 'X';
    startSymbol = 'S';
    endSymbol = 'E';

    direction = {
        { 0, 1 }, { 1, 0 }, { 0, -1 }, { -1, 0 },
        { -1, -1 }, { 1, 1 }, { -1, 1 }, { 1, -1 }
    };
}

void AStar::setMapSize(Coordinate worldSize_)
{
    worldSize = worldSize_;
}

void AStar::setDiagonalMovement(bool enable_)
{
    directions = (enable_ ? 8 : 4);
}

void AStar::setHeuristic(HeuristicFunction heuristic_)
{
    heuristic = std::bind(heuristic_, _1, _2);
}

void AStar::addCollision(Coordinate coordinates_)
{
    walls.push_back(coordinates_);
}

void AStar::removeCollision(Coordinate coordinates_)
{
    auto it = std::find(walls.begin(), walls.end(), coordinates_);

    if (it != walls.end()) {
        walls.erase(it);
    }
}

void AStar::clearCollisions()
{
    walls.clear();
}

void AStar::updateMap()//aktualizacja mapy
{
    map = new char*[worldSize.x];

    for (int x = 0; x < worldSize.x; ++x)
    {
        map[x] = new char[worldSize.y];
    }

    for (auto x = 0; x < worldSize.x; x++)
    {
        for (auto y = 0; y < worldSize.y; y++)
        {
            map[x][y] = freeSymbol;
        }
    }

    for (auto wall : walls)
    {
        map[wall.x][wall.y] = wallSymbol;
    }

    if (start == end) {
        return;
    }

    map[start.x][start.y] = startSymbol;
    map[end.x][end.y] = endSymbol;
}

void AStar::updateMap(CoordinateList path_)
{
    updateMap();

    for (auto& coordinate : path_) {
        if (coordinate == start || coordinate == end) {
            continue;
        }

        map[coordinate.x][coordinate.y] = pathSymbol;
    }
}

void AStar::showMap() //wyswietlenie mapy
{
    for (auto y = 0; y < worldSize.y; y++)
    {
        for (auto x = 0; x < worldSize.x; x++)
        {
            std::cout << map[x][y] << " ";
        }

        std::cout << "\n";
    }
}

CoordinateList AStar::findPath(Coordinate start_, Coordinate end_)
{
    start = start_;
    end = end_;

    Node *currentNode = nullptr;
    NodeSet openSet, closedSet;

    openSet.reserve(300);
    closedSet.reserve(300);

    openSet.push_back(new Node(start_));

    while (!openSet.empty()) {
        auto currentNodeIteartor = openSet.begin();
        currentNode = *currentNodeIteartor;

        for (auto iterator = openSet.begin(); iterator != openSet.end(); iterator++) {
            auto node = *iterator;
            if (node->getScore() <= currentNode->getScore()) {
                currentNode = node;
                currentNodeIteartor = iterator;
            }
        }

        if (currentNode->coordinates == end_) {
            break;
        }

        //MPI_Bcast(&current, 1, MPI_INT, 0, MPI_COMM_WORLD);

        closedSet.push_back(currentNode);
        openSet.erase(currentNodeIteartor);

        for (int i = 0; i < directions; ++i) {
            Coordinate newCoordinates(currentNode->coordinates + direction[i]);

            //SendCoordinateToItsHandlingProcess(newCoordinates);

            if (detectCollision(newCoordinates) ||
                findNodeOnList(closedSet, newCoordinates)) {
                continue;
            }

            int totalCost = currentNode->G + ((i < 4) ? 10 : 14);

            Node *successor = findNodeOnList(openSet, newCoordinates);
            if (successor == nullptr) {
                successor = new Node(newCoordinates, currentNode);
                successor->G = totalCost;
                successor->H = heuristic(successor->coordinates, end_);
                openSet.push_back(successor);
            }
            else if (totalCost < successor->G) {
                successor->parent = currentNode;
                successor->G = totalCost;
            }
        }
    }

    CoordinateList path;
    while (currentNode != nullptr) {
        path.push_back(currentNode->coordinates);
        currentNode = currentNode->parent;
    }

    releaseNodes(openSet);
    releaseNodes(closedSet);

    return path;
}

CoordinateList AStar::findPathMPI(Coordinate start_, Coordinate end_, AStarMPICommunication communication, int rank, int size)
{
    if (rank == 0) {
        coordinate(start_, end_, communication, rank, size);
    }
    else {
        performComputations(rank);
    }
}

CoordinateList AStar::coordinate(Coordinate start_, Coordinate end_, AStarMPICommunication communication, int rank, int size) {//fcja koordynatora
    start = start_;//start
    end = end_;//i koniec

    Node* currentNode = nullptr;//wskaznik na aktualny wierzcholek
    NodeSet openSet, closedSet;//"listy" open i close

    openSet.reserve(300);//rezerwacja pamieci
    closedSet.reserve(300);

    openSet.push_back(new Node(start_));//umieszcza na liście otwartej wierzcholek startowy

    while (!openSet.empty()) {//działa do momentu aż w liście otwartej coś jest
        auto currentNodeIteartor = openSet.begin();//iterator do przechodzenia po wierzcholkach - ustawiamy na poczatek listy
        currentNode = *currentNodeIteartor;//aktualna pozycja najlepszej ścieżki



        std::vector<int> proc_to_task_id(size, -1);//ktory proces dostal zadanie
        //std::vector<int> results(openSet.size());//tu ma byc rozmiar tablicy open
        std::vector<Para> pary(openSet.size());//tablica z aktualnie sprawdzanymi węzłami
        const auto par_start_time = MPI_Wtime();
        int task_id = 0;


        for (auto iterator = openSet.begin(); iterator != openSet.end(); iterator++) {
            Para p;
            p.G = INT_MAX;
            p.setNode(iterator);//tutaj przydałoby się przypisać node-a
            pary.push_back(p);
        }


        /*napisać od 0 przydzielanie zadań procesom : proces master przydziela procesom slave zadanie obliczenia kosztu dojścia do wszystkich node - ów
        sąsiadujących z tym, w którym się znajdujemy, procesy zwracają wartości funkcji liczącej koszt dojścia, następnie proces master wybiera z listy
        zwróconych wartości najmniejszą i potem dodaje do listy kolejne wierzchołki sąsiadujące z tym poprzednim i pętla leci od nowa
        po przejściu pętli while funkcja działa dalej*/



        //znalezienie kolejnego wierzcholka
        for (auto iterator = openSet.begin(); iterator != openSet.end(); iterator++) {//przechodzimy przez liste otwarta
            MPI_Status status;
            int result;
            const int code = MPI_Recv(&result, 1, MPI_INT, MPI_ANY_SOURCE, RESULT_TAG, MPI_COMM_WORLD, &status);
            if (code != MPI_SUCCESS) {
                MPI_Abort(MPI_COMM_WORLD, code);
            }
            //jeśli proces miał przydzielone zadanie to zapisujemy jego wynik
            const auto sender = status.MPI_SOURCE;
            const auto prev_task_id = proc_to_task_id[sender];
            if (prev_task_id != -1) {
                results[prev_task_id] = result;
            }

            //wysyłamy kolejne zadanie
            auto node = *iterator;//aktualna pozycja - wierzcholek
            proc_to_task_id[sender] = task_id;
            MPI_Send(&node->coordinates.x, 1, MPI_INT, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania
            MPI_Send(&node->coordinates.y, 1, MPI_INT, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania
            MPI_Send(&node->G, 1, MPI_INT, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania
            MPI_Send(&node->H, 1, MPI_INT, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania
            MPI_Send(&node->parent->coordinates.x, 1, MPI_INT, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania
            MPI_Send(&node->parent->coordinates.y, 1, MPI_INT, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania
            //MPI_Send(&node, 1, MPI_PACKED, sender, TASK_TAG, MPI_COMM_WORLD);//przeslanie danych do wykonania zadania

            ++task_id;

            //sprawdzenie który wynik najmniejszy

            /*if (node->getScore() <= currentNode->getScore()) {//jesli całkowity koszt dla danego jest mniejszy lub rowny niż do globalnego
                currentNode = node;//to ustaw go jako globalny
                currentNodeIteartor = iterator;
            }*/
        }

        if (currentNode->coordinates == end_) {//jesli dotarlismy do ostatniego to wyjdz z pętli
            break;
        }

        closedSet.push_back(currentNode);//dodaj odwiedzony do listy zamknietej
        openSet.erase(currentNodeIteartor);//usun odwiedzony z listy otwartej






        //znalezienie nastepnikow wybranego wierzcholka
        for (int i = 0; i < directions; ++i) {//dodanie do listy kolejnych wierzcholkow następnikow wybranego
            Coordinate newCoordinates(currentNode->coordinates + direction[i]);

            //SendCoordinateToItsHandlingProcess(newCoordinates);

            if (detectCollision(newCoordinates) || findNodeOnList(closedSet, newCoordinates)) {//jesli kolizja lub wierzcholek już jest na liście to pomiń
                continue;
            }

            int totalCost = currentNode->G + ((i < 4) ? 10 : 14);//podlicz całkowity koszt dojścia

            Node* successor = findNodeOnList(openSet, newCoordinates);
            if (successor == nullptr) {
                successor = new Node(newCoordinates, currentNode);
                successor->G = totalCost;
                successor->H = heuristic(successor->coordinates, end_);
                openSet.push_back(successor);
            }
            else if (totalCost < successor->G) {
                successor->parent = currentNode;
                successor->G = totalCost;
            }
        }
    }

    CoordinateList path;
    while (currentNode != nullptr) {
        path.push_back(currentNode->coordinates);
        currentNode = currentNode->parent;
    }

    releaseNodes(openSet);
    releaseNodes(closedSet);

    return path;
}

void performComputations(int myrank) {//funkcja slave-a
    const int master = 0;
    int result = -1;

    while (true) {
        std::cout << myrank << " przesyła wynik " << result << " do master-a" << std::endl;
        MPI_Send(&result, 1, MPI_INT, master, RESULT_TAG, MPI_COMM_WORLD);

        int a, b, c, d, e, f;//zmienne do przechowania tymczasowych wartosci
        Node n;//obiekt do działań - wierzcholek na ktorym wykonuje obliczenia
        MPI_Status status;
        MPI_Recv(&a, 1, MPI_INT, master, TASK_TAG, MPI_COMM_WORLD, &status);
        MPI_Recv(&b, 1, MPI_INT, master, TASK_TAG, MPI_COMM_WORLD, &status);
        MPI_Recv(&c, 1, MPI_INT, master, TASK_TAG, MPI_COMM_WORLD, &status);
        MPI_Recv(&d, 1, MPI_INT, master, TASK_TAG, MPI_COMM_WORLD, &status);
        MPI_Recv(&e, 1, MPI_INT, master, TASK_TAG, MPI_COMM_WORLD, &status);
        MPI_Recv(&f, 1, MPI_INT, master, TASK_TAG, MPI_COMM_WORLD, &status);
        //MPI_Recv(&n, 1, MPI_PACKED, master, TASK_TAG, MPI_COMM_WORLD, &status);
        n.coordinates.x = a;
        n.coordinates.y = b;
        n.G = c;
        n.H = d;
        n.parent->coordinates.x = e;
        n.parent->coordinates.y = f;
        if (is_empty_task(a) || is_empty_task(b) || is_empty_task(c) || is_empty_task(d) || is_empty_task(e) || is_empty_task(f))break;


        std::cout << myrank << " otrzymał node: [" << n.coordinates.x << " | " << n.coordinates.y << "] od mastera" << std::endl;

        result = n.getScore();
    }
}
int empty_task() {
    return -1;
}
bool is_empty_task(int task) {
    return -1 == task;
}



































Node* AStar::findNodeOnList(NodeSet& nodes_, Coordinate coordinates_)
{
    for (auto node : nodes_) {
        if (node->coordinates == coordinates_) {
            return node;
        }
    }

    return nullptr;
}

void AStar::releaseNodes(NodeSet& nodes_)
{
    for (auto it = nodes_.begin(); it != nodes_.end();) {
        delete *it;
        it = nodes_.erase(it);
    }
}

bool AStar::detectCollision(Coordinate coordinates_)
{
    if (coordinates_.x < 0 || coordinates_.x >= worldSize.x ||
        coordinates_.y < 0 || coordinates_.y >= worldSize.y ||
        std::find(walls.begin(), walls.end(), coordinates_) != walls.end()
    ) {
        return true;
    }

    return false;
}