import time
from collections import deque
import Adafruit_ADS1x15

adc = Adafruit_ADS1x15.ADS1115()
GAIN = 1
LSB = 4.096 / 32768.0

WIN_SEC = 1.0        # <-- after unplug, should settle within ~1s
PRINT_EVERY = 0.25

keys = ["SE0","SE1","SE2","SE3","D01","D03","D13","D23"]
bufs = {k: deque() for k in keys}  # store (t, value)

def prune(now):
    cutoff = now - WIN_SEC
    for d in bufs.values():
        while d and d[0][0] < cutoff:
            d.popleft()

def p2p_mv(d):
    if len(d) < 8:
        return None
    vals = [v for _, v in d]
    return (max(vals) - min(vals)) * 1000.0

print(f"Time-window scan. WIN_SEC={WIN_SEC}s. Ctrl+C to stop.")
print("Unplug test: after unplugging all belt wires from A0-A3, p2p should drop within ~WIN_SEC.")

t_last_print = time.time()
t_rate0 = t_last_print
n_rate = 0
hz = 0.0

while True:
    now = time.time()

    # reads (slow part)
    v0 = adc.read_adc(0, gain=GAIN) * LSB
    v1 = adc.read_adc(1, gain=GAIN) * LSB
    v2 = adc.read_adc(2, gain=GAIN) * LSB
    v3 = adc.read_adc(3, gain=GAIN) * LSB

    d01 = adc.read_adc_difference(0, gain=GAIN) * LSB  # A0-A1
    d03 = adc.read_adc_difference(1, gain=GAIN) * LSB  # A0-A3
    d13 = adc.read_adc_difference(2, gain=GAIN) * LSB  # A1-A3
    d23 = adc.read_adc_difference(3, gain=GAIN) * LSB  # A2-A3

    # append timestamped samples
    bufs["SE0"].append((now, v0)); bufs["SE1"].append((now, v1))
    bufs["SE2"].append((now, v2)); bufs["SE3"].append((now, v3))
    bufs["D01"].append((now, d01)); bufs["D03"].append((now, d03))
    bufs["D13"].append((now, d13)); bufs["D23"].append((now, d23))

    prune(now)

    # measure loop rate
    n_rate += 1
    if now - t_rate0 >= 1.0:
        hz = n_rate / (now - t_rate0)
        t_rate0 = now
        n_rate = 0

    if now - t_last_print >= PRINT_EVERY:
        parts = []
        for k in keys:
            mv = p2p_mv(bufs[k])
            parts.append(f"{k}:{mv:6.1f}mV" if mv is not None else f"{k}:  ... ")
        print(f"Hz~{hz:4.1f}  " + "  ".join(parts))
        t_last_print = now