5. Алгоритм навигации робота

using Graphs, GraphRecipes

# Простой код для создания графа:
g = Graph()
add_vertices!( g, 3 )
add_edge!( g, 1, 2 )
add_edge!( g, 1, 3 )
graphplot( g, names=1:nv(g) )

куда_можно_идти = изученная_карта .== можно_ехать
heatmap( куда_можно_идти )

индексы_доступных_точек = LinearIndices( изученная_карта ) .* куда_можно_идти  # Отберем только индексы тех площадок карты, куда можно наступить

count( индексы_доступных_точек .> 0)

индексы_доступных_точек = reshape( индексы_доступных_точек, 1, : )          # Растянем их в вектор 
только_индексы = индексы_доступных_точек[ индексы_доступных_точек .> 0 ]    # Отсеим из вектора нулевые элементы

координаты_всех_точек_карты = CartesianIndices( size(изученная_карта) )     # Матрица с координатами каждой площадки

координаты_точек_графа = Dict(zip( 1:length(только_индексы), координаты_всех_точек_карты[только_индексы] )) # Этот словарь возвращает координату на карте для той или иной вершины графа
индексы_точек_по_координатам = Dict(value => key for (key, value) in координаты_точек_графа); # Это инвертированный словать: из координаты в индекс вершины графа

first( sort(collect(координаты_точек_графа)), 5 )

first( sort(collect(индексы_точек_по_координатам)), 5 )

xy = Tuple.(first.(sort(collect(индексы_точек_по_координатам))));
xx = first.(xy);
yy = last.(xy);

MAX_X,MAX_Y = size( изученная_карта )
какие_связи_добавить_в_граф = []
g = Graph()

add_vertices!( g, length(координаты_всех_точек_карты[ куда_можно_идти ][:]) ) # Добавляем много вершин в граф

for вершина in 1:nv(g)
    x,y = Tuple(координаты_точек_графа[вершина])
    if x == MAX_X || y == MAX_Y continue; end;

    точка_справа = CartesianIndex( x + 1, y )
    точка_снизу = CartesianIndex( x, y + 1 )
    
    # Соединим текущую точку с точкой справа, если есть связанность согласно карте пройденных точек 
    if изученная_карта[точка_справа] == можно_ехать add_edge!(g, вершина, индексы_точек_по_координатам[точка_справа]); end;
    # То же самое для точки снизу
    if изученная_карта[точка_снизу] == можно_ехать add_edge!(g, вершина, индексы_точек_по_координатам[точка_снизу]); end;
end

g

heatmap( изученная_карта', yflip=:true, title="Изученная карта\n(неизведано/стенка/посетили)", titlefont=font(9), leg=:false )
graphplot!( g, x = xx, y = yy, curves=true, nodeshape=:circle, yflip=:true, size=(1000,400) )

path = Tuple( a_star( g, индексы_точек_по_координатам[CartesianIndex(2,2)], индексы_точек_по_координатам[CartesianIndex(24,19)] ) )
points = [ ( координаты_точек_графа[src(v)][1], координаты_точек_графа[dst(v)][2] ) for v in path ]
points

heatmap( изученная_карта', yflip=:true, title="Изученная карта\n(1 - неизведано, 2 - стенка, 3 - посетили)", titlefont=font(6) )
graphplot!( g, x = xx, y = yy, curves=true, nodeshape=:circle, yflip=:true, size=(1000,400) )
plot!( first.(points), last.(points), lw=3, c=:red )