#include <stdio.h>
#include <string.h>

#include "pcan_id.h"
#include "pcan_modbus_server.h"

static uint16_t holding[32];
static uint16_t input[32];
static uint16_t coils;

static bool read_word(void *user, pcan_modbus_bank_t bank,
                      uint16_t address, uint16_t *value)
{
    (void)user;
    if (address >= 32U) return false;
    if (bank == PCAN_MODBUS_HOLDING) *value = holding[address];
    else if (bank == PCAN_MODBUS_INPUT) *value = input[address];
    else if (bank == PCAN_MODBUS_COIL) *value = (uint16_t)((coils >> address) & 1U);
    else return false;
    return true;
}

static bool write_word(void *user, pcan_modbus_bank_t bank,
                       uint16_t address, uint16_t value)
{
    (void)user;
    if (address >= 32U) return false;
    if (bank == PCAN_MODBUS_HOLDING) holding[address] = value;
    else if (bank == PCAN_MODBUS_COIL && address < 16U) {
        if (value) coils |= (uint16_t)(1U << address);
        else coils &= (uint16_t)~(1U << address);
    } else return false;
    return true;
}

static pcan_frame_t request(uint8_t type, uint16_t address, uint8_t count)
{
    pcan_id_t id = {0};
    pcan_frame_t frame = {0};
    id.priority = PCAN_PRIORITY_STANDARD;
    id.route = PCAN_ROUTE_FROM_PM;
    id.device_type = 7U;
    id.device_id = 3U;
    id.msg_type = type;
    id.msg_body = pcan_body_modbus(address, count);
    frame.id = pcan_id_pack(&id);
    frame.flags = PCAN_FLAG_IDE;
    return frame;
}

#define CHECK(x) do { if (!(x)) { \
    fprintf(stderr, "FAIL %s:%d: %s\n", __FILE__, __LINE__, #x); return 1; \
} } while (0)

int main(void)
{
    pcan_modbus_server_t server;
    pcan_frame_t req;
    pcan_frame_t rsp;
    pcan_id_t id;
    memset(holding, 0, sizeof(holding));
    memset(input, 0, sizeof(input));
    input[10] = 0x1234U;
    input[11] = 0xABCDU;

    CHECK(pcan_modbus_server_init(&server, 7U, 3U, read_word, write_word, NULL));
    req = request(PCAN_MODBUS_INPUT, 10U, 2U);
    CHECK(pcan_modbus_server_handle(&server, &req, &rsp) == PCAN_MODBUS_RESPONSE);
    CHECK(rsp.dlc == 4U && rsp.data[0] == 0x34U && rsp.data[1] == 0x12U);
    CHECK(rsp.data[2] == 0xCDU && rsp.data[3] == 0xABU);
    pcan_id_unpack(rsp.id, &id);
    CHECK(id.route == PCAN_ROUTE_FROM_DEVICE);

    req = request(PCAN_MODBUS_HOLDING, 3U, 2U);
    req.dlc = 4U;
    req.data[0] = 0x22U; req.data[1] = 0x11U;
    req.data[2] = 0x44U; req.data[3] = 0x33U;
    CHECK(pcan_modbus_server_handle(&server, &req, &rsp) == PCAN_MODBUS_RESPONSE);
    CHECK(holding[3] == 0x1122U && holding[4] == 0x3344U);

    req = request(PCAN_MODBUS_COIL, 2U, 3U);
    req.dlc = 2U; req.data[0] = 0x05U; req.data[1] = 0U;
    CHECK(pcan_modbus_server_handle(&server, &req, &rsp) == PCAN_MODBUS_RESPONSE);
    CHECK(coils == 0x0014U);

    req = request(PCAN_MODBUS_INPUT, 0U, 5U);
    CHECK(pcan_modbus_server_handle(&server, &req, &rsp) == PCAN_MODBUS_HANDLED_NO_RESPONSE);
    CHECK(server.requests == 4U && server.responses == 3U && server.rejected == 1U);
    return 0;
}