#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <SPI.h>
#include <ESP32Servo.h>
#define PI 3.14159265359

int cs = 5;
int dc = 2;
int trig = 16;
int echo = 17;
unsigned long duration;
int distance; 
int distancee;
Servo servo;
Adafruit_ILI9341 tft = Adafruit_ILI9341(cs, dc);

void radar(int a,int b) {
  float radians = a * (PI / 180);
  float dox = cos(radians) * 159;
  float doy = sin(radians) * 159;
  float xanhx = cos(radians) * b;
  float xanhy = sin(radians) * b;
  tft.drawLine(240,160,240 + dox,doy + 160,ILI9341_RED);
  tft.drawLine(240,160,240 + xanhx,160 + xanhy,ILI9341_GREEN);
}

void khung() {
  tft.fillScreen(ILI9341_BLACK);
  tft.drawCircle(240, 160, 159, ILI9341_GREEN);
  tft.drawCircle(240, 160, 109, ILI9341_GREEN);
  tft.drawCircle(240, 160, 59, ILI9341_GREEN);
  tft.drawLine(240, 0, 240, 320, ILI9341_GREEN);
  tft.drawLine(81, 160, 240, 160, ILI9341_GREEN);
}

void setup() {
  Serial.begin(9600);
  tft.begin();
  tft.setSPISpeed(40000000);
  servo.attach(25);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  Serial.println("OK");
}

void loop() {
  khung();
  for (int i = 90; i <= 270; i++) {
    servo.write(i-90);
	  digitalWrite(trig, LOW);  
	  delayMicroseconds(2);  
	  digitalWrite(trig, HIGH);  
  	delayMicroseconds(5);  
  	digitalWrite(trig, LOW);
    duration = pulseIn(echo, HIGH);
    distance =  int(duration/2/29.412);
    if (distance<101){
    distancee = map(distance,0,100,0,159);
    radar(i,distancee);
  }
    if (distance>100){
      radar(i,159);
      
    }
    Serial.println(distance);
  }
  khung();
  for (int i = 270; i >=90; i--) {
        servo.write(i-90);
	  digitalWrite(trig, LOW);  
	  delayMicroseconds(2);  
	  digitalWrite(trig, HIGH);  
  	delayMicroseconds(5);  
  	digitalWrite(trig, LOW);
    duration = pulseIn(echo,HIGH);  
    distance =  int(duration/2/29.412);  
    if (distance<101){
      distancee = map(distance,0,100,0,159);
    radar(i,distancee);
    
  }
    if (distance>100){
      radar(i,159);
    }
  }

  
}