/*
 * vroom920 version 2009-08-25
 * Displays a stereoscopic cylindrical room on a VR920 HMD with head tracking.
 *
 * Compile with: gcc --std=gnu99 vroom920.c -lX11 -lm -lusb -o vroom920 
 * Run with: vroom920 < /dev/hidraw0
 *
 * Copyright (c) 2009 pascal@pabr.org
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; version 2.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#define _GNU_SOURCE
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <math.h>
#include <sys/time.h>
#include <errno.h>

#include <X11/Xlib.h>
#include <X11/Xutil.h>

// Enable stereoscopic mode on the VR920 ?
#define VR920_STEREO 1

/***** Utilities *****/

void fatal(const char *msg) { perror(msg); exit(1); }

void xopen(int width, int height,
	   Display **display, int *screen, Window *window, GC *gc) {
  *display = XOpenDisplay(getenv("DISPLAY"));
  if ( ! *display ) fatal("XOpenDisplay");
  *screen = DefaultScreen(*display);
  XSetWindowAttributes xswa;
  xswa.background_pixel = BlackPixel(*display, *screen);
  *window = XCreateWindow(*display, DefaultRootWindow(*display), 
			  0, 0, width, height, 1,
			  CopyFromParent, InputOutput,
			  CopyFromParent, CWBackPixel, &xswa);
  if ( ! *window ) fatal("XCreateWindow");
  XMapWindow(*display, *window);
  *gc = XCreateGC(*display, *window, 0, NULL);
  if ( ! *gc ) fatal("XCreateGC");
  XSync(*display, False);
}

/***** VR920 stereo *****/

#if VR920_STEREO
#include <usb.h>
#define VID 0x1bae
#define PID 0x0002

static char side = 0;

usb_dev_handle *find_vr920_stereo() {
  usb_init();
  if ( usb_find_busses() < 0 ) fatal("usb_find_busses");
  if ( usb_find_devices() < 0 ) fatal("usb_find_devices");
  struct usb_bus *busses = usb_get_busses();
  if ( ! busses ) fatal("usb_get_busses");
  for ( struct usb_bus *bus=busses; bus; bus=bus->next ) {
    for ( struct usb_device *dev=bus->devices; dev; dev=dev->next) {
      if ( dev->descriptor.idVendor == VID &&
	   dev->descriptor.idProduct == PID ) {
	usb_dev_handle *devh = usb_open(dev);
	if ( ! devh ) fatal("usb_open");
	int res = usb_claim_interface(devh, 4);
	if ( res < 0 ) fatal("usb_claim_interface");
	fprintf(stderr, "Found VR920 stereo control\n");
	return devh;
      }
    }
  }
  return NULL;
}
#endif

/***** 3D math *****/

#if 1
typedef float num;
#define SQRT sqrtf
#define SINCOS sincosf
#else
typedef double num;
#define SINCOS sincos
#endif

void normalize(const num s[3], num d[3]) {
  num k = 1 / SQRT(s[0]*s[0]+s[1]*s[1]+s[2]*s[2]);
  d[0] = k * s[0];
  d[1] = k * s[1];
  d[2] = k * s[2];
}

void cross(const num u[3], const num v[3], num d[3]) {
  d[0] = u[1]*v[2] - u[2]*v[1];
  d[1] = u[2]*v[0] - u[0]*v[2];
  d[2] = u[0]*v[1] - u[1]*v[0];
}

void make_matrix(const num acc[3], const num mag[3], num m[3][3]) {
  // Gravity points down.
  normalize(acc, m[1]);
  // gravity^magnetic points east.
  num tmp[3];
  cross(acc, mag, tmp);
  normalize(tmp, m[0]);
  // Complete to an orthogonal matrix.
  cross(m[0], m[1], m[2]);
}

num kproj;

num ox = 0.0;
num oy = -1.7;
num oz = 1.0;

int proj(const num m[3][3], const num p[3], int s[2]) {
  num pr[3] = { p[0]-ox, p[1]-oy, p[2]-oz };
  num q[3] = { 0, 0, 0 };
  for ( int i=0; i<3; ++i ) for ( int j=0; j<3; ++j ) q[i] += m[j][i]*pr[j];
#if VR920_STEREO
  q[0] += side ? -0.1 : +0.1;
#endif
  s[0] = 320 - kproj*q[0]/q[2];
  s[1] = 240 - kproj*q[1]/q[2];
  return q[2]<-0.1;
}

void filter(const num v[3], num filtered[3]) {
  num k = 0.05;
  for ( int i=0; i<3; ++i ) filtered[i] = (1-k)*filtered[i] + k*v[i];
}

/***** main *****/

int main(int argc, char *argv[]) {
  Display *display; int screen; Window window; GC gc;
  int ww = 640, wh = 480;
  xopen(ww, wh, &display, &screen, &window, &gc);
  
  void draw_cylinder(const num m[3][3]) {
#define CNAZ 24
#define CNALT 8
    int pts[CNALT][CNAZ][2];
    num r0 = 3;
    for ( int alt=0; alt<CNALT; ++alt ) {
      num p[3], r;
      if ( alt <= CNALT/4 ) {
	r = r0 * alt / (CNALT/4);
	p[1] = 0;
      } else if ( alt >= CNALT-CNALT/4 ) {
	r = r0 * (CNALT-alt) / (CNALT/4);
	p[1] = -2.5;
      } else {
	r = r0;
	p[1] = -2.5*(alt-CNALT/4)/(CNALT/2);
      }
      for ( int az=0; az<CNAZ; ++az ) {
	SINCOS(az*2*M_PI/CNAZ, &p[2], &p[0]);
	p[0] *= r;
	p[2] *= r;
	if ( ! proj(m, p, pts[alt][az] ) )
	  pts[alt][az][0] = pts[alt][az][1] = 0;
      }
    }
    for ( int alt=1; alt<CNALT; ++alt )
      for ( int az=0; az<CNAZ; ++az ) {
	if ( az == CNAZ/4 ) continue;
	int a1 = (az+1) % CNAZ;
	if ( pts[alt][az][0] && pts[alt][az][1] &&
	     pts[alt][a1][0] && pts[alt][a1][1] ) {
	  XDrawLine(display, window, gc,
		    pts[alt][az][0], pts[alt][az][1],
		    pts[alt][a1][0], pts[alt][a1][1]);
	}
      }
    for ( int az=0; az<CNAZ; ++az ) {
      for ( int alt=0; alt<CNALT-1; ++alt )
	if ( pts[alt][az][0] && pts[alt][az][1] &&
	     pts[alt+1][az][0] && pts[alt+1][az][1] ) {
	  XDrawLine(display, window, gc,
		    pts[alt][az][0], pts[alt][az][1],
		    pts[alt+1][az][0], pts[alt+1][az][1]);
	}
      }
  }

  num accf[3] = { 0,1,0 };
  num magf[3] = { 0,0,1 };
  int init = 1;

#if 0
  num fov = 32 * M_PI/180;  // Actual FoV
#else
  num fov = 90 * M_PI/180;  // More convenient FoV
#endif
  kproj = 640/2/tan(fov/2);

#if VR920_STEREO
  usb_dev_handle *devh = find_vr920_stereo();
  if ( ! devh ) fprintf(stderr, "VR920 USB stereo control not found\n");

  if ( devh ) {  // Enable stereo mode ?
    usb_reset(devh);
    char cbuf[15] = { 2,7,0,0,0,0,0,0,0,0,0,0,0,0,0 };		   
    int si = usb_control_msg
      (devh, USB_TYPE_VENDOR | USB_RECIP_DEVICE,
       0x09, 0x0302, 0x03, cbuf, sizeof(cbuf), 1000);
    if ( si != 15 ) fatal("usb_control_msg(init)");
    for ( char side=0; side<=1; ++side ) {
      int nw = usb_bulk_write(devh, 0x05, &side, 1, 1000);
      if ( nw != 1 ) fatal("usb_bulk_write(init)");
      int si = usb_control_msg
	(devh, USB_TYPE_VENDOR | USB_RECIP_DEVICE,
	 0x09, 0x0302, 0x03, cbuf, sizeof(cbuf), 1000);
      if ( si != 15 ) fatal("usb_control_msg(init2)");
    }
  }
#endif

  fcntl(0, F_SETFL, O_NONBLOCK);

  struct timeval prev;
  gettimeofday(&prev, NULL);
  int tf = 0;

  while ( 1 ) {

#if VR920_STEREO
    if ( devh ) { // Sync and flip
      char vsync = 0;
      int nr = usb_bulk_read(devh, 0x84, &vsync, 1, 1000);
      if ( nr != 1 ) perror("vsync failed");
      // VSync seems to be triggered 1/3 into the frame,
      // so we need this...
      usleep(10000);

      int nw = usb_bulk_write(devh, 0x05, &side, 1, 1000);
      if ( nw != 1 ) fatal("usb_bulk_write");
      side ^= 1;
    }
#endif

    struct pkt {
      unsigned char b0;
      unsigned char b1;
      unsigned char b2;
      unsigned char b3;
      signed short ax;
      signed short ay;
      signed short az;
      signed short mx;
      signed short my;
      signed short mz;
      unsigned char b16;
    } *pkt = NULL;

    unsigned char buf[32];
    memset(buf, 0, sizeof(buf));
    int nr;
    do {
      nr = read(0, buf, sizeof(buf));
      if ( nr == 17 ) pkt = (struct pkt*)buf;
    } while ( nr > 0 );
    if ( errno != EAGAIN ) { perror("read(stdin)"); exit(1); }
    
    if ( pkt ) {
      num acc[3] = {  pkt->ax,  pkt->ay,  pkt->az };
      num mag[3] = { -pkt->mx,  pkt->my, -pkt->mz };
      filter(acc, accf);
      filter(mag, magf);
    }

#if 0
    magf[0] = 0;
    magf[1] = 0;
    magf[2] = 1;
    accf[0] = 0;
    accf[1] = 1;
    accf[2] = 0;
#endif

    num axes[3][3];
    make_matrix(accf, magf, axes);

    XClearWindow(display, window);
    XSetForeground(display, gc, WhitePixel(display,screen));
    draw_cylinder(axes);
#if 0
    XDrawLine(display, window, gc, 320,240, 320+magf[0]/2,240+magf[1]/2);
    XDrawLine(display, window, gc, 320,230, 320+magf[0]/2,230+magf[2]/2);
#endif
    XSync(display, False);

#if 0
    // Show frame time.
    struct timeval now;
    gettimeofday(&now, NULL);
    int t = (now.tv_sec-prev.tv_sec)*1000000+(now.tv_usec-prev.tv_usec);
    tf = tf*0.95 + t*0.05;
    fprintf(stderr, "T = %4.1f %4.1f ms\n", t*0.001, tf*0.001);
    prev = now;
#endif
  }
  return 0;
}