#include <LiquidCrystal_I2C.h>
 
String lcdString;
const int numBits = 8;
const int dataPin = 10;
const int clockPin = 11;
const int latchPin = 12;
LiquidCrystal_I2C lcd(0x27,20,4);

void setup() 
{
  Serial.begin(115200);
  lcd.init();
  lcd.backlight();
  pinMode(dataPin, INPUT);
  pinMode(clockPin, OUTPUT);
  pinMode(latchPin, OUTPUT);
  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
}

void loop() 
{ 
  for (uint8_t sendByte = 0; sendByte < 255; sendByte++)
  {
    // Clear LCD display.
    lcdString = "";
    lcd.clear();

    // Set pins on PORTD.
    asm volatile(
      "out %0, %1"
      :: "I" (_SFR_IO_ADDR(PORTD)), "r" (sendByte)
    );

    // Get bytes 0 and 1 in sendByte.
    uint8_t zero = bitRead(sendByte, 0);
    uint8_t one = bitRead(sendByte, 1);

    //// Read pins A0 and A1 /////
    uint8_t pinc;
    asm volatile(
      "in %0, %1"
      : "=r" (pinc)
      : "I" (_SFR_IO_ADDR(PINC))
    );
    uint8_t C0 = bitRead(pinc, 0);
    uint8_t C1 = bitRead(pinc, 1);
    //////////////////////////////

    // Skip the next instruction if bit 0 in sendByte matches A0.
    asm volatile(
      "cpse %0, %1"
      :: "r" (C0), "r" (zero)
    );

    // Toggle A0.
    asm volatile(
      "sbi %0, %1"
      :: "I" (_SFR_IO_ADDR(PINC)), "I" (0)
    );

    // Skip the next instruction if bit 1 in sendByte matches A1.
    asm volatile(
      "cpse %0, %1"
      :: "r" (C1), "r" (one)
    );
    
    // Toggle A1.
    asm volatile(
      "sbi %0, %1"
      :: "I" (_SFR_IO_ADDR(PINC)), "I" (1)
    );

    // Skip the next instruction if D12 is LOW.
    asm volatile(
      "sbic %0, %1"
      :: "I" (_SFR_IO_ADDR(PINB)), "I" (4)
    );

    // Toggle pin D12.
    asm volatile(
      "sbi %0, %1"
      :: "I" (_SFR_IO_ADDR(PINB)), "I" (4)
    );

    // Toggle pin D12.
    asm volatile(
      "sbi %0, %1"
      :: "I" (_SFR_IO_ADDR(PINB)), "I" (4)
    );

    for (int i = 0; i < numBits; i++) 
    {
      //// Read pin D10 ////
      uint8_t pinb;
      asm volatile(
        "in %0, %1"
        : "=r" (pinb)
        : "I" (_SFR_IO_ADDR(PINB))
      );
      uint8_t B2 = bitRead(pinb, 2);
      //////////////////////

      lcdString += B2;

      // Toggle pin D11.
      asm volatile(
        "sbi %0, %1"
        :: "I" (_SFR_IO_ADDR(PINB)), "I" (3)
      );

      // Toggle pin D11.
      asm volatile(
        "sbi %0, %1"
        :: "I" (_SFR_IO_ADDR(PINB)), "I" (3)
      );
    }

    Serial.println(sendByte);
    lcd.setCursor(4,0);
    lcd.print(lcdString);
    asmdelay(1000);
  }
}

// Delay.
void asmdelay(uint8_t ms)
{
  uint16_t count;
  asm volatile(
    "\n"            "\n\t"
    "L_dl1%=:"      "\n\t"
    "mov %A0, %A2"  "\n\t"
    "mov %B0, %B2"  "\n\t"
    "L_dl2%=:"      "\n\t"
    "sbiw %A0, 1"   "\n\t"
    "brne L_dl2%="  "\n\t"
    "dec %1"        "\n\t"
    "brne L_dl1%="  "\n\t"
    : "=&w" (count)  
    : "r" (ms), "r" (2000)
  );
}