#include <iostream>
#include <fstream>
#include <algorithm>
#include <vector>
#include <string>
#include <sstream>
#include <list>

#define INFINITY 1000000;

using namespace std;

const int arrayXsize = 10; // 10 for testing, 100 for final
const int arrayYsize = 10; // 10 for testing, 100 for final
int costMap[arrayYsize][arrayXsize];

class AStarSort {
    public:
    struct sNode
    {
        bool bVisited = false;          // Has this node been visited?
        int iDistRemain;                // Distance remaining to goal
        int iDistTaken;                 // Distance from start
        int iNodeWeight;                // Node wieght
        int x;                          // X-coordinate of node
        int y;                          // Y-coordinate of node
        vector<sNode*> vecNeighbors;    // Neighbors
        sNode* parent;                  // Node we came through to get here
    };

    void mapImport()
    {
        // The Following code reads in the textfile of choice, and
        // makes a 2D array of points with the cost values of each point
        int xIndex = 0;
        int yIndex = 0;
        int tempInt = 0;
        string rawInput;
        ifstream readFile ("data/testInput.txt", ios::in);

        while(getline(readFile, rawInput))
        {
            for(xIndex = 0;
                xIndex < rawInput.size();
                xIndex++)
            {
                tempInt = rawInput[xIndex]-'0';
                costMap[yIndex][xIndex] = tempInt;
            }
            yIndex++;
        }

        // End of file read

        return;
    };

    public:
    void solveAStar()
    {
        // this creates all the nodes needed, and initializes them with
        // a location, an unvisited state, no parent, and all neighbors
        auto nodes = new sNode[arrayYsize * arrayXsize];
        for (int y = 0; y < arrayYsize; y++)
        {
            for (int x = 0; x < arrayXsize; x++)
            {
                nodes[x * arrayXsize + y].x=x;
                nodes[x * arrayXsize + y].y=y;
                nodes[x * arrayXsize + y].bVisited = false;
                nodes[x * arrayXsize + y].iDistRemain = INFINITY;
                nodes[x * arrayXsize + y].iDistTaken = INFINITY;
                nodes[x * arrayXsize + y].iNodeWeight = costMap[y][x];
                nodes[x * arrayXsize + y].parent = nullptr;
                if(y>0)
                nodes[x * arrayXsize + y].vecNeighbors.push_back
                (
                    &nodes[(x + 0) * arrayXsize + (y - 1)]
                );
                if(y< arrayYsize - 1)
                nodes[x * arrayXsize + y].vecNeighbors.push_back
                (
                    &nodes[(x + 0) * arrayXsize + (y + 1)]
                );
                if(x>0)
                nodes[x * arrayXsize + y].vecNeighbors.push_back
                (
                    &nodes[(x - 1) * arrayXsize + (y - 0)]
                );
                if(x< arrayYsize - 1)
                nodes[x * arrayXsize + y].vecNeighbors.push_back
                (
                    &nodes[(x + 1) * arrayXsize + (y + 0)]
                );
            }
        }

        // This creates the starting and ending nodes, and initializes them
        sNode *nodeStart = &nodes[0];
        sNode *nodeEnd = &nodes[ (arrayXsize * arrayYsize) - 1 ];

        auto manhattanDist = [](sNode* a)
            {
                return  ((arrayXsize - 1) - a->x) + ((arrayYsize - 1) - a->y);
            };
        sNode *nodeCurrent = nodeStart;
        nodeStart->iDistTaken = 0;
        nodeStart->iDistRemain = manhattanDist(nodeStart);
        list<sNode*> listNotTestedNodes;
        listNotTestedNodes.push_back(nodeStart);

        while(!listNotTestedNodes.empty())
        {
            // Sort by order of remaining distance to the goal
            listNotTestedNodes.sort([](const sNode* lhs, const sNode* rhs)
                { return lhs ->iDistRemain < rhs->iDistRemain; });

            // Clear out visited nodes
            while(!listNotTestedNodes.empty() && listNotTestedNodes.front()->bVisited)
                listNotTestedNodes.pop_front();

            // If list is empty, leave loop
            if(listNotTestedNodes.empty()) break;

            // Visit the node at the front of the list
            nodeCurrent = listNotTestedNodes.front();
            nodeCurrent->bVisited = true;

            //check each node's neighbors
            for (auto nodeNeighbor : nodeCurrent->vecNeighbors)
            {
                if (!nodeNeighbor->bVisited)
                    listNotTestedNodes.push_back(nodeNeighbor);
            }
        }

        return;
    };
};

int main(void)
{
    AStarSort functionCall;
    functionCall.mapImport();
    functionCall.solveAStar();
    return 0;
}