#include <Adafruit_MPU6050.h>
#include <EEPROM.h>

Adafruit_MPU6050 mpu;

// The EEPROM address the next Reading structure is written to.
// Will wrap back around once all space is used.
int end_addr = 0;
int test = 0;

// dump_EEPROM - Dumps all EEPROM values as integers to the serial console.
void dump_EEPROM() {
  Serial.println("Current EEPROM Values: ");
  for (int i = 0; i < EEPROM.length(); i++) {
    Serial.print("[");
    Serial.print("0x");
    Serial.print(i);
    Serial.print("]: ");
    Serial.println(EEPROM[i]);
  }
}

// BUP_main_sensor -- initializes the main MPU6050 sensor.
// WARNING: this function can potentially NEVER return if the MPU6050 is not found!
void BUP_main_sensor() {
 Wire.begin();

  Serial.println("Connecting to GY521...");
  while (mpu.begin(0x69) == false) {
    Serial.print(".");
    delay(100);
  }
  Serial.println("Connected!");

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
  mpu.setCycleRate(MPU6050_CYCLE_1_25_HZ); // 1.25hz = 0.8s, but we only read at 1hz
}


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  
  dump_EEPROM();

  BUP_main_sensor();
 
}

// Reading - A structure that represents a reading from the sensor and the timestamp
// at which it was obtained.
//
// This structure is used to have easy access to sensor data and a set storage size
// for storing in EEPROM.
// 
// size: 26 bytes
struct Reading {
  // :8 sets the integers to always use 8 bits
  // Floats are always 32-bits (4 bytes) long.
  unsigned int time:8;

  float accel_x;
  float accel_y;
  float accel_z;

  float gyro_x;
  float gyro_y;
  float gyro_z;

  signed short int temp:8;
};

// get_reading() - Obtains a reading from the MPU6050 sensor and returns it in a Reading structure.
struct Reading get_reading() {
  Reading reading;

  unsigned long runsecs = millis() / 1000;

  reading.time = runsecs;

  sensors_event_t accel, gyro, temp;
  mpu.getEvent(&accel, &gyro, &temp);

  reading.accel_x = accel.acceleration.x;
  reading.accel_y = accel.acceleration.y;
  reading.accel_z = accel.acceleration.z;

  reading.gyro_x = gyro.gyro.x;
  reading.gyro_y = gyro.gyro.y;
  reading.gyro_z = gyro.gyro.z;

  reading.temp = temp.temperature;

  return reading;
}

// print_reading -- Dumps a Reading structure to the Serial console.
void print_reading(struct Reading reading) {
  Serial.print("Reading at t = ");
  Serial.println(reading.time);

  Serial.print("Reading Size: ");
  Serial.println(sizeof(reading));

  Serial.print("Acceleration X: ");
  Serial.print(reading.accel_x);
  Serial.print(", Y: ");
  Serial.print(reading.accel_y);
  Serial.print(", Z: ");
  Serial.print(reading.accel_z);
  Serial.println(" m/s^2");

  Serial.print("Rotation X: ");
  Serial.print(reading.gyro_x);
  Serial.print(", Y: ");
  Serial.print(reading.gyro_y);
  Serial.print(", Z: ");
  Serial.print(reading.gyro_z);
  Serial.println(" rad/s");

  Serial.print("Temperature: ");
  Serial.print(reading.temp);
  Serial.println(" degC");

  Serial.println("");
}

// write_reading -- writes a reading structure to EEPRON
// Returns: integer value of the ending EEPROM address
int write_reading(struct Reading reading, int start_byte) {
  if ((start_byte + sizeof(reading)) > EEPROM.length()) {
    Serial.println("WARNING: OVERFLOW of EEPROM occured!");
    start_byte = 0;
  }

  Serial.print("Writing reading at EEPROM address: ");
  Serial.println(start_byte);

  EEPROM.put(start_byte, reading);

  return start_byte + sizeof(reading);
}

int read_reading(int start_byte) {
  Serial.print("EEPROM Reading at Byte ");
  Serial.print(start_byte);
  Serial.println(": ");

  struct Reading reading;
  EEPROM.get(start_byte, reading);

  print_reading(reading);

  return start_byte + sizeof(reading);
}

void loop() {
  // put your main code here, to run repeatedly:
  struct Reading reading = get_reading();

  //print_reading(reading);
  end_addr = write_reading(reading, end_addr);

  test = read_reading(test);

  //dump_EEPROM();

  // 1 reading per second.
  // This is longer than our sensor rate of 1 reading per 0.8 seconds
  // but it saves our EEPROM life and power.
  delay(1000);
}