#include <stdio.h>
#include <inttypes.h>
#include <stdbool.h>

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

#include "motors.h"
#include "bmi160.h"
#include "fixedpoint.h"
#include "mahony_filter.h"

#define BBOT_DRDY_INPUT_PIN GPIO_NUM_1

#define BBOT_I2C_PORT I2C_NUM_0
#define BBOT_I2C_SCL_IO GPIO_NUM_3
#define BBOT_I2C_SDA_IO GPIO_NUM_2
#define BBOT_I2C_FREQ_HZ 100000

#define BBOT_FILTER_FRAC_BITS 12
#define BBOT_FILTER_KP FIXED_FROM_INT(2, BBOT_FILTER_FRAC_BITS)
#define BBOT_FILTER_KI 0
#define BBOT_PROCESS_TASK_STACK_SIZE 4096
#define BBOT_PROCESS_TASK_PRIORITY 5
#define BBOT_PRINT_PERIOD_MS 20

static bmi160_t imu;
static QueueHandle_t imu_queue;
static SemaphoreHandle_t imu_state_mutex;

typedef struct
{
    mahony_filter_t filter;
    bmi160_value_t sample;
    int32_t yaw;
    int32_t pitch;
    int32_t roll;
    uint32_t last_time;
    bool has_sample;
    bool has_orientation;
} imu_state_t;

static imu_state_t imu_state;

static void imu_process_task(void *arg)
{
    QueueHandle_t queue = (QueueHandle_t)arg;

    for (;;)
    {
        bmi160_value_t sample;

        if (xQueueReceive(queue, &sample, portMAX_DELAY) != pdTRUE)
        {
            continue;
        }

        if (xSemaphoreTake(imu_state_mutex, portMAX_DELAY) != pdTRUE)
        {
            continue;
        }

        if (imu_state.has_sample)
        {
            const uint32_t dt_ticks = sample.time - imu_state.last_time;
            const int32_t dt = FIXED_FROM_RATIO((int32_t)dt_ticks, 100, BBOT_FILTER_FRAC_BITS);

            mahony_filter_update_imu(
                &imu_state.filter,
                sample.gyr.x,
                sample.gyr.y,
                sample.gyr.z,
                sample.acc.x,
                sample.acc.y,
                sample.acc.z,
                dt);
            mahony_filter_get_yaw_pitch_roll(
                &imu_state.filter,
                &imu_state.yaw,
                &imu_state.pitch,
                &imu_state.roll);
            imu_state.has_orientation = true;
        }

        imu_state.sample = sample;
        imu_state.last_time = sample.time;
        imu_state.has_sample = true;
        xSemaphoreGive(imu_state_mutex);
    }
}

static esp_err_t init_i2c()
{
    const i2c_config_t i2c_conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = BBOT_I2C_SDA_IO,
        .scl_io_num = BBOT_I2C_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = BBOT_I2C_FREQ_HZ,
    };

    ESP_ERROR_CHECK(i2c_param_config(BBOT_I2C_PORT, &i2c_conf));
    ESP_ERROR_CHECK(i2c_driver_install(BBOT_I2C_PORT, i2c_conf.mode, 0, 0, 0));
    return ESP_OK;
}

void app_main(void)
{
    imu_state_mutex = xSemaphoreCreateMutex();
    if (imu_state_mutex == NULL)
    {
        ESP_ERROR_CHECK(ESP_ERR_NO_MEM);
    }

    mahony_filter_init(&imu_state.filter, BBOT_FILTER_FRAC_BITS);
    mahony_filter_set_gains(&imu_state.filter, BBOT_FILTER_KP, BBOT_FILTER_KI);

    ESP_ERROR_CHECK(init_motors());
    ESP_ERROR_CHECK(init_i2c());
    ESP_ERROR_CHECK(imu_init(&imu, BBOT_I2C_PORT, BBOT_DRDY_INPUT_PIN, &imu_queue));
    if (xTaskCreate(
            imu_process_task,
            "imu_process",
            BBOT_PROCESS_TASK_STACK_SIZE,
            imu_queue,
            BBOT_PROCESS_TASK_PRIORITY,
            NULL) != pdPASS)
    {
        ESP_ERROR_CHECK(ESP_ERR_NO_MEM);
    }

    while (1)
    {
        bmi160_value_t sample;
        int32_t q0;
        int32_t q1;
        int32_t q2;
        int32_t q3;
        int32_t yaw;
        int32_t pitch;
        int32_t roll;
        bool has_sample;
        bool has_orientation;

        if (xSemaphoreTake(imu_state_mutex, pdMS_TO_TICKS(50)) == pdTRUE)
        {
            sample = imu_state.sample;
            mahony_filter_get_quaternion(&imu_state.filter, &q0, &q1, &q2, &q3);
            yaw = imu_state.yaw;
            pitch = imu_state.pitch;
            roll = imu_state.roll;
            has_sample = imu_state.has_sample;
            has_orientation = imu_state.has_orientation;
            xSemaphoreGive(imu_state_mutex);
        }
        else
        {
            vTaskDelay(pdMS_TO_TICKS(BBOT_PRINT_PERIOD_MS));
            continue;
        }

        if (has_sample && has_orientation)
        {
            ESP_LOGI(
                "[<IMU>]",
                "%" PRIu32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32 " %" PRId32,
                sample.time,
                sample.acc.x,
                sample.acc.y,
                sample.acc.z,
                sample.gyr.x,
                sample.gyr.y,
                sample.gyr.z,
                q0,
                q1,
                q2,
                q3,
                yaw,
                pitch,
                roll);
        }

        vTaskDelay(pdMS_TO_TICKS(BBOT_PRINT_PERIOD_MS));
    }
}