"""
PROJECT SLOW-SWEEP — Focused second-pass scan
Phase 0: Calibrate mic beep threshold
Phase 1: Speed range in continuous mode (real rev timing)
Phase 2: Backlash measurement (CW/CCW reversal vs no-reversal)
Phase 3: Focused test of interesting unknown commands
"""

import serial, time, subprocess, threading, struct
import numpy as np

LOG = "/home/person/Desktop/Turntable_Hacking/focused_scan.log"

WROOM_PORT = "/dev/ttyACM0"
ATOM_PORT  = "/dev/ttyACM1"
MIC_DEVICE = "hw:1,0"
MIC_RATE   = 44100

INTERESTING = {
    0x01: "CCW ~126deg?",
    0x03: "CW ~18deg?",
    0x04: "CW ~10deg?",
    0x05: "CW ~14deg?",
    0x06: "CW ~32deg?",
    0x08: "CW ~310deg? (360?)",
    0x09: "CW ~7deg?",
    0x0C: "CW ~174deg? (180?)",
    0x0D: "CW ~100deg? (90?)",
    0x12: "CW ~15deg?",
    0x15: "CW ~25deg?",
    0x18: "CW ~85deg? (90?)",
    0x19: "CW ~9deg?",
    0x1A: "CW ~43deg? (45?)",
    0x1C: "CW ~44deg? (45?)",
    0x1D: "CW ~45deg?",
    0x20: "CW ~46deg? (45?)",
}

KNOWN = {
    0x40: "step", 0x41: "ccw", 0x42: "startstop",
    0x43: "slowdown", 0x44: "cw", 0x45: "continuous",
    0x02: "45deg", 0x07: "180deg", 0x0A: "speedup",
    0x11: "swing", 0x1B: "90deg",
}

beep_threshold = 100  # calibrated in phase 0

def log(msg):
    ts = time.strftime("%H:%M:%S")
    line = f"[{ts}] {msg}"
    print(line, flush=True)
    with open(LOG, "a") as f:
        f.write(line + "\n")

# ── Mic beep detection ───────────────────────────────────────────────────────

class BeepDetector:
    """Records audio in background, reports whether a beep was heard."""
    def __init__(self, duration=2.0):
        self.duration = duration
        self.peak_rms = 0.0
        self.chunks_above = 0
        self._proc = None
        self._thread = None

    def start(self):
        self._proc = subprocess.Popen(
            ['arecord', '-D', MIC_DEVICE, '-f', 'S16_LE',
             '-r', str(MIC_RATE), '-c', '2', '-'],
            stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
        self._thread = threading.Thread(target=self._record, daemon=True)
        self._thread.start()

    def _record(self):
        chunk_bytes = int(MIC_RATE * 0.05) * 2 * 2  # 50ms chunks, stereo, 16-bit
        deadline = time.time() + self.duration
        while time.time() < deadline:
            chunk = self._proc.stdout.read(chunk_bytes)
            if not chunk:
                break
            samples = np.frombuffer(chunk, dtype=np.int16).astype(np.float32)
            rms = float(np.sqrt(np.mean(samples ** 2)))
            if rms > self.peak_rms:
                self.peak_rms = rms
            if rms > beep_threshold:
                self.chunks_above += 1
        self._proc.kill()

    def stop_and_check(self):
        if self._thread:
            self._thread.join(timeout=self.duration + 0.5)
        return self.chunks_above > 0, self.peak_rms

def measure_ambient(seconds=2.0):
    """Measure ambient RMS to set beep threshold."""
    proc = subprocess.Popen(
        ['arecord', '-D', MIC_DEVICE, '-f', 'S16_LE',
         '-r', str(MIC_RATE), '-c', '2', f'-d', '3'],
        stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
    data = proc.stdout.read()
    proc.kill()
    if not data:
        return 5.0
    samples = np.frombuffer(data, dtype=np.int16).astype(np.float32)
    return float(np.sqrt(np.mean(samples ** 2)))

# ── Serial helpers ───────────────────────────────────────────────────────────

def send_ir(atom, cmd_byte):
    atom.write(f"0x{cmd_byte:02X}\n".encode())
    time.sleep(0.25)

def send_named(atom, name):
    atom.write(f"{name}\n".encode())
    time.sleep(0.25)

def read_enc(wroom, timeout=2.0):
    deadline = time.time() + timeout
    while time.time() < deadline:
        line = wroom.readline().decode(errors="replace").strip()
        if "[ALIVE]" in line and "enc=" in line:
            try:
                return int(line.split("enc=")[1].split()[0])
            except:
                pass
    return None

def wait_settle(wroom, timeout=45.0, settle=2.0):
    enc_start = None
    last_enc = None
    last_move = time.time()
    move_start = None
    deadline = time.time() + timeout
    while time.time() < deadline:
        line = wroom.readline().decode(errors="replace").strip()
        if "[ALIVE]" not in line or "enc=" not in line:
            continue
        try:
            enc = int(line.split("enc=")[1].split()[0])
        except:
            continue
        if enc_start is None:
            enc_start = enc
        if last_enc is not None and enc != last_enc:
            if move_start is None:
                move_start = time.time()
            last_move = time.time()
        last_enc = enc
        if time.time() - last_move >= settle:
            break
    delta = (last_enc - enc_start) if (last_enc is not None and enc_start is not None) else 0
    dur = round(last_move - move_start, 1) if move_start else 0.0
    return last_enc, delta, dur

def stop(atom, wroom):
    send_named(atom, "startstop")
    time.sleep(0.3)
    send_named(atom, "startstop")
    wait_settle(wroom, timeout=6.0, settle=1.5)
    wroom.reset_input_buffer()

def press_n(atom, cmd_byte, n, gap=0.4):
    for _ in range(n):
        atom.write(f"0x{cmd_byte:02X}\n".encode())
        time.sleep(gap)

# ── Open ports ───────────────────────────────────────────────────────────────

log("=== FOCUSED SCAN START ===")
log(f"WROOM={WROOM_PORT}  ATOM={ATOM_PORT}  MIC={MIC_DEVICE}")

wroom = serial.Serial(WROOM_PORT, 115200, timeout=1)
atom  = serial.Serial(ATOM_PORT,  115200, timeout=1)
time.sleep(2)
wroom.reset_input_buffer()
atom.reset_input_buffer()

# ── PHASE 0: Mic calibration ─────────────────────────────────────────────────
log("")
log("=== PHASE 0: MIC CALIBRATION ===")

ambient = measure_ambient()
beep_threshold = max(ambient * 10, 30)  # 10x ambient, minimum 30
log(f"Ambient RMS={ambient:.1f}  Beep threshold set to {beep_threshold:.1f}")

# Calibrate: send a known command that beeps and measure
log("Sending startstop to calibrate beep level...")
stop(atom, wroom)
bd = BeepDetector(duration=1.5)
bd.start()
send_named(atom, "startstop")
beep_heard, peak = bd.stop_and_check()
log(f"Beep calibration: heard={beep_heard}  peak_rms={peak:.1f}  threshold={beep_threshold:.1f}")
if peak > beep_threshold:
    log("Mic beep detection CONFIRMED working.")
else:
    log("WARNING: beep not detected above threshold — mic may be too far from turntable.")
    beep_threshold = max(peak * 0.5, ambient * 5)
    log(f"Adjusting threshold to {beep_threshold:.1f}")

stop(atom, wroom)

# ── PHASE 1: Speed range in continuous mode ───────────────────────────────────
log("")
log("=== PHASE 1: SPEED RANGE (CONTINUOUS MODE) ===")

def time_full_revolution(atom, wroom, label, timeout=300):
    """Start continuous rotation, time exactly 2400 encoder counts (360deg)."""
    stop(atom, wroom)
    enc_start = read_enc(wroom)
    log(f"  [{label}] Starting continuous CW... enc_start={enc_start}")
    send_named(atom, "continuous")
    time.sleep(0.5)
    send_named(atom, "cw")  # ensure CW direction

    target = 2400  # one full revolution
    t_start = time.time()
    last_enc = enc_start
    deadline = time.time() + timeout

    while time.time() < deadline:
        line = wroom.readline().decode(errors="replace").strip()
        if "[ALIVE]" not in line or "enc=" not in line:
            continue
        try:
            enc = int(line.split("enc=")[1].split()[0])
        except:
            continue
        delta = abs(enc - enc_start)
        if delta >= target:
            elapsed = time.time() - t_start
            stop(atom, wroom)
            log(f"  [{label}] 360deg in {elapsed:.1f}s = {elapsed/60:.2f} min/rev")
            return elapsed
        last_enc = enc

    elapsed = time.time() - t_start
    enc_now = read_enc(wroom)
    delta = abs(enc_now - enc_start) if enc_now else 0
    stop(atom, wroom)
    log(f"  [{label}] TIMEOUT after {elapsed:.0f}s, only moved {delta} counts ({delta*0.15:.1f}deg)")
    return None

log("Setting speed to FLOOR (25x slowdown)...")
press_n(atom, 0x43, 25)
time.sleep(1)
time_full_revolution(atom, wroom, "MIN SPEED", timeout=300)

log("Setting speed to CEILING (25x speedup)...")
press_n(atom, 0x0A, 25)
time.sleep(1)
time_full_revolution(atom, wroom, "MAX SPEED", timeout=120)

stop(atom, wroom)

# ── PHASE 2: Backlash measurement ─────────────────────────────────────────────
log("")
log("=== PHASE 2: BACKLASH MEASUREMENT ===")

# Leave speed at max for backlash test
press_n(atom, 0x0A, 25)
time.sleep(1)

def move_angle(atom, wroom, cmd_byte, label):
    wroom.reset_input_buffer()
    enc_before = read_enc(wroom)
    send_ir(atom, cmd_byte)
    _, delta, dur = wait_settle(wroom, timeout=20.0, settle=1.5)
    enc_after = read_enc(wroom)
    deg = round(delta * 0.15, 2)
    log(f"    {label}: delta={delta} counts ({deg}deg) dur={dur}s")
    return delta

# Use 0x07 (180deg) as our test move — large enough to measure cleanly
log("Control test (CW then CW, no direction change):")
stop(atom, wroom)
enc_ref = read_enc(wroom)
d1 = move_angle(atom, wroom, 0x07, "CW 180deg (1st)")
d2 = move_angle(atom, wroom, 0x07, "CW 180deg (2nd)")
enc_end = read_enc(wroom)
log(f"  Control: total={round((d1+d2)*0.15,2)}deg  sum_expected={round(d1*0.15+d2*0.15,2)}deg")

log("Backlash test (CW then CCW reversal):")
stop(atom, wroom)
enc_start_bl = read_enc(wroom)
d_cw  = move_angle(atom, wroom, 0x07, "CW 180deg")
stop(atom, wroom)
# CCW 180deg — use 0x41 (ccw continuous) timed, or find CCW angle command
# Use the known 180deg command but in CCW direction (0x41 continuous + time)
enc_before_ccw = read_enc(wroom)
send_named(atom, "continuous")
time.sleep(0.3)
send_named(atom, "ccw")
time.sleep(0.3)
_, d_ccw, dur_ccw = wait_settle(wroom, timeout=30.0, settle=1.5)
stop(atom, wroom)
enc_final_bl = read_enc(wroom)
net = (enc_final_bl - enc_start_bl) if (enc_final_bl and enc_start_bl) else None
backlash_counts = abs(net) if net is not None else None
backlash_deg = round(backlash_counts * 0.15, 2) if backlash_counts is not None else None
log(f"  Backlash result: net displacement={backlash_counts} counts = {backlash_deg}deg")
log(f"  (0 would mean perfect return; positive = mechanical slop)")

stop(atom, wroom)

# ── PHASE 3: Focused command investigation ────────────────────────────────────
log("")
log("=== PHASE 3: FOCUSED COMMAND INVESTIGATION ===")
log("For each command: send, detect beep, wait settle, measure angle vs hypothesis")
log(f"Noise threshold: >30 counts to count as real movement")

# Max speed for all focused tests
press_n(atom, 0x0A, 25)
time.sleep(1)

results = {}

for cmd_byte, hypothesis in INTERESTING.items():
    stop(atom, wroom)
    wroom.reset_input_buffer()

    # Start beep detector before sending
    bd = BeepDetector(duration=2.0)
    bd.start()

    t_send = time.time()
    send_ir(atom, cmd_byte)
    enc_final, delta, dur = wait_settle(wroom, timeout=40.0, settle=2.0)
    beep_heard, peak_rms = bd.stop_and_check()

    deg = round(delta * 0.15, 2)
    real_move = abs(delta) > 30
    total_time = round(time.time() - t_send, 1)

    direction = "CW" if delta < 0 else "CCW" if delta > 0 else "NONE"
    verdict = "REAL MOVE" if real_move else "noise/no response"

    log(f"  0x{cmd_byte:02X} [{hypothesis}]")
    log(f"    beep={beep_heard} (peak={peak_rms:.0f})  delta={delta} ({deg}deg) {direction}  dur={dur}s  total={total_time}s")
    log(f"    verdict: {verdict}")

    results[cmd_byte] = {
        "hypothesis": hypothesis, "delta": delta, "deg": deg,
        "direction": direction, "dur": dur, "beep": beep_heard,
        "real": real_move,
    }

stop(atom, wroom)

# ── Summary ───────────────────────────────────────────────────────────────────
log("")
log("=== FINAL SUMMARY ===")
log("Commands confirmed as real angle presets:")
for cmd_byte, r in sorted(results.items()):
    if r["real"]:
        log(f"  0x{cmd_byte:02X}: {r['deg']}deg {r['direction']}  beep={r['beep']}  [{r['hypothesis']}]")

log("")
log("Commands with no real movement (noise or modal):")
for cmd_byte, r in sorted(results.items()):
    if not r["real"]:
        log(f"  0x{cmd_byte:02X}: beep={r['beep']}  [{r['hypothesis']}]")

log("=== DONE ===")