"""Experimental temporal qualification; never grants vehicle authority."""

import numpy as np

from .registration import angle_deg, rigid, transform

TRACKING_POLICY = dict(
    version="causal-consistency/v1",
    consecutive=3,
    maximum_position_change_m=0.5,
    maximum_rotation_change_deg=5.0,
    maximum_age_s=8.0,
)


class CausalTracking:
    def __init__(self):
        self.matrix = None
        self.sample_ns = 0
        self.segment = 0
        self.streak = 0
        self.state = "acquiring"
        self.reason = "initial"

    def clear(self, reason):
        self.matrix = None
        self.sample_ns = 0
        self.streak = 0
        self.state = "lost"
        self.reason = reason

    def tick(self, now_ns, segment):
        if segment != self.segment:
            self.clear("receipt-gap")
            self.segment = segment
        elif (
            self.matrix is not None
            and (now_ns - self.sample_ns) / 1e9 > TRACKING_POLICY["maximum_age_s"]
        ):
            self.clear("stale")

    def accept(self, result, sample, now_ns, segment):
        self.tick(now_ns, segment)
        age = (now_ns - sample["monotonic_ns"]) / 1e9
        evidence = dict(age_s=age, position_change_m=None, rotation_change_deg=None)
        if sample["segment"] != segment:
            # An old job must never overwrite new-segment state.
            return {**evidence, "accepted": False, "reason": "old-segment"}
        if not 0 <= age <= TRACKING_POLICY["maximum_age_s"]:
            self.clear("stale-result")
        elif result["status"] != "candidate":
            self.clear("registration-rejected")
        else:
            matrix = rigid(result["T_reference_query"])
            if self.matrix is not None:
                position = sample["path"][-1:]
                delta = float(
                    np.linalg.norm(transform(position, matrix) - transform(position, self.matrix))
                )
                rotation = angle_deg(matrix[:3, :3] @ self.matrix[:3, :3].T)
                evidence.update(position_change_m=delta, rotation_change_deg=rotation)
                if (
                    delta > TRACKING_POLICY["maximum_position_change_m"]
                    or rotation > TRACKING_POLICY["maximum_rotation_change_deg"]
                ):
                    self.clear("inconsistent-candidate")
                    return {**evidence, "accepted": False, "reason": self.reason}
            self.matrix = matrix
            self.sample_ns = sample["monotonic_ns"]
            self.streak += 1
            self.state = (
                "tracking" if self.streak >= TRACKING_POLICY["consecutive"] else "acquiring"
            )
            self.reason = "consistent-candidate"
            return {**evidence, "accepted": True, "reason": self.reason}
        return {**evidence, "accepted": False, "reason": self.reason}