import json
import math
from dataclasses import dataclass
from typing import List, Dict, Any, Optional
import matplotlib.pyplot as plt
import numpy as np

@dataclass
class MotionData:
    """运动数据点"""
    time: float
    sine: float
    cosine: float
    sineX: float
    cosineX: float
    sineY: float
    cosineY: float
    sineZ: float
    cosineZ: float
    targetX: float
    targetY: float
    targetZ: float
    positionX: float
    positionY: float
    positionZ: float
    accelerationX: float
    accelerationY: float
    accelerationZ: float

class MotionAnalyzer:
    """运动数据分析器"""
    
    def __init__(self):
        self.data_points: List[MotionData] = []
        self.sampling_rate = 100  # Hz
    
    def parse_json_data(self, json_string: str) -> List[MotionData]:
        """解析JSON格式的运动数据"""
        try:
            # 修复JSON格式（添加缺失的引号和括号）
            if not json_string.strip().startswith('['):
                json_string = '[' + json_string + ']'
            
            # 确保是有效的JSON数组
            json_string = json_string.replace('}{', '},{')
            
            data_list = json.loads(json_string)
            motion_data = []
            
            for item in data_list:
                motion_data.append(MotionData(
                    time=item.get('time', 0),
                    sine=item.get('sine', 0),
                    cosine=item.get('cosine', 0),
                    sineX=item.get('sineX', 0),
                    cosineX=item.get('cosineX', 0),
                    sineY=item.get('sineY', 0),
                    cosineY=item.get('cosineY', 0),
                    sineZ=item.get('sineZ', 0),
                    cosineZ=item.get('cosineZ', 0),
                    targetX=item.get('targetX', 0),
                    targetY=item.get('targetY', 0),
                    targetZ=item.get('targetZ', 0),
                    positionX=item.get('positionX', 0),
                    positionY=item.get('positionY', 0),
                    positionZ=item.get('positionZ', 0),
                    accelerationX=item.get('accelerationX', 0),
                    accelerationY=item.get('accelerationY', 0),
                    accelerationZ=item.get('accelerationZ', 0)
                ))
            
            self.data_points = motion_data
            print(f"✅ 成功解析 {len(motion_data)} 个数据点")
            return motion_data
            
        except json.JSONDecodeError as e:
            print(f"❌ JSON解析错误: {e}")
            # 尝试手动解析
            return self._manual_parse(json_string)
        except Exception as e:
            print(f"❌ 解析错误: {e}")
            return []
    
    def _manual_parse(self, data_string: str) -> List[MotionData]:
        """手动解析数据（当JSON格式不标准时）"""
        motion_data = []
        
        # 分割数据点
        data_entries = data_string.split('},{')
        
        for entry in data_entries:
            entry = entry.replace('{', '').replace('}', '').strip()
            if not entry:
                continue
                
            data_dict = {}
            pairs = entry.split(',')
            
            for pair in pairs:
                if ':' in pair:
                    key, value = pair.split(':', 1)
                    key = key.strip().strip('"\'')
                    try:
                        data_dict[key] = float(value.strip())
                    except ValueError:
                        data_dict[key] = 0.0
            
            if data_dict:
                motion_data.append(MotionData(
                    time=data_dict.get('time', 0),
                    sine=data_dict.get('sine', 0),
                    cosine=data_dict.get('cosine', 0),
                    sineX=data_dict.get('sineX', 0),
                    cosineX=data_dict.get('cosineX', 0),
                    sineY=data_dict.get('sineY', 0),
                    cosineY=data_dict.get('cosineY', 0),
                    sineZ=data_dict.get('sineZ', 0),
                    cosineZ=data_dict.get('cosineZ', 0),
                    targetX=data_dict.get('targetX', 0),
                    targetY=data_dict.get('targetY', 0),
                    targetZ=data_dict.get('targetZ', 0),
                    positionX=data_dict.get('positionX', 0),
                    positionY=data_dict.get('positionY', 0),
                    positionZ=data_dict.get('positionZ', 0),
                    accelerationX=data_dict.get('accelerationX', 0),
                    accelerationY=data_dict.get('accelerationY', 0),
                    accelerationZ=data_dict.get('accelerationZ', 0)
                ))
        
        self.data_points = motion_data
        print(f"🔧 手动解析完成，找到 {len(motion_data)} 个数据点")
        return motion_data
    
    def calculate_velocity(self) -> List[Dict[str, float]]:
        """计算速度数据"""
        velocities = []
        
        for i in range(1, len(self.data_points)):
            dt = self.data_points[i].time - self.data_points[i-1].time
            if dt <= 0:
                continue
                
            vel_x = (self.data_points[i].positionX - self.data_points[i-1].positionX) / dt
            vel_y = (self.data_points[i].positionY - self.data_points[i-1].positionY) / dt
            vel_z = (self.data_points[i].positionZ - self.data_points[i-1].positionZ) / dt
            
            velocities.append({
                'time': self.data_points[i].time,
                'velocityX': vel_x,
                'velocityY': vel_y,
                'velocityZ': vel_z,
                'speed': math.sqrt(vel_x**2 + vel_y**2 + vel_z**2)
            })
        
        return velocities
    
    def calculate_trajectory_error(self) -> Dict[str, Any]:
        """计算轨迹误差"""
        if not self.data_points:
            return {}
        
        errors = []
        for point in self.data_points:
            error_x = point.targetX - point.positionX
            error_y = point.targetY - point.positionY
            error_z = point.targetZ - point.positionZ
            total_error = math.sqrt(error_x**2 + error_y**2 + error_z**2)
            
            errors.append({
                'time': point.time,
                'errorX': error_x,
                'errorY': error_y,
                'errorZ': error_z,
                'totalError': total_error
            })
        
        avg_error = np.mean([e['totalError'] for e in errors])
        max_error = np.max([e['totalError'] for e in errors])
        
        return {
            'errors': errors,
            'averageError': avg_error,
            'maxError': max_error,
            'rmsError': math.sqrt(np.mean([e['totalError']**2 for e in errors]))
        }
    
    def analyze_motion_pattern(self) -> Dict[str, Any]:
        """分析运动模式"""
        if len(self.data_points) < 2:
            return {}
        
        times = [p.time for p in self.data_points]
        positions_x = [p.positionX for p in self.data_points]
        positions_y = [p.positionY for p in self.data_points]
        
        # 计算频率成分
        if len(times) > 1:
            dt = times[1] - times[0]
            freqs = np.fft.fftfreq(len(times), dt)
            fft_x = np.fft.fft(positions_x)
            fft_y = np.fft.fft(positions_y)
            
            # 找到主要频率
            dominant_freq_x = abs(freqs[np.argmax(np.abs(fft_x))])
            dominant_freq_y = abs(freqs[np.argmax(np.abs(fft_y))])
        else:
            dominant_freq_x = dominant_freq_y = 0
        
        return {
            'duration': times[-1] - times[0] if times else 0,
            'dataPoints': len(self.data_points),
            'dominantFrequencyX': dominant_freq_x,
            'dominantFrequencyY': dominant_freq_y,
            'samplingRate': self.sampling_rate
        }
    
    def plot_trajectory(self, save_path: Optional[str] = None):
        """绘制运动轨迹"""
        if not self.data_points:
            print("❌ 没有数据可绘制")
            return
        
        fig, axes = plt.subplots(2, 2, figsize=(15, 10))
        
        # 位置轨迹
        times = [p.time for p in self.data_points]
        axes[0, 0].plot(times, [p.positionX for p in self.data_points], 'r-', label='X位置')
        axes[0, 0].plot(times, [p.targetX for p in self.data_points], 'r--', label='X目标', alpha=0.7)
        axes[0, 0].set_ylabel('X轴位置')
        axes[0, 0].legend()
        axes[0, 0].grid(True)
        
        axes[0, 1].plot(times, [p.positionY for p in self.data_points], 'g-', label='Y位置')
        axes[0, 1].plot(times, [p.targetY for p in self.data_points], 'g--', label='Y目标', alpha=0.7)
        axes[0, 1].set_ylabel('Y轴位置')
        axes[0, 1].legend()
        axes[0, 1].grid(True)
        
        # 误差分析
        error_data = self.calculate_trajectory_error()
        axes[1, 0].plot([e['time'] for e in error_data['errors']], 
                       [e['totalError'] for e in error_data['errors']], 'b-')
        axes[1, 0].set_ylabel('总误差')
        axes[1, 0].set_xlabel('时间 (s)')
        axes[1, 0].grid(True)
        
        # 3D轨迹
        ax_3d = fig.add_subplot(2, 2, 4, projection='3d')
        ax_3d.plot([p.positionX for p in self.data_points],
                  [p.positionY for p in self.data_points],
                  [p.positionZ for p in self.data_points], 'b-', alpha=0.7)
        ax_3d.scatter([p.targetX for p in self.data_points],
                     [p.targetY for p in self.data_points],
                     [p.targetZ for p in self.data_points], c='red', marker='o')
        ax_3d.set_xlabel('X')
        ax_3d.set_ylabel('Y')
        ax_3d.set_zlabel('Z')
        
        plt.tight_layout()
        
        if save_path:
            plt.savefig(save_path, dpi=300, bbox_inches='tight')
            print(f"💾 图表已保存到: {save_path}")
        
        plt.show()
    
    def export_to_csv(self, filename: str = "motion_data.csv"):
        """导出数据到CSV文件"""
        import csv
        
        with open(filename, 'w', newline='', encoding='utf-8') as csvfile:
            writer = csv.writer(csvfile)
            # 写入标题
            writer.writerow(['time', 'sine', 'cosine', 'positionX', 'positionY', 'positionZ', 
                           'targetX', 'targetY', 'targetZ', 'accelerationX', 'accelerationY', 'accelerationZ'])
            
            # 写入数据
            for point in self.data_points:
                writer.writerow([
                    point.time, point.sine, point.cosine,
                    point.positionX, point.positionY, point.positionZ,
                    point.targetX, point.targetY, point.targetZ,
                    point.accelerationX, point.accelerationY, point.accelerationZ
                ])
        
        print(f"💾 数据已导出到: {filename}")
    
    def generate_report(self) -> Dict[str, Any]:
        """生成分析报告"""
        motion_pattern = self.analyze_motion_pattern()
        error_analysis = self.calculate_trajectory_error()
        velocities = self.calculate_velocity()
        
        report = {
            'basic_info': motion_pattern,
            'error_analysis': error_analysis,
            'velocity_stats': {
                'avg_speed': np.mean([v['speed'] for v in velocities]) if velocities else 0,
                'max_speed': np.max([v['speed'] for v in velocities]) if velocities else 0,
            },
            'data_quality': {
                'total_points': len(self.data_points),
                'time_range': f"{self.data_points[0].time:.3f}s - {self.data_points[-1].time:.3f}s" if self.data_points else "N/A",
                'data_completeness': '完整' if len(self.data_points) > 10 else '数据量不足'
            }
        }
        
        return report

# 使用示例
def main():
    # 你的数据样本
    sample_data = """{"time":0.23,"sine":0.227978,"cosine":0.973666,"sineX":0.227978,"cosineX":0.973666,"sineY":0.227978,"cosineY":0.973666,"sineZ":0.227978,"cosineZ":0.973666,"targetX":0.000000,"targetY":1.800000,"targetZ":0.000000,"positionX":0.227978,"positionY":1.792548,"positionZ":0.000000,"accelerationX":0.000000,"accelerationY":-9.810000,"accelerationZ":0.000000},{"time":0.24,"sine":0.237703,"cosine":0.971338,"sineX":0.237703,"cosineX":0.971338,"sineY":0.237703,"cosineY":0.971338,"sineZ":0.237703,"cosineZ":0.971338,"targetX":0.000000,"targetY":1.800000,"targetZ":0.000000,"positionX":0.237703,"positionY":1.792212,"positionZ":0.000000,"accelerationX":0.000000,"accelerationY":-9.810000,"accelerationZ":0.000000},{"time":0.25,"sine":0.247404,"cosine":0.968912,"sineX":0.247404,"cosineX":0.968912,"sineY":0.247404,"cosineY":0.968912,"sineZ":0.247404,"cosineZ":0.968912,"targetX":0.000000,"targetY":1.800000,"targetZ":0.000000,"positionX":0.247404,"positionY":1.791877,"positionZ":0.000000,"accelerationX":0.000000,"accelerationY":-9.810000,"accelerationZ":0.000000}"""
    
    # 创建分析器
    analyzer = MotionAnalyzer()
    
    # 解析数据
    motion_data = analyzer.parse_json_data(sample_data)
    
    if motion_data:
        # 显示前几个数据点
        for i, point in enumerate(motion_data[:3]):
            print(f"数据点 {i+1}: 时间={point.time:.3f}s, 位置=({point.positionX:.3f}, {point.positionY:.3f}, {point.positionZ:.3f})")
        
        # 生成分析报告
        report = analyzer.generate_report()
        print(f"\n📊 分析报告:")
        print(f"数据点数: {report['basic_info']['dataPoints']}")
        print(f"持续时间: {report['basic_info']['duration']:.3f}s")
        print(f"平均误差: {report['error_analysis']['averageError']:.6f}")
        print(f"最大误差: {report['error_analysis']['maxError']:.6f}")
        
        # 导出数据
        analyzer.export_to_csv("motion_analysis.csv")
        
        # 绘制图表（可选，需要matplotlib）
        # analyzer.plot_trajectory("trajectory_plot.png")
        
    else:
        print("❌ 数据解析失败")

if __name__ == "__main__":
    main()