// Original: https://wokwi.com/projects/389082239442601985

// Replaced for loops with a state machine.

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

#define SCREEN_WIDTH 128 // Largeur de l'écran en pixels
#define SCREEN_HEIGHT 64 // Hauteur de l'écran en pixels

// Adresse I2C par défaut de l'écran SSD1306 (0x3C)
#define OLED_ADDR 0x3C

// Création d'une instance de l'écran SSD1306
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_ADDR);

// Définition des broches pour le capteur ultrasonique
const int trigPin = 10;
const int echoPin = 11;

// Variables pour la durée et la distance
long duration;
int distance;

Servo myServo; // Crée un objet servo pour contrôler le servo-moteur

class Timer
{
private:
    uint32_t timestamp;
    uint32_t timespan;

public:
    Timer()
    {
    }

    void Start(uint32_t timespan)
    {
        this->timespan = timespan;
        timestamp = millis();
    }

    bool Expired()
    {
        return (millis() - timestamp) >= timespan;
    }
};

Timer timer;

enum class States : byte
{
    Wait30,
    Wait500
};

States state;
States nextState;
int angle;
int delta;
const int MIN_ANGLE = 15;
const int MAX_ANGLE = 165;
const int DELTA = 1;

void setup()
{
    // Initialisation de la communication série
    Serial.begin(9600);

    // Initialisation de l'écran OLED
    if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR))
    {
        Serial.println(F("Erreur lors de l'initialisation de l'écran OLED"));
        while (true)
            ; // Boucle infinie en cas d'erreur
    }

    display.setTextSize(1);      // text size
    display.setTextColor(WHITE); // text color
    display.setCursor(0, 0);     // position to display
    display.clearDisplay();
    display.print("RADAR:"); // text to display
    display.display();       // show on OLED
    delay(2000);
    display.clearDisplay();

    // Initialisation des broches pour le capteur ultrasonique
    pinMode(trigPin, OUTPUT);
    pinMode(echoPin, INPUT);

    // Attachement du servo-moteur à la broche 12
    myServo.attach(12);
    display.setTextSize(1);      // text size
    display.setTextColor(WHITE); // text color

    // Initialise servo angle.
    angle = MIN_ANGLE;
    myServo.write(angle);
    timer.Start(30);
    state = States::Wait30;
}

void loop()
{
    switch (state)
    {
    case States::Wait30:
        if (timer.Expired())
        {
            distance = calculateDistance(); // Appel d'une fonction pour calculer la distance mesurée par le capteur ultrasonique pour chaque degré

            // Affichage des valeurs dans le moniteur série
            Serial.print(angle);
            Serial.print(",");
            Serial.print(distance);
            Serial.println(".");

            display.setCursor(0, 0); // position to display
            display.print("Angle:"); // text to display
            display.println(angle);

            display.print("Dist:"); // text to display
            display.print(distance);
            display.display(); // show on OLED

            timer.Start(500);
            nextState = States::Wait500;
        }
        break;

    case States::Wait500:
        if (timer.Expired())
        {
            display.clearDisplay();

            if (angle >= MAX_ANGLE)
            {
                angle = MAX_ANGLE;
                delta = -DELTA;
            }
            else if (angle <= MIN_ANGLE)
            {
                angle = MIN_ANGLE;
                delta = +DELTA;
            }
            angle += delta;
            myServo.write(angle);

            timer.Start(30);
            nextState = States::Wait30;
        }
        break;
    }

    state = nextState;
}

// Fonction pour calculer la distance mesurée par le capteur ultrasonique
int calculateDistance()
{
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    duration = pulseIn(echoPin, HIGH); // Lecture de l'écho, renvoie le temps de trajet de l'onde sonore en microsecondes
    distance = duration * 0.034 / 2;   // Calcul de la distance en centimètres
    return distance;
}