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

int atxy(int x, int y)
{
  return (y << 6) + x;
}

#define FAIL do { Serial.print("Fail at line: "); Serial.println(__LINE__, DEC); return 0; } while (0)
#define PASS return 1

static void log(const char*s)
{
  Serial.print(s);
}

#define BLACK RGB(0,0,0)
#define RED RGB(255,0,0)
#define GREEN RGB(0,255,0)

// Take a snapshot of screen line yy (0 <= yy < 300)
// After this function, SCREENSHOT contains the 400 pixels

static int snapline(int yy)
{
  GD.wr16(SCREENSHOT_Y, 2 * yy + 2);
  GD.waitvblank();
  GD.waitvblank();
  GD.wr16(SCREENSHOT_Y, 1023);    // disable
}

static int allgreen(void)
{
  int xx, yy;

  for (yy = 2; yy < 602; yy += 2) {
    GD.wr16(SCREENSHOT_Y, yy);
    GD.waitvblank();
    GD.waitvblank();
    GD.wr16(SCREENSHOT_Y, 1023);

    for (xx = 0; xx < 800; xx += 2) {
      uint16_t v = GD.rd16(SCREENSHOT + xx);
      if (v != GREEN) {
        Serial.println(yy, DEC);
        return 0;
      }
    }
  }
  return 1;
}

static void green_1x1(void)
{
  // image 0 is a green pixel at top left
  GD.fill(RAM_SPRIMG, 0, 256);
  GD.wr(RAM_SPRIMG, 1);
  GD.wr16(RAM_SPRPAL + 0, TRANSPARENT);
  GD.wr16(RAM_SPRPAL + 2, RGB(0, 255, 0));
}

// Verify screen display area

static int test_400x300(void)
{
  // All red pixels, except for 400x300 green pixels at (0,0)

  GD.wr16(RAM_PAL + 0, RED);    // char 0: red
  GD.wr16(RAM_PAL + 8, GREEN);  // char 1: green
  GD.wr16(RAM_PAL + 16, RED);   // char 2: 0=red, 3=green
  GD.wr16(RAM_PAL + 22, GREEN);

  GD.fill(0, 0, 4096);          // All red
  GD.fill(RAM_CHR, 0, 4096);    // All zeroes
  GD.fill(RAM_CHR + 32, 0xff, 8); // Top half 3=green

                              // Green full lines
  byte i;
  for (i = 0; i < 37; i++) {
    GD.fill(atxy(0, i), 1, 50);
  }
  GD.fill(atxy(0, 37), 2, 50);
  // GD.screenshot(0);
  // GD.fill(0, 1, 4096);          // All green

  if (!allgreen())
    FAIL;
  return 1;
}

#include "splitscreen.h"


static int test_splitscreen(void)
{
  GD.begin();

  GD.fill(0, 0, 4096);          // All red
  GD.wr16(RAM_PAL + 0, RED);    // char 0: red
  GD.wr16(RAM_PAL + 8, GREEN);  // char 1: green
  GD.fill(atxy(0, 50), 1, 64);  // lines 400-407

  GD.wr16(COMM+0, 0);
  GD.wr16(COMM+2, 0);
  GD.wr16(COMM+4, 199);   // split at line 100
  GD.wr16(COMM+6, 0);
  GD.wr16(COMM+8, 200);
  GD.wr16(COMM+10, 300);   // split at line 300
  GD.wr16(COMM+12, 0);
  GD.wr16(COMM+14, 0);

  GD.microcode(splitscreen_code, sizeof(splitscreen_code));

  int i;
  for (i = 0; i < 300; i++) {
    Serial.print(i, DEC);
    Serial.print(' ');
    snapline(i);
    Serial.println(GD.rd16(SCREENSHOT), DEC);
  }
  for (;;)
    ;

}

// test_coll: Collision detection around screen boundary

// Sprites at (x0, y0) and (x1, y1): returns true if colliding
static int iscollide(int x0, int y0, int x1, int y1)
{
  GD.sprite(0, x0, y0, 0, 0, 0);
  GD.sprite(1, x1, y1, 0, 0, 0);
  while (GD.rd(VBLANK) == 0)  // Wait until vblank
    ;
  while (GD.rd(VBLANK) == 1)  // Wait until display
    ;
  while (GD.rd(VBLANK) == 0)  // Wait until vblank
    ;
  byte coll = GD.rd(COLLISION + 1);
  return coll == 0;
}

// given two different points on screen, returns true if collisions are good
static int goodcollisions(int x0, int y0, int x1, int y1)
{
  return
    iscollide(x0, y0, x0, y0) &&
    iscollide(x1, y1, x1, y1) &&
    !iscollide(x0, y0, x1, y1) &&
    !iscollide(x1, y1, x0, y0);
}

static int test_coll(void)
{
  GD.begin();
  green_1x1();

  if (!goodcollisions(100, 200, 100, 201))
    FAIL;

  if (!goodcollisions(0, 0, 1, 0))
    FAIL;
  if (!goodcollisions(399, 0, 398, 0))
    FAIL;
  if (!goodcollisions(0, 299, 1, 299))
    FAIL;
  if (!goodcollisions(399, 299, 398, 299))
    FAIL;
  return 1;
}

// test_spr96: Verify that 96 sprites per line is true

static int test_spr96(void)
{
  GD.begin();
  GD.ascii();
  green_1x1();

  int y, i;
  int yi;
  int yys[] = { 0, 1, 16, 100, 298, 299 };

  for (yi = 0; yi < 6; yi++) {
    y = yys[yi];
    for (i = 0; i < 96; i++) {
      GD.sprite(i, 2 * i, y, 0, 0, 0);
    }
    snapline(y);
    for (i = 0; i < 400; i++) {
      unsigned short pix = GD.rd16(SCREENSHOT + 2 * i);
      unsigned short expected = ((i < (2 * 96)) && ((i & 1) == 0)) ? GREEN : BLACK;
      if (pix != expected)
        FAIL;
      // Serial.print(i, DEC);
      // Serial.print(" ");
      // Serial.println(pix, HEX);
    }
  }
}

#include "regressfreq.h"

static float measure_freq(void)
{
  long t0 = millis();
  byte p2 = digitalRead(2);
  long ticks = 0;
  long duration;
  do {
    byte d = digitalRead(2);
    if (d != p2) {
      p2 = d;
      ticks++;
    }
    duration = millis() - t0;
  } while (duration < 1000);
  return float(ticks) / (float(duration) / 1000.0);
}

// Return true if freq works

static int try_freq(uint16_t f)
{
  GD.wr16(COMM, f);
  GD.microcode(regressfreq_code, sizeof(regressfreq_code));
  return (uint16_t(measure_freq() + 0.5) == f);
}

static int test_freq(void)
{
  if (!try_freq(8000))
    FAIL;
}

#define RUNTEST(NAME) \
  do { \
  log(#NAME ":  "); \
  r = NAME(); \
  log(r ? "pass\n" : "FAIL\n"); \
  pass &= r; \
  } while (0)

static void runtests()
{
  GD.begin();
  
  Serial.begin(1000000);

  byte r, pass = 1;

  log("starting tests\n");
  RUNTEST(test_freq);
  // RUNTEST(test_splitscreen);
  // RUNTEST(test_coll);
  // RUNTEST(test_spr96);
  // RUNTEST(test_400x300);
}

void setup()
{
  int pass = 1;
  runtests();

  if (pass) {
    log("All tests passed\n");
  }
}

void loop()
{
}
