:- consult('generate_maze.pl').


drawScreen(Maze) :- drawScreen(Maze, 0.15).
drawScreen(Maze, Duration) :- write('\e[H\e[2J'), sleep(Duration), bdisplay_maze(Maze).

solve_maze(Maze):-
    solve_maze(Maze, naive).
solve_maze(Maze, Style):-
    find_start_end_point(Maze, Start, End),
    solve_maze(Maze,Style, Start, End),
    !.


% Start and end are defined by the single empty space that exists in the top and bottom row
find_start_end_point(Maze, X1-1, X2-Y2):-
    functor(Maze, _, Y2),
    arg(1,Maze, FirstRow),
    arg(X1,FirstRow, ' '),
    arg(Y2,Maze, LastRow),
    arg(X2, LastRow, ' ').

solve_maze(Maze, naive, Start, End):-
    solve_maze_naive(Maze, Start, End).

solve_maze(Maze, ast, Start, End):-
solve_maze_astar(Maze, Start, End, []).

solve_maze_naive(Maze ,EndPos, EndPos):-
    mark_pos(Maze, EndPos, o),
    drawScreen(Maze).
    %!.

% -> mark_pos(Maze, CurrentPos, 'o'), drawScreen(Maze,0.1);  fail).
% try right>left>up>down

solve_maze_naive(Maze, CurrentPos, EndPos):-
    CurrentPos == EndPos, !.

solve_maze_naive(Maze, CurrentPos, EndPos):-
    write('['),
    write(CurrentPos),
    write(']'),
    mark_pos(Maze, CurrentPos, 'x'),
    drawScreen(Maze),
    find_next_space(Maze, CurrentPos, NextPos),
    solve_maze_naive(Maze, NextPos, EndPos).

solve_maze_astar( Maze, EndPos, EndPos, _):-
    mark_pos(Maze, EndPos, o), drawScreen(Maze).

solve_maze_astar( Maze, CurrentPos, EndPos, Stack):-
    \+ CurrentPos == EndPos,
    mark_pos(Maze, CurrentPos, 'x'),
    drawScreen(Maze),
    a_star(Maze, CurrentPos, EndPos, Stack, TmpStack),
    pop_stack(TmpStack, NextPos, NewStack),
    solve_maze_astar(Maze, NextPos, EndPos, NewStack),
    (
    %get_mark(Maze, NextPos, o), euclid_distance(CurrentPos, NextPos, 1.0)
    has_correct_neighbour(Maze, CurrentPos) ->
    mark_pos(Maze, CurrentPos, 'o'), drawScreen(Maze, 0.1); true).


pop_stack([_-NewPos|Stack], NewPos, Stack).

a_star(Maze, Pos, Goal, Stack, NewStack):-
    a_star_direction(Maze, Pos, Goal, DistancePos),
    append(Stack, DistancePos, TmpStack),
    keysort(TmpStack, NewStack).


euclid_distance(X1-Y1, X2-Y2, Distance):-
    Distance is sqrt((X1 - X2)**2 + (Y1 - Y2) ** 2).

a_star_direction(Maze, Pos, Goal, DistPos):-
    findall(Distance-NextPos, (find_next_space(Maze,' ', _ ,Pos,NextPos), euclid_distance(NextPos, Goal, Distance)), DistPos).

has_correct_neighbour(Maze, X-Y):-
    find_next_space(Maze, o ,_, X-Y, _).

find_next_space(Maze, Pos, NextPos):-
    find_next_space(Maze, ' ', _, Pos, NextPos).

find_next_space(Maze, Kind, Pos, NextPos):-
    find_next_space(Maze, Kind, _, Pos, NextPos).


find_next_space(Maze, Kind, right ,X0-Y0, X1-Y0):-
    succ(X0,X1),
    get_mark(Maze, X1-Y0, Kind).

find_next_space(Maze, Kind, left ,X0-Y0, X1-Y0):-
    succ(X1,X0),
    get_mark(Maze, X1-Y0, Kind).

find_next_space(Maze, Kind, up ,X0-Y0, X0-Y1):-
    succ(Y0,Y1),
    get_mark(Maze, X0-Y1, Kind).

find_next_space(Maze, Kind, down ,X0-Y0, X0-Y1):-
    succ(Y1,Y0),
    get_mark(Maze, X0-Y1, Kind).

get_mark(Maze, X-Y, Mark):-
    write('('),
    write(X-Y),
    write(')'),
    arg(Y, Maze, Row),
    arg(X, Row, Mark).

mark_pos(Maze, X-Y, Mark):-
    arg(Y, Maze, Row),
    nb_setarg(X, Row, Mark).

% display maze by creating a string that is printed on the terminal at once
bdisplay_maze(Maze):-
    functor(Maze, _, Height),
    nl,
    bdisplay_maze(Maze, Height, 1, "", MazeString),
    write(MazeString).

bdisplay_maze(Maze, Height, Index, Acc, String):-
    Index =< Height ->
        arg(Index, Maze, Row),
        Row =.. [r|List],
        format(string(Acc1), "~w~n",[List]),
        string_concat(Acc,Acc1,Acc2),
        succ(Index, NextIndex),
        bdisplay_maze(Maze, Height, NextIndex, Acc2, String);
        String = Acc.

% Display Graph

show_maze(Maze):-
    show_maze(Maze, naive).
show_maze(Maze, Style):-
    find_start_end_point(Maze, Start, End),
    show_maze(Maze,Style, Start, End),!.

show_maze(Maze, naive, Start, End):-
    show_maze_naive(Maze, Start, End).


show_maze_naive(Maze ,EndPos, EndPos):-
    mark_pos(Maze, EndPos, o),
    !.

% -> mark_pos(Maze, CurrentPos, 'o'), drawScreen(Maze,0.1);  fail).
% try right>left>up>down

show_maze_naive(Maze, CurrentPos, EndPos):-
    \+ CurrentPos == EndPos,
    write('['),
    write(CurrentPos),
    write(']'),
    mark_pos(Maze, CurrentPos, 'x'),
    %drawScreen(Maze),
    find_next_space(Maze, CurrentPos, NextPos),
    show_maze_naive(Maze, NextPos, EndPos).