#ifdef __cplusplus
#include <cstdlib>
#else
#include <stdlib.h>
#endif

#include <stdio.h>

#include <SDL/SDL.h>
#include <SDL/SDL_gfxPrimitives.h>

#include <vector>
#include <list>
#include <algorithm>
#include <string>
#include <math.h>
#include <iterator>
#include <random>



//SDL_Surface* screen = nullptr;

bool images = true;
unsigned int sizex, sizey;
unsigned int usersize = 10;

bool needupdatebestpath = false;
float cost_path = 0.0f;

bool manualrandomlimit=false;

SDL_Surface* montagne = nullptr;
SDL_Surface* plaine = nullptr;
SDL_Surface* colline = nullptr;
SDL_Surface* mer = nullptr;
SDL_Surface* marecage = nullptr;
SDL_Surface* donjon = nullptr;
SDL_Surface* village = nullptr;
SDL_Surface* player = nullptr;
SDL_Surface* foret = nullptr;

float currentx, currenty;
float currentangle=0;
float dx, dy;

// define the cost for score and life for all types of walkable tiles
float scoreforet = 0.3;     // always walkable
float vieforet = 0.3;

float scoreplaine = 0.2;     // always walkable
float vieplaine = 0.2;

float scorecolline = 0.4;     // always walkable
float viecolline = 0.4;

float scoremarecage = 0.5;     // always walkable
float viemarecage = 0.5;

float scoremer = 0.2;     // walkable only when boat is available (9 donjon visited + 1 village right after donjon #9)
float viemer = 0.2;

// define the cost/life consummed at each click (1 big segment betten than ots of unitary segments)
// maybe not used right now for future revisions
float scoreclick = 0.1;
float vieclick = 0;

int nMapWidth = 146;
int nMapHeight = 91;

int offsetx = 0;
int offsety = 0;

int offsetx_max = 0;
int offsety_max = 0;

float playerx = 47.5;
float playery = 43.5;
float playera = 0;
int passvillage = 1;
float life = 250.0;
float score = 0.0;
bool hasboat = false;
int lastvillage;
std::list<int> visitedDungeons;

float scoreinit;
float scoreoptim;


int mousex = 0;
int mousey = 0;

int selectedNode = 1;
int nbNodeMax = 0;

bool firstround = true ;


char* maptile;

// THE NODE STRUCTURE OF THE MAP
struct sNode
{
    bool Obstacle = false;
    bool Visited = false;
    float GlobalGoal;
    float LocalGoal;
    int x;
    int y;
    float CostScore;
    float CostLife;
    std::vector <sNode*> Neighbours;
    sNode* Parent;
};

struct point2D
{
    float x;
    float y;
};


sNode* nodes = new sNode[nMapWidth * nMapHeight];

// CREATE SPECIAL POINTS OF INTEREST
sNode* nodeStart = nullptr;
sNode* nodeEnd = nullptr;
sNode* DonjonA = nullptr;
sNode* DonjonB = nullptr;
sNode* DonjonC = nullptr;
sNode* DonjonD = nullptr;
sNode* DonjonE = nullptr;
sNode* DonjonF = nullptr;
sNode* DonjonG = nullptr;
sNode* DonjonH = nullptr;
sNode* DonjonI = nullptr;
sNode* DonjonJ = nullptr;
sNode* Village1 = nullptr;
sNode* Village2 = nullptr;
sNode* Village3 = nullptr;
sNode* Village4 = nullptr;
sNode* Village5 = nullptr;
sNode* Village6 = nullptr;
sNode* Village7 = nullptr;
sNode* Village8 = nullptr;
sNode* Village9 = nullptr;
sNode* PlayerStart = nullptr;


SDL_Surface* init_SDL( void )
{
    if ( SDL_Init( SDL_INIT_VIDEO ) < 0 )
    {
        printf( "Unable to init SDL: %s\n", SDL_GetError() );
        return nullptr;
    }

    SDL_Surface* screen = SDL_SetVideoMode(1920, 1040, 16, SDL_HWSURFACE|SDL_DOUBLEBUF);

    if ( !screen )
    {
        printf("Unable to set 1920x1080 video: %s\n", SDL_GetError());
        return nullptr;
    }

    SDL_EnableKeyRepeat( SDL_DEFAULT_REPEAT_DELAY, SDL_DEFAULT_REPEAT_INTERVAL );

    return screen;
}

void load_Map_Ressources( int testsmall )
{
    // load an image
    if (testsmall==32)
    {
        montagne = SDL_LoadBMP("../tiles/montagne32.bmp");
        plaine = SDL_LoadBMP("../tiles/plaine32.bmp");
        colline = SDL_LoadBMP("../tiles/colline32.bmp");
        mer = SDL_LoadBMP("../tiles/mer32.bmp");
        marecage = SDL_LoadBMP("../tiles/marecage32.bmp");
        donjon = SDL_LoadBMP("../tiles/donjon32.bmp");
        village = SDL_LoadBMP("../tiles/village32.bmp");
        player = SDL_LoadBMP("../tiles/player32.bmp");
        foret = SDL_LoadBMP("../tiles/foret32.bmp");
        sizex = sizey = 32;
        images = true;
    }
    else if (testsmall==16)
    {
        montagne = SDL_LoadBMP("../tiles/montagne16.bmp");
        plaine = SDL_LoadBMP("../tiles/plaine16.bmp");
        colline = SDL_LoadBMP("../tiles/colline16.bmp");
        mer = SDL_LoadBMP("../tiles/mer16.bmp");
        marecage = SDL_LoadBMP("../tiles/marecage16.bmp");
        donjon = SDL_LoadBMP("../tiles/donjon16.bmp");
        village = SDL_LoadBMP("../tiles/village16.bmp");
        player = SDL_LoadBMP("../tiles/player16.bmp");
        foret = SDL_LoadBMP("../tiles/foret16.bmp");
        sizex = sizey = 16;
        images = true;
    }
    else if (testsmall==8)
    {
        montagne = SDL_LoadBMP("../tiles/montagne8.bmp");
        plaine = SDL_LoadBMP("../tiles/plaine8.bmp");
        colline = SDL_LoadBMP("../tiles/colline8.bmp");
        mer = SDL_LoadBMP("../tiles/mer8.bmp");
        marecage = SDL_LoadBMP("../tiles/marecage8.bmp");
        donjon = SDL_LoadBMP("../tiles/donjon8.bmp");
        village = SDL_LoadBMP("../tiles/village8.bmp");
        player = SDL_LoadBMP("../tiles/player8.bmp");
        foret = SDL_LoadBMP("../tiles/foret8.bmp");
        sizex = sizey = 8;
        images = true;
    }
    else
    {
        montagne = nullptr;
        plaine = nullptr;
        colline = nullptr;
        mer = nullptr;
        marecage = nullptr;
        donjon = nullptr;
        village = nullptr;
        player = nullptr;
        foret = nullptr;
        sizex = sizey = usersize;
        images = false;
    }
}

void load_Map_Layout( char* maptile )
{
    // START READING THE MAPDATA
    FILE *p;
    char temp;
    p=fopen("../map/map.txt", "r" );
    for(int j = 0; j<nMapHeight; j++)
    {
        for(int i = 0; i<=nMapWidth; i++)
        {
            fread( &maptile[i+j*nMapWidth], sizeof(char), 1, p );
            if (maptile[i+j*nMapWidth]=='D') printf( "Donjon  : X= %d, Y= %d\n", i, j );
            if (maptile[i+j*nMapWidth]=='V') printf( "Village : X= %d, Y= %d\n", i, j );
        }
        fread( &temp, sizeof(char), 1, p ); // read the "\n" at the end of the line
    }
    fclose( p );
}

void init_Nodes_From_Map( char* maptile, sNode* nodes, bool navigable )
{
    for(int j = 0; j<nMapHeight; j++)
    {
        for(int i = 0; i<nMapWidth; i++)
        {
            nodes[i+j*nMapWidth].x=i;
            nodes[i+j*nMapWidth].y=j;
            nodes[i+j*nMapWidth].Parent=nullptr;
            nodes[i+j*nMapWidth].Visited=false;

            if (maptile[i+j*nMapWidth]=='M') // Montains are obstacle
            {
                nodes[i+j*nMapWidth].Obstacle=true;
            }
            else if ((maptile[i+j*nMapWidth]=='O') && (navigable==false)) // Oceans are obstacle
            {
                nodes[i+j*nMapWidth].Obstacle=true;
            }
            else if ((maptile[i+j*nMapWidth]=='O') && (navigable==true)) // Oceans are navigable when boat is achieved
            {
                nodes[i+j*nMapWidth].Obstacle=false;
                nodes[i+j*nMapWidth].CostScore=0.2;
                nodes[i+j*nMapWidth].CostLife=0.2;
            }
            else if (maptile[i+j*nMapWidth]=='z')  // the map has been locally corrected manually to avoid impossible jumps
            {
                nodes[i+j*nMapWidth].Obstacle=true;
            }
            else
            {
                nodes[i+j*nMapWidth].Obstacle=false;

                if (maptile[i+j*nMapWidth]=='P')   // Plains
                {
                    nodes[i+j*nMapWidth].CostScore=0.2;
                    nodes[i+j*nMapWidth].CostLife=0.2;
                }
                else if (maptile[i+j*nMapWidth]=='F') // Forest
                {
                    nodes[i+j*nMapWidth].CostScore=0.3;
                    nodes[i+j*nMapWidth].CostLife=0.3;
                }
                else if (maptile[i+j*nMapWidth]=='C') // Hills
                {
                    nodes[i+j*nMapWidth].CostScore=0.4;
                    nodes[i+j*nMapWidth].CostLife=0.4;
                }
                else if (maptile[i+j*nMapWidth]=='E') // Mangrove
                {
                    nodes[i+j*nMapWidth].CostScore=0.5;
                    nodes[i+j*nMapWidth].CostLife=0.5;
                }
                else if (maptile[i+j*nMapWidth]=='V') // Village
                {
                    nodes[i+j*nMapWidth].CostScore=0.2;
                    nodes[i+j*nMapWidth].CostLife=0.2;
                }
                else if (maptile[i+j*nMapWidth]=='D') // Donjon
                {
                    nodes[i+j*nMapWidth].CostScore=0.2;
                    nodes[i+j*nMapWidth].CostLife=0.2;
                }
                else // Player start (Forest in fact)
                {
                    nodes[i+j*nMapWidth].CostScore=0.3;
                    nodes[i+j*nMapWidth].CostLife=0.3;
                }
            }
        }
    }
}

void make_Nodes_Connections( sNode* nodes )
{
    // Create connections - in this case nodes are on a regular grid
    for (int x = 0; x < nMapWidth; x++)
        for (int y = 0; y < nMapHeight; y++)
        {
            /*
            if(y>0)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y - 1) * nMapWidth + (x + 0)]);
            if(y<nMapHeight-1)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 1) * nMapWidth + (x + 0)]);
            if (x>0)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 0) * nMapWidth + (x - 1)]);
            if(x<nMapWidth-1)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 0) * nMapWidth + (x + 1)]);

            // We can also connect diagonally

            if (y>0 && x>0)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y - 1) * nMapWidth + (x - 1)]);
            if (y<nMapHeight-1 && x>0)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 1) * nMapWidth + (x - 1)]);
            if (y>0 && x<nMapWidth-1)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y - 1) * nMapWidth + (x + 1)]);
            if (y<nMapHeight - 1 && x<nMapWidth-1)
                nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 1) * nMapWidth + (x + 1)]);
            */

            if(y>0)
                if(nodes[(y - 1) * nMapWidth + (x + 0)].Obstacle == false)
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y - 1) * nMapWidth + (x + 0)]);

            if(y<nMapHeight-1)
                if(nodes[(y + 1) * nMapWidth + (x + 0)].Obstacle == false)
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 1) * nMapWidth + (x + 0)]);

            if (x>0)
                if(nodes[(y + 0) * nMapWidth + (x - 1)].Obstacle == false)
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 0) * nMapWidth + (x - 1)]);

            if(x<nMapWidth-1)
                if(nodes[(y + 0) * nMapWidth + (x + 1)].Obstacle == false)
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 0) * nMapWidth + (x + 1)]);

            // We can also connect diagonally

            if (y>0 && x>0)
                if((nodes[(y - 0) * nMapWidth + (x - 1 )].Obstacle == false) && (nodes[(y - 1) * nMapWidth + (x - 0 )].Obstacle == false))
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y - 1) * nMapWidth + (x - 1)]);

            if (y<nMapHeight-1 && x>0)
                if((nodes[(y - 0) * nMapWidth + (x - 1 )].Obstacle == false) && (nodes[(y + 1) * nMapWidth + (x - 0 )].Obstacle == false))
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 1) * nMapWidth + (x - 1)]);

            if (y>0 && x<nMapWidth-1)
                if((nodes[(y - 1) * nMapWidth + (x + 0 )].Obstacle == false) && (nodes[(y + 0) * nMapWidth + (x + 1 )].Obstacle == false))
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y - 1) * nMapWidth + (x + 1)]);

            if (y<nMapHeight - 1 && x<nMapWidth-1)
                if((nodes[(y - 0) * nMapWidth + (x + 1 )].Obstacle == false) && (nodes[(y + 1) * nMapWidth + (x - 0 )].Obstacle == false))
                    nodes[y*nMapWidth + x].Neighbours.push_back(&nodes[(y + 1) * nMapWidth + (x + 1)]);
        }
}

void reinit_Nodes_Status( sNode* nodes )
{
    for (int x = 0; x < nMapWidth; x++)
        for (int y = 0; y < nMapHeight; y++)
        {
            nodes[y*nMapWidth + x].Visited = false;
            nodes[y*nMapWidth + x].GlobalGoal = INFINITY;
            nodes[y*nMapWidth + x].LocalGoal = INFINITY;
            nodes[y*nMapWidth + x].Parent = nullptr;
            nodes[y*nMapWidth + x].Neighbours.clear();
        }
}

void draw_Map( SDL_Surface* screen )
{
    // clear screen
    SDL_FillRect(screen, 0, SDL_MapRGB(screen->format, 0, 0, 0));

    SDL_Rect dstrect;

    for(int j = 0; j<nMapHeight; j++)
    {
        for(int i = 0; i<nMapWidth; i++)
        {
            dstrect.x = i*sizex - offsetx;
            dstrect.y = j*sizey - offsety;

            if (images == true)
            {

                switch (maptile[i+j*nMapWidth])
                {
                case 'M' :
                    SDL_BlitSurface(montagne, 0, screen, &dstrect);
                    break;
                case 'P' :
                    SDL_BlitSurface(plaine, 0, screen, &dstrect);
                    break;
                case 'C' :
                    SDL_BlitSurface(colline, 0, screen, &dstrect);
                    break;
                case 'O' :
                    SDL_BlitSurface(mer, 0, screen, &dstrect);
                    break;
                case 'E' :
                    SDL_BlitSurface(marecage, 0, screen, &dstrect);
                    break;
                case 'D' :
                    SDL_BlitSurface(donjon, 0, screen, &dstrect);
                    break;
                case 'V' :
                    SDL_BlitSurface(village, 0, screen, &dstrect);
                    break;
                case 'X' :
                    SDL_BlitSurface(player, 0, screen, &dstrect);
                    playerx = (i*sizex)+sizex/2;
                    playery = (j*sizey)+sizex/2;
                    filledCircleRGBA( screen, playerx, playery, sizex/4, 255, 0, 0, 255 );
                    break;
                case 'F' :
                    SDL_BlitSurface(foret, 0, screen, &dstrect);
                    break;
                }
            }
            else
            {
                sizex=sizey=usersize;
                switch (maptile[i+j*nMapWidth])
                {
                case 'M' : // brown - marroon
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 200, 100, 0, 255 );
                    break;
                case 'P' : // light green
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 128, 255, 0, 255 );
                    break;
                case 'C' : // orangish
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 255, 128, 0, 255 );
                    break;
                case 'O' : // dark blue
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 0, 0, 204, 255 );
                    break;
                case 'E' : // light blue
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 102, 178, 255, 255 );
                    break;
                case 'D' : // Red
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 255, 0, 0, 255 );
                    break;
                case 'V' : // yellow
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 255, 255, 0, 255 );
                    break;
                case 'X' : // white
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 255, 255, 255, 255 );
                    playerx = (i*sizex)+sizex/2;
                    playery = (j*sizey)+sizex/2;
                    filledCircleRGBA( screen, playerx, playery, sizex/4, 255, 0, 0, 255 );
                    break;
                case 'F' : // dark green
                    boxRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 0, 102, 51, 255 );
                    break;
                }

                rectangleRGBA( screen, i*sizex - offsetx, j*sizey - offsety, (i+1)*sizex - offsetx, (j+1)*sizey - offsety, 0, 0, 0, 255 );
            }
        }

    }
}

float calculate_Path( sNode* nodes, sNode* nodeStart, sNode* nodeEnd, bool minimizeturns )
{
    auto distance = [](sNode* a, sNode* b)
    {
        return sqrtf((a->x - b->x)*(a->x - b->x) + (a->y - b->y)*(a->y - b->y));
    };

    auto heuristic = [distance](sNode* a, sNode* b)
    {
        return distance(a, b);
    };

    float cost=0.0f;
    float penalty = 0.0f;
    float scale = 10.0f;

    // Setup starting conditions
    sNode *nodeCurrent = nodeStart;
    nodeStart->LocalGoal = 0.0f;
    nodeStart->GlobalGoal = heuristic(nodeStart, nodeEnd);

    std::list <sNode*> listNotTestedNodes;
    listNotTestedNodes.push_back(nodeStart);

    while (!listNotTestedNodes.empty() && nodeCurrent != nodeEnd)
    {
        listNotTestedNodes.sort([](const sNode* lhs, const sNode* rhs)
        {
            return lhs->GlobalGoal < rhs->GlobalGoal;
        } );

        while(!listNotTestedNodes.empty() && listNotTestedNodes.front()->Visited)
            listNotTestedNodes.pop_front();

        if (listNotTestedNodes.empty())
            break;

        nodeCurrent = listNotTestedNodes.front();
        nodeCurrent->Visited = true;


        float PossiblyLowerGoal = 0.0f;

        // Check each of this node's neighbours...
        for (auto nodeNeighbour : nodeCurrent->Neighbours)
        {
            if (!nodeNeighbour->Visited && nodeNeighbour->Obstacle == 0)
                listNotTestedNodes.push_back(nodeNeighbour);

            if (minimizeturns==true)
            {
                if (nodeCurrent==nodeStart)
                {
                    penalty = 0.1f * scale;
                }
                else
                {
                    int xprevious = nodeCurrent->x - nodeCurrent->Parent->x;
                    int yprevious = nodeCurrent->y - nodeCurrent->Parent->y;
                    int xnext = nodeCurrent->x - nodeNeighbour->x;
                    int ynext = nodeCurrent->y - nodeNeighbour->y;

                    if (((xprevious+xnext)==0) and ((yprevious+ynext)==0)) // si on vient de la même direction
                    {
                        penalty = 0.0f;
                    }
                    else
                    {
                        penalty = 0.1f;
                    }
                }
                PossiblyLowerGoal = nodeCurrent->LocalGoal + (distance(nodeCurrent, nodeNeighbour) * nodeNeighbour->CostScore + penalty)*scale;
            }
            else
            {
                PossiblyLowerGoal = nodeCurrent->LocalGoal + distance(nodeCurrent, nodeNeighbour) * nodeNeighbour->CostScore * scale;
            }

            /*
            // PONDERATION OF THE HEURISTIC FUNCTION
            */

            if (PossiblyLowerGoal < nodeNeighbour->LocalGoal)
            {
                nodeNeighbour->Parent = nodeCurrent;
                nodeNeighbour->LocalGoal = PossiblyLowerGoal;

                nodeNeighbour->GlobalGoal = nodeNeighbour->LocalGoal + heuristic(nodeNeighbour, nodeEnd);
            }
        }


    }

    cost = nodeEnd->GlobalGoal;

    return cost/scale;
}

float interpolx2y( float x1, float y1, float x2, float y2, float x )
{
    return (y1+(y2-y1)*(x-x1)/(x2-x1));
}

float interpoly2x( float x1, float y1, float x2, float y2, float y )
{
    return (x1+(x2-x1)*(y-y1)/(y2-y1));
}

float sign( float x )
{
    if (x==0) return 0.0f;
    else if (x>0) return 1.0f;
    else return -1.0f;
}

char get_From_Map( int x, int y)
{
    return maptile[x+y*nMapWidth];
}

std::list<point2D> segment( float x1, float y1, float x2, float y2 )
{
    float dx = sign(x2-x1);
    float dy = sign(y2-y1);
    std::list<point2D> lx;
    std::list<point2D> ly;
    std::list<point2D> l;
    int rmin;
    int rmax;

    if (dx!=0)
    {
        if (dx>0)
        {
            rmin = (int) (ceil(x1));
            rmax = (int) (floor(x2));

            for( int k=rmin; k<=rmax; k++ )
                lx.push_back( { k, interpolx2y(x1,y1,x2,y2,k) } );
        }
        else
        {
            rmin = (int) (ceil(x2));
            rmax = (int) (floor(x1));

            for( int k=rmax; k>=rmin; k-- )
                lx.push_back( { k, interpolx2y(x1,y1,x2,y2,k) } );
        }
    }

    if (dy!=0)
    {
        if (dy>0)
        {
            rmin = (int) (ceil(y1));
            rmax = (int) (floor(y2));

            for( int k=rmin; k<=rmax; k++ )
                ly.push_back( { interpoly2x(x1,y1,x2,y2,k), k } );
        }
        else
        {
            rmin = (int) (ceil(y2));
            rmax = (int) (floor(y1));

            for( int k=rmax; k>=rmin; k-- )
                ly.push_back( { interpoly2x(x1,y1,x2,y2,k), k } );
        }
    }

    l.push_back( {x1, y1 } );

    //while ((lx.size()!=0) || (ly.size()!=0))
    while (!lx.empty() || !ly.empty())
    {
        if ((ly.size()==0) || ((lx.size()!=0) && ((abs(lx.front().x - l.back().x))<(abs(ly.front().x-l.back().x)))))
        {
            l.push_back( { lx.front().x, lx.front().y } );
            lx.pop_front();
        }
        else
        {
            l.push_back( { ly.front().x, ly.front().y } );
            ly.pop_front();
        }
    }
    l.push_back( {x2, y2 } );

    return l;
}

bool can_Go( char id, bool boat )
{
    if ((id=='M') || (id=='z'))
        return false;
    else if ((id=='O') && (boat==false))
        return false;
    else
        return true;
}

float calculate_Distance_To( float x, float y )
{
    return sqrt((x-playerx)*(x-playerx)+(y-playery)*(y-playery));
}

void draw_Segment( SDL_Surface* screen, std::list<point2D> passage )
{

    int z;
    float A, B;
    //int Q;

    z=1;
    //Q=passage.size();
    for( auto n : passage )
    {
        A=n.x;
        B=n.y;
        //printf( " element %d / %d : X=%f - Y= %f \n", z, Q, A, B );

        circleRGBA( screen, A*sizex - offsetx, B*sizey - offsety, 4, 0, 255, 255, 255 );
        rectangleRGBA( screen, floor(A)*sizex - offsetx, floor(B)*sizey - offsety, (floor(A)+1)*sizex - offsetx, (floor(B)+1)*sizey - offsety, 255, 255, 0, 255 );
        rectangleRGBA( screen, floor(A)*sizex+1 - offsetx, floor(B)*sizey+1 - offsety, (floor(A)+1)*sizex-1 - offsetx, (floor(B)+1)*sizey-1 - offsety, 255, 255, 0, 255 );

        z++;
    }

    printf("Segment x1= %f y1= %f --> x2= %f y2= %f\n", passage.front().x, passage.front().y, passage.back().x, passage.back().y );

    filledCircleRGBA( screen, passage.front().x*sizex - offsetx, passage.front().y*sizey - offsety, 4, 255, 0, 100, 255 );
    lineRGBA( screen, passage.front().x*sizex - offsetx, passage.front().y*sizey - offsety, passage.back().x*sizex - offsetx, passage.back().y*sizey - offsety, 255, 0, 100, 255 );
    filledCircleRGBA( screen, passage.back().x*sizex - offsetx, passage.back().y*sizey - offsety, 4, 255, 0, 100, 255 );

    //SDL_Flip( screen );
}

float minimum( float x, float y )
{
    return x<=y ? x : y;
}

float maximum( float x, float y )
{
    return x>=y ? x : y;
}

float get_Score( char c )
{
    if ((c=='P') || (c=='O'))
        return 0.2f;
    else if (c=='F')
        return 0.3f;
    else if (c=='C')
        return 0.4f;
    else if (c=='E')
        return 0.5f;
    else
        return 0.2f;
}

bool is_visited(std::list<int> listing, int value)
{
    if (listing.empty()) return false;
    for( int n : listing )
    {
        if (n==value) return true;
    }
    return false;
}

void move_Forward( SDL_Surface* screen, float l )
{
    char c;

    score -=0.1;
    float a = currentangle * 3.14159265359 / 180.0;

    float destix = playerx + l*cos(a);
    float destiy = playery + l*sin(a);

    std::list<point2D> passage;
    passage = segment( playerx, playery, destix, destiy );

    //draw_Segment( screen, passage );

    passage.pop_front(); // le premier point est la position actuelle du joueur


    for( auto k : passage )
    {
        int Xr = floor( minimum(playerx, k.x ) );
        int Yr = floor( minimum(playery, k.y ) );
        c = get_From_Map( Xr, Yr );

        if ((!can_Go(c, hasboat)) || (!can_Go( get_From_Map( k.x, k.y ), hasboat))) break;

        float dist = calculate_Distance_To( k.x, k.y );
        float coefficient = get_Score(c);

        float d=dist*coefficient;

        score -= d;
        life -=d;

        //printf("Score : %f Life : %f\n", score, life );

        playerx = k.x;
        playery = k.y;


        if (c=='F')
        {
            rectangleRGBA( screen, Xr*sizex-offsetx, Yr*sizey-offsety, (Xr+1)*sizex-offsetx, (Yr+1)*sizey-offsety, 0, 255, 255, 255 );
            rectangleRGBA( screen, Xr*sizex-offsetx+1, Yr*sizey-offsety+1, (Xr+1)*sizex-offsetx-1, (Yr+1)*sizey-offsety-1, 0, 255, 255, 255 );
            //rectangleRGBA( screen, Xr*sizex-offsetx+2, Yr*sizey-offsety+2, (Xr+1)*sizex-offsetx-2, (Yr+1)*sizey-offsety-2, 0, 255, 255, 255 );
        }
        if (c=='C')
        {
            rectangleRGBA( screen, Xr*sizex-offsetx, Yr*sizey-offsety, (Xr+1)*sizex-offsetx, (Yr+1)*sizey-offsety, 255, 127, 127, 255 );
            rectangleRGBA( screen, Xr*sizex-offsetx+1, Yr*sizey-offsety+1, (Xr+1)*sizex-offsetx-1, (Yr+1)*sizey-offsety-1, 255, 127, 127, 255 );
            //rectangleRGBA( screen, Xr*sizex-offsetx+2, Yr*sizey-offsety+2, (Xr+1)*sizex-offsetx-2, (Yr+1)*sizey-offsety-2, 0, 255, 255, 255 );
        }
        if (c=='E')
        {
            rectangleRGBA( screen, Xr*sizex-offsetx, Yr*sizey-offsety, (Xr+1)*sizex-offsetx, (Yr+1)*sizey-offsety, 255, 0, 0, 255 );
            rectangleRGBA( screen, Xr*sizex-offsetx+1, Yr*sizey-offsety+1, (Xr+1)*sizex-offsetx-1, (Yr+1)*sizey-offsety-1, 255, 0, 0, 255 );
            rectangleRGBA( screen, Xr*sizex-offsetx+2, Yr*sizey-offsety+2, (Xr+1)*sizex-offsetx-2, (Yr+1)*sizey-offsety-2, 255, 0, 0, 255 );
        }

        if ((c=='D') && (!is_visited(visitedDungeons, Xr)))
        {
            if (life>=100)
            {
                life=maximum( life-100.0, 0.0 );
                score += 100;
                passvillage += 1;
                visitedDungeons.push_back(Xr);
                rectangleRGBA( screen, Xr*sizex-offsetx, Yr*sizey-offsety, (Xr+1)*sizex-offsetx, (Yr+1)*sizey-offsety, 0, 255, 0, 255 );
                rectangleRGBA( screen, Xr*sizex-offsetx+1, Yr*sizey-offsety+1, (Xr+1)*sizex-offsetx-1, (Yr+1)*sizey-offsety-1, 0, 255, 0, 255 );
                rectangleRGBA( screen, Xr*sizex-offsetx+2, Yr*sizey-offsety+2, (Xr+1)*sizex-offsetx-2, (Yr+1)*sizey-offsety-2, 0, 255, 0, 255 );
            }
            else
            {
                rectangleRGBA( screen, Xr*sizex-offsetx, Yr*sizey-offsety, (Xr+1)*sizex-offsetx, (Yr+1)*sizey-offsety, 0, 0, 0, 255 );
                rectangleRGBA( screen, Xr*sizex-offsetx+1, Yr*sizey-offsety+1, (Xr+1)*sizex-offsetx-1, (Yr+1)*sizey-offsety-1, 0, 0, 0, 255 );
                rectangleRGBA( screen, Xr*sizex-offsetx+2, Yr*sizey-offsety+2, (Xr+1)*sizex-offsetx-2, (Yr+1)*sizey-offsety-2, 0, 0, 0, 255 );
            }
            if (visitedDungeons.size()>=10)
            {
                score += 1000;
            }
        }
        if ((c=='V') && (Xr!=lastvillage))
        {
            if ((!hasboat) && (visitedDungeons.size()>=9))
            {
                hasboat = true;
            }
            else if (passvillage>0)
            {
                passvillage -= 1;
                life=minimum( 250.0, life+120.0 );
            }
            lastvillage=Xr;
        }
    }

    passage.clear();

}

/*
double liste_deplacements[] =
{

};
*/


double liste_deplacements[] =
{
  1, -18.3662809302951, 0, 1.58208801714759,
  1, -123.131838658264, 0, 6.43090714751831,
};


std::vector<point2D> initialPoints;

std::vector<point2D> optimizePoints;

std::vector<point2D> bestreducedPath;


float simulate_Path_Initial( SDL_Surface* screen )
{

    int nbpoints;
    nbpoints = sizeof(liste_deplacements)/sizeof(double)/2;

    //filledCircleRGBA( screen, mousex - offsetx, mousey - offsety, sizex/4, 255, 255, 255, 125 );
    currentx=playerx=47.5;
    currenty=playery=43.5;
    currentangle=0;
    score = 0;
    life = 250;
    hasboat = false;
    passvillage = 1;
    lastvillage = 0;
    visitedDungeons.clear();


    initialPoints.clear();

    optimizePoints.clear();

    initialPoints.push_back( { playerx, playery } );
    if (firstround==true) // this is the first time we run the loop
    {
        optimizePoints.push_back( { playerx, playery } );
    }

    for(int u=0; u<nbpoints; u++)
    {
        if (liste_deplacements[u*2] == 0)
        {
            initialPoints.push_back( { playerx + liste_deplacements[u*2+1]*cos( currentangle * 3.14159265359 / 180.0 ),  playery + liste_deplacements[u*2+1]*sin( currentangle * 3.14159265359 / 180.0 ) } );
            if (firstround==true) // this is the first time we run the loop
            {
                optimizePoints.push_back( { playerx + liste_deplacements[u*2+1]*cos( currentangle * 3.14159265359 / 180.0 ),  playery + liste_deplacements[u*2+1]*sin( currentangle * 3.14159265359 / 180.0 ) } );
                nbNodeMax = optimizePoints.size()-1;
            }
            move_Forward( screen, liste_deplacements[u*2+1] );
        }
        else if (liste_deplacements[u*2] == 1)
        {
            currentangle += liste_deplacements[u*2+1];
        }



        //lineRGBA( screen, playerx-offsetx, playery-offsety, currentx-offsetx, currenty-offsety, 255, 0, 255, 255 );
        //filledCircleRGBA( screen, currentx-offsetx, currenty-offsety, sizex/4, 255, 0, 255, 255 );

        //playerx = currentx;
        //playery = currenty;

    }
    firstround=false;
    return score;
}




float simulate_Path_Optimized( SDL_Surface* screen )
{

    int nbpoints;
    nbpoints = sizeof(liste_deplacements)/sizeof(int)/2;

    if (firstround==true) return -1;

    //filledCircleRGBA( screen, mousex - offsetx, mousey - offsety, sizex/4, 255, 255, 255, 125 );
    currentx=playerx=47.5;
    currenty=playery=43.5;
    currentangle=0;
    score = 0;
    life = 250;
    hasboat = false;
    passvillage = 1;
    lastvillage = 0;
    visitedDungeons.clear();



    for(int u=0; u<optimizePoints.size()-1; u++)
    {
        currentangle = atan2(optimizePoints[u+1].y - optimizePoints[u].y, optimizePoints[u+1].x - optimizePoints[u].x) * 180.0 / 3.14159265359;
        float distance = calculate_Distance_To( optimizePoints[u+1].x, optimizePoints[u+1].y );
        move_Forward( screen, distance );
    }
    return score;
}



float simulate_Current_Path( SDL_Surface* screen, std::vector<point2D> path )
{

    if (firstround==true) return -1;

    //filledCircleRGBA( screen, mousex - offsetx, mousey - offsety, sizex/4, 255, 255, 255, 125 );
    currentx=playerx=47.5;
    currenty=playery=43.5;
    currentangle=0;
    score = 0;
    life = 250;
    hasboat = false;
    passvillage = 1;
    lastvillage = 0;
    visitedDungeons.clear();



    for(int u=0; u<path.size()-1; u++)
    {
        currentangle = atan2(path[u+1].y - path[u].y, path[u+1].x - path[u].x) * 180.0 / 3.14159265359;
        float distance = calculate_Distance_To( path[u+1].x, path[u+1].y );
        move_Forward( screen, distance );
    }
    return score;
}


void draw_Path_Initial( SDL_Surface* screen )
{

    int n = initialPoints.size();

    for(int i=0; i<n-1; i++)
    {
        filledCircleRGBA( screen, initialPoints[i].x*sizex - offsetx, initialPoints[i].y*sizey - offsety, 4, 0, 0, 0, 255 );
        lineRGBA( screen, initialPoints[i].x*sizex - offsetx, initialPoints[i].y*sizey - offsety, initialPoints[i+1].x*sizex - offsetx, initialPoints[i+1].y*sizey - offsety, 0, 0, 0, 255 );
        filledCircleRGBA( screen, initialPoints[i+1].x*sizex - offsetx, initialPoints[i+1].y*sizey - offsety, 4, 0, 0, 0, 255 );
    }
    //SDL_Flip( screen );
};


void draw_Path_Optimised( SDL_Surface* screen )
{

    int n = optimizePoints.size();

    for(int i=0; i<n-1; i++)
    {
        filledCircleRGBA( screen, optimizePoints[i].x*sizex - offsetx, optimizePoints[i].y*sizey - offsety, 4, 255, 0, 128, 255 );
        lineRGBA( screen, optimizePoints[i].x*sizex - offsetx, optimizePoints[i].y*sizey - offsety, optimizePoints[i+1].x*sizex - offsetx, optimizePoints[i+1].y*sizey - offsety, 255, 0, 128, 255 );
        filledCircleRGBA( screen, optimizePoints[i+1].x*sizex - offsetx, optimizePoints[i+1].y*sizey - offsety, 4, 255, 0, 128, 255 );
    }

    rectangleRGBA( screen, (optimizePoints[selectedNode].x-0.05)*sizex - offsetx, (optimizePoints[selectedNode].y-0.05)*sizey - offsety, (optimizePoints[selectedNode].x+0.05)*sizex - offsetx, (optimizePoints[selectedNode].y+0.05)*sizey - offsety, 255, 255, 255, 255 );

    //SDL_Flip( screen );
};


double euclidian_Distance( double x1, double y1, double x2, double y2 )
{
    return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
}

double manhattan_Distance( double x1, double y1, double x2, double y2 )
{
    return abs(x1-x2)+abs(y1-y2);
}


void save_Alrys_PY( void )
{
    FILE* fichier;
    fichier = fopen("./alrys.py","w");
    float currentangleold = 0;
    float anglerotation = 0;

       fprintf( fichier, "from polycal4 import *\n");
       fprintf( fichier, "from alryslib import *\n");
       fprintf( fichier, "\n");
       fprintf( fichier, "def mon_itineraire():\n");
       fprintf( fichier, "  if len(liste_deplacements):\n");
       fprintf( fichier, "    (en_avant, a_droite)[liste_deplacements[0]](liste_deplacements[1])\n");
       fprintf( fichier, "    liste_deplacements.pop(0)\n");
       fprintf( fichier, "    liste_deplacements.pop(0)\n");
       fprintf( fichier, "    return 1\n");
       fprintf( fichier, "  return action_clavier()\n");
       fprintf( fichier, "\n");
       fprintf( fichier, "liste_deplacements = [\n");

    for(int u=0; u<optimizePoints.size()-1; u++)
    {

        if (u>0)
        {
            currentangleold += anglerotation;
        }
        else
        {
            currentangleold = 0;
        }
        anglerotation = atan2(optimizePoints[u+1].y - optimizePoints[u].y, optimizePoints[u+1].x - optimizePoints[u].x) * 180.0 / 3.14159265359;
        anglerotation = anglerotation - currentangleold;

        if (anglerotation>180) anglerotation-=360;
        else if (anglerotation<-180) anglerotation+=360;

        float distance = euclidian_Distance( optimizePoints[u].x, optimizePoints[u].y, optimizePoints[u+1].x, optimizePoints[u+1].y );

        fprintf( fichier, "  1, %f, 0, %f,\n", anglerotation, distance );
    }

       fprintf( fichier, "]\n");
       fprintf( fichier, "\n");
       fprintf( fichier, "print(aller_selon(mon_itineraire))    \n");

    fclose( fichier );
}

float run_Alrys_PY( void )
{
    FILE * fptr;                    // file holder
    float score = 0.0;


    system("python3 ./alrys.py >> score.txt");      // call dir and put it's contents in a temp using redirects.
    fptr = fopen("score.txt", "r");  // open said file for reading.
                                    // oh, and check for fptr being NULL.
    fscanf( fptr, "%f", &score);

    fclose(fptr);                   // don't call this is fptr is NULL.
    remove("score.txt");             // clean up

    printf(" --> Validated Score : %f \n", score );

    return score;
}

void save_Optimised_Path( void )
{
    FILE* fichier;
    fichier = fopen("./optimised.txt","w");
    float currentangleold = 0;
    float anglerotation = 0;



    for(int u=0; u<optimizePoints.size()-1; u++)
    {

        if (u>0)
        {
            currentangleold += anglerotation;
        }
        else
        {
            currentangleold = 0;
        }
        anglerotation = atan2(optimizePoints[u+1].y - optimizePoints[u].y, optimizePoints[u+1].x - optimizePoints[u].x) * 180.0 / 3.14159265359;
        anglerotation = anglerotation - currentangleold;

        if (anglerotation>180) anglerotation-=360;
        else if (anglerotation<-180) anglerotation+=360;

        float distance = euclidian_Distance( optimizePoints[u].x, optimizePoints[u].y, optimizePoints[u+1].x, optimizePoints[u+1].y );

        fprintf( fichier, "  1, %f, 0, %f,\n", anglerotation, distance );
    }
    fclose( fichier );
}


void save_Optimised_Path_XY( void )
{
    FILE* fichier;
    fichier = fopen("./optimised_XY.txt","w");

    for(int u=0; u<optimizePoints.size(); u++)
    {
        fprintf( fichier, "%f\t%f\n", optimizePoints[u].x, optimizePoints[u].y );
    }
    fclose( fichier );
}


void load_Optimised_Path( void )
{
}


void load_Optimised_Path_XY( void )
{
}



void update_Map_Name_Location( SDL_Surface* screen )
{
    for (int x = 0; x < nMapWidth; x++)
        for (int y = 0; y < nMapHeight; y++)
        {

            if (nodes[y * nMapWidth + x].Obstacle==true)
            {
                boxRGBA( screen, x*sizex - offsetx, y*sizey - offsety, (x+1)*sizex - offsetx, (y+1)*sizey - offsety, 0, 0, 0, 180 );
            }

            if (nodes[y * nMapWidth + x].Visited==true)
            {
                boxRGBA( screen, x*sizex - offsetx, y*sizey - offsety, (x+1)*sizex - offsetx, (y+1)*sizey - offsety, 255, 255, 255, 127 );
            }

            if(&nodes[y * nMapWidth + x] == nodeStart)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                filledCircleRGBA( screen, djx, djy, sizex/2, 0, 255, 0, 255 );
                stringRGBA( screen, djx-sizex/2, djy-sizey/2, "S", 255, 0, 0, 255 );
            }

            if(&nodes[y * nMapWidth + x] == nodeEnd)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                filledCircleRGBA( screen, djx, djy, sizex/2, 255, 0, 0, 255 );
                stringRGBA( screen, djx-sizex/2, djy-sizey/2, "E", 255, 0, 0, 255 );
            }

            if(&nodes[y * nMapWidth + x] == PlayerStart)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "PS", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonA)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dA", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonB)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dB", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonC)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dC", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonD)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dD", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonE)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dE", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonF)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dF", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonG)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dG", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonH)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dH", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonI)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dI", 255, 255, 255, 255 );
            }
            if(&nodes[y * nMapWidth + x] == DonjonJ)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "dJ", 255, 255, 255, 255 );
            }

            if(&nodes[y * nMapWidth + x] == Village1)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V1", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village2)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V2", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village3)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V3", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village4)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V4", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village5)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V5", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village6)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V6", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village7)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V7", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village8)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V8", 0, 0, 0, 255 );
            }
            if(&nodes[y * nMapWidth + x] == Village9)
            {
                int djx = (x*sizex)+sizex/2-offsetx;
                int djy = (y*sizey)+sizex/2-offsety;
                stringRGBA( screen, djx-sizex/3, djy-sizey/3, "V9", 0, 0, 0, 255 );
            }
        }

}


point2D get(std::list<point2D> _list, int _i)
{
    std::list<point2D>::iterator it = _list.begin();
    for(int i=0; i<_i; i++)
    {
        ++it;
    }
    return *it;
}


void reduce_Best_Path( SDL_Surface* screen, std::list<point2D> Path )
{

    std::vector<point2D> reducedPath;
    reducedPath.push_back( Path.front() );
    circleRGBA( screen, Path.front().x*sizex - offsetx, Path.front().y*sizey - offsety, 8, 255, 255, 255, 255 );

    for (int u=1; u<Path.size(); u++)
    {
        if(((get(Path,u).x-get(Path,u-1).x)==(get(Path,u+1).x-get(Path,u).x)) && ((get(Path,u).y-get(Path,u-1).y)==(get(Path,u+1).y-get(Path,u).y)))
        {

        }
        else
        {
            reducedPath.push_back( get( Path, u ) );
            circleRGBA( screen, (get( Path, u ).x)*sizex - offsetx, (get( Path, u ).y)*sizey - offsety, 8, 255, 255, 255, 255 );
        }
    }

//    reducedPath.push_back( get( Path, Path.size() ) );
//    circleRGBA( screen, (get( Path, Path.size() ).x+0.5)*sizex - offsetx, (get( Path, Path.size() ).y+0.5)*sizey - offsety, 8, 255, 255, 255, 255 );
}


void draw_Found_Path( SDL_Surface* screen )
{
    std::list<point2D> bestPathPoint;

    if (nodeEnd != nullptr)
    {
        boxRGBA( screen, nodeStart->x*sizex - offsetx, nodeStart->y*sizey - offsety, (nodeStart->x+1)*sizex - offsetx, (nodeStart->y+1)*sizey - offsety, 148, 0, 211, 180 );
        sNode *p = nodeEnd;
        while (p->Parent != nullptr)
        {
            boxRGBA( screen, p->x*sizex - offsetx, p->y*sizey - offsety, (p->x+1)*sizex - offsetx, (p->y+1)*sizey - offsety, 148, 0, 211, 180 );

            p = p->Parent;
        }

        p = nodeEnd;
        bestPathPoint.clear();


        while (p->Parent != nullptr)
        {
            bestPathPoint.push_front( {p->x+0.5, p->y+0.5 } );

            filledCircleRGBA( screen, (p->x+0.5)*sizex - offsetx, (p->y+0.5)*sizey - offsety, 5, 0, 0, 0, 255 );
            lineRGBA( screen, (p->x+0.5)*sizex - offsetx, (p->y+0.5)*sizey - offsety, (p->Parent->x+0.5)*sizex - offsetx, (p->Parent->y+0.5)*sizey - offsety, 0, 0, 0, 255 );
            filledCircleRGBA( screen, (p->Parent->x+0.5)*sizex - offsetx, (p->Parent->y+0.5)*sizey - offsety, 5, 0, 0, 0, 255 );

            p = p->Parent;
        }
    }

    reduce_Best_Path( screen, bestPathPoint );
}

/*
struct target
{
    float x, y;
    float radius;
    float bestcost;
    int bestnode;
};
*/
/*struct nodeRRT
{
    float x, y;
    float cost;
    int parent;
    int bestnode;
    float radius;
};
*/
struct nodeRRT
{
    float x, y;
    float cost;
    int parent;
    float bestcost;
    int bestnode;
    float radius;

    inline bool operator!=(nodeRRT a)
    {
        if (a.x!=x || a.y!=y) return true;
        else return false;
    }
};

std::vector<nodeRRT> nodeTree;
nodeRRT TreeStart, TreeTarget;

nodeRRT DonjA;
nodeRRT DonjB;
nodeRRT DonjC;
nodeRRT DonjD;
nodeRRT DonjE;
nodeRRT DonjF;
nodeRRT DonjG;
nodeRRT DonjH;
nodeRRT DonjI;
nodeRRT DonjJ;
nodeRRT Vill1;
nodeRRT Vill2;
nodeRRT Vill3;
nodeRRT Vill4;
nodeRRT Vill5;
nodeRRT Vill6;
nodeRRT Vill7;
nodeRRT Vill8;
nodeRRT Vill9;
nodeRRT PlayStart;


//target treeTarget;


void init_Random_Generator( void )
{
    srand((unsigned int)time(NULL));
}


float get_Random_Number( float maxvalue )
{
    return (float(rand())/float((RAND_MAX)) * maxvalue);
}

float get_Random_Number_2F(float mini, float maxi)
{
    if (mini<=maxi)
        return mini + (((float) rand()) / (float) RAND_MAX) * (maxi - mini);
    else
        return maxi + (((float) rand()) / (float) RAND_MAX) * (mini - maxi);
}

float refinedminX = 0.0f, refinedmaxX=146.0f, refinedminY=0.0f, refinedmaxY=91.0f;

//float refinedminX = 10.0f, refinedmaxX=60.0f, refinedminY=20.0f, refinedmaxY=60.0f;



float cost_Segment( float x1, float y1, float x2, float y2, bool boatAv )
{
    std::list<point2D> passage;
    passage = segment( x1, y1, x2, y2 );
    passage.pop_front(); // le premier point est la position actuelle du joueur
    char c;

    float cost=0.0f;

    playerx = x1;
    playery = y1;

    for( auto k : passage )
    {
        int Xr = floor( minimum(playerx, k.x ) );
        int Yr = floor( minimum(playery, k.y ) );
        c = get_From_Map( Xr, Yr );

        if ((!can_Go(c, boatAv)) || (!can_Go( get_From_Map( k.x, k.y ), boatAv))) break;

        float dist = calculate_Distance_To( k.x, k.y );
        float coefficient = get_Score(c);

        float d=dist*coefficient;

        playerx = k.x;
        playery = k.y;

        cost += d;
    }

    return cost;
}

float walkable_distance( float x1, float y1, float x2, float y2, bool boatAv )
{
    std::list<point2D> passage;
    passage = segment( x1, y1, x2, y2 );
    passage.pop_front(); // le premier point est la position actuelle du joueur
    char c;

    float cost=0.0f;

    playerx = x1;
    playery = y1;

    for( auto k : passage )
    {
        int Xr = floor( minimum(playerx, k.x ) );
        int Yr = floor( minimum(playery, k.y ) );
        c = get_From_Map( Xr, Yr );

        if ((!can_Go(c, boatAv)) || (!can_Go( get_From_Map( k.x, k.y ), boatAv))) return -1.0;

        float dist = calculate_Distance_To( k.x, k.y );

        playerx = k.x;
        playery = k.y;

        cost += dist;
    }

    return cost;
}



void draw_RRT( SDL_Surface* screen, float range )
{
    char coststr[50];

    for( int j=nodeTree.size()-1; j>1; j-- )
    {
        lineRGBA( screen, nodeTree[j].x*sizex-offsetx, nodeTree[j].y*sizey-offsety, nodeTree[nodeTree[j].parent].x*sizex-offsetx, nodeTree[nodeTree[j].parent].y*sizey-offsety, nodeTree[j].cost/range*255, nodeTree[j].cost/range*255, nodeTree[j].cost/range*255, 127 );
        filledCircleRGBA(screen, nodeTree[j].x*sizex-offsetx, nodeTree[j].y*sizey-offsety, 2, nodeTree[j].cost/range*255, nodeTree[j].cost/range*255, nodeTree[j].cost/range*255, 127 );

        //pixelRGBA(screen, nodeTree[j].x*sizex-offsetx, nodeTree[j].y*sizey-offsety, 255, 255, 255, 255 );

        //sprintf( coststr, "%f", nodeTree[j].cost );
        //stringRGBA( screen, nodeTree[j].x*sizex-offsetx, nodeTree[j].y*sizey-offsety, coststr, 255, 0, 0, 255 );
    }

    circleRGBA(screen, TreeStart.x*sizex-offsetx, TreeStart.y*sizey-offsety, 10, 0, 255, 0, 255 );
    circleRGBA(screen, TreeStart.x*sizex-offsetx, TreeStart.y*sizey-offsety, 9, 0, 255, 0, 255 );
    circleRGBA(screen, TreeStart.x*sizex-offsetx, TreeStart.y*sizey-offsety, 8, 0, 255, 0, 255 );

    circleRGBA(screen, TreeTarget.x*sizex-offsetx, TreeTarget.y*sizey-offsety, 10, 255, 0, 0, 255 );
    circleRGBA(screen, TreeTarget.x*sizex-offsetx, TreeTarget.y*sizey-offsety, 9, 255, 0, 0, 255 );
    circleRGBA(screen, TreeTarget.x*sizex-offsetx, TreeTarget.y*sizey-offsety, 8, 255, 0, 0, 255 );

}

void draw_best_RRT_Path( SDL_Surface* screen )
{
    float security=5.0f;

    char coststr[50];
    nodeRRT p1, p2;

    p1 = nodeTree[TreeTarget.bestnode];
    if (p1.parent!=-1)
        p2 = nodeTree[p1.parent];


    if (manualrandomlimit==false)
    {
        refinedmaxX = 0;
        refinedminX = 146;
        refinedmaxY = 0;
        refinedminY = 91;

        if (p1.x > refinedmaxX) refinedmaxX = ceil(p1.x);
        if (p1.x < refinedminX) refinedminX = floor(p1.x);
        if (p1.y > refinedmaxY) refinedmaxY = ceil(p1.y);
        if (p1.y < refinedminY) refinedminY = floor(p1.y);

        if (p2.x > refinedmaxX) refinedmaxX = ceil(p2.x);
        if (p2.x < refinedminX) refinedminX = floor(p2.x);
        if (p2.y > refinedmaxY) refinedmaxY = ceil(p2.y);
        if (p2.y < refinedminY) refinedminY = floor(p2.y);
    }

    while(p2.parent!=-1)
    {
        lineRGBA( screen, p1.x*sizex-offsetx, p1.y*sizey-offsety, p2.x*sizex-offsetx, p2.y*sizey-offsety, 255, 0, 0, 255 );
        filledCircleRGBA(screen, p1.x*sizex-offsetx, p1.y*sizey-offsety, 5, 255, 0, 0, 255 );
        p1 = p2;
        p2 = nodeTree[p2.parent];

        if (manualrandomlimit==false)
        {
            if (p2.x > refinedmaxX) refinedmaxX = ceil(p2.x);
            if (p2.x < refinedminX) refinedminX = floor(p2.x);
            if (p2.y > refinedmaxY) refinedmaxY = ceil(p2.y);
            if (p2.y < refinedminY) refinedminY = floor(p2.y);
        }
    }

    lineRGBA( screen, p1.x*sizex-offsetx, p1.y*sizey-offsety, p2.x*sizex-offsetx, p2.y*sizey-offsety, 255, 0, 0, 255 );
    filledCircleRGBA(screen, p1.x*sizex-offsetx, p1.y*sizey-offsety, 5, 255, 0, 0, 255 );

    if (manualrandomlimit==false)
    {
        if (p2.x > refinedmaxX) refinedmaxX = ceil(p2.x);
        if (p2.x < refinedminX) refinedminX = floor(p2.x);
        if (p2.y > refinedmaxY) refinedmaxY = ceil(p2.y);
        if (p2.y < refinedminY) refinedminY = floor(p2.y);

        if (refinedmaxX<=146.0f-security)
        {
            refinedmaxX += security;
        }
        else refinedmaxX = 146.0f;

        if (refinedmaxY<=91.0f-security)
        {
            refinedmaxY += security;
        }
        else refinedmaxY = 91.0f;

        if (refinedminX>=0.0f+security)
        {
            refinedminX -= security;
        }
        else refinedminX = 0.0f;

        if (refinedminY>=0.0f+security)
        {
            refinedminY -= security;
        }
        else refinedminY = 0.0f;

    }
}

float RRT( SDL_Surface* screen, int nbMaxIteration, bool boatAv )
{

    if (TreeStart!=TreeTarget)
    {

        float currentdistance, bestdistance, walkabledistance;
        int currentbestparent=0;

        bool targetreached=false;

        float maxcost = 0.0f;

        for( int i = 0; i< nbMaxIteration; i++)
        {
            bool validPoint=false;
            nodeRRT nodeNewPoint;

            while(!validPoint)
            {


                float tempx=get_Random_Number_2F( refinedminX, refinedmaxX );
                float tempy=get_Random_Number_2F( refinedminY, refinedmaxY );

                nodeNewPoint = { tempx, tempy, INFINITY, -1, INFINITY, -1, 0.5 };
                bestdistance = INFINITY;


                for( int j=0; j<nodeTree.size(); j++ )
                {
                    currentdistance = euclidian_Distance( nodeTree[j].x, nodeTree[j].y, nodeNewPoint.x, nodeNewPoint.y );

                    if (nodeTree[j].cost > maxcost)
                        maxcost = nodeTree[j].cost;

                    if (currentdistance<bestdistance)
                    {
                        bestdistance=currentdistance;
                        nodeNewPoint.parent = j;
                    }
                }

                if (bestdistance>1.0f)
                {
                    nodeNewPoint.x = nodeTree[nodeNewPoint.parent].x + (nodeNewPoint.x-nodeTree[nodeNewPoint.parent].x)/bestdistance;
                    nodeNewPoint.y = nodeTree[nodeNewPoint.parent].y + (nodeNewPoint.y-nodeTree[nodeNewPoint.parent].y)/bestdistance;
                    nodeNewPoint.cost = nodeTree[nodeNewPoint.parent].cost + cost_Segment( nodeTree[nodeNewPoint.parent].x, nodeTree[nodeNewPoint.parent].y, nodeNewPoint.x, nodeNewPoint.y, boatAv );
                }
                else
                {
                    nodeNewPoint.x = nodeTree[nodeNewPoint.parent].x + (nodeNewPoint.x-nodeTree[nodeNewPoint.parent].x);
                    nodeNewPoint.y = nodeTree[nodeNewPoint.parent].y + (nodeNewPoint.y-nodeTree[nodeNewPoint.parent].y);
                    nodeNewPoint.cost = nodeTree[nodeNewPoint.parent].cost + cost_Segment( nodeTree[nodeNewPoint.parent].x, nodeTree[nodeNewPoint.parent].y, nodeNewPoint.x, nodeNewPoint.y, boatAv );
                }

                char c=get_From_Map( floor(nodeNewPoint.x), floor(nodeNewPoint.y) );

                if (can_Go(c, boatAv) == true)
                    validPoint = true;
                else
                    validPoint = false;

            }

            // rewiring of the tree based on minimized cost
            for( int k=0; k<nodeTree.size(); k++ )
            {
                //float dist = manhattan_Distance( nodeTree[k].x, nodeTree[k].y, nodeNewPoint.x, nodeNewPoint.y );
                float dist = euclidian_Distance( nodeTree[k].x, nodeTree[k].y, nodeNewPoint.x, nodeNewPoint.y );

                //if (abs(nodeTree[k].x-nodeNewPoint.x)<=10)
                //    if (abs(nodeTree[k].y-nodeNewPoint.y)<=10)
                    if ( dist <= 10.0)
                    {
                        float walk = walkable_distance( nodeTree[k].x, nodeTree[k].y, nodeNewPoint.x, nodeNewPoint.y, boatAv ); // should contain -1 if encounter an obstacle

                        if (walk!=-1)
                        {
                            float costinterim = cost_Segment( nodeTree[k].x, nodeTree[k].y, nodeNewPoint.x, nodeNewPoint.y, boatAv ) + 0.1;
                            float temp = nodeTree[k].cost + costinterim;

                            if (nodeNewPoint.cost >= temp)
                            {
                                nodeNewPoint.parent = k;
                                nodeNewPoint.cost = temp;
                            }
                        }
                    }
            }

            if (nodeNewPoint.cost > maxcost)
                maxcost = nodeNewPoint.cost;

            for( int k=0; k<nodeTree.size(); k++ )
            {
                float xmin = TreeTarget.x-TreeTarget.radius;
                float xmax = TreeTarget.x+TreeTarget.radius;
                float ymin = TreeTarget.y-TreeTarget.radius;
                float ymax = TreeTarget.y+TreeTarget.radius;

                if((nodeTree[k].x>=xmin) && (nodeTree[k].x<=xmax) && (nodeTree[k].y>=ymin) && (nodeTree[k].y<=ymax))
                {
                    targetreached = true;

                    if (nodeTree[k].cost < TreeTarget.bestcost)
                    {
                        TreeTarget.bestcost = nodeTree[k].cost;
                        TreeTarget.bestnode = k;
                    }
                }
            }
            nodeTree.push_back( nodeNewPoint );
        }

        draw_RRT( screen, maxcost );

        rectangleRGBA( screen, (TreeTarget.x-TreeTarget.radius)*sizex-offsetx, (TreeTarget.y-TreeTarget.radius)*sizey-offsety, (TreeTarget.x+TreeTarget.radius)*sizex-offsetx, (TreeTarget.y+TreeTarget.radius)*sizey-offsety, 0, 255, 255, 255 );

        if (targetreached==true)
        {
            draw_best_RRT_Path( screen );
        }


        rectangleRGBA( screen, refinedminX*sizex-offsetx, refinedminY*sizey-offsety, refinedmaxX*sizex-offsetx, refinedmaxY*sizey-offsety, 255, 255, 255, 255 );
        rectangleRGBA( screen, refinedminX*sizex-offsetx+1, refinedminY*sizey-offsety+1, refinedmaxX*sizex-offsetx-1, refinedmaxY*sizey-offsety-1, 255, 255, 255, 255 );
        rectangleRGBA( screen, refinedminX*sizex-offsetx+2, refinedminY*sizey-offsety+2, refinedmaxX*sizex-offsetx-2, refinedmaxY*sizey-offsety-2, 255, 255, 255, 255 );


        char coststr[50];

        boxRGBA( screen, 0, 1025, 400, 1040, 0, 0, 0, 255 );
        stringRGBA( screen, 10, 1030, "RRT Tree : ", 255, 255, 255, 255 );
        sprintf( coststr, "%d", nodeTree.size() );
        stringRGBA( screen, 100, 1030, coststr, 255, 0, 0, 255 );

        stringRGBA( screen, 200, 10300, "Lowest cost : ", 255, 255, 255, 255 );
        sprintf( coststr, "%f", TreeTarget.bestcost );
        stringRGBA( screen, 330, 1030, coststr, 255, 0, 0, 255 );

        return maxcost;
    }
    else return 0.0f;
}





int main ( int argc, char** argv )
{
    float increment = 0.04f;
    bool minimizeturns = false;
    bool menuON = false;
    int testsmall = 1;
    usersize = 16;

    bool modeManual = false;
    bool modeAstar = false;
    bool modeRRT = false;

    bool modePicture = true;

    bool boatavailableRRT = false;

    float rangecost;

    float currentpathscore;

    maptile = new char[nMapWidth * nMapHeight];

    SDL_Surface* screen = nullptr;
    screen = init_SDL( );

    load_Map_Ressources( testsmall );

    load_Map_Layout( maptile );



    refinedminX = 0.0f;
    refinedmaxX = 146.0f;
    refinedminY = 0.0f;
    refinedmaxY = 91.0f;


    init_Random_Generator();

    nodeTree.clear();

    DonjA = { 1.5f, 89.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjB = { 8.5f, 58.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjC = { 17.5f, 38.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjD = { 49.5f, 43.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjE = { 69.5f, 51.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjF = { 82.5f, 82.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjG = { 106.5f, 39.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjH = { 114.5f, 26.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjI = { 119.5f, 83.5f, 0, -1, INFINITY, -1, 0.5 };
    DonjJ = { 141.5f, 71.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill1 = { 12.5f, 80.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill2 = { 18.5f, 12.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill3 = { 38.5f, 49.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill4 = { 69.5f, 17.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill5 = { 72.5f, 83.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill6 = { 103.5f, 56.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill7 = { 107.5f, 69.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill8 = { 130.5f, 62.5f, 0, -1, INFINITY, -1, 0.5 };
    Vill9 = { 138.5f, 89.5f, 0, -1, INFINITY, -1, 0.5 };
    PlayStart = { 47.5f, 43.5f, 0, -1, INFINITY, -1, 0.5 };

    nodeRRT HorizontalEntryRRT[20] = { PlayStart, DonjA, DonjB, DonjC, DonjD,
                                       DonjE, DonjF, DonjG, DonjH, DonjI,
                                       DonjJ, Vill1, Vill2, Vill3, Vill4,
                                       Vill5, Vill6, Vill7, Vill8, Vill9
                                     };

    nodeRRT VerticalEntryRRT[20] = { PlayStart, DonjA, DonjB, DonjC, DonjD,
                                     DonjE, DonjF, DonjG, DonjH, DonjI,
                                     DonjJ, Vill1, Vill2, Vill3, Vill4,
                                     Vill5, Vill6, Vill7, Vill8, Vill9
                                   };

    TreeStart = PlayStart;
    TreeTarget = PlayStart;

    nodeTree.push_back( TreeStart );


    init_Nodes_From_Map( maptile, nodes, false );
    make_Nodes_Connections( nodes );



    // Manually position the start and end markers so they are not nullptr
    DonjonA = &nodes[1+nMapWidth*89];
    DonjonB = &nodes[8+nMapWidth*58];
    DonjonC = &nodes[17+nMapWidth*38];
    DonjonD = &nodes[49+nMapWidth*43];
    DonjonE = &nodes[69+nMapWidth*51];
    DonjonF = &nodes[82+nMapWidth*82];
    DonjonG = &nodes[106+nMapWidth*39];
    DonjonH = &nodes[114+nMapWidth*26];
    DonjonI = &nodes[119+nMapWidth*83];
    DonjonJ = &nodes[141+nMapWidth*71];
    Village1 = &nodes[12+nMapWidth*80];
    Village2 = &nodes[18+nMapWidth*12];
    Village3 = &nodes[38+nMapWidth*49];
    Village4 = &nodes[69+nMapWidth*17];
    Village5 = &nodes[72+nMapWidth*83];
    Village6 = &nodes[103+nMapWidth*56];
    Village7 = &nodes[107+nMapWidth*69];
    Village8 = &nodes[130+nMapWidth*62];
    Village9 = &nodes[138+nMapWidth*89];
    PlayerStart = &nodes[47+nMapWidth*43];

    sNode* HorizontalEntry[20] = { PlayerStart, DonjonA, DonjonB, DonjonC, DonjonD,
                                   DonjonE, DonjonF, DonjonG, DonjonH, DonjonI,
                                   DonjonJ, Village1, Village2, Village3, Village4,
                                   Village5, Village6, Village7, Village8, Village9
                                 };

    sNode* VerticalEntry[20] = { PlayerStart, DonjonA, DonjonB, DonjonC, DonjonD,
                                 DonjonE, DonjonF, DonjonG, DonjonH, DonjonI,
                                 DonjonJ, Village1, Village2, Village3, Village4,
                                 Village5, Village6, Village7, Village8, Village9
                               };

    std::string ChoiceStr[20] = { "PlayerStart", "DonjonA", "DonjonB", "DonjonC", "DonjonD",
                                  "DonjonE", "DonjonF", "DonjonG", "DonjonH", "DonjonI",
                                  "DonjonJ", "Village1", "Village2", "Village3", "Village4",
                                  "Village5", "Village6", "Village7", "Village8", "Village9"
                                };

    int Choice1 = 0;
    int Choice2 = 0;


    float MatrixDistance[20][20];

    offsetx_max = (146-screen->w/sizex)*sizex;
    offsety_max = (91-screen->h/sizey)*sizey;


    // program main loop
    bool done = false;


    for (int x=0; x<20; x++)
        for(int y=0; y<20; y++)
            MatrixDistance[x][y]=0.0f;


    nodeStart = HorizontalEntry[0];
    nodeEnd = VerticalEntry[0];


//    for (int x=0; x<20; x++)
//        for(int y=0; y<20; y++)
    {


        /*
                nodeStart = Village8;
                nodeEnd = Village5;
        */

        //draw_Map( screen );
        //update_Map_Name_Location( screen );

        while (!done)
        {




            // DRAWING STARTS HERE
            draw_Map( screen );

            if (modeManual==true)
            {

                if (firstround==true) scoreinit=simulate_Path_Initial( screen );
                else scoreoptim=simulate_Path_Optimized( screen );

//                scoreinit = simulate_Current_Path( screen, optimizePoints );

               printf( "Score Init = %f - Optimized = %f \n", scoreinit, scoreoptim);

                draw_Path_Initial( screen );

                draw_Path_Optimised( screen );


                scoreinit = 0.0;
                save_Alrys_PY();
                scoreoptim = run_Alrys_PY();
            }

            if (modeAstar==true)
            {
                if (needupdatebestpath)
                {
                    // Reset Navigation Graph - default all node states
                    reinit_Nodes_Status( nodes );
                    make_Nodes_Connections( nodes );
                    cost_path = calculate_Path( nodes, nodeStart, nodeEnd, minimizeturns );

                    needupdatebestpath = false;
                }
                draw_Found_Path( screen );
            }

            update_Map_Name_Location( screen );

            //


            // DRAW OPTIMAL PATH

            if (modeRRT==true)
            {
                rangecost = RRT( screen, 100, boatavailableRRT );
                //draw_RRT( screen, rangecost );
            }




            boxRGBA( screen, 1745, 45, 1900, 280, 0, 0, 0, 255 );
            stringRGBA( screen, 1750, 50, "Start : ", 255, 255, 255, 255 );
            stringRGBA( screen, 1770, 70, ChoiceStr[Choice1].c_str(), 255, 0, 255, 255 );
            stringRGBA( screen, 1750, 90, "End   : ", 255, 255, 255, 255 );
            stringRGBA( screen, 1770, 110, ChoiceStr[Choice2].c_str(), 255, 0, 255, 255 );

            char coststr[50];

            sprintf( coststr, "%f", cost_path );

            stringRGBA( screen, 1750, 130, "Cost : ", 255, 255, 255, 255 );
            stringRGBA( screen, 1770, 150, coststr, 0, 255, 255, 255 );

            currentpathscore = simulate_Current_Path( screen, initialPoints );

            sprintf( coststr, "%f", currentpathscore );
            stringRGBA( screen, 1750, 170, "Original Path : ", 255, 255, 255, 255 );
            stringRGBA( screen, 1770, 190, coststr, 0, 255, 255, 255 );


            currentpathscore = simulate_Current_Path( screen, optimizePoints );

            sprintf( coststr, "%f", currentpathscore );
            stringRGBA( screen, 1750, 210, "Optimized Path : ", 255, 255, 255, 255 );
            stringRGBA( screen, 1770, 230, coststr, 0, 255, 255, 255 );

            sprintf( coststr, "%f", increment );

            stringRGBA( screen, 1750, 250, "Increment : ", 255, 255, 255, 255 );
            stringRGBA( screen, 1770, 270, coststr, 0, 255, 255, 255 );


            if (menuON)
            {

                boxRGBA( screen, 0, 0, 1600, 75, 0, 0, 0, 255 );
                stringRGBA( screen, 5, 5,"[ESC]-Quit      [TAB]-switch pix/rect view  [F2]-Save Current Path     [F9]-A* min turns ON             [F10]-A* min turns OFF         [F11]-Make Ocean Navigable       [F12]-Make Ocean Non Navigable", 255, 255, 255, 255 );

                stringRGBA( screen, 5, 15,"Current Path :  [PAGEUP]-Select Next Node  [PAGEDOWN]-Select Previous Node  [DELETE]-Delete Selected Node  [INSERT]-Insert One Node before  [HOME]-Select First Node         [END]-Select Last Node", 255, 255, 255, 255 );

                stringRGBA( screen, 5, 25,"Current Node :  [LEFT]-Move Left (1 step)  [RIGHT]-Move Right (1 step)      [UP]-Move Up (1 step)          [DOWN]-Move Down (1 step)        [KP+]-Increase Step Size         [KP-]-Decrease Step Size", 255, 255, 255, 255 );

                stringRGBA( screen, 5, 35,"View         :  [KP4]-Move View Left       [KP6]-Move View Right            [KP8]-Move View Up             [KP2]-Move View Down             [WHEELUP]-Zoom View x2           [WHEELDOWN]-Unzoom View /2", 255, 255, 255, 255 );

                stringRGBA( screen, 5, 45,"PathFinding  :  [KP7]-Select next Start    [KP9]-Select Next End            [E]-A* pathfinding mode        [A]-Manual Mode", 255, 255, 255, 255 );
                stringRGBA( screen, 5, 55,"& Path Edit     [KP1]-Select Prev Start    [KP3]-Select Prev End            [R]-RRT pathfinding mode       [Z]-Edit Mode", 255, 255, 255, 255 );
                stringRGBA( screen, 5, 65,"                [F5]-up-left corner RRT    [F6]-down-right for RRT          [F7]-reset RRT limits", 255, 255, 255, 255 );
            }
            else
            {
                boxRGBA( screen, 0, 0, 400, 20, 0, 0, 0, 255 );
                stringRGBA( screen, 5, 5,"Press [F1] to see all available commands", 255, 255, 255, 255 );
            }


            SDL_Event event;
            while (SDL_PollEvent(&event))
            {
                switch (event.type)
                {
                case SDL_QUIT:
                    done = true;
                    break;
                case SDL_KEYDOWN:
                {
                    if (event.key.keysym.sym == SDLK_a)
                    {
                        modeManual = false;
                        modeAstar = false;
                        modeRRT = false;
                    }
                    if (event.key.keysym.sym == SDLK_z)
                    {
                        modeManual = true;
                        modeAstar = false;
                        modeRRT = false;
                    }
                    if (event.key.keysym.sym == SDLK_e)
                    {
                        modeManual = false;
                        modeAstar = true;
                        modeRRT = false;
                    }
                    if (event.key.keysym.sym == SDLK_r)
                    {
                        modeManual = false;
                        modeAstar = false;
                        modeRRT = true;
                    }

                    if (event.key.keysym.sym == SDLK_TAB)
                    {
                        modePicture = !modePicture;
                        if (modePicture==true)
                        {
                            if (usersize>=32)
                            {
                                usersize = 32;
                                testsmall = 32;
                                load_Map_Ressources( testsmall );
                            }
                            else if (usersize<=8)
                            {
                                usersize = 8;
                                testsmall = 8;
                                load_Map_Ressources( testsmall );
                            }
                            else
                            {
                                usersize = 16;
                                testsmall = 16;
                                load_Map_Ressources( testsmall );
                            }
                        }
                        else
                        {
                            testsmall = 1;
                            load_Map_Ressources( testsmall );
                        }
                    }

                    if (event.key.keysym.sym == SDLK_ESCAPE)
                        done = true;

                    if (event.key.keysym.sym == SDLK_KP8)
                        if (offsety>0) offsety-=sizey;

                    if (event.key.keysym.sym == SDLK_KP2)
                        if (offsety<offsety_max) offsety+=sizey;

                    if (event.key.keysym.sym == SDLK_KP4)
                        if (offsetx>0) offsetx-=sizex;

                    if (event.key.keysym.sym == SDLK_KP6)
                        if (offsetx<offsetx_max) offsetx+=sizex;

                    if (event.key.keysym.sym == SDLK_KP_PLUS)
                    {
                        increment *= 2;
                    }
                    if (event.key.keysym.sym == SDLK_KP_MINUS)
                    {
                        increment /= 2;
                    }
                    if (event.key.keysym.sym == SDLK_UP)
                    {
                        optimizePoints[selectedNode].y -= increment;
                    }
                    if (event.key.keysym.sym == SDLK_DOWN)
                    {
                        optimizePoints[selectedNode].y += increment;
                    }
                    if (event.key.keysym.sym == SDLK_LEFT)
                    {
                        optimizePoints[selectedNode].x -= increment;
                    }
                    if (event.key.keysym.sym == SDLK_RIGHT)
                    {
                        optimizePoints[selectedNode].x += increment;
                    }

                    if (event.key.keysym.sym == SDLK_PAGEDOWN)
                        if (selectedNode>1)
                            selectedNode--;

                    if (event.key.keysym.sym == SDLK_PAGEUP)
                        if (selectedNode<(optimizePoints.size()-1))
                            selectedNode++;

                    if (event.key.keysym.sym == SDLK_HOME)
                        selectedNode=1;

                    if (event.key.keysym.sym == SDLK_END)
                        selectedNode=optimizePoints.size()-1;

                    if (event.key.keysym.sym == SDLK_INSERT)
                        if ((selectedNode>0) && (selectedNode<(optimizePoints.size()-1)))
                        {
                            std::vector<point2D>::iterator it = optimizePoints.begin();
                            point2D temp = { (optimizePoints[selectedNode-1].x+optimizePoints[selectedNode].x)/2.0, (optimizePoints[selectedNode-1].y+optimizePoints[selectedNode].y)/2.0 } ;
                            optimizePoints.insert( it+selectedNode, temp );
                        }

                    if (event.key.keysym.sym == SDLK_DELETE)
                        if ((selectedNode>0) && (selectedNode<(optimizePoints.size()-1)))
                        {
                            std::vector<point2D>::iterator it = optimizePoints.begin();
                            point2D temp = { (optimizePoints[selectedNode-1].x+optimizePoints[selectedNode].x)/2.0, (optimizePoints[selectedNode-1].y+optimizePoints[selectedNode].y)/2.0 } ;
                            optimizePoints.erase( it+selectedNode );
                        }

                    if (event.key.keysym.sym == SDLK_F1)
                    {
                        menuON = !menuON;
                    }

                    if (event.key.keysym.sym == SDLK_F2)
                    {
                        save_Optimised_Path( );
                        save_Optimised_Path_XY( );
                        save_Alrys_PY();
                        run_Alrys_PY();
                    }

                    if (event.key.keysym.sym == SDLK_F3)
                    {
                        load_Optimised_Path( );
                        load_Optimised_Path_XY( );
                    }

                    if (event.key.keysym.sym == SDLK_F5)
                    {
                        manualrandomlimit=true;

                        SDL_GetMouseState(&mousex, &mousey);
                        //mousex = event.motion.x;
                        //mousey = event.motion.y;
                        float cellx = (float) (((float) mousex + (float) offsetx)/((float) sizex) );
                        float celly = (float) (((float) mousey + (float) offsety)/((float) sizey) );

                        refinedminX = cellx;
                        refinedminY = celly;
                    }
                    if (event.key.keysym.sym == SDLK_F6)
                    {
                        manualrandomlimit=true;

                        SDL_GetMouseState(&mousex, &mousey);
                        //mousex = event.motion.x;
                        //mousey = event.motion.y;
                        float cellx = (float) (((float) mousex + (float) offsetx)/((float) sizex) );
                        float celly = (float) (((float) mousey + (float) offsety)/((float) sizey) );

                        refinedmaxX = cellx;
                        refinedmaxY = celly;
                    }
                    if (event.key.keysym.sym == SDLK_F7)
                    {
                        manualrandomlimit=false;
                        refinedminX = 0.0f;
                        refinedmaxX = 146.0f;
                        refinedminY = 0.0f;
                        refinedmaxY = 91.0f;
                    }

                    if (event.key.keysym.sym == SDLK_F9)
                    {
                        minimizeturns = true;
                        needupdatebestpath = true;
                    }

                    if (event.key.keysym.sym == SDLK_F10)
                    {
                        minimizeturns = false;
                        needupdatebestpath = true;
                    }

                    if (event.key.keysym.sym == SDLK_F11)
                    {
                        init_Nodes_From_Map( maptile, nodes, false );
                        boatavailableRRT = false;
                        needupdatebestpath = true;
                    }

                    if (event.key.keysym.sym == SDLK_F12)
                    {
                        init_Nodes_From_Map( maptile, nodes, true );
                        boatavailableRRT = true;
                        needupdatebestpath = true;
                    }

                    if ((event.key.keysym.sym == SDLK_KP1) && (Choice1>0))
                    {
                        Choice1--;
                        nodeStart = HorizontalEntry[Choice1];
                        needupdatebestpath = true;

                        manualrandomlimit=false;

                        TreeStart= HorizontalEntryRRT[Choice1];
                        nodeTree.clear();
                        nodeTree.push_back( TreeStart );

                        refinedminX = 0.0f;
                        refinedmaxX = 146.0f;
                        refinedminY = 0.0f;
                        refinedmaxY = 91.0f;

                    }
                    if ((event.key.keysym.sym == SDLK_KP7) && (Choice1<20))
                    {
                        Choice1++;
                        nodeStart = HorizontalEntry[Choice1];
                        needupdatebestpath = true;

                        manualrandomlimit=false;
                        TreeStart= HorizontalEntryRRT[Choice1];
                        nodeTree.clear();
                        nodeTree.push_back( TreeStart );

                        refinedminX = 0.0f;
                        refinedmaxX = 146.0f;
                        refinedminY = 0.0f;
                        refinedmaxY = 91.0f;
                    }
                    if ((event.key.keysym.sym == SDLK_KP3) && (Choice2>0))
                    {
                        Choice2--;
                        nodeEnd = VerticalEntry[Choice2];
                        needupdatebestpath = true;

                        manualrandomlimit=false;
                        TreeTarget = VerticalEntryRRT[Choice2];
                        nodeTree.clear();
                        nodeTree.push_back( TreeStart );

                        refinedminX = 0.0f;
                        refinedmaxX = 146.0f;
                        refinedminY = 0.0f;
                        refinedmaxY = 91.0f;
                    }
                    if ((event.key.keysym.sym == SDLK_KP9) && (Choice2<20))
                    {
                        Choice2++;
                        nodeEnd = VerticalEntry[Choice2];
                        needupdatebestpath = true;

                        manualrandomlimit=false;
                        TreeTarget = VerticalEntryRRT[Choice2];
                        nodeTree.clear();
                        nodeTree.push_back( TreeStart );

                        refinedminX = 0.0f;
                        refinedmaxX = 146.0f;
                        refinedminY = 0.0f;
                        refinedmaxY = 91.0f;
                    }

                    break;
                }
                case SDL_MOUSEMOTION:
                {
                    if ((SDL_GetMouseState(NULL, NULL) & SDL_BUTTON_LMASK) !=0)
                    {
                        int startlinex, startliney;

                        if (initialPoints.size()==0)
                        {
                            initialPoints.push_back( { 47.5, 43.5 } );
                            optimizePoints.push_back( { 47.5, 43.5 } );
                            startlinex=playerx*sizex-offsetx;
                            startliney=playery*sizey-offsety;
                        }
                        else
                        {
                            startlinex=initialPoints.back().x*sizex-offsetx;
                            startliney=initialPoints.back().y*sizey-offsety;
                        }

                        mousex = event.motion.x;
                        mousey = event.motion.y;
                        float cellx = (float) (((float) mousex + (float) offsetx)/((float) sizex) );
                        float celly = (float) (((float) mousey + (float) offsety)/((float) sizey) );
                        char data[50];

                        boxRGBA( screen, mousex+25, mousey-50, mousex+175, mousey+60, 0, 0, 0, 255 );
                        rectangleRGBA(screen, mousex+25, mousey-50, mousex+175, mousey+60, 255, 255, 255, 255 );

                        sprintf( data, "%d", mousex );
                        stringRGBA( screen, mousex+30, mousey-45, "MoX : ", 255, 255, 255, 255 );
                        stringRGBA( screen, mousex+70, mousey-45, data, 255, 0, 255, 255 );

                        sprintf( data, "%d", mousey );
                        stringRGBA( screen, mousex+30, mousey-35, "MoY : ", 255, 255, 255, 255 );
                        stringRGBA( screen, mousex+70, mousey-35, data, 255, 0, 255, 255 );

                        sprintf( data, "%.5f", cellx );
                        stringRGBA( screen, mousex+30, mousey-15, "-> CelX : ", 255, 255, 255, 255 );
                        stringRGBA( screen, mousex+110, mousey-15, data, 0, 255, 255, 255 );

                        sprintf( data, "%.5f", celly );
                        stringRGBA( screen, mousex+30, mousey+5, "-> CelY : ", 255, 255, 255, 255 );
                        stringRGBA( screen, mousex+110, mousey+5, data, 0, 255, 255, 255 );


                        initialPoints.push_back( { cellx, celly } );
                        float newscore = simulate_Current_Path( screen, initialPoints );
                        initialPoints.pop_back( );


                        sprintf( data, "%.5f", currentpathscore );
                        stringRGBA( screen, mousex+30, mousey+25, "P Score : ", 255, 255, 255, 255 );
                        stringRGBA( screen, mousex+110, mousey+25, data, 0, 255, 0, 255 );

                        sprintf( data, "%.5f", newscore );
                        stringRGBA( screen, mousex+30, mousey+45, "N Score : ", 255, 255, 255, 255 );
                        stringRGBA( screen, mousex+110, mousey+45, data, 255, 0, 0, 255 );

                        lineRGBA( screen, startlinex, startliney, mousex, mousey, 255, 255, 255, 255 );

                        SDL_Flip( screen );
                    }
                    break;
                }
                case SDL_MOUSEBUTTONDOWN:
                {
                    if (testsmall==1)
                    {
                        if ((event.button.button == SDL_BUTTON_WHEELDOWN) && (usersize<256))
                        {
                            usersize*=2;
                            //sizex=sizey=usersize;
                            offsetx_max = (146-screen->w/usersize)*usersize;
                            offsety_max = (91-screen->h/usersize)*usersize;

                            if (offsetx>offsetx_max) offsetx=offsetx_max;
                            if (offsety>offsety_max) offsety=offsety_max;
                            if (offsetx<0) offsetx=0;
                            if (offsety<0) offsety=0;
                            //offsetx = 0;
                            //offsety = 0;
                        }
                        if ((event.button.button == SDL_BUTTON_WHEELUP) && (usersize>2))
                        {
                            usersize/=2;
                            //sizex=sizey=usersize;
                            offsetx_max = (146-screen->w/usersize)*usersize;
                            offsety_max = (91-screen->h/usersize)*usersize;

                            if (offsetx>offsetx_max) offsetx=offsetx_max;
                            if (offsety>offsety_max) offsety=offsety_max;
                            if (offsetx<0) offsetx=0;
                            if (offsety<0) offsety=0;
                            //offsetx = 0;
                            //offsety = 0;
                        }
                    }
                    else
                    {
                        if (usersize >32) usersize=32;
                        if (usersize<8) usersize=8;

                        if ((event.button.button == SDL_BUTTON_WHEELDOWN) && (usersize<32))
                        {
                            usersize*=2;
                            //sizex=sizey=usersize;
                            offsetx_max = (146-screen->w/usersize)*usersize;
                            offsety_max = (91-screen->h/usersize)*usersize;

                            if (offsetx>offsetx_max) offsetx=offsetx_max;
                            if (offsety>offsety_max) offsety=offsety_max;
                            if (offsetx<0) offsetx=0;
                            if (offsety<0) offsety=0;
                            //offsetx = 0;
                            //offsety = 0;
                        }
                        if ((event.button.button == SDL_BUTTON_WHEELUP) && (usersize>8))
                        {
                            usersize/=2;
                            //sizex=sizey=usersize;
                            offsetx_max = (146-screen->w/usersize)*usersize;
                            offsety_max = (91-screen->h/usersize)*usersize;

                            if (offsetx>offsetx_max) offsetx=offsetx_max;
                            if (offsety>offsety_max) offsety=offsety_max;
                            if (offsetx<0) offsetx=0;
                            if (offsety<0) offsety=0;
                            //offsetx = 0;
                            //offsety = 0;
                        }

                        load_Map_Ressources( usersize );

                    }

                    break;
                }
                case SDL_MOUSEBUTTONUP:
                {

                    if (event.button.button == SDL_BUTTON_LEFT)
                    {
                        initialPoints.push_back( { (float) (((float) mousex + (float) offsetx)/((float) sizex) ), (float) (((float) mousey + (float) offsety)/((float) sizey) ) } );
                        optimizePoints.push_back( { (float) (((float) mousex + (float) offsetx)/((float) sizex) ), (float) (((float) mousey + (float) offsety)/((float) sizey) ) } );
                    }

                    if (event.button.button == SDL_BUTTON_RIGHT)
                    {
                        if (!initialPoints.empty()) initialPoints.pop_back();
                        if (!optimizePoints.empty()) optimizePoints.pop_back();
                    }

                }
                }
            }




            /*
                        if (nodeEnd!=nullptr)
                        {
                            printf( "Best Way from nodeStart->nodeEnd : %f points \n", nodeEnd->GlobalGoal );
                            ///MatrixDistance[x][y] = nodeEnd->GlobalGoal;
                        }
            */
            // DRAWING ENDS HERE

            // finally, update the screen :)
            SDL_Flip(screen);
        } // end main loop

        ///done=false;

    }

    printf("\n");
    printf( "Score = %f - Life = %f \n", score, life);
    printf("\n");

    printf("\n");
    printf( "Score Init = %f - Optimized = %f \n", scoreinit, scoreoptim);
    printf("\n");
    /*
        FILE *out;
        out=fopen("../map/output.txt", "w" );
        for(int j = 0; j<20; j++)
        {
            for(int i = 0; i<20; i++)
            {
                //fread( &maptile[i+j*nMapWidth], sizeof(char), 1, p );
                fprintf( out, "%f\t ", MatrixDistance[i][j] );
            }
            fprintf( out, "\n " );
        }
        fclose( out );
    */

    // free loaded bitmap
    if ((testsmall==8) || (testsmall==16) || (testsmall==32))
    {
        if (montagne) SDL_FreeSurface(montagne);
        if (plaine) SDL_FreeSurface(plaine);
        if (colline) SDL_FreeSurface(colline);
        if (marecage) SDL_FreeSurface(marecage);
        if (donjon) SDL_FreeSurface(donjon);
        if (mer) SDL_FreeSurface(mer);
        if (village) SDL_FreeSurface(village);
        if (player) SDL_FreeSurface(player);
        if (foret) SDL_FreeSurface(foret);
    }

    delete[] maptile;
//    delete[] nodes;

    SDL_Quit();



    // all is well ;)
    printf("Exited cleanly\n");
    return 0;
}