#pragma once

#include <stdbool.h>
#include <stdint.h>

#include "driver/gpio.h"
#include "esp_err.h"

typedef struct {
    uint8_t motor_id;
    float position_rad;
    float velocity_rad_s;
    float torque_nm;
    float temperature_c;
    uint8_t mode_state;
    bool fault;
    bool warning;
    uint32_t last_feedback_ms;
} rs00_feedback_t;

esp_err_t rs00_mit_init(gpio_num_t tx_gpio, gpio_num_t rx_gpio);
esp_err_t rs00_mit_send_enable(uint8_t motor_id);
esp_err_t rs00_mit_send_stop(uint8_t motor_id);
esp_err_t rs00_mit_send_clear_fault(uint8_t motor_id);
esp_err_t rs00_mit_send_set_mode(uint8_t motor_id, uint8_t mode);
esp_err_t rs00_mit_send_control(
    uint8_t motor_id,
    float position_rad,
    float velocity_rad_s,
    float kp,
    float kd,
    float torque_ff_nm);
bool rs00_mit_poll_feedback(rs00_feedback_t *feedback, uint32_t timeout_ms);
