#include <hardware/adc.h>
#include <hardware/gpio.h>
#include <pico/stdlib.h>
#include <pico/time.h>
#include <sys/types.h>
#include <stdint.h>
#include <math.h>

#include "const.h"

#include "input.h"

bool is_toggle[BUTTON_COUNT] = { false };
bool btn_prev[BUTTON_COUNT] = { false };

void set_mux_addr(uint8_t addr) {
        gpio_put(MUX_S0, addr & 1);
        gpio_put(MUX_S1, (addr >> 1) & 1);
        gpio_put(MUX_S2, (addr >> 2) & 1);
}

void update_inputs(input_t* input) {
	// Pots
	for (uint8_t j = 0; j < 2; j++) {
		adc_select_input(j);
		for (uint8_t i = 0; i < 8; i++) {
			set_mux_addr(i);
			sleep_us(50); // Let multiplexers multiplex
			float old_val = input->buttons[i + j * 8];
			float new_val = (float)adc_read() / 4096.0f;
			if (fabs(new_val - old_val) >= 0.01f) input->pots[i + j * 8] = new_val;
		}
	}
	
	// Buttons
	for (uint8_t i = 0; i < BUTTON_COUNT; i++) {
		bool btn_curr = gpio_get(BUTTON_BASE + i);

		if (btn_curr == true && btn_prev[i] == false) {
			if (is_toggle[i]) input->buttons[i] = !input->buttons[i];
		}

		if (!is_toggle[i]) input->buttons[i] = btn_curr;

		btn_prev[i] = btn_curr;
		sleep_ms(1); // Why don't you bounce on this dihh
	}
}