#include "bmi160.h"

#include <stdbool.h>
#include <string.h>

#include "driver/gpio.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"

#define BMI160_MAX_WRITE_LEN 32
#define BMI160_SAMPLE_QUEUE_LEN 16
#define BMI160_QUEUE_MIN_FREE_SLOTS 1
#define BMI160_READ_TASK_STACK_SIZE 4096
#define BMI160_READ_TASK_PRIORITY 5

static bool s_gpio_isr_service_installed = false;

static void IRAM_ATTR bmi160_drdy_isr(void *arg)
{
    bmi160_t *dev = (bmi160_t *)arg;
    BaseType_t higher_priority_task_woken = pdFALSE;

    if (dev != NULL && dev->read_task != NULL)
    {
        vTaskNotifyGiveFromISR(dev->read_task, &higher_priority_task_woken);
    }

    if (higher_priority_task_woken == pdTRUE)
    {
        portYIELD_FROM_ISR();
    }
}

static void bmi160_read_task(void *arg)
{
    bmi160_t *dev = (bmi160_t *)arg;

    for (;;)
    {
        bmi160_value_t value;

        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
        if (imu_read(dev, &value) != ESP_OK)
        {
            continue;
        }

        if (dev->sample_queue == NULL)
        {
            continue;
        }

        if (uxQueueSpacesAvailable(dev->sample_queue) <= BMI160_QUEUE_MIN_FREE_SLOTS)
        {
            continue;
        }

        xQueueSend(dev->sample_queue, &value, 0);
    }
}

static esp_err_t bmi160_init_streaming(bmi160_t *dev, gpio_num_t drdy_io, QueueHandle_t *queue_handle)
{
    const gpio_config_t io_conf = {
        .pin_bit_mask = (1ULL << drdy_io),
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_DISABLE,
        .pull_down_en = GPIO_PULLDOWN_DISABLE,
        .intr_type = GPIO_INTR_POSEDGE,
    };

    if (queue_handle == NULL)
    {
        return ESP_ERR_INVALID_ARG;
    }

    ESP_ERROR_CHECK(gpio_config(&io_conf));

    if (!s_gpio_isr_service_installed)
    {
        esp_err_t err = gpio_install_isr_service(0);
        if (err != ESP_OK && err != ESP_ERR_INVALID_STATE)
        {
            return err;
        }
        s_gpio_isr_service_installed = true;
    }

    dev->sample_queue = xQueueCreate(BMI160_SAMPLE_QUEUE_LEN, sizeof(bmi160_value_t));
    if (dev->sample_queue == NULL)
    {
        return ESP_ERR_NO_MEM;
    }

    *queue_handle = dev->sample_queue;

    BaseType_t task_ok = xTaskCreate(
        bmi160_read_task,
        "bmi160_read",
        BMI160_READ_TASK_STACK_SIZE,
        dev,
        BMI160_READ_TASK_PRIORITY,
        &dev->read_task);
    if (task_ok != pdPASS)
    {
        vQueueDelete(dev->sample_queue);
        dev->sample_queue = NULL;
        *queue_handle = NULL;
        return ESP_ERR_NO_MEM;
    }

    ESP_ERROR_CHECK(gpio_isr_handler_add(drdy_io, bmi160_drdy_isr, dev));
    return ESP_OK;
}

esp_err_t imu_init(bmi160_t *dev, i2c_port_t port, gpio_num_t drdy_io, QueueHandle_t *queue_handle)
{
    esp_err_t err;

    err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_LOW);
    if (err != ESP_OK)
    {
        ESP_LOGW("bmi160", "BMI160 not found at 0x%02X: %s", BMI160_I2C_ADDRESS_LOW, esp_err_to_name(err));
        err = bmi160_init(dev, port, BMI160_I2C_ADDRESS_HIGH);
    }
    if (err != ESP_OK)
    {
        ESP_LOGE("bmi160", "BMI160 probe failed at 0x%02X and 0x%02X: %s",
                 BMI160_I2C_ADDRESS_LOW,
                 BMI160_I2C_ADDRESS_HIGH,
                 esp_err_to_name(err));
        return err;
    }

    dev->drdy_io = drdy_io;
    dev->sample_queue = NULL;
    dev->read_task = NULL;
    if (queue_handle != NULL)
    {
        *queue_handle = NULL;
    }

    // Soft reset
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0xB6));
    vTaskDelay(pdMS_TO_TICKS(50));

    // Check chip ID
    uint8_t chip_id;
    ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id));
    ESP_LOGI("bmi160", "BMI160 chip id: 0x%02x at 0x%02x", chip_id, dev->i2c_address);
    if (chip_id != BMI160_CHIP_ID)
    {
        ESP_LOGE("bmi160", "BMI160 chip id not right");
        return ESP_FAIL;
    }

    // Configure ACC: 100Hz, normal mode filter, no undersampling
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_CONF, 0x28));
    // Configure ACC range: +-16g
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_ACC_RANGE, 0x0C));

    // Configure GYR: 100Hz, normal mode filter, no undersampling
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_GYR_CONF, 0x28));
    // Configure GYR range: +-2000dps
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_GYR_RANGE, 0x00));

    // Both sensors in normal mode
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0x11));
    vTaskDelay(pdMS_TO_TICKS(100));
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_CMD, 0x15));
    vTaskDelay(pdMS_TO_TICKS(100));

    // Poll until active
    bool startup_complete = false;
    for (int timeout = 0; timeout < 1000; timeout += 10)
    {
        uint8_t status;
        ESP_ERROR_CHECK(bmi160_read_register(dev, BMI160_REG_PMU_STATUS, &status));
        int acc_pmu_status = (status & 0b00110000) >> 4;
        int gyr_pmu_status = (status & 0b00001100) >> 2;
        if (acc_pmu_status == 0b01 && gyr_pmu_status == 0b01)
        {
            startup_complete = true;
            break;
        }
        vTaskDelay(pdMS_TO_TICKS(10));
    }
    if (!startup_complete)
    {
        ESP_LOGE("bmi160", "Acc or gyr not set in normal mode");
    }

    // enable DRDY interrupt
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_EN1, 0x10));
    // enable INT1, active high push pull
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_OUT_CTRL, 0x0a));
    // Disable INT1 input, non-latched
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_LATCH, 0x00));
    // Map DRDY to INT1
    ESP_ERROR_CHECK(bmi160_write_register(dev, BMI160_REG_INT_MAP1, 0x80));

    // Clear data registers
    uint8_t data[20];
    ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));

    if (drdy_io >= 0 && queue_handle != NULL)
    {
        return bmi160_init_streaming(dev, drdy_io, queue_handle);
    }

    return ESP_OK;
}

esp_err_t imu_read(const bmi160_t *dev, bmi160_value_t *value)
{
    if (dev == NULL || value == NULL)
    {
        return ESP_ERR_INVALID_ARG;
    }

    uint8_t data[20];
    ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_DATA, data, BMI160_SIZE_REG_DATA));
    value->acc.x = data[14] | (data[15] << 8);
    value->acc.y = data[16] | (data[17] << 8);
    value->acc.z = data[18] | (data[19] << 8);
    value->gyr.x = data[8] | (data[9] << 8);
    value->gyr.y = data[10] | (data[11] << 8);
    value->gyr.z = data[12] | (data[13] << 8);

    uint8_t time[3];
    ESP_ERROR_CHECK(bmi160_read_registers(dev, BMI160_REG_SENSORTIME, time, BMI160_SIZE_SENSORTIME));
    value->time = (time[0] | (time[1]<<8) | (time[2]<<16)) >> 8; // shift right to set resolution

    return ESP_OK;
}

// ----------------------------------------------------
// Low level driver

static esp_err_t bmi160_check_dev(const bmi160_t *dev)
{
    if (dev == NULL)
    {
        return ESP_ERR_INVALID_ARG;
    }

    return ESP_OK;
}

esp_err_t bmi160_init(bmi160_t *dev, i2c_port_t i2c_port, uint8_t i2c_address)
{
    uint8_t chip_id = 0;
    esp_err_t err;

    if (dev == NULL)
    {
        return ESP_ERR_INVALID_ARG;
    }

    dev->i2c_port = i2c_port;
    dev->i2c_address = i2c_address;
    dev->timeout_ticks = pdMS_TO_TICKS(BMI160_DEFAULT_TIMEOUT_MS);

    err = bmi160_read_register(dev, BMI160_REG_CHIP_ID, &chip_id);
    if (err != ESP_OK)
    {
        return err;
    }

    if (chip_id != BMI160_CHIP_ID)
    {
        return ESP_ERR_NOT_FOUND;
    }

    return ESP_OK;
}

esp_err_t bmi160_read_register(const bmi160_t *dev, uint8_t reg, uint8_t *value)
{
    if (value == NULL)
    {
        return ESP_ERR_INVALID_ARG;
    }

    return bmi160_read_registers(dev, reg, value, 1);
}

esp_err_t bmi160_read_registers(const bmi160_t *dev, uint8_t start_reg, uint8_t *data, size_t len)
{
    esp_err_t err = bmi160_check_dev(dev);
    if (err != ESP_OK)
    {
        return err;
    }

    if (data == NULL || len == 0)
    {
        return ESP_ERR_INVALID_ARG;
    }

    return i2c_master_write_read_device(
        dev->i2c_port,
        dev->i2c_address,
        &start_reg,
        1,
        data,
        len,
        dev->timeout_ticks);
}

esp_err_t bmi160_write_register(const bmi160_t *dev, uint8_t reg, uint8_t value)
{
    return bmi160_write_registers(dev, reg, &value, 1);
}

esp_err_t bmi160_write_registers(const bmi160_t *dev, uint8_t start_reg, const uint8_t *data, size_t len)
{
    uint8_t buffer[1 + BMI160_MAX_WRITE_LEN];
    esp_err_t err = bmi160_check_dev(dev);

    if (err != ESP_OK)
    {
        return err;
    }

    if (data == NULL || len == 0 || len > BMI160_MAX_WRITE_LEN)
    {
        return ESP_ERR_INVALID_ARG;
    }

    buffer[0] = start_reg;
    memcpy(&buffer[1], data, len);

    return i2c_master_write_to_device(
        dev->i2c_port,
        dev->i2c_address,
        buffer,
        len + 1,
        dev->timeout_ticks);
}