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

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

def test_turn_90_degrees(ctrl, msg, observe=True):
    """测试原地旋转90度"""
    section('测试：原地旋转90度', "旋转")
    info('开始测试原地旋转90度...', "测试")
    
    # 记录初始角度
    initial_angle = ctrl.odo_msg.rpy[2]
    info(f"初始角度: {initial_angle:.2f}度", "角度")
    
    # 执行90度旋转
    turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True)
    
    # 记录最终角度
    final_angle = ctrl.odo_msg.rpy[2]
    angle_diff = final_angle - initial_angle
    info(f"最终角度: {final_angle:.2f}度", "角度")
    info(f"角度变化: {angle_diff:.2f}度", "角度")
    
    success("原地旋转90度测试完成", "完成")

def test_turn_180_degrees(ctrl, msg, observe=True):
    """测试原地旋转180度"""
    section('测试：原地旋转180度', "旋转")
    info('开始测试原地旋转180度...', "测试")
    
    # 记录初始角度
    initial_angle = ctrl.odo_msg.rpy[2]
    info(f"初始角度: {initial_angle:.2f}度", "角度")
    
    # 执行180度旋转
    turn_degree_v2(ctrl, msg, degree=180, absolute=False, precision=True)
    
    # 记录最终角度
    final_angle = ctrl.odo_msg.rpy[2]
    angle_diff = final_angle - initial_angle
    info(f"最终角度: {final_angle:.2f}度", "角度")
    info(f"角度变化: {angle_diff:.2f}度", "角度")
    
    success("原地旋转180度测试完成", "完成")

def test_go_straight_1m(ctrl, msg, observe=True):
    """测试直线前进1米"""
    section('测试：直线前进1米', "移动")
    info('开始测试直线前进1米...', "测试")
    
    # 记录初始位置
    initial_pos = ctrl.odo_msg.xyz
    info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
    
    # 执行1米直线移动
    go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)
    
    # 记录最终位置
    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"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
    info(f"实际移动距离: {distance_moved:.3f}米", "距离")
    
    success("直线前进1米测试完成", "完成")

def test_go_straight_backward_1m(ctrl, msg, observe=True):
    """测试直线后退1米"""
    section('测试：直线后退1米', "移动")
    info('开始测试直线后退1米...', "测试")
    
    # 记录初始位置
    initial_pos = ctrl.odo_msg.xyz
    info(f"初始位置: x={initial_pos[0]:.3f}, y={initial_pos[1]:.3f}", "位置")
    
    # 执行1米直线后退
    go_straight(ctrl, msg, distance=-1.0, speed=0.5, observe=observe)
    
    # 记录最终位置
    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"最终位置: x={final_pos[0]:.3f}, y={final_pos[1]:.3f}", "位置")
    info(f"实际移动距离: {distance_moved:.3f}米", "距离")
    
    success("直线后退1米测试完成", "完成")

def test_lateral_movement(ctrl, msg, observe=True):
    """测试侧向移动"""
    section('测试：侧向移动', "移动")
    
    # 测试右侧移动
    info('开始测试右侧移动0.5米...', "测试")
    initial_pos = ctrl.odo_msg.xyz
    go_lateral(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
    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"右侧移动距离: {distance_moved:.3f}米", "距离")
    
    time.sleep(1)
    
    # 测试左侧移动
    info('开始测试左侧移动0.5米...', "测试")
    initial_pos = ctrl.odo_msg.xyz
    go_lateral(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
    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"左侧移动距离: {distance_moved:.3f}米", "距离")
    
    success("侧向移动测试完成", "完成")

def test_square_movement(ctrl, msg, observe=True):
    """测试正方形移动路径"""
    section('测试：正方形移动路径', "移动")
    info('开始测试正方形移动路径...', "测试")
    
    # 记录起始位置
    start_pos = ctrl.odo_msg.xyz
    info(f"起始位置: x={start_pos[0]:.3f}, y={start_pos[1]:.3f}", "位置")
    
    # 正方形的四条边
    for i in range(4):
        info(f"正方形第{i+1}条边", "移动")
        # 前进1米
        go_straight(ctrl, msg, distance=1.0, speed=0.5, observe=observe)
        time.sleep(0.5)
        # 右转90度
        turn_degree_v2(ctrl, msg, degree=90, absolute=False, precision=True)
        time.sleep(0.5)
    
    # 记录结束位置
    end_pos = ctrl.odo_msg.xyz
    distance_from_start = ((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"与起始位置的距离: {distance_from_start:.3f}米", "距离")
    
    success("正方形移动路径测试完成", "完成")

def run_basic_movement_tests(ctrl, msg):
    """运行所有基础运动测试"""
    section('基础运动测试套件', "开始")
    
    tests = [
        ("原地旋转90度", test_turn_90_degrees),
        ("原地旋转180度", test_turn_180_degrees),
        ("直线前进1米", test_go_straight_1m),
        ("直线后退1米", test_go_straight_backward_1m),
        ("侧向移动", test_lateral_movement),
        ("正方形移动路径", test_square_movement),
    ]
    
    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(2)
    
    success("所有基础运动测试完成", "完成")

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