#ifndef MYO_H
#define MYO_H

#include <Arduino.h>
#include <FlexCAN_T4.h>
#include <stdint.h>
#include <stddef.h>

// =============================================================================
// Build-time Configuration
//
// Override any of these with -D flags in platformio.ini if your wiring differs.
// =============================================================================

#ifndef MYO_CAN_BUS
#define MYO_CAN_BUS CAN3
#endif

#ifndef MYO_UART_PORT
#define MYO_UART_PORT Serial1
#endif

#ifndef MYO_DEBUG_PORT
#define MYO_DEBUG_PORT Serial
#endif

#ifndef MYO_RX_BUFFER_SIZE
#define MYO_RX_BUFFER_SIZE 256
#endif

// -----------------------------------------------------------------------------
// Mechanical Configuration
//
// The controller works in rotor units. The public API works in output-shaft
// units, so speed commands are scaled by the reduction on the way out and
// telemetry is scaled back on the way in.
// -----------------------------------------------------------------------------

#ifndef MYO_GEAR_RATIO
#define MYO_GEAR_RATIO 8.0f
#endif

#ifndef MYO_MAX_ROTOR_RPM
#define MYO_MAX_ROTOR_RPM 1152.0f
#endif

// =============================================================================
// Myo
//
// Single high-level driver for the Myo-20 actuator over either UART
// (ASPEP / MCP) or CAN.
//
// The transport is chosen once, at begin(), and every motion command
// (start / stop / setSpeed / setTorque / setPosition) behaves identically
// regardless of which transport is active.
//
// CAN configuration (node ID, bus baudrate) is always negotiated over UART,
// because that is the only channel the controller accepts config commands on.
// setCanId() / setCanBaud() send the UART command AND update the local
// FlexCAN_T4 side to match.
// =============================================================================

class Myo
{
public:
    // -------------------------------------------------------------------------
    // Types
    // -------------------------------------------------------------------------

    enum class Interface : uint8_t
    {
        UART = 0,
        CAN = 1
    };

    enum class Command : uint8_t
    {
        Start = 0x01,
        Stop = 0x02,
        Speed = 0x03,
        Torque = 0x04,
        Position = 0x05,
        Telemetry = 0x06,
        Invalid = 0xFF
    };

    enum class Response : uint8_t
    {
        StartAck = 0xA1,
        StopAck = 0xA2,
        SpeedAck = 0xA3,
        TorqueAck = 0xA4,
        PositionAck = 0xA5,
        Telemetry = 0xA6,
        Error = 0xEE
    };

    struct MotorTelemetry
    {
        uint8_t state;
        float rpm;        // was int16_t — now output shaft
        int16_t rotorRpm; // new — raw value from the controller
    };

    struct MotorAck
    {
        Command command;
        bool success;
    };

    struct MotorError
    {
        uint8_t badCommand;
    };

    // -------------------------------------------------------------------------
    // Callback Types
    //
    // Only fire in CAN mode. The UART transport returns raw ASPEP frames,
    // which are exposed through response() / responseLength().
    // -------------------------------------------------------------------------

    typedef void (*TelemetryCallback)(const MotorTelemetry &telemetry);
    typedef void (*AckCallback)(const MotorAck &ack);
    typedef void (*ErrorCallback)(const MotorError &error);

    // -------------------------------------------------------------------------
    // Construction
    // -------------------------------------------------------------------------

    /**
     * @param canId     CAN ID used for outgoing motor command frames.
     *                  Must match the controller's CAN_NodeID RX filter.
     * @param canBaud   CAN bus baudrate.
     * @param uartBaud  ASPEP UART baudrate.
     * @param motorId   MCP motor index (UART only).
     * @param rxCanId   CAN ID the controller replies on (its CAN_ReplyID).
     *                  Frames with any other ID are discarded.
     */
    Myo(uint32_t canId = 0x321,
        uint32_t canBaud = 500000,
        uint32_t uartBaud = 1843200,
        uint8_t motorId = 0,
        uint32_t rxCanId = 0x322);

    // -------------------------------------------------------------------------
    // Initialization
    // -------------------------------------------------------------------------

    /**
     * @brief Bring up the driver.
     *
     * @param useCan            true  -> motion commands go out over CAN.
     *                          false -> motion commands go out over UART.
     *
     * @param enableUartConfig  When running in CAN mode, also bring up the
     *                          UART link so setCanId() / setCanBaud() / ping()
     *                          remain usable. Set false if UART is not wired.
     *                          Ignored in UART mode (UART is always brought up).
     *
     * @return true if the selected transport came up. In UART mode this
     *         additionally requires a successful ASPEP handshake.
     */
    bool begin(bool useCan, bool enableUartConfig = true);

    /**
     * @brief Run the ASPEP handshake: BEACON -> capability sync -> PING.
     *
     * Called automatically by begin() whenever the UART link is enabled.
     */
    bool connect();

    // -------------------------------------------------------------------------
    // Motion Commands (transport-agnostic)
    // -------------------------------------------------------------------------

    bool start();
    bool stop();

    /**
     * @brief Ramp to a target speed.
     *
     * @param rpm       Target OUTPUT SHAFT speed [RPM]. Scaled by the gear
     *                  ratio before transmission and clamped to
     *                  +/- maxOutputRpm().
     * @param rampMs    Ramp duration [ms].
     */
    bool setSpeed(float rpm, uint16_t rampMs = 0);

    /**
     * @brief Ramp to a target Iq current.
     *
     * @param amps      Target current [A].
     * @param rampMs    Ramp duration [ms].
     */
    bool setTorque(float amps, uint16_t rampMs = 0);

    /**
     * @brief Move to a target position.
     *
     * @param radians       Target position [rad].
     * @param durationSec   Move duration [s].
     */
    bool setPosition(float radians, float durationSec = 0.0f);

    /**
     * @brief Request a telemetry frame. CAN only.
     */
    bool requestTelemetry();

    /**
     * @brief Send a deliberately invalid command to exercise error handling.
     *        CAN only.
     */
    bool sendInvalidCommand();

    /**
     * @brief ASPEP keepalive. Sent over UART whenever the UART link is up.
     */
    bool ping();

    // -------------------------------------------------------------------------
    // CAN Configuration
    //
    // Both of these send the MCP command over UART and then apply the matching
    // change locally, so the host stays in sync with the controller.
    // They require the UART link (always present in UART mode; present in CAN
    // mode unless begin() was called with enableUartConfig = false).
    // -------------------------------------------------------------------------

    bool setCanBaud(uint32_t baudrate);
    bool setCanId(uint16_t canId);

    /**
     * @brief Change only the local outgoing CAN ID, without touching the
     *        controller's stored node ID.
     */
    void setTxCanId(uint16_t canId);

    /**
     * @brief Set the CAN ID that responses are accepted on.
     *
     * Must match the controller's CAN_ReplyID. Frames arriving on any other
     * ID are discarded before decoding.
     */
    void setRxCanId(uint16_t canId);

    // -------------------------------------------------------------------------
    // Reception
    // -------------------------------------------------------------------------

    /**
     * @brief Wait for and consume a response on the active transport.
     *
     * CAN  -> polls update() until a frame arrives or the timeout expires.
     * UART -> waits timeoutMs, then drains the ASPEP frame into the internal
     *         receive buffer (see response() / responseLength()).
     *
     * @return CAN:  number of frames processed.
     *         UART: number of bytes received.
     */
    size_t receiveResponse(uint32_t timeoutMs = 50);

    /**
     * @brief Non-blocking drain of the active transport. Call from loop().
     *
     * @return CAN:  number of frames processed.
     *         UART: number of bytes moved into the receive buffer.
     */
    size_t update();

    // -------------------------------------------------------------------------
    // Decoded Data (CAN)
    // -------------------------------------------------------------------------

    bool getTelemetry(MotorTelemetry &telemetry);
    bool getAck(MotorAck &ack);
    bool getError(MotorError &error);

    void onTelemetry(TelemetryCallback callback);
    void onAck(AckCallback callback);
    void onError(ErrorCallback callback);

    // -------------------------------------------------------------------------
    // Raw Access
    // -------------------------------------------------------------------------

    /** @brief Raw ASPEP bytes from the last UART reception. */
    const uint8_t *response() const { return _rxBuffer; }
    size_t responseLength() const { return _rxLength; }

    /** @brief Raw ASPEP transmit. */
    void sendPacket(const uint8_t *data,
                    size_t length,
                    bool includePayloadPause = false);

    /** @brief Raw ASPEP receive. */
    size_t receivePacket(uint8_t *buffer,
                         size_t maxLen,
                         uint32_t delayMs = 50);

    /** @brief Raw CAN receive. */
    bool readRaw(CAN_message_t &message);

    // -------------------------------------------------------------------------
    // Status
    // -------------------------------------------------------------------------

    Interface interface() const { return _interface; }
    bool usingCan() const { return _interface == Interface::CAN; }
    bool uartAvailable() const { return _uartActive; }
    bool canAvailable() const { return _canActive; }

    uint32_t txCanId() const { return _canID; }
    uint32_t rxCanId() const { return _rxCanID; }
    uint32_t canBaud() const { return _canBaud; }

    /** @brief Gearbox reduction (rotor turns per output turn). */
    float gearRatio() const { return MYO_GEAR_RATIO; }

    /** @brief Maximum commandable output shaft speed [RPM]. */
    float maxOutputRpm() const { return MYO_MAX_ROTOR_RPM / MYO_GEAR_RATIO; }

    /** @brief True if the last setSpeed() call hit the speed limit. */
    bool speedWasClamped() const { return _speedClamped; }

    void setDebug(bool enabled) { _debug = enabled; }

private:
    // -------------------------------------------------------------------------
    // Transport State
    // -------------------------------------------------------------------------

    FlexCAN_T4<MYO_CAN_BUS, RX_SIZE_256, TX_SIZE_16> _can;

    Interface _interface;

    bool _uartActive;
    bool _canActive;
    bool _debug;
    bool _speedClamped;

    uint32_t _canID;
    uint32_t _rxCanID;
    uint32_t _canBaud;
    uint32_t _uartBaud;
    uint8_t _motorId;

    // -------------------------------------------------------------------------
    // Receive State
    // -------------------------------------------------------------------------

    uint8_t _rxBuffer[MYO_RX_BUFFER_SIZE];
    size_t _rxLength;

    MotorTelemetry _telemetry;
    MotorAck _ack;
    MotorError _error;

    bool _telemetryAvailable;
    bool _ackAvailable;
    bool _errorAvailable;

    TelemetryCallback _telemetryCallback;
    AckCallback _ackCallback;
    ErrorCallback _errorCallback;

    // -------------------------------------------------------------------------
    // UART / ASPEP / MCP Internals
    // -------------------------------------------------------------------------

    void sendBeacon();
    size_t capabilitySync(uint8_t *buffer, size_t maxLen);

    uint16_t buildMcpHeader(uint16_t command, uint8_t motorId);

    size_t buildSetDataElementPacket(uint16_t regId,
                                     const uint8_t *regData,
                                     size_t dataLen,
                                     uint8_t *outBuf,
                                     uint8_t motorId);

    void sendMcpCommand(const uint8_t *payload, size_t payloadLen);

    bool uartStart();
    bool uartStop();
    bool uartSetSpeed(float rpm, uint16_t rampMs);
    bool uartSetTorque(float amps, uint16_t rampMs);
    bool uartSetPosition(float radians, float durationSec);

    // -------------------------------------------------------------------------
    // CAN Internals
    // -------------------------------------------------------------------------

    bool sendCanCommand(uint8_t command,
                        const uint8_t *data = nullptr,
                        uint8_t dataLength = 0);

    void processMessage(const CAN_message_t &message);
    void processAck(Command command, const CAN_message_t &message);
    void processTelemetry(const CAN_message_t &message);
    void processError(const CAN_message_t &message);

    // -------------------------------------------------------------------------
    // Utility
    // -------------------------------------------------------------------------

    void printHex(const uint8_t *buffer, size_t length);
};

#endif // MYO_H