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

#include "adxl345.h"
#include "driver/gpio.h"
#include "driver/i2c_master.h"
#include "esp_err.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "l3g4200d.h"

// Seeed Studio XIAO ESP32-C3: D4/SDA = GPIO6, D5/SCL = GPIO7.
#define TRIKKE_I2C_PORT       I2C_NUM_0
#define TRIKKE_I2C_SDA_GPIO   GPIO_NUM_6
#define TRIKKE_I2C_SCL_GPIO   GPIO_NUM_7
#define TRIKKE_I2C_FREQ_HZ    400000
#define TRIKKE_SAMPLE_RATE_HZ 100
#define TRIKKE_SAMPLE_TICKS   pdMS_TO_TICKS(1000 / TRIKKE_SAMPLE_RATE_HZ)

static const char *TAG = "trikke";

typedef struct {
    int32_t x;
    int32_t y;
    int32_t z;
} trikke_axes_sample_t;

static trikke_axes_sample_t map_accel_to_enclosure(const adxl345_sample_t *native)
{
    // Enclosure frame: +X right, +Y toward the top, +Z toward the cover.
    // Mounted ADXL345: native +Y right, native +X down, native +Z toward cover.
    return (trikke_axes_sample_t) {
        .x = native->y,
        .y = -(int32_t)native->x,
        .z = native->z,
    };
}

static trikke_axes_sample_t map_gyro_to_enclosure(const l3g4200d_sample_t *native)
{
    // The mounted L3G4200D axes already match the enclosure frame.
    return (trikke_axes_sample_t) {
        .x = native->x,
        .y = native->y,
        .z = native->z,
    };
}

static esp_err_t init_i2c(i2c_master_bus_handle_t *bus)
{
    const i2c_master_bus_config_t config = {
        .i2c_port = TRIKKE_I2C_PORT,
        .sda_io_num = TRIKKE_I2C_SDA_GPIO,
        .scl_io_num = TRIKKE_I2C_SCL_GPIO,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };

    return i2c_new_master_bus(&config, bus);
}

void app_main(void)
{
    // Emit each CSV record immediately while testing over USB.
    setvbuf(stdout, NULL, _IOLBF, 0);

    ESP_LOGI(TAG, "Trikke motion telemetry prototype v0");
    ESP_LOGI(TAG, "I2C: SDA=GPIO%d, SCL=GPIO%d, clock=%d Hz",
             TRIKKE_I2C_SDA_GPIO, TRIKKE_I2C_SCL_GPIO, TRIKKE_I2C_FREQ_HZ);

    i2c_master_bus_handle_t bus = NULL;
    esp_err_t err = init_i2c(&bus);
    if (err != ESP_OK) {
        ESP_LOGE(TAG, "I2C initialization failed: %s", esp_err_to_name(err));
        return;
    }

    adxl345_t accelerometer = {0};
    err = adxl345_init(&accelerometer, bus, TRIKKE_I2C_FREQ_HZ);
    if (err != ESP_OK) {
        ESP_LOGE(TAG,
                 "ADXL345 not found at 0x53 or 0x1D (expected DEVID 0xE5): %s",
                 esp_err_to_name(err));
        i2c_del_master_bus(bus);
        return;
    }
    ESP_LOGI(TAG, "ADXL345 detected at 0x%02X; 100 Hz, +/-8 g, full resolution",
             adxl345_address(&accelerometer));

    l3g4200d_t gyroscope = {0};
    err = l3g4200d_init(&gyroscope, bus, TRIKKE_I2C_FREQ_HZ);
    if (err != ESP_OK) {
        ESP_LOGE(TAG,
                 "L3G4200D not found at 0x69 or 0x68 (expected WHO_AM_I 0xD3): %s",
                 esp_err_to_name(err));
        adxl345_deinit(&accelerometer);
        i2c_del_master_bus(bus);
        return;
    }
    ESP_LOGI(TAG, "L3G4200D detected at 0x%02X; 100 Hz, 25 Hz BW, +/-500 dps",
             l3g4200d_address(&gyroscope));

    // Discard the gyroscope's visible startup transient before beginning the stream.
    vTaskDelay(pdMS_TO_TICKS(500));

    printf("# format=trikke_freshness_validation_v2\n");
    printf("# nominal_accel_scale_g_per_lsb=0.0039,"
           "nominal_gyro_scale_dps_per_lsb=0.0175\n");
    printf("# enclosure_axes=+x:right,+y:top,+z:toward_cover\n");
    printf("# mapping=accel(x,y,z)=(native_y,-native_x,native_z);"
           "gyro(x,y,z)=(native_x,native_y,native_z)\n");
    printf("# status_masks=accel_data_ready:0x80,accel_overrun:0x01,"
           "gyro_data_ready:0x08,gyro_overrun:0x80\n");
    printf("sequence,poll_timestamp_us,accel_x_raw,accel_y_raw,accel_z_raw,"
           "gyro_x_raw,gyro_y_raw,gyro_z_raw,"
           "accel_native_x_raw,accel_native_y_raw,accel_native_z_raw,"
           "gyro_native_x_raw,gyro_native_y_raw,gyro_native_z_raw,"
           "accel_int_source,gyro_status,loop_overrun_count\n");

    uint32_t sequence = 0;
    uint32_t read_error_count = 0;
    uint32_t loop_overrun_count = 0;
    TickType_t last_wake = xTaskGetTickCount();

    while (true) {
        adxl345_sample_t accel = {0};
        l3g4200d_sample_t gyro = {0};
        uint8_t accel_int_source = 0;
        uint8_t gyro_status = 0;
        const int64_t timestamp_us = esp_timer_get_time();

        const esp_err_t accel_err = adxl345_read_raw(&accelerometer, &accel,
                                                     &accel_int_source);
        const esp_err_t gyro_err = l3g4200d_read_raw(&gyroscope, &gyro, &gyro_status);

        if (accel_err == ESP_OK && gyro_err == ESP_OK) {
            const trikke_axes_sample_t enclosure_accel = map_accel_to_enclosure(&accel);
            const trikke_axes_sample_t enclosure_gyro = map_gyro_to_enclosure(&gyro);

            printf("%" PRIu32 ",%" PRId64 ",%" PRId32 ",%" PRId32 ",%" PRId32
                   ",%" PRId32 ",%" PRId32 ",%" PRId32
                   ",%" PRId16 ",%" PRId16 ",%" PRId16
                   ",%" PRId16 ",%" PRId16 ",%" PRId16
                   ",%" PRIu8 ",%" PRIu8 ",%" PRIu32 "\n",
                   sequence, timestamp_us,
                   enclosure_accel.x, enclosure_accel.y, enclosure_accel.z,
                   enclosure_gyro.x, enclosure_gyro.y, enclosure_gyro.z,
                   accel.x, accel.y, accel.z,
                   gyro.x, gyro.y, gyro.z,
                   accel_int_source, gyro_status, loop_overrun_count);
        } else {
            ++read_error_count;
            ESP_LOGE(TAG,
                     "sample %" PRIu32 " read failed (accel=%s, gyro=%s, total_errors=%" PRIu32 ")",
                     sequence, esp_err_to_name(accel_err), esp_err_to_name(gyro_err),
                     read_error_count);
        }

        ++sequence;
        if (xTaskDelayUntil(&last_wake, TRIKKE_SAMPLE_TICKS) == pdFALSE) {
            // Do not issue a burst of back-to-back samples after a stalled output path.
            ++loop_overrun_count;
            last_wake = xTaskGetTickCount();
        }
    }
}