/*
 * Copyright (c) 2021 Nordic Semiconductor ASA
 * SPDX-License-Identifier: Apache-2.0
 */

#define DT_DRV_COMPAT bosch_bmg160

#include <zephyr/device.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/i2c.h>
#include <zephyr/drivers/sensor.h>
#include <zephyr/sys/byteorder.h>

#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(bmg160, CONFIG_SENSOR_LOG_LEVEL);

const uint8_t GYRO_REG_CHIP_ID = 0x00;
const uint8_t GYRO_REG_RATE_X_LSB = 0x02;
const uint8_t GYRO_REG_RATE_X_MSB = 0x03;
const uint8_t GYRO_REG_RATE_Y_LSB = 0x04;
const uint8_t GYRO_REG_RATE_Y_MSB = 0x05;
const uint8_t GYRO_REG_RATE_Z_LSB = 0x06;
const uint8_t GYRO_REG_RATE_Z_MSB = 0x07;
const uint8_t GYRO_REG_INT_STATUS_0 = 0x09;
const uint8_t GYRO_REG_INT_STATUS_1 = 0x0A;
const uint8_t GYRO_REG_INT_STATUS_2 = 0x0B;
const uint8_t GYRO_REG_INT_STATUS_3 = 0x0C;
const uint8_t GYRO_REG_FIFO_STATUS = 0x0E;
const uint8_t GYRO_REG_RANGE = 0x0F;
const uint8_t GYRO_REG_BW = 0x10;
const uint8_t GYRO_REG_LPM1 = 0x11;
const uint8_t GYRO_REG_LPM2 = 0x12;
const uint8_t GYRO_REG_RATE_HBW = 0x13;
const uint8_t GYRO_REG_BGW_SOFTRESET = 0x14;
const uint8_t GYRO_REG_INT_EN_0 = 0x15;
const uint8_t GYRO_REG_INT_EN_1 = 0x16;
const uint8_t GYRO_REG_INT_MAP_0 = 0x17;
const uint8_t GYRO_REG_INT_MAP_1 = 0x18;
const uint8_t GYRO_REG_INT_MAP_2 = 0x19;
const uint8_t GYRO_REG_INT_SOURCE_1 = 0x1A;
const uint8_t GYRO_REG_INT_SOURCE_2 = 0x1B;
// const uint8_t GYRO_REG_ = 0x1C;
// const uint8_t GYRO_REG_ = 0x1E;
const uint8_t GYRO_REG_INT_RST_LATCH = 0x21;
const uint8_t GYRO_REG_HIGH_TH_X = 0x22;
const uint8_t GYRO_REG_HIGH_DUR_X = 0x23;
const uint8_t GYRO_REG_HIGH_TH_Y = 0x24;
const uint8_t GYRO_REG_HIGH_DUR_Y = 0x25;
const uint8_t GYRO_REG_HIGH_TH_Z = 0x26;
const uint8_t GYRO_REG_HIGH_DUR_Z = 0x27;
const uint8_t GYRO_REG_SOC = 0x31;
const uint8_t GYRO_REG_A_FOC = 0x32;
const uint8_t GYRO_REG_TRIM_NVM_CTRL = 0x33;
const uint8_t GYRO_REG_BGW_SPI3_WDT = 0x34;
const uint8_t GYRO_REG_OFC1 = 0x36;
const uint8_t GYRO_REG_OFC2 = 0x37;
const uint8_t GYRO_REG_OFC3 = 0x38;
const uint8_t GYRO_REG_OFC4 = 0x39;
const uint8_t GYRO_REG_TRIM_GP0 = 0x3A;
const uint8_t GYRO_REG_TRIM_GP1 = 0x3B;
const uint8_t GYRO_REG_BIST = 0x3C;
const uint8_t GYRO_REG_FIFO_CONFIG_0 = 0x3D;
const uint8_t GYRO_REG_FIFO_CONFIG_1 = 0x3E;

struct bmg160_data
{
    int16_t gyro_x;
    int16_t gyro_y;
    int16_t gyro_z;
    int8_t temperature; // FIXME: does gyro have die temp sensor?
};

struct bmg160_config
{
    struct i2c_dt_spec bus;
};

static int bmg160_sample_fetch(const struct device *dev,
                               enum sensor_channel chan)
{
    const struct bmg160_config *config = dev->config;
    struct bmg160_data *data = dev->data;
    int ret;

    uint8_t gyro[6];
    ret = i2c_burst_read_dt(&config->bus, GYRO_REG_RATE_X_LSB, gyro, sizeof(gyro));
    if (ret < 0)
    {
        LOG_ERR("Failed to read gyro registers!");
        return ret;
    }
    data->gyro_x = ((int16_t)sys_get_le16(&gyro[0]));
    data->gyro_y = ((int16_t)sys_get_le16(&gyro[2]));
    data->gyro_z = ((int16_t)sys_get_le16(&gyro[4]));

    return 0;
}

static int bmg160_channel_get(const struct device *dev,
                              enum sensor_channel chan,
                              struct sensor_value *val)
{
    struct bmg160_data *data = dev->data;

    switch (chan)
    {
    // // degrees C
    // case SENSOR_CHAN_DIE_TEMP:
    //     // 0.5K/LSB, center temperature is 23C
    //     val->val1 = 23 + data->temperature / 2;
    //     val->val2 = 0; // TODO: don't throw out LSB
    //     break;
    // radians/second
    case SENSOR_CHAN_GYRO_X:
    {
        float rate = data->gyro_x * 2000.0 / 32767;
        val->val1 = rate;
        val->val2 = (rate - val->val1) * 1000000;
        break;
    }
    case SENSOR_CHAN_GYRO_Y:
    {
        float rate = data->gyro_y * 2000.0 / 32767;
        val->val1 = rate;
        val->val2 = (rate - val->val1) * 1000000;
        break;
    }
    case SENSOR_CHAN_GYRO_Z:
    {
        float rate = data->gyro_z * 2000.0 / 32767;
        val->val1 = rate;
        val->val2 = (rate - val->val1) * 1000000;
        break;
    }
    default:
        return -ENOTSUP;
    }

    return 0;
}

static const struct sensor_driver_api bmg160_api = {
    .sample_fetch = &bmg160_sample_fetch,
    .channel_get = &bmg160_channel_get,
};

static int bmg160_init(const struct device *dev)
{
    const struct bmg160_config *const config = dev->config;
    int ret;

    if (!device_is_ready(config->bus.bus))
    {
        LOG_ERR("I2C bus %s is not ready", config->bus.bus->name);
        return -ENODEV;
    }

    // TODO: reset IC?
    // Wait for device to reset

    uint8_t gyro_chip_id;
    ret = i2c_burst_read_dt(&config->bus, GYRO_REG_CHIP_ID, &gyro_chip_id, sizeof(gyro_chip_id));
    if (ret < 0)
    {
        LOG_ERR("Failed to read gyro chip ID register!");
        return ret;
    }
    const uint8_t GYRO_CHIP_ID = 0x0f;
    if (gyro_chip_id != GYRO_CHIP_ID)
    {
        LOG_ERR("Gyro chip ID read from %s incorrect. Read 0x%02X, expected 0x%02X", dev->name, gyro_chip_id, GYRO_CHIP_ID);
    }

    return 0;
}

#define BMG160_INIT(i)                                            \
    static struct bmg160_data bmg160_data_##i;                    \
                                                                  \
    static const struct bmg160_config bmg160_config_##i = {       \
        .bus = I2C_DT_SPEC_INST_GET(i),                           \
    };                                                            \
                                                                  \
    SENSOR_DEVICE_DT_INST_DEFINE(i, &bmg160_init, NULL,           \
                                 &bmg160_data_##i,                \
                                 &bmg160_config_##i, POST_KERNEL, \
                                 CONFIG_SENSOR_INIT_PRIORITY, &bmg160_api);

DT_INST_FOREACH_STATUS_OKAY(BMG160_INIT)