#define OLC_PGE_APPLICATION
#include "pixelGameEngine.h"
#include "olcutils.h"

using namespace olc;

struct node{
    char elevation='a';
    bool visited=false;
    std::vector<node*>connections;
    int myDist=INFINITY;
    int storedDist=INFINITY;
    vi2d pos;
    node*parent=nullptr;
    node(char elevation){
        this->elevation=elevation;
    };
};


vi2d playerPos={0,0};
vi2d endingPos={0,0};
std::vector<std::vector<node>>grid;
node*startNode;
node*endNode;

int main()
{
	std::ifstream file("testinput2");
    while (file.good()){
        std::string line;
        std::getline(file,line);
        std::vector<node>row;
        if (line.length()>0){
            for (int x=0;x<line.length();x++){
                row.push_back(node(line[x]));
                if (line[x]=='S'){
                    playerPos={x,(int)grid.size()};
                }
                if (line[x]=='E'){
                    endingPos={x,(int)grid.size()};
                }
            }
            grid.push_back(row);
        }
    }

    std::cout<<"================"<<std::endl;
    for (int y=0;y<grid.size();y++){
        for (int x=0;x<grid[y].size();x++){
            std::cout<<grid[y][x].elevation;
            grid[y][x].pos={x,y};
            if (x>0){
                grid[y][x].connections.push_back(&grid[y][x-1]);
            }
            if (x<grid[y].size()-1){
                grid[y][x].connections.push_back(&grid[y][x+1]);
            }
            if (y>0){
                grid[y][x].connections.push_back(&grid[y-1][x]);
            }
            if (y<grid.size()-1){
                grid[y][x].connections.push_back(&grid[y+1][x]);
            }
            if (x==playerPos.x&&y==playerPos.y){
                startNode=&grid[y][x];
                std::cout<<"Starting node set."<<std::endl;
            }
            if (x==endingPos.x&&y==endingPos.y){
                endNode=&grid[y][x];
                std::cout<<"Ending node set."<<std::endl;
            }
        }
        std::cout<<std::endl;
    }

    std::vector<node*>searchNodes;
    searchNodes.push_back(startNode);
    while (searchNodes.size()>0){
        node*currentNode=searchNodes.front();
        if (currentNode==startNode){
            currentNode->myDist=pow(currentNode->pos.x-endNode->pos.x,2)+pow(currentNode->pos.y-endNode->pos.y,2);
        }
        currentNode->visited=true;
        if (currentNode->elevation=='S'){
            currentNode->elevation='a'-1;
        }
        if (currentNode->elevation=='E'){
            currentNode->elevation='z'+1;
        }
        for (int i=0;i<currentNode->connections.size();i++){
            node*neighbor=currentNode->connections[i];
            if (neighbor==endNode){
                //We can't go any higher.
                endNode->parent=currentNode;
                break;
            }
            if (neighbor->elevation-1<=currentNode->elevation){
                if (neighbor->parent==nullptr||currentNode->myDist<neighbor->storedDist){
                    neighbor->parent=currentNode;
                    neighbor->storedDist=currentNode->myDist;
                    neighbor->myDist=pow(neighbor->pos.x-endNode->pos.x,2)+pow(neighbor->pos.y-endNode->pos.y,2);
                    if (!neighbor->visited){
                        searchNodes.push_back(neighbor);
                    }
                }
            }
        }
        searchNodes.erase(searchNodes.begin());
    }

    node*searchNode=endNode;
    int jumps=0;
    while (searchNode!=startNode){
        std::cout<<"Node goes from "<<searchNode->pos<<" to "<<searchNode->parent->pos<<std::endl;
        jumps++;
        searchNode=searchNode->parent;
    }
    std::cout<<"Shortest path is "<<jumps<<" hops"<<std::endl;

	return 0;
}