"""
任务调度器 - 管理拍摄任务的执行
"""
import os
import json
import time
import logging
import subprocess
import math
from typing import List, Dict, Optional
from enum import Enum

from .arm_client import ArmClient
from .agv_controller_ros2 import AGVController
from .qr_scanner import QRScanner
from .image_uploader import ImageUploader

logger = logging.getLogger(__name__)

# ROS2 环境设置（与 agv_controller_ros2.py 保持一致）
ROS2_SETUP_CMD = "export ROS_DOMAIN_ID=1 && source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"


class TaskStatus(Enum):
    PENDING = "pending"
    RUNNING = "running"
    COMPLETED = "completed"
    FAILED = "failed"
    PAUSED = "paused"


class MissionExecutor:
    """任务执行器 - 负责按顺序执行点位拍摄任务"""

    def __init__(self, config: dict):
        self.config = config
        self.status = TaskStatus.PENDING
        self.current_point_index = 0
        self.current_pose_index = 0
        self.snapshot_serial_map = {}  # {point_id: serial_number} 缓存已扫描的 serialNumber

        # 初始化各模块
        self.agv = AGVController(
            device=config.get("device", "/dev/agvpro_controller"),
            baudrate=config.get("baudrate", 1000000)
        )
        self.arm_client: Optional[ArmClient] = None
        self.uploader = ImageUploader(
            upload_url=config["upload_url"],
            timeout=config.get("upload_timeout", 30),
            max_retries=config.get("upload_retries", 3)
        )
        self.qr_scanner = QRScanner(device_index=config.get("camera_index", 0))

    # ========== 连接管理 ==========

    def connect_all(self) -> Dict[str, bool]:
        """连接 AGV、机械臂、摄像头"""
        results = {}

        # 连接 AGV
        results["agv"] = self.agv.connect()

        # 连接机械臂（通过 TCP）
        arm_cfg = self.config["arm"]
        self.arm_client = ArmClient(arm_cfg["host"], arm_cfg["port"])
        results["arm"] = self.arm_client.connect()

        # 打开摄像头
        results["camera"] = self.qr_scanner.open()

        return results

    def disconnect_all(self):
        """断开所有连接"""
        if self.arm_client:
            self.arm_client.close()
        self.agv.disconnect()
        self.qr_scanner.close()

    # ========== 任务执行 ==========

    def execute_mission(self, mission_data: dict) -> dict:
        """
        执行一个完整任务（一个地图的所有点位）
        mission_data: 包含点位列表的完整任务配置
        返回执行报告
        """
        self.status = TaskStatus.RUNNING
        report = {
            "total_points": len(mission_data.get("points", [])),
            "completed": 0,
            "failed": 0,
            "details": []
        }

        points = mission_data.get("points", [])
        for i, point in enumerate(points):
            self.current_point_index = i
            try:
                result = self._execute_point(point)
                report["details"].append(result)
                if result["status"] == "completed":
                    report["completed"] += 1
                else:
                    report["failed"] += 1
            except Exception as e:
                logger.error(f"点位 {i} 执行异常: {e}")
                report["failed"] += 1
                report["details"].append({
                    "point_index": i,
                    "point_name": point.get("name", f"point_{i}"),
                    "status": "failed",
                    "error": str(e)
                })

        self.status = TaskStatus.COMPLETED if report["failed"] == 0 else TaskStatus.PAUSED
        return report

    def _execute_point(self, point: dict) -> dict:
        """执行单个点位的拍摄"""
        point_name = point.get("name", "unknown")
        logger.info(f"开始执行点位: {point_name}")

        result = {
            "point_name": point_name,
            "poses": []
        }

        # 1. AGV 移动到点位（Nav2 导航）
        coords = point.get("coords", {})
        x, y = coords.get("x", 0), coords.get("y", 0)
        rz = coords.get("rz", 0.0)  # 目标朝向
        logger.info(f"AGV Nav2 导航到 ({x}, {y}), yaw={rz}")

        nav_result = self._nav2_go_to_point(x, y, rz)
        if not nav_result:
            logger.warning(f"AGV 导航到点位 {point_name} 失败，跳过")
            return {"point_name": point_name, "status": "failed", "error": "导航失败"}

        # 2. 执行该点位的所有姿态
        poses = point.get("poses", [])
        for j, pose in enumerate(poses):
            self.current_pose_index = j
            pose_result = self._execute_pose(point, pose, j)
            result["poses"].append(pose_result)

            # 如果是"两者都要"类型，需要按顺序执行两台机器
            if pose.get("type") == "both":
                # 执行顺序由 pose.sequence 配置
                sequence = pose.get("sequence", ["front_first"])
                for step in sequence:
                    if step == "front":
                        self._capture_and_upload(point, pose, "front", j)
                    elif step == "back":
                        self._capture_and_upload(point, pose, "back", j)
            else:
                photo_type = pose.get("photo_type", "front")
                self._capture_and_upload(point, pose, photo_type, j)

        result["status"] = "completed"
        return result

    def _execute_pose(self, point: dict, pose: dict, pose_idx: int) -> dict:
        """执行单个姿态的拍摄"""
        photo_type = pose.get("photo_type", "front")
        camera_source = pose.get("camera", "agv")  # agv 或 arm

        # 如果需要机械臂运动
        arm_angles = pose.get("arm_angles", None)
        if arm_angles and self.arm_client:
            self.arm_client.set_angles(arm_angles, speed=pose.get("speed", 500))
            time.sleep(1)  # 等待运动到位

        return {
            "pose_index": pose_idx,
            "photo_type": photo_type,
            "arm_angles": arm_angles,
            "status": "ready"
        }

    def _capture_and_upload(self, point: dict, pose: dict, photo_type: str, pose_idx: int):
        """拍摄并上传"""
        point_id = point.get("id", str(point))

        # 确定 serialNumber
        if photo_type == "front":
            # 正面：从二维码获取 serialNumber
            serial = self.qr_scanner.scan_with_retry(max_attempts=5, interval=0.5)
            if not serial:
                logger.warning(f"点位 {point.get('name')} 正面拍摄未扫描到二维码，跳过")
                return
            self.snapshot_serial_map[point_id] = serial
        else:
            # 背面：使用缓存的 serialNumber
            serial = self.snapshot_serial_map.get(point_id)
            if not serial:
                logger.warning(f"点位 {point.get('name')} 背面拍摄但无缓存 serialNumber")
                return

        # 拍摄图片（AGV 端摄像头）
        frame = self.qr_scanner.read_frame()
        if frame is None:
            logger.error("摄像头读取失败")
            return

        # 保存图片
        photo_dir = os.path.join(os.path.dirname(__file__), "..", "photos")
        os.makedirs(photo_dir, exist_ok=True)
        photo_path = os.path.join(photo_dir, f"{serial}_{photo_type}_{int(time.time())}.jpg")
        import cv2
        cv2.imwrite(photo_path, frame)

        # 上传
        self.uploader.upload(photo_path, serial, pose_idx, photo_type)
        logger.info(f"上传完成: {serial} {photo_type}")

    # ========== Nav2 导航 ==========

    def _nav2_check_available(self) -> bool:
        """检查 Nav2 action server 是否可用"""
        try:
            rc, out, err = self._run_ros2_cmd("ros2 action list")
            if rc != 0:
                return False
            return "/navigate_to_pose" in out or "navigate_to_pose" in out
        except:
            return False

    def _nav2_send_goal(self, x: float, y: float, yaw: float, timeout_sec: float = 120.0) -> bool:
        """
        通过 ros2 action 发送 navigate_to_pose 目标并等待结果

        Returns:
            是否成功到达
        """
        # 检查 Nav2 可用性
        if not self._nav2_check_available():
            logger.error("Nav2 action server 不可用，请确保 navigation2_active.launch.py 已启动")
            return False

        # 构建四元数
        qz = math.sin(yaw / 2.0)
        qw = math.cos(yaw / 2.0)

        pose_yaml = (
            f"pose:\n"
            f"  header:\n"
            f"    stamp:\n"
            f"      sec: 0\n"
            f"      nanosec: 0\n"
            f"    frame_id: 'map'\n"
            f"  position:\n"
            f"    x: {x}\n"
            f"    y: {y}\n"
            f"    z: 0.0\n"
            f"  orientation:\n"
            f"    x: 0.0\n"
            f"    y: 0.0\n"
            f"    z: {qz}\n"
            f"    w: {qw}"
        )

        logger.info(f"Nav2 发送导航目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")

        # 发送 goal 并监听反馈和结果
        cmd = (
            f"ros2 action send_goal /navigate_to_pose "
            f"navigation_action_msgs/NavigateToPose "
            f"'{pose_yaml}' "
            f"--feedback"
        )

        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"

        try:
            process = subprocess.Popen(
                full_cmd, shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
            )

            succeeded = False
            check_interval = 1.0
            elapsed = 0.0

            while elapsed < timeout_sec:
                import select
                reads = [process.stdout]
                ready, _, _ = select.select(reads, [], [], check_interval)
                elapsed += check_interval

                if process.stdout in ready:
                    line = process.stdout.readline()
                    if not line:
                        break
                    line_stripped = line.strip()

                    if "succeeded" in line_stripped.lower():
                        logger.info("✅ Nav2 导航成功到达目标")
                        succeeded = True
                        break
                    elif "failed" in line_stripped.lower() or "aborted" in line_stripped.lower():
                        logger.warning(f"⚠️ Nav2 导航失败: {line_stripped}")
                        break
                    elif "canceled" in line_stripped.lower() or "cancelled" in line_stripped.lower():
                        logger.info("Nav2 导航被取消")
                        break

                if process.poll() is not None:
                    # 进程结束但没读到 succeeded
                    break

            # 确保进程结束
            if process.poll() is None:
                process.terminate()
                try:
                    process.wait(timeout=3)
                except subprocess.TimeoutExpired:
                    process.kill()

            return succeeded

        except Exception as e:
            logger.error(f"Nav2 导航异常: {e}")
            return False

    def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
                          timeout_sec: float = 120.0) -> bool:
        """
        AGV 通过 Nav2 导航到目标点位

        Args:
            x, y:    目标世界坐标（米）
            yaw:     目标朝向（弧度）
            timeout_sec: 超时时间

        Returns:
            是否成功到达
        """
        return self._nav2_send_goal(x, y, yaw, timeout_sec)

    def _nav2_cancel(self):
        """取消当前 Nav2 导航"""
        cancel_cmd = f"bash -c '{ROS2_SETUP_CMD} && ros2 action cancel /navigate_to_pose 2>/dev/null || ros2 action cancel /navigate_through_poses 2>/dev/null || true'"
        try:
            subprocess.run(cancel_cmd, shell=True, timeout=3)
            logger.info("Nav2 导航已取消")
        except:
            pass

    def _run_ros2_cmd(self, cmd: str, timeout: float = 5.0) -> tuple:
        """执行 ros2 命令"""
        full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
        try:
            result = subprocess.run(
                full_cmd, shell=True, capture_output=True, text=True, timeout=timeout
            )
            return result.returncode, result.stdout.strip(), result.stderr.strip()
        except subprocess.TimeoutExpired:
            return -1, "", "Timeout"
        except Exception as e:
            return -1, "", str(e)

    # ========== 状态查询 ==========

    def get_status(self) -> dict:
        return {
            "task_status": self.status.value,
            "current_point": self.current_point_index,
            "current_pose": self.current_pose_index,
            "agv_connected": self.agv.is_connected(),
            "arm_connected": self.arm_client is not None,
            "camera_opened": self.qr_scanner._cap is not None and self.qr_scanner._cap.isOpened()
        }

    def pause(self):
        self.status = TaskStatus.PAUSED

    def resume(self):
        self.status = TaskStatus.RUNNING

    def stop(self):
        if self.arm_client:
            self.arm_client.task_stop()
        self.agv.stop()
        self.status = TaskStatus.PENDING