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

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)

using LinearAlgebra
using Plots

"""
    simulate_drone_realistic(landscape_data, min_height, max_height, base_speed;
                             max_acceleration=1.0, vz_max=1.0, max_turn_rate_deg=15.0,
                             fov_angle=45.0, view_distance=10.0)

Симулирует полет дрона с более реалистичной динамикой.
"""
function simulate_drone_realistic(landscape_data, min_height, max_height, base_speed;
                                  max_acceleration=0.2, vz_max=0.5, max_turn_rate_deg=10.0,
                                  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) # [vx, vy, vz]
    # Параметры динамики
    max_speed = base_speed
    turn_rate_rad = deg2rad(max_turn_rate_deg) # Максимальная угловая скорость в радианах

    # Параметры для посадки
    landing_phase = false
    landing_steps = 10
    landing_step_count = 0
    landing_target_z = 0.0

    for step in 1:2000 # Увеличиваем максимальное количество шагов из-за более медленного движения
        # --- Определяем цель ---
        if !landing_phase
            if norm(current_pos[1:2] .- finish[1:2]) < base_speed * 1.5 # Порог для начала посадки
                # Начинаем посадку
                landing_phase = true
                landing_step_count = 0
                xi_end = argmin(abs.(x .- finish[1]))
                yi_end = argmin(abs.(y .- finish[2]))
                landing_target_z = z[xi_end, yi_end] # Высота рельефа в точке финиша
                println("Начало посадки на шаге $step")
            end
        end

        if landing_phase
            # --- Логика посадки ---
            landing_step_count += 1
            if landing_step_count > landing_steps
                # Посадка завершена
                break
            end
            # Цель посадки - точка финиша на уровне земли
            target_pos_xy = finish[1:2]
            # Плавное снижение
            target_z = current_pos[3] - (current_pos[3] - landing_target_z) / (landing_steps - landing_step_count + 1)
            target_pos = [target_pos_xy[1], target_pos_xy[2], target_z]
        else
            # --- Нормальный полет к цели ---
            target_pos = finish
        end

        # --- Желаемое направление и скорость ---
        direction_to_target_xy = normalize(target_pos[1:2] .- current_pos[1:2])
        # Максимальная желаемая скорость в направлении цели
        desired_velocity_xy = direction_to_target_xy * max_speed

        # --- Ограничение на поворот (изменение направления скорости) ---
        current_speed_xy = norm(velocity[1:2])
        if current_speed_xy > 1e-6 # Если дрон движется
            current_direction_xy = velocity[1:2] / current_speed_xy
            # Угол между текущим и желаемым направлением
            dot_product = clamp(dot(current_direction_xy, direction_to_target_xy), -1.0, 1.0)
            angle_to_target = acos(dot_product)

            if angle_to_target > turn_rate_rad
                # Поворачиваем скорость на максимальный угол
                # Определяем знак поворота (влево или вправо)
                cross_product_z = current_direction_xy[1] * direction_to_target_xy[2] - current_direction_xy[2] * direction_to_target_xy[1]
                sign = cross_product_z > 0 ? 1 : -1

                # Поворачиваем вектор скорости
                cos_turn = cos(turn_rate_rad)
                sin_turn = sin(turn_rate_rad) * sign
                new_vx = current_direction_xy[1] * cos_turn - current_direction_xy[2] * sin_turn
                new_vy = current_direction_xy[1] * sin_turn + current_direction_xy[2] * cos_turn
                # Новая скорость с тем же модулем
                desired_velocity_xy = [new_vx, new_vy] * current_speed_xy
            end
        else
            # Если дрон стоит, начинаем движение в направлении цели
            desired_velocity_xy = direction_to_target_xy * min(max_acceleration, max_speed)
        end

        # --- Применение ускорения к горизонтальной скорости ---
        delta_v_xy = desired_velocity_xy .- velocity[1:2]
        acceleration_magnitude = norm(delta_v_xy)
        if acceleration_magnitude > 1e-10
            # Ограничиваем ускорение
            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

        # --- Вертикальное движение ---
        xi_next = argmin(abs.(x .- (current_pos[1] + velocity[1])))
        yi_next = argmin(abs.(y .- (current_pos[2] + velocity[2])))
        xi_next = clamp(xi_next, 1, length(x))
        yi_next = clamp(yi_next, 1, length(y))
        terrain_height_next = z[xi_next, yi_next]

        # Желаемая высота над рельефом
        desired_altitude = max(min_height, 0.0) # Минимальная высота над землей
        desired_z = terrain_height_next + desired_altitude

        # Желаемая вертикальная скорость
        desired_vz = desired_z - current_pos[3]

        # Ограничиваем вертикальную скорость
        desired_vz = clamp(desired_vz, -vz_max, vz_max)

        # Применяем вертикальное ускорение (для простоты будем считать, что vz может мгновенно измениться к желаемому, но не более чем на vz_max)
        # Более точная модель потребовала бы отдельного параметра вертикального ускорения
        # Пока ограничимся скоростью
        velocity[3] = desired_vz

        # --- Вычисляем новую позицию ---
        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])
        new_z = current_pos[3] + velocity[3]

        # --- Обнаружение препятствий и ориентиров ---
        visible_obstacles = Tuple{Float64, Float64, Float64}[]
        visible_landmarks_indices = Set{Int}()

        # Проверяем 12 лучей в поле зрения
        current_direction_xy = velocity[1:2]
        current_speed_xy = norm(current_direction_xy)
        if current_speed_xy > 1e-6
            current_direction_xy ./= current_speed_xy
        else
            # Если скорость нулевая, используем направление к цели
            current_direction_xy = direction_to_target_xy
        end

        for angle in range(-fov_angle/2, fov_angle/2, length=12)
            rad = deg2rad(angle)
            # Поворачиваем вектор направления для луча
            dir_x = current_direction_xy[1]*cos(rad) - current_direction_xy[2]*sin(rad)
            dir_y = current_direction_xy[1]*sin(rad) + current_direction_xy[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 - 0.1 # Небольшой буфер
                    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_lm = dot(current_direction_xy, dir_to_lm)
                    # Если угол меньше половины FOV
                    if dot_product_lm >= cos(deg2rad(fov_angle / 2))
                        push!(visible_landmarks_indices, idx)
                    end
                end
            end
        end

        # --- Логика обхода препятствий (с учетом динамики) ---
        if !isempty(visible_obstacles) && !landing_phase
            max_obstacle = maximum(obs[3] for obs in visible_obstacles)
            # Проверяем, действительно ли препятствие угрожает столкновением
            if max_obstacle + min_height > new_z - 0.1
                # 50% шанс обойти в сторону, 50% - подняться
                if rand() < 0.5
                    # Случайный поворот на 90 градусов (влево или вправо)
                    # Это будет реализовано как изменение желаемого направления скорости
                    side = rand([-1.0, 1.0])
                    avoidance_dir_x = -current_direction_xy[2] * side
                    avoidance_dir_y = current_direction_xy[1] * side
                    # Добавляем компоненту обхода к желаемой скорости
                    # Это заставит дрон постепенно поворачивать
                    avoidance_velocity_xy = [avoidance_dir_x, avoidance_dir_y] * (max_speed * 0.7)
                    # Модифицируем desired_velocity_xy для этого шага
                    desired_velocity_xy = (desired_velocity_xy + avoidance_velocity_xy) / 2
                    # Повторно применяем логику ускорения и ограничений
                    delta_v_xy = desired_velocity_xy .- velocity[1:2]
                    acceleration_magnitude = norm(delta_v_xy)
                    if acceleration_magnitude > 1e-10
                        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
                else
                    # Команда на подъем: увеличиваем желаемую высоту
                    # Это будет обработано в следующем шаге цикла при расчете desired_vz
                    # Пока просто отмечаем, что нужно подняться
                    # Можно добавить логику, чтобы дрон "знал", что он поднимается из-за препятствия
                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

    println("Время полёта дрона: $(length(drone_path)) шагов")
    return drone_path, drone_heights, obstacles, seen_landmarks_history, start, finish, landmarks
end

# --- Перегенерируем ландшафт, если нужно, или используем существующий ---
# Убедимся, что у нас есть все необходимые данные
# Предполагается, что переменная `landscape_data` уже существует из предыдущих шагов
# x_l, y_l, z_l, start_l, finish_l, landmarks_l = generate_landscape(100, 100)
# landscape_data = (x_l, y_l, z_l, start_l, finish_l, landmarks_l)

# --- Запускаем симуляцию с реалистичной динамикой ---
println("Запуск симуляции с реалистичной динамикой...")
base_speed = 1.0
drone_path_real, drone_heights_real, obstacles_real, seen_landmarks_history_real, start_real, finish_real, landmarks_real = simulate_drone_realistic(landscape_data, -5, 5, base_speed)

# --- Визуализация ---
# Используем обновленную функцию визуализации (если она была сохранена как visualize_flight_with_landmarks)
# Если нет, копируем её сюда

# Допустим, у нас есть функция visualize_flight, модифицированная ранее
# Для полноты картины, вот её адаптированная версия:

function visualize_flight_realistic(landscape_data, drone_path, drone_heights, obstacles, seen_landmarks_history, speed, start, finish, landmarks; fps=10)
    x, y, z, _, _, _ = landscape_data

    # Для оптимизации поиска индексов
    x_unique = unique(p[1] for p in drone_path)
    y_unique = unique(p[2] for p in drone_path)
    xidx = Dict(px => argmin(abs.(x .- px)) for px in x_unique)
    yidx = Dict(py => argmin(abs.(y .- py)) for py in y_unique)

    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=:pentagon, label="Видимые ориентиры")
scatter!(plt2, v_lm_x, v_lm_y, markersize=6, markercolor=:cyan, marker=:pentagon, 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)) # Анимируем каждый шаг для плавности

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

# --- Визуализируем результат с реалистичной динамикой ---
println("Создание анимации с реалистичной динамикой...")
visualize_flight_realistic(landscape_data, drone_path_real, drone_heights_real, obstacles_real, seen_landmarks_history_real, base_speed, start_real, finish_real, landmarks_real, fps=5)

# --- Также можно сравнить траектории ---
# Визуализируем обе траектории на одном графике для сравнения
function compare_trajectories(landscape_data, path1, path2, name1="Базовый", name2="Реалистичный")
    x, y, z, start, finish, _ = landscape_data
    plt = surface(x, y, z', c=:terrain, xlabel="X (м)", ylabel="Y (м)", zlabel="Z (м)", title="Сравнение траекторий", legend=true, alpha=0.7)

    # Траектория 1
    x1 = [p[1] for p in path1]
    y1 = [p[2] for p in path1]
    z1 = [p[3] for p in path1]
    plot3d!(plt, x1, y1, z1, label=name1, linewidth=2, color=:blue)

    # Траектория 2
    x2 = [p[1] for p in path2]
    y2 = [p[2] for p in path2]
    z2 = [p[3] for p in path2]
    plot3d!(plt, x2, y2, z2, label=name2, linewidth=2, color=:red)

    # Старт и финиш
    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="Финиш")

    display(plt)
    return plt
end

println("Сравнение траекторий...")
compare_trajectories(landscape_data, drone_path, drone_path_real, "Базовый", "Реалистичный")