#include<Wire.h>
#include<SPI.h>
#include<Adafruit_SSD1306.h>
#include<Adafruit_GFX.h>
#define SCREEN_WIDHT 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C
#define LOGO_HEIGHT 90
#define LOGO_WIDTH 90
#define LOGO_HEIGHT1 64
#define LOGO_WIDTH1 128
#define A 15
#define B 14
#define X 16
#define Y 17
#define B1 13
#define B2 6
#define B3 3
#define B4 2

int i = 45;
int sensor[8] = {0,0,0,0,0,0,0,0};
Adafruit_SSD1306 display(SCREEN_WIDHT, SCREEN_HEIGHT, &Wire, OLED_RESET);

void ReadSensor(){
  for(int i = 0; i < 4; i++){
    digitalWrite(A, bool(i & 1));
    digitalWrite(B, bool(i & 2));
    delay(10);
    sensor[i] = analogRead(X);
    sensor[i+4] = analogRead(Y);
  }
  int buffer = sensor[0];
  sensor[0] = sensor[3];
  sensor[3] = sensor[2];
  sensor[2] = sensor[1];
  sensor[1] = buffer;

  buffer = sensor[4];
  sensor[4] = sensor[5];
  sensor[5] = sensor[7];
  sensor[7] = buffer;
}

void menu1(int move = -1){
  if(move == 1){
    (i <= 5) ? i = 45 : i -= 10;
  }else if(move == 0){
    (i >= 45) ? i = 5 : i += 10;
  }
  display.clearDisplay();
  display.setCursor(5,5);
  display.print("Up Left");
  display.setCursor(5,15);
  display.print("Up Right");
  display.setCursor(5,25);
  display.print("Down Left");
  display.setCursor(5,35);
  display.print("Down Right");
  display.setCursor(5,45);
  display.print("Scan Sensor");
  display.setCursor(100,i);
  display.print("<==");
  display.display();
}

void menu2(int sensor_buffer[8]){
  display.clearDisplay();
  display.setCursor(5,6);
  display.print(sensor_buffer[0]);
  display.setCursor(5,20);
  display.print(sensor_buffer[1]);
  display.setCursor(5,34);
  display.print(sensor_buffer[2]);
  display.setCursor(5,48);
  display.print(sensor_buffer[3]);
  display.setCursor(108,6);
  display.print(sensor_buffer[7]);
  display.setCursor(108,20);
  display.print(sensor_buffer[6]);
  display.setCursor(108,34);
  display.print(sensor_buffer[5]);
  display.setCursor(108,48);
  display.print(sensor_buffer[4]);
  display.display();
}

void setup() {
  Serial.begin(9600);
  pinMode(B3, INPUT);
  pinMode(13, OUTPUT);
  Serial.println("checking");
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)){
    Serial.println("enter");
    while(1){
      if(digitalRead(B3)){
        Serial.println("Detecting...");
        while(digitalRead(B3));
        if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)){
          Serial.println("Display not found");
          digitalWrite(13, HIGH);
        }else{
          break;
        }
      }
    }
    Serial.println("out");
  }
  digitalWrite(13,0);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  //SENSOR
  pinMode(X, INPUT);
  pinMode(Y, INPUT);
  pinMode(A,OUTPUT);
  pinMode(B,OUTPUT);
  //BUTTON
  pinMode(B1,INPUT);
  pinMode(B2,INPUT);
  pinMode(B3,INPUT);
  pinMode(B4,INPUT);
  menu1(0);
}

void loop(){
  if(digitalRead(B1)){
    while(digitalRead(B1));
    menu1(1);
  }else if(digitalRead(B2)){
    while(digitalRead(B2));
    menu1(0);
  }else if(digitalRead(B3)){
    while(digitalRead(B3));
    if(i/10 == 4){
      while(1){
        if(digitalRead(B4)){
          while(digitalRead(B4));
          break;
        }
        ReadSensor();
        menu2(sensor);
      }
      menu1();
    }else if(i/10 == 0){
      display.clearDisplay();
      display.display();
      while(1){
        if(digitalRead(B4)){
          while(digitalRead(B4));
          break;
        }
      }
      menu1();
    }else if(i/10 == 1){
      display.clearDisplay();
      display.display();
      while(1){
        if(digitalRead(B4)){
          while(digitalRead(B4));
          break;
        }
      }
      menu1();
    }else if(i/10 == 2){
      display.clearDisplay();
      display.display();
      while(1){
        if(digitalRead(B4)){
          while(digitalRead(B4));
          break;
        }
      }
      menu1();
    }else if(i/10 == 3){
      display.clearDisplay();
      display.display();
      while(1){
        if(digitalRead(B4)){
          while(digitalRead(B4));
          break;
        }
      }
      menu1();
    }
  }else if(digitalRead(B4)){
    while(digitalRead(B4));
    
  }
}