File content as of revision 20:d62cec0a0f5c:

#include "mbed.h"
#include "mbed_events.h"
#include "MAX30101.h"
#include "string.h"
#include "Hexi_OLED_SSD1351.h"
#include "Hexi_KW40Z.h"

#include "OLED_types.h"
#include "OpenSans_Font.h"

#include <math.h>
#include <vector>
using namespace std;

#define FIFO_DATA_MAX 288
//#define M_PI 3.14159265358979323846
#define M_PI 3.14159

void UpdateSensorData(void);
void StartHaptic(void);
void StopHaptic(void const *n);
void txTask(void);

DigitalOut blueLed(LED3,1); // i added this

DigitalOut pwr1v8(PTA29);
DigitalOut pwr3v3b(PTC13);
DigitalOut pwr15v(PTB12);
I2C i2c0(PTB1, PTB0);
InterruptIn maximInterrupt(PTB18);
Serial pc(USBTX, USBRX);
SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15);
KW40Z kw40z_device(PTE24, PTE25);
DigitalOut haptic(PTB9);
EventQueue evqueue(32 * EVENTS_EVENT_SIZE);
Thread t;
/* Define timer for haptic feedback */
RtosTimer hapticTimer(StopHaptic, osTimerOnce);

/*Create a Thread to handle sending BLE Sensor Data */
Thread txThread;
MAX30101 hr(i2c0);
int ppg_single_sample;
int mask_ppg = 0;
uint32_t count = 0;
uint32_t num;
// uint8_t testsignal = 60;

uint8_t battery = 0;

char text[20];  /* Text Buffer */

// I added this
const int num_samples = 500; // for 5 sec
int ppg[num_samples];
int SDNN, valence_arousal;
double HF_LF = 0.0;
double HF_LF_n;
int SDNN_n = 0;
bool ready = false;
bool ready_to_send = false;

void StartHaptic(void)
{
    hapticTimer.start(50);
    haptic = 1;
}
void ButtonRight(void)
{
    StartHaptic();
    kw40z_device.ToggleAdvertisementMode();
}

void ButtonLeft(void)
{
    StartHaptic();
    kw40z_device.ToggleAdvertisementMode();
}

void StopHaptic(void const *n)
{
    haptic = 0;
    hapticTimer.stop();
}

void PassKey(void)
{
    StartHaptic();
    strcpy((char *) text,"PAIR CODE");
    oled.TextBox((uint8_t *)text,0,25,95,18);
  
    /* Display Bond Pass Key in a 95px by 18px textbox at x=0,y=40 */
    sprintf(text,"%d", kw40z_device.GetPassKey());
    oled.TextBox((uint8_t *)text,0,40,95,18);
}

void txTask(void)
{

    while (true) {
        /*
        while(ready_to_send == false) {
            Thread::wait(1000); // wait until has something to send
        }
*/

        if(ready_to_send) {
            UpdateSensorData();
            
            printf("Sending!");
            kw40z_device.SendBatteryLevel(battery);
            
            ready_to_send = false;
        }
        /*Notify Hexiwear App that it is running Sensor Tag mode*/
        kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
        
        
        //send heartrate
//        kw40z_device.SendHeartRate(testsignal);
        /*The following is sending dummy data over BLE. Replace with real data*/

        /*Send Battery Level for 20%
        kw40z_device.SendBatteryLevel(battery);

        Send Ambient Light Level at 50%
        kw40z_device.SendAmbientLight(light);*/

        /*Send Humidity at 90% */
        //kw40z_device.SendHumidity(humidity);

        /*Send Temperature at 25 degrees Celsius
        kw40z_device.SendTemperature(temperature);

        //Send Pressure at 100kPA 
        //kw40z_device.SendPressure(pressure); */

        /*Send Mag,Accel,Gyro Data.
        kw40z_device.SendGyro(x,y,z);
        kw40z_device.SendAccel(z,x,y);
        kw40z_device.SendMag(y,z,x);*/

        Thread::wait(1000);
    }
}
void UpdateSensorData(void)
{
//    testsignal+=1;
    battery = valence_arousal;
    /*battery -= 5;
    if(battery < 5) battery = 100;

    light += 20;
    if(light > 100) light = 0;

    humidity += 500;
    if(humidity > 8000) humidity = 2000;

    temperature -= 200;
    if(temperature < 200) temperature = 4200;

    pressure += 300;
    if(pressure > 10300) pressure = 7500;

    x += 1400;
    y -= 2300;
    z += 1700;*/
}

void interruptHandlerQueued()
{
//    int temp_ppg[num_samples];
    for(int iter = 0; iter < num_samples; iter+=0) {
        
        MAX30101::InterruptBitField_u interruptStatus;
        hr.getInterruptStatus(interruptStatus);
//    printf("Interrupt Status: 0x%02x\r\n", interruptStatus.all);

        if (interruptStatus.bits.pwr_rdy == 0x1) {
//        printf("Powered on\r\n");

            // Soft reset
            MAX30101::ModeConfiguration_u modeConf;
            modeConf.all = 0;
            modeConf.bits.reset = 1;
            hr.setModeConfiguration(modeConf);
            wait(0.01);

            // Configure FIFO
            MAX30101::FIFO_Configuration_u fifoConf;
            hr.getFIFOConfiguration(fifoConf);
//        pc.printf("FIFO Configuration: 0x%02x\r\n", fifoConf.all);

            // Set LED power
            hr.setLEDPulseAmplitude(MAX30101::LED1_PA, 0x0C);
            hr.setLEDPulseAmplitude(MAX30101::ProxModeLED_PA, 0x19);
//        pc.printf("LED set\r\n");

            MAX30101::SpO2Configuration_u spo2Conf;
            hr.getSpO2Configuration(spo2Conf);
            spo2Conf.bits.led_pw = MAX30101::PW_1;
            spo2Conf.bits.spo2_sr = MAX30101::SR_100_Hz;
            hr.setSpO2Configuration(spo2Conf);
            hr.getSpO2Configuration(spo2Conf);
//        pc.printf("SpO2 Configuration: 0x%02x\r\n", spo2Conf.all);

            // Proximity settings
            hr.setProxIntThreshold(0x14);

            // Enable HR mode
            modeConf.all = 0;
            modeConf.bits.mode = MAX30101::HeartRateMode;
            hr.setModeConfiguration(modeConf);
//        printf("Mode set\r\n");
        }

        if (interruptStatus.bits.prox_int == 0x1) {
//        printf("Proximity Triggered, entered HR Mode.");
        }

        if (interruptStatus.bits.ppg_rdy == 0x1) {
//        printf("PPG Ready.\r\n");
            mask_ppg = 1;
        }

        if (interruptStatus.bits.a_full == 0x1) {
//        printf("FIFO Almost Full.\r\n");
            uint8_t data[FIFO_DATA_MAX];
            uint16_t readBytes = 0;

            hr.readFIFO(MAX30101::OneLedChannel, data, readBytes);
//            printf("data length: %u \r\n",readBytes);
            //printf("data length: %u \r\n",data);
            for (uint16_t i = 0; i < readBytes; i += 3) {
                uint8_t sample[4] = {0};
                sample[0] = data[i + 2];
                sample[1] = data[i + 1];
                sample[2] = data[i];

                num = *(uint32_t *) sample;
                
                if (num < 0) {
                    ppg_single_sample = 0;
//                    printf("keep closer to your hand \r\n");
                } else {

                    //ppg_single_sample = 65;
                    ppg_single_sample = num;
                    if(iter < num_samples && !ready) {
                        ppg[iter] = num;                        
//                        printf("%d   \n", ppg[iter]);
                        if(iter == num_samples-1)
                            ready = true;
                    }

//                    printf("%d   ", ppg_single_sample); // I commented this out
                    iter++;
                }
//                printf("%d; %d\r\n", iter, ppg[iter-1]);


            }
        }

        interruptStatus.all = 0xFF;

        if (mask_ppg == 1) {
            interruptStatus.bits.ppg_rdy = 0;
        }
        hr.enableInterrupts(interruptStatus);
    }
    /*
    for(int i = 0; i < num_samples; i++) {
        ppg[i] = temp_ppg[i];
    }
    */
    //ppg = temp_ppg;

}

void interruptHandler()
{
    evqueue.call(interruptHandlerQueued);
//    interruptHandlerQueued();
//    printf("\nLezzgo\n");
    if(ready) {
        printf("\nStarting...  ");

        int i = 0;
        int j = 0;
        
        double TIME[num_samples];
        for(int i = 0; i < num_samples; i++) {
            TIME[i] = (double)i*(5.0/(double)num_samples); // change to 30.0 later
        }
        
        // moving average
        double movave[num_samples];
        int avecap = 25;
        for(i = 0; i < (avecap-1)/2; i++) {
            movave[i] = (double)ppg[0];
            for(j = 1; j < 1+(avecap-1)/2; j++) {
                movave[i] = movave[i] + (double)ppg[j];
            }
            movave[i] = (double)(movave[i]/(i+(avecap-1)/2-1));
        }
        for(i = (num_samples-(avecap-1)/2); i < num_samples; i++) {
            movave[i] = (double)ppg[i-(avecap-1)/2-1];
            for(j = (i-(avecap-1)/2); j < num_samples; j++) {
                movave[i] = movave[i] + (double)ppg[j];
            }
            movave[i] = movave[i]/(double)(num_samples-i+(avecap-1)/2);
        }
        for(i = (avecap-1)/2; i < (num_samples-(avecap-1)/2); i++) {
            movave[i] = (double)ppg[i-(avecap-1)/2-1];
            for(j = i-(avecap-1)/2; j < i+(avecap-1)/2; j++) {
                movave[i] = movave[i] + (double)ppg[j];
            }
            movave[i] = movave[i]/(double)avecap;
        }

        // normalize ppg
        for(i = 0; i < num_samples; i++) {
            ppg[i] = ppg[i] - (int)movave[i];
            if(ppg[i] > 1000 || ppg[i] < -1000)
                ppg[i] = 0;
        }

        // smoothing curve
        for(i = 1; i < num_samples; i++) {
            ppg[i] = ppg[i] + ppg[i-1];
            //printf("%d   ", ppg[i]);
        }

        // AMPD Algorithm
        const int kcap = 25;
        int m[kcap][num_samples];
        for(int k = 1; k <= kcap; k++) {
            for(i = 1; i < num_samples; i++) {
                if(i-1 < 0 || i-k-1 < 0 || i+k-1 >= num_samples)
                    m[k-1][i] = 0;
                else if(ppg[i-1] > ppg[i-k-1] && ppg[i-1] > ppg[i+k-1])
                    m[k-1][i] = 1;
                else
                    m[k-1][i] = 0;
            }
        }
        
        int max_mult[num_samples];
        for(i = 0; i < num_samples; i++) { // max_mult = m(1,:)';
            max_mult[i] = m[0][i];
        }

        for(int k = 1; k < kcap; k++) {
            for(i = 0; i < num_samples; i++) {
                max_mult[i] = max_mult[i]*m[k][i];
            }
        }
        
        int num_max = 0;
        // extract times that are max
        for(i = 0; i < num_samples; i++) {
            num_max = num_max + max_mult[i];
        }

//    printf("num_max = %d    ", num_max);

        vector<double> time_of_max;
        vector<int> index_of_max;
        vector<int> max_points;
        for(i = 0; i < num_samples; i++) {
            if(max_mult[i] == 1) {
                time_of_max.push_back(TIME[i-1]);
                index_of_max.push_back(i-1);
                max_points.push_back(ppg[i-1]);
            }
        }

        // calculating HRV
        vector<double> r;
        vector<int> index_r;
        double mean_inter_time = 0;
        for(i = 0; i < num_max-1; i++) {
            r.push_back(time_of_max.at(i+1)-time_of_max.at(i));
            index_r.push_back(index_of_max.at(i+1) - index_of_max.at(i));
            mean_inter_time = mean_inter_time + r.at(i);
        }

        //printf("r: %.2f %.2f %.2f   ", r.at(0), r.at(1), r.at(2));
        mean_inter_time = mean_inter_time/(double)(num_max-1);

        // getting rid of outlier points in r
        for(i = 0; i < num_max-1; i++) {
            if(r.at(i) > mean_inter_time + 0.11)
                r.at(i) = mean_inter_time + 0.11;
            else if(r.at(i) < mean_inter_time - 0.11)
                r.at(i) = mean_inter_time - 0.11;
        }

        // SDNN -- std of normal to normal R-R intervals
        mean_inter_time = 0;
        for(i = 0; i < num_max-1; i++) {
            mean_inter_time = mean_inter_time + r.at(i);
        }
        mean_inter_time = double(mean_inter_time/(num_max-1));
        double SDNN_doub = 0.0;
        for(i = 0; i < num_max-1; i++) {
            SDNN_doub = SDNN_doub + (r.at(i)-mean_inter_time)*(r.at(i)-mean_inter_time);
        }
        SDNN = (int)(sqrt(SDNN_doub/(num_max-1))*1000);
        printf("SDNN = %d    ", SDNN);

        // TIME TO CALCULATE HF/LF
        // FFT: use movave as fftppg 
        for(i = 0; i < num_samples; i++) {
            double real = 0;
            double im = 0;
            double dum_ppg;
            for(j = 0; j < num_samples; j++) {
                dum_ppg = (double) ppg[j];
                real = real + dum_ppg * cos(2.0*M_PI*(double)(j*i)/(double)num_samples);
                im = im + dum_ppg * sin(2.0*M_PI*(double)(j*i)/(double)num_samples);
            }
            movave[i] = sqrt(real*real + im*im)/(double)num_samples;
        }
        
        
        // make frequency array: use TIME
        double sampling_freq = (double)(1/TIME[1]);
        for(i = 0; i < num_samples; i++) {
            TIME[i] = sampling_freq*(double)(i)/(double)(num_samples);
        }

        double LF = 0.0;
        i = 0;
        while(TIME[i] < 0.15) {
            LF = LF + movave[i];
            i++;
        }

        double HF = 0.0;
        while(TIME[i] < 0.4) {
            HF = HF + movave[i];
            i++;
        }
        HF_LF = HF/LF;
        printf("HF/LF = %.2f    \n", HF_LF);
        
        if(SDNN_n < 1) {
            SDNN_n = SDNN;
            HF_LF_n = HF_LF;
            valence_arousal = 0;
        }
        else {
            // mapping SDNN, HF/LF to valence, arousal
            int comp_sdnn = SDNN-SDNN_n;
            if(comp_sdnn > 20)
                valence_arousal = 5;
            else if(comp_sdnn > 5)
                valence_arousal = 4;
            else if(comp_sdnn > -5)
                valence_arousal = 3;
            else if(comp_sdnn > -20)
                valence_arousal = 2;
            else
                valence_arousal = 1;
            
            double comp_hf_lf = HF_LF/HF_LF_n;
            if(comp_hf_lf > 5.0)
                valence_arousal += 50;
            else if(comp_hf_lf > 1.5)
                valence_arousal += 40;
            else if(comp_hf_lf > 0.9)
                valence_arousal += 30;
            else if(comp_hf_lf > 0.7)
                valence_arousal += 20;
            else
                valence_arousal += 10;
            ready_to_send = true;
            printf("valence_arousal = %d   ", valence_arousal);
            
        }
        
        ready = false; // last line

    }

}

// main() runs in its own thread in the OS
int main()
{
//    printf("Hello world.\r\n");
    
    t.start(callback(&evqueue, &EventQueue::dispatch_forever));
    kw40z_device.attach_buttonLeft(&ButtonLeft);
    kw40z_device.attach_buttonRight(&ButtonRight);
    kw40z_device.attach_passkey(&PassKey);

    pwr1v8 = 1;
    pwr3v3b = 1;
    pwr15v = 0;

    maximInterrupt.fall(interruptHandler);
    maximInterrupt.enable_irq();

    MAX30101::InterruptBitField_u interruptStatus;
    interruptStatus.all = 0xFF;
    hr.enableInterrupts(interruptStatus);

    
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);
    /* Turn on the backlight of the OLED Display */
    oled.DimScreenON();

    /* Fills the screen with solid black */
    oled.FillScreen(COLOR_BLACK);
    strcpy((char *) text, "Raw PPG:");
    oled.Label((uint8_t *)text,7,0);

    strcpy((char *) text, "         LOFI");
    oled.Label((uint8_t *)text,7,40);
    //dynamic text setup
    textProperties.fontColor = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
    oled.SetTextProperties(&textProperties);

    txThread.start(txTask);
    
    // added below
    
    /* Change font color to Blue */ 
    textProperties.fontColor   = COLOR_BLUE;
    oled.SetTextProperties(&textProperties);
    
    /* Display Bluetooth Label at x=17,y=65 */ 
    strcpy((char *) text,"BLUETOOTH");
    oled.Label((uint8_t *)text,17,65);
    
    /* Change font color to white */ 
    textProperties.fontColor   = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_CENTER;
    oled.SetTextProperties(&textProperties);
    
    /* Display Label at x=22,y=80 */ 
    strcpy((char *) text,"Tap to turn OFF");
    oled.Label((uint8_t *)text,7,80);
    
    // added above
    
    while (true) {

        // Format the time reading 
        sprintf(text,"%d",ppg_single_sample);
        oled.TextBox((uint8_t *)text,55,15,35,15); //Increase textbox for more digits
        
/*
        // Display time reading in 35px by 15px textbox at(x=55, y=40) 
        if (ppg_single_sample > 45) {
            sprintf(text,"%d; %.2f",SDNN, HF_LF);

            // Display time reading in 35px by 15px textbox at(x=55, y=40) 
            oled.TextBox((uint8_t *)text,55,55,35,15); //Increase textbox for more digits
        } else {
            sprintf(text,"wait HR");

            // Display time reading in 35px by 15px textbox at(x=55, y=40) 
            oled.TextBox((uint8_t *)text,55,55,35,15);
        }*/
        
        blueLed = !kw40z_device.GetAdvertisementMode();  // added this
        
        Thread::wait(1000);
    }
    return 0;
}