/*
 * non-blocking touch read code
 * copied and modified from ESP32 HAL
 */


#include "esp32-hal-touch.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "rom/ets_sys.h"
#include "esp_attr.h"
#include "esp_intr.h"
#include "soc/rtc_io_reg.h"
#include "soc/rtc_cntl_reg.h"
#include "soc/sens_reg.h"


static uint32_t v0;
static int8_t lastpin = 0;

static uint16_t __touchSleepCycles = 0x1000;
static uint16_t __touchMeasureCycles = 0x1000;


void touchSetCyclesNB(uint16_t measure, uint16_t sleep)
{
    __touchSleepCycles = sleep;
    __touchMeasureCycles = measure;
    //Touch pad SleepCycle Time
    SET_PERI_REG_BITS(SENS_SAR_TOUCH_CTRL2_REG, SENS_TOUCH_SLEEP_CYCLES, __touchSleepCycles, SENS_TOUCH_SLEEP_CYCLES_S);
    //Touch Pad Measure Time
    SET_PERI_REG_BITS(SENS_SAR_TOUCH_CTRL1_REG, SENS_TOUCH_MEAS_DELAY, __touchMeasureCycles, SENS_TOUCH_MEAS_DELAY_S);
}

void touchInitNB()
{
    static bool initialized = false;
    if(initialized){
        return;
    }
    initialized = true;
    SET_PERI_REG_BITS(RTC_IO_TOUCH_CFG_REG, RTC_IO_TOUCH_XPD_BIAS, 1, RTC_IO_TOUCH_XPD_BIAS_S);
    SET_PERI_REG_MASK(SENS_SAR_TOUCH_CTRL2_REG, SENS_TOUCH_MEAS_EN_CLR);
    //clear touch enable
    WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, 0x0);
    SET_PERI_REG_MASK(RTC_CNTL_STATE0_REG, RTC_CNTL_TOUCH_SLP_TIMER_EN);

    touchSetCyclesNB(__touchMeasureCycles, __touchSleepCycles);
}

uint16_t touchReadGet()
{
    int8_t pad = digitalPinToTouchChannel(lastpin);
    if(pad < 0){
        return 0;
    }

    while (GET_PERI_REG_MASK(SENS_SAR_TOUCH_CTRL2_REG, SENS_TOUCH_MEAS_DONE) == 0) {};

    uint16_t touch_value = READ_PERI_REG(SENS_SAR_TOUCH_OUT1_REG + (pad / 2) * 4) >> ((pad & 1) ? SENS_TOUCH_MEAS_OUT1_S : SENS_TOUCH_MEAS_OUT0_S);

    //clear touch force ,select the Touch mode is Timer
    CLEAR_PERI_REG_MASK(SENS_SAR_TOUCH_CTRL2_REG, SENS_TOUCH_START_EN_M|SENS_TOUCH_START_FORCE_M);

    //restore previous value
    WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG, v0);

    return touch_value;
}

void touchReadNB(uint8_t pin)
{
    int8_t pad = digitalPinToTouchChannel(pin);
    if(pad < 0){
        return;
    }

    lastpin = pin;

    pinMode(pin, ANALOG);

    touchInitNB();

    v0 = READ_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG);
    //Disable Intr & enable touch pad
    WRITE_PERI_REG(SENS_SAR_TOUCH_ENABLE_REG,
            (v0 & ~((1 << (pad + SENS_TOUCH_PAD_OUTEN2_S)) | (1 << (pad + SENS_TOUCH_PAD_OUTEN1_S))))
            | (1 << (pad + SENS_TOUCH_PAD_WORKEN_S)));

    SET_PERI_REG_MASK(SENS_SAR_TOUCH_ENABLE_REG, (1 << (pad + SENS_TOUCH_PAD_WORKEN_S)));

    uint32_t rtc_tio_reg = RTC_IO_TOUCH_PAD0_REG + pad * 4;
    WRITE_PERI_REG(rtc_tio_reg, (READ_PERI_REG(rtc_tio_reg)
                      & ~(RTC_IO_TOUCH_PAD0_DAC_M))
                      | (7 << RTC_IO_TOUCH_PAD0_DAC_S)//Touch Set Slope
                      | RTC_IO_TOUCH_PAD0_TIE_OPT_M   //Enable Tie,Init Level
                      | RTC_IO_TOUCH_PAD0_START_M     //Enable Touch Pad IO
                      | RTC_IO_TOUCH_PAD0_XPD_M);     //Enable Touch Pad Power on

    //force oneTime test start
    SET_PERI_REG_MASK(SENS_SAR_TOUCH_CTRL2_REG, SENS_TOUCH_START_EN_M|SENS_TOUCH_START_FORCE_M);

    SET_PERI_REG_BITS(SENS_SAR_TOUCH_CTRL1_REG, SENS_TOUCH_XPD_WAIT, 10, SENS_TOUCH_XPD_WAIT_S);
}