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

#define screen_wide 128
#define screen_height 64
#define SSD1306_I2C_ADDRESS 0x3c

Adafruit_SSD1306 moj_zaslon(screen_wide, screen_height, &Wire);

const int snum = 2;
Servo servo[snum];

const int trig = 7;
const int echo = 6;

#define vert A1
#define hor A0
#define gumb 0

int pozh = 1200;
int pozv = 1200;

long duration;
int distance;

bool state = false;

void setup() {
  moj_zaslon.begin(SSD1306_SWITCHCAPVCC, SSD1306_I2C_ADDRESS); 
  for (int i = 0; i < snum; i++) {
    servo[i].attach(i + 10);
  }
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(gumb, INPUT_PULLUP);
}

void loop() {
  int vertv = analogRead(hor);
  int horv = analogRead(vert);
  

  if (horv < 100 && pozh < 2400) {
    pozh = pozh - 1;
    servo[0].writeMicroseconds(pozh);
  }
  else if (horv > 900 && pozh > 0) {
    pozh = pozh + 1;
    servo[0].writeMicroseconds(pozh);
  }


  if (vertv < 100 && pozv < 2400) {
    pozv = pozv + 1;
    servo[1].writeMicroseconds(pozv);
  }
  else if (vertv > 900 && pozv > 0) {
    pozv = pozv - 1;
    servo[1].writeMicroseconds(pozv);
  }


  if (digitalRead(gumb) == LOW) {
    state = true;
  }


  if (state == true) {
    displayDistance();
    state = false;
  }

  delay(5);
}


void displayDistance() {

  digitalWrite(trig, LOW);
  delay(20);
  digitalWrite(trig, HIGH);
  delay(100);
  digitalWrite(trig, LOW);
  
  duration = pulseIn(echo, HIGH);
  distance = (duration * 0.0343) / 2;
  

  moj_zaslon.clearDisplay();
  moj_zaslon.setTextSize(1);
  moj_zaslon.setTextColor(SSD1306_WHITE);
  moj_zaslon.setCursor(10, 10);
  moj_zaslon.print("Distance: ");
  moj_zaslon.print(distance);
  moj_zaslon.display();
  
  delay(2000);
}