#define DATA_PIN    2    // Pin connected to DS of 74HC595
#define LATCH_PIN   4    // Pin connected to STCP of 74HC595
#define CLOCK_PIN   3    // Pin connected to SHCP of 74HC595

void setup() {
  // put your setup code here, to run once:
  Serial1.begin(115200);
  Serial1.println("Hello, Raspberry Pi Pico!");

  // Set pins to output
  pinMode(DATA_PIN, OUTPUT);
  pinMode(CLOCK_PIN, OUTPUT);
  pinMode(LATCH_PIN, OUTPUT);
}

const uint16_t digitMask = 0b1100100001101100;
const uint16_t colonPin = 12;

uint16_t timeval[4] = {0, 0, 0, 0};
uint16_t colon = HIGH;

uint16_t registers = 0;

uint8_t digitPins[] = {4, 7, 10, 13}; // 1-Q4, 1-Q7, 2-Q2, 2-Q5
uint16_t digits[] = {
  0b0100000000000000, // 0
  0b0100000001101100, // 1
  0b1000000001000000, // 2
  0b0000000001001000, // 3
  0b0000000000101100, // 4
  0b0000100000001000, // 5
  0b0000100000000000, // 6
  0b0100000001001100, // 7
  0b0000000000000000, // 8
  0b0000000000001000, // 9
};

void writeRegisters()
{
  // Set and display registers
  digitalWrite(LATCH_PIN, LOW);
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, highByte(registers));
  shiftOut(DATA_PIN, CLOCK_PIN, MSBFIRST, lowByte(registers));
  digitalWrite(LATCH_PIN, HIGH);
}

uint32_t timer = 0;

uint32_t value = 0;

void scanDigit(int index)
{
  registers = digits[timeval[index]] | bit(digitPins[index]);
  if (!((index == 1) && colon)) {
    registers |= bit(colonPin);
  }
  writeRegisters();
}

void loop() {
  timer++;
  scanDigit(0);
  scanDigit(1);
  scanDigit(2);
  scanDigit(3);
  if (timer % 1000 == 0) {
    value++;
    colon ^= 1;
    timeval[0] = (value / 1000) % 10;
    timeval[1] = (value / 100) % 10;
    timeval[2] = (value / 10) % 10;
    timeval[3] = value % 10;
  }

  // put your main code here, to run repeatedly:
  delay(1); // this speeds up the simulation
}