#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 "esp_mesh.h"
#include "main.h"

#include "sha256.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;

#if CONFIG_MESH_ROOT == 0
extern "C" void req_release(request_t *req) {
    free(req->command);
    req->command = 0;
    free(req->data);
    req->data = 0;
    
    free(req);

    return;
}

extern "C" int command_update_image(uint64_t index, char *buf, size_t len) {
    // check
    if (!buf) {
        return -2;
    }

    // main
    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 DEBUG == 1
    printf("sha256 of image_buf: ");
    show_sha256((char *)image_buf, CONFIG_IMAGE_BUF_SIZE);
#endif

    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 mesh_client_callback(char *buf, size_t len) {
    request_t *req = client_parse_data(buf, len);
    if (!req) { // error -> invalid data
        return -2;
    }

    int ret = 0;

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

mesh_client_callback_ret:
    req_release(req);
    return ret;
}
#else // root -> 送測試資料
#define PAYLOAD_SIZE 1024
#include "IMAGE.h"
void mesh_server_send_test_data() {
    /*
    mpack_writer_t *writer = (mpack_writer_t *)malloc(sizeof(mpack_writer_t));
    char *payload = (char *)malloc(PAYLOAD_SIZE);
    if (!writer || !payload) {
        printf("failed to allocate memory\n");
        return;
    }

    const unsigned char *image_start = &gImage_1[0];
    const unsigned char *image_end = image_start + sizeof(gImage_1);

    printf("sha256 of gImage_1: ");
    show_sha256((char *)image_start, sizeof(gImage_1));

    uint32_t counter = 0;
    for (const unsigned char *image = image_start; image < image_end; image += 950) {
        vTaskDelay(2000 / portTICK_PERIOD_MS);

        memset(writer, 0, sizeof(mpack_writer_t));
        memset(payload, 0, PAYLOAD_SIZE);

        mpack_writer_init(writer, payload, PAYLOAD_SIZE);
        mpack_start_map(writer, 3);

        mpack_write_cstr(writer,"command");
        mpack_write_cstr(writer,"update_image");
        mpack_write_cstr(writer,"index");
        printf("index -> %d\n", image - image_start);
        mpack_write_u64(writer, image - image_start);

        //mpack_write_cstr(writer,"data");
        //size_t count = 950;
        //if (image + 950 > image_end)
        //    count = image_end - image;
        //mpack_start_bin(writer, count);
        //mpack_write_bin(writer, (const char *)image, count);
        //mpack_finish_bin(writer);

        char testdata[64];
        memset(testdata, 0, sizeof(testdata));
        snprintf(testdata, sizeof(testdata), "Hello, World! -> %lu", counter++);
        mpack_start_bin(writer, sizeof(testdata));
        mpack_write_bin(writer, testdata, sizeof(testdata));
        mpack_finish_bin(writer);

        mpack_finish_map(writer);

        if (mpack_writer_destroy(writer) != mpack_ok) {
            printf("error!\n");
            continue;
        }

        //fwrite("payload: ", 1, 9, stdout);
        //fwrite(payload, 1, PAYLOAD_SIZE, stdout);
        //fwrite("\n", 1, 1, stdout);

        esp_err_t err = esp_mesh_boardcast_to_nodes((uint8_t *)payload, PAYLOAD_SIZE);
        printf("mesh_server_send_test_data result -> %d\n", err);
    }
    

    vTaskDelay(1500 / portTICK_PERIOD_MS);

    memset(writer, 0, sizeof(mpack_writer_t));
    memset(payload, 0, PAYLOAD_SIZE);

    mpack_writer_init(writer, payload, PAYLOAD_SIZE);
    mpack_start_map(writer, 1);
    mpack_write_cstr(writer,"command");
    mpack_write_cstr(writer,"push_image");
    mpack_finish_map(writer);

    if (mpack_writer_destroy(writer) != mpack_ok) {
        printf("error!\n");
    } else {
        esp_err_t err = esp_mesh_boardcast_to_nodes((uint8_t *)payload, PAYLOAD_SIZE);
        printf("mesh_server_send_test_data result -> %d\n", err);
    }

    free(writer);
    free(payload);
    */

    char payload[64] = {0};
    
    for (int i = 0; i < 6; i++) {
        memset(payload, 0, sizeof(payload));
        snprintf(payload, sizeof(payload), "Hello, World! -> %d", i);
        esp_err_t err = esp_mesh_boardcast_to_nodes((uint8_t *)payload, sizeof(payload));
        printf("[i=%d] result -> %d\n", i, err);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }

    return;
}
#endif

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);
    
    // start mesh network
    mesh_main();
    xTaskCreate(esp_mesh_p2p_rx_main, "MPRX", 3072, NULL, 5, NULL);

    // Arduino-like setup()

    // Arduino-like loop()
    int32_t counter = 6;
    while (true)
    {  
#if CONFIG_MESH_ROOT == 1
        printf("counter -> %ld\n", counter);
        if (counter == 0)
            mesh_server_send_test_data();
        if (counter >= 0)
            counter--;
#endif
        printf("Am I root? -> %d\n", esp_mesh_is_root());
        vTaskDelay(5 * 1000 / portTICK_PERIOD_MS);
    }

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