#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

#define TFT_CS 10
#define TFT_RST 9
#define TFT_DC 8

#define trig 7
#define echo 6

int r = 1010;
int g = 100;
int b = 10111;

int tr = 100;
int tg = 1011;
int tb = 11;


Adafruit_ILI9341 TFT = Adafruit_ILI9341 (TFT_CS, TFT_DC, TFT_RST);




void setup() {
  TFT.begin();
  TFT.setRotation(0);
  TFT.fillScreen(ILI9341_BLACK);


  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);



}

void loop() {
  long razdalja_senzorja = get_dis();

  drawSquareWithText(razdalja_senzorja);
  delay(500);

}

long get_dis() {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long duration = pulseIn(echo,HIGH);
  long razdalja = (duration * 0.0343) / 2; // pretvorba v centimetre
  return constrain(razdalja, 2, 200); // omejimo vrednosti od 2 do 40 cm
}



void drawSquareWithText(long prikazana_razdalja) {
  long raz = prikazana_razdalja;
  int x = 40; // kordinata kota zgornjega levega kota 
  int y = 20; // kordinata kota zgornjega levega kota 
  int xl = 160; // dolžina stranice kvadrata
  int yl = 140;
  if (raz != prikazana_razdalja) {
  TFT.fillRect(0,0,200,y+160,ILI9341_BLACK);
  raz = prikazana_razdalja;
  }
  uint16_t barva = TFT.color565(r,g,b);
  TFT.fillRect(x,prikazana_razdalja,xl,yl, barva); // narišemo modri kvadrat
  // TFT.fillRect(x,y,xl,yl, ILI9341_BLUE); // narišemo modri kvadrat
  uint16_t tbarva = TFT.color565(tr,tg,tb);
  TFT.setTextColor(tbarva);
  TFT.setCursor(x+26,prikazana_razdalja+85);
  TFT.setTextSize(2);
  TFT.print("ovira");

  TFT.fillRect(x,y+160,xl,yl, ILI9341_RED); // narišemo modri kvadrat
  TFT.setCursor(x+43,y+210);
  TFT.setTextColor(ILI9341_WHITE); // nastavi barvo pisave
  TFT.setTextSize(3); // nastavi velikost pisave
  TFT.print("AVTO");
}