#include "bmi160.h"

#include <string.h>
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

esp_err_t imu_init(bmi160_t *dev, i2c_port_t port){
    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));
    }

    // 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");
    }

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

    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);

    return ESP_OK;
}


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

#define BMI160_MAX_WRITE_LEN 32

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);
}