import math
from collections import defaultdict

def analyze_speeds(filepath):
    print(f"Analyzing speeds in {filepath}...")
    
    state_data = defaultdict(list)
    
    prev_x = None
    prev_y = None
    prev_time = None
    prev_heading = None
    
    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) < 10:
            continue
            
        try:
            time_ms = int(parts[2])
            state = parts[3]
            pwm_thr = int(parts[4])
            pwm_str = int(parts[5])
            x = float(parts[6])
            y = float(parts[7])
            heading = float(parts[9])
            
            if prev_x is not None and prev_y is not None:
                dx = x - prev_x
                dy = y - prev_y
                dist = math.sqrt(dx*dx + dy*dy)
                dt = (time_ms - prev_time) / 1000.0
                
                if dt > 0:
                    speed = dist / dt
                    
                    # Calculate yaw rate
                    d_head = heading - prev_heading
                    if d_head > 180: d_head -= 360
                    if d_head < -180: d_head += 360
                    yaw_rate = d_head / dt # deg/s
                    
                    if 0 < dt < 0.5: 
                        state_data[state].append({
                            'speed': speed,
                            'yaw_rate': yaw_rate,
                            'pwm_thr': pwm_thr,
                            'pwm_str': pwm_str,
                            'heading': heading
                        })
            
            prev_x = x
            prev_y = y
            prev_time = time_ms
            prev_heading = heading
            
        except ValueError:
            continue

    print("\n=== Average Speeds per State ===")
    sorted_states = sorted(state_data.keys())
    
    for state in sorted_states:
        if 'REST' in state or 'IDLE' in state or 'BRAKE' in state or 'WARMUP' in state:
            continue
            
        speeds = [d['speed'] for d in state_data[state]]
        yaw_rates = [d['yaw_rate'] for d in state_data[state]]
        pwms_t = [d['pwm_thr'] for d in state_data[state]]
        # pwm_str is 6th column in log, but 4th in parts (0-based index: 0=$CAL, 1=cnt, 2=time, 3=state, 4=thr, 5=str)
        # Wait, in the code: pwm_thr = int(parts[4]), pwm_str = int(parts[5])
        pwms_s = [d['pwm_str'] for d in state_data[state]]
        
        if not speeds:
            continue
            
        avg_speed = sum(speeds) / len(speeds)
        avg_yaw = sum(yaw_rates) / len(yaw_rates)
        avg_pwm_t = sum(pwms_t) / len(pwms_t)
        avg_pwm_s = sum(pwms_s) / len(pwms_s)
        count = len(speeds)
        
        print(f"{state:<15} | Thr: {avg_pwm_t:.0f} | Str: {avg_pwm_s:.0f} | Speed: {avg_speed:.3f} m/s | Yaw: {avg_yaw:.3f} deg/s | N: {count}")

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