#include "adc.h"
#include "mux.h"

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

LOG_MODULE_REGISTER(adc);


#define ZEPHYR_USER_NODE DT_PATH(zephyr_user)
static const struct adc_dt_spec adc_mux_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 0);
static const struct adc_dt_spec adc_ch16_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 1);
static const struct adc_dt_spec adc_ch17_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 2);

int16_t adc_buffer;
struct adc_sequence sequence = {
    .buffer = &adc_buffer,
    .buffer_size = sizeof(adc_buffer),
};

int adc_init_all(void) {
	int ret;

	ret = mux_init();
	if (ret != 0) {
		LOG_ERR("Failed to setup multiplexer: %d", ret);
		return ret;
	}

	if (!adc_is_ready_dt(&adc_mux_spec)) {
		LOG_ERR("ADC mux channel not ready");
		return -1;
	}
	if (!adc_is_ready_dt(&adc_ch16_spec)) {
		LOG_ERR("ADC channel 17 not ready");
		return -1;
	}
	if (!adc_is_ready_dt(&adc_ch17_spec)) {
		LOG_ERR("ADC channel 18 not ready");
		return -1;
	}

	ret = adc_channel_setup_dt(&adc_mux_spec);
	if (ret != 0) {
		LOG_ERR("Failed to setup ADC mux: %d", ret);
		return ret;
	}
	ret = adc_channel_setup_dt(&adc_ch16_spec);
	if (ret != 0) {
		LOG_ERR("Failed to setup ADC channel 17: %d", ret);
		return ret;
	}
	ret = adc_channel_setup_dt(&adc_ch17_spec);
	if (ret != 0) {
		LOG_ERR("Failed to setup ADC channel 18: %d", ret);
		return ret;
	}

	adc_sequence_init_dt(&adc_mux_spec, &sequence);
	adc_sequence_init_dt(&adc_ch16_spec, &sequence);
	adc_sequence_init_dt(&adc_ch17_spec, &sequence);

    return 0;
}


int32_t adc_read_id(uint8_t id) {
	int ret;
    int32_t val_mv = 0;
    adc_buffer = 0;

	if (id < MUX_CHANNELS) {
		// Read muxed channels
		(void)adc_sequence_init_dt(&adc_mux_spec, &sequence);

		// Set mux
		ret = mux_select_channel(id);
		if (ret < 0) {
			LOG_ERR("Could not set multiplexer (%d)\n", ret);
			return 0;
		}
		
		ret = adc_read_dt(&adc_mux_spec, &sequence);
		if (ret < 0) {
			LOG_ERR("Could not read (%d)\n", ret);
			return 0;
		}

		val_mv = (int32_t)adc_buffer;
		// ret = adc_raw_to_millivolts_dt(&adc_mux_spec, &val_mv);
	}
	else if (id == MUX_CHANNELS) {
		// Read unmuxed channel 17
		(void)adc_sequence_init_dt(&adc_ch16_spec, &sequence);

		ret = adc_read_dt(&adc_ch16_spec, &sequence);
		if (ret < 0) {
			LOG_ERR("Could not read (%d)\n", ret);
			return 0;
		}

		val_mv = (int32_t)adc_buffer;
		// ret = adc_raw_to_millivolts_dt(&adc_ch16_spec, &val_mv);
	}
	else if (id == (MUX_CHANNELS + 1)) {
		// Read unmuxed channel 18
		(void)adc_sequence_init_dt(&adc_ch17_spec, &sequence);

		ret = adc_read_dt(&adc_ch17_spec, &sequence);
		if (ret < 0) {
			LOG_ERR("Could not read (%d)\n", ret);
			return 0;
		}

		val_mv = (int32_t)adc_buffer;
		// ret = adc_raw_to_millivolts_dt(&adc_ch17_spec, &val_mv);
	}

    
	return val_mv;
}