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

#include "../../stack/stack.h"
#include "../../cell/cell.h"
#include "../../communication/communication.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 cell neighbours[4];
extern cell crossing_neighbours[4];
int findNearestCrossing(robot r,int visited [MAZE_SIZE][ MAZE_SIZE],cell *result,int *crossing_counter);
int compute_path_length(cell from, const cell to) ;


void challenge_4(cell start) {
	comm_init_communication();
	cell finalStation,finalCrossing;
	printf("Final destination? ");
	readStation(&finalStation.x, &finalStation.y);
	direction finalDirection = getStartDirection(finalStation); // direction away from the ending start

	robot r;
	r.pos = start;
	r.dir = getStartDirection(start);
	cell_add(&finalCrossing,finalStation,neighbours[getStartDirection(finalStation)]);
	int visited[MAZE_SIZE][MAZE_SIZE] = {0};
	memcpy(visited, unexpanded_matrix, sizeof(unexpanded_matrix));


	cell result;
	stack moves;
	int crossing_counter=0,obstacle_counter=0;
	stack_init(&moves);

	stack first_move;
	stack_init(&first_move);
	stack_push(&first_move,neighbours[r.dir]);
	followPath_with_blockages(&r,&first_move,visited);

	while (findNearestCrossing(r,visited,&result,&crossing_counter)) {
		lee_run(r,result,&moves);
		if (!followPath_with_blockages(&r,&moves,visited)) { // ensure all is visited, no check for nr of obstacles because sometimes it creates bugs
			obstacle_counter++;
			if (validatePath(r,moves)) {
				printf("following path \n");
				followPath(&r,&moves,visited);
			}
			else {
				crossing_counter--;
			}
		}
		else { // if we know where all 12 blocks are, we go
			followPath(&r,&moves,visited);
		}
		stack_reinit(&moves);
		printf("Crossings:%d \n",crossing_counter);
		//if (crossing_counter==25) {
		//	break;
		//}
	}
	lee_run(r,finalCrossing,&moves); // Go to crossing in front of the final destination
	followPath(&r,&moves,visited);
	stack_free(&moves);
	spin(&r,finalDirection);
	printMatrix_with_current_pos(visited,r);
	parkBackwards(&r);
	printMatrix_with_current_pos(visited,r);
	printf("done");
	printAllMoves();
	comm_end_communication();

}


/**
 * Function to find the nearest/best crossing to go to
 * Steps:
 * - Unvisited and without blocks
 * - Unvisited but with a block
 * - Visited
 * In all cases forwards gets priority
 * @param r robot
 * @param visited array of all visited cells
 * @param result crossing that was chosen as the best next possible move
 * @param crossing_counter count amount of crossings
 * @return 1 if found an unvisited crossing, 0 if all crossings visited.
 */
int findNearestCrossing(robot r,int visited [MAZE_SIZE][MAZE_SIZE],cell *result,int *crossing_counter) {
	cell tempcell;
	robot temp_robot;
	temp_robot.pos = r.pos;
	temp_robot.dir = r.dir;

	if(!isValidCrossing(temp_robot.pos)){ // if robot is in a intersection, move it to crossing
		cell_add(&temp_robot.pos,temp_robot.pos,neighbours[r.dir]);
	}

	// check for nearby unvisited crossings and without blockages
	for (int i=0;i<4;i++) {
		cell_add(&tempcell,temp_robot.pos,neighbours[(r.dir+i)%4]);
		if (isInBound(tempcell) && unexpanded_matrix[tempcell.x][tempcell.y]!=-1) {
			cell_add(&tempcell,temp_robot.pos,crossing_neighbours[(r.dir+i)%4]);
			if (isValidCrossing(tempcell) && visited[tempcell.x][tempcell.y] == 0) {
				*result= tempcell;
				*crossing_counter+=1;
				printf("nearby_unvisited \n");
				return 1;
			}
		}
	}

	//find closest neighbour of neighbours
	int min_distance = INT_MAX;
	cell min_cell = (cell){0,0};

	for (int i=0;i<4;i++) {
		cell neighbour_crossing;
		cell_add(&neighbour_crossing,temp_robot.pos,crossing_neighbours[i]);
		for (int j=0;j<4;j++) {
			cell_add(&tempcell,neighbour_crossing,neighbours[j]);
			if (isInBound(tempcell) && unexpanded_matrix[tempcell.x][tempcell.y]!=-1) {
				cell_add(&tempcell,tempcell,neighbours[j]);
				if (isValidCrossing(tempcell) && visited[tempcell.x][tempcell.y] == 0) {
					int tmp = compute_path_length(r.pos,tempcell);
					if (tmp <= min_distance) {
						min_distance=tmp;
						min_cell = tempcell;
					}
				}
			}
		}
	}
	if (!cell_equals(min_cell, (cell){0,0})) {
		*result=min_cell;
		*crossing_counter+=1;
		printf("neighb_of_neighb \n");
		return 1;
	}

	min_distance = INT_MAX;
	min_cell = (cell){0,0};
	//find the closest unvisited crossing that isnt a neighbour of a neighbour.
	for (int i=0;i<MAZE_SIZE;i++) {
		for (int j=0;j<MAZE_SIZE;j++) {
			tempcell = (cell) {i,j};
			if (isValidCrossing(tempcell) && visited[i][j] == 0) {
				int tmp = compute_path_length(r.pos,tempcell);
					if (tmp <= min_distance) {
						min_distance=tmp;
						min_cell = tempcell;
					}
			}
		}
	}
	if (!cell_equals(min_cell, (cell){0,0})) {
		*result=min_cell;
		*crossing_counter+=1;
		printf("random_iterate \n");
		return 1;
	}
	return 0;
}