/*
 * 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 "esp_heap_caps.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 const char *TAG = "main";

/*********************************
 * STATIC FUNCTION DECLARATIONS
 ********************************/

static void print_heap_info(const char* tag) {
    size_t free_heap = esp_get_free_heap_size();
    size_t min_free_heap = esp_get_minimum_free_heap_size();
    size_t largest_block = heap_caps_get_largest_free_block(MALLOC_CAP_DEFAULT);
    
    ESP_LOGI(TAG, "[%s] Free heap: %zu bytes, Min free: %zu bytes, Largest block: %zu bytes", 
             tag, free_heap, min_free_heap, largest_block);
}
typedef struct {
    float alpha;        // smoothing factor, 0<alpha<1
    float prev_output;  // y[n-1]
} LowPassFilter;

#define ALPHA_MIN   0.2f
#define ALPHA_MAX   0.4f

// Clamp function to bound value between min and max
double clamp(double value, double min, double max) {
    if (value < min) return min;
    if (value > max) return max;
    return value;
}

// Compute alpha using linear ramp
float compute_alpha_linear(double x, float alpha_min, float alpha_max, float threshold) {
    float abs_x = fabs(x);
    float t = clamp(abs_x / threshold, 0.0, 1.0);  // Normalize |x| to [0,1]
    return alpha_min + (alpha_max - alpha_min) * t;
}

// Compute sigmoid-based dynamic alpha
float compute_alpha_sigmoid(float x, float alpha_min, float alpha_max, float k, float midpoint) {
    float abs_x = fabs(x);
    float exponent = -k * (abs_x - midpoint);
    float sigmoid = 1.0 / (1.0 + exp(exponent));
    return alpha_min + (alpha_max - alpha_min) * sigmoid;
}

// 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 alpha = compute_alpha_sigmoid(input, ALPHA_MIN, ALPHA_MAX, 1.0f, 0.0f);
    float alpha = compute_alpha_linear(input, ALPHA_MIN, ALPHA_MAX, 2.0f);
    float out = alpha * input + (1.0f - alpha) * f->prev_output;
    //float out = f->alpha * input + (1.0f - f->alpha) * f->prev_output;
    f->prev_output = out;
    return out;
}


/*********************************
 * STATIC VARIABLE DEFINITIONS
 ********************************/

/*********************************
 * 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)
{
    print_heap_info("STARTUP");
    
    /* 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);
    print_heap_info("POST_NVS");

    init_gpio();
    print_heap_info("POST_GPIO");

    system_init();
    print_heap_info("POST_SYSTEM");

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

    // Create IMU task
    TaskHandle_t h = xTaskCreate(imu_task, "imu_task", 2048, NULL, 5, NULL);
    print_heap_info("POST_IMU_TASK");
    
    bt_app_init();
    print_heap_info("POST_BLUETOOTH");

    gui_start();
    print_heap_info("POST_GUI");
    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)
    {
        system_processNvsRequests();
        vTaskDelay(pdMS_TO_TICKS(10));
    }    
#endif
}