#pragma once

#include "AP_Proximity.h"
#include "AP_Proximity_Backend.h"

#define PROXIMITY_TRTOWER_TIMEOUT_MS            300                               // requests timeout after 0.3 seconds

class AP_Proximity_TeraRangerTowerEvo : public AP_Proximity_Backend {

public:
    // constructor
    AP_Proximity_TeraRangerTowerEvo(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state, AP_SerialManager &serial_manager);

    // static detection function
    static bool detect(AP_SerialManager &serial_manager);

    // update state
    void update(void);

    // get maximum and minimum distances (in meters) of sensor
    float distance_max() const;
    float distance_min() const; 

private:

    // check and process replies from sensor
    void initialise_modes();
    bool read_sensor_data();
    void update_sector_data(int16_t angle_deg, uint16_t distance_cm);
    uint16_t process_distance(uint8_t buf1, uint8_t buf2);
    void set_mode(const uint8_t *c, int length);

    enum InitState {
        InitState_Printout = 0,
        InitState_Sequence,
        InitState_Rate,
        InitState_StreamStart,
        InitState_Finished
    };
    
    // reply related variables
    AP_HAL::UARTDriver *uart = nullptr;
    uint8_t buffer[21]; // buffer where to store data from serial
    uint8_t buffer_count;

    // request related variables
    uint32_t _last_distance_received_ms;    // system time of last distance measurement received from sensor
    uint32_t _last_request_sent_ms;         // system time of last command set
    const uint16_t _mode_request_delay = 1000;
    enum InitState _current_init_state = InitState_Printout;

    // tower evo operating modes

    const uint8_t BINARY_MODE[4] = {(uint8_t)0x00, (uint8_t)0x11, (uint8_t)0x02, (uint8_t)0x4C};
    const uint8_t TOWER_MODE[4] = {(uint8_t)0x00, (uint8_t)0x31, (uint8_t)0x03, (uint8_t)0xE5};
    const uint8_t SEQUENCE_MODE[4] = {(uint8_t)0x00, (uint8_t)0x31, (uint8_t)0x02, (uint8_t)0xE2};
    const uint8_t ACTIVATE_STREAM[5] = {(uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x02, (uint8_t)0x01, (uint8_t)0xDF};
    const uint8_t REFRESH_50_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x02, (uint8_t)0xC3};
    const uint8_t REFRESH_100_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x03, (uint8_t)0xC4};
    const uint8_t REFRESH_250_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x04, (uint8_t)0xD1};
    const uint8_t REFRESH_500_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x05, (uint8_t)0xD6};
    const uint8_t REFRESH_600_HZ[5] = { (uint8_t)0x00, (uint8_t)0x52, (uint8_t)0x03, (uint8_t)0x06, (uint8_t)0xDF};
};