#include <Adafruit_SSD1306.h>
#include <splash.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <dht11.h>
#include <Arduino_FreeRTOS.h>
#include <queue.h>
#define DHTTYPE DHT11
#define DHTPIN 2
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

TaskHandle_t task1Handler;
TaskHandle_t task2Handler;
QueueHandle_t xQueue;
dht11 DHT11;

void task1(void *parameter) {
  Serial.println("Task 1 started");
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C, true, true);
  for (;;) {
    int lReceivedValue = -100;
    if (xQueueReceive( xQueue, &lReceivedValue, portMAX_DELAY ) == pdPASS) {
      Serial.print( "Received = ");
      Serial.println(lReceivedValue);
      display.clearDisplay();
      display.setTextSize(2);
      display.setTextColor(SSD1306_WHITE);
      display.setCursor(0, 0);
      display.println(F("Hello, world!"));
      display.setTextSize(2);
      display.setTextColor(WHITE);
      display.setCursor(10, 8);
      display.println(lReceivedValue);
      display.display();
    } else {
      Serial.println("Queue empty");
    }
    vTaskDelay(25 / portTICK_PERIOD_MS);
    taskYIELD();
  }
}

void task2(void *parameter) {
  Serial.println("Task 2 started");
  int counter = 0;
  int chk = DHT11.read(DHTPIN);
  counter = DHT11.temperature;

  for (;;) {
    Serial.println("READING TEMP");
    DHT11.read(DHTPIN);
    counter = DHT11.temperature;
    Serial.print("Task 2 sending: ");
    Serial.println(counter);
    vTaskDelay(100 / portTICK_PERIOD_MS);
    xQueueSend( xQueue, &counter, portMAX_DELAY == pdPASS);
  }
}

void setup() {
  Serial.begin(9600);

  xQueue = xQueueCreate( 5, sizeof( int ) );
  if ( xQueue != NULL )
  { Serial.println("ALL GOOD");
    Serial.println(xQueue != NULL);
    xTaskCreate(
      task2,
      "SENDING QUEUE TASK",
      128,
      NULL,
      1,
      &task2Handler);
    xTaskCreate(
      task1,
      "READING QUEUE TASK",
      128,
      NULL,
      1,
      &task1Handler);
    vTaskStartScheduler();
  } else {
    Serial.print("QUEUE CANNOT BE CREATED :");
    Serial.println(xQueue != NULL);
  }
}

void loop()
{
  // put your main code here, to run repeatedly:
}