agrobot_base/Python/OAK/datasets/_9_test_segformer.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Teste/visualização do SegFormer (.pt) com suporte a estrutura AGRUPADA.

- Modo imagens (dataset/512x288 ou split/test/...)
- Modo câmera (--camera) no mesmo estilo do _9_test_fastscnn.py
- ROI + resize_keep_width + overlay + legenda

Requisitos:
  pip install transformers timm

Obs:
  Este script assume que seus ids de classe batem com o labelmap.txt (mask em IDs 0..K-1).
"""

from collections import deque
from enum import IntEnum
import json
import os
import time
from typing import Dict, Tuple
import cv2
import glob
import argparse
import torch
import numpy as np
import depthai as dai
from PIL import Image

from transformers import SegformerForSemanticSegmentation

# Reaproveita utilidades do seu projeto (iguais no script do FastSCNN)
from utils import (
    carregar_labelmap_completo, compute_roi_indices, converter_mask_ids_para_rgb,
    desenhar_legenda_horizontal, desenhar_legenda_vertical, resize_keep_width
)

IMG_EXTS = (".jpg", ".jpeg", ".png")
MSK_EXTS = (".png", ".jpg", ".jpeg")  # preferir .png quando houver


def infer_ignore_id(ignore_rgb, default_id=255):
    """Tenta inferir ID de ignore a partir do labelmap (mesma ideia do script FastSCNN)."""
    if isinstance(ignore_rgb, (list, tuple)):
        if len(ignore_rgb) == 1 and isinstance(ignore_rgb[0], (int, np.integer)):
            return int(ignore_rgb[0])
        if len(ignore_rgb) == 3:
            return default_id
    if isinstance(ignore_rgb, (int, np.integer)):
        return int(ignore_rgb)
    return default_id


def list_groups(group_root):
    if not os.path.isdir(group_root):
        return []
    out = []
    for g in sorted(os.listdir(group_root)):
        gdir = os.path.join(group_root, g)
        if os.path.isdir(os.path.join(gdir, "images")) and os.path.isdir(os.path.join(gdir, "masks")):
            out.append(g)
    return out


def mask_for_base(msk_dir, base):
    """Encontra máscara correspondente, priorizando .png."""
    best = None
    for ext in MSK_EXTS:
        cand = os.path.join(msk_dir, base + ext)
        if os.path.isfile(cand):
            if best is None:
                best = cand
            if os.path.splitext(cand)[1].lower() == ".png":
                return cand
    return best


def collect_pairs_grouped(test_root, want_groups=None):
    """Coleta pares img/mask de test_root com estrutura 'group/'."""
    group_root = os.path.join(test_root, "group")
    if not os.path.isdir(group_root):
        return [], [], []
    groups = list_groups(group_root)
    if want_groups:
        filt = {g.strip() for g in want_groups.split(",") if g.strip()}
        groups = [g for g in groups if g in filt]

    imgs, msks, groups_idx = [], [], []
    for g in groups:
        img_dir = os.path.join(group_root, g, "images")
        msk_dir = os.path.join(group_root, g, "masks")
        for p in sorted(glob.glob(os.path.join(img_dir, "*"))):
            base, ext = os.path.splitext(os.path.basename(p))
            if ext.lower() not in IMG_EXTS:
                continue
            m = mask_for_base(msk_dir, base)
            if m:
                imgs.append(p)
                msks.append(m)
                groups_idx.append(g)
    return imgs, msks, groups_idx


def collect_pairs_legacy(test_root):
    """Coleta pares img/mask sem 'group/'."""
    img_dir = os.path.join(test_root, "images")
    msk_dir = os.path.join(test_root, "masks")
    imgs, msks, groups_idx = [], [], []
    for p in sorted(glob.glob(os.path.join(img_dir, "*"))):
        base, ext = os.path.splitext(os.path.basename(p))
        if ext.lower() not in IMG_EXTS:
            continue
        m = mask_for_base(msk_dir, base)
        if m:
            imgs.append(p)
            msks.append(m)
            groups_idx.append("legacy")
    return imgs, msks, groups_idx


def _extract_state_dict(ckpt):
    """
    Aceita:
      - state_dict puro (dict de tensores)
      - checkpoint com chaves comuns: state_dict / model_state_dict / model
    """
    if not isinstance(ckpt, dict):
        return None

    # caso já seja um state_dict puro
    if any(isinstance(v, torch.Tensor) for v in ckpt.values()):
        return ckpt

    for k in ("state_dict", "model_state_dict", "model"):
        if k in ckpt and isinstance(ckpt[k], dict):
            return ckpt[k]

    return None


def load_segformer_from_checkpoint(
    pt_path: str,
    backbone: str,
    num_classes: int,
    device: torch.device,
):
    """
    Carrega um SegFormer (B0, B1, B2, B3...) compatível com o treino:

      - Cria o modelo via from_pretrained(backbone, num_labels=num_classes)
      - Carrega o state_dict salvo pelo script de treino
    """
    ckpt = torch.load(pt_path, map_location="cpu", weights_only=True)
    state_dict = _extract_state_dict(ckpt)
    if state_dict is None:
        raise RuntimeError(f"Não consegui extrair state_dict de {pt_path}. keys={list(ckpt.keys())}")

    # limpar prefixos comuns
    cleaned = {}
    for k, v in state_dict.items():
        nk = k
        if nk.startswith("model."):
            nk = nk[len("model."):]
        if nk.startswith("module."):
            nk = nk[len("module."):]
        cleaned[nk] = v

    # Cria o modelo igual ao treino (_8_train_segformer_b3.py)
    model = SegformerForSemanticSegmentation.from_pretrained(
        backbone,
        num_labels=num_classes,
        ignore_mismatched_sizes=True,
        use_safetensors=True
    )

    missing, unexpected = model.load_state_dict(cleaned, strict=False)
    print(f"[load] missing={len(missing)} unexpected={len(unexpected)}")
    if missing:
        print("[load] missing sample:", missing[:10])
    if unexpected:
        print("[load] unexpected sample:", unexpected[:10])

    model.to(device).eval()
    return model


@torch.no_grad()
def segformer_predict_ids(model, img_tensor):
    """
    img_tensor: [1,3,H,W] float32 normalizado.
    retorna: pred_ids [H,W] (numpy int)
    """
    out = model(pixel_values=img_tensor)
    logits = out.logits  # [B, C, h, w] (pode ser menor que input)
    # Upsample logits para o tamanho do input
    logits = torch.nn.functional.interpolate(
        logits,
        size=img_tensor.shape[-2:],
        mode="bilinear",
        align_corners=False
    )
    pred = torch.argmax(logits, dim=1)  # [B,H,W]
    return pred.squeeze(0).cpu().numpy().astype(np.uint8)


def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("--camera", action="store_true", help="Usar câmera em vez de imagens")
    parser.add_argument("--groups", type=str, default=None, help="Filtrar grupos (ex: chao,erva_cana)")
    parser.add_argument("--split_folder", type=str, default="val", help="split padrão (se usar split/val/test)")
    parser.add_argument("--test_folder", type=str, default=None, help="pasta para teste")
    args = parser.parse_args()

    device = torch.device("cuda" if torch.cuda.is_available() else "cpu")

    # Lê config do projeto (mesmo padrão do fastscnn)
    with open("config.json", "r") as f:
        config = json.load(f)

    MODELO = config["camera"]
    MODEL_NAME = config["model_name"]
    RESOLUCAO = config["resolucao"]          # [W,H] ex: [512,288]
    ROI_INICIO = config["roi_inicio"]        # fração
    ROI_TAMANHO = config["roi_tamanho"]      # fração
    BACKBONE = config["backbone"]

    model_to_use = config["model_to_use"]
    dataset_path = os.path.join(MODELO, "dataset")
    labelmap_path = os.path.join(dataset_path, "labelmap.txt")

    # Backbone default (se não passar por argumento)
    backbone = BACKBONE

    # Caminho do .pt (se não passar por argumento)
    model_path = os.path.join(MODELO, "backup", config["modelo"], MODEL_NAME)
    model_name = ""
    if model_to_use == "geral":
        model_name = f"best_miou.pt"
    elif model_to_use == "main_class":
        model_name = f"best_main.pt"
    else:
        model_name = f"last.pt"
    pt_path = os.path.join(model_path, model_name)

    if not pt_path:
        raise SystemExit(
            "Faltou apontar o .pt do SegFormer.\n"
            "Use: --pt caminho/do/best_miou.pt\n"
            "ou adicione 'segformer_pt' no seu config.json."
        )

    # Labelmap
    _, colormap_rgb, classes, ignore_rgb = carregar_labelmap_completo(labelmap_path)
    ignore_id = infer_ignore_id(ignore_rgb, default_id=255)
    num_classes = len(classes)
    print(f"[cfg] classes={num_classes} | backbone={backbone}")
    print(f"[cfg] pt={pt_path}")

    # Modelo
    model = load_segformer_from_checkpoint(pt_path, backbone=backbone, num_classes=num_classes, device=device)

    # Normalização (ImageNet, padrão de muita coisa; se teu treino usou outro, troca aqui)
    from _8_train_segformer import normalize_img

    if args.camera:
        # === Modo câmera (igual estilo do fastscnn) ===
        pipeline = dai.Pipeline()
        cam_rgb = pipeline.createColorCamera()
        cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
        cam_rgb.setBoardSocket(dai.CameraBoardSocket.CAM_A)
        cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.RGB)
        cam_rgb.setInterleaved(False)
        cam_rgb.setFps(30)

        xout_rgb = pipeline.createXLinkOut()
        xout_rgb.setStreamName("rgb")
        cam_rgb.video.link(xout_rgb.input)

        with dai.Device(pipeline) as oak_device:
            rgb_queue = oak_device.getOutputQueue(name="rgb", maxSize=4, blocking=False)

            prev_time = time.time()
            while True:
                in_rgb = rgb_queue.get()
                frame_bgr = in_rgb.getCvFrame()  # BGR
                frame = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2RGB)
                H, W = frame.shape[:2]
                y_fim, y_inicio = compute_roi_indices(H, ROI_INICIO, ROI_TAMANHO)

                roi = frame[y_fim:y_inicio, 0:W]
                roi_resized = resize_keep_width(roi, RESOLUCAO[0], RESOLUCAO[1], cv2.INTER_AREA)

                # tensor
                roi_norm = roi_resized.astype(np.float32) / 255.0
                img_tensor = torch.from_numpy(roi_norm).permute(2, 0, 1).unsqueeze(0).to(device)
                img_tensor = normalize_img(img_tensor)
                img_tensor = img_tensor.float()

                pred_ids = segformer_predict_ids(model, img_tensor)

                pred_rgb = converter_mask_ids_para_rgb(pred_ids, colormap_rgb, ignore_id)
                pred_rgb_resized = cv2.resize(pred_rgb, (roi.shape[1], roi.shape[0]), interpolation=cv2.INTER_NEAREST)

                overlay = frame.copy()
                overlay[y_fim:y_inicio, 0:W] = cv2.addWeighted(
                    overlay[y_fim:y_inicio, 0:W], 0.4, pred_rgb_resized, 0.6, 0
                )

                now = time.time()
                fps = 1.0 / max(1e-6, (now - prev_time))
                prev_time = now

                mask_nav = (pred_ids == ClassesSegmentacao.NAVEGAVEL.value)
                status_now, status_final, debug = classificar_status_corredor(mask_nav)

                cv2.putText(overlay, f"FPS: {fps:.1f} - {status_now.name}", (10, 30),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)

                legenda = desenhar_legenda_vertical(colormap_rgb, classes)
                legenda_resized = cv2.resize(legenda, (150, 30 * len(colormap_rgb)), interpolation=cv2.INTER_AREA)

                h, w = overlay.shape[:2]
                h_leg, w_leg = legenda_resized.shape[:2]
                x_offset = w - w_leg - 10
                y_offset = h - h_leg - 30
                overlay[y_offset:y_offset + h_leg, x_offset:x_offset + w_leg] = legenda_resized

                cv2.imshow("Segmentação SegFormer (OAK + PyTorch)", cv2.cvtColor(overlay, cv2.COLOR_RGB2BGR))
                if cv2.waitKey(1) & 0xFF == ord('q'):
                    break

            cv2.destroyAllWindows()

    else:
        # === Modo imagens (agrupado + fallback) ===
        def collect_images_only(folder):
            exts = (".jpg", ".jpeg", ".png")
            paths = [
                os.path.join(folder, f)
                for f in sorted(os.listdir(folder))
                if f.lower().endswith(exts)
            ]
            return paths
        
        test_root = args.test_folder if args.test_folder else ""

        image_paths = []
        mask_paths = []
        groups_idx = []

        if test_root:
            # tenta modo "imagens puras"
            image_paths = collect_images_only(test_root)
            if image_paths:
                mask_paths = None
                groups_idx = None
                print(f"[TEST] Modo inferência pura: {len(image_paths)} imagens")
            else:
                # fallback: dataset estruturado
                image_paths, mask_paths, groups_idx = collect_pairs_grouped(
                    test_root, want_groups=args.groups
                )
        else:
            test_root = os.path.join(dataset_path, "split", args.split_folder)
            image_paths, mask_paths, groups_idx = collect_pairs_grouped(
                test_root, want_groups=args.groups
            )
            if not image_paths:
                image_paths, mask_paths, groups_idx = collect_pairs_legacy(test_root)

        assert len(image_paths) > 0, "Nenhuma imagem encontrada."
        if mask_paths is not None:
            assert len(image_paths) == len(mask_paths), "Mismatch imagem/máscara"

        idx = 0
        window_name = "Original | GroundTruth | Predito (SegFormer)"
        cv2.namedWindow(window_name, cv2.WINDOW_NORMAL) # permite redimensionar/maximizar
        while True:
            img_path = image_paths[idx]
            grupo = groups_idx[idx] if groups_idx else "?"

            img_rgb = np.array(Image.open(img_path).convert("RGB"))

            if mask_paths is not None:
                mask_path = mask_paths[idx]
                mask_gt = np.array(Image.open(mask_path).convert("L"))
            else:
                mask_gt = None

            H, W = img_rgb.shape[:2]
            y_fim, y_inicio = compute_roi_indices(H, ROI_INICIO, ROI_TAMANHO)

            img_roi = img_rgb[y_fim:y_inicio, 0:W]

            if mask_gt is not None:
                mask_roi = mask_gt[y_fim:y_inicio, 0:W]
            else:
                mask_roi = None

            img_resized = resize_keep_width(img_roi, RESOLUCAO[0], RESOLUCAO[1], cv2.INTER_AREA)

            if mask_roi is not None:
                mask_resized = resize_keep_width(mask_roi, RESOLUCAO[0], RESOLUCAO[1], cv2.INTER_NEAREST)
            else:
                mask_resized = None

            img_norm = img_resized.astype(np.float32) / 255.0
            img_tensor = torch.from_numpy(img_norm).permute(2, 0, 1).unsqueeze(0).to(device)
            img_tensor = normalize_img(img_tensor)
            img_tensor = img_tensor.float()

            pred_ids = segformer_predict_ids(model, img_tensor)

            pred_rgb = converter_mask_ids_para_rgb(pred_ids, colormap_rgb, ignore_id)

            overlay_pred = cv2.addWeighted(img_resized, 0.6, pred_rgb, 0.4, 0.0)
            if mask_gt is not None:
                mask_gt_rgb = converter_mask_ids_para_rgb(mask_resized, colormap_rgb, ignore_id)
                resultado = np.concatenate([img_resized, mask_gt_rgb, overlay_pred], axis=1)
            else:
                resultado = np.concatenate([img_resized, overlay_pred], axis=1)

            legenda = desenhar_legenda_horizontal(colormap_rgb, classes)
            legenda_resized = cv2.resize(legenda, (resultado.shape[1], legenda.shape[0]), interpolation=cv2.INTER_NEAREST)
            resultado_completo = np.concatenate([resultado, legenda_resized], axis=0)


            mask_nav = (pred_ids == ClassesSegmentacao.NAVEGAVEL.value)
            classificar_status_corredor(mask_nav)


            cv2.putText(resultado_completo, f"grupo: {grupo}", (10, 24), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 0), 2)
            # Converte pra BGR pra exibir
            vis_bgr = cv2.cvtColor(resultado_completo, cv2.COLOR_RGB2BGR)
            # Limites máximos da janela (ajusta se quiser)
            MAX_WIDTH = 1600
            MAX_HEIGHT = 900
            h, w = vis_bgr.shape[:2]
            scale = min(MAX_WIDTH / w, MAX_HEIGHT / h, 1.0)
            if scale < 1.0:
                new_w = int(w * scale)
                new_h = int(h * scale)
                vis_bgr = cv2.resize(vis_bgr, (new_w, new_h), interpolation=cv2.INTER_AREA)
            cv2.imshow(window_name, vis_bgr)
            key = cv2.waitKey(0) & 0xFF

            if key == ord('q'):
                break
            elif key == ord('d'):
                idx = (idx + 1) % len(image_paths)
            elif key == ord('a'):
                idx = (idx - 1 + len(image_paths)) % len(image_paths)

        cv2.destroyAllWindows()



class ClassesSegmentacao(IntEnum):
    NAONAVEGAVEL = 0
    NAVEGAVEL = 1


class StatusCarroMapa(IntEnum):
    Parado = 0
    EntrandoRua = 1
    CaminhandoRua = 2
    SaindoRua = 3
    Manobrando = 4
    Direcionando = 5
    RetornandoBase = 6  # não usamos aqui, mas mantido
    Indefinido = 7


# ===== HISTER ESE TEMPORAL =====

janela_s_padrao = 1.5
# deixa espaço pra um histórico decente, a janela em segundos faz o recorte real
_status_hist = deque(maxlen=60)


def _now() -> float:
    """Tempo monotônico (não sofre com ajustes de relógio do SO)."""
    return time.monotonic()


def _maioria_ultimos(janela_s: float | None = None) -> StatusCarroMapa:
    """Maioria dos statuses dentro da última janela de tempo (em segundos)."""
    J = float(janela_s) if janela_s is not None else float(janela_s_padrao)

    t_now = _now()

    # limpa itens FORA da janela
    while _status_hist and (t_now - _status_hist[0][1] > J):
        _status_hist.popleft()

    if not _status_hist:
        # fallback razoável
        return StatusCarroMapa.Direcionando

    # maioria simples
    cont: Dict[StatusCarroMapa, int] = {}
    for st, _t in _status_hist:
        cont[st] = cont.get(st, 0) + 1

    top_freq = max(cont.values())
    empatados = [st for st, c in cont.items() if c == top_freq]

    if len(empatados) == 1:
        return empatados[0]

    # desempata olhando do fim pro início (mais recente primeiro)
    for st, _t in reversed(_status_hist):
        if st in empatados:
            return st


def classificar_status_corredor(mask_nav: np.ndarray):
    """
    mask_nav: (H,W) com 1 = navegável, 0 = não-navegável

    Retorna:
      status_now   : StatusCarroMapa (instantâneo deste frame)
      status_final : StatusCarroMapa (com histerese)
      probs        : dict[StatusCarroMapa, float]  (one-hot)
      debug        : métricas pra log/diagnóstico
    """
    if mask_nav is None or mask_nav.size == 0:
        status_now = StatusCarroMapa.Manobrando
        _status_hist.append((status_now, _now()))
        status_final = _maioria_ultimos()
        return status_now, status_final, {
            "nav_near": 0.0,
            "nav_mid": 0.0,
            "nav_far": 0.0,
            "nav_global": 0.0,
        }

    H, W = mask_nav.shape
    nav = (mask_nav > 0).astype(np.float32)

    def faixa_mean(y0: int, y1: int) -> float:
        fatia = nav[y0:y1, :]
        if fatia.size == 0:
            return 0.0
        return float(fatia.mean())

    # 3 faixas verticais: far (topo), mid (meio), near (embaixo)
    y_far_top  = 0
    y_far_bot  = int(0.25 * H)
    y_mid_top  = y_far_bot
    y_mid_bot  = int(0.5 * H)
    y_near_top = y_mid_bot
    y_near_bot = H

    nav_far    = faixa_mean(y_far_top,  y_far_bot)
    nav_mid    = faixa_mean(y_mid_top,  y_mid_bot)
    nav_near   = faixa_mean(y_near_top, y_near_bot)
    nav_global = float(nav.mean())

    # diferenças entre faixas (pra medir quão "desbalanceado" está)
    d_nm = abs(nav_near - nav_mid)
    d_mf = abs(nav_mid  - nav_far)
    d_nf = abs(nav_near - nav_far)
    max_delta = max(d_nm, d_mf, d_nf)

    # ---- Blobs 2D na região distante (parede esquerda x direita) ----
    y_blob_top = 0
    y_blob_bot = int(0.35 * H)  # um pouco mais profundo que o nav_far

    faixa_obs = (mask_nav[y_blob_top:y_blob_bot, :] == 0).astype(np.uint8)  # 1 = obstáculo

    H_blob, W_blob = faixa_obs.shape
    num_blobs_far = 0
    corridor_nav_far = 0.0
    corridor_width_frac = 0.0

    if H_blob > 0 and W_blob > 0 and faixa_obs.max() > 0:
        # connectedComponents espera 0/255
        faixa_obs_bin = (faixa_obs * 255).astype(np.uint8)

        num_labels, labels = cv2.connectedComponents(faixa_obs_bin)

        # ignora blobs muito pequenos (ruído)
        MIN_AREA = 0.005 * H_blob * W_blob  # 0.5% da área da faixa
        blobs = []

        for label in range(1, num_labels):  # 0 é o fundo
            ys, xs = np.where(labels == label)
            area = len(xs)
            if area < MIN_AREA:
                continue

            x_min, x_max = xs.min(), xs.max()
            y_min, y_max = ys.min(), ys.max()
            blobs.append({
                "area": area,
                "bbox": (x_min, y_min, x_max, y_max),
                "x_center": float(xs.mean()),
            })

        num_blobs_far = len(blobs)

        if num_blobs_far >= 2:
            # ordena da parede mais à esquerda pra mais à direita
            blobs_sorted = sorted(blobs, key=lambda b: b["x_center"])
            left_blob  = blobs_sorted[0]
            right_blob = blobs_sorted[-1]

            # corredor é a região entre x_max da esquerda e x_min da direita
            x_left  = left_blob["bbox"][2] + 1  # x_max (inclusivo) -> +1 pra slice
            x_right = right_blob["bbox"][0]     # x_min

            if x_right > x_left:
                corridor_width = x_right - x_left
                corridor_width_frac = corridor_width / float(W)

                faixa_nav_corridor = (mask_nav[y_blob_top:y_blob_bot, x_left:x_right] > 0).astype(np.float32)
                if faixa_nav_corridor.size > 0:
                    corridor_nav_far = float(faixa_nav_corridor.mean())

    # ===== Limiares (podemos tunar depois) =====
    THR_BLOCKED   = 0.30  # global bem baixo -> quase sem caminho
    THR_OPEN      = 0.90  # global bem alto -> mundo aberto
    THR_DELTA_COR = 0.15  # diferença "significativa" entre faixas
    THR_COR_FAR_LARGO = 0.65  # far ainda relativamente alto em corredor largo
    THR_OPEN_MUITO_LIMPO = 0.90  # quase tudo navegável
    THR_DELTA_OBST_PEQ = 0.20  # desbalance vertical máximo para considerar "campo aberto com obstáculo pequeno"
    MIN_CORRIDOR_WIDTH_FRAC = 0.10  # corredor tem que ter pelo menos ~10% da largura
    MAX_CORRIDOR_WIDTH_FRAC = 0.70  # evita chamar de corredor quando é um "campo" gigante

    # 1) Parado: quase não há caminho à frente (meio+frente mortos)
    cond_parado = (
        nav_global <= THR_BLOCKED
        and nav_mid < 0.20
        and nav_far < 0.10
    )

    # 2) Direcionando: mundo aberto, sem corredor marcado
    #    2.1 base: tudo alto e muito homogêneo
    cond_direcionando_base = (
        nav_global >= THR_OPEN
        and max_delta < 0.10
    )

    #    2.2 modo "campo aberto com obstáculo pequeno":
    #        quase tudo navegável, e até o far é bem alto
    cond_direcionando_obst_peq = (
        nav_global >= THR_OPEN_MUITO_LIMPO and  # >= 0.90
        nav_near   >= 0.95 and
        nav_mid    >= 0.80 and                  # um pouco mais permissivo
        nav_far    >= 0.50 and                  # aceita far um pouco mais fechado
        num_blobs_far <= 1                      # no máximo UMA parede grande
    )

    #    2.3 modo "campo aberto com parede na frente":
    #        chão bem navegável perto, sem corredor definido, e FAR quase todo bloqueado
    cond_direcionando_frente_fe_chada = (
        nav_near >= 0.80 and          # perto bem aberto
        nav_mid  >= 0.30 and          # meio ainda razoável
        nav_far  <= 0.10 and          # topo praticamente bloqueado (parede)
        nav_global >= 0.50 and        # ainda tem bastante área navegável no frame
        num_blobs_far <= 1            # no máximo uma "parede", nada de corredor
    )

    # 2.3 modo "campo aberto com borda lateral":
    #     cena razoavelmente aberta, uma parede forte de um lado, mas sem corredor fechado
    cond_direcionando_borda_lateral = (
        nav_global >= 0.60 and        # já tem boa área navegável
        nav_near   >= 0.70 and
        nav_mid    >= 0.50 and
        nav_far    >= 0.40 and        # far não está "morrendo", só mais sujo
        nav_far    <= 0.80 and        # não é mundão 100% limpo
        num_blobs_far == 1            # exatamente UMA parede grande
    )

    cond_direcionando_aberto = (
        nav_global >= 0.75 and
        nav_near   >= 0.70 and
        nav_mid    >= 0.70 and
        nav_far    >= 0.70 and
        max_delta  <= 0.12 and
        num_blobs_far <= 1
    )

    cond_direcionando = (
        cond_direcionando_base
        or cond_direcionando_obst_peq
        or cond_direcionando_frente_fe_chada
        or cond_direcionando_borda_lateral
        or cond_direcionando_aberto
    )

    # 3) CaminhandoRua: dentro do corredor "clássico"
    cond_caminhando_base = (
        nav_near >= 0.55 and
        nav_mid  >= 0.25 and
        nav_far  <= 0.50 and
        (nav_near - nav_far) >= 0.20 and
        num_blobs_far >= 2          # precisa de DUAS paredes
    )

    #    3.1 CaminhandoRua em corredor mais largo, com parede só de um lado
    #        enquadra bem os casos:
    #        nav_global ~0.66–0.77, near ~0.75–0.88, mid ~0.57–0.70, far ~0.56–0.63
    cond_caminhando_largo = (
        nav_global >= 0.60 and
        nav_near   >= 0.75 and
        nav_mid    >= 0.50 and
        nav_far    >= 0.50 and
        nav_far    <= THR_COR_FAR_LARGO and
        (nav_near - nav_far) >= THR_DELTA_COR and
        num_blobs_far >= 2          # corredor largo, mas ainda corredor
    )

    cond_caminhando_multi_corredores = (
        nav_global >= 0.50 and             # tem chão suficiente
        nav_mid    >= 0.55 and             # meio bem limpo
        nav_near   >= 0.50 and             # perto também ok
        num_blobs_far >= 2 and             # pelo menos duas "paredes"
        corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and
        corridor_width_frac <= MAX_CORRIDOR_WIDTH_FRAC and
        corridor_nav_far >= 0.55           # corredor entre paredes bem navegável
    )

    cond_caminhando = (
        cond_caminhando_base
        or cond_caminhando_largo
        or cond_caminhando_multi_corredores
    )

    cond_entrando = (
        nav_global >= 0.75 and 
        nav_near >= 0.90 and
        nav_mid  >= 0.60 and
        nav_far  >= 0.40 and
        nav_far  <= 0.85 and
        (nav_near - nav_far) >= 0.10 and          # far mais fechado que near
        num_blobs_far >= 2 and                    # duas paredes detectadas
        corridor_width_frac >= MIN_CORRIDOR_WIDTH_FRAC and
        corridor_nav_far >= 0.60                  # corredor entre as paredes bem navegável
    )

    # 5) SaindoRua
    cond_saindo_1 = (
        nav_near >= 0.50 and
        nav_mid  >= 0.25 and
        nav_far  >= 0.55 and
        (nav_far - nav_mid) >= 0.10 and
        nav_far >= nav_near - 0.15
    )

    cond_saindo_2 = (
        nav_global >= 0.75 and
        nav_near   >= 0.70 and
        nav_mid    >= 0.60 and
        nav_far    >= 0.75 and
        nav_far >= nav_mid
    )

    cond_saindo = cond_saindo_1 or cond_saindo_2

    # ===== Decisão (ordem importa!) =====
    if cond_parado:
        status = StatusCarroMapa.Parado
    elif cond_direcionando:
        status = StatusCarroMapa.Direcionando
    elif cond_saindo:
        status = StatusCarroMapa.SaindoRua
    elif cond_entrando:
        status = StatusCarroMapa.EntrandoRua
    elif cond_caminhando:
        status = StatusCarroMapa.CaminhandoRua
    else:
        status = StatusCarroMapa.Indefinido

    debug = {
        "nav_global": nav_global,
        "nav_near": nav_near,
        "nav_mid": nav_mid,
        "nav_far": nav_far,
        "d_nm": d_nm,
        "d_mf": d_mf,
        "d_nf": d_nf,
        "max_delta": max_delta,
        "num_blobs_far": num_blobs_far,
        "corridor_nav_far": corridor_nav_far,
        "corridor_width_frac": corridor_width_frac,
        "cond_parado": cond_parado,
        "cond_direcionando_base": cond_direcionando_base,
        "cond_direcionando_obst_peq": cond_direcionando_obst_peq,
        "cond_caminhando_base": cond_caminhando_base,
        "cond_caminhando_largo": cond_caminhando_largo,
        "cond_entrando": cond_entrando,
        "cond_saindo": cond_saindo,
        "THR_BLOCKED": THR_BLOCKED,
        "THR_OPEN": THR_OPEN,
        "THR_DELTA_COR": THR_DELTA_COR,
        "THR_COR_FAR_LARGO": THR_COR_FAR_LARGO,
        "THR_OPEN_MUITO_LIMPO": THR_OPEN_MUITO_LIMPO,
    }

    status_now = status
    print(status_now.name, debug)

    _status_hist.append((status_now, _now()))
    status_final = _maioria_ultimos()

    return status_now, status_final, debug




if __name__ == "__main__":
    main()