#include "stdlib.h"

#include "Arduino.h"
#include "nvs_flash.h"
#include <mutex>

#include "mpack.h"
#include "ESP32epdx.h"
#include "EPD.h"
#include "mesh_netif.h"
#include "tcphelper.h"
#include "main.h"

std::mutex epd_mtx;

/*
SPI: SCK = 6 ; MISO = 2 ; MOSI = 7 ; SS = 16
EPD: RES = 22 ; DC = 23 ; CS = 1 ; BUSY = 0
*/

uint8_t *image_buf = nullptr;

typedef struct request_t {
    // command : update_image, push_image, ping
    char *command;
    
    // arguments for update_image
    uint64_t index;
    char *data;
    size_t data_len;
} request_t;


extern "C" request_t* parse_data(char *buf, size_t len) {
    // variables
    char *command = NULL;
    uint64_t index = 0;
    char *data = NULL;
    size_t data_len = 0;

    // init reader
    mpack_reader_t reader;
    mpack_reader_init_data(&reader, buf, len);

    size_t count = mpack_expect_map_max(&reader, 100);
    if (mpack_reader_error(&reader) != mpack_ok) {
        return NULL;
    }

    // read data
    for (size_t i = 0; i < count; i++) {
        char *key = mpack_expect_cstr_alloc(&reader, 100);

        if (strcmp(key, "command") == 0) {
            command = mpack_expect_cstr_alloc(&reader, 32);
        } else if (strcmp(key, "index") == 0) {
            index = mpack_expect_u64(&reader);
        } else if (strcmp(key, "data") == 0) {
            data = mpack_expect_bin_alloc(&reader, CONFIG_IMAGE_BUF_SLICE_SIZE, &data_len);
        } else {
            mpack_discard(&reader);
        }

        free(key);
    }
    
    // finish
    mpack_done_map(&reader);
    if (mpack_reader_destroy(&reader) != mpack_ok) {
        return NULL;
    }

    // return
    request_t *result = (request_t *)malloc(sizeof(request_t));
    if (!result) {
        return NULL;
    }
    memset(result, 0, sizeof(request_t));
    result->command = command;
    result->index = index;
    result->data = data;
    result->data_len = data_len;

    return result;
}

extern "C" int command_update_image(uint64_t index, char *buf, size_t len) {
    if (!epd_mtx.try_lock()) {
        return -3; // locked
    }
    
    if (
        index < 0 || index >= CONFIG_IMAGE_BUF_SIZE ||
        len < 0 || len > CONFIG_IMAGE_BUF_SIZE ||
        (index+len) < 0 || (index+len) > CONFIG_IMAGE_BUF_SIZE
    )
    {
        epd_mtx.unlock();
        return -2;
    }

    memcpy(image_buf + index, buf, len);

    epd_mtx.unlock();
    return 0;
}

extern "C" int command_push_image() {
    if (epd_mtx.try_lock()) {
        EPD_HW_Init_Fast();
        EPD_Display((unsigned char *)(image_buf));
        EPD_DeepSleep();

        epd_mtx.unlock();
        return 0;
    } else {
        return -3;
    }
}

extern "C" int tcp_client_callback(char *buf, size_t len) {
    request_t *req = parse_data(buf, len);
    if (!req) { // error -> invalid data
        return -2;
    }

    // execute command
    if (strcmp(req->command, "ping") == 0)
    {
        return 0;
    }
    else if (strcmp(req->command, "update_image") == 0)
    {
        return command_update_image(req->index, req->data, req->data_len);
    }
    else if (strcmp(req->command, "push_image") == 0)
    {
        return command_push_image();
    }
    else 
    {
        return -2;
    }
}

extern "C" void app_main() {
    initArduino();

    // init image_buf
    image_buf = (uint8_t *)malloc(CONFIG_IMAGE_BUF_SIZE);
    if (!image_buf) {
        return;
    }
    memset(image_buf, 0, CONFIG_IMAGE_BUF_SIZE);

    // initialize NVS
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
      ESP_ERROR_CHECK(nvs_flash_erase());
      ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    // Initialize EPD
    EPD_Pin_Init(6, 2, 7, 16,
      22, 23, 1, 0);

    mesh_main();

    // tcp client
    //start_tcp_client();

    // Arduino-like setup()

    // Arduino-like loop()
    while (true)
    {
      vTaskDelay(500);
    }

    // WARNING: if program reaches end of function app_main() the MCU will restart.
}