:- use_module(library(pio)).
:- use_module(library(dcg/basics)).
:- initialization(main, main).
% :- table cost/3.
% :- table to_key/2.

main([FileName|_]) :-
    input(FileName, Map),
    solve(Map, N, Gs, Width, Height),
    dict_pairs(Gs, _, GList), transpose_pairs(GList, Gx), max_member(Max-_, Gx),
    visualize(Width, Height, Gs, Max, OutMap),
    print(OutMap),
    write(N), write(" - "), write(Max), nl.

solve(Map, N, EndGs, Width, Height) :-
    length(Map, Height), EndX is Height - 1,
    Start1 = 0-0-horz, Start2 = 0-0-vert,
    to_key(Start1, K1), to_key(Start2, K2), Gs = cost{}.put(K1, 0).put(K2, 0),
    Map = [Row0|_], length(Row0, Width), EndY is Width - 1,
    heur(0-0, EndX-EndY, Heur0),
    list_to_heap([Heur0-Start1, Heur0-Start2], Queue),
    astar(Map, Gs, Queue, EndX-EndY, N, EndGs).

from_key(Key, X-Y-Dir) :-
    X is Key div 10000,
    Y is (Key mod 10000) div 10,
    (Key mod 2 =:= 0 -> Dir = horz; Dir = vert).

replace(I, List, E, NewList) :- nth0(I, List, _, R), nth0(I, NewList, E, R).

display(0, "  ", _).
display(G, "▗ ", Max) :- 0 < G, G =< (Max div 4).
display(G, "▝▖", Max) :- (Max div 4) < G, G =< 2*(Max div 4).
display(G, "▞▖", Max) :- 2*(Max div 4) < G, G =< 3*(Max div 4).
display(G, "▞▞", Max) :- 3*(Max div 4) < G, G =< 4*(Max div 4).
display(G, "▟▞", Max) :- 4*(Max div 4) < G, G =< 5*(Max div 4).
display(G, "▟▛", Max) :- 5*(Max div 4) < G, G =< 6*(Max div 4).
display(G, "█▛", Max) :- 6*(Max div 4) < G, G =< 7*(Max div 4).
display(G, "██", Max) :- 7*(Max div 4) < G.

put_g(Key-G, OldMap, NewMap) :-
    from_key(Key, X-Y-_),
    nth0(X, OldMap, OldRow), nth0(Y, OldRow, OldG), NewG is G + OldG,
    replace(Y, OldRow, NewG, NewRow),
    replace(X, OldMap, NewRow, NewMap).

visualize(Width, Height, Gs, Max, OutMap) :-
    length(Row, Width), maplist(=(0), Row),
    length(Map, Height), maplist(=(Row), Map),
    dict_pairs(Gs, _, GList),
    foldl(put_g, GList, Map, GMap),
    maplist(
        {Max}/[OldRow, NewRow]>>(
            maplist(
                {Max}/[G, Unicode]>>(
                    display(G, Unicode, Max)), OldRow, NewRow)),
        GMap, OutMap).

astar(_Map, Gs, PQueue, DestX-DestY, N, Gs) :-
    get_from_heap(PQueue, _, X-Y-From, _), X = DestX, Y = DestY,
    to_key(DestX-DestY-From, FromKey), N = Gs.FromKey, !.

astar(Map, Gs, PQueue, DestX-DestY, N, EndGs) :-
    get_from_heap(PQueue, _, X-Y-From, PQueueAfterPop),
    to_key(X-Y-From, CurrentKey), CurrentG = Gs.CurrentKey,

    findall(To-Cost, next(Map, X-Y-From, To, Cost), Neighbors),
    foldl(add_neighbor(CurrentG, DestX-DestY),
          Neighbors, PQueueAfterPop-Gs, NewPQueue-NewGs),
    astar(Map, NewGs, NewPQueue, DestX-DestY, N, EndGs).

add_neighbor(CurrentG, DestX-DestY, (X-Y-Dir)-Cost, HeapIn-GsIn, HeapOut-GsOut) :-
    NewGCandidate is CurrentG + Cost,
    to_key(X-Y-Dir, Key), ExistingG = GsIn.get(Key, 9999999),
    (   NewGCandidate < ExistingG
    ->  NewG = NewGCandidate, GsOut = GsIn.put(Key, NewG),
        heur(X-Y, DestX-DestY, Heur), Weight is NewG + Heur,
        (   get_from_heap(HeapIn, _, X-Y-Dir, _), HeapOut = HeapIn
        ;   add_to_heap(HeapIn, Weight, X-Y-Dir, HeapOut)
        )
    ;   GsOut = GsIn, HeapOut = HeapIn
    ).

to_key(X-Y-horz, Key) :- Key is 0 + Y*10 + X* 10000.
to_key(X-Y-vert, Key) :- Key is 1 + Y*10 + X* 10000.

next(Map, X-Y-horz, NextX-Y-vert, Cost) :-
    LowX1 is X - 3, HighX1 is X - 1,
    LowX2 is X + 1, HighX2 is X + 3,
    (between(LowX1, HighX1, NextX); between(LowX2, HighX2, NextX)),
    vert(Map, X-Y, NextX, Cost).
next(Map, X-Y-vert, X-NextY-horz, Cost) :-
    LowY1 is Y - 3, HighY1 is Y - 1,
    LowY2 is Y + 1, HighY2 is Y + 3,
    (between(LowY1, HighY1, NextY); between(LowY2, HighY2, NextY)),
    horz(Map, X-Y, NextY, Cost).

vert(Map, X-Y, NextX, Cost) :-
    cost(Map, NextX-Y, _),
    findall(
        Ci,
        (   (X < NextX -> between(X, NextX, Xi);  between(NextX, X, Xi)),
            Xi =\= X,
            cost(Map, Xi-Y, Ci)),
        Costs),
    sum_list(Costs, Cost).
    % Cost = Costs.

horz(Map, X-Y, NextY, Cost) :-
    cost(Map, X-NextY, _),
    findall(
        Ci,
        (   (Y < NextY -> between(Y, NextY, Yi);  between(NextY, Y, Yi)),
            Yi =\= Y,
            cost(Map, X-Yi, Ci)),
        Costs),
    sum_list(Costs, Cost).
    % Cost = Costs.


heur(X1-Y1, X2-Y2, Dist) :- Dist is abs(X1 - X2) + abs(Y1 - Y2).

cost(Map, X-Y, Cost) :- nth0(X, Map, Row), nth0(Y, Row, Cost).

print(Map) :- maplist([X]>>(atomic_list_concat(X, N), write(N), nl), Map).

input(Name, Map) :- phrase_from_file(lines(Map), Name).
lines([]) --> eos, !.
lines([Nums|Lines]) -->
    line(Line), lines(Lines),
    {maplist([C, N]>>(N is C - 48), Line, Nums)}.
line(Nums) --> digits(Nums), "\n".