#include <Arduino.h>
#include "SSD1306Wire.h"
#include <stdio.h>
#include <time.h>
#include <stdlib.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/semphr.h"
#include "sdkconfig.h"
#include <SoftwareSerial.h>
#include <pthread.h>

#define ADDRESS 0x3C
#define SDA 5
#define SCL 4
#define RX 4
#define TX 5

#define SPD_START_BYTE 0x01
#define SPD_END_BYTE 0x04
#define SPD_STRING 0x02
#define SPD_INTEGER 0x03
#define SPD_FLOAT 0x05

#define N 20

// Initialize the OLED display using Wire library
SSD1306Wire display(0x3c, SDA, SCL); // ADDRESS, SDA, SCL  -  SDA and SCL usually populate automatically based on your board's pins_arduino.h e.g. https://github.com/esp8266/Arduino/blob/master/variants/nodemcu/pins_arduino.h
// SH1106Wire display(0x3c, SDA, SCL);

const int OLED_CENTER_X = 64;
const int OLED_CENTER_Y = 20;
const int OLED_BOTTOM_Y = 40;

int timeCounter = 0;
String timer = "";

SemaphoreHandle_t producers_semaphore;
SemaphoreHandle_t consumer_semaphore;
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
char* buffer[N];
int p_index = 0;
int c_index = 0;

void vTaskSender(void *vParameters)
{
  for(;;) {
    Serial.println("DHT");
  }
}

void vTaskProducerCO2(void *vParameters)
{
  for (;;)
  {
    xSemaphoreTake(producers_semaphore, portMAX_DELAY);
    pthread_mutex_lock(&mutex);
    if (p_index % 2 == 0)
    {
      if (Serial.available())
      {
        Serial.println("Test");
        byte b = Serial.read();
        if (b == SPD_START_BYTE)
        {
          b = Serial.read();
          if (b == SPD_STRING)
          {
            char buf[3];
            Serial.readBytes(buf, sizeof(buf));
            if (buf == "MHZ")
            {
              b = Serial.read();
              if (b == SPD_INTEGER)
              {
                // Niveau de carbone
                int c;
                Serial.readBytes((byte *)&c, sizeof(int));
                sprintf(buffer[p_index % N], "%d ppm", c);
                p_index++;
              }
            }
          }
          Serial.read();
        }
      }
    }
    pthread_mutex_unlock(&mutex);
    xSemaphoreGive(consumer_semaphore);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void vTaskProducerTemp(void *vParameters)
{
  for (;;)
  {
    xSemaphoreTake(producers_semaphore, portMAX_DELAY);
    pthread_mutex_lock(&mutex);
    if (p_index % 2 == 1)
    {
      if (Serial.available())
      {
        byte b = Serial.read();
        if (b == SPD_START_BYTE)
        {
          b = Serial.read();
          if (b == SPD_STRING)
          {
            char buf[3];
            Serial.readBytes(buf, sizeof(buf));
            if (buf == "DHT")
            {
              // Niveau d'humitidé
              float h = 0.0;
              // Température
              float t = 0.0;
              b = Serial.read();
              if (b == SPD_FLOAT)
              {
                Serial.readBytes((byte *)&h, sizeof(float));
              }
              b = Serial.read();
              if (b == SPD_FLOAT)
              {
                Serial.readBytes((byte *)&t, sizeof(float));
              }
              sprintf(buffer[p_index % N], "%f  %%- %f °C", h, t);
              p_index++;
            }
          }
          Serial.read();
        }
      }
    }
    pthread_mutex_unlock(&mutex);
    xSemaphoreGive(consumer_semaphore);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void vTaskConsumer(void *vParameters)
{
  for (;;)
  {
    xSemaphoreTake(consumer_semaphore, portMAX_DELAY);
    pthread_mutex_lock(&mutex);
    if (c_index < p_index)
    {
      display.clear();
      display.drawStringMaxWidth(OLED_CENTER_X, OLED_CENTER_Y, 128, buffer[c_index % N]);
      display.display();
      c_index++;
    }
    pthread_mutex_unlock(&mutex);
    xSemaphoreGive(producers_semaphore);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
}

void vTaskOledTimer(void *vParameters)
{
  for (;;)
  {
    static uint16_t start_at = 0;
    timeCounter = (timeCounter + 1) % 60;
    display.clear();
    uint16_t firstline = display.drawStringMaxWidth(OLED_CENTER_X, OLED_BOTTOM_Y, 128, String(timeCounter) + "s");
    display.display();
    if (firstline != 0)
    {
      start_at += firstline;
    }
    else
    {
      start_at = 0;
    }
    delay(1000);
  }
}

void setup()
{
  display.init();
  display.flipScreenVertically();
  display.setContrast(255);
  display.setTextAlignment(TEXT_ALIGN_CENTER);
  display.setFont(ArialMT_Plain_10);
  display.display();

  Serial.begin(9600);
  Serial.println("Initialisation de la communication ok!");

  xTaskCreate(vTaskSender, "Sender", 10000, NULL, 1, NULL);

  producers_semaphore = xSemaphoreCreateCounting(2, 2);
  consumer_semaphore = xSemaphoreCreateCounting(1, 0);
  xTaskCreate(vTaskProducerCO2, "ProducerCO2", 10000, NULL, 1, NULL);
  xTaskCreate(vTaskProducerTemp, "ProducerTemp", 10000, NULL, 1, NULL);
  xTaskCreate(vTaskConsumer, "Consumer", 10000, NULL, 1, NULL);

  // xTaskCreate(vTaskOledTimer, "OledTimer", 10000, NULL, 1, NULL);
}

void loop()
{
  vTaskDelay(1000 / portTICK_PERIOD_MS);
}