#include <esp_log.h>
#include "mbiface.h"
#include "socket_server.h"
#include "tasks.h"
#include "modbus.h"

static const char * TAG = "mb";

Tcpd_t g_mbifc_server = NULL;
ModbusSlave_t g_modbus;
static volatile uint16_t register2 = 0;

/**
 * Socket read handler
 */
static esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
{
    uint8_t buf[1024];
    uint8_t buf2[1024];
    int nbytes = read(sockfd, buf, sizeof(buf));
    if (nbytes <= 0) return ESP_FAIL;

    size_t resp_len = 0;
    ModbusError_t e = mb_handleRequest(&g_modbus, buf, nbytes, buf2, 1024, &resp_len);
    if (e == 0) {
        tcpd_send(client, buf2, (ssize_t) resp_len);
    } else {
        ESP_LOGE(TAG, "Error %d, closing socket", e);
        tcpd_kick(client);
    }

    return ESP_OK;
}

ModbusException_t startOfAccess(ModbusSlave_t *pSlave, ModbusFunction_t function, uint8_t i) {
    return 0;
}

void endOfAccess(ModbusSlave_t *ms) {
    //
}

ModbusException_t rh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t *pValue) {
    ESP_LOGD(TAG, "Read holding %d", ref);

    if (ref == 1) {
        *pValue = 1042; // device ID
        return 0;
    }
    if (ref == 2) {
        *pValue = register2;
        return 0;
    }

    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}

ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t value) {
    ESP_LOGD(TAG, "Write holding %d := %02x", ref, value);

    if (ref == 2) {
        register2 = value;
        return 0;
    }

    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}

void mbiface_setup()
{
    ESP_LOGI(TAG, "initing MB iface");
    g_modbus.proto = MB_PROTO_TCP;
    g_modbus.addr = 1;
    g_modbus.startOfAccess = startOfAccess;
    g_modbus.endOfAccess = endOfAccess;
    g_modbus.readHolding = rh;
    g_modbus.writeHolding = wh;

    tcpd_config_t server_config = TCPD_INIT_DEFAULT();
    server_config.max_clients = 1;
    server_config.task_prio = MBIFC_TASK_PRIO;
    server_config.task_stack = MBIFC_TASK_STACK;
    server_config.task_name = "MBIFC";
    server_config.port = 502;
    server_config.read_fn = read_fn;

    ESP_ERROR_CHECK(tcpd_init(&server_config, &g_mbifc_server));
}