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 = "FIA constraint GP"

    # === Reference GPS location (realistic) ===
    base_lat = 52.0733
    base_lon = -1.0147

    # === Simulation settings ===
    dt = 0.05
    lap_time = 92.0
    samples = int(lap_time / dt)

    # === Track geometry (meters) ===
    a = 1100.0
    b = 750.0

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

    x_m = (
        a * np.cos(theta)
        + 200 * np.cos(3 * theta)
        + 70 * np.sin(5 * theta)
    )
    y_m = (
        b * np.sin(theta)
        + 160 * np.sin(2 * theta)
        - 60 * np.cos(4 * theta)
    )

    # === Elevation profile (meters) ===
    z_m = (
        12 * np.sin(theta)
        + 6 * np.sin(3 * theta)
    )

    # === Derivatives (meters) ===
    dx = np.gradient(x_m, dt)
    dy = np.gradient(y_m, dt)
    ddx = np.gradient(dx, dt)
    ddy = np.gradient(dy, dt)

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

    # === Banking angle (radians) ===
    banking = 0.08 * np.tanh(800 * curvature)

    # === Grip-limited speed model ===
    g = 9.81
    mu = 1.6  # racing slicks
    v_max_curve = np.sqrt((mu * g * (1 + np.sin(banking))) / (curvature + 1e-6))

    speed = np.clip(v_max_curve, 30.0, 83.0)

    # === Acceleration ===
    norm = np.hypot(dx, dy)
    vx = speed * dx / norm
    vy = speed * dy / norm

    ax = np.gradient(vx, dt)
    ay = np.gradient(vy, dt)
    az = np.gradient(np.gradient(z_m, dt), dt)

    # === Forces ===
    mass = 1250.0
    fx = mass * ax
    fy = mass * ay
    fz = mass * (g + az)

    # === Convert meters → lat/long ===
    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)

    # === OPTIONAL GPS noise (centimeter scale) ===
    gps_noise_m = 0.25
    lat += np.random.normal(0, gps_noise_m / meters_per_deg_lat, samples)
    lon += np.random.normal(0, gps_noise_m / meters_per_deg_lon, samples)

    # === Force exact loop closure ===
    lat[-1] = lat[0]
    lon[-1] = lon[0]

    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("realistic.rtl")
    print("Realistic simulation complete.")