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

#include <utility>
#include <AP_HAL/AP_HAL.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/utility/sparse-endian.h>

#include "AP_InertialSensor_ADIS1647x.h"

#define BACKEND_SAMPLE_RATE 2000

/*
  device registers
 */
#define REG_PROD_ID  0x72
#define  PROD_ID_16470     0x4056
#define  PROD_ID_16477     0x405d

#define REG_GLOB_CMD 0x68
#define  GLOB_CMD_SW_RESET 0x80

#define REG_RANG_MDL 0x5E // 16477 only

#define REG_DATA_CNTR 0x22

/*
  timings
 */
#define T_STALL_US   20U
#define T_RESET_MS   250U

#define TIMING_DEBUG 0
#if TIMING_DEBUG
#define DEBUG_SET_PIN(n,v) hal.gpio->write(55-n, v)
#define DEBUG_TOGGLE_PIN(n) hal.gpio->toggle(55-n)
#else
#define DEBUG_SET_PIN(n,v)
#define DEBUG_TOGGLE_PIN(n)
#endif

extern const AP_HAL::HAL& hal;

AP_InertialSensor_ADIS1647x::AP_InertialSensor_ADIS1647x(AP_InertialSensor &imu,
                                                   AP_HAL::OwnPtr<AP_HAL::Device> _dev,
                                                   enum Rotation _rotation)
    : AP_InertialSensor_Backend(imu)
    , dev(std::move(_dev))
    , rotation(_rotation)
{
}

AP_InertialSensor_Backend *
AP_InertialSensor_ADIS1647x::probe(AP_InertialSensor &imu,
                                AP_HAL::OwnPtr<AP_HAL::Device> dev,
                                enum Rotation rotation)
{
    if (!dev) {
        return nullptr;
    }
    auto sensor = new AP_InertialSensor_ADIS1647x(imu, std::move(dev), rotation);

    if (!sensor) {
        return nullptr;
    }

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

    return sensor;
}

void AP_InertialSensor_ADIS1647x::start()
{
    accel_instance = _imu.register_accel(BACKEND_SAMPLE_RATE, dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X));
    gyro_instance = _imu.register_gyro(BACKEND_SAMPLE_RATE,   dev->get_bus_id_devtype(DEVTYPE_INS_ADIS1647X));

    // setup sensor rotations from probe()
    set_gyro_orientation(gyro_instance, rotation);
    set_accel_orientation(accel_instance, rotation);

    /*
      as the sensor does not have a FIFO we need to jump through some
      hoops to ensure we don't lose any samples. This creates a thread
      to do the capture, running at very high priority
     */
    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_ADIS1647x::loop, void),
                                      "ADIS1647x",
                                      1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
        AP_HAL::panic("Failed to create ADIS1647x thread");
    }
}

/*
  check product ID
 */
bool AP_InertialSensor_ADIS1647x::check_product_id(void)
{
    uint16_t prod_id = read_reg16(REG_PROD_ID);
    switch (prod_id) {
    case PROD_ID_16470:
        // can do up to 40G
        accel_scale = 1.25 * GRAVITY_MSS * 0.001;
        _clip_limit = 39.5f * GRAVITY_MSS;
        gyro_scale = radians(0.1);
        return true;

    case PROD_ID_16477:
        // can do up to 40G
        accel_scale = 1.25 * GRAVITY_MSS * 0.001;
        _clip_limit = 39.5f * GRAVITY_MSS;
        // RANG_MDL register used for gyro range
        uint16_t rang_mdl = read_reg16(REG_RANG_MDL);
        switch ((rang_mdl >> 2) & 3) {
        case 0:
            gyro_scale = radians(1.0/160);
            break;
        case 1:
            gyro_scale = radians(1.0/40);
            break;
        case 3:
            gyro_scale = radians(1.0/10);
            break;
        default:
            return false;
        }
        return true;
    }
    return false;
}


bool AP_InertialSensor_ADIS1647x::init()
{
    WITH_SEMAPHORE(dev->get_semaphore());
    if (!check_product_id()) {
        return false;
    }

    // perform software reset
    write_reg16(REG_GLOB_CMD, GLOB_CMD_SW_RESET);
    hal.scheduler->delay(T_RESET_MS);

    // re-check after reset
    if (!check_product_id()) {
        return false;
    }

    // we leave all config registers at defaults

#if TIMING_DEBUG
    // useful for debugging scheduling of transfers
    hal.gpio->pinMode(52, HAL_GPIO_OUTPUT);
    hal.gpio->pinMode(53, HAL_GPIO_OUTPUT);
    hal.gpio->pinMode(54, HAL_GPIO_OUTPUT);
    hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
#endif

    // we need to use low speed for burst transfers
    dev->set_speed(AP_HAL::Device::SPEED_LOW);

    return true;
}


/*
  read a 16 bit register value
 */
uint16_t AP_InertialSensor_ADIS1647x::read_reg16(uint8_t regnum) const
{
    uint8_t req[2] = {regnum, 0};
    uint8_t reply[2] {};
    dev->transfer(req, sizeof(req), nullptr, 0);
    hal.scheduler->delay_microseconds(T_STALL_US);
    dev->transfer(nullptr, 0, reply, sizeof(reply));
    uint16_t ret = (reply[0]<<8U) | reply[1];
    return ret;
}


/*
  write a 16 bit register value
 */
void AP_InertialSensor_ADIS1647x::write_reg16(uint8_t regnum, uint16_t value) const
{
    uint8_t req[2];
    req[0] = (regnum | 0x80);
    req[1] = value & 0xFF;
    dev->transfer(req, sizeof(req), nullptr, 0);
    hal.scheduler->delay_microseconds(T_STALL_US);

    req[0] = ((regnum+1) | 0x80);
    req[1] = (value>>8) & 0xFF;
    dev->transfer(req, sizeof(req), nullptr, 0);
    hal.scheduler->delay_microseconds(T_STALL_US);
}

/*
  loop to read the sensor
 */
void AP_InertialSensor_ADIS1647x::read_sensor(void)
{
    struct adis_data {
        uint8_t cmd[2];
        uint16_t diag_stat;
        int16_t  gx;
        int16_t  gy;
        int16_t  gz;
        int16_t  ax;
        int16_t  ay;
        int16_t  az;
        int16_t  temp;
        uint16_t counter;
        uint8_t  pad;
        uint8_t  checksum;
    } data {};

    uint64_t sample_start_us = AP_HAL::micros64();
    do {
        WITH_SEMAPHORE(dev->get_semaphore());
        data.cmd[0] = REG_GLOB_CMD;
        DEBUG_SET_PIN(2, 1);
        if (!dev->transfer((const uint8_t *)&data, sizeof(data), (uint8_t *)&data, sizeof(data))) {
            break;
        }
        DEBUG_SET_PIN(2, 0);
    } while (be16toh(data.counter) == last_counter);

    DEBUG_SET_PIN(1, 1);

    /*
      check the 8 bit checksum of the packet
     */
    uint8_t sum = 0;
    const uint8_t *b = (const uint8_t *)&data.diag_stat;
    for (uint8_t i=0; i<offsetof(adis_data, pad) - offsetof(adis_data, diag_stat); i++) {
        sum += b[i];
    }
    if (sum != data.checksum) {
        DEBUG_TOGGLE_PIN(3);
        DEBUG_TOGGLE_PIN(3);
        DEBUG_TOGGLE_PIN(3);
        DEBUG_TOGGLE_PIN(3);
        // corrupt data
        return;
    }

    /*
      check if we have lost a sample
     */
    uint16_t counter = be16toh(data.counter);
    if (done_first_read && uint16_t(last_counter+1) != counter) {
        DEBUG_TOGGLE_PIN(3);
    }
    done_first_read = true;
    last_counter = counter;

    Vector3f accel{float(int16_t(be16toh(data.ax))), float(int16_t(be16toh(data.ay))), float(int16_t(be16toh(data.az)))};
    Vector3f gyro{float(int16_t(be16toh(data.gx))), float(int16_t(be16toh(data.gy))), float(int16_t(be16toh(data.gz)))};

    accel *= accel_scale;
    gyro *= gyro_scale;

    _rotate_and_correct_accel(accel_instance, accel);
    _notify_new_accel_raw_sample(accel_instance, accel, sample_start_us);

    _rotate_and_correct_gyro(gyro_instance, gyro);
    _notify_new_gyro_raw_sample(gyro_instance, gyro, sample_start_us);

    /*
      publish average temperature at 20Hz
     */
    temp_sum += float(int16_t(be16toh(data.temp))*0.1);
    temp_count++;

    if (temp_count == 100) {
        _publish_temperature(accel_instance, temp_sum/temp_count);
        temp_sum = 0;
        temp_count = 0;
    }
    DEBUG_SET_PIN(1, 0);
}

/*
  sensor read loop
 */
void AP_InertialSensor_ADIS1647x::loop(void)
{
    while (true) {
        uint32_t tstart = AP_HAL::micros();
        // we deliberately set the period a bit fast to ensure we
        // don't lose a sample
        const uint32_t period_us = 480;
        read_sensor();
        uint32_t dt = AP_HAL::micros() - tstart;
        if (dt < period_us) {
            hal.scheduler->delay_microseconds(period_us - dt);
        }
    }
}

bool AP_InertialSensor_ADIS1647x::update()
{
    update_accel(accel_instance);
    update_gyro(gyro_instance);
    return true;
}