Ghost Hand

/*
 * learn.c : faucet timeout / blip estimator
 *
 * The problem
 * -----------
 * An automatic faucet runs while it sees a reflection, but only up to a safety
 * timeout (10 to 60 s typical). After that it treats the reflection as a still
 * object and shuts the water off until the "hand" goes away and comes back.
 * Ghost Hand therefore has to make the hand go away briefly ("blip": stop
 * echoing) before the timeout, so the faucet starts a fresh run without the
 * water ever stopping. The blip must be long enough that the faucet notices
 * the hand left (its release threshold, a few missed interrogations) and short
 * enough that the water does not stop during it (its run-on delay).
 *
 * What we can observe
 * -------------------
 * Only the faucet's own IR interrogation pulses. Many faucets pulse faster
 * while they see a user (active rate) and slower when idle or timed out (idle
 * rate). That rate is the window into the faucet's internal state:
 *   - echo on and the rate drops to idle  -> the faucet timed out (a "drop")
 *   - the time from activation to that drop -> the faucet's real timeout
 *
 * The estimator
 * -------------
 *  1. CLASSIFY: echo on. If an interval below 60 % of the idle period appears
 *     the faucet's rate is observable (TRACK), else FALLBACK.
 *  2. TRACK, first time: echo until the drop. timeout = last fast pulse minus
 *     activation. Recovery blip, faucet restarts.
 *  3. TRACK, learnt: re-arm at activation + timeout - margin (1.5 s): blip,
 *     then VERIFY: the faucet must be at the active rate again within ~1 s
 *     and must not drop until just past the old timeout (a blip that was too
 *     short does not reset the faucet's timer, which shows up as a drop right
 *     after the old timeout).
 *       success -> blip *= 0.85 (never below the floor, min 120 ms)
 *       miss    -> blip *= 1.30, floor = max(1.15 * missed blip,
 *                  last good blip + 1.1 active periods), margin += 0.5 s,
 *                  and the observed timeout updates the estimate
 *     The floor is what stops the water dropping out once learnt: we only
 *     probe below the known good length until the first miss, then never
 *     again. "last good + one active period" guarantees the blip spans one
 *     more interrogation than a blip that worked, whatever the pulse phase.
 *  4. EWMA of the timeout: up slowly (alpha 1/4), down quickly (alpha 3/4),
 *     because overestimating costs a water drop and underestimating only
 *     costs a slightly earlier blip.
 *  5. FALLBACK (no observable rate change): blip every 8 s with
 *     max(blip, 2 idle periods + 50 ms) so at least two interrogations are
 *     missed. 8 s is under the shortest timeout we expect (10 s).
 *  6. Everything that matters is stored (EEPROM) with the faucet's idle period
 *     as a fingerprint; a different faucet (idle period off by > 30 %) starts
 *     from the defaults.
 */
#include "learn.h"
#include <stddef.h>

/* ------------------------------------------------------------------ utils */
static uint16_t u16max(uint16_t a, uint16_t b) { return a > b ? a : b; }
static uint16_t u16min(uint16_t a, uint16_t b) { return a < b ? a : b; }
static uint16_t clamp16(uint32_t v, uint16_t lo, uint16_t hi)
{
    if (v < lo) return lo;
    if (v > hi) return hi;
    return (uint16_t)v;
}

static uint8_t crc8(const uint8_t *p, uint8_t n)
{
    uint8_t c = 0xFF;
    while (n--) {
        c ^= *p++;
        for (uint8_t i = 0; i < 8; i++)
            c = (c & 0x80) ? (uint8_t)((c << 1) ^ 0x31) : (uint8_t)(c << 1);
    }
    return c;
}

/* ------------------------------------------------------------- listening */
void learn_listen_init(learn_listen_t *l)
{
    l->last = 0; l->n_iv = 0; l->n_pulses = 0;
}

void learn_listen_pulse(learn_listen_t *l, uint32_t t)
{
    if (l->n_pulses) {
        uint32_t iv = t - l->last;
        if (l->n_iv < 8 && iv > 0 && iv < 60000u) l->iv[l->n_iv++] = (uint16_t)iv;
    }
    l->last = t;
    if (l->n_pulses < 255) l->n_pulses++;
}

/* median of the measured intervals (robust to one missed or extra pulse) */
uint16_t learn_listen_period(const learn_listen_t *l)
{
    uint16_t v[8];
    uint8_t n = l->n_iv;
    if (!n) return 0;
    for (uint8_t i = 0; i < n; i++) v[i] = l->iv[i];
    for (uint8_t i = 1; i < n; i++) {           /* insertion sort, n <= 8 */
        uint16_t x = v[i]; int8_t j = (int8_t)i - 1;
        while (j >= 0 && v[j] > x) { v[j + 1] = v[j]; j--; }
        v[j + 1] = x;
    }
    return v[n / 2];
}

/* ----------------------------------------------------------------- store */
uint8_t learn_store_crc(const learn_store_t *s)
{
    return crc8((const uint8_t *)s, (uint8_t)offsetof(learn_store_t, crc));
}

bool learn_store_valid(const learn_store_t *s)
{
    return s && s->magic == LEARN_STORE_MAGIC && s->version == LEARN_STORE_VERSION &&
           s->crc == learn_store_crc(s);
}

void learn_store_defaults(learn_store_t *s)
{
    s->magic = LEARN_STORE_MAGIC;
    s->version = LEARN_STORE_VERSION;
    s->observable = 0;
    s->timeout_ms = 0;
    s->blip_ms = LEARN_BLIP_START_MS;
    s->blip_floor_ms = 0;
    s->blip_last_ok_ms = 0;
    s->margin_ms = LEARN_MARGIN_MS;
    s->idle_period_ms = 0;
    s->active_period_ms = 0;
    s->crc = learn_store_crc(s);
}

const learn_store_t *learn_store(learn_t *L)
{
    L->s.idle_period_ms = L->p_idle;
    L->s.active_period_ms = L->p_act;
    L->s.crc = learn_store_crc(&L->s);
    return &L->s;
}

/* ------------------------------------------------------------- internals */
/* midpoint between active and idle period: faster than this = active rate */
static uint32_t thr_mid(const learn_t *L) { return ((uint32_t)L->p_idle + L->p_act) / 2; }

static bool is_fast(const learn_t *L, uint32_t iv)
{
    if (L->mode == LEARN_MODE_TRACK) return iv < thr_mid(L);
    return iv * LEARN_RATE_RATIO_DEN < (uint32_t)L->p_idle * LEARN_RATE_RATIO_NUM;
}

static void dirty(learn_t *L) { L->events |= LEARN_EV_STORE_DIRTY; }

static void start_blip(learn_t *L, uint16_t len, bool recovery)
{
    L->phase = LEARN_PH_BLIP;
    L->blip_cur = clamp16(len, LEARN_BLIP_MIN_MS, recovery ? LEARN_RECOVER_MAX_MS : LEARN_BLIP_MAX_MS);
    L->t_phase_end = L->now + L->blip_cur;
    L->recovering = recovery;
    L->n_blips++;
    L->events |= LEARN_EV_BLIP_START;
}

static void update_timeout(learn_t *L, uint32_t t_obs)
{
    if (t_obs < LEARN_TIMEOUT_MIN_MS) return;      /* not a timeout: lost echo, ignore */
    uint16_t obs = clamp16(t_obs, LEARN_TIMEOUT_MIN_MS, LEARN_TIMEOUT_MAX_MS);
    uint16_t est = L->s.timeout_ms;
    if (!est || obs < est) est = obs;              /* shorter: believe it at once */
    else est = (uint16_t)(est + (obs - est) / 4u); /* longer: EWMA, alpha 1/4 */
    L->s.timeout_ms = est;
    L->timeout_measured = true;
    L->events |= LEARN_EV_TIMEOUT_LEARNT;
    dirty(L);
}

static void blip_ok(learn_t *L)
{
    uint16_t b = L->blip_cur;
    L->n_ok++;
    L->s.blip_last_ok_ms = b;
    uint16_t nb = (uint16_t)(((uint32_t)b * 85u) / 100u);
    nb = u16max(nb, LEARN_BLIP_MIN_MS);
    nb = u16max(nb, L->s.blip_floor_ms);
    L->s.blip_ms = nb;
    L->events |= LEARN_EV_BLIP_OK;
    dirty(L);
}

static void blip_miss(learn_t *L)
{
    uint16_t b = L->blip_cur;
    L->n_miss++;
    uint32_t fl = ((uint32_t)b * 115u) / 100u;
    if (L->s.blip_last_ok_ms) {
        uint32_t f2 = (uint32_t)L->s.blip_last_ok_ms + ((uint32_t)L->p_act * 11u) / 10u;
        if (f2 > fl) fl = f2;
    }
    L->s.blip_floor_ms = u16max(L->s.blip_floor_ms, clamp16(fl, 0, LEARN_BLIP_MAX_MS));
    uint16_t nb = clamp16(((uint32_t)b * 13u) / 10u, LEARN_BLIP_MIN_MS, LEARN_BLIP_MAX_MS);
    L->s.blip_ms = u16max(nb, L->s.blip_floor_ms);
    L->s.margin_ms = u16min((uint16_t)(L->s.margin_ms + LEARN_MARGIN_STEP_MS), LEARN_MARGIN_MAX_MS);
    L->events |= LEARN_EV_BLIP_MISS;
    dirty(L);
}

/* A timed-out faucet only re-arms after it has missed its release count of
 * polls at the IDLE rate, so a recovery blip is sized from the idle period and
 * grows on its own; it never touches the learnt (active-rate) blip. */
static void start_recovery(learn_t *L)
{
    uint32_t r = 2u * (uint32_t)L->p_idle + 100u;
    if (L->recover_ms > r) r = L->recover_ms;
    if (L->s.blip_ms > r) r = L->s.blip_ms;
    start_blip(L, clamp16(r, LEARN_BLIP_MIN_MS, LEARN_RECOVER_MAX_MS), true);
}

/* the faucet fell back to the idle rate (stopped seeing a user) */
static void on_drop(learn_t *L)
{
    if (L->mode != LEARN_MODE_TRACK) return;

    if (L->phase == LEARN_PH_ECHO) {
        uint32_t t_obs = L->last_fast - L->t_act;
        uint16_t prev = L->s.timeout_ms;
        update_timeout(L, t_obs);
        /* it timed out before our scheduled re-arm: re-arm earlier from now on */
        if (prev && t_obs >= LEARN_TIMEOUT_MIN_MS && t_obs < prev) {
            L->s.margin_ms = u16min((uint16_t)(L->s.margin_ms + LEARN_MARGIN_STEP_MS), LEARN_MARGIN_MAX_MS);
            dirty(L);
        }
        L->n_drops++;
        L->events |= LEARN_EV_DROP;
        start_recovery(L);                           /* let it see us leave */
    } else if (L->phase == LEARN_PH_VERIFY && L->seen_active_after) {
        /* back on after the blip, then timed out anyway: the blip did not reset
         * the faucet's timer. That drop also measures the timeout again. */
        if (!L->recovering) {
            update_timeout(L, L->last_fast - L->t_act);
            blip_miss(L);
        }
        L->n_drops++;
        L->events |= LEARN_EV_DROP;
        start_recovery(L);
    }
    /* PH_BLIP: expected, judged at the end of VERIFY */
}

static void set_active(learn_t *L, bool a)
{
    if (L->faucet_active == a) return;
    L->faucet_active = a;
    if (!a) on_drop(L);
    else if (L->phase == LEARN_PH_VERIFY) L->seen_active_after = true;
}

/* ------------------------------------------------------------- interface */
void learn_begin(learn_t *L, const learn_store_t *stored, uint16_t idle, uint32_t now)
{
    bool use = learn_store_valid(stored);
    if (use && idle && stored->idle_period_ms) {
        /* fingerprint: same faucet if the idle period is within 30 % */
        uint32_t a = idle, b = stored->idle_period_ms;
        uint32_t d = a > b ? a - b : b - a;
        if (d * 10u > b * 3u) use = false;
    }
    if (use) L->s = *stored; else learn_store_defaults(&L->s);

    L->p_idle = idle ? idle : (L->s.idle_period_ms ? L->s.idle_period_ms : LEARN_DEFAULT_IDLE_MS);
    L->p_act = L->s.active_period_ms ? L->s.active_period_ms : (uint16_t)(L->p_idle / 3u);
    if (L->p_act >= L->p_idle) L->p_act = (uint16_t)(L->p_idle / 3u);

    L->mode = LEARN_MODE_CLASSIFY;
    L->phase = LEARN_PH_ECHO;
    L->events = 0;
    L->now = now;
    L->last_pulse = now; L->last_iv = 0; L->last_fast = now; L->npulses = 0;
    L->faucet_active = false;
    L->t_arm = now; L->t_act = now; L->t_act_cand = now; L->act_cand_set = false;
    L->timeout_measured = false; L->recovering = false;
    L->t_phase_end = L->t_return_by = L->t_verify_end = now;
    L->t_classify_end = now + u16max(3000u, (uint16_t)u16min(20000u, (uint16_t)(2u * L->p_idle + 1000u)));
    L->blip_cur = L->s.blip_ms;
    L->recover_ms = 0;
    L->seen_active_after = false;
    L->n_drops = L->n_blips = L->n_ok = L->n_miss = 0;
}

void learn_pulse(learn_t *L, uint32_t t)
{
    uint32_t iv = L->npulses ? t - L->last_pulse : 0;
    L->last_pulse = t;
    L->npulses++;
    if (L->now < t) L->now = t;

    if (L->phase != LEARN_PH_BLIP && !L->act_cand_set) {
        L->t_act_cand = t;                          /* first echoed pulse */
        L->act_cand_set = true;
    }
    if (!iv) return;

    bool fast = is_fast(L, iv);

    if (L->mode != LEARN_MODE_TRACK && fast) {
        /* the faucet sped up when we echoed: its state is observable */
        L->mode = LEARN_MODE_TRACK;
        L->s.observable = 1;
        L->p_act = (uint16_t)iv;
        L->t_act = L->act_cand_set ? L->t_act_cand : t - iv;
        L->events |= LEARN_EV_MODE;
        dirty(L);
    }

    if (L->mode == LEARN_MODE_TRACK) {
        if (fast) {
            int32_t d = (int32_t)iv - (int32_t)L->p_act;
            L->p_act = (uint16_t)((int32_t)L->p_act + d / 8);
            if (L->p_act < 5) L->p_act = 5;
            L->last_fast = t;
        } else if (iv < 3u * (uint32_t)L->p_idle && iv > thr_mid(L)) {
            int32_t d = (int32_t)iv - (int32_t)L->p_idle;
            L->p_idle = (uint16_t)((int32_t)L->p_idle + d / 8);
        }
    }
    L->last_iv = iv;
    if (fast && L->phase == LEARN_PH_VERIFY) L->seen_active_after = true;
    set_active(L, fast);
}

void learn_tick(learn_t *L, uint32_t now)
{
    L->now = now;

    /* idle-rate detection between pulses: a gap longer than the midpoint, and
     * either clearly more than 2.5 active periods or the previous interval was
     * already slow (so one lost pulse is not mistaken for a drop). */
    if (L->mode == LEARN_MODE_TRACK && L->faucet_active) {
        uint32_t gap = now - L->last_pulse, th = thr_mid(L);
        if (gap > th && (gap * 2u > 5u * (uint32_t)L->p_act || L->last_iv > th))
            set_active(L, false);
    }

    switch (L->mode) {
    case LEARN_MODE_CLASSIFY:
        if (now >= L->t_classify_end) {
            L->mode = LEARN_MODE_FALLBACK;
            L->s.observable = 2;               /* t_arm stays at the echo start */
            L->events |= LEARN_EV_MODE;
            dirty(L);
        }
        return;

    case LEARN_MODE_FALLBACK:
        if (L->phase == LEARN_PH_BLIP) {
            if (now >= L->t_phase_end) {
                L->phase = LEARN_PH_ECHO;
                L->t_arm = now;
                L->events |= LEARN_EV_BLIP_END;
            }
        } else if (now >= L->t_arm + LEARN_FALLBACK_REARM_MS) {
            uint32_t b = 2u * (uint32_t)L->p_idle + 50u;
            start_blip(L, (uint16_t)(b > L->s.blip_ms ? clamp16(b, 0, LEARN_BLIP_MAX_MS) : L->s.blip_ms), false);
        }
        return;

    case LEARN_MODE_TRACK:
        break;
    }

    switch (L->phase) {
    case LEARN_PH_ECHO:
        if (!L->s.timeout_ms) {
            if (now - L->t_act > LEARN_NO_TIMEOUT_MS) {    /* never timed out: assume 60 s */
                L->s.timeout_ms = LEARN_TIMEOUT_ASSUME_MS;
                dirty(L);
            }
        } else if (L->faucet_active) {
            uint32_t rearm = L->t_act + L->s.timeout_ms;
            rearm = (rearm > L->t_act + L->s.margin_ms + 1000u) ? rearm - L->s.margin_ms : L->t_act + 1000u;
            if (now >= rearm) start_blip(L, L->s.blip_ms, false);
        }
        break;

    case LEARN_PH_BLIP:
        if (now >= L->t_phase_end) {
            uint32_t ret = (uint32_t)L->p_idle + 2u * L->p_act;
            L->phase = LEARN_PH_VERIFY;
            L->events |= LEARN_EV_BLIP_END;
            L->act_cand_set = false;
            L->seen_active_after = false;   /* needs a fast interval after the blip */
            L->t_return_by = now + (ret > LEARN_RETURN_MS ? ret : LEARN_RETURN_MS);
            L->t_verify_end = now + 1500u;
            if (!L->recovering && L->s.timeout_ms) {
                uint32_t old_to = L->t_act + L->s.timeout_ms + 1000u + thr_mid(L); /* + drop detection latency */
                if (old_to > L->t_verify_end) L->t_verify_end = old_to;
            }
            if (L->t_verify_end < L->t_return_by) L->t_verify_end = L->t_return_by;
        }
        break;

    case LEARN_PH_VERIFY:
        if (!L->seen_active_after) {
            if (now > L->t_return_by) {
                /* dropped out and did not come back: blip too short (it never saw
                 * us leave, typical when it had already timed out) */
                if (L->recovering) L->recover_ms = clamp16(((uint32_t)L->blip_cur * 3u) / 2u, 0, LEARN_RECOVER_MAX_MS);
                else blip_miss(L);
                L->n_drops++;
                L->events |= LEARN_EV_DROP;
                start_recovery(L);
            }
        } else if (now >= L->t_verify_end && L->faucet_active) {
            L->t_act = L->act_cand_set ? L->t_act_cand : now;
            if (!L->recovering) blip_ok(L);
            L->recovering = false;
            L->phase = LEARN_PH_ECHO;
        }
        break;
    }
}

bool learn_echo_on(const learn_t *L) { return L->phase != LEARN_PH_BLIP; }

uint8_t learn_take_events(learn_t *L)
{
    uint8_t e = L->events;
    L->events = 0;
    return e;
}