#pragma once

#include <AP_HAL/AP_HAL.h>
#include "AP_HAL_QURT.h"
#include <AP_ESC_Telem/AP_ESC_Telem.h>

class QURT::RCOutput : public AP_HAL::RCOutput, AP_ESC_Telem_Backend
{
public:
    void init() override;
    void set_freq(uint32_t chmask, uint16_t freq_hz) override;
    uint16_t get_freq(uint8_t ch) override;
    void enable_ch(uint8_t ch) override;
    void disable_ch(uint8_t ch) override;
    void write(uint8_t ch, uint16_t period_us) override;
    uint16_t read(uint8_t ch) override;
    void read(uint16_t *period_us, uint8_t len) override;
    void cork(void) override;
    void push(void) override;

    float get_voltage(void) const
    {
        return esc_voltage;
    }
    float get_current(void) const
    {
        return esc_current;
    }


private:
    const uint32_t baudrate = 2000000;

    void send_receive(void);
    void check_response(void);
    void send_esc_packet(uint8_t type, uint8_t *data, uint16_t size);

    struct PACKED esc_response_v2 {
        uint8_t  id_state;     // bits 0:3 = state, bits 4:7 = ID

        uint16_t rpm;          // Current RPM of the motor
        uint8_t  cmd_counter;  // Number of commands received by the ESC
        uint8_t  power;        // Applied power [0..100]

        uint16_t voltage;      // Voltage measured by the ESC in mV
        int16_t  current;      // Current measured by the ESC in 8mA resolution
        int16_t  temperature;  // Temperature measured by the ESC in 0.01 degC resolution
    };

    struct PACKED esc_power_status {
        uint8_t  id;       //ESC Id (could be used as system ID elsewhere)
        uint16_t voltage;  //Input voltage (Millivolts)
        int16_t  current;  //Total Current (8mA resolution)
    };

    void handle_esc_feedback(const struct esc_response_v2 &pkt);
    void handle_power_status(const struct esc_power_status &pkt);

    // order is RL, FL, FR, RR, map to X frame
    const uint8_t esc_map[4] {2, 0, 1, 3};
    const uint8_t esc_map_rev[4] {1, 2, 0, 3};

    int fd = -1;
    uint16_t enable_mask;
    static const uint8_t channel_count = 4;
    uint16_t period[channel_count];
    volatile bool need_write;
    bool corked;
    HAL_Semaphore mutex;
    uint8_t last_fb_idx;
    uint32_t last_fb_req_ms;

    float esc_voltage;
    float esc_current;
};