//
// Created by nano on 5/9/25.
//

#include <stdio.h>

#include "robot.h"

#include "../lee-algorithm/lee-algorithm.h"

extern int unexpanded_matrix[MAZE_SIZE][MAZE_SIZE];
extern const cell neighbours[4];
char *arrow[4] = {"↑", "→", "↓", "←"};

// Richting in tekst
const char *directionToString(direction dir) {
	switch (dir) {
		case NORTH: return "NORTH";
		case EAST: return "EAST";
		case SOUTH: return "SOUTH";
		case WEST: return "WEST";
		default: return "UNKNOWN";
	}
}

// Bepaal gewenste richting van current naar next

int determineDirection(cell from, cell to) {
	if (to.x == from.x - 1) return NORTH;
	if (to.x == from.x + 1) return SOUTH;
	if (to.y == from.y - 1) return WEST;
	if (to.y == from.y + 1) return EAST;
	return -1;
}

int determineDirectionFromMove(cell move) {
	if (move.x == -1) return NORTH;
	if (move.x == 1) return SOUTH;
	if (move.y == -1) return WEST;
	if (move.y == 1) return EAST;
	return -1;
}

int getStartDirection(cell startCell) {
	if (startCell.x == 0) return SOUTH;
	if (startCell.x == MAZE_SIZE - 1) return NORTH;
	if (startCell.y == 0) return EAST;
	if (startCell.y == MAZE_SIZE - 1) return WEST;

	//fprintf(stderr, "Warning: startCell is not on the maze edge!\n");
	return NORTH; // fallback
}

void spin(robot *robot, direction desiredDirection) {
	switch (robot->dir - desiredDirection) {
		case -2:
		case 2:
			turnAround(robot);
			break;
		case 1:
		case -3:
			turnLeft(robot);
			break;
		case -1:
		case 3:
			turnRight(robot);
			break;
		case 0:
		default:
			break;
	}
}


/**
 * move the robot
 * @param r robot
 * @param move which move to make
 * @return 1 if forward
 * @return 0 if spin
 */
int move(robot *r, cell move) {
	if (r->dir != (direction) determineDirectionFromMove(move)) {
		spin(r, determineDirectionFromMove(move));
		return 0;
	}
	forward(r);
	return 1;
}


// Sensorinput: geeft 1–4 aan, blok in die richting, gemeten vanaf huidige robotpositie
void register_sensor_blockages(robot* r, int sensorInput,int visited[MAZE_SIZE][MAZE_SIZE]) {

	if (!isValidCrossing(r->pos)) {
		printf("Waarschuwing: robot staat niet op een kruispunt (%d,%d)", r->pos.x, r->pos.y);
	}

	cell step = r->pos;
	switch (r->dir) {
		case NORTH: step.x -= sensorInput; break;
		case EAST:  step.y += sensorInput; break;
		case SOUTH: step.x += sensorInput; break;
		case WEST:  step.y -= sensorInput; break;
	}
	if (step.x >= 1 && step.x < MAZE_SIZE-1 &&
		step.y >= 1 && step.y < MAZE_SIZE-1 &&
		((step.x%2==1 && step.y%2==0)|| (step.x%2==0 && step.y%2==1))) {
		unexpanded_matrix[step.x][step.y] = -1;
		visited[step.x][step.y] = -1;
		printf("Blokkade geregistreerd op (%d, %d)", step.x, step.y);
	}
}

void printMatrix_with_current_pos(int m[][MAZE_SIZE], robot r) {
	//system("clear");
	printf("\n");
	for (int i = 0; i < MAZE_SIZE; i++) {
		for (int j = 0; j < MAZE_SIZE; j++) {
			if (r.pos.x == i && r.pos.y == j) {
				printf("\033[31;47;1;4m %s \033[0m", arrow[r.dir]);
				continue;
			}
			if (m[i][j] == 5) {
				printf("\033[34;47;1m%3s\033[0m", "░░░");
				continue;
			}
			printf("\033[%dm%3s\33[0m",m[i][j]<0?40:107, "░░░");
		}
		printf("\n");
	}
}