#include <Arduino.h>
#include <string.h>
#include "ssd1306.h"
//#include "MPU6050.h"

//#include "ctwi.h"
using namespace oled_ssd1306;
using namespace ShapeDraw;

CTWI ctwi;
DISPLAY_SSD1306 display;
MPU6050 mpu;
RTC rtc;

byte addr = SSD1306_ADDRESS;

const byte MPU_ADDRESS = 0x69;  // I2C address of the MPU-6050. Either 0x68 or 0x69.
const byte DS1307_ADDRESS = 0x68;  // I2C address of the DS1307. Either 0x68 or 0x69.

const byte ADO_PIN = 12;        // For MPU: LOW = 0x68 (default); HIGH = 0x69.

int sec;

#define g 9.81


bool Oled_Init = false;

void setup() {
  Serial.begin(115200);
  ctwi.EnableTWI();

  if (bitRead(MCUSR, WDRF))
  {
    Serial.println("System was reset due to watchdog timeout.");
    Serial.println(WDTCSR);
    Serial.println();
  }

  ResetMPU();
  ResetRTC();

  if (!Oled_Init) Oled_Init = InitOLED();

}


//
// Perform full reset as per MPU-6000/MPU-6050 Register Map and Descriptions
//
void ResetMPU() {
  pinMode(ADO_PIN, OUTPUT);
  digitalWrite(ADO_PIN, MPU_ADDRESS & 1);               //  Modify the address of the MPU6050 to 0x69 by setting pin 12 to high

  // Resetting MPU - PWR_MGMT_1 register: DEVICE_RESET = 1
  while (!ctwi.Write(MPU_ADDRESS, 0x6B, 0b10000000));   // Keep trying until transmission begins.
  while (!ctwi.IsComplete());                           // Wait for transmission to complete.

  // SIGNAL_PATH_RESET register: GYRO_RESET = 1, ACCEL_RESET = 1, TEMP_RESET = 1.
  while (!ctwi.Write(MPU_ADDRESS, 0x69, 0b00000111));   // Keep trying until transmission begins.
  while (!ctwi.IsComplete());                           // Wait for transmission to complete.

  // Disable SLEEP mode because the reset re-enables it. PWR_MGMT_1 register
  // PWR_MGMT_1 register: SLEEP = 0
  while (!ctwi.Write(MPU_ADDRESS, 0x6B, 0b00000000)); // Reset MPU sleep mode
  while (!ctwi.IsComplete());                         // Wait for transmission to complete.
}


//
// Perform full reset as per RTC DS1307 Register Map and Descriptions
//
void ResetRTC() {
  // Resetting RTC - PWR_MGMT_1 register: DEVICE_RESET = 1
  while (!ctwi.Write(DS1307_ADDRESS, 0x6B, 0b10000000));  // Keep trying until transmission begins.
  while (!ctwi.IsComplete());                             // Wait for transmission to complete.

  // SIGNAL_PATH_RESET register: CLOCK_RESET = 1.
  while (!ctwi.Write(DS1307_ADDRESS, 0x69, 0b00000111));  // Keep trying until transmission begins.
  while (!ctwi.IsComplete());                             // Wait for transmission to complete.

  // Disable SLEEP mode because the reset re-enables it. PWR_MGMT_1 register.
  // PWR_MGMT_1 register: SLEEP = 0
  while (!ctwi.Write(DS1307_ADDRESS, 0x6B, 0b00000000));
  while (!ctwi.IsComplete());                             // Wait for transmission to complete.
}


void loop() {

  if (bitRead(MCUSR, WDRF))
  {
    Serial.println("System was reset due to watchdog timeout.");
    Serial.println(WDTCSR);
    Serial.println();
  }

  rtcCheckForUpdate();
  mpuCheckForUpdate();
}


ISR(WDT_vect)
{
  // Disable any further WDT interrrupts.
  // It will be re-enabled at the next START condition.
  WDTCSR = 0;

  ctwi.HandleTimeout();

  timeout = true;
  state = States::Ready;
  result = Results::Timeout;
}


ISR(TWI_vect)
{
  ctwi.UpdateStateMachine();  // TWI ISR mode.
}

// init ssd1306
bool InitOLED() {
  display.SSD1306_Init(addr);

  // clear screen
  display.clear();

  drawLine(&display, 0, 15, 127, 15);
  drawLine(&display, 0, 55, 127, 55);

  drawText(&display, font_8x8, "404: Success", 2, 2);
  drawText(&display, font_12x16, "Project C", 2, 20);
  drawText(&display, font_8x8, "Part B", 2, 40);

  display.UpdateScreen (addr);

  delay(4000);
  display.clear();

  drawLine(&display, 0, 48, 127, 48);
  drawRoundRect(&display, 56, 50, 70, 13, 5); // Time Border
  drawRoundRect(&display, 7, 50, 45, 13, 5); //  Temp Border

  drawRoundRect(&display, 2, 2, 60, 11, 5); //  Acceleration Degrees Border
  drawRoundRect(&display, 65, 2, 60, 11, 5); //  Gyro Degrees Border
  display.UpdateScreen (addr);
  return true;
}


bool mpuCheckForUpdate() {

  // Blocking burst-read of 14 bytes. (Uses a while loop by specifying the Wait flag.)

  bool result = ctwi.Read(MPU_ADDRESS, mpu.reg, mpu.buffer, sizeof(mpu.buffer), Modes::Wait);

  if (result)
  {
    if (ctwi.GetResult() == Results::Success)
    {
      mpu.AccelTempGyroData();
      mpu.MovementAngle();

      fillRect(&display, 0, 15, 124, 40, WriteMode::SUBTRACT); //  Middle Section clear

      // Acceleration Clear Data
      fillRect(&display, 3, 3, 58, 10, WriteMode::SUBTRACT); //  Acceleration Degrees Border

      //&display, int16_t x0, int16_t y0, int16_t x1, int16_t y1, int16_t x2, int16_t y2
      fillTriangle(&display, 54, 4, mpu.accDirX == 0 ? 54 : mpu.accDirX == 1 ? 56 : 52, 6, 54, 8);
      fillTriangle(&display, 44, 6, 46, mpu.accDirY == 0 ? 6 : mpu.accDirY == 1 ? 4 : 8, 48, 6);

      // Acceleration Text Data
      static char dirAccFormat[5];
      dtostrf(mpu.deg[0], 3, 1, dirAccFormat);

      drawText(&display, font_5x8, dirAccFormat, 5, 2);
      drawCircle(&display, ((String(mpu.deg[0], 1).length() + 2) * 5), 5, 1);  // Acc Degrees Sign
      //drawText(&display, font_5x8, mpu.state[0], 3, 20);

      // Acceleration Plot
      fillCircle(&display, 59 - 20, 30, (mpu.accDirX == 0 && mpu.accDirY == 0) ? mpu.radius[0] : 1);

      drawCircle(&display, 59 - 20, 30, mpu.radius[0]);
      drawLine(&display, 59 - 20, 30, (59 + mpu.adj[0] * mpu.radius[0]) - 20, 30 + mpu.opp[0] * mpu.radius[0]);

      // Gyro Clear Data
      fillRect(&display, 67, 3, 120, 10, WriteMode::SUBTRACT); //  Gyro Degrees Border

      fillTriangle(&display, 118, 4, mpu.gyroDirX == 0 ? 118 : mpu.gyroDirX == 1 ? 120 : 116, 6, 118, 8);
      fillTriangle(&display, 108, 6, 110, mpu.gyroDirY == 0 ? 6 : mpu.gyroDirY == 1 ? 4 : 8, 112, 6);

      // Gyro Text Data
      static char dirGyroFormat[5];
      dtostrf(mpu.deg[1], 3, 1, dirGyroFormat);

      drawText(&display, font_5x8, dirGyroFormat, 68, 2);
      drawCircle(&display, ((String(mpu.deg[1], 1).length() + 2) * 5) + 64, 5, 1);  // Gyro Degrees Sign

      // Gyro Plot
      fillCircle(&display, 59 + 20, 30, (mpu.gyroDirX == 0 && mpu.gyroDirY == 0) ? mpu.radius[1] : 1);

      drawCircle(&display, 59 + 20, 30, mpu.radius[1]);
      drawLine(&display, 59 + 20, 30, (59 + mpu.adj[1] * mpu.radius[1]) + 20, 30 + mpu.opp[1] * mpu.radius[1]);

      static char tempFormat[4];
      dtostrf(mpu.temperatureC, 2, 1, tempFormat);
      fillRect(&display, 9, 52, 47, 58, WriteMode::SUBTRACT); //  Temp Border
      drawText(&display, font_8x8, tempFormat, 9, 51);

      display.UpdateScreen (addr);

    }
    else if (ctwi.GetResult() == Results::Timeout)
    {
      Serial.println("Timeout.");
    }
    else
    {
      Serial.println("Comms error");
    }
  }
  return true;
}


void rtcCheckForUpdate() {
  //Blocking burst-read of 14 bytes. (Uses a while loop by specifying the Wait flag.)
  bool result = ctwi.Read(DS1307_ADDRESS, rtc.reg, rtc.buffer, sizeof(rtc.buffer), Modes::Wait);

  if (result)
  {
    if (ctwi.GetResult() == Results::Success)
    {
      rtc.TimeData();
      int secNow = rtc.getSecond();
      if (sec != secNow) {
        sec = secNow;

        static char timeFormat[12];
        sprintf(timeFormat, "%02d:%02d:%02d", rtc.getHour(), rtc.getMinute(), rtc.getSecond());


        fillRect(&display, 58, 52, 120, 58, WriteMode::SUBTRACT);  // Time Clear
        drawText(&display, font_8x8, timeFormat, 58, 51);   // Print Time Formated Text

        display.UpdateScreen (addr);
      }
    }
    else if (ctwi.GetResult() == Results::Timeout) Serial.println("Timeout.");
    else Serial.println("Comms error");
  }
}