#ifndef RX_SERVO_H
#define RX_SERVO_H
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/logging/log.h>
#include <zephyr/drivers/pwm.h>
#include <stdint.h>

#ifdef CONFIG_RX_ENCODER
#include "rx_encoder.h"
#endif

#ifdef CONFIG_RX_MOTOR
#include "rx_motor.h"
#endif

typedef enum
{
    SERVO_MOTOR_CCW,  /**< Motor is rotating counter-clockwise. */
    SERVO_MOTOR_CW,   /**< Motor is rotating clockwise. */
    SERVO_MOTOR_STOP, /**< Motor is stopped. */
} rx_servo_motor_state_t;

// Include any necessary headers here
const enum {
    SERVO_LOST, /**< Servo is lost. */
    SERVO_IDLE, /**< Servo is idle. */
    SERVO_STOP, /**< Servo is stopped. */
    SERVO_JOG,  /**< Servo is moving with no setpoint. */
    SERVO_MOVE, /**< Servo is moving to a setpoint. */
    SERVO_HOME, /**< Servo is homing. */
};

// Declare any classes or structs here
typedef struct
{
    rx_encoder_t encoder;                      /**< Encoder object. */
    rx_motor_t motor;                          /**< Motor object. */
    char name[32];                             /**< Name of the servo object. */
    void (*home)();                            /**< Function pointer to home the servo. */
    void (*move)(int32_t position);            /**< Function pointer to move the servo to a position. */
    void (*work_handler)(struct k_work *work); /**< Function pointer to handle the servo work. */
    rx_servo_motor_state_t motor_state;        /**< Current state of the motor. */
    uint8_t state;                             /**< Current state of the servo. */
    int32_t position;                          /**< Current position of the servo. */
    int32_t speed_raw;                         /**< Raw speed of the servo. */
    int32_t target_position;                   /**< Target position of the servo. */
    int32_t max_position;                      /**< Maximum position of the servo. */
    int32_t min_position;                      /**< Minimum position of the servo. */
    int32_t max_speed;                         /**< Maximum speed of the servo. */
    struct k_work work;                        /**< Work item for the servo. */
    struct k_timer timer;                      /**< Timer for the servo. */
    uint16_t initialized;                 /**< Flag to indicate if the servo is initialized. */

} rx_servo_t;
// Declare any function prototypes here

/**
 * @brief Initializes a servo.
 *
 * @param servo Pointer to the servo structure.
 * @param enc_a Pointer to the GPIO specification for the encoder A pin.
 * @param enc_b Pointer to the GPIO specification for the encoder B pin.
 * @param mtr_ph Pointer to the GPIO specification for the motor phase pin.
 * @param mtr_en Pointer to the PWM specification for the motor enable pin.
 */
void rx_servo_init(rx_servo_t *svo, const struct gpio_dt_spec *enc_a, const struct gpio_dt_spec *enc_b,
                   const struct gpio_dt_spec *mtr_ph, const struct pwm_dt_spec *mtr_en);

#endif // RX_SERVO_H