#include <SPI.h>
#include <GD.h>
#include "debug.h"

#include "logo.h"

class GDflashbits {
public:
  void begin(prog_uchar *s) {
    src = s;
    mask = 0x01;
  }
  byte get1(void) {
    byte r = (pgm_read_byte_near(src) & mask) != 0;
    mask <<= 1;
    if (!mask) {
      mask = 1;
      src++;
    }
    return r;
  }
  unsigned short getn(byte n) {
    unsigned short r = 0;
    while (n--) {
      r <<= 1;
      r |= get1();
    }
    return r;
  }
private:
  prog_uchar *src;
  byte mask;
};

static GDflashbits GDFB;

static void GD_uncompress(unsigned int addr, PROGMEM prog_uchar *src)
{
  GDFB.begin(src);
  byte b_off = GDFB.getn(4);
  byte b_len = GDFB.getn(4);
  byte minlen = GDFB.getn(2);
  unsigned short items = GDFB.getn(16);
  while (items--) {
    if (GDFB.get1() == 0) {
      GD.wr(addr++, GDFB.getn(8));
    } else {
      int offset = -GDFB.getn(b_off) - 1;
      int l = GDFB.getn(b_len) + minlen;
      while (l--) {
        GD.wr(addr, GD.rd(addr + offset));
        addr++;
      }
    }
  }
}

#include "palcopy.h"

void setup()
{
  GD.begin();

  // GD.copy(RAM_PIC, logo_pic, sizeof(logo_pic));
  // GD.copy(RAM_CHR, logo_chr, sizeof(logo_chr));
  // GD.copy(RAM_PAL, logo_pal, sizeof(logo_pal));
  GD.ascii();
  GD.putstr(0, 30, "MANUFACTURING TEST ROM");
  GD.putstr(0, 32, "NOT FOT SALE");
  GD.putstr(0, 34, "(C) gameduino.com 2011");

  GD.copy(RAM_SPR, sprvals, sizeof(sprvals));
  GD_uncompress(RAM_SPRIMG, sprimg_compressed);

  // Use the first two 256-color palettes as pseudo-16 color palettes
  int i;
  for (i = 0; i < 256; i++) {
    // palette 0 decodes low nibble, hence (i & 15)
    uint16_t rgb = pgm_read_word_near(palette16a + ((i & 15) << 1));
    GD.wr16(RAM_SPRPAL + (i << 1), rgb);

    // palette 1 decodes nigh nibble, hence (i >> 4)
    rgb = pgm_read_word_near(palette16a + ((i >> 4) << 1));
    GD.wr16(RAM_SPRPAL + 512 + (i << 1), rgb);
  }

  Serial.begin(1000000);
  dump(0, 32768);
}

void loop()
{
}
