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

#define DIM_SQUARES 16
#define DIS_INITIAL 146
#define DIS_UNIT 125
#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;
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;

	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) {       // je suis dans une case , dependant de ma direction elle me donne le numero de la case
	// Value decomposition.
	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(int num) {
    if (s.top == (STACK_SIZE - 1)) {
        printf ("Stack is full.\n");
        return;
    }

    s.top = s.top + 1;
    s.stk[s.top] = num;
}

int stack_pop() {
    if (s.top == - 1) {
        printf ("Stack is empty.\n");
        return 0;
    }

    int num = s.stk[s.top];
    s.top -= 1;
	return num;
}

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

    int num = s.stk[s.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);
}

int traverse() {
	int current = stack_top();
	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);

	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);

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

	printf("U: %i, D: %i, L: %i, R: %i.\n", distance_up, distance_down, distance_left, distance_right);
	pause(1000);

	v[current] = true;

	int idx_dominant;
	DIRECTION direction_dominant;

	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;

	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_right;
		direction_dominant = DOWN;
	} else {
		idx_dominant = -1;
		direction_dominant = NONE;
	}

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

	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(idx_dominant);
	} else {
		stack_pop();
		int previous = stack_top();
		direction_dominant = direction_between(current, 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();
	}

	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(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:
}