#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/gpio.h"

#define FIRST_GPIO 2

int dig1 = 9;
int dig2 = 10;
int dig3 = 11;
int dig4 = 12;

int32_t mask = 0;

int bits[10] = {
  0x3f, //0
  0x06, //1
  0x5b, //2
  0x4f, //3
  0x66, // 4
  0x6d, // 5
  0x7d, // 6
  0x07, // 7
  0x7f, // 8
  0x67  // 9
};

void multiplex_digits(int num) {
int pos4 = num % 10;
int pos3 = ((int) (num/10)) % 10;
int pos2 = ((int) (num/100)) % 10;
int pos1 = ((int) (num/1000)) % 10;

  
     mask = bits[pos4] << FIRST_GPIO;
     gpio_put(dig4, false);
     gpio_set_mask(mask);
     sleep_ms(1);
     gpio_put(dig4, true);
     gpio_clr_mask(mask);

     mask = bits[pos3] << FIRST_GPIO;
     gpio_put(dig3, false);
     gpio_set_mask(mask);
     sleep_ms(1);
     gpio_put(dig3, true);
     gpio_clr_mask(mask);

     mask = bits[pos2] << FIRST_GPIO;
     gpio_put(dig2, false);
     gpio_set_mask(mask);
     sleep_ms(1);
     gpio_put(dig2, true);
     gpio_clr_mask(mask);

     mask = bits[pos1] << FIRST_GPIO;
     gpio_put(dig1, false);
     gpio_set_mask(mask);
     sleep_ms(1);
     gpio_put(dig1, true);
     gpio_clr_mask(mask);
   

}

int main() {
 
 int value = 0;
 int number = 0;
 int i = 0;
 stdio_init_all();
    for (int gpio = FIRST_GPIO; gpio < FIRST_GPIO + 12; gpio++) {
      gpio_init(gpio);
      gpio_set_dir(gpio, GPIO_OUT);
      gpio_set_outover(gpio, GPIO_OVERRIDE_INVERT);
    }


    
  while (true) {
    multiplex_digits(number);
    i++;
    if (i >= 200) {
      number++;
      i = 0;
    }
    
    
  }

  }