uint8_t *txbuf;
uint8_t *rxbuf;

bool btn1;
bool btn2;
bool btn3;
bool btn4;
bool btn5;
bool btn6;
bool btn7;
bool btn8;

bool btn1_on;
bool btn2_on;
bool btn3_on;
bool btn4_on;
bool btn5_on;
bool btn6_on;
bool btn7_on;
bool btn8_on;

uint8_t buttons_prev;

// Send a command to the TM1638 Chip
// ------------------------------------

void tm1638_send_command(uint8_t x) {
  SPID1.spi->CR1 |= SPI_CR1_BIDIOE;
  spiSelect(&SPID1);
  txbuf[0] = x;
  spiSend(&SPID1, 1, &txbuf[0]);
  spiUnselect(&SPID1);
}

// Send data to the TM1638 Chip
// ------------------------------------

void tm1638_send_data(uint8_t addr, uint8_t data) {
  SPID1.spi->CR1 |= SPI_CR1_BIDIOE;
  tm1638_send_command(0x44);
  txbuf[0] = addr | 0xc0;
  txbuf[1] = data;
  spiSelect(&SPID1);
  spiSend(&SPID1, 2, txbuf);
  spiUnselect(&SPID1);
}

uint8_t tm1638_read_buttons(void) {

  // LogTextMessage("Reading Buttons!");

  txbuf[0] = 0x42;
  txbuf[1] = 0;
  txbuf[2] = 0;
  txbuf[3] = 0;
  txbuf[4] = 0;
  txbuf[5] = 0;
  txbuf[6] = 0;
  txbuf[7] = 0;

  spiSelect(&SPID1);
  spiSend(&SPID1, 1, &txbuf[0]);
  palSetPadMode(GPIOA, 7, PAL_MODE_INPUT); // MOSI -> tristate
  spiReceive(&SPID1, 4, &rxbuf[0]);
  spiUnselect(&SPID1);
  palSetPadMode(GPIOA, 7, PAL_MODE_ALTERNATE(5)); // MOSI -> output

  uint8_t out = 0;

  out += rxbuf[0] & 0x01 ? 1 : 0;
  out += rxbuf[1] & 0x01 ? 2 : 0;
  out += rxbuf[2] & 0x01 ? 4 : 0;
  out += rxbuf[3] & 0x01 ? 8 : 0;
  out += rxbuf[0] & 0x10 ? 16 : 0;
  out += rxbuf[1] & 0x10 ? 32 : 0;
  out += rxbuf[2] & 0x10 ? 64 : 0;
  out += rxbuf[3] & 0x10 ? 128 : 0;

  btn1 = rxbuf[0] & 0x01 ? true : false;
  btn2 = rxbuf[1] & 0x01 ? true : false;
  btn3 = rxbuf[2] & 0x01 ? true : false;
  btn4 = rxbuf[3] & 0x01 ? true : false;
  btn5 = rxbuf[0] & 0x10 ? true : false;
  btn6 = rxbuf[1] & 0x10 ? true : false;
  btn7 = rxbuf[2] & 0x10 ? true : false;
  btn8 = rxbuf[3] & 0x10 ? true : false;

  return out;
}

// Set the color for a single LED
// ------------------------------------
// 0 = OFF
// 1 = GREEN
// 2 = RED

void tm1638_set_led(uint8_t led, uint8_t cols) {
  tm1638_send_data((led << 1) + 1, cols);
}

// SETUP
// ------------------

void setup() {

  static uint8_t _txbuf[8] __attribute__((section(".sram2")));
  static uint8_t _rxbuf[8] __attribute__((section(".sram2")));

  txbuf = _txbuf;
  rxbuf = _rxbuf;
}

// LOOP
// ------------------

void loop() {

  chThdSleepMilliseconds(1);

  uint8_t buttons = tm1638_read_buttons();

  // LogTextMessage("Buttons: %i", buttons );
  // LogTextMessage("PREV Buttons: %i", buttons_prev );

  uint8_t buttons_pressed = buttons & ~buttons_prev;

  // LogTextMessage("Buttons Pressed: %i", buttons_pressed );

  if (buttons_pressed) { // or buttons pressed > 0 ???

    if (btn1 == true) {
      btn1_on = !btn1_on;
    }
    if (btn2 == true) {
      btn2_on = !btn2_on;
    }
    if (btn3 == true) {
      btn3_on = !btn3_on;
    }
    if (btn4 == true) {
      btn4_on = !btn4_on;
    }
    if (btn5 == true) {
      btn5_on = !btn5_on;
    }
    if (btn6 == true) {
      btn6_on = !btn6_on;
    }
    if (btn7 == true) {
      btn7_on = !btn7_on;
    }
    if (btn8 == true) {
      btn8_on = !btn8_on;
    }

    if (btn1_on == true) {
      tm1638_set_led(0, 2);
    } else {
      tm1638_set_led(0, 0);
    }

    if (btn2_on == true) {
      tm1638_set_led(1, 2);
    } else {
      tm1638_set_led(1, 0);
    }

    if (btn3_on == true) {
      tm1638_set_led(2, 2);
    } else {
      tm1638_set_led(2, 0);
    }

    if (btn4_on == true) {
      tm1638_set_led(3, 2);
    } else {
      tm1638_set_led(3, 0);
    }

    if (btn5_on == true) {
      tm1638_set_led(4, 2);
    } else {
      tm1638_set_led(4, 0);
    }

    if (btn6_on == true) {
      tm1638_set_led(5, 2);
    } else {
      tm1638_set_led(5, 0);
    }

    if (btn7_on == true) {
      tm1638_set_led(6, 2);
    } else {
      tm1638_set_led(6, 0);
    }

    if (btn8_on == true) {
      tm1638_set_led(7, 2);
    } else {
      tm1638_set_led(7, 0);
    }
  }

  buttons_prev = buttons;
}

// ------------------------------------------------------------------------------------------------
// THREADS
// ------------------------------------------------------------------------------------------------

msg_t ThreadX2() {
  setup();
  while (!chThdShouldTerminate()) {
    loop();
    chThdSleepMilliseconds(1);
  }

  chThdExit((msg_t)0);
}

static msg_t ThreadX(void *arg) { ((attr_parent *)arg)->ThreadX2(); }

WORKING_AREA(waThreadX, 1024);

Thread *Thd;

Thd = chThdCreateStatic(waThreadX, sizeof(waThreadX), NORMALPRIO, ThreadX,
                        (void *)this);

btn1 = false;
btn2 = false;
btn3 = false;
btn4 = false;
btn5 = false;
btn6 = false;
btn7 = false;
btn8 = false;

btn1_on = false;
btn2_on = false;
btn3_on = false;
btn4_on = false;
btn5_on = false;
btn6_on = false;
btn7_on = false;
btn8_on = false;

buttons_prev = 0;

outlet_toggle1 = btn1_on;
outlet_toggle2 = btn2_on;
outlet_toggle3 = btn3_on;
outlet_toggle4 = btn4_on;
outlet_toggle5 = btn5_on;
outlet_toggle6 = btn6_on;
outlet_toggle7 = btn7_on;
outlet_toggle8 = btn8_on;

chThdTerminate(Thd);
chThdWait(Thd);