/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

/*
 * Crossfire constants provided by Team Black Sheep under terms of the 2-Clause BSD License
 */
#pragma once

#include "AP_RCProtocol.h"
#include <AP_Math/AP_Math.h>
#include "SoftSerial.h"

#define CRSF_MAX_CHANNELS   16U      // Maximum number of channels from crsf datastream
#define CRSF_FRAMELEN_MAX   64U      // maximum possible framelength
#define CRSF_BAUDRATE       416666

class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend {
public:
    AP_RCProtocol_CRSF(AP_RCProtocol &_frontend);
    virtual ~AP_RCProtocol_CRSF();
    void process_byte(uint8_t byte, uint32_t baudrate) override;
    void process_pulse(uint32_t width_s0, uint32_t width_s1) override;
    void update(void) override;
    // get singleton instance
    static AP_RCProtocol_CRSF* get_singleton() {
        return _singleton;
    }

    enum FrameType {
        CRSF_FRAMETYPE_GPS = 0x02,
        CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
        CRSF_FRAMETYPE_HEARTBEAT = 0x0B,
        CRSF_FRAMETYPE_VTX = 0x0F,
        CRSF_FRAMETYPE_VTX_TELEM = 0x10,
        CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
        CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
        CRSF_FRAMETYPE_ATTITUDE = 0x1E,
        CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
        // Extended Header Frames, range: 0x28 to 0x96
        CRSF_FRAMETYPE_PARAM_DEVICE_PING = 0x28,
        CRSF_FRAMETYPE_PARAM_DEVICE_INFO = 0x29,
        CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
        CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
        CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
        CRSF_FRAMETYPE_COMMAND = 0x32,
        // Custom Telemetry Frames 0x7F,0x80
        CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY = 0x7F,   // as suggested by Remo Masina for fw < 4.06
        CRSF_FRAMETYPE_AP_CUSTOM_TELEM = 0x80,          // reserved for ArduPilot by TBS, requires fw >= 4.06
    };

    // Command IDs for CRSF_FRAMETYPE_COMMAND
    enum CommandID {
        CRSF_COMMAND_FC = 0x01,
        CRSF_COMMAND_BLUETOOTH = 0x03,
        CRSF_COMMAND_OSD = 0x05,
        CRSF_COMMAND_VTX = 0x08,
        CRSF_COMMAND_LED = 0x09,
        CRSF_COMMAND_FW_UPDATE = 0x0A,
        CRSF_COMMAND_RX = 0x10,
    };

    // Commands for CRSF_COMMAND_FC
    enum CommandFC {
        CRSF_COMMAND_FC_DISARM = 0x01,
        CRSF_COMMAND_SCALE_CHANNEL = 0x02,
    };

    // Commands for CRSF_COMMAND_BLUETOOTH
    enum CommandBluetooth {
        CRSF_COMMAND_BLUETOOTH_RESET = 0x01,
        CRSF_COMMAND_BLUETOOTH_ENABLE = 0x02,
        CRSF_COMMAND_BLUETOOTH_ECHO = 0x64,
    };

    // Commands for CRSF_COMMAND_OSD
    enum CommandOSD {
        CRSF_COMMAND_OSD_SEND = 0x01,
    };

    // Commands for CRSF_COMMAND_VTX
    enum CommandVTX {
        CRSF_COMMAND_VTX_CHANNEL = 0x01,
        CRSF_COMMAND_VTX_FREQ = 0x02,
        CRSF_COMMAND_VTX_POWER = 0x03,
        CRSF_COMMAND_VTX_PITMODE = 0x04,
        CRSF_COMMAND_VTX_PITMODE_POWERUP = 0x05,
        CRSF_COMMAND_VTX_POWER_DBM = 0x08,
    };

    // Commands for CRSF_COMMAND_LED
    enum CommandLED {
        CRSF_COMMAND_LED_SET_DEFAULT = 0x01,
        CRSF_COMMAND_LED_COLOR = 0x02,
        CRSF_COMMAND_LED_PULSE = 0x03,
        CRSF_COMMAND_LED_BLINK = 0x04,
        CRSF_COMMAND_LED_SHIFT = 0x05,
    };

    // Commands for CRSF_COMMAND_FW_UPDATE
    enum CommandFirmwareUpdate {
        CRSF_COMMAND_FIRMWARE_UPDATE_BOOTLOADER = 0x0A,
        CRSF_COMMAND_FIRMWARE_UPDATE_ERASE = 0x0B,
    };

    // Commands for CRSF_COMMAND_RX
    enum CommandRX {
        CRSF_COMMAND_RX_BIND = 0x01,
    };

    // SubType IDs for CRSF_FRAMETYPE_CUSTOM_TELEM
    enum CustomTelemSubTypeID : uint8_t {
        CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0,
        CRSF_AP_CUSTOM_TELEM_STATUS_TEXT = 0xF1,
        CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH = 0xF2,
    };

    enum DeviceAddress {
        CRSF_ADDRESS_BROADCAST = 0x00,
        CRSF_ADDRESS_USB = 0x10,
        CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80,
        CRSF_ADDRESS_RESERVED1 = 0x8A,
        CRSF_ADDRESS_PNP_PRO_CURRENT_SENSOR = 0xC0,
        CRSF_ADDRESS_PNP_PRO_GPS = 0xC2,
        CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
        CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8,
        CRSF_ADDRESS_RESERVED2 = 0xCA,
        CRSF_ADDRESS_RACE_TAG = 0xCC,
        CRSF_ADDRESS_VTX = 0xCE,
        CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
        CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
        CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
    };

    enum ExtendedFrameOffset {
        CRSF_EXTENDED_FRAME_LENGTH_OFFSET = 1,
        CRSF_EXTENDED_FRAME_TYPE_OFFSET = 2,
        CRSF_EXTENDED_FRAME_DESTINATION_OFFSET = 3,
        CRSF_EXTENDED_FRAME_ORIGIN_OFFSET = 4,
        CRSF_EXTENDED_FRAME_PAYLOAD_OFFSET = 5,
    };

    struct Frame {
        uint8_t device_address;
        uint8_t length;
        uint8_t type;
        uint8_t payload[CRSF_FRAMELEN_MAX - 3]; // +1 for crc
    } PACKED;

    struct LinkStatisticsFrame {
        uint8_t uplink_rssi_ant1; // ( dBm * -1 )
        uint8_t uplink_rssi_ant2; // ( dBm * -1 )
        uint8_t uplink_status; // Package success rate / Link quality ( % )
        int8_t uplink_snr; // ( db )
        uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
        uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
        uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
        uint8_t downlink_rssi; // ( dBm * -1 )
        uint8_t downlink_status; // Downlink package success rate / Link quality ( % )
        int8_t downlink_dnr; // ( db )
    } PACKED;

    enum class RFMode : uint8_t {
        CRSF_RF_MODE_4HZ = 0,
        CRSF_RF_MODE_50HZ,
        CRSF_RF_MODE_150HZ,
        CRSF_RF_MODE_250HZ,
        CRSF_RF_MODE_UNKNOWN,
    };

    struct LinkStatus {
        int16_t rssi = -1;
        int16_t link_quality = -1;
        RFMode rf_mode;
    };

    // this will be used by AP_CRSF_Telem to access link status data
    // from within AP_RCProtocol_CRSF thread so no need for cross-thread synch
    const volatile LinkStatus& get_link_status() const {
        return _link_status;
    }

private:
    struct Frame _frame;
    struct Frame _telemetry_frame;
    uint8_t _frame_ofs;

    const uint8_t MAX_CHANNELS = MIN((uint8_t)CRSF_MAX_CHANNELS, (uint8_t)MAX_RCIN_CHANNELS);

    static AP_RCProtocol_CRSF* _singleton;

    void _process_byte(uint32_t timestamp_us, uint8_t byte);
    bool decode_csrf_packet();
    bool process_telemetry(bool check_constraint = true);
    void process_link_stats_frame(const void* data);
    void write_frame(Frame* frame);
    void start_uart();
    AP_HAL::UARTDriver* get_current_UART() { return (_uart ? _uart : get_available_UART()); }

    uint16_t _channels[CRSF_MAX_CHANNELS];    /* buffer for extracted RC channel data as pulsewidth in microseconds */

    void add_to_buffer(uint8_t index, uint8_t b) { ((uint8_t*)&_frame)[index] = b; }

    uint32_t _last_frame_time_us;
    uint32_t _last_uart_start_time_ms;
    uint32_t _last_rx_time_us;
    uint32_t _start_frame_time_us;
    bool telem_available;

    volatile struct LinkStatus _link_status;

    AP_HAL::UARTDriver *_uart;

    SoftSerial ss{CRSF_BAUDRATE, SoftSerial::SERIAL_CONFIG_8N1};
};

namespace AP {
    AP_RCProtocol_CRSF* crsf();
};