import math

def check_jumps(filepath):
    print(f"Checking {filepath} for GPS jumps...")
    
    prev_x = None
    prev_y = None
    prev_time = None
    
    jumps = []
    
    with open(filepath, 'r', encoding='utf-8', errors='ignore') as f:
        lines = f.readlines()
        
    for i, line in enumerate(lines):
        line = line.strip()
        if not line.startswith('$CAL'):
            continue
            
        parts = line.split(',')
        # Expected format has about 18 fields
        if len(parts) < 10:
            continue
            
        try:
            # Extract time and coordinates
            # $CAL,count,time,state,pwm_t,pwm_s,x,y,z,...
            time_ms = int(parts[2])
            x = float(parts[6])
            y = float(parts[7])
            
            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
                    # Threshold: e.g., speed > 3.0 m/s (10.8 km/h) is suspicious for a mower
                    # or just absolute jump > 0.3m in one frame (approx 50ms) -> 6m/s
                    if speed > 5.0 or dist > 0.5: 
                        jumps.append({
                            'line': i+1,
                            'time': time_ms,
                            'prev_time': prev_time,
                            'dist': dist,
                            'speed': speed,
                            'pos': (x, y),
                            'prev_pos': (prev_x, prev_y)
                        })
            
            prev_x = x
            prev_y = y
            prev_time = time_ms
            
        except ValueError:
            continue
            
    if not jumps:
        print("No significant GPS jumps detected.")
    else:
        print(f"Detected {len(jumps)} potential GPS jumps:")
        for jump in jumps[:20]: # Show first 20
            print(f"  Line {jump['line']}: Time {jump['time']}ms, Jump {jump['dist']:.3f}m, Speed {jump['speed']:.2f}m/s, Pos {jump['prev_pos']} -> {jump['pos']}")
        if len(jumps) > 20:
            print(f"  ... and {len(jumps)-20} more.")

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