#include <stdio.h>
#include <limits.h>
#include <string.h>

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

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


int compute_path_length(cell from, cell to);



void challenge_2() {
	comm_init_communication();

	cell start;
	printf("Starting station: ");
	readStation(&start.x, &start.y);
	printf("\n");

    robot r;
	r.pos = start;
	r.dir = getStartDirection(start);
	//cell_add(&r.pos,r.pos,neighbours[r.dir]);
    cell stations[3];
	for(int i=0;i<3;i++){
		printf("Station to visit %d: ",i+1);
		readStation(&(stations[i].x),&(stations[i].y));
		printf("\n");
	}
	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;
				}
			}
		}
	}
	int visited[MAZE_SIZE][MAZE_SIZE]={0};
	memcpy(visited, unexpanded_matrix, sizeof(unexpanded_matrix));

	// volg beste volgorde
	for (int i = 0; i < 3; i++) {
		cell target = stations[bestOrder[i]];
		printf("\n[Station %d] Naar (%d, %d):\n", i+1, target.x, target.y);
		stack path;
		stack_init(&path);
		lee_run(r, target, &path);
		followPath(&r, &path,visited);
		visited[r.pos.x][r.pos.y] = 5;
		stack_free(&path);
	}
	printMatrix_with_current_pos(visited,r);
	printAllMoves();
	comm_end_communication();
}

int compute_path_length(const cell from, const cell to) { //modified expand phase to get maximum length
	if (to.x == 999 && to.y == 999) return 999;
	int tempMatrix[MAZE_SIZE][MAZE_SIZE];
	memcpy(tempMatrix, unexpanded_matrix, sizeof(tempMatrix));
	int steps = 1;
	tempMatrix[to.x][to.y] = steps;
	while (tempMatrix[from.x][from.y] == 0) {
		for (int i = 0; i < MAZE_SIZE; i++) {
			for (int j = 0; j < MAZE_SIZE; j++) {
				if (tempMatrix[i][j] == steps) {
					for (int k = 0; k < 4; k++) {
						cell n;
						cell_add(&n,(cell){i,j}, neighbours[k]);
						if (isInBound(n) && tempMatrix[n.x][n.y] == 0) {
							tempMatrix[n.x][n.y] = steps + 1;
						}
					}
				}
			}
		}
		steps++;
		if (steps > 1000) break; 
	}
	return tempMatrix[from.x][from.y] ? tempMatrix[from.x][from.y] : INT_MAX;
}