#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "driver/periph_ctrl.h"
#include "driver/timer.h"
#include "driver/ledc.h"
#include "freertos/semphr.h"

#include <string.h>
#include "esp_system.h"
#include "esp_log.h"
#include "esp_console.h"
#include "argtable3/argtable3.h"
#include "nvs.h"
#include "nvs_flash.h"

/**
 * Brief:
 * This test code shows how to configure multiple gpio interrupts
 *
 * GPIO status:
 * GPIO2   : output ( built-in led on Devkit-V1 )
 * GPIO34  : output ( externally pulled up )
 * GPIO35  : output ( externally pulled up )
 *
 * Test:
 * Connect GPIO34 with simple switch and ground
 * Connect GPIO35 with simple switch and ground
 */

#define ESP_INTR_FLAG_DEFAULT 0

#define CALIB_N 64
#define CALIB_PAUSE 50

#define BUILTIN_LED 2
#define GPIO_INPUT_DET_A 25
#define GPIO_INPUT_CAL_A 17
#define GPIO_INPUT_DET_B 26
#define GPIO_INPUT_CAL_B 18
#define GPIO_INPUT_DET_C 27
#define GPIO_INPUT_CAL_C 19

#define TIMER_DIVIDER         2  //  Hardware timer clock divider
#define TIMER_SCALE           (TIMER_BASE_CLK / TIMER_DIVIDER)  // convert counter value to seconds

SemaphoreHandle_t xSemaphore_a = NULL;
SemaphoreHandle_t xSemaphore_b = NULL;
SemaphoreHandle_t xSemaphore_c = NULL;
bool detection_armed = false;

uint64_t a_timestamp_cal = 0ULL;
uint64_t a_timestamp_det = 0ULL;
uint64_t b_timestamp_cal = 0ULL;
uint64_t b_timestamp_det = 0ULL;
uint64_t c_timestamp_cal = 0ULL;
uint64_t c_timestamp_det = 0ULL;

float ab_delta = 0;
float ac_delta = 0;
float ba_delta = 0;
float bc_delta = 0;
float ca_delta = 0;
float cb_delta = 0;

void IRAM_ATTR gpio_a_isr_handler(void* arg){   //++ First ISR
    timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &a_timestamp_det);
    gpio_intr_disable(GPIO_INPUT_DET_A);
    xSemaphoreGiveFromISR(xSemaphore_a, NULL);
}

void IRAM_ATTR gpio_b_isr_handler(void* arg){   //++ Second ISR
    timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &b_timestamp_det);
    gpio_intr_disable(GPIO_INPUT_DET_B);
    xSemaphoreGiveFromISR(xSemaphore_b, NULL);
}

void IRAM_ATTR gpio_c_isr_handler(void* arg) {
    timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &c_timestamp_det);
    gpio_intr_disable(GPIO_INPUT_DET_C);
    xSemaphoreGiveFromISR(xSemaphore_c, NULL);
}

static bool measure_a(uint32_t *p_delta_b, uint32_t *p_delta_c)
{
    bool ab_okay = false;
    bool ac_okay = false;
    uint64_t timestep_cal = 0ULL;
    // reset timestamps
    b_timestamp_det = 0;
    c_timestamp_det = 0;
    timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &timestep_cal);
    
    // pulse the IO
    gpio_intr_enable(GPIO_INPUT_DET_B);
    gpio_intr_enable(GPIO_INPUT_DET_C);
    ledc_set_pin(GPIO_INPUT_CAL_A, LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);

    for (int i = 0; i < 0xFFFF; ++i);
    gpio_set_direction(GPIO_INPUT_CAL_A, GPIO_MODE_INPUT);
    
    // get the a-b delta
    if (b_timestamp_det > 0)
    {
        uint64_t delta = b_timestamp_det - timestep_cal;
        *p_delta_b = delta;
        ab_okay = true;
    }
    // get the a-c delta
    if (c_timestamp_det > 0)
    {
        uint64_t delta = c_timestamp_det - timestep_cal;
        *p_delta_c = delta;
        ac_okay = true;
    }
    return ab_okay && ac_okay;
}

void calibrate_a(float *p_delta_b, float *p_delta_c)
{
    uint32_t delta_b = 0UL;
    uint32_t delta_c = 0UL;
    
    uint32_t sum_b = 0UL;
    uint32_t sum_c = 0UL;
    uint32_t samples = 0UL;
    
    for (int i = 0; i < CALIB_N; ++i)
    {
        bool okay = measure_a(&delta_b, &delta_c);
        if (okay)
        {
            sum_b += delta_b;
            sum_c += delta_c;
            samples++;
        }
        
        vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
    }
    if (samples > 0)
    {
        *p_delta_b = sum_b / samples;
        *p_delta_c = sum_c / samples;
    }
}

static bool measure_b(uint32_t *p_delta_a, uint32_t *p_delta_c)
{
    bool ba_okay = false;
    bool bc_okay = false;
    uint64_t timestep_cal = 0ULL;
    // reset timestamps
    a_timestamp_det = 0;
    c_timestamp_det = 0;
    timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &timestep_cal);
    
    // pulse the IO
    gpio_intr_enable(GPIO_INPUT_DET_A);
    gpio_intr_enable(GPIO_INPUT_DET_C);
    ledc_set_pin(GPIO_INPUT_CAL_B, LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);

    for (int i = 0; i < 0xFFFF; ++i);
    gpio_set_direction(GPIO_INPUT_CAL_B, GPIO_MODE_INPUT);
    
    // get the b-a delta
    if (a_timestamp_det > 0)
    {
        uint64_t delta = a_timestamp_det - timestep_cal;
        *p_delta_a = delta;
        ba_okay = true;
    }
    // get the b-c delta
    if (c_timestamp_det > 0)
    {
        uint64_t delta = c_timestamp_det - timestep_cal;
        *p_delta_c = delta;
        bc_okay = true;
    }
    return ba_okay && bc_okay;
}

void calibrate_b(float *p_delta_a, float *p_delta_c)
{
    uint32_t delta_a = 0UL;
    uint32_t delta_c = 0UL;
    
    uint32_t sum_a = 0UL;
    uint32_t sum_c = 0UL;
    uint32_t samples = 0UL;
    
    for (int i = 0; i < CALIB_N; ++i)
    {
        bool okay = measure_b(&delta_a, &delta_c);
        if (okay)
        {
            sum_a += delta_a;
            sum_c += delta_c;
            samples++;
        }
        
        vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
    }
    if (samples > 0)
    {
        *p_delta_a = sum_a / samples;
        *p_delta_c = sum_c / samples;
    }
}


static bool measure_c(uint32_t *p_delta_a, uint32_t *p_delta_b)
{
    bool ca_okay = false;
    bool cb_okay = false;
    uint64_t timestep_cal = 0ULL;
    // reset timestamps
    a_timestamp_det = 0;
    b_timestamp_det = 0;
    timer_get_counter_value(TIMER_GROUP_0, TIMER_0, &timestep_cal);
    
    // pulse the IO
    gpio_intr_enable(GPIO_INPUT_DET_A);
    gpio_intr_enable(GPIO_INPUT_DET_B);
    ledc_set_pin(GPIO_INPUT_CAL_C, LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0);

    for (int i = 0; i < 0xFFFF; ++i);
    gpio_set_direction(GPIO_INPUT_CAL_C, GPIO_MODE_INPUT);
    
    // get the c-a delta
    if (a_timestamp_det > 0)
    {
        uint64_t delta = a_timestamp_det - timestep_cal;
        *p_delta_a = delta;
        ca_okay = true;
    }
    // get the c-b delta
    if (b_timestamp_det > 0)
    {
        uint64_t delta = b_timestamp_det - timestep_cal;
        *p_delta_b = delta;
        cb_okay = true;
    }
    return ca_okay && cb_okay;
}

void calibrate_c(float *p_delta_a, float *p_delta_b)
{
    uint32_t delta_a = 0UL;
    uint32_t delta_b = 0UL;
    
    uint32_t sum_a = 0UL;
    uint32_t sum_b = 0UL;
    uint32_t samples = 0UL;
    
    for (int i = 0; i < CALIB_N; ++i)
    {
        bool okay = measure_c(&delta_a, &delta_b);
        if (okay)
        {
            sum_a += delta_a;
            sum_b += delta_b;
            samples++;
        }
        
        vTaskDelay(CALIB_PAUSE/portTICK_PERIOD_MS);
    }
    if (samples > 0)
    {
        *p_delta_a = sum_a / samples;
        *p_delta_b = sum_b / samples;
    }
}

static struct {
    struct arg_end *end;
} calib_args;

static int calibrate(int argc, char **argv)
{
    
    ab_delta = 0;
    ac_delta = 0;
    ba_delta = 0;
    bc_delta = 0;
    ca_delta = 0;
    cb_delta = 0;
    //for(;;)
    calibrate_a(&ab_delta, &ac_delta);
    calibrate_b(&ba_delta, &bc_delta);
    calibrate_c(&ca_delta, &cb_delta);
    printf("AB: %f\n", ab_delta);
    printf("AC: %f\n", ac_delta);
    printf("BA: %f\n", ba_delta);
    printf("BC: %f\n", bc_delta);
    printf("CA: %f\n", ca_delta);
    printf("CB: %f\n", cb_delta);

    return 0;
}

static void example_tg0_timer_init(int timer_idx, bool auto_reload)
{
    /* Select and initialize basic parameters of the timer */
    timer_config_t config = {
        .divider = TIMER_DIVIDER,
        .counter_dir = TIMER_COUNT_UP,
        .counter_en = TIMER_PAUSE,
        .alarm_en = TIMER_ALARM_EN,
        .auto_reload = auto_reload,
    }; // default clock source is APB
    timer_init(TIMER_GROUP_0, timer_idx, &config);

    /* Timer's counter will initially start from value below.
       Also, if auto_reload is set, this value will be automatically reload on alarm */
    timer_set_counter_value(TIMER_GROUP_0, timer_idx, 0x00000000ULL);
    timer_start(TIMER_GROUP_0, timer_idx);
}

static void initialize_nvs(void) {
    esp_err_t err = nvs_flash_init();
    if (err == ESP_ERR_NVS_NO_FREE_PAGES || err == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        err = nvs_flash_init();
    }
    ESP_ERROR_CHECK(err);
}

static struct {
    struct arg_end *end;
} arm_args;

static int arm_command(int argc, char **argv)
{
    while( xSemaphoreTake( xSemaphore_a, 0 ) == pdPASS );
    while( xSemaphoreTake( xSemaphore_b, 0 ) == pdPASS );
    while( xSemaphoreTake( xSemaphore_c, 0 ) == pdPASS );
    a_timestamp_det = 0ULL;
    b_timestamp_det = 0ULL;
    c_timestamp_det = 0ULL;
    gpio_intr_enable(GPIO_INPUT_DET_A);
    gpio_intr_enable(GPIO_INPUT_DET_B);
    gpio_intr_enable(GPIO_INPUT_DET_C);
    detection_armed = true;
    return 0;
}

void detect_task(void* arg)
{
    for(;;)
    {
        if (detection_armed)
        {
            while( xSemaphoreTake( xSemaphore_a, portMAX_DELAY ) != pdPASS );
            while( xSemaphoreTake( xSemaphore_b, portMAX_DELAY ) != pdPASS );
            while( xSemaphoreTake( xSemaphore_c, portMAX_DELAY ) != pdPASS );
            printf("A: %ju\n", a_timestamp_det);
            printf("B: %ju\n", b_timestamp_det);
            printf("C: %ju\n", c_timestamp_det);
            detection_armed = false;
        }
        else
        {
            vTaskDelay( 250 / portTICK_PERIOD_MS);
        }
    }
}

void app_main(void)
{
    gpio_reset_pin(GPIO_INPUT_DET_A);
    gpio_reset_pin(GPIO_INPUT_DET_B);
    gpio_reset_pin(GPIO_INPUT_DET_C);
    


    //PWM
    // Prepare and then apply the LEDC PWM timer configuration
    ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_LOW_SPEED_MODE,
        .timer_num        = LEDC_TIMER_0,
        .duty_resolution  = LEDC_TIMER_13_BIT,
        .freq_hz          = 3200,  // Set output frequency at 5 kHz
        .clk_cfg          = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    // Prepare and then apply the LEDC PWM channel configuration
    ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_LOW_SPEED_MODE,
        .channel        = LEDC_CHANNEL_0,
        .timer_sel      = LEDC_TIMER_0,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = GPIO_INPUT_CAL_A,
        .duty           = 4096, // Set duty to 50%
        .hpoint         = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
    gpio_set_direction(GPIO_INPUT_CAL_A, GPIO_MODE_INPUT);

    /* Set the GPIO & LED direction */
    
    gpio_set_direction(GPIO_INPUT_DET_A, GPIO_MODE_INPUT);
    gpio_set_pull_mode(GPIO_INPUT_DET_A, GPIO_PULLDOWN_ONLY); //GPIO_PULLUP_ONLY
    
    gpio_set_direction(GPIO_INPUT_DET_B, GPIO_MODE_INPUT);
    gpio_set_pull_mode(GPIO_INPUT_DET_B, GPIO_PULLDOWN_ONLY);
    
    gpio_set_direction(GPIO_INPUT_DET_C, GPIO_MODE_INPUT);
    gpio_set_pull_mode(GPIO_INPUT_DET_C, GPIO_PULLDOWN_ONLY);
    
    gpio_set_direction(GPIO_INPUT_CAL_A, GPIO_MODE_INPUT);
    gpio_set_pull_mode(GPIO_INPUT_CAL_A, GPIO_FLOATING);
    gpio_set_direction(GPIO_INPUT_CAL_B, GPIO_MODE_INPUT);
    gpio_set_pull_mode(GPIO_INPUT_CAL_B, GPIO_FLOATING);
    gpio_set_direction(GPIO_INPUT_CAL_C, GPIO_MODE_INPUT);
    gpio_set_pull_mode(GPIO_INPUT_CAL_C, GPIO_FLOATING);
    
    example_tg0_timer_init(TIMER_0, false);

    //enable interrupt on anyedge for button pin
    gpio_set_intr_type(GPIO_INPUT_DET_A, GPIO_INTR_POSEDGE);
    gpio_set_intr_type(GPIO_INPUT_DET_B, GPIO_INTR_POSEDGE);
    gpio_set_intr_type(GPIO_INPUT_DET_C, GPIO_INTR_POSEDGE);

    //install ISR service with default configuration
    gpio_install_isr_service(ESP_INTR_FLAG_DEFAULT);

    //attach the interrupt service routine 
    gpio_isr_handler_add(GPIO_INPUT_DET_A, gpio_a_isr_handler, NULL);
    gpio_isr_handler_add(GPIO_INPUT_DET_B, gpio_b_isr_handler, NULL);
    gpio_isr_handler_add(GPIO_INPUT_DET_C, gpio_c_isr_handler, NULL);
    gpio_intr_disable(GPIO_INPUT_DET_A);
    gpio_intr_disable(GPIO_INPUT_DET_B);
    gpio_intr_disable(GPIO_INPUT_DET_C);
    
    //semaphores
    xSemaphore_a = xSemaphoreCreateBinary();
    xSemaphore_b = xSemaphoreCreateBinary();
    xSemaphore_c = xSemaphoreCreateBinary();
    
    xTaskCreate(detect_task, "detect_task", 2048, NULL, configMAX_PRIORITIES-1, NULL);
    
    esp_console_repl_t *repl = NULL;
    esp_console_repl_config_t repl_config = ESP_CONSOLE_REPL_CONFIG_DEFAULT();
    /* Prompt to be printed before each line.
     * This can be customized, made dynamic, etc.
     */
    repl_config.prompt = ">";
    repl_config.max_cmdline_length = 255;

    initialize_nvs();
    
    esp_console_dev_uart_config_t hw_config = ESP_CONSOLE_DEV_UART_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_console_new_repl_uart(&hw_config, &repl_config, &repl));
    
    calib_args.end = arg_end(1);
    esp_console_cmd_t calib_cmd = {
        .command = "c",
        .help = "Perform calibration",
        .func = &calibrate,
        .argtable = &calib_args
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&calib_cmd));
    
    arm_args.end = arg_end(1);
    esp_console_cmd_t arm_cmd = {
        .command = "a",
        .help = "Arm detections",
        .func = &arm_command,
        .argtable = &arm_args
    };
    ESP_ERROR_CHECK(esp_console_cmd_register(&arm_cmd));
    
    // Start the REPL
    ESP_ERROR_CHECK(esp_console_start_repl(repl));
}