#include <Wire.h>
#include <MPU6050_light.h>
MPU6050 mpu(Wire);

#include <SoftwareSerial.h>

SoftwareSerial bluetooth(6, 7); // RX, TX


#include <Tiny4kOLED.h>
#include "SolomonSystech.h"


//pedometer

float vectorprevious; //previous movement
float vector; //current movement
float totalvector; //difference between vector and previous vector
int steps = 0; //steps
int prevSteps = 0; //saves previous steps

unsigned long lastHappyTime; //each time that the face is happy save the time


void setup() {

  // set the data rate for the SoftwareSerial port
  bluetooth.begin(9600);

  // put your setup code here, to run once:

  oled.begin(128, 64, sizeof(tiny4koled_init_128x64r), tiny4koled_init_128x64r);

  // intended to be drawn onto a clear screen
  oled.clear();

  // Now that the display is all setup, turn on the display
  oled.on();

  Serial.begin(9600);
  Wire.begin();
  mpu.begin();

  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  mpu.calcOffsets(true, true); // gyro and accelero
  Serial.println("Done!\n");
}

void loop() {
  mpu.update();


  delay(50);
  float angley = mpu.getAngleY(); // declare the angle Y of the gyro on the variable angle
  float anglex = mpu.getAngleX(); // declare the angle X of the gyro on the variable angle

  calculatesteps(); //calls the acction defined as calculatesteps

  Serial.print(angley);
  Serial.print(":");
  Serial.print(anglex);
  Serial.print(":");
  Serial.println(steps);//sending the amount of steps

  //If the user walks then change the face
  if (steps - prevSteps > 0) {
    oled.bitmap(0, 0, 128 , 8, happyface_bitmap); //happy face
    lastHappyTime = millis(); //asign the amount of time since the board was on and the face was happy
    //decide how much to wait to change the face back to sad
  } else if ( millis() - lastHappyTime > 5000) {
    oled.bitmap(0, 0, 128 , 8, sadface_bitmap); //sad face

  }

  prevSteps = steps; //update step count

  //read the value from the bluetooth
  char readCommand = bluetooth.read();
  resetSteps(readCommand);
  calibrate(readCommand);

}


void resetSteps(char c) {
  if (c == 'Y') {
    steps = 0;
  }
}

void calibrate(char c) {
  if (c == 'C') {
    mpu.calcOffsets(true, true); // gyro and accelero

  }
}

void calculatesteps() {

  float accelX = mpu.getAccX();
  float accelY =   mpu.getAccY();
  float accelZ =   mpu.getAccZ();
  //  Serial.print(accelX);
  //  Serial.print(",");
  //Serial.print(accelY);
  //Serial.print(",");
  //Serial.println(accelZ);

  vector = sqrt( (accelX * accelX) + (accelY * accelY) + (accelZ * accelZ) ); //Calculates a uniform acceleration vector
  totalvector = vector - vectorprevious; //Difference between vectors

  if (accelY > 1) {
    steps++;
  }
  // Uses the value 6 to define the difference between 2 moments and understand if there was a 'step'

  vectorprevious = vector; //saves the last value to be able to compare it with the next value

} //calculates the steps by asking the accelerometer for the current acceleration