/* UART asynchronous example, that uses separate RX and TX tasks sync_slave
 */
#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_slave.h"
#include "loadbalancer.h"
#include "evse_api.h"
#include "evse_error.h"
#include "evse_state.h"
#include "evse_config.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 ErrorFlags error_flags;

static const char *TAG = "SLAVE";

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(TAG, "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(TAG, "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(LoToHi *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, LoToHi_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");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_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_relais_state(bool on)
{
    ESP_LOGI(TAG, "send_relais_state");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_relais_state_tag;
    msg_out.payload.relais_state = on;
    linkWrite(&msg_out);
}

void send_error_flags(ErrorFlags e)
{
    ESP_LOGI(TAG, "send_error_flags");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_error_flags_tag;
    msg_out.payload.error_flags = e;
    linkWrite(&msg_out);
}

void send_cp_state(CpState cp_state)
{
    ESP_LOGI(TAG, "send_cp_state");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_cp_state_tag;
    msg_out.payload.cp_state = cp_state;
    linkWrite(&msg_out);
}

void send_pp_state(PpState pp_state)
{
    ESP_LOGI(TAG, "send_pp_state");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_pp_state_tag;
    msg_out.payload.pp_state = pp_state;
    linkWrite(&msg_out);
}

void send_max_charging_current(uint32_t max_charging_current)
{
    ESP_LOGI(TAG, "send_max_charging_current");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_max_charging_current_tag;
    msg_out.payload.max_charging_current = max_charging_current;
    linkWrite(&msg_out);
}

void send_max_grid_current(uint32_t max_grid_current)
{
    ESP_LOGI(TAG, "send_max_grid_current");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_max_grid_current_tag;
    msg_out.payload.max_grid_current = max_grid_current;
    linkWrite(&msg_out);
}

void send_lock_state(LockState lock_state)
{
    ESP_LOGI(TAG, "send_lock_state");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_lock_state_tag;
    msg_out.payload.lock_state = lock_state;
    linkWrite(&msg_out);
}

void send_power_meter(PowerMeter p)
{
    ESP_LOGI(TAG, "send_power_meter");
    LoToHi msg_out = LoToHi_init_default;
    msg_out.which_payload = LoToHi_power_meter_tag;
    msg_out.payload.power_meter = p;
    linkWrite(&msg_out);
}

void handlePacket(uint8_t *buf, int 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;

    HiToLo msg_in;

    pb_istream_t istream = pb_istream_from_buffer(buf, len);

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

        switch (msg_in.which_payload)
        {
        case HiToLo_time_stamp_tag:
            printf("Received time stamp tag \n");
            break;
        case HiToLo_connector_lock_tag:
            printf("Received connector_lock %i\n", (int)msg_in.payload.connector_lock);
            break;
        case HiToLo_max_charging_current_tag:
            printf("Received max_charging_current %i\n",
                   (int)msg_in.payload.max_charging_current);
            evse_set_max_charging_current((int)msg_in.payload.max_charging_current);
            break;
        case HiToLo_max_grid_current_tag:
            printf("Received max_grid_current %i\n",
                   (int)msg_in.payload.max_grid_current);
            //grid_set_max_current((int)msg_in.payload.max_grid_current);
            break;
        case HiToLo_allow_power_on_tag:
            printf("Received allow_poweron %i\n",
                   (int)msg_in.payload.allow_power_on);
            break;
        case HiToLo_reset_tag:
            printf("Received reset\n");
            break;
        case HiToLo_grid_current_tag:
            printf("Received grid_current %i\n",
                   (int)msg_in.payload.grid_current);

            //setMaxGridCurrent(grid_get_max_current() * 10);
            //setLiveGridCurrent((int)msg_in.payload.grid_current);

            break;
        default:
            // printf("not found");
            printf("Not Found RemoteControl recvd %i \n", msg_in.which_payload);
            break;
        }
    }
}

/*
const int evse_state_to_int(evse_state_t state)
{
    switch (state)
    {
    case EVSE_STATE_A:
        return 1;
    case EVSE_STATE_B1:
        return 2;
    case EVSE_STATE_B2:
        return 3;
    case EVSE_STATE_C1:
        return 4;
    case EVSE_STATE_C2:
        return 5;
    case EVSE_STATE_D1:
        return 6;
    case EVSE_STATE_D2:
        return 7;
    case EVSE_STATE_E:
        return 8;
    case EVSE_STATE_F:
        return 9;
    default:
        return 9;
    }
}
*/

CpState evse_state_to_cpState(evse_state_t state)
{
    switch (state)
    {
    case EVSE_STATE_A:
        return CpState_EVSE_STATE_A;
    case EVSE_STATE_B1:
        return CpState_EVSE_STATE_B1;
    case EVSE_STATE_B2:
        return CpState_EVSE_STATE_B2;
    case EVSE_STATE_C1:
        return CpState_EVSE_STATE_C1;
    case EVSE_STATE_C2:
        return CpState_EVSE_STATE_C2;
    case EVSE_STATE_D1:
        return CpState_EVSE_STATE_D1;
    case EVSE_STATE_D2:
        return CpState_EVSE_STATE_D2;
    case EVSE_STATE_E:
        return CpState_EVSE_STATE_E;
    case EVSE_STATE_F:
        return CpState_EVSE_STATE_F;
    default:
        return CpState_EVSE_STATE_F;
    }
}

static void tx_task(void *arg)
{

    /*
    error_flags.connector_lock_failed = false;
    error_flags.cp_signal_fault = false;
    error_flags.diode_fault = false;
    error_flags.rcd_selftest_failed = false;
    error_flags.rcd_triggered = false;
    error_flags.ventilation_not_available = false;
    */

    while (1)
    {
        // send_time_stamp();
        // vTaskDelay(1000 / portTICK_PERIOD_MS);

        // send_relais_state(false);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);

        // send_error_flags(error_flags);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);

        send_cp_state(evse_state_to_cpState(evse_get_state()));
        vTaskDelay(5000 / portTICK_PERIOD_MS);

        // send_pp_state(PpState_STATE_32A);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);

        send_max_charging_current(evse_get_max_charging_current());
        vTaskDelay(5000 / portTICK_PERIOD_MS);


        //send_max_grid_current(grid_get_max_current());
        vTaskDelay(5000 / portTICK_PERIOD_MS);

        // send_lock_state(false);
        // vTaskDelay(1000 / portTICK_PERIOD_MS);

        /*
                PowerMeter p;
                p.vrmsL1 = 230;
                p.vrmsL2 = 230;
                p.vrmsL3 = 230;
                p.irmsL1 = 0;
                p.irmsL2 = 0;
                p.irmsL3 = 0;
                p.irmsN = 0;
                p.wattHrL1 = 0;
                p.wattHrL2 = 0;
                p.wattHrL3 = 0;
                p.totalWattHr = 0;

                p.tempL1 = 20;
                p.tempL2 = 20;
                p.tempL3 = 20;
                p.tempN = 20;

                p.wattL1 = 0;
                p.wattL2 = 0;
                p.wattL3 = 0;

                p.freqL1 = 50;
                p.freqL2 = 50;
                p.freqL3 = 50;

                p.phaseSeqError = 0;

                // send_power_meter(p);
                // 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 slave_sync_start()
{

    ESP_LOGI(TAG, "Starting SYNC Serial");

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

void slave_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;
    }*/
}