#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>
//#include "twi.h"

// EEPROM Constants
#define PASSWORD_ADDRESS 0
#define MAX_PASSWORD_LENGTH 20
#define DEFAULT_PASSWORD "EEE20003"
#define ROTATION_ADDRESS (PASSWORD_ADDRESS + MAX_PASSWORD_LENGTH)
#define ROTATION_LENGTH 9

// Display setup Constants
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32

int leftx = 130;
int rightx = -30;
int up_y = 90;
int down_y = -25;
int steady_r = 0;
#define SLIDER_SPEED 10

#define TRIGGER_PIN 4
#define ECHO_PIN 5
#define BUZZER_PIN A5

// Gyro Constants
#define STEADY_RADIUS 0.26 // Steady radius of the gyroscope in radians.

// Display Graphic Constants
#define STEADY_CIRCLE_RADIUS 10 // Radius of the circle on the display

// Animation Constants
#define CICRLE_ANIMATION_SPEED 2

Adafruit_MPU6050 mpu;
sensors_event_t accelation_event, gyro_event, temp_events;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


bool refreshIO = false;
unsigned long buzzerCounter = 0;
int distance = 500;

unsigned long testCounter = 0;
unsigned long testCounter1 = 0;
bool pauseTimer = false;

void setup() {
  setupComponents();
  saveDefaultPassword();
  bool loggedIn = false;

  pinMode(TRIGGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUZZER_PIN, OUTPUT);

  while (!loggedIn){
    loggedIn = validateUser();
  }

  displayMenu();

  
  display.setTextSize(0.8);
  display.setTextColor(SSD1306_WHITE);

}


void readWriteRotation(){
  Serial.println("Retrieving rotation values from sensor...");
  mpu.getEvent(&accelation_event, &gyro_event, &temp_events);
  float x =gyro_event.gyro.x;
  float y =gyro_event.gyro.y;
  float z =gyro_event.gyro.z;
  Serial.println("X: " + String(x) + ", Y: " +String(y) + ", Z: " + String(z) + " rads");
  SaveRotationToEEPROM(x,y,z);
  Serial.println("Data stored successfully.\n");

  float loadX, loadY, loadZ;
  Serial.println("Retrieving rotation values from EEPROM...");
  loadRotationToEEPROM(&loadX,&loadY,&loadZ);
  Serial.println("X: " + String(loadX) + ", Y: " + String(loadY) + ", Z: " + String(loadZ) + " rads");

}

void loadRotationToEEPROM(float* x, float* y, float* z){
  int storeCursor = ROTATION_ADDRESS;
  *x = pow(-1 , EEPROM.read(storeCursor)) * ( EEPROM.read(storeCursor + 1) + EEPROM.read(storeCursor + 2)/100.0);
  storeCursor += 3;
  *y = pow(-1 , EEPROM.read(storeCursor)) * ( EEPROM.read(storeCursor + 1) + EEPROM.read(storeCursor + 2)/100.0);
  storeCursor += 3;
  *z = pow(-1 , EEPROM.read(storeCursor)) * ( EEPROM.read(storeCursor + 1) + EEPROM.read(storeCursor + 2)/100.0);
}

void SaveRotationToEEPROM(float x, float y, float z){
  int xNeg = x < 0;
  int xInt = (int)x;
  int xDec = (int)((x- xInt)*100);

  int yNeg = y < 0;
  int yInt = (int)y;
  int yDec = (int)((y-yInt)*100);

  int zNeg = z < 0;
  int zInt = (int)z;
  int zDec = (int)((z-zInt)*100);

  int storeCursor = ROTATION_ADDRESS;
  EEPROM.update(storeCursor, xNeg);
  EEPROM.update(storeCursor + 1, abs(xInt));
  EEPROM.update(storeCursor + 2, abs(xDec));
  
  storeCursor += 3;
  EEPROM.update(storeCursor, yNeg);
  EEPROM.update(storeCursor + 1, abs(yInt));
  EEPROM.update(storeCursor + 2, abs(yDec));

  storeCursor += 3;
  EEPROM.update(storeCursor, zNeg);
  EEPROM.update(storeCursor + 1, abs(zInt));
  EEPROM.update(storeCursor + 2, abs(zDec));
}



void displayMenu(){
  Serial.println("-> Option 1: Read/Write initial rotation of IMU");
  Serial.println("-> Option 2: Read/Write sensor data");
  Serial.println("-> Option 3: Start main routine");
  Serial.println("-> Option 4: Change password");
  Serial.print("Please select an option (1 to 4):");

}


void setupTimer(){

  cli();//stop interrupts


  //set timer1 interrupt at 10Hz
  TCCR1A = 0;// set entire TCCR1A register to 0
  TCCR1B = 0;// same for TCCR1B
  TCNT1  = 0;//initialize counter value to 0
  // set compare match register for 1hz increments
  OCR1A = 24999;// = (16*10^6) / (1*256) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12 and CS10 bits for 64 prescaler
  TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);  
  // enable timer compare interrupt
  TIMSK1 |= (1 << OCIE1A);

  sei();//allow interrupts
}

void saveDefaultPassword(){
  //Chekcking whether the password has already been initialised. 
  if(EEPROM.read(PASSWORD_ADDRESS) == 0){
    return;
  }
  savePassword(DEFAULT_PASSWORD);
}

void savePassword(String password){
  String savePassword = password + ' ';
  EEPROM.update(PASSWORD_ADDRESS, 0);
  for(int i = PASSWORD_ADDRESS + 1; i < MAX_PASSWORD_LENGTH; i++){
    EEPROM.update(i, (byte)savePassword[i- 1]);
    if(savePassword[i - 1] == ' '){
      break;
    }
  }
}


String loadPassword(){
  char password[MAX_PASSWORD_LENGTH];
  int i;
  bool passwordCompleted = false;
  for(i = PASSWORD_ADDRESS+1; i < MAX_PASSWORD_LENGTH; i++){
    password[i-1] = (char)EEPROM.read(i);
    if((char)EEPROM.read(i) == ' '){
      break;
    }
  }
  password[i-1] = 0;
  return password;
}



bool validateUser(){
  Serial.print("Please enter the password: ");

  while(Serial.peek() == '\n'){Serial.read();}
  while(Serial.available() == 0){}
  String input = Serial.readStringUntil('\n');
  input.trim();
  Serial.println(input);

  if(input == loadPassword()){
    Serial.println("Access Granted!");
    return true;
  }else{
    Serial.println("Incorrect Password!");
    return false;
  }

}


void setupComponents(){
  Serial.begin(2400);
  Serial.println("Initialising...");
  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");
    delay(1000);
  }
  while (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println("Display not connected!");
    delay(1000);
  }
  Serial.println("Component setup successful.");
}

ISR(TIMER1_COMPA_vect){
  refreshIO = true;
  display.clearDisplay();

  display.setCursor(0, SCREEN_HEIGHT- 8);

   if(sq(STEADY_RADIUS) >= sq(gyro_event.gyro.x)+ sq(gyro_event.gyro.y)){
   animateSteady();
   display.print("STEADY");
   }else{
    if(abs(gyro_event.gyro.x) > abs(gyro_event.gyro.y)){
      if(gyro_event.gyro.x > 0){
        animateRightArrow();
        display.print("RIGHT");
      }else{
        animateLeftArrow();
        display.print("LEFT");
      }
    }else{
      if(gyro_event.gyro.y > 0){
        animateUpArrow();
        display.print("UP");
      }else{
        animateDownArrow();
        display.print("DOWN");
      }
    }
  }
   displayDistance();
}

void loop() {
  // event-driven programming.
  if(refreshIO){
    mpu.getEvent(&accelation_event, &gyro_event, &temp_events);
    display.display();
    //getDistance();  //Saves the distance for later use. Fetching it in the interrupt breaks it due to the pulseIn function.
    refreshIO = false;
  }
  testCounter += 1;

  if (testCounter > 1100) {
      stopTimer();
      // Chech if it is a new line feed in ASCII
      while(Serial.peek() == 10){ Serial.read(); }
      if(Serial.available() != 0 && testCounter == 1201){
        checkSerial();
      }
      if(!pauseTimer && testCounter >= 6715){
        resumeTimer();
      }
  }
  if (testCounter > 6717){
      testCounter = 0;
  }
}

void checkSerial(){
  
  Serial.println(Serial.peek());
  int input = Serial.parseInt();
  Serial.println(input);
  
  if (input < 1 || input > 4){
    Serial.println("Incorrect input");
  }else{
    pauseTimer = true;
    switch(input){
      case 1:{
        readWriteRotation();
      }
      break;
      case 2:{
        display.display();
      }
      break;
      case 3: {
        setupTimer();
        pauseTimer = false;
      }
      break;
      case 4: {
        changePassword();
      }
      break;
    }
    Serial.println(' ');
  }
  displayMenu();
}

void serialEvent(){
  
}

void changePassword(){
  if(validateUser()){
    Serial.print("New Password: ");
    while(Serial.peek() == '\n'){Serial.read();}
    while(Serial.available() == 0){}
    String pass = Serial.readStringUntil('\n');
    pass.trim();
    Serial.println(pass);

    Serial.print("Confirm Password: ");
    while(Serial.peek() == '\n'){Serial.read();}
    while(Serial.available() == 0){}
    String confirmPass = Serial.readStringUntil('\n');
    confirmPass.trim();
    Serial.println(confirmPass);

    if(pass == confirmPass){
      savePassword(pass);
      Serial.println("Password changed successfully.");
    }else{
      Serial.println("Passwords do not match.");
    }
  }
}

void stopTimer(){
  cli();//stop interrupts
  TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10));
  sei(); 
}

void resumeTimer(){
  cli();//resume interrupts
  TCCR1B |= (0 << CS12) | (1 << CS11) | (1 << CS10);  
  sei(); 
}

void animateSteady(){
  if(steady_r < STEADY_CIRCLE_RADIUS){
    steady_r += CICRLE_ANIMATION_SPEED;
  }
  display.fillCircle(SCREEN_WIDTH / 2, SCREEN_HEIGHT / 2, steady_r, WHITE);
}

void animateRightArrow(){

  display.fillRect((rightx-60), 22, 60, 20, WHITE);
  display.fillTriangle((rightx-60), 15, (rightx-60), 22, rightx, 22, WHITE);
  display.fillTriangle((rightx-60), 49, (rightx-60), 42, rightx, 42, WHITE);
  display.fillTriangle(rightx,15,rightx,49,(rightx+25),32, WHITE);
  rightx = rightx + SLIDER_SPEED;
  if (rightx==200){
    rightx = -30;
  }
}                          

void animateLeftArrow(){
  
  display.fillRect(leftx, 20, 60, 20, WHITE);
  display.fillTriangle(leftx,20, (leftx+60), 13, (leftx+60), 20, WHITE);
  display.fillTriangle(leftx, 40, (leftx+60), 40, (leftx+60), 47, WHITE);
  display.fillTriangle(leftx,13,leftx,47,(leftx-25),30, WHITE);
  leftx = (leftx-SLIDER_SPEED);
  if (leftx <= -71){
    leftx = 130;
  }

}

void animateUpArrow(){
 
  display.fillRect(54,up_y,20,40, WHITE);
  display.fillTriangle(48,up_y,80,up_y,64,(up_y-20),WHITE);
  display.fillTriangle(48,(up_y+40),54,(up_y+40),54,up_y, WHITE);
  display.fillTriangle(74,(up_y+40),80,(up_y+40),74, up_y, WHITE);
  up_y = up_y-SLIDER_SPEED;
  if (up_y<=-40){
    up_y = 90;
  }
}

void animateDownArrow(){

   display.fillRect(54,(down_y-40),20,40, WHITE);
  display.fillTriangle(48,(down_y-40),54,(down_y-40),54,down_y, WHITE);
  display.fillTriangle(74,(down_y-40),80,(down_y-40),74,down_y, WHITE);
  display.fillTriangle(48,down_y,64,(down_y+20),80,down_y, WHITE);
  down_y = down_y + SLIDER_SPEED;
  if (down_y >= 110){
    down_y = -25;
  }
}

void getDistance(){
  digitalWrite(TRIGGER_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIGGER_PIN, LOW);

  float timeDuration = pulseIn(ECHO_PIN, HIGH);
  distance =  ((int)(timeDuration / (23355/400)));
}

void displayDistance(){

  //Serial.println(distance);
  getDistance();
  display.setCursor(SCREEN_WIDTH - 37, SCREEN_HEIGHT - 8);
  display.print(String(distance) + " cm");
  switch(distance){
    case 0 ... 59:
    {
      tone(BUZZER_PIN, 700, 500);
      
      break;
    }
    case 60 ... 129:
    {
      if(buzzerCounter % 10  == 0){
          tone(BUZZER_PIN, 700, 500);
      }     
       break;
    }
    case 130 ... 200:
    {
      if(buzzerCounter % 20 == 0){
          tone(BUZZER_PIN, 700, 500);
      }       
      break;
    }
    default:
    {
      break;
    }
  }
  if (buzzerCounter == pow(2,32) - 2){
    buzzerCounter = 0;
  }
  buzzerCounter += 1;
}