#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h> 
#include <LiquidCrystal_I2C.h>

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

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

// Defines Trig and Echo pins of the Ultrasonic Sensor
const int trigPin = 10;
const int echoPin = 11;
const int servoPin = 12;

// Variables for the duration and the distance
long duration;
int distance, angle;

Servo servo; // Creates a servo object for controlling the servo motor
LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  Serial.begin(9600);
  lcd.init();
  lcd.backlight();

  lcd.setCursor(0,0);
  lcd.print("Pemetaan 2D");
  lcd.setCursor(1,0);
  lcd.println("Berbasis HCSR04");

  Serial.println("Pemetaan 2D berbasis VL53L0X");

  pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
  pinMode(echoPin, INPUT); // Sets the echoPin as an Input
  servo.attach(servoPin); // Defines on which pin is the servo motor attached
  
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();
  delay(2000);
}

void loop() {
  for(int i=15; i<165;i++){
    angle = i;
    distance = calculateDistance();
    servo.write(angle);
    draw(distance,angle);
    lcdi2c(distance,angle);
    delay(100);
  }
  for(int i=165; i>15;i--){
    angle = i;
    distance = calculateDistance();
    servo.write(angle);
    draw(distance,angle);
    lcdi2c(distance,angle);
    delay(100);
  }
}

// Function for calculating the distance measured by the Ultrasonic sensor
int calculateDistance(){ 
  digitalWrite(trigPin, LOW); 
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);  // Sets the trigPin on HIGH state for 10 micro seconds
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH); // Reads the echoPin, returns the sound wave travel time in microseconds
  distance = duration*0.034/2;
  return distance;
}

void draw (int distance , int angle){
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SCREEN_COLOR);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.print(angle);display.print(",");display.println(distance);
  drawLine(distance,angle);
  display.display();
}

void drawLine(int distance, int angle){
  int x0,y0,x1,y1;
  x0 = (SCREEN_WIDTH/8)*4;
  y0 = (SCREEN_HEIGHT/8)*7;

  x1 = x0/2 - (SCREEN_WIDTH/8)*2*cos(radians(angle));
  y1 = y0/2 - (SCREEN_HEIGHT/8)*2*sin(radians(angle));

  display.drawLine(x0, y0, x1, y1, SCREEN_COLOR);

  Serial.print("Angle=");
  Serial.print(angle);
  Serial.print("\tDistance=");
  Serial.print(distance);
  Serial.print("\t(x0,y0)=(");
  Serial.print(x0);
  Serial.print(",");
  Serial.print(y0);
  Serial.print(") \t(x1,y1)=(");
  Serial.print(x1);
  Serial.print(",");
  Serial.print(y1);
  Serial.println(")");
}

void lcdi2c(int distance, int angle){
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Pos ");lcd.print(angle);lcd.print(" , ");
    lcd.print(distance);lcd.print(" cm");
   
    lcd.setCursor(0,1);
    lcd.print(distance);
}