#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"

#include "driver/touch_pad.h"
#include "soc/rtc_cntl_reg.h"
#include "soc/sens_reg.h"

#include "Arduino.h"
#include "keyboard.h"

using namespace std;

void (*Keyboard::callbacks[TOUCH_PAD_NUM])();

Keyboard::Keyboard() {
}

/*
  Handle an interrupt triggered when a pad is touched.
  Recognize what pad has been touched and save it in a table.
  https://github.com/espressif/esp-idf/blob/master/examples/peripherals/touch_pad_interrupt/main/tp_interrupt_main.c
 */
static void tp_example_rtc_intr(void *arg) {
 uint32_t pad_intr = READ_PERI_REG(SENS_SAR_TOUCH_CTRL2_REG) & 0x3ff;
 uint32_t rtc_intr = READ_PERI_REG(RTC_CNTL_INT_ST_REG);
 //clear interrupt
 WRITE_PERI_REG(RTC_CNTL_INT_CLR_REG, rtc_intr);
 SET_PERI_REG_MASK(SENS_SAR_TOUCH_CTRL2_REG, SENS_TOUCH_MEAS_EN_CLR);

  if (rtc_intr & RTC_CNTL_TOUCH_INT_ST) {
    for (int i = 0; i < TOUCH_PAD_NUM; i++) {
      if ((pad_intr >> i) & 0x01) {
        s_pad_activated[i] = true;
        //if (Keyboard::callbacks[i]) Keyboard::callbacks[i]();
      }
    }
  }
}

bool Keyboard::getTouchDetected(uint8_t pad) {
  if (pad==8) pad=9;  // fix hardware wiring
  if (pad==1) pad=8;
  bool res = s_pad_activated[pad];
  s_pad_activated[pad] = false;
  return res;
}

void Keyboard::setCallback(uint8_t key_id, void(*new_function)()) {
  Keyboard::callbacks[key_id] = new_function;
}

void Keyboard::init() {
  touch_pad_init();

  Serial.println("touchpad values:");
  for (int i=0; i<TOUCH_PAD_NUM; i++) {
    uint16_t touch_value;
    touch_pad_read((touch_pad_t)i, &touch_value);
    Serial.println(touch_value, DEC);
  }
  // set thresholds // ToDo: what if someone touches something on startup?
  uint16_t touch_value;
  for (int i=0; i<TOUCH_PAD_NUM; i++) {
    touch_pad_read((touch_pad_t)i, &touch_value);
    touch_pad_config((touch_pad_t)i, touch_value/2);
  }
/*
  threshold[0] = 50;
  threshold[1] = 0;
  threshold[2] = 0;
  threshold[3] = 50;
  threshold[4] = 50;
  threshold[5] = 50;
  threshold[6] = 50;
  threshold[7] = 50;
  threshold[8] = 50;
  threshold[9] = 50;
  for (int i=0; i<TOUCH_PAD_NUM; i++) touch_pad_config((touch_pad_t)i, threshold[i]);
*/

  // interrupt handler
  touch_pad_isr_handler_register(tp_example_rtc_intr, NULL, 0, NULL);
}