# ====== =====    ====   ====  ====  = ===== =====
#     =      =    =   = =    = =   = =   =   =
#    =     ===    ====  =    = ====  =   =   =====
#   =        =    =  =  =    = =   = =   =       =
#  =     =====    =   =  ====  ====  =   =   =====
# ======================================================================================

from controller import Robot
import math
import numpy as np
import struct
import heapq
import os
import sys
import time
from collections import deque
from scipy.spatial import Delaunay

# ==================== INITIALIZATION ====================

# Define o timeStep e a velocidade máxima
timeStep = 16
max_velocity = 6.28
robot_radius = 0.036

# Cria instância do robô
robot = Robot()

# Definição de rodas e sensores de navegação
wheel_L = robot.getDevice("wheel2 motor")
wheel_R = robot.getDevice("wheel1 motor")
wheel_L.setPosition(float("inf"))
wheel_R.setPosition(float("inf"))

gps = robot.getDevice("gps")
gyro = robot.getDevice("gyro")
lidar = robot.getDevice("lidar")

colour = robot.getDevice("colour_sensor")

# Definição do Emitter
emitter = robot.getDevice("emitter")
receiver = robot.getDevice("receiver")

# Liga todos os sensores
gps.enable(timeStep)
gyro.enable(timeStep)
lidar.enable(timeStep)
lidar.enablePointCloud()
colour.enable(timeStep)
receiver.enable(timeStep)

# ==================== VARIABLES ====================

# Ticks
tick = 0
tick_action = 0
tick_areas = 0
_tile_transicao = None 
tick_evacuation = 0
lack_count = 0
lastRequestTime = robot.getTime()
simulation_time = 480
real_time = 600

# GPS
values_gps = [0.0, 0.0, 0.0]
initial_position = [0.0, 0.0]
final_position = [0.0, 0.0]
current_position = [0.0, 0.0]
hr_position = [0.0, 0.0]
hr_falhas = 0
target_position = [0.0, 0.0]
path_to_target = []

# GYRO
values_gyro = [0.0, 0.0, 0.0]
initial_angle = 0.0
current_angle = 0.0

# LIDAR
values_lidar = [0.0] * 512

# Color
colour_data = [0.0, 0.0, 0.0]
colour_sensor_lidar = [0.0] * 30

# Action
action_type = "none"
action_parameter = "none"

# Walls
walls = set()           
wall_spatial = {}
_walls_swept = set()
wall_hits = {}         
MIN_WALL_HITS = 3       
WALL_HITS_MAX = 5      
WALL_DECAY_RADIUS = 0.22  

HOSPITAL_ARRIVE_RADIUS = 0.02   
COLOUR_SENSOR_OFFSET = 0.055
COLOUR_SENSOR_LATERAL = 0.0023
TILE_CENTER_MAX_OFFSET = 0.055  
TRANSFER_MIN_LEITURAS = 3       
LEITURA_MARGEM = 0.01          
GIRO_CONFERIR = 0.4            
TILE_CENTER_MARGIN = 0.025      
STOP_SPEED_MAX = 0.001         
STOP_GYRO_MAX = 0.001           
TEMPO_MAX_ASSENTAR = 2.0       
HOSPITAL_DWELL = 0.5           
FINAL_SLOW_RADIUS = 0.08       
FINAL_MIN_SPEED = 0.25         
TRECHO_PADRAO = 60.0
FIM_MARGEM = 15.0
ESTACAO_RAIO_SEM_PAREDE = 0.45  
FIM_SEGUNDOS = 30.0   
DEBUG_RELAY_B = True           
pos_amarelo = None
indo_hospital = False       
relay_fase = "indo"           
carregando = False         
finalizando = False        
_relay_t0 = 0.0
_trecho_t0 = None
_duracao_trecho = None        
_ultima_cor = None
_assentou = False             
_giro = None                  
_tentativas_tile = 0          
_cand_tile = None             
_cand_n = 0                   
_tiles_rejeitados = set()     
_tile_confirmado = None       
placar = 0.0
_placar_espera = 0.0
_hist_pos = deque(maxlen=16)  
velocidade_robo = 0.0


CORES_TRANSFER = {
    'amarelo': (242, 242, 51),
    'azul':    (51, 51, 241),
    'verde':   (34, 225, 34),
    'laranja': (242, 169, 51),
    'roxo':    (99, 51, 169),
}
COR_MAX_DIST = 60           
COR_MAX_DIST_ESTACAO = 120  

# ==================== NAVIGATION PARAMETERS ====================
PC_UPDATE_INTERVAL       = 25          
PC_INNER_RADIUS          = robot_radius + 0.02 
PC_OUTER_RADIUS          = robot_radius + 0.060 

PC_MAX_EDGE_DISTANCE      = math.sqrt(2)*12 + .025  

PC_WALL_CLEARANCE        = robot_radius * 0.65
PC_EDGE_WALL_CLEARANCE   = robot_radius * 0.65

PC_SPATIAL_CELL          = 0.010     

ARENA_HALF_SIZE          = 2         
SHORT_TILE_LENGTH        = 0.02/3    

LOCAL_EDGE_RADIUS        = robot_radius * 2.0  
ASTAR_WEIGHT             = 1.2        
PATH_RECOMPUTE_TICKS     = 80
EVAC_RECOMPUTE_TICKS     = 8        
PATH_WAYPOINT_RADIUS     = 0.040
GOTO_ALIGN_COARSE_DEG    = 30         
GOTO_ALIGN_FINE_DEG      = 20         
GOTO_ROTATION_TICKS      = 0         

# ==================== TILE GRID ====================
EVAC_FAIL_STREAK   = 8
_no_frontier_streak = 0

STAGNATION_SECONDS = 45
_stuck_tiles       = set()   
_explore_target    = None
_last_progress_time = None
_last_pc_size      = 0      

visited          = set()
open_edges       = set()   
_last_tile       = None

point_cloud      = {}
point_edges      = {}
border_points    = set()

tile_length    = 0.12
MAX_TILE       = int(math.ceil(ARENA_HALF_SIZE / tile_length)) 

# Flags
current_mood = "exploration_mood"
_lop_fase = 0
_lop_p0 = [0.0, 0.0]
_trilha = deque(maxlen=400)   
_trilha_volta = []          

# Path to evacuation
path_to_evacuation = []

# ==================== FUNCTIONS ===================

# -------------------- GLOBAL ---------------------

def save_plot():
    import matplotlib.pyplot as plt   

    PLOT_MARGIN_FRAC = 0.35  
    PLOT_MARGIN_MIN  = 0.60  
    PLOT_MIN_SPAN    = 2.0   

    xs = [0.0, current_position[0]]
    ys = [0.0, current_position[1]]

    for (px, py), ptype in point_cloud.items():
        if ptype == 'interior':
            xs.append(px); ys.append(py)
    for (wx, wy) in walls:
        xs.append(wx); ys.append(wy)

    data_xmin, data_xmax = min(xs), max(xs)
    data_ymin, data_ymax = min(ys), max(ys)

    raw_span_x = data_xmax - data_xmin
    raw_span_y = data_ymax - data_ymin
    margin = max(PLOT_MARGIN_MIN, max(raw_span_x, raw_span_y) * PLOT_MARGIN_FRAC)

    span_x = max(raw_span_x + 2 * margin, PLOT_MIN_SPAN)
    span_y = max(raw_span_y + 2 * margin, PLOT_MIN_SPAN)

    center_x = (data_xmin + data_xmax) / 2
    center_y = (data_ymin + data_ymax) / 2

    PLOT_XMIN = center_x - span_x / 2
    PLOT_XMAX = center_x + span_x / 2
    PLOT_YMIN = center_y - span_y / 2
    PLOT_YMAX = center_y + span_y / 2

    PLOT_TARGET_AREA = 24 * 24
    PLOT_MIN_FIGSIZE = 12
    PLOT_MAX_FIGSIZE = 36
    aspect = span_x / span_y
    fig_w = min(PLOT_MAX_FIGSIZE, max(PLOT_MIN_FIGSIZE, (PLOT_TARGET_AREA * aspect) ** 0.5))
    fig_h = min(PLOT_MAX_FIGSIZE, max(PLOT_MIN_FIGSIZE, (PLOT_TARGET_AREA / aspect) ** 0.5))
    plt.figure(figsize=(fig_w, fig_h))
    ax = plt.gca()

    def _in_window(x, y, pad=0.0):
        return (PLOT_XMIN - pad <= x <= PLOT_XMAX + pad
                and PLOT_YMIN - pad <= y <= PLOT_YMAX + pad)
    
    if len(point_cloud) >= 4:
        from scipy.spatial import Voronoi
        from matplotlib.collections import PolyCollection

        keys = list(point_cloud.keys())
        types = [point_cloud[k] for k in keys]
        far = 10.0 * max(ARENA_HALF_SIZE, 1.0)
        ghosts = [(far, far), (-far, far), (far, -far), (-far, -far)]
        vor = Voronoi(np.array(keys + ghosts, dtype=float))

        border_polys, interior_polys = [], []
        for i, ptype in enumerate(types):
            gx, gy = keys[i]
            if not _in_window(gx, gy, pad=0.05):
                continue
            region = vor.regions[vor.point_region[i]]
            if not region or -1 in region:
                continue
            poly = [vor.vertices[v] for v in region]
            (border_polys if ptype == 'border' else interior_polys).append(poly)

        if interior_polys:
            ax.add_collection(PolyCollection(
                interior_polys, facecolors=(0.75, 0.75, 0.75),
                edgecolors='white', linewidths=0.3, zorder=1))
        if border_polys:
            ax.add_collection(PolyCollection(
                border_polys, facecolors=(0.70, 0.95, 0.70),
                edgecolors='white', linewidths=0.3, zorder=1))

    # ── Delaunay edge overlay —─────
    if len(point_edges) >= 2:
        try:
            for p1, neighbors in point_edges.items():
                p1_in = _in_window(p1[0], p1[1])
                for p2 in neighbors:
                    if not (p1_in or _in_window(p2[0], p2[1])):
                        continue
                    ax.plot([p1[0], p2[0]], [p1[1], p2[1]],
                            '-', color=(0, 0, 0), linewidth=0.25,
                            alpha=0.5, zorder=2)
        except Exception:
            pass

    # ── Wall forbidden region —──
    if walls:
        blob_color = (1.00, 0.86, 0.74)
        wx_arr, wy_arr = [], []
        for (wx, wy) in walls:
            if not _in_window(wx, wy, pad=PC_WALL_CLEARANCE):
                continue
            ax.add_patch(plt.Circle((wx, wy), PC_WALL_CLEARANCE,
                                    facecolor=blob_color,
                                    edgecolor='none',
                                    alpha=0.05,
                                    zorder=4))
            wx_arr.append(wx); wy_arr.append(wy)
        if wx_arr:
            ax.plot(wx_arr, wy_arr, 'o',
                    color=(85/255, 170/255, 183/255), markersize=4, alpha=0.9,
                    zorder=5)

    # ── Current position ──────────────────────────────────────────────────
    ax.plot(current_position[0], current_position[1], 'go', markersize=10,
            label='Current Position', zorder=8)

    ax.set_xlim(PLOT_XMIN, PLOT_XMAX)
    ax.set_ylim(PLOT_YMIN, PLOT_YMAX)
    ax.set_xlabel('X-axis')
    ax.set_ylabel('Y-axis')
    ax.set_title(f'Delaunay Navigation Graph – Tick {tick}  '
                 f'interior={sum(v=="interior" for v in point_cloud.values())}  '
                 f'border={len(border_points)}  '
                 f'walls={len(walls)}')
    ax.grid(True, alpha=0.3)
    ax.legend()

    output_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'plots')
    if not os.path.exists(output_dir):
        os.makedirs(output_dir)
    plt.savefig(f"{output_dir}/plot_tick_{tick}.png")
    plt.close()


def first_ticks():
    global initial_angle, current_angle

    # Get initial position
    if tick <= 20:
        initial_position[0] += current_position[0]
        initial_position[1] += current_position[1]

    if tick == 20:
        initial_position[0] = initial_position[0] / 21
        initial_position[1] = initial_position[1] / 21

    # Get initial orientation
    if tick >= 30 and tick <= 50:
        final_position[0] += current_position[0]
        final_position[1] += current_position[1]

    if tick == 50:
        final_position[0] = final_position[0] / 21
        final_position[1] = final_position[1] / 21

        initial_angle = np.arctan2(final_position[1], final_position[0])
        if initial_angle < 0:
            initial_angle = initial_angle + 2 * np.pi
        if initial_angle > 2 * np.pi:
            initial_angle = initial_angle - 2 * np.pi
        current_angle = initial_angle

def update_sensors():
    global values_gps, current_position
    global values_gyro, current_angle
    global values_lidar
    global colour_data, colour_sensor_lidar

    # ----- Navigation -----

    # Update GPS sensors
    values_gps = gps.getValues()
    current_position = [values_gps[0], -values_gps[2]]
    if(tick > 20):
        current_position[0] = current_position[0] - initial_position[0]
        current_position[1] = current_position[1] - initial_position[1]

    global velocidade_robo
    _hist_pos.append((current_position[0], current_position[1]))
    velocidade_robo = (distance(_hist_pos[0], _hist_pos[-1])
                       / (max(len(_hist_pos) - 1, 1) * timeStep / 1000))

    # Update GYRO sensors
    values_gyro = gyro.getValues()
    current_angle = current_angle + values_gyro[1] * timeStep / 1000
    if current_angle > 2 * np.pi:
        current_angle = current_angle - 2 * np.pi
    if current_angle < 0:
        current_angle = current_angle + 2 * np.pi

    # Update LIDAR sensors —
    values_lidar = [1 if v == math.inf else v
                    for v in lidar.getRangeImage()[1024:1536]]

    # ----- Colour -----

    # Update Colour sensor
    colour_data = colour.getImage()

    colour_data = [colour.imageGetRed(colour_data, 1, 0, 0), colour.imageGetGreen(colour_data, 1, 0, 0), colour.imageGetBlue(colour_data, 1, 0, 0)]
    colour_sensor_lidar = values_lidar[0:35] 
    colour_sensor_lidar += values_lidar[482:512] 

_PROF = {'plan_n': 0, 'plan_s': 0.0}


def _advance_waypoints(path, keep_last=False):
    limit = 1 if keep_last else 0
    while len(path) > limit and distance(current_position, path[0]) < PATH_WAYPOINT_RADIUS:
        path.pop(0)


def get_next_action():
    global target_position
    global current_mood
    global path_to_evacuation
    global tick_evacuation
    global _no_frontier_streak
    global _lop_fase, current_angle
    global indo_hospital, relay_fase, finalizando

    if tick <= 20:
        return "stop", "none"

    elif tick <= 30:
        return "walk", "none"

    elif tick <= 60:
        return "stop", "none"

    if not _trilha or distance(current_position, _trilha[-1]) >= 0.01:
        _trilha.append((current_position[0], current_position[1]))

    if _lop_fase == 1:
        _lop_fase = 2
        return "stop", "none"
    if _lop_fase in (3, 5):
        dx, dy = current_position[0] - _lop_p0[0], current_position[1] - _lop_p0[1]
        if math.hypot(dx, dy) >= 0.01:
            current_angle = (math.atan2(dy, dx) + (math.pi if _lop_fase == 5 else 0)) % (2 * math.pi)
            _lop_fase = 0
        elif _lop_fase == 3:
            _lop_fase = 4
        else:
            _lop_fase = 0
        path_to_target.clear()
    if _lop_fase in (2, 4):
        _lop_p0[:] = current_position
        _lop_fase += 1
        return "walk", ("re" if _lop_fase == 5 else "none")


    if relay_fase != "finalizado" and current_mood != "evacuation_mood" and _tempo_curto():
        current_mood = "evacuation_mood"
        indo_hospital = False
        relay_fase = "indo"
        finalizando = True
        path_to_evacuation.clear()
        path_to_target.clear()

    if DEBUG_RELAY_B and current_mood == "evacuation_mood" and tick % 50 == 0:
        _alvo = pos_amarelo if indo_hospital else (0.0, 0.0)
        _rota = bool(astar("evacuation_mood")[0]) if _alvo is not None else False
        print(f"[relay-B] t={tick} fase={relay_fase} "
              f"indo_transf={indo_hospital} carreg={carregando} final={finalizando} "
              f"pos=({current_position[0]:.2f},{current_position[1]:.2f}) "
              f"alvo={_alvo} dist={distance(current_position, _alvo):.3f} "
              f"rota={'SIM' if _rota else 'NAO'} walls={len(walls)}", flush=True)

    if current_mood == "evacuation_mood":
        acao = _relay_step()
        if acao is not None:
            return acao

    if path_to_evacuation != []:

        raio = HOSPITAL_ARRIVE_RADIUS if len(path_to_evacuation) == 1 else PATH_WAYPOINT_RADIUS
        if distance(current_position, path_to_evacuation[0]) < raio:
            path_to_evacuation.pop(0)
            tick_evacuation = 0
        if path_to_evacuation != []:
            if tick_evacuation <= 32:
                tick_evacuation += 1
                target_position = path_to_evacuation[0]
                return "go_to", [target_position[0], target_position[1],
                                 len(path_to_evacuation) == 1]

    _check_stagnation()

    _advance_waypoints(path_to_target)

    # Navigation –
    if (not path_to_target or tick % PATH_RECOMPUTE_TICKS == 0
            or (current_mood == "evacuation_mood" and tick % EVAC_RECOMPUTE_TICKS == 0)):
        _t0 = time.perf_counter()
        new_path, new_target, new_dist = astar(current_mood)
        _PROF['plan_n'] += 1; _PROF['plan_s'] += time.perf_counter() - _t0

        if not new_path:
            update_point_cloud()
            new_path, new_target, new_dist = astar(current_mood)

        if new_path:
            _trilha_volta.clear()
        elif (not path_to_target and current_mood != "evacuation_mood"
                and (len(_trilha) >= 10 or _trilha_volta)
                and _no_frontier_streak >= EVAC_FAIL_STREAK - 1 and not astar("evacuation_mood")[0]):
            if not _trilha_volta:
                _trilha_volta.extend(list(reversed(_trilha))[::3])
                _trilha.clear()
            new_path = _trilha_volta[:35]
            del _trilha_volta[:35]

        if current_mood == "evacuation_mood" and new_path:
            path_to_evacuation = new_path
            _advance_waypoints(path_to_evacuation)  

        if current_mood != "evacuation_mood":
            _no_frontier_streak = 0 if new_path else _no_frontier_streak + 1

        if ((_no_frontier_streak >= EVAC_FAIL_STREAK)
                and current_mood != "evacuation_mood" and tick > 200):
            _stuck_tiles.clear()
            _no_frontier_streak = 0

        if new_path:
            path_to_target[:] = new_path
        elif current_mood == "evacuation_mood":
            path_to_target.clear()  
        _advance_waypoints(path_to_target, keep_last=True)

    if not path_to_target:
        return "stop", "none"

    target_position = path_to_target[0]
    return "go_to", [target_position[0], target_position[1],
                     current_mood == "evacuation_mood" and len(path_to_target) == 1]

def execute_action(action_type, parameter):
    global tick_action

    if action_type == "hard_reset":

        path_to_target.clear()  
        wheel_L.setVelocity(-max_velocity)
        wheel_R.setVelocity(-max_velocity)
        tick_action = 50

 

    if action_type == "go_to":
        target_angle = np.arctan2(parameter[1] - current_position[1],
                                  parameter[0] - current_position[0])

        delta_angle = target_angle - current_angle
        if delta_angle > np.pi:
            delta_angle -= 2 * np.pi
        elif delta_angle < -np.pi:
            delta_angle += 2 * np.pi

        coarse = GOTO_ALIGN_COARSE_DEG * np.pi / 180
        fine   = GOTO_ALIGN_FINE_DEG   * np.pi / 180

        fator = 1.0
        if len(parameter) > 2 and parameter[2]:
            fator = max(FINAL_MIN_SPEED,
                        min(1.0, distance(current_position, parameter) / FINAL_SLOW_RADIUS))

        if delta_angle > coarse:
            wheel_L.setVelocity(-max_velocity * 0.75)
            wheel_R.setVelocity( max_velocity * 0.75)
            tick_action = GOTO_ROTATION_TICKS
        elif delta_angle < -coarse:
            wheel_L.setVelocity( max_velocity * 0.75)
            wheel_R.setVelocity(-max_velocity * 0.75)
            tick_action = GOTO_ROTATION_TICKS
        elif delta_angle > fine:
            wheel_L.setVelocity(-max_velocity / 2)
            wheel_R.setVelocity( max_velocity / 2)
            tick_action = GOTO_ROTATION_TICKS
        elif delta_angle < -fine:
            wheel_L.setVelocity( max_velocity / 2)
            wheel_R.setVelocity(-max_velocity / 2)
            tick_action = GOTO_ROTATION_TICKS
        else:
            wheel_L.setVelocity(max_velocity * fator)
            wheel_R.setVelocity(max_velocity * fator)
            tick_action = GOTO_ROTATION_TICKS

    elif action_type == "stop":
        wheel_L.setVelocity(0)
        wheel_R.setVelocity(0)
        tick_action = 20

    elif action_type == "walk":
        v = -max_velocity if parameter == "re" else max_velocity
        wheel_L.setVelocity(v)
        wheel_R.setVelocity(v)
        tick_action = 10

    elif action_type == "rotate":
        wheel_L.setVelocity(parameter*max_velocity)
        wheel_R.setVelocity(-parameter*max_velocity)

# -------------------- NAVIGATION ---------------------

def receiver_data():
    global lack_count, lastRequestTime, simulation_time, real_time
    global _lop_fase, tick_action, _last_tile
    global carregando, relay_fase, indo_hospital, _trecho_t0, placar

    if robot.getTime() - lastRequestTime > 1:
        message = struct.pack('c', 'G'.encode())
        emitter.send(message)
        lastRequestTime = robot.getTime()

    if receiver.getQueueLength() > 0: 
        receivedData = receiver.getBytes()
        rDataLen = len(receivedData)
        if rDataLen == 1:
            tup = struct.unpack('c', receivedData)
            if tup[0].decode("utf-8") == 'L':
                lack_count += 1
                _lop_fase = 1
                _last_tile = None          
                tick_action = 0
                _trilha.clear()
                _trilha_volta.clear()
                path_to_target.clear()
                path_to_evacuation.clear()
                carregando = False
                if relay_fase != "finalizado":
                    relay_fase = "indo"
                    indo_hospital = pos_amarelo is not None
                    _trecho_t0 = None
        elif rDataLen == 16:
            tup = struct.unpack('c f i i', receivedData)
            if tup[0].decode("utf-8") == 'G':
                placar = tup[1]
                simulation_time = tup[2]
                real_time = tup[3]
        receiver.nextPacket()


def update_walls():
    values_pointCloud = lidar.getPointCloud()

    points_layer_3 = np.array([[-p.x, -p.y] for p in values_pointCloud[1024:1536]])
    points_layer_4 = np.array([[-p.x, -p.y] for p in values_pointCloud[1536:2048]])

    # Matriz de rotação
    cos_a = np.cos(current_angle)
    sin_a = np.sin(current_angle)
    rotation_matrix = np.array([[cos_a, -sin_a], [sin_a,  cos_a]])

    # Pontos rotacionados
    rotated_points_3 = points_layer_3 @ rotation_matrix.T
    rotated_points_4 = points_layer_4 @ rotation_matrix.T

    # Distância até robô
    dist_3 = np.linalg.norm(rotated_points_3, axis=1)
    dist_4 = np.linalg.norm(rotated_points_4, axis=1)

    # Substitui NaN
    dist_3[np.isnan(dist_3)] = np.inf
    dist_4[np.isnan(dist_4)] = np.inf

    # Menores distâncias em cada raio
    final_points = np.where(dist_3[:, None] < dist_4[:, None], rotated_points_3, rotated_points_4)
    final_dists = np.minimum(dist_3, dist_4)

    # Pontos no mundo
    world_points = final_points + current_position

    # Tira pontos muito distantes
    valid_indices = final_dists < (0.12 * 1.5 * np.sqrt(2))
    valid_world_points = world_points[valid_indices]

    original_indices = np.where(valid_indices)[0]


    if len(valid_world_points) > 1:
        manter = np.diff(original_indices) <= 5
        register_walls(valid_world_points[:-1][manter])

def register_walls(points):
    pts = np.asarray(points, dtype=float)
    if pts.size == 0:
        return
    pts = np.round(pts.reshape(-1, 2), 2)

    seen = set()
    for wx, wy in pts.tolist():
        p = (float(wx), float(wy))
        seen.add(p)
        n = min(wall_hits.get(p, 0) + 1, WALL_HITS_MAX)
        wall_hits[p] = n
        if n >= MIN_WALL_HITS and p not in walls:
            walls.add(p)
            _wall_spatial_add(p[0], p[1])

    _decay_walls(seen)

def _decay_walls(seen):
    rx, ry = current_position
    r2 = WALL_DECAY_RADIUS * WALL_DECAY_RADIUS
    remover = []
    for p in walls:
        if p in seen:
            continue
        if (p[0] - rx) ** 2 + (p[1] - ry) ** 2 > r2:
            continue
        n = wall_hits.get(p, 0) - 1
        if n <= 0:
            remover.append(p)
        else:
            wall_hits[p] = n
    for p in remover:
        walls.discard(p)
        wall_hits.pop(p, None)
        cell = _pc_cell(p[0], p[1])
        s = wall_spatial.get(cell)
        if s:
            s.discard(p)
            if not s:
                del wall_spatial[cell]

# ==================== POINT CLOUD INTERNALS ====================

def _pc_cell(x, y):
    return (int(math.floor(x / PC_SPATIAL_CELL)),
            int(math.floor(y / PC_SPATIAL_CELL)))

_seeded_flag = False  

def _wall_spatial_add(wx, wy):
    cell = _pc_cell(wx, wy)
    if cell not in wall_spatial:
        wall_spatial[cell] = set()
    wall_spatial[cell].add((wx, wy))

def _segment_blocked_by(p1, p2, obstacle_hash, clearance):
    if not obstacle_hash or clearance <= 0:
        return False
    x1, y1 = p1
    x2, y2 = p2
    dx = x2 - x1
    dy = y2 - y1
    seg_len2 = dx*dx + dy*dy
    cl2 = clearance * clearance
    if seg_len2 == 0.0:
        cx, cy = _pc_cell(x1, y1)
        span = int(clearance / PC_SPATIAL_CELL) + 1
        for ddx in range(-span, span + 1):
            for ddy in range(-span, span + 1):
                for (ox, oy) in obstacle_hash.get((cx+ddx, cy+ddy), ()):
                    if (ox-x1)**2 + (oy-y1)**2 <= cl2:
                        return True
        return False
    pad = clearance + PC_SPATIAL_CELL
    cmin_x = int(math.floor((min(x1, x2) - pad) / PC_SPATIAL_CELL))
    cmax_x = int(math.floor((max(x1, x2) + pad) / PC_SPATIAL_CELL))
    cmin_y = int(math.floor((min(y1, y2) - pad) / PC_SPATIAL_CELL))
    cmax_y = int(math.floor((max(y1, y2) + pad) / PC_SPATIAL_CELL))
    for cx in range(cmin_x, cmax_x + 1):
        for cy in range(cmin_y, cmax_y + 1):
            for (ox, oy) in obstacle_hash.get((cx, cy), ()):
                t = ((ox - x1) * dx + (oy - y1) * dy) / seg_len2
                if   t < 0.0: t = 0.0
                elif t > 1.0: t = 1.0
                qx = x1 + t * dx
                qy = y1 + t * dy
                if (qx - ox)**2 + (qy - oy)**2 <= cl2:
                    return True
    return False

def _segment_blocked_by_wall(p1, p2, clearance=None):
    if clearance is None:
        clearance = PC_EDGE_WALL_CLEARANCE
    return _segment_blocked_by(p1, p2, wall_spatial, clearance)

def distance(p, q):
    return math.sqrt((p[0] - q[0]) ** 2 + (p[1] - q[1]) ** 2)
# ==================== TILE-GRID NAVIGATION ====================

WALL_EDGE_CLEARANCE = 0.03    

def _tile_center(i, j):
    return (round(i * tile_length, 4), round(j * tile_length, 4))

def _world_to_tile(x, y):
    return (int(round(x / tile_length)), int(round(y / tile_length)))

def _in_bounds(t):
    return abs(t[0]) <= MAX_TILE and abs(t[1]) <= MAX_TILE

def _tile_neighbors(t):
    i, j = t
    return ((i + 1, j), (i - 1, j), (i, j + 1), (i, j - 1))

def _registrar_trilha_livre():
    global _last_tile
    cur = _world_to_tile(current_position[0], current_position[1])
    if _last_tile is not None and cur != _last_tile:
        di = abs(cur[0] - _last_tile[0])
        dj = abs(cur[1] - _last_tile[1])
        if di + dj == 1:                  
            open_edges.add((min(cur, _last_tile), max(cur, _last_tile)))
    _last_tile = cur

def _edge_blocked(a, b):
    if (min(a, b), max(a, b)) in open_edges:    
        return False
    return _segment_blocked_by_wall(_tile_center(*a), _tile_center(*b), WALL_EDGE_CLEARANCE)

def _bfs(start, is_goal):
    pred = {start: None}
    fila = deque([start])
    destino = None
    while fila:
        u = fila.popleft()
        if u != start and is_goal(u):
            destino = u
            break
        for v in _tile_neighbors(u):
            if v in pred or not _in_bounds(v):
                continue
            if _edge_blocked(u, v):
                continue
            pred[v] = u
            fila.append(v)
    if destino is None:
        return []
    caminho = []
    no = destino
    while pred[no] is not None:
        caminho.append(no)
        no = pred[no]
    caminho.reverse()
    return caminho

def _bfs_evac(start, goal):
    def perto(t):
        return max(abs(t[0] - goal[0]), abs(t[1] - goal[1])) <= 1
    pred = {start: None}
    fila = deque([start])
    destino = None
    while fila:
        u = fila.popleft()
        if u == goal:
            destino = u
            break
        for v in _tile_neighbors(u):
            if v in pred or not _in_bounds(v):
                continue
            if not (perto(u) or perto(v)) and _edge_blocked(u, v):
                continue
            pred[v] = u
            fila.append(v)
    if destino is None:
        return []
    caminho = []
    no = destino
    while pred[no] is not None:
        caminho.append(no)
        no = pred[no]
    caminho.reverse()
    return caminho

def _explorou(t):
    return t not in visited and t not in _stuck_tiles

def _explorou_par(t):
    return _explorou(t) and t[0] % 2 == 0 and t[1] % 2 == 0

def update_point_cloud():
    visited.add(_world_to_tile(current_position[0], current_position[1]))

def _ponto_lido():
    c, s = math.cos(current_angle), math.sin(current_angle)
    return (current_position[0] + COLOUR_SENSOR_OFFSET * c - COLOUR_SENSOR_LATERAL * s,
            current_position[1] + COLOUR_SENSOR_OFFSET * s + COLOUR_SENSOR_LATERAL * c)

def _ponto_no_tile(p, tile, margem):
    lim = tile_length / 2 - margem
    return abs(p[0] - tile[0]) <= lim and abs(p[1] - tile[1]) <= lim

def _tile_under_sensor():
    sx, sy = _ponto_lido()
    cx = round(sx / tile_length) * tile_length
    cy = round(sy / tile_length) * tile_length
    dentro = abs(sx - cx) <= TILE_CENTER_MAX_OFFSET and abs(sy - cy) <= TILE_CENTER_MAX_OFFSET
    return (round(cx, 4), round(cy, 4)), dentro

# -------------------- REVEZAMENTO (B) ---------------------

def _cor_transferencia(rgb, max_dist=COR_MAX_DIST):
    melhor, melhor_d = None, max_dist
    for nome, ref in CORES_TRANSFER.items():
        d = math.dist(rgb, ref)
        if d < melhor_d:
            melhor, melhor_d = nome, d
    return melhor

def _centro_no_tile(tile, margem=TILE_CENTER_MARGIN):
    return _ponto_no_tile(current_position, tile, margem)

def _teste_giro():
    global _giro
    if _giro is None:
        _giro = {'acum': 0.0, 'ultimo': current_angle, 'n': 0, 'sim': 0, 'cx': 0.0, 'cy': 0.0}
    d = (current_angle - _giro['ultimo'] + math.pi) % (2 * math.pi) - math.pi
    _giro['acum'] += abs(d)
    _giro['ultimo'] = current_angle
    _giro['n'] += 1
    if _cor_transferencia(colour_data) is not None:
        _giro['sim'] += 1
        _giro['cx'] += math.cos(current_angle)
        _giro['cy'] += math.sin(current_angle)
    if _giro['acum'] < 2 * math.pi:
        return "girando", None
    g, _giro = _giro, None
    fracao = g['sim'] / g['n']
    if fracao > 0.5:
        return "ok", None
    if g['sim'] == 0:
        return "nada", None
    return "vizinho", math.atan2(g['cy'], g['cx'])

def _mover_para_vizinho(tile, rumo):
    global pos_amarelo, relay_fase
    c, s = math.cos(rumo), math.sin(rumo)
    if abs(c) >= abs(s):
        novo = (round(tile[0] + math.copysign(tile_length, c), 4), tile[1])
    else:
        novo = (tile[0], round(tile[1] + math.copysign(tile_length, s), 4))
    _tiles_rejeitados.add(tile)
    pos_amarelo = novo
    relay_fase = "indo"
    path_to_evacuation[:] = [novo]
    path_to_target.clear()

def _rejeitar_transferencia(tile):
    global pos_amarelo, current_mood, relay_fase, indo_hospital, _no_frontier_streak
    global _cand_tile, _cand_n
    _tiles_rejeitados.add(tile)
    pos_amarelo = None
    current_mood = "exploration_mood"
    relay_fase = "indo"
    indo_hospital = False
    _no_frontier_streak = 0
    _cand_tile, _cand_n = None, 0
    path_to_evacuation.clear()
    path_to_target.clear()

def _robo_parado():
    return velocidade_robo < STOP_SPEED_MAX and abs(values_gyro[1]) < STOP_GYRO_MAX

def _parado_de_proposito():
    return current_mood == "evacuation_mood" and relay_fase != "indo"

def _perto_da_estacao():
    if current_mood != "evacuation_mood":
        return False
    alvo = pos_amarelo if indo_hospital else (0.0, 0.0)
    if alvo is None:
        return False
    return distance(current_position, alvo) < ESTACAO_RAIO_SEM_PAREDE

def _tempo_curto():
    return min(simulation_time, real_time) <= FIM_SEGUNDOS

def _sem_tempo_para(n_trechos):
    restante = min(simulation_time, real_time)
    return restante < n_trechos * 1.3 * (_duracao_trecho or TRECHO_PADRAO) + FIM_MARGEM

def _enviar_relatorio():
    x = int(round(values_gps[0] * 100))
    z = int(round(values_gps[2] * 100))
    emitter.send(struct.pack('i i c', x, z, b'H'))

def _proximo_trecho():
    global indo_hospital, relay_fase, _trecho_t0
    indo_hospital = not indo_hospital
    relay_fase = "indo"
    _trecho_t0 = None
    path_to_evacuation.clear()
    path_to_target.clear()

def _relay_step():
    global relay_fase, carregando, finalizando, _relay_t0, _trecho_t0, _duracao_trecho, _ultima_cor
    global _assentou, _tile_confirmado, _placar_espera, _giro, _tentativas_tile
    alvo = pos_amarelo if indo_hospital else (0.0, 0.0)
    nome = "transferencia" if indo_hospital else "hospital"
    agora = robot.getTime()

    if relay_fase == "finalizado":
        return "stop", "none"

    if not finalizando and _tempo_curto():
        finalizando = True
        if indo_hospital:        
            _proximo_trecho()
            return None

    if relay_fase == "indo":
        if _trecho_t0 is None:
            _trecho_t0 = agora
        if distance(current_position, alvo) >= HOSPITAL_ARRIVE_RADIUS:
            return None
        relay_fase = "assentando"
        _relay_t0 = agora
        _assentou = False
        _giro = None
        path_to_evacuation.clear()
        path_to_target.clear()
        return "stop", "none"

    if not _centro_no_tile(alvo):
        relay_fase = "indo"
        path_to_evacuation[:] = [tuple(alvo)]
        return None

    if relay_fase == "assentando":
        if not _assentou:
            if not _robo_parado() and agora - _relay_t0 < TEMPO_MAX_ASSENTAR:
                return "stop", "none"
            _assentou = True
        if indo_hospital and alvo != _tile_confirmado:
            estado, rumo = _teste_giro()
            if estado == "girando":
                if agora - _relay_t0 < TEMPO_MAX_ASSENTAR + 10:
                    return "rotate", GIRO_CONFERIR
                _giro = None
            elif estado != "ok":
                _tentativas_tile += 1
                if estado == "vizinho" and _tentativas_tile < 3:
                    _mover_para_vizinho(alvo, rumo)
                else:
                    _rejeitar_transferencia(alvo)
                return "stop", "none"
            _tile_confirmado = alvo
            _tentativas_tile = 0
        if _trecho_t0 is not None:
            _duracao_trecho = max(_duracao_trecho or 0.0, agora - _trecho_t0)
            _trecho_t0 = None
        relay_fase = "esperando" if indo_hospital else "entregando"
        _relay_t0 = agora
        _ultima_cor = None
        _placar_espera = placar
        return "stop", "none"

    if relay_fase == "esperando":
        if not _ponto_no_tile(_ponto_lido(), alvo, LEITURA_MARGEM):
            return "rotate", GIRO_CONFERIR
        cor = _cor_transferencia(colour_data, COR_MAX_DIST_ESTACAO)
        if cor != _ultima_cor:
            _ultima_cor = cor
        if cor in ("laranja", "roxo") or placar >= _placar_espera + 50:
            carregando = True
            _proximo_trecho()
        elif _sem_tempo_para(1):
            finalizando = True
            _proximo_trecho()
        return "stop", "none"

    if relay_fase == "entregando":
        if agora - _relay_t0 < HOSPITAL_DWELL:
            return "stop", "none"
        if carregando:
            _enviar_relatorio()
            carregando = False
        if finalizando or pos_amarelo is None or _sem_tempo_para(2):
            emitter.send(struct.pack('c', b'E'))
            relay_fase = "finalizado"
            return "stop", "none"
        _proximo_trecho()
        return "stop", "none"

    return None

def _note_progress():
    global _last_progress_time, _last_pc_size
    _last_progress_time = simulation_time
    _last_pc_size = len(visited)

def _check_stagnation():
    global _last_progress_time, _last_pc_size
    if _last_progress_time is None:
        _note_progress()
        return
    if len(visited) > _last_pc_size:
        _note_progress()
        return
    if _last_progress_time - simulation_time < STAGNATION_SECONDS:
        return
    if _explore_target is not None:
        _stuck_tiles.add(_explore_target)
    path_to_target.clear()
    _note_progress()

def astar(current_mood='none'):
    cur = _world_to_tile(current_position[0], current_position[1])
    visited.add(cur)

    if current_mood == 'exploration_mood':
        global _explore_target
        tiles = _bfs(cur, _explorou_par)     
        if not tiles:
            tiles = _bfs(cur, _explorou)     
        if not tiles:
            _explore_target = None
            return [], None, float('inf')
        _explore_target = tiles[-1]
        pts = [_tile_center(i, j) for (i, j) in tiles]
        return pts, pts[-1], len(pts) * tile_length

    if current_mood == 'evacuation_mood':
        alvo = pos_amarelo if indo_hospital else (0.0, 0.0)
        goal = _world_to_tile(alvo[0], alvo[1])
        tiles = _bfs_evac(cur, goal)
        if not tiles and cur != goal:
            return [], None, float('inf')
        pts = [_tile_center(i, j) for (i, j) in tiles]
        if not pts or tuple(pts[-1]) != tuple(alvo):
            pts = pts + [tuple(alvo)]         
        return pts, pts[-1], len(pts) * tile_length

    return [], None, float('inf')



# ==================== MAIN LOOP ====================

while robot.step(timeStep) != -1:
    update_sensors()
    if tick > 60 and not _lop_fase:
        _registrar_trilha_livre()
    if pos_amarelo is None and tick > 60 and not finalizando:
        tile_lido, dentro = _tile_under_sensor()
        cor_piso = _cor_transferencia(colour_data)
        _cand_n = _cand_n + 1 if cor_piso is not None else 0
        if (_cand_n >= TRANSFER_MIN_LEITURAS and (dentro or _cand_n >= 30)
                and tile_lido not in _tiles_rejeitados):
            _cand_tile = tile_lido
    if pos_amarelo is None and _cand_tile is not None and not finalizando:
        pos_amarelo = _cand_tile
        _cand_tile, _cand_n = None, 0
        _tentativas_tile = 0
        indo_hospital = True
        relay_fase = "indo"
        _trecho_t0 = None
        current_mood = "evacuation_mood"
        path_to_target.clear()
        path_to_evacuation.clear()

    if(tick >= 60):
        if(tick % 30 == 0 and not _lop_fase and not _perto_da_estacao()):
            update_walls()
        if tick % PC_UPDATE_INTERVAL == 0:
            update_point_cloud()
        receiver_data()

    if tick % 400 == 0 and not _parado_de_proposito():
        if distance(hr_position, current_position) < 0.02:
            hr_falhas += 1
            if hr_falhas >= 3:
                emitter.send(struct.pack('c', b'L'))
                hr_falhas = 0
            else:
                execute_action("hard_reset", "None")
        else:
            hr_position = current_position
            hr_falhas = 0

    if(tick <= 50):
        first_ticks()

    if tick >= 3000 and tick % 250 == 0: save_plot()    
        
    # ----- Main -----

    if tick_evacuation > 0:
        tick_evacuation = tick_evacuation - 1

    if tick_action > 0:
        tick_action = tick_action - 1
    else:
        action_type, action_parameter = get_next_action()
        execute_action(action_type, action_parameter)
    tick += 1
