#!/usr/bin/env python3
"""
Demo 0001: Body坐标系画正方形
演示基于机体坐标系的增量位置控制
Body坐标系: X=前(Forward), Y=右(Right), Z=下(Down)

特点:
- 基于无人机自身的朝向
- 相对当前位置的增量移动
- 与地理方向无关，适合航向机动的场景

⚠️ 注意: 使用 goto_body() 前需要确保无人机已确定航向（起飞后会自动对准）
"""
from ros2_px4_swarm_manager import SimpleSwarmDemo


class BodySquareFlight(SimpleSwarmDemo):
    """Body坐标系正方形飞行演示"""
    
    def __init__(self):
        super().__init__(uav_ids=[1])
        self.side = 3.0      # 边长 3 米
        self.height = 2.0    # 飞行高度 2 米
        
    def run(self):
        """执行任务"""
        self.log("=" * 60)
        self.log("Demo 0001: Body坐标系画正方形")
        self.log("坐标系: Body (机体坐标系)")
        self.log("特点: 基于机头朝向的增量移动")
        self.log(f"参数: 边长={self.side}m, 高度={self.height}m")
        self.log("=" * 60)
        
        # 1. 起飞
        self.log("\n🚀 步骤 1: 起飞")
        if not self.takeoff(1, self.height):
            self.log("❌ 起飞失败")
            return
        self.sleep(2)
        
        # 记录起始位置
        status = self.controller.get_uav_status(1)
        x0, y0, z0 = status.local_x, status.local_y, status.local_z
        yaw0 = status.yaw
        self.log(f"📍 当前位置: 东={x0:.2f}m, 北={y0:.2f}m, 高={z0:.2f}m")
        self.log(f"🧭 当前航向: {yaw0:.1f}° (0°=北, 90°=东)")
        
        # 2. Body坐标系下画正方形
        # Body: X=前(+), Y=右(+), Z=下(+)
        # goto_body() 是增量控制：输入相对于当前位置的偏移量
        self.log("\n📐 步骤 2: Body坐标系画正方形")
        self.log("  飞行顺序: 前 → 右 → 后 → 左（相对于机头）")
        self.log("  ⚠️ 假设机头朝向北方，则 Body 的前/右/后/左 = 地理 北/东/南/西")
        
        # 航点 1: 向前飞
        self.log(f"  ➡️ 边 1/4: 向前飞 {self.side}m (Body: X+{self.side})")
        self.goto_body(1, self.side, 0, 0)
        self._wait_distance(1, self.side)
        self.sleep(1)
        
        # 航点 2: 向右飞
        self.log(f"  ➡️ 边 2/4: 向右飞 {self.side}m (Body: Y+{self.side})")
        self.goto_body(1, 0, self.side, 0)
        self._wait_distance(1, self.side)
        self.sleep(1)
        
        # 航点 3: 向后飞
        self.log(f"  ➡️ 边 3/4: 向后飞 {self.side}m (Body: X-{self.side})")
        self.goto_body(1, -self.side, 0, 0)
        self._wait_distance(1, self.side)
        self.sleep(1)
        
        # 航点 4: 向左飞，回到起点
        self.log(f"  ➡️ 边 4/4: 向左飞 {self.side}m (Body: Y-{self.side}) 回到起点")
        self.goto_body(1, 0, -self.side, 0)
        self._wait_distance(1, self.side)
        self.sleep(1)
        
        # 3. 检查精度
        final = self.controller.get_uav_status(1)
        error = ((final.local_x - x0)**2 + (final.local_y - y0)**2)**0.5
        self.log(f"\n📏 闭合误差: {error:.2f}m")
        
        # 4. 降落
        self.log("\n🛬 步骤 3: 降落")
        self.land(1)
        self.sleep(5)
        
        self.log("\n✅ 任务完成！")
        self.log("\n💡 Body坐标系特点:")
        self.log("   • X轴指向机头方向（前）")
        self.log("   • Y轴指向右侧（右）")
        self.log("   • Z轴指向下方（下）")
        self.log("   • goto_body() 是增量控制（相对当前位置移动）")
        self.log("   • 适合需要配合航向控制的场景")
    
    def _wait_distance(self, uav_id, distance, tolerance=0.5, timeout=15):
        """等待移动指定距离"""
        import time
        
        # 获取起始位置
        status = self.controller.get_uav_status(uav_id)
        start_x, start_y, start_z = status.local_x, status.local_y, status.local_z
        
        start_time = time.time()
        while time.time() - start_time < timeout:
            status = self.controller.get_uav_status(uav_id)
            traveled = ((status.local_x - start_x)**2 + 
                       (status.local_y - start_y)**2 + 
                       (status.local_z - start_z)**2)**0.5
            # 检查是否已经移动了接近目标距离
            if abs(traveled - distance) < tolerance or traveled > distance - tolerance:
                return True
            time.sleep(0.2)
        return False


if __name__ == '__main__':
    BodySquareFlight().start()
