/**
 * WijiBoard – BLE Stepper Controller Firmware
 * src/main.cpp
 *
 * Refactored modular architecture.
 */

#include <Arduino.h>
#include "Config.h"
#include "BleManager.h"
#include "MotorController.h"
#include "StatusLed.h"
#include "CommandParser.h"

BleManager bleManager;
MotorController motorController;
StatusLed statusLed;
CommandParser commandParser(motorController, bleManager);

unsigned long lastNotify = 0;

void onCommandReceived(const String &cmd) {
    commandParser.parse(cmd);
}

void onIdleTimeout() {
    bleManager.notifyMessage("SYS:IDLE_TIMEOUT");
}

void setup() {
    Serial.setTxTimeoutMs(0);
    Serial.begin(115200);
    delay(1000); // Wait for USB CDC to enumerate
    Serial.println("\n[BOOT] WijiBoard Stepper Controller");

    statusLed.init();
    motorController.init();
    motorController.setOnTimeoutCallback(onIdleTimeout);
    bleManager.init(onCommandReceived);
}

void loop() {
    static bool wasMoving = false;
    
    motorController.update();
    bool isMoving = motorController.isMoving();
    statusLed.update(bleManager.isConnected(), isMoving);

    if (bleManager.isConnected()) {
        unsigned long now = millis();
        // Notify periodically while moving, or once when stopping
        if (isMoving && (now - lastNotify >= NOTIFY_INTERVAL_MS)) {
            lastNotify = now;
            bleManager.notifyPosition(motorController.getPositionString());
        } else if (wasMoving && !isMoving) {
            // Just stopped moving
            bleManager.notifyPosition(motorController.getPositionString());
        }
    }
    
    wasMoving = isMoving;
}