#include "waterlevel_sensor.h"
#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>
#include <zephyr/modbus/modbus.h>
#include <stdlib.h>
#include "canbus.h"

LOG_MODULE_REGISTER(wls, CONFIG_LOG_WATERLEVELSENSOR_LEVEL);

#define MODBUS_NODE DT_COMPAT_GET_ANY_STATUS_OKAY(zephyr_modbus_serial)

struct k_thread waterlevel_sensor_thread_data;
K_THREAD_STACK_DEFINE(waterlevel_sensor_stack, WATERLEVEL_SENSOR_STACK_SIZE);
K_MSGQ_DEFINE(waterlevel_sensor_msgq, sizeof(waterlevel_command_t), WATERLEVEL_MESSAGE_QUEUE_SIZE, 4);

static int modbus_client_iface;

volatile static struct
{
    int level;   // Water level value
    int zeropoint; // Minimum value
    int maxpoint; // Maximum value
    int factor;  // Factor for unit conversion
    bool factor_set; // Flag to indicate if factor is set
} waterlevel_measurement = {
    .factor_set = false,
};

struct modbus_iface_param client_param = {
    .mode = MODBUS_MODE_RTU,
    .rx_timeout = 50000, // Timeout for receiving data in milliseconds
    .serial = {
        .baud = 9600,
        .parity = UART_CFG_PARITY_NONE,
        .stop_bits_client = UART_CFG_STOP_BITS_1, // 1 stop bit for Modbus RTU
    },
};

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

static int waterlevel_modbus_read(void) {
    int rc;
    union 
    {
        struct
        {
            int16_t unit;      // Unit of measurement (e.g., cm, mm)
            int16_t decimals;  // Number of decimal places for the measurement
            int16_t level;     // Water level value
            int16_t zeropoint; // Zero point for the measurement
            int16_t maxpoint;  // Maximum point for the measurement
        };
        int16_t data[5]; // Data array for holding registers
    } waterlevel_modbus_data;
    rc = modbus_read_holding_regs(modbus_client_iface, WATERLEVEL_SENSOR_MODBUS_NODE_ID, 0x0002, waterlevel_modbus_data.data, sizeof(waterlevel_modbus_data.data) / sizeof(waterlevel_modbus_data.data[0]));
    if (rc < 0)
    {
        LOG_ERR("Failed to read holding registers, node <%d>, returncode: %d", WATERLEVEL_SENSOR_MODBUS_NODE_ID, rc);
        return rc;
    }

    LOG_DBG("Got values. Unit: %d, Decimals: %d, Level: %d, Zero Point: %d, Max Point: %d", 
            waterlevel_modbus_data.unit, 
            waterlevel_modbus_data.decimals, 
            waterlevel_modbus_data.level, 
            waterlevel_modbus_data.zeropoint, 
            waterlevel_modbus_data.maxpoint);

    LOG_HEXDUMP_DBG(waterlevel_modbus_data.data, sizeof(waterlevel_modbus_data.data), "Waterlevel Sensor Holding Registers Data");
           
    switch (waterlevel_modbus_data.unit)
    {
    case 1: // cm
        waterlevel_measurement.factor = 10;
        break;
    case 2: // mm
        waterlevel_measurement.factor = 1;
        break;
    default:
        LOG_ERR("Unknown unit: %d", waterlevel_modbus_data.unit);
        waterlevel_measurement.factor_set = false;
        return -EINVAL;
    }
    switch (waterlevel_modbus_data.decimals)
    {
    case 0: // no decimals
        waterlevel_measurement.factor /= 1;
        break;
    case 1: // one decimal
        waterlevel_measurement.factor /= 10;
        break;
    case 2: // two decimals
        waterlevel_measurement.factor /= 100;
        break;
    default:
        LOG_ERR("Unknown decimals: %d", waterlevel_modbus_data.decimals);
        waterlevel_measurement.factor_set = false;
        return -EINVAL;
    }
    waterlevel_measurement.factor_set = true;
    waterlevel_measurement.level = waterlevel_modbus_data.level * waterlevel_measurement.factor;
    waterlevel_measurement.zeropoint = waterlevel_modbus_data.zeropoint * waterlevel_measurement.factor;
    waterlevel_measurement.maxpoint = waterlevel_modbus_data.maxpoint * waterlevel_measurement.factor;
    
    LOG_DBG("Water level: %dmm, zero point: %dmm, maximum point: %dmm", 
            waterlevel_measurement.level, 
            waterlevel_measurement.zeropoint, 
            waterlevel_measurement.maxpoint);
    LOG_HEXDUMP_DBG(waterlevel_modbus_data.data, sizeof(waterlevel_modbus_data.data), "Waterlevel Sensor Holding Registers Data");
    return 0;
}

static int waterlevel_send_level(void) {
    if (!waterlevel_measurement.factor_set) {
        LOG_ERR("Factor not set, cannot send water level");
        return -EINVAL;
    }

    LOG_INF("Sending water level: %dmm", waterlevel_measurement.level);
    canbus_send16(CANBUS_REG_WATERLEVEL_LEVEL, waterlevel_measurement.level);
    return 0;
}

static int waterlevel_send_zero_point(void) {
    if (!waterlevel_measurement.factor_set) {
        LOG_ERR("Factor not set, cannot send zero point");
        return -EINVAL;
    }

    LOG_INF("Sending water zero point: %dmm", waterlevel_measurement.zeropoint);
    canbus_send16(CANBUS_REG_WATERLEVEL_ZERO_POINT, waterlevel_measurement.zeropoint);
    return 0;
}

static int waterlevel_send_max_point(void) {
    if (!waterlevel_measurement.factor_set) {
        LOG_ERR("Factor not set, cannot send maximum point");
        return -EINVAL;
    }

    LOG_INF("Sending water maximum point: %dmm", waterlevel_measurement.maxpoint);
    canbus_send16(CANBUS_REG_WATERLEVEL_MAX_POINT, waterlevel_measurement.maxpoint);
    return 0;
}

static int waterlevel_set_zero_point(int zeropoint) {
    if (!waterlevel_measurement.factor_set) {
        LOG_ERR("Factor not set, cannot set zero point");
        return -EINVAL;
    }

    int16_t zeropoint_modbus = zeropoint / waterlevel_measurement.factor;
    int rc = modbus_write_holding_regs(modbus_client_iface, WATERLEVEL_SENSOR_MODBUS_NODE_ID, 0x0005, &zeropoint_modbus, 1);
    if (rc < 0) {
        LOG_ERR("Failed to write zero point: %d", rc);
        return rc;
    }

    waterlevel_measurement.zeropoint = zeropoint; // Update the local measurement structure    
    LOG_INF("Zero point set to: %dmm", waterlevel_measurement.zeropoint);
    rc = waterlevel_send_zero_point();
    if (rc < 0) {
        LOG_ERR("Failed to send zero point: %d", rc);
        return rc;
    }
    return 0;
}

static int waterlevel_set_max_point(int maxpoint) {
    if (!waterlevel_measurement.factor_set) {
        LOG_ERR("Factor not set, cannot set maximum point");
        return -EINVAL;
    }

    int16_t maxpoint_modbus = maxpoint / waterlevel_measurement.factor;
    int rc = modbus_write_holding_regs(modbus_client_iface, WATERLEVEL_SENSOR_MODBUS_NODE_ID, 0x0006, &maxpoint_modbus, 1);
    if (rc < 0) {
        LOG_ERR("Failed to write maximum point: %d", rc);
        return rc;
    }

    waterlevel_measurement.maxpoint = maxpoint; // Update the local measurement structure
    LOG_INF("Maximum point set to: %dmm", waterlevel_measurement.maxpoint);
    rc = waterlevel_send_max_point();
    if (rc < 0) {
        LOG_ERR("Failed to send maximum point: %d", rc);
        return rc;
    }
    return 0;
}

void waterlevel_sensor_thread(void *arg1, void *arg2, void *arg3)
{
    ARG_UNUSED(arg1);
    ARG_UNUSED(arg2);
    ARG_UNUSED(arg3);

    // Initialize the Modbus client
    int rc = waterlevel_modbus_init();
    if (rc < 0)
    {
        LOG_ERR("Failed to initialize Modbus client: %d", rc);
        return;
    }

    rc = waterlevel_modbus_read();
    if (rc < 0) {
        LOG_ERR("Failed to read initial water level: %d", rc);  
        return;
    }
    waterlevel_send_level();
    waterlevel_send_zero_point();
    waterlevel_send_max_point();

    // Initialize the last transmission time and level
    // Use k_uptime_get_32() to get the current uptime in milliseconds
    // and store the initial water level measurement.
    // This will be used to determine when to send updates.

    uint32_t last_transmission_time_ms = k_uptime_get_32();
    int32_t last_transmission_level = waterlevel_measurement.level;

    while (1)
    {     
        uint32_t current_time_ms = k_uptime_get_32();
        uint32_t delta_time = current_time_ms-last_transmission_time_ms;
        waterlevel_command_t command;

        rc = waterlevel_modbus_read();
        if (rc < 0)
        {
            LOG_ERR("Failed to read water level: %d", rc);
            continue;
        }

        if (delta_time >= WATERLEVEL_SENSOR_MAX_UPDATE_INTERVAL_MS ||
            abs(waterlevel_measurement.level - last_transmission_level) >= WATERLEVEL_SENSOR_MIN_DELTA) {
            rc = waterlevel_send_level();
            if (rc < 0) {
                LOG_ERR("Failed to send water level: %d", rc);
            } else {
                last_transmission_time_ms = current_time_ms;
                last_transmission_level = waterlevel_measurement.level;
            }
        }

        while (k_msgq_get(&waterlevel_sensor_msgq, &command, K_NO_WAIT) == 0)
        {
            switch(command.cmd)
            {
                case WATERLEVEL_CMD_SET:
                    switch (command.reg)
                    {
                        case CANBUS_REG_WATERLEVEL_ZERO_POINT: // Set zero point
                            rc = waterlevel_set_zero_point(command.data);
                            if (rc < 0) {
                                LOG_ERR("Failed to set zero point: %d", rc);
                            }
                            break;
                        case CANBUS_REG_WATERLEVEL_MAX_POINT: // Set maximum point
                            rc = waterlevel_set_max_point(command.data);
                            if (rc < 0) {
                                LOG_ERR("Failed to set maximum point: %d", rc);
                            }
                            break;
                        default:
                            LOG_ERR("Unknown register for SET command: 0x%02X", command.reg);
                            break;
                    }
                    break;
                case WATERLEVEL_CMD_GET:
                    switch (command.reg)
                    {
                        case CANBUS_REG_WATERLEVEL_LEVEL: // Get water level
                            waterlevel_send_level();
                            break;
                        case CANBUS_REG_WATERLEVEL_ZERO_POINT: // Get zero point
                            waterlevel_send_zero_point();
                            break;
                        case CANBUS_REG_WATERLEVEL_MAX_POINT: // Get maximum point
                            waterlevel_send_max_point();
                            break;
                        default:
                            LOG_ERR("Unknown register for GET command: 0x%02X", command.reg);
                            break;
                    }
                    break;
                default:
                    LOG_ERR("Unknown command type: %d", command.cmd);
                    break;
            }
        }   
    }
}

int waterlevel_sensor_start_thread(void)
{
    k_tid_t waterlevel_sensor_thread_id;

    // Start the thread
    waterlevel_sensor_thread_id = k_thread_create(&waterlevel_sensor_thread_data, waterlevel_sensor_stack,
                    K_THREAD_STACK_SIZEOF(waterlevel_sensor_stack), waterlevel_sensor_thread,
                    NULL, NULL, NULL,
                    WATERLEVEL_SENSOR_THREAD_PRIORITY, 0, K_NO_WAIT);

    if (waterlevel_sensor_thread_id == NULL)
    {
        LOG_ERR("Failed to create water level sensor thread");
        return -ENOMEM;
    }
    k_thread_name_set(waterlevel_sensor_thread_id, "waterlevel_sensor");

    LOG_INF("Water level sensor thread started successfully");
    return 0;
}

#ifdef CONFIG_SHELL
#include <zephyr/shell/shell.h>

void waterlevel_set_zero_point_shell(const struct shell *shell, size_t argc, char **argv) {
    if (argc != 2) {
        shell_error(shell, "Usage: waterlevel_sensor set_zero_point <zeropoint>");
        return;
    }

    int zeropoint = atoi(argv[1]);
    int rc = waterlevel_set_zero_point(zeropoint);
    if (rc < 0) {
        shell_error(shell, "Failed to set zero point: %d", rc);
    } else {
        shell_print(shell, "Zero point set to: %dmm", zeropoint);
    }
}

void waterlevel_set_max_point_shell(const struct shell *shell, size_t argc, char **argv) {
    if (argc != 2) {
        shell_error(shell, "Usage: waterlevel_sensor set_max_point <maxpoint>");
        return;
    }

    int maxpoint = atoi(argv[1]);
    int rc = waterlevel_set_max_point(maxpoint);
    if (rc < 0) {
        shell_error(shell, "Failed to set maximum point: %d", rc);
    } else {
        shell_print(shell, "Maximum point set to: %dmm", maxpoint);
    }
}

void waterlevel_sensor_print_shell(const struct shell *shell, size_t argc, char **argv) {
    ARG_UNUSED(argc);
    ARG_UNUSED(argv);

    waterlevel_modbus_read();
    if (!waterlevel_measurement.factor_set) {
        shell_error(shell, "Factor not set, cannot print water level");
        return;
    }

    shell_print(shell, "Current water level: %4dmm", waterlevel_measurement.level);
    shell_print(shell, "Zero point:          %4dmm", waterlevel_measurement.zeropoint);
    shell_print(shell, "Maximum point:       %4dmm", waterlevel_measurement.maxpoint);
}

// Define the shell commands for the water level sensor
SHELL_STATIC_SUBCMD_SET_CREATE(
    waterlevel_sensor_cmds,
    SHELL_CMD(print, NULL, "Print the current water level, zero point, and maximum point", waterlevel_sensor_print_shell),
    SHELL_CMD(setzero, NULL, "Set the zero point for the water level sensor", waterlevel_set_zero_point_shell),
    SHELL_CMD(setmax, NULL, "Set the maximum point for the water level sensor", waterlevel_set_max_point_shell),
    SHELL_SUBCMD_SET_END);

SHELL_CMD_REGISTER(wls, &waterlevel_sensor_cmds, "Water level sensor commands", NULL);
#endif // CONFIG_SHELL