/*
 * orbfly.c - Example code showing how to use a SpaceOrb to "fly".
 * 
 *  $Id: orbfly.c,v 1.1 1998/08/29 20:41:36 mdanks Exp $
 */

#if defined(WIN32) || defined(_MSC_VER)
#include <windows.h>  /* needed by windows builds */
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <glut.h>

#include "orb.h"         /* SpaceOrb 360, 6DOF controller */
#include "orbquat.h"     /* quaternion math routines */

typedef struct {
  float x;
  float y;
  float z;
} vector;


void orbmotion(void);

OrbHandle orb;
int displaycleared = 0;
int displaylistready = 0;

float curtrans[3];
float newtrans[3];
float curquat[4];
float lastquat[4];

vector glob_camcent;
vector glob_camviewvec;
vector glob_camupvec;
vector orig_camcent;
vector orig_camviewvec;
vector orig_camupvec;

int displaywidth = 512;
int displayheight = 512;

void my_reshape(GLsizei w, GLsizei h) {
  glViewport(0, 0, w, h);

  displaywidth = w;
  displayheight = h;
}

void keys(unsigned char c, int x, int y) {
  switch(c) {
    case 27:
    case 'q':
    case 'Q':
      exit(0);
  }
}

#define NUMDIVS 40
#define DIVLEN 4000

void drawplane() {
  int y;
  
  glBegin(GL_LINES);
    glColor3f(0.0, 0.0, 1.0);
    for (y=0; y<NUMDIVS; y++) {
      glVertex3f(-DIVLEN, (2*DIVLEN/NUMDIVS * y) - DIVLEN, -50.0); 
      glVertex3f(DIVLEN, (2*DIVLEN/NUMDIVS * y) - DIVLEN, -50.0); 
    } 
    for (y=0; y<NUMDIVS; y++) {
      glVertex3f((2*DIVLEN/NUMDIVS * y) - DIVLEN, -DIVLEN, -50.0); 
      glVertex3f((2*DIVLEN/NUMDIVS * y) - DIVLEN, DIVLEN, -50.0); 
    } 
  glEnd();
}

void do_objects() {
  GLfloat mat_specular[] = {1.0, 1.0, 1.0, 1.0};
  GLfloat mat_shininess[] = {50.0} ;
  GLfloat light_position[] = {100.0, 100.0, 100.0, 0.0};
 
  glShadeModel(GL_SMOOTH);
  glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
  glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
  glLightfv(GL_LIGHT0, GL_POSITION, light_position);
 
  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_LEQUAL);
  glEnable(GL_DEPTH_TEST);

  glColorMaterial(GL_FRONT, GL_DIFFUSE);
  glEnable(GL_COLOR_MATERIAL);

  glColor3f(1.0, 1.0, 0.0);
  glutSolidSphere(1.0, 10, 10);
  glutSolidTorus(5.0, 95.0, 4.0, 8.0);

  glDisable(GL_LIGHTING);
  drawplane();
}


void recalcModelView(void) {
  GLfloat m[4][4];
  float t[3];
 
  glPopMatrix();  /* ditch original modelview matrix */
  glPushMatrix(); /* replace with our own */

  build_rotmatrix(m, curquat);

  glMultMatrixf(&m[0][0]);

  t[0] = m[0][0] * newtrans[0] + m[0][1] * newtrans[1] + m[0][2] * newtrans[2];
  t[1] = m[1][0] * newtrans[0] + m[1][1] * newtrans[1] + m[1][2] * newtrans[2];
  t[2] = m[2][0] * newtrans[0] + m[2][1] * newtrans[1] + m[2][2] * newtrans[2];

  curtrans[0] += t[0];
  curtrans[1] += t[1]; 
  curtrans[2] += t[2]; 
  glTranslatef(-curtrans[0], -curtrans[1], -curtrans[2]);
}

void orbmotion(void) {
  int tx, ty, tz, rx, ry, rz, buttons, tmp;
  float qq[4];
  float xx[3]={1.0, 0.0, 0.0};
  float yy[3]={0.0, 1.0, 0.0};
  float zz[3]={0.0, 0.0, 1.0};

  static float transdivisor = 1000.0;
  static float angdivisor = 5000.0;
 
  if (orb_getstatus(orb, &tx, &ty, &tz, &rx, &ry, &rz, &buttons)) {
    /* Swap Y and Z axes, and negate the Z axis (both trans and rot) */
    tmp = ty;
    ty = -tz;
    tz = tmp;
 
    tmp = ry;
    ry = -rz;
    rz = tmp;

    if (buttons) {
      if (buttons & ORB_BUTTON_RESET) {
        curtrans[0] = 0.0;
        curtrans[1] = 0.0;
        curtrans[2] = 0.0;
        curquat[0] = 0.0;
        curquat[1] = 0.0;
        curquat[2] = 0.0;
        curquat[3] = 1.0;
 
        transdivisor = 1000.0;
        angdivisor = 5000.0; 
  
        glob_camviewvec = orig_camviewvec;
        glob_camupvec = orig_camupvec;
        glob_camcent = orig_camcent;
      }

      if (buttons & ORB_BUTTON_A) {
        transdivisor /= 1.2;
        angdivisor /= 1.2;
      }
      else if (buttons & ORB_BUTTON_B) {
        transdivisor *= 1.2; 
        angdivisor *= 1.2;
      }

      if (buttons & ORB_BUTTON_C)
        transdivisor /= 1.2;
      else if (buttons & ORB_BUTTON_F)
        transdivisor *= 1.2; 

      if (buttons & ORB_BUTTON_D) 
        angdivisor *= 1.2;
      else if (buttons & ORB_BUTTON_E)
        angdivisor /= 1.2;
    } /* end of button handling */
      
    newtrans[0] = tx / transdivisor; 
    newtrans[1] = ty / transdivisor;
    newtrans[2] = tz / transdivisor;
    
    /* do rotations */ 
    axis_to_quat(xx, rx / angdivisor, lastquat);

    axis_to_quat(yy, ry / angdivisor, qq);
    add_quats(qq, lastquat, lastquat);

    axis_to_quat(zz, rz / angdivisor, qq);
    add_quats(qq, lastquat, lastquat);

    add_quats(lastquat, curquat, curquat);
    glutPostRedisplay();
  }
#if defined(WIN32) || defined(_MSC_VER)
  else {
    Sleep(5); /* Win32: if no movement then sleep for a tiny bit.. */
  }
#else
  else {
    usleep(5); /* Unix: if no movement then sleep for a tiny bit.. */
  }
#endif

}



void ply_display() {
  float fov, lookx, looky, lookz;

  if (displaycleared == 0) {
    glClearColor(0.0, 0.0, 0.0, 0.0);
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    glutSwapBuffers();
    printf("Building display lists and rendering. . . .\n");
    glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, GL_TRUE);

    displaycleared = 1;
  }

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  fov = 45;
  gluPerspective(fov, (GLfloat) displaywidth / (GLfloat) displayheight, 
                 2.0, 100000.0);

  recalcModelView(); /* do the trackball thing! */

  glScalef(-1.0, 1.0, 1.0); /* flip coords for matching with raytracer */

  /* define a "look at" point from the view vectors */
  lookx = glob_camcent.x + glob_camviewvec.x;
  looky = glob_camcent.y + glob_camviewvec.y;
  lookz = glob_camcent.z + glob_camviewvec.z;

  gluLookAt(glob_camcent.x, glob_camcent.y, glob_camcent.z,
            lookx, looky, lookz,
            glob_camupvec.x, glob_camupvec.y, glob_camupvec.z);

  glMatrixMode(GL_MODELVIEW);

if (displaylistready) {
  glCallList(1);
} 
else {
#if !defined(NODISPLAYLIST)
  printf("Creating display list...\n");
  glNewList(1, GL_COMPILE); /* start display list */
#endif
  glLoadIdentity(); /* load identity matrix to start with */

  glClearColor(0.0, 0.0, 0.0, 0.0);
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  /* display objects */
  do_objects(); 

#if !defined(NODISPLAYLIST) 
  glEndList(); /* end display list */
  glCallList(1);
  displaylistready = 1;
#endif
  } /* else create display list */
  glFlush();

  glutSwapBuffers();
}



int main(int argc, char **argv) {
  char * orbname;

#if defined(WIN32) || defined(_MSC_VER)
  printf("Orbfly -- Win32 demo program, by John Stone (j.stone@acm.org)\n");
#else
  printf("Orbfly -- Unix demo program, by John Stone (j.stone@acm.org)\n");
#endif

  if (argc != 2) {
    printf("Usage error, no SpaceOrb port specified!\n");
    printf("  Usage: orbfly portname\n");
    printf("  Example port names are:\n");

#if defined(WIN32) || defined(_MSC_VER)
    printf("    1 2 3 4 5, which correspond to com1, com2, com3 etc.\n");
#else
    printf("    /dev/ttya, /dev/ttyb, /dev/ttyc, /dev/ttyd,\n");
    printf("    /dev/term/a, /dev/term/b, /dev/cua, /dev/cub\n");
#endif

    return -1; /* same as a call to exit() */
  }
  orbname = argv[1];

  printf("Opening SpaceOrb: using port %s for I/O\n", orbname);
  if ((orb = orb_open(orbname)) == NULL) {
    printf("Could not open Orb comm port!\n");
    return -1; /* same as a call to exit() */
  }
  orb_set_nullregion(orb, 65, 65, 65, 65, 65, 65);

 printf("\nUse the Orb buttons to control relative sensitivity/velocity:\n");
 printf("A B: lower/raise rotational+translational speed (in lockstep)\n");
 printf("F C: lower/raise translational speed\n");
 printf("D E: lower/raise rotational speed\n");
 printf("reset (back of orb): go back to default viewpoint and re-zero orb.\n");
 printf("\n");
 printf("Enjoy!\n\n");
     
  glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
  glutInitWindowPosition(0, 0);
  glutInitWindowSize(512, 512);
  glutCreateWindow("Orbfly");
  glutReshapeFunc((void (*)(int, int)) my_reshape);
  glutDisplayFunc(ply_display);
  glutKeyboardFunc(keys);

  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glutSwapBuffers();
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  glutSwapBuffers();
  glShadeModel(GL_SMOOTH);
  glEnable(GL_LINE_SMOOTH);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
  glLineWidth(1.5);

  /* initialize quaternion system */
  curquat[0] = 0.0;
  curquat[1] = 0.0;
  curquat[2] = 0.0;
  curquat[3] = 1.0;

  newtrans[0] = curtrans[0] = 0.0;
  newtrans[1] = curtrans[1] = 0.0;
  newtrans[2] = curtrans[2] = 0.0;
 
  glutIdleFunc(orbmotion);

  glob_camcent.x = -15.0;
  glob_camcent.y = 0.0;
  glob_camcent.z = 0.0;
  orig_camcent = glob_camcent;
  
  glob_camviewvec.x = 1.0;
  glob_camviewvec.y = 0.0;
  glob_camviewvec.z = 0.0;
  orig_camviewvec = glob_camviewvec;

  glob_camupvec.x = 0.0;
  glob_camupvec.y = 0.0;
  glob_camupvec.z = 1.0;
  orig_camupvec = glob_camupvec;

  glutMainLoop();

  return 0; /* we never actually get here, this is just to please compilers */
}

