#include <GD.h>

#include "r.h"

void setpixel(byte x, byte y)
{
  byte imgy = 3 & (y >> 4);
  byte imgx = (x >> 4);
  byte img = (imgy << 4) | imgx;
  byte subx = (x & 0xf);
  byte suby = (y & 0xf);

  unsigned int addr = RAM_SPRIMG + (img << 8) + (suby << 4) + subx;
  byte mask = 0x03 << (2 * (y >> 6));
  GD.wr(addr, GD.rd(addr) | mask);
}

void line(byte x0, byte y0, byte x1, byte y1)
{
  byte swap;
#define SWAP(a, b) (swap = (a), (a) = (b), (b) = swap)

  byte steep = abs(y1 - y0) > abs(x1 - x0);
  if (steep) {
    SWAP(x0, y0);
    SWAP(x1, y1);
  }
  if (x0 > x1) {
    SWAP(x0, x1);
    SWAP(y0, y1);
  }
  int deltax = x1 - x0;
  int deltay = abs(y1 - y0);
  int error = deltax / 2;
  char ystep;
  if (y0 < y1)  
    ystep = 1;
  else
    ystep = -1;
  byte x;
  byte y = y0;
  for (x = x0; x < x1; x++) {
    if (steep)
      setpixel(y, x);
    else
      setpixel(x, y);
    error -= deltay;
    if (error < 0) {
      y += ystep;
      error += deltax;
    }
  }
}

static char asteroid_v[] = {
 0x00,0x50,0x00,
 -0x50,-0x0A,0x00,
 0x00,-0x50,0x00,
 0x46,-0x28,0x00,
 0x3C,0x32,0x00,
 0x32,0x00,0x3C,
 -0x28,0x00,0x46,
 0x00,0x1E,-0x4B,
 0x00,-0x32,-0x3C,
};
static byte asteroid_f[] = {
 0x09,0x42,0x51,3,6,0,5,
 0x09,-0x42,0x51,3,6,5,2,
 -0x48,0x40,0x1F,3,6,0,1,
 -0x40,-0x49,0x2F,3,2,6,1,
 0x2D,-0x4F,0x41,3,3,5,2,
 0x87,0x0F,0x23,3,4,5,3,
 0x26,0x4C,0x46,3,4,5,0,
 -0x42,0x3B,-0x27,3,7,0,1,
 -0x43,-0x0F,-0x50,3,8,7,1,
 0x42,-0x0E,-0x4B,3,7,8,3,
 -0x46,-0x50,-0x28,3,2,8,1,
 0x3A,-0x66,-0x33,3,8,2,3,
 0x51,0x09,-0x43,3,7,3,4,
 0x2F,0x5E,-0x3F,3,4,7,0,
 0,
};

int nlines;
static byte x0, y0, x1, y1; // endpoints of random lines

void setup()
{
  int i;

  GD.begin();
  
  Serial.begin(115200);
  Serial.println("");
  Serial.println("");

  GD.ascii();
  GD.fill(0, ' ', 4096);

  GD.putstr(0, 0,"Bitmap demonstration");

  GD.wr16(PALETTE4A, RGB(0,0,0));
  GD.wr16(PALETTE4A + 2, RGB(0,255,0));
  GD.wr16(PALETTE4A + 4, RGB(255,0,0));
  GD.wr16(PALETTE4A + 6, RGB(255,255,255));
  for (i = 0; i < 256; i++) {
    int x =     72 + 16 * (i & 15);
    int y =     22 + 16 * ((i >> 4) & 15);
    int image = i & 63;     /* image 0-63 */
    int pal =   (i >> 6);   /* palette 0-4 */
    GD.sprite(i, x, y, image, 0x8 | (pal << 1), 0);
  }
  if (0)
    for (i = 0; i < 16384; i++)
      GD.wr(RAM_SPRIMG + i, random(255));

  GD.fill(RAM_SPRIMG, 0, 16384);
}

long started;

static byte points[20];

static float th = 0.1;

static int visible(char *normal)
{
  char sinth = int(64 * sin(th));
  char costh = int(64 * cos(th));
  char x, y, z;
  x = *normal++;
  y = *normal++;
  z = *normal++;
  int zt = (x * sinth) + (z * costh);
  return zt > 0;
}

static void project(byte npoints, char *pts3d)
{
  byte i;
  char sinth = int(64 * sin(th));
  char costh = int(64 * cos(th));
  for (i = 0; i < npoints; i++) {
    char x, y, z;
    x = *pts3d++;
    y = *pts3d++;
    z = *pts3d++;
    char xt = ((x * costh) - (z * sinth)) >> 6;
    char yt = y;
    char zt = ((x * sinth) + (z * costh)) >> 6;
    points[(i << 1) + 0] = 128 + xt;
    points[(i << 1) + 1] = 128 + yt;
  }
}

static void draw_asteroid()
{
  byte i;
  byte *pf = asteroid_f;
  byte numvertices;
  char *normal;

  project(sizeof(asteroid_v) / 3, asteroid_v);
  while (*pf) {
    normal = (char*)pf;
    pf += 3;
    numvertices = *pf++;

    if (visible(normal)) {
      byte vx = *pf++;
      x1 = points[2 * vx + 0];
      y1 = points[2 * vx + 1];
      byte xa = x1, ya = y1;
      for (i = 1; i < numvertices; i++) {
        byte vx = *pf++;
        x0 = x1, y0 = y1;
        x1 = points[2 * vx + 0];
        y1 = points[2 * vx + 1];
        line(x0, y0, x1, y1);
      }
      line(x1, y1, xa, ya);
    } else {
      pf += numvertices;
    }
  }
}

void loop()
{
  GD.fill(RAM_SPRIMG, 0, 16384);
  draw_asteroid();
  delay(100);
  th += 0.05;
}
