#include <pthread.h>
#include "DHT.h";
#include "NewPing.h"

#define SONARS 3
#define DHTPIN 4
#define DHTTYPE DHT22
#define TRIGGER_PIN 5
#define TRIGGER_PIN1 17
#define TRIGGER_PIN2 14
#define ECHO_PIN 16
#define ECHO_PIN1 18
#define ECHO_PIN2 13
#define MAX_DISTANCE 150
#define MIN_DISTANCE 15
#define MAX_DUTY 512
#define PWM_FREQUENCY 2.5e3
#define ITERATIONS 5

NewPing sonars[SONARS] = {
    NewPing(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE),
    NewPing(TRIGGER_PIN1, ECHO_PIN1, MAX_DISTANCE),
    NewPing(TRIGGER_PIN2, ECHO_PIN2, MAX_DISTANCE)};
DHT dht(DHTPIN, DHTTYPE);

const int vibroPins[SONARS] = {27, 32, 33};
float hum, temp,
    distance[SONARS],
    soundsp, soundcm;

void serialPrint(void);

void *measureThread(void *)
{
    while (true)
    {
        hum = dht.readHumidity();
        temp = dht.readTemperature();

        soundsp = 331.4 + (0.606 * temp) + (0.0124 * hum);
        soundcm = soundsp / 10e3;
        delay(1000); // Temperature and humidity update delay
        serialPrint();
    }
}

void *sonarThread(void *sonarId)
{
    int id = (int)sonarId;
    float dutyCycle;
    while (true)
    {
        delay(1e3); // Sonars check delay
        distance[id] = (sonars[id].ping_median(ITERATIONS) / 2.0) * soundcm;
        dutyCycle = 0;
        if (distance[id] <= MAX_DISTANCE && distance[id] >= MIN_DISTANCE)
        {
            dutyCycle = (int)((distance[id] - 150) * (MAX_DUTY - 50) / (-150) + 50);
        }
        else if (distance[id] <= MIN_DISTANCE)
        {
            dutyCycle = MAX_DUTY;
        }
        ledcWrite(id, dutyCycle);
    }
}

void setup()
{
    pthread_t sonars[SONARS], measure;
    Serial.begin(115200);
    dht.begin();
    pthread_create(&measure, NULL, measureThread, nullptr);
    for (int i = 0; i < SONARS; i++)
    {
        ledcSetup(i, PWM_FREQUENCY, 10);
        ledcAttachPin(vibroPins[i], i);
        pthread_create(&sonars[i], NULL, sonarThread, (void *)i);
    }
}

void loop() {}

void serialPrint(void)
{
    char tmp[100];
    sprintf(tmp, "Sound %f m/s, Humid: %f%%, Temp: %f", soundsp, hum, temp);
    Serial.println(tmp);
    sprintf(tmp, "Distance: %f, Distance1: %f, Distance2: %f", distance[0], distance[1], distance[2]);
    Serial.println(tmp);
    Serial.println(" ");
}