#include <iostream>
#include <cstring>
#include <cstdlib>
#include <iomanip>
#include <math.h>
#include <ctime>
#include <string>
#include <queue>


using namespace std;

const int IDIM = 15; // horizontal size of the squares
const int JDIM = 15; // vertical size size of the squares
const int NDIR = 4; // number of possible directions to go at any position

// if NDIR = 4
const int iDir[NDIR] = {1, 0, -1, 0};
const int jDir[NDIR] = {0, 1, 0, -1};

// if NDIR = 8
//const int iDir[NDIR] = {1, 1, 0, -1, -1, -1, 0, 1};
//const int jDir[NDIR] = {0, 1, 1, 1, 0, -1, -1, -1};

int squares[IDIM][JDIM];

// list of closed (check-out) nodes
int closedNodes[IDIM][JDIM];

// list of open (not-yet-checked-out) nodes
int openNodes[IDIM][JDIM];

// map of directions (0: East, 1: North, 2: West, 3: South)
int dirMap[IDIM][JDIM];

struct Location
{
    int row, col;

    Location()
    {
        row = col = 0;
    };

    Location(int r, int c)
    {
        row = r;
        col = c;
    };
};

class Node
{
    // current position
    int rPos;
    int cPos;

    // total distance already travelled to reach the node
    int GValue;

    // FValue = GValue + remaining distance estimate
    int FValue;  // smaller FValue gets priority

    public:
        Node(const Location &loc, int g, int f)
            {rPos = loc.row; cPos = loc.col; GValue = g; FValue = f;}

		Location getLocation() const {return Location(rPos,cPos);}
        int getGValue() const {return GValue;}
        int getFValue() const {return FValue;}

        void calculateFValue(const Location& locDest)
        {
             FValue = GValue + getHValue(locDest) * 10;
        }


        void updateGValue(const int & i) // i: direction
        {
            GValue += (NDIR == 8 ? (i % 2 == 0 ? 10 : 14) : 10);
        }

        // Estimation function for the remaining distance to the goal.
        const int & getHValue(const Location& locDest) const
        {
            static int rd, cd, d;
            rd = locDest.row - rPos;
            cd = locDest.col - cPos;

            // Euclidian Distance
            // d = static_cast<int>(sqrt((double)(rd*rd+cd*cd)));

            // Manhattan distance
            d = abs(rd) + abs(cd);

            // Chebyshev distance
            //d = max(abs(rd), abs(cd));

            return(d);
        }

	// Determine FValue (in the priority queue)
	friend bool operator<(const Node & a, const Node & b)
	{
	    return a.getFValue() > b.getFValue();
	}
};

// A-star algorithm.
// The path returned is a string of direction digits.
string pathFind( const Location &locStart ,
                 const Location &locFinish )

{
    // list of open (not-yet-checked-out) nodes
    static priority_queue<Node> q[2];

    // q index
    static int qi;

    static Node* pNode1;
    static Node* pNode2;
    static int i, j, row, col, iNext, jNext;
    static char c;
    qi = 0;

    // reset the Node lists (0 = ".")
    for(j = 0; j < JDIM; j++) {
        for(i = 0; i < IDIM; i++) {
            closedNodes[i][j] = 0;
            openNodes[i][j] = 0;
        }
    }

    // create the start node and push into list of open nodes
    pNode1 = new Node(locStart, 0, 0);
    pNode1->calculateFValue(locFinish);
    q[qi].push(*pNode1);

    // A* search
    while(!q[qi].empty()) {
        // get the current node w/ the lowest FValue
        // from the list of open nodes
        pNode1 = new Node( q[qi].top().getLocation(),
                     q[qi].top().getGValue(), q[qi].top().getFValue());

        row = (pNode1->getLocation()).row;
	col = pNode1->getLocation().col;
	cout << "row, col=" << row << "," << col << endl;

	// remove the node from the open list
        q[qi].pop();
        openNodes[row][col] = 0;

        // mark it on the closed nodes list
        closedNodes[row][col] = 1;

        // stop searching when the goal state is reached
        if(row == locFinish.row && col == locFinish.col) {
		// drawing direction map
		cout << endl;
		for(j = JDIM - 1; j >= 0; j--) {
			for(i = 0; i < IDIM; i++) {
				cout << dirMap[i][j];
			}
			cout << endl;
		}
		cout << endl;

	    // generate the path from finish to start from dirMap
            string path = "";
            while(!(row == locStart.row && col == locStart.col)) {
                j = dirMap[row][col];
                c = '0' + (j + NDIR/2) % NDIR;
                path = c + path;
                row += iDir[j];
                col += jDir[j];
            }

            // garbage collection
            delete pNode1;

            // empty the leftover nodes
            while(!q[qi].empty()) q[qi].pop();
            return path;
        }

        // generate moves in all possible directions
        for(i = 0; i < NDIR; i++) {
            iNext = row + iDir[i];
	    jNext = col + jDir[i];

	    // if not wall (obstacle) nor in the closed list
            if(!(iNext < 0 || iNext > IDIM - 1 || jNext < 0 || jNext > JDIM - 1 ||
			squares[iNext][jNext] == 1 || closedNodes[iNext][jNext] == 1)) {

		// generate a child node
                pNode2 = new Node( Location(iNext, jNext), pNode1->getGValue(), pNode1->getFValue());
                pNode2->updateGValue(i);
                pNode2->calculateFValue(locFinish);

                // if it is not in the open list then add into that
                if(openNodes[iNext][jNext] == 0) {
                    openNodes[iNext][jNext] = pNode2->getFValue();
                    q[qi].push(*pNode2);
                    // mark its parent node direction
                    dirMap[iNext][jNext] = (i + NDIR/2) % NDIR;
                }

		// already in the open list
                else if(openNodes[iNext][jNext] > pNode2->getFValue()) {
                    // update the FValue info
                    openNodes[iNext][jNext] = pNode2->getFValue();

                    // update the parent direction info,  mark its parent node direction
                    dirMap[iNext][jNext] = (i + NDIR/2) % NDIR;

                    // replace the node by emptying one q to the other one
                    // except the node to be replaced will be ignored
                    // and the new node will be pushed in instead
                    while(!(q[qi].top().getLocation().row == iNext &&
                        q[qi].top().getLocation().col == jNext)) {
                        q[1 - qi].push(q[qi].top());
                        q[qi].pop();
                    }

		    // remove the wanted node
                    q[qi].pop();

                    // empty the larger size q to the smaller one
                    if(q[qi].size() > q[1 - qi].size()) qi = 1 - qi;
                    while(!q[qi].empty()) {
                        q[1 - qi].push(q[qi].top());
                        q[qi].pop();
                    }
                    qi = 1 - qi;

		    // add the better node instead
                    q[qi].push(*pNode2);
                }
                else delete pNode2;
            }
        }
        delete pNode1;
    }
    // no path found
    return "";
}

int main()
{
    // create empty squares
    for(int j = 0; j < JDIM; j++) {
        for(int i = 0; i < IDIM; i++) squares[i][j] = 0;
    }

    // make wall
    squares[4][2] = 1;
    squares[4][3] = 1;
    squares[4][4] = 1;

    // starting and ending positions
    int iStart = 2,jStart = 3;
	int iEnd = 6,jEnd = 3;

    cout << "Grid Size (IDIM,JDIM): "<< IDIM<< "," << JDIM << endl;
    cout << "Start: " << iStart<<","<< jStart << endl;
    cout << "Finish: " << iEnd<<","<< jEnd << endl;

    clock_t start = clock();

    // get the path
    string path = pathFind(Location(iStart, jStart), Location(iEnd, jEnd));

    clock_t end = clock();
    double time = double(end - start);
    cout << "Time (ms): "<< time << endl;
    cout << "path: " << path << endl;

    // follow the path on the squares and display it
    if(path.length() > 0) {
        char c;
	int m,n;
        int i = iStart;
        int j = jStart;
        squares[i][j] = 2;
        for(m = 0; m < path.length(); m++) {
            c = path.at(m);
            n = atoi(&c);
            i = i + iDir[n];
            j = j + jDir[n];
            squares[i][j] = 3;
        }
        squares[i][j] = 4;

        // display the squares with the path
        for(j = JDIM - 1; j >= 0; j--) {
            for(i = 0; i < IDIM; i++) {
                if(squares[i][j] == 0)
                    cout << ".";
                else if(squares[i][j] == 1)
                    cout << "O"; //obstacle
                else if(squares[i][j] == 2)
                    cout << "I"; //Initial
                else if(squares[i][j] == 3)
                    cout << "P"; //path
                else if(squares[i][j] == 4)
                    cout << "F"; //final
	    }
            cout << endl;
        }
    }
    return(0);
}