#include <zephyr/kernel.h>
#include <zephyr/logging/log.h>

#include "canbus.h"
#include "canbus_registers.h"
#include "valve.h"

LOG_MODULE_REGISTER(valve, CONFIG_LOG_VALVE_LEVEL);

K_THREAD_STACK_DEFINE(valve_thread_stack, VALVE_THREAD_STACK_SIZE);
K_MSGQ_DEFINE(valve_msgq, sizeof(int), VALVE_MSGQ_SIZE, 4);

k_tid_t valve_thread_id;
struct k_thread valve_thread_data;

valve_status_t valve_status_data = {
    .valve_state = VALVE_STATE_UNKNOWN,
    .valve_operation = VALVE_OPERATION_IDLE,
};

int valve_start_thread(void)
{
    int rc;

    // Initialize the valve
    rc = valve_init();
    if (rc < 0)
    {
        LOG_ERR("Failed to initialize valve: %d", rc);
        return rc;
    }

    // Create the valve thread
    valve_thread_id = k_thread_create(&valve_thread_data, valve_thread_stack,
                                      K_THREAD_STACK_SIZEOF(valve_thread_stack),
                                      (k_thread_entry_t)valve_cmd, NULL, NULL, NULL,
                                      VALVE_THREAD_PRIORITY, 0, K_NO_WAIT);
    k_thread_name_set(valve_thread_id, "valve");

    LOG_INF("Valve thread started successfully");
while (1)
    {
        // Wait for commands from the message queue
        int cmd;
        rc = k_msgq_get(&valve_msgq, &cmd, VALVE_STATE_INTERVAL);
        if (rc == 0)
        {
            // Process the command
            rc = valve_cmd(cmd);
            if (rc < 0)
            {
                LOG_ERR("Failed to process valve command: %d", rc);
            }
        }
        else
        {
            valve_send_status();    // Send current valve status periodically
        }
    }  
    return 0;
}
int valve_init(void)
{
    return 0;
}

int valve_cmd(int cmd)
{
    switch (cmd)
    {
    case VALVE_COMMAND_OPEN:
        if (valve_status_data.valve_state != VALVE_STATE_OPEN)
        {
            valve_status_data.valve_state = VALVE_STATE_OPEN;
            valve_status_data.valve_operation = VALVE_OPERATION_OPENING;
            valve_send_status();    // Send updated status before opening
            valve_send_operation(); // Send updated operation state before opening
            k_sleep(VALVE_OPENING_TIME);                              // Simulate opening time
            valve_status_data.valve_operation = VALVE_OPERATION_IDLE; // Set operation to idle after opening
            valve_send_status();                                      // Send updated status after opening
            valve_send_operation();                                   // Send updated operation state after opening
        }
        break;
    case VALVE_COMMAND_CLOSE:
        if (valve_status_data.valve_state != VALVE_STATE_CLOSED)
        {
            valve_status_data.valve_operation = VALVE_OPERATION_CLOSING;
            valve_send_operation();                                   // Send updated operation state before closing
            k_sleep(VALVE_CLOSING_TIME);                              // Simulate closing time
            valve_status_data.valve_state = VALVE_STATE_CLOSED;       // Set valve state to closed after closing
            valve_status_data.valve_operation = VALVE_OPERATION_IDLE; // Set operation to idle after closing
            valve_send_status();                                      // Send updated status after closing
            valve_send_operation();                                   // Send updated operation state after closing
        }
        break;
    case VALVE_COMMAND_STOP:
        valve_status_data.valve_operation = VALVE_OPERATION_IDLE;
        break;
    default:
        LOG_ERR("Unknown valve command: %d", cmd);
        return -EINVAL; // Invalid command
    }
    return 0;
}

int valve_send_status(void)
{
    int rc = canbus_send8(CANBUS_REG_VALVE_STATUS, valve_status_data.valve_state);
    if (rc != 0)
    {
        LOG_ERR("Failed to send valve status: %d", rc);
        return rc;
    }
    char *state_str;
    switch (valve_status_data.valve_state)
    {
    case VALVE_STATE_CLOSED:
        state_str = "CLOSED";
        break;
    case VALVE_STATE_OPEN:
        state_str = "OPEN";
        break;
    case VALVE_STATE_ERROR:
        state_str = "ERROR";
        break;
    case VALVE_STATE_UNKNOWN:
        state_str = "UNKNOWN";
        break;
    default:
        state_str = "INVALID";
        break;
    }
    LOG_INF("Valve status sent: %s", state_str);
    return 0;
}

int valve_send_operation(void)
{
    int rc = canbus_send8(CANBUS_REG_VALVE_OPERATION, valve_status_data.valve_operation);
    if (rc != 0)
    {
        LOG_ERR("Failed to send valve operation: %d", rc);
        return rc;
    }
    char *operation_str;
    switch (valve_status_data.valve_operation)
    {
    case VALVE_OPERATION_IDLE:
        operation_str = "IDLE";
        break;
    case VALVE_OPERATION_OPENING:
        operation_str = "OPENING";
        break;
    case VALVE_OPERATION_CLOSING:
        operation_str = "CLOSING";
        break;
    default:
        operation_str = "UNKNOWN";
        break;
    }
    LOG_INF("Valve operation sent: %s", operation_str);
    return 0;
}