/*
  minimal test program for ICM20789 baro with IMU on SPI and baro on I2C
 */

#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <utility>
#include <stdio.h>

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

static AP_HAL::OwnPtr<AP_HAL::Device> i2c_dev;
static AP_HAL::OwnPtr<AP_HAL::Device> spi_dev;
static AP_HAL::OwnPtr<AP_HAL::Device> dev;

// SPI registers
#define MPUREG_WHOAMI                           0x75
#define MPUREG_USER_CTRL                        0x6A
#define MPUREG_PWR_MGMT_1                       0x6B

#define MPUREG_INT_PIN_CFG                      0x37
#       define BIT_BYPASS_EN                        0x02
#       define BIT_INT_RD_CLEAR                     0x10    // clear the interrupt when any read occurs
#       define BIT_LATCH_INT_EN                     0x20    // latch data ready pin

#define BIT_USER_CTRL_I2C_MST_EN             0x20 // Enable MPU to act as the I2C Master to external slave sensors
#define BIT_PWR_MGMT_1_DEVICE_RESET          0x80 // reset entire device
#define BIT_USER_CTRL_I2C_IF_DIS             0x10 // Disable primary I2C interface and enable hal.spi->interface
#define BIT_PWR_MGMT_1_CLK_XGYRO             0x01 // PLL with X axis gyroscope reference
#define BIT_PWR_MGMT_1_CLK_ZGYRO             0x03 // PLL with Z axis gyroscope reference

// baro commands
#define CMD_SOFT_RESET     0x805D
#define CMD_READ_ID        0xEFC8

void setup(void);
void loop(void);


#ifdef HAL_INS_MPU60x0_NAME
static void spi_init()
{
    // SPI reads have flag 0x80 set
    spi_dev->set_read_flag(0x80);

    // run the SPI bus at low speed
    spi_dev->set_speed(AP_HAL::Device::SPEED_LOW);

    uint8_t whoami = 0;
    uint8_t v;

    spi_dev->write_register(0x6B, 0x01);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6C, 0x3f);
    spi_dev->write_register(0xF5, 0x00);
    spi_dev->write_register(0x19, 0x09);
    spi_dev->write_register(0xEA, 0x00);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x1D, 0xC0);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x1A, 0xC0);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x38, 0x01);

    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->read_registers(0x6B, &v, 1);
    printf("reg 0x6B=0x%02x\n", v);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x41);
    spi_dev->write_register(0x6C, 0x3f);
    spi_dev->write_register(0x6B, 0x41);

    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x41);
    spi_dev->write_register(0x6C, 0x3f);
    spi_dev->write_register(0x6B, 0x41);

    spi_dev->read_registers(0x6A, &v, 1);
    printf("reg 0x6A=0x%02x\n", v);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6A, 0x10);
    spi_dev->write_register(0x6B, 0x41);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x6B, 0x01);

    hal.scheduler->delay(1);
    spi_dev->write_register(0x23, 0x00);
    spi_dev->write_register(0x6B, 0x41);

    // print the WHOAMI
    spi_dev->read_registers(MPUREG_WHOAMI, &whoami, 1);
    printf("20789 SPI whoami: 0x%02x\n", whoami);

    // wait for sensor to settle
    hal.scheduler->delay(100);

    // dump registers
    printf("ICM20789 registers\n");
#if 0
    for (uint8_t reg = 0; reg<0x80; reg++) {
        v=0;
        spi_dev->read_registers(reg, &v, 1);
        printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
        if ((reg+1) % 16 == 0) {
            printf("\n");
        }
    }
    printf("\n");
#endif

    spi_dev->get_semaphore()->give();
}
#endif

/*
  send a 16 bit command to the baro
 */
static bool send_cmd16(uint16_t cmd)
{
    uint8_t cmd_b[2] = { uint8_t(cmd >> 8), uint8_t(cmd & 0xFF) };
    return i2c_dev->transfer(cmd_b, 2, nullptr, 0);
}

/*
  read baro calibration data
 */
static bool read_calibration_data(void)
{
    // setup for OTP read
    const uint8_t cmd[5] = { 0xC5, 0x95, 0x00, 0x66, 0x9C };
    if (!i2c_dev->transfer(cmd, sizeof(cmd), nullptr, 0)) {
        return false;
    }
    for (uint8_t i=0; i<4; i++) {
        if (!send_cmd16(0xC7F7)) {
            return false;
        }
        uint8_t d[3];
        if (!i2c_dev->transfer(nullptr, 0, d, sizeof(d))) {
            return false;
        }
        uint16_t c = int16_t((d[0]<<8) | d[1]);
        printf("sensor_constants[%u]=%d\n", i, c);
    }
    return true;
}

#ifdef HAL_INS_MPU60x0_NAME
// initialise baro on i2c
static void i2c_init(void)
{
    if (!i2c_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        AP_HAL::panic("Failed to get baro semaphore");
    }

    if (send_cmd16(CMD_READ_ID)) {
        printf("ICM20789: read ID ******\n");
        uint8_t id[3] {};
        if (!i2c_dev->transfer(nullptr, 0, id, 3)) {
            printf("ICM20789: failed to read ID\n");
        }
        printf("ICM20789: ID %02x %02x %02x\n", id[0], id[1], id[2]);
    } else {
        printf("ICM20789 read ID failed\n");
    }

    if (send_cmd16(CMD_SOFT_RESET)) {
        printf("ICM20789: reset OK ************###########*********!!!!!!!!\n");
    } else {
        printf("ICM20789 baro reset failed\n");
    }
    hal.scheduler->delay(1);


    read_calibration_data();

    i2c_dev->get_semaphore()->give();

    printf("checking baro\n");
    if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        AP_HAL::panic("Failed to get device semaphore");
    }

    uint8_t regs[3] = { 0xC0, 0xC1, 0xC2 };
    for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {
        uint8_t v = 0;
        dev->read_registers(regs[i], &v, 1);
        printf("0x%02x : 0x%02x\n", regs[i], v);
    }
    dev->get_semaphore()->give();
}
#endif

void setup()
{
    printf("ICM20789 test\n");

    AP_BoardConfig{}.init();

    hal.scheduler->delay(1000);

#ifdef HAL_INS_MPU60x0_NAME
    spi_dev = std::move(hal.spi->get_device(HAL_INS_MPU60x0_NAME));

    if (!spi_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        AP_HAL::panic("Failed to get spi semaphore");
    }


    i2c_dev = std::move(hal.i2c_mgr->get_device(1, 0x63));
    dev = std::move(hal.i2c_mgr->get_device(1, 0x29));

    while (true) {
        spi_init();
        i2c_init();
        hal.scheduler->delay(1000);
    }
#else
    while (true) {
        printf("HAL_INS_MPU60x0_NAME not defined for this board\n");
        hal.scheduler->delay(1000);
    }
#endif
}


void loop()
{
}

AP_HAL_MAIN();