#include <stdio.h>
#include "../../cell/cell.h"
#include <limits.h>
#include <string.h>

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

#define MAZE_SIZE 11

extern int unexpanded_matrix[MAZE_SIZE][MAZE_SIZE];
extern const cell neighbours[4]; // volgorde: SOUTH, EAST, WEST, NORTH


int compute_path_length(cell from, cell to);
void find_best_order_of_3(cell start,cell stations[3],int bestOrder[3]);
void find_best_order_of_2(cell start,cell stations[2],int bestOrder[2]);


void challenge_3(cell start) {
	comm_init_communication();
	int visited[MAZE_SIZE][MAZE_SIZE]={0};
	memcpy(visited, unexpanded_matrix, sizeof(unexpanded_matrix));

	robot r;
	r.pos = start;
	r.dir = getStartDirection(start);

	stack path;
	stack_init(&path);

	int visitedStations=0,run=1,bestOrder[3];

	cell all_stations[3];
	cell last_stations[2];

	for(int i=0;i<3;i++){
		printf("Station to visit %d: ",i+1);
		readStation(&(all_stations[i].x),&(all_stations[i].y));
		printf("\n");
	}

	find_best_order_of_3(start,all_stations,bestOrder);

	cell target = all_stations[bestOrder[0]];
	lee_run(r, target, &path);

	while (run){
		printf("\n[Station %d] Naar (%d, %d):\n", visitedStations +1 ,target.x, target.y);
		switch (visitedStations) {
			case 0:
				if (!followPath_with_blockages(&r, &path,visited)) {
					find_best_order_of_3(r.pos,all_stations,bestOrder);
					target = all_stations[bestOrder[0]];
					stack_reinit(&path);
					lee_run(r, target, &path);
				}
				else {
					//robot is in the station, manually spin
					unexpanded_matrix[r.pos.x][r.pos.y]=0; //bugfix, somehow something was causing for a blockage to show up at station.
					turnRight(&r);
					r.dir=(r.dir+1)%4;
					comm_await_crossing(2000);
					comm_discard_sensor_data(1000);
					visitedStations++;
					last_stations[0] = all_stations[bestOrder[1]];
					last_stations[1] = all_stations[bestOrder[2]];
					find_best_order_of_2(r.pos,last_stations,bestOrder);
					stack_reinit(&path);
					target = last_stations[bestOrder[0]];
					lee_run(r, target, &path);
				}
				break;
			case 1:
				if (!followPath_with_blockages(&r, &path,visited)) {
					find_best_order_of_2(r.pos,last_stations,bestOrder);
					target = last_stations[bestOrder[0]];
					stack_reinit(&path);
					lee_run(r, target, &path);
				}
				else {
					//robot is in the station, manually spin
					unexpanded_matrix[r.pos.x][r.pos.y]=0; //bugfix, somehow something was causing for a blockage to show up at station.
					turnRight(&r);
					r.dir=(r.dir+1)%4;
					comm_await_crossing(2000);
					comm_discard_sensor_data(1000);
					visitedStations++;
					stack_reinit(&path);
					target = last_stations[bestOrder[1]];
					lee_run(r, target, &path);
				}
				break;
			case 2:
				if (!followPath_with_blockages(&r,&path,visited)) {
					stack_reinit(&path);
					lee_run(r, target, &path);
				}
				else {
					//robot is in the station, manually spin
					unexpanded_matrix[r.pos.x][r.pos.y]=0; //bugfix, somehow something was causing for a blockage to show up at station.
					turnRight(&r);
					r.dir=(r.dir+1)%4;
					comm_await_crossing(2000);
					comm_discard_sensor_data(1000);
					visitedStations++;
				}
				break;
			case 3:
				run=0;
				break;
			default: ;
		}

	}
	stack_free(&path);
	printMatrix_with_current_pos(visited,r);
	printAllMoves();
	comm_end_communication();
}

void find_best_order_of_3(cell start,cell stations[3],int bestOrder[3]) {
	int minTotal = INT_MAX;

	// test alle 6
	for (int a = 0; a < 3; a++) {
		for (int b = 0; b < 3; b++) {
			if (b == a) continue;
			for (int c = 0; c < 3; c++) {
				if (c == a || c == b) continue;
				int total = 0;
				total += compute_path_length(start, stations[a]);
				total += compute_path_length(stations[a], stations[b]);
				total += compute_path_length(stations[b], stations[c]);
				if (total < minTotal) {
					minTotal = total;
					bestOrder[0] = a;
					bestOrder[1] = b;
					bestOrder[2] = c;
				}
			}
		}
	}
}

void find_best_order_of_2(cell start,cell stations[2],int bestOrder[2]) {

	int total1 = 0;
	total1 += compute_path_length(start, stations[0]);
	total1 += compute_path_length(stations[0], stations[1]);

	int total2 = 0;
	total2 += compute_path_length(start, stations[1]);
	total2 += compute_path_length(stations[1], stations[0]);

	if (total1 < total2) {
		bestOrder[0] = 0;
		bestOrder[1] = 1;
	}
	else {
		bestOrder[0] = 1;
		bestOrder[1] = 0;
	}
}