/*
 * Copyright (c) 2023 Brendan Haines
 */

#define DT_DRV_COMPAT bosch_bmm150

#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(bmm150, CONFIG_SENSOR_LOG_LEVEL);

const uint8_t MAG_REG_CHIP_ID = 0x40;
const uint8_t MAG_REG_DATA_X_LSB = 0x42;
const uint8_t MAG_REG_DATA_X_MSB = 0x43;
const uint8_t MAG_REG_DATA_Y_LSB = 0x44;
const uint8_t MAG_REG_DATA_Y_MSB = 0x45;
const uint8_t MAG_REG_DATA_Z_LSB = 0x46;
const uint8_t MAG_REG_DATA_Z_MSB = 0x47;
const uint8_t MAG_REG_RHALL_LSB = 0x48;
const uint8_t MAG_REG_RHALL_MSB = 0x49;
const uint8_t MAG_REG_INT_STATUS = 0x4A;
const uint8_t MAG_REG_POWER = 0x4B;
typedef enum mag_power_t
{
    MAG_POWER_POWER_ON = 0x01,
    MAG_POWER_RESET = 0x82,
} mag_power_t;
const uint8_t MAG_REG_MODE = 0x4C;
typedef enum mag_mode_t
{
    MAG_MODE_OPMODE_NORMAL = 0x00 << 1,
    MAG_MODE_OPMODE_FORCED = 0x01 << 1,
    MAG_MODE_OPMODE_SLEEP_MODE = 0x11 << 1,
} mag_mode_t;
const uint8_t MAG_REG_INT_ENABLE = 0x4D;
const uint8_t MAG_REG_INT_SETTINGS = 0x4E;
const uint8_t MAG_REG_LOW_THRESH = 0x4F;
const uint8_t MAG_REG_HIGH_THRESH = 0x50;
const uint8_t MAG_REG_REP_XY = 0x51;
const uint8_t MAG_REG_REP_Z = 0x52;

struct bmm150_data
{
    int16_t mag_x;
    int16_t mag_y;
    int16_t mag_z;
    int8_t temperature; // TODO: does mag have die temp sensor?
};

struct bmm150_config
{
    struct i2c_dt_spec bus;
#ifdef CONFIG_INA230_TRIGGER
    bool trig_enabled;
    uint16_t mask;
    const struct gpio_dt_spec alert_gpio;
    uint16_t alert_limit;
#endif /* CONFIG_INA230_TRIGGER */
};

static int bmm150_sample_fetch(const struct device *dev,
                               enum sensor_channel chan)
{
    const struct bmm150_config *config = dev->config;
    struct bmm150_data *data = dev->data;
    int ret;

    uint8_t mag[6];
    ret = i2c_burst_read_dt(&config->bus, MAG_REG_DATA_X_LSB, mag, sizeof(mag));
    if (ret < 0)
    {
        LOG_ERR("Failed to read mag registers!");
        return ret;
    }
    data->mag_x = ((int16_t)sys_get_le16(&mag[0])) >> 1;
    data->mag_y = ((int16_t)sys_get_le16(&mag[2])) >> 1;
    data->mag_z = ((int16_t)sys_get_le16(&mag[4])) >> 1;

    return 0;
}

static int bmm150_channel_get(const struct device *dev,
                              enum sensor_channel chan,
                              struct sensor_value *val)
{
    struct bmm150_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;
    // Gauss
    case SENSOR_CHAN_MAGN_X:
    {
        float rate = data->mag_x * 1300.0 / (2 << 15) / 100; // to uT, to gauss
        val->val1 = rate;
        val->val2 = (rate - val->val1) * 1000000;
        break;
    }
    case SENSOR_CHAN_MAGN_Y:
    {
        float rate = data->mag_y * 1300.0 / (2 << 15) / 100; // to uT, to gauss
        val->val1 = rate;
        val->val2 = (rate - val->val1) * 1000000;
        break;
    }
    case SENSOR_CHAN_MAGN_Z:
    {
        float rate = data->mag_z * 2500.0 / (2 << 15) / 100; // to uT, to gauss
        val->val1 = rate;
        val->val2 = (rate - val->val1) * 1000000;
        break;
    }
    default:
        return -ENOTSUP;
    }

    return 0;
}

static const struct sensor_driver_api bmm150_api = {
    .sample_fetch = &bmm150_sample_fetch,
    .channel_get = &bmm150_channel_get,
};

static int bmm150_init(const struct device *dev)
{
    const struct bmm150_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 the sensor

    // Wait for device to reset

    uint8_t mag_power = MAG_POWER_POWER_ON;
    ret = i2c_burst_write_dt(&config->bus, MAG_REG_POWER, &mag_power, sizeof(mag_power));
    if (ret < 0)
    {
        LOG_ERR("Failed to power up magnetometer");
        return ret;
    }
    uint8_t mag_mode = MAG_MODE_OPMODE_NORMAL;
    ret = i2c_burst_write_dt(&config->bus, MAG_REG_MODE, &mag_mode, sizeof(mag_mode));
    if (ret < 0)
    {
        LOG_ERR("Failed to start magnetometer");
        return ret;
    }

    uint8_t mag_chip_id;
    ret = i2c_burst_read_dt(&config->bus, MAG_REG_CHIP_ID, &mag_chip_id, sizeof(mag_chip_id));
    if (ret < 0)
    {
        LOG_ERR("Failed to read magnetometer chip ID register!");
        return ret;
    }
    const uint8_t MAG_CHIP_ID = 0x32;
    if (mag_chip_id != MAG_CHIP_ID)
    {
        LOG_ERR("Mag chip ID read from %s incorrect. Read 0x%02X, expected 0x%02X", dev->name, mag_chip_id, MAG_CHIP_ID);
    }

    return 0;
}

#define BMM150_INIT(i)                                            \
    static struct bmm150_data bmm150_data_##i;                    \
                                                                  \
    static const struct bmm150_config bmm150_config_##i = {       \
        .bus = I2C_DT_SPEC_INST_GET(i),                           \
    };                                                            \
                                                                  \
    SENSOR_DEVICE_DT_INST_DEFINE(i, &bmm150_init, NULL,           \
                                 &bmm150_data_##i,                \
                                 &bmm150_config_##i, POST_KERNEL, \
                                 CONFIG_SENSOR_INIT_PRIORITY, &bmm150_api);

DT_INST_FOREACH_STATUS_OKAY(BMM150_INIT)