#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;

#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 BLACKBOX_START_X 20
#define BLACKBOX_START_Y 2
#define BLACKBOX_WIDTH 92
#define BLACKBOX_HEIGHT 62
#define ARROW_ANIMATION_SPEED 10
#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);


enum tiltState{
  RIGHT,
  LEFT,
  UP,
  DOWN,
  STEADY
};


int x = 0;
tiltState prevState;

bool refreshIO = false;

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

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

  //TODO:: remove comment
  while (!loggedIn){
    loggedIn = validateUser();
  }

  switch(displayMenu()){
    case 1:{
      readWriteRotation();
    }
    break;
    case 2:{
    }
    break;
    case 3: {
      setupTimer();
    }
    break;
  }

  
  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));
}



int 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");

  int input = 0;
  while (input < 1 || input > 3){
    Serial.print("Please select an option (1 to 3):");
    while(Serial.peek() == '\n'){Serial.read();}
    while(Serial.available() == 0);
    input = Serial.readStringUntil('\n').toInt();
    Serial.println(input);
  }

  Serial.println(' ');
  return input;
}


void setupTimer(){

  cli();//stop interrupts


//set timer1 interrupt at 100Hz
  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 = 2496;// = (16*10^6) / (1*256) - 1 (must be <65536)
  // turn on CTC mode
  TCCR1B |= (1 << WGM12);
  // Set CS12 and CS10 bits for 256 prescaler
  TCCR1B |= (1 << CS12) | (0 << CS11) | (0 << 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;
  }
  String savePassword = DEFAULT_PASSWORD + ' ';
  EEPROM.update(PASSWORD_ADDRESS, 0);
  for(int i = PASSWORD_ADDRESS + 1; i < MAX_PASSWORD_LENGTH; i++){
    if(DEFAULT_PASSWORD[i - 1] == ' '){
      break;
    }
    EEPROM.update(i, (byte)DEFAULT_PASSWORD[i- 1]);
  }
}

String loadPassword(){
  char password[MAX_PASSWORD_LENGTH] = "";
  int i;
  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] = "";
  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(9600);
  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){
    display.clearDisplay();

  //displayDistance();
  refreshIO = true;
  
  tiltState currState;

  display.setCursor(0, SCREEN_HEIGHT- 8);

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

  if(currState != prevState){
    x = 0;
    prevState = currState;
  }

}
void loop() {
  // event-driven programming.
  if(refreshIO){
    mpu.getEvent(&accelation_event, &gyro_event, &temp_events);
    display.display();
    refreshIO = false;
  }
}

void animateSteady(){
  if(x < STEADY_CIRCLE_RADIUS){
    x += CICRLE_ANIMATION_SPEED;
  }
  display.fillCircle(SCREEN_WIDTH / 2, SCREEN_HEIGHT / 2, x, 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 + 5;
  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-5);
  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-5;
  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 + 5;
  if (down_y >= 110){
    down_y = -25;
  }
}