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

#define trigPin 8
#define echoPin 9
#define TFT_DC 6
#define TFT_CS 10
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

long duration;
float distance;

Servo myservo;

int calculateDistance()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2.0; // Correction du calcul de la distance avec un type de données flottant
  return (int)distance; // Conversion de float en int pour retourner la distance
}

void setup()
{
  tft.begin();

  tft.setCursor(26, 120);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(3);
  tft.println("Hello, TFT!");

  tft.setCursor(20, 160);
  tft.setTextColor(ILI9341_GREEN);
  tft.setTextSize(2);
  tft.println("I can has colors?");

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  myservo.attach(3);
  myservo.write(90); // Positionner le servo au milieu au démarrage
  Serial.begin(9600);
}

void loop()
{
  int i;
  for (i = 15; i <= 165; i++)
  {
    myservo.write(i);
    delay(15);
    calculateDistance();
    Serial.print(i);
    Serial.print(",");
    Serial.print(distance);
    Serial.print(".");
  }
  for (i = 165; i >= 15; i--)
  {
    myservo.write(i);
    delay(15);
    calculateDistance();
    Serial.print(i);
    Serial.print(",");
    Serial.print(distance);
    Serial.print(".");
  }
}