/******************************************************************************
*
* Copyright 2006-2015 Freescale Semiconductor, Inc.
* Copyright 2016-2017 NXP
*
******************************************************************************/

/****************************************************************************//*!
*
* @file     ets.c
*
* @brief    Electrode touch sense routines for S32K144
*
*******************************************************************************/

/*******************************************************************************
* Includes
*******************************************************************************/
#include "device_registers.h"
#include "timer.h"
#include "main.h"
#include "ets.h"
#include "ts_cfg_hw.h"
#include "ts_cfg_app.h"
#include "filter.h"
#include "adConv1.h"

/*******************************************************************************
* Variables
******************************************************************************/
extern volatile uint16_t Potentiometer; /* Variable of the Potentiometer value */
#define ADC_INSTANCE        0UL
// All electrodes status
tElecStatus   electrodesStatus;

// Electrode voltage sample
int16_t   adcDataElectrodeDischargeRaw[NUMBER_OF_ELECTRODES];
int16_t   adcDataElectrodeDischargeRawSample[NUMBER_OF_ELECTRODE_SENSING_CYCLES_PER_SAMPLE];
int16_t   tmpAdcDataElectrodeDischargeRaw;
uint8_t   electrodeTouchAct;
uint16_t  electrodeSampleCounter;
uint8_t   elecTouchTmp;
uint8_t   elecNum, elecNumAct, sampleNum;
uint8_t   electrodeWakeUpActivateCounter;

// Electrode charge distribution period
uint16_t  chargeDistributionPeriod, chargeDistributionPeriodTmp;

// Electrode DC tracker self-trim after power-up or reset
int32_t   adcDataElectrodeDischargeBuffer[NUMBER_OF_ELECTRODES];
uint16_t  adcDataElectrodeDischargeBufferCounter[NUMBER_OF_ELECTRODES];

// DC Tracker
int32_t   DCTrackerDataBufferRawThrBased[NUMBER_OF_ELECTRODES]; 
int16_t   DCTrackerDataBufferThrBased[NUMBER_OF_ELECTRODES];
uint8_t   DCTrackerDataShiftThrBased[NUMBER_OF_ELECTRODES];

// LP Filter IIR1
tFrac16   LPFilterSlowDataBufferThrBased[NUMBER_OF_ELECTRODES];
tFrac16   LPFilterMediumDataBufferThrBased[NUMBER_OF_ELECTRODES];
tFrac16   LPFilterFastDataBufferThrBased[NUMBER_OF_ELECTRODES];
tFrac16   LPFilterIIRDataBufferThrBased[NUMBER_OF_ELECTRODES];
uint8_t   LPFilterType[NUMBER_OF_ELECTRODES];

// Detector
int16_t   detectorThresholdTouchThrBased[NUMBER_OF_ELECTRODES], detectorThresholdReleaseThrBased[NUMBER_OF_ELECTRODES];
int16_t   detectorThresholdTouchDeltaThrBased[NUMBER_OF_ELECTRODES], detectorThresholdReleaseDeltaThrBased[NUMBER_OF_ELECTRODES];
uint8_t   electrodeTouchThrBased[NUMBER_OF_ELECTRODES];

// Touch qualification
uint8_t   electrodeTouchQualifiedThrBased[NUMBER_OF_ELECTRODES];
int16_t   electrodeTouchAmplitude[NUMBER_OF_ELECTRODES];
int16_t   electrodeTouchAmplitudeRef;
uint8_t   electrodeTouchNum[NUMBER_OF_ELECTRODES];
uint8_t   electrodeTouchNumRef;
uint8_t   electrodeTouchAmplitudeCntr;

// Low power mode
extern uint8_t  lowPowerModeCtrl;

/*******************************************************************************
* Typedefs
******************************************************************************/
tElecStruct  elecStruct[NUMBER_OF_ELECTRODES], *pElecStruct;

/*****************************************************************************
*
* Function: void electrodeStructureInit(void)
*
* Description: Init electrode touch sense variables
*
*****************************************************************************/
void electrodeStructureInit(void)
{
#ifdef ELEC0
    // Load electrode 0 hardware data
    elecStruct[0].adcBasePtr = ELEC0_ADC;
    elecStruct[0].adcChNum = ELEC0_ADC_CHANNEL;
    elecStruct[0].portBasePtr = ELEC0_PORT;
    elecStruct[0].gpioBasePtr = ELEC0_GPIO;
    elecStruct[0].pinNumberElec = ELEC0_ELEC_GPIO_PIN;
    elecStruct[0].pinNumberCext = ELEC0_CEXT_GPIO_PIN;
    elecStruct[0].portMask = ELEC0_PORT_MASK;
    // Load electrode 0 application data
    DCTrackerDataShiftThrBased[0] = ELEC0_DCTRACKER_FILTER_FACTOR;
    LPFilterType[0] = ELEC0_LPFILTER_TYPE;
    detectorThresholdTouchDeltaThrBased[0] = ELEC0_TOUCH_THRESHOLD_DELTA;
    detectorThresholdReleaseDeltaThrBased[0] = ELEC0_RELEASE_THRESHOLD_DELTA;
#endif

#ifdef ELEC1
    // Load electrode 1 hardware data
    elecStruct[1].adcBasePtr = ELEC1_ADC;
    elecStruct[1].adcChNum = ELEC1_ADC_CHANNEL;
    elecStruct[1].portBasePtr = ELEC1_PORT;
    elecStruct[1].gpioBasePtr = ELEC1_GPIO;
    elecStruct[1].pinNumberElec = ELEC1_ELEC_GPIO_PIN;
    elecStruct[1].pinNumberCext = ELEC1_CEXT_GPIO_PIN;
    elecStruct[1].portMask = ELEC1_PORT_MASK;
    // Load electrode 1 application data
    DCTrackerDataShiftThrBased[1] = ELEC1_DCTRACKER_FILTER_FACTOR;
    LPFilterType[1] = ELEC1_LPFILTER_TYPE;
    detectorThresholdTouchDeltaThrBased[1] = ELEC1_TOUCH_THRESHOLD_DELTA;
    detectorThresholdReleaseDeltaThrBased[1] = ELEC1_RELEASE_THRESHOLD_DELTA;
#endif

#ifdef ELEC2
    // Load electrode 2 hardware data
    elecStruct[2].adcBasePtr = ELEC2_ADC;
    elecStruct[2].adcChNum = ELEC2_ADC_CHANNEL;
    elecStruct[2].portBasePtr = ELEC2_PORT;
    elecStruct[2].gpioBasePtr = ELEC2_GPIO;
    elecStruct[2].pinNumberElec = ELEC2_ELEC_GPIO_PIN;
    elecStruct[2].pinNumberCext = ELEC2_CEXT_GPIO_PIN;
    elecStruct[2].portMask = ELEC2_PORT_MASK;
    // Load electrode 2 application data
    DCTrackerDataShiftThrBased[2] = ELEC2_DCTRACKER_FILTER_FACTOR;
    LPFilterType[2] = ELEC2_LPFILTER_TYPE;
    detectorThresholdTouchDeltaThrBased[2] = ELEC2_TOUCH_THRESHOLD_DELTA;
    detectorThresholdReleaseDeltaThrBased[2] = ELEC2_RELEASE_THRESHOLD_DELTA;
#endif

#ifdef ELEC3
    // Load electrode 3 hardware data
    elecStruct[3].adcBasePtr = ELEC3_ADC;
    elecStruct[3].adcChNum = ELEC3_ADC_CHANNEL;
    elecStruct[3].portBasePtr = ELEC3_PORT;
    elecStruct[3].gpioBasePtr = ELEC3_GPIO;
    elecStruct[3].pinNumberElec = ELEC3_ELEC_GPIO_PIN;
    elecStruct[3].pinNumberCext = ELEC3_CEXT_GPIO_PIN;
    elecStruct[3].portMask = ELEC3_PORT_MASK;
#endif

#ifdef ELEC4
    // Load electrode 4 hardware data
    elecStruct[4].adcBasePtr = ELEC4_ADC;
    elecStruct[4].adcChNum = ELEC4_ADC_CHANNEL;
    elecStruct[4].portBasePtr = ELEC4_PORT;
    elecStruct[4].gpioBasePtr = ELEC4_GPIO;
    elecStruct[4].pinNumberElec = ELEC4_ELEC_GPIO_PIN;
    elecStruct[4].pinNumberCext = ELEC4_CEXT_GPIO_PIN;
    elecStruct[4].portMask = ELEC4_PORT_MASK;
#endif

#ifdef ELEC5
    // Load electrode 5 hardware data
    elecStruct[5].adcBasePtr = ELEC5_ADC;
    elecStruct[5].adcChNum = ELEC5_ADC_CHANNEL;
    elecStruct[5].portBasePtr = ELEC5_PORT;
    elecStruct[5].gpioBasePtr = ELEC5_GPIO;
    elecStruct[5].pinNumberElec = ELEC5_ELEC_GPIO_PIN;
    elecStruct[5].pinNumberCext = ELEC5_CEXT_GPIO_PIN;
    elecStruct[5].portMask = ELEC5_PORT_MASK;
#endif

#ifdef ELEC6
    // Load electrode 6 hardware data
    elecStruct[6].adcBasePtr = ELEC6_ADC;
    elecStruct[6].adcChNum = ELEC6_ADC_CHANNEL;
    elecStruct[6].portBasePtr = ELEC6_PORT;
    elecStruct[6].gpioBasePtr = ELEC6_GPIO;
    elecStruct[6].pinNumberElec = ELEC6_ELEC_GPIO_PIN;
    elecStruct[6].pinNumberCext = ELEC6_CEXT_GPIO_PIN;
    elecStruct[6].portMask = ELEC6_PORT_MASK;
#endif

#ifdef ELEC7
    // Load electrode 6 hardware data
    elecStruct[7].adcBasePtr = ELEC7_ADC;
    elecStruct[7].adcChNum = ELEC7_ADC_CHANNEL;
    elecStruct[7].portBasePtr = ELEC7_PORT;
    elecStruct[7].gpioBasePtr = ELEC7_GPIO;
    elecStruct[7].pinNumberElec = ELEC7_ELEC_GPIO_PIN;
    elecStruct[7].pinNumberCext = ELEC7_CEXT_GPIO_PIN;
    elecStruct[7].portMask = ELEC7_PORT_MASK;
#endif

#ifdef ELEC8
    // Load electrode 6 hardware data
    elecStruct[8].adcBasePtr = ELEC8_ADC;
    elecStruct[8].adcChNum = ELEC8_ADC_CHANNEL;
    elecStruct[8].portBasePtr = ELEC8_PORT;
    elecStruct[8].gpioBasePtr = ELEC8_GPIO;
    elecStruct[8].pinNumberElec = ELEC8_ELEC_GPIO_PIN;
    elecStruct[8].pinNumberCext = ELEC8_CEXT_GPIO_PIN;
    elecStruct[8].portMask = ELEC8_PORT_MASK;
#endif

#ifdef ELEC9
    // Load electrode 6 hardware data
    elecStruct[9].adcBasePtr = ELEC9_ADC;
    elecStruct[9].adcChNum = ELEC9_ADC_CHANNEL;
    elecStruct[9].portBasePtr = ELEC9_PORT;
    elecStruct[9].gpioBasePtr = ELEC9_GPIO;
    elecStruct[9].pinNumberElec = ELEC9_ELEC_GPIO_PIN;
    elecStruct[9].pinNumberCext = ELEC9_CEXT_GPIO_PIN;
    elecStruct[9].portMask = ELEC9_PORT_MASK;
#endif

#ifdef ELEC10
    // Load electrode 6 hardware data
    elecStruct[10].adcBasePtr = ELEC10_ADC;
    elecStruct[10].adcChNum = ELEC10_ADC_CHANNEL;
    elecStruct[10].portBasePtr = ELEC10_PORT;
    elecStruct[10].gpioBasePtr = ELEC10_GPIO;
    elecStruct[10].pinNumberElec = ELEC10_ELEC_GPIO_PIN;
    elecStruct[10].pinNumberCext = ELEC10_CEXT_GPIO_PIN;
    elecStruct[10].portMask = ELEC10_PORT_MASK;
#endif
}

/*****************************************************************************
*
* Function: void electrodeTouchSenseInit(void)
*
* Description: Init electrode touch sense variables
*
*****************************************************************************/
void electrodeTouchSenseInit(void)
{
    // Electrodes structure init
    electrodeStructureInit();

    // Electrode and Cext charge distribution period XXus
    chargeDistributionPeriod = 0;

    // Reset electrode sample counter
    electrodeSampleCounter = 0;

    // Reset electrodes status
    electrodesStatus.byte = 0;
    
    // Set first electrode to convert
    electrodeTouchAct = 0;

    // Init LP IIR Filter
    FilterIIR1Init();
}
/*****************************************************************************
*
* Function: void electrodeCapToVoltConvELCH(uint32_t electrodeNum)
*
* Description: Convert electrode capacitance to equivalent voltage
*
*****************************************************************************/
void electrodeCapToVoltConvELCH(uint32_t electrodeNum)
{
    signed short Potentiometer_tmp;

    // Run conversion
    PORTA->PCR[7] = PCR_ANA; /* Configure electrode pin as analog input */
    // Cext voltage conversion
    ADC0->SC1[0] = 3;
    // Wait for conversion complete flag
    while(ADC0->SC1[0] < 0x80){}
    // Read conversion from Potentiometer
    Potentiometer = ADC0->R[0];
    Potentiometer_tmp = (4064 - (Potentiometer&0xFFF0));
    Potentiometer = (Potentiometer_tmp < 0) ? (-Potentiometer_tmp) : (Potentiometer_tmp);
    PORTA->PCR[7] = 1;
    // Delay to redistribute charge
    chargeDistributionPeriodTmp = 20;
    while (chargeDistributionPeriodTmp)
    {
        chargeDistributionPeriodTmp--;
    }

    #ifdef DEBUG_ELECTRODE_SENSE
        // Pin clear
        PINS_DRV_ClearPins(DES_GPIO,1<<DES_PIN);
    #endif

    // Sampling
    for (sampleNum = 0; sampleNum < NUMBER_OF_ELECTRODE_SENSING_CYCLES_PER_SAMPLE; sampleNum++)
    {
        // Drive electrode GPIO low
        PINS_DRV_ClearPins((elecStruct[electrodeNum].gpioBasePtr),1<<elecStruct[electrodeNum].pinNumberElec);
        // Configure electrode pin as GPIO to precharge electrode
        elecStruct[electrodeNum].portBasePtr->PCR[elecStruct[electrodeNum].pinNumberElec] = PCR_GPIO;
        // Drive Cext GPIO high
        PINS_DRV_SetPins((elecStruct[electrodeNum].gpioBasePtr),1<<elecStruct[electrodeNum].pinNumberCext);
        // Configure Cext pin as GPIO to precharge Cext
        elecStruct[electrodeNum].portBasePtr->PCR[elecStruct[electrodeNum].pinNumberCext] = PCR_GPIO;
        // Configure Electrode and Cext pins as outputs at the same time
        REG_WRITE32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR, ((REG_READ32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR)) | (elecStruct[electrodeNum].portMask)));

        // Delay to redistribute charge
        chargeDistributionPeriodTmp = chargeDistributionPeriod;
        while (chargeDistributionPeriodTmp)
        {
            chargeDistributionPeriodTmp--;
        }

        // Electrode and Cext charge redistribution
        // Configure Electrode and Cext pins as inputs at the same time
        REG_WRITE32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR, ((REG_READ32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR)) & (~elecStruct[electrodeNum].portMask)));

        // Run conversion
        // Configure electrode pin as analog input
        elecStruct[electrodeNum].portBasePtr->PCR[elecStruct[electrodeNum].pinNumberElec] = PCR_ANA;
        // Cext voltage conversion
        elecStruct[electrodeNum].adcBasePtr->SC1[0] = elecStruct[electrodeNum].adcChNum;
        // Wait for conversion complete flag
        while(elecStruct[electrodeNum].adcBasePtr->SC1[0] < 0x80)
        {}
        // Store result, clear COCO flag
        adcDataElectrodeDischargeRawSample[sampleNum] = elecStruct[electrodeNum].adcBasePtr->R[0];
    }

    // Calculate value average of 3rn and following samples
    adcDataElectrodeDischargeRaw[electrodeNum] = 0;
    for (sampleNum = 0; sampleNum < NUMBER_OF_ELECTRODE_SENSING_CYCLES_PER_SAMPLE; sampleNum++)
    {
        adcDataElectrodeDischargeRaw[electrodeNum] = adcDataElectrodeDischargeRaw[electrodeNum] + adcDataElectrodeDischargeRawSample[sampleNum];
    }

    adcDataElectrodeDischargeRaw[electrodeNum] = adcDataElectrodeDischargeRaw[electrodeNum] / NUMBER_OF_ELECTRODE_SENSING_CYCLES_PER_SAMPLE;

#ifdef DEBUG_ELECTRODE_SENSE
    // Pin set
    PINS_DRV_SetPins(DES_GPIO,1<<DES_PIN);
#endif
}

/*****************************************************************************
*
* Function: void electrodeGnd(uint32_t electrode)
*
* Description: Drive electrode pins low level
*
*****************************************************************************/
void electrodeGnd(uint32_t electrodeNum)
{
    // Drive electrode GPIO low
    PINS_DRV_ClearPins((elecStruct[electrodeNum].gpioBasePtr),1<<elecStruct[electrodeNum].pinNumberElec);
    // Configure electrode pin as GPIO
    elecStruct[electrodeNum].portBasePtr->PCR[elecStruct[electrodeNum].pinNumberElec] = PCR_GPIO;
    // Drive Cext GPIO low
    PINS_DRV_ClearPins((elecStruct[electrodeNum].gpioBasePtr),1<<elecStruct[electrodeNum].pinNumberCext);
    // Configure Cext pin as GPIO
    elecStruct[electrodeNum].portBasePtr->PCR[elecStruct[electrodeNum].pinNumberCext] = PCR_GPIO;
    // Configure Electrode and Cext pins as outputs at the same time
    REG_WRITE32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR, ((REG_READ32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR)) | (elecStruct[electrodeNum].portMask)));
}

/*****************************************************************************
*
* Function: void electrodeFloat(uint32_t electrodeNum)
*
* Description: Drive electrodes pins as an inputs (high impedance)
*
*****************************************************************************/
void electrodeFloat(uint32_t electrodeNum)
{
    // Configure Electrode and Cext pins as outputs at the same time
    REG_WRITE32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR, ((REG_READ32(&(elecStruct[electrodeNum].gpioBasePtr)->PDDR)) & (~(elecStruct[electrodeNum].portMask))));
}

/*****************************************************************************
*
* Function: void ElectodeBufferInitVal(int16_t inputSignal, int32_t *outputSignalPtr, uint16_t *sumCounterPtr)
*
* Description: Sum I/O data
*
*****************************************************************************/
void ElectodeBufferInitVal(int16_t inputSignal, int32_t *outputSignalPtr, uint16_t *sumCounterPtr)
{
    int32_t  outputSignal;
    uint16_t sumCounter;


    outputSignal = *outputSignalPtr;
    sumCounter = *sumCounterPtr;

    sumCounter++;
    outputSignal = outputSignal + inputSignal;

    *outputSignalPtr = outputSignal;
    *sumCounterPtr = sumCounter;
}

/*****************************************************************************
*
* Function: void ElectodeCalInitVal(int32_t *outputSignalPtr, uint16_t *sumCounterPtr)
*
* Description: Calculate data average value
*
*****************************************************************************/
void ElectodeCalInitVal(int32_t *outputSignalPtr, uint16_t *sumCounterPtr)
{
    int32_t  outputSignal;
    uint16_t sumCounter;
    

    outputSignal = *outputSignalPtr;
    sumCounter = *sumCounterPtr;

    outputSignal = outputSignal / sumCounter;

    *outputSignalPtr = outputSignal;
    *sumCounterPtr = DCTRACKER_PWRUP_INIT_DONE;
}

/*****************************************************************************
*
* Function: int16_t DCTracker(int16_t inputSignal, int32_t *outputSignalRawPtr, uint8_t shift)
*
* Description: Slow low pass filter
*
*****************************************************************************/
int16_t DCTracker(int16_t inputSignal, int32_t *outputSignalRawPtr, uint8_t shift)
{    
    int32_t  outputSignalRaw;
    
    
    outputSignalRaw = *outputSignalRawPtr;
    
    if(inputSignal > (outputSignalRaw >> shift))
    {
        outputSignalRaw = outputSignalRaw + 1;
    }
    else if (inputSignal < (outputSignalRaw >> shift))
    {
        outputSignalRaw = outputSignalRaw - 1;
    }
    
    *outputSignalRawPtr = outputSignalRaw;
    
    return  (outputSignalRaw >> shift);
}

/*****************************************************************************
*
* Function: void electrodePowerUp(uint8_t elecWakeUp)
*
* Description: Run electrode self-trim algorithm
*
*****************************************************************************/
void electrodeSelfTrim(void)
{
    // Device after power-up / reset ?
    if (adcDataElectrodeDischargeBufferCounter[0] < NUMBER_OF_CYCLES_DCTRACKER_PWRUP)
    {
        // Touch electrodes
        for (elecNum = 0; elecNum < (NUMBER_OF_ELECTRODES); elecNum++)
        {
            // Store touch electrode init value
            ElectodeBufferInitVal(adcDataElectrodeDischargeRaw[elecNum], &(adcDataElectrodeDischargeBuffer[elecNum]), &(adcDataElectrodeDischargeBufferCounter[elecNum]));
        }
    }
    // Calculate and load init value
    else
    {
        // Touch electrodes
        for (elecNum = 0; elecNum < (NUMBER_OF_ELECTRODES); elecNum++)
        {
            // Calculate touch electrode init value
            ElectodeCalInitVal(&(adcDataElectrodeDischargeBuffer[elecNum]), &(adcDataElectrodeDischargeBufferCounter[elecNum]));
        }

        // Set electrode status "self-trim done" flag
        electrodesStatus.bit.selfTrimDone = YES;
    }
}

/*****************************************************************************
*
* Function: void electrodeTouchQualify(void)
*
* Description: Qualify touch
*
*****************************************************************************/
void electrodeTouchQualify(void)
{
     // Reset counter of touched electrodes
     electrodeTouchAmplitudeCntr = 0;
     // For all electrodes check touch event
     for (elecNum = 0; elecNum < NUMBER_OF_TOUCH_ELECTRODES; elecNum++)
     {
         // Electrode touched?
         if (electrodeTouchThrBased[elecNum] == 1)
         {
             // Calculate amplitude
             electrodeTouchAmplitude[electrodeTouchAmplitudeCntr] = DCTrackerDataBufferThrBased[elecNum] - LPFilterIIRDataBufferThrBased[elecNum];
             // Store electrode number
             electrodeTouchNum[electrodeTouchAmplitudeCntr] = elecNum;
             // Increment counter of touched electrodes
             electrodeTouchAmplitudeCntr++;
         }
     }

     // More electrodes reports touch event?
    if (electrodeTouchAmplitudeCntr > 1)
    {
        // Load reference value for comparison
        electrodeTouchAmplitudeRef = electrodeTouchAmplitude[0];
        // Load reference value electrode number
        electrodeTouchNumRef = electrodeTouchNum[0];

        // For all electrodes reporting touch event
        for (elecNum = 1; elecNum <= electrodeTouchAmplitudeCntr; elecNum++)
        {
            // Search for electrode with maximum touch amplitude
            if (electrodeTouchAmplitude[electrodeTouchAmplitudeCntr] > electrodeTouchAmplitudeRef)
            {
                // Reference value stores the electrode with highest touch amplitude
                electrodeTouchAmplitudeRef = electrodeTouchAmplitude[electrodeTouchAmplitudeCntr];
                electrodeTouchNumRef = electrodeTouchNum[electrodeTouchAmplitudeCntr];
            }
        }

        // Clean touch reports
        for (elecNum = 0; elecNum < NUMBER_OF_TOUCH_ELECTRODES; elecNum++)
        {
            electrodeTouchQualifiedThrBased[elecNum] = 0;
        }

        // Electrode with highest touch amplitude reports touch event
        electrodeTouchQualifiedThrBased[electrodeTouchNumRef] = 1;
    }
    // Single electrode report touch event
    else if (electrodeTouchAmplitudeCntr == 1)
    {
        // Clean touch reports
        for (elecNum = 0; elecNum < NUMBER_OF_TOUCH_ELECTRODES; elecNum++)
        {
            electrodeTouchQualifiedThrBased[elecNum] = 0;
        }

        // Report touch event
        electrodeTouchQualifiedThrBased[electrodeTouchNum[0]] = 1;
    }
    // None electrode report touch event
    else
    {
        // Clean touch reports
        for (elecNum = 0; elecNum < NUMBER_OF_TOUCH_ELECTRODES; elecNum++)
        {
            electrodeTouchQualifiedThrBased[elecNum] = 0;
        }
    }
}

/*****************************************************************************
*
* Function: void electrodeSelfTrimSense(void)
*
* Description: Electrodes self-trim after power-up or reset.
*              Sampling frequency 1ms.
*
*****************************************************************************/
void electrodeSelfTrimSense(void)
{
#ifdef DEBUG_ALGORITHM
    // Pin clear
    PINS_DRV_ClearPins(DA_GPIO,1<<DA_PIN);
#endif

    // Configure all electrodes floating
    for (elecNum = 0; elecNum < NUMBER_OF_ELECTRODES; elecNum++)
    {
        electrodeFloat(elecNum);
    }

    // Convert electrode capacitance to equivalent voltage
    for (elecNum = 0; elecNum < NUMBER_OF_ELECTRODES; elecNum++)
    {
        electrodeCapToVoltConvELCH(elecNum);
    }

    // Drive all electrodes to GND
  for (elecNum = 0; elecNum < NUMBER_OF_ELECTRODES; elecNum++)
  {
    electrodeGnd(elecNum);
     }

    // Wake-up electrode used
    electrodeSelfTrim();

    // Electrodes self-trim done?
    if(electrodesStatus.bit.selfTrimDone == YES)
    {
        // Touch electrodes
        for (elecNum = 0; elecNum < (NUMBER_OF_ELECTRODES); elecNum++)
        {
            // Touch electrodes DC tracker, compensate DC tracker offset change with sampling period
            DCTrackerDataBufferThrBased[elecNum] = adcDataElectrodeDischargeBuffer[elecNum];
            // Load DC tracker buffer
            DCTrackerDataBufferRawThrBased[elecNum] = (DCTrackerDataBufferThrBased[elecNum]) << ((DCTrackerDataShiftThrBased[elecNum]));
            // Touch electrodes LP IIR filter buffer init (FM only)
            LPFilterFastDataBufferThrBased[elecNum] = DCTrackerDataBufferThrBased[elecNum];
            // Touch electrodes touch and release thresholds
            detectorThresholdTouchThrBased[elecNum] = DCTrackerDataBufferThrBased[elecNum] - detectorThresholdTouchDeltaThrBased[elecNum];
            detectorThresholdReleaseThrBased[elecNum] = DCTrackerDataBufferThrBased[elecNum] - detectorThresholdReleaseDeltaThrBased[elecNum];
        }

#ifdef WAKE_UP_ELECTRODE
        // Init IIR filter for wake-up electrode
        FilterIIR1BufferInit(WAKE_UP_ELECTRODE, ((tFrac32)(DCTrackerDataBufferThrBased[WAKE_UP_ELECTRODE])), ((tFrac32)(DCTrackerDataBufferThrBased[WAKE_UP_ELECTRODE] << 16)));

        // Reset wake-up touch variable
        electrodeTouchThrBased[WAKE_UP_ELECTRODE] = 0;

#else
        // Touch electrodes
        for (elecNum = 0; elecNum < (NUMBER_OF_ELECTRODES); elecNum++)
        {
            // Init IIR filter all electrodes
            FilterIIR1BufferInit(elecNum, ((tFrac32)(DCTrackerDataBufferThrBased[elecNum])), ((tFrac32)(DCTrackerDataBufferThrBased[elecNum] << 16)));
        }
#endif
        // Enable LPTMR
        LPTMR0_Init(LPTMR_ELEC_SENSE);

        // Actual electrode number
        elecNumAct = 0;

        // Enable low power mode
        lowPowerModeCtrl = ON;
    }

#ifdef DEBUG_ALGORITHM
    // Pin set
    PINS_DRV_SetPins(DA_GPIO,1<<DA_PIN);
#endif
}

/*****************************************************************************
*
* Function: void electrodeTouchElecSense(void)
*
* Description: Sense all keyboard electrodes
*
*****************************************************************************/
void electrodeTouchElecSense(void)
{
#ifdef DEBUG_ALGORITHM
    // Pin clear
    PINS_DRV_ClearPins(DA_GPIO,1<<DA_PIN);
#endif

    // Configure all electrodes floating
    for (elecNum = 0; elecNum < NUMBER_OF_ELECTRODES; elecNum++)
    {
        electrodeFloat(elecNum);
    }

    // Convert electrode capacitance to equivalent voltage
    for (elecNum = 0; elecNum < NUMBER_OF_TOUCH_ELECTRODES; elecNum++)
    {
        // Convert electrode capacitance to equivalent voltage
        electrodeCapToVoltConvELCH(elecNum);

        // Update DC Tracker, if electrode not touched
        if (electrodeTouchThrBased[elecNum] == 0)
        {
            // Wake-up electrode DC tracker, fed by LP filter output
            DCTrackerDataBufferThrBased[elecNum] = DCTracker(adcDataElectrodeDischargeRaw[elecNum], &(DCTrackerDataBufferRawThrBased[elecNum]), DCTrackerDataShiftThrBased[elecNum]);
        }

        // IIR LP filter
        LPFilterFastDataBufferThrBased[elecNum] = FilterIIR1(elecNum, (tFrac16)(adcDataElectrodeDischargeRaw[elecNum]));

        // Touch electrodes touch and release thresholds
        detectorThresholdTouchThrBased[elecNum] = DCTrackerDataBufferThrBased[elecNum] - detectorThresholdTouchDeltaThrBased[elecNum];
        detectorThresholdReleaseThrBased[elecNum] = DCTrackerDataBufferThrBased[elecNum] - detectorThresholdReleaseDeltaThrBased[elecNum];
        // Electrode touched ?
        if (LPFilterFastDataBufferThrBased[elecNum] < detectorThresholdTouchThrBased[elecNum])
        {
            // Touched
            electrodeTouchThrBased[elecNum] = 1;
        }
        // Electrode released?
        else if ((LPFilterFastDataBufferThrBased[elecNum] > detectorThresholdReleaseThrBased[elecNum]) & (electrodeTouchQualifiedThrBased[elecNum] == 1))
        {
            // Released
            electrodeTouchThrBased[elecNum] = 0;
        }
    }

    // Drive all electrodes to GND
     for (elecNum = 0; elecNum < NUMBER_OF_ELECTRODES; elecNum++)
     {
         electrodeGnd(elecNum);
     }

     // Qualify the touch event
     // More the one electrode reports touch?
     electrodeTouchQualify();

#ifdef DEBUG_ALGORITHM
    // Pin set
    PINS_DRV_SetPins(DA_GPIO,1<<DA_PIN);
#endif
}
