#include <Arduino.h>
#include "polisher_protocol.h"
#include "polisher_telemetry.h"

static constexpr uint32_t HEARTBEAT_LED_MS = 500;
uint32_t lastLedToggleMs = 0;
bool ledState = false;

void setup() {
    pinMode(LED_BUILTIN, OUTPUT);
    Serial.begin(115200);
    // TODO: initialize protocol parser, CAN, encoders, drive I/O, watchdogs.
}

void loop() {
    const uint32_t now = millis();
    if (now - lastLedToggleMs >= HEARTBEAT_LED_MS) {
        lastLedToggleMs = now;
        ledState = !ledState;
        digitalWrite(LED_BUILTIN, ledState ? HIGH : LOW);
    }

    // TODO v1 loop order:
    // 1. Read host frames and validate CRC/version.
    // 2. Read KWR75B-CAN and encoder inputs.
    // 3. Update state/safety/watchdogs.
    // 4. Compute setpoint ramp and force PID.
    // 5. Apply actuator/drive outputs.
    // 6. Emit telemetry >=100 Hz and events on transitions/faults.
}
