#include <Arduino.h>
#include "rc522.h"
#include "SPI.h"
#include <MFRC522.h>
#include "rotary_encoder.h"

#include "containers/LedStripRGBW.h"
#include "drivers/Esp32DriverRGBW.h"
#include "effects/Circular.h"
#include "effects/Static.h"
#include "effects/StaticDetailed.h"
#include "effects/AlexaSwipe.h"
#include "effects/RandomTwoColorInterpolation.h"

#include "AceButton.h"

#include "driver/touch_pad.h"

#include "Messages.h"

#include "TaskLed.h"

// -------------------------------------------------- RFID Reader ----------------------------------------
MFRC522 rfid;
MFRC522::MIFARE_Key key;

void tagHandler(uint8_t *sn)
{
  if (sn != nullptr)
    sendMessageToHost(MsgRfidTokenRead{{sn[0], sn[1], sn[2], sn[3], sn[4]}});
  else
    sendMessageToHost(MsgRfidTokenRead{{0, 0, 0, 0, 0}});
}

void setupRfidReader()
{
  const rc522_start_args_t start_args = {
      21, // MISO
      5,  // MOSI
      18, // SCK
      19, // SDA
      VSPI_HOST,
      &tagHandler,
      125,      // scan_interval_ms
      8 * 1024, // stacksize
      4         // task priority
  };
  rc522_start(start_args);
}

// -------------------------------------------------- Rotary Enc ----------------------------------------
QueueHandle_t eventQueueRotaryEncoder;
rotary_encoder_info_t info;

void setupRotaryEncoder()
{
  ESP_ERROR_CHECK(gpio_install_isr_service(0));
  ESP_ERROR_CHECK(rotary_encoder_init(&info, GPIO_NUM_26, GPIO_NUM_27));
  ESP_ERROR_CHECK(rotary_encoder_enable_half_steps(&info, false));
  eventQueueRotaryEncoder = rotary_encoder_create_queue();
  ESP_ERROR_CHECK(rotary_encoder_set_queue(&info, eventQueueRotaryEncoder));
}

int32_t lastRotaryPosition = 0;
bool lastRotaryPositionValid = false;
void handleRotaryEncoder()
{
  rotary_encoder_event_t event = {0};
  if (xQueueReceive(eventQueueRotaryEncoder, &event, 0) == pdTRUE)
  {
    int32_t increment = 0;
    if (lastRotaryPositionValid)
      increment = lastRotaryPosition - event.state.position;
    sendMessageToHost(MsgRotaryEncoder{event.state.position, increment, (uint8_t)(event.state.direction)});
    lastRotaryPositionValid = true;
    lastRotaryPosition = event.state.position;
  }
}

// -------------------------------------------------- Buttons    ----------------------------------------

constexpr int BUTTON_RIGHT_PIN = 25;
constexpr int BUTTON_LEFT_PIN = 14;
constexpr int ROTARY_PRESS_PIN = 13;
constexpr int BUTTON_RIGHT_LED_PIN = 33;
constexpr int BUTTON_LEFT_LED_PIN = 12;
constexpr int PWM_FREQ = 5000;
constexpr int PWM_RESOLUTION = 8;

using ace_button::AceButton;
AceButton buttonLeft(BUTTON_LEFT_PIN);
AceButton buttonRight(BUTTON_RIGHT_PIN);
AceButton buttonRotary(ROTARY_PRESS_PIN);

void handleButtonEvent(AceButton *button, uint8_t eventType, uint8_t /*buttonState*/)
{
  uint8_t buttonNr = 0;
  if (button == &buttonLeft)
    buttonNr = 1;
  else if (button == &buttonRight)
    buttonNr = 2;
  else if (button == &buttonRotary)
    buttonNr = 3;
  sendMessageToHost(MsgButtonEvent{buttonNr, eventType});
}

void setupButtons()
{
  pinMode(BUTTON_RIGHT_PIN, INPUT_PULLUP);
  pinMode(BUTTON_LEFT_PIN, INPUT_PULLUP);
  pinMode(ROTARY_PRESS_PIN, INPUT_PULLUP);

  pinMode(BUTTON_RIGHT_LED_PIN, OUTPUT);
  pinMode(BUTTON_LEFT_LED_PIN, OUTPUT);

  buttonLeft.setEventHandler(handleButtonEvent);
  buttonRight.setEventHandler(handleButtonEvent);
  buttonRotary.setEventHandler(handleButtonEvent);

  ledcSetup(0, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(BUTTON_LEFT_LED_PIN, 0);

  ledcSetup(1, PWM_FREQ, PWM_RESOLUTION);
  ledcAttachPin(BUTTON_RIGHT_LED_PIN, 1);
}

void handleButtons()
{
  buttonLeft.check();
  buttonRight.check();
  buttonRotary.check();
}

// -------------------------------------------------- Led circle ------------------------------------------

LedStripRGBW<51> ledStripCircle;
Esp32DriverRGBW ledDriverCircle;
LedTask<decltype(ledStripCircle)> ledTaskCircle;

void setupLedCircle()
{
  ledDriverCircle.begin(22, 0);
  ledTaskCircle.begin(ledStripCircle, ledDriverCircle);
  ledTaskCircle.startEffect(EffectStaticConfig(ColorRGBW{0, 0, 0, 0}));
}

// -------------------------------------------------- Mouse Leds -- ----------------------------------------

LedStripRGBW<12 + 16 + 17> ledStripMouse;
Esp32DriverRGBW ledDriverMouse;
LedTask<decltype(ledStripMouse)> ledTaskMouse;

void setupMouseLeds()
{
  ledDriverMouse.begin(16, 1);
  ledTaskMouse.begin(ledStripMouse, ledDriverMouse);
  ledTaskMouse.startEffect(EffectStaticConfig{ColorRGBW{0, 0, 0, 0}, 0, 0});
}

// -------------------------------------------------- Shelf Leds -------------------------------------------

LedStripRGBW<252> ledStripShelf;
Esp32DriverRGBW ledDriverShelf;
LedTask<decltype(ledStripShelf)> ledTaskShelf;

void setupShelfLeds()
{
  ledDriverShelf.begin(17, 2);
  ledTaskShelf.begin(ledStripShelf, ledDriverShelf);
  ledTaskShelf.startEffect(EffectStaticConfig{ColorRGBW{0, 0, 0, 0}, 0, 0});
}

// -------------------------------------------------- Touch Buttons ----------------------------------------

constexpr auto TOUCH_PAD_RIGHT_EAR = TOUCH_PAD_NUM0;
constexpr auto TOUCH_PAD_LEFT_EAR = TOUCH_PAD_NUM9;
constexpr auto TOUCH_PAD_RIGHT_FOOT = TOUCH_PAD_NUM2;
constexpr auto TOUCH_PAD_LEFT_FOOT = TOUCH_PAD_NUM3;

void setupTouchButtons()
{
  touch_pad_init();
  touch_pad_set_voltage(TOUCH_HVOLT_2V7, TOUCH_LVOLT_0V5, TOUCH_HVOLT_ATTEN_1V);
  for (auto pad : {TOUCH_PAD_RIGHT_EAR, TOUCH_PAD_LEFT_EAR, TOUCH_PAD_RIGHT_FOOT, TOUCH_PAD_LEFT_FOOT})
    touch_pad_config(pad, 0);

  touch_pad_filter_start(2);
}

void handleTouchInputs()
{
  static bool previousState[4];

  uint16_t touchLeftEar = 0;
  uint16_t touchRightEar = 0;
  uint16_t touchLeftFoot = 0;
  uint16_t touchRightFoot = 0;

  touch_pad_read(TOUCH_PAD_LEFT_FOOT, &touchLeftFoot);
  touch_pad_read(TOUCH_PAD_RIGHT_FOOT, &touchRightFoot);
  touch_pad_read(TOUCH_PAD_LEFT_EAR, &touchLeftEar);
  touch_pad_read(TOUCH_PAD_RIGHT_EAR, &touchRightEar);
  //Serial.printf("Feet %d %d, Ears %d, %d\n", touchLeftFoot, touchRightFoot, touchLeftEar, touchRightEar);
  //delay(100);
  bool currentState[4];
  currentState[int(TouchButton::LEFT_FOOT)] = touchLeftFoot < 380;
  currentState[int(TouchButton::RIGHT_FOOT)] = touchRightFoot < 380;
  currentState[int(TouchButton::LEFT_EAR)] = touchLeftEar < 430;
  currentState[int(TouchButton::RIGHT_EAR)] = touchRightEar < 400;

  for (int i = 0; i < 4; ++i)
  {
    if (previousState[i] == false && currentState[i] == true)
      sendMessageToHost(MsgTouchButtonPress{TouchButton(i)});
    else if (previousState[i] == true && currentState[i] == false)
      sendMessageToHost(MsgTouchButtonRelease{TouchButton(i)});
    previousState[i] = currentState[i];
  }
}

//-------------------------------------------------------------------------------------------------------

void setup()
{
  Serial.begin(115200);
  setupRfidReader();
  setupRotaryEncoder();
  setupLedCircle();
  setupButtons();
  setupTouchButtons();
  setupMouseLeds();
  setupShelfLeds();
}

void loop()
{
  handleIncomingMessagesFromHost(&ledTaskCircle, &ledTaskMouse, &ledTaskShelf, 0, 1);
  handleTouchInputs();
  handleRotaryEncoder();
  handleButtons();
}