% To Run Progragm in SWIPL, Enter the Following: go(board(<width of the grid>, <height of the grid>, (<start position>), (<end position>), <list of cars>, Path, CarConfig)).
% A 3 Cars Setup:
	% go(board(6, 6, (0,2), (5,2), [car(1,(3,0),(3,2)), car(2,(1,3),(3,3)), car(3,(4,5),(4,3))], Path, CarConfig)).
% A 9 Cars Setup: 
	% go(board(6, 6, (0,2), (5,2), [car(1,(2,0),(2,1)), car(2,(3,0),(4,0)), car(3,(5,0),(5,1)), car(4,(4,1),(4,2)), car(5,(5,2),(5,3)), car(6,(2,3),(3,3)), car(7,(0,4),(2,4)), car(8,(3,4),(3,5)), car(9,(4,4),(5,4))], Path, CarConfig)).

% -------------------------------------------------- ↓ Program Starts Here ↓ --------------------------------------------------

% 1st Goto: Passes in from Command Line, then moves to 1st Traversal
go(board(BoardWidth, BoardHeight, CurrentCell, ExitCell, SortedCars), VisistedCells, Path, CarConfig) :-
	traversal(board(Width, Height, Current, Exit, SortedCars), VisistedCells, Path),
	print(SortedCars).

% 1st Traversal: checks to see whether current position of robot is at end point (current cell equals end point cell)
traversal(board(Width, Height, Current, Current, SortedCars), VisistedCells, VisistedCells).

% 2nd Traversal: if 1st Traversal fails, it's because current is not at the end point, so the robot can and must still move (consults heuristics)
traversal(board(Width, Height, Current, Exit, SortedCars), VisistedCells, Path) :-
	robot_heuristic(Current, Exit, (CarRightX, CarBottomY)),
	\+ is_car_cell((CarRightX, CarBottomY), SortedCars),
	is_inside_board((CarRightX, CarBottomY), board(Width, Height, _, _, _)),
	\+ memberchk((CarRightX, CarBottomY), VisistedCells),
	traversal(board(Width, Height, (CarRightX, CarBottomY), Exit, SortedCars), [(CarRightX, CarBottomY)|VisistedCells], Path).	

% Heuristics for Robot:
robot_heuristic((CurrentX, CurrentY), (EndPointX, EndPointY), (NewX, NewY)) :-
	CurrentX < EndPointX,
	NewX is CurrentX + 1,
	NewY = CurrentY.
robot_heuristic(board(BoardWidth, BoardHeight, (CurrentX, CurrentY), (EndPointX, EndPointY), SortedCars), (NewX, NewY)) :-
	CurrentY > EndPointY,
	NewY is CurrentY - 1,
	NewX = CurrentX.	
robot_heuristic(board(BoardWidth, BoardHeight, (CurrentX, CurrentY), (EndPointX, EndPointY), SortedCars), (NewX, NewY)) :-
	CurrentY < EndPointY,
	NewY is CurrentY + 1,
	NewX = CurrentX.	
robot_heuristic(board(BoardWidth, BoardHeight, (CurrentX, CurrentY), (EndPointX, EndPointY), SortedCars), (NewX, NewY)) :-
	NewY is CurrentY - 1,
	NewX = CurrentX.
robot_heuristic(board(BoardWidth, BoardHeight, (CurrentX, CurrentY), (EndPointX, EndPointY), SortedCars), (NewX, NewY)) :-
	NewY is CurrentY + 1,
	NewX = CurrentX.	
	
% 2nd Goto: If 1st Goto fails, it passes in SortedCars. If 'not' any of the Traversals, then newcars is updated carlist
go(board(BoardWidth, BoardHeight, CurrentCell, ExitCell, SortedCars), VisistedCells, Path, CarConfig) :-
	\+ traversal(board(Width, Height, Current, Exit, SortedCars), VisistedCells, Path),
	get_car_config(board(BoardWidth, BoardHeight, CurrentCell, ExitCell, SortedCars), SortedCars, NewCars, CarConfig, NewCarConfig),
	go(board(Width, Height, (0, 2), Exit, NewCars), [(0,2)], Path, NewCarConfig).

% Definition for get_car_config:
get_car_config(Board, Cars, NewCars, CarConfig, NewCarConfig) :-
	move_car(Cars, NewCars, Board),
	\+memberchk(NewCars, CarConfig),
	append([NewCars], [CarConfig], NewCarConfig).

% All Types of car movements Defined Below -----------------------------------------------------------

	% Moving Car Up and deleting old position
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_vertical(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarTopY is CarTopY - 1,
			\+ is_car_cell((CarLeftX, NewCarTopY), Cars),
			is_inside_board((CarLeftX, NewCarTopY), Board),
			NewCarBottomY is CarBottomY - 1,
			NewCar = car(CarID, (CarLeftX, NewCarTopY), (CarRightX, NewCarBottomY)),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp, [NewCar], NewCars).
			
	% Can't Move Car Up, so puts it back at end of list	
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_vertical(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarTopY is CarTopY - 1,
			is_car_cell((CarLeftX, NewCarTopY), Cars),
			is_inside_board((CarLeftX, NewCarTopY), Board),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp,[car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))], NewCars).
			
	% Moving Car Down and deleting old position
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_vertical(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarTopY is CarBottomY + 1,
			\+ is_car_cell((CarLeftX, NewCarTopY), Cars),
			is_inside_board((CarLeftX, NewCarTopY), Board),
			NewCarBottomY is CarTopY + 1,
			NewCar = car(CarID, (CarLeftX, NewCarBottomY), (CarRightX, NewCarTopY)),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp, [NewCar], NewCars).
			
	% Can't	Move Car Down, so puts it back at end of list
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_vertical(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarTopY is CarBottomY + 1,
			is_car_cell((CarLeftX, NewCarTopY), Cars),
			is_inside_board((CarLeftX, NewCarTopY), Board),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp,[car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))], NewCars).
			
	% Moving Car Left and deleting old position
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_horizontal(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarLeftX is CarLeftX - 1,
			\+ is_car_cell((NewCarLeftX, CarTopY), Cars),
			is_inside_board((NewCarLeftX, CarTopY), Board),
			NewCarRightX is CarRightX - 1,
			NewCar = car(CarID, (NewCarLeftX, CarTopY), (NewCarRightX, CarBottomY)),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp, [NewCar], NewCars).
			
	% Can't Move Car Left, so puts it back at the end of list
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_horizontal(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarLeftX is CarLeftX - 1,
			is_car_cell((NewCarLeftX, CarTopY), Cars),
			is_inside_board((NewCarLeftX, CarTopY), Board),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp,[car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))], NewCars).
			
	% Moving Car Right and deleting old position
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_horizontal(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarLeftX is CarRightX + 1,
			\+ is_car_cell((NewCarLeftX, CarBottomY), Cars),
			is_inside_board((NewCarLeftX, CarBottomY), Board),
			NewCarRightX is CarLeftX + 1,
			NewCar = car(CarID, (NewCarRightX, CarTopY), (NewCarLeftX, CarBottomY)),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp, [NewCar], NewCars).	
			
	% Can't	Move Car Right, so puts it back at end of list
		move_car(Cars, NewCars, Board) :-
			Cars = [car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))|Rest],
			is_horizontal(car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))),
			NewCarLeftX is CarRightX + 1,
			is_car_cell((NewCarLeftX, CarBottomY), Cars),
			is_inside_board((NewCarLeftX, CarBottomY), Board),
			delete(Cars, car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY)), Temp),
			append(Temp,[car(CarID, (CarLeftX, CarTopY), (CarRightX, CarBottomY))], NewCars).	
		
% All Tests for Valid Movement Coordinates Defined Below -----------------------------------------------------

	% Testing for Verticle Cars
		is_vertical(car(_, Start, End)) :-
			(CarLeftX, _) = Start,
			(CarRightX, _) = End,
			CarLeftX = CarRightX.
			
	% Test for Horizontal Cars
		is_horizontal(car(_, Start, End)) :-
			(_, CarTopY) = Start,
			(_, CarBottomY) = End,
			CarTopY = CarBottomY.
			
	% Test for whether or not the new cell is in a car (for Verticle)
		is_within_car(car(_, Start, End), (NewCarLeftX, NewCarTopY)) :-
			is_vertical(car(_, Start, End)),
			(CarLeftX, CarTopY) = Start,
			(_, CarBottomY) = End,
			CarLeftX = NewCarLeftX,
			NewCarTopY >= CarTopY,
			NewCarTopY =< CarBottomY.
			
	% Test for whether or not the new cell is in a car (for Horizonal)
		is_within_car(car(_, Start, End), (NewCarLeftX, NewCarTopY)) :-
			is_horizontal(car(_, Start, End)),
				(CarLeftX, CarTopY) = Start,
				(CarRightX, _) = End,
				CarTopY = NewCarTopY,
				NewCarLeftX >= CarLeftX,
				NewCarLeftX =< CarRightX.
				
	% Test for every car in search of new coordinate
		is_car_cell(Cell, [Car|_]) :-
			is_within_car(Car, Cell),
				!.
				
	% Test for new coordinate that's inside the grid
		is_inside_board((NewCarLeftX, NewCarTopY), board(Width, Height, _, _, _)) :-
			NewCarLeftX >= 0,
			NewCarLeftX < Width,
			NewCarTopY >= 0,
			NewCarTopY < Height.					
	
% General Iterator for moving 1st car in list to last car in list ------------------------------
is_car_cell(Cell, [_|Cars]) :-
	is_car_cell(Cell, Cars).