#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

static QueueHandle_t dataQueue = NULL;
static bool queueReady = false;

void displayAndSendTask(void *pvParameters) {

    uint8_t aliveCnt = 0;
    DisplayData data;
    char txt[50];

    ESP_LOGI("DISPnSND", "displayAndSendTask starting. Core:%d",xPortGetCoreID());

    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("kWh:", &Font_7x10,SSD1306_COLOR_WHITE);
    SSD1306_UpdateScreen();

    // Create the data input queue, now that all is setup
    dataQueue = xQueueCreate( QUEUE_SIZE, sizeof( DisplayData ) );
    if(dataQueue == NULL) {
        ESP_LOGE("DISPnSND","Error creating the queue");
    }
    ESP_LOGI("DISPnSND", "Data input queue ready.");
    queueReady = true;

    while( 1 ) {

        if( xQueueReceive(dataQueue,&data, 1000 / portTICK_PERIOD_MS) == pdTRUE ) {

            switch( data.type ) {
                case type_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 type_TempA:
                          sprintf(txt,"%.2f\044C",data.dData);
                          SSD1306_SetPosition(127-(strlen(txt)*8),0);
                          SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
                          SSD1306_UpdateScreen();
                          break;
                case type_TempB:
                          sprintf(txt,"%.2f\044C",data.dData);
                          SSD1306_SetPosition(127-(strlen(txt)*8),11);
                          SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
                          SSD1306_UpdateScreen();
                          break;
                case type_kWh:
                          sprintf(txt,"%.2f",data.dData);
                          SSD1306_SetPosition(127-(strlen(txt)*8),33);
                          SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
                          SSD1306_UpdateScreen();
                          break;
                default:
                          break;
            }
        }
        else {
            sprintf(txt,"%u",aliveCnt);
            if( ++aliveCnt == 10 ) aliveCnt=0;
            SSD1306_SetPosition(127-(strlen(txt)*8),50);
            SSD1306_Puts(txt, &Font_7x10,SSD1306_COLOR_WHITE);
            SSD1306_UpdateScreen();
        }
    }

    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( queueReady == true && dataQueue != NULL  ) {
        if( xQueueSend(dataQueue, &d, 0) != pdPASS ) {
            ESP_LOGE("DISPnSND","Queue full");
        }
    }
    else {
        ESP_LOGE("DISPnSND","Queue not ready.");
    }
}


void initDisplayAndSend() {

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

#endif