//
// forcedevice_test_client.cpp - Program to test out the various functions
//	of a vrpn_ForceDevice server, inspired by Randy Heiland.  First,
//	it generates a box within which the user can feel around; the walls
//	have different characteristics.  It later generates force fields
//	and instant buzzing effects.
//
//	It is meant to test devices that have a button and tracker associated
//	with them, because it uses these devices to set coordinate systems and
//	locations for effects.
//

#include <stdlib.h>
#include <stdio.h>
#include <vrpn_ForceDevice.h>
#include <vrpn_Tracker.h>
#include <vrpn_Button.h>

/*****************************************************************************
 *
   Classes and types and such
 *
 *****************************************************************************/

// What type of effect the application is presenting
typedef	enum { box, pointconstraint, lineconstraint, planeconstraint, forcefield, buzzing, geometry, quit } APP_STATE;

// Which Object ID for the cube geometry object
vrpn_int32  CUBE_ID = 0;

// For the display of the box, this holds a descripton of the paramters
// for each side.
class BoxSide {
public:
  BoxSide(double ox, double oy, double oz,
	  double nx, double ny, double nz,
	  double sMult, double fMult, double dMult)
  { d_oX = (float)ox; d_oY = (float)oy; d_oZ = (float)oz;
    d_nX = (float)nx; d_nY = (float)ny; d_nZ = (float)nz;
    d_sMult = (float)sMult;
    d_fMult = (float)fMult;
    d_dMult = (float)dMult;
  }

  float	d_oX;	  //< Offset from center in X direction
  float	d_oY;	  //< Offset from center in Y direction
  float	d_oZ;	  //< Offset from center in Z direction

  float	d_nX;	  //< Normal in X;
  float	d_nY;	  //< Normal in Y;
  float	d_nZ;	  //< Normal in Z;

  float	d_sMult;  //< (signed) How many multiples of change in spring constant to add
  float	d_fMult;  //< (signed) How many multiples of change in friction to add
  float	d_dMult;  //< (signed) How many multiples of change in damping to add
};

/*****************************************************************************
 *
   Global variables
 *
 *****************************************************************************/

// Force device client to use to control the server.
static	vrpn_ForceDevice_Remote *g_forceDevice;

// Standard stiffness of a surface, and factor to multiply/divide by when it varies
static	float g_kSpring = (float)0.5;	  // Unit is unknown, seems to be range is 0 < value <= 1.
static	float g_kSpringMult = 2;

// Standard damping of a surface, and factor to multiply/divide by when it varies
static	float g_kDamping = (float)0.001;	  // Unit is dynes*sec/cm.  Range is 0 <= value <= 0.005
static	float g_kDampingMult = 2;

// Standard values for static and dynamic friction
// Factors by which to multiple/divide static and dynamic friction when they vary
static  float g_sFric = (float)0.2;	  // Unit is fraction of normal force, range is 0 <= value <= 1
//static	float g_dFric = (float)0.2;	  // Unit is fraction of normal force, range is 0 <= value <= 1, dynamic <= static
static	float g_sFricMult = (float)2;
//static	float g_dFricMult = (float)2;

// State of the application (what should be generated now).
static	APP_STATE g_state = box;	  //< What mode are we in?
static	bool	  g_active = false;	  //< Is the current mode active?

// Current position of the device
static	float	g_xPos = (float)0.0;
static	float	g_yPos = (float)0.0;
static	float	g_zPos = (float)0.0;

// Center position for the current effect.
static	float	g_xCenter = g_xPos;
static	float	g_yCenter = g_yPos;
static	float	g_zCenter = g_zPos;

// Box side descriptions.  They are in the following order: -X. -Y. -Z. +X. +Y, +Z.
// They are offset by 2cm from the center.  Their characteristics match those
// found in the button-press description message.
static	BoxSide	g_box[6] = {  BoxSide(-0.02, 0.00, 0.00,  1, 0, 0,  -1, 0, 0),
			      BoxSide( 0.00,-0.02, 0.00,  0, 1, 0,   0,-1, 0),
			      BoxSide( 0.00, 0.00,-0.02,  0, 0, 1,   0, 0,-1),
			      BoxSide( 0.02, 0.00, 0.00, -1, 0, 0,   1, 0, 0),
			      BoxSide( 0.00, 0.02, 0.00,  0,-1, 0,   0, 1, 0),
			      BoxSide( 0.00, 0.00, 0.02,  0, 0,-1,   0, 0, 1) };

// Stiffness of the point, line and plane constraint
static	float g_pointConstraintStiffness = (float)100.0;
static  float g_lineConstraintStiffness = (float) 300.0;
static  float g_planeConstraintStiffness = (float) 500.0;

// Strength and variability of the force field
static	float g_forceFieldStrength = (float)0.0;
static	float g_forceFieldVaries = (float)20.0;
static	float g_forceFieldRadius = (float)0.3;

/*****************************************************************************
 *
   Callback handlers
 *
 *****************************************************************************/

void    VRPN_CALLBACK handle_force_change(void *userdata, const vrpn_FORCECB f)
{
/*XXX
  static vrpn_FORCECB lr;        // last report
  static int first_report_done = 0;

  if ((!first_report_done) ||
    ((f.force[0] != lr.force[0]) || (f.force[1] != lr.force[1])
      || (f.force[2] != lr.force[2]))) {
    printf("force is (%f,%f,%f)\n", f.force[0], f.force[1], f.force[2]);
  }

  first_report_done = 1;
  lr = f;
  XXX*/
}


void    VRPN_CALLBACK handle_tracker_change(void *userdata, const vrpn_TRACKERCB t)
{
  // Record the current position of the device in global variables
  // so that the button routine can know where to store the center
  // position.
  g_xPos = (float)t.pos[0];
  g_yPos = (float)t.pos[1];
  g_zPos = (float)t.pos[2];

  // This is the workhorse function for the program.

  // If we are not active, then don't do anything.
  if (!g_active) { return; }

  // Depending on the
  // current state of the application, it generates the appropriate
  // effect.

  switch (g_state) {
    case box:
      // We let the user feel around a box whose sides have different
      // characteristics as described in the print statements in the
      // button-press code.
      {
	// Figure out which side of the box to use.  The index we are computing
	// should have 0 = negative X from center, 1 = negative Y from center,
	// 2 = negative Z, 3 = +X, 4 = +Y, 5 = +Z.  This is determined based
	// on which wall we are closest to touching.
	double  dx = g_xPos - g_xCenter;
	double  dy = g_yPos - g_yCenter;
	double  dz = g_zPos - g_zCenter;

	double	magx = fabs(dx);
	double	magy = fabs(dy);
	double	magz = fabs(dz);

	double	magmax = magx;
	if (magy > magmax) { magmax = magy; }
	if (magz > magmax) { magmax = magz; }

	int index;

	if (magx == magmax) {
	  if (dx < 0) {
	    index = 0;      // X largest mag, cursor in -X from center
	  } else {
	    index = 3;      // X largest mag, cursor in +X from center
	  }
	}
	if (magy == magmax) {
	  if (dy < 0) {
	    index = 1;      // Y largest mag, cursor in -Y from center
	  } else {
	    index = 4;      // Y largest mag, cursor in +Y from center
	  }
	}
	if (magz == magmax) {
	  if (dz < 0) {
	    index = 2;      // Z largest mag, cursor in -Z from center
	  } else {
	    index = 5;      // Z largest mag, cursor in +Z from center
	  }
	}

	// Find the A,B,C coefficients for the plane, which are just
	// the normal pulled from the box side descriptor
	float a = g_box[index].d_nX;
	float b = g_box[index].d_nY;
	float c = g_box[index].d_nZ;

	// Find a point on the plane, one of which is located at the
	// box wall's offset from the center location.  We're going to
	// need this to solve for D in the plane equation.
	float ppx = g_xCenter + g_box[index].d_oX;
	float ppy = g_yCenter + g_box[index].d_oY;
	float ppz = g_zCenter + g_box[index].d_oZ;

	// Find the value of D in the plane equation that makes it zero
	// at the point on the plane we just found above: this requires
	// D = - (Ax + By + Cz) for that point
	float d = - ( a*ppx + b*ppy + c*ppz );

	// Set plane parameters.  First, the plane equation and then the
	// surface parameters.  We raise the multiplier to the power of
	// the box-description multiplier and then apply this to the surface
	// parameter to vary each for different sides.  A box parameter of
	// 0 will therefore cause no change (multiply by one), a paramter of
	// -1 will reduce by the factor and a parameter of 1 will increase
	// by the parameter.  NOTE: Static and dynamic friction are set equal.
	g_forceDevice->set_plane(a, b, c, d);
	float	spring = g_kSpring, springmult = g_box[index].d_sMult;
	if (springmult ==  1) { spring *= g_kSpringMult; }
	if (springmult == -1) { spring /= g_kSpringMult; }
	g_forceDevice->setSurfaceKspring(spring);
	float	damping = g_kDamping, dampingmult = g_box[index].d_dMult;
	if (dampingmult ==  1) { damping *= g_kDampingMult; }
	if (dampingmult == -1) { damping /= g_kDampingMult; }
	g_forceDevice->setSurfaceKdamping(damping);
	float	friction = g_sFric, frictionmult = g_box[index].d_fMult;
	if (frictionmult ==  1) { friction *= g_sFricMult; }
	if (frictionmult == -1) { friction /= g_sFricMult; }
	g_forceDevice->setSurfaceFstatic(friction);
	g_forceDevice->setSurfaceFdynamic(friction);

	// texture and buzzing stuff:
	// this turns off buzzing and texture
	g_forceDevice->setSurfaceBuzzAmplitude(0.0);
	g_forceDevice->setSurfaceBuzzFrequency(60.0); // Hz
	g_forceDevice->setSurfaceTextureAmplitude(0.00); // meters!!!
	g_forceDevice->setSurfaceTextureWavelength((float)0.01); // meters!!!

	g_forceDevice->setRecoveryTime(10);	// recovery occurs over 10
					  // force update cycles

	// enable force device and send surface.
	g_forceDevice->startSurface();
      }
      break;

    case pointconstraint:
      // We produce a point constraint that pulls the user back towards the place where
      // they pressed the button.  The strength of this constraint is stored in a global. 
      {
	vrpn_float32  center[3] = { g_xCenter, g_yCenter, g_zCenter };
	g_forceDevice->setConstraintMode(vrpn_ForceDevice::POINT_CONSTRAINT);
	g_forceDevice->setConstraintKSpring(g_pointConstraintStiffness);
	g_forceDevice->setConstraintPoint(center);
	g_forceDevice->enableConstraint(1);
      }
      break;

    case lineconstraint:
      // We produce a point constraint that pulls the user back towards the place where
      // they pressed the button.  The strength of this constraint is stored in a global. 
      {
	vrpn_float32  center[3] = { g_xCenter, g_yCenter, g_zCenter };
	vrpn_float32  direction[3] = { 0, 0, 1};
	g_forceDevice->setConstraintMode(vrpn_ForceDevice::LINE_CONSTRAINT);
	g_forceDevice->setConstraintKSpring(g_lineConstraintStiffness);
	g_forceDevice->setConstraintLinePoint(center);
	g_forceDevice->setConstraintLineDirection( direction );
	g_forceDevice->enableConstraint(1);
      }
      break;

    case planeconstraint:
      // We produce a point constraint that pulls the user back towards the place where
      // they pressed the button.  The strength of this constraint is stored in a global. 
      {
	vrpn_float32  center[3] = { g_xCenter, g_yCenter, g_zCenter };
	vrpn_float32  direction[3] = { 0, 0, 1};
	g_forceDevice->setConstraintMode(vrpn_ForceDevice::PLANE_CONSTRAINT);
	g_forceDevice->setConstraintKSpring(g_planeConstraintStiffness);
	g_forceDevice->setConstraintPlanePoint(center);
	g_forceDevice->setConstraintPlaneNormal( direction );
	g_forceDevice->enableConstraint(1);
      }
      break;
   
    case forcefield:
      // Produce a force field pointing in +Z, starting at the center position.  As the
      // user moves more in the +Z direction, the field will get smaller; larger as they
      // move in -Z.
      {
	vrpn_float32  center[3] = { g_xCenter, g_yCenter, g_zCenter };
	vrpn_float32  force[3] = { 0, 0, g_forceFieldStrength };
	vrpn_float32  jacobian[3][3] = { { 0, 0, 0 }, {0, 0, 0}, {g_forceFieldVaries, 0, 0} };
	g_forceDevice->sendForceField(center, force, jacobian, g_forceFieldRadius);
      }
      break;

    case buzzing:
      {
	// Black magic dredged up from vrpn_Phantom.C
	vrpn_float32  amplitude;    //< 
	vrpn_float32  frequency;    //< Frequency of the buzzing
	vrpn_float32  duration;	    //< Duration of the effect
	vrpn_float32  direction[3]; //< Normalized?
	vrpn_float32  params[6];    //< In the order shown above
	vrpn_int32    effectID = 0; //< 0 means "instant buzzing effect"

	// We only send this every half second; the duration of the effect is
	// 1/4 second.
	{ static  struct timeval  last_sent = {0,0};
	  struct  timeval now;
	  gettimeofday(&now, NULL);
	  if (vrpn_TimevalMsecs(vrpn_TimevalDiff(now, last_sent)) < 500) { break; }
	  last_sent = now;
	}

	// See how far we are from the center in meters.
	vrpn_float32  offset[3] = { g_xPos - g_xCenter, g_yPos - g_yCenter, g_zPos - g_zCenter };

	// Adjust the amplitude based on motion in X
	amplitude = (float)(0.5 + 0.5 * (offset[0] / 0.2));
	if (amplitude < 0) { amplitude = 0; }

	// Adjust the frequency based on motion in Y
	frequency = (float)(200 + 200 * (offset[1] / 0.2));
	if (frequency < 1) { frequency = 1; }

	// Duration is 1/4 second, send twice per second!
	duration = (float)0.25;

	// Normalize the direction from the center and put it into direction.
	// Set the direction to +Z if the probe is at the center.
	vrpn_float32  length = (float)sqrt(offset[0]*offset[0] + offset[1]*offset[1] + offset[2]*offset[2]);
	if (length == 0) {
	  direction[0] = direction[1] = 0; direction[2] = 1.0;
	} else {
	  direction[0] = offset[0] / length;
	  direction[1] = offset[1] / length;
	  direction[2] = offset[2] / length;
	}

	params[0] = amplitude;
	params[1] = frequency;
	params[2] = duration;
	params[3] = direction[0];
	params[4] = direction[1];
	params[5] = direction[2];
	g_forceDevice->setCustomEffect(effectID, params, 6);
	g_forceDevice->startEffect();
      }
      break;

    case geometry:
      {
	// Only do this creation once!
	static bool first_time = true;
	if (first_time) {
	  first_time = false;
	} else {
	  break;
	}

	const vrpn_float32  halfwidth = static_cast<vrpn_float32>(0.02);
	vrpn_float32  left = g_xCenter - halfwidth;
	vrpn_float32  right = g_xCenter + halfwidth;
	vrpn_float32  top = g_yCenter - halfwidth;
	vrpn_float32  bottom = g_yCenter + halfwidth;
	vrpn_float32  front = g_zCenter - halfwidth;
	vrpn_float32  back = g_zCenter + halfwidth;

	// Create the cube object, rooted in the world.
	g_forceDevice->useGhost();
	g_forceDevice->addObject(CUBE_ID);

	// Create a set of points at the cube corners.
	const vrpn_int32 LBF = 0, RBF = 1, LTF = 2, RTF = 3,
			 LBB = 4, RBB = 5, LTB = 6, RTB = 7;
	g_forceDevice->setObjectVertex(CUBE_ID, LBF, left, bottom, front);
	g_forceDevice->setObjectVertex(CUBE_ID, RBF, right, bottom, front);
	g_forceDevice->setObjectVertex(CUBE_ID, LTF, left, top, front);
	g_forceDevice->setObjectVertex(CUBE_ID, RTF, right, top, front);
	g_forceDevice->setObjectVertex(CUBE_ID, LBB, left, bottom, back);
	g_forceDevice->setObjectVertex(CUBE_ID, RBB, right, bottom, back);
	g_forceDevice->setObjectVertex(CUBE_ID, LTB, left, top, back);
	g_forceDevice->setObjectVertex(CUBE_ID, RTB, right, top, back);

	// Create a set of triangles that are the cube faces.
	// REMEMBER to use the right-hand rule for creating of the
	// triangles: the normal points out of that face.

	// Front
	g_forceDevice->setObjectTriangle(CUBE_ID, 0, LBF, LTF, RBF);
	g_forceDevice->setObjectTriangle(CUBE_ID, 1, RBF, LTF, RTF);

	// Back
	g_forceDevice->setObjectTriangle(CUBE_ID, 2, LBB, RBB, LTB);
	g_forceDevice->setObjectTriangle(CUBE_ID, 3, RBB, RTB, LTB);

	// Left
	g_forceDevice->setObjectTriangle(CUBE_ID, 4, LBF, LBB, LTF);
	g_forceDevice->setObjectTriangle(CUBE_ID, 5, LTF, LBB, LTB);

	// Right
	g_forceDevice->setObjectTriangle(CUBE_ID, 6, RBF, RTF, RBB);
	g_forceDevice->setObjectTriangle(CUBE_ID, 7, RTF, RTB, RBB);

	// Top
	g_forceDevice->setObjectTriangle(CUBE_ID, 8, LTF, LTB, RTF);
	g_forceDevice->setObjectTriangle(CUBE_ID, 9, RTF, LTB, RTB);

	// Bottom
	g_forceDevice->setObjectTriangle(CUBE_ID, 10, LBF, RBF, LBB);
	g_forceDevice->setObjectTriangle(CUBE_ID, 11, RBF, RBB, LBB);

	// Push all of the changes and make them active.
	g_forceDevice->updateObjectTrimeshChanges(CUBE_ID);
      }
      break;

    default:
      break;
  };
}

void	VRPN_CALLBACK handle_button_change(void *userdata, const vrpn_BUTTONCB b)
{
  // When the button is pressed, start things going for the state
  // we are in and tell that the forces are active.
  // When the button is released, switch to a new state and set things
  // as they need to be set for that state.  Also, tell that the
  // forces are inactive.
  if (b.state) {

    // Forces becoming active in the current state
    g_active = true;

    // Set the location at which the button was pressed based on
    // the last tracker position so that effects which depend on
    // it can know.
    g_xCenter = g_xPos;
    g_yCenter = g_yPos;
    g_zCenter = g_zPos;

    // Set things up to generate forces in the state we are in
    switch (g_state) {
      case box:
	printf("  -X wall will have low spring constant\n");
	printf("  +X wall will have high spring constant\n");
	printf("  -Y wall will have low friction\n");
	printf("  +Y wall will have high friction\n");
	printf("  -Z wall will have low damping\n");
	printf("  +Z wall will have high damping\n");
	printf("\n");
	printf("Release button to stop feeling inside box\n");
	break;

      case pointconstraint:
	printf("  You will be pulled back towards where you pressed the button\n");
	printf("\n");
	printf("Release button to stop point constraint\n");
	break;

      case lineconstraint: 
	printf("  You will be able to pull along a line in the z direction\n");
	printf("\n");
	printf("Release button to stop line constraint\n");
	break;
   
      case planeconstraint: 
	printf("  You will be able to pull along the x-y plane\n");
	printf("\n");
	printf("Release button to stop plane constraint\n");
	break;

      case forcefield:
	printf("  You will be pushed in Z while you are within the field\n");
	printf("  The push is in +Z when you move in +X, in -Z when you move in -X\n");
	printf("\n");
	printf("Release button to stop force field\n");
	break;

      case buzzing:
	printf("  Amplitude will increase as you move in +X, decrease as you move in -X\n");
	printf("  Frequency will increase as you move in +Y, decrease as you move in -Y\n");
	printf("  Direction will be towards where the button was pressed\n");
	printf("\n");
	printf("Release button to stop buzzing\n");
	break;

      case geometry:
	printf("  A cube 4cm on a side will be created, centered at your start location.\n");
	printf("\n");
	printf("Release button to stop feeling the cube\n");
	break;

      default:
	break;
    };
  } else {

    // Not doing any forces until button pressed again.
    g_active = false;

    // Move from the state we are in to the next state.
    switch (g_state) {
      case box:
	g_forceDevice->stopSurface();
	printf("\n");
	printf("Press button to start point constraint\n");
	g_state = pointconstraint;
	break;

      case pointconstraint:
	g_forceDevice->enableConstraint(0);
	printf("\n");
	printf("Press button to line constraint\n");
	g_state = lineconstraint;
	break;

      case lineconstraint:
	g_forceDevice->enableConstraint(0);
	printf("\n");
	printf("Press button to start plane constraint\n");
	g_state = planeconstraint;
	break;

      case planeconstraint:
	g_forceDevice->enableConstraint(0);
	printf("\n");
	printf("Press button to start force field\n");
	g_state = forcefield;
	break;

      case forcefield:
	g_forceDevice->stopForceField();
	printf("\n");
	printf("Press button to start buzzing\n");
	g_state = buzzing;
	break;

   
      case buzzing:
	g_forceDevice->stopEffect();
	printf("\n");
	printf("Press button to start geometry cube!\n");
	g_state = geometry;
	break;

      case geometry:
	g_forceDevice->removeObject(CUBE_ID);
	printf("\n");
	printf("Qutting program!\n");
	g_state = quit;
	break;

      default:
	g_state = quit;
	break;
    };
  }
}

int main(int argc, char *argv[])
{
  vrpn_Tracker_Remote *tracker;
  vrpn_Button_Remote *button;

  if (argc != 2) {
    printf("Usage: %s device_name\n",argv[0]);
    printf("   Example: %s Phantom@localhost\n",argv[0]);
    exit(-1);
  }

  char *device_name = argv[1];
  printf("Connecting to %s:\n",device_name);

  /* initialize the force device */
  g_forceDevice = new vrpn_ForceDevice_Remote(device_name);
  g_forceDevice->register_force_change_handler(NULL, handle_force_change);

  /* initialize the tracker */
  tracker = new vrpn_Tracker_Remote(device_name);
  tracker->register_change_handler(NULL, handle_tracker_change);

  /* initialize the button */
  button = new vrpn_Button_Remote(device_name);
  button->register_change_handler(NULL, handle_button_change);

  // Wait until we get connected to the server.
  while (!g_forceDevice->connectionPtr()->connected()) {
      g_forceDevice->mainloop();
  }

  printf("Press forceDevice's first button to begin feeling inside a box\n");

  // main loop
  while ( g_state != quit )
  {
    // Let tracker receive position information from remote tracker
    tracker->mainloop();

    // Let button receive button status from remote button
    button->mainloop();

    // Let the forceDevice send its planes to remote force device
    g_forceDevice->mainloop();
  }

  delete tracker;
  delete button;
  delete g_forceDevice;

  return 0;
}   /* main */

