#!/usr/bin/env python3
"""
USV无人船对讲机通信系统

专为水面无人船设计的对讲机通信解决方案
支持双向通信、GPS追踪、紧急控制等
"""

import sys
import time
import json
from pathlib import Path
from datetime import datetime

sys.path.insert(0, str(Path(__file__).parent / "src"))

from src.morse_protocol_integration import MorseProtocolSystem
from src.morse_protocol import MorseMessageType
from src.morse_messages import (
    TextMessage,
    GPSPosition,
    EmergencyMessage,
    HeartbeatMessage
)


class USVBoatController:
    """
    USV无人船控制器（岸端）

    功能：
    1. 发送控制指令到船
    2. 接收船的状态信息
    3. GPS追踪
    4. 紧急控制
    """

    def __init__(self, boat_id: str = "USV-001"):
        """
        初始化控制器

        参数：
        boat_id: 船的ID
        """
        self.boat_id = boat_id

        # 通信系统
        self.comm_system = MorseProtocolSystem(
            sys_id=1,  # 岸端系统ID
            comp_id=1,
            enable_ai=True,  # 启用AI纠错
            enable_protocol=True  # 使用协议
        )

        # 船的状态
        self.boat_status = {
            'connected': False,
            'last_heartbeat': None,
            'battery': 100,
            'position': None,
            'speed': 0,
            'heading': 0,
            'mode': 'AUTO'
        }

        # 指令历史
        self.command_history = []

    def send_navigate_command(self,
                              latitude: float,
                              longitude: float,
                              speed: float = 5.0):
        """
        发送导航指令

        参数：
        latitude: 目标纬度
        longitude: 目标经度
        speed: 速度（节）
        """
        print(f"\n🚢 发送导航指令到 {self.boat_id}")
        print(f"  目标位置: {latitude:.6f}, {longitude:.6f}")
        print(f"  速度: {speed} 节")

        # 创建导航指令消息
        command = {
            'type': 'NAVIGATE',
            'lat': latitude,
            'lon': longitude,
            'speed': speed,
            'timestamp': datetime.now().isoformat()
        }

        # 转换为摩斯音频
        command_text = f"NAV {latitude:.4f} {longitude:.4f} S{speed}"
        result = self.comm_system.encode_text(command_text, use_protocol=True)

        # 记录指令
        self.command_history.append({
            'command': command,
            'time': datetime.now(),
            'status': 'SENT'
        })

        print(f"  ✓ 指令已编码")
        print(f"  摩斯: {result.morse_code[:50]}...")
        print(f"  时长: {sum(self.comm_system.morse.get_timing().values())/1000:.1f}秒")

        return result

    def send_stop_command(self):
        """发送停止指令"""
        print(f"\n🛑 发送STOP指令到 {self.boat_id}")

        command_text = "STOP IMMEDIATELY"
        result = self.comm_system.encode_text(command_text, use_protocol=True)

        print(f"  ✓ 停止指令已发送")

        return result

    def send_return_command(self):
        """发送返航指令"""
        print(f"\n🏠 发送返航指令到 {self.boat_id}")

        command_text = "RETURN HOME"
        result = self.comm_system.encode_text(command_text, use_protocol=True)

        print(f"  ✓ 返航指令已发送")

        return result

    def send_speed_command(self, speed: float):
        """发送速度指令"""
        print(f"\n⚡ 发送速度指令到 {self.boat_id}")
        print(f"  目标速度: {speed} 节")

        command_text = f"SPEED {speed}"
        result = self.comm_system.encode_text(command_text, use_protocol=True)

        print(f"  ✓ 速度指令已发送")

        return result

    def request_status(self):
        """请求船的状态"""
        print(f"\n📊 请求 {self.boat_id} 状态")

        command_text = "STATUS REQUEST"
        result = self.comm_system.encode_text(command_text, use_protocol=True)

        print(f"  ✓ 状态请求已发送")

        return result

    def create_emergency(self,
                        emergency_type: int,
                        message: str,
                        severity: int = 9):
        """
        创建紧急消息

        参数：
        emergency_type: 紧急类型（1-5）
        message: 紧急描述
        severity: 严重程度（0-10）
        """
        print(f"\n🚨 创建紧急消息")
        print(f"  类型: {emergency_type}")
        print(f"  消息: {message}")
        print(f"  严重程度: {severity}/10")

        # 使用协议创建紧急消息
        emergency = self.comm_system.create_emergency(
            emergency_type,
            message,
            severity
        )

        return emergency

    def decode_boat_response(self, morse_audio):
        """
        解码船的响应

        参数：
        morse_audio: 接收到的摩斯音频

        返回：
        解码后的消息
        """
        print(f"\n📥 解码 {self.boat_id} 响应")

        # 使用AI解码
        result = self.comm_system.decode_morse(morse_audio, use_ai=True)

        print(f"  原始摩斯: {result.morse_code}")
        print(f"  解码文字: {result.ai_corrections if result.ai_corrections else result.morse_code}")

        return result

    def update_boat_status(self, status_data: dict):
        """
        更新船的状态

        参数：
        status_data: 状态数据
        """
        self.boat_status.update(status_data)
        self.boat_status['last_heartbeat'] = datetime.now()
        self.boat_status['connected'] = True

        print(f"\n✓ {self.boat_id} 状态已更新")
        self.print_boat_status()

    def print_boat_status(self):
        """打印船的状态"""
        print(f"\n{'='*60}")
        print(f"  {self.boat_id} 状态")
        print('='*60)

        status = self.boat_status

        print(f"  连接状态: {'✓ 已连接' if status['connected'] else '✗ 未连接'}")

        if status['last_heartbeat']:
            print(f"  最后心跳: {status['last_heartbeat'].strftime('%H:%M:%S')}")

        print(f"  电池: {status['battery']}%")

        if status['position']:
            pos = status['position']
            print(f"  位置: {pos.get('lat', 'N/A')}, {pos.get('lon', 'N/A')}")

        print(f"  速度: {status['speed']} 节")
        print(f"  航向: {status['heading']}°")
        print(f"  模式: {status['mode']}")

    def export_mission_log(self, filename: str = None):
        """
        导出任务日志

        参数：
        filename: 文件名
        """
        if not filename:
            filename = f"usv_mission_{datetime.now().strftime('%Y%m%d_%H%M%S')}.json"

        log_data = {
            'boat_id': self.boat_id,
            'start_time': datetime.now().isoformat(),
            'boat_status': self.boat_status,
            'command_history': [
                {
                    'command': cmd['command'],
                    'time': cmd['time'].isoformat(),
                    'status': cmd['status']
                }
                for cmd in self.command_history
            ]
        }

        with open(filename, 'w', encoding='utf-8') as f:
            json.dump(log_data, f, indent=2, ensure_ascii=False)

        print(f"\n✓ 任务日志已导出: {filename}")


class USVBoatReceiver:
    """
    USV无人船接收器（船端）

    功能：
    1. 接收岸端指令
    2. 自动执行指令
    3. 发送状态回岸端
    4. GPS定位
    5. 紧急处理
    """

    def __init__(self,
                 boat_id: str = "USV-001",
                 gps_port: str = "/dev/ttyUSB0"):
        """
        初始化船接收器

        参数：
        boat_id: 船的ID
        gps_port: GPS端口
        """
        self.boat_id = boat_id
        self.gps_port = gps_port

        # 通信系统
        self.comm_system = MorseProtocolSystem(
            sys_id=2,  # 船端系统ID
            comp_id=1,
            enable_ai=True,
            enable_protocol=True
        )

        # 船的状态
        self.current_status = {
            'mode': 'AUTO',
            'battery': 100,
            'position': {'lat': 30.5928, 'lon': 114.3055},  # 武汉坐标
            'speed': 5.0,
            'heading': 0,
            'last_command': None
        }

        # 任务队列
        self.mission_queue = []

    def receive_command(self, morse_audio):
        """
        接收并处理指令

        参数：
        morse_audio: 接收到的摩斯音频
        """
        print(f"\n📡 {self.boat_id} 接收指令")

        # 解码
        result = self.comm_system.decode_morse(morse_audio, use_ai=True)
        command_text = result.ai_corrections if result.ai_corrections else result.morse_code

        print(f"  接收指令: {command_text}")

        # 解析并执行指令
        self.execute_command(command_text)

    def execute_command(self, command: str):
        """
        执行指令

        参数：
        command: 指令字符串
        """
        command = command.upper().strip()
        self.current_status['last_command'] = command

        # 导航指令
        if command.startswith('NAV '):
            parts = command.split()
            if len(parts) >= 3:
                try:
                    lat = float(parts[1])
                    lon = float(parts[2])
                    speed = float(parts[3].replace('S', '')) if len(parts) > 3 else 5.0

                    print(f"\n  ✓ 执行导航: {lat:.4f}, {lon:.4f}, {speed}节")
                    self.current_status['mode'] = 'NAVIGATING'
                    self.current_status['position'] = {'lat': lat, 'lon': lon}
                    self.current_status['speed'] = speed

                except ValueError as e:
                    print(f"  ✗ 指令格式错误: {e}")

        # 停止指令
        elif command == 'STOP IMMEDIATELY':
            print(f"\n  ✓ 执行停止")
            self.current_status['mode'] = 'STOPPED'
            self.current_status['speed'] = 0

        # 返航指令
        elif command == 'RETURN HOME':
            print(f"\n  ✓ 执行返航")
            home_position = {'lat': 30.5928, 'lon': 114.3055}  # 武汉坐标
            self.current_status['mode'] = 'RETURNING'
            self.current_status['position'] = home_position

        # 速度指令
        elif command.startswith('SPEED '):
            try:
                speed = float(command.split()[1])
                print(f"\n  ✓ 设置速度: {speed}节")
                self.current_status['speed'] = speed
            except ValueError:
                print(f"  ✗ 速度值错误")

        # 状态请求
        elif command == 'STATUS REQUEST':
            print(f"\n  ✓ 发送状态")
            self.send_status()

        else:
            print(f"\n  ⚠️  未知指令: {command}")

    def send_status(self):
        """发送状态回岸端"""
        print(f"\n📤 {self.boat_id} 发送状态")

        # 构建状态消息
        status_text = (
            f"POS {self.current_status['position']['lat']:.4f} "
            f"{self.current_status['position']['lon']:.4f} "
            f"SPD {self.current_status['speed']:.1f} "
            f"BAT {self.current_status['battery']} "
            f"MOD {self.current_status['mode']}"
        )

        # 编码为摩斯音频
        result = self.comm_system.encode_text(status_text, use_protocol=True)

        print(f"  状态消息: {status_text}")
        print(f"  ✓ 状态已编码")

        return result

    def send_heartbeat(self):
        """发送心跳"""
        heartbeat = HeartbeatMessage(system_status=3)  # 3 = standby
        return self.comm_system.protocol.send_message(
            MorseMessageType.HEARTBEAT,
            heartbeat.pack()
        )

    def send_gps_position(self):
        """发送GPS位置"""
        gps = GPSPosition(
            lat=int(self.current_status['position']['lat'] * 1e7),
            lon=int(self.current_status['position']['lon'] * 1e7),
            alt=int(0),  # 水面，高度为0
            satellites=8
        )
        return self.comm_system.protocol.send_message(
            MorseMessageType.GPS_POSITION,
            gps.pack()
        )

    def handle_emergency(self, emergency_type: int):
        """
        处理紧急情况

        参数：
        emergency_type: 紧急类型
        """
        print(f"\n🚨 {self.boat_id} 紧急情况: {emergency_type}")

        # 紧急类型定义
        emergencies = {
            1: "LOW_BATTERY",
            2: "OBSTACLE_DETECTED",
            3: "MANUAL_OVERRIDE",
            4: "SYSTEM_FAILURE",
            5: "LOSS_OF_SIGNAL"
        }

        emergency_name = emergencies.get(emergency_type, "UNKNOWN")

        # 创建紧急消息
        emergency_msg = EmergencyMessage(
            emergency_type=emergency_type,
            message=f"{emergency_name} at {datetime.now().strftime('%H:%M:%S')}",
            severity=9
        )

        # 发送紧急消息
        result = self.comm_system.protocol.send_message(
            MorseMessageType.EMERGENCY,
            emergency_msg.pack()
        )

        # 执行紧急动作
        if emergency_type == 1:  # 低电量
            self.current_status['mode'] = 'RETURNING'
        elif emergency_type == 2:  # 障碍物
            self.current_status['mode'] = 'STOPPED'
            self.current_status['speed'] = 0
        elif emergency_type == 4:  # 系统故障
            self.current_status['mode'] = 'EMERGENCY_STOP'

        print(f"  ✓ 紧急消息已发送")
        print(f"  ✓ 执行紧急动作: {self.current_status['mode']}")

        return result


def demo_usv_communication():
    """演示USV通信"""
    print("""
╔══════════════════════════════════════════════════════════╗
║     USV无人船对讲机通信系统 - 演示                       ║
║                                                          ║
║  武汉 → USV船（长江/汉江）                                ║
╚══════════════════════════════════════════════════════════╝
    """)

    print("="*60)
    print("  场景：武汉岸边 → USV船（长江中）")
    print("="*60)
    print("  位置:")
    print("    岸端: 武汉 (30.5928°N, 114.3055°E)")
    print("    船端: 长江中 (30.6000°N, 114.3100°E)")
    print("  距离: 约5公里")
    print("  对讲机: GMRS (462 MHz, 5瓦)")
    print("  频率: 800Hz摩斯音频")
    print("  速度: 0.15秒/点（水面可靠模式）")

    # 创建岸端控制器
    print("\n" + "="*60)
    print("  岸端控制器（武汉）")
    print("="*60)

    controller = USVBoatController(boat_id="USV-WUHAN-001")

    # 示例1: 发送导航指令
    print("\n【示例1】发送导航指令")
    print("-"*60)

    result = controller.send_navigate_command(
        latitude=30.6100,
        longitude=114.3200,
        speed=8.0
    )

    print(f"\n生成的音频文件:")
    print(f"  - 摩斯: {result.morse_code[:60]}...")
    print(f"  - 时长: 约{len(result.signal_sequence) * 0.15 / 60:.1f}分钟")
    print(f"\n操作:")
    print(f"  1. 保存为WAV文件")
    print(f"  2. 连接对讲机")
    print(f"  3. 按PTT播放")
    print(f"  4. 发送到5公里外的USV船")

    # 示例2: 请求状态
    print("\n【示例2】请求船的状态")
    print("-"*60)

    result = controller.request_status()

    # 示例3: 紧急情况
    print("\n【示例3】紧急控制")
    print("-"*60)

    emergency = controller.create_emergency(
        emergency_type=2,
        message="障碍物，立即停止",
        severity=9
    )

    print(f"\n生成的紧急消息:")
    print(f"  - 类型: 障碍物检测")
    print(f"  - 严重程度: 9/10")
    print(f"  - 立即发送到船")

    # 船端响应
    print("\n" + "="*60)
    print("  船端接收器（USV船）")
    print("="*60)

    receiver = USVBoatReceiver(boat_id="USV-WUHAN-001")

    # 模拟接收指令
    print("\n【模拟】接收导航指令")
    print("-"*60)

    sample_morse = "NAV 30.6100 114.3200 S8.0"
    receiver.execute_command(sample_morse)

    print(f"\n船的状态:")
    receiver.current_status['mode'] = 'NAVIGATING'
    print(f"  模式: {receiver.current_status['mode']}")
    print(f"  目标: 30.6100°N, 114.3200°E")
    print(f"  速度: 8.0节")

    # 船端发送状态
    print("\n【模拟】发送状态回岸端")
    print("-"*60)

    result = receiver.send_status()

    # 总结
    print("\n" + "="*60)
    print("  系统总结")
    print("="*60)

    print(f"\n通信距离: 5-10公里")
    print(f"传输方式: 对讲机 (462 MHz)")
    print(f"编码方式: 摩斯音频 (800Hz)")
    print(f"数据速率: 约0.5字/秒")
    print(f"可靠性: 85-90% (AI纠错)")

    print(f"\n适用场景:")
    print(f"  ✓ 长江USV巡逻")
    print(f"  ✓ 汉江水质监测")
    print(f"  ✓ 水库无人船")
    print(f"  ✓ 湖泊科考船")

    print(f"\n硬件需求:")
    print(f"  岸端: 电脑 + GMRS对讲机 + 室外天线")
    print(f"  船端: 单片机 + GMRS对讲机 + GPS")
    print(f"  总成本: 约$300-500")

    print(f"\n下一步:")
    print(f"  1. 测试通信距离")
    print(f"  2. 集成到USV船")
    print(f"  3. 实际水域测试")
    print(f"  4. 优化参数")


if __name__ == "__main__":
    demo_usv_communication()