File content as of revision 21:4753996b0bcb:

#ifndef DEVICE_SERIAL_FC
    #define DEVICE_SERIAL_FC                1
#endif

#define APPLICATION_TIMEOUT
#define LOW_POWER_CONSTRAINT

#include "mbed.h"
#include "ble/BLE.h"
#include "ble/Gap.h"
#include "ringBuffer.h"
#include "application.h"

#define DEVICE_NAME                      "ket_045"
#define CODE_VERSION                     8

#define SDA_PIN                          P0_8
#define SCL_PIN                          P0_9
#define UARTTX_PIN                       P0_18
#define UARTRX_PIN                       P0_17
#define POWER_LED_PIN                    P0_11
#define INSERTION_LED_PIN                P0_10
#define ANI_BATTERY_VOLTAGE_PIN          P0_2
#define ANI_USB_CHARGE_PIN               P0_3
#define BTN_POWER_ON_PIN                 P0_4
#define ANI_SALIVA_VOLTAGE_PIN           P0_5
#define STM32_ENABLE_PIN                 P0_12
#define CHECK_BUTTON_TIMES               2
#define CHECK_BUTTON_VOLTAGE_LEVEL       0.05f

#ifdef LOW_POWER_CONSTRAINT
    #define LOW_POWER_THERS                  103        // changed 5/21
    #define LOW_POWER_THERS_MARGIN           2
    #define MAXIMUM_LOW_POWER_COUNTER        30
#endif

#define UART_BAUDRATE                    14400
#define MINIMUM_TICKER_PERIOD            2500

#define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
#define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
#define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */

#define BLE_GAP_CP_MIN_CONN_INTVL_MIN    0x0006 //Lowest mimimum connection interval permitted, in units of 1.25 ms, i.e. 7.5 ms.
#define BLE_GAP_CP_MAX_CONN_INTVL_MAX    0x0C80 //Highest maximum connection interval permitted, in units of 1.25 ms, i.e. 4 s.
#define BLE_GAP_CP_SLAVE_LATENCY_MAX     0x01F3 //Highest slave latency permitted, in connection events.
#define BLE_GAP_CP_CONN_SUP_TIMEOUT_MIN  0x000A //Lowest supervision timeout permitted, in units of 10 ms, i.e. 100 ms.
#define BLE_GAP_CP_CONN_SUP_TIMEOUT_MAX  0x0C80 //Highest supervision timeout permitted, in units of 10 ms, i.e. 32 s.

#define BLE_BUF_LEN                      20
#define BLE_RETRANSMISSION_BUF           18
#define MONITORING_PAYLOAD               5
#define SINGLE_UINT16_PAYLOAD            3
#define UART_DATA_PACKET_PAYLOAD         1
#define UART_PACKET_SIZE                 111

typedef enum {
    SYS_POWERSAVING = 0,
    SYS_RELAXING_BUTTON,
    SYS_LOCKING_BUTTON
}SystemStateTypeDef;

typedef enum {
    BTN_IDLE = 0,
    BTN_CHANGE_ANALOG,
    BTN_CHECK_LONG_PRESS
}ButtonStateTypeDef;

typedef enum {
    STM32_IDLE = 0,                                 //Cannot receive any byte from UARTRX
    STM32_CHECK_HEADER,                             //Check incoming header and forward to the client
    STM32_IMAGE_FORWARDING                          //Forward image data to the client
}STM32StateTypeDef;

typedef enum {
    SYS_HEADER_IMAGE_INFO = 0,
    SYS_HEADER_SALIVA_VOLTAGE,                      // Not used (for debug purpose)
    SYS_HEADER_MONITORING,                          // Not used (for debug purpose)
    SYS_HEADER_IMAGE_PACKET,
    SYS_HEADER_DEVICE_INFO,
    SYS_HEADER_NO_SPECIFIC
}HeaderTypeDef;

typedef enum {
    UARTRX_CHECK_FIRST_PREAMBLE = 0,
    UARTRX_CHECK_SECOND_PREAMBLE,
    UARTRX_TRANSFERING_DATA
}UARTRxStateTypeDef;

typedef enum {
    BLE_TURNON_MONITORING = 0,                      // Enable/Disable device monitoring
    BLE_CHANGE_CASSETTE_ID,                         // Write ID to saliva cassette
    BLE_REQUEST_SALIVA_VOLTAGE,                     // Turn on saliva sampling periodically
    BLE_REQUEST_IMAGE_INFO,                         // Request image header
    BLE_REQUEST_IMAGE_BY_INDEX,                     // Request image data by uart packet IDs
    BLE_END_IMAGE_TRANSFER,                         // Directly terminate image transferring
    BLE_DEREQUEST_SALIVA_VOLTAGE,                   // Turn off saliva sampling periodically
    BLE_SHUTDOWN,                                   // Shutdown device
    BLE_UNLOCK_BUTTON,                              // Unlock Button
    BLE_UART_DEBUG,                                 // Reserved for debug   Value: 0x09
    BLE_REQUEST_DEVICE_INFO,                        // Request code version
    BLE_CLOSE_MONITORING
}BLECommandTypeDef;

/* BLE Callback functions */
void bleInitCompleteCB(struct BLE::InitializationCompleteCallbackContext *context);
void bleOnConnectionCB(const Gap::ConnectionCallbackParams_t *p_conn_param);
void bleDisconnectionCB(const Gap::DisconnectionCallbackParams_t *params);
void bleCharacteristicWrittenCB(const GattWriteCallbackParams *Handler);
void bleReduceConnInterval(void);

void timerPeriodicalCB(void);
void timerFlashPowerLed(void);
void timerFlashInsertionLed(void);
void timerCheckBuffer(void);
void timerRoutineTasks(void);

void uartTxTransmit(uint8_t* buf, uint16_t length);
void uartRxCB(void);

void buttonInterruptServiceRoutine(void);
void buttonCheckLongPress(uint8_t counter);
void buttonLongPressVerified(void);
void changeButtonPinToAnalogIn(void);
void changeButtonPinToInterrupt(void);
void shutdownDevice(void);

void resetSTM32StateType(void);
void requestSalivaVoltage(void);
void requestForImagePacket(uint16_t packetId);

BLE ble;
Gap::Handle_t mHandle = 0;
Ticker ticker;
Serial pc(UARTTX_PIN, UARTRX_PIN);
I2C *pI2C = NULL;
DigitalOut ledPower(POWER_LED_PIN);              //P0_11
DigitalOut *pLedInsertion = NULL;                //P0_10            Must be changed 
DigitalOut uartEnable(STM32_ENABLE_PIN);         //P0_12
InterruptIn *pButton = NULL;                     //P0_4
AnalogIn *pButtonAnalogIn = NULL;                //P0_4
AnalogIn aniSalivaVolt(ANI_SALIVA_VOLTAGE_PIN);  //P0_5
AnalogIn aniUSBChargeVolt(ANI_USB_CHARGE_PIN);
AnalogIn aniBatterVolt(ANI_BATTERY_VOLTAGE_PIN); //P0_2 

static const uint16_t codeVersion = CODE_VERSION; 

/* State variables for application status, UARTRX data parsing status, 
    waiting header type and main loop trigger */
static SystemStateTypeDef systemState = SYS_RELAXING_BUTTON;
static ButtonStateTypeDef buttonState = BTN_IDLE;
static STM32StateTypeDef stm32State = STM32_IDLE;
static UARTRxStateTypeDef uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
static HeaderTypeDef waitHeader = SYS_HEADER_NO_SPECIFIC;

/* Variables to handle data transferring. */
static uint8_t packetNum = 0;
static uint8_t lastPacketSize = 0;
static uint8_t currentPacketIndex = 0;
static uint8_t targetPayload = 0;
static uint8_t alreadyTransferred = 0;
static uint8_t waitCamera = 0;
static const uint8_t MAX_WAIT_TARGET_PAYLOAD = 2; 
static bool isRetransmit = false;
static uint8_t retransmitIndex = 0;
static uint8_t retransmitSize = 0;
static bool setImgPktPayloadByID = false;
static bool directForward = false;

static bool isCheckBuf = false;
static bool isRoutine = false;
static bool isInserted = false;
static bool isEnterMainLoop = false;
static bool isMonitoring = false;
static bool salivaPeriodically = false;

static uint8_t checkButtonCounter = 0;

#ifdef LOW_POWER_CONSTRAINT
static bool isLowPower = false;
static uint8_t lowPowerCounter = 0;
#endif

#ifdef APPLICATION_TIMEOUT
static uint16_t disConnIdleCnt = 0;
static uint16_t connIdleCnt = 0;
static const uint16_t MAX_DISCONN_IDLE = 300;
static const uint16_t MAX_CONN_IDLE = 300;
#endif

static RingBuffer ringBuffer;
static const uint8_t preamble[] = {0xFF, 0x7F};
uint8_t uartTxPayload[BLE_BUF_LEN] = {0,};
uint8_t uartRxPayload[BLE_BUF_LEN] = {0,};
uint8_t retransmitIndices[BLE_RETRANSMISSION_BUF] = {0,};

// The Nordic UART Service
static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};

/* GATTs */
GattCharacteristic  txCharacteristic (uart_tx_uuid, uartTxPayload, 1, BLE_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);                                   
GattCharacteristic  rxCharacteristic (uart_rx_uuid, uartRxPayload, 1, BLE_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);                                
GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));

int main()
{
    pc.baud(UART_BAUDRATE);
    pc.attach(uartRxCB , pc.RxIrq);
    pc.set_flow_control(pc.Disabled);
    RingBuffer_init(&ringBuffer);
    uartEnable = 1;
    
    pI2C = new I2C(SDA_PIN, SCL_PIN);
    pLedInsertion = new DigitalOut(INSERTION_LED_PIN);
    pLedInsertion->write(1);
    ledPower = 0;
    
    ticker.attach_us(timerPeriodicalCB, MINIMUM_TICKER_PERIOD);
    pButton = new InterruptIn(BTN_POWER_ON_PIN);
    pButton->fall(buttonInterruptServiceRoutine);
    
    ble.init(bleInitCompleteCB);
    ble.onConnection(bleOnConnectionCB);
    ble.onDisconnection(bleDisconnectionCB);
    ble.onDataWritten(bleCharacteristicWrittenCB); 

    // Adjust interrupt priority
//    NVIC_SetPriority(RADIO_IRQn, 0);
//    NVIC_SetPriority(UART0_IRQn, 1);
//    NVIC_SetPriority(RTC1_IRQn, 2);
    
    // setup advertising     // Restricted to 31 bytes
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, (const uint8_t *)"BLETest", sizeof("BLETest") - 1);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
    ble.gap().setDeviceName("BLETest");                             
    
    ble.gap().accumulateScanResponse(GapAdvertisingData::COMPLETE_LOCAL_NAME, (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1); 
    ble.setAdvertisingInterval(160);                // 100ms; in multiples of 0.625ms.
    ble.addService(uartService);
    
    ble.startAdvertising(); 
    
    while(true){
        
        #ifdef LOW_POWER_CONSTRAINT
        if(lowPowerCounter > MAXIMUM_LOW_POWER_COUNTER){
            shutdownDevice();
        }
        #endif
        
        #ifdef APPLICATION_TIMEOUT
        if(disConnIdleCnt >= MAX_DISCONN_IDLE && systemState == SYS_RELAXING_BUTTON){
            shutdownDevice();
        }
        else if(connIdleCnt >= MAX_CONN_IDLE){
            if(mHandle != 0)
                ble.disconnect(mHandle, Gap::CONNECTION_TIMEOUT);
            systemState = SYS_RELAXING_BUTTON;
        }
        #endif
        
        switch (systemState) {
        case SYS_POWERSAVING:
        case SYS_RELAXING_BUTTON:
            switch (buttonState) {
            case BTN_CHANGE_ANALOG:
                changeButtonPinToAnalogIn();
                break;
            case BTN_CHECK_LONG_PRESS:
                buttonCheckLongPress(checkButtonCounter);
                break;
            case BTN_IDLE:
            default:
                break;
            }
            break;
        case SYS_LOCKING_BUTTON:
            buttonState = BTN_IDLE;
            break;
        }
        
        if(isCheckBuf){
            isCheckBuf = false;
            timerCheckBuffer();
        }
        
        if(isRoutine){
            isRoutine = false;
            timerRoutineTasks();
        }
        
        if( isEnterMainLoop ){
            switch (stm32State){
            case STM32_IMAGE_FORWARDING:
            {
                if (setImgPktPayloadByID == false){
                    uint8_t check = 0;
                    RingBuffer_read(&ringBuffer, &check, 1);
                    HeaderTypeDef header = static_cast<HeaderTypeDef>(check);
                    if (header == SYS_HEADER_IMAGE_PACKET){
                        if (currentPacketIndex != (packetNum-1))
                            targetPayload = UART_PACKET_SIZE + 3;
                        else
                            targetPayload = lastPacketSize + 3;
                    
                        alreadyTransferred = 0;
                        setImgPktPayloadByID = true;
                        waitCamera = 0;
                    }
                }
                else {
                    uint16_t numBytes = RingBuffer_availableDataAmount(&ringBuffer);
                    while((numBytes >= (BLE_BUF_LEN-1)) || (directForward == true)){
                        waitCamera = 0;
                        uint16_t diff = numBytes;
                        if(diff > (BLE_BUF_LEN-1))
                            diff = (BLE_BUF_LEN-1);
                        uartRxPayload[0] = static_cast<uint8_t>(SYS_HEADER_IMAGE_PACKET);
                        
                        RingBuffer_read(&ringBuffer, uartRxPayload+1, diff);
                        
                        if(mHandle != 0){
                            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), uartRxPayload, diff+1);
                            alreadyTransferred += diff;
                            numBytes -= diff;
                        }
                        
                        if(directForward == true || alreadyTransferred >= targetPayload){
                            __disable_irq();                                // Disable Interrupts
                            setImgPktPayloadByID = false;
                            __enable_irq();  
                            directForward = false;
                            RingBuffer_init(&ringBuffer);
                            
                            if(isRetransmit == false){
                                currentPacketIndex++;
                            }
                            else{
                                retransmitIndex++;
                                if(retransmitIndex >= retransmitSize){
                                    retransmitSize = 0;
                                    resetSTM32StateType();
                                    break;
                                }
                                else{
                                    currentPacketIndex = retransmitIndices[retransmitIndex];
                                }
                            }
                            
                            if(currentPacketIndex == packetNum && isRetransmit == false){
                                /* TODO retransmission */
                                isRetransmit = true;
                                if(retransmitSize > 0){
                                    uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
                                    retransmitIndex = 0;
                                    currentPacketIndex = retransmitIndices[retransmitIndex];
                                    requestForImagePacket(currentPacketIndex);
                                }
                                else{
                                    resetSTM32StateType();
                                }
                            }
                            else{
                                uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
                                requestForImagePacket(currentPacketIndex);
                            }
                            break;
                        }
                    }    
                }
                break;
            }
            case STM32_CHECK_HEADER:
            {
                uint8_t buf[BLE_BUF_LEN] = {0,};
                HeaderTypeDef header;  
                switch (waitHeader) {
                /* Image info */
                case SYS_HEADER_IMAGE_INFO:
                {
                    RingBuffer_read(&ringBuffer, buf, 1);
                    header = static_cast<HeaderTypeDef>(buf[0]);
                    if (header == SYS_HEADER_IMAGE_INFO){
                        RingBuffer_read(&ringBuffer, buf+1, SINGLE_UINT16_PAYLOAD-1);
                        if(mHandle != 0)
                            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, SINGLE_UINT16_PAYLOAD);
                        
                        uint16_t totalDataLength = buf[1]*256 + buf[2];
                        packetNum = totalDataLength/UART_PACKET_SIZE;
                        lastPacketSize = totalDataLength%UART_PACKET_SIZE;
                        if(totalDataLength%UART_PACKET_SIZE != 0)
                            packetNum++;
                        else
                            lastPacketSize = UART_PACKET_SIZE;
                    
                        currentPacketIndex = 0;
                        resetSTM32StateType();
                    }
                    break;
                }
                case SYS_HEADER_SALIVA_VOLTAGE:
//                {
//                    RingBuffer_read(&ringBuffer, buf, 1);
//                    header = static_cast<HeaderTypeDef>(buf[0]);
//                    if (header == SYS_HEADER_SALIVA_VOLTAGE){
//                        RingBuffer_read(&ringBuffer, buf+1, SINGLE_UINT16_PAYLOAD-1);
//                        if(mHandle != 0)
//                            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, SINGLE_UINT16_PAYLOAD);
//
//                        stm32State = STM32_CHECK_HEADER;
//                        uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
//                    }
//                    break;
//                }
                case SYS_HEADER_NO_SPECIFIC:
                default:
                    break;
                }
                break;
            }
            case STM32_IDLE:
            default:
                break;
            }
            isEnterMainLoop = false;
        }
        ble.waitForEvent();
    }
}

void bleInitCompleteCB(struct BLE::InitializationCompleteCallbackContext *context){
    if (context->error != BLE_ERROR_NONE)
        return;

    if (context->ble.getInstanceID() == BLE::DEFAULT_INSTANCE){
        /* use the BLE instance */
        return;
    }
}

void bleOnConnectionCB(const Gap::ConnectionCallbackParams_t *p_conn_param){
    mHandle = p_conn_param->handle;
    #ifdef APPLICATION_TIMEOUT
    connIdleCnt = 0;
    #endif
    bleReduceConnInterval();
}

void bleDisconnectionCB(const Gap::DisconnectionCallbackParams_t *params){
    mHandle = 0;
    if(systemState != SYS_POWERSAVING){
    #ifdef APPLICATION_TIMEOUT
        disConnIdleCnt = 0;
    #endif
        ble.startAdvertising();
    }
}

void bleCharacteristicWrittenCB(const GattWriteCallbackParams *Handler){
    uint16_t bleBytesRead;
    if (Handler->handle == txCharacteristic.getValueAttribute().getHandle()) 
    {
    #ifdef APPLICATION_TIMEOUT
        connIdleCnt = 0;
    #endif
        ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), uartTxPayload, &bleBytesRead);
        if(bleBytesRead == 0)
            return;
                 
        BLECommandTypeDef command = static_cast<BLECommandTypeDef>(uartTxPayload[0]);
        bool isUARTCommand = false;
        switch(command){
        case BLE_TURNON_MONITORING:
            isMonitoring = true;
            break;
        case BLE_CLOSE_MONITORING:
            isMonitoring = false;
            break;
        case BLE_REQUEST_SALIVA_VOLTAGE:
            waitHeader = SYS_HEADER_SALIVA_VOLTAGE;
            salivaPeriodically = true;
            systemState = SYS_LOCKING_BUTTON;       // Lock Begin
            break;
        case BLE_DEREQUEST_SALIVA_VOLTAGE:
            salivaPeriodically = false;
            break;
        case BLE_REQUEST_IMAGE_INFO:
            waitHeader = SYS_HEADER_IMAGE_INFO;
            isUARTCommand = true;
            salivaPeriodically = false;
            break;
        case BLE_END_IMAGE_TRANSFER:
            isRetransmit = false;
            packetNum = 0;
            resetSTM32StateType();
            break;
        case BLE_SHUTDOWN:
            changeButtonPinToInterrupt();
            shutdownDevice();
            break;
        case BLE_UNLOCK_BUTTON:
        {
            systemState = SYS_RELAXING_BUTTON;
            buttonState = BTN_IDLE;
            break;
        }
        case BLE_UART_DEBUG:
        {
            uint8_t debugHeader = 0xF1;
            uartTxTransmit(&debugHeader, 1);
            break;
        }
        case BLE_REQUEST_IMAGE_BY_INDEX:
            if(bleBytesRead >= 2){
                uint8_t check = uartTxPayload[1];
                if(check == 0){
                    if(packetNum > 0){
                        setImgPktPayloadByID = false;
                        directForward = false;
                        RingBuffer_init(&ringBuffer);
                        currentPacketIndex = 0;
                        isRetransmit = false;
                        retransmitSize = 0;
                        stm32State = STM32_IMAGE_FORWARDING;
                        uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
                        requestForImagePacket(currentPacketIndex);
                    }
                }
                else if(check <= 18){
                    if(bleBytesRead < check+2)
                        break;
                    retransmitSize = check;
                    memcpy(retransmitIndices, uartTxPayload+2, retransmitSize);
                    if(isRetransmit == true){
                        stm32State = STM32_IMAGE_FORWARDING;
                        uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
                        retransmitIndex = 0;
                        currentPacketIndex = retransmitIndices[retransmitIndex];
                        requestForImagePacket(currentPacketIndex);
                    }
                    else{
                        uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
                        requestForImagePacket(currentPacketIndex);
                    }
                }
                else{
                    /* Invalid check number */
                }
            }
            break;
        case BLE_CHANGE_CASSETTE_ID:
            if(bleBytesRead >= 3)
                writeAT24EEPROMBuffer(*pI2C, 0, (char*)uartTxPayload+1, 2);
            break;
        case BLE_REQUEST_DEVICE_INFO:
        {
            uint8_t buf[SINGLE_UINT16_PAYLOAD] = {0,};
            buf[0] = SYS_HEADER_DEVICE_INFO;
            buf[1] = (codeVersion >> 8) & 0xFF;
            buf[2] = codeVersion & 0xFF;
            if(mHandle != 0)
                ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, SINGLE_UINT16_PAYLOAD);
                
            break;
        }
        default:
            break;
        }
        if(isUARTCommand){
            stm32State = STM32_CHECK_HEADER;
            uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
            uartTxTransmit(uartTxPayload, 1);
        }
    }
}

void bleReduceConnInterval(void){
    Gap::ConnectionParams_t bleGapConnParamInitStruct;
    bleGapConnParamInitStruct.minConnectionInterval = BLE_GAP_CP_MIN_CONN_INTVL_MIN;    /*Minimum Connection Interval in 1.25 ms*/                       
    bleGapConnParamInitStruct.maxConnectionInterval = BLE_GAP_CP_MIN_CONN_INTVL_MIN;    /*Minimum Connection Interval in 1.25 ms*/   
    bleGapConnParamInitStruct.slaveLatency = 0;                                         /*Slave Latency in number of connection events*/            
    bleGapConnParamInitStruct.connectionSupervisionTimeout = BLE_GAP_CP_CONN_SUP_TIMEOUT_MIN; /*Connection Supervision Timeout in 10 ms*/

    ble.gap().updateConnectionParams(mHandle, &bleGapConnParamInitStruct);
}

void timerPeriodicalCB(void){
    static uint32_t time = 0;
    time++;
    
    if ( time % 100 == 2 ) {                            // 250ms
        if(buttonState == BTN_CHECK_LONG_PRESS)
            checkButtonCounter++;
    }
    
    if(systemState != SYS_POWERSAVING){
        if ( time % 3 == 1 ) {                          // 7.5ms
            isCheckBuf = true;
        }
        
//        if ( time % 50 == 2 ) {                         // 250ms
//            if(isInserted)
//                timerFlashPowerLed();
//        }

        if ( time % 200 == 3 ) {                        // 500ms
            #ifdef LOW_POWER_CONSTRAINT
            if(isLowPower){
                timerFlashPowerLed();
                lowPowerCounter++;
            }
            #endif
            
            if(isInserted && systemState == SYS_LOCKING_BUTTON){
                timerFlashInsertionLed();
            }   
        }
    
        if ( time % 400 == 4 ) {                       // 1s
            isRoutine = true;
            #ifdef APPLICATION_TIMEOUT
            if(mHandle == 0)
                disConnIdleCnt++;
            else
                connIdleCnt++;
            #endif
        }
        
        if ( time % 800 == 5 ) {
            if(salivaPeriodically == true)
                requestSalivaVoltage();
        }
    }
}

void timerFlashPowerLed(void){
    ledPower = !ledPower;
}

void timerFlashInsertionLed(void){
    int tmp = pLedInsertion->read();
    pLedInsertion->write(!tmp);
}

void timerRoutineTasks(void){
    uint8_t buf[BLE_BUF_LEN] = {0,};
    buf[0] = SYS_HEADER_MONITORING; 
    bool result = readAT24EEPROMBuffer(*pI2C, 0, (char*)buf+1, 2);
    
    if(result == false){
        pLedInsertion->write(1);
        isInserted = false;
        buf[1] = 0xFF;
        buf[2] = 0xFF;
        
        // If no ID detected, system will unlock button automatically.  5/17
        if(systemState == SYS_LOCKING_BUTTON){
            systemState = SYS_RELAXING_BUTTON;
            buttonState = BTN_IDLE;
        }
    }
    else{
        isInserted = true;
        if(systemState != SYS_LOCKING_BUTTON){
            pLedInsertion->write(0);
        }
            
        if(systemState == SYS_POWERSAVING){
            pLedInsertion->write(1);
        }
    }
    
    float fvalue = (float)aniBatterVolt.read()*3.3*100;
    uint16_t ivalue = (uint16_t)fvalue;
    buf[3] = (ivalue >> 8) & 0xFF;
    buf[4] = ivalue & 0xFF;
    
    #ifdef LOW_POWER_CONSTRAINT
    if(!isLowPower && ivalue < (LOW_POWER_THERS-LOW_POWER_THERS_MARGIN)){
        isLowPower = true;
        lowPowerCounter = 0;
    }
    else if(isLowPower && ivalue > (LOW_POWER_THERS+LOW_POWER_THERS_MARGIN)){
        isLowPower = false;
    }
    #endif
    
    if(mHandle != 0 && isMonitoring == true){
        ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, MONITORING_PAYLOAD);
    }
}

void uartTxTransmit(uint8_t* buf, uint16_t length){
    uint16_t index;
    pc.putc(preamble[0]); 
    pc.putc(preamble[1]); 
    for(index = 0; index < length; index++)
    {
        pc.putc(buf[index]);        
    }
}

// UARTRX Callback function
void uartRxCB(void){   
    while(pc.readable())    
    {
        uint8_t byteIn = pc.getc();
        if(stm32State == STM32_IDLE)
            continue;
    
        if(uartRxState == UARTRX_TRANSFERING_DATA){
            RingBuffer_writebyte(&ringBuffer, byteIn);
        }
        else if(uartRxState == UARTRX_CHECK_FIRST_PREAMBLE && byteIn == preamble[0]){
            uartRxState = UARTRX_CHECK_SECOND_PREAMBLE;
        }
        else if(uartRxState == UARTRX_CHECK_SECOND_PREAMBLE && byteIn == preamble[1]){
            uartRxState = UARTRX_TRANSFERING_DATA;
        }
    }
}

void buttonInterruptServiceRoutine(void){
    if(buttonState == BTN_IDLE)
        buttonState = BTN_CHANGE_ANALOG;
        
    if(systemState == SYS_POWERSAVING)
        ticker.attach_us(timerPeriodicalCB, MINIMUM_TICKER_PERIOD);
}

void buttonCheckLongPress(uint8_t counter){
    if(counter < CHECK_BUTTON_TIMES){
        if(pButtonAnalogIn != NULL && (float)pButtonAnalogIn->read() < CHECK_BUTTON_VOLTAGE_LEVEL){
            return;
        }
        else if(systemState == SYS_POWERSAVING){
            ticker.detach();
        }
    }
    else{
        buttonLongPressVerified();
    }
    changeButtonPinToInterrupt();
}

void buttonLongPressVerified(void){
    checkButtonCounter = 0;
    if(systemState == SYS_POWERSAVING){
        systemState = SYS_RELAXING_BUTTON;
        ble.startAdvertising();
        #ifdef APPLICATION_TIMEOUT
        disConnIdleCnt = 0;
        #endif
        ticker.attach_us(timerPeriodicalCB, MINIMUM_TICKER_PERIOD);
        ledPower = 0;
        uartEnable = 1;
        resetSTM32StateType();
    }
    else if(systemState == SYS_RELAXING_BUTTON){
        shutdownDevice();
    }
}

void changeButtonPinToAnalogIn(void){
    pButton->fall(NULL);
    delete pButton;
    checkButtonCounter = 0;
    pButtonAnalogIn = new AnalogIn(BTN_POWER_ON_PIN);
    buttonState = BTN_CHECK_LONG_PRESS;    
}

void changeButtonPinToInterrupt(void){
    delete pButtonAnalogIn;
    pButton = new InterruptIn(BTN_POWER_ON_PIN);
    pButton->fall(buttonInterruptServiceRoutine);
    float dummy = (float)pButtonAnalogIn->read(); 
    buttonState = BTN_IDLE;
}

void shutdownDevice(void){
    systemState = SYS_POWERSAVING;
    if(mHandle != 0)
        ble.disconnect(mHandle, Gap::REMOTE_USER_TERMINATED_CONNECTION);
    pLedInsertion->write(1);
    ticker.detach();
    ble.gap().stopAdvertising();
    ledPower = 1;
    uartEnable = 0;
    isMonitoring = false;
    salivaPeriodically = false;
    #ifdef LOW_POWER_CONSTRAINT
    lowPowerCounter = 0;
    #endif
    deepsleep();
}

void resetSTM32StateType(void){
    stm32State = STM32_IDLE;
    uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
    waitHeader = SYS_HEADER_NO_SPECIFIC;
}

void requestSalivaVoltage(void){
    uint8_t buf[BLE_BUF_LEN] = {0,};
    float fvalue = aniSalivaVolt.read()*3.3*100;
    uint16_t ivalue = (uint16_t)fvalue;
    
    buf[0] = SYS_HEADER_SALIVA_VOLTAGE;
    buf[1] = (ivalue >> 8) & 0xFF;
    buf[2] = ivalue & 0xFF;
    
    if(mHandle != 0)
        ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, SINGLE_UINT16_PAYLOAD);
}

void timerCheckBuffer(void){
    uint16_t numBytes = RingBuffer_availableDataAmount(&ringBuffer);
    switch (stm32State) {
    case STM32_CHECK_HEADER:{
        if (numBytes >= SINGLE_UINT16_PAYLOAD){
            isEnterMainLoop = true;
        }
        break;
    }
    case STM32_IMAGE_FORWARDING:
    {
        if(setImgPktPayloadByID == false){
            if(numBytes >= UART_DATA_PACKET_PAYLOAD){
                isEnterMainLoop = true;
            }
            else{
                if (waitCamera > MAX_WAIT_TARGET_PAYLOAD){
                    setImgPktPayloadByID = false;
                    directForward = false;
                    RingBuffer_init(&ringBuffer);
                    requestForImagePacket(currentPacketIndex);
                    waitCamera = 0;
                }
            }
        }
        else {
            if((targetPayload-alreadyTransferred) <= (BLE_BUF_LEN-1)){
                if(numBytes+alreadyTransferred == targetPayload)
                    directForward = true;
            }
            
            if (numBytes > 0){
                isEnterMainLoop = true;
            }
            
            else if (waitCamera > MAX_WAIT_TARGET_PAYLOAD){
                uartRxState = UARTRX_CHECK_FIRST_PREAMBLE;
                requestForImagePacket(currentPacketIndex);
                waitCamera = 0;
            }
       
        }
        break;
    }
    default:
        break;
    }
}

void requestForImagePacket(uint16_t packetId){
    uint8_t tempBuf[3];
    tempBuf[0] = static_cast<uint8_t>(BLE_REQUEST_IMAGE_BY_INDEX);
    tempBuf[1] = (packetId >> 8) & 0xFF;
    tempBuf[2] = packetId & 0xFF;
    uartTxTransmit(tempBuf, 3);
}

void SerialBase::set_flow_control(Flow type, PinName flow1, PinName flow2) {
    FlowControl flow_type = (FlowControl)type;
    switch(type) {
        case RTS:
            serial_set_flow_control(&_serial, flow_type, flow1, NC);
            break;

        case CTS:
            serial_set_flow_control(&_serial, flow_type, NC, flow1);
            break;

        case RTSCTS:
        case Disabled:
            serial_set_flow_control(&_serial, flow_type, flow1, flow2);
            break;

        default:
            break;
    }
}