#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/AlexaSwipe.h"
#include "effects/RandomTwoColorInterpolation.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));
}

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

constexpr int BUTTON1_PIN = 25;
constexpr int BUTTON2_PIN = 14;
constexpr int ROTARY_PRESS_PIN = 13;
constexpr int BUTTON1_LED_PIN = 33;
constexpr int BUTTON2_LED_PIN = 12;

void setupButtons()
{
  pinMode(BUTTON1_PIN, INPUT_PULLUP);
  pinMode(BUTTON2_PIN, INPUT_PULLUP);
  pinMode(ROTARY_PRESS_PIN, INPUT_PULLUP);

  pinMode(BUTTON1_LED_PIN, OUTPUT);
  pinMode(BUTTON2_LED_PIN, OUTPUT);

  digitalWrite(BUTTON1_LED_PIN, 1);
  digitalWrite(BUTTON2_LED_PIN, 1);
}

// -------------------------------------------------- Led circle ----------------------------------------
LedStripRGBW<51> ledStripCircle;
Esp32DriverRGBW ledDriverCircle;
LedTask<decltype(ledStripCircle)> ledTaskCircle;

void setupLedCircle()
{
  ledDriverCircle.begin(23, 0);
  ledTaskCircle.begin(ledStripCircle, ledDriverCircle);
  ledTaskCircle.startEffect(EffectStaticConfig{ColorRGBW{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);
}

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

LedStripRGBW<12 + 16 + 17> ledStripMouse;
Esp32DriverRGBW ledDriverMouse;

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

  ledDriverMouse.begin(16, 1);
}

void handleTouchInputs()
{
}

void loop()
{
  handleIncomingMessagesFromHost(&ledTaskCircle);

  int btn1 = !digitalRead(BUTTON1_PIN);
  int btn2 = !digitalRead(BUTTON2_PIN);
  int rotaryBtn = !digitalRead(ROTARY_PRESS_PIN);

  uint16_t touchLeftEar = 1, touchRightEar = 1, touchLeftFoot = 1, touchRightFoot = 1;

  touch_pad_read(TOUCH_PAD_LEFT_FOOT, &touchLeftFoot);
  touch_pad_read(TOUCH_PAD_RIGHT_FOOT, &touchRightFoot);
  touch_pad_read(TOUCH_PAD_LEFT_EAR, &touchLeftEar);   // 430
  touch_pad_read(TOUCH_PAD_RIGHT_EAR, &touchRightEar); // 430

  //Serial.printf("%d, %d, %d | re %d, le %d, rf %d, lf %d\n", btn1, btn2, rotaryBtn, touchRightEar, touchLeftEar, touchRightFoot, touchLeftFoot);

  for (int i = 0; i < ledStripMouse.numLeds(); ++i)
    ledStripMouse.set(i, 0, 0, 30, 0);

  bool rightFootPressed = touchRightFoot < 380;
  bool leftFootPressed = touchLeftFoot < 380;
  bool leftEarPressed = touchLeftEar < 430;
  bool rightEarPressed = touchRightEar < 430;

  if (btn1 || rightFootPressed)
  {
    for (int i = 0; i < 6; ++i)
      ledStripMouse.set(i, 180, 0, 255, 0);
  }

  if (btn2 || leftFootPressed)
  {
    for (int i = 6; i < 12; ++i)
      ledStripMouse.set(i, 180, 0, 255, 0);
  }

  if (leftEarPressed)
    for (int i = 12; i < 12 + 16; ++i)
      ledStripMouse.set(i, 28, 241, 234, 0);

  if (rightEarPressed)
    for (int i = 12 + 16; i < ledStripMouse.numLeds(); ++i)
      ledStripMouse.set(i, 28, 241, 234, 0);

  ledDriverMouse.writeSync(ledStripMouse.rawData(), ledStripMouse.numLeds());
  delay(50);

  //delay(150);
}