import time
import json
import numpy as np


class RadiometricController:
    def __init__(
        self,
        client,
        enabled=True,
        config_json_path=None,
        interval_s=0.5,
        strip_y0_pct=0.95,
        strip_y1_pct=1.0,
        patch_x0_pct=0.35,
        patch_x1_pct=0.75,
        target_mean=0.70,
        deadband=0.03,
        alpha=0.20,
        exp_min_us=100,
        exp_max_us=80000,
        gain_min=1.0,
        gain_max=8.0,
        exp_step_gain=0.65,
        prefer_exposure=True,
        verbose=False,
    ):
        self.client = client

        cfg = self._load_config_json(config_json_path)

        interval_s = cfg.get("interval_s", interval_s)
        strip_y0_pct = cfg.get("strip_y0_pct", strip_y0_pct)
        strip_y1_pct = cfg.get("strip_y1_pct", strip_y1_pct)
        patch_x0_pct = cfg.get("patch_x0_pct", patch_x0_pct)
        patch_x1_pct = cfg.get("patch_x1_pct", patch_x1_pct)
        target_mean = cfg.get("target_mean", target_mean)
        deadband = cfg.get("deadband", deadband)
        alpha = cfg.get("alpha", alpha)
        exp_min_us = cfg.get("exp_min_us", exp_min_us)
        exp_max_us = cfg.get("exp_max_us", exp_max_us)
        gain_min = cfg.get("gain_min", gain_min)
        gain_max = cfg.get("gain_max", gain_max)
        exp_step_gain = cfg.get("exp_step_gain", exp_step_gain)
        prefer_exposure = cfg.get("prefer_exposure", prefer_exposure)
        verbose = cfg.get("verbose", verbose)
        exp_apply_threshold_us = cfg.get("exp_apply_threshold_us", 50)
        gain_apply_threshold = cfg.get("gain_apply_threshold", 0.02)

        self.enabled = bool(enabled)
        self.interval_s = float(interval_s)

        self.strip_y0_pct = float(strip_y0_pct)
        self.strip_y1_pct = float(strip_y1_pct)
        self.patch_x0_pct = float(patch_x0_pct)
        self.patch_x1_pct = float(patch_x1_pct)

        self.target_mean = float(target_mean)
        self.deadband = float(deadband)
        self.alpha = float(alpha)

        self.exp_min_us = int(exp_min_us)
        self.exp_max_us = int(exp_max_us)
        self.gain_min = float(gain_min)
        self.gain_max = float(gain_max)

        self.exp_step_gain = float(exp_step_gain)
        self.prefer_exposure = bool(prefer_exposure)
        self.verbose = bool(verbose)

        self.exp_apply_threshold_us = int(exp_apply_threshold_us)
        self.gain_apply_threshold = float(gain_apply_threshold)

        self.last_update_ts = 0.0
        self.last_result = {}

        self.state = {
            "cam0": {"exp": 15000, "gain": 1.0},
            "cam1": {"exp": 15000, "gain": 1.0},
            "cam2": {"exp": 15000, "gain": 1.0},
        }
        self._ae_disabled = set()
        self._last_applied = {
            "cam0": {"exp": None, "gain": None},
            "cam1": {"exp": None, "gain": None},
            "cam2": {"exp": None, "gain": None},
        }

    def _load_config_json(self, path):
        if not path:
            return {}

        try:
            with open(path, "r", encoding="utf-8") as f:
                data = json.load(f)
        except Exception:
            return {}

        cfg = data.get("radiometric_config", {})
        return cfg if isinstance(cfg, dict) else {}

    def sync_from_camera_controls(self, camera_controls: dict | None):
        if not isinstance(camera_controls, dict):
            return

        for cam_id, ctrl in camera_controls.items():
            if cam_id not in self.state:
                continue

            exp = ctrl.get("exposure_time_us")
            gain = ctrl.get("analogue_gain")

            if exp is not None:
                self.state[cam_id]["exp"] = int(exp)

            if gain is not None:
                self.state[cam_id]["gain"] = float(gain)

    def update(self, decoded: dict, meta: dict | None = None):
        if not self.enabled:
            return None

        now = time.perf_counter()
        if now - self.last_update_ts < self.interval_s:
            return None

        self.last_update_ts = now

        results = {}

        for cam_id in ("cam2", "cam0", "cam1"):
            if cam_id not in decoded:
                continue

            img = decoded[cam_id].get("image")
            if img is None:
                continue

            metrics = self.measure_reference_patch(img)
            decision = self.compute_control(cam_id, metrics)
            apply_resp = self.apply_control(cam_id, decision)

            results[cam_id] = {
                "metrics": metrics,
                "decision": decision,
                "apply": apply_resp,
            }

        self.last_result = results
        return results

    def measure_reference_patch(self, img01: np.ndarray) -> dict:
        if img01.ndim == 3:
            # RGB: usa luminância simples
            img_gray = (
                0.299 * img01[:, :, 0] +
                0.587 * img01[:, :, 1] +
                0.114 * img01[:, :, 2]
            ).astype(np.float32)
        else:
            img_gray = img01.astype(np.float32)

        h, w = img_gray.shape[:2]

        y0 = int(h * self.strip_y0_pct)
        y1 = int(h * self.strip_y1_pct)
        x0 = int(w * self.patch_x0_pct)
        x1 = int(w * self.patch_x1_pct)

        y0 = max(0, min(h - 1, y0))
        y1 = max(y0 + 1, min(h, y1))
        x0 = max(0, min(w - 1, x0))
        x1 = max(x0 + 1, min(w, x1))

        patch = img_gray[y0:y1, x0:x1]
        arr = patch.reshape(-1)

        return {
            "valid": arr.size > 0,
            "mean": float(arr.mean()) if arr.size else 0.0,
            "p05": float(np.percentile(arr, 5)) if arr.size else 0.0,
            "p95": float(np.percentile(arr, 95)) if arr.size else 0.0,
            "sat_pct": float((arr >= 0.98).mean() * 100.0) if arr.size else 0.0,
            "dark_pct": float((arr <= 0.02).mean() * 100.0) if arr.size else 0.0,
            "roi": [x0, y0, x1, y1],
        }

    def compute_control(self, cam_id: str, metrics: dict) -> dict:
        st = self.state.setdefault(cam_id, {"exp": 15000, "gain": 1.0})

        old_exp = int(st["exp"])
        old_gain = float(st["gain"])

        if not metrics.get("valid"):
            return {
                "action": "hold",
                "reason": "patch inválido",
                "old_exp": old_exp,
                "new_exp": old_exp,
                "old_gain": old_gain,
                "new_gain": old_gain,
            }

        mean = float(metrics["mean"])
        p95 = float(metrics["p95"])
        sat_pct = float(metrics["sat_pct"])
        error = self.target_mean - mean

        new_exp = old_exp
        new_gain = old_gain
        action = "hold"
        reason = "dentro da faixa morta"

        # Proteção contra saturação
        if sat_pct > 1.0 or p95 > 0.96:
            desired_exp = max(self.exp_min_us, int(old_exp * 0.85))
            new_exp = self._smooth_int(old_exp, desired_exp)
            action = "decrease_exposure"
            reason = f"saturação detectada: sat={sat_pct:.2f}% p95={p95:.3f}"

        elif abs(error) > self.deadband:
            factor = 1.0 + self.exp_step_gain * error
            factor = max(0.70, min(1.35, factor))

            if self.prefer_exposure:
                desired_exp = int(old_exp * factor)
                desired_exp = self._clamp(desired_exp, self.exp_min_us, self.exp_max_us)
                new_exp = self._smooth_int(old_exp, desired_exp)

                # Se exposição bateu limite e ainda precisa clarear/escurecer, mexe no ganho
                if desired_exp in (self.exp_min_us, self.exp_max_us):
                    desired_gain = old_gain * factor
                    desired_gain = self._clamp(desired_gain, self.gain_min, self.gain_max)
                    new_gain = self._smooth_float(old_gain, desired_gain)

                action = "increase_exposure" if error > 0 else "decrease_exposure"
                reason = f"corrigindo erro radiométrico: error={error:.3f}"
            else:
                desired_gain = old_gain * factor
                desired_gain = self._clamp(desired_gain, self.gain_min, self.gain_max)
                new_gain = self._smooth_float(old_gain, desired_gain)
                action = "increase_gain" if error > 0 else "decrease_gain"
                reason = f"corrigindo ganho: error={error:.3f}"

        new_exp = int(self._clamp(new_exp, self.exp_min_us, self.exp_max_us))
        new_gain = float(self._clamp(new_gain, self.gain_min, self.gain_max))

        return {
            "action": action,
            "reason": reason,
            "mean": mean,
            "target_mean": self.target_mean,
            "error": error,
            "old_exp": old_exp,
            "new_exp": new_exp,
            "old_gain": old_gain,
            "new_gain": new_gain,
        }

    def apply_control(self, cam_id: str, decision: dict):
        new_exp = int(decision["new_exp"])
        new_gain = float(decision["new_gain"])

        self.state[cam_id]["exp"] = new_exp
        self.state[cam_id]["gain"] = new_gain

        responses = {}
        last = self._last_applied.setdefault(cam_id, {"exp": None, "gain": None})

        try:
            if cam_id not in self._ae_disabled:
                responses["ae"] = self.client.svc.set_ae_enable(cam_id, False)

                if cam_id == "cam2":
                    responses["awb"] = self.client.svc.set_awb_enable(cam_id, False)

                self._ae_disabled.add(cam_id)

            if last["exp"] is None or abs(new_exp - last["exp"]) >= self.exp_apply_threshold_us:
                responses["exposure"] = self.client.svc.set_exposure_time(cam_id, new_exp)
                last["exp"] = new_exp

            if last["gain"] is None or abs(new_gain - last["gain"]) >= self.gain_apply_threshold:
                responses["gain"] = self.client.svc.set_analogue_gain(cam_id, new_gain)
                last["gain"] = new_gain

        except Exception as e:
            responses["error"] = str(e)

        if self.verbose:
            print(f"[RAD] {cam_id}: {json.dumps(decision, ensure_ascii=False)} | apply={responses}")

        return responses

    def _smooth_int(self, old, desired):
        return int(round((1.0 - self.alpha) * old + self.alpha * desired))

    def _smooth_float(self, old, desired):
        return float((1.0 - self.alpha) * old + self.alpha * desired)

    @staticmethod
    def _clamp(v, lo, hi):
        return max(lo, min(hi, v))