#include <avr/pgmspace.h>

#include "map.h"
#include "textures.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 = 1;
int8_t y_player_pos = 1;
uint16_t collected = 0;

// some globals
static byte oled_addr;
static void oledWriteCommand(unsigned char c);

#define DIRECT_PORT
#define I2CPORT PORTB
// A bit set to 1 in the DDR is an output, 0 is an INPUT
#define I2CDDR DDRB

// Pin or port numbers for SDA and SCL
#define BB_SDA 0
#define BB_SCL 2

#define I2C_CLK_LOW() I2CPORT &= ~(1 << BB_SCL) //compiles to cbi instruction taking 2 clock cycles, extending the clock pulse

//
// Transmit a byte and ack bit
//
static inline void i2cByteOut(byte b)
{
byte i;
byte bOld = I2CPORT & ~((1 << BB_SDA) | (1 << BB_SCL));
     for (i=0; i<8; i++)
     {
         bOld &= ~(1 << BB_SDA);
         if (b & 0x80)
            bOld |= (1 << BB_SDA);
         I2CPORT = bOld;
         I2CPORT |= (1 << BB_SCL);
         I2C_CLK_LOW();
         b <<= 1;
     } // for i
// ack bit
  I2CPORT = bOld & ~(1 << BB_SDA); // set data low
  I2CPORT |= (1 << BB_SCL); // toggle clock
  I2C_CLK_LOW();
} /* i2cByteOut() */

void i2cBegin(byte addr)
{
   I2CPORT |= ((1 << BB_SDA) + (1 << BB_SCL));
   I2CDDR |= ((1 << BB_SDA) + (1 << BB_SCL));
   I2CPORT &= ~(1 << BB_SDA); // data line low first
   I2CPORT &= ~(1 << BB_SCL); // then clock line low is a START signal
   i2cByteOut(addr << 1); // send the slave address
} /* i2cBegin() */

void i2cWrite(byte *pData, byte bLen)
{
byte i, b;
byte bOld = I2CPORT & ~((1 << BB_SDA) | (1 << BB_SCL));

   while (bLen--)
   {
      b = *pData++;
      if (b == 0 || b == 0xff) // special case can save time
      {
         bOld &= ~(1 << BB_SDA);
         if (b & 0x80)
            bOld |= (1 << BB_SDA);
         I2CPORT = bOld;
         for (i=0; i<8; i++)
         {
            I2CPORT |= (1 << BB_SCL); // just toggle SCL, SDA stays the same
            I2C_CLK_LOW();
         } // for i    
     }
     else // normal byte needs every bit tested
     {
        for (i=0; i<8; i++)
        {
          
         bOld &= ~(1 << BB_SDA);
         if (b & 0x80)
            bOld |= (1 << BB_SDA);

         I2CPORT = bOld;
         I2CPORT |= (1 << BB_SCL);
         I2C_CLK_LOW();
         b <<= 1;
        } // for i
     }
// ACK bit seems to need to be set to 0, but SDA line doesn't need to be tri-state
      I2CPORT &= ~(1 << BB_SDA);
      I2CPORT |= (1 << BB_SCL); // toggle clock
      I2CPORT &= ~(1 << BB_SCL);
   } // for each byte
} /* i2cWrite() */

//
// Send I2C STOP condition
//
void i2cEnd()
{
  I2CPORT &= ~(1 << BB_SDA);
  I2CPORT |= (1 << BB_SCL);
  I2CPORT |= (1 << BB_SDA);
  I2CDDR &= ~((1 << BB_SDA) | (1 << BB_SCL)); // let the lines float (tri-state)
} /* i2cEnd() */

// Wrapper function to write I2C data on Arduino
static void I2CWrite(int iAddr, unsigned char *pData, int iLen)
{
  i2cBegin(oled_addr);
  i2cWrite(pData, iLen);
  i2cEnd();
} /* I2CWrite() */

//
// Initializes the OLED controller into "page mode"
//
void oledInit(byte bAddr, int bFlip, int bInvert)
{
unsigned char uc[4];
unsigned char oled_initbuf[]={0x00,0xae,0xa8,0x3f,0xd3,0x00,0x40,0xa1,0xc8,
      0xda,0x12,0x81,0xff,0xa4,0xa6,0xd5,0x80,0x8d,0x14,
      0xaf,0x20,0x02};

  oled_addr = bAddr;
  I2CDDR &= ~(1 << BB_SDA);
  I2CDDR &= ~(1 << BB_SCL); // let them float high
  I2CPORT |= (1 << BB_SDA); // set both lines to get pulled up
  I2CPORT |= (1 << BB_SCL);
  
  I2CWrite(oled_addr, oled_initbuf, sizeof(oled_initbuf));
  if (bInvert)
  {
    uc[0] = 0; // command
    uc[1] = 0xa7; // invert command
    I2CWrite(oled_addr, uc, 2);
  }
  if (bFlip) // rotate display 180
  {
    uc[0] = 0; // command
    uc[1] = 0xa0;
    I2CWrite(oled_addr, uc, 2);
    uc[1] = 0xc0;
    I2CWrite(oled_addr, uc, 2);
  }
} /* oledInit() */
//
// Sends a command to turn off the OLED display
//
void oledShutdown()
{
    oledWriteCommand(0xaE); // turn off OLED
}

// Send a single byte command to the OLED controller
static void oledWriteCommand(unsigned char c)
{
unsigned char buf[2];

  buf[0] = 0x00; // command introducer
  buf[1] = c;
  I2CWrite(oled_addr, buf, 2);
} /* oledWriteCommand() */

static void oledWriteCommand2(unsigned char c, unsigned char d)
{
unsigned char buf[3];

  buf[0] = 0x00;
  buf[1] = c;
  buf[2] = d;
  I2CWrite(oled_addr, buf, 3);
} /* oledWriteCommand2() */

//
// Sets the brightness (0=off, 255=brightest)
//
void oledSetContrast(unsigned char ucContrast)
{
  oledWriteCommand2(0x81, ucContrast);
} /* oledSetContrast() */

//
// Send commands to position the "cursor" (aka memory write address)
// to the given row and column
//
static void oledSetPosition(int x, int y)
{
  oledWriteCommand(0xb0 | y); // go to page Y
  oledWriteCommand(0x00 | (x & 0xf)); // // lower col addr
  oledWriteCommand(0x10 | ((x >> 4) & 0xf)); // upper col addr
}

//
// Write a block of pixel data to the OLED
// Length can be anything from 1 to 1024 (whole display)
//
static void oledWriteDataBlock(unsigned char *ucBuf, int iLen)
{
unsigned char ucTemp[17];

  ucTemp[0] = 0x40; // data command
  memcpy(&ucTemp[1], ucBuf, iLen);
  I2CWrite(oled_addr, ucTemp, iLen+1);
  // Keep a copy in local buffer
}

//
// Fill the frame buffer with a byte pattern
// e.g. all off (0x00) or all on (0xff)
//
void oledFill(unsigned char ucData)
{
int x, y;
unsigned char temp[8];

  memset(temp, ucData, 8);
  for (y=0; y<8; y++)
  {
    oledSetPosition(2,y); // set to (0,Y)
    for (x=0; x<16; x++)
    {
      oledWriteDataBlock(temp, 8); 
    } // for x
  } // for y

} /* oledFill() */

void setup()
{
  // put your setup code here, to run once:
  delay(200); // wait for the OLED to fully power up
  oledInit(0x3c, 0, 0);
  pinMode(PIN_LEFT, INPUT_PULLUP);
  pinMode(PIN_RIGHT, INPUT_PULLUP);
  pinMode(PIN_UP, INPUT_PULLUP);
  pinMode(PIN_DOWN, INPUT_PULLUP);
}

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

  bpos = y_pos * 16 + (x_pos >> 1);
  mapb = pgm_read_byte(maze_map + bpos);
  bitindex = (x_pos & 1) ? 0 : 4;
  return (mapb >> bitindex) & 0x0f;
}

void draw_map(int8_t xo, int8_t yo)
{
  uint8_t x, y, i;
  uint8_t buf[8];

  for (y=0; y<8; y++)
  {
    oledSetPosition(2,y); // set to (0,Y)
    for (x=0; x<16; x++)
    {
      //uint8_t sx = x * 8;
      //uint8_t sy = y;
      uint8_t xm = xo + x;
      uint8_t ym = yo + y;
      uint8_t map_element;
      if (xm == x_player_pos && ym == y_player_pos)
      {
        map_element = 16;
      }
      else
      {
        map_element = get_map_element(xm, ym);
      }
      if (collected & (1 << map_element))
      {
        map_element = 0;
      }
      //oled.bitmap(sx, sy, sx + 8, sy + 1, img_blocks + map_element * 8);
      for (i=0; i<8; i++)
      {
        buf[i] = pgm_read_byte(img_blocks + map_element * 8 + i);
      }
      oledWriteDataBlock(buf, 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)
    {
      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);
      int8_t new_x_player_pos = x_player_pos;
      int8_t new_y_player_pos = y_player_pos;
      if (dir_l && x_player_pos > 0)
      {
        new_x_player_pos--;
      }
      if (dir_r && x_player_pos < 31)
      {
        new_x_player_pos++;
      }
      if (dir_u && y_player_pos > 0)
      {
        new_y_player_pos--;
      }
      if (dir_d && y_player_pos < 15)
      {
        new_y_player_pos++;
      }
      int8_t field = get_map_element(new_x_player_pos, new_y_player_pos);
      if (field == 0 || field >=6)
      {
        //Debug.println(x_player_pos);
        lastPress = millis();
        changed = 1;
        x_player_pos = new_x_player_pos;
        y_player_pos = new_y_player_pos;
        if (field >= 6)
        {
          collected |= (1<<field);
        }

        if (x_player_pos < x_frame_pos + 1)
        {
          x_frame_pos = x_player_pos - 1;
        }
        if (x_player_pos > x_frame_pos + 14)
        {
          x_frame_pos = x_player_pos - 14;
        }
        if (y_player_pos < y_frame_pos + 1)
        {
          y_frame_pos = y_player_pos - 1;
        }
        if (y_player_pos > y_frame_pos + 6)
        {
          y_frame_pos = y_player_pos - 6;
        }
      }

      //Debug.print(x_player_pos);
      //Debug.print(F(" "));
      //Debug.println(y_player_pos);
    }
  }
}


void loop__()
{
uint8_t pin;

  //pin = 3*digitalRead(PIN_LEFT) + 12*digitalRead(PIN_RIGHT) + 48*digitalRead(PIN_UP) + 192*digitalRead(PIN_DOWN);
  pin = millis();
  oledFill(pin);
  
  delay(100);
}
