#include <stdio.h>
#include "modbus.h"

/* https://gist.github.com/ccbrown/9722406 */
void hexdump(const void *data, size_t size)
{
    char ascii[17];
    size_t i, j;
    ascii[16] = '\0';
    for (i = 0; i < size; ++i) {
        printf("%02X ", ((unsigned char *) data)[i]);
        const uint8_t c = ((unsigned char *) data)[i];
        if (c >= ' ' && c <= '~') {
            ascii[i % 16] = (char) c;
        } else {
            ascii[i % 16] = '.';
        }
        if ((i + 1) % 8 == 0 || i + 1 == size) {
            printf(" ");
            if ((i + 1) % 16 == 0) {
                printf("|  %s \n", ascii);
            } else if (i + 1 == size) {
                ascii[(i + 1) % 16] = '\0';
                if ((i + 1) % 16 <= 8) {
                    printf(" ");
                }
                for (j = (i + 1) % 16; j < 16; ++j) {
                    printf("   ");
                }
                printf("|  %s \n", ascii);
            }
        }
    }
}

static ssize_t parsehex(const char *s, uint8_t *out, size_t cap)
{
    uint8_t buf = 0;
    uint8_t v;
    bool first = true;
    char c;
    size_t sz = 0;
    while (0 != (c = *s++)) {
        if (c >= '0' && c <= '9') {
            v = c - '0';
        } else if (c >= 'a' && c <= 'f') {
            v = 10 + (c - 'a');
        } else if (c >= 'A' && c <= 'F') {
            v = 10 + (c - 'A');
        } else {
            continue;
        }

        if (first) {
            buf |= (v & 15) << 4;
            first = false;
        } else {
            buf |= (v & 15);
            if (cap == 0) {
                return -1;
            }
            out[sz++] = buf;
            buf = 0;
            first = true;
            cap--;
        }
    }
    return (ssize_t) sz;
}


ModbusException_t soa(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id) {
    printf("Start of access: fc%02d, slave %d\n", fcx, slave_id);
    return 0;
}
void eoa(ModbusSlave_t *ms) {
    printf("End of access\n");
}
ModbusException_t rh(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
    printf("rh %d\n", i);
    switch (i) {
        case 107:
            *out = 0xAE41;
            break;
        case 108:
            *out = 0x5652;
            break;
        case 109:
            *out = 0x4340;
            break;
        default:
            return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }
    return 0;
}
ModbusException_t ri(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
    printf("ri %d\n", i);
    switch (i) {
        case 8:
            *out = 0xa;
            break;
        default:
            return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }
    return 0;
}
ModbusException_t rc(ModbusSlave_t *ms, uint16_t i, bool *out) {
    uint8_t store[] = {
        0b11001101,
        0b01101011,
        0b10110010,
        0b00001110,
        0b00011011,
    };

    if (i >= 19 && i <= 55) {
        int pos = i-19;
        *out = 0 != (store[pos/8] & (1 << (pos%8)));
    } else {
        return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }
    return 0;
}
ModbusException_t rd(ModbusSlave_t *ms, uint16_t i, bool *out) {
    uint8_t store[] = {
            0b10101100,
            0b11011011,
            0b00110101,
    };

    if (i >= 196 && i <= 217) {
        int pos = i - 196;
        *out = 0 != (store[pos/8] & (1 << (pos%8)));
    } else {
        return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
    }
    return 0;
}

ModbusException_t wc(ModbusSlave_t *pSlave, uint16_t i, bool b) {
    printf("Write coil %d <- %d\n", i, b);
    return 0;
}
ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t i, uint16_t i1) {
    printf("Write reg %d <- %d\n", i, i1);
    return 0;
}

int main()
{
#define buflen 1024
    uint8_t buf[buflen];
    uint8_t buf2[buflen];

    // 11 03 006B 0003 7687
    // 11 04 0008 0001 B298
    // 11 01 0013 0025 0E84
    // 11 02 00C4 0016 BAA9
    // 11 05 00AC FF00 4E8B
    // 11 06 0001 0003 9A9B
    // 11 0F 0013 000A 02 CD01 BF0B
    // 11 10 0001 0002 04 000A 0102 C6F0
    ssize_t pldlen = parsehex("11 03 006B 0003 7687", buf, buflen);

    ModbusSlave_t ms = {
           .addr = 17,
           .proto = MB_PROTO_RTU,
           .startOfAccess = soa,
           .endOfAccess = eoa,
           .readHolding = rh,
           .readInput = ri,
           .readCoil = rc,
           .readDiscrete = rd,
           .writeCoil = wc,
           .writeHolding = wh,
    };

    printf("Req:\n");
    hexdump(buf, pldlen);

    size_t respsize;
    ModbusError_t e = mb_handleRequest(&ms, buf, pldlen, buf2, buflen, &respsize);
    if (e) {
        printf("Err %d\n", e);
    } else {
        printf("Resp:\n");
        hexdump(buf2, respsize);
    }

    return 0;
}