Моделирование полёта дронов

using LinearAlgebra
plotlyjs()

function generate_landscape(width_m, length_m)
    x = 0:1:width_m
    y = 0:1:length_m

    z = [10(sin(xi / 15) + cos(yi / 15)) + 5rand() + 10 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]]
    finish = [x[xi_finish], y[random_yi_finish], z[xi_finish, random_yi_finish]]

    plt = surface(x, y, z', c=:terrain, xlabel="X (м)", ylabel="Y (м)", zlabel="Z (м)", title="3D ландшафт", legend=false)
    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 x, y, z, start, finish
end

landscape = generate_landscape(100, 100);

using LinearAlgebra
using Plots
plotlyjs()

"""
    find_landmarks(x, y, z, n_min::Int=3, n_max::Int=3)

Находит `n_min` локальных минимумов и `n_max` локальных максимумов высоты на ландшафте.
Возвращает вектор векторов координат `[x, y, z]` найденных ориентиров.
"""
function find_landmarks(x, y, z, n_min::Int=3, n_max::Int=3)
    landmarks = Vector{Vector{Float64}}()
    nx, ny = size(z)
    
    # Проверим все внутренние точки ландшафта на локальные минимумы/максимумы
    for i in 2:nx-1
        for j in 2:ny-1
            is_local_min = true
            is_local_max = true
            current_z = z[i, j]
            
            # Проверяем 8 соседей
            for di in -1:1
                for dj in -1:1
                    if di == 0 && dj == 0
                        continue
                    end
                    neighbor_z = z[i + di, j + dj]
                    if neighbor_z <= current_z
                        is_local_min = false
                    end
                    if neighbor_z >= current_z
                        is_local_max = false
                    end
                    # Если точка не является ни минимумом, ни максимумом, 
                    # нет смысла продолжать проверку
                    if !is_local_min && !is_local_max
                        break
                    end
                end
                if !is_local_min && !is_local_max
                    break
                end
            end
            
            if is_local_min || is_local_max
                push!(landmarks, [x[i], y[j], z[i, j]])
            end
        end
    end
    
    # Если мы нашли меньше ориентиров, чем нужно, добавим случайные точки
    # или просто вернем столько, сколько нашли
    
    # Сортируем по высоте для удобства выбора
    sort!(landmarks, by = p -> p[3])
    
    # Выбираем n_min самых низких (минимумов)
    min_landmarks = landmarks[1:min(n_min, length(landmarks))]
    # Выбираем n_max самых высоких (максимумов)
    max_landmarks = landmarks[max(1, end - n_max + 1):end]
    
    # Объединяем и убираем дубликаты (на случай, если min и max пересекаются)
    all_landmarks = unique(vcat(min_landmarks, max_landmarks))
    
    # Если все же ориентиров оказалось мало, добавим старт и финиш
    # (они уже есть в коде, но для полноты картины)
    
    return all_landmarks
end

"""
    generate_landscape(width_m, length_m)

Генерирует 3D-ландшафт и находит на нем визуальные ориентиры.
"""
function generate_landscape(width_m, length_m)
    x = 0:1:width_m
    y = 0:1:length_m

    z = [10(sin(xi / 15) + cos(yi / 15)) + 5rand() + 10 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]]
    finish = [x[xi_finish], y[random_yi_finish], z[xi_finish, random_yi_finish]]

    # Найти ориентиры
    landmarks = find_landmarks(x, y, z, 4, 4) # 4 минимума и 4 максимума
    
    plt = surface(x, y, z', c=:terrain, xlabel="X (м)", ylabel="Y (м)", zlabel="Z (м)", title="3D ландшафт", legend=false)
    scatter!(plt, [start[1]], [start[2]], [start[3]+3], marker=(:circle), color=:green, label="Старт")
    scatter!(plt, [finish[1]], [finish[2]], [finish[3]+3], marker=(:circle), color=:red, label="Финиш")
    
    # Отметить ориентиры на графике
    if !isempty(landmarks)
        lm_x = [lm[1] for lm in landmarks]
        lm_y = [lm[2] for lm in landmarks]
        lm_z = [lm[3] .+ 1.0 for lm in landmarks] # Немного выше поверхности для видимости
        scatter!(plt, lm_x, lm_y, lm_z, marker=(:diamond), color=:yellow, markersize=6, label="Ориентиры")
    end

    display(plt)
    return x, y, z, start, finish, landmarks
end

landscape_data = generate_landscape(100, 100)

"""
    simulate_drone(landscape_data, min_height, max_height, speed; fov_angle=45.0, view_distance=10.0)

Симулирует полет дрона, учитывая обнаруженные ориентиры.
"""
function simulate_drone(landscape_data, min_height, max_height, speed; fov_angle=45.0, view_distance=10.0)
    x, y, z, start, finish, landmarks = landscape_data
    drone_path = [copy(start)]
    drone_heights = [start[3]]
    obstacles = [[]]
    seen_landmarks_history = [Set{Int}()] # Храним индексы увиденных ориентиров на каждом шаге

    current_pos = copy(start)
    # Для более плавного движения будем хранить вектор скорости
    velocity = zeros(3) 
    max_speed = speed
    max_acceleration = speed * 0.5 # Ускорение за шаг
    vz_max = 1.0 # Максимальная скорость изменения высоты за шаг
    
    for _ in 1:1000
        if norm(current_pos[1:2] .- finish[1:2]) < speed
            break
        end
        
        # Желаемое направление
        direction_to_target = normalize(finish[1:2] .- current_pos[1:2])
        
        # Желаемая скорость (в направлении цели, но не более max_speed)
        desired_velocity_xy = direction_to_target * max_speed
        desired_velocity_z = 0.0 # Пока что вертикальную скорость будем корректировать отдельно
        
        # Применяем ускорение к горизонтальной скорости
        delta_v_xy = desired_velocity_xy .- velocity[1:2]
        acceleration_magnitude = norm(delta_v_xy)
        if acceleration_magnitude > 0
            # Ограничиваем ускорение
            unit_accel = delta_v_xy / acceleration_magnitude
            applied_accel = unit_accel * min(acceleration_magnitude, max_acceleration)
            velocity[1:2] .+= applied_accel
        end
        
        # Ограничиваем общую горизонтальную скорость
        speed_xy = norm(velocity[1:2])
        if speed_xy > max_speed
            velocity[1:2] .*= (max_speed / speed_xy)
        end
        
        # Определяем следующую позицию на основе скорости
        next_pos_xy = current_pos[1:2] .+ velocity[1:2]
        next_pos_xy[1] = clamp(next_pos_xy[1], x[1], x[end])
        next_pos_xy[2] = clamp(next_pos_xy[2], y[1], y[end])

        xi = argmin(abs.(x .- next_pos_xy[1]))
        yi = argmin(abs.(y .- next_pos_xy[2]))
        terrain_height = z[xi, yi]
        
        # Желаемая высота
        desired_z = max(terrain_height + min_height, current_pos[3])
        desired_z = min(desired_z, terrain_height + max_height)
        
        # Корректируем вертикальную скорость
        desired_vz = desired_z - current_pos[3]
        desired_vz = clamp(desired_vz, -vz_max, vz_max)
        velocity[3] = desired_vz
        
        # Новая позиция с учетом вертикальной скорости
        new_z = current_pos[3] + velocity[3]

        visible_obstacles = Tuple{Float64, Float64, Float64}[]
        # Проверка ориентиров в поле зрения
        visible_landmarks_indices = Set{Int}()
        
        # Проверяем 12 лучей в поле зрения
        for angle in range(-fov_angle/2, fov_angle/2, length=12)
            rad = deg2rad(angle)
            # Поворачиваем вектор направления
            dir_x = direction_to_target[1]*cos(rad) - direction_to_target[2]*sin(rad)
            dir_y = direction_to_target[1]*sin(rad) + direction_to_target[2]*cos(rad)
            
            # Проверяем на препятствия
            check_pos = next_pos_xy .+ [dir_x, dir_y] * view_distance
            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 + min_height > new_z
                    push!(visible_obstacles, (x[xi_check], y[yi_check], obstacle_height))
                end
            end
            
            # Проверяем ориентиры в поле зрения
            for (idx, lm) in enumerate(landmarks)
                # Простая проверка: ориентир в пределах дистанции и примерно в направлении
                lm_vector = lm[1:2] .- current_pos[1:2]
                distance_to_lm = norm(lm_vector)
                if distance_to_lm > 0 && distance_to_lm <= view_distance
                    # Угол между направлением дрона и направлением на ориентир
                    dir_to_lm = lm_vector / distance_to_lm
                    dot_product = dot(direction_to_target, dir_to_lm)
                    # Если угол меньше половины FOV
                    if dot_product >= cos(deg2rad(fov_angle / 2))
                        push!(visible_landmarks_indices, idx)
                    end
                end
            end
        end

        # Логика обхода препятствий с учетом ориентиров
        if !isempty(visible_obstacles)
            max_obstacle = maximum(obs[3] for obs in visible_obstacles)
            if max_obstacle + min_height > new_z
                # 50% шанс обойти в сторону, 50% - подняться
                if rand() < 0.5
                    # Случайный поворот на 90 градусов
                    side = rand([-1.0, 1.0])
                    new_dir_x = -direction_to_target[2] * side
                    new_dir_y = direction_to_target[1] * side
                    next_pos_xy .+= [new_dir_x, new_dir_y] * speed * 0.5 # Движение вбок
                    next_pos_xy[1] = clamp(next_pos_xy[1], x[1], x[end])
                    next_pos_xy[2] = clamp(next_pos_xy[2], y[1], y[end])
                else
                    # Подъем над препятствием
                    new_z = max_obstacle + min_height + 1.0
                    velocity[3] = min(vz_max, new_z - current_pos[3]) # Обновляем верт. скорость
                end
            end
        end

        current_pos = [next_pos_xy[1], next_pos_xy[2], new_z]
        push!(drone_path, copy(current_pos))
        push!(drone_heights, new_z)
        push!(obstacles, visible_obstacles)
        push!(seen_landmarks_history, visible_landmarks_indices)
    end

    # Посадка
    xi_end = argmin(abs.(x .- finish[1]))
    yi_end = argmin(abs.(y .- finish[2]))
    final_ground = z[xi_end, yi_end]

    for _ in 1:10
        new_z = drone_path[end][3] - (drone_path[end][3] - final_ground) / 10
        push!(drone_path, [finish[1], finish[2], new_z])
        push!(drone_heights, new_z)
        push!(obstacles, [])
        push!(seen_landmarks_history, Set{Int}())
    end

    println("Время полёта дрона: $(length(drone_path)) шагов")
    # Возвращаем также историю увиденных ориентиров
    return drone_path, drone_heights, obstacles, seen_landmarks_history, start, finish, landmarks
end

speed = 1.0
# Распаковываем данные ландшафта
x_l, y_l, z_l, start_l, finish_l, landmarks_l = landscape_data

# Запускаем симуляцию
drone_path, drone_heights, obstacles, seen_landmarks_history, start, finish, landmarks = simulate_drone(landscape_data, -5, 5, speed)

using Plots
gr() # Используем GR для анимации

"""
    visualize_flight(landscape_data, drone_path, drone_heights, obstacles, seen_landmarks_history, speed, start, finish, landmarks; fps=10)

Визуализирует полет дрона с учетом ориентиров.
"""
function visualize_flight(landscape_data, drone_path, drone_heights, obstacles, seen_landmarks_history, speed, start, finish, landmarks; fps=10)
    x, y, z, _, _, _ = landscape_data

    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="Старт", markersize=6)
    scatter!(base_surface, [finish[1]], [finish[2]], [z[end, end]], marker=:circle, markercolor=:red, label="Финиш", markersize=6)
    scatter!(base_2d, [start[1]], [start[2]], marker=:circle, markercolor=:green, label="Старт")
    scatter!(base_2d, [finish[1]], [finish[2]], marker=:circle, markercolor=:red, label="Финиш")
    
    # Отображаем все ориентиры
    if !isempty(landmarks)
        lm_x_all = [lm[1] for lm in landmarks]
        lm_y_all = [lm[2] for lm in landmarks]
        lm_z_all = [lm[3] .+ 1.0 for lm in landmarks] # Немного выше
        scatter!(base_surface, lm_x_all, lm_y_all, lm_z_all, marker=:diamond, markercolor=:yellow, markersize=5, label="Ориентиры")
        scatter!(base_2d, lm_x_all, lm_y_all, marker=:diamond, markercolor=:yellow, markersize=4, label="Ориентиры")
    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=6, markercolor=:blue, marker=:circle, label=false) # Дрон синий
        xlims!(plt1, px - 15, px + 15)
        ylims!(plt1, py - 15, py + 15)

        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, "Высота: $(round(height_above, digits=1)) м $arrow | Шаг: $i")

        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, label=false)
        end

        scatter!(plt2, [px], [py], markersize=6, markercolor=:blue, marker=:circle, label=false)

        # Отображаем препятствия
        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] .+ 1.0 for o in obstacles[i]] # Немного выше для видимости
            scatter!(plt1, obs_x, obs_y, obs_z, markersize=4, markercolor=:orange, marker=:xcross, label="Препятствия")
            scatter!(plt2, obs_x, obs_y, markersize=3, markercolor=:orange, marker=:xcross, label="Препятствия")
        end
        
        # Отображаем ВИДИМЫЕ ориентиры на этом шаге
        if !isempty(landmarks) && !isempty(seen_landmarks_history[i])
            visible_lm_indices = collect(seen_landmarks_history[i])
            visible_lm_coords = landmarks[visible_lm_indices]
            v_lm_x = [lm[1] for lm in visible_lm_coords]
            v_lm_y = [lm[2] for lm in visible_lm_coords]
            v_lm_z = [lm[3] .+ 2.0 for lm in visible_lm_coords] # Ещё выше
            
            # Подсвечиваем видимые ориентиры
            scatter!(plt1, v_lm_x, v_lm_y, v_lm_z, markersize=7, markercolor=:cyan, marker=:star5, label="Видимые ориентиры")
            scatter!(plt2, v_lm_x, v_lm_y, markersize=6, markercolor=:cyan, marker=:star5, label="Видимые ориентиры")
            
            # Рисуем линии от дрона к видимым ориентирам
            for (j, lm) in enumerate(visible_lm_coords)
                 plot!(plt1, [px, lm[1]], [py, lm[2]], [pz, lm[3] + 2.0], 
                       linecolor=:cyan, linestyle=:dash, linewidth=1, label=false)
            end
        end

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

    gif_file = gif(anim, "drone_flight_with_landmarks.gif", fps=fps)
    println("Анимация сохранена в файл: drone_flight_with_landmarks.gif")
    return gif_file
end

# Визуализируем
visualize_flight(landscape_data, drone_path, drone_heights, obstacles, seen_landmarks_history, speed, start, finish, landmarks, fps=5)