#include <Wire.h>

const int trigPin = 12;
const int echoPin = 11;

volatile int pozivanje1;
volatile int pozivanje2;
volatile int pozivanje3;
long duration;
int distance;

void setup() {

  Wire.begin();

  pinMode(trigPin, OUTPUT); // Trigger Pin OUTPUT
  pinMode(echoPin, INPUT); // Echo Pin INPUT
  pinMode(4, INPUT);
  pinMode(2, INPUT);
  pinMode(3, INPUT);
  pinMode(13, OUTPUT);





Serial.begin(9600);
}

void loop() {

  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;
  Serial.print("Distance: ");
  Serial.println(distance);


  /* PRVI KAT */

  if (digitalRead(2) == 1) {
    pozivanje1=1;
  }

  if(pozivanje1==1){


    if (distance > 10) {
      Wire.beginTransmission(4);
      Wire.write(2);
      Wire.endTransmission();
    }


    if (distance < 10) {
      Wire.beginTransmission(4);
      Wire.write(1);
      Wire.endTransmission();
    }
                          }

  if (distance >= 10 && distance <= 10 && pozivanje1==1) { // Kada dode na 1. kat, stopiraj motor i update-aj display

    Wire.beginTransmission(4);
    Wire.write(0);
    Wire.endTransmission();

    Wire.beginTransmission(6);  // pokreni prijenos sa uredajem 2
    Wire.write(1);              // posalji 1
    Wire.endTransmission();     // prekini prijenos
    pozivanje1=0;


  }


  /* DRUGI */

  if (digitalRead(3) == 1) {
    pozivanje2=1;
  }

  if(pozivanje2==1){


    if (distance > 27) {
      Wire.beginTransmission(4);
      Wire.write(2);
      Wire.endTransmission();
    }


    if (distance < 27) {
      Wire.beginTransmission(4);
      Wire.write(1);
      Wire.endTransmission();
    }
                           }

  if (distance >= 27 && distance <= 27 && pozivanje2==1) { // Kada dode na 1. kat, stopiraj motor i update-aj display

    Wire.beginTransmission(4);
    Wire.write(0);
    Wire.endTransmission();

    Wire.beginTransmission(6);  // pokreni prijenos sa uredajem 2
    Wire.write(2);              // posalji 1
    Wire.endTransmission();     // prekini prijenos
    pozivanje2=0;


  }


/* TRECI KAT */

  if (digitalRead(4) == 1) {
    pozivanje3=1;
  }

  if(pozivanje3==1){


    if (distance > 43) {
      Wire.beginTransmission(4);
      Wire.write(2);
      Wire.endTransmission();
    }


    if (distance < 43) {
      Wire.beginTransmission(4);
      Wire.write(1);
      Wire.endTransmission();
    }
                           }

  if (distance >= 43 && distance <= 43 && pozivanje3==1) { // Kada dode na 1. kat, stopiraj motor i update-aj display

    Wire.beginTransmission(4);
    Wire.write(0);
    Wire.endTransmission();

    Wire.beginTransmission(6);  // pokreni prijenos sa uredajem 2
    Wire.write(3);              // posalji 3
    Wire.endTransmission();     // prekini prijenos
    pozivanje3=0;

  }

}