/* ------------------------------------------------------------------------------------
 *  Testing a way to efficiently handle block graphics
 *
 *  Ed: This will have to wait until we get an option for C++11 style bitset<>
 *  GCC 4.7
 *
 *  Auth:     EA
 *  Last rev: 2024-01-21
 * ------------------------------------------------------------------------------------
*/ 

#include <TinyWireM.h>
#include "SSD1306_config.h"

#define GRID_WIDTH 32
#define GRID_HEIGHT 2

byte brick[4] = {
  0b00000000,
  0b00001111,
  0b11110000,
  0b11111111
};


//std::bitset<32> row1(string("11100000000000001000000100000001");


byte grid[2][32] = {
  { 0x27, 0x41, 0x85, 0x00, 0x81, 0x80, 0x83, 0x80, 0x07, 0x00, 0x0f, 0xe0, 0x21, 0x21, 0xc1, 0x01, 0x02, 0x02, 0x02, 0x82, 0x84, 0x84, 0x04, 0x04, 0xe0, 0x20, 0x20, 0x20, 0x40, 0x85, 0x01, 0x07 },
  { 0xe2, 0x81, 0xa0, 0x00, 0x0c, 0xd2, 0x52, 0xd2, 0x1f, 0xc0, 0x40, 0x5f, 0xd1, 0x11, 0xd1, 0x4e, 0x40, 0x40, 0xcf, 0x10, 0xd0, 0x50, 0x40, 0x40, 0x5f, 0xd0, 0x10, 0xd0, 0x08, 0xa7, 0x80, 0xe0 }
};


// My plan fails! Stupid bool matrices taking up more memory than they should.
bool grid_full[16][32] = {

  { 1,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,   1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1 },
  { 1,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,   1,0,0,0,0,0,1,0,1,0,0,0,0,0,0,1 },
  { 1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,   1,0,0,0,0,1,0,0,0,1,0,0,0,0,0,1 },
  { 0,0,2,1,0,0,0,0,0,0,0,0,0,0,0,0,   1,0,0,0,1,0,0,0,0,0,1,0,0,0,0,1 },
  { 0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,   1,0,0,1,0,0,0,0,0,0,0,1,0,0,0,1 },
  { 0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,   1,0,1,0,0,0,0,0,0,0,0,0,1,0,0,1 },
  { 0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,   1,1,0,0,0,0,0,0,0,0,0,0,0,1,0,1 },
  { 0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,   1,0,1,1,0,0,1,1,0,0,1,1,0,0,1,1 },

  { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1 },
  { 1,0,0,0,0,0,0,0,0,0,0,0,1,0,0,1,   1,0,0,0,0,0,0,1,1,0,0,0,0,0,0,1 },
  { 1,0,0,0,0,0,0,0,0,0,0,1,0,0,0,1,   1,0,0,0,0,0,1,0,0,1,0,0,0,0,0,1 },
  { 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,1,   1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1 },
  { 1,0,0,0,0,0,0,0,0,1,0,0,0,0,0,1,   1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1 },
  { 1,0,0,0,0,0,0,0,1,0,0,0,0,0,0,1,   1,0,0,0,0,0,1,0,0,1,0,0,0,0,0,1 },
  { 1,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,   1,0,0,0,0,0,0,1,1,0,0,0,0,0,0,1 },
  { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,   1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,1 },
};


bool grid_small[8][16] = {
  { 1,1,1,0,0,0,0,1,1,1,1,1,1,1,1,1 },
  { 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1 },
  { 1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,1 },
  { 1,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1 },
  { 1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,1 },
  { 1,0,1,1,0,0,0,1,1,0,1,1,0,1,0,1 },
  { 1,0,1,1,1,0,0,0,0,0,0,0,0,0,0,1 },
  { 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1 }
};

byte digits[10][4] =
{
	{ 0x7e, 0x81, 0x81, 0x7e },
	{ 0x82, 0xff, 0x80, 0x00 },
	{ 0xc2, 0xa1, 0x91, 0x8f },
	{ 0x42, 0x91, 0x91, 0x6e },
	{ 0x1f, 0x10, 0x10, 0xff },
	{ 0x4f, 0x91, 0x91, 0x61 },
	{ 0x7c, 0x92, 0x91, 0x61 },
	{ 0x01, 0xf1, 0x09, 0x07 },
	{ 0x6e, 0x91, 0x91, 0x6e },
	{ 0x0e, 0x91, 0x51, 0x3e }
};


void setup() {
  TinyWireM.begin();
  config_display();
}


void loop() {
  byte slice = 0b00000000;
  byte index = 0;
  byte page = 0;
  
  for (byte y=0; y<GRID_HEIGHT; y++) {
    for (byte p=0; p<8; p+=2) {
      cursorTo(0, page);
      for (byte x=0; x<GRID_WIDTH; x++) {
          index = 0;
          if (bit_check(grid[y][x], p)) 
            index |= 0b00000001;
          if (bit_check(grid[y][x], p+1)) 
            index |= 0b00000010;
          slice = brick[index];

          // Four pixels wide
          send_data(slice);
          send_data(slice);
          send_data(slice);
          send_data(slice);
        }
      page++;
      }
  }

  delay(99999);
}


void drawAll() {

}


void cursorTo(byte x, byte page) {
  send_command(0x22);
  send_command(page);
  send_command(7);
  send_command(0x21);
  send_command(x);
  send_command(127);
}



void config_display() {
  for(int i=0; i<sizeof(initializeCmds); i++) {
    send_command(pgm_read_byte(initializeCmds+i));
  }
}

void send_command(unsigned char cmd) {
  TinyWireM.beginTransmission(0x3c);
  TinyWireM.write(0x00);
  TinyWireM.write(cmd);
  TinyWireM.endTransmission();
}

void send_data(byte slice) {
  TinyWireM.beginTransmission(0x3C);
  TinyWireM.write(0x40);
  TinyWireM.write(slice);
  TinyWireM.endTransmission();
}

/*
  send_data(0b11000011);
  send_data(0b00111100);
  send_data(0b00111100);
  send_data(0b11000011);
  send_data(0b00111100);
  send_data(0b00111100);
  send_data(0b11000011);
*/

void displayNumberAt(byte x, byte y, int number) {
  // count number of digits 
  int c = 0; // digit position 
  int n = number;

	cursorTo(x, y);

  while (n != 0) {
      n /= 10;
      c++;
  }
	if (number == 0) {
		// pgm_read_byte
		// pgm_read_byte( &(digits[number][i]) );
		send_data( pgm_read_byte( &(digits[0][0])) );
		send_data( pgm_read_byte( &(digits[0][1]))  );
		send_data( pgm_read_byte( &(digits[0][2]))  );
		send_data( pgm_read_byte( &(digits[0][3]))  );
		send_data( 0x00 );		
		return;
	}

  int numberArray[c];
  c = 0;    
  n = number;

  // extract each digit 
  while (n != 0) {
      numberArray[c] = n % 10;
      n /= 10;
      c++;
  }

	//for (int i=0; i<c; i++) {
	for (int i=c-1; i>=0; i--) {
		byte digit = numberArray[i];
		send_data( pgm_read_byte( &(digits[digit][0])) );
		send_data( pgm_read_byte( &(digits[digit][1]))  );
		send_data( pgm_read_byte( &(digits[digit][2]))  );
		send_data( pgm_read_byte( &(digits[digit][3]))  );
		if (i > 0)
			send_data( 0x00 );		
	}

}


inline byte bit_set(byte number, byte n) {
    return number | ((byte)1 << n);
}

inline bool bit_check(byte number, byte n) {
    return (number >> n) & (byte)1;
}

inline byte bit_set_to(byte number, byte n, bool x) {
    return (number & ~((byte)1 << n)) | ((byte)x << n);
}

inline byte bit_toggle(byte number, byte n) {
    return number ^ ((byte)1 << n);
}

inline byte bit_clear(byte number, byte n) {
    return number & ~((byte)1 << n);
}