#!/usr/bin/env python3
import rospy
import math
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
import tf

class FinalCircleController:
    def __init__(self):
        rospy.init_node('final_circle_controller')
        
        # 经过调整的控制器参数
        self.target_radius = 1.5  # 减小半径，更容易观察
        self.linear_speed = 0.5   # 降低速度，更稳定
        self.center_x = 0.0
        self.center_y = 1.5
        
        # 计算角速度
        self.angular_speed = self.linear_speed / self.target_radius
        
        # 发布器
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        
        # 订阅器
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.imu_sub = rospy.Subscriber('/imu/data', Imu, self.imu_callback)
        
        self.last_odom_time = rospy.Time.now()
        self.odom_count = 0
        self.trajectory_points = []
        
        rospy.loginfo("=== 最终圆形轨迹控制器 ===")
        rospy.loginfo(f"目标半径: {self.target_radius}米")
        rospy.loginfo(f"圆心坐标: ({self.center_x}, {self.center_y})")
        rospy.loginfo(f"线速度: {self.linear_speed}米/秒")
        rospy.loginfo(f"角速度: {self.angular_speed:.3f}弧度/秒")
        rospy.loginfo("等待Gazebo启动...")
        
    def odom_callback(self, msg):
        # 每秒显示一次里程数据
        current_time = rospy.Time.now()
        if (current_time - self.last_odom_time).to_sec() >= 1.0:
            x = msg.pose.pose.position.x
            y = msg.pose.pose.position.y
            
            # 记录轨迹点
            self.trajectory_points.append((x, y))
            if len(self.trajectory_points) > 50:
                self.trajectory_points.pop(0)
            
            # 计算到圆心的距离
            distance_to_center = math.sqrt((x - self.center_x)**2 + (y - self.center_y)**2)
            
            # 获取朝向
            orientation = msg.pose.pose.orientation
            (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(
                [orientation.x, orientation.y, orientation.z, orientation.w])
            
            # 获取速度
            vx = msg.twist.twist.linear.x
            vy = msg.twist.twist.linear.y
            actual_speed = math.sqrt(vx**2 + vy**2)
            angular_speed = msg.twist.twist.angular.z
            
            # 计算轨迹误差
            radius_error = abs(distance_to_center - self.target_radius)
            
            rospy.loginfo(f"里程[{self.odom_count:03d}]: "
                         f"位置=({x:6.3f}, {y:6.3f}) | "
                         f"半径误差={radius_error:5.3f}m | "
                         f"速度={actual_speed:4.2f}m/s | "
                         f"朝向={math.degrees(yaw):6.1f}°")
            
            # 每10秒显示轨迹质量
            if self.odom_count % 10 == 0 and len(self.trajectory_points) > 10:
                self.analyze_trajectory()
            
            self.last_odom_time = current_time
            self.odom_count += 1
    
    def imu_callback(self, msg):
        # 显示IMU数据（可选）
        if self.odom_count % 20 == 0:  # 减少显示频率
            accel = msg.linear_acceleration
            gyro = msg.angular_velocity
            rospy.loginfo(f"IMU - 加速度: [{accel.x:5.2f}, {accel.y:5.2f}, {accel.z:5.2f}] | "
                         f"角速度: [{gyro.x:5.2f}, {gyro.y:5.2f}, {gyro.z:5.2f}]")
    
    def analyze_trajectory(self):
        """分析轨迹质量"""
        if len(self.trajectory_points) < 5:
            return
            
        # 计算平均半径和标准差
        radii = []
        for x, y in self.trajectory_points:
            radius = math.sqrt((x - self.center_x)**2 + (y - self.center_y)**2)
            radii.append(radius)
        
        avg_radius = sum(radii) / len(radii)
        radius_std = math.sqrt(sum((r - avg_radius)**2 for r in radii) / len(radii))
        
        rospy.loginfo(f"轨迹分析: 平均半径={avg_radius:.3f}m, 标准差={radius_std:.3f}m")
        
        if radius_std < 0.05:
            rospy.loginfo("✅ 轨迹质量: 优秀 - 圆形轨迹稳定")
        elif radius_std < 0.1:
            rospy.loginfo("⚠️  轨迹质量: 良好 - 轻微波动")
        elif radius_std < 0.2:
            rospy.loginfo("❌ 轨迹质量: 一般 - 需要调整参数")
        else:
            rospy.loginfo("🚫 轨迹质量: 差 - 参数需要重大调整")
    
    def run(self):
        # 等待Gazebo完全启动
        rospy.sleep(3.0)
        rospy.loginfo("开始发布控制命令...")
        
        rate = rospy.Rate(20)  # 20Hz控制频率
        
        while not rospy.is_shutdown():
            try:
                twist_msg = Twist()
                twist_msg.linear.x = self.linear_speed
                twist_msg.angular.z = self.angular_speed
                
                self.cmd_pub.publish(twist_msg)
                rate.sleep()
                
            except Exception as e:
                rospy.logerr(f"控制循环错误: {e}")
                break

if __name__ == '__main__':
    try:
        controller = FinalCircleController()
        controller.run()
    except rospy.ROSInterruptException:
        rospy.loginfo("控制器正常退出")
    except Exception as e:
        rospy.logerr(f"控制器启动错误: {e}")