Ghost Hand

/*
 * main.c : Ghost Hand state machine
 *
 *   SLEEP ----motion----> SETTLE ---still + level 1.5 s---> SESSION
 *     ^  \---VBUS-------> CHARGING <---------VBUS-------------|
 *     |                      | unplug -> show charge 2 s -> SLEEP
 *     +--- picked up / tipped (lights off) ----------------- SESSION
 *     +--- 10 min cap -> DONE pattern ----------------------- SESSION
 *     +--- battery < 3.45 V -> LOW pattern ------------------ SESSION / SETTLE
 *
 * SESSION runs three sub-phases:
 *   GAUGE + LISTEN  charge gauge fills (3 s) while the receiver settles, the
 *                   threshold is calibrated and the faucet is heard with the
 *                   echo disabled (2 s, up to 5 s for slow faucets)
 *   ARMED           echo enabled, learn.c decides blips, emitters rotate,
 *                   the STATUS dot breathes
 * After DONE or LOW the device must be picked up (0.25 g / 30 deg change)
 * before the next set-down can start a session.
 *
 * Safety: the session cap, the pick-up/tilt stop and the low-battery stop are
 * checked on every 64 Hz tick, independent of learn.c, and the watchdog (1 s)
 * resets the MCU if the loop ever stalls; after reset the emitter gates fall
 * to their 100k pull-downs and the receiver loses power.
 */
#include <avr/io.h>
#include <avr/interrupt.h>
#include "board.h"
#include "power.h"
#include "echo.h"
#include "accel.h"
#include "leds.h"
#include "learn.h"

typedef enum { ST_SLEEP, ST_SETTLE, ST_SESSION, ST_DONE, ST_LOW, ST_CHARGING, ST_UNPLUGGED } state_t;
typedef enum { SUB_RXSETTLE, SUB_LISTEN, SUB_ARMED } sub_t;

static state_t st;
static sub_t sub;
static uint32_t t_state, t_session, t_batt, t_rotate, t_save;
static bool acc_ok, need_pickup, pickup_seen, store_dirty, full_shown;
static uint8_t tick_n, low_count, emit_i;
static vec_mg_t v_now, v_anchor, v_ref, v_done;
static uint32_t t_still;

static learn_store_t store;
static learn_t L;
static learn_listen_t LS;
static uint16_t widths[8];
static uint8_t n_widths;
static uint32_t last_pulse_ticks;
static bool have_pulse;

static const vec_mg_t v_rest = { REST_X_MG, REST_Y_MG, REST_Z_MG };
static const uint8_t emit_seq[3] = { EMIT_BOTH, EMIT_850, EMIT_940 };

#define STORE_ADDR 0
#define STORE_SAVE_MIN_MS 30000u

static void enter(state_t s, uint32_t now) { st = s; t_state = now; }

static uint8_t battery_pct(uint32_t now, bool charging)
{
    uint16_t mv = vbat_mv();
    if (charging && mv > 100) mv -= 100;     /* charger lifts the terminal voltage (77 mA into the 150 mAh cell) */
    return vbat_percent(mv);
}

static void save_store(bool force, uint32_t now)
{
    if (!store_dirty) return;
    if (!force && now - t_save < STORE_SAVE_MIN_MS) return;
    store = *learn_store(&L);
    eeprom_write_bytes(STORE_ADDR, &store, sizeof store);
    store_dirty = false;
    t_save = now;
}

/* ------------------------------------------------------------- sleeping */
static void go_sleep(void)
{
    echo_rx_off();
    leds_off();
    if (acc_ok) acc_mode_sleep();
    twi_off();
    time_suspend();
    wdt_off();
    wake_take_accel();
    wake_take_vbus();

    for (;;) {
        cli();
        bool pending = (PORTA.IN & (1 << PIN_ACC_INT1)) || vbus_present();
        if (pending) { sei(); break; }
        sleep_now(SLEEP_POWERDOWN_MODE);            /* sei + sleep inside */
        if (wake_take_accel() || wake_take_vbus() || vbus_present()) break;
    }

    time_resume();
    wdt_on();
    uint32_t now = time_ms();
    if (vbus_present()) {
        enter(ST_CHARGING, now);
        full_shown = false;
        leds_pattern(PAT_CHARGING, battery_pct(now, true), now);
        if (acc_ok) acc_mode_watch(PICKUP_DELTA_MG);
    } else {
        enter(ST_SETTLE, now);
        if (acc_ok) { acc_mode_watch(PICKUP_DELTA_MG); acc_read(&v_anchor); }
        t_still = now;
    }
}

/* ------------------------------------------------------------- sessions */
static void session_start(uint32_t now)
{
    uint16_t mv = vbat_mv();
    if (mv < LOW_BATT_MV) {
        enter(ST_LOW, now);
        need_pickup = true; pickup_seen = false; v_done = v_now;
        if (mv >= NO_LIGHT_BATT_MV) leds_pattern(PAT_LOW, 0, now); else leds_pattern(PAT_OFF, 0, now);
        return;
    }
    enter(ST_SESSION, now);
    t_session = now; t_batt = now; low_count = 0;
    v_ref = v_now;                                  /* pick-up / tilt reference */
    acc_mode_watch(PICKUP_DELTA_MG);
    wake_take_accel();
    leds_pattern(PAT_GAUGE, vbat_percent(mv), now);
    echo_rx_on();
    sub = SUB_RXSETTLE;
}

static void session_stop(void)
{
    echo_rx_off();
    save_store(true, time_ms());
}

static uint32_t ticks_to_session_ms(uint32_t t) { return ((t * 125u) >> 7) - t_session; }

static void drain_listen(void)
{
    echo_pulse_t p;
    while (echo_pop(&p)) {
        if (have_pulse && p.t - last_pulse_ticks < PULSE_GROUP_MS) continue;   /* burst: same interrogation */
        have_pulse = true;
        last_pulse_ticks = p.t;
        learn_listen_pulse(&LS, ticks_to_session_ms(p.t));
        if (n_widths < 8 && p.width_cyc) widths[n_widths++] = p.width_cyc;
    }
}

static uint8_t echo_width_from_listen(void)
{
    if (!n_widths) return ECHO_DEFAULT_US;
    for (uint8_t i = 1; i < n_widths; i++) {       /* median */
        uint16_t x = widths[i]; int8_t j = (int8_t)i - 1;
        while (j >= 0 && widths[j] > x) { widths[j + 1] = widths[j]; j--; }
        widths[j + 1] = x;
    }
    uint16_t us = (uint16_t)(widths[n_widths / 2] / (F_CPU / 1000000UL)) + ECHO_EXTRA_US;
    if (us < ECHO_MIN_US) us = ECHO_MIN_US;
    if (us > ECHO_MAX_US) us = ECHO_MAX_US;
    return (uint8_t)us;
}

static void drain_armed(void)
{
    echo_pulse_t p;
    while (echo_pop(&p)) {
        if (have_pulse && p.t - last_pulse_ticks < PULSE_GROUP_MS) continue;
        have_pulse = true;
        last_pulse_ticks = p.t;
        learn_pulse(&L, ticks_to_session_ms(p.t));
    }
}

static void session_tick(uint32_t now)
{
    uint32_t sms = now - t_session;

    switch (sub) {
    case SUB_RXSETTLE:
        if (sms >= RX_SETTLE_MS) {
            echo_calibrate();
            learn_listen_init(&LS);
            n_widths = 0; have_pulse = false;
            echo_listen_start();
            sub = SUB_LISTEN;
        }
        break;

    case SUB_LISTEN:
        drain_listen();
        if ((sms >= RX_SETTLE_MS + LISTEN_MS && LS.n_pulses >= 3) || sms >= RX_SETTLE_MS + LISTEN_MAX_MS) {
            echo_arm(echo_width_from_listen());
            learn_begin(&L, &store, learn_listen_period(&LS), sms);
            have_pulse = false;
            emit_i = 0; t_rotate = now;
            echo_set(learn_echo_on(&L), emit_seq[0]);
            sub = SUB_ARMED;
        }
        break;

    case SUB_ARMED: {
        drain_armed();
        learn_tick(&L, sms);
        uint8_t ev = learn_take_events(&L);
        if (ev & LEARN_EV_STORE_DIRTY) store_dirty = true;
        save_store(false, now);

        /* "moving hand": rotate 850 / 940 / both every few hundred ms */
        if (now - t_rotate >= EMITTER_ROTATE_MS) {
            t_rotate = now;
            emit_i = (uint8_t)((emit_i + 1) % 3);
        }
        bool on = learn_echo_on(&L);
        echo_set(on, emit_seq[emit_i]);
        leds_flags(on && L.faucet_active, L.phase == LEARN_PH_BLIP);
        break;
    }
    }

    if (sms >= GAUGE_SHOW_MS && leds_current() == PAT_GAUGE) leds_pattern(PAT_ARMED, 0, now);
}

/* returns true if the session must stop now */
static bool session_safety(uint32_t now)
{
    if (now - t_session >= SESSION_CAP_MS) {
        session_stop();
        enter(ST_DONE, now);
        need_pickup = true; pickup_seen = false; v_done = v_now;
        leds_pattern(PAT_DONE, 0, now);
        return true;
    }
    if (vbus_present()) {
        session_stop();
        enter(ST_CHARGING, now);
        full_shown = false;
        leds_pattern(PAT_CHARGING, battery_pct(now, true), now);
        return true;
    }
    bool picked = wake_take_accel();                 /* INT1: > 0.25 g slope */
    if (!picked && (tick_n & 3) == 0) {
        if (!acc_read(&v_now)) picked = true;        /* lost the accelerometer: stop */
        else if (vec_dist2(&v_now, &v_ref) > (uint32_t)PICKUP_DELTA_MG * PICKUP_DELTA_MG ||
                 vec_tilt_over_30(&v_now, &v_ref)) picked = true;
    }
    if (picked) {
        session_stop();
        leds_off();
        go_sleep();                                  /* returns on the next motion or VBUS */
        return true;
    }
    if (now - t_batt >= BATT_CHECK_MS && echo_quiet()) {
        t_batt = now;
        uint16_t mv = vbat_mv();                     /* under load: RX on, light on */
        low_count = mv < LOW_BATT_MV ? (uint8_t)(low_count + 1) : 0;
        if (low_count >= 2) {
            session_stop();
            enter(ST_LOW, now);
            need_pickup = true; pickup_seen = false; v_done = v_now;
            leds_pattern(mv >= NO_LIGHT_BATT_MV ? PAT_LOW : PAT_OFF, 0, now);
            return true;
        }
    }
    return false;
}

/* ------------------------------------------------------------ settling */
static void settle_tick(uint32_t now)
{
    if (vbus_present()) {
        enter(ST_CHARGING, now);
        full_shown = false;
        leds_pattern(PAT_CHARGING, battery_pct(now, true), now);
        return;
    }
    if (!acc_ok) { go_sleep(); return; }
    if ((tick_n & 3) != 0) return;                   /* 16 Hz */
    if (!acc_read(&v_now)) return;

    if (vec_dist2(&v_now, &v_anchor) > (uint32_t)STILL_TOL_MG * STILL_TOL_MG) {
        v_anchor = v_now;                            /* moved: restart the still timer */
        t_still = now;
    }
    if (need_pickup && !pickup_seen &&
        (vec_dist2(&v_now, &v_done) > (uint32_t)PICKUP_DELTA_MG * PICKUP_DELTA_MG || vec_tilt_over_30(&v_now, &v_done)))
        pickup_seen = true;

    bool level = !vec_tilt_over_30(&v_now, &v_rest);
    bool allowed = !need_pickup || pickup_seen;
    if (now - t_still >= SETTLE_STILL_MS && level && allowed) {
        need_pickup = false;
        session_start(now);
        return;
    }
    if (now - t_still >= SETTLE_GIVEUP_MS || now - t_state >= 120000u) go_sleep();
}

/* ------------------------------------------------------------- charging */
static void charging_tick(uint32_t now)
{
    if (!vbus_present()) {
        enter(ST_UNPLUGGED, now);
        leds_pattern(PAT_SHOW_CHARGE, battery_pct(now, false), now);
        return;
    }
    if (charge_complete()) {
        if (!full_shown) { full_shown = true; leds_pattern(PAT_FULL, 100, now); }
    } else {
        if (full_shown) { full_shown = false; leds_pattern(PAT_CHARGING, battery_pct(now, true), now); }
        else if (now - t_state >= 10000u) {       /* refresh the level */
            t_state = now;
            leds_pattern(PAT_CHARGING, battery_pct(now, true), now);
        }
    }
}

/* ----------------------------------------------------------------- main */
int main(void)
{
    clock_init();
    pins_init();
    time_init();
    leds_init();
    echo_init();
    wake_arm();
    sei();
    wdt_on();

    eeprom_read_bytes(STORE_ADDR, &store, sizeof store);
    if (!learn_store_valid(&store)) learn_store_defaults(&store);

    acc_ok = acc_init();
    uint32_t now = time_ms();
    if (acc_ok) acc_mode_watch(PICKUP_DELTA_MG);

    /* power-up (battery connected or reset): show the charge, then sleep */
    if (vbus_present()) {
        enter(ST_CHARGING, now);
        leds_pattern(PAT_CHARGING, battery_pct(now, true), now);
    } else {
        enter(ST_UNPLUGGED, now);
        leds_pattern(acc_ok ? PAT_SHOW_CHARGE : PAT_LOW, battery_pct(now, false), now);
    }

    for (;;) {
        bool ticked = tick_take();
        if (ticked) {
            wdt_kick();
            now = time_ms();
            tick_n++;

            switch (st) {
            case ST_SLEEP:
                go_sleep();
                break;
            case ST_SETTLE:
                settle_tick(now);
                break;
            case ST_SESSION:
                if (!session_safety(now)) session_tick(now);
                break;
            case ST_DONE:
                if (now - t_state >= 3000u) go_sleep();
                break;
            case ST_LOW:
                if (now - t_state >= 2000u) go_sleep();
                break;
            case ST_CHARGING:
                charging_tick(now);
                break;
            case ST_UNPLUGGED:
                if (vbus_present()) { enter(ST_CHARGING, now); leds_pattern(PAT_CHARGING, battery_pct(now, true), now); }
                else if (now - t_state >= 2000u) go_sleep();
                break;
            }
            if ((tick_n & 1) == 0 && st != ST_SLEEP) leds_frame(time_ms());   /* 32 Hz light updates */
        }
        if (!tick_pending()) sleep_now(echo_blanking() ? SLEEP_IDLE_MODE : SLEEP_STANDBY_MODE);
    }
}