#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <TimerOne.h>
#include "SdFat.h"

void Compress(unsigned char *szOut, const char *szMessage) {
	unsigned long long nBuffer = 0; //This is enough to store 8 uncompressed characters or 9 compressed. We will only use 8 tho.
	char nBitsInBuffer = 0;
	while (*szMessage != 0) {
		nBuffer |= (unsigned long long)(*szMessage++ & 0x7F) << nBitsInBuffer;
		nBitsInBuffer += 7;
		if (nBitsInBuffer == 7 * 8) { //We have 8 chars in the buffer, dump them
			while (nBitsInBuffer > 0) {
				*szOut++ = nBuffer & 0xFF;
				nBuffer >>= 8;
				nBitsInBuffer -= 8;
			}
			//The following should be redundant, but just to be safe
			nBitsInBuffer = 0;
			nBuffer = 0;
		}
	}
	//Write out whatever is left in the buffer
	while (nBitsInBuffer > 0) {
		*szOut++ = nBuffer & 0xFF;
		nBuffer >>= 8;
		nBitsInBuffer -= 8;
	}
}

//uncomp()
//nCompressedLen is the number of bytes in the compressed buffer.
//NOTE: the compressed buffer does not have a NULL terminating character
void Uncompress(char *szOut, const unsigned char *szCompressed, unsigned nCompressedLen) {
	unsigned long long nBuffer = 0; //This is enough to store 8 uncompressed characters or 9 compressed. We will only use 8 tho.
	char nBitsInBuffer = 0;
	while (nCompressedLen) {
		while (nCompressedLen && nBitsInBuffer < 7 * 8) {
			nBuffer |= (unsigned long long)*szCompressed++ << nBitsInBuffer;
			nBitsInBuffer += 8;
			--nCompressedLen;
		}
		while (nBitsInBuffer > 0) {
			*szOut++ = nBuffer & 0x7F;
			nBuffer >>= 7;
			nBitsInBuffer -= 7;
		}
		//The following should be redundant, but just to be safe
		nBitsInBuffer = 0;
		nBuffer = 0;
	}
}

SdFat SD;

File file;

Adafruit_SSD1306 OLED(128, 64, &Wire, -1);

void setup() {
  OLED.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  OLED.clearDisplay();
  OLED.display();
  Serial.begin(115200);
  
  Serial.println("Initializing SD card...");
  if (!SD.begin(53)) {
    Serial.println("COULD NOT INITIALIZE SD CARD");
    return;
  }

  Serial.println("Loading file...");
  file = SD.open("video.rawr", FILE_READ);
  if (!file){
    Serial.print("COULD NOT LOAD FILE");
    return;
  }
  
  Serial.println("Done.");
  while (1){
    displayFrame();
  }
}
void logger(char* message, ...)
{
  char tmpPrintfBuffer[4096];
  sprintf(tmpPrintfBuffer, message);
  Serial.write(tmpPrintfBuffer);
}

void loop() {
  // Do Nothing...
}
int frame=0;
void displayFrame(void)
{
    for (int y = 0; y < 64; y+=2){
      for (int x = 0; x < 128; x+=4){
      switch(file.read()) {
        case 0x2E:
          OLED.drawFastHLine(x, y, 4, WHITE);
          OLED.drawFastHLine(x, y+1, 4, WHITE);
					//OLED.drawPixel(x,y, WHITE);	

          break;
        case 0x20:
        	OLED.drawFastHLine(x, y, 4, BLACK);
          OLED.drawFastHLine(x, y+1, 4, BLACK);
					//OLED.drawPixel(x,y, BLACK);
          break;
        }
      }
    }
  OLED.display();
	delayMicroseconds(6000);
}