#include <poll.h>
#include <math.h>
#include <stdio.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <signal.h>
#include <unistd.h>
#include <strings.h>
#include <termios.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <machine/kbd.h>
#include <machine/fbio.h>
#include <machine/kbio.h>
#include <machine/vuid_event.h>

#include "cg6.h"

extern const char *__progname;

static const char *fbpath = "/dev/cgsix0";
static const char *kbpath = "/dev/kbd1";

static int fbfd;
static volatile struct cg6fb *fb;
static volatile struct brooktree *bt;
static volatile unsigned char *vram;
static struct fbcmap oldcm;
static unsigned char oldcm_r[256];
static unsigned char oldcm_g[256];
static unsigned char oldcm_b[256];
static int drawbit;

typedef struct xyz XYZ;
typedef struct xy XY;

struct xyz {
  double x;
  double y;
  double z;
  } ;

struct xy {
  double x;
  double y;
  } ;

static int kbdfd;
#define EVQ_SIZE 64 /* must be a power of two */
static int evq[EVQ_SIZE];
static int evqh;
static int evqt;
#define EV_TYPE  0x000f0000
#define EVT_NIL  0x00000000
#define EVT_KBD  0x00010000
#define EVK_CODE 0x0000007f /* must be low bits */
#define EVK_UPDN 0x00000080
#define EVK_UP   0x00000080
#define EVK_DN   0x00000000
#define EVT_TICK 0x00020000

static int curtime;
static volatile int rawtime;
static int timerunning;

static const XYZ xyz0 = { 0, 0, 0 };

static XYZ up;
static XYZ out;
static XYZ rt;
static XYZ lea;
static XYZ ler;
static XYZ rea;
static XYZ rer;
static double ed;
static double es;
static double ipd;
static int rotbits;
#define ROT_UP  0x01
#define ROT_LF  0x02
#define ROT_RT  0x04
#define ROT_DN  0x08
#define ROT_CW  0x10
#define ROT_CCW 0x20
static int wantrender;

static void savecmap(void)
{
 oldcm.index = 0;
 oldcm.count = 256;
 oldcm.red = &oldcm_r[0];
 oldcm.green = &oldcm_g[0];
 oldcm.blue = &oldcm_b[0];
 if (ioctl(fbfd,FBIOGETCMAP,&oldcm) < 0)
  { fprintf(stderr,"%s: FBIOGETCMAP: %s\n",__progname,strerror(errno));
    exit(1);
  }
}

static void setcmap(int bit)
{
 int i;
 int v;

 bt->addr = 0;
 for (i=0;i<256;i++)
  { v = bit ? (i >> 4) : (i & 15);
    bt->cmap = v * 0x11000000;
    bt->cmap = v * 0x11000000;
    bt->cmap = v * 0x11000000;
  }
}

static void initfb(void)
{
 void *mrv;
 struct fbgattr a;

 fbfd = open(fbpath,O_RDWR,0);
 if (fbfd < 0)
  { fprintf(stderr,"%s: can't open %s: %s\n",__progname,fbpath,strerror(errno));
    exit(1);
  }
 if ( (ioctl(fbfd,FBIOGATTR,&a) < 0) ||
      (a.fbtype.fb_type != FBTYPE_SUNFAST_COLOR) ||
      (a.fbtype.fb_width != 1152) ||
      (a.fbtype.fb_height != 900) ||
      (a.fbtype.fb_depth != 8) )
  { fprintf(stderr,"%s: %s isn't an 1152x900 cgsix\n",__progname,fbpath);
    exit(1);
  }
 savecmap();
 mrv = mmap(0,0x16000+a.fbtype.fb_size,PROT_READ|PROT_WRITE,MAP_SHARED,fbfd,0x70000000);
 if (mrv == MAP_FAILED)
  { fprintf(stderr,"%s: can't mmap %s: %s\n",__progname,fbpath,strerror(errno));
    exit(1);
  }
 fb = mrv;
 bt = (void *)(0x2000+(unsigned char *)mrv);
 vram = 0x16000 + (unsigned char *)mrv;
 setcmap(1);
 drawbit = 0;
 fb->bg = 0;
 fb->pixelm = ~0;
 fb->s = 0;
 fb->mode = MODE_NORMAL;
 fb->alu = ALU_NORMAL | ALU_FG;
 fb->clip = 0;
 fb->offx = 0;
 fb->offy = 0;
 fb->clipminx = 0;
 fb->clipminy = 0;
 fb->clipmaxx = 1151;
 fb->clipmaxy = 899;
 fb->fg = 0;
 fb->pm = ~0;
 fb->arecty = 0;
 fb->arectx = 0;
 fb->arecty = 900;
 fb->arectx = 1152;
 while (fb->s & 0x10000000) ;
 fb->clipminx = 0;
 fb->clipminy = 0;
 fb->clipmaxx = 1151;
 fb->clipmaxy = 899;
 fb->pm = 0x0f;
}

static void initkbd(void)
{
 int type;
 int mode;

 kbdfd = open(kbpath,O_RDWR,0);
 if (kbdfd < 0)
  { fprintf(stderr,"%s: can't open %s: %s\n",__progname,kbpath,strerror(errno));
    exit(1);
  }
 if ( (ioctl(kbdfd,KIOCTYPE,&type) < 0) ||
      (type != KB_SUN3) )
  { close(kbdfd);
    fprintf(stderr,"%s: %s isn't a type-3\n",__progname,kbpath);
    exit(1);
  }
 mode = 1;
 ioctl(kbdfd,KIOCSDIRECT,&mode);
 mode = TR_UNTRANS_EVENT;
 ioctl(kbdfd,KIOCTRANS,&mode);
 mode = 1;
 ioctl(kbdfd,FIONBIO,&mode);
}

static void initevq(void)
{
 evqh = 0;
 evqt = 0;
}

static void raw_tick(int sig __attribute__((__unused__)))
{
 rawtime ++;
}

static void starttiming(void)
{
 struct itimerval itv;

 if (timerunning) return;
 timerunning = 1;
 itv.it_value.tv_sec = 0;
 itv.it_value.tv_usec = 10000;
 itv.it_interval = itv.it_value;
 setitimer(ITIMER_REAL,&itv,0);
}

static void stoptiming(void)
{
 struct itimerval itv;

 if (! timerunning) return;
 timerunning = 0;
 itv.it_value.tv_sec = 0;
 itv.it_value.tv_usec = 0;
 itv.it_interval = itv.it_value;
 setitimer(ITIMER_REAL,&itv,0);
}

static void inittiming(void)
{
 struct sigaction sa;

 curtime = 0;
 rawtime = 0;
 sa.sa_handler = raw_tick;
 sigemptyset(&sa.sa_mask);
 sa.sa_flags = SA_RESTART;
 sigaction(SIGALRM,&sa,0);
 timerunning = 1;
 stoptiming();
}

static void queue_event(int ev)
{
 if (((evqh-evqt) & (EVQ_SIZE-1)) == (EVQ_SIZE-1))
  { write(1,"evq full\n",9);
  }
 else
  { evq[evqh] = ev;
    evqh = (evqh+1) & (EVQ_SIZE-1);
  }
}

static int dequeue_event(void)
{
 int ev;

 if (evqh == evqt) return(EVT_NIL);
 ev = evq[evqt];
 evqt = (evqt+1) & (EVQ_SIZE-1);
 return(ev);
}

static int checktick(void)
{
 int now;

 now = rawtime;
 if (curtime != now)
  { queue_event(EVT_TICK);
    curtime ++;
    return(1);
  }
 return(0);
}

static void fbdraw(int x1, int y1, int x2, int y2, int z)
{
 fb->fg = z * 0x11;
 fb->aliney = y1;
 fb->alinex = x1;
 fb->aliney = y2;
 fb->alinex = x2;
 while ((fb->draw & 0xa0000000) == 0xa0000000) ;
}

static double xyzlen(XYZ v)
{
 return(hypot(hypot(v.x,v.y),v.z));
}

static XYZ norm(XYZ v)
{
 double l;

 l = xyzlen(v);
 if (l < 1e-20)
  { return(xyz0);
  }
 v.x /= l;
 v.y /= l;
 v.z /= l;
 return(v);
}

static XYZ normcross(XYZ a, XYZ b)
{
 XYZ c;

 c.x = (a.y * b.z) - (a.z * b.y);
 c.y = (a.z * b.x) - (a.x * b.z);
 c.z = (a.x * b.y) - (a.y * b.x);
 return(norm(c));
}

static double dot(XYZ a, XYZ b)
{
 return((a.x*b.x)+(a.y*b.y)+(a.z*b.z));
}

static XYZ xyzscale(XYZ v, double s)
{
 v.x *= s;
 v.y *= s;
 v.z *= s;
 return(v);
}

static XYZ xyzsub(XYZ a, XYZ b)
{
 a.x -= b.x;
 a.y -= b.y;
 a.z -= b.z;
 return(a);
}

static XYZ xyzadd(XYZ a, XYZ b)
{
 a.x += b.x;
 a.y += b.y;
 a.z += b.z;
 return(a);
}

static XYZ rotunit(XYZ v, XYZ axis, double angle)
{
 return(xyzadd(xyzscale(v,cos(angle)),xyzscale(normcross(axis,v),sin(angle))));
}

static XY project(XYZ p, XYZ ea, XYZ er)
{
 XY s;
 double d;

 p = xyzsub(p,ea);
 d = dot(p,norm(ea));
 s.x = dot(p,er) / d;
 s.y = dot(p,up) / d;
 return(s);
}

static void draw3line(XYZ p1, XYZ p2, XYZ ea, XYZ er, int xo)
{
 XY p1s;
 XY p2s;

 p1s = project(p1,ea,er);
 p2s = project(p2,ea,er);
 fbdraw(288+xo+(p1s.x*es),450-(p1s.y*es),288+xo+(p2s.x*es),450-(p2s.y*es),15);
}

static void mayberender(void)
{
 if (! wantrender) return;
 wantrender = 0;
 while (fb->s & 0x10000000) ;
 fb->fg = 0;
 fb->arecty = 0;
 fb->arectx = 0;
 fb->arecty = 900;
 fb->arectx = 1152;
 while ((fb->draw & 0xa0000000) == 0xa0000000) ;
 rt = normcross(up,out);
 rea = xyzadd(xyzscale(out,ed),xyzscale(rt,ipd/2));
 rer = normcross(up,rea);
 lea = xyzadd(xyzscale(out,ed),xyzscale(rt,-ipd/2));
 ler = normcross(up,lea);
  { static const XYZ pts[] = { {  1,  1,  1 },		/* 0 */
			       {  1,  1, -1 },		/* 1 */
			       {  1, -1,  1 },		/* 2 */
			       {  1, -1, -1 },		/* 3 */
			       { -1,  1,  1 },		/* 4 */
			       { -1,  1, -1 },		/* 5 */
			       { -1, -1,  1 },		/* 6 */
			       { -1, -1, -1 },		/* 7 */
			       { 1, -.5, 0 },		/* 8 */
			       { 1, -.1, 0 },		/* 9 */
			       { 1, -.3, -.2 },		/* 10 */
			       { 1, -.3, .2 },		/* 11 */
			       { 1, .1, -.3 },		/* 12 */
			       { 1, .5, .3 },		/* 13 */
			       { 1, .1, .3 },		/* 14 */
			       { 1, .5, -.3 },		/* 15 */
			       { 0, 1, -.5 },		/* 16 */
			       { 0, 1, -.1 },		/* 17 */
			       { -.2, 1, -.3 },		/* 18 */
			       { .2, 1, -.3 },		/* 19 */
			       { .3, 1, .1 },		/* 20 */
			       { 0, 1, .3 },		/* 21 */
			       { .3, 1, .5 },		/* 22 */
			       { -.3, 1, .3 },		/* 23 */
			       { -.5, 0, 1 },		/* 24 */
			       { -.1, 0, 1 },		/* 25 */
			       { -.3, -.2, 1 },		/* 26 */
			       { -.3, .2, 1 },		/* 27 */
			       { .1, .3, 1 },		/* 28 */
			       { .5, .3, 1 },		/* 29 */
			       { .1, -.3, 1 },		/* 30 */
			       { .5, -.3, 1 },		/* 31 */ };
    static const int clx[][2] = { { 0, 1 },	/* cube */
				  { 0, 2 },
				  { 0, 4 },
				  { 7, 6 },
				  { 7, 5 },
				  { 7, 3 },
				  { 1, 3 },
				  { 3, 2 },
				  { 2, 6 },
				  { 6, 4 },
				  { 4, 5 },
				  { 5, 1 },
				  { 8, 9 },	/* +X */
				  { 10, 11 },
				  { 12, 13 },
				  { 14, 15 },
				  { 16, 17 },	/* +Y */
				  { 18, 19 },
				  { 20, 21 },
				  { 21, 22 },
				  { 21, 23 },
				  { 24, 25 },	/* +Z */
				  { 26, 27 },
				  { 28, 29 },
				  { 29, 30 },
				  { 30, 31 }, };
    int i;
    static int x;
    fbdraw(0,0,x++&31,31,15);
    for (i=0;i<(sizeof(clx)/sizeof(clx[0]));i++)
     { draw3line(pts[clx[i][0]],pts[clx[i][1]],lea,ler,0);
       draw3line(pts[clx[i][0]],pts[clx[i][1]],rea,rer,450);
     }
  }
 setcmap(drawbit);
 drawbit = ! drawbit;
 while (fb->s & 0x10000000) ;
 fb->pm = drawbit ? 0xf0 : 0x0f;
}

static int checkkbd(void)
{
 struct firm_event ev;
 int r;

 r = read(kbdfd,&ev,sizeof(ev));
 if (r == sizeof(ev))
  { queue_event(EVT_KBD|(ev.id&0x7f)|((ev.value==VKEY_UP)?EVK_UP:EVK_DN));
    return(1);
  }
 return(0);
}

static void resethw(void)
{
 fb->bg = 0;
 fb->pixelm = ~0;
 fb->s = 0;
 fb->alu =   0xc0000000 /* GX_PLANE_MASK */
	   | 0x20000000 /* GX_PIXEL_ONES */
	   | 0x00800000 /* GX_ATTR_SUPP (?) */
	   | 0x00000000 /* GX_RAST_BOOL (?) */
	   | 0x00000000 /* GX_PLOT_PLOT (?) */
	   | 0x08000000 /* GX_PATTERN_ONES */
	   | 0x01000000 /* GX_POLYG_OVERLAP */
	   | 0x0000ff00 /* ALU = set to fg color */
	   ; /* ALU */
 fb->clip = 0;
 fb->offx = 0;
 fb->offy = 0;
 fb->clipminx = 0;
 fb->clipminy = 0;
 fb->clipmaxx = 1151;
 fb->clipmaxy = 899;
 fb->fg = 0;
 fb->pm = ~0;
}

static void process_evq(void)
{
 int ev;

 while (1)
  { ev = dequeue_event();
    switch (ev & EV_TYPE)
     { case EVT_NIL:
	  return;
	  break;
       case EVT_KBD:
	  switch (ev & (EVK_CODE|EVK_UPDN))
	   { case  16|EVK_DN: resethw();          break;
	     case  16|EVK_UP:                     break;
	     case  29|EVK_DN:                     break;
	     case  29|EVK_UP: exit(0);            break;
	     case  68|EVK_DN: rotbits |= ROT_CCW; break;
	     case  68|EVK_UP: rotbits &=~ROT_CCW; break;
	     case  69|EVK_DN: rotbits |= ROT_UP;  break;
	     case  69|EVK_UP: rotbits &=~ROT_UP;  break;
	     case  70|EVK_DN: rotbits |= ROT_CW;  break;
	     case  70|EVK_UP: rotbits &=~ROT_CW;  break;
	     case  91|EVK_DN: rotbits |= ROT_LF;  break;
	     case  91|EVK_UP: rotbits &=~ROT_LF;  break;
	     case  93|EVK_DN: rotbits |= ROT_RT;  break;
	     case  93|EVK_UP: rotbits &=~ROT_RT;  break;
	     case 113|EVK_DN: rotbits |= ROT_DN;  break;
	     case 113|EVK_UP: rotbits &=~ROT_DN;  break;
	     default:
		printf("kbd: %d ",ev&EVK_CODE);
		switch (ev & EVK_UPDN)
		 { case EVK_UP: printf("up\n"); break;
		   case EVK_DN: printf("dn\n"); break;
		 }
		break;
	   }
	  break;
       case EVT_TICK:
	  switch (rotbits & (ROT_LF|ROT_RT))
	   { case ROT_RT:
		out = rotunit(out,up,.01);
		wantrender = 1;
		break;
	     case ROT_LF:
		out = rotunit(out,up,-.01);
		wantrender = 1;
		break;
	   }
	  switch (rotbits & (ROT_UP|ROT_DN))
	   { case ROT_DN:
		rt = normcross(up,out);
		up = rotunit(up,rt,.01);
		out = normcross(rt,up);
		wantrender = 1;
		break;
	     case ROT_UP:
		rt = normcross(up,out);
		up = rotunit(up,rt,-.01);
		out = normcross(rt,up);
		wantrender = 1;
		break;
	   }
	  switch (rotbits & (ROT_CW|ROT_CCW))
	   { case ROT_CW:
		rt = normcross(up,out);
		up = rotunit(up,out,.01);
		wantrender = 1;
		break;
	     case ROT_CCW:
		rt = normcross(up,out);
		up = rotunit(up,out,-.01);
		wantrender = 1;
		break;
	   }
	  break;
     }
  }
}

static void initsim(void)
{
 up.x = 0;
 up.y = 0;
 up.z = -1;
 out.x = 1;
 out.y = .2;
 out.z = .3;
 out = norm(out);
 rt = normcross(up,out);
 up = normcross(out,rt);
 ed = 10;
 es = 1000;
 ipd = 1;
 rotbits = 0;
}

static void run(void)
{
 struct pollfd pfd;

 process_evq();
 mayberender();
 if (checkkbd()) return;
 if (rotbits) starttiming(); else stoptiming();
 if (checktick()) return;
 pfd.fd = kbdfd;
 pfd.events = POLLIN | POLLRDNORM;
 if (poll(&pfd,1,INFTIM) < 0)
  { if (errno == EINTR) return;
    fprintf(stderr,"%s: poll: %s\n",__progname,strerror(errno));
    exit(1);
  }
}

static void handleargs(int ac, char **av)
{
 int skip;
 int errs;

 skip = 0;
 errs = 0;
 for (ac--,av++;ac;ac--,av++)
  { if (skip > 0)
     { skip --;
       continue;
     }
    if (**av != '-')
     { fprintf(stderr,"%s: extra argument `%s'\n",__progname,*av);
       errs ++;
       continue;
     }
    if (0)
     {
needarg:;
       fprintf(stderr,"%s: %s needs a following argument\n",__progname,*av);
       errs ++;
       continue;
     }
#define WANTARG() do { if (++skip >= ac) goto needarg; } while (0)
    if (!strcmp(*av,"-fb"))
     { WANTARG();
       fbpath = av[skip];
       continue;
     }
    if (!strcmp(*av,"-kb") || !strcmp(*av,"-kbd"))
     { WANTARG();
       kbpath = av[skip];
       continue;
     }
#undef WANTARG
    fprintf(stderr,"%s: unrecognized option `%s'\n",__progname,*av);
    errs ++;
  }
 if (errs) exit(1);
}

int main(int, char **);
int main(int ac, char **av)
{
 handleargs(ac,av);
 initsim();
 initevq();
 initfb();
 initkbd();
 inittiming();
 wantrender = 1;
 while (1) run();
}