#include "servo.hpp"
#include "driver/ledc.h"
#include "defines.h"
#include <freertos/FreeRTOS.h>
#include "esp_log.h"
#include "socketIO.hpp"
#include "nvs_flash.h"

std::atomic<bool> calibListen{false};
std::atomic<int32_t> baseDiff{0};
std::atomic<int32_t> target{0};

std::atomic<bool> runningManual{false};
std::atomic<bool> runningServer{false};
std::atomic<bool> clearCalibFlag{false};
std::atomic<bool> savePosFlag{false};
std::atomic<bool> startLess{false};

void servoInit() {
  // LEDC timer configuration (C++ aggregate initialization)
  ledc_timer_config_t ledc_timer = {};
  ledc_timer.speed_mode = LEDC_LOW_SPEED_MODE;
  ledc_timer.timer_num = LEDC_TIMER_0;
  ledc_timer.duty_resolution = LEDC_TIMER_16_BIT;
  ledc_timer.freq_hz = 50;
  ledc_timer.clk_cfg = LEDC_AUTO_CLK;
  ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

  // LEDC channel configuration
  ledc_channel_config_t ledc_channel = {};
  ledc_channel.speed_mode = LEDC_LOW_SPEED_MODE;
  ledc_channel.channel = servoLEDCChannel;
  ledc_channel.timer_sel = LEDC_TIMER_0;
  ledc_channel.intr_type = LEDC_INTR_DISABLE;
  ledc_channel.gpio_num = servoPin;
  ledc_channel.duty = offSpeed; // Start off
  ledc_channel.hpoint = 0;
  ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));

  // Configure servo power switch pin as output
  gpio_set_direction(servoSwitch, GPIO_MODE_OUTPUT);
  gpio_set_level(servoSwitch, 0); // Start with servo power off

  // Configure debug LED pin as output
  gpio_reset_pin(GPIO_NUM_22);
  gpio_set_direction(debugLED, GPIO_MODE_OUTPUT);
  gpio_set_level(debugLED, 0); // Start with LED off

  topEnc->count = servoReadPos();
  if (Calibration::getCalibrated()) initMainLoop();
  debugLEDSwitch(1);
}

void servoOn(uint8_t dir, uint8_t manOrServer) {
  servoMainSwitch(1);
  ledc_set_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel, (dir ? ccwSpeed : cwSpeed));
  ledc_update_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel);
  runningManual = !manOrServer;
  runningServer = manOrServer;
}

void servoOff() {
  ledc_set_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel, offSpeed);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel);
  runningManual = false;
  runningServer = false;
  servoMainSwitch(0);
}

void servoMainSwitch(uint8_t onOff) {
  gpio_set_level(servoSwitch, onOff ? 1 : 0);
}

void debugLEDSwitch(uint8_t onOff) {
  gpio_set_level(debugLED, onOff ? 1 : 0);
}

void debugLEDTgl() {
  static bool onOff = false;
  gpio_set_level(debugLED, onOff);
  onOff = !onOff;
}

bool servoInitCalib() {
  topEnc->pauseWatchdog();
  // get ready for calibration by clearing all these listeners
  bottomEnc->wandListen.store(false, std::memory_order_release);
  topEnc->wandListen.store(false, std::memory_order_release);
  topEnc->serverListen.store(false, std::memory_order_release);
  if (!Calibration::clearCalibrated()) return false;
  if (topEnc == nullptr || bottomEnc == nullptr) {
    printf("ERROR: CALIBRATION STARTED BEFORE SERVO INITIALIZATION\n");
    return false;
  }
  baseDiff = bottomEnc->getCount() - topEnc->getCount();
  calibListen = true;
  return true;
}

void servoCancelCalib() {
  calibListen = false;
  servoOff();
}

void servoCalibListen() {
  int32_t effDiff = (bottomEnc->getCount() - topEnc->getCount()) - baseDiff;
  if (effDiff > 1) servoOn(CCW, manual);
  else if (effDiff < -1) {
    servoOn(CW, manual);
  }
  else {
    servoOff();
  }
}

bool servoBeginDownwardCalib() {
  calibListen = false;
  servoOff();
  vTaskDelay(pdMS_TO_TICKS(1000));
  if (!Calibration::beginDownwardCalib(*topEnc)) return false;
  baseDiff = bottomEnc->getCount() - topEnc->getCount();
  calibListen = true;
  return true;
}

bool servoCompleteCalib() {
  calibListen = false;
  servoOff();
  vTaskDelay(pdMS_TO_TICKS(1000));
  if (!Calibration::completeCalib(*topEnc)) return false;
  initMainLoop();
  return true;
}

void initMainLoop() {
  topEnc->setupWatchdog();
  servoSavePos();
  bottomEnc->wandListen.store(true, std::memory_order_release);
}

void IRAM_ATTR watchdogCallback(void* arg) {
  BaseType_t xHigherPriorityTaskWoken = pdFALSE;
  
  if (runningManual || runningServer) {
    // if we're trying to move and our timer ran out, we need to recalibrate
    xEventGroupSetBitsFromISR(g_system_events, EVENT_CLEAR_CALIB, &xHigherPriorityTaskWoken);
    topEnc->pauseWatchdog();

    // get ready for recalibration by clearing all these listeners
    bottomEnc->wandListen.store(false, std::memory_order_release);
    topEnc->wandListen.store(false, std::memory_order_release);
    topEnc->serverListen.store(false, std::memory_order_release);
    servoOff();
  }
  else {
    // if no movement is running, we're fine
    // save current servo-encoder position for reinitialization
    xEventGroupSetBitsFromISR(g_system_events, EVENT_SAVE_POSITION, &xHigherPriorityTaskWoken);
  }
  // clear running flags
  runningManual = false;
  runningServer = false;
  
  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

void servoSavePos() {
  // save current servo-encoder position for use on reinitialization
  nvs_handle_t servoHandle;
  if (nvs_open(nvsServo, NVS_READWRITE, &servoHandle) == ESP_OK) {
    int32_t topCount = topEnc->getCount();
    if (nvs_set_i32(servoHandle, posTag, topCount) != ESP_OK)
      printf("Error saving current position\n");
    else printf("Success - Current position saved as: %d\n", topCount);
    nvs_commit(servoHandle);
    nvs_close(servoHandle);
  }
  else {
    printf("Error opening servoPos NVS segment.\n");
  }
}

int32_t servoReadPos() {
  // save current servo-encoder position for use on reinitialization
  int32_t val = 0;
  nvs_handle_t servoHandle;
  if (nvs_open(nvsServo, NVS_READONLY, &servoHandle) == ESP_OK) {
    if (nvs_get_i32(servoHandle, posTag, &val) != ESP_OK)
      printf("Error reading current position\n");
    else printf("Success - Current position read as: %d\n", val);
    nvs_close(servoHandle);
  }
  else {
    printf("Error opening servoPos NVS segment.\n");
  }
  return val;
}

void stopServerRun() {
  // stop listener and stop running if serverRun is still active.
  topEnc->serverListen.store(false, std::memory_order_release);
  if (runningServer) servoOff();
}

void servoWandListen() {
  // stop any remote-initiated movement
  stopServerRun();

  // freeze atomic values
  int32_t upBound = Calibration::UpTicks;
  int32_t downBound = Calibration::DownTicks;
  int32_t bottomCount = bottomEnc->getCount();
  int32_t topCount = topEnc->getCount();

  // ensure the baseDiff doesn't wait on wand to turn all the way back to original range.
  if ((upBound > downBound && bottomCount - baseDiff > upBound)
       || (upBound < downBound && bottomCount - baseDiff < upBound))
    baseDiff = bottomCount - upBound;
  else if ((upBound > downBound && bottomCount - baseDiff < downBound)
            || (upBound < downBound && bottomCount - baseDiff > downBound))
    baseDiff = bottomCount - downBound;
  
  // calculate the difference between wand and top servo
  int32_t effDiff = (bottomCount - topCount) - baseDiff;

  // if we are at either bound, stop servo and servo-listener
  // if effective difference is 0, stop servo and servo-listener
  // otherwise, run servo in whichever direction necessary and
  // ensure servo-listener is active.
  if (topCount >= (MAX(upBound, downBound) - 1)
      && effDiff > 1) { // TODO: see whether these margins need to be removed.
    servoOff();
    topEnc->wandListen.store(false, std::memory_order_release);
  }
  else if (topCount <= (MIN(upBound, downBound) + 1)
      && effDiff < -1) {
    servoOff();
    topEnc->wandListen.store(false, std::memory_order_release);
  }
  else if (effDiff > 1) {
    topEnc->wandListen.store(true, std::memory_order_release);
    servoOn(CCW, manual);
  }
  else if (effDiff < -1) {
    topEnc->wandListen.store(true, std::memory_order_release);
    servoOn(CW, manual);
  }
  else {
    servoOff();
    topEnc->wandListen.store(false, std::memory_order_release);
  }
}

void servoServerListen() {
  // If we have reached or passed our goal, stop running and stop listener.
  if (topEnc->getCount() >= target && startLess) stopServerRun();
  else if (topEnc->getCount() <= target && !startLess) stopServerRun();
  baseDiff = bottomEnc->getCount() - topEnc->getCount();
}

void runToAppPos(uint8_t appPos) {
  // manual control takes precedence over remote control, always.
  // also do not begin operation if not calibrated;
  if (runningManual || !Calibration::getCalibrated()) return;
  servoOff();

  target = Calibration::convertToTicks(appPos); // calculate target encoder position
  printf("runToAppPos Called, running to %d from %d", target.load(), topEnc->getCount());

  // allow servo position to settle
  vTaskDelay(pdMS_TO_TICKS(500));
  int32_t topCount = topEnc->getCount();
  if (abs(topCount - target) <= 1) return;
  startLess = topCount < target;
  if (runningManual) return; // check again before starting remote control
  if (startLess) servoOn(CCW, server); // begin servo movement
  else servoOn(CW, server);
  topEnc->serverListen.store(true, std::memory_order_release); // start listening for shutoff point
}

// Servo control task - processes encoder events and servo commands
void servoControlTask(void* arg) {
    encoder_event_t enc_event;
    servo_cmd_msg_t cmd;
    
    printf("Servo control task started\n");
    
    while (1) {
        // Block waiting for encoder events (higher priority)
        if (xQueueReceive(g_encoder_event_queue, &enc_event, pdMS_TO_TICKS(10)) == pdTRUE) {
            // Process encoder event (work that was done in ISR before)
            
            // Handle calibration listening
            if (calibListen) {
                int32_t effDiff = (bottomEnc->getCount() - topEnc->getCount()) - baseDiff;
                if (effDiff > 1) {
                    servoOn(CCW, manual);
                }
                else if (effDiff < -1) {
                    servoOn(CW, manual);
                }
                else {
                    servoOff();
                }
            }
            
            // Only process top encoder events for watchdog and listeners
            if (enc_event.is_top_encoder) {
                // Feed watchdog in task context (not ISR)
                if (topEnc->feedWDog) {
                    esp_timer_restart(topEnc->watchdog_handle, 500000);
                }
                
                // Check wand listener - now safe in task context
                if (topEnc->wandListen) {
                    servoWandListen();
                }
                
                // Check server listener - now safe in task context
                if (topEnc->serverListen) {
                    servoServerListen();
                }
            }
        }
        
        // Check for direct servo commands (lower priority)
        if (xQueueReceive(g_servo_command_queue, &cmd, 0) == pdTRUE) {
            switch (cmd.command) {
                case SERVO_CMD_STOP:
                    servoOff();
                    break;
                case SERVO_CMD_MOVE_CCW:
                    servoOn(CCW, cmd.is_manual ? manual : server);
                    break;
                case SERVO_CMD_MOVE_CW:
                    servoOn(CW, cmd.is_manual ? manual : server);
                    break;
                case SERVO_CMD_MOVE_TO_POSITION:
                    runToAppPos(cmd.target_position);
                    break;
            }
        }
    }
}