/* The x accelaration slider will simulate the backward and forward tilting
   The y acceleration slider will simulate the left and right tilting */


#include <EEPROM.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <MD_MAX72xx.h> 
#include <SPI.h> 

 
#define HARDWARE_TYPE MD_MAX72XX::PAROLA_HW 
#define MAX_DEVICES 1 // Define the number of displays connected 
#define CLK_PIN    52 // CLK or SCK 
#define DATA_PIN   51 // DATA or MOSI 
#define CS_PIN     53 // CS or SS 
 
MD_MAX72XX mx = MD_MAX72XX(HARDWARE_TYPE, CS_PIN, MAX_DEVICES);

Adafruit_MPU6050 mpu; 

sensors_event_t event;   //This Program only uses accellerometer 

char pass[]="EEE20003";                          //sets password in character array 'pass'
int passSize=sizeof(pass),count=0,countA=0;       
float initialX=0,initialY=0;                    //values for initial accellerometer reading

volatile float X,Y;                             //variables for accelerometer reading in setup
volatile long dela,prox;                        //prox = proximitiy (distance sensor)
volatile int pulse;
volatile boolean check=0;                       //used in main loop and timer interrupt

void symbol(int);       //prototype for LED matrix function

void setup() {

  for(int i=0; i<passSize;i++){
    EEPROM.write(i,pass[i]);      //stores password in EEPROM
  }
  
  pinMode(3, OUTPUT);   //Pinmodes for distance sensor (pulseIn an pulseOUT)
  pinMode(2, INPUT);

  Serial.begin(9600);
  Serial.println("Enter password:");
  
  boolean passCorrect=0;
  do{
    while(Serial.available()==0){}
    delay(20);
    if(Serial.available()==passSize)
    {
      char input[sizeof(pass)];
      int val;
      for(int i=0;i<passSize;i++)
      {
        val=Serial.read();          //stores characters in vector 'input'
        input[i]=val;
      }
      boolean passWrong=0;
      for(int i=0;i<passSize-1;i++){
        char letter=EEPROM.read(i);       //checks each character stored in EEPROM 
        if(input[i]!=letter){             //if there is a discrepancy, break and mark wrong password
           passWrong=1;
           break;
        }
      }
      if(passWrong==0){
        passCorrect=1;
      }  
    }
    if(passCorrect==0){
      Serial.println("Password incorrect");
    }
    while(Serial.available()!=0){
      Serial.read();
    }
  }
  while(passCorrect==0);    //end of do-while loop, loops again if password is incorrect, otherwise proceed

  while (!mpu.begin()) {
    Serial.println("MPU6050 not connected!");   //mandatory for function of accellerometer
    delay(1000);
  }
  Serial.println("MPU6050 ready!");

  Serial.println("Select an option:\n 1-Calibrate IMU Unit\n 2-test ultrasonic sensor\n 3-continue to full operation");
  //prompt user with menu
  int option=0,cont=0;
  do{
    while(Serial.available()==0){}
    option=Serial.parseInt();
    if(option==1){                                          //calibrate IMU
      mpu.getAccelerometerSensor()->getEvent(&event);
      X=event.acceleration.x;                               //reads X and Y accelleration in m/s/s(orientation)
      Y=event.acceleration.y;
      initialX=X;                                           //stores initial values of accelleration to variables
      initialY=Y;
      Serial.print(initialX/9.8);                           //displays axial accelleration in g-forces for user
      Serial.println(" g");
      Serial.print(initialY/9.8);
      Serial.println(" g");
      EEPROM.put(200,initialX);                             //stores initial axial accelleration values in EEPROM
      EEPROM.put(204,initialY);
    }
    else if(option==2){                               //Distance sensor test
      digitalWrite(3, HIGH);                             
      delayMicroseconds(10);
      digitalWrite(3, LOW);

      int duration = pulseIn(2, HIGH);
      long distance = duration/58;          //converts echo time to distance using speed of sound
      EEPROM.write(100,distance);           //stores in EEPROM
      Serial.print(EEPROM.read(100));       //displays distance for user
      Serial.println(" cm");
    }
    else if(option==3){                     //breaks do-while loop 
      cont=1;
    }
  }while(cont==0);                          //repeats loop/prompts user again

  mx.begin(); 
  mx.control(MD_MAX72XX::INTENSITY, MAX_INTENSITY / 2); 
  mx.clear();
  
  cli();

  TCCR1A=0;
  TCCR1B=0;
  TCNT1=0;

  OCR1A = 3906;                 //timer interrupt is set to occur on the 1/2 second

  TCCR1B |= (1<<WGM12);
  TCCR1B |=(1<<CS12)|(1<<CS10);
  TIMSK1 |=(1<<OCIE1A);

  sei();
}

//tilt = acc/9.8 * 90

float tilt=4.9;     //tilt is in m/s/s ... 4.9 corresponds 0.5 g, or a 45 degree tilt
int mode=1;

void loop() {
  if(check==1){                                         //checks accelerometer reading when interrupt is triggered
    mpu.getAccelerometerSensor()->getEvent(&event);
    X=event.acceleration.x;     //x corresponds to forwards (-ve) and backwards (+ve) head movement
    Y=event.acceleration.y;     //y corresponds to left (+ve) and right (-ve)

digitalWrite(3, HIGH);
delayMicroseconds(10);
digitalWrite(3, LOW);

  pulse = pulseIn(2, HIGH);
  prox = pulse/58;
    check=0;
  }

  float Xinitial;           
  EEPROM.get(200,Xinitial);    //passes EEPROM value stored in the setup to variables Xinitial and Yinitial
  float Yinitial;
  EEPROM.get(204,Yinitial);     
  float x=X-Xinitial;
  float y=Y-Yinitial;         //calibration/offset of accellerometer reading (sets 0)
  
if (x <= -tilt)         //forwards threshold
{
  if (y >= tilt)        //tilting left and foward
  {
    symbol(6);          //diagonal, forwards and left
  }
  else if (y <= -tilt)  //tilting right and forward
  {
    symbol(7);          //diagonal, forwards and right
  }
  else 
  {
    symbol(2);          //default (forwards)
  }
}

else if (x >= tilt && prox>100)     //tilting backwards and no object detected within 1 metre
{
  if (y >= tilt)                    //tiling left and back
  {
    symbol(8);                      //diagonal, left and back
  }
  else if (y <= -tilt)              //tilting right and back
  {
    symbol(9);                      //diagonal, right and back
  }
  else 
  {
    symbol(3);                      //default (backwards)
  }
}

else if (y <= -tilt)                //commands for right and left arrows
{
  symbol(4);                        //right
}

else if (y >= tilt)
{
  symbol(5);                        //left
}
else
symbol(1);                          //motionless

  }


ISR(TIMER1_COMPA_vect){


  check=1;                  //prompts program to read accelerometer data
}

int pause=75;
void symbol(int a){
  countA++;
  if(countA>6*pause){
    countA=1*pause;
  }
  count=countA/pause;

  //switch statment displays the symbol selected in the function call
  switch (a){
    //Displays a square in the center of the LED Matrix
    case 1:
      mx.setRow(0, 0);//the first parameter is the row being set 
      mx.setRow(1, 0); //the second parameter is converted into an 8 digit binary number
      mx.setRow(2, 60);//each of these digits will represent the state of each led int the row 
      mx.setRow(3, 60); 
      mx.setRow(4, 60); 
      mx.setRow(5, 60); 
      mx.setRow(6, 0);
      mx.setRow(7, 0);
      mx.update(); // update the display once the led states have been set 
      break;
    
    
    case 2: //displays a forward arrow on the LED Matrix
      switch(count){
        case 1:
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 0); 
          mx.setRow(3, 0); 
          mx.setRow(4, 24); 
          mx.setRow(5, 60); 
          mx.setRow(6, 126);
          mx.setRow(7, 255);  
          mx.update(); 
          break;
        case 2:
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 0); 
          mx.setRow(3, 24); 
          mx.setRow(4, 60); 
          mx.setRow(5, 126); 
          mx.setRow(6, 255);
          mx.setRow(7, 24);  
          mx.update(); 
          break;
        case 3:
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 24); 
          mx.setRow(3, 60); 
          mx.setRow(4, 126); 
          mx.setRow(5, 255); 
          mx.setRow(6, 24);
          mx.setRow(7, 24);  
          mx.update(); 
          break;
        case 4:
          mx.setRow(0, 0); 
          mx.setRow(1, 24); 
          mx.setRow(2, 60); 
          mx.setRow(3, 126); 
          mx.setRow(4, 255); 
          mx.setRow(5, 24); 
          mx.setRow(6, 24);
          mx.setRow(7, 24);  
          mx.update(); 
          break;
        case 5:
          mx.setRow(0, 24); 
          mx.setRow(1, 60); 
          mx.setRow(2, 126); 
          mx.setRow(3, 255); 
          mx.setRow(4, 24); 
          mx.setRow(5, 24); 
          mx.setRow(6, 24);
          mx.setRow(7, 24);  
          mx.update(); 
          break;
      }
      break;
    
    case 3: //displays a backward arrow on the LED Matrix
      switch(count){
        case 1:
          mx.setRow(0, 255); 
          mx.setRow(1, 126); 
          mx.setRow(2, 60); 
          mx.setRow(3, 24); 
          mx.setRow(4, 0); 
          mx.setRow(5, 0); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 2:
          mx.setRow(0, 24); 
          mx.setRow(1, 255); 
          mx.setRow(2, 126); 
          mx.setRow(3, 60); 
          mx.setRow(4, 24); 
          mx.setRow(5, 0); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 3:
          mx.setRow(0, 24); 
          mx.setRow(1, 24); 
          mx.setRow(2, 255); 
          mx.setRow(3, 126); 
          mx.setRow(4, 60); 
          mx.setRow(5, 24); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 4:
          mx.setRow(0, 24); 
          mx.setRow(1, 24); 
          mx.setRow(2, 24); 
          mx.setRow(3, 255); 
          mx.setRow(4, 126); 
          mx.setRow(5, 60); 
          mx.setRow(6, 24);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 5:
          mx.setRow(0, 24); 
          mx.setRow(1, 24); 
          mx.setRow(2, 24); 
          mx.setRow(3, 24); 
          mx.setRow(4, 255); 
          mx.setRow(5, 126); 
          mx.setRow(6, 60);
          mx.setRow(7, 24);  
          mx.update(); 
          break;
      }    
      break;


    case 4: //displays a Right arrow on the LED Matrix
      switch(count){
        case 1:
          mx.setRow(0, 128); 
          mx.setRow(1, 192); 
          mx.setRow(2, 224); 
          mx.setRow(3, 240); 
          mx.setRow(4, 240); 
          mx.setRow(5, 224); 
          mx.setRow(6, 192);
          mx.setRow(7, 128);  
          mx.update(); 
          break;
        case 2:
          mx.setRow(0, 64); 
          mx.setRow(1, 96); 
          mx.setRow(2, 112); 
          mx.setRow(3, 248); 
          mx.setRow(4, 248); 
          mx.setRow(5, 112); 
          mx.setRow(6, 96);
          mx.setRow(7, 64);  
          mx.update(); 
          break;
        case 3:
          mx.setRow(0, 32); 
          mx.setRow(1, 48); 
          mx.setRow(2, 56); 
          mx.setRow(3, 252); 
          mx.setRow(4, 252); 
          mx.setRow(5, 56); 
          mx.setRow(6, 48);
          mx.setRow(7, 32);  
          mx.update(); 
          break;
        case 4:
          mx.setRow(0, 16); 
          mx.setRow(1, 24); 
          mx.setRow(2, 28); 
          mx.setRow(3, 254); 
          mx.setRow(4, 254); 
          mx.setRow(5, 28); 
          mx.setRow(6, 24);
          mx.setRow(7, 16);  
          mx.update(); 
          break;
        case 5:
          mx.setRow(0, 8); 
          mx.setRow(1, 12); 
          mx.setRow(2, 14); 
          mx.setRow(3, 255); 
          mx.setRow(4, 255); 
          mx.setRow(5, 14); 
          mx.setRow(6, 12);
          mx.setRow(7, 8);  
          mx.update(); 
          break;
      }
      break;


    case 5: //displays a Left arrow on the LED Matrix
      switch(count){
        case 1:  
          mx.setRow(0, 1); 
          mx.setRow(1, 3); 
          mx.setRow(2, 7); 
          mx.setRow(3, 15); 
          mx.setRow(4, 15); 
          mx.setRow(5, 7); 
          mx.setRow(6, 3);
          mx.setRow(7, 1);  
          mx.update(); 
          break;
        case 2:  
          mx.setRow(0, 2); 
          mx.setRow(1, 6); 
          mx.setRow(2, 14); 
          mx.setRow(3, 31); 
          mx.setRow(4, 31); 
          mx.setRow(5, 14); 
          mx.setRow(6, 6);
          mx.setRow(7, 2);  
          mx.update(); 
          break;
        case 3:  
          mx.setRow(0, 4); 
          mx.setRow(1, 12); 
          mx.setRow(2, 28); 
          mx.setRow(3, 63); 
          mx.setRow(4, 63); 
          mx.setRow(5, 28); 
          mx.setRow(6, 12);
          mx.setRow(7, 4);  
          mx.update(); 
          break;
        case 4:  
          mx.setRow(0, 8); 
          mx.setRow(1, 24); 
          mx.setRow(2, 56); 
          mx.setRow(3, 127); 
          mx.setRow(4, 127); 
          mx.setRow(5, 56); 
          mx.setRow(6, 24);
          mx.setRow(7, 8);  
          mx.update(); 
          break;
        case 5:  
          mx.setRow(0, 16); 
          mx.setRow(1, 48); 
          mx.setRow(2, 112); 
          mx.setRow(3, 255); 
          mx.setRow(4, 255); 
          mx.setRow(5, 112); 
          mx.setRow(6, 48);
          mx.setRow(7, 16);  
          mx.update(); 
          break;
      }
      break;


    case 6: 
      switch(count){
        case 1:  
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 0); 
          mx.setRow(3, 0); 
          mx.setRow(4, 15); 
          mx.setRow(5, 14); 
          mx.setRow(6, 15);
          mx.setRow(7, 11);  
          mx.update(); 
          break;
        case 2:  
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 0); 
          mx.setRow(3, 30); 
          mx.setRow(4, 28); 
          mx.setRow(5, 30); 
          mx.setRow(6, 23);
          mx.setRow(7, 3);  
          mx.update(); 
          break;
        case 3:  
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 60); 
          mx.setRow(3, 56); 
          mx.setRow(4, 60); 
          mx.setRow(5, 46); 
          mx.setRow(6, 7);
          mx.setRow(7, 3);  
          mx.update(); 
          break;
        case 4:  
          mx.setRow(0, 0); 
          mx.setRow(1, 120); 
          mx.setRow(2, 112); 
          mx.setRow(3, 120); 
          mx.setRow(4, 92); 
          mx.setRow(5, 14); 
          mx.setRow(6, 7);
          mx.setRow(7, 2);  
          mx.update(); 
          break;
        case 5:  
          mx.setRow(0, 240); 
          mx.setRow(1, 224); 
          mx.setRow(2, 240); 
          mx.setRow(3, 56); 
          mx.setRow(4, 28); 
          mx.setRow(5, 14); 
          mx.setRow(6, 4);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        }
        break;


    case 7: 
      switch(count){
        case 1:  
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 0); 
          mx.setRow(3, 0); 
          mx.setRow(4, 240); 
          mx.setRow(5, 112); 
          mx.setRow(6, 240);
          mx.setRow(7, 208);  
          mx.update(); 
          break;
        case 2:  
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 0); 
          mx.setRow(3, 120); 
          mx.setRow(4, 56); 
          mx.setRow(5, 120); 
          mx.setRow(6, 232);
          mx.setRow(7, 192);  
          mx.update(); 
          break;
        case 3:  
          mx.setRow(0, 0); 
          mx.setRow(1, 0); 
          mx.setRow(2, 60); 
          mx.setRow(3, 28); 
          mx.setRow(4, 60); 
          mx.setRow(5, 116); 
          mx.setRow(6, 224);
          mx.setRow(7, 192);  
          mx.update(); 
          break;
        case 4:  
          mx.setRow(0, 0); 
          mx.setRow(1, 30); 
          mx.setRow(2, 14); 
          mx.setRow(3, 30); 
          mx.setRow(4, 58); 
          mx.setRow(5, 112); 
          mx.setRow(6, 224);
          mx.setRow(7, 64);  
          mx.update(); 
          break;
        case 5:  
          mx.setRow(0, 15); 
          mx.setRow(1, 7); 
          mx.setRow(2, 15); 
          mx.setRow(3, 29); 
          mx.setRow(4, 56); 
          mx.setRow(5, 112); 
          mx.setRow(6, 32);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        }
        break;

    
    case 8: 
      switch(count){
        case 1:  
          mx.setRow(0, 11); 
          mx.setRow(1, 15); 
          mx.setRow(2, 14); 
          mx.setRow(3, 15); 
          mx.setRow(4, 0); 
          mx.setRow(5, 0); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 2:  
          mx.setRow(0, 3); 
          mx.setRow(1, 23); 
          mx.setRow(2, 30); 
          mx.setRow(3, 28); 
          mx.setRow(4, 30); 
          mx.setRow(5, 0); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 3:  
          mx.setRow(0, 3); 
          mx.setRow(1, 7); 
          mx.setRow(2, 46); 
          mx.setRow(3, 60); 
          mx.setRow(4, 56); 
          mx.setRow(5, 60); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 4:  
          mx.setRow(0, 2); 
          mx.setRow(1, 7); 
          mx.setRow(2, 14); 
          mx.setRow(3, 92); 
          mx.setRow(4, 120); 
          mx.setRow(5, 112); 
          mx.setRow(6, 120);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 5:  
          mx.setRow(0, 0); 
          mx.setRow(1, 4); 
          mx.setRow(2, 14); 
          mx.setRow(3, 28); 
          mx.setRow(4, 56); 
          mx.setRow(5, 240); 
          mx.setRow(6, 224);
          mx.setRow(7, 240);  
          mx.update(); 
          break;
        }
        break;


    case 9: 
      switch(count){
        case 1:  
          mx.setRow(0, 208); 
          mx.setRow(1, 240); 
          mx.setRow(2, 112); 
          mx.setRow(3, 240); 
          mx.setRow(4, 0); 
          mx.setRow(5, 0); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 2:  
          mx.setRow(0, 192); 
          mx.setRow(1, 232); 
          mx.setRow(2, 120); 
          mx.setRow(3, 56); 
          mx.setRow(4, 120); 
          mx.setRow(5, 0); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 3:  
          mx.setRow(0, 192); 
          mx.setRow(1, 224); 
          mx.setRow(2, 116); 
          mx.setRow(3, 60); 
          mx.setRow(4, 28); 
          mx.setRow(5, 60); 
          mx.setRow(6, 0);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 4:  
          mx.setRow(0, 64); 
          mx.setRow(1, 224); 
          mx.setRow(2, 112); 
          mx.setRow(3, 58); 
          mx.setRow(4, 30); 
          mx.setRow(5, 14); 
          mx.setRow(6, 30);
          mx.setRow(7, 0);  
          mx.update(); 
          break;
        case 5:  
          mx.setRow(0, 0); 
          mx.setRow(1, 32); 
          mx.setRow(2, 112); 
          mx.setRow(3, 56); 
          mx.setRow(4, 29); 
          mx.setRow(5, 15); 
          mx.setRow(6, 7);
          mx.setRow(7, 15);  
          mx.update(); 
          break;
        }
        break;
    default:
      break;
  }
  delay(1);
}