#include <iostream>
#include <string>
#include <set>
#include <cstdlib>
#include <map>
#include <chrono>

using namespace std;
using namespace std::chrono;

struct tile {
    int x;
    int y;
    int cost;
    int distGoal;
    int mapWidth; //we need to stock that so our < operator works properly
    bool operator<(const tile& t2) const
    {
        if(distGoal!=t2.distGoal){
            return distGoal < t2.distGoal;
        }
        else{
            return abs(x+(mapWidth*y)) < abs(t2.x+(mapWidth*t2.y));
        }
    }
    bool operator==(const tile& t2) const
    {
        return (x == t2.x && y == t2.y);
    }
};



int FindPath(const int nStartX, const int nStartY,
             const int nTargetX, const int nTargetY,
             const unsigned char* pMap, const int nMapWidth, const int nMapHeight,
             int* pOutBuffer, const int nOutBufferSize);

int main(){


    const unsigned char pMap[] = {1,1,1,1,1,1,0,0,0
                                 ,0,1,0,1,0,1,1,1,1
                                 ,0,1,1,1,0,0,0,0,1};
    int pOutBuffer[12];
    int pathLength;
    high_resolution_clock::time_point t1 = high_resolution_clock::now();
    for(int i =0;i<100000;i++){
        pathLength = FindPath(0,0,8,2,pMap,9,3,pOutBuffer,12);
    }
    high_resolution_clock::time_point t2 = high_resolution_clock::now();

    auto duration = duration_cast<microseconds>( t2 - t1 ).count();

    cout << duration << endl;
    for(int i = 0; i<pathLength; i++){
        cout << pOutBuffer[i] << " ";
    }



}

void buildPath(map<tile,tile> cameFrom,tile cur,int* pOutBuffer);


int FindPath(const int nStartX,const int nStartY,const int nTargetX,const int nTargetY,
            const unsigned char* pMap,const int nMapWidth,const int nMapHeight,
            int* pOutBuffer,const int nOutBufferSize){
    struct tile start;
    start.x = nStartX;
    start.y = nStartY;
    start.cost = 0;
    start.distGoal = 0;
    start.mapWidth = nMapWidth;
    set<tile> openQueue, closeQueue;
    map<tile,tile> cameFrom; //maps a tile with the one to use to get to here
    openQueue.insert(start);
    while(!openQueue.empty()){
        struct tile cur = *(openQueue.begin());
        closeQueue.insert(cur);
        if(cur.cost > nOutBufferSize){ //this tile is the less costly. If it's above limits, stop
            return -1;
        }
        if(cur.x == nTargetX and cur.y == nTargetY){ //we've reached the goal
            buildPath(cameFrom,cur,pOutBuffer);
            return cur.distGoal;
        }
        set<tile> neighbours;
        for(int k=-1;k<2;k+=2){
            if(!(cur.x+k < 0|| cur.x+k>=nMapWidth)){ //if we're out of bonds, there's nowhere to go
                int positionInMap = cur.x+k + (cur.y*nMapWidth);
                if(!(pMap[positionInMap] == 0)){ //if we can walk there
                    struct tile neigh;
                    neigh.x = cur.x+k;
                    neigh.y = cur.y; //yum
                    neigh.mapWidth = nMapWidth;
                    neigh.cost = cur.cost + 1;
                    neighbours.insert(neigh);
                }
            }

            if(!(cur.y+k < 0 || cur.y+k>=nMapHeight)){ //if we're out of bonds, there's nowhere to go
                int positionInMap = cur.x + ((cur.y+k)*nMapWidth);
                if(!(pMap[positionInMap] == 0)){ //if we can walk there
                    struct tile neigh;
                    neigh.x = cur.x;
                    neigh.y = cur.y+k;
                    neigh.mapWidth = nMapWidth;
                    neigh.cost = cur.cost + 1;
                    neighbours.insert(neigh);
                }
            }
        }
            for(auto n : neighbours)
            {
                if(closeQueue.find(n) == closeQueue.end()){
                    set<tile>::iterator it;
                    it = openQueue.find(n);
                    if(it == openQueue.end()){
                        n.distGoal = abs(n.x - nTargetX) + abs(n.y - nTargetY) + n.cost;
                        cameFrom.insert({n, cur});
                        openQueue.insert(n);
                    }
                    else if((*it).cost <= n.cost){
                        continue;
                    }
                    else{
                    }


               }
            }
        openQueue.erase(cur);
    }
    return -1;
}

void buildPath(map<tile,tile> cameFrom, tile cur, int* pOutBuffer){
    int i = cur.distGoal - 1;
    pOutBuffer[i] = cur.x + cur.y*cur.mapWidth;
    map<tile,tile>::iterator it = cameFrom.find(cur);
    while (it != cameFrom.end()){
        i--;
        if(i<0){
            break;
        }
        cur = it->second;
        pOutBuffer[i] = cur.x + cur.y*cur.mapWidth;
        it = cameFrom.find(cur);
    }
}