#include <Arduino.h>
#include "Myo.h"

// =============================================================================
// Transport Selection
//
// This is the only line you change to switch between transports.
//
//   true  -> motion commands go out over CAN
//   false -> motion commands go out over UART (ASPEP / MCP)
//
// Everything below this point is identical either way.
// =============================================================================

#define USE_CAN true

// When true, the sketch pushes new CAN settings to the controller over UART
// during setup(). Leave this false for normal operation; the controller stores
// its node ID and baudrate in flash, so it only needs to be done once.
#define CONFIGURE_CAN 0

// =============================================================================
// Driver
//
// Myo(canId, canBaud, uartBaud, motorId)
// =============================================================================

Myo motor(0x321,   // outgoing CAN ID   -> controller's CAN_NodeID filter
          500000,  // CAN baudrate [bit/s]
          1843200, // ASPEP UART baudrate [bit/s]
          0,       // MCP motor index
          0x322);  // incoming CAN ID   -> controller's CAN_ReplyID

// =============================================================================
// Menu
// =============================================================================

void printMenu()
{
    Serial.println();
    Serial.println("----------------------------------------");
    Serial.println("            Myo-20 Control              ");
    Serial.println("----------------------------------------");
    Serial.println("1              Start motor");
    Serial.println("2              Stop motor");
    Serial.println("s <rpm> <ms>   Speed ramp (output shaft RPM, max 1152)");
    Serial.println("t <amps> <ms>  Torque ramp");
    Serial.println("p <rad> <sec>  Position move");
    Serial.println("6              Request telemetry (CAN)");
    Serial.println("k              Ping / keepalive (UART)");
    Serial.println("x              Invalid command test (CAN)");
    Serial.println("?              Print menu");
    Serial.println("----------------------------------------");
}

// =============================================================================
// Setup
// =============================================================================

void setup()
{
    Serial.begin(115200);

    while (!Serial && millis() < 4000)
        ;

    Serial.println();
    Serial.println("========================================");
    Serial.println("         Myo-20 Driver Test");
    Serial.print("Transport: ");
    Serial.println(USE_CAN ? "CAN" : "UART");
    Serial.println("========================================");

    // -------------------------------------------------------------------------
    // Bring up the link.
    //
    // In CAN mode the UART is also brought up so the CAN configuration
    // commands stay available. Pass false as the second argument if UART
    // is not wired on your setup.
    // -------------------------------------------------------------------------

    if (!motor.begin(USE_CAN))
    {
        Serial.println("WARNING: link did not come up cleanly.");
    }

    // -------------------------------------------------------------------------
    // Decoded CAN responses
    // -------------------------------------------------------------------------

    motor.onTelemetry(
        [](const Myo::MotorTelemetry &telemetry)
        {
            Serial.printf(
                "[RX] TELEMETRY | State: %d | Output: %.1f RPM | Rotor: %d RPM\n",
                telemetry.state,
                telemetry.rpm,
                telemetry.rotorRpm);
        });

    motor.onAck(
        [](const Myo::MotorAck &ack)
        {
            Serial.printf(
                "[RX] ACK | Command: 0x%02X | Status: %s\n",
                static_cast<uint8_t>(ack.command),
                ack.success ? "OK" : "FAILED");
        });

    motor.onError(
        [](const Myo::MotorError &error)
        {
            Serial.printf(
                "[RX] ERROR | Bad Command: 0x%02X\n",
                error.badCommand);
        });

    // -------------------------------------------------------------------------
    // CAN configuration
    //
    // These go out over UART and also retime / retarget the local CAN side.
    // -------------------------------------------------------------------------

#if CONFIGURE_CAN
    if (motor.uartAvailable())
    {
        Serial.println();
        Serial.println("=== CAN CONFIG ===");

        motor.setCanBaud(500000);
        motor.setCanId(2);
    }
#endif

    // -------------------------------------------------------------------------
    // Demo sequence
    //
    // Not one line of this changes with the transport.
    // -------------------------------------------------------------------------

    // Serial.println();
    // Serial.println("=== POSITION ===");

    // motor.setPosition(100.0f, 0.5f);
    // motor.receiveResponse();

    // Serial.println();
    // Serial.println("=== START ===");

    // motor.start();
    // motor.receiveResponse();

    // delay(5000);

    // Serial.println();
    // Serial.println("Returning to 0 rad...");

    // motor.setPosition(0.0f, 0.5f);
    // motor.receiveResponse();

    // delay(5000);

    // Serial.println();
    // Serial.println("=== SPEED ===");

    // Output shaft RPM. 144 is the ceiling with the 8:1 reduction.
    // motor.setSpeed(100.0f, 500);
    // motor.receiveResponse();

    // delay(4000);

    // Serial.println();
    // Serial.println("=== TORQUE ===");

    // motor.setTorque(1.0f, 500);
    // motor.receiveResponse();

    // delay(2000);

    // Serial.println();
    // Serial.println("=== STOP ===");

    // motor.stop();
    // motor.receiveResponse();

    printMenu();
}

// =============================================================================
// Loop
// =============================================================================

void loop()
{
    // Non-blocking: drains CAN frames (or UART bytes) as they arrive.
    motor.update();

    if (!Serial.available())
    {
        return;
    }

    String input = Serial.readStringUntil('\n');
    input.trim();

    if (input.length() == 0)
    {
        return;
    }

    char command = input.charAt(0);

    switch (command)
    {
    case '1':
        motor.start();
        Serial.println("[TX] START");
        break;

    case '2':
        motor.stop();
        Serial.println("[TX] STOP");
        break;

    case 's':
    case 'S':
    {
        float rpm;
        uint16_t rampMs;

        if (sscanf(input.c_str(),
                   "%*c %f %hu",
                   &rpm,
                   &rampMs) != 2)
        {
            Serial.println("Usage: s <rpm> <ramp_ms>");
            break;
        }

        motor.setSpeed(rpm, rampMs);

        Serial.printf(
            "[TX] SPEED %.2f RPM / %u ms\n",
            rpm,
            rampMs);

        break;
    }

    case 't':
    case 'T':
    {
        float amps;
        uint16_t rampMs;

        if (sscanf(input.c_str(),
                   "%*c %f %hu",
                   &amps,
                   &rampMs) != 2)
        {
            Serial.println("Usage: t <amps> <ramp_ms>");
            break;
        }

        motor.setTorque(amps, rampMs);

        Serial.printf(
            "[TX] TORQUE %.2f A / %u ms\n",
            amps,
            rampMs);

        break;
    }

    case 'p':
    case 'P':
    {
        float radians;
        float seconds;

        if (sscanf(input.c_str(),
                   "%*c %f %f",
                   &radians,
                   &seconds) != 2)
        {
            Serial.println("Usage: p <radians> <seconds>");
            break;
        }

        motor.setPosition(radians, seconds);

        Serial.printf(
            "[TX] POSITION %.3f rad / %.2f sec\n",
            radians,
            seconds);

        break;
    }

    case '6':
        motor.requestTelemetry();
        Serial.println("[TX] TELEMETRY REQUEST");
        break;

    case 'k':
    case 'K':
        motor.ping();
        Serial.println("[TX] PING");
        break;

    case 'x':
    case 'X':
        motor.sendInvalidCommand();
        Serial.println("[TX] INVALID COMMAND");
        break;

    case '?':
        printMenu();
        break;

    default:
        Serial.println("Unknown command. Press '?' for help.");
        break;
    }
}