DMG LCD Capture w/Teensy

int vsync = 0;
int clock = 0;
unsigned char data0 = 0;
unsigned char data1 = 0;
int hsync = 0;

int vsync_prev = 0;
int clock_prev = 0;
int hsync_prev = 0;
int hsync_read = 0;

unsigned char pixel = 0;

int line = 0;
int offset = 0;

int array_offset = 0;
int byte_offset = 0;
int bit_offset;


unsigned char frame[5760];


void setup () {
    Serial.begin(4000000);

    for (int i=2; i<7; i++){
        pinMode(i, INPUT);
    }

     for (int i=0; i<5760; frame[i++]=0);
}


void loop() {
    vsync = digitalRead(2);
    clock = digitalRead(3);
    data1 = digitalRead(4);
    data0 = digitalRead(5);
    hsync = digitalRead(6);


    // Read CLOCK  (falling edge)
    if (clock_prev==HIGH && clock==LOW){
        // Read pixels
        if (offset<160 && line<144){

          pixel = 2*data1+data0;

          // ************ IMPORTANT ************
          // This part could be implemented as 2-dimensional array, I'm just saving RAM. I tried to make it 2D and the result
          // was the same.
          // ****************************************
          // Offset in the frame bit map
          bit_offset = (line*320)+offset*2;

          // The actual char in the frame array
          array_offset = bit_offset/8;

          // The offset within one byte
          byte_offset = bit_offset%8;

         // Put the pixel into frame
          frame[array_offset]+=(pixel<<byte_offset);
        }

        offset++;
    }

    // Read HSYNC (rising edge)
    if (hsync_prev==LOW && hsync==HIGH){
       // Next line
       line++;
       offset = 0;
    }


    // Read VSYNC (rising edge)
    if (vsync_prev==LOW && vsync==HIGH){
       // Next frame

       // Frame divider 0x1E00
       Serial.write(0x1E);
       Serial.write(0x00);

       // Send and nullify frame data
       for (int i=0; i<5760; i++){
          Serial.write(frame[i]);
          frame[i] = 0;
       }
       line = 0;
       offset = 0;
      }

    clock_prev = clock;
    vsync_prev = vsync;
    hsync_prev = hsync;
}