#include <digitalWriteFast.h>

#define NOP __asm__ __volatile__ ("nop\n\t")
#define READ_BYTES_LEN 32

const int pinCount = 4;  // Number of GPIO pins to control
int pins[pinCount] = {2,3,4,5};
// int pins[pinCount] = {2, 3, 4, 5, 6, 7};  // GPIO pins to use
unsigned long delayTime = 0.01;  // Set delay in microseconds
char buf[READ_BYTES_LEN];
uint8_t scan_chain_in[2];

void setup() {
  // Initialize serial communication at a higher baud rate
  Serial.begin(1000000);  // Increase baud rate for faster communication

  // setup PORTD directions
  DDRD = 0b00111110;

  // Set the GPIO pins as OUTPUT
  // for (int i = 0; i < pinCount; i++) {
  //   pinMode(pins[i], OUTPUT);
  // }
}

void loop() {
  if (Serial.readBytes(buf, READ_BYTES_LEN)) { // Read bytes from serial input
    for(size_t i = 0; i < READ_BYTES_LEN; i++){
      scan_chain_in[0] = (buf[i] & 0b11110000) >> 2; // shift to align with PORTD write
      scan_chain_in[1] = (buf[i] & 0b00001111) << 2; // shift to align with PORTD write
      PORTD = scan_chain_in[0];
      for (size_t i = 0; i < 2; i++) NOP;
      PORTD = scan_chain_in[1];
      for (size_t i = 0; i < 2; i++) NOP;
    }
    // scan_chain_in[0] = (incoming & 0b11110000) >> 2; // shift to align with PORTD write
    // scan_chain_in[1] = (incoming & 0b00001111) << 2; // shift to align with PORTD write
    // PORTD = scan_chain_in[0];
    // for (size_t i = 0; i < 4; i++) NOP;
    // PORTD = scan_chain_in[1];
    // for (size_t i = 0; i < 4; i++) NOP;
      // for (int j = 0; j < 2; j++){
      //   for (int i = 0; i < pinCount; i++) {
      //     bool val = incoming & (0b10000000 >> (i + (j*4)));
      //     if (val != 0) {
      //       digitalWriteFast(pins[i], HIGH);
      //     } else {
      //       digitalWriteFast(pins[i], LOW);
      //     }
      //   }
      //   // delayMicroseconds(10);
      // }

    // Add microsecond delay (adjust the value as needed)
    //  delayMicroseconds(50);  // Replace delay(500) with microsecond delay
  }

}