#include <Wire.h> // Must include Wire library for I2C
#include <SparkFun_MMA8452Q.h> // Includes the SFE_MMA8452Q library
#include <SPI.h>
#include <LoRa.h>

MMA8452Q accel;

float prevAX,prevAY,prevAZ;
float dervAX,dervAY,dervAZ;
float dervAXAVG,dervAYAVG,dervAZAVG;
unsigned long timeHolder;
float delta;

#define BPM_SENSOR_PORT         A3
#define BPM_LED_PORT            8

#define BPM_SAMPLEPERIOD        10
#define BPM_HIGHTHRESH          250
#define NUM_AVG                 10
#define BPM_DEBOUNCE			600   //limits the max bpm to 150, got a heart issue?

#define TRANSMIT_PERIOD         50

#define JERK_THRESH				50 //jerk greater than this will trigger a jesture in that axis
#define JERK_DEBOUNCE			200 //time in msec allowed between single axis repeated jestures
#define JERK_AVG				10
#define JERK_SAMPLE_PERIOD		10

#define X_POS_MASK				1
#define X_NEG_MASK				2
#define Y_POS_MASK				4
#define Y_NEG_MASK				8
#define Z_POS_MASK				16
#define Z_NEG_MASK				32


unsigned long loopTimer, sampleLoop;

uint16_t bpmMinVal = 1024,bpmMaxVal = 0;
uint8_t accMask;

uint16_t prevBPMValue;
uint16_t currentBPMValue;

unsigned long bpmPeriod,bpmTime,bpmDebounceTimer;
int16_t bpm;
uint8_t bpmAvg = 70;
bool seen = false;

unsigned long xTimer,yTimer,zTimer,jerkTimer;

void setup()
{
  Serial.begin(115200);
  pinMode(BPM_SENSOR_PORT,INPUT);
  accel.init();
    if (!LoRa.begin(433E6)) {
    Serial.println("Starting LoRa failed!");
    while (1);
  }
  pinMode(BPM_LED_PORT,OUTPUT);

    /**
   * BPM ANALOG DISCORVERY PERIOD
   */
  /*while(millis() - loopTimer < 5000 || bpmMaxVal - bpmMinVal < 300){
    uint16_t currentReading = analogRead(BPM_SENSOR_PORT);
    if(currentReading > bpmMaxVal){
      bpmMaxVal = currentReading;
    }else if(currentReading < bpmMinVal){
      bpmMinVal = currentReading;
    }

    if(millis() - sampleLoop > 100){
      sampleLoop = millis();
      digitalWrite(BPM_LED_PORT,!digitalRead(BPM_LED_PORT));
    }
  }*/
  bpmMaxVal = 900;
  Serial.printf("max: %d min: %d\n",bpmMaxVal,bpmMinVal);
  digitalWrite(BPM_LED_PORT,LOW);
}

void loop()
{
  if (accel.available())
  {
    accel.read();
    delta = (millis() - timeHolder)*1.0;
    timeHolder = millis();
    dervAX = (accel.cx - prevAX)/delta*1000;
    dervAY = (accel.cy - prevAY)/delta*1000;
    dervAZ = (accel.cz - prevAZ)/delta*1000;
    prevAX = accel.cx;
    prevAY = accel.cy;
    prevAZ = accel.cz;
    dervAXAVG = ((dervAXAVG*(JERK_AVG-1))+dervAX)/JERK_AVG;
    dervAYAVG = ((dervAYAVG*(JERK_AVG-1))+dervAY)/JERK_AVG;
    dervAZAVG = ((dervAZAVG*(JERK_AVG-1))+dervAZ)/JERK_AVG;
  }

  if(millis() - loopTimer > TRANSMIT_PERIOD){
    loopTimer = millis();
    sendDataSmall();
  }

    if(millis() - sampleLoop > BPM_SAMPLEPERIOD){
    sampleLoop = millis();

    currentBPMValue = analogRead(BPM_SENSOR_PORT);


    if(millis() - jerkTimer > JERK_SAMPLE_PERIOD){
    	jerkTimer = millis();
	   	if(millis() - xTimer > JERK_DEBOUNCE){
	    	accMask &= ~(X_POS_MASK | X_NEG_MASK);
	    	if(dervAXAVG > JERK_THRESH){
	    		xTimer = millis();
	    		accMask |= X_POS_MASK;
	    	}else if(dervAXAVG < -JERK_THRESH){
	    		xTimer = millis();
	    		accMask |= X_NEG_MASK;
	    	}
	    }

	    if(millis() - yTimer > JERK_DEBOUNCE){
	    	accMask &= ~(Y_POS_MASK | Y_NEG_MASK);
	    	if(dervAYAVG > JERK_THRESH){
	    		yTimer = millis();
	    		accMask |= Y_POS_MASK;
	    	}else if(dervAYAVG < -JERK_THRESH){
	    		yTimer = millis();
	    		accMask |= Y_NEG_MASK;
	    	}
	    }

	    if(millis() - zTimer > JERK_DEBOUNCE){
	    	accMask &= ~(Z_POS_MASK | Z_NEG_MASK);
	    	if(dervAZAVG > JERK_THRESH){
	    		zTimer = millis();
	    		accMask |= Z_POS_MASK;
	    	}else if(dervAZAVG < -JERK_THRESH){
	    		zTimer = millis();
	    		accMask |= Z_NEG_MASK;
	    	}
	    }
    }


    if(currentBPMValue > (bpmMaxVal - BPM_HIGHTHRESH) && !seen && millis() - bpmDebounceTimer > BPM_DEBOUNCE){
    	bpmDebounceTimer = millis();
		bpmPeriod = millis() - bpmTime;
		seen = true;
		bpmTime = millis();
		bpm = 1.0/(bpmPeriod/1000.0)*60.0;
		bpmAvg = ((bpmAvg*(NUM_AVG-1))+bpm)/NUM_AVG;
		Serial.println(bpmAvg);
		digitalWrite(BPM_LED_PORT,HIGH);
    }else if(currentBPMValue < (bpmMaxVal - 2*BPM_HIGHTHRESH)){
      seen = false;
      digitalWrite(BPM_LED_PORT,LOW);
    }
  }
}


void sendData(){
  int16_t x = (int16_t)dervAX;
  int16_t y = (int16_t)dervAY;
  int16_t z = (int16_t)dervAZ;
  Serial.printf("%d\n",(int)bpmAvg);
  LoRa.beginPacket();
  LoRa.write(0xFA);
  LoRa.write(x&0xFF);
  LoRa.write(0xFF&(x>>8));
  LoRa.write(y&0xFF);
  LoRa.write(0xFF&(y>>8));
  LoRa.write(z&0xFF);
  LoRa.write(0xFF&(z>>8));
  LoRa.write(bpmAvg&0xFF);
  LoRa.endPacket();
}


void sendDataSmall(){
	LoRa.beginPacket();
	LoRa.write(0xFA); //header
	LoRa.write(accMask); //acceleration directions masked into one byte
	LoRa.write(bpmAvg&0xFF);//bpm avg
	LoRa.endPacket();
	accMask = 0;
}