#!/usr/bin/env python3
"""
坐标系统调试脚本
验证KDL运动学与PyBullet仿真的坐标一致性
"""

import sys
import os
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))

from src.config_loader import ConfigLoader
from src.robot.arm_controller import create_arm_controller
from src.simulation.environment import Environment
import pybullet as p
import pybullet_data
import numpy as np

def debug_coordinates():
    """调试坐标系统一致性"""
    print("=== 坐标系统调试分析 ===")
    
    # 初始化仿真
    physics_client = p.connect(p.GUI)
    p.setAdditionalSearchPath(pybullet_data.getDataPath())
    
    try:
        # 加载配置
        config_loader = ConfigLoader()
        config = config_loader.get_full_config()
        
        print(f"机械臂基座位置: {config['robot']['base_position']}")
        print(f"机械臂基座朝向: {config['robot']['base_orientation']}")
        
        # 创建机械臂控制器
        arm_controller = create_arm_controller(config_loader, physics_client)
        
        # 创建环境（包括墙体和任务点）
        environment = Environment(config_loader, physics_client)
        
        print("\n=== 关节映射信息 ===")
        kdl_names = arm_controller.kinematics_engine.get_joint_names()
        pb_names = arm_controller.robot_loader.get_joint_names()
        print(f"KDL关节顺序: {kdl_names}")
        print(f"PyBullet关节顺序: {pb_names}")
        print(f"KDL→PyBullet映射: {arm_controller._kdl_to_pb}")
        print(f"PyBullet→KDL映射: {arm_controller._pb_to_kdl}")
        
        print("\n=== 当前机械臂状态测试 ===")
        
        # 测试1：获取当前关节位置
        current_joints = arm_controller.get_current_joint_positions()
        print(f"当前关节位置(KDL顺序): {[f'{x:.3f}' for x in current_joints]}")
        
        # 测试2：正向运动学 - 计算末端位置
        fk_position, fk_orientation = arm_controller.forward_kinematics(current_joints)
        print(f"正向运动学计算的末端位置: {[f'{x:.3f}' for x in fk_position]}")
        print(f"正向运动学计算的末端姿态: {[f'{x:.3f}' for x in fk_orientation]}")
        
        # 测试3：直接从PyBullet获取末端位置（验证）
        robot_id = arm_controller.robot_loader.get_robot_id()
        end_effector_index = len(pb_names)  # 末端执行器通常是最后一个link
        link_state = p.getLinkState(robot_id, end_effector_index - 1, physicsClientId=physics_client)
        pb_end_position = link_state[0]  # 世界坐标中的位置
        pb_end_orientation = link_state[1]  # 世界坐标中的姿态(四元数)
        
        print(f"PyBullet直接查询的末端位置: {[f'{x:.3f}' for x in pb_end_position]}")
        print(f"PyBullet直接查询的末端姿态: {[f'{x:.3f}' for x in pb_end_orientation]}")
        
        # 测试4：坐标差异分析
        pos_diff = np.array(fk_position) - np.array(pb_end_position)
        pos_error = np.linalg.norm(pos_diff)
        print(f"位置差异: {[f'{x:.4f}' for x in pos_diff]}")
        print(f"位置误差幅度: {pos_error:.4f} 米")
        
        if pos_error > 0.01:  # 1cm tolerance
            print("⚠️  发现显著位置差异！KDL和PyBullet坐标不一致")
        else:
            print("✅ 位置一致性良好")
        
        print("\n=== 逆向运动学测试 ===")
        
        # 测试5：任务点可达性
        task_points = config_loader.get_task_points()
        
        for point_name, point_info in task_points.items():
            target_pos = point_info['position']
            print(f"\n测试 {point_name}: {target_pos}")
            
            # 使用逆运动学求解
            joint_solution = arm_controller.inverse_kinematics(target_pos, seed_angles=current_joints)
            
            if joint_solution is None:
                print(f"❌ {point_name} 不可达")
                continue
            else:
                print(f"✅ {point_name} 可达，关节解: {[f'{x:.3f}' for x in joint_solution]}")
                
                # 验证：使用求得的关节配置计算正向运动学
                verify_pos, verify_ori = arm_controller.forward_kinematics(joint_solution)
                target_error = np.linalg.norm(np.array(verify_pos) - np.array(target_pos))
                print(f"   验证位置: {[f'{x:.3f}' for x in verify_pos]}")
                print(f"   目标误差: {target_error:.4f} 米")
                
                if target_error > 0.01:
                    print(f"   ⚠️  逆运动学验证失败，误差过大")
                else:
                    print(f"   ✅ 逆运动学验证通过")
        
        print("\n=== 路径执行测试模拟 ===")
        
        # 测试6：模拟路径执行中的坐标转换
        test_joint_config = current_joints.copy()
        test_joint_config[0] += 0.1  # 微调第一个关节
        
        print(f"测试关节配置(KDL顺序): {[f'{x:.3f}' for x in test_joint_config]}")
        
        # 计算预期末端位置（通过KDL）
        expected_pos, _ = arm_controller.forward_kinematics(test_joint_config)
        print(f"KDL计算预期末端位置: {[f'{x:.3f}' for x in expected_pos]}")
        
        # 模拟路径执行器的关节设置
        arm_controller.set_joint_positions(test_joint_config)
        
        # 等待仿真稳定
        for _ in range(100):
            p.stepSimulation(physicsClientId=physics_client)
        
        # 检查实际到达的位置
        actual_joints = arm_controller.get_current_joint_positions()
        actual_pos, _ = arm_controller.get_current_pose()
        
        print(f"实际关节位置(KDL顺序): {[f'{x:.3f}' for x in actual_joints]}")
        print(f"实际末端位置: {[f'{x:.3f}' for x in actual_pos]}")
        
        # 分析执行误差
        joint_error = np.linalg.norm(np.array(test_joint_config) - np.array(actual_joints))
        pos_error = np.linalg.norm(np.array(expected_pos) - np.array(actual_pos))
        
        print(f"关节执行误差: {joint_error:.4f} rad")
        print(f"位置执行误差: {pos_error:.4f} 米")
        
        if pos_error > 0.05:  # 5cm tolerance
            print("❌ 发现路径执行误差，坐标转换存在问题")
        else:
            print("✅ 路径执行精度良好")
        
    except Exception as e:
        print(f"调试过程中发生错误: {e}")
        import traceback
        traceback.print_exc()
        
    finally:
        p.disconnect(physicsClientId=physics_client)

if __name__ == "__main__":
    debug_coordinates()