import math
from collections import defaultdict
import re

def analyze_turn_rates(filepath):
    print(f"Analyzing turn rates in {filepath} using Z-gyro...")
    
    # Format: $CAL,count,time,state,pwm_t,pwm_s,x,y,z,head,pitch,roll,gx,gy,gz,ax,ay,az
    
    state_data = defaultdict(list)
    
    with open(filepath, 'r', encoding='utf-8', errors='ignore') as f:
        lines = f.readlines()
        
    for line in lines:
        line = line.strip()
        if not line.startswith('$CAL'):
            continue
            
        parts = line.split(',')
        if len(parts) < 15:
            continue
            
        try:
            state = parts[3]
            pwm_thr = int(parts[4])
            pwm_str = int(parts[5])
            
            # Get gyro Z (15th element, index 14)
            # Unit check: Usually LSB or deg/s or rad/s. User said "Z轴角速度挺大的"
            # If raw LSB, we need scale. If processed, maybe deg/s.
            # Let's look at values: e.g. -1000, -1200.
            # Assuming raw LSB for now, let's see the range.
            gz_raw = int(parts[14])
            
            # Also check heading change rate for verification if GPS heading is valid
            heading = float(parts[9])
            
            state_data[state].append({
                'gz': gz_raw,
                'pwm_thr': pwm_thr,
                'pwm_str': pwm_str,
                'heading': heading
            })
            
        except ValueError:
            continue

    print("\n=== Analysis of Turning Speeds ===")
    print(f"{'State':<15} | {'PWM_Str':<8} | {'Avg Gyro Z':<10} | {'Est. Deg/s':<10}")
    print("-" * 60)
    
    results = []
    
    sorted_states = sorted(state_data.keys())
    for state in sorted_states:
        if 'TURN' not in state:
            continue
            
        data = state_data[state]
        gz_values = [d['gz'] for d in data]
        pwm_str = data[0]['pwm_str']
        
        if not gz_values:
            continue
            
        # Calculate average
        avg_gz = sum(gz_values) / len(gz_values)
        
        # Try to estimate actual deg/s from Gyro LSB
        # Typical IMU scale (e.g. BMI088, MPU6050) at +/- 2000dps is ~16.4 LSB/(deg/s)
        # at +/- 250dps is ~131 LSB/(deg/s)
        # Let's guess scale based on user feedback or typical values.
        # If raw values are around 1000, and it's turning visibly...
        # 1000 / 16.4 = 60 deg/s. 
        # 1000 / 131 = 7.6 deg/s.
        # Let's just report raw first.
        
        results.append((state, pwm_str, avg_gz))
        print(f"{state:<15} | {pwm_str:<8} | {avg_gz:<10.1f} | {avg_gz/16.4:<10.1f}?")

    print("\n=== Proposed Mapping Table ===")
    # Separate Left and Right turns
    left_turns = []
    right_turns = []
    
    for state, pwm, gz in results:
        if 'TURN_L' in state:
            left_turns.append((pwm, gz))
        elif 'TURN_R' in state:
            right_turns.append((pwm, gz))
            
    # Sort by PWM deviation from 1500
    left_turns.sort(key=lambda x: x[0]) # 1000, 1100...
    right_turns.sort(key=lambda x: x[0]) # 1600, 1700...

    print("PWM  | Gyro Z (Avg) | Direction")
    for pwm, gz in left_turns:
         print(f"{pwm:<4} | {gz:<12.1f} | Left")
    for pwm, gz in right_turns:
         print(f"{pwm:<4} | {gz:<12.1f} | Right")

if __name__ == "__main__":
    analyze_turn_rates("python/calibration_20251205_105312.log")