#include "encoder.hpp"
#include "driver/gpio.h"
#include "esp_log.h"
#include "soc/gpio_struct.h"
#include "servo.hpp"

static const char *TAG = "ENCODER";

// Constructor
Encoder::Encoder(gpio_num_t pinA, gpio_num_t pinB) 
    : pin_a(pinA), pin_b(pinB), count(0), 
      last_state_a(0), last_state_b(0), last_count_base(0),
      watchdog_handle(nullptr) {}

// Static ISR - receives Encoder instance via arg
void IRAM_ATTR Encoder::isr_handler(void* arg)
{
  Encoder* encoder = static_cast<Encoder*>(arg);
  
  // Read GPIO levels directly from hardware
  uint32_t gpio_levels = GPIO.in.val;
  uint8_t current_a = (gpio_levels >> encoder->pin_a) & 0x1;
  uint8_t current_b = (gpio_levels >> encoder->pin_b) & 0x1;

  // Quadrature decoding logic
  if (current_a != encoder->last_state_a) {
    if (!current_a) { 
      if (current_b) encoder->last_count_base++;
      else encoder->last_count_base--;
    }
    else { 
      if (current_b) encoder->last_count_base--;
      else encoder->last_count_base++;
    }
  }
  else if (current_b != encoder->last_state_b) {
    if (!current_b) { 
      if (current_a) encoder->last_count_base--;
      else encoder->last_count_base++;
    }
    else { 
      if (current_a) encoder->last_count_base++;
      else encoder->last_count_base--;
    }
  }
  
  // Accumulate to full detent count
  if (encoder->last_count_base > 3) {
    encoder->count += 1;
    encoder->last_count_base -= 4;
    if (calibListen) servoCalibListen();
    if (encoder->feedWDog) {
      esp_timer_stop(encoder->watchdog_handle);
      esp_timer_start_once(encoder->watchdog_handle, 500000);
      debugLEDTgl();
    }
    if (encoder->wandListen) servoWandListen();
    if (encoder->serverListen) servoServerListen();
  }
  else if (encoder->last_count_base < 0) {
    encoder->count -= 1;
    encoder->last_count_base += 4;
    if (calibListen) servoCalibListen();
    if (encoder->feedWDog) {
      esp_timer_stop(encoder->watchdog_handle);
      esp_timer_start_once(encoder->watchdog_handle, 500000);
      debugLEDTgl();
    }
    if (encoder->wandListen) servoWandListen();
    if (encoder->serverListen) servoServerListen();
  }
  
  encoder->last_state_a = current_a;
  encoder->last_state_b = current_b;
}

void Encoder::init()
{
    gpio_config_t io_conf = {};
    io_conf.intr_type = GPIO_INTR_ANYEDGE;
    io_conf.pin_bit_mask = (1ULL << pin_a) | (1ULL << pin_b);
    io_conf.mode = GPIO_MODE_INPUT;
    io_conf.pull_up_en = GPIO_PULLUP_ENABLE; 
    io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
    gpio_config(&io_conf);

    // Install ISR service if not already installed
    gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);

    // Attach ISR with THIS instance as argument
    gpio_isr_handler_add(pin_a, Encoder::isr_handler, this);
    gpio_isr_handler_add(pin_b, Encoder::isr_handler, this);

    ESP_LOGI(TAG, "Encoder initialized on pins %d and %d", pin_a, pin_b);
}

void Encoder::deinit()
{
    gpio_isr_handler_remove(pin_a);
    gpio_isr_handler_remove(pin_b);
    ESP_LOGI(TAG, "Encoder deinitialized");
}

void Encoder::setupWatchdog() {
  if (watchdog_handle == NULL) {
    const esp_timer_create_args_t enc_watchdog_args = {
      .callback = &watchdogCallback,
      .dispatch_method = ESP_TIMER_ISR,
      .name = "encoder_wdt",
    };

    ESP_ERROR_CHECK(esp_timer_create(&enc_watchdog_args, &watchdog_handle));
  }

  ESP_ERROR_CHECK(esp_timer_start_once(watchdog_handle, 500000));
  feedWDog = true;
}

void IRAM_ATTR Encoder::pauseWatchdog() {
  if (watchdog_handle != nullptr) esp_timer_stop(watchdog_handle);
  feedWDog = false;
}

Encoder::~Encoder() {
  if (watchdog_handle != NULL) {
    esp_timer_stop(watchdog_handle);
    esp_timer_delete(watchdog_handle);
    watchdog_handle = NULL;
  }
}