/*
Makefile :

CXXFLAGS+= -Wall -g -O0 `sdl-config --cflags`
LDFLAGS+=`sdl-config --libs`

Demo : Demo.cc

clean :
	rm -f *.o *~ Demo

*/

#include <cstdio>
#include <cstring>
#include <cstdlib>

#include <vector>

#include <inttypes.h>
#include <SDL.h>

using namespace std;

/**
 * Structure permettant de gerer des coordonnes 2D
 */
struct Vect
{
    int _x;
    int _y;

    Vect(int x = 0, int y = 0) : _x(x), _y(y) {}

    bool operator != (const Vect &vect) const { return ((_x != vect._x) || (_y != vect._y));}

    bool operator == (const Vect &vect) const { return ((_x == vect._x) && (_y == vect._y));}

    friend Vect operator-(const Vect &v1, const Vect &v2)
        {
            return Vect(v1._x - v2._x, v1._y - v2._y);
        }

    friend Vect operator+(const Vect &v1, const Vect &v2)
        {
            return Vect(v1._x + v2._x, v1._y + v2._y);
        }

};

#undef DEBUG

/**
 * Cette methode permet d'enregistrer le point dans la liste des waypoints.
 * 
 */
bool enregistre(const uint8_t *image, unsigned pitch, vector<Vect> &waypoints, const Vect &point)
{
    // On parcours la liste des waypoints a la recherche de doublons
    for(unsigned i = 0; i < waypoints.size(); i++)
    {
        Vect curr = waypoints[i];
	
        // si on retombe sur un point deja enregistre on tourne en rond
        if(curr == point)
        {
            printf("  on tourne en rond !\n");	  
            return false;
        }

#if 1
        // on essai de rejoindre le nouveau point en ligne droite depuis chacun des points deja enregistres
        if(i < waypoints.size() - 1)
        {
            Vect len = point - curr;

            Vect delta;
            delta._x = (len._x >= 0)?1:-1;
            delta._y = (len._y >= 0)?1:-1;
            // ligne plutot verticale
            if(abs(len._y) > abs(len._x))
            {
                int err = -abs(len._y);
                while(curr._y != point._y)
                {
                    err += 2 * abs(len._x);
                    if(err >= 0)
                    {
                        if(image[curr._y * pitch + curr._x + delta._x] == 0)
                        {
                            break;
                        }
                        curr._x += delta._x;
                        err -= 2 * abs(len._y);
                    }
                    if(image[(curr._y + delta._y) * pitch + curr._x] == 0)
                    {
                        break;
                    }
                    curr._y += delta._y;
                }
            }
            // ligne plutot horizontale
            else
            {
                int err = -abs(len._x);
                while(curr._x != point._x)
                {
                    err += 2 * abs(len._y);
                    if(err >= 0)
                    {
                        if(image[(curr._y + delta._y) * pitch + curr._x] == 0)
                        {
                            break;
                        }
                        curr._y += delta._y;
                        err -= 2 * abs(len._x);
                    }
                    if(image[curr._y * pitch + curr._x + delta._x] == 0)
                    {
                        break;
                    }
                    curr._x += delta._x;
                }
            }

            // si on trouve un racourcis, on coupe les points inutiles
            if(curr == point)
            {
                waypoints.resize(i + 1);
            }
        }
#endif

    }  

    // enfin on enregistre le point en queue de liste
    waypoints.push_back(point);

    return true;
}

/**
 * Cette methode permet de trouver un chemin entre from et to.
 * Le resultat est range dans waypoints
 */
bool pathFinding(const uint8_t *image, unsigned pitch, const Vect &from, const Vect &to, vector<Vect> &waypoints)
{
    // On exclus les consignes sur des murs
    if(image[from._y * pitch + from._x] == 0 || image[to._y * pitch + to._x] == 0)
    {
        printf("on ne peut ni arriver ni partir sur les mur !\n");
        return false;
    }

    // initialisation des waypoints avec le point de depart
    Vect curr = from;
    if(!enregistre(image, pitch, waypoints, curr)) return false;

    // la face est le cote par lequel on contourne un obstacle
    enum {Aucune, Nord, Est, Sud, Ouest} face = Aucune;
    // le sens est celui duquel on contourne l'obstacle
    enum {CCW = -1, Aucun = 0, CW = 1} sens = Aucun;
    // l'objectif est remplis lorsqu'un obstacle est franchis
    struct {Vect _min; Vect _max; } objectif = {Vect(INT_MIN, INT_MIN), Vect(INT_MAX, INT_MAX)};

    // Ce parametre permet de ne pas bloquer l'appli en cas de chemin introuvabe
    int ttl = 2000;
    while(ttl--)
    {
        // On avance en ligne droite
        if(face == Aucune)
        {
#ifdef DEBUG
            printf("avance\n");
#endif

            Vect len = to - curr;

            Vect delta;
            delta._x = (len._x >= 0)?1:-1;
            delta._y = (len._y >= 0)?1:-1;
      
            // ligne plutot verticale
            if(abs(len._y) > abs(len._x))
            {
                int err = -abs(len._y);
                while(curr._y != to._y)
                {
                    err += 2 * abs(len._x);
                    if(err >= 0)
                    {
                        if(image[curr._y * pitch + curr._x + delta._x] == 0)
                        {
                            break;
                        }
                        curr._x += delta._x;
                        err -= 2 * abs(len._y);
                    }
                    if(image[(curr._y + delta._y) * pitch + curr._x] == 0)
                    {
                        break;
                    }
                    curr._y += delta._y;
                }
            }
            // ligne plutot horizontale
            else
            {
                int err = -abs(len._x);
                while(curr._x != to._x)
                {
                    err += 2 * abs(len._y);
                    if(err >= 0)
                    {
                        if(image[(curr._y + delta._y) * pitch + curr._x] == 0)
                        {
                            break;
                        }
                        curr._y += delta._y;
                        err -= 2 * abs(len._x);
                    }
                    if(image[curr._y * pitch + curr._x + delta._x] == 0)
                    {
                        break;
                    }
                    curr._x += delta._x;
                }
            }

            if(!enregistre(image, pitch, waypoints, curr)) return false;

            // On a atteint l'objectif on arrete tout
            if(curr == to)
            {
                return true;
            }

            // Determination de la face sur laquelle on bute, du sens dans lequel on la contourne, et de l'objectif a atteindre
            if(image[curr._y * pitch + curr._x] == 255 && image[(curr._y + 1) * pitch + curr._x] == 0)
            {
                face = Nord;
                if(waypoints[waypoints.size() - 1]._x < waypoints[waypoints.size() - 2]._x)
                {
                    sens = CCW;
                }
                else
                {
                    sens = CW;
                }
                objectif._min = Vect(INT_MIN, curr._y);
                objectif._max = Vect(INT_MAX, INT_MAX);
            } 
            else if(image[curr._y * pitch + curr._x] == 255 && image[curr._y * pitch + curr._x - 1] == 0)
            {
                face = Est;
                if(waypoints[waypoints.size() - 1]._y < waypoints[waypoints.size() - 2]._y)
                {
                    sens = CCW;
                }
                else
                {
                    sens = CW;
                }
                objectif._min = Vect(INT_MIN, INT_MIN);
                objectif._max = Vect(curr._x, INT_MAX);
            }
            else if(image[curr._y * pitch + curr._x] == 255 && image[(curr._y - 1) * pitch + curr._x] == 0)
            {
                face = Sud;
                if(waypoints[waypoints.size() - 1]._x < waypoints[waypoints.size() - 2]._x)
                {
                    sens = CW;
                }
                else
                {
                    sens = CCW;
                }
                objectif._min = Vect(INT_MIN, INT_MIN);
                objectif._max = Vect(INT_MAX, curr._y);
            }
            else if(image[curr._y * pitch + curr._x] == 255 && image[curr._y * pitch + curr._x + 1] == 0)
            {
                face = Ouest;
                if(waypoints[waypoints.size() - 1]._y < waypoints[waypoints.size() - 2]._y)
                {
                    sens = CW;
                }
                else
                {
                    sens = CCW;
                }
                objectif._min = Vect(curr._x, INT_MIN);
                objectif._max = Vect(INT_MAX, INT_MAX);
            }
            else
            {
                printf("impossible : on doit au moins buter sur une face nord, est, sud ou ouest\n");
                return false;
            }
        }
        else
        {
            // On contourne l'obstacle par le nord
            if(face == Nord)
            {
#ifdef DEBUG
                printf("contourne par la face nord  dans le sens %s\n", (sens == CCW)?"CCW":"CW");
#endif
                // decalage vers l'est ou l'ouest en fonction du sens
                while(image[curr._y * pitch + curr._x] == 255 && image[(curr._y + 1) * pitch + curr._x] == 0)
                {
                    curr._x += sens;
                }

                if(image[(curr._y + 1) * pitch + curr._x] == 255)
                {
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On s'avance sur la face verticale
                    face = (sens == CCW)?Ouest:Est;
                    curr._y += 1;
                }
                else
                {
                    // ooops, on est alle trop loin
                    curr._x -= sens;
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On est deja sur la face verticale
                    face = (sens == CCW)?Est:Ouest;
                }		
#ifdef DEBUG
                printf("  passage face %s\n", (face==Ouest)?"ouest":"est");
#endif
            }

            // On contourne l'obstacle par l'est
            else if(face == Est)
            {
#ifdef DEBUG
                printf("contourne par la face est   dans le sens %s\n", (sens == CCW)?"CCW":"CW");
#endif
                // decalage vers le nord ou le sud en fonction du sens
                while(image[curr._y * pitch + curr._x] == 255 && image[curr._y * pitch + curr._x - 1] == 0)
                {
                    curr._y += sens;
                } 

                if(image[curr._y * pitch + curr._x - 1] == 255)
                {
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On s'avance sur la face horizontale
                    face = (sens == CCW)?Nord:Sud;
                    curr._x -= 1;
                }
                else if(image[curr._y * pitch + curr._x] == 0)
                {
                    // ooops, on est alle trop loin
                    curr._y -= sens;
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On est deja sur la face horizontale
                    face = (sens == CCW)?Sud:Nord;
                }		
#ifdef DEBUG
                printf("  passage face %s\n", (face==Nord)?"nord":"sud");
#endif
            }

            // On contourne l'obstacle par le sud
            else if(face == Sud)
            {
#ifdef DEBUG
                printf("contourne par la face sud   dans le sens %s\n", (sens == CCW)?"CCW":"CW");
#endif
                // decalage vers l'est ou l'ouest en fonction du sens
                while(image[curr._y * pitch + curr._x] == 255 && image[(curr._y - 1) * pitch + curr._x] == 0)
                {
                    curr._x -= sens;
                }

                if(image[(curr._y - 1) * pitch + curr._x] == 255)
                {
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On s'avance sur la face verticale
                    face = (sens == CCW)?Est:Ouest;
                    curr._y -= 1;
                }
                else if(image[curr._y * pitch + curr._x] == 0)
                {
                    // ooops, on est alle trop loin
                    curr._x += sens;
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On est deja sur la face verticale
                    face = (sens == CCW)?Ouest:Est;
                }		
#ifdef DEBUG
                printf("  passage face %s\n", (face==Ouest)?"ouest":"est");
#endif
            }

            // on contourne l'obstacle par l'ouest
            else if(face == Ouest)
            {
#ifdef DEBUG
                printf("contourne par la face ouest dans le sens %s\n", (sens == CCW)?"CCW":"CW");
#endif
                // decalage vers le nord ou le sud en fonction du sens
                while(image[curr._y * pitch + curr._x] == 255 && image[curr._y * pitch + curr._x + 1] == 0)
                {
                    curr._y -= sens;
                }

                if(image[curr._y * pitch + curr._x + 1] == 255)
                {
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On s'avance sur la face horizontale
                    face = (sens == CCW)?Sud:Nord;
                    curr._x += 1;
                }
                else
                {
                    // ooops, on est alle trop loin
                    curr._y += sens;
                    // On enregistre le point
                    if(!enregistre(image, pitch, waypoints, curr)) return false;
                    // On est deja sur la face horizontale
                    face = (sens == CCW)?Nord:Sud;
                }		
#ifdef DEBUG
                printf("  passage face %s\n", (face==Nord)?"nord":"sud");
#endif
            }

            // On teste la condition de contournement
            if(objectif._min._x < curr._x && 
               objectif._min._y < curr._y && 
               curr._x < objectif._max._x && 
               curr._y < objectif._max._y)
            {
#ifdef DEBUG
                printf("  contournement effectif\n");
#endif
                // objectif atteint : on repard en ligne droite
                face = Aucune;
            }
        }
    }

    printf("trop d'iterations\n");
    return false;
}

void dessine(uint8_t *screen, unsigned pitch, uint8_t color, vector<Vect> &waypoints)
{
  if(waypoints.empty())
    {
      return;
    }

  Vect from = waypoints.back();
  waypoints.pop_back();

  while(!waypoints.empty())
    {
      Vect to = waypoints.back();
      waypoints.pop_back();
      
      Vect len = to - from;

      Vect delta;
      delta._x = (len._x >= 0)?1:-1;
      delta._y = (len._y >= 0)?1:-1;

      Vect curr = from;

      // ligne plutot verticale
      if(abs(len._y) > abs(len._x))
	{
	  int err = -abs(len._y);
	  while(curr._y != to._y)
	    {
	      screen[curr._y * pitch + curr._x] = color;
	      err += 2 * abs(len._x);
	      if(err >= 0)
		{
		  curr._x += delta._x;
		  err -= 2 * abs(len._y);
		}
	      curr._y += delta._y;
	    }
	}
      // ligne plutot horizontale
      else
	{
	  int err = -abs(len._x);
	  while(curr._x != to._x)
	    {
	      screen[curr._y * pitch + curr._x] = color;
	      err += 2 * abs(len._y);
	      if(err >= 0)
		{
		  curr._y += delta._y;
		  err -= 2 * abs(len._x);
		}
	      curr._x += delta._x;
	    }
	}

#ifdef DEBUG
      printf("%3d %3d -> %3d %3d\n", from._x, from._y, to._x, to._y);
#endif
      from = to;
    }

}

int main(int argc, char **argv)
{
    if(argc < 2)
    {
        printf("Un argument est requis\n");
        return 1;
    }
    
    /* SDL init */
    printf("Initializing SDL.\n");
    if((SDL_Init(SDL_INIT_VIDEO) == -1)) 
    { 
        printf("Could not initialize SDL: %s.\n", SDL_GetError());
        exit(1);
    }
    atexit(SDL_Quit);
    printf("SDL initialized.\n");

    /* Image init */
    SDL_Surface *buffer = SDL_LoadBMP(argv[1]);
    if (buffer == NULL) 
    {
        fprintf(stderr, "Couldn't open %s\n", argv[0]);
        exit(1);
    }

    /* Screen init */
    SDL_Surface *screen = SDL_SetVideoMode(buffer->w, buffer->h, 8, SDL_HWSURFACE);
    if (screen == NULL) 
    {
        fprintf(stderr, "Couldn't set 640x480x8 video mode : %s\n", SDL_GetError());
        exit(1);
    }

    SDL_Surface *image = SDL_DisplayFormat(buffer);
    SDL_FreeSurface(buffer);

    SDL_BlitSurface(image, 0, screen, 0);  
 
    SDL_UpdateRect(screen, 0, 0, 0, 0);

    /* Enable Unicode translation */
    SDL_EnableUNICODE( 1 );

    bool stop = false;
    SDL_Event event;
    Vect A;
    Vect B;

    while (!stop) 
    {
        while (SDL_PollEvent(&event) && !stop) 
        {
            switch (event.type) 
            {
            case SDL_QUIT: 
            {
                stop = true;
            }
            break;
            case SDL_KEYDOWN:
                switch(event.key.keysym.unicode)
                {
                case 'a' :
                    SDL_GetMouseState(&A._x, &A._y);
                    break;
                case 'b' :
                    SDL_GetMouseState(&B._x, &B._y);
                    SDL_BlitSurface(image, 0, screen, 0);
		  
                    vector<Vect> waypoints;
                    printf("--------------------------\n");
                    pathFinding((const uint8_t*)image->pixels, image->pitch, A, B, waypoints);
                    printf("~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
                    dessine((uint8_t*)screen->pixels, screen->pitch, 60, waypoints);

                    SDL_UpdateRect(screen, 0, 0, 0, 0);
                    break;
                }
                break;
            }
        }

    }
    SDL_FreeSurface(image);
    SDL_FreeSurface(screen);

    exit(0);
}