#include <Wire.h>
#include <math.h>
const int accel = 0x68;
const int heartMonitor = 2;
long accel_x, accel_y, accel_z, stepCount; //longs to avoid overflow on acceleration squared
float accelMod, distance;
float distPerStep = 0.2;
float heartRate = 150;
int power = 100;
bool batteryWarned = false;
String comnd; 
long lastPress, timeChange;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  Wire.beginTransmission(accel);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true); //initialise accelerometer
}

void loop() {
  
  Wire.beginTransmission(accel);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(accel, 6, true); 
  
  // Read accelerometer data
  accel_x = Wire.read() << 8 | Wire.read();
  accel_y = Wire.read() << 8 | Wire.read();
  accel_z = Wire.read() << 8 | Wire.read(); // read acceleration in each axis
  
  accelMod = sqrt(accel_x * accel_x+accel_y * accel_y+accel_z * accel_z);
  
  if (accelMod > 20000){
    stepCount ++;
  }

  heartRate = measureHeartRate();
  power = measureBattery();
  
  if (power < 16){
    if (!batteryWarned){
      lowPower();
      batteryWarned = true;
    }
  }
  else if (power > 25){
    batteryWarned = false;
  }
  
  if (Serial.available() > 0){ //message received
    comnd = Serial.readStringUntil('\n');
    if (comnd.equals("submitData")){
        distance = stepCount * distPerStep;
        Serial.println(power);
        Serial.println(heartRate);
        Serial.println(stepCount);
        Serial.println(distance);
    }
  }
  delay(50); //Delay for 50ms to prevent counting the same step
}

int measureBattery(){
  int battery = 100;
  return battery;
}

void lowPower() {
  Serial.println("lowPower");
};

float measureHeartRate(){
  while (digitalRead(heartMonitor) == LOW){}
  lastPress = millis();
  while (digitalRead(heartMonitor) == HIGH){}
  timeChange = millis() - lastPress;
  return (1000 / timeChange);
}