"""Runnable check for the control law.  `python3 test_control.py`

No framework, no fixtures - it needs to run on a tech's laptop and in CI with
nothing installed. Every assert here corresponds to a rule that exists because
its absence caused an observed failure on real hardware.

If you change control.py, run this. If it fails, the inverter would have done
something you did not intend.
"""

import sys

from app.control import Tuning, compute, maintenance_charge_floor, peak_at_risk

T = Tuning()
fails = []


def check(name, cond):
    if cond:
        print(f"  ok   {name}")
    else:
        print(f"  FAIL {name}")
        fails.append(name)


print("control law")

# Deadband: inside meter noise, hold exactly - do not drift.
d = compute(prev_w=900, grid_w=10, actual_w=900, tuning=T)
check("deadband holds the command", d.target_w == 900 and d.reason == "deadband")

d = compute(prev_w=900, grid_w=20, actual_w=900, tuning=T)
check("outside deadband it acts", d.target_w != 900)

# Proportional: 0 + 0.6*500 = 300
d = compute(prev_w=0, grid_w=500, actual_w=0, tuning=T)
check("proportional step (gain 0.6)", d.target_w == 300)

# Sign: exporting (negative grid) must CHARGE (negative target).
d = compute(prev_w=0, grid_w=-500, actual_w=0, tuning=T)
check("export drives charging", d.target_w == -300)

# Clamp
d = compute(prev_w=1900, grid_w=1000, actual_w=1900, tuning=Tuning(max_w=2000, slew_w=5000))
check("clamped to max_w", d.target_w == 2000)

# Slew: from 0 with a huge error, no more than slew_w in one cycle.
d = compute(prev_w=0, grid_w=5000, actual_w=0, tuning=Tuning(max_w=5000, slew_w=1000))
check("slew limits one cycle", d.target_w == 1000)

# Saturation needs DURATION: one diverging cycle must NOT freeze.
t = Tuning(saturation_w=500, saturation_cycles=3)
d1 = compute(prev_w=2000, grid_w=500, actual_w=1000, tuning=t, sat_count=0)
check("one saturated cycle does not freeze", not d1.frozen and d1.sat_count == 1)
d2 = compute(prev_w=2000, grid_w=500, actual_w=1000, tuning=t, sat_count=d1.sat_count)
d3 = compute(prev_w=2000, grid_w=500, actual_w=1000, tuning=t, sat_count=d2.sat_count)
check("three consecutive saturated cycles freeze", d3.frozen)
check("freeze forbids raising magnitude", d3.target_w <= 2000)

# ...and one good cycle clears the counter immediately.
d4 = compute(prev_w=2000, grid_w=500, actual_w=1990, tuning=t, sat_count=3)
check("counter resets when tracking resumes", d4.sat_count == 0 and not d4.frozen)

# Freeze must still allow the magnitude to FALL (that is the escape route).
d = compute(prev_w=2000, grid_w=-800, actual_w=1000, tuning=t, sat_count=3)
check("freeze still allows magnitude to fall", d.target_w < 2000)

# Maintenance shaping (charge-only, cheap-window floor) is no longer this
# function's business - it is expressed as limit claims. See test_arbiter.py.

# Quantisation
d = compute(prev_w=0, grid_w=7, actual_w=0, tuning=Tuning(deadband_w=1, step_w=10))
check("quantised to step_w", d.target_w % 10 == 0)

print("SAFETY-04: the integrator is bounded apart from the output")

# The historical runaway, with its real numbers. A commercial controller on
# this site, with the inverter switched OFF, wound ~130 W every 4 s past 10 kW
# and reported 14 768 W while its output clamp sat at 5 kW. At gain 0.6 that
# rate is a standing error of 130/0.6 = 217 W that never resolves, because the
# inverter is not there to resolve it. 150 cycles is past the ~113 it took to
# reach 14 768 W at that rate.
RUNAWAY_ERROR = 130.0 / 0.6
RUNAWAY_CYCLES = 150
HISTORICAL_W = 14768.0


def runaway(tuning):
    """Inverter off: it reports 0 W forever, the error never clears."""
    prev, i_w, sat = 0.0, 0.0, 0
    worst_i, worst_cmd = 0.0, 0.0
    for _ in range(RUNAWAY_CYCLES):
        d = compute(prev_w=prev, grid_w=RUNAWAY_ERROR, actual_w=0.0,
                    tuning=tuning, sat_count=sat, i_w=i_w)
        prev, i_w, sat = d.target_w, d.i_w, d.sat_count
        worst_i = max(worst_i, abs(i_w))
        worst_cmd = max(worst_cmd, abs(prev))
    return worst_i, worst_cmd


TR = Tuning(max_w=2000)          # integrator_max_w unset => follows max_w
wi, wc = runaway(TR)
check(f"runaway: integrator plateaus at {wi:.0f} W (<= 2000)", wi <= TR.max_w)
check(f"runaway: emitted command peaks at {wc:.0f} W (<= 2000)", wc <= TR.max_w)
check("runaway: nowhere near the historical 14 768 W", wc < HISTORICAL_W / 4)

# ...and with the saturation detector deliberately defeated, so that only the
# clamp is holding. Kill one mechanism, the other still bounds it.
TD = Tuning(max_w=2000, saturation_w=1e9)
wi, wc = runaway(TD)
check(f"runaway with the detector defeated: integrator still bounded ({wi:.0f} W)",
      wi <= TD.max_w)
check("runaway with the detector defeated: command still <= max_w", wc <= TD.max_w)

# The bound is a separate quantity, and the useful direction is BELOW max_w:
# there it binds first and caps unwind latency tighter than the rail does.
d = compute(prev_w=0, grid_w=6000, actual_w=0,
            tuning=Tuning(max_w=2000, integrator_max_w=1000, slew_w=5000))
check("integrator bound binds independently of the output clamp",
      d.i_w == 1000 and d.target_w == 1000)

# Freeze = may not wind further in the direction it is already pushing.
TF = Tuning(saturation_w=500, saturation_cycles=3)
f1 = compute(prev_w=2000, grid_w=800, actual_w=0, tuning=TF, sat_count=3, i_w=2000.0)
check("frozen: integration does not wind further", f1.i_w == 2000.0 and f1.frozen)
f2 = compute(prev_w=2000, grid_w=-800, actual_w=0, tuning=TF, sat_count=3, i_w=2000.0)
check("frozen: unwinding is still allowed", f2.i_w < 2000.0)

# ⚠️ REGRESSION, and the reason the first cut of SAFETY-04 was rejected. A
# freeze encoded as "only corrections that shrink |i_w|" is unsatisfiable for
# BOTH signs of error whenever |correction| > 2*|i_w|, so near zero the loop
# stops moving forever - the freeze cannot clear, because clearing it needs the
# inverter to track and not-tracking is what saturation means. Measured on that
# encoding: 0 W held into a 2 kW import for as long as the sim ran.
z = compute(prev_w=0, grid_w=2000, actual_w=600, tuning=T, sat_count=3, i_w=0.0)
check("frozen at i_w=0: a 2 kW import still moves the command",
      z.frozen and z.target_w == 1000)
# ...and the next cycle the inverter is inside saturation_w of the command, so
# the freeze clears on its own. Deadlock would show up here as frozen=True.
z2 = compute(prev_w=1000, grid_w=1000, actual_w=600, tuning=T,
             sat_count=z.sat_count, i_w=z.i_w)
check("frozen at i_w=0: the freeze then clears", not z2.frozen)
# Same stranding on the other side: a small positive integrator against export.
z3 = compute(prev_w=100, grid_w=-1000, actual_w=800, tuning=T, sat_count=3, i_w=100.0)
check("frozen at i_w=+100: a 1 kW export still moves the command",
      z3.frozen and z3.target_w < 0)

# False-positive guard: a normal 2 kW load step must not trip the detector,
# because the plant needs several cycles to catch up on every one of them.
prev, actual, sat, i_w, froze = 0.0, 0.0, 0, 0.0, False
for _ in range(12):
    d = compute(prev, 2000.0 - actual, actual, T, sat, i_w)
    prev, sat, i_w = d.target_w, d.sat_count, d.i_w
    actual = actual + 0.94 * (prev - actual)
    froze = froze or d.frozen
check("a normal 2 kW load step does not trip the saturation freeze", not froze)

# The convergence sim below runs WITHOUT a carried integrator. This is the same
# 2 kW step in the configuration that actually ships, where main.py carries it.
prev, actual, sat, i_w = 0.0, 0.0, 0, 0.0
carried = 0
for _ in range(12):
    d = compute(prev, 2000.0 - actual, actual, T, sat, i_w)
    prev, sat, i_w = d.target_w, d.sat_count, d.i_w
    actual = actual + 0.94 * (prev - actual)
    carried += 1
    if abs(2000.0 - actual) < T.deadband_w:
        break
check(f"carried integrator converges in {carried} cycles (<=6)", carried <= 6)
check("carried integrator does not overshoot the load", actual <= 2000.0 + T.deadband_w)

# ⚠️ REGRESSION: an integrator allowed to wind past the rail buys nothing (the
# output clamp already bounds the wire) and costs extra cycles of
# wrong-direction power after every saturation event. 4000 W load held to
# saturation, then dropped to 0; the figure is the command on the first cycle
# after the drop. This is what makes the DOCS advice checkable.
def unwind(t):
    prev, actual, sat, i_w, load = 0.0, 0.0, 0, 0.0, 4000.0
    for c in range(16):
        if c == 15:
            load = 0.0
        d = compute(prev, load - actual, actual, t, sat, i_w)
        prev, sat, i_w = d.target_w, d.sat_count, d.i_w
        actual = actual + 0.94 * (prev - actual)
    return prev


tight, loose = unwind(Tuning(max_w=2000)), unwind(Tuning(max_w=2000, integrator_max_w=3000))
check(f"after saturation ends the command is {tight:.0f} W (<= 1000)", tight <= 1000)
check(f"headroom above max_w makes that worse ({loose:.0f} W) - hence the default",
      loose > tight)

print("SAFETY-04: the i_w=None path is still release/1.0, exactly")


def legacy(prev, grid, actual, t, sat_count):
    """release/1.0's control law, transcribed. Do not 'improve' this."""
    reason = "tracking"
    sc = min(sat_count + 1, 10) if abs(prev - actual) > t.saturation_w else 0
    frozen = sc >= t.saturation_cycles
    error = grid - t.target_grid_w
    if abs(error) < t.deadband_w:
        want, reason = prev, "deadband"
    else:
        want = prev + t.gain * error
    target = max(-t.max_w, min(t.max_w, want))
    if target != want:
        reason = "clamped"
    slewed = max(prev - t.slew_w, min(prev + t.slew_w, target))
    if slewed != target:
        reason = "slew-limited"
    target = slewed
    if frozen:
        target = min(target, prev) if prev > 0 else max(target, prev)
        reason = "saturated-freeze"
    step = max(1, int(t.step_w))
    return float(round(target / step) * step), sc, frozen, reason


# ⚠️ Compare EVERYTHING observable, not just the number. A previous version of
# this sweep compared (target_w, sat_count) only and passed 3024 cases while
# `reason` had silently lost a value - which is the kind of thing a sweep this
# broad exists to catch. `frozen` and `reason` are both in the tuple now.
#
# The one deliberate rename: what release/1.0 called "clamped" is now
# "i-clamped", because the truncation happens on the integrator before the
# command is derived from it. Aliased here rather than papered over - if any
# OTHER reason ever diverges, this check goes red.
ALIAS = {"i-clamped": "clamped"}
diffs = []
seen = set()
for tune in (Tuning(), Tuning(target_grid_w=-10.0), Tuning(max_w=5000, slew_w=5000)):
    for prev in (-2000.0, -500.0, -100.0, 0.0, 100.0, 500.0, 2000.0):
        for grid in (-6000.0, -1000.0, -500.0, -14.0, 0.0, 14.0, 500.0, 1000.0, 6000.0):
            for actual in (-2000.0, 0.0, 600.0, 2000.0):
                for sc in (0, 2, 3, 9):
                    d = compute(prev, grid, actual, tune, sc)   # i_w defaults to None
                    seen.add(d.reason)
                    got = (d.target_w, d.sat_count, d.frozen,
                           ALIAS.get(d.reason, d.reason))
                    if got != legacy(prev, grid, actual, tune, sc):
                        diffs.append((prev, grid, actual, sc, got,
                                      legacy(prev, grid, actual, tune, sc)))
check(f"i_w=None reproduces release/1.0 over {3*7*9*4*4} cases, reason included"
      + (f" (first diff {diffs[0]})" if diffs else ""), not diffs)

# ...and the rename is not a quiet deletion: the signal SAFETY-03 alarms on has
# to actually occur in that sweep, or its hook is dead.
check("the integrator clamp reports itself as 'i-clamped'", "i-clamped" in seen)

# "clamped" stays reachable, but only where the integrator is deliberately
# allowed above the rail - then BOTH fire and the output clamp, which describes
# the value actually emitted, is the one reported.
dc = compute(prev_w=0, grid_w=6000, actual_w=0,
             tuning=Tuning(max_w=2000, integrator_max_w=3000, slew_w=5000))
check("the output clamp still reports 'clamped' when it is the binding one",
      dc.reason == "clamped" and dc.i_w == 3000 and dc.target_w == 2000)

print("capacity tariff")
check("no forecast means no cap", maintenance_charge_floor(2500, None, 3500) == 2500)
check("headroom caps the charge", maintenance_charge_floor(2500, 2000, 3500) == 1500)
check("no headroom means no charge", maintenance_charge_floor(2500, 4000, 3500) == 0)
check("peak risk detected", peak_at_risk(4000, 3500) is True)
check("peak risk off without forecast", peak_at_risk(None, 3500) is False)

print("behaviour: 2 kW load step converges")
# Closed-loop sim. The plant is modelled as first-order-ish: it moves most of
# the way to the command each cycle (measured: 94 % by 3.3 s against a ~5 s
# cycle). House load steps by 2000 W at t=0.
prev, actual, sat, load = 0.0, 0.0, 0, 2000.0
cycles = 0
for i in range(12):
    grid = load - actual          # what the meter sees
    d = compute(prev, grid, actual, T, sat)
    prev, sat = d.target_w, d.sat_count
    actual = actual + 0.94 * (prev - actual)   # plant follows
    cycles += 1
    if abs(load - actual) < T.deadband_w:
        break
check(f"converges within deadband in {cycles} cycles (<=6)", cycles <= 6)
check("no overshoot past the load", actual <= load + T.deadband_w)

print("grid bias: the deadband must not rest on the import register")
# The billed asymmetry: import and export are separate registers, so a resting
# point inside the deadband on the import side is paid for every second it
# holds. 14 W held all day is 0.34 kWh.
T0 = Tuning(target_grid_w=0.0)
TB = Tuning(target_grid_w=-10.0)
check("unbiased, +14 W import rests forever",
      compute(500.0, 14.0, 500.0, T0).reason == "deadband")
d = compute(500.0, 14.0, 500.0, TB)
check("biased, the same +14 W is corrected", d.reason != "deadband" and d.target_w > 500.0)
check("biased, a small export rests", compute(500.0, -10.0, 500.0, TB).reason == "deadband")
check("biased, the band still ends before -25 W",
      compute(500.0, -30.0, 500.0, TB).reason != "deadband")
# Worst-case billed leak: the band is [bias - deadband, bias + deadband], so it
# drops from 15 W to 5 W. Set target_grid_w to -deadband_w to remove it entirely,
# at the cost of giving that much away as export.
check("worst billed rest point falls from 15 W to under 5 W",
      compute(500.0, 4.9, 500.0, TB).reason == "deadband"
      and compute(500.0, 5.0, 500.0, TB).reason != "deadband")

print()
if fails:
    print(f"{len(fails)} FAILED: {', '.join(fails)}")
    sys.exit(1)
print("all checks passed")