cd( @__DIR__ )
include( "room_functions.jl" );
include( "шагаем.jl" );
include( "get_path.jl" );

gr()

using Random
Random.seed!(3);

function поворот( a::CartesianIndex, b::Number )
    vecA = [ a[1], a[2] ];
    rotM = [  cosd(b) sind(b)
             -sind(b) cosd(b) ]
    CartesianIndex( (Int.(round.(rotM * vecA )))...)
end;

##################################
# Инициализация переменных
##################################

карта, старт_x, старт_y = gen_room_rend( 25, 20 )
старт = CartesianIndex( старт_x, старт_y )
положение = CartesianIndex( старт )
направление = CartesianIndex(1,0)

маршрут = []
путь = [ Tuple(старт) ];
изученная_карта = не_знаю .* ones( size(карта) ); # Теперь используем эту матрицу по назначению
изученная_карта[ старт ] = можно_ехать; # Робот определенно находится в точке, где нет стены
пройденная_карта = не_посещали .* ones( size(карта) );


a = Plots.Animation()
plt = plot();
i = 0

for i = 1:10000
    global plt
    положение, направление, маршрут, изученная_карта, пройденная_карта, конец_процедуры = шагаем( положение, направление, маршрут, изученная_карта, пройденная_карта, карта )
    
    if конец_процедуры == false push!(путь, Tuple(положение)); else break; end;
    
    plt = heatmap( ones(size(карта')), yflip=:true, aspect_ratio=:equal, leg=:false )
    spy!( plt, replace(изученная_карта' .== не_знаю, 0=>NaN), markersize=3, markercolor=:gray ) # c=:tab20, 
    heatmap!(plt, replace(изученная_карта', 1=>NaN) .- 2, yflip=:true, c=:greys, aspect_ratio=:equal, leg=:false );
    plot!( plt, first.(путь), last.(путь), aspect_ratio=:equal, lw=2, linez=range(0.0, stop=1.0, length=length(путь)), c=:batlow )
    if length(маршрут) > 0 scatter!( plt, [маршрут[1][1]], [маршрут[1][2]], markersize=5, markerstrokewidth=5, markercolor=:red, shape=:xcross ); end;
    
    # Отразим направление робота на графике треугольником
    if направление == CartesianIndex(1,0) scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:rtriangle, c=:red );
    elseif направление == CartesianIndex(0,1) scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:dtriangle, c=:red );
    elseif направление == CartesianIndex(-1,0) scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:ltriangle, c=:red );
    else scatter!( plt, [положение[1]], [положение[2]], markersize=7, shape=:utriangle, c=:red ); end;
    
    frame(a, plt)
end


# И повторим последний кадр 50 раз
for i = 1:50
    frame(a, plt)
end


gif( a, "robot_animation.gif", fps=10 )

plot( heatmap( изученная_карта, title="изученная_карта"), heatmap( пройденная_карта, title="пройденная_карта" ), size=(1100,400))

# engee.script.edit("/user/start/examples/base_simulation/engee_function_basics/engee_function_basics.ngscript")