"""2D kinematic bicycle simulator with ray-cast lidar and collision checks."""
from __future__ import annotations

from dataclasses import dataclass, field
from typing import List, Tuple

import math
import numpy as np


@dataclass
class Car:
    x: float = 0.0
    y: float = 0.0
    heading: float = 0.0   # radians
    speed: float = 0.0     # m/s
    length: float = 4.5
    width: float = 1.8


@dataclass
class Obstacle:
    x: float
    y: float
    w: float
    h: float  # half-extents aligned to world axes

    def contains(self, px: float, py: float) -> bool:
        return (abs(px - self.x) <= self.w) and (abs(py - self.y) <= self.h)

    def ray_hit(self, ox: float, oy: float, dx: float, dy: float, max_dist: float) -> float:
        """Axis-aligned box slab test; returns distance along ray or max_dist."""
        tmin = 0.0
        tmax = max_dist
        for (o, d, c, e) in ((ox, dx, self.x, self.w), (oy, dy, self.y, self.h)):
            if abs(d) < 1e-9:
                if o < c - e or o > c + e:
                    return max_dist
            else:
                t1 = (c - e - o) / d
                t2 = (c + e - o) / d
                lo, hi = min(t1, t2), max(t1, t2)
                tmin = max(tmin, lo)
                tmax = min(tmax, hi)
                if tmin > tmax:
                    return max_dist
        return tmin if tmin > 0 else max_dist


@dataclass
class World:
    """Straight-road world with optional obstacles and a goal position."""
    road_half_width: float = 4.0      # lane from y=-4 to y=+4 (two 4m half-lanes)
    road_length: float = 200.0        # x in [0, 200]
    target_speed: float = 10.0        # m/s (~36 km/h)
    max_steer: float = 0.6            # radians
    max_accel: float = 3.0            # m/s^2
    max_speed: float = 20.0
    dt: float = 0.1
    obstacles: List[Obstacle] = field(default_factory=list)
    goal: Tuple[float, float] = (380.0, 0.0)
    goal_heading: float = 0.0
    goal_radius: float = 2.0


LIDAR_RAYS = 12
LIDAR_MAX = 30.0


def lidar_scan(car: Car, world: World) -> np.ndarray:
    """Cast LIDAR_RAYS evenly in front-biased fan around the car heading."""
    angles = np.linspace(-math.pi / 2, math.pi / 2, LIDAR_RAYS)
    scans = np.full(LIDAR_RAYS, LIDAR_MAX, dtype=np.float32)
    for i, a in enumerate(angles):
        theta = car.heading + a
        dx, dy = math.cos(theta), math.sin(theta)
        dist = LIDAR_MAX
        # Road boundary walls (y = ±road_half_width) as virtual obstacles
        # Solve for t where car.y + t*dy = ±road_half_width
        for wall_y in (world.road_half_width, -world.road_half_width):
            if abs(dy) > 1e-6:
                t = (wall_y - car.y) / dy
                if 0 < t < dist:
                    dist = t
        # Front/back walls
        for wall_x in (0.0, world.road_length):
            if abs(dx) > 1e-6:
                t = (wall_x - car.x) / dx
                if 0 < t < dist:
                    dist = t
        # Obstacles
        for obs in world.obstacles:
            t = obs.ray_hit(car.x, car.y, dx, dy, dist)
            if t < dist:
                dist = t
        scans[i] = dist
    return np.clip(scans / LIDAR_MAX, 0.0, 1.0)


def step_dynamics(car: Car, throttle: float, steering: float, world: World) -> Car:
    """Kinematic bicycle update (rear-axle reference)."""
    throttle = float(np.clip(throttle, -1.0, 1.0))
    steering = float(np.clip(steering, -1.0, 1.0))
    steer_angle = steering * world.max_steer
    accel = throttle * world.max_accel
    new_speed = car.speed + accel * world.dt
    new_speed = float(np.clip(new_speed, -0.3 * world.max_speed, world.max_speed))
    # Bicycle model
    beta = 0.0  # rear-axle ref => slip angle ~ 0
    dx = new_speed * math.cos(car.heading + beta) * world.dt
    dy = new_speed * math.sin(car.heading + beta) * world.dt
    dtheta = (new_speed / car.length) * math.tan(steer_angle) * world.dt
    return Car(
        x=car.x + dx,
        y=car.y + dy,
        heading=_wrap_angle(car.heading + dtheta),
        speed=new_speed,
        length=car.length,
        width=car.width,
    )


def _wrap_angle(a: float) -> float:
    return (a + math.pi) % (2 * math.pi) - math.pi


def car_collides(car: Car, world: World) -> bool:
    # Sample 4 corners of the car AABB (approx, ignores heading rotation for speed)
    hl, hw = car.length / 2, car.width / 2
    corners = []
    cos_h, sin_h = math.cos(car.heading), math.sin(car.heading)
    for sx, sy in ((hl, hw), (hl, -hw), (-hl, hw), (-hl, -hw)):
        cx = car.x + sx * cos_h - sy * sin_h
        cy = car.y + sx * sin_h + sy * cos_h
        corners.append((cx, cy))
    for (cx, cy) in corners:
        if cy > world.road_half_width or cy < -world.road_half_width:
            return True
        if cx < 0 or cx > world.road_length:
            return True
        for obs in world.obstacles:
            if obs.contains(cx, cy):
                return True
    return False