/*
  Zen Pen v1 code by Michael Ogrinz 
  This code uses snippets from:
  "Meditation Trainer by Collin Cunningham for Adafruit Industries" https://learn.adafruit.com/heart-rate-variability-sensor/overview
  which in turn was based on 
  "Pulse Sensor Amped 1.4 by Joel Murphy and Yury Gitman"   http://www.pulsesensor.com
  
  Tip: Program the Pro Micro as Arduino Micro not as Sparkfun Pro Micro
*/

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

MAX30105 particleSensor;

const byte RATE_SIZE = 4; //Increase this for more averaging. 4 is good.
byte rates[RATE_SIZE]; //Array of heart rates
byte rateSpot = 0;
long lastBeat = 0; //Time at which the last beat occurred
long irValue = 0;

boolean beatToggle=true;

float beatsPerMinute;
int beatAvg;

#define NUM_LEDS 8
#define HEADLAMP_LED1 1
#define HEADLAMP_LED2 2
#define FIRST_BREATH_LED 1
#define LAST_BREATH_LED 60
#define DATA_PIN 4

CRGB leds[NUM_LEDS];
CRGB currentColor=CRGB(0,0,0);
CRGB pulseColor=CRGB(0,0,0);

int readingCount = -1;
float readingTimeSeries[20];
float calcTimeSeries[20];
int totalReadings=0;

int fadeRate = 0;              
int breathLED = FIRST_BREATH_LED;
bool breathToggle = false;
bool pulse = false;
unsigned long lastBreath = 0;
unsigned long ledTimes[60];
unsigned long headlampLevel[60];
volatile int lastIBI = 0;
volatile int preserveLastIBI = 0;
volatile unsigned long lastBeatTime = 0;           
volatile unsigned long firstBeatTime = 0;           
volatile unsigned long secondBeatTime = 0;           
volatile int IBI = 600;             // int that holds the time interval between beats! Must be seeded!
volatile int tCoh = 10;             // coherence values total

//Coherence vars
volatile int coh = 0;              // running coherence rating
volatile unsigned long lastCoh = 0;
volatile unsigned long cohPeriod = 12000;


void setup()
{
  Serial.begin(115200);
  Serial.println("Initializing...");

  // Initialize sensor
  if (!particleSensor.begin(Wire, I2C_SPEED_FAST)) //Use default I2C port, 400kHz speed
  {
    Serial.println("MAX30105 was not found. Please check wiring/power. ");
    while (1);
  }
  particleSensor.setup(); //Configure sensor with default settings
  particleSensor.setPulseAmplitudeRed(0x0A); //Turn Red LED to low to indicate sensor is running
  particleSensor.setPulseAmplitudeGreen(0); //Turn off Green LED

  FastLED.addLeds<WS2811, DATA_PIN, RGB>(leds, NUM_LEDS);
  // Note: If you ran the test progam and your LEDs blinked in the order Green, Red, Blue instead of RGB, uncomment this line and comment out the line above
  //FastLED.addLeds<WS2811, DATA_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(255);

  for (int i=0;i<60;i++){
    headlampLevel[i]=5+i;
  }

  int slopeFactor = 4;
  for (int i=0;i<30;i++){
      ledTimes[i]=50;
  }
  for (int i=29;i>=0;i--){
      ledTimes[59-i]=50;
  }
  ledTimes[0]=500;
  ledTimes[1]=250;

}

void loop()
{
  irValue = particleSensor.getIR();

  if (checkForBeat(irValue) == true)
  {
      leds[0] = currentColor;
      FastLED.show();
      
      //We sensed a beat!
      long delta = millis() - lastBeat;
      lastBeat = millis();
  
      beatsPerMinute = 60 / (delta / 1000.0);
  
      if (beatsPerMinute < 255 && beatsPerMinute > 20){
        rates[rateSpot++] = (byte)beatsPerMinute; //Store this reading in the array
        rateSpot %= RATE_SIZE; //Wrap variable
  
        //Take average of readings
        beatAvg = 0;
        for (byte x = 0 ; x < RATE_SIZE ; x++)
          beatAvg += rates[x];
        beatAvg /= RATE_SIZE;
      }
      processBeat();
  } else {
      leds[0] =CRGB(0,0,0);
  }

  if (irValue < 50000){
    Serial.println("No reading");
    leds[0] =CRGB(0,0,0);
    tCoh=0;
    beatToggle = true;                      
    secondBeatTime = millis();
  }
  
  updateBreathLED();
  FastLED.show();
}
float RMSSD(int valCount){
  float rmssd  = 0.0;
  float tsdiff = 0.0;
  if (valCount<=1){
    return 0;
  }
  for (int i=1;i<valCount;i++){
    tsdiff = calcTimeSeries[i-1]-calcTimeSeries[i];
    rmssd += tsdiff*tsdiff;
  }
  return sqrt(rmssd/float(valCount-1));
}
float STDDEV(int valCount){
  float mean  = 0.0;
  float total = 0.0;
  if (valCount==0){
    return 0;
  }
  for (int i=0;i<valCount;i++){
    mean += calcTimeSeries[i];
  }
  mean = mean/float(valCount);
  for (int i=0;i<valCount;i++){
    total += (calcTimeSeries[i]-mean) * (calcTimeSeries[i]-mean);
  }
  return sqrt(total/float(valCount));
}
void processBeat(){
  
  lastIBI=IBI;
  if (beatToggle){
    firstBeatTime = millis();
    IBI=firstBeatTime-secondBeatTime;
    beatToggle=false;
  } else {
    secondBeatTime= millis();
    IBI=secondBeatTime-firstBeatTime;
    beatToggle=true;
  }

  if (IBI>1600){
    return;
  }
  
  Serial.print(",");

  //update coherence
  unsigned long now = millis();
  if ((now - lastCoh) > cohPeriod) {
    lastCoh = now;
    tCoh = coh;
    coh = 0;
  }
   
  if (IBI < lastIBI) {
    coh += 1;
  }

  readingCount++;
  if (readingCount>19){
    int valCount = scrubReadings();
    Serial.print(RMSSD(valCount));
    Serial.print(",");
    Serial.println(STDDEV(valCount));
    Serial.println("--------------------------------------");
    totalReadings++;
    readingCount=0;
  }
  if (readingCount>=0 && readingCount<=19){
    readingTimeSeries[readingCount]=IBI;
  } 
  
  Serial.print(IBI);
  
  fadeRate = 255;
}

int scrubReadings(){
 int keepCount=0;
 Serial.println();
 Serial.print("Scrubbed IBI series:");
 for (int i=1;i<20;i++){
    float temp = abs(readingTimeSeries[i-1]-readingTimeSeries[i]);
    if (temp<=200){
      calcTimeSeries[keepCount]=temp;
      Serial.print(temp);
      Serial.print(",");
      keepCount++;
    }
 }    
 Serial.println();
 Serial.print("RMSSD and STDDEV:");
 return keepCount; 
}

void updateBreathLED() {

  if (breathLED <= FIRST_BREATH_LED) {
    breathLED = FIRST_BREATH_LED;
    breathToggle = false;
  }
  else if (breathLED >= LAST_BREATH_LED) {
    breathLED = LAST_BREATH_LED;
    breathToggle = true;
  }

  //check if we're due to update the breath indicator
  unsigned long timeNow = millis();
  unsigned long timeSince = timeNow - lastBreath;

  unsigned long breathTime = ledTimes[breathLED-1];
  if (timeSince >= breathTime) {
    if (breathToggle) {
      breathLED -= 1;
    } else {
      breathLED += 1;
    }
    lastBreath = timeNow;
  }

  // breathLED is now being used to set a brightness level on the headlamp(s)
  FastLED.setBrightness(headlampLevel[breathLED-1]);
  // Set headlamp color based on score
  if (tCoh<=0 || tCoh>=10){
    currentColor=CRGB(255,0,0);
  } else if (tCoh==1) {
    currentColor=CRGB(255,128,0);
  } else if (tCoh==2) {
    currentColor=CRGB(255,255,0);
  } else if (tCoh==3) {
    currentColor=CRGB(128,255,0);
  } else if (tCoh==4) {
    currentColor=CRGB(0,255,0);
  } else if (tCoh==5) {
    currentColor=CRGB(0,255,255);
  } else if (tCoh==6) {
    currentColor=CRGB(0,128,255);
  } else if (tCoh==7) {
    currentColor=CRGB(64,0,255);
  } else if (tCoh==8) {
    currentColor=CRGB(191,0,255);
  } else if (tCoh==9) {
    currentColor=CRGB(255,0,255);
  }  
  headlampColor( leds, currentColor, 1);
}

// Helper function that blends one uint8_t toward another by a given amount
void nblendU8TowardU8( uint8_t& cur, const uint8_t target, uint8_t amount)
{
  if( cur == target) return;
  
  if( cur < target ) {
    uint8_t delta = target - cur;
    delta = scale8_video( delta, amount);
    cur += delta;
  } else {
    uint8_t delta = cur - target;
    delta = scale8_video( delta, amount);
    cur -= delta;
  }
}

CRGB fadeTowardColor( CRGB& cur, const CRGB& target, uint8_t amount)
{
  nblendU8TowardU8( cur.red,   target.red,   amount);
  nblendU8TowardU8( cur.green, target.green, amount);
  nblendU8TowardU8( cur.blue,  target.blue,  amount);
  return cur;
}

// Fade an entire array of CRGBs toward a given background color by a given amount
// This function modifies the pixel array in place.
void headlampColor( CRGB* L, const CRGB& bgColor, uint8_t fadeAmount)
{
    fadeTowardColor(leds[HEADLAMP_LED1], bgColor, fadeAmount);
    fadeTowardColor(leds[HEADLAMP_LED2], bgColor, fadeAmount);
}