#include <pch.h>
#include "Projects/ProjectTwo.h"
#include "P2_Pathfinding.h"

#pragma region Extra Credit

std::list<AStarPather::Node*> list;
AStarPather::Node map[61][61];

bool ProjectTwo::implemented_floyd_warshall()
{
    return false;
}

bool ProjectTwo::implemented_goal_bounding()
{
    return false;
}

bool ProjectTwo::implemented_jps_plus()
{
    return false;
}
#pragma endregion

bool AStarPather::initialize()
{
    // handle any one-time setup requirements you have

    /*
        If you want to do any map-preprocessing, you'll need to listen
        for the map change message.  It'll look something like this:

        Callback cb = std::bind(&AStarPather::your_function_name, this);
        Messenger::listen_for_message(Messages::MAP_CHANGE, cb);

        There are other alternatives to using std::bind, so feel free to mix it up.
        Callback is just a typedef for std::function<void(void)>, so any std::invoke'able
        object that std::function can wrap will suffice.
    */

    return true; // return false if any errors actually occur, to stop engine initialization
}

void AStarPather::shutdown()
{
    /*
        Free any dynamically allocated memory or any other general house-
        keeping you need to do during shutdown.
    */
}
/*
    This is where you handle pathing requests, each request has several fields:

    start/goal - start and goal world positions
    path - where you will build the path upon completion, path should be
        start to goal, not goal to start
    heuristic - which heuristic calculation to use
    weight - the heuristic weight to be applied
    newRequest - whether this is the first request for this path, should generally
        be true, unless single step is on

    smoothing - whether to apply smoothing to the path
    rubberBanding - whether to apply rubber banding
    singleStep - whether to perform only a single A* step
    debugColoring - whether to color the grid based on the A* state:
        closed list nodes - yellow
        open list nodes - blue

        use terrain->set_color(row, col, Colors::YourColor);
        also it can be helpful to temporarily use other colors for specific states
        when you are testing your algorithms

    method - which algorithm to use: A*, Floyd-Warshall, JPS+, or goal bounding,
        will be A* generally, unless you implement extra credit features

    The return values are:
        PROCESSING - a path hasn't been found yet, should only be returned in
            single step mode until a path is found
        COMPLETE - a path to the goal was found and has been built in request.path
        IMPOSSIBLE - a path from start to goal does not exist, do not add start position to path
*/
PathResult AStarPather::compute_path(PathRequest &request)
{

    //start/goal - start and goal world positions
    GridPos start = terrain->get_grid_position(request.start);
    GridPos goal = terrain->get_grid_position(request.goal);

    terrain->set_color(start, Colors::Red);
    //set color to orange
    terrain->set_color(goal, Colors::Red);

    //request.path.push_back(request.start);


/***********************************A* SEARCH ALGORITHM*********************************/
    //Push Start Node onto the Open List.

    if (request.newRequest)
    {
        for (int i = 0; i <= 40; i++)
        {
            for (int j = 0; j <= 40; j++)
            {
                map[i][j].parent = NULL;
                map[i][j].pos = GridPos{j, i};
                map[i][j].onList_ = onList::NONE;
                map[i][j].cost = 0.0f;
                map[i][j].given = 0.0f;
            }
        }
        list.clear();
        list.push_back(&map[start.col][start.row]);
    }

    //While (Open List is not empty) {
    while (!list.empty())
    {
        //Pop cheapest node off Open List (parent node).
        Node* parentNode = findCheapest(list);
        
        std::list<Node*>::iterator it;
        it = list.begin();
        std::advance(it, findNodeIndex(list, parentNode));
        it = list.erase(it);

        //request.path.push_back(terrain->get_world_position(parentNode->pos));
        //If node is the Goal Node, then path found (RETURN �found�).
        if (parentNode->pos == goal)
        {
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
            Node* cur = parentNode;
            while (cur) {
                //push request
                request.path.push_front(terrain->get_world_position(cur->pos));
                //go to next parent
                cur = cur->parent;
            }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

            terrain->set_color(start, Colors::Orange);
            terrain->set_color(goal, Colors::Orange);
            return PathResult::COMPLETE;
        }

        
        bool NW = true;
        bool NE = true;
        bool SE = true;
        bool SW = true;

        //For (all neighboring child nodes)
        for (int i = 1; i <= 8; i++)
        {
            //set parent to parent
            GridPos childPos = getChild(parentNode->pos, i); //get child
            //deleted line
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
            //Node* oldParent = map[childPos.col][childPos.row].parent;

            if (childPos != parentNode->pos)
            {
                //set map's parent to new parent after getting position
                //map[childNode->pos.col][childNode->pos.row].parent = &map[parentNode->pos.col][parentNode->pos.row];
                
                //grid is on the map and isn't a wall
                if (terrain->is_valid_grid_position(childPos) && !terrain->is_wall(childPos))
                {
                    //i is non diagonals or is a valid diagonal
                    if (i <= 4 || (i == 5 && NE) || (i == 6 && SE) || (i == 7 && SW) || (i == 8 && NW))
                    {
                        //Compute its cost, f(x) = g(x) + h(x)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
                        float given = parentNode->given;
                        if (i >= 4)
                        {
                            //tile is a diagonal
                            given += (float)std::sqrt(2);
                            //map[childPos.col][childPos.row].given = map[parentNode->pos.col][parentNode->pos.row].given + (float)std::sqrt(2);
                        }
                        else
                        {
                            //tile is N, S, W, E
                            given += 1;
                            //map[childPos.col][childPos.row].given = map[parentNode->pos.col][parentNode->pos.row].given + 1;
                        }

                        float h = getHeuristic(request.settings.heuristic, childPos, goal);
                        //map[childPos.col][childPos.row].cost = map[parentNode->pos.col][parentNode->pos.row].given + h * request.settings.weight;
                        float newCost = given + h * request.settings.weight;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////

                        //find if child exists on curr list, and assign it
                        map[parentNode->pos.col][parentNode->pos.row].onList_ = assignList(list, map[parentNode->pos.col][parentNode->pos.row].pos);

                        //If child node isn't on Open or Closed list, put it on Open List.
                        if (map[childPos.col][childPos.row].onList_ == onList::NONE)
                        {
////////////////////////////////////////////////////////////////////////////////////////////////////////////
                            map[childPos.col][childPos.row].parent = parentNode;
                            map[childPos.col][childPos.row].given = given;
                            map[childPos.col][childPos.row].cost = newCost;
////////////////////////////////////////////////////////////////////////////////////////////////////////////
                            map[childPos.col][childPos.row].onList_ = onList::OPEN;
                            terrain->set_color(childPos, Colors::Blue);
                            list.push_back(&map[childPos.col][childPos.row]);
                        }
                        //Else if child node is on Open or Closed List,
                        else if (map[childPos.col][childPos.row].onList_ == onList::OPEN || map[childPos.col][childPos.row].onList_ == onList::CLOSE)
                        {
                            //AND this new one is cheaper,
                            //then take the old expensive one off both lists
                            //if oldCost == 0 then it's our first time setting it
                            if (map[childPos.col][childPos.row].cost > newCost)
                            {
                                //and put this new cheaper one on the Open List.
////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                map[childPos.col][childPos.row].parent = parentNode;
                                map[childPos.col][childPos.row].given = given;
                                map[childPos.col][childPos.row].cost = newCost;
////////////////////////////////////////////////////////////////////////////////////////////////////////////
                                map[childPos.col][childPos.row].onList_ = onList::OPEN;
                                terrain->set_color(childPos, Colors::Blue);
                                list.push_back(&map[childPos.col][childPos.row]);
                            }
                            /*
                            else
                            {
                                map[childPos.col][childPos.row].cost = oldCost;
                                map[childPos.col][childPos.row].parent = oldParent;
                            }*/
                        }
                    }
                }
                //grid is valid but the non-diagonals is a wall, skip the diagonals
                else if (terrain->is_valid_grid_position(childPos) && terrain->is_wall(childPos) && i <= 4)
                {
                    if (i == 1) //NORTH
                    {
                        NE = false;
                        NW = false;
                    }

                    if (i == 2) //EAST
                    {
                        NE = false;
                        SE = false;
                    }

                    if (i == 3) //SOUTH
                    {
                        SE = false;
                        SW = false;
                    }

                    if (i == 4) //WEST
                    {
                        SW = false;
                        NW = false;
                    }
                }
            }
        }

/***************************************************************************************************************/
        //Place parent node on the Closed List (we're done with it).
        parentNode->onList_ = onList::CLOSE;
        map[parentNode->pos.col][parentNode->pos.row].onList_ = onList::CLOSE;
        terrain->set_color(parentNode->pos, Colors::Yellow);
        map[parentNode->pos.col][parentNode->pos.row] = *parentNode;
        //If taken too much time this frame (or in single step mode), 
        if (request.settings.singleStep == true)
        {
            //abort search for now and resume next frame (RETURN �working�).
            return PathResult::PROCESSING;
        }
    }
    //Open List empty, thus no path possible (RETURN �fail�).
    return PathResult::IMPOSSIBLE;

}

float AStarPather::getHeuristic(Heuristic method, GridPos position, GridPos goal)
{
    float dx = (float)std::fabs(position.row - goal.row);
    float dy = (float)std::fabs(position.col - goal.col);

    if (method == Heuristic::OCTILE)
    {
        return 1 * (dx + dy) + (float)(sqrt(2) - 2 * 1) * std::min(dx, dy);
    }

    if (method == Heuristic::CHEBYSHEV)
    {
        return 1 * (dx + dy) + (1 - 2 * 1) * std::min(dx, dy);
    }  

    if (method == Heuristic::MANHATTAN)
    {
        return dx + dy;
    }

    if (method == Heuristic::EUCLIDEAN)
    {
        return (float)sqrt(dx * dx + dy * dy);
    }

    return 0.0f;
}

AStarPather::onList AStarPather::assignList(std::list<Node*> list, GridPos position)
{
    //go through list
    for (const Node* node : list)
    {
        //if node exists in list
        //and is labeled as open
        if (node->pos == position && node->onList_ == onList::OPEN)
        {
            //return open
            return onList::OPEN;
        }
        //and is labeled as closed
        if (node->pos == position && node->onList_ == onList::CLOSE)
        {
            //return closed
            return onList::CLOSE;
        }
        //and is labeled as none
        if (node->pos == position && node->onList_ == onList::NONE)
        {
            return onList::NONE;
        }
    }

    //else it's not on either list
    return onList::NONE;
}

GridPos AStarPather::getChild(GridPos node, int i)
{
    GridPos pos;
    if (i == 1) //NORTH
    {
        pos = { node.row + 1, node.col};
    }
    else if (i == 5) //NE
    {
        pos = { node.row + 1, node.col + 1 };
    }
    else if (i == 2) //EAST
    {
        pos = { node.row, node.col + 1};
    }
    else if (i == 6) //SE
    {
        pos = { node.row - 1, node.col + 1 };
    }
    else if (i == 3) //SOUTH
    {
        pos = { node.row - 1, node.col };
    }
    else if (i == 7) //SW
    {
        pos = { node.row - 1, node.col - 1 };
    }
    else if (i == 4) //WEST
    {
        pos = { node.row, node.col - 1};
    }
    else if (i == 8) //NW
    {
        pos = { node.row + 1, node.col - 1};
    }

    return pos;
}

AStarPather::Node* AStarPather::findCheapest(std::list<Node*> list)
{
    Node* cheapestNode;
    float minCost = -1.0f;
    for (Node* node : list)
    {
        if ((node->cost < minCost || minCost == -1.0f) && node->onList_ != onList::CLOSE)
        {
            //is a valid node
            if (terrain->is_valid_grid_position(node->pos) && node->cost >= 0.0f)
            {
                cheapestNode = node;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
                minCost = cheapestNode->cost;
            }
        }
    }

    return cheapestNode;
}

int AStarPather::findNodeIndex(std::list<Node*> list, Node* node)
{
    int i = 0;
    int index = 0;
    for (Node* node_ : list)
    {
        if (node_->pos == node->pos)
        {
                index = i;
        }
        i++;
    }

    return index;
}