"""SAFETY-128: a reading that does not answer the last command is not current.

When Home Assistant delivers readings late but still changing, every gate that
judges arrival (SAFETY-01) or an unchanged value (SAFETY-18, SAFETY-37) passes
them, and the loop corrects again, each cycle, an error its last command has
already fixed. The readings show it: they answer each power request late.

`Answer.watch()` (each 1 s read) fits two delays every FIT_EVERY_S:
- `Delay`: how long after its request each battery's reading shows it. The
  house does not move that reading, so a repeating load cannot fool it. It
  needs the request to move FIT_MOVE_W; one reading per 10 s makes it slow.
- `MeterFit`: how long after the site request the meter shows it. Quick at a
  1-3 s meter, slower and for long lags only at a 5 s one (SAFETY-129), and a
  house load that repeats can fool it.
The meter counts only when an ask was slow to start and no battery fit said
"on time"; with hysteresis (ON_S / OFF_S) the result is `late_seen`. Then (`observe()`, each cycle):
- a new command more than ANSWER_W from the battery reading is PENDING until
  that reading comes within ANSWER_W of it, or half way;
- while pending the loop may not lead the battery reading by more than
  ANSWER_W that way (`hold`, `hold_w`; not while `exempt`: a battery that
  gives nothing under its ask or refuses is SAFETY-52/94's case) and the peak
  guard may not push past what stands (`peak_cap`). Pending for LATE_S with
  no reading moving towards it is LATE: control holds, then 0 W after
  `stale_input_s`, as for a missing input.

Not `late_seen`, nothing here acts. DECISIONS/SAFETY-128-late-readings.md.
"""

import bisect
from collections import deque

from . import freshness
from .control import PROBE_ANSWER_W

# Within this of the command the reading has answered it (a probe's answer).
ANSWER_W = PROBE_ANSWER_W                                         # 100
# The slowest healthy battery answer: SAFETY-75's 6 s dead time, the 6 s plant
# settle, one 10 s publish and one 1 s read.
LATE_S = (2 * freshness.PLANT_SETTLE_S + freshness.PUBLISH_PERIOD_S
          + freshness.READ_PERIOD_S)                              # 23.0


# Each battery reading's delay is fitted over this much history, up to this.
FIT_S = 60.0
MAX_DELAY_S = 90
FIT_EVERY_S = 2.0
# A fit needs this many new readings (all batteries) and this much movement
# of the reading and the request.
FIT_ROWS = 5
FIT_MOVE_W = 3 * ANSWER_W                                         # 300
# Late from ON_S, on time again under OFF_S (hysteresis).
ON_S = 9.5
OFF_S = 6.5
# ...after this many late fits in a row.
ON_FITS = 2
# With no fit for this long the evidence is dropped (a steady site shows none).
FIT_GONE_S = 120.0
# A fit counts only if the commands explain this share of the reading's swing.
FIT_SHARE = 0.5
# A late delay must explain this share.
FIT_SHARE_LATE = 0.75
# ...and a late delay only if it fits this much better than any fresh one.
FIT_BETTER = 0.5
# A shorter delay within this factor of the best fit is taken instead.
FIT_NEAR = 2.0
# ...or within this of it (W^2): two fits this good are both exact.
FIT_FLOOR_W2 = (ANSWER_W / 2) ** 2                               # 2 500


def per_second(cmds, first, last):
    """The request standing at each whole second from `first` to `last`."""
    ts = [t for t, _c in cmds]
    return [cmds[max(0, bisect.bisect_right(ts, k) - 1)][1]
            for k in range(first, last + 1)]


def pooled(sets, d):
    """Pooled variance over [(rows, c, first, sign)] of value + sign *
    c[t - d], each set about its own mean."""
    tot, n = 0.0, 0
    for rows, c, first, sign in sets:
        e = [v + sign * c[int(t) - d - first] for t, v in rows]
        m = sum(e) / len(e)
        tot += sum((x - m) ** 2 for x in e)
        n += len(e)
    return tot / n


class UnitFit:
    """One battery's power requests and readings, as they were seen."""

    def __init__(self):
        self.cmds = deque()      # (t, request) when it changed
        self.reads = deque()     # (t, reading) when it changed

    def add(self, now, cmd, reading):
        if cmd is not None and (not self.cmds or self.cmds[-1][1] != cmd):
            self.cmds.append((now, float(cmd)))
        if reading is not None and (not self.reads
                                    or self.reads[-1][1] != reading):
            self.reads.append((now, float(reading)))
        while self.reads and self.reads[0][0] < now - FIT_S:
            self.reads.popleft()
        while (len(self.cmds) > 1
               and self.cmds[1][0] <= now - FIT_S - MAX_DELAY_S):
            self.cmds.popleft()

    def series(self):
        """(rows, request each second, first second) for the fit, or None:
        too few new readings, or a reading or a request that has not moved
        FIT_MOVE_W. The request is averaged over the 2 s before each second:
        the battery's own lag and the read."""
        rows = [r for r in self.reads
                if self.cmds and r[0] >= self.cmds[0][0] + MAX_DELAY_S]
        vals = [v for _t, v in rows]
        if len(rows) < 3 or max(vals) - min(vals) < FIT_MOVE_W:
            return None
        cs = [c for _t, c in self.cmds]
        if max(cs) - min(cs) < FIT_MOVE_W:
            return None
        first, last = int(rows[0][0]) - MAX_DELAY_S - 2, int(rows[-1][0])
        c = per_second(self.cmds, first, last)
        avg = [c[0], c[0]] + [(c[k - 2] + c[k - 1] + c[k]) / 3
                              for k in range(2, len(c))]
        return rows, avg, first


class Delay:
    """How long after a power request the batteries' readings show it.

    A reading follows its battery's own request and the house does not move
    it, so `reading(t) - request(t - d)` is flattest at the right `d`. Each new
    reading counts once, at the second it was seen, so the 10 s publish adds no
    delay. One `d` for every battery (Home Assistant delays them all). None
    with too few readings, or when the requests explain too little of them (a
    battery that does not follow); not late unless it fits clearly better than
    any fresh delay, and then the shortest delay that fits about as well."""

    def __init__(self):
        self.units = {}

    def add(self, now, unit, cmd, reading):
        self.units.setdefault(unit, UnitFit()).add(now, cmd, reading)

    def estimate(self):
        ser = [x for x in (u.series() for u in self.units.values()) if x]
        if sum(len(rows) for rows, _c, _f in ser) < FIT_ROWS:
            return None
        sets = [(rows, c, first, -1.0) for rows, c, first in ser]
        r_var = pooled([(rows, [0.0] * len(c), first, -1.0)
                        for rows, c, first in ser], 0)
        pure = [pooled(sets, d) for d in range(0, MAX_DELAY_S + 1)]
        # The shortest that fits about as well: a swing's period fits again.
        best = next(d for d, v in enumerate(pure)
                    if v <= FIT_NEAR * min(pure) + FIT_FLOOR_W2)
        if r_var <= 0 or pure[best] > (1 - FIT_SHARE) * r_var:
            return None
        if best < ON_S:
            return float(best)
        if (pure[best] > (1 - FIT_SHARE_LATE) * r_var
                or pure[best] > FIT_BETTER * min(pure[:int(OFF_S) + 1])):
            return None
        return float(best)


# The meter's delay: fitted over this much history, from this many new
# values (a meter of about 3.5 s or faster; a slower one see M_SLOW_FIT_S);
# late over M_LATE_S (the plant's 6 s settle and one 2 s meter tick), but only
# when the batteries' own readings have not just shown them on time.
M_FIT_S = 60.0
M_ROWS = 16
M_LATE_S = freshness.PLANT_SETTLE_S + 2.0                          # 8.0
M_SHARE, M_BETTER = 0.4, 0.7
# SAFETY-129: a meter slower than that (a 5 s P1) has fewer than M_ROWS values
# in M_FIT_S, so it is fitted over M_SLOW_FIT_S instead, from the same M_ROWS
# values. It calls a delay late only over M_SLOW_LATE_S and on ON_FITS fits in
# a row. Alone it reads 35-90 s on healthy runs, a few times in a row; what
# keeps those from a verdict is the slow-start and battery-fit check in
# watch(), not the limit. A 10 s lag is not found: it cannot be told from a
# slow battery (R2-1), nor does it swing at 5 s.
M_SLOW_FIT_S = 120.0
M_SLOW_LATE_S = 16.0
# A meter fit counts only after an ask that took this long to start moving a
# reading: on time they start within 1-2 s (the recorded house, at 1-5 s).
SLOW_START_S = 3.0


class MeterFit:
    """How long after a site request the meter shows it (round 1's fit).

    `meter(t) + request(t - d)` is the house alone at the right `d`. Quick,
    as the meter reads every 1-5 s, but a house load that repeats can line
    up with an earlier request, so it only counts while the batteries' own
    fit is not on time (`Answer.watch`)."""

    def __init__(self, fit_s=M_FIT_S, late_s=M_LATE_S):
        self.fit_s, self.late_s = fit_s, late_s
        self.cmds = deque()      # (t, request) when it changed
        self.grid = deque()      # (t, meter) new values only

    def slow(self):
        """The meter is too slow for M_ROWS values in `fit_s`: the usual gap
        between its new values (median, so a pause does not count) is over
        fit_s / M_ROWS."""
        ts = [t for t, _g in self.grid]
        if len(ts) < 5:
            return False
        gaps = sorted(b - a for a, b in zip(ts, ts[1:]))
        return gaps[len(gaps) // 2] * M_ROWS > self.fit_s

    def add(self, now, cmd, grid):
        if cmd is not None and (not self.cmds or self.cmds[-1][1] != cmd):
            self.cmds.append((now, float(cmd)))
        if grid is not None and (not self.grid or self.grid[-1][1] != grid):
            self.grid.append((now, float(grid)))
        while self.grid and self.grid[0][0] < now - self.fit_s:
            self.grid.popleft()
        while (len(self.cmds) > 1
               and self.cmds[1][0] <= now - self.fit_s - MAX_DELAY_S):
            self.cmds.popleft()

    def estimate(self):
        if len(self.cmds) < 2:
            return None
        rows = [(t, g) for t, g in self.grid
                if t >= self.cmds[0][0] + MAX_DELAY_S]
        cs = [c for _t, c in self.cmds]
        if len(rows) < M_ROWS or max(cs) - min(cs) < ANSWER_W:
            return None
        first, last = int(rows[0][0]) - MAX_DELAY_S, int(rows[-1][0])
        c = per_second(self.cmds, first, last)
        g_var = pooled([(rows, [0.0] * len(c), first, 1.0)], 0)
        pure = [pooled([(rows, c, first, 1.0)], d)
                for d in range(0, MAX_DELAY_S + 1)]
        best = min(range(len(pure)), key=lambda d: (pure[d], d))
        if g_var <= 0 or pure[best] > (1 - M_SHARE) * g_var:
            return None
        if best <= self.late_s:
            return float(best)
        if pure[best] > M_BETTER * min(pure[:int(self.late_s) + 1]):
            return None
        return float(best)


class Answer:
    """The answer gate. `observe()` once per cycle, before the loop."""

    def __init__(self):
        self.ask = None          # the pending command, W (site, + = give)
        self.from_w = None       # the battery reading when it was asked
        self.from_grid = None    # the meter when it was asked
        self.since = None        # when it was asked
        self.started = False     # a reading has moved towards it
        self.late_seen = False   # the batteries' readings run late
        self.late_at = None      # when the late verdict started
        self.exempt = False      # a battery that cannot follow explains it
        self._was = None         # (command, reading, meter) at the last call
        self.delay = Delay()
        self.meter = MeterFit()
        self.meter_slow = MeterFit(M_SLOW_FIT_S, M_SLOW_LATE_S)   # SAFETY-129
        self._ms_run = 0         # late slow-meter fits in a row
        self._batt_fit = None     # (when, d): the batteries' last fit
        self._b_run = 0          # late battery fits in a row
        self._start_age = None   # (when, how long the last ask took to start)
        self._exempt_at = None   # when a battery last gave nothing under its ask
        self._ask_exempt = False
        self._start = None
        self.delay_s = None      # the last fitted delay (either fit), s
        self.shown_s = None      # the batteries' last late delay, for the page
        self.fits = 0            # fits made (instrumentation)
        self._fit_at = self._fit_ok = None

    @property
    def active(self) -> bool:
        return self.ask is not None and self.late_seen

    @property
    def hold(self) -> int:
        """+1 / -1: the loop may not push further that way. 0: no hold."""
        if not self.active or self.exempt:
            return 0
        return 1 if self.ask > self.from_w else -1

    @property
    def hold_w(self):
        """How far the loop may go while held: ANSWER_W past the reading."""
        if not self.hold:
            return None
        return self._was[1] + self.hold * ANSWER_W

    def peak_cap(self, standing_w):
        """The most the peak guard may ask, or None."""
        return max(float(standing_w), 0.0) if self.active else None

    def late_s(self, now):
        """Seconds the late verdict has stood, or None."""
        return None if self.late_at is None else max(0.0, now - self.late_at)

    def watch(self, now, units, standing_w=None, grid_w=None):
        """Each read (1 s): `units` = [(name, request on its wire, its battery
        reading)], the site request and the meter. Every FIT_EVERY_S, fit both
        delays. Late on ON_FITS battery fits from ON_S, or on a meter fit
        over M_LATE_S when an ask was slow to start (SLOW_START_S) and no
        battery fit within FIT_S said on time: a house load that repeats can
        fool the meter fit, not these. A meter too slow for M_ROWS values in
        M_FIT_S (a 5 s P1) is fitted over M_SLOW_FIT_S and counts only over
        M_SLOW_LATE_S, on ON_FITS fits in a row. On time again on a battery fit
        under OFF_S, or a meter fit within M_LATE_S unless a battery fit said
        late.
        """
        if self._start is None:
            self._start = now
        for name, cmd, reading in units:
            self.delay.add(now, name, cmd, reading)
        self.meter.add(now, standing_w, grid_w)
        self.meter_slow.add(now, standing_w, grid_w)
        if self._fit_at is not None and now - self._fit_at < FIT_EVERY_S:
            return
        self._fit_at = now
        b = self.delay.estimate()
        if (b or 0.0) >= ON_S and self._exempt_at is not None \
                and now - self._exempt_at <= FIT_S:
            b = None          # a battery that stopped following: no late fit
        m = self.meter.estimate()
        ms = None
        if self.meter.slow():
            ms = self.meter_slow.estimate()
        if b is not None:
            self._batt_fit = (now, b)
            self.shown_s = b if b >= ON_S else None
        recent = (self._batt_fit is not None
                  and now - self._batt_fit[0] <= FIT_S)
        b_fresh = recent and self._batt_fit[1] < ON_S
        b_late = recent and self._batt_fit[1] >= ON_S

        self._b_run = self._b_run + 1 if (b or 0.0) >= ON_S else 0
        self._ms_run = self._ms_run + 1 if (ms or 0.0) > M_SLOW_LATE_S else 0
        # A slow-meter fit only ever adds a late verdict: it counts as a fit
        # when it is late on ON_FITS in a row, and never ends one, so a slow
        # battery (R2-1) runs exactly as before.
        if self._ms_run < ON_FITS:
            ms = None
        d = b if b is not None else m if m is not None else ms
        if d is None:
            if self._fit_ok is None or now - self._fit_ok > FIT_GONE_S:
                self.late_seen = False            # nothing to tell it by
            return
        self.fits += 1
        self.delay_s, self._fit_ok = d, now
        warm = now - self._start >= MAX_DELAY_S + FIT_S
        m_late = ((m or 0.0) > M_LATE_S and warm) or (
            self._ms_run >= ON_FITS
            and now - self._start >= MAX_DELAY_S + M_SLOW_FIT_S)
        slow = (self._start_age is not None
                and now - self._start_age[0] <= M_FIT_S
                and self._start_age[1] >= SLOW_START_S)
        if self._b_run >= ON_FITS or (m_late and slow and not b_fresh):
            self.late_seen = True
        elif (b is not None and b < OFF_S) or (
                m is not None and m <= M_LATE_S and not b_late):
            self.late_seen = False

    def observe(self, now, standing_w, batt_w, *, grid_w=None, exempt=False,
                idle_w=0.0):
        """`standing_w` the site command on the wire, `batt_w` the site
        battery reading (None: unknown, nothing changes), `grid_w` the meter
        (+ = import), `idle_w` what the batteries read while giving nothing.
        `exempt`: a battery gives nothing under its ask, or refuses."""
        if batt_w is None or standing_w is None:
            return
        cmd, r = float(standing_w), float(batt_w) - float(idle_w)
        g = None if grid_w is None else float(grid_w)
        self.exempt = bool(exempt)
        if self.exempt:
            self._exempt_at = now
        was, self._was = self._was, (cmd, r, g)
        if self.ask is None:
            if was is None or cmd == was[0] or abs(cmd - was[1]) < ANSWER_W:
                return
            self.ask, self.from_w, self.from_grid = cmd, was[1], was[2]
            self.since, self.started = now, False
            self._ask_exempt = False
        self.ask = cmd
        # A battery that gives nothing under its ask explains a slow start.
        self._ask_exempt = self._ask_exempt or self.exempt
        age = now - self.since
        gap = self.ask - self.from_w
        way = 1.0 if gap > 0 else -1.0
        batt_to = (r - self.from_w) * way
        grid_to = (-1e9 if g is None or self.from_grid is None
                   else (self.from_grid - g) * way)
        if not self.started and max(batt_to, grid_to) >= ANSWER_W:
            self.started, self.late_at = True, None
            self._start_age = (now, 0.0 if self._ask_exempt else age)
        elif (not self.started and age >= SLOW_START_S
              and not self._ask_exempt):
            self._start_age = (now, age)
        if (self.ask - r) * way < ANSWER_W or batt_to >= abs(gap) / 2:
            self._clear()                         # answered
        elif abs(gap) < ANSWER_W:
            self._clear()                         # withdrawn
        elif exempt:
            self.late_at = None
        elif (self.late_at is None and self.late_seen and not self.started
              and age >= LATE_S):
            self.late_at = now

    def _clear(self):
        self.ask = self.from_w = self.from_grid = self.since = None
        self.started, self.late_at = False, None