import math
import numpy as np

try:
    from lib import RTLFWriter
except ImportError:
    class RTLFWriter:
        def __init__(self, *args, **kwargs): pass
        def __enter__(self): return self
        def __exit__(self, *args): pass
        def write_record(self, *args): pass


def simulate_closed_circuit(filename, car_id=7):
    track_name = "Closed Circuit GP"

    # Real-world reference point (Silverstone-ish)
    base_lat = 52.0733
    base_lon = -1.0147

    dt = 0.05
    lap_time = 95.0
    samples = int(lap_time / dt)

    # Track size in METERS (realistic)
    a = 1200.0   # main straight length scale
    b = 800.0

    theta = np.linspace(0, 2 * math.pi, samples)

    # Track shape (meters)
    x_m = (
        a * np.cos(theta)
        + 220 * np.cos(3 * theta)
        + 90 * np.sin(5 * theta)
    )
    y_m = (
        b * np.sin(theta)
        + 170 * np.sin(2 * theta)
        - 70 * np.cos(4 * theta)
    )

    # Convert meters → degrees
    meters_per_deg_lat = 111_320.0
    meters_per_deg_lon = meters_per_deg_lat * math.cos(math.radians(base_lat))

    lat = base_lat + (y_m / meters_per_deg_lat)
    lon = base_lon + (x_m / meters_per_deg_lon)

    # Derivatives (still done in meters for physics)
    dx = np.gradient(x_m, dt)
    dy = np.gradient(y_m, dt)
    ddx = np.gradient(dx, dt)
    ddy = np.gradient(dy, dt)

    curvature = np.abs(dx * ddy - dy * ddx) / np.power(dx*dx + dy*dy, 1.5)
    curvature = np.nan_to_num(curvature)

    # Speed profile
    max_speed = 82.0   # ~295 km/h
    min_speed = 30.0
    speed = max_speed - 850 * curvature
    speed = np.clip(speed, min_speed, max_speed)

    # Acceleration
    heading_norm = np.hypot(dx, dy)
    ax = np.gradient(speed * dx / heading_norm, dt)
    ay = np.gradient(speed * dy / heading_norm, dt)
    az = np.zeros_like(ax)

    # Forces
    mass = 1250.0
    fx = mass * ax
    fy = mass * ay
    fz = np.ones_like(fx) * mass * 9.81

    with RTLFWriter(filename, car_id, track_name) as writer:
        t = 0.0
        for i in range(samples):
            writer.write_record(
                t,
                float(lon[i]),
                float(lat[i]),
                float(fx[i]),
                float(fy[i]),
                float(fz[i]),
                float(ax[i]),
                float(ay[i]),
                float(az[i]),
                float(speed[i])
            )
            t += dt


if __name__ == "__main__":
    simulate_closed_circuit("closed_loop.rtl")
    print("Closed circuit simulation complete.")