#include <stdio.h>
#include <stdlib.h>


#define HEIGHT 10
#define WIDTH 10

struct node {
    int mode;
    struct node *up;
    struct node *left;
    struct node *down;
    struct node *right;
    struct node *next;
};

//Handles the queue
struct node *ptrCurrent = NULL; //First in que
struct node *ptrLast = NULL; //Last added in que
//-----------------------------

int bfs(void);
void setupMatrix(struct node matrix[HEIGHT][WIDTH]);
void printMatrix(struct node matrix[HEIGHT][WIDTH]);

int main()
{
    struct node matrix[HEIGHT][WIDTH];

setupMatrix(matrix);
    //Tweak it to setup the puzzle
    //Search from point:
    int beginY = 0;
    int beginX = 0;
    //Destination point:
    int desinY = 2;
    int destinX = 2;
    //----------------------------
    matrix[desinY][destinX].mode = 1; //Destination: mode1
    matrix[beginY][beginX].mode = 3; //Begining: mode3

    //WALL: (make a function that draws a wall from two points)
    //matrix[0][1].mode = 3;
    //matrix[1][1].mode = 3;
    //matrix[1][0].mode = 3;
    //---------------------------------------------------------


    //Inicialise the queue from the begining node
    ptrCurrent = &matrix[beginY][beginX];
    ptrLast = &matrix[beginY][beginX];
    //--------------------

    //START:
    int tries = bfs();

    //Check
    printf("Tries = %dn", tries);

    //When something unexpected happends
    if(tries == 0) {
        printf("ERROR BFSn");
        return -1;
    }
    //----------------------------------

    //When finished:
    printMatrix(matrix); //Mode5 represents the found solution
    system("PAUSE");
    return 0;
}

int bfs(void)
{
    //Use to test
    int tries = 0;

    while(1)
    {
        ptrCurrent->mode = 3;
        if(ptrCurrent->up != NULL)
        {
            if(ptrCurrent->up->mode == 1)
            {
                printf("Found the solutionn");
                ptrCurrent->up->mode = 5;
                return tries;//break;
            }
            else if(ptrCurrent->up->mode == 0)
            {
                ptrCurrent->next = ptrCurrent->up;
                ptrLast = ptrCurrent->up;
            }
        }

        if(ptrCurrent->left != NULL)
        {
            if(ptrCurrent->left->mode == 1)
            {
                printf("Found the solutionn");
                ptrCurrent->left->mode = 5;
                return tries;//break;
            }
            else if(ptrCurrent->left->mode == 0)
            {
                ptrCurrent->next = ptrCurrent->left;
                ptrLast = ptrCurrent->left;
            }
        }

        if(ptrCurrent->down != NULL)
        {
            if(ptrCurrent->down->mode == 1)
            {
                printf("Found the solutionn");
                ptrCurrent->down->mode = 5;
                return tries;//break;
            } else if(ptrCurrent->down->mode == 0)
            {
                ptrCurrent->next = ptrCurrent->down;
                ptrLast = ptrCurrent->down;
            }
        }

        if(ptrCurrent->right != NULL) {
            if(ptrCurrent->right->mode == 1)
            {
                printf("Found the solutionn");
                ptrCurrent->right->mode = 5;
                return tries;//break;
            }
            else if(ptrCurrent->right->mode == 0)
            {
                ptrCurrent->next = ptrCurrent->right;
                ptrLast = ptrCurrent->right;
            }
        }
        if(tries > (HEIGHT * WIDTH)) {
            system("ERROR");
            return 0;
        }
        ptrCurrent = ptrCurrent->next;
        tries++;

    }
    return 0;
}

void setupMatrix(struct node matrix[HEIGHT][WIDTH])
{
    int i,j;

    for(j = 0; j < HEIGHT; j++)
    {
        for(i = 0; i < WIDTH; i++)
        {
            //All nodes.mode is set to 0, this can be changed later
            matrix[j][i].mode = 0;
            //All nodes.next is set to NULL, this pointer is only for linked list purposes
            matrix[j][i].next = NULL;

            //Connect UP
            if(j == 0)
                matrix[j][i].up = NULL;
            else
                matrix[j][i].up = &matrix[j-1][i];

            //Connect LEFT
            if(i == 0)
                matrix[j][i].left = NULL;
            else
                matrix[j][i].left = &matrix[j][i-1];

            //Connect DOWN
            if(j == HEIGHT-1)
                matrix[j][i].down = NULL;
            else
                matrix[j][i].down = &matrix[j+1][i];

            //Connect RIGHT
            if(i == WIDTH-1)
                matrix[j][i].right = NULL;
            else
                matrix[j][i].right = &matrix[j][i+1];

        }
    }
}

ptrCurrent->next = ptrCurrent->up;

ptrLast->next = ptrCurrent->up;