base_agent/uas/route/base_segment.py

# -*- coding: utf-8 -*-
"""
航段抽象基类
─────────────────────────────────────────────────────────────────
BaseSegment      —— 所有直线/曲线航段的公共抽象基类
BaseTransition   —— 两段之间过渡段（转弯/机动）的抽象基类

继承体系：
  BaseSegment
  ├── AirSegment       空中直线段（适配 RouteSegment）
  ├── GroundSegment    陆地直线段
  └── WaterSegment     水中直线段

  BaseTransition
  ├── AirTurnSegment   空中转弯圆弧段（适配 TurnSegment）
  ├── GroundTurnSegment 陆地转弯段
  └── WaterTurnSegment 水中机动段
─────────────────────────────────────────────────────────────────
"""

from __future__ import annotations

from abc import ABC, abstractmethod
from dataclasses import dataclass, field
from enum import Enum
from typing import Any, Dict, TYPE_CHECKING

from uas.route.base_params import BaseParams
from uas.route.base_waypoint import BaseWaypoint, EquipmentDomain

if TYPE_CHECKING:
    pass


# ─────────────────────────────────────────────────────────────────
# 航段类型枚举
# ─────────────────────────────────────────────────────────────────

class SegmentKind(Enum):
    STRAIGHT   = "straight"    # 直线段
    TURN       = "turn"        # 转弯/圆弧段
    HOLD       = "hold"        # 等待/盘旋段
    APPROACH   = "approach"    # 进近段
    MANEUVER   = "maneuver"    # 机动段（水下/地面）
    CUSTOM     = "custom"      # 自定义


# ─────────────────────────────────────────────────────────────────
# 航段抽象基类
# ─────────────────────────────────────────────────────────────────

@dataclass
class BaseSegment(ABC):
    """
    航段公共抽象基类

    每段代表航线上一个连续的运动片段，
    包含起止坐标、时间区间、速度/方位等核心参数。

    子类必须实现：
      query(t)          —— 查询段内 t 时刻的装备状态
      interpolate(ratio)—— 按比例插值（0.0=起点，1.0=终点）
      validate()        —— 校验航段合法性
    """

    # ── 逻辑路径点引用 ────────────────────────────────────────────
    wp_from:      BaseWaypoint          # 起始逻辑路径点
    wp_to:        BaseWaypoint          # 终止逻辑路径点

    # ── 实际飞行起终点坐标 ────────────────────────────────────────
    start_lat:    float                 # 实际起点纬度（度）
    start_lon:    float                 # 实际起点经度（度）
    start_alt:    float                 # 实际起点高度（米）
    end_lat:      float                 # 实际终点纬度（度）
    end_lon:      float                 # 实际终点经度（度）
    end_alt:      float                 # 实际终点高度（米）

    # ── 几何参数 ──────────────────────────────────────────────────
    distance:     float                 # 水平距离（米）
    bearing:      float                 # 初始方位角（度，真北顺时针）

    # ── 时间参数 ──────────────────────────────────────────────────
    t_start:      float                 # 航段起始时刻（秒）
    t_end:        float                 # 航段结束时刻（秒）
    duration:     float                 # 飞行时长（秒）

    # ── 速度参数 ──────────────────────────────────────────────────
    v_start:      float                 # 起点速度（m/s）
    v_end:        float                 # 终点速度（m/s）
    acceleration: float  = 0.0         # 匀加速度（m/s²）

    # ── 域与类型 ──────────────────────────────────────────────────
    domain:       EquipmentDomain = EquipmentDomain.AIR
    kind:         SegmentKind     = SegmentKind.STRAIGHT

    # ── 扩展元数据 ────────────────────────────────────────────────
    metadata:     Dict[str, Any]  = field(default_factory=dict)

    # ── 衍生属性 ──────────────────────────────────────────────────

    @property
    def avg_speed(self) -> float:
        return (self.v_start + self.v_end) / 2.0

    @property
    def vertical_distance(self) -> float:
        return self.end_alt - self.start_alt

    @property
    def vertical_speed(self) -> float:
        if self.duration < 1e-6:
            return 0.0
        return self.vertical_distance / self.duration

    @property
    def is_climb(self) -> bool:
        return self.vertical_distance > 10

    @property
    def is_descent(self) -> bool:
        return self.vertical_distance < -10

    @property
    def is_level(self) -> bool:
        return abs(self.vertical_distance) <= 10

    @property
    def start_position(self) -> tuple:
        return (self.start_lat, self.start_lon, self.start_alt)

    @property
    def end_position(self) -> tuple:
        return (self.end_lat, self.end_lon, self.end_alt)

    def contains_time(self, t: float) -> bool:
        """判断时刻 t 是否落在本段时间范围内"""
        return self.t_start <= t <= self.t_end

    # ── 抽象接口 ──────────────────────────────────────────────────

    @abstractmethod
    def query(self, t: float) -> BaseParams:
        """
        查询段内 t 时刻的装备状态
        :param t: 查询时刻（秒）
        :return:  BaseParams 或其子类实例
        """
        ...

    @abstractmethod
    def interpolate(self, ratio: float) -> BaseParams:
        """
        按比例插值
        :param ratio: 0.0=起点，1.0=终点
        :return:      BaseParams 或其子类实例
        """
        ...

    @abstractmethod
    def validate(self) -> list:
        """
        校验航段合法性
        :return: 错误信息列表（空列表=合法）
        """
        ...

    def __str__(self) -> str:
        phase = "爬升" if self.is_climb else ("下降" if self.is_descent else "平飞")
        return (f"{self.__class__.__name__}["
                f"{self.wp_from.identifier}→{self.wp_to.identifier}] "
                f"{self.distance/1000:.2f}km/{self.bearing:.1f}°/"
                f"{self.duration:.1f}s/{phase}]")


# ─────────────────────────────────────────────────────────────────
# 过渡段抽象基类（转弯/机动）
# ─────────────────────────────────────────────────────────────────

@dataclass
class BaseTransition(ABC):
    """
    两段之间过渡段的公共抽象基类

    过渡段连接两个直线段，描述转弯、机动等非直线运动。
    典型实例：
      AirTurnSegment   —— 飞机 Fly-by 圆弧转弯
      GroundTurnSegment—— 车辆转弯段
      WaterTurnSegment —— 舰艇回转机动

    子类必须实现 query(t) 和 validate()。
    """

    # ── 入口/出口参数 ─────────────────────────────────────────────
    entry_lat:      float               # 入口纬度（度）
    entry_lon:      float               # 入口经度（度）
    entry_alt:      float               # 入口高度（米）
    entry_heading:  float               # 入口航向（度）
    exit_heading:   float               # 出口航向（度）

    # ── 时间参数 ──────────────────────────────────────────────────
    t_start:        float               # 过渡段起始时刻（秒）
    duration:       float  = 0.0        # 过渡段时长（秒，子类计算后填充）

    # ── 运动参数 ──────────────────────────────────────────────────
    speed:          float  = 0.0        # 过渡段速度（m/s）
    altitude:       float  = 0.0        # 过渡段高度（米）

    # ── 域与类型 ──────────────────────────────────────────────────
    domain:         EquipmentDomain = EquipmentDomain.AIR
    kind:           SegmentKind     = SegmentKind.TURN

    # ── 扩展元数据 ────────────────────────────────────────────────
    metadata:       Dict[str, Any]  = field(default_factory=dict)

    # ── 衍生属性 ──────────────────────────────────────────────────

    @property
    def t_end(self) -> float:
        return self.t_start + self.duration

    @property
    def turn_angle(self) -> float:
        """转弯角（度，右转为正，左转为负）"""
        diff = self.exit_heading - self.entry_heading
        while diff >  180: diff -= 360
        while diff < -180: diff += 360
        return diff

    @property
    def turn_direction(self) -> int:
        """转弯方向：+1=右转，-1=左转，0=直线"""
        a = self.turn_angle
        if   a >  0.5: return  1
        elif a < -0.5: return -1
        return 0

    def contains_time(self, t: float) -> bool:
        return self.t_start <= t <= self.t_end

    # ── 抽象接口 ──────────────────────────────────────────────────

    @abstractmethod
    def query(self, t: float) -> BaseParams:
        """查询过渡段内 t 时刻的装备状态"""
        ...

    @abstractmethod
    def validate(self) -> list:
        """校验过渡段合法性"""
        ...

    @property
    @abstractmethod
    def exit_lat(self) -> float:
        """出口纬度（子类计算）"""
        ...

    @property
    @abstractmethod
    def exit_lon(self) -> float:
        """出口经度（子类计算）"""
        ...

    def __str__(self) -> str:
        dir_str = "右转" if self.turn_direction > 0 else "左转"
        return (f"{self.__class__.__name__}["
                f"{dir_str}{abs(self.turn_angle):.1f}° "
                f"t={self.t_start:.1f}~{self.t_end:.1f}s]")