// ============================================================================
//  GD23-04.cpp
//  A-Star Pathfinding Demo
// ============================================================================
#include "SDLGUI.h"
#include <stdlib.h>
#include <time.h>
#include "Array2D.h"
#include "Queue.h"
#include "Heap.h"

// ============================================================================
//  Global Constants
// ============================================================================
const char PROGRAM_NAME[]   = "A* Pathfinding Graphical Demonstration";
const int WIDTH             = 800;
const int HEIGHT            = 600;
const int ITEMS             = 32;
const int ARIAL             = 0;
const int MAPX              = 32;
const int MAPY              = 20;


const int QUEUESIZE = 1024;
const int DIRTABLE[8][2] = { { 0, -1 },
                           { 1, 0 },
                           { 0, 1 },
                           { -1, 0 },
                           { 1, -1 },
                           { 1, 1 },
                           { -1, 1 },
                           { -1, -1 } };

const float DISTTABLE[8] = { 1.0f, 1.0f, 1.0f, 1.0f,
                             1.414214f, 1.414214f, 1.414214f, 1.414214f };

// ============================================================================
//  classes
// ============================================================================
class Cell
{
public:
    // is the cell marked or in the queue?
    bool m_marked;
    bool m_inQueue;

    // what is the distance from this cell to the starting cell?
    float m_distance;

    // the previous node in the path.
    int m_lastx;
    int m_lasty;


    // is the cell passable?
    bool m_passable;

    // what is the "cost" of travelling into this cell?
    float m_weight;
};


enum COMMAND
{
    ENQUEUE,
    DEQUEUE,
    CHANGEPOINTER
};

class Command
{
public:
    COMMAND c;
    int x;
    int y;
    int newx;
    int newy;
};

class Coordinate
{
public:
    int x;
    int y;
    float heuristic;
};


// ============================================================================
//  Global Variables
// ============================================================================
SDLGUI* g_gui;

SDL_Surface* g_blackbox = 0;
SDL_Surface* g_redbox = 0;
SDL_Surface* g_greenbox = 0;
SDL_Surface* g_bluebox = 0;
SDL_Surface* g_wallbox = 0;
SDL_Surface* g_start = 0;
SDL_Surface* g_goal = 0;

Array2D<Cell> g_map( MAPX, MAPY );

int g_startx = -1, 
    g_starty = -1;
int g_goalx = -1, 
    g_goaly = -1;
int g_currentx = -1,
    g_currenty = -1;

bool g_processing = false;
bool g_done = false;

bool g_mousedown = false;
bool g_walldraw = false;
float g_weight = 1.0f;

int g_timer;
int g_timeLength = 100;

LQueue<Command> g_queue;


// ============================================================================
//  Drawing
// ============================================================================
void DrawMap()
{
    int x;
    int y;

    SDL_Surface* color;
    SDL_Color col;
    Cell cell;

    for( y = 0; y < MAPY; y++ )
    {
        for( x = 0; x < MAPX; x++ )
        {
            // get the cell
            cell = g_map.Get( x, y );

            // get the color of the cell
            if( x == g_currentx && y == g_currenty )
                color = g_redbox;
            else if( cell.m_inQueue == true )
                color = g_greenbox;
            else if( cell.m_marked == true )
                color = g_bluebox;
            else if( cell.m_passable == false )
                color = g_wallbox;
            else
                color = g_blackbox;
                
            // blit the cell
            col.r = 255 - (cell.m_weight - 1) * 28;
            col.g = 255 - (cell.m_weight - 1) * 28;
            col.b = 255 - (cell.m_weight - 1) * 28;

            g_gui->Box( x * 24, y * 24, 24, 24, col );
            g_gui->Blit( color, x * 24, y * 24 );
        }
    }

    // go through again and draw the pointer lines
    for( y = 0; y < MAPY; y++ )
    {
        for( x = 0; x < MAPX; x++ )
        {
            // get the cell
            cell = g_map.Get( x, y );
            // if the cell has a pointer, draw the line
            if( cell.m_lastx != -1 && cell.m_lasty != -1 )
            {
                g_gui->Line( x * 24 + 12, 
                             y * 24 + 12, 
                             cell.m_lastx * 24 + 12, 
                             cell.m_lasty * 24 + 12,
                             BLACK );
            }
        }
    }

    if( g_startx != -1 && g_starty != -1 )
        g_gui->Blit( g_start, g_startx * 24, g_starty * 24 );
    if( g_goalx != -1 && g_goaly != -1 )
        g_gui->Blit( g_goal, g_goalx * 24, g_goaly * 24 );

    if( g_done == true )
    {
        x = g_goalx;
        y = g_goaly;

        // make sure there was a path
        if( g_map.Get( x, y ).m_lastx != -1 )
        {
            while( x != g_startx || y != g_starty )
            {
                cell = g_map.Get( x, y );
                g_gui->Line( x * 24 + 12, 
                             y * 24 + 12, 
                             cell.m_lastx * 24 + 12, 
                             cell.m_lasty * 24 + 12,
                             RED );
                g_gui->Line( x * 24 + 12, 
                             y * 24 + 13, 
                             cell.m_lastx * 24 + 12, 
                             cell.m_lasty * 24 + 13,
                             RED );
                g_gui->Line( x * 24 + 12, 
                             y * 24 + 11, 
                             cell.m_lastx * 24 + 12, 
                             cell.m_lasty * 24 + 11,
                             RED );
                x = cell.m_lastx;
                y = cell.m_lasty;
            }
        }
    }

    // draw the color square
    col.r = 255 - (g_weight - 1) * 28;
    col.g = 255 - (g_weight - 1) * 28;
    col.b = 255 - (g_weight - 1) * 28;
    g_gui->Box( WIDTH - 50, HEIGHT - 50, 50, 50, BLACK );
    g_gui->Box( WIDTH - 49, HEIGHT - 49, 48, 48, col );
}





// ============================================================================
//  Other Algorithms
// ============================================================================
void Clear()
{
    int x;
    int y;

    for( y = 0; y < MAPY; y++ )
    {
        for( x = 0; x < MAPX; x++ )
        {
            g_map.Get( x, y ).m_weight = 1.0f;
            g_map.Get( x, y ).m_passable = true;
        }
    }
}

void ClearMarks()
{
    int x;
    int y;

    for( y = 0; y < MAPY; y++ )
    {
        for( x = 0; x < MAPX; x++ )
        {
            g_map.Get( x, y ).m_marked = false;
            g_map.Get( x, y ).m_inQueue = false;
            g_map.Get( x, y ).m_lastx = -1;
            g_map.Get( x, y ).m_lasty = -1;
        }
    }
}

int CompareCoordinatesDescending( Coordinate left, Coordinate right )
{
    if( left.heuristic < right.heuristic )
        return 1;
    if( left.heuristic > right.heuristic )
        return -1;
    return 0;
}


void ClearCells( Array2D<Cell>& p_map )
{
    int x;
    int y;

    for( x = 0; x < p_map.Width(); x++ )
    {
        for( y = 0; y < p_map.Height(); y++ )
        {
            p_map.Get( x, y ).m_marked = false;
            p_map.Get( x, y ).m_inQueue = false;
            p_map.Get( x, y ).m_distance = 0.0f;
            p_map.Get( x, y ).m_lastx = -1;
            p_map.Get( x, y ).m_lasty= -1;
        }
    }
}


float CellDistance( int x1, int y1, int x2, int y2 )
{
    int dx = x1 - x2;
    int dy = y1 - y2;

    dx = dx * dx;
    dy = dy * dy;

    return (float)sqrt( (double)dx + (double)dy );
}



float AStarHeuristic( int x, int y, int gx, int gy, int dir )
{
    x = x + DIRTABLE[dir][0];
    y = y + DIRTABLE[dir][1];

    return CellDistance( x, y, gx, gy );
}


void PathAStar( Array2D<Cell>& p_map, 
                           int p_x, int p_y, 
                           int p_gx, int p_gy )
{
    Command com;
    Coordinate c;
    int x, y;
    int ax, ay;
    int dir;
    float distance;

    Heap<Coordinate> queue( QUEUESIZE, CompareCoordinatesDescending );

    // clear the cells first.
    ClearCells( p_map );
    
    // enqueue the starting cell in the queue
    c.x = p_x;
    c.y = p_y;
    queue.Enqueue( c );

    com.c = ENQUEUE;
    com.x = p_x;
    com.y = p_y;
    g_queue.Enqueue( com );

    // start the main loop
    while( queue.m_count != 0 )
    {
        // pull the first cell off the queue and process it.
        x = queue.Item().x;
        y = queue.Item().y;
        queue.Dequeue();

        com.c = DEQUEUE;
        com.x = x;
        com.y = y;
        g_queue.Enqueue( com );

        // make sure the node isn't already marked. If it is, do
        // nothing.
        if( p_map.Get( x, y ).m_marked == false )
        {
            // mark the cell as it is pulled off the queue.
            p_map.Get( x, y ).m_marked = true;

            // quit out if the goal has been reached.
            if( x == p_gx && y == p_gy )
                break;

            // loop through each direction.
            for( dir = 0; dir < 8; dir++ )
            {
                // retrieve the coordinates of the current adjacent cell
                ax = x + DIRTABLE[dir][0];
                ay = y + DIRTABLE[dir][1];

                // check to see if the adjacent cell is a valid index, 
                // passable, and not marked.
                if( ax >= 0 && ax < p_map.Width() &&