#include <stdio.h>

#include "driver/gpio.h"
#include "esp_err.h"
#include "freertos/FreeRTOS.h"
#include "freertos/portmacro.h"
#include "freertos/projdefs.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_log.h"
#include "driver/pulse_cnt.h"
#include "driver/ledc.h"
#include "hal/gpio_types.h"
#include "hal/ledc_types.h"
#include "hal/pcnt_types.h"

/* Pinouts */
#define PCNT_A_PIN 4 /* Rotary Encoder A Pin */
#define PCNT_B_PIN 5 /* Rotary Encoder B Pin */
#define GPIO_BTN_PIN 6 /* Rotary Encoder switch */
#define LEDC_R_PIN 7 /* Red LED */
#define LEDC_G_PIN 15 /* Green LED */
#define LEDC_B_PIN 16 /* Blue LED */

/* Pulse Count Settings */
#define PCNT_HIGH_LIMIT 2     /* 100 total steps */
#define PCNT_LOW_LIMIT -2
pcnt_unit_handle_t pcnt_unit;
static QueueHandle_t pcnt_queue = NULL;

/* Button Settings */
static QueueHandle_t control_event_queue = NULL;
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
static const uint16_t DEBOUNCE_TIMEOUT = 20; /* Polling period lenght for debounce */
static uint64_t debounce_time = 0; /* system time of last intterupt */
static uint8_t btn_state = 1;      /* state of gpio last recorded */

static const char *TAG = "RE Demo"; /* logging tag */

static void IRAM_ATTR gpio_isr_handler(void *arg) {
  uint32_t gpio_num = (uint32_t)arg;
  portENTER_CRITICAL_ISR(&mux);
  debounce_time = xTaskGetTickCount();  /* set time of interrupt */
  btn_state = gpio_get_level(GPIO_BTN_PIN); /* set state */
  portEXIT_CRITICAL_ISR(&mux);
  xQueueSendFromISR(control_event_queue, &gpio_num, NULL);
}

static void control_event_handler(void *arg) {
  uint8_t last_btn_state;
  uint64_t last_debounce_time;
  ESP_LOGI(TAG, "Starting Control Event Handler");
  while (1) {
    /* save these so we can make comparisons */
    portENTER_CRITICAL_ISR(&mux);
    last_btn_state = btn_state;
    last_debounce_time = debounce_time;
    portEXIT_CRITICAL_ISR(&mux);
    uint8_t current_state = gpio_get_level(GPIO_BTN_PIN);
    uint32_t gpio_num = GPIO_BTN_PIN;
    if (xQueueReceive(control_event_queue, &gpio_num, portMAX_DELAY)) {
      /* Check if interrupt triggered stays the same for long enough */
      if ((current_state == last_btn_state) &&
          (xTaskGetTickCount() - last_debounce_time > DEBOUNCE_TIMEOUT)) {
        ESP_LOGI(TAG, "Switch pushed!");
      }
    }
  }
}

/* Initialize encoder switch */
static void btn_init(void) {
    ESP_LOGI(TAG, "Initializing Encoder Switch");

    /* Setup gpio control event queue */
    control_event_queue = xQueueCreate(10, sizeof(uint32_t));
    /* install gpio isr hservice */
    gpio_install_isr_service(0);
    /* Setup Encoder button */
    gpio_reset_pin(GPIO_BTN_PIN);
    gpio_pullup_dis(GPIO_BTN_PIN);
    gpio_pulldown_en(GPIO_BTN_PIN);
    gpio_set_direction(GPIO_BTN_PIN, GPIO_MODE_INPUT);
    gpio_set_intr_type(GPIO_BTN_PIN, GPIO_INTR_POSEDGE);
    /* Hook in handler for button */
    gpio_isr_handler_add(GPIO_BTN_PIN, gpio_isr_handler, (void *)GPIO_BTN_PIN);
}

/* Initialize LEDC for RGB LEDS in encoder */
static void ledc_init(void) {
  ESP_LOGI(TAG, "Initializing LEDC Driver");
  /* Setup 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 = 5000,
    .clk_cfg = LEDC_AUTO_CLK
  };
  ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

  /* Setup Clock LED */
  ledc_channel_config_t ledc_R_channel = {
    .channel = LEDC_CHANNEL_0,
    .gpio_num = LEDC_R_PIN,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .timer_sel = LEDC_TIMER_0,
    .intr_type = LEDC_INTR_DISABLE,
    .duty = 0,
    .hpoint = 0
  };
  ESP_ERROR_CHECK(ledc_channel_config(&ledc_R_channel));

  /* Setup Gate LED */
  ledc_channel_config_t ledc_G_channel = {
    .channel = LEDC_CHANNEL_1,
    .gpio_num = LEDC_G_PIN,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .timer_sel = LEDC_TIMER_0,
    .intr_type = LEDC_INTR_DISABLE,
    .duty = 0,
    .hpoint = 0
  };
  ESP_ERROR_CHECK(ledc_channel_config(&ledc_G_channel));

  /* Setup Gate LED */
  ledc_channel_config_t ledc_B_channel = {
    .channel = LEDC_CHANNEL_2,
    .gpio_num = LEDC_B_PIN,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .timer_sel = LEDC_TIMER_0,
    .intr_type = LEDC_INTR_DISABLE,
    .duty = 0,
    .hpoint = 0
  };
  ESP_ERROR_CHECK(ledc_channel_config(&ledc_B_channel));
}

/* Pulse Counter Callback */
static bool pcnt_cb(pcnt_unit_handle_t unit, const pcnt_watch_event_data_t *edata, void *user_ctx) {
  BaseType_t high_task_wakeup;
  /* Put the watchpoint value in a queue to be handled later */
  xQueueSendFromISR(pcnt_queue, &(edata->watch_point_value), &high_task_wakeup);
  return (high_task_wakeup == pdTRUE);
}

/* Initialize the pulse counter peripheral */
static void pcnt_init() {
  /* Configure the PCNT unit and get a handle */
  ESP_LOGI(TAG, "Setting up Pulse Counter Unit");
  pcnt_unit_config_t pcnt_unit_config = {
    .high_limit = PCNT_HIGH_LIMIT,
    .low_limit = PCNT_LOW_LIMIT
  };
  ESP_ERROR_CHECK(pcnt_new_unit(&pcnt_unit_config, &pcnt_unit));

  /* Setup glitch filter */
  pcnt_glitch_filter_config_t filter_config = {
    .max_glitch_ns = 1000
  };
  ESP_ERROR_CHECK(pcnt_unit_set_glitch_filter(pcnt_unit, &filter_config));

  /* Configure PCNT A & B channels and get handles */
  ESP_LOGI(TAG, "Setting up Pulse Counter Channels");
  pcnt_chan_config_t pcnt_chan_a_config = {
    .edge_gpio_num = PCNT_A_PIN,
    .level_gpio_num = PCNT_B_PIN
  };
  pcnt_channel_handle_t pcnt_chan_a = NULL;
  ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &pcnt_chan_a_config, &pcnt_chan_a));

  pcnt_chan_config_t pcnt_chan_b_config = {
    .edge_gpio_num = PCNT_B_PIN,
    .level_gpio_num = PCNT_A_PIN
  };
  pcnt_channel_handle_t pcnt_chan_b = NULL;
  ESP_ERROR_CHECK(pcnt_new_channel(pcnt_unit, &pcnt_chan_b_config, &pcnt_chan_b));

  /* Configure edge/level actions on channels */
  ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_a, PCNT_CHANNEL_EDGE_ACTION_DECREASE, PCNT_CHANNEL_EDGE_ACTION_INCREASE));
  ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_a, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));
  ESP_ERROR_CHECK(pcnt_channel_set_edge_action(pcnt_chan_b, PCNT_CHANNEL_EDGE_ACTION_INCREASE, PCNT_CHANNEL_EDGE_ACTION_DECREASE));
  ESP_ERROR_CHECK(pcnt_channel_set_level_action(pcnt_chan_b, PCNT_CHANNEL_LEVEL_ACTION_KEEP, PCNT_CHANNEL_LEVEL_ACTION_INVERSE));

  /* Install watch points */
  ESP_LOGI(TAG, "Installing watch points and callback handler");
  int watch_points[] = {-2, 2}; /* This encoder has detents which change in steps of 2 */
  for (size_t i = 0; i < sizeof(watch_points) / sizeof(watch_points[0]); i++) {
    ESP_ERROR_CHECK(pcnt_unit_add_watch_point(pcnt_unit, watch_points[i]));
  }

  /* Initialize pcnt event queue */
  pcnt_queue = xQueueCreate(10, sizeof(int));

  /* Install callback */
  pcnt_event_callbacks_t cbs = {
    .on_reach = pcnt_cb
  };
  ESP_ERROR_CHECK(pcnt_unit_register_event_callbacks(pcnt_unit, &cbs, NULL));

  /* Enable, clear, and start pcnt Unit */
  ESP_LOGI(TAG, "Starting Pulse Counter");
  ESP_ERROR_CHECK(pcnt_unit_enable(pcnt_unit));
  ESP_ERROR_CHECK(pcnt_unit_clear_count(pcnt_unit));
  ESP_ERROR_CHECK(pcnt_unit_start(pcnt_unit));
}

/* Main Loop */
void app_main(void){

  /* Initialize pulse counter */
  pcnt_init();
  /* Initialize led driver */
  ledc_init();
  /* Initialize gpio switch */
  btn_init();

  /* Start task to handle control events */
  xTaskCreate(control_event_handler, "control event handler", 2048, NULL, 10, NULL);

  /* ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, 4095)); */
  /* ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); */

  /* Log counter value */
  int event_count = 0;

  while (1) {
    if (xQueueReceive(pcnt_queue, &event_count, portMAX_DELAY)) {
      ESP_LOGI(TAG, "Watch point event, count: %d", event_count);
    }
  }
}