#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>


#define echoPin 4 // attach pin D2 Arduino to pin Echo of HC-SR04
#define trigPin 2 //attach pin D3 Arduino to pin Trig of HC-SR04
#define up_button 14
#define down_button 27
#define laser 13
#define EEPROM_SIZE 1

// defines variables
long duration; // variable for the duration of sound wave travel
int distance; // variable for the distance measurement
int sel_dist [] = {2, 3, 4, 6, 8, 12, 16, 18, 20, 24, 30};


int dist_array_selected = 0;

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// 'DnD logo', 128x64px
const unsigned char DnD_logo [] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xe0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xfe, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf8, 0x3f, 0xc3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x7c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf0, 0x1f, 0xc1, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x3f, 0xff, 0xff, 0xff, 0x00, 0x00, 0x1f, 0xe0, 0x1f, 0xc1, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x00, 
	0x3f, 0xff, 0xff, 0xff, 0xe0, 0x00, 0x1f, 0xc0, 0x0f, 0xe0, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 
	0x1f, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x1b, 0xc0, 0x0f, 0xe0, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x00, 
	0x0f, 0xff, 0xff, 0xff, 0xff, 0x00, 0x1b, 0xc0, 0x0f, 0xe0, 0x7f, 0xff, 0xff, 0xff, 0xf8, 0x00, 
	0x07, 0xff, 0xff, 0xff, 0xff, 0x80, 0x11, 0xc0, 0x0f, 0xe0, 0x3f, 0xff, 0xff, 0xff, 0xfe, 0x00, 
	0x03, 0xff, 0xff, 0xff, 0xff, 0xe0, 0x11, 0x88, 0x0f, 0xe0, 0x1f, 0xff, 0xff, 0xff, 0xff, 0x00, 
	0x01, 0xff, 0xff, 0xff, 0xff, 0xf0, 0x10, 0x88, 0x1f, 0xc0, 0x0f, 0xff, 0xff, 0xff, 0xff, 0x80, 
	0x00, 0xff, 0xff, 0xff, 0xff, 0xf8, 0x00, 0x88, 0x9f, 0xc0, 0x07, 0xff, 0xff, 0xff, 0xff, 0xc0, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0xfc, 0x00, 0x0c, 0x8f, 0xc0, 0x03, 0xff, 0xff, 0xff, 0xff, 0xe0, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0xfc, 0x02, 0x0e, 0x47, 0x80, 0x03, 0xff, 0xff, 0xff, 0xff, 0xf0, 
	0x00, 0x7f, 0xf8, 0x0f, 0xff, 0xfe, 0x03, 0x8f, 0x23, 0x80, 0x03, 0xff, 0xe0, 0x3f, 0xff, 0xf0, 
	0x00, 0x7f, 0xf8, 0x00, 0xff, 0xfe, 0x1b, 0xc7, 0x91, 0x00, 0x03, 0xb0, 0x00, 0x07, 0xff, 0xf8, 
	0x00, 0x7f, 0xf8, 0x00, 0x3f, 0xff, 0x0f, 0xf3, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xf8, 
	0x00, 0x7f, 0xf8, 0x00, 0x1f, 0xff, 0x0f, 0xf9, 0xe4, 0x00, 0x00, 0x00, 0x00, 0x00, 0xff, 0xfc, 
	0x00, 0x7f, 0xf8, 0x00, 0x0f, 0xff, 0x8f, 0xff, 0xf2, 0x00, 0x00, 0x20, 0x00, 0x00, 0x7f, 0xfc, 
	0x00, 0x7f, 0xf8, 0x00, 0x0f, 0xfe, 0x0f, 0xff, 0xff, 0x00, 0x00, 0x67, 0xf0, 0x00, 0x7f, 0xfc, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xf8, 0x3f, 0xf3, 0xef, 0x00, 0x00, 0xff, 0xfc, 0x00, 0x3f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xf0, 0xff, 0x87, 0xe7, 0x80, 0x1f, 0x0f, 0xfe, 0x00, 0x3f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xe1, 0xfe, 0x1f, 0xf3, 0x80, 0x3c, 0x7f, 0xf0, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xc0, 0x1e, 0x0f, 0xfb, 0x80, 0x31, 0xc1, 0x80, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0x80, 0x1e, 0x0f, 0x07, 0x80, 0x47, 0x00, 0x00, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0x88, 0x18, 0x1e, 0x03, 0xc0, 0x0e, 0x18, 0x00, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xf8, 0x90, 0x2f, 0x83, 0xf0, 0x3c, 0x7f, 0xe0, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xf1, 0x80, 0x03, 0x00, 0xf0, 0x78, 0xff, 0xe0, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xe3, 0x06, 0x01, 0x00, 0x30, 0xf1, 0xff, 0xe0, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xe7, 0x0c, 0x61, 0x8f, 0x21, 0xf3, 0xff, 0xe0, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xc7, 0x1c, 0xc1, 0x87, 0x03, 0xe3, 0xff, 0xe0, 0x00, 0x1f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xcf, 0x3f, 0xc1, 0x83, 0x03, 0xc3, 0xff, 0xe0, 0x00, 0x3f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x07, 0xcf, 0x7f, 0x80, 0xc3, 0x81, 0xc3, 0xff, 0xe0, 0x00, 0x3f, 0xfe, 
	0x00, 0x7f, 0xf8, 0x00, 0x0f, 0xc7, 0xff, 0x80, 0x81, 0xf8, 0x02, 0x7f, 0xe0, 0x00, 0x7f, 0xfc, 
	0x00, 0x7f, 0xf8, 0x00, 0x0f, 0x87, 0xff, 0x00, 0x01, 0xfc, 0x02, 0x1f, 0xe0, 0x00, 0x7f, 0xfc, 
	0x00, 0x7f, 0xf8, 0x00, 0x1f, 0x87, 0xff, 0x00, 0x01, 0xfe, 0x03, 0x00, 0x00, 0x00, 0xff, 0xfc, 
	0x00, 0x7f, 0xf8, 0x00, 0x3f, 0x87, 0xff, 0x00, 0x00, 0xff, 0x83, 0x00, 0x00, 0x01, 0xff, 0xf8, 
	0x00, 0x7f, 0xf8, 0x00, 0xff, 0x87, 0xff, 0x00, 0x00, 0x7f, 0xc3, 0x00, 0x00, 0x07, 0xff, 0xf8, 
	0x00, 0x7f, 0xf8, 0x0f, 0xff, 0x8f, 0xff, 0x00, 0x00, 0x3f, 0xe0, 0x20, 0x60, 0x3f, 0xff, 0xf0, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0x8f, 0xff, 0x00, 0x00, 0x0f, 0xf0, 0x32, 0x30, 0xff, 0xff, 0xe0, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0xcf, 0xff, 0x00, 0x00, 0x87, 0xfc, 0x72, 0x3c, 0xff, 0xff, 0xe0, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0xcf, 0xff, 0x00, 0x01, 0xc3, 0xff, 0xf2, 0x3c, 0x7f, 0xff, 0xc0, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0xcf, 0xff, 0x80, 0x03, 0xf1, 0xff, 0xf0, 0x3e, 0x7f, 0xff, 0x80, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0xc7, 0xff, 0x80, 0x07, 0xf1, 0xff, 0xf0, 0x7e, 0x7f, 0xff, 0x00, 
	0x00, 0x7f, 0xff, 0xff, 0xff, 0x87, 0xff, 0xc0, 0x0f, 0xf0, 0xff, 0xe0, 0xfe, 0x7f, 0xfc, 0x00, 
	0x00, 0xff, 0xff, 0xff, 0xff, 0x07, 0xff, 0xf0, 0x3f, 0xf0, 0xff, 0xff, 0xfe, 0x7f, 0xf8, 0x00, 
	0x01, 0xff, 0xff, 0xff, 0xfc, 0x07, 0xff, 0xfd, 0xff, 0xf0, 0xff, 0xff, 0xfe, 0x7f, 0xe0, 0x00, 
	0x07, 0xff, 0xff, 0xff, 0xe0, 0x03, 0xff, 0xff, 0xff, 0xe0, 0xff, 0xff, 0xfc, 0x7f, 0x00, 0x00, 
	0x1f, 0xff, 0xff, 0xff, 0x00, 0x03, 0xff, 0xff, 0xff, 0xc0, 0xff, 0xff, 0xfc, 0xf8, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x3f, 0xff, 0xff, 0x80, 0xe7, 0xff, 0xf8, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1f, 0xff, 0xff, 0x00, 0xe7, 0xff, 0xe0, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0xfe, 0x00, 0x67, 0xfc, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0xff, 0xfc, 0x00, 0x63, 0xfc, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xf0, 0x00, 0x23, 0xff, 0x80, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0x00, 0x00, 0x00, 0xff, 0x80, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3e, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

// Array of all bitmaps for convenience. (Total bytes used to store images in PROGMEM = 1040)
const int epd_bitmap_allArray_LEN = 1;
const unsigned char* epd_bitmap_allArray[1] = {
	DnD_logo
};


void setup() {
  // esp_bluedroid_disable();
  // esp_bt_controller_disable();
  // esp_wifi_stop();
  
  pinMode(trigPin, OUTPUT); // Sets the trigPin as an OUTPUT
  pinMode(echoPin, INPUT); // Sets the echoPin as an INPUT
  Serial.begin(9600); // // Serial Communication is starting with 9600 of baudrate speed

  pinMode(up_button, INPUT_PULLUP);
  pinMode(down_button, INPUT_PULLUP);
  pinMode(laser, OUTPUT);
  
  Wire.begin();
  EEPROM.begin(EEPROM_SIZE);
  dist_array_selected = EEPROM.read(0);
  if (dist_array_selected == 255) {
    dist_array_selected = 0;
  }
  display.begin(SSD1306_SWITCHCAPVCC,0x3C);
  display.clearDisplay();

  display.drawBitmap(0, 0, DnD_logo, 128, 64, WHITE);
  display.display();
  delay(1000);
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
  display.setCursor(0,38);
  display.println("Radius");
  display.setTextSize(2);
  display.setCursor(0,20);
  display.print(sel_dist[dist_array_selected]*5/2);
  display.println("\'");
  display.setTextSize(1);
  display.setCursor(80,38);
  display.println("Cone");
  display.setTextSize(2);
  display.setCursor(80,20);
  display.print(sel_dist[dist_array_selected]*5);
  display.println("\'");
  display.display();

}

void loop() {
  // Clears the trigPin condition
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin HIGH (ACTIVE) for 10 microseconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2; // Speed of sound wave divided by 2 (go and back)
  //distance = distance / 2.54;

  display.setTextSize(2);

  if (digitalRead(up_button) == LOW) {
    if (dist_array_selected == 10) {
      dist_array_selected = 0;
    } else {
      dist_array_selected = dist_array_selected + 1;
    }
    EEPROM.write(0,dist_array_selected);
    display.setCursor(0,20);
    display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
    if (dist_array_selected != 1) {
      display.print(sel_dist[dist_array_selected]*5/2);
      display.print("\' ");
    } else {
      display.print("7.5\'");
    }
    display.setCursor(80,20);
    if (dist_array_selected >= 2 && dist_array_selected <=10) {
      display.print(sel_dist[dist_array_selected]*5);
      display.print("\'");
    } else {
      display.print(sel_dist[dist_array_selected]*5);
      display.print("\' ");
    }
    display.display();
  }

    if (digitalRead(down_button) == LOW) {
      if (dist_array_selected == 0) {
        dist_array_selected = 10;
      } else {
        dist_array_selected = dist_array_selected - 1;
      }
      EEPROM.write(0,dist_array_selected);
      display.setCursor(0,20);
      display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
      if (dist_array_selected == 0) {
        display.print(sel_dist[dist_array_selected]*5/2);
        display.print("\'  ");
      } else if (dist_array_selected == 1) {
          display.print("7.5\'");
      } else {
          display.print(sel_dist[dist_array_selected]*5/2);
          display.print("\'  ");
      }

      display.setCursor(80,20);
      display.print(sel_dist[dist_array_selected]*5);
      if (dist_array_selected == 0) {
        //display.setCursor(11,48);
        display.print("\' ");
      }
      if (dist_array_selected >= 8 && dist_array_selected <= 10) {
        //display.setCursor(22,48);
        display.print("\'");
      }
      if (dist_array_selected >= 2 && dist_array_selected <= 7) {
        //display.setCursor(22,48);
        display.print("\' ");
      }
      display.display();
  }

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
  display.setCursor(8,57);
  display.print("Actual Distance:");
  display.print(distance*5);
  display.print("\'  ");
  display.display();

  if (distance >= sel_dist[dist_array_selected]-1 && distance <= sel_dist[dist_array_selected]+1) {
    digitalWrite(laser, HIGH);
  } else {
    digitalWrite(laser, LOW);
  }

}