/*
   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/>.
 */
#include "AP_RangeFinder_LightWareI2C.h"
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>

extern const AP_HAL::HAL& hal;

#define LIGHTWARE_DISTANCE_READ_REG 0
#define LIGHTWARE_LOST_SIGNAL_TIMEOUT_READ_REG 22
#define LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG 23
#define LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE 5

#define LIGHTWARE_OUT_OF_RANGE_ADD_CM   100

static const size_t lx20_max_reply_len_bytes = 32;
static const size_t lx20_max_expected_stream_reply_len_bytes = 14;

#define stream_the_median_distance_to_the_first_return "ldf,0"
#define stream_the_raw_distance_to_the_first_return    "ldf,1"
#define stream_the_signal_strength_first_return        "lhf"
#define stream_the_raw_distance_to_the_last_return     "ldl,1"
#define stream_the_signal_strength_last_return         "lhl"
#define stream_the_level_of_background_noise           "ln"

/* Data streams from the LiDAR can include any sf20 LiDAR measurement.
 * A request to stream the desired measurement is made on a 20Hz basis and
 * on the next 20Hz service 50ms later, the result is read and a streaming
 * request is made for the next desired measurement in the sequence.
 * Results are generally available from the LiDAR within 10mS of request.
 */
#define STREAM1_VAL stream_the_raw_distance_to_the_last_return
#define STREAM2_VAL stream_the_signal_strength_last_return
#define STREAM3_VAL stream_the_raw_distance_to_the_first_return
#define STREAM4_VAL stream_the_signal_strength_first_return
#define STREAM5_VAL stream_the_level_of_background_noise
static const char *parse_stream_id[NUM_SF20_DATA_STREAMS] = { // List of stream identifier strings. Must match init_stream_id[].
    STREAM1_VAL ":"
};

static const char *init_stream_id[NUM_SF20_DATA_STREAMS] = {// List of stream initialization strings. Must match parse_stream_id[].
    "$1" STREAM1_VAL "\r\n"
};

static const uint8_t streamSequence[] = { 0 }; // List of 0 based stream Ids that determine the LiDAR values collected.

static const uint8_t numStreamSequenceIndexes = sizeof(streamSequence)/sizeof(streamSequence[0]);

AP_RangeFinder_LightWareI2C::AP_RangeFinder_LightWareI2C(RangeFinder::RangeFinder_State &_state,
        AP_RangeFinder_Params &_params,
        AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
    : AP_RangeFinder_Backend(_state, _params)
    , _dev(std::move(dev)) {}

/*
   Detects if a Lightware rangefinder is connected. We'll detect by
   trying to take a reading on I2C. If we get a result the sensor is
   there.
*/
AP_RangeFinder_Backend *AP_RangeFinder_LightWareI2C::detect(RangeFinder::RangeFinder_State &_state,
        AP_RangeFinder_Params &_params,
        AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev)
{
    if (!dev) {
        return nullptr;
    }

    AP_RangeFinder_LightWareI2C *sensor
        = new AP_RangeFinder_LightWareI2C(_state, _params, std::move(dev));

    if (!sensor) {
        return nullptr;
    }

    WITH_SEMAPHORE(sensor->_dev->get_semaphore());
    if (!sensor->init()) {
        delete sensor;
        return nullptr;
    }
    return sensor;
}

/**
 * Wrapper function over #transfer() to write a sequence of bytes to
 * device. No values are read.
 */
bool AP_RangeFinder_LightWareI2C::write_bytes(uint8_t *write_buf_u8, uint32_t len_u8)
{
    return _dev->transfer(write_buf_u8, len_u8, NULL, 0);
}

/**
 * Disables "address tagging" in the sf20 response packets.
 */
void AP_RangeFinder_LightWareI2C::sf20_disable_address_tagging()
{
    sf20_send_and_expect("#CT,0\r\n", "ct:0");
}

/*
  send a native command and check for an expected reply
 */
bool AP_RangeFinder_LightWareI2C::sf20_send_and_expect(const char* send_msg, const char* expected_reply)
{
    const size_t expected_reply_len = strlen(expected_reply);
    uint8_t rx_bytes[expected_reply_len + 1];
    memset(rx_bytes, 0, sizeof(rx_bytes));

    if ((expected_reply_len > lx20_max_reply_len_bytes) ||
        (expected_reply_len < 2)) {
        return false;
    }

    if (!write_bytes((uint8_t*)send_msg,
                     strlen(send_msg))) {
        return false;
    }

    if (!sf20_wait_on_reply(rx_bytes)) {
        return false;
    }

    if ((rx_bytes[0] != expected_reply[0]) ||
        (rx_bytes[1] != expected_reply[1]) ) {
        return false;
    }

    for (uint8_t i=0; i<10; i++) {
        if (_dev->read(rx_bytes, expected_reply_len)) {
            break;
        }
        // give a bit of time for the remaining bytes to be available
        hal.scheduler->delay(1);
    }

    return memcmp(rx_bytes, expected_reply, expected_reply_len) == 0;
}

/*
  send a native command and fill a reply into a buffer. Used for
  version string
 */
void AP_RangeFinder_LightWareI2C::sf20_get_version(const char* send_msg, const char *reply_prefix, char reply[15])
{
    const size_t expected_reply_len = 16;
    uint8_t rx_bytes[expected_reply_len + 1];
    memset(rx_bytes, 0, sizeof(rx_bytes));

    if (!write_bytes((uint8_t*)send_msg, strlen(send_msg))) {
        return;
    }

    if (!sf20_wait_on_reply(rx_bytes)) {
        return;
    }

    if ((rx_bytes[0] != uint8_t(reply_prefix[0])) ||
        (rx_bytes[1] != uint8_t(reply_prefix[1])) ) {
        return;
    }

    for (uint8_t i=0; i<10; i++) {
        if (_dev->read(rx_bytes, expected_reply_len)) {
            break;
        }
        // give a bit of time for the remaining bytes to be available
        hal.scheduler->delay(1);
    }
    memcpy(reply, &rx_bytes[2], 14);
}

/* Driver first attempts to initialize the sf20.
 * If for any reason this fails, the driver attempts to initialize the legacy LightWare LiDAR.
 * If this fails, the driver returns false indicating no LightWare LiDAR is present.
 */
bool AP_RangeFinder_LightWareI2C::init()
{
    if (sf20_init()) {
        hal.console->printf("Found SF20 native Lidar\n");
        return true;
    }
    if (legacy_init()) {
        hal.console->printf("Found SF20 legacy Lidar\n");
        return true;
    }
    hal.console->printf("SF20 not found\n");
    return false;
}

/*
  initialise lidar using legacy 16 bit protocol
 */
bool AP_RangeFinder_LightWareI2C::legacy_init()
{
    union {
        be16_t be16_val;
        uint8_t bytes[2];
    } timeout;

    // Retrieve lost signal timeout register
    const uint8_t read_reg = LIGHTWARE_LOST_SIGNAL_TIMEOUT_READ_REG;
    if (!_dev->transfer(&read_reg, 1, timeout.bytes, 2)) {
        return false;
    }

    // Check lost signal timeout register against desired value and write it if it does not match
    if (be16toh(timeout.be16_val) != LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE) {
        timeout.be16_val = htobe16(LIGHTWARE_TIMEOUT_REG_DESIRED_VALUE);
        const uint8_t send_buf[3] = {LIGHTWARE_LOST_SIGNAL_TIMEOUT_WRITE_REG, timeout.bytes[0], timeout.bytes[1]};
        if (!_dev->transfer(send_buf, sizeof(send_buf), nullptr, 0)) {
            return false;
        }
    }

    // call timer() at 20Hz
    _dev->register_periodic_callback(50000,
                                     FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::legacy_timer, void));

    return true;
}

/*
  initialise using newer text based protocol
 */
bool AP_RangeFinder_LightWareI2C::sf20_init()
{
    // version strings for reporting
    char version[15] {};

    sf20_get_version("?P\r\n", "p:", version);

    if (version[0]) {
        hal.console->printf("SF20 Lidar version %s\n", version);
    }

    // Makes sure that "address tagging" is turned off.
    // Address tagging starts every response with "0x66".
    // Turns off Address Tagging just in case it was previously left on in the non-volatile configuration.
    sf20_disable_address_tagging();
    // Disconnect the servo (if applicable)
    sf20_send_and_expect("#SC,0\r\n", "sc:0");

    // Change the power consumption:
    // 0 = power off
    // 1 = power on
    // As of 7/10/17 sw and fw version 1.0 the "#E,1" command does not seem to be supported.
    // When it is supported the expected response would be "e:1".

    // Changes the number of lost signal confirmations: 1 [1..250].
    if (!sf20_send_and_expect("#LC,20\r\n", "lc:20")) {
        return false;
    }

#if 0
    // This location in the code may be uncommented to do a one time change of the devices address.
    // It should be commented out again and immediately reloaded to the pixhawk after the device has
    // been modified by this initialization process.
    // Address change to 0x65 = 101
    write_bytes((uint8_t*)"#CI,0x65\r\n",10);
    _dev->set_address(0x65);
    uint8_t rx_bytes[lx20_max_reply_len_bytes + 1];
    sf20_wait_on_reply(rx_bytes);
    // Save the comm settings
    if (!sf20_send_and_expect("%C\r\n", "%c:")) {
        return false;
    }
#endif

#if 0
    /*
      this can be used to change the laser encoding pattern
      Changes the laser encoding pattern: 3 (Random A) [0..4].
    */
    if (!sf20_send_and_expect("#LE,3\r\n", "le:3")) {
        return false;
    }
#endif

    // Sets datum offset [-10.00 ... 10.00].
    if (!sf20_send_and_expect("#LO,0.00\r\n", "lo:0.00")) {
        return false;
    }

    // Changes to a new measuring mode (update rate):
    //    1 = 388 readings per second
    //    2 = 194 readings per second
    //    3 = 129 readings per second
    //    4 = 97 readings per second
    //    5 = 78 readings per second
    //    6 = 65 readings per second
    //    7 = 55 readings per second
    //    8 = 48 readings per second
    if (!sf20_send_and_expect("#LM,7\r\n", "lm:7")) {
        return false;
    }

    // Changes the gain boost value:
    //     Adjustment range = -20.00 ... 5.00
    if (!sf20_send_and_expect("#LB,0.00\r\n", "lb:0.00")) {
        return false;
    }

    // Switches distance streaming on or off:
    // 0 = off
    // 1 = on
    if (!sf20_send_and_expect("#SU,1\r\n", "su:1")) {
        return false;
    }

    // Changes the laser state:
    //    0 = laser is off
    //    1 = laser is running
    if (!sf20_send_and_expect("#LF,1\r\n", "lf:1")) {
        return false;
    }

    // Requests the measurement specified in the first stream.
    write_bytes((uint8_t*)init_stream_id[0], strlen(init_stream_id[0]));

    // call timer() at 20Hz
    _dev->register_periodic_callback(50000,
                                     FUNCTOR_BIND_MEMBER(&AP_RangeFinder_LightWareI2C::sf20_timer, void));

    return true;
}

// read - return last value measured by sensor
bool AP_RangeFinder_LightWareI2C::legacy_get_reading(uint16_t &reading_cm)
{
    be16_t val;

    const uint8_t read_reg = LIGHTWARE_DISTANCE_READ_REG;

    // read the high and low byte distance registers
    if (_dev->transfer(&read_reg, 1, (uint8_t *)&val, sizeof(val))) {
        int16_t signed_val = int16_t(be16toh(val));
        if (signed_val < 0) {
            // some lidar firmwares will return 65436 for out of range
            reading_cm = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
        } else {
            reading_cm = uint16_t(signed_val);
        }
        return true;
    }
    return false;
}

// read - return last value measured by sf20 sensor
bool AP_RangeFinder_LightWareI2C::sf20_get_reading(uint16_t &reading_cm)
{
    // Parses up to 5 ASCII streams for LiDAR data.
    // If a parse fails, the stream measurement is not updated until it is successfully read in the future.
    uint8_t stream[lx20_max_expected_stream_reply_len_bytes+1]; // Maximum response length for a stream ie "ldf,0:40.99" is 11 characters

    uint8_t i = streamSequence[currentStreamSequenceIndex];
    size_t num_processed_chars = 0;

    /* Reads the LiDAR value requested during the last interrupt. */
    if (!_dev->read(stream, sizeof(stream))) {
        return false;
    }
    stream[lx20_max_expected_stream_reply_len_bytes] = 0;

    if (!sf20_parse_stream(stream, &num_processed_chars, parse_stream_id[i], sf20_stream_val[i])) {
        return false;
    }

    if (i==0) {
        reading_cm = sf20_stream_val[0];
    }

    // Increment the stream sequence
    currentStreamSequenceIndex++;
    if (currentStreamSequenceIndex >= numStreamSequenceIndexes) {
        currentStreamSequenceIndex = 0;
    }
    i = streamSequence[currentStreamSequenceIndex];

    // Request the next stream in the sequence from the SF20
    write_bytes((uint8_t*)init_stream_id[i], strlen(init_stream_id[i]));

    return true;
}

bool AP_RangeFinder_LightWareI2C::sf20_parse_stream(uint8_t *stream_buf,
        size_t *p_num_processed_chars,
        const char *string_identifier,
        uint16_t &val)
{
    size_t string_identifier_len = strlen(string_identifier);
    for (uint32_t i = 0 ; i < string_identifier_len ; i++) {
        if (stream_buf[*p_num_processed_chars] != string_identifier[i]) {
            return false;
        }
        (*p_num_processed_chars)++;
    }

    /*
      special case for being beyond maximum range, we receive a message like this:
      ldl,1:-1.00
      we will return max distance
     */
    if (strncmp((const char *)&stream_buf[*p_num_processed_chars], "-1.00", 5) == 0) {
        val = uint16_t(max_distance_cm() + LIGHTWARE_OUT_OF_RANGE_ADD_CM);
        (*p_num_processed_chars) += 5;
        return true;
    }

    /* Number is always returned in hundredths. So 6.33 is returned as 633. 6.3 is returned as 630.
     * 6 is returned as 600.
     * Extract number in format 6.33 or 123.99 (meters to be converted to centimeters).
     * Percentages such as 100 (percent), are returned as 10000.
     */
    uint32_t final_multiplier = 100;
    bool decrement_multiplier = false;
    bool number_found = false;
    uint16_t accumulator = 0;
    uint16_t digit_u16 = (uint16_t)stream_buf[*p_num_processed_chars];
    while ((((digit_u16 <= '9') &&
             (digit_u16 >= '0')) ||
            (digit_u16 == '.')) &&
           (*p_num_processed_chars < lx20_max_reply_len_bytes)) {
        (*p_num_processed_chars)++;
        if (decrement_multiplier) {
            final_multiplier /=10;
        }
        if (digit_u16 == '.') {
            decrement_multiplier = true;
            digit_u16 = (uint16_t)stream_buf[*p_num_processed_chars];
            continue;
        }
        number_found = true;
        accumulator *= 10;
        accumulator += digit_u16 - '0';
        digit_u16 = (uint16_t)stream_buf[*p_num_processed_chars];
    }

    accumulator *= final_multiplier;
    val = accumulator;
    return number_found;
}

/*
   update the state of the sensor
*/
void AP_RangeFinder_LightWareI2C::update(void)
{
    // nothing to do - its all done in the timer()
}

void AP_RangeFinder_LightWareI2C::legacy_timer(void)
{
    if (legacy_get_reading(state.distance_cm)) {
        // update range_valid state based on distance measured
        update_status();
    } else {
        set_status(RangeFinder::Status::NoData);
    }
}

void AP_RangeFinder_LightWareI2C::sf20_timer(void)
{
    if (sf20_get_reading(state.distance_cm)) {
        // update range_valid state based on distance measured
        update_status();
    } else {
        set_status(RangeFinder::Status::NoData);
    }
}

// Only for use during init as this blocks while waiting for the SF20 to be ready.
bool AP_RangeFinder_LightWareI2C::sf20_wait_on_reply(uint8_t *rx_two_byte)
{
    // Waits for a non-zero first byte while repeatedly reading 16 bits.
    // This is used after a read command to allow the sf20 time to provide the result.
    uint32_t start_time_ms = AP_HAL::millis();
    const uint32_t max_wait_time_ms = 50;

    while (AP_HAL::millis() - start_time_ms < max_wait_time_ms) {
        if (!_dev->read(rx_two_byte, 2)) {
            hal.scheduler->delay(1);
            continue;
        }
        if (rx_two_byte[0] != 0) {
            // normal exit
            return true;
        }
    }
    return false;
}