#include <TinyWireM.h>
#include <avr/pgmspace.h>
//#include <Tiny4kOLED.h>
#include <TinyDebug.h>
#include <Tiny4kOLED_bitbang.h>


#define PIN_LEFT 5
#define PIN_RIGHT 3
#define PIN_UP 1
#define PIN_DOWN 4

int8_t x_frame_pos = 0;
int8_t y_frame_pos = 0;
int8_t x_player_pos = 0;
int8_t y_player_pos = 0;

const uint8_t  img_blocks[] PROGMEM =
{  //Real image is got if this is rotated 90 deg CCW
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,
  0b00000000,

  0b00010000,
  0b00000000,
  0b00010000,
  0b10000100,
  0b00001000,
  0b10000000,
  0b00010000,
  0b00100000,

  0b00100010,
  0b00000100,
  0b00100001,
  0b01110000,
  0b00101000,
  0b00001000,
  0b10001100,
  0b00101010,

  0b01001000,
  0b11011100,
  0b01010011,
  0b11100110,
  0b11010101,
  0b10100100,
  0b01011100,
  0b10001100,

  0b11111111,
  0b11110010,
  0b11011110,
  0b11111111,
  0b01111101,
  0b01111101,
  0b11111111,
  0b11111111,

};

void setup()
{
  Debug.begin();
  Debug.println(F("Hello, TinyDebug!"));
  oled.begin(128, 64, sizeof(tiny4koled_init_128x64br), tiny4koled_init_128x64br);

  // Two fonts are supplied with this library, FONT8X16 and FONT6X8
  
  oled.setFont(FONT6X8);

  // To clear all the memory
  oled.clear();
  oled.on();

  delay(200);
  
}

const uint8_t maze_map[] PROGMEM =
{
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b01111101, 0b00010101, 0b11110100, 0b01001000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00010000, 0b10100100, 0b01000010, 0b10010100, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00010000, 0b01000100, 0b01000001, 0b00111110, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00010000, 0b01000100, 0b01000001, 0b00100010, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00010000, 0b01000100, 0b01000001, 0b00100010, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000, 0b00000000,
};

int8_t get_map_element(int8_t x_pos, int8_t y_pos)
{
  uint8_t bpos, mapb, bitindex;

  pinMode(PIN_LEFT, INPUT_PULLUP);
  pinMode(PIN_RIGHT, INPUT_PULLUP);
  pinMode(PIN_UP, INPUT_PULLUP);
  pinMode(PIN_DOWN, INPUT_PULLUP);

  bpos = y_pos * 8 + (x_pos >> 3);
  mapb = pgm_read_byte(maze_map + bpos);
  bitindex = 7 - (x_pos & 0b111);
  return (mapb >> bitindex) & 1;
}

void draw_map(int8_t xo, int8_t yo)
{
  uint8_t x, y;

  for (y=0; y<8; y++)
  {
    for (x=0; x<16; x++)
    {
      uint8_t sx = x * 8;
      uint8_t sy = y;
      uint8_t map_element = get_map_element(x + xo, y + yo) * 2;
      if (map_element)
      {
        if (x < 2 || x > 13 || y < 2 || y > 5)
        {
          map_element = 1;
        }
      }
      oled.bitmap(sx, sy, sx + 8, sy + 1, img_blocks + map_element * 8);
    }
  }

}

void loop()
{
  uint8_t changed = 1;
  uint32_t lastPress = 0;

  while (1)
  {
    uint8_t dir_l = 0, dir_r = 0, dir_u = 0, dir_d = 0;
    if (changed)
    {
      x_frame_pos = x_player_pos;
      y_frame_pos = y_player_pos;
      draw_map(x_frame_pos, y_frame_pos);
    }
    changed = 0;

    if (millis() - lastPress > 130)
    {
      dir_l = 1 - digitalRead(PIN_LEFT);
      dir_r = 1 - digitalRead(PIN_RIGHT);
      dir_u = 1 - digitalRead(PIN_UP);
      dir_d = 1 - digitalRead(PIN_DOWN);
      if (dir_l && x_player_pos > 0)
      {
        x_player_pos--;
        changed = 1;
        lastPress = millis();
      }
      if (dir_r && x_player_pos < 100)
      {
        x_player_pos++;
        changed = 1;
        lastPress = millis();
      }
      if (dir_d && y_player_pos > 0)
      {
        y_player_pos--;
        changed = 1;
        lastPress = millis();
      }
      if (dir_u && y_player_pos < 30)
      {
        y_player_pos++;
        changed = 1;
        lastPress = millis();
      }
      Debug.print(x_player_pos);
      Debug.print(F(" "));
      Debug.println(y_player_pos);
    }
  }
}