// MANUALLY ADD PHOTORESISTORS(USED TO CHECK IF NIGHT) LATER
// MANUALLY ADD SERVOS(USED TO MOVE) LATER

// SSD1306: Screen
// WIRING
// VCC is: 5v
// GND is:
// SCL is: A5
// SDA is: A4

// Screen Includes
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include "Animations.hpp"

// Screen Defines
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Other Defines
#define SWITCH 13

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

const int trigPin = 7;
const int echoPin = 8;
bool isOn = false; // Is Rocker Switch On

// Helper Functions
void displayImage(const unsigned char PROGMEM img[]){
  display.clearDisplay();
  display.drawBitmap(32, 0, img, 64, 64, 1);
  display.display();
}

void displayAnimation(unsigned char PROGMEM *anim[], int len, int speed=150) {
  for (int i = 0; i < len; i++) {
    displayImage(anim[i]);
    delay(speed);
  }
}

void setup() {
  Serial.begin(115200);
 
  // // Setup for LED
  // if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
  //   Serial.println(F("SSD1306 allocation failed"));
  //   for(;;);
  // }
  // delay(2000); // Pause for 2 seconds
 
  // // Clear the buffer.
  // display.clearDisplay();

  // // Other Setup
  // pinMode(SWITCH, INPUT);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
}
 
void loop() {
  // isOn = digitalRead(SWITCH);
  // if (isOn){
  //   displayAnimation(BLINK, 5);
  // }
  // else{
  //   display.clearDisplay();
  //   display.display();
  // }
  // establish variables for duration of the ping, and the distance result
  // in inches and centimeters:
  long duration, inches, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH pulse
  // whose duration is the time (in microseconds) from the sending of the ping
  // to the reception of its echo off of an object.
  duration = pulseIn(echoPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);

  Serial.print(inches);
  Serial.print("in, ");
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();

  delay(100);
}

long microsecondsToCentimeters(long microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the object we
  // take half of the distance travelled.
  return microseconds / 29 / 2;
}