/*
    pls install MAX30100lib by library manager first
    addr: https://github.com/oxullo/Arduino-MAX30100
*/

#include <M5StickC.h>
#include "MAX30100.h"
#include <math.h>

#define SAMPLING_RATE                       MAX30100_SAMPRATE_50HZ

// The LEDs currents must be set to a level that avoids clipping and maximises the
// dynamic range
#define IR_LED_CURRENT                      MAX30100_LED_CURR_11MA
#define RED_LED_CURRENT                     MAX30100_LED_CURR_11MA

// The pulse width of the LEDs driving determines the resolution of
// the ADC (which is a Sigma-Delta).
// set HIGHRES_MODE to true only when setting PULSE_WIDTH to MAX30100_SPC_PW_1600US_16BITS
#define PULSE_WIDTH                         MAX30100_SPC_PW_400US_14BITS
#define HIGHRES_MODE                        true

TFT_eSprite Disbuff = TFT_eSprite(&M5.Lcd);
MAX30100 sensor;

uint64_t realTime[4], time_count = 0;
bool k_ready = false;

void setup() {
  // put your setup code here, to run once:
    M5.begin();

    M5.Lcd.setRotation(3);
    M5.Lcd.setSwapBytes(false);
    Disbuff.createSprite(160, 80);
    Disbuff.setSwapBytes(true);
    
    Disbuff.createSprite(160, 80);

    if (!sensor.begin()) 
    {
        Serial.println("FAILED");
        for(;;);
    } 
    else 
    {
        Serial.println("SUCCESS");
    }

    sensor.setMode(MAX30100_MODE_SPO2_HR);
    sensor.setLedsCurrent(IR_LED_CURRENT, RED_LED_CURRENT);
    sensor.setLedsPulseWidth(PULSE_WIDTH);
    sensor.setSamplingRate(SAMPLING_RATE);
    sensor.setHighresModeEnabled(HIGHRES_MODE);
    sensor.resetFifo();
}

uint16_t line[2][320] = {0};
int16_t  k_number[320]={0};
int16_t  raw_data[512];
int16_t  k_min;
int16_t  k_threshold;
uint32_t k_pos_buff[32];
double   k_pos_time[32];
double   k_standard_deviation, k_sumdata;
uint32_t k_pos, k_pos_count = 0;

uint32_t led_pos = 0, ir_Pos = 0;
uint16_t ir_max = 0, led_max = 0, ir_min = 0, led_min = 0, ir_last = 0, led_last = 0;
int16_t  k_number_min = 0, k_number_max = 0, k_last = 0;
uint16_t ir_last_raw = 0, led_last_raw = 0, k_last_raw = 0;
uint16_t ir_disdata, led_disdata, k_disdata;
uint16_t Alpha = 0.3 * 256;
uint16_t count_min = 0, count_max = 0;

void loop() {
  // put your main code here, to run repeatedly:
    uint16_t ir, red;

    //Serial.printf("raed fifo Length :%d\n", read_length); 
    while(sensor.getRawValues(&ir, &red) == false)
    {
        delay(10);
        sensor.update();
    }

    while (1)
    {   
        line[0][( led_pos + 160 )%320] = ( led_last_raw * ( 256 - Alpha ) + red * Alpha )/ 256;
        line[1][( ir_Pos + 160 )%320] =  ( ir_last_raw * ( 256 - Alpha ) + ir * Alpha )/ 256;

        k_number[( led_pos + 160 )%320] = line[0][( led_pos + 160 )%320] - led_last_raw;

        led_last_raw = line[0][( led_pos + 160 )%320];
        ir_last_raw = line[1][( led_pos + 160 )%320];
        led_pos ++;
        ir_Pos ++;
        if(sensor.getRawValues(&ir, &red) == false) 
        {
            break;
        }
    }

    sensor.resetFifo();

    for (int i = 0; i < 160; i++)
    {
        if( i == 0 )
        {
            led_max = led_min = line[0][( led_pos + i ) %320];
            ir_max = ir_min = line[1][( ir_Pos + i ) %320];
            k_number_min = k_number_max = k_number[( ir_Pos + i ) %320];
        }
        else
        {
            led_max = ( line[0][( led_pos + i ) %320] > led_max ) ? line[0][( led_pos + i ) %320] : led_max;
            led_min = ( line[0][( led_pos + i ) %320] < led_min ) ? line[0][( led_pos + i ) %320] : led_min;

            ir_max  = ( line[1][( ir_Pos + i ) %320] > ir_max ) ? line[1][( ir_Pos + i ) %320] : ir_max;
            ir_min  = ( line[1][( ir_Pos + i ) %320] < ir_min ) ? line[1][( ir_Pos + i ) %320] : ir_min;

            k_number_max = ( k_number[( ir_Pos + i ) %320] > k_number_max ) ? k_number[( ir_Pos + i ) %320] : k_number_max;
            k_number_min = ( k_number[( ir_Pos + i ) %320] < k_number_min ) ? k_number[( ir_Pos + i ) %320] : k_number_min;
        }
    }

    Disbuff.fillRect(0,0,160,80,BLACK);

    for (int i = 0; i < 160; i++ )
    {
        led_disdata = map( line[0][( led_pos + i ) %320] , led_max, led_min, 0, 80);
        ir_disdata  = map( line[1][( ir_Pos + i ) %320]  , ir_max, ir_min, 0, 80);
        k_disdata   = map( k_number[( ir_Pos + i ) %320]  , k_number_max, k_number_min, 0, 80);
        {
            Disbuff.drawLine( i, led_last, i + 1, led_disdata, WHITE);
            Disbuff.drawLine( i, ir_last, i + 1, ir_disdata, RED);
            Disbuff.drawLine( i, k_last, i + 1, k_disdata, GREEN);
        }
        ir_last = ir_disdata;
        led_last = led_disdata;
        k_last = k_disdata;
    }

    Disbuff.setTextSize(1);
    Disbuff.setTextColor(WHITE);

    Disbuff.setCursor(5,5);
    Disbuff.printf("led:%d,%d,%d",led_max, led_min, led_max - led_min );
    Disbuff.setCursor(5,15);
    Disbuff.printf("ir:%d,%d,%d",ir_max, ir_min, ir_max - ir_min );
    
    Disbuff.pushSprite(0,0);
    
    
    delay(10);
}