#include "displayAndSend.h"
#include "ssd1306.h"
#include "ssd1306_driver.h"
#include "esp_log.h"
#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"

#ifdef ENABLE_SSD1306

#define QUEUE_SIZE 3

QueueHandle_t dataQueue = NULL;

void displayAndSendTask(void *pvParameters) {

    DisplayData data;
    char txt[50];

    ESP_LOGI("DISPLAY", "displayAndSendTask starting");

    i2c_master_init();
    oled_ssd1306_init();

    vTaskDelay(100 / portTICK_PERIOD_MS);
    SSD1306_SetPosition(4,18);
    SSD1306_DrawVertLine(29,0,128);
    SSD1306_Puts("Home Energy Meter", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_SetPosition(4,31);
    SSD1306_Puts("v1.0", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_UpdateScreen();
    vTaskDelay(3000 / portTICK_PERIOD_MS);

    SSD1306_Clear();
    SSD1306_SetPosition(0,0);
    SSD1306_Puts("Temp A:", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_SetPosition(0,11);
    SSD1306_Puts("Temp B:", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_SetPosition(0,22);
    SSD1306_Puts("ADC Vpp:", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_SetPosition(0,33);
    SSD1306_Puts("Pulses:", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_UpdateScreen();

    while( 1 ) {

        if( xQueueReceive(dataQueue,&data,portMAX_DELAY) == pdTRUE ) {

            switch( data.type ) {
                case VPP: sprintf(txt,"%d",data.iData);
                          SSD1306_SetPosition(127-(strlen(txt)*8),22);
                          SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
                          SSD1306_UpdateScreen();
                          break;
                case TempA:
                          sprintf(txt,"%.2f",data.dData);
                          SSD1306_SetPosition(127-(strlen(txt)*8),0);
                          SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
                          SSD1306_UpdateScreen();
                          break;
                case TempB:
                          sprintf(txt,"%.2f",data.dData);
                          SSD1306_SetPosition(127-(strlen(txt)*8),11);
                          SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
                          SSD1306_UpdateScreen();
                          break;
                case Pulses:
                          break;
            }
        }
    }


    vTaskDelete(NULL);
}

void addDataToQueue(int data_type, double dData_in, int iData_in) {

    DisplayData d;

    d.type = data_type;
    d.dData = dData_in;
    d.iData = iData_in;

    if( dataQueue != NULL  ) {
        if( xQueueSend(dataQueue, &d, 0) != pdPASS ) {
            ESP_LOGE("SERIAL","Queue full");
        }
    }
    else {
        ESP_LOGE("SERIAL","Queue not ready for send");
    }
}


void initDisplayAndSend() {

    dataQueue = xQueueCreate( QUEUE_SIZE, sizeof( DisplayData ) );
    if(dataQueue == NULL) {
        ESP_LOGE("SERIAL","Error creating the queue");
    }

    xTaskCreate(displayAndSendTask, "MQTT-Task", 1024*10, NULL, 2, NULL);
}

#endif