//
// Created by nano on 5/14/25.
//
// Functies om de richting aan te passen
#include <ctype.h>
#include <stdio.h>

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

//Dummy moves for the simulator / when a machine with libserialport is unavailable.

extern cell neighbours[4];
//extern cell crossing_neighbours[4];

char *moves[100];
int indx;

void turnLeft(robot *r) {
	r->dir = (r->dir + 3) % 4;
	moves[indx] = "\33[1;102mL\33[0m";
	indx++;
}

void turnRight(robot *r) {
	r->dir = (r->dir + 1) % 4;
	moves[indx] = "\33[1;101mR\33[0m";
	indx++;
}

void turnAround(robot *r) {
	r->dir = (r->dir + 2) % 4;
	moves[indx] = "\33[1;103mS\33[0m";
	indx++;
}

void forward(robot *r) {
	cell_add(&r->pos, r->pos, neighbours[r->dir]);
	moves[indx] = "\33[1;105mF\33[0m";
	indx++;
}

void backward(robot *r) {
	cell_add(&r->pos, r->pos, neighbours[(r->dir + 2) % 4]);
	moves[indx] = "\33[1;44mB\33[0m";
	indx++;
}


int followPath_with_blockages(robot *r, stack *path,int visited[MAZE_SIZE][MAZE_SIZE]) {
	while (path->length > 0) {
		cell current_move = stack_pop(path);
		int result = move(r,current_move);
		visited[r->pos.x][r->pos.y] = 5;
		printMatrix_with_current_pos(visited,*r);
		if (isValidCrossing(r->pos)) {
			printf("ADD BLOCKAGE? (Y/N) ");
			char input[10];
			scanf("%s", input);
			printf("\n");
			if (tolower(input[0]) == 'y' ){
				printf("HOW FAR? (1-4) ");
				int len;
				scanf("%d",&len);
				printf("\n");
				len=(len-1)*2+1;
				register_sensor_blockages(r,len,visited);
				return 0;
			}
		}
		if (result == 0) {
			move(r,current_move);
			visited[r->pos.x][r->pos.y] = 5;
			printMatrix_with_current_pos(visited,*r);
			if (isValidCrossing(r->pos)) {
				printf("ADD BLOCKAGE? (Y/N) ");
				char input[10];
				scanf("%s", input);
				printf("\n");
				if (tolower(input[0]) == 'y' ){
					printf("HOW FAR? (1-4) ");
					int len;
					scanf("%d",&len);
					printf("\n");
					len=(len-1)*2+1;
					register_sensor_blockages(r,len,visited);
					return 0;
				}
			}
		}
	}
	return 1;
}

void printAllMoves() {
	printf("\n");
	for (int i = 0; i < indx; i++) {
		printf("%s",moves[i]);
	}
	printf("\n");
}

void parkBackwards(robot *r) {
	backward(r);
}

void comm_discard_sensor_data(int timeout){}
void comm_await_crossing(){}
void comm_init_communication(){}
void comm_end_communication(){}