#include "twi.h"
#include <SPI.h>
#include <SD.h>

#define CS_PIN 10

const byte MPU_ADDRESS = 0x69;  // I2C address of gyro.
#define CTRL_REG1 0x20
#define CTRL_REG2 0x21
#define CTRL_REG3 0x22
#define CTRL_REG4 0x23
int x, y, z;

const byte LED_PIN = 4;
const byte BUTTON_PIN = 5;

int16_t Denoise(int16_t value)
{
  return value / 114;
}

struct GyroData
{
  //const uint32_t reg = 0x28;  // For other gyro.
  const uint32_t reg = 0x43;  // For MPU-6050.
  const byte reg_size = 1;
  volatile byte buffer[6];
  int16_t GyroX() const { return buffer[0] << 8 | buffer[1]; }  // Double check the byte order for your gyro.
  int16_t GyroY() const { return buffer[2] << 8 | buffer[2]; }
  int16_t GyroZ() const { return buffer[4] << 8 | buffer[3]; }
};

GyroData data;
File myFile;

void setup()
{
  //PFUpdateStateMachine();  // Should generate a compile error.

  pinMode(LED_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  twi.Enable();
  twi.SetFrequency(400000u);

  Serial.begin(115200);

  Serial.print("Initializing SD card...");

  if (!SD.begin(CS_PIN))
  {
    Serial.println("initialization failed!");
    while (1);
  }
  Serial.println("initialization done.");

  /**
  Write(MPU_ADDRESS, CTRL_REG1, 0x1F);  // Turn on all axes, disable power down
  Write(MPU_ADDRESS, CTRL_REG3, 0x08);  // Enable control ready signal
  Write(MPU_ADDRESS, CTRL_REG4, 0x80);  // Set scale (500 deg/sec)
  delay(100); // Wait to synchronize
  /**/

  //
  // Perform full reset as per MPU-6000/MPU-6050 Register Map and Descriptions, Section 4.28, pages 40 to 41.
  //

  // PWR_MGMT_1 register: DEVICE_RESET = 1
  Serial.println("Resetting MPU...");
  twi.Write(MPU_ADDRESS, 0x6B, 1, 0b10000000, TWI::Modes::Wait);
  delay(100);  // Wait for reset to complete.
  Serial.println("Done.");

  // SIGNAL_PATH_RESET register: GYRO_RESET = 1, ACCEL_RESET = 1, TEMP_RESET = 1.
  Serial.println("Resetting MPU gyro, accel and temp...");
  twi.Write(MPU_ADDRESS, 0x68, 1, 0b00000111, TWI::Modes::Wait);
  delay(100);  // Wait for reset to complete.
  Serial.println("Done.");

  // Disable SLEEP mode because the reset re-enables it. Section 3, PWR_MGMT_1 register, page 8.
  // PWR_MGMT_1 register: SLEEP = 0
  Serial.println("Resetting MPU sleep mode...");
  twi.Write(MPU_ADDRESS, 0x6B, 1, 0b00000000, TWI::Modes::Wait);
  Serial.println("Done.");

  myFile = SD.open("test.txt", FILE_WRITE);
}

void loop()
{
  uint16_t timestamp = millis();

  if (!digitalRead(BUTTON_PIN))
  {
    Serial.println("Closing file.");
    myFile.close();
    while (1);
  }

  //
  // TASK 1: Read the gyro every interval.
  //

  const uint16_t READ_GYRO_INTERVAL = 10;
  static uint16_t read_gyro_timestamp = timestamp;
  static bool receiving = false;
  if (timestamp - read_gyro_timestamp >= READ_GYRO_INTERVAL)
  {
    if (!receiving)
    {
      //Serial.println("\nBurst-read of 6 bytes (using non-blocking ISR).");
      receiving = twi.Read(MPU_ADDRESS, data.reg, data.reg_size, data.buffer, sizeof(data.buffer));
    }
  }
  if (receiving && twi.GetState() == TWI::States::Ready)
  {
    receiving = false;
    if (twi.IsSuccess())
    {
      Serial.print(timestamp);
      //Serial.print(timestamp - read_gyro_timestamp);  // Delta time.
      read_gyro_timestamp += READ_GYRO_INTERVAL;

      x = Denoise(data.GyroX());
      y = Denoise(data.GyroY());
      z = Denoise(data.GyroZ());

      Serial.print("\tRaw X:"); Serial.print(x);
      Serial.print(" Raw Y:"); Serial.print(y);
      Serial.print(" Raw Z:"); Serial.println(z);

      myFile.print(timestamp);
      myFile.print(" Raw X:"); myFile.print(x);
      myFile.print(" Raw Y:"); myFile.print(y);
      myFile.print(" Raw Z:"); myFile.println(z);
    }
  }

  //
  // TASK 2: Blink the external LED.
  //

  const uint16_t LED_BLINK_INTERVAL = 500;
  static uint16_t led_blink_timestamp = timestamp;
  static bool led_state = false;
  if (timestamp - led_blink_timestamp >= LED_BLINK_INTERVAL)
  {
    led_state = !led_state;
    digitalWrite(LED_PIN, led_state);
    led_blink_timestamp += LED_BLINK_INTERVAL;
  }
}