#include <hardware/adc.h>
#include <hardware/gpio.h>
#include <math.h>
#include <pico/stdlib.h>
#include <pico/time.h>
#include <stdint.h>
#include <sys/types.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_pot(input_t *input) {
  static volatile uint function_call_counter = 0;
  function_call_counter++;
  static uint8_t i = 0;
  for (; i < 2; i++, adc_select_input(i)) {
    {
      static uint8_t j = 0;
      for (; j < 8;) {
        set_mux_addr(j);
        sleep_ms(5); // Let multiplexers multiplex (5ms smallest reliable delay)
        float old_val = input->pots[j + i * 8];
        float new_val = (float)adc_read() / 4096.0f;
        
        if (!(new_val > 0.99f) && (fabs(new_val - old_val) >= 0.01f)) {

          input->pots[j + i * 8] = new_val;
        }
        
        j++;
        return;
      }
      j = 0;
    }
  }
  i = 0;
  adc_select_input(i);
  sleep_ms(5);
  update_pot(input); // dont skip the tick
}

void update_inputs(input_t *input) {

  // Pots
  update_pot(input);
  __asm__("nop");

  // 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;
  }
}