"""
scenarios/showcase.py
---------------------
Custom high-impact demo scene: Left Turn Across Path (LTAP).
Built step-by-step for full control and learning.

Phase 1: Ego Foundation
"""

import carla
import math
from scenarios.base import ScenarioBase

class ShowcaseScenario(ScenarioBase):
    @property
    def name(self):
        return "showcase"

    @property
    def ego_spawn_point(self):
        # Working Town10 intersection point (Southbound)
        return carla.Transform(carla.Location(x=107.412, y=45.309, z=0.5), carla.Rotation(yaw=-87.7))

    def setup(self, world, ego_vehicle, traffic_manager) -> None:
        """
        Step 11: Deep Intersection Guidance for Ego.
        """
        self._world = world
        self._ego = ego_vehicle
        self._tm = traffic_manager
        
        # User Coordinates
        self.P1 = carla.Transform(carla.Location(x=101.573, y=-8.183, z=0.5), carla.Rotation(yaw=98.4))
        self.P2 = carla.Transform(carla.Location(x=107.412, y=45.309, z=0.5), carla.Rotation(yaw=-87.7))
        self.P3 = carla.Location(x=82.312, y=24.801, z=0.5)

        # NPC Waypoint Guidance
        self.P1_MID = carla.Location(x=101.573, y=18.0, z=0.5)
        self._npc_reached_mid = False

        # NEW: EGO Mid-point to force a larger turn radius
        # Aligned with P2, but 10m further south into intersection
        self.EGO_MID = carla.Location(x=107.412, y=32.0, z=0.5)
        self._ego_reached_mid = False

        # 1. Setup Ego (Already spawned at ego_spawn_point by main.py)
        self._ego.set_autopilot(False)

        # 2. Spawn NPC
        bp_lib = world.get_blueprint_library()
        npc_bp = bp_lib.filter("vehicle.nissan.micra")[0]
        self._npc = world.try_spawn_actor(npc_bp, self.P1)
        
        if self._npc:
            self._actors.append(self._npc)
            self._npc.set_autopilot(False)
            print(f"[SUCCESS] Actors ready. Multi-stage pathing ENABLED.")
            
        # 3. Visual Debug: Dimmed Boxes (Commented out for final presentation)
        # world.debug.draw_box(
        #     carla.BoundingBox(self.P3 + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)),
        #     carla.Rotation(), thickness=0.1, color=carla.Color(178, 0, 0), life_time=20.0
        # )
        # world.debug.draw_box(
        #     carla.BoundingBox(self.P1_MID + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)),
        #     carla.Rotation(), thickness=0.1, color=carla.Color(0, 178, 0), life_time=20.0
        # )
        # world.debug.draw_box(
        #     carla.BoundingBox(self.EGO_MID + carla.Location(z=1), carla.Vector3D(0.5, 0.5, 1.0)),
        #     carla.Rotation(), thickness=0.1, color=carla.Color(0, 0, 178), life_time=20.0
        # )

    def _get_steering_to_target(self, actor, target_loc):
        t = actor.get_transform()
        v = target_loc - t.location
        target_yaw = math.degrees(math.atan2(v.y, v.x))
        delta_yaw = target_yaw - t.rotation.yaw
        while delta_yaw > 180: delta_yaw -= 360
        while delta_yaw < -180: delta_yaw += 360
        return max(-1.0, min(1.0, delta_yaw / 60.0))

    def step(self, frame: int, ego_vehicle, pbar=None) -> None:
        """
        Step 11: Multi-stage steering logic.
        """
        # --- Ego Control ---
        e_loc = ego_vehicle.get_location()
        if not self._ego_reached_mid:
            e_target = self.EGO_MID
            if e_loc.distance(self.EGO_MID) < 2.5:
                self._ego_reached_mid = True
                msg = "[DEBUG] Ego reached MID point. Swinging into turn."
                if pbar: pbar.write(msg)
                else: print(msg)
        else:
            e_target = self.P3
            
        e_dist = e_loc.distance(e_target)
        ego_steer = self._get_steering_to_target(ego_vehicle, e_target) if e_dist > 1.0 else 0.0
        ego_vehicle.apply_control(carla.VehicleControl(throttle=0.4, steer=ego_steer))
        
        # --- NPC Control ---
        n_steer, n_dist = 0.0, 0.0
        if hasattr(self, "_npc") and self._npc and self._npc.is_alive:
            n_loc = self._npc.get_location()
            
            if not self._npc_reached_mid:
                n_target = self.P1_MID
                if n_loc.distance(self.P1_MID) < 2.5:
                    self._npc_reached_mid = True
                    msg = "[DEBUG] NPC reached MID point. Switching to P3."
                    if pbar: pbar.write(msg)
                    else: print(msg)
            else:
                n_target = self.P3
            
            n_dist = n_loc.distance(n_target)
            n_steer = self._get_steering_to_target(self._npc, n_target) if n_dist > 1.0 else 0.0
            
            # Physics Injection (35 km/h)
            fwd = self._npc.get_transform().get_forward_vector()
            self._npc.set_target_velocity(fwd * 9.72)
            self._npc.apply_control(carla.VehicleControl(throttle=1.0, steer=n_steer))
        
        # Verbose Logging
        if frame % 10 == 0:
            e_vel = ego_vehicle.get_velocity()
            e_speed = 3.6 * math.sqrt(e_vel.x**2 + e_vel.y**2 + e_vel.z**2)
            msg = f"[FRAME {frame:03d}] EGO (spd={e_speed:.1f}kph) target={'MID' if not self._ego_reached_mid else 'P3'} | NPC target={'MID' if not self._npc_reached_mid else 'P3'}"
            if pbar:
                pbar.write(msg)
            else:
                print(msg)

    def cleanup(self):
        self._destroy_actors()