/*
   This program 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 program 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/>.
 */

/*
 * ArduPilot device driver for SLAMTEC RPLIDAR A2 (16m range version)
 *
 * ALL INFORMATION REGARDING PROTOCOL WAS DERIVED FROM RPLIDAR DATASHEET:
 *
 * https://www.slamtec.com/en/Lidar
 * http://bucket.download.slamtec.com/63ac3f0d8c859d3a10e51c6b3285fcce25a47357/LR001_SLAMTEC_rplidar_protocol_v1.0_en.pdf
 *
 * Author: Steven Josefs, IAV GmbH
 * Based on the LightWare SF40C ArduPilot device driver from Randy Mackay
 *
 */


#pragma once

#include "AP_Proximity.h"
#include "AP_Proximity_Backend_Serial.h"
#include <AP_HAL/AP_HAL.h>                   ///< for UARTDriver


class AP_Proximity_RPLidarA2 : public AP_Proximity_Backend_Serial
{

public:

    using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;

    // update state
    void update(void) override;

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

private:
    enum rp_state {
            rp_unknown = 0,
            rp_resetted,
            rp_responding,
            rp_measurements,
            rp_health
        };

    enum ResponseType {
        ResponseType_Descriptor = 0,
        ResponseType_SCAN,
        ResponseType_EXPRESS,
        ResponseType_Health
    };

    // initialise sensor (returns true if sensor is successfully initialised)
    bool initialise();
    void set_scan_mode();

    // send request for something from sensor
    void send_request_for_health();
    void parse_response_data();
    void parse_response_descriptor();
    void get_readings();
    void reset_rplidar();

    // reply related variables
    uint8_t _descriptor[7];
    char _rp_systeminfo[63];
    bool _descriptor_data;
    bool _information_data;
    bool _resetted;
    bool _initialised;

    uint8_t _payload_length;
    uint8_t _cnt;
    uint8_t _sync_error ;
    uint16_t _byte_count;

    // request related variables
    enum ResponseType _response_type;         ///< response from the lidar
    enum rp_state _rp_state;
    uint32_t  _last_request_ms;               ///< system time of last request
    uint32_t  _last_distance_received_ms;     ///< system time of last distance measurement received from sensor
    uint32_t  _last_reset_ms;

    // face related variables
    AP_Proximity_Boundary_3D::Face _last_face;///< last face requested
    float _last_angle_deg;                    ///< yaw angle (in degrees) of _last_distance_m
    float _last_distance_m;                   ///< shortest distance for _last_face
    bool _last_distance_valid;                ///< true if _last_distance_m is valid

    struct PACKED _sensor_scan {
        uint8_t startbit      : 1;            ///< on the first revolution 1 else 0
        uint8_t not_startbit  : 1;            ///< complementary to startbit
        uint8_t quality       : 6;            ///< Related the reflected laser pulse strength
        uint8_t checkbit      : 1;            ///< always set to 1
        uint16_t angle_q6     : 15;           ///< Actual heading = angle_q6/64.0 Degree
        uint16_t distance_q2  : 16;           ///< Actual Distance = distance_q2/4.0 mm
    };

    struct PACKED _sensor_health {
        uint8_t status;                       ///< status definition: 0 good, 1 warning, 2 error
        uint16_t error_code;                  ///< the related error code
    };

    union PACKED {
        DEFINE_BYTE_ARRAY_METHODS
        _sensor_scan sensor_scan;
        _sensor_health sensor_health;
    } payload;
};