#pragma once

#include <Filter/Filter.h>
#include "AP_Proximity.h"
#include "AP_Proximity_LightWareSerial.h"

class AP_Proximity_LightWareSF45B : public AP_Proximity_LightWareSerial
{

public:
    // constructor
    AP_Proximity_LightWareSF45B(AP_Proximity &_frontend,
                                AP_Proximity::Proximity_State &_state) :
            AP_Proximity_LightWareSerial(_frontend, _state) {}

    uint16_t rxspace() const override {
        return 1280;
    };

    // update state
    void update(void) override;

    // get maximum and minimum distances (in meters) of sensor
    float distance_max() const override { return 50.0f; }
    float distance_min() const override { return 0.20f; }

private:

    // message ids
    enum class MessageID : uint8_t {
        PRODUCT_NAME = 0,
        HARDWARE_VERSION = 1,
        FIRMWARE_VERSION = 2,
        SERIAL_NUMBER = 3,
        TEXT_MESSAGE = 7,
        USER_DATA = 9,
        TOKEN = 10,
        SAVE_PARAMETERS = 12,
        RESET = 14,
        STAGE_FIRMWARE = 16,
        COMMIT_FIRMWARE = 17,
        DISTANCE_OUTPUT = 27,
        STREAM = 30,
        DISTANCE_DATA_CM = 44,
        DISTANCE_DATA_MM = 45,
        LASER_FIRING = 50,
        TEMPERATURE = 57,
        UPDATE_RATE = 66,
        NOISE = 74,
        ZERO_OFFSET = 75,
        LOST_SIGNAL_COUNTER = 76,
        BAUD_RATE = 79,
        I2C_ADDRESS = 80,
        STEPPER_STATUS = 93,
        SCAN_ON_STARTUP = 94,
        SCAN_ENABLE = 96,
        SCAN_POSITION = 97,
        SCAN_LOW_ANGLE = 98,
        SCAN_HIGH_ANGLE = 99
    };

    // initialise sensor
    void initialise();

    // request start of streaming of distances
    void request_stream_start();

    // check and process replies from sensor
    void process_replies();

    // process the latest message held in the msg structure
    void process_message();

    // convert an angle (in degrees) to a mini sector number
    uint8_t convert_angle_to_minisector(float angle_deg) const;

    // internal variables
    uint32_t _last_init_ms;                 // system time of last re-initialisation
    uint32_t _last_distance_received_ms;    // system time of last distance measurement received from sensor
    bool _init_complete;                    // true once sensor initialisation is complete
    ModeFilterInt16_Size5 _distance_filt{2};// mode filter to reduce glitches

    // sector (45 degrees) angles and distances (used to build mini fence for simple avoidance)
    uint8_t _sector = UINT8_MAX;            // sector number (from 0 to 7) of most recently received distance
    float _sector_distance;                 // shortest distance (in meters) in sector
    float _sector_angle;                    // angle (in degrees) of shortest distance in sector
    bool _sector_distance_valid;            // true if sector has at least one valid distance

    // mini sector (5 degrees) angles and distances (used to populate obstacle database for path planning)
    uint8_t _minisector = UINT8_MAX;        // mini sector number (from 0 to 71) of most recently received distance
    float _minisector_distance;             // shortest distance (in meters) in mini sector
    float _minisector_angle;                // angle (in degrees) of shortest distance in mini sector
    bool _minisector_distance_valid;        // true if mini sector has at least one valid distance

    // state of sensor
    struct {
        uint8_t update_rate;        // sensor reported update rate enum from UPDATE_RATE message
        uint32_t streaming_fields;  // sensor reported bitmask of fields sent in DISTANCE_DATA_CM message
        uint32_t stream_data_type;  // sensor reported stream value.  5 if DISTANCE_DATA_CM messages are being streamed
    } _sensor_state;

};