#include <Wire.h>
#include "MAX30105.h"

MAX30105 particleSensor;
const int MPU = 0x68;
float Accel_X, Accel_Y, Accel_Z;

class  FilterBuBp2
{
  public:
    FilterBuBp2()
    {
      for(int i=0; i <= 4; i++)
        v[i]=0.0;
    }
  private:
    float v[5];
  public:
    float step(float x) //class II
    {
      v[0] = v[1];
      v[1] = v[2];
      v[2] = v[3];
      v[3] = v[4];
      v[4] = (5.586996620231363987e-3 * x)
         + (-0.80080264666570755150 * v[0])
         + (3.37021291056280425380 * v[1])
         + (-5.33686791231501622690 * v[2])
         + (3.76742616976629829395 * v[3]);
      return
         (v[0] + v[4])
        - 2 * v[2];
    }
};

FilterBuBp2 c_filter;
long present, seconds, past, green, past2, present2;
long past_gValue[8]={9999,9999,9999,9999,9999,9999,9999,9999};
int arr_size=7;
bool beatG;
int flag;
float voltage;

void setup()
{
  Serial.begin(9600);

  // Initialize sensor
  if (!particleSensor.begin(Wire, I2C_SPEED_FAST)) //Use default I2C port, 400kHz speed
  {
    Serial.println("MAX30105 was not found.");
    while (1);
  }

  //Setup to sense a nice looking saw tooth on the plotter
  byte ledBrightness = 0x3F; //Options: 0=Off to 255=50mA
  byte sampleAverage = 4; //Options: 1, 2, 4, 8, 16, 32
  byte ledMode = 3; //Options: 1 = Red only, 2 = Red + IR, 3 = Red + IR + Green
  int sampleRate = 400; //Options: 50, 100, 200, 400, 800, 1000, 1600, 3200
  int pulseWidth = 411; //Options: 69, 118, 215, 411
  int adcRange = 4096; //Options: 2048, 4096, 8192, 16384

  particleSensor.setup(ledBrightness, sampleAverage, ledMode, sampleRate, pulseWidth, adcRange); //Configure sensor with these settings

  particleSensor.setPulseAmplitudeRed(0x00);
  particleSensor.setPulseAmplitudeIR(0x00);
  particleSensor.setPulseAmplitudeGreen(0xFF);

  past = millis();
  past2 = millis();//send data every 0.5s

  Wire.endTransmission(true);        //end the transmission

  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
  Serial.println("MAX30105 was not found.");
  Serial.println("MAX30105 was not found.");

}

void loop()
{
  green=c_filter.step(particleSensor.getGreen());


  past_gValue[0]=green;

  for(int i = 0; i<arr_size; i++){
    past_gValue[arr_size-i]=past_gValue[arr_size-1-i];
  }


  for(int i = 0; i<arr_size; i++){
    if (past_gValue[arr_size-i]>past_gValue[arr_size-1-i]){
      flag++;
    }
  }

  if (flag==arr_size-1) {
    present=millis();
    if ((present-past) > 300) {
      seconds=60000/(present-past);
    }
    past=millis();
    /*Serial.print(200);
  }
  else {
    Serial.print(0);*/
  }

  Wire.endTransmission();
  present2=millis();

  if (present2-past2 > 500) {
    Serial.print("hr=");
    Serial.print(seconds);
    Serial.print(",sv=");
    past2=millis();

    voltage = analogRead(PA3);
    voltage = voltage * 3.3 / 4096;
    Serial.print(voltage);

    // default dry skin is around 0.5-0.6, wet skin around 0.9-1.1, threshold should be 0.7-0.8

    Serial.print(",mm=");

    Wire.beginTransmission(MPU);
    Wire.write(0x43); // Accel data first register address 0x43
    Wire.endTransmission();
    Wire.requestFrom(MPU, 6); // Read 4 registers total, each axis value is stored in 2 registers
    Accel_X =  (Wire.read() << 8 | Wire.read()) / 131.0;
    Accel_Y =  (Wire.read() << 8 | Wire.read()) / 131.0;
    Accel_Z =  (Wire.read() << 8 | Wire.read()) / 131.0;
    Serial.print(Accel_Z);
    Serial.println(",");
  }


    flag=0;

}