#include <stdio.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <unistd.h>
#include <time.h>
#include "cave_ogl.h"

/*
 * "deskconf" calibrates tracking for a 1-wall (desk) Cave.
 * 
 * It prompts for environmental information, invites the user to
 * hold the wand's sensor blob to various points on the screen, and then
 * writes (to standard output) appropriate CAVE configuration commands,
 * suitable for inclusion in e.g. /usr/local/CAVE/etc/cave.config.
 * The only text written to stdout (as opposed to stderr) is valid CAVE config
 * commands, so you could test by invoking as
 *   deskconf >> .caverc; cavevars
 * It does write both ProjectionCorners and 2.6's new ProjectionData
 * directives, so the config should work with both kinds of apps.
 * 
 * This version has been tried with CAVElibs 2.5.6 and 2.6.
 * It uses some internal CAVE functions to get the wall name, and to
 * get hold of the library's X display pointer so that it can open X fonts.
 * Both of these internal functions changed between 2.5.6 and 2.6, and
 * may presumably change again.  If it breaks, look for code around the
 * "#pragma weak" statements.
 * 
 * If the tracker's output doesn't appear to be in feet, we emit a non-unit
 * CaveTransmitterRotationMatrix (which applies both scaling and rotation)
 * to compensate.  I don't know whether this is a good idea, but haven't found
 * any bad effects so far.
 * 
 * There's not much provision for internal consistency checking.  We do tell
 * the user what the real-world distances between our three calibration marks
 * should be, assuming that our screen has square pixels and a uniformly-scaled
 * raster (i.e. no linearity error or other distortions), and that they've
 * given us the correct screen width.  Also, you can get a measure of tracker
 * jitter by clicking the *middle* wand button rather than the left one when
 * marking a point; you can do this several times, and the range of tracker
 * measurements (midvalue +- range) is displayed on the screen.
 * 
 * Stuart Levy, NCSA, slevy@ncsa.uiuc.edu   Nov 1997.
 */

static char version[] = "deskconf 1.0  slevy@ncsa.uiuc.edu";

#define COUNT(array)	( sizeof(array) / sizeof((array)[0]) )

int *done;	/* Shared "quit now" flag */

static char *fontname = "-adobe-courier-medium-r-*-*-*-120-*-*-*-*-*-*";

static void reduce();

/* Handy vector arithmetic */

void vcomb( float *dst, float sa, float *a, float sb, float *b )
{
  dst[0] = sa*a[0] + sb*b[0];
  dst[1] = sa*a[1] + sb*b[1];
  dst[2] = sa*a[2] + sb*b[2];
}

void vsmul( float *dst, float s, float *src )
{
  dst[0] = s*src[0];
  dst[1] = s*src[1];
  dst[2] = s*src[2];
}

float vdot( float *a, float *b )
{
  return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

float vmag( float *a )
{
  return sqrt( vdot( a, a ) );
}

float vunit( float *a )
{
  float s = vmag(a);
  if(s>0) vsmul( a, 1/s, a );
  return s;
}

float vproj( float *dst, float *v, float *onto )
{
  float scl = vdot( v, onto ) / vdot( onto, onto );
  vcomb( dst,  1,v, -scl, onto );
  return scl;
}

void vcross( float *dst, float *a, float *b )
{
  dst[0] = a[1]*b[2] - a[2]*b[1];
  dst[1] = a[2]*b[0] - a[0]*b[2];
  dst[2] = a[0]*b[1] - a[1]*b[0];
}

void vcopy( float *dst, float *src )
{
  memcpy(dst, src, 3*sizeof(float) );
}


  /* Hacks, since we're getting at an internal Cave library item,
   * and Dave Pape feels free to change its name between
   * cave library releases.  Try both alternatives.
   */
#pragma weak CAVEXDisplay
#pragma weak CAVEXdisplay

extern Display *CAVEXDisplay();
extern Display *CAVEXdisplay;

static Display *caveDisplay() {
  if(&CAVEXDisplay != NULL)
    return CAVEXDisplay();
  if(&CAVEXdisplay != NULL)
    return CAVEXdisplay;
  return XOpenDisplay(NULL);
}

#pragma weak CAVEWallName
#pragma weak CAVEDebugConfigWallName

extern char *CAVEWallName( int wallid );
extern char *CAVEDebugConfigWallName( int wallid );

static char *caveWallName( int wallid ) {
  if(&CAVEWallName != NULL)
    return CAVEWallName( wallid );
  if(&CAVEDebugConfigWallName != NULL)
    return CAVEDebugConfigWallName( wallid );
  return "somewall";
}

static int fontbase = -1;
static Font xfont;

static int initFont(char *fname) {
  xfont = XLoadFont( caveDisplay(), fname );
  if(xfont != NULL) {
    fontbase = glGenLists( 96 );
    glXUseXFont( xfont, 32, 95, fontbase );
  }
  return xfont != NULL;
}


static int pasting = 0;

void pushPasteTfm()
{
  if(pasting++ > 0) return;	/* Don't overflow projection stack! */
  glMatrixMode( GL_PROJECTION );
  glPushMatrix();
  glLoadIdentity();
  glOrtho( -1., 1.,  -1., 1., -1., 1. );
  glMatrixMode( GL_MODELVIEW );
  glPushMatrix();
  glLoadIdentity();
}

void putStringHere(char *str)
{
  int c;

  if(fontbase < 0) {
    if(! initFont(fontname))
        initFont("fixed");
  }
  while((c = *str++) != '\0') {
    if(c >= 32 && c <= 32+95)
	glCallList( fontbase + c - 32 );
  }
}

void popPasteTfm() {
  if(--pasting > 0) return;
  glMatrixMode( GL_PROJECTION );
  glPopMatrix();
  glMatrixMode( GL_MODELVIEW );
  glPopMatrix();
}

void putStringAt(char *str, float scrx0, float scry0)
{
  pushPasteTfm();
  glRasterPos2f( scrx0, scry0 );
  putStringHere( str );
  popPasteTfm();
}

#define RMARK .05
static GLfloat markshape[][2] = {
  RMARK,0,  0,RMARK,  -RMARK,0,  0,-RMARK,  RMARK,0,  -RMARK,0,
  0,RMARK,  0,-RMARK
};


int curmark;

struct mark {
  float screenpos[2];
  int nsamples;
  float pos[3], min[3], max[3];
} marks[] = {
  {  { 0, .75 },	0, { 0,0,0 } },
  {  { -.75, -.75 },	0, { 0,0,0 } },
  {  { .75, -.75 },	0, { 0,0,0 } },
};

#define	INCH	(1./12.)

float screenwidth = 69*INCH;	/* Real screen width in inches */
float screentilt = 30;		/* Degrees down from vertical */

	/* User's advice on the location of the CAVE origin
	 * relative to the center of the bottom edge of the screen.
	 * Let's say the screen's bottom edge determines the X-axis direction,
	 * and that the X=0 plane passes through the screen's center.
	 * So screenmid[0] = 0.
	 * screenmid[1] is the height of the screen's lower edge (assumed level)
	 * above the Y=0 plane, normally the ground.
	 * screenmid[2] is the (typically negative) Z-value at the screen's
	 * lower edge -- the distance by which the origin lies in front
	 * of the screen bottom.
	 */
float screenmid[3] = { 0, 34*INCH, -24*INCH };

int vpw, vph;		/* size of screen in pixels */

void alignmark( int index, int wanted )
{
  register struct mark *mp = &marks[index];
  static GLubyte colors[2][4] = { 0x60,0x90,0x90,0xff, 0xff,0xff,0xff,0xff };
  int i;
  char str[128];
  static int poll;

  glColor4ubv( &colors[wanted][0] );
  glLineWidth( 2 );

  glPushMatrix();
  glTranslatef( mp->screenpos[0], mp->screenpos[1], 0 );
  glBegin(GL_LINE_STRIP);
  for(i = 0; i < COUNT(markshape); i++)
    glVertex2fv( &markshape[i][0] );
  glEnd();

  glRasterPos2f( RMARK*1.5, 0 );
  if(wanted)
    putStringHere( "Click here" );

  glRasterPos2f( -.2, -.1 );
  if(mp->nsamples == 1) {
    sprintf(str, " [%.3f %.3f %.3f]", mp->pos[0],mp->pos[1],mp->pos[2]);
    putStringHere( str );
  } else if(mp->nsamples > 1) {
    float mid[3], semi[3];
    vcomb( mid, .5,mp->max, .5,mp->min );
    vcomb( semi, .5,mp->max, -.5,mp->min );
    sprintf(str, " [%.3f+-%.3f %.3f+-%.3f %.3f+-%.3f]",
	mid[0],semi[0], mid[1],semi[1], mid[2],semi[2] );
    putStringHere( str );
  }

  glPopMatrix();
}

void drawall(void)
{
  char str[128];
  float wand[3], head[3];
  struct mark *mp = &marks[curmark];
  int i;
  int chg;
  GLint vp[4];

  glGetIntegerv( GL_VIEWPORT, vp );
  vpw = vp[2], vph = vp[3];
  
  chg = (CAVEButtonChange(1)>0 ? 1 : 0) |
	(CAVEButtonChange(2)>0 ? 2 : 0) |
	(CAVEButtonChange(3)>0 ? 4 : 0);
  if( chg ) {
    CAVEGetPosition( CAVE_WAND, wand );
    if(mp->nsamples == 0) {
	vcopy(mp->pos, wand);
	vcopy(mp->min, wand);
	vcopy(mp->max, wand);
    } else {
	for(i=0;i<3;i++) {
	    if(mp->min[i] > wand[i]) mp->min[i] = wand[i];
	    else if(mp->max[i] < wand[i]) mp->max[i] = wand[i];
	    mp->pos[i] = (mp->pos[i]*mp->nsamples + wand[i]) / (mp->nsamples+1);
	}
    }
    fprintf(stderr, "Snapped mark %d at %.3f %.3f %.3f\n",
		curmark, wand[0],wand[1],wand[2]);
    mp->nsamples++;
    if(chg & (1|4))
	curmark++;
    if(curmark >= COUNT(marks)) {
	curmark = 0;
	reduce();
	if(chg & (1|4)) {
	    *done = 1;
	}
    }
  }

  pushPasteTfm();

  glShadeModel(GL_FLAT);
  glColor3ub(0,  0,  0);
  glClearColor(0.,0.,0.,0.);
  glClearDepth(1.);
  glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

  glColor3ub( 255, 255, 0 );

  putStringAt( "Put wand's sensor (white blob) on crosshair and click", -.95, .90 );
  putStringAt( "  wand button 1 to measure (or button 2 to remeasure)", -.95, .82 );

  CAVEGetPosition(CAVE_WAND, wand);
  CAVEGetPosition(CAVE_HEAD, head);
  glRasterPos2f( -.95, -.90 );
  sprintf(str, "wand at %.3f %.3f %.3f", wand[0],wand[1],wand[2]);
  putStringHere( str );
  sprintf(str, " head at %.3f %.3f %.3f", head[0],head[1],head[2]);
  putStringHere( str );

  glRasterPos2f( -.95, 0 );
  sprintf(str, "Expect %.1f inches btwn lower two marks, %.1f from upper to others, with %.1f-inch screen",
	screenwidth * .75/INCH,
	screenwidth * hypot(.5*.75, .75*vph/vpw)/INCH,
	screenwidth/INCH);
  putStringHere( str );

  for(i = 0; i < COUNT(marks); i++)
    alignmark( i, i == curmark );

  popPasteTfm();
}

void toCave( float *dst, float *src, float scale, float TM[3][3], float offset[3] )
{
  int i;
  float tv[3];

  vsmul( tv, scale, src );
  for(i = 0; i < 3; i++)
    dst[i] = tv[0]*TM[i][0] + tv[1]*TM[i][1] + tv[2]*TM[i][2] + offset[i];
}
  

static void reduce()
{
  float vx[3], vy[3], vz[3];
  float TM[3][3];
  float halfwidth;		/* half-screen-width in feet */
  float sx, sy;			/* Scale: cave-units (ft) per tracker-unit */
  float tilt, c, s;
  float llc[3], ulc[3], lrc[3];
  float cllc[3], culc[3], clrc[3], cmks[3][3];
  float mid12[3], sdx[3], sdy[3], midedge[3], offset[3];
  char host[128];
  time_t now;


  /* Map sensor values at measured points into values at screen corners.
   * (Why not just put the reference points right at the screen corners?
   * They'd be farther from the sensor, and readings might be less reliable.
   * Also, the raster's liable to be more distorted at the extreme edges.)
   */
  vcomb( mid12, .5, marks[1].pos, .5, marks[2].pos );
  /* Screen X-vector is 4/3 the displacement from P1 to P2 */
  vcomb( sdx, 1/.75, marks[2].pos, -1/.75, marks[1].pos );
  /* Screen Y-vector is 4/3 the displacement from the P1-P2 midpoint to P0 */
  vcomb( sdy, 1/.75, marks[0].pos, -1/.75, mid12 );
  /* Take orthogonal-projection of screen-Y-vector on screen-X-vector */
  vproj( sdy, sdy, sdx );

  vcomb( llc, 1,mid12, -.5, sdx );
  vcomb( llc, 1,llc, -.125, sdy );

  vcomb( ulc, 1,llc, 1,sdy );
  vcomb( lrc, 1,llc, 1,sdx );

  vcopy( vx, sdx );
  vcopy( vy, sdy );
  sx = screenwidth / vunit( vx );
  sy = (screenwidth * vph / vpw) / vunit( vy );
 
  vcross( vz, vx, vy );

  /* Map to our desired upright coordinate system,
   * using presumed screen tilt.  +Y is up, +Z is backward (out from screen).
   */
  c = cos( screentilt * M_PI / 180. );
  s = sin( screentilt * M_PI / 180. );

  vcopy(TM[0], vx);
  vcomb(TM[1], c,vy, s,vz);
  vcomb(TM[2], -s,vy, c,vz);


  /* Now, determine where the CAVE origin should go (and thus what the
   * TransmitterOffset should be).  
   * What would the sensor read in the middle of the screen's lower edge?
   * In [-1..+1,-1..+1] screen coords, that's midway from marks[1] to marks[2]
   * and a bit away from marks[0].  Should do it directly with matrix
   * inversion from the marks[i].screenpos coords, but this is too easy.
   */
  vcomb( midedge, 7./6., midedge, -1./6., marks[0].pos );
 
  /* Scale by our adopted CAVEScale, determined by comparing the real
   * screen's width to the sensor-displacement across it.   This becomes
   * the scaled-sensor position of the screen-bottom-center.
   */
  vsmul( midedge, sx, midedge );

  /* We know the CAVE-coord position of that point: it's screenmid[].
   * Subtract to get the offset from scaled-sensor coords to CAVE coords.
   */
  vcomb( offset, 1,screenmid, -1,midedge );

  /* Now, in that coord system, where are the screen corners? */

  toCave( cllc, llc, sx, TM, offset );
  toCave( culc, ulc, sx, TM, offset );
  toCave( clrc, lrc, sx, TM, offset );

  /* And, FYI, where should our measured points go in CAVE coords? */
  toCave( cmks[0], marks[0].pos, sx, TM, offset );
  toCave( cmks[1], marks[1].pos, sx, TM, offset );
  toCave( cmks[2], marks[2].pos, sx, TM, offset );

  gethostname(host, sizeof(host));

  printf("\n\n");
  time(&now);
  printf("# CAVE parameters derived by %s@%s using deskconf on %s\n",
	getenv("LOGNAME"), host, ctime(&now));
  printf("# Screen %dx%d pixels, assumed %.1f in wide,\n", vpw, vph, screenwidth/INCH);
  printf("# lower edge assumed %.1f inch above Y=0 ground plane, %.1f inch back from Z=0,\n",
	screenmid[1]/INCH, -screenmid[2]/INCH);
  printf("#    and assumed tilted %.1f degrees back from the vertical.\n",
	screentilt);
  printf("# Horizontal scale %.3f, vertical scale %.3f feet per tracker unit\n\n",
	sx, sy);
  printf("# Screen corners: sensor coords:  %.3f %.3f %.3f  %.3f %.3f %.3f  %.3f %.3f %.3f\n",
	llc[0],llc[1],llc[2], ulc[0],ulc[1],ulc[2], lrc[0],lrc[1],lrc[2]);
  printf("# Measured points: expect cave %.3f %.3f %.3f  %.3f %.3f %.3f  %.3f %.3f %.3f\n",
	cmks[0][0], cmks[0][1], cmks[0][2],
	cmks[1][0], cmks[1][1], cmks[1][2],
	cmks[2][0], cmks[2][1], cmks[2][2]);

  printf("\n\n");

  printf("CaveScale %.4f\n", 1. /* 1/sx */);

  printf("TransmitterRotationMatrix %.4f %.4f %.4f  %.4f %.4f %.4f  %.4f %.4f %.4f\n",
	sx*TM[0][0], sx*TM[1][0], sx*TM[2][0],
	sx*TM[0][1], sx*TM[1][1], sx*TM[2][1],
	sx*TM[0][2], sx*TM[1][2], sx*TM[2][2]);
  printf("TransmitterOffset %.3f %.3f %.3f\n",
	offset[0], offset[1], offset[2]);

  printf("ProjectionData %s both wall  %.3f %.3f %.3f  %.3f %.3f %.3f  %.3f %.3f %.3f\n",
	caveWallName( CAVEWall ),
	cllc[0],cllc[1],cllc[2],
	culc[0],culc[1],culc[2],
	clrc[0],clrc[1],clrc[2] );

  printf("ProjectionCorners %s  %.3f %.3f %.3f  %.3f %.3f %.3f  %.3f %.3f %.3f\n",
	caveWallName( CAVEWall ),
	cllc[0],cllc[1],cllc[2],
	culc[0],culc[1],culc[2],
	clrc[0],clrc[1],clrc[2] );

  printf("\n# end of deskconf portion of cave config.\n\n\n"); 
}

float getval(char *prompt, float defval, float scale)
{
  char line[128];

  float v = defval / scale;
  fprintf(stderr, prompt, v);
  fgets(line, sizeof(line), stdin);
  sscanf(line, "%f", &v);
  return v*scale;
}


static char cleanconf[] = "\
CaveScale 1\n\
WandSensorOffset 0 0 0\n\
WandSensorRotationMatrix 1 0 0  0 1 0  0 0 1\n\
TransmitterOffset 0 0 0\n\
TransmitterRotationMatrix 1 0 0  0 1 0  0 0 1\n\
CaveRotationMatrix  1 0 0  0 1 0  0 0 1\n\
CaveTranslation  0 0 0\n";

main(int argc, char* argv[])
{
  int nargc;
  char **nargv;
  FILE *tconf;
  char *tname;

  tname = tempnam( NULL, "conf" );
  tconf = fopen( tname, "w" );
  fputs(cleanconf, tconf);
  fclose(tconf);

  nargv = (char **)malloc( (argc+3) * sizeof(char **) );
  memcpy( &nargv[3], &argv[1], argc*sizeof(char **) );
  nargv[0] = argv[0];
  nargv[1] = "-caveconfig";
  nargv[2] = tname;

  nargc = argc+2;
  CAVEConfigure(&nargc,nargv,NULL);
  unlink(tname);

  fprintf(stderr, "Autoconfigures CAVE settings for a 1-wall CAVElet.\n\
This program writes the resulting CAVE config commands to stdout,\n\
normally the terminal; you might invoke it as\n\
    deskconf >> .caverc\n\
to test the settings, then edit them into /usr/local/CAVE/etc/<host>.config\n\
if they seem accurate.\n\
The questions below are only needed to relate CAVE coordinates to the real\n\
world, determining the up (+Y) direction, scaling, and the position of the\n\
origin.  If you only care that the tracker matches the screen, you can just\n\
press <Return> to all four.\n");


  screentilt = getval(
    "Screen's tilt angle (0 if screen vertical, 90 if horizontal) [%.0f] ? ",
		screentilt, 1 );

  screenwidth = getval(
    "Screen's displayable width (inches) [%.0f] ? ",
		screenwidth, INCH );

  screenmid[1] = getval(
    "Height of bottom-edge of screen above the Y=0 floor (inches) [%.0f] ? ",
		screenmid[1], INCH );

  screenmid[2] = getval(
    "Distance back from bottom-edge of screen to chosen Z=0 origin (in) [%.0f] ? ",
		screenmid[2], -INCH );

  fprintf(stderr, "\n\n");

  done = (int *)CAVEMalloc( sizeof(int) );
  *done = 0;

  CAVEInit();
  CAVEDisplay(drawall, 0);
  while(!CAVEgetbutton(CAVE_ESCKEY) && !*done) {
    sginap(10);
  }
  CAVEExit();
}