/* UART asynchronous example, that uses separate RX and TX tasks
 */
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "esp_timer.h"
#include "driver/uart.h"
#include "string.h"
#include "driver/gpio.h"

#include "nanopb/pb_decode.h"
#include "nanopb/pb_encode.h"
#include "nanopb/pb_common.h"

#include "LoToHi.pb.h"
#include "HiToLo.pb.h"
// #include "inc/version_autogen.h"
#include "sync_master.h"
#include "evse_api.h"

#define VERSION_STRING "2.2"

static const int RX_BUF_SIZE = 1024;

#define TXD_PIN (GPIO_NUM_27)
#define RXD_PIN (GPIO_NUM_26)

static uint8_t msg[2048];
static uint8_t code;
static uint8_t block;
static uint8_t *decode;

static const char *TAG = "MASTER_TASK";

void cobsDecodeReset()
{
    code = 0xff;
    block = 0;
    decode = msg;
}

void handlePacket(uint8_t *buf, int len);

void init(void)
{
    const uart_config_t uart_config = {
        .baud_rate = 115200,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };
    // We won't use a buffer for sending data.
    uart_driver_install(UART_NUM_2, RX_BUF_SIZE * 2, 0, 0, NULL, 0);
    uart_param_config(UART_NUM_2, &uart_config);
    uart_set_pin(UART_NUM_2, TXD_PIN, RXD_PIN, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);

    cobsDecodeReset();
}

uint32_t crc32(uint8_t *buf, int len)
{
    int i, j;
    uint32_t b, crc, msk;

    i = 0;
    crc = 0xFFFFFFFF;
    while (i < len)
    {
        b = buf[i];
        crc = crc ^ b;
        for (j = 7; j >= 0; j--)
        {
            msk = -(crc & 1);
            crc = (crc >> 1) ^ (0xEDB88320 & msk);
        }
        i = i + 1;
    }
    // printf("%X",crc);
    return crc;
}

void cobsDecodeByte(uint8_t byte)
{

    // ESP_LOGI("RX_TASK", "init cobsDecodeByte");
    // ESP_LOGI("RX_TASK", "Read bytes: '%02X'", byte);

    // check max length
    if ((decode - msg == 2048 - 1) && byte != 0x00)
    {
        ESP_LOGI(TAG, "cobsDecode: Buffer overflow");
        cobsDecodeReset();
    }

    if (block)
    {
        // ESP_LOGI("RX_TASK", "block");
        //  we're currently decoding and should not get a 0
        if (byte == 0x00)
        {
            // probably found some garbage -> reset
            ESP_LOGI("RX_TASK", "cobsDecode: Garbage detected");
            cobsDecodeReset();
            return;
        }
        *decode++ = byte;
    }
    else
    {
        // ESP_LOGI("RX_TASK", "not block");
        if (code != 0xff)
        {
            *decode++ = 0;
        }
        block = code = byte;
        if (code == 0x00)
        {
            // we're finished, reset everything and commit
            if (decode == msg)
            {
                // we received nothing, just a 0x00
                ESP_LOGI("RX_TASK", "cobsDecode: Received nothing");
            }
            else
            {
                // set back decode with one, as it gets post-incremented
                handlePacket(msg, decode - 1 - msg);
            }
            cobsDecodeReset();
            return; // need to return here, because of block--
        }
    }
    block--;
}

void cobsDecode(uint8_t *buf, int len)
{
    for (int i = 0; i < len; i++)
    {
        cobsDecodeByte(buf[i]);
    }
}

size_t cobsEncode(const void *data, size_t length, uint8_t *buffer)
{
    uint8_t *encode = buffer;  // Encoded byte pointer
    uint8_t *codep = encode++; // Output code pointer
    uint8_t code = 1;          // Code value

    for (const uint8_t *byte = (const uint8_t *)data; length--; ++byte)
    {
        if (*byte) // Byte not zero, write it
            *encode++ = *byte, ++code;

        if (!*byte || code == 0xff) // Input is zero or block completed, restart
        {
            *codep = code, code = 1, codep = encode;
            if (!*byte || length)
                ++encode;
        }
    }
    *codep = code; // Write final code value

    // add final 0
    *encode++ = 0x00;

    return encode - buffer;
}

int linkWrite(HiToLo *m)
{

    ESP_LOGI(TAG, "linkWrite");
    uint8_t tx_packet_buf[200];
    uint8_t encode_buf[208];
    pb_ostream_t ostream = pb_ostream_from_buffer(tx_packet_buf, sizeof(tx_packet_buf) - 4);
    bool status = pb_encode(&ostream, HiToLo_fields, m);

    if (!status)
    {
        // couldn't encode
        return false;
    }

    size_t tx_payload_len = ostream.bytes_written;

    // add crc32 (CRC-32/JAMCRC)
    uint32_t crc = crc32(tx_packet_buf, tx_payload_len);

    for (int byte_pos = 0; byte_pos < 4; ++byte_pos)
    {
        tx_packet_buf[tx_payload_len] = (uint8_t)crc & 0xFF;
        crc = crc >> 8;
        tx_payload_len++;
    }
    size_t tx_encode_len = cobsEncode(tx_packet_buf, tx_payload_len, encode_buf);
    const int txBytes = uart_write_bytes(UART_NUM_2, encode_buf, tx_encode_len);
    return txBytes;
}

void send_time_stamp()
{
    ESP_LOGI(TAG, "send_time_stamp");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_time_stamp_tag;
    msg_out.payload.time_stamp = esp_timer_get_time() / 1000;
    linkWrite(&msg_out);
    printf("time stamp sent %i.\n", (int)msg_out.payload.time_stamp);
}

void send_connector_lock(bool on)
{
    ESP_LOGI(TAG, "connector_lock");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_connector_lock_tag;
    msg_out.payload.connector_lock = on;
    linkWrite(&msg_out);
}

void send_max_charging_current(uint32_t max_charging_current)
{
    ESP_LOGI(TAG, "send_max_charging_current");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_max_charging_current_tag;
    msg_out.payload.max_charging_current = max_charging_current;
    linkWrite(&msg_out);
}

void send_allow_power_on(bool allow_power_on)
{
    ESP_LOGI(TAG, "allow_power_on");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_allow_power_on_tag;
    msg_out.payload.allow_power_on = allow_power_on;
    linkWrite(&msg_out);
}

void send_reset(bool reset)
{
    ESP_LOGI(TAG, "reset");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_reset_tag;
    msg_out.payload.reset = reset;
    linkWrite(&msg_out);
}

void send_grid_current(uint32_t grid_current)
{
    ESP_LOGI(TAG, "send_grid_current");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_grid_current_tag;
    msg_out.payload.grid_current = grid_current;
    linkWrite(&msg_out);
}

void send_max_grid_current(uint32_t max_grid_current)
{
    ESP_LOGI(TAG, "send_max_grid_current");
    HiToLo msg_out = HiToLo_init_default;
    msg_out.which_payload = HiToLo_max_grid_current_tag;
    msg_out.payload.max_grid_current = max_grid_current;
    linkWrite(&msg_out);
}
/*
const evse_state_t cp_state_to_evse_state(CpState cp_state)
{
    switch (state)
    {
    case CpState_EVSE_STATE_A:
        return EVSE_STATE_A;
    case CpState_EVSE_STATE_B1:
        return EVSE_STATE_B1;
    case CpState_EVSE_STATE_B2:
        return EVSE_STATE_B2;
    case CpState_EVSE_STATE_C1:
        return EVSE_STATE_C1;
    case CpState_EVSE_STATE_C2:
        return EVSE_STATE_C2;
    case CpState_EVSE_STATE_D1:
        return EVSE_STATE_D1;
    case CpState_EVSE_STATE_D2:
        return EVSE_STATE_D2;
    case CpState_EVSE_STATE_E:
        return EVSE_STATE_E;
    case CpState_EVSE_STATE_F:
        return EVSE_STATE_F;
    default:
        return EVSE_STATE_F;
    }
}
*/

void handlePacket(uint8_t *buf, int len)
{
    // printf ("packet received len %u\n", len);

    // Check CRC32 (last 4 bytes)
    //  uint32_t crc = calculateCrc(rx_packet_buf, rx_packet_len);
    if (crc32(buf, len))
    {
        printf("CRC mismatch\n");
        return;
    }

    len -= 4;

    LoToHi msg_in;

    pb_istream_t istream = pb_istream_from_buffer(buf, len);

    if (pb_decode(&istream, LoToHi_fields, &msg_in))
    {
        //printf("LoToHi recvd %i \n", msg_in.which_payload);

        switch (msg_in.which_payload)
        {
        case LoToHi_time_stamp_tag:
            printf("Received time stamp %i\n", (int)msg_in.payload.time_stamp);
            break;
        case LoToHi_relais_state_tag:
            printf("Received relais_state %i\n", (int)msg_in.payload.relais_state);
            break;
        case LoToHi_error_flags_tag:
            // printf("Received error_flags %i\n", (int)msg_in.payload.error_flags);
            break;
        case LoToHi_cp_state_tag:
            printf("Received cp_state %i\n", (int)msg_in.payload.cp_state);
            // state = cp_state_to_evse_state(msg_in.payload.cp_state);
            break;
        case LoToHi_pp_state_tag:
            printf("Received cp_state %i\n", (int)msg_in.payload.pp_state);
            break;
        case LoToHi_max_charging_current_tag:
            printf("Received max_charging_current %i\n", (int)msg_in.payload.max_charging_current);

            int max_charging_current = (int)msg_in.payload.max_charging_current;

            if (max_charging_current != evse_get_max_charging_current())
            {
                send_max_charging_current(evse_get_max_charging_current());
            }
            break;

        case LoToHi_max_grid_current_tag:
            printf("Received max_grid_current %i\n", (int)msg_in.payload.max_grid_current);

            int max_grid_current = (int)msg_in.payload.max_grid_current;

            if (max_grid_current !=  grid_get_max_current())
            {
                send_max_grid_current(grid_get_max_current());
            }
            break;

        case LoToHi_lock_state_tag:
            printf("Received lock_state %i\n", (int)msg_in.payload.lock_state);
            break;
        case LoToHi_power_meter_tag:
            printf("Received power_meter %i\n", (int)msg_in.payload.power_meter.time_stamp);
            break;

        default:
            printf("Not Found Sync Master recvd %i \n", msg_in.which_payload);
            break;
        }
    }
}

static void tx_task(void *arg)
{

    while (1)
    {

        // send_time_stamp();
        // vTaskDelay(1000 / portTICK_PERIOD_MS);
        // send_connector_lock(true);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);

        send_max_charging_current(32);
        vTaskDelay(1000 / portTICK_PERIOD_MS);

        // send_allow_power_on(true);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);
        // send_reset(true);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);
    }
}

static void rx_task(void *arg)
{
    uint8_t *data = (uint8_t *)malloc(RX_BUF_SIZE + 1);
    while (1)
    {

        const int rxBytes = uart_read_bytes(UART_NUM_2, data, RX_BUF_SIZE, 1000 / portTICK_PERIOD_MS);
        if (rxBytes > 0)
        {
            data[rxBytes] = 0;
            // ESP_LOGI(TAG, "Read %d bytes: '%s'", rxBytes, data);
            // ESP_LOG_BUFFER_HEXDUMP(TAG, data, rxBytes, ESP_LOG_INFO);
            cobsDecode(data, rxBytes);
        }
    }
    free(data);
}

void master_sync_start()
{
    ESP_LOGI(TAG, "Master SYNC Serial");

    init();
    xTaskCreate(rx_task, "uart_rx_task", 1024 * 5, NULL, 5, NULL);
    //xTaskCreate(tx_task, "uart_tx_task", 1024 * 5, NULL, 5, NULL);
}

void master_sync_stop(void)
{
    /*
    ESP_LOGI(TAG, "Stopping");

    if (rx_task)
    {
        vTaskDelete(rx_task);
        rx_task = NULL;
    }

    if (tx_task)
    {
        vTaskDelete(tx_task);
        tx_task = NULL;
    }

    if (port != -1)
    {
        uart_driver_delete(port);
        port = -1;
    }*/
}