#!/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 base_move.move_base_hori_line import arc_turn_around_hori_line
from base_move.turn_degree import turn_degree_v2
from base_move.go_straight import go_straight
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing

# 创建日志记录器
logger = get_logger("弧形运动测试")

def test_arc_turn_90_degrees(ctrl, msg, observe=True):
    """测试90度弧形转弯"""
    section('测试：90度弧形转弯', "弧形转弯")
    info('开始测试90度弧形转弯...', "测试")
    
    # 记录初始位置和角度
    initial_pos = ctrl.odo_msg.xyz
    initial_angle = ctrl.odo_msg.rpy[2]
    info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
    info(f"初始角度: {initial_angle:.2f}度", "角度")
    
    # 执行90度弧形转弯
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=90,  # 90度左转
        radius=0.5,    # 0.5米转弯半径
        observe=observe,
        pass_align=True,
        no_end_reset=True,
    )
    
    # 记录最终位置和角度
    final_pos = ctrl.odo_msg.xyz
    final_angle = ctrl.odo_msg.rpy[2]
    distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
    angle_diff = final_angle - initial_angle
    
    info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
    info(f"最终角度: {final_angle:.2f}度", "角度")
    info(f"移动距离: {distance_moved:.3f}米", "距离")
    info(f"角度变化: {angle_diff:.2f}度", "角度")
    
    if turn_success:
        success("90度弧形转弯测试成功", "成功")
        if res:
            info(f"转弯半径: {res.get('radius', 'N/A')}", "参数")
    else:
        error("90度弧形转弯测试失败", "失败")

def test_arc_turn_180_degrees(ctrl, msg, observe=True):
    """测试180度弧形转弯"""
    section('测试：180度弧形转弯', "弧形转弯")
    info('开始测试180度弧形转弯...', "测试")
    
    # 记录初始位置和角度
    initial_pos = ctrl.odo_msg.xyz
    initial_angle = ctrl.odo_msg.rpy[2]
    info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
    info(f"初始角度: {initial_angle:.2f}度", "角度")
    
    # 执行180度弧形转弯
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=180,  # 180度转弯
        radius=0.6,     # 0.6米转弯半径
        observe=observe,
        pass_align=True,
        no_end_reset=True,
    )
    
    # 记录最终位置和角度
    final_pos = ctrl.odo_msg.xyz
    final_angle = ctrl.odo_msg.rpy[2]
    distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
    angle_diff = final_angle - initial_angle
    
    info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
    info(f"最终角度: {final_angle:.2f}度", "角度")
    info(f"移动距离: {distance_moved:.3f}米", "距离")
    info(f"角度变化: {angle_diff:.2f}度", "角度")
    
    if turn_success:
        success("180度弧形转弯测试成功", "成功")
        if res:
            info(f"转弯半径: {res.get('radius', 'N/A')}", "参数")
    else:
        error("180度弧形转弯测试失败", "失败")

def test_arc_turn_right_90_degrees(ctrl, msg, observe=True):
    """测试右转90度弧形转弯"""
    section('测试：右转90度弧形转弯', "弧形转弯")
    info('开始测试右转90度弧形转弯...', "测试")
    
    # 记录初始位置和角度
    initial_pos = ctrl.odo_msg.xyz
    initial_angle = ctrl.odo_msg.rpy[2]
    info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
    info(f"初始角度: {initial_angle:.2f}度", "角度")
    
    # 执行右转90度弧形转弯
    turn_success, res = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=-90,  # 负值表示右转
        radius=0.5,     # 0.5米转弯半径
        observe=observe,
        pass_align=True,
        no_end_reset=True,
    )
    
    # 记录最终位置和角度
    final_pos = ctrl.odo_msg.xyz
    final_angle = ctrl.odo_msg.rpy[2]
    distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
    angle_diff = final_angle - initial_angle
    
    info(f"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
    info(f"最终角度: {final_angle:.2f}度", "角度")
    info(f"移动距离: {distance_moved:.3f}米", "距离")
    info(f"角度变化: {angle_diff:.2f}度", "角度")
    
    if turn_success:
        success("右转90度弧形转弯测试成功", "成功")
        if res:
            info(f"转弯半径: {res.get('radius', 'N/A')}", "参数")
    else:
        error("右转90度弧形转弯测试失败", "失败")

def test_s_curve_movement(ctrl, msg, observe=True):
    """测试S形弯道运动"""
    section('测试：S形弯道运动', "复杂运动")
    info('开始测试S形弯道运动...', "测试")
    
    # 记录起始位置
    start_pos = ctrl.odo_msg.xyz
    info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
    
    # 第一个弧形转弯（左转）
    info("执行第一个弧形转弯（左转90度）", "运动")
    turn_success1, res1 = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=90,
        radius=0.5,
        observe=observe,
        pass_align=True,
        no_end_reset=True,
    )
    
    time.sleep(1)
    
    # 直线段
    info("执行直线段", "运动")
    go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)
    
    time.sleep(1)
    
    # 第二个弧形转弯（右转）
    info("执行第二个弧形转弯（右转90度）", "运动")
    turn_success2, res2 = arc_turn_around_hori_line(
        ctrl=ctrl,
        msg=msg,
        angle_deg=-90,
        radius=0.5,
        observe=observe,
        pass_align=True,
        no_end_reset=True,
    )
    
    # 记录结束位置
    end_pos = ctrl.odo_msg.xyz
    total_distance = ((end_pos[0] - start_pos[0])**2 + (end_pos[1] - start_pos[1])**2)**0.5
    info(f"结束位置: x={end_pos[0]:.3f}, y={end_pos[1]:.3f}", "位置")
    info(f"总移动距离: {total_distance:.3f}米", "距离")
    
    if turn_success1 and turn_success2:
        success("S形弯道运动测试成功", "成功")
    else:
        error("S形弯道运动测试失败", "失败")

def test_different_radius_turns(ctrl, msg, observe=True):
    """测试不同半径的弧形转弯"""
    section('测试：不同半径的弧形转弯', "弧形转弯")
    
    radii = [0.3, 0.5, 0.8, 1.0]  # 不同的转弯半径
    
    for radius in radii:
        info(f'开始测试半径为{radius}米的弧形转弯...', "测试")
        
        # 记录初始位置
        initial_pos = ctrl.odo_msg.xyz
        
        # 执行弧形转弯
        turn_success, res = arc_turn_around_hori_line(
            ctrl=ctrl,
            msg=msg,
            angle_deg=90,
            radius=radius,
            observe=observe,
            pass_align=True,
            no_end_reset=True,
        )
        
        # 记录最终位置
        final_pos = ctrl.odo_msg.xyz
        distance_moved = ((final_pos[0] - initial_pos[0])**2 + (final_pos[1] - initial_pos[1])**2)**0.5
        
        info(f"半径{radius}米转弯完成，移动距离: {distance_moved:.3f}米", "结果")
        
        if turn_success:
            success(f"半径{radius}米弧形转弯测试成功", "成功")
        else:
            error(f"半径{radius}米弧形转弯测试失败", "失败")
        
        # 每次测试之间暂停
        time.sleep(2)
        
        # 回到原始方向
        turn_degree_v2(ctrl, msg, degree=0, absolute=True, precision=True)
        time.sleep(1)

def run_arc_movement_tests(ctrl, msg):
    """运行所有弧形运动测试"""
    section('弧形运动测试套件', "开始")
    
    tests = [
        ("90度弧形转弯", test_arc_turn_90_degrees),
        ("180度弧形转弯", test_arc_turn_180_degrees),
        ("右转90度弧形转弯", test_arc_turn_right_90_degrees),
        ("S形弯道运动", test_s_curve_movement),
        ("不同半径弧形转弯", test_different_radius_turns),
    ]
    
    for test_name, test_func in tests:
        try:
            info(f"开始执行测试: {test_name}", "测试")
            test_func(ctrl, msg)
            success(f"测试 {test_name} 成功完成", "成功")
        except Exception as e:
            error(f"测试 {test_name} 失败: {str(e)}", "失败")
        
        # 每个测试之间暂停
        time.sleep(3)
    
    success("所有弧形运动测试完成", "完成")

if __name__ == "__main__":
    # 这里可以添加独立运行的代码
    print("弧形运动测试脚本")
    print("使用方法：")
    print("from single_test.test_arc_movements import run_arc_movement_tests")
    print("run_arc_movement_tests(ctrl, msg)")