# /// script
# requires-python = ">=3.11"
# dependencies = [
#   "numpy",
#   "pyarrow",
# ]
# ///
"""
Crops three revolutions (Wattbike) and three strokes (Concept2 PM5) out of real
recordings, for "Resampling discontinuous power to 1 Hz".

    uv run static/files/resampling-discontinuous-power/extract_crops.py \\
        --wse-parser ../sweatstack-tools/apps/wattbike-wse-viewer \\
        --wse ../sweatstack-tools/apps/wattbike-wse-viewer/public/sample.WSE \\
        --pm5 ../concept2-web-recorder/test/fixtures/pm5-fw175-intervals-3x1min.jsonl

Writes cycling_crop.csv and rowing_crop.csv next to this script, at 100 Hz:

    time_s   device clock (Wattbike session time / PM5 interval elapsed time)
    power_w  instantaneous power, derived (see below)
    cycle    0, 1, 2 for the three revolutions/strokes, empty outside them

Each crop covers the whole seconds around the three cycles, so per-second
averages at the edges are averages over a full second.

Also writes rowing_strokes.csv: the three strokes plus one before and one
after, as a stroke-boundary file would hold them (start = catch, end = next
catch, work, and power = work / (end - start)).

Neither device measures speed within a revolution or stroke, so instantaneous
power is derived from force with speed held constant:

- Cycling: the parser's power_est, force x pi x crank length / half-revolution
  time. Averages back to the bike's own power per revolution.
- Rowing: force curve points (whole lbf) spaced evenly in time over the drive,
  times average handle speed (drive length / drive time). Zero from the end of
  the drive to the next catch. Each stroke is then scaled so its work matches
  the PM5's work per stroke: shape from the curve, level from the monitor.
"""

import argparse
import csv
import json
import sys
from pathlib import Path

import numpy as np

HERE = Path(__file__).resolve().parent
FS = 100  # Hz
LBF_TO_N = 4.448222

# Chosen as the steadiest three consecutive cycles in each recording.
WSE_FIRST_REVOLUTION = 519  # ~200 W, ~88 rpm, around 370 s into sample.WSE
PM5_INTERVAL = 1  # second interval (0-based) of the 3 x 1 min workout
PM5_FIRST_STROKE = 14  # stroke count as reported by the PM5, ~112 W, ~22 spm
N_CYCLES = 3


# --- Cycling -------------------------------------------------------------------


def cycling_crop(wse_parser: Path, wse_path: Path):
    sys.path.insert(0, str(wse_parser))
    import wse

    act = wse.Activity.load_wse(wse_path)
    force = act.force.to_pydict()
    t = np.asarray(force["time"], dtype=float)
    power = np.asarray(force["power_est"], dtype=float)
    rev = np.array([-1 if r is None else r for r in force["revolution"]])

    revs = range(WSE_FIRST_REVOLUTION, WSE_FIRST_REVOLUTION + N_CYCLES)
    in_crop = np.isin(rev, revs)
    first, last = np.floor(t[in_crop].min()), np.ceil(t[in_crop].max() + 1 / FS)
    keep = (t >= first) & (t < last)

    cycle = np.where(in_crop, rev - WSE_FIRST_REVOLUTION, -1)

    # Check: per-revolution mean of power_est vs the bike's power per revolution.
    data = act.data.to_pydict()
    for r in revs:
        print(f"  revolution {r}: power_est mean {power[rev == r].mean():6.1f} W, "
              f"bike {data['power'][r]:6.1f} W, {data['cadence'][r]:5.1f} rpm")
    return t[keep], power[keep], cycle[keep]


# --- Rowing --------------------------------------------------------------------


def u(b: bytes, offset: int, size: int) -> int:
    return int.from_bytes(b[offset : offset + size], "little")


def pm5_strokes(path: Path) -> list[list[dict]]:
    """Strokes per interval from a PM5 Bluetooth trace (one JSON notification per line).

    0x35 (stroke data) arrives at the end of the drive and again at the end of
    the recovery; the first one per stroke count is the end of the drive.
    0x3D (force curve) fragments arrive right after, and belong to that stroke.
    The stroke count restarts each interval.
    """
    lines = path.read_text().splitlines()[1:]  # first line is a header
    intervals: list[list[dict]] = []
    parts: list[list[int]] = []
    expected = 0
    for line in lines:
        e = json.loads(line)
        char, b = e["char"].lower(), bytes.fromhex(e["hex"])
        if char == "0035":
            n = u(b, 18, 2)
            current = intervals[-1] if intervals else None
            if current is None or n < current[-1]["n"]:
                intervals.append([])
                current = intervals[-1]
            if not current or n != current[-1]["n"]:
                current.append({
                    "n": n,
                    "drive_end": u(b, 0, 3) / 100,  # s
                    "drive_length": b[6] / 100,  # m
                    "drive_time": b[7] / 100,  # s
                    "work": u(b, 16, 2) / 10,  # J
                })
        elif char == "0036" and intervals and intervals[-1]:
            if u(b, 7, 2) == intervals[-1][-1]["n"]:
                intervals[-1][-1]["power"] = u(b, 3, 2)  # W
        elif char == "003d":
            total, count, seq = b[0] >> 4, b[0] & 0x0F, b[1]
            points = [u(b, 2 + 2 * i, 2) for i in range(count)]
            if seq == 0:
                parts, expected = [points], total
            elif parts and len(parts) == seq:
                parts.append(points)
            else:
                parts = []  # out of order: drop rather than glue strokes together
            if parts and len(parts) == expected and intervals and intervals[-1]:
                intervals[-1][-1]["curve"] = [p for part in parts for p in part]
                parts = []
    for strokes in intervals:
        for s in strokes:
            s["catch"] = round(s["drive_end"] - s["drive_time"], 2)
    return intervals


def rowing_crop(pm5_path: Path):
    strokes = pm5_strokes(pm5_path)[PM5_INTERVAL]
    index = next(i for i, s in enumerate(strokes) if s["n"] == PM5_FIRST_STROKE)
    crop = strokes[index : index + N_CYCLES + 1]  # +1 for the last stroke's end

    first = np.floor(crop[0]["catch"])
    last = np.ceil(crop[-1]["catch"])
    t = np.round(np.arange(first, last, 1 / FS), 2)
    power = np.zeros_like(t)
    cycle = np.full(len(t), -1)

    for k, (s, nxt) in enumerate(zip(crop, crop[1:])):
        period = nxt["catch"] - s["catch"]
        cycle[(t >= s["catch"]) & (t < nxt["catch"])] = k

        force = np.asarray(s["curve"], dtype=float) * LBF_TO_N
        curve_t = s["catch"] + np.linspace(0, s["drive_time"], len(force))
        drive = (t >= s["catch"]) & (t < s["catch"] + s["drive_time"])
        speed = s["drive_length"] / s["drive_time"]
        p = np.interp(t[drive], curve_t, force) * speed

        derived_work = p.sum() / FS
        scale = s["work"] / derived_work
        power[drive] = p * scale
        print(f"  stroke {s['n']}: {60 / period:4.1f} spm, drive {s['drive_time']:.2f} s, "
              f"{len(force)} curve points, derived work {derived_work:5.1f} J vs PM5 "
              f"{s['work']:5.1f} J (scale {scale:.2f}), {s['work'] / period:5.1f} W "
              f"vs PM5 stroke power {s.get('power')} W")
    return t, power, cycle


def rowing_strokes(pm5_path: Path):
    """Per-stroke values around the crop: one stroke before, one after.

    What a stroke-boundary file would hold: when each stroke starts and ends
    (catch to next catch) and its average power over that whole cycle.
    """
    strokes = pm5_strokes(pm5_path)[PM5_INTERVAL]
    index = next(i for i, s in enumerate(strokes) if s["n"] == PM5_FIRST_STROKE)
    rows = []
    for s, nxt in zip(strokes[index - 1 : index + N_CYCLES + 1], strokes[index : index + N_CYCLES + 2]):
        period = nxt["catch"] - s["catch"]
        rows.append([s["n"], f"{s['catch']:.2f}", f"{nxt['catch']:.2f}", f"{s['work']:.1f}",
                     f"{s['work'] / period:.1f}"])
    return rows


# --- Output --------------------------------------------------------------------


def write_csv(path: Path, t, power, cycle):
    with path.open("w", newline="") as f:
        w = csv.writer(f)
        w.writerow(["time_s", "power_w", "cycle"])
        for ti, pi, ci in zip(t, power, cycle):
            w.writerow([f"{ti:.2f}", f"{pi:.1f}", "" if ci < 0 else int(ci)])
    print(f"Wrote {path}")


def main():
    parser = argparse.ArgumentParser(description=__doc__.split("\n\n")[0])
    parser.add_argument("--wse-parser", type=Path, required=True, help="directory containing wse.py")
    parser.add_argument("--wse", type=Path, required=True, help="Wattbike .WSE file")
    parser.add_argument("--pm5", type=Path, required=True, help="PM5 Bluetooth trace (.jsonl)")
    args = parser.parse_args()

    print("Cycling")
    write_csv(HERE / "cycling_crop.csv", *cycling_crop(args.wse_parser, args.wse))
    print("Rowing")
    write_csv(HERE / "rowing_crop.csv", *rowing_crop(args.pm5))
    path = HERE / "rowing_strokes.csv"
    with path.open("w", newline="") as f:
        w = csv.writer(f)
        w.writerow(["stroke", "start_s", "end_s", "work_j", "power_w"])
        w.writerows(rowing_strokes(args.pm5))
    print(f"Wrote {path}")


if __name__ == "__main__":
    main()
