//
// Created by nano on 5/14/25.
//
#include <stdio.h>
#include <stdlib.h>

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


#define FORWARD 4
#define BACKWARD 5//TBD
#define TURN_LEFT 1
#define TURN_RIGHT 2
#define TURN_AROUND 3

#define PARK 6

#define CROSSING 32

int firstMove = 1;
/*
 * FIRST MOVE:
 * SEND: FWD
 * RECV: ACK, SENSOR_DATA, CROSSING
 * SEND: next move
 */

extern cell neighbours[4];
extern cell crossing_neighbours[4];
/* For any move:
 * C SEND: LEFT / RIGHT / FWD
 * C RECV: ACK , CROSSING, <SENSOR DATA>
 * C SEND: NEXT INSTRUCTION
 */
void turnLeft(robot *r) {
	comm_write_until_successful(TURN_LEFT, 1000);
	r->dir = (r->dir + 3) % 4;
}

void turnRight(robot *r) {
	comm_write_until_successful(TURN_RIGHT, 1000);
	r->dir = (r->dir + 1) % 4;
}

void turnAround(robot *r) {
/*
	comm_write_until_successful(TURN_AROUND,1000);
	r->dir = (r->dir + 2) % 4;*/
	//THIJS:
	turnRight(r);
	comm_await_crossing(2000);
	comm_discard_sensor_data(1000);
	turnRight(r);
}

void forward(robot *r) {
	comm_write_until_successful(FORWARD, 1000);
	cell_add(&r->pos, r->pos, neighbours[r->dir]);
}

void backward(robot *r) {
	comm_write_until_successful(BACKWARD,1000);
	cell_add(&r->pos, r->pos, crossing_neighbours[(r->dir + 2) % 4]);
	//printf("Doesnt exist rn");
	//exit(-1);
}

void parkBackwards(robot *r) {
	comm_write_until_successful(PARK,1000);
	cell_add(&r->pos,r->pos,neighbours[(r->dir + 2) % 4]);
}

//BJORN CODE
/*
int first_move = 1;
int followPath_with_blockages(robot *r, stack *path, int visited[MAZE_SIZE][MAZE_SIZE]) {

	while (path->length > 0 ) {
		int len=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);

		switch (result) {
			case 0: //SPIN --> we expect sensor data
				comm_get_distance_sensor(&len, 1000);
				if (len != 0) {
					len=(len-1)*2+1;
					register_sensor_blockages(r, len, visited);
					comm_await_crossing(2000);
					return 0;
				}
				comm_await_crossing(2000);
				break;
			case 1: // FWD --> we do not expect sensor data, except fist move
				comm_await_crossing(2000);
				if (first_move) {
					comm_get_distance_sensor(&len, 1000);
					if (len != 0) {
						len=(len-1)*2+1;
						register_sensor_blockages(r, len, visited);
						return 0;
					}
				}
				break;
			default : ;
		}
	}
	return 1;
}*/


int followPath_with_blockages(robot *r, stack *path, int visited[MAZE_SIZE][MAZE_SIZE]) {
	while (path->length > 0) {
		int len = 0;
		cell current_move = stack_pop(path);
		int result = move(r, current_move);
		visited[r->pos.x][r->pos.y] = 5;
		switch (isValidCrossing(r->pos)) {
			case 1:
				comm_await_crossing(2000);
				comm_get_distance_sensor(&len, 1000);
				if (len != 0) {
					//len=(len-1)*2+1;
					register_sensor_blockages(r, len, visited);
					return 0;
				}
				break;
			case 0:
				comm_await_crossing(2000);
				comm_discard_sensor_data(1000);
				break;
			default: ;
		}
		printMatrix_with_current_pos(visited, *r);
		if (result==0) { //spin
			move(r, current_move); // advance forward
			comm_await_crossing(2000);
			comm_get_distance_sensor(&len, 1000);
			if (len != 0) {
				//len=(len-1)*2+1;
				register_sensor_blockages(r, len, visited);
				return 0;
			}
			printf("Continuing after spin cause no block \n");
			printMatrix_with_current_pos(visited, *r);
		}
	}
	return 1;
}

void printAllMoves() {
}

void comm_discard_sensor_data(int timeout) {
	printf("DISCARDING DATA\n");
	byte *b = malloc(sizeof(byte));
	int run = 1;
	//while (run) {
		comm_blocking_read(b, 1, timeout);
	//	for (int i = 1; i <= 5; i++) {
	//		if (*b == (byte) i) {
	//			run = 0;
	//			break;
	//		}
	//	}
	//}
	printf("%d\n", *b);
	free(b);
}