/*
 GNU Maverik - a system for managing display and interaction in 
               Virtual Environment applications.
 Copyright (C) 1999 Advanced Interfaces Group

 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; either version 2
 of the License, or (at your option) any later version.

 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., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA


 The authors can be contacted via:
 www   - http://aig.cs.man.ac.uk
 email - maverik@aig.cs.man.ac.uk
 mail  - Advanced Interfaces Group, Room 2.90, Computer Science Building, 
         University of Manchester, Manchester, M13 9PL, UK
*/


#include "mavlib_tdm.h"
#include <stdio.h>

#ifdef MAV_TDM
MAV_TDMPos mav_TDM_pos[TDM_MAX_TRACKERS];
MAV_matrix mav_TDM_matrix[TDM_MAX_TRACKERS];
#else
MAV_TDMPos mav_TDM_pos[4];
MAV_matrix mav_TDM_matrix[4];
#endif

int mavlib_TDM= MAV_FALSE;
float mavlib_TDM_xorigin= 0.0;
float mavlib_TDM_yorigin= 0.0;
float mavlib_TDM_zorigin= 20.0;
float mavlib_TDM_xscale= 1.0;
float mavlib_TDM_yscale= 1.0;
float mavlib_TDM_zscale= 1.0;
float mavlib_TDM_offset= 20.0;
float mavlib_TDM_drawScale= 1.0;



/* Routine to copy between the different data structures */

#ifdef MAV_TDM
void mavlib_tdm2mav(MAV_TDMPos *pos, TDM_position tdmpos)
{
  /* Copy the position */
  pos->pos.x= tdmpos.xpos;
  pos->pos.y= tdmpos.ypos;
  pos->pos.z= tdmpos.zpos;

  /* Copy the axes */
  pos->u.x= tdmpos.u[0];
  pos->u.y= tdmpos.u[1];
  pos->u.z= tdmpos.u[2];

  pos->v.x= tdmpos.v[0];
  pos->v.y= tdmpos.v[1];
  pos->v.z= tdmpos.v[2];

  pos->n.x= tdmpos.n[0];
  pos->n.y= tdmpos.n[1];
  pos->n.z= tdmpos.n[2];

  /* Make an orientation matrix */
  pos->matrix.mat[0][0]= pos->u.x;
  pos->matrix.mat[0][1]= pos->u.y;
  pos->matrix.mat[0][2]= pos->u.z;
  pos->matrix.mat[0][3]= 0.0;
  pos->matrix.mat[1][0]= pos->v.x;
  pos->matrix.mat[1][1]= pos->v.y;
  pos->matrix.mat[1][2]= pos->v.z;
  pos->matrix.mat[1][3]= 0.0;
  pos->matrix.mat[2][0]= pos->n.x;
  pos->matrix.mat[2][1]= pos->n.y;
  pos->matrix.mat[2][2]= pos->n.z;
  pos->matrix.mat[2][3]= 0.0;
  pos->matrix.mat[3][0]= pos->pos.x;
  pos->matrix.mat[3][1]= pos->pos.y;
  pos->matrix.mat[3][2]= pos->pos.z;
  pos->matrix.mat[3][3]= 1.0;

  /* Make a quaternion */
  pos->quaternion= mav_quaternionMatrixConvert(pos->matrix);
}
#endif



/* Routine to get a trackers button status */

int mav_TDMButtonQuery(int tracker, int button)
{
#ifdef MAV_TDM
  int bs[TDM_MAX_TRACKERS][TDM_MAX_BUTTONS];
  int rv;

  if (tracker>TDM_MAX_TRACKERS-1) 
  {
    if (mav_opt_output==MAV_VERBOSE) fprintf(stderr, "Warning: tracker not valid\n");
    rv=MAV_RELEASED;
  } 
  else if (button>TDM_MAX_BUTTONS-1)
  {
    if (mav_opt_output==MAV_VERBOSE) fprintf(stderr, "Warning: button not valid\n");
    rv=MAV_RELEASED;
  }
  else if (mavlib_TDM) 
  {
    /* Get TDM button status */
    TDM_getButtons(bs);
    
    rv= !bs[tracker][button]; /* NB TDM reports pressed=1, the opposite to Maverik and X */
  }
  else
  {    
    /* Dummy values if TDM not available */
    rv= MAV_RELEASED;
  }

  return rv;
#else
  return MAV_RELEASED;
#endif
}



/* Routine to get a trackers position */

MAV_TDMPos mav_TDMPosQuery(int tracker)
{
  MAV_TDMPos rv;
  
  /* Fill in with a default set of values */
  rv.pos.x= mavlib_TDM_xorigin;
  rv.pos.y= mavlib_TDM_yorigin;
  rv.pos.z= mavlib_TDM_zorigin;

  rv.u.x=1;
  rv.u.y=0;
  rv.u.z=0;

  rv.v.x=0;
  rv.v.y=1;
  rv.v.z=0;

  rv.n.x=0;
  rv.n.y=0;
  rv.n.z=1;

  rv.quaternion= MAV_ID_QUATERNION;
  rv.matrix= mav_matrixXYZSet(MAV_ID_MATRIX, rv.pos);

  /* Override them with real values if we can */
#ifdef MAV_TDM
  if (tracker>TDM_MAX_TRACKERS-1) 
  {
    if (mav_opt_output==MAV_VERBOSE) fprintf(stderr, "Warning: tracker not valid\n");
  } 
  else if (mavlib_TDM) 
  {  
    TDM_position tdmpos;

    /* Get TDM position */
    TDM_getPosition(tracker, &tdmpos);

    /* Convert between different structures */
    mavlib_tdm2mav(&rv, tdmpos);
  }
#endif

  return rv;
}



/* Routine to poll for the trackers positions */

void mavlib_TDM_poll(void)
{
  int i;
#if MAV_TDM
  for (i=0; i<TDM_MAX_TRACKERS; i++) mav_TDM_pos[i]= mav_TDMPosQuery(i);
#else
  for (i=0; i<4; i++) mav_TDM_pos[i]= mav_TDMPosQuery(i);
#endif
}



/* Routines to calculate the trackers world positions */

MAV_matrix mavlib_TDM_calcPos(MAV_TDMPos trk, MAV_matrix iv)
{
  MAV_vector shift;
  MAV_matrix rv;

  /* Calc trackers position, relative to eye, taking into account origin and scale */
  shift.x=(trk.pos.x-mavlib_TDM_xorigin)*mavlib_TDM_xscale;
  shift.y=(trk.pos.y-mavlib_TDM_yorigin)*mavlib_TDM_yscale;
  shift.z=(trk.pos.z-mavlib_TDM_zorigin)*mavlib_TDM_zscale;

  /* Give a bit of z shift to move the tracker origin away from the eye */
  shift.z-=mavlib_TDM_offset;

  /* Calculate trackers orientation */
  rv= mav_matrixMult(iv, mav_matrixXYZSet(trk.matrix, shift));

  return rv;
}

MAV_matrix mavlib_TDM_iv(void)
{
#if 0
  /* Calculate the inverse of the view matrix for the *modified* view vectors, ie. trans_up ... */
  return (mav_matrixInverse(mav_gfxMatrixGet()));
#else
  /* Calculate the inverse of the view matrix for the un-modified view vectors, ie. up ... */
  MAV_viewParams *vp;
  MAV_matrix iv, tr;

  vp= mav_win_current->vp;

  iv.mat[0][0]= vp->right.x;
  iv.mat[0][1]= vp->up.x;
  iv.mat[0][2]= -vp->view.x;
  iv.mat[0][3]= 0.0;

  iv.mat[1][0]= vp->right.y;
  iv.mat[1][1]= vp->up.y;
  iv.mat[1][2]= -vp->view.y;
  iv.mat[1][3]= 0.0;

  iv.mat[2][0]= vp->right.z;
  iv.mat[2][1]= vp->up.z;
  iv.mat[2][2]= -vp->view.z;
  iv.mat[2][3]= 0.0;

  iv.mat[3][0]= 0.0;
  iv.mat[3][1]= 0.0;
  iv.mat[3][2]= 0.0;
  iv.mat[3][3]= 1.0;

  tr= mav_matrixXYZSet(MAV_ID_MATRIX, mav_vectorScalar(vp->eye, -1.0));
  iv= mav_matrixMult(iv, tr);

  return (mav_matrixInverse(iv));
#endif
}

void mavlib_TDM_calc(void)
{
  MAV_matrix iv;
  int i;

  if (mav_win_current && mav_win_current->vp) {
    
    /* Calculate inverse of view matrix */
    iv= mavlib_TDM_iv();

    /* For each tracker */
#ifdef MAV_TDM
    for (i=0; i<TDM_MAX_TRACKERS; i++) mav_TDM_matrix[i]= mavlib_TDM_calcPos(mav_TDM_pos[i], iv);
#else
    for (i=0; i<4; i++) mav_TDM_matrix[i]= mavlib_TDM_calcPos(mav_TDM_pos[i], iv);
#endif
  }
}



/* Routine to check for, and act upon, tracker event */

int mavlib_TDM_event(void)
{
  int rv=0;

#ifdef MAV_TDM
  if (mavlib_TDM) {
    TDM_buttonEvent mavlib_tdm_event;
    if (TDM_getEvent(&mavlib_tdm_event)) rv= mavlib_dealWithTDMEvent(&mavlib_tdm_event);
  }
#endif

  return rv;
}



/* Routines to initialise the module */

char *mav_TDMModuleID(void)
{
  return "TDM";
}

int mav_TDMModuleInit(void)
{
  /* Add the new module */
  mav_moduleNew(mav_TDMModuleID);

  mavlib_TDM= MAV_FALSE;

#ifdef MAV_TDM
  /* Connect to TDM */
  if (TDM_init()) mavlib_TDM= MAV_TRUE;

  if (mavlib_TDM==MAV_FALSE) {
    if (mav_opt_output==MAV_VERBOSE) fprintf(stderr, "Warning: TDM initialisation failed, ignoring\n");
  }
#else
  if (mav_opt_output==MAV_VERBOSE) fprintf(stderr, "Warning: code not compiled with TDM option, ignoring\n");
#endif

  /* Add the trackers as new devices */
  mav_deviceNew(mavlib_TDM_poll, mavlib_TDM_calc, mavlib_TDM_event);  

  /* Define new callbacks for events */
  mav_callback_TDM= mav_callbackNew();
  mav_callback_sysTDM= mav_callbackNew();

  /* Define new cursor object */
  mav_class_TDMCursor= mav_classNew();
  mav_callbackDrawSet(mav_win_all, mav_class_TDMCursor, mav_TDMCursorDraw);
  mav_callbackBBSet(mav_win_all, mav_class_TDMCursor, mav_TDMCursorBB);
  mav_callbackIDSet(mav_win_all, mav_class_TDMCursor, mav_TDMCursorID);
  mav_callbackGetUserdefSet(mav_win_all, mav_class_TDMCursor, mav_TDMCursorGetUserdef);
  mav_callbackGetSurfaceParamsSet(mav_win_all, mav_class_TDMCursor, mav_TDMCursorGetSurfaceParams);
  mav_callbackDumpSet(mav_win_all, mav_class_TDMCursor, mav_TDMCursorDump);

  /* Initialise the cursor shapes */
  mavlib_TDM_cursorInit();

  return 1;
}



/* Routines to set TDM origin and scale */

void mav_TDMXYZScaleSet(float x, float y, float z)
{
  mavlib_TDM_xscale= x;
  mavlib_TDM_yscale= y;
  mavlib_TDM_zscale= z;
}

void mav_TDMXYZOriginSet(float x, float y, float z)
{
  mavlib_TDM_xorigin= x;
  mavlib_TDM_yorigin= y;
  mavlib_TDM_zorigin= z;
}

void mav_TDMOffsetSet(float off)
{
  mavlib_TDM_offset= off;
}

void mav_TDMDrawScaleSet(float sc)
{
  mavlib_TDM_drawScale= sc;
}

void mav_TDMScaleSet(float sc)
{
  mavlib_TDM_xscale= sc;
  mavlib_TDM_yscale= sc;
  mavlib_TDM_zscale= sc;
  mavlib_TDM_drawScale= sc;
  mavlib_TDM_offset= 20.0*sc;
}
