#include "simpletools.h"                        // Library includes
#include "abdrive.h"
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdbool.h>

#define DIM_SQUARES 16
#define DIS_INITIAL 146
#define DIS_UNIT 125
#define GRA_START 0
#define GRA_END 15
#define GRA_INVW 999
#define STACK_SIZE DIM_SQUARES * DIM_SQUARES
#define IR_DISTANCE_THRESHOLD 39

typedef enum {UP, DOWN, LEFT, RIGHT, NONE} DIRECTION;
typedef struct STACK {
    int stk[STACK_SIZE];
    int top;
} STACK;

STACK s;
STACK p;
bool c[DIM_SQUARES][DIM_SQUARES];
bool v[DIM_SQUARES];

int distance_ir_left() {
	int left = 0;

	for(int dacVal = 0; dacVal < 160; dacVal += 4) {
		dac_ctr(26, 0, dacVal);
		freqout(11, 1, 38000);
		left += input(10);
	}

	return left;
}

int distance_ir_right() {
	int right = 0;

	for(int dacVal = 0; dacVal < 160; dacVal += 4) {
		dac_ctr(27, 1, dacVal);
		freqout(1, 1, 38000);
		right += input(2);
	}

	return right;
}

int direction_from(int current, DIRECTION direction) {
	int x = current % 4;
	int y = (current - x) / 4;

	if(direction == UP) y += 1;
	if(direction == DOWN) y -= 1;
	if(direction == LEFT) x -= 1;
	if(direction == RIGHT) x += 1;

	current = y * 4 + x; // Value recomposition.

	if(current >= DIM_SQUARES || current < 0) return -1;
	return current;
}

DIRECTION direction_between(int source, int destination) {
	int source_x = source % 4;
	int source_y = (source - source_x) / 4;

	int destination_x = destination % 4;
	int destination_y = (destination - destination_x) / 4;

	int delta_x = source_x - destination_x;
	int delta_y = source_y - destination_y;

	if(delta_x == 1) return LEFT;
	if(delta_x == -1) return RIGHT;
	if(delta_y == 1) return DOWN;
	if(delta_y == -1) return UP;
}

void stack_push(STACK *stack, int num) {
    if (stack->top == (STACK_SIZE - 1)) {
        printf("Stack is full.\n");
        return;
    }

    stack->top = stack->top + 1;
    stack->stk[stack->top] = num;
}

int stack_pop(STACK *stack) {
    if (stack->top == - 1) {
        printf("Stack is empty.\n");
        return -1;
    }

    int num = stack->stk[stack->top];
    stack->top -= 1;
	return num;
}

int stack_top(STACK *stack) {
    if (stack->top == - 1) {
        printf("Stack is empty.\n");
        return 0;
    }

    int num = stack->stk[stack->top];
	return num;
}

void move_initial() {
	drive_goto(DIS_INITIAL, DIS_INITIAL);
}

void move_up() {
	drive_goto(DIS_UNIT, DIS_UNIT);
}

void move_down() {
	drive_goto(-DIS_UNIT, -DIS_UNIT);
}

void move_left() {
	drive_goto(-26, 25);
	drive_goto(DIS_UNIT, DIS_UNIT);
	drive_goto(25, -26);
}

void move_right() {
	drive_goto(25, -26);
	drive_goto(DIS_UNIT, DIS_UNIT);
	drive_goto(-26, 25);
}

void print_stack(STACK *stack) {
	for(int i = 0; i < stack->top; ++i) {
		printf("%i : ", stack->stk[i]);
	}

	printf("\n");
}

void dijsktra() {
    int dist[DIM_SQUARES], prev[DIM_SQUARES], selected[DIM_SQUARES] = {0}, i, m, min, start, d, j;
    int path_reversed[DIM_SQUARES];

    for(i = 0; i < DIM_SQUARES; i++) {
        dist[i] = GRA_INVW;
        prev[i] = -1;
    }

    start = GRA_START;
    selected[start] = 1;
    dist[start] = 0;

    while(selected[GRA_END] == 0) {
        min = GRA_INVW;
        m = 0;

        for(i = 0; i < DIM_SQUARES; i++) {
            d = dist[start] + c[start][i] ? 1 : GRA_INVW;

            if(d < dist[i] && selected[i] == 0) {
                dist[i] = d;
                prev[i] = start;
            }

            if(min > dist[i] && selected[i] == 0) {
                min = dist[i];
                m = i;
            }
        }

        start = m;
        selected[start] = 1;
    }

    start = GRA_END;
    j = 0;

    while(start != -1) {
        path_reversed[j++] = start;
        start = prev[start];
    }

	for(int k = 0; k < j; ++k) stack_push(&p, path_reversed[k]);
}

void follow_shortest() {
	int current = stack_pop(&p);
	int next;

	while(current > 0) {
		next = stack_pop(&p);

		DIRECTION direction_dominant = direction_between(current, next);
		printf("Direction: %i, ID: %i.\n", direction_dominant, next);

		if(direction_dominant == UP) move_up();
		if(direction_dominant == LEFT) move_left();
		if(direction_dominant == RIGHT) move_right();
		if(direction_dominant == DOWN) move_down();

		current = next;
	}
}

int traverse() {
	int current = stack_top(&s);
	if(current == 0 && v[current]) return true;

	int id_left = direction_from(current, LEFT);
	int id_right = direction_from(current, RIGHT);
	int id_up = direction_from(current, UP);
	int id_down = direction_from(current, DOWN);
	bool direction_left, direction_right, direction_up, direction_down;

	if(!v[current]) {
		int distance_left = distance_ir_left();
		int distance_right = distance_ir_right();
		drive_goto(25, -26);

		int distance_up = distance_ir_left();
		int distance_down = distance_ir_right();
		drive_goto(-26, 25);

		direction_left = distance_left > IR_DISTANCE_THRESHOLD;
		direction_right = distance_right > IR_DISTANCE_THRESHOLD;
		direction_up = distance_up > IR_DISTANCE_THRESHOLD;
		direction_down = distance_down > IR_DISTANCE_THRESHOLD;

		c[current][id_left] = direction_left;
		c[current][id_right] = direction_right;
		c[current][id_up] = direction_up;
		c[current][id_down] = direction_down;

		c[id_left][current] = direction_left;
		c[id_right][current] = direction_right;
		c[id_up][current] = direction_up;
		c[id_down][current] = direction_down;

		printf("ID: %i, U: %i, D: %i, L: %i, R: %i.\n", current, distance_up, distance_down, distance_left, distance_right);
	} else {
		direction_left = c[current][id_left];
		direction_right = c[current][id_right];
		direction_up = c[current][id_up];
		direction_down = c[current][id_down];
	}

	v[current] = true;

	int idx_dominant;
	DIRECTION direction_dominant;

	if(direction_left && id_left != -1 && !v[id_left]) {
		idx_dominant = id_left;
		direction_dominant = LEFT;
	} else if(direction_right && id_right != -1 && !v[id_right]) {
		idx_dominant = id_right;
		direction_dominant = RIGHT;
	} else if(direction_up && id_up != -1 && !v[id_up]) {
		idx_dominant = id_up;
		direction_dominant = UP;
	} else if(direction_down && id_down != -1 && !v[id_down]) {
		idx_dominant = id_down;
		direction_dominant = DOWN;
	} else {
		idx_dominant = -1;
		direction_dominant = NONE;
	}

	if(idx_dominant != -1) {
		if(direction_dominant == UP) move_up();
		if(direction_dominant == LEFT) move_left();
		if(direction_dominant == RIGHT) move_right();
		if(direction_dominant == DOWN) move_down();
		stack_push(&s, idx_dominant);

		printf("Direction: %i, ID: %i.\n", direction_dominant, idx_dominant);
	} else {
		stack_pop(&s);
		int previous = stack_top(&s);
		direction_dominant = direction_between(current, previous);

		printf("Direction: %i, ID: %i.\n", direction_dominant, previous);

		if(direction_dominant == UP) move_up();
		if(direction_dominant == LEFT) move_left();
		if(direction_dominant == RIGHT) move_right();
		if(direction_dominant == DOWN) move_down();
	}

	print_stack(&s);

	return traverse();
}

int main() {
	// Initialization
	s.top = -1;

	for(int i = 0; i < DIM_SQUARES; ++i) {
		v[i] = false;

		for(int j = 0; j < DIM_SQUARES; ++j) c[i][j] = i == j ? true : false;
	}

	stack_push(&s, 0);

	// Step 0: Move to the first square.
	move_initial();

	// Step 1: Build a connection matrix by traversing the maze and return back.
	traverse();

	// Step 2:
    printf("Test 1.\n");
	dijsktra();
    printf("Test 2.\n");
	print_stack(&p);
	follow_shortest();
}