Моделирование полета дрона (облет ориентиров)

]add LinearAlgebra

]add Colors

using LinearAlgebra, Colors
Plots.PlotlyJSBackend()
const deltaH_mark=10

const θcam = 30
const steps_around_group = 24
const max_steps_around = 72
const min_radius_flyby = 1.0
const plateau = 10
const MaxSteps = 1000
const pract_ceiling = 33.0
const minh_over_terrain = 3.0
const descend_speed = 2.0
const steps_descend = 10

function generate_landscape(width_m, length_m)
    X = 0:1:width_m
    Y = 0:1:length_m

    Z = [10(sin(xi / 10) + cos(yi / 15)) + 5rand() + plateau for xi in X, yi in Y]
    Z = reverse(Z, dims=(1, 2))
    
    random_yi_start = rand(1:length(Y))
    random_yi_finish = rand(1:length(Y))
    xi_start = 10
    xi_finish = length(X)-10
    Start = [X[xi_start], Y[random_yi_start], Z[xi_start, random_yi_start] + minh_over_terrain]
    Finish = [X[xi_finish], Y[random_yi_finish], Z[xi_finish, random_yi_finish]]

    plt1 = surface(X, Y, Z', c=:terrain, xlabel="X (м)", ylabel="Y (м)", zlabel="Z (м)",
                            title="3D ландшафт", size=(800,800), legend=false, camera = (15, 85))
    scatter!(plt1, [Start[1]], [Start[2]], [Start[3]+deltaH_mark], marker=(:diamond), markersize=5, color=:orange)
    scatter!(plt1, [Finish[1]], [Finish[2]], [Finish[3]+deltaH_mark], marker=(:xcross), markersize=5, color=:red)
#    println(typeof(plt1))
    landscape = X, Y, Z, Start, Finish
#    return x, y, z, start, finish, plt
    return landscape, plt1
end

function generate_refpoints(ref_count, plt, ls_x, ls_y, ls_z,
                            refstartX=last(ls_x)÷5, refstartY=last(ls_y)÷5,
                            refendX=last(ls_x)-refstartX, refendY=last(ls_y)-refstartY)
    RefPoints = Tuple{Int64, Int64, Float64}[]
    
    for _ in 1:ref_count
        xi = rand(refstartX:refendX)
        yi = rand(refstartY:refendY)
        zi = ls_z[xi, yi]
        push!(RefPoints, (xi, yi, zi))
        scatter!(plt, [xi], [yi], [zi+deltaH_mark], marker=(:circle), color=:yellow)
    end
#    println("refpoints:", refpoints)
#    println("Type of refpoints:", typeof(refpoints))
    return RefPoints
end

function put_start_finish(X, Y, Z, plt)
    random_yi_start = rand(1:length(Y))
    random_yi_finish = rand(1:length(Y))
    xi_start = 10
    xi_finish = length(X)-10
    Start = [X[xi_start], Y[random_yi_start], Z[xi_start, random_yi_start]]
    Finish = [X[xi_finish], Y[random_yi_finish], Z[xi_finish, random_yi_finish]]
    scatter!(plt, [Start[1]], [Start[2]], [Start[3]+3], marker=(:circle), color=:green)
    scatter!(plt, [Finish[1]], [Finish[2]], [Finish[3]+3], marker=(:circle), color=:red)
    display(plt)
    return Start, Finish
end

LScape, plt = generate_landscape(150, 150)
Xls, Yls, Zls, Start, Finish = LScape
maxdepth = Base.minimum(Zls)
maxheight = Base.maximum(Zls)
RefPts = generate_refpoints(45, plt, Xls, Yls, Zls)
#println("Type of refs:", typeof(RefPts))
display(plt)
println("max_h=", maxheight)
println("max_d=", maxdepth)

function camera_beam(x0, y0, z0, θ)
    n=128
    
    cone_h = z0-maxdepth
    tanθ = 0
#    try
    if (Base.abs(θ) < 89.0)
        tanθ = tand(θ)
    else
#    catch
#        println("Поймана ошибка: ", e)
        return nothing
    end

    cone_r = cone_h*tanθ

    cone_base = LinRange(0, cone_r, n)
    z = LinRange(0, 1, n)
#    z = LinRange(0, cone_h, n)
    ϕ = LinRange(0, 2π, n)
    
    println("cone_h=", cone_h)
    println("cone_r=", cone_r)
    println("cone_base=", cone_base)
    println("z=", z)

    x1 = x0.+ [u * cone_r * cos(v) for u in z, v in ϕ]
    y1 = y0.+ [u * cone_r * sin(v) for u in z, v in ϕ]
    z1 = z0.+ [-cone_h*u for u in z, _ in ϕ]
#    z1 = z0.+ [-u for u in z, _ in ϕ]
    cone_plt = Plots.surface(x1, y1, z1, c=:jet, legend=false, size=(600,600), cbar=:none)
#    return z1
end

#cone_surf_z = camera_beam(30,40,50,θcam)

using Interpolations

function calc_refpoints_visible(ownposition, Z_ls, θ, Refs::AbstractVector{<:Tuple})
    n = 256
    
    x0, y0, z0 = ownposition
    z = LinRange(maxheight, maxdepth, n)
    θ = Base.abs(θ)

    refpts_detected = Tuple{Int64, Int64}[]
    pt_cntr = 0
    for t in Refs
#        println("Ref[]= ", t)
        Sref = sqrt((t[1] - x0)^2 + (t[2] - y0)^2)
#        println("Sref=", Sref)

        if (z0 < t[3])  # if drone position is lower than z-coordinate of a refpoint
            continue    # then don't take this refpoint
        end
        
        tanψ = +Inf
        try
            tanψ = Sref / (z0-t[3])
        catch
            continue
        end
        
#        println("tanψ=",tanψ)
        if (Base.abs(tanψ) < tand(θ)) && (Sref > 0)
            cosϕ = (t[1]-x0) / Sref
            sinϕ = (t[2]-y0) / Sref

#            println("cosϕ=", cosϕ)
            for i in 1:n
#                println("z[i]=", z[i])
                Sk = (z0 - z[i]) * tanψ
                xk = x0 + Sk*cosϕ
                yk = y0 + Sk*sinϕ
                xk1 = trunc(Int64, floor(xk))
                xk2 = trunc(Int64, ceil(xk))
                yk1 = trunc(Int64, floor(yk))
                yk2 = trunc(Int64, ceil(yk))
#                println(xk1, " ", yk1)
#                println(xk2, " ", yk2)

                Vicinity = [Z_ls[xk1, yk1] Z_ls[xk2, yk1];
                            Z_ls[xk1, yk2] Z_ls[xk2, yk2]]
                itp = interpolate(Vicinity, BSpline(Linear()))
                if (xk1 == xk2) && (yk1 == yk2)
                    idxX = 1
                    idxY = 1
                elseif (xk1 == xk2)
                    idxX = 1
                    idxY = 1+Base.abs((yk-yk1)/(yk2-yk1))
                elseif (yk1 == yk2)
                    idxY = 1
                    idxX = 1+Base.abs((xk-xk1)/(xk2-xk1))
                else
                    idxX = 1+Base.abs((xk-xk1)/(xk2-xk1))
                    idxY = 1+Base.abs((yk-yk1)/(yk2-yk1))
                end

                vzLS = itp(idxX, idxY)
                
                if (z[i] <= vzLS)
#                    println("(xk1,yk1):", xk1, " ", yk1)
#                    println("(xk2,yk2):", xk2, " ", yk2)

                    dist = sqrt((xk - t[1])^2 + (yk - t[2])^2)
                    if (dist < sqrt(2))
                        push!(refpts_detected,(t[1], t[2])) #store (x,y) coordinates of a refpoint
#                        println("--> pt_cntr[i]: ", pt_cntr, "[", i, "]", " caught refpt[", t[1],",", t[2], "]")
                        pt_cntr += 1
                    else
#                        println("xk=", xk)
#                        println("yk=", yk)
#                        println("dist = ", dist)
                        println("Landscape hedge blocks a refpt[$(t[1]),$(t[2])]\n")
                    end
                    
#                    println("Vicinity=", Vicinity)
#                    println("vzLS=", vzLS)
#                    println("z[i]=", z[i])
#                    println("\n")
                    break
                end
            end
        else
#            println("Contitions are not met!\n")
        end
    end
    return refpts_detected
end

function Calc_Radius_Increment( radius, curr_pos, RefCenter, RefPts::AbstractVector{<:Tuple} )
    radius_flyby_inc = 0.0
    curr_posX, curr_posY = curr_pos
    ref_cenX, ref_cenY = RefCenter
    
    RefCen_vec = [ref_cenX - curr_posX, ref_cenY - curr_posY]

#    RefPts_XY = first.(RefPts)
    
    for t in RefPts
        ref_point = first.(t)
#        println("ref_point=", ref_point)
        EachRefPt_vec = [ref_point[1] - curr_posX, ref_point[2] - curr_posY]
        dot_pr = dot(RefCen_vec, EachRefPt_vec)
        if (dot_pr < 0)
            radius_flyby_inc = 1.2 * radius / steps_around_group
            println("radius flyby increment", radius_flyby_inc)
            break
        end
    end
    
    return radius_flyby_inc
end

function calc_Distances( arrA::AbstractVector{<:Tuple}, pos )
    Distances = sqrt.( (first.(arrA) .- first(pos)).^2 + (last.(arrA) .- last(pos)).^2 )
    return Distances
end

function calcGroup_CenterXY(Pts_group::AbstractVector{<:Tuple})
    radius = 0.0
    Group_Center = Tuple{Float64, Float64}
    
    if !isempty(Pts_group)
        RefPts_grp_xy = first.(Pts_group) # extract x,y coorduinates
        RefPts_grp_x = map(t -> t[1], RefPts_grp_xy)
        RefPts_grp_y = map(t -> t[2], RefPts_grp_xy)
        Group_Center = (sum(RefPts_grp_x)/length(RefPts_grp_xy),
                         sum(RefPts_grp_y)/length(RefPts_grp_xy)) # found a geometric center of a group of points or one point

        if length(Pts_group) > 1 # fly around a group, calculate radius of flying around
            println("<c> Group_Center=", Group_Center)
#            println("<c> first.(Pts_group)=", first.(Pts_group))
                
            tmp = first.(Pts_group)
            RefPts_distances = sqrt.( (first.(tmp) .- first(Group_Center)).^2 +
                                      (last.(tmp) .- last(Group_Center)).^2 )
                
#            println("<c> RefPts_distances=", RefPts_distances)
            radius = 1.1 * maximum(RefPts_distances)
            println("<c> flying radius=", radius)
        end
#    else
#        println("ERROR array Pts_group size")
    end
    
    return radius, Group_Center
end

function Get_Refpts_byRefGroup( AllRefPts::AbstractVector{<:Tuple}, group )
    RefPts_grp = Tuple{Tuple{Int64, Int64}, Int64}[] #  === DEBUG === option for get a group quickly
    RefPts_center = Tuple{Float64, Float64}
    
    flying_radius = 0.0
#    println("AllRefPts=", AllRefPts)
#    println("get by group:", group)

    if !isempty(AllRefPts)
        for p in AllRefPts
            if last(p) == group  # if the group of RefPts[i] is equal to given
                push!(RefPts_grp, p)
            end
        end

        flying_radius, RefPts_center = calcGroup_CenterXY(RefPts_grp)
#        println("RefPts_grp = ", RefPts_grp, "\n")
    end

    return RefPts_grp, RefPts_center, flying_radius
end

function Get_Refpts_byRefPts( AllRefPts::AbstractVector{<:Tuple}, Detected_Refs::AbstractVector{<:Tuple}, Flown_gr::AbstractVector{<:Int64}, curr_pos )
    RefPts_grp = Tuple{Tuple{Int64, Int64}, Int64}[] #  === DEBUG === option for get a group quickly
    RefPts_center = Tuple{Float64, Float64}

    flying_radius = 0.0
#    println("AllRefPts=", AllRefPts)
    
    if !isempty(AllRefPts)
        RefPts_XY = first.(AllRefPts)

        for p in Detected_Refs
            if (p in RefPts_XY)
#                println("Get by coords: ", p)
                idx = findall(x -> x == p, RefPts_XY)
#                println("idx=", idx)

                if (length(idx) == 1) # each detected point must have been added earlier in one group only!
                    group = last(AllRefPts[idx[1]])
                    
                    if isempty(Flown_gr)
                        point = (p, last(AllRefPts[idx[1]]) )
                        push!( RefPts_grp, point )
                    elseif !(group in Flown_gr)
                        point = (p, last(AllRefPts[idx[1]]) )
                        push!( RefPts_grp, point )
                    end
                else
                    println("!!! ERROR of Refpoints Group numbering")
                end

            end
        end

        if ( length(RefPts_grp) > 1 )
#            println("RefPts_grp=", RefPts_grp)
            Groups = last.(RefPts_grp)
            elem1 = Groups[1]
#            println("------->Groups = ", Groups)
           
            if !all(x -> x==elem1, Groups) # if there are different groups in array, then keep only the clothest point and its group
                Distances = calc_Distances(first.(RefPts_grp), curr_pos)
#                println("Distances = ", Distances)
                
                idx = argmin(Distances)
                
                grp = last(RefPts_grp[idx])
                RefPts_grp = filter!( x -> (last(x)==grp), RefPts_grp)
#            else
#                println("ALL ELEMENTS ARE EQUAL!")
            end
        end
        
        flying_radius, RefPts_center = calcGroup_CenterXY(RefPts_grp)
        
#        if !isempty(RefPts_grp)
#            println("RefPts_grp = ", RefPts_grp, "\n")
#        end
    end

    return RefPts_grp, RefPts_center, flying_radius
end

function simulate_drone(Landscape, h_over_lscape, service_ceiling, speed; fov_angle=45.0, view_distance=10.0)
    if (h_over_lscape * service_ceiling <= 0) || (service_ceiling <= plateau) # nonsense conditions
        return
    end
    X, Y, Z, Start, Finish = Landscape
    
    if (Start[3] > service_ceiling)
        println("!!! Flight started too high: ceiling = $(service_ceiling), started at $(Start[3])")
    end
    
    drone_path = [copy(Start)]
    drone_heights = [Start[3]]
    gnd_height_below = [Start[3]]
    obstacles = [[]]
    dir_around = [0.0, 0.0]
    direction = [0.0, 0.0]
    curr_direction = [0.0, 0.0]
    curr_speed = speed
    #stack_of_dir = []

    All_RefPts = Tuple{Tuple{Int64, Int64}, Int64}[]
    RefPts_Group = Tuple{Tuple{Int64, Int64}, Int64}[]
    RefPtsGr_center = Tuple{Float64, Float64}
    FlownGroups = Int64[]
    
    curr_group = ref_group = 0
    radius_flyby = 0.0
    radius_flyby_inc = 0.0
    min_refpt_dist = -Inf
    Φr = 0.0
    steps_around = 0
    
    current_pos = copy(Start)
    for _ in 1:MaxSteps
        if norm(current_pos[1:2] .- Finish[1:2]) < curr_speed
            break
        end

        direction = normalize(Finish[1:2] .- current_pos[1:2]) # always estimate targer direction
        Detected_Refs = calc_refpoints_visible(current_pos, Z, θcam, RefPts)
        
        if !isempty(Detected_Refs)  # if refpoints have been detected
            
            ref_points_xy = first.(All_RefPts)                     # extract coordinates x,y from array of All_RefPts detected before
            
            added_newpts = 0
            
            for i in 1:length(Detected_Refs)                       # by one check if coordinates of the detected points are already in All_RefPts
                if ((Detected_Refs[i] in ref_points_xy) == false)  # if these refpoints are new ones
                    added_newpts += 1
                    push!(All_RefPts, (Detected_Refs[i], ref_group+1)) # push them by one into All_RefPts with (ref_group+1) for all of them
                end
            end
            
            if (added_newpts > 0)
                ref_group += 1
            end
            
            if (curr_group == 0) # if fly towards the target, then can process detected points
                if (added_newpts > 0) # get new refpoints by their group
                    RefPts_Group, RefPtsGr_center, radius_flyby = Get_Refpts_byRefGroup(All_RefPts, ref_group)
                    
                    if (added_newpts != length(RefPts_Group))
                        println("!!! ERROR of extracting Group of refpoints")
                    end
                else # get refpoints that were former detected by coords (and get their group)
                    RefPts_Group, RefPtsGr_center, radius_flyby = Get_Refpts_byRefPts(All_RefPts, Detected_Refs, FlownGroups, (current_pos[1], current_pos[2]))
                end
            end #else continue our flying around detected group of refpoints
        else
#            println("RefPts_Group=", RefPts_Group)
        end

        if (curr_group == 0)
            
            next_pos = current_pos[1:2] .+ direction * curr_speed  # calc next_pos as we wouldn't have refpopints
            next_pos[1] = clamp(next_pos[1], X[1], X[end])
            next_pos[2] = clamp(next_pos[2], Y[1], Y[end])
        
            if (length(RefPts_Group) > 0)  # must fly around something
                dist_to_refcenter = sqrt( (current_pos[1] - RefPtsGr_center[1])^2 + (current_pos[2] - RefPtsGr_center[2])^2 )  # found distance to ref. center
            
                if (min_refpt_dist < 0)  # set the first actual value of distance from current position to the center of refpoints
                    min_refpt_dist = dist_to_refcenter
                elseif (dist_to_refcenter < min_refpt_dist) # if distance is decreasing, then approach as close as possible
                    min_refpt_dist = dist_to_refcenter # update the shortest distance to center
                else #the distance to ref. center is increasing again
                    if (length(RefPts_Group) == 1) # fly around a single refpoint
                        if (min_refpt_dist >= min_radius_flyby) # only if it is little aside from us, not too close
                            curr_group = last.(RefPts_Group)[1] # set a new curr_group, so this group becomes now the actually flying around
                            radius_flyby = min_refpt_dist
                            println("--> Start now flying around single refpoint ", RefPts_Group[1])
                        else # otherwise if we are too close to the center of refpoints/single refpoint
                            empty!(RefPts_Group) # clear the ignored reference point
                            min_refpt_dist = -Inf
                            println("--> Ignored a single refpoint - too close!")
                        end
                    else # otherwise if flying around a group of refpoints
                        if (radius_flyby <= min_radius_flyby)
                            empty!(RefPts_Group) # clear the ignored reference point
                            min_refpt_dist = -Inf
                            println("--> Ignored a group of refpoints - too close!")                        
                        else #if (dist_to_refcenter >= radius_flyby) # if the distance from current position to ref. center exceeded the flyby radius
                            curr_group = last.(RefPts_Group)[1] # set a new curr_group, so this group becomes now the actually flying around
                            
                            radius_flyby_inc = Calc_Radius_Increment(radius_flyby, (current_pos[1], current_pos[2]), RefPtsGr_center, RefPts_Group)
                            
                            println("--> Start now flying around a group of refpoints with center in ", RefPtsGr_center)
                        end
                    end
                end

                if (curr_group > 0) #if flying around is started, find isparameters
                    min_refpt_dist = -Inf #reset min distance to the ref. center
                    Φr = 2π / steps_around_group
                    
                    ref_cen = [RefPtsGr_center[1], RefPtsGr_center[2], 0]
                    dir_xy = [direction[1], direction[2], 0]
                    curr_pos_xy = copy(current_pos)
                    curr_pos_xy[3] = 0
                    
                    cross_product = cross(dir_xy, ref_cen .- curr_pos_xy)
                    if (cross_product[3] < 0)
                        Φr = -Φr
                        println(">>> Clockwise flight")
                    else
                        println("<<< Counterclockwise flight")
                    end
                    steps_around = 0
                end
            end
            curr_direction = direction
        end
        
        if (curr_group > 0)  # if fly around is started,
            radius_flyby += radius_flyby_inc

            curr_speed = 2π*radius_flyby / steps_around_group

            dir_around = [curr_direction[1]*cos(Φr) - curr_direction[2]*sin(Φr),
                          curr_direction[1]*sin(Φr) + curr_direction[2]*cos(Φr)]
            dir_around = normalize(dir_around)
            dot_product = dot(dir_around, direction)
#            println("dot_product=", dot_product)
            
            steps_around += 1
            next_pos = current_pos[1:2] .+ dir_around[1:2] * curr_speed  # calc next_pos as we wouldn't have refpopints
            next_pos[1] = clamp(next_pos[1], X[1], X[end])
            next_pos[2] = clamp(next_pos[2], Y[1], Y[end])
            curr_direction = dir_around

            if (norm(direction) * norm(curr_direction) - dot_product < 0.005) || (steps_around > max_steps_around) # about 5 degrees for cos(x)
                push!(FlownGroups, curr_group)
                curr_group = 0
                radius_flyby_inc = 0.0
                curr_speed = speed
                empty!(RefPts_Group)
                println("Flown around in $(steps_around) steps\n")
                steps_around = 0
                continue
            end
            
        end
        
        xi = argmin(abs.(X .- next_pos[1])) # find next position of drone on the grid
        yi = argmin(abs.(Y .- next_pos[2]))
        terrain_height = Z[xi, yi] # terrain height in the next planned position
        
        if (current_pos[3] >= service_ceiling) # still flying higher than our ceiling permits (e.g. started there)
#            new_z = min(current_pos[3], terrain_height + h_over_lscape)
            if (terrain_height + h_over_lscape < service_ceiling)
                new_z = terrain_height + h_over_lscape
            else
                new_z = service_ceiling
            end
#            new_z = clamp( max(current_pos[3], terrain_height + h_over_lscape), -Inf, service_ceiling )
        else
            new_z = clamp( min(terrain_height + h_over_lscape, current_pos[3]), -Inf, service_ceiling )
        end
        
        visible_obstacles = Tuple{Float64, Float64, Float64}[]
        
        for angle in range(-fov_angle/2, fov_angle/2, length=12)
            rad = deg2rad(angle)
            
            dir = [curr_direction[1]*cos(rad) - curr_direction[2]*sin(rad),
                   curr_direction[1]*sin(rad) + curr_direction[2]*cos(rad)]
            
            for b in range(1.0, view_distance, length=2*trunc(Int64, floor(view_distance)))
                check_pos = next_pos .+ dir * b
            
                xi_check = argmin(abs.(X .- check_pos[1]))
                yi_check = argmin(abs.(Y .- check_pos[2]))

                if 1 ≤ xi_check ≤ size(Z, 1) && 1 ≤ yi_check ≤ size(Z, 2)
                    obstacle_height = Z[xi_check, yi_check]
                
                    if obstacle_height > new_z # obstacle is higher than our ceiling
                        push!(visible_obstacles, (X[xi_check], Y[yi_check], obstacle_height))
                    end
                end
            end
        end

        if !isempty(visible_obstacles)
#            if (current_pos[3] >= service_ceiling)
#                min_obstacle = minimum(obs[3] for obs in visible_obstacles)
#            else
                max_obstacle = maximum(obs[3] for obs in visible_obstacles)
                min_obstacle = minimum(obs[3] for obs in visible_obstacles)
            
                if (max_obstacle + h_over_lscape > service_ceiling)
#=
                    orth_dir = curr_direction
                    if rand() < 0.5
                        orth_dir = [-curr_direction[2] curr_direction[1]]
                    else
                        orth_dir = [curr_direction[2] -curr_direction[1]]
                    end
                    
                    next_pos = current_pos[1:2] .+ orth_dir * curr_speed  # move orthogonal instead of towards the curr_direction
                    next_pos[1] = clamp(next_pos[1], X[1], X[end])
                    next_pos[2] = clamp(next_pos[2], Y[1], Y[end])
=#
                    next_pos[1] = clamp(next_pos[1] + rand([-0.7, 0.7]) * curr_speed, X[1], X[end])
                    next_pos[2] = clamp(next_pos[2] + rand([-0.7, 0.7]) * curr_speed, Y[1], Y[end])
                    new_z = clamp( current_pos[3] + descend_speed, -Inf, service_ceiling )
                elseif max_obstacle + h_over_lscape > new_z
                    new_z = clamp( max_obstacle + h_over_lscape, -Inf, service_ceiling )
                end
#            end
        end
        
        current_pos = [next_pos[1], next_pos[2], new_z]
        push!(drone_path, copy(current_pos))
        push!(drone_heights, new_z)
        
        xi = argmin(abs.(X .- next_pos[1])) # find next position of drone on the grid
        yi = argmin(abs.(Y .- next_pos[2]))
        push!(gnd_height_below, Z[xi,yi])

        push!(obstacles, visible_obstacles)
    end
    
    if (length(drone_path) > MaxSteps)
        println("CRASHED - Fuel is over!!!")
    else
        xi_end = argmin(abs.(X .- Finish[1]))
        yi_end = argmin(abs.(Y .- Finish[2]))
        final_ground = Z[xi_end, yi_end]

        for _ in 1:steps_descend
            new_z = drone_path[end][3] - (drone_path[end][3] - final_ground) / steps_descend
            push!(drone_path, [Finish[1], Finish[2], new_z])
            push!(drone_heights, new_z)
            push!(gnd_height_below, final_ground)
            push!(obstacles, [])
        end
    end
    println("Время полёта дрона: $(length(drone_path)) шагов")
    println("FlownGroups: ", FlownGroups)
    println("All visible Ref. points: ", All_RefPts)
    
    return drone_path, drone_heights, gnd_height_below, obstacles, Start, Finish, All_RefPts
end

println("Start position of drone: ", Start)
println( "Terrain height at start = ", Zls[trunc(Int64, Start[1]), trunc(Int64, Start[2])] )
speed = 1
drone_path, drone_heights, gnd_height, obstacles, start, finish, refs = simulate_drone(LScape, minh_over_terrain, pract_ceiling, speed)

path_x = [p[1] for p in drone_path]
path_y = [p[2] for p in drone_path]
path_z = [p[3] for p in drone_path]

plt1 = plot(path_x, path_y, lw=2, c=:blue, xlabel="X (м)", ylabel="Y (м)", title="Путь дрона", legend=false)
scatter!(plt1, [path_x[1]], [path_y[1]], marker=:diamond, markersize=6, color=:orange)
scatter!(plt1, [path_x[end]], [path_y[end]], marker=:xcross, markersize=6, color=:red)

#plt2 = plot(t, path_z, lw=2, c=:purple, xlabel="Шаги", ylabel="Высота (м)", title="Высоты дрона", legend=false)
plt2 = plot(path_x, path_y, path_z, lw=2, c=:purple, xlabel="X (м)", ylabel="Высота (м)", title="Путь дрона в 3D", legend=false, camera = (-15, 5))

scatter!(plt2, [path_x[1]], [path_y[1]], [path_z[1]], marker=:diamond, markersize=4, color=:orange)
scatter!(plt2, [path_x[end]], [path_y[end]], [path_z[end]], marker=:xcross, markersize=4, color=:red)

for p in 1:length(RefPts)
    scatter!(plt1, [RefPts[p][1]], [RefPts[p][2]], marker=:circle, markersize=3, color=:yellow)
    scatter!(plt2, [RefPts[p][1]], [RefPts[p][2]], [RefPts[p][3]], marker=:circle, markersize=3, color=:yellow)
end
#=
for (name, rgb_tuple) in Colors.color_names
    println("$name: $rgb_tuple")
end
=#
my_color = colorant"goldenrod1"

refs_coords = first.(refs)
for p in 1:length(refs_coords)
    scatter!(plt1, [refs_coords[p][1]],[refs_coords[p][2]], marker=:circle, markersize=5, color=my_color)
    scatter!(plt2, [refs_coords[p][1]], [refs_coords[p][2]], [Zls[refs_coords[p][1], refs_coords[p][2]]], marker=:circle, markersize=5, color=my_color)
end

t = 1:length(drone_path)# - steps_descend

ceiling = [pract_ceiling for p in t]
plt3 = plot(t, ceiling, lw=2, c=:DeepSkyBlue, legend=true, label="потолок", xticks=1:1:length(path_z), xformatter=(_...) -> "")
plt3 = plot!(t, path_z, lw=2, c=:purple, xlabel="Шаги, 1 шаг", ylabel="Высота (м)", title="Высота дрона и рельефа", legend=true, label="дрон")
plt3 = plot!(t, gnd_height, lw=2, c=:grey, legend=true, label="рельеф", yticks= trunc(Int64, floor(maxdepth)):2:max(pract_ceiling, trunc(Int64, ceil(maxheight))))

l = @layout([a b; c])
plot(plt1, plt2, plt3, layout=l, size=(1200, 1200))

#display(plt3)
#plot(plt3, plt4, layout=(1, 1), size=(1000, 1000))

gr()

function visualize_flight(landscape, drone_path, drone_heights, obstacles, speed, start, finish; fps=10)
    x, y, z = landscape

    xidx = Dict(px => argmin(abs.(x .- px)) for px in unique(p[1] for p in drone_path))
    yidx = Dict(py => argmin(abs.(y .- py)) for py in unique(p[2] for p in drone_path))

    base_surface = surface(x, y, z', c=:terrain, legend=false, aspect_ratio=:auto, axis=false)
    base_2d = plot(aspect_ratio=1, legend=false, axis=false)
    contour!(base_2d, x, y, z', c=:terrain, alpha=0.3)
    
    scatter!(base_surface, [start[1]], [start[2]], [start[3]], marker=:circle, markercolor=:green, label=false)
    scatter!(base_surface, [finish[1]], [finish[2]], [z[end, end]], marker=:circle, markercolor=:red, label=false)
    for p in 1:length(RefPts)
        scatter!(base_surface, [RefPts[p][1]], [RefPts[p][2]], marker=:circle, markersize=3, color=:yellow)    
    end
    
    scatter!(base_2d, [start[1]], [start[2]], marker=:circle, markercolor=:green)
    scatter!(base_2d, [finish[1]], [finish[2]], marker=:circle, markercolor=:red)
    for p in 1:length(RefPts)
        scatter!(base_2d, [RefPts[p][1]], [RefPts[p][2]], marker=:circle, markersize=3, color=:yellow)    
    end

    anim = @animate for i in 1:length(drone_path)
        plt1 = deepcopy(base_surface)
        plt2 = deepcopy(base_2d)

        px, py, pz = drone_path[i]
        scatter!(plt1, [px], [py], [pz], markersize=5, markercolor=:red, marker=:circle)
        xlims!(plt1, px - 10, px + 10)
        ylims!(plt1, py - 10, py + 10)

        xi, yi = xidx[px], yidx[py]
        height_above = pz - z[xi, yi]
        arrow = i == 1 ? "↑" : (height_above > (drone_path[i - 1][3] - z[xidx[drone_path[i - 1][1]], yidx[drone_path[i - 1][2]]]) ? "↑" : "↓")
        title!(plt1, "Высота дрона: $(floor(height_above)) м $arrow")

        if i > 1
            path = drone_path[1:i]
            plot!(plt2, [p[1] for p in path], [p[2] for p in path], linewidth=2, linecolor=:blue)
        end

        scatter!(plt2, [px], [py], markersize=5, markercolor=:red)

        if !isempty(obstacles[i])
            obs_x = [o[1] for o in obstacles[i]]
            obs_y = [o[2] for o in obstacles[i]]
            obs_z = [o[3] for o in obstacles[i]]
            scatter!(plt1, obs_x, obs_y, obs_z, markersize=3, markercolor=:yellow, marker=:xcross)
            scatter!(plt2, obs_x, obs_y, markersize=3, markercolor=:yellow, marker=:xcross)
        end

        plot(plt1, plt2, layout = @layout([a{0.7w} b]), size=(900, 300))
    end every max(1, round(Int, 1/speed))

    gif(anim, fps=fps)
end

visualize_flight(LScape, drone_path, drone_heights, obstacles, speed, Start, Finish; fps=5)