135 lines
3.1 KiB
C
135 lines
3.1 KiB
C
//
|
||
// 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");
|
||
}
|
||
}
|
||
|
||
|