#include <iostream>
#include "mpi.h"
#include <cstdio>
#include <ctime>

#include "AStar.h"
#include "AStarMPICommunication.h"
#include "Coordinate.h"
#include "CoordinateList.h"
#include "Heuristic.h"

//Global variable definitions
std::clock_t start;

void StartClock() {
    start = std::clock();
}

void StopClockAndDisplayTime() {
    double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
    std::cout << "Took " << duration << " seconds to execute";
}

void SetWalls(AStar& mapGenerator)
{
    mapGenerator.addCollision({0, 1});
    mapGenerator.addCollision({1, 1});
    mapGenerator.addCollision({2, 1});

    mapGenerator.addCollision({4, 0});
    mapGenerator.addCollision({4, 1});
    mapGenerator.addCollision({4, 2});

    mapGenerator.addCollision({10, 13});
    mapGenerator.addCollision({10, 14});
    mapGenerator.addCollision({10, 15});

    mapGenerator.addCollision({11, 13});

    mapGenerator.addCollision({12, 13});

    mapGenerator.addCollision({13, 11});
    mapGenerator.addCollision({13, 12});
    mapGenerator.addCollision({13, 13});
    mapGenerator.addCollision({13, 14});
    mapGenerator.addCollision({13, 15});
}

void ListPathNodes(CoordinateList& path)
{
    for (auto& coordinate : path) {
        std::cout << coordinate.x << " " << coordinate.y << "\n";
    }
}

void ExecuteAStarAlgorithm(AStarMPICommunication aStarMPICommunication, int rank, int size)
{
    int map_size_x = 25,
        map_size_y = 25;

    AStar mapGenerator;

    mapGenerator.setMapSize({map_size_x, map_size_y});
    mapGenerator.setHeuristic(Heuristic::euclidean);
    mapGenerator.setDiagonalMovement(false);

    SetWalls(mapGenerator);

    mapGenerator.updateMap();
    mapGenerator.showMap();

    std::cout << "Running path generation ... \n";

    CoordinateList path;

    if (size < 5) {
        path = mapGenerator.findPath(
            {0, 0},
            {map_size_x - 1, map_size_y - 1}
        );
    } else {
        path = mapGenerator.findPathMPI(
            {0, 0},
            {map_size_x - 1, map_size_y - 1},
            aStarMPICommunication,
            rank
        );
    }


    mapGenerator.updateMap(path);
    mapGenerator.showMap();

    ListPathNodes(path);
}

void DisplayNode(Node node) {
    std::cout << "Node:"
        << "\n\tX - " << node.coordinates.x
        << "\n\tY - " << node.coordinates.y
        << "\n\tG - " << node.G
        << "\n\tH - " << node.H
        << "\n\tP - " << node.parent
        << "\n\tPX - " << (*(node.parent)).coordinates.x
        << "\n\tPY - " << (*(node.parent)).coordinates.y
        << "\n\tPG - " << (*(node.parent)).G
        << "\n\tPH - " << (*(node.parent)).H
        << "\n\tPP - " << (*(node.parent)).parent;
}

void SendNodeToOtherHost(AStarMPICommunication aStarMPICommunication, int rank) {
    std::cout << "SendNodeToOtherHost #" << rank;

    if (rank == 0) {
        auto parent_coordinates = *new Coordinate;
        parent_coordinates.x = 24;
        parent_coordinates.y = 21;

        auto parentNode = new Node(parent_coordinates);
        parentNode->G = 255;
        parentNode->H = 277;

        auto coordinates = *new Coordinate;
        coordinates.x = 4;
        coordinates.y = 5;

        auto node = *new Node(coordinates, parentNode);

        node.G = 17;
        node.H = 19;

        aStarMPICommunication.SendNode(1, node);
    } else {
        auto node = aStarMPICommunication.ReceiveNode(0);

        DisplayNode(node);
    }
}

int main(int argc, char **argv)
{
    //MPI initialization
    int rank, size;

    MPI_Init(&argc, &argv);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    MPI_Comm_size(MPI_COMM_WORLD, &size);

    auto aStarMPICommunication = * new AStarMPICommunication();
    aStarMPICommunication.DefineCoordinateTypeInMPI();
    aStarMPICommunication.DefineNodeTypeInMPI();

    bool shouldUseBuiltInClock = false;

    if (shouldUseBuiltInClock) {
        StartClock();
    }

    //Current program selection
    //ExecuteAStarAlgorithm(aStarMPICommunication, rank, size);
    //aStarMPICommunication.CommunicationWithCoordinateExample(rank);
    //aStarMPICommunication.CommunicationWithNodeExample(rank);
    SendNodeToOtherHost(aStarMPICommunication, rank);

    //MPI finalization
    MPI_Finalize();

    if (shouldUseBuiltInClock) {
        StopClockAndDisplayTime();
    }

    return 0;
}