import sys
import os
import time
import argparse

sys.path.append(os.path.dirname(os.path.abspath(__file__)))
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

def main():
    """
    测试双轨道线居中功能
    """
    # 解析命令行参数
    parser = argparse.ArgumentParser(description='测试双轨道线居中功能')
    parser.add_argument('--distance', type=float, default=0.0,
                        help='居中后要前进的距离(米)，默认为0，即仅执行居中操作')
    parser.add_argument('--speed', type=float, default=0.5,
                        help='前进速度(米/秒)，默认为0.5')
    parser.add_argument('--max_time', type=float, default=15.0,
                        help='最大居中时间(秒)，默认为15秒')
    parser.add_argument('--stone_path', action='store_true',
                        help='强制使用石板路模式')
    parser.add_argument('--normal_path', action='store_true',
                        help='强制使用普通路径模式')
    args = parser.parse_args()
    
    # 初始化日志
    LogHelper.init('dual_track_centering')
    section("双轨道线居中测试", "开始")
    
    try:
        # 导入机器人控制模块
        from robot_controller import RobotCtrl
        
        # 初始化机器人控制器
        ctrl = RobotCtrl(observe=True)
        msg = ctrl.initialize()
        
        if msg is None:
            error("初始化机器人控制器失败", "失败")
            return
        
        info("机器人控制器初始化成功", "初始化")
        
        # 等待机器人准备就绪
        time.sleep(1)
        
        # 确定使用哪种模式
        stone_path_mode = None  # 默认自动检测
        if args.stone_path:
            stone_path_mode = True
            info("强制使用石板路模式", "模式")
        elif args.normal_path:
            stone_path_mode = False
            info("强制使用普通路径模式", "模式")
        else:
            info("使用自动检测模式", "模式")
        
        # 导入双轨道线居中模块
        from base_move.center_on_dual_tracks import center_on_dual_tracks, center_and_follow_dual_tracks
        
        # 执行居中操作
        if args.distance > 0:
            # 居中后沿轨道前进
            info(f"执行居中并前进 {args.distance} 米，速度 {args.speed} m/s", "任务")
            success = center_and_follow_dual_tracks(
                ctrl, msg,
                distance=args.distance,
                speed=args.speed,
                max_centering_time=args.max_time,
                observe=True,
                stone_path_mode=stone_path_mode
            )
        else:
            # 仅执行居中
            info("仅执行居中操作", "任务")
            success = center_on_dual_tracks(
                ctrl, msg,
                max_time=args.max_time,
                observe=True,
                stone_path_mode=stone_path_mode
            )
        
        # 显示结果
        if success:
            success("测试成功完成", "结果")
        else:
            warning("测试未完全成功", "结果")
        
        # 清理资源
        ctrl.cleanup()
        
    except KeyboardInterrupt:
        warning("用户中断测试", "中断")
    except Exception as e:
        error(f"测试过程中发生异常: {str(e)}", "异常")
    
    section("双轨道线居中测试", "结束")

if __name__ == "__main__":
    main()