#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
主测试脚本
整合所有测试模块，提供统一的测试入口和菜单选择
"""

import time
import sys
import os

# 添加父目录到路径，以便能够导入utils
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))

from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

# 导入所有测试模块
from test_basic_movements import run_basic_movement_tests
from test_arc_movements import run_arc_movement_tests
from test_custom_gait import run_custom_gait_tests
from test_vision_functions import run_vision_function_tests
from test_complex_movements import run_complex_movement_tests

# 创建日志记录器
logger = get_logger("主测试")

def print_menu():
    """打印测试菜单"""
    print("\n" + "="*60)
    print("           机器狗子运动函数测试套件")
    print("="*60)
    print("1. 基础运动测试")
    print("   - 原地旋转90度/180度")
    print("   - 直线前进/后退")
    print("   - 侧向移动")
    print("   - 正方形移动路径")
    print()
    print("2. 弧形运动测试")
    print("   - 90度/180度弧形转弯")
    print("   - 左转/右转弧形转弯")
    print("   - S形弯道运动")
    print("   - 不同半径弧形转弯")
    print()
    print("3. 自定义步态测试")
    print("   - 正常行走步态")
    print("   - 石板路步态（快步跑）")
    print("   - 俯身步态")
    print("   - 爬坡步态")
    print("   - 躺下和站起动作")
    print()
    print("4. 视觉功能测试")
    print("   - QR码扫描功能")
    print("   - 箭头检测功能")
    print("   - 横线检测功能")
    print("   - 天空/黄色区域分析")
    print("   - 移动过程中的QR码扫描")
    print()
    print("5. 复杂运动测试")
    print("   - 连续弯道运动")
    print("   - 8字形运动")
    print("   - 螺旋运动")
    print("   - 精确定位")
    print("   - 双轨道跟随")
    print("   - 组合运动")
    print()
    print("6. 运行所有测试")
    print("7. 单个测试选择")
    print("8. 交互式测试模式")
    print("0. 退出")
    print("="*60)
    print("💡 提示: 建议先从基础运动测试开始，确保机器狗处于安全位置")

def run_single_test_menu(ctrl, msg):
    """运行单个测试的子菜单"""
    while True:
        print("\n" + "="*60)
        print("           单个测试选择菜单")
        print("="*60)
        print("📍 基础运动测试:")
        print("  11. 原地旋转90度         12. 原地旋转180度")
        print("  13. 直线前进1米          14. 直线后退1米")
        print("  15. 侧向移动            16. 正方形移动路径")
        print()
        print("🔄 弧形运动测试:")
        print("  21. 90度弧形转弯        22. 180度弧形转弯")
        print("  23. 右转90度弧形转弯     24. S形弯道运动")
        print("  25. 不同半径弧形转弯")
        print()
        print("🚶 自定义步态测试:")
        print("  31. 正常行走步态        32. 石板路步态")
        print("  33. 躺下和站起动作      34. 俯身步态")
        print("  35. 爬坡步态")
        print()
        print("👁️ 视觉功能测试:")
        print("  41. QR码扫描功能        42. 箭头检测功能")
        print("  43. 横线检测功能        44. 天空分析功能")
        print("  45. 黄色区域分析        46. 移动中QR扫描")
        print()
        print("🎯 复杂运动测试:")
        print("  51. 连续弯道运动        52. 8字形运动")
        print("  53. 螺旋运动           54. 精确定位")
        print("  55. 双轨道跟随         56. 组合运动")
        print()
        print("🔧 快捷操作:")
        print("  h. 帮助信息            s. 显示当前状态")
        print("  c. 清屏               0. 返回主菜单")
        print("="*60)
        
        try:
            choice = input("请选择要执行的测试 (输入编号或命令): ").strip().lower()
            
            if choice == '0':
                break
            elif choice == 'h' or choice == 'help':
                show_help_info()
                continue
            elif choice == 's' or choice == 'status':
                show_current_status(ctrl)
                continue
            elif choice == 'c' or choice == 'clear':
                clear_screen()
                continue
            
            # 导入测试函数
            from test_basic_movements import (
                test_turn_90_degrees, test_turn_180_degrees, test_go_straight_1m,
                test_go_straight_backward_1m, test_lateral_movement, test_square_movement
            )
            from test_arc_movements import (
                test_arc_turn_90_degrees, test_arc_turn_180_degrees, 
                test_arc_turn_right_90_degrees, test_s_curve_movement
            )
            from test_custom_gait import (
                test_normal_walking_gait, test_stone_road_gait, test_lie_down_stand_up,
                test_stoop_gait, test_climbing_gait
            )
            from test_vision_functions import (
                test_qr_code_scanning, test_arrow_detection, test_horizontal_line_detection,
                test_sky_analysis, test_yellow_area_analysis
            )
            from test_complex_movements import (
                test_continuous_curves, test_figure_eight_movement, test_spiral_movement,
                test_precision_positioning, test_dual_track_following, test_combined_movements
            )
            from test_arc_movements import test_different_radius_turns
            from test_vision_functions import test_movement_with_qr_scanning
            
            # 执行对应的测试
            test_map = {
                '11': ("原地旋转90度", test_turn_90_degrees),
                '12': ("原地旋转180度", test_turn_180_degrees),
                '13': ("直线前进1米", test_go_straight_1m),
                '14': ("直线后退1米", test_go_straight_backward_1m),
                '15': ("侧向移动", test_lateral_movement),
                '16': ("正方形移动路径", test_square_movement),
                '21': ("90度弧形转弯", test_arc_turn_90_degrees),
                '22': ("180度弧形转弯", test_arc_turn_180_degrees),
                '23': ("右转90度弧形转弯", test_arc_turn_right_90_degrees),
                '24': ("S形弯道运动", test_s_curve_movement),
                '25': ("不同半径弧形转弯", test_different_radius_turns),
                '31': ("正常行走步态", test_normal_walking_gait),
                '32': ("石板路步态", test_stone_road_gait),
                '33': ("躺下和站起动作", test_lie_down_stand_up),
                '34': ("俯身步态", test_stoop_gait),
                '35': ("爬坡步态", test_climbing_gait),
                '41': ("QR码扫描功能", test_qr_code_scanning),
                '42': ("箭头检测功能", test_arrow_detection),
                '43': ("横线检测功能", test_horizontal_line_detection),
                '44': ("天空分析功能", test_sky_analysis),
                '45': ("黄色区域分析功能", test_yellow_area_analysis),
                '46': ("移动中QR扫描", test_movement_with_qr_scanning),
                '51': ("连续弯道运动", test_continuous_curves),
                '52': ("8字形运动", test_figure_eight_movement),
                '53': ("螺旋运动", test_spiral_movement),
                '54': ("精确定位", test_precision_positioning),
                '55': ("双轨道跟随", test_dual_track_following),
                '56': ("组合运动", test_combined_movements),
            }
            
            if choice in test_map:
                test_name, test_func = test_map[choice]
                print(f"\n开始执行测试: {test_name}")
                try:
                    test_func(ctrl, msg)
                    success(f"测试 {test_name} 成功完成", "成功")
                except Exception as e:
                    error(f"测试 {test_name} 失败: {str(e)}", "失败")
                
                input("\n按回车键继续...")
            else:
                print("无效的选择，请重新输入")
                
        except KeyboardInterrupt:
            print("\n用户中断操作")
            break
        except Exception as e:
            error(f"执行测试时出错: {str(e)}", "错误")

def run_all_tests(ctrl, msg):
    """运行所有测试"""
    section('运行所有测试', "开始")
    
    test_suites = [
        ("基础运动测试", run_basic_movement_tests),
        ("弧形运动测试", run_arc_movement_tests),
        ("自定义步态测试", run_custom_gait_tests),
        ("视觉功能测试", run_vision_function_tests),
        ("复杂运动测试", run_complex_movement_tests),
    ]
    
    total_start_time = time.time()
    passed_tests = 0
    failed_tests = 0
    
    for suite_name, test_func in test_suites:
        try:
            info(f"开始执行测试套件: {suite_name}", "测试套件")
            suite_start_time = time.time()
            
            test_func(ctrl, msg)
            
            suite_duration = time.time() - suite_start_time
            success(f"测试套件 {suite_name} 成功完成，耗时: {suite_duration:.2f}秒", "成功")
            passed_tests += 1
            
        except Exception as e:
            suite_duration = time.time() - suite_start_time
            error(f"测试套件 {suite_name} 失败: {str(e)}，耗时: {suite_duration:.2f}秒", "失败")
            failed_tests += 1
        
        # 测试套件之间暂停
        time.sleep(3)
    
    # 总结报告
    total_duration = time.time() - total_start_time
    section('所有测试完成', "总结")
    info(f"总测试时间: {total_duration:.2f}秒", "统计")
    info(f"成功的测试套件: {passed_tests}", "统计")
    info(f"失败的测试套件: {failed_tests}", "统计")
    info(f"总测试套件数: {passed_tests + failed_tests}", "统计")
    
    if failed_tests == 0:
        success("所有测试套件都成功完成！", "完成")
    else:
        warning(f"有 {failed_tests} 个测试套件失败", "警告")

def main_menu(ctrl, msg):
    """主菜单"""
    while True:
        print_menu()
        
        try:
            choice = input("请选择要执行的测试 (输入编号): ").strip()
            
            if choice == '0':
                print("退出测试程序")
                break
            elif choice == '1':
                print("执行基础运动测试...")
                run_basic_movement_tests(ctrl, msg)
            elif choice == '2':
                print("执行弧形运动测试...")
                run_arc_movement_tests(ctrl, msg)
            elif choice == '3':
                print("执行自定义步态测试...")
                run_custom_gait_tests(ctrl, msg)
            elif choice == '4':
                print("执行视觉功能测试...")
                run_vision_function_tests(ctrl, msg)
            elif choice == '5':
                print("执行复杂运动测试...")
                run_complex_movement_tests(ctrl, msg)
            elif choice == '6':
                print("运行所有测试...")
                run_all_tests(ctrl, msg)
            elif choice == '7':
                print("进入单个测试选择...")
                run_single_test_menu(ctrl, msg)
            elif choice == '8':
                print("进入交互式测试模式...")
                run_interactive_test_selection(ctrl, msg)
            else:
                print("无效的选择，请重新输入")
                continue
                
            input("\n按回车键继续...")
            
        except KeyboardInterrupt:
            print("\n用户中断操作")
            break
        except Exception as e:
            error(f"执行测试时出错: {str(e)}", "错误")
            input("\n按回车键继续...")

def print_usage():
    """打印使用说明"""
    print("="*60)
    print("           机器狗子运动函数测试套件")
    print("="*60)
    print("使用方法：")
    print("1. 在Python脚本中导入并使用:")
    print("   from single_test.test_main import main_menu")
    print("   main_menu(ctrl, msg)")
    print()
    print("2. 导入单个测试模块:")
    print("   from single_test.test_basic_movements import run_basic_movement_tests")
    print("   run_basic_movement_tests(ctrl, msg)")
    print()
    print("3. 导入单个测试函数:")
    print("   from single_test.test_basic_movements import test_turn_90_degrees")
    print("   test_turn_90_degrees(ctrl, msg)")
    print()
    print("测试模块说明：")
    print("- test_basic_movements.py: 基础运动测试（旋转、直线移动等）")
    print("- test_arc_movements.py: 弧形运动测试（弧形转弯、绕行等）")
    print("- test_custom_gait.py: 自定义步态测试（俯身、爬坡等）")
    print("- test_vision_functions.py: 视觉功能测试（QR码、箭头检测等）")
    print("- test_complex_movements.py: 复杂运动测试（连续弯道、组合运动等）")
    print("- test_main.py: 主测试脚本（提供菜单选择）")
    print("="*60)

def clear_screen():
    """清屏函数"""
    import os
    os.system('clear' if os.name == 'posix' else 'cls')

def show_help_info():
    """显示帮助信息"""
    print("\n" + "="*60)
    print("                    帮助信息")
    print("="*60)
    print("🎯 测试编号说明:")
    print("  1x: 基础运动测试 (11-16)")
    print("  2x: 弧形运动测试 (21-25)")
    print("  3x: 自定义步态测试 (31-35)")
    print("  4x: 视觉功能测试 (41-46)")
    print("  5x: 复杂运动测试 (51-56)")
    print()
    print("🔧 快捷命令:")
    print("  h/help: 显示此帮助信息")
    print("  s/status: 显示机器狗当前状态")
    print("  c/clear: 清屏")
    print("  0: 返回主菜单")
    print()
    print("💡 使用提示:")
    print("  - 测试前确保机器狗处于安全位置")
    print("  - 每个测试都会显示详细的执行信息")
    print("  - 可以随时按 Ctrl+C 中断测试")
    print("  - 建议先从基础运动测试开始")
    print("="*60)
    input("\n按回车键继续...")

def show_current_status(ctrl):
    """显示机器狗当前状态"""
    try:
        print("\n" + "="*60)
        print("                机器狗当前状态")
        print("="*60)
        
        # 位置信息
        pos = ctrl.odo_msg.xyz
        print(f"📍 位置信息:")
        print(f"  X坐标: {pos[0]:.3f} 米")
        print(f"  Y坐标: {pos[1]:.3f} 米")
        print(f"  Z坐标: {pos[2]:.3f} 米")
        
        # 姿态信息
        rpy = ctrl.odo_msg.rpy
        print(f"\n🧭 姿态信息:")
        print(f"  Roll (横滚):  {rpy[0]:.2f}°")
        print(f"  Pitch (俯仰): {rpy[1]:.2f}°")
        print(f"  Yaw (偏航):   {rpy[2]:.2f}°")
        
        # 速度信息
        vel = ctrl.odo_msg.vxyz
        print(f"\n🏃 速度信息:")
        print(f"  X轴速度: {vel[0]:.3f} 米/秒")
        print(f"  Y轴速度: {vel[1]:.3f} 米/秒")
        print(f"  Z轴速度: {vel[2]:.3f} 米/秒")
        
        # 系统状态
        print(f"\n⚙️ 系统状态:")
        print(f"  连接状态: ✅ 正常")
        print(f"  控制模式: 运动控制")
        
        # 视觉系统状态
        try:
            current_image = ctrl.image_processor.get_current_image()
            if current_image is not None:
                print(f"  相机状态: ✅ 正常 (图像尺寸: {current_image.shape})")
            else:
                print(f"  相机状态: ❌ 无图像")
        except:
            print(f"  相机状态: ❓ 未知")
        
        print("="*60)
        
    except Exception as e:
        print(f"\n❌ 获取状态信息失败: {str(e)}")
        print("请检查机器狗连接状态")
    
    input("\n按回车键继续...")

def run_interactive_test_selection(ctrl, msg):
    """运行交互式测试选择"""
    print("\n🎯 进入交互式测试模式")
    print("您可以通过输入测试编号快速选择测试项目")
    print("输入 'h' 获取帮助，输入 'q' 退出")
    
    while True:
        try:
            choice = input("\n请输入测试编号或命令 > ").strip().lower()
            
            if choice in ['q', 'quit', 'exit']:
                print("退出交互式测试模式")
                break
            elif choice in ['h', 'help']:
                show_help_info()
            elif choice in ['s', 'status']:
                show_current_status(ctrl)
            elif choice in ['c', 'clear']:
                clear_screen()
            elif choice.isdigit() and len(choice) == 2:
                # 运行对应的测试
                run_test_by_number(choice, ctrl, msg)
            else:
                print("❌ 无效输入，请输入两位数字测试编号或命令")
                print("   例如: 11 (原地旋转90度), h (帮助), q (退出)")
                
        except KeyboardInterrupt:
            print("\n用户中断操作")
            break
        except Exception as e:
            error(f"执行过程中出错: {str(e)}", "错误")

def run_test_by_number(test_number, ctrl, msg):
    """根据编号运行测试"""
    # 导入所有测试函数
    from test_basic_movements import (
        test_turn_90_degrees, test_turn_180_degrees, test_go_straight_1m,
        test_go_straight_backward_1m, test_lateral_movement, test_square_movement
    )
    from test_arc_movements import (
        test_arc_turn_90_degrees, test_arc_turn_180_degrees, 
        test_arc_turn_right_90_degrees, test_s_curve_movement, test_different_radius_turns
    )
    from test_custom_gait import (
        test_normal_walking_gait, test_stone_road_gait, test_lie_down_stand_up,
        test_stoop_gait, test_climbing_gait
    )
    from test_vision_functions import (
        test_qr_code_scanning, test_arrow_detection, test_horizontal_line_detection,
        test_sky_analysis, test_yellow_area_analysis, test_movement_with_qr_scanning
    )
    from test_complex_movements import (
        test_continuous_curves, test_figure_eight_movement, test_spiral_movement,
        test_precision_positioning, test_dual_track_following, test_combined_movements
    )
    
    test_map = {
        '11': ("原地旋转90度", test_turn_90_degrees),
        '12': ("原地旋转180度", test_turn_180_degrees),
        '13': ("直线前进1米", test_go_straight_1m),
        '14': ("直线后退1米", test_go_straight_backward_1m),
        '15': ("侧向移动", test_lateral_movement),
        '16': ("正方形移动路径", test_square_movement),
        '21': ("90度弧形转弯", test_arc_turn_90_degrees),
        '22': ("180度弧形转弯", test_arc_turn_180_degrees),
        '23': ("右转90度弧形转弯", test_arc_turn_right_90_degrees),
        '24': ("S形弯道运动", test_s_curve_movement),
        '25': ("不同半径弧形转弯", test_different_radius_turns),
        '31': ("正常行走步态", test_normal_walking_gait),
        '32': ("石板路步态", test_stone_road_gait),
        '33': ("躺下和站起动作", test_lie_down_stand_up),
        '34': ("俯身步态", test_stoop_gait),
        '35': ("爬坡步态", test_climbing_gait),
        '41': ("QR码扫描功能", test_qr_code_scanning),
        '42': ("箭头检测功能", test_arrow_detection),
        '43': ("横线检测功能", test_horizontal_line_detection),
        '44': ("天空分析功能", test_sky_analysis),
        '45': ("黄色区域分析功能", test_yellow_area_analysis),
        '46': ("移动中QR扫描", test_movement_with_qr_scanning),
        '51': ("连续弯道运动", test_continuous_curves),
        '52': ("8字形运动", test_figure_eight_movement),
        '53': ("螺旋运动", test_spiral_movement),
        '54': ("精确定位", test_precision_positioning),
        '55': ("双轨道跟随", test_dual_track_following),
        '56': ("组合运动", test_combined_movements),
    }
    
    if test_number in test_map:
        test_name, test_func = test_map[test_number]
        print(f"\n🚀 开始执行测试: {test_name}")
        try:
            test_func(ctrl, msg)
            success(f"✅ 测试 {test_name} 成功完成", "成功")
        except Exception as e:
            error(f"❌ 测试 {test_name} 失败: {str(e)}", "失败")
    else:
        print(f"❌ 未找到测试编号 {test_number}")

if __name__ == "__main__":
    print_usage()
    print("\n注意：此脚本需要在机器狗控制环境中运行")
    print("请确保已正确初始化 ctrl 和 msg 对象")