#include "modbus.h"
#include <zephyr/kernel.h>
#include <zephyr/sys/util.h>
#include <zephyr/modbus/modbus.h>
#include <zephyr/logging/log.h>

LOG_MODULE_REGISTER(mbc, CONFIG_LOG_DEFAULT_LEVEL);

#define MODBUS_NODE DT_COMPAT_GET_ANY_STATUS_OKAY(zephyr_modbus_serial)

static int client_iface;

static struct
{
    int level;   // Water level value
    int minimum; // Minimum value
    int maximum; // Maximum value
    int factor; // Factor for unit conversion
} measurement;

int mb_init_client(void)
{
    const char iface_name[] = {DEVICE_DT_NAME(MODBUS_NODE)};
    client_iface = modbus_iface_get_by_name(iface_name);
    LOG_DBG("Modbus client interface: %d", client_iface);
    if (client_iface < 0)
    {
        LOG_ERR("Failed to get Modbus interface by name: %s", iface_name);
        return client_iface;
    }
    return modbus_init_client(client_iface, client_param);
}

int mb_read_holding_registers(int node, uint16_t reg_addr, uint16_t *data, size_t len)
{
    return modbus_read_holding_regs(client_iface, node, reg_addr, data, len);
}

int mb_read()
{
    int rc;
    int16_t data[5] = {0};
    rc = mb_read_holding_registers(1, 0x0002, data, sizeof(data) / sizeof(data[0]));
    if (rc < 0)
    {
        LOG_ERR("Failed to read holding registers: %d", rc);
        return rc;
    }
    LOG_HEXDUMP_DBG(data, sizeof(data), "Holding Registers Data");

    int unit, decimals;
    unit = data[0];
    decimals = data[1];
    int factor;
    switch (unit)
    {
    case 1: // cm
        factor = 10;
        break;
    case 2: // mm
        factor = 1;
        break;
    default:
        LOG_ERR("Unknown unit: %d", unit);
        return -EINVAL;
    }
    switch(decimals)
    {
    case 0: // no decimals
        factor /= 1;
        break;
    case 1: // one decimal
        factor /= 10;
        break;
    case 2: // two decimals
        factor /= 100;
        break;
    default:
        LOG_ERR("Unknown decimals: %d", decimals);
        return -EINVAL;
    }
    measurement.factor = factor;
    measurement.level = data[2] * factor;
    measurement.minimum = data[3] * factor;
    measurement.maximum = data[4] * factor; 
    LOG_DBG("Water level: %dmm, Minimum: %dmm, Maximum: %dmm", 
            measurement.level, measurement.minimum, measurement.maximum);
    return 0;
}

int mb_read_water_level(double *mb_read_water_level)
{  
    int rc = mb_read();
    if (rc < 0)
    {
        LOG_ERR("Failed to read water level: %d", rc);
        return rc;
    }
    *mb_read_water_level = (double)measurement.level / 1000.0; // Convert to meters

    return 0;
}

int mb_read_water_level_mm(int *mb_read_water_level)
{  
    int rc = mb_read();
    if (rc < 0)
    {
        LOG_ERR("Failed to read water level: %d", rc);
        return rc;
    }
    *mb_read_water_level = measurement.level;

    return 0;
}

int mb_read_minimum_mm(int *mb_read_minimum)
{
    int rc = mb_read();
    if (rc < 0)
    {
        LOG_ERR("Failed to read water level: %d", rc);
        return rc;
    }
    *mb_read_minimum = measurement.minimum;
    return 0;
}

int mb_read_maximum_mm(int *mb_read_maximum)
{
    int rc = mb_read();
    if (rc < 0)
    {
        LOG_ERR("Failed to read water level: %d", rc);
        return rc;
    }
    *mb_read_maximum = measurement.maximum;
    return 0;
}

int mb_write_minimum_mm(int minimum)
{
    int rc = mb_read();
    if (rc < 0)
    {
        LOG_ERR("Failed to read water level: %d", rc);
        return rc;
    }
    modbus_write_holding_reg(client_iface, 1, 0x0005, minimum / measurement.factor);
    return 0;
}

int mb_write_maximum_mm(int maximum)
{
    int rc = mb_read();
    if (rc < 0)
    {
        LOG_ERR("Failed to read water level: %d", rc);
        return rc;
    }
    modbus_write_holding_reg(client_iface, 1, 0x0006, maximum / measurement.factor);
    return 0;
}