#include "power_monitor.h"
#include "command_handler.h"

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

LOG_MODULE_REGISTER(power_monitor, LOG_LEVEL_INF);

// DEVICE
#define POWER_MONITOR_NODE DT_NODELABEL(power_monitor)
const struct device *power_monitor_dt = DEVICE_DT_GET(POWER_MONITOR_NODE);


// THREAD
static struct k_thread power_monitor_thread_data;
static k_tid_t power_monitor_thread_id = NULL;
#define POWER_MONITOR_THREAD_STACK_SIZE 2048
#define POWER_MONITOR_THREAD_PRIORITY 5
K_THREAD_STACK_DEFINE(power_monitor_thread_stack, POWER_MONITOR_THREAD_STACK_SIZE);

// TIMER
struct k_timer power_monitor_timer;
struct k_sem power_monitor_semaphore;

// BUFFER
#define PWR_MSG_BUFFER_SIZE 10
struct command_message_t pwr_msg_buffer[PWR_MSG_BUFFER_SIZE];
struct command_message_t *pwr_msg_buffer_ptr;


static void power_monitor_timer_handler(struct k_timer *timer) {
    // Timer trigger
	k_sem_give(&power_monitor_semaphore);
}

static void power_monitor_thread(void *p1, void *p2, void *p3) {
	ARG_UNUSED(p1);
	ARG_UNUSED(p2);
	ARG_UNUSED(p3);

	while (1) {
		k_sem_take(&power_monitor_semaphore, K_FOREVER);

        struct sensor_value values[3][3];
        sensor_sample_fetch(power_monitor_dt);

		// Read channel 1
		struct sensor_value value = {1, 0};
		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[0][0]);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[0][1]);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[0][2]);

		// Read channel 2
		value.val1 = 2;
		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[1][0]);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[1][1]);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[1][2]);

		// Read channel 3
		value.val1 = 3;
		sensor_attr_set(power_monitor_dt, SENSOR_CHAN_ALL, (SENSOR_ATTR_PRIV_START+1), &value);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_VOLTAGE, &values[2][0]);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_CURRENT, &values[2][1]);
        sensor_channel_get(power_monitor_dt, SENSOR_CHAN_POWER, &values[2][2]);

		// Send to USB
		command_create_message(pwr_msg_buffer_ptr, sizeof(values), POWER_MONITOR_READ, (uint8_t *)values);
		command_handler_tx(pwr_msg_buffer_ptr);
		pwr_msg_buffer_ptr++;

		if (pwr_msg_buffer_ptr > &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1]) {
			pwr_msg_buffer_ptr = pwr_msg_buffer;
		}
	}
}

int power_monitor_init() {

	k_sem_init(&power_monitor_semaphore, 0, 1);
	
	// TIMER
	k_timer_init(&power_monitor_timer, power_monitor_timer_handler, NULL);

	// BUFFER
	memset(pwr_msg_buffer, 0, sizeof(pwr_msg_buffer));
	pwr_msg_buffer_ptr = pwr_msg_buffer;

	// DEVICE
    if (!device_is_ready(power_monitor_dt)) {
        LOG_ERR("INA3221 not ready\n");
        return 0;
    }
	
	// THREAD
	power_monitor_thread_id = k_thread_create(
		&power_monitor_thread_data,
		power_monitor_thread_stack,
		K_THREAD_STACK_SIZEOF(power_monitor_thread_stack),
		power_monitor_thread,
		NULL, NULL, NULL,
		POWER_MONITOR_THREAD_PRIORITY,
		0,
		K_NO_WAIT
	);

	return 0;
}

int power_monitor_timer_set(int interval_ms) {

	if (interval_ms > 0) {
		k_timer_start(&power_monitor_timer, K_MSEC(interval_ms), K_MSEC(interval_ms));
	}
	else {
		k_timer_stop(&power_monitor_timer);
	}
	
	return 0;
}

void* power_monitor_get_buffer() {
	struct command_message_t *ptr = pwr_msg_buffer_ptr;
	ptr--;

	if (ptr < &pwr_msg_buffer[0]) {
		ptr = &pwr_msg_buffer[PWR_MSG_BUFFER_SIZE-1];
	}

	return (void *)ptr;
}