//#define LOG_LOCAL_LEVEL ESP_LOG_DEBUG

#include <assert.h>
#include "modbus_fn.h"
#include "modbus_crc.h"
#include <esp_log.h>

static const char *TAG = "mb";

static bool crc_matches(const uint8_t *buf, size_t len) {
    assert(buf);
    uint16_t real = modbus_crc(buf, len - 2);
    uint16_t given = ((uint16_t) buf[len - 2] << 8) | (uint16_t) buf[len - 1];
    if (real != given) {
        ESP_LOGE(TAG, "CRC computed of %d bytes: %04x, given %04x", len-2, real, given);
    }
    return real == given;
}

// Read holding registers
int mb_build_fc3(uint8_t *buf, size_t len, size_t *resplen, uint8_t mbAddr, uint16_t ref, uint16_t qty) {
    assert(buf);
    assert(resplen);
    if (len < 8) {
        ESP_LOGE(TAG, "Buf len too short for FC3");
        return -1;
    }
    if (qty > 127) {
        ESP_LOGE(TAG, "FC3 qty too high");
        return -1;
    }

    size_t wp = 0;
    buf[wp++] = mbAddr;
    buf[wp++] = 0x03;
    buf[wp++] = (ref & 0xFF00) >> 8;
    buf[wp++] = (ref & 0xFF);
    buf[wp++] = (qty & 0xFF00) >> 8;
    buf[wp++] = (qty & 0xFF);
    uint16_t crc = modbus_crc(buf, wp);
    buf[wp++] = (crc & 0xFF00) >> 8;
    buf[wp++] = (crc & 0xFF);

    *resplen = 5 + qty * 2;
    return (int) wp;
}

static int mb_parse_fc3_4(const uint8_t *buf, size_t len, uint16_t *dst, size_t capacity, int fcX) {
    assert(buf);

    if (!crc_matches(buf, len)) {
        ESP_LOGE(TAG, "FC%d CRC mismatch!", fcX);
        return -1;
    }

    //11 03 06 AE41 5652 4340 49AD
    if (len == 5 && buf[1] == (0x80+fcX)) {
        ESP_LOGE(TAG, "FC%d exception: %d", fcX, buf[2]);
        return -1;
    }

    if (buf[1] != fcX) {
        ESP_LOGE(TAG, "FC%d decode fail: not FC%d", fcX, fcX);
        return -1;
    }

    int num = buf[2] / 2; // this is the number of bytes
    if (capacity < num) {
        ESP_LOGE(TAG, "FC%d decode fail: dst too small for %d registers", fcX, num);
        return -1;
    }

    for (int i = 0; i < num; i++) {
        *dst++ = (((uint16_t) buf[3 + i * 2]) << 8) | (((uint16_t) buf[3 + i * 2 + 1]));
    }
    return num;
}

int mb_parse_fc3(const uint8_t *buf, size_t len, uint16_t *dst, size_t capacity) {
    return mb_parse_fc3_4(buf, len, dst, capacity, 3);
}

//// Read input registers
int mb_build_fc4(uint8_t *buf, size_t len, size_t *resplen, uint8_t mbAddr, uint16_t ref, uint16_t qty) {
    assert(buf);
    assert(resplen);
    if (len < 8) {
        ESP_LOGE(TAG, "Buf len too short for FC4");
        return -1;
    }
    if (qty > 127) {
        ESP_LOGE(TAG, "FC3 qty too high");
        return -1;
    }

    size_t wp = 0;
    buf[wp++] = mbAddr;
    buf[wp++] = 0x04;
    buf[wp++] = (ref & 0xFF00) >> 8;
    buf[wp++] = (ref & 0xFF);
    buf[wp++] = (qty & 0xFF00) >> 8;
    buf[wp++] = (qty & 0xFF);
    uint16_t crc = modbus_crc(buf, wp);
    buf[wp++] = (crc & 0xFF00) >> 8;
    buf[wp++] = (crc & 0xFF);

    *resplen = 5 + qty * 2;
    return (int) wp;
}

int mb_parse_fc4(const uint8_t *buf, size_t len, uint16_t *dst, size_t capacity) {
    return mb_parse_fc3_4(buf, len, dst, capacity, 4);
}

// Write holding registers
int mb_build_fc16(uint8_t *buf, size_t len, size_t *resplen, uint8_t mbAddr, uint16_t ref, const uint16_t *data, uint16_t qty) {
    assert(buf);
    assert(data);
    assert(resplen);
    if (len < 9 + qty * 2) {
        ESP_LOGE(TAG, "Buf len too short for FC16, or bad len");
        return -1;
    }
    if (qty > 127) {
        ESP_LOGE(TAG, "FC16 qty too high");
        return -1;
    }

    size_t wp = 0;
    buf[wp++] = mbAddr;
    buf[wp++] = 0x10;
    buf[wp++] = (ref & 0xFF00) >> 8;
    buf[wp++] = (ref & 0xFF);
    buf[wp++] = (qty & 0xFF00) >> 8;
    buf[wp++] = (qty & 0xFF);
    buf[wp++] = qty * 2;

    for (int i = 0; i < qty; i++) {
        buf[wp++] = (data[i] & 0xFF00) >> 8;
        buf[wp++] = (data[i] & 0xFF);
    }

    uint16_t crc = modbus_crc(buf, wp);
    buf[wp++] = (crc & 0xFF00) >> 8;
    buf[wp++] = (crc & 0xFF);
    *resplen = 8;
    return (int) wp;
}


bool mb_parse_fc16(const uint8_t *buf, size_t len)
{
    assert(buf);

    if (!crc_matches(buf, len)) {
        ESP_LOGE(TAG, "FC16 CRC mismatch!");
        return -1;
    }

    //11 10 0001 0002 1298
    if (len == 5 && buf[1] == 0x90) {
        ESP_LOGE(TAG, "FC16 exception: %d", buf[2]);
        return -1;
    }

    if (buf[1] != 16) {
        ESP_LOGE(TAG, "FC16 decode fail: not FC16");
        return -1;
    }

    return 0;
}