/*
 * SPDX-FileCopyrightText: 2021-2024 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: Unlicense OR CC0-1.0
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <inttypes.h>
#include "math.h"

#include "esp_system.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/timers.h"
#include "driver/gpio.h"
#include "nvs.h"
#include "nvs_flash.h"


#include "bt_app.h"
#include "lsm6dsv.h"
#include "gui.h"
#include "gpio.h"
#include "keypad.h"
#include "system.h"




/*********************************
 * STATIC FUNCTION DECLARATIONS
 ********************************/
typedef struct {
    float alpha;        // smoothing factor, 0<alpha<1
    float prev_output;  // y[n-1]
} LowPassFilter;

// Initialize the filter.  alpha = smoothing factor.
//   e.g. alpha = dt/(RC+dt) if you derive from cutoff freq & sample time.
//   init_output can be 0 or your first sample to avoid startup glitch.
static inline void LPF_Init(LowPassFilter *f, float alpha, float init_output) {
    f->alpha       = alpha;
    f->prev_output = init_output;
}

// Run one input sample through the filter.
// Returns y[n] = alpha * x[n] + (1-alpha) * y[n-1].
static inline float LPF_Update(LowPassFilter *f, float input) {
    float out = f->alpha * input + (1.0f - f->alpha) * f->prev_output;
    f->prev_output = out;
    return out;
}


/*********************************
 * STATIC VARIABLE DEFINITIONS
 ********************************/
static const char *TAG = "main";

/*********************************
 * STATIC FUNCTION DEFINITIONS
 ********************************/

static void init_gpio(void)
{
    gpio_config_t io_conf;


    // Configure output GPIO
    io_conf.pin_bit_mask = (1ULL << PIN_NUM_LED_1) | (1ULL << PIN_NUM_LED_2);
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    gpio_config(&io_conf);



    
#if 1
    // Configure output power signal
    gpio_set_level(PIN_NUM_nON, 1);
    
    io_conf.pin_bit_mask = (1ULL << PIN_NUM_nON);
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    gpio_config(&io_conf);
#endif
    


    // Configure LCD Backlight GPIO
    io_conf.pin_bit_mask =  (1ULL << PIN_NUM_BK_LIGHT);
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    gpio_config(&io_conf);
}




// IMU task to read sensor data
static void imu_task(void *pvParameters) {
    lsm6dsv_data_t imu_data;
    char data_str[128];

    float roll, pitch, yaw;
    lsm6dsv_fifo_t fifo;

    ImuData_t imu;

    LowPassFilter lpf;
    LPF_Init(&lpf, FILTER_COEFF, 0);

    while (1) {

       // uint8_t samples = lsm6dsv_fifo_ready();

        //ESP_LOGI(TAG, "Samples: %d", samples);
        // while (samples)
        // {
        //     lsm6dsv_fifo_read(&fifo);

            

        //     // snprintf(data_str, sizeof(data_str),
        //     //     "tag: %d, count: %d, (%d, %d, %d)",
        //     //     fifo.tag, fifo.count, x, y, z);

        //     // ESP_LOGI(TAG, "%s", data_str);

        //     samples--;
        // }

        // lsm6dsv_getAttitude(&fifo, &roll, &pitch, &yaw);

        // snprintf(data_str, sizeof(data_str),
        //     "r: %0.3f, p: %0.3f, y: %0.3f",
        //     roll, pitch, yaw);

        // ESP_LOGI(TAG, "%s", data_str);

    float xz;
    float yz;
    float xy;

#if 1
        if (lsm6dsv_read_data(&imu_data) == ESP_OK) 
        {
            // snprintf(data_str, sizeof(data_str),
            //         "Acc: %.2f, %.2f, %.2f; "
            //         "Gyro: %.2f, %.2f, %.2f (%d)\n",
            //         imu_data.acc_x, imu_data.acc_y, imu_data.acc_z,
            //         imu_data.gyro_x, imu_data.gyro_y, imu_data.gyro_z, lsm6dsv_fifo_ready());

            

            
#if 1            
            imu.raw[ANGLE_XZ] = atan2f((float)imu_data.acc_z, (float)imu_data.acc_x) * 180 / M_PI;
            imu.raw[ANGLE_YZ] = atan2f((float)imu_data.acc_z, (float)imu_data.acc_y) * 180 / M_PI;
            imu.raw[ANGLE_XY] = atan2f((float)imu_data.acc_x, (float)imu_data.acc_y) * 180 / M_PI;

            
            float angle;


            angle = system_getAngle(); 

            if (angle > MAX_INDICATION_ANGLE)
            {
                angle = MAX_INDICATION_ANGLE;
            }
            else
            if (angle < -MAX_INDICATION_ANGLE)
            {
                angle = -MAX_INDICATION_ANGLE;
            }
            
            // low pass filter the angle measurement
            imu.angle = LPF_Update(&lpf, angle);

            //imu.filtered[ANGLE_XY] = LPF_Update(&lpf, imu.raw[ANGLE_XY]);

            system_setImuData(imu);

            // snprintf(data_str, sizeof(data_str),
            //         "(%.2f, %.2f, %.2f)", xy, yz, xy);
#endif                    

#if 0
            snprintf(data_str, sizeof(data_str),
                    "(%.1f, %.1f, %.1f) (%.2f, %.2f, %.2f)", 
                    imu.raw[ANGLE_XZ], imu.raw[ANGLE_YZ], imu.raw[ANGLE_XY],
                    (float)imu_data.acc_x, (float)imu_data.acc_y, (float)imu_data.acc_z);
            ESP_LOGI(TAG, "%s", data_str);
#endif
            // Update label text in LVGL task
            //lv_label_set_text(imu_label, data_str);
            
        }
#endif        
        vTaskDelay(pdMS_TO_TICKS(100)); // Update every 100ms
    }
}

/*********************************
 * MAIN ENTRY POINT
 ********************************/

void app_main(void)
{
    
    /* initialize NVS — it is used to store PHY calibration data */
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    init_gpio();

    system_init();

   

    // Initialize IMU
    ESP_ERROR_CHECK(lsm6dsv_init(22, 21));  // SCL = IO14, SDA = IO15



    // Create IMU task
    TaskHandle_t h = xTaskCreate(imu_task, "imu_task", 4096, NULL, 5, NULL);
    

    
    bt_app_init();

    gui_start();
    
    gpio_set_level(PIN_NUM_LED_2, 1);


#if 0

    keypad_start();
    QueueHandle_t q = keypad_getQueue();
    uint32_t ev = 0;

    //gpio_set_level(PIN_NUM_LED_1, 1);
    gpio_set_level(PIN_NUM_LED_2, 1);

    // Main loop - LVGL task will run automatically
    while (1) {
        
        if (xQueueReceive(q, &ev, pdMS_TO_TICKS(10)) == pdTRUE) 
        {
            switch (ev) 
            {
                case (KEY_UP << KEY_LONG_PRESS):
                {
                    system_setZeroAngle();
                    break;
                }

                case (KEY_DOWN << KEY_LONG_PRESS):
                {
                    system_clearZeroAngle();
                    break;
                }
            }
            
        }
 
    }
#else
    while (1)
    {
        vTaskDelay(pdMS_TO_TICKS(1000));
    }    
#endif
}