#!/usr/bin/env python3
"""
低头匍匐步态集成测试脚本
用于验证所有模块是否正确导入和配置
"""

import sys
import os

# 添加路径
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

def test_imports():
    """测试所有模块导入"""
    print("测试模块导入...")
    
    try:
        # 测试基本模块导入
        from task_2.crawl_gait import (
            low_crawl_gait,
            transition_to_crawl_position,
            recover_from_crawl_position,
            run_low_crawl_demo
        )
        print("✓ crawl_gait模块导入成功")
        
        from task_2.task_2 import (
            run_task_2_with_crawl_demo,
            test_crawl_gait_only,
            test_crawl_transition_only
        )
        print("✓ task_2模块导入成功")
        
        from task_2.file_send_lcmt import file_send_lcmt
        from task_2.robot_control_cmd_lcmt import robot_control_cmd_lcmt
        print("✓ LCM消息模块导入成功")
        
        return True
        
    except ImportError as e:
        print(f"✗ 导入失败: {e}")
        return False

def test_config_files():
    """测试配置文件存在性"""
    print("\n测试配置文件...")
    
    files_to_check = [
        "./task_2/Gait_Def_crawl.toml",
        "./task_2/Gait_Params_crawl.toml",
    ]
    
    all_exist = True
    for file_path in files_to_check:
        if os.path.exists(file_path):
            print(f"✓ {file_path} 存在")
        else:
            print(f"✗ {file_path} 不存在")
            all_exist = False
    
    return all_exist

def test_toml_syntax():
    """测试TOML文件语法"""
    print("\n测试TOML文件语法...")
    
    try:
        import toml
        
        # 测试步态定义文件
        with open("./task_2/Gait_Def_crawl.toml", 'r') as f:
            gait_def = toml.load(f)
            print(f"✓ Gait_Def_crawl.toml 语法正确，包含 {len(gait_def['section'])} 个步态段")
        
        # 测试步态参数文件  
        with open("./task_2/Gait_Params_crawl.toml", 'r') as f:
            gait_params = toml.load(f)
            print(f"✓ Gait_Params_crawl.toml 语法正确，包含 {len(gait_params['step'])} 个步态参数")
            
        return True
        
    except ImportError:
        print("! toml模块未安装，跳过语法检查")
        return True
    except Exception as e:
        print(f"✗ TOML文件语法错误: {e}")
        return False

def validate_gait_parameters():
    """验证步态参数的合理性"""
    print("\n验证步态参数...")
    
    try:
        import toml
        
        with open("./task_2/Gait_Params_crawl.toml", 'r') as f:
            gait_params = toml.load(f)
        
        # 检查关键参数
        first_step = gait_params['step'][0]
        
        # 检查body_pos_des
        body_pos = first_step['body_pos_des']
        if len(body_pos) == 6:
            print(f"✓ body_pos_des格式正确: roll={body_pos[0]}, pitch={body_pos[1]}, yaw={body_pos[2]}, x={body_pos[3]}, y={body_pos[4]}, z={body_pos[5]}")
        else:
            print(f"✗ body_pos_des格式错误，应为6个元素，实际{len(body_pos)}个")
            return False
        
        # 检查body_vel_des
        body_vel = first_step['body_vel_des']
        if len(body_vel) == 3:
            print(f"✓ body_vel_des格式正确: vx={body_vel[0]}, vy={body_vel[1]}, vyaw={body_vel[2]}")
        else:
            print(f"✗ body_vel_des格式错误，应为3个元素，实际{len(body_vel)}个")
            return False
        
        # 检查landing_pos_des
        landing_pos = first_step['landing_pos_des']
        if len(landing_pos) == 12:
            print(f"✓ landing_pos_des格式正确: 12个落足位置参数")
        else:
            print(f"✗ landing_pos_des格式错误，应为12个元素，实际{len(landing_pos)}个")
            return False
        
        # 检查step_height
        step_height = first_step['step_height']
        if len(step_height) == 4:
            print(f"✓ step_height格式正确: FR={step_height[0]}, FL={step_height[1]}, RR={step_height[2]}, RL={step_height[3]}")
        else:
            print(f"✗ step_height格式错误，应为4个元素，实际{len(step_height)}个")
            return False
        
        return True
        
    except ImportError:
        print("! toml模块未安装，跳过参数验证")
        return True
    except Exception as e:
        print(f"✗ 参数验证失败: {e}")
        return False

def main():
    """主测试函数"""
    print("=" * 50)
    print("低头匍匐步态集成测试")
    print("=" * 50)
    
    tests = [
        ("模块导入", test_imports),
        ("配置文件检查", test_config_files), 
        ("TOML语法检查", test_toml_syntax),
        ("参数验证", validate_gait_parameters),
    ]
    
    results = []
    for test_name, test_func in tests:
        print(f"\n[{test_name}]")
        result = test_func()
        results.append((test_name, result))
    
    print("\n" + "=" * 50)
    print("测试结果汇总:")
    print("=" * 50)
    
    all_passed = True
    for test_name, result in results:
        status = "✓ 通过" if result else "✗ 失败"
        print(f"{test_name:15} {status}")
        if not result:
            all_passed = False
    
    print("\n" + "=" * 50)
    if all_passed:
        print("🎉 所有测试通过！低头匍匐步态已准备就绪。")
        print("\n使用方法:")
        print("1. 完整演示: run_low_crawl_demo(ctrl, msg, distance=2.0)")
        print("2. 与任务2集成: run_task_2_with_crawl_demo(ctrl, msg)")
        print("3. 仅测试步态: test_crawl_gait_only(ctrl, msg)")
        print("4. 仅测试姿态: test_crawl_transition_only(ctrl, msg)")
    else:
        print("❌ 部分测试失败，请检查配置。")
    print("=" * 50)

if __name__ == "__main__":
    main()