"""Stateful map-matcher — stable per-vessel binding to the fairway network.

Wraps a FairwayNetwork and keeps a short track history per MMSI so that the
route a vessel is bound to stays stable through junctions and survives GPS
jitter, instead of the stateless per-fix nearest-route snapping.

Five mechanisms, all pure-Python and tunable:
  * TRACK-COG   — the signal that ranks parallel/crossing routes is the *course
                  made good* over the last `min_track_hits` fixes (the leg
                  bearings between them), not a single reported COG. A vessel is
                  not bound until it has that many fixes and a derivable course,
                  so the route it lands on is the one running along the stretch
                  it is actually travelling.
  * SLOW-DETACH — a vessel under `slow_speed_kn` for more than `slow_detach_hits`
                  consecutive fixes is released from its route (moored/anchored
                  vessels stop being glued to whatever fairway they sit beside).
  * STICKINESS  — the current route gets a cost bonus, so a slightly-farther
                  same-route segment beats a slightly-closer parallel route.
  * DEBOUNCE    — switching to a *different* route requires that route to win for
                  `switch_patience` consecutive fixes; a lone outlier can't flip it.
  * OFF-GRACE   — a fix with no route in range marks off-fairway but keeps the
                  last route as context for `off_grace_s` (AIS gaps happen).

Also derives, while a vessel stays on one route: direction of travel (km rising
or falling) and along-route speed (km/h) — the inputs the wrong-way alarm needs.

The per-MMSI history buffer here is the shared position+timestamp track buffer
that calcuSOG / CPA / trails / silent-vessel detection will also draw on.
"""
import math
from collections import deque


class Binding:
    __slots__ = ("mmsi", "on_fairway", "routeid", "route_name", "km", "offset_m",
                 "snap_lon", "snap_lat", "bearing", "direction", "along_kmh",
                 "switched", "confidence", "course_sense", "side", "flow",
                 "ship_type", "sog")

    def __init__(self, mmsi):
        """Start an empty (off-fairway) binding for one MMSI; fields are filled
        in by MapMatcher.update as fixes arrive."""
        self.mmsi = mmsi
        self.on_fairway = False
        self.routeid = None
        self.route_name = None
        self.km = None
        self.offset_m = None
        self.snap_lon = None
        self.snap_lat = None
        self.bearing = None
        self.direction = 0          # +1 km rising, -1 falling, 0 unknown/!moving
        self.along_kmh = None
        self.switched = False       # route changed on this update
        self.confidence = 0.0       # 0..1, higher = closer + more stable
        self.course_sense = 0       # +1 travelling with +km, -1 against, 0 unknown
        self.side = ""              # 'port' / 'stbd' relative to travel
        self.flow = ""              # 'inbound' / 'outbound' (toward sea) / ''
        self.ship_type = None       # AIS ship-type code (for portrayal)
        self.sog = None             # reported speed over ground (knots)

    def __repr__(self):
        """Compact one-line view: off-fairway, or route/km/offset/direction."""
        if not self.on_fairway and self.routeid is None:
            return "Binding(%s OFF-FAIRWAY)" % self.mmsi
        d = {1: "+", -1: "-", 0: "?"}[self.direction]
        flag = "" if self.on_fairway else " (off, last route)"
        return ("Binding(%s route=%s '%s' km=%.3f%s off=%.0fm dir=%s)"
                % (self.mmsi, self.routeid, self.route_name, self.km, flag,
                   self.offset_m or 0.0, d))


class _State:
    __slots__ = ("hist", "routeid", "last_km", "last_t", "pending", "pend_n",
                 "off_since", "slow_n")

    def __init__(self, maxlen):
        """Per-MMSI matcher memory: a capped track buffer plus the current
        route, last km/time, and the pending-switch / off-fairway / slow-speed
        counters."""
        self.hist = deque(maxlen=maxlen)      # (t, lon, lat, cog, sog)
        self.routeid = None
        self.last_km = None
        self.last_t = None
        self.pending = None                   # candidate route awaiting patience
        self.pend_n = 0
        self.off_since = None
        self.slow_n = 0                       # consecutive fixes under slow_speed_kn


class MapMatcher:
    def __init__(self, network, max_offset_m=150.0, stick_bonus_m=40.0,
                 switch_patience=3, history_len=12, off_grace_s=120.0,
                 cog_weight=2.0, min_track_hits=3, slow_speed_kn=2.0,
                 slow_detach_hits=3, min_move_m=3.0, route_tol=None):
        """Wrap a FairwayNetwork with the track-COG / slow-detach / stickiness /
        debounce / off-grace tunables that keep each vessel's route binding
        stable over time.

        min_track_hits   : position fixes required before a vessel is bound; the
                           alignment course is the leg bearing across them.
        slow_speed_kn    : speeds below this (knots) count as 'slow'.
        slow_detach_hits : a vessel slow for MORE than this many consecutive
                           fixes is detached from its route.
        min_move_m       : minimum net displacement across the track window for a
                           course to be considered derivable.
        route_tol        : optional {routeid: max_offset_m} dict giving a route
                           its OWN perpendicular tolerance (e.g. a wide tidal
                           fairway), overriding max_offset_m for that route. Held
                           by reference, so live edits take effect immediately.
        """
        self.net = network
        self.max_offset_m = max_offset_m
        self.route_tol = route_tol if route_tol is not None else {}
        self.stick_bonus_m = stick_bonus_m
        self.switch_patience = switch_patience
        self.history_len = history_len
        self.off_grace_s = off_grace_s
        self.cog_weight = cog_weight
        self.min_track_hits = min_track_hits
        self.slow_speed_kn = slow_speed_kn
        self.slow_detach_hits = slow_detach_hits
        self.min_move_m = min_move_m
        self._state = {}
        self._last = {}             # mmsi -> (t, lon, lat, Binding)

    def active_bindings(self, max_age_s=180.0, now=None):
        """Current bindings seen within max_age_s (for the route-map window)."""
        if now is None:
            now = max((t for t, _, _, _ in self._last.values()), default=0.0)
        return [b for (t, lo, la, b) in self._last.values()
                if now - t <= max_age_s]

    def footprint_points(self, max_age_s=600.0, now=None):
        """Recent received positions — a proxy for the coverage footprint."""
        if now is None:
            now = max((t for t, _, _, _ in self._last.values()), default=0.0)
        return [(lo, la) for (t, lo, la, b) in self._last.values()
                if now - t <= max_age_s and lo is not None]

    def forget(self, mmsi):
        """Drop all matcher state for an MMSI (e.g. a vessel gone out of range),
        including its last fix so it leaves the route strip / footprint at once."""
        self._state.pop(mmsi, None)
        self._last.pop(mmsi, None)

    def binding(self, mmsi):
        """Return the internal _State for an MMSI, or None if never seen."""
        return self._state.get(mmsi)

    def update(self, mmsi, t, lon, lat, cog=None, sog=None, ship_type=None):
        """Feed one timestamped fix and return its Binding.

        A vessel is bound only once it has at least `min_track_hits` position
        fixes, and the route-alignment signal is the *course made good* across
        those fixes (the leg bearings), not a single reported COG — so parallel
        and crossing fairways are separated by the direction the ship is actually
        travelling, and the route it lands on shares the stretch it is on. A
        vessel under `slow_speed_kn` for more than `slow_detach_hits` consecutive
        fixes is detached from its route (moored/anchored). Stickiness +
        switch-debounce + off-fairway grace keep the chosen route stable, then
        direction, along-route speed, course sense, side and inbound/outbound
        flow are derived while it stays on one route. Never raises on a fix with
        no route in range — it returns an off-fairway Binding instead.
        """
        st = self._state.get(mmsi)
        if st is None:
            st = self._state[mmsi] = _State(self.history_len)
        st.hist.append((t, lon, lat, cog, sog))

        b = Binding(mmsi)
        b.ship_type = ship_type
        b.sog = sog

        # slow-speed detach: a vessel that has sat under slow_speed_kn for more
        # than slow_detach_hits fixes is moored/anchored — release it entirely.
        spd = self._speed_kn(st, sog)
        if spd is not None:
            st.slow_n = st.slow_n + 1 if spd < self.slow_speed_kn else 0
        if st.slow_n > self.slow_detach_hits:
            st.routeid = st.last_km = st.last_t = None
            st.pending = None; st.pend_n = 0; st.off_since = None
            b.on_fairway = False
            b.confidence = 0.0
            self._last[mmsi] = (t, lon, lat, b)
            return b

        # course made good over the last min_track_hits fixes drives alignment;
        # only fall back to a single reported COG when no track course exists yet.
        heading, _moved = self._course_made_good(st)
        if heading is None:
            heading = cog

        cands = self.net.candidates(lon, lat, heading, self.max_offset_m,
                                    self.cog_weight, k=6, route_tol=self.route_tol)

        if not cands:
            # nothing in range: off-fairway, keep last route as context briefly
            if st.routeid is not None:
                if st.off_since is None:
                    st.off_since = t
                if t - st.off_since <= self.off_grace_s:
                    b.routeid, b.route_name = st.routeid, self._name(st.routeid)
                else:
                    st.routeid = st.last_km = st.last_t = None
                    st.pending = None; st.pend_n = 0
            b.on_fairway = False
            b.confidence = 0.0
            self._last[mmsi] = (t, lon, lat, b)
            return b
        st.off_since = None

        # establish a binding only with enough track to compute a course made
        # good; an unbound vessel with too few fixes stays off-fairway until it
        # has min_track_hits and a derivable heading.
        if st.routeid is None and (len(st.hist) < self.min_track_hits
                                   or heading is None):
            b.on_fairway = False
            b.confidence = 0.0
            self._last[mmsi] = (t, lon, lat, b)
            return b

        prev_route = st.routeid

        # current-route candidate (lowest-cost one) and best challenger (other route)
        cur = next(((c, m) for c, m in cands if m.routeid == st.routeid), None) \
            if st.routeid is not None else None
        challenger = next(((c, m) for c, m in cands
                           if st.routeid is None or m.routeid != st.routeid), None)

        switched = False
        if st.routeid is None:
            chosen = cands[0][1]                       # first bind
            st.pending = None; st.pend_n = 0
        elif cur is not None and (challenger is None
                                  or challenger[0] > cur[0] - self.stick_bonus_m):
            chosen = cur[1]                            # current still good -> stay
            st.pending = None; st.pend_n = 0
        else:
            # a challenger clearly beats the current route (or current left range)
            ch = challenger or cands[0]
            if st.pending == ch[1].routeid:
                st.pend_n += 1
            else:
                st.pending = ch[1].routeid; st.pend_n = 1
            if st.pend_n >= self.switch_patience or cur is None:
                chosen = ch[1]; st.pending = None; st.pend_n = 0
            else:
                chosen = cur[1] if cur is not None else ch[1]
        switched = prev_route is not None and chosen.routeid != prev_route

        # direction + along-route speed (only meaningful staying on a route)
        direction, along_kmh = 0, None
        if not switched and st.routeid == chosen.routeid and st.last_km is not None \
                and st.last_t is not None and t > st.last_t:
            dkm = chosen.km - st.last_km
            dt_h = (t - st.last_t) / 3600.0
            if abs(dkm) > 1e-4:
                direction = 1 if dkm > 0 else -1
            if dt_h > 0:
                along_kmh = dkm / dt_h

        st.routeid = chosen.routeid
        st.last_km = chosen.km
        st.last_t = t

        b.on_fairway = True
        b.routeid = chosen.routeid
        b.route_name = chosen.route_name
        b.km = chosen.km
        b.offset_m = chosen.offset_m
        b.snap_lon = chosen.snap_lon
        b.snap_lat = chosen.snap_lat
        b.bearing = chosen.bearing
        b.direction = direction
        b.along_kmh = along_kmh
        b.switched = switched

        # course sense: from the track course vs axis bearing, else km-delta sign
        if heading is not None:
            cs = 1 if math.cos(math.radians(heading - chosen.bearing)) >= 0 else -1
        else:
            cs = direction
        b.course_sense = cs
        # side relative to travel (axis_side is +1 left of +km direction)
        if chosen.axis_side and cs:
            left_of_travel = (chosen.axis_side > 0) == (cs > 0)
            b.side = "port" if left_of_travel else "stbd"
        # flow toward/away from sea
        seaward = self.net.meta.get("route_seaward", {}).get(chosen.routeid)
        if seaward is not None and cs:
            outbound = (cs > 0) == seaward      # with-km heads seaward iff seaward
            b.flow = "outbound" if outbound else "inbound"

        # confidence: close + on a settled route = high. Measured against the
        # route's OWN tolerance, so an off-centre bind in a wide tidal fairway
        # reads as lower-confidence rather than impossible.
        eff_tol = self.route_tol.get(chosen.routeid, self.max_offset_m) \
            if self.route_tol else self.max_offset_m
        prox = max(0.0, 1.0 - chosen.offset_m / eff_tol)
        b.confidence = prox * (0.6 if switched else 1.0)
        self._last[mmsi] = (t, lon, lat, b)
        return b

    def _speed_kn(self, st, sog):
        """Best speed estimate in knots: the reported SOG when present, else the
        speed over ground from the last track leg. None if neither is available."""
        if sog is not None:
            return sog
        if len(st.hist) >= 2:
            t0, lo0, la0, _, _ = st.hist[-2]
            t1, lo1, la1, _, _ = st.hist[-1]
            if t1 > t0 and lo0 is not None and lo1 is not None:
                x0, y0 = self.net.to_m(lo0, la0)
                x1, y1 = self.net.to_m(lo1, la1)
                return math.hypot(x1 - x0, y1 - y0) / (t1 - t0) * 1.943844
        return None

    def _course_made_good(self, st):
        """Course made good (compass deg) over the last `min_track_hits` position
        fixes — the net leg bearing the route-alignment ranking uses. Returns
        (bearing, dist_m), or (None, dist_m) when the track is too short or the
        net displacement is under `min_move_m` (no reliable direction)."""
        pts = [(lo, la) for (_t, lo, la, _c, _s) in st.hist
               if lo is not None and la is not None]
        if len(pts) < self.min_track_hits:
            return None, 0.0
        lo0, la0 = pts[-self.min_track_hits]
        lo1, la1 = pts[-1]
        x0, y0 = self.net.to_m(lo0, la0)
        x1, y1 = self.net.to_m(lo1, la1)
        dx, dy = x1 - x0, y1 - y0
        dist = math.hypot(dx, dy)
        if dist < self.min_move_m:
            return None, dist
        return math.degrees(math.atan2(dx, dy)) % 360.0, dist

    def _name(self, routeid):
        """Route-name lookup used only on the off-fairway grace path, where we
        have no fresh candidate to read the name from; returns None (unknown)."""
        return None
