#include <stdio.h>
#include <string.h>

#include "lee-algorithm.h"

#include "../stack/stack.h"
#include "../cell/cell.h"
#include "../communication/utils.h"
#include "../robot/robot.h"


// Maze en standaard Lee-functies

int unexpanded_matrix[MAZE_SIZE][MAZE_SIZE] = {
	{ -1, -1, -1, 0, -1, 0, -1, 0, -1, -1, -1},
	{ -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1},
	{ -1, 0, -1, 0, -1, 0, -1, 0, -1, 0, -1},
	{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	{ -1, 0, -1, 0, -1, 0, -1, 0, -1, 0, -1},
	{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	{ -1, 0, -1, 0, -1, 0, -1, 0, -1, 0, -1},
	{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
	{ -1, 0, -1, 0, -1, 0, -1, 0, -1, 0, -1},
	{ -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, -1},
	{ -1, -1, -1, 0, -1, 0, -1, 0, -1, -1, -1,}
};

const cell neighbours[4] = {{-1, 0}, {0, 1}, {1,}, {0, -1}}; // NORTH EAST SOUTH WEST
const cell crossing_neighbours[4] = {{-2, 0}, {0, 2}, {2,0}, {0, -2}};

void lee_run(robot r, cell targetCell, stack *moves) {
	int assignmentMatrix[MAZE_SIZE][MAZE_SIZE];
	memcpy(assignmentMatrix, unexpanded_matrix, sizeof(assignmentMatrix));
	lee_expand(assignmentMatrix, r.pos, targetCell);
	lee_trace(assignmentMatrix, r, targetCell, moves);
	stack_invert(moves);
}

void lee_expand(int assignmentMatrix[MAZE_SIZE][MAZE_SIZE], cell startCell, cell targetCell) {
	int v = 1;
	assignmentMatrix[targetCell.x][targetCell.y] = v;
	while (assignmentMatrix[startCell.x][startCell.y] == 0) {
		for (int i = 0; i < MAZE_SIZE; i++) {
			for (int j = 0; j < MAZE_SIZE; j++) {
				if (assignmentMatrix[i][j] == v) {
					for (int k = 0; k < 4; k++) {
						cell neighbour;
						cell_add(&neighbour, (cell){i, j}, neighbours[k]);
						if (isInBound(neighbour) && assignmentMatrix[neighbour.x][neighbour.y] == 0) {
							assignmentMatrix[neighbour.x][neighbour.y] = v + 1;
						}
					}
				}
			}
		}
		v += 1;
	}
}

void lee_trace(int assignmentMatrix[MAZE_SIZE][MAZE_SIZE], robot r, cell targetCell, stack *stack) {
	robot tempBot = r;
	printf("\n");
	while (!cell_equals(tempBot.pos, targetCell)) {
		// check forward first,as its most efficient move
		cell neighbourCell;
		cell_add(&neighbourCell, tempBot.pos, neighbours[tempBot.dir]);
		if (isInBound(neighbourCell) &&
			assignmentMatrix[neighbourCell.x][neighbourCell.y] > 0 &&
			assignmentMatrix[neighbourCell.x][neighbourCell.y] < assignmentMatrix[tempBot.pos.x][tempBot.pos.y]) {
			tempBot.pos = neighbourCell;
			//if (isValidCrossing(tempBot.pos) ) {
				stack_push(stack, neighbours[tempBot.dir]);
			//}
			continue;
		}
		// check rest of directions
		for (int i = 0; i < 4; i++) {
			cell_add(&neighbourCell, tempBot.pos, neighbours[(tempBot.dir + i) % 4]);
			//ugly calculation as to check all 3 other directions other than the main one
			if (isInBound(neighbourCell) &&
				assignmentMatrix[neighbourCell.x][neighbourCell.y] > 0 &&
				assignmentMatrix[neighbourCell.x][neighbourCell.y] < assignmentMatrix[tempBot.pos.x][tempBot.pos.y]) {
				tempBot.pos = neighbourCell;
				spin(&r, tempBot.dir);
				//if (isValidCrossing(tempBot.pos)) {
					stack_push(stack, neighbours[(tempBot.dir + i) % 4]);
				//}
				break;
			}
		}
	}
}

int validatePath(robot r, stack s) {
	robot temp = r;
	for (int i = 0; i < s.length; i++) {
			cell_add(&temp.pos, temp.pos,s.data[i]);
			if (unexpanded_matrix[temp.pos.x][temp.pos.y] < 0) {
				return 0;
		}
	}
	return 1;
}

extern int firstMove;
void followPath(robot *r, stack *path,int visited[MAZE_SIZE][MAZE_SIZE]) {
	while (path->length > 0) {
		cell currentCell = stack_pop(path);
		int result = move(r,currentCell);
		comm_await_crossing(2000);
		comm_discard_sensor_data(1000);
		visited[r->pos.x][r->pos.y] = 5;
		if (result == 0) {
			move(r,currentCell);
			comm_await_crossing(2000);
			comm_discard_sensor_data(1000);
			visited[r->pos.x][r->pos.y] = 5;
		}
		printMatrix_with_current_pos(visited,*r);
	}
}

int isInBound(const cell c) {
	if (c.x >= 0 && c.x < MAZE_SIZE &&
		c.y >= 0 && c.y < MAZE_SIZE) {
		return 1;
		}
	return 0;
}


int isStation(const cell c) {
	if (c.x == 0 || c.x == MAZE_SIZE - 1 ||
		c.y == 0 || c.y == MAZE_SIZE - 1){
		return 1;
		}
	return 0;

}
int isValidCrossing(const cell c) {
	if (c.x > 0 && c.x < MAZE_SIZE - 1 &&
		c.y > 0 && c.y < MAZE_SIZE - 1 &&
		c.x % 2 == 1 && c.y % 2 == 1 &&
		unexpanded_matrix[c.x][c.y]==0) {
		return 1;
		}
	return 0;
}