#include <stdio.h>
#include "../h/cdefs.h"
#include "../h/pendant.h"
#include "robotsim.h"

#define EPSILON 1e-8

/* ----- Generic serial line unit procedures ------ */

initSpecialIO (r)
RobotDesc *r;
{
	r->sio.forceAvail = 0;
	r->sio.torqueAvail = 1;
	/* teleop is not available now because the RCI code expects
	   an array of floats, and we need to hack around a bit to make
	   this portable. */
	r->sio.teleopAvail = 0;
	r->sio.jstickAvail = 0;
	r->sio.potsAvail = 1;
	r->sio.pendantAvail = 1;

	initForce(r);
	initTorque(r);
	initTeleop(r);
	initJstick(r);
	initPendant(r);
}

/* Force sensor routines */

initForce(r)
RobotDesc *r;
{
	r->sio.forceTscale = 1.0;
	r->sio.forceRscale = 1.0;
}

startForce(r)
RobotDesc *r;
{
	if (r->sio.forceAvail)
	 { r->sio.forceOn = 1;
	 }
}

stopForce(r)
RobotDesc *r;
{
	r->sio.forceOn = 0;
}

getForce(r, buf)
RobotDesc *r;
short *buf;
{
	int j;

	if (r->sio.forceOn)
	 { buf[0] = r->sio.forceTscale * r->rs.extForce.f.x;
	   buf[1] = r->sio.forceTscale * r->rs.extForce.f.y;
	   buf[2] = r->sio.forceTscale * r->rs.extForce.f.z;

	   buf[3] = r->sio.forceTscale * r->rs.extForce.m.x;
	   buf[4] = r->sio.forceTscale * r->rs.extForce.m.y;
	   buf[5] = r->sio.forceTscale * r->rs.extForce.m.z;
	 }
	return (1);
}

/* Torque sensor routines */

initTorque (r)
RobotDesc *r;
{
}

startTorque (r)
RobotDesc *r;
{
	if (r->sio.torqueAvail)
	 { r->sio.torqueOn = 1;
	 }
}

stopTorque (r)
RobotDesc *r;
{
	r->sio.torqueOn = 0;
}

getTorque(r, buf)
RobotDesc *r;
short *buf;
{
	float ADC_to_DAC;
	int j;

	if (r->sio.torqueOn)
	 { for (j=0; j<r->jls->numJ; j++)
	    { ADC_to_DAC = r->jls->currentToMtorque[j]*
			   r->jls->mtorqueToCurrent[j];
	      if (ABS(ADC_to_DAC) > EPSILON)
	       { buf[j] = (r->rs.jDACvalues[j] - r->jls->currentBias[j]) /
			   ADC_to_DAC;
	       }
	      else
	       { buf[j] = 0;
	       }
	    }
	   return (1);
	 }
	else
	 { return (0);
	 }
}

/* teleop I/O routines */

initTeleop (r)
RobotDesc *r;
{
}

startTeleop (r)
RobotDesc *r;
{
	if (r->sio.teleopAvail)
	 { r->sio.teleopOn = 1;
	 }
}

stopTeleop (r)
RobotDesc *r;
{
	r->sio.teleopOn = 0;
}

getTeleop(r, buf)
RobotDesc *r;
short *buf;
{
	int j;

	if (r->sio.teleopOn)
	 { for (j=0; j<6; j++)
	    { buf[j] = r->sio.teleopBuf[j];
	    }
	 }
}

/* joystick I/O routines */

initJstick(r)	/* called once by when moper initializes */
RobotDesc *r;
{
}

startJstick (r)
RobotDesc *r;
{
	if (r->sio.jstickAvail)
	 { r->sio.jstickOn = 1;
	 }
}

stopJstick (r)
RobotDesc *r;
{
	r->sio.jstickOn = 0;
}

getJstick (r, buf)
RobotDesc *r;
unsigned short *buf;
{
	int j;

	if (r->sio.jstickOn)
	 { for (j=0; j<6; j++)
	    { buf[j] = r->sio.jstickBuf[j];
	    }
	 }
	return (1);
}

/* getPots is handled by the getj() command */

/* pendant I/O routines */

initPendant(r)
RobotDesc *r;
{
}

startPendant(r)
RobotDesc *r;
{
	if (r->sio.pendantAvail)
	 { r->sio.pendantOn = 1;
	 }
}

stopPendant(r)
RobotDesc *r;
{
	r->sio.pendantOn = 0;
}

servicePendant(r)
RobotDesc *r;
{
}

getPendant(r, buf)
RobotDesc *r;
short *buf;
{
	if (r->sio.pendantOn)
	 { int i;
	   for (i=0; i<PDNT_VBUFSIZE; i++)
	    { buf[i] = r->sio.pendantVBuf[i];
	    }
	   return (1);
	 }
	else
	 { return (0);
	 }
}

setPendant(r,new)
RobotDesc *r;
unsigned short new;
{
	unsigned short old = r->sio.pendantWBuf;
	updateGraphicPendant(r, new, old);
	r->sio.pendantWBuf = new;
}

/*---------------------------------------------------------------------------
 * SIMULATOR support routines: used by the simulator emulation code
 *--------------------------------------------------------------------------*/

setTeleop (r, num, value)
RobotDesc *r;
int num, value;
{
	if (num >= 0 && num < 6)
	 { r->sio.teleopBuf[num] = value;
	 }
}

setJstick (r, num, value)
RobotDesc *r;
int num, value;
{
	if (num >= 0 && num < 6)
	 { r->sio.jstickBuf[num] = value;
	 }
}

/* simulator support routines for the teach pendant */

struct button {
	char *name;
	int code, word; } buttons[] =
 { "REC", 	RECORD, 	0,
   "hold-run",  HOLD_RUN1, 	0,
   "x-",	X_MINUS,	0,
   "x+",	X_PLUS,		0,
   "y-",	Y_MINUS	,	0,
   "y+",	Y_PLUS	,	0,
   "z-",	Z_MINUS	,	0,
   "z+",	Z_PLUS	,	0,
   "rx-",	RX_MINUS,	0,
   "rx+",	RX_PLUS	,	0,
   "ry-",	RY_MINUS,	0,
   "ry+",	RY_PLUS	,	0,
   "rz-",	RZ_MINUS,	0,
   "rz+",	RZ_PLUS	,	0,
   "clamp1",	CLAMP1	,	0,
   "clamp2",	CLAMP2	,	0,
   "tool",	(TOOL_BUTTON>>16),	1,
   "world",	(WORLD_BUTTON>>16),	1,
   "joint",	(JOINT_BUTTON>>16),	1,
   "free",	(FREE_BUTTON>>16),	1,
   "comp",	(COMP>>16),		1,
   "tp+",	(TP_MINUS>>16),		1,
   "tp-",	(TP_PLUS>>16),		1,
   "next-step", NEXT_STEP,	2,
   "move-tp",	MOVE_TP,	2,
   "pound",	POUND_SIGN,	2,
 };

/* ----- Functions with which the pendant is 'set' ----- */

pendantButtonHit(r, s)
RobotDesc *r;
char *s;
{
	/* simulate hitting the pendant button described by the string 's' */

	struct button *bp;	
	int i;

	if (!r->sio.pendantButPtr)
	 { for (i=0; i<ARRAY_SIZE(buttons); i++)
  	    { if (strcmp(s, buttons[i].name) == 0)
	       { bp = &buttons[i];
		 r->sio.pendantVBuf[bp->word] |= bp->code;
		 r->sio.pendantButPtr = (void*)bp;
		 return;
	       }
	    }
	 }
	if (strcmp (s, "tick") == 0)
  	 { if (r->sio.pendantFlags & PDNT_TICK_SET)
	    { r->sio.pendantFlags &= ~PDNT_TICK_SET;
	    }
	   else
	    { r->sio.pendantFlags |= PDNT_TICK_SET;
	    }
	 }
	else if (strcmp (s, "slow") == 0)
  	 { r->sio.pendantSpeed = 0;
	 }
	else if (strcmp (s, "fast") == 0)
	 { r->sio.pendantSpeed = 2;
	 }
	else if (strcmp (s, "medium") == 0)
	 { r->sio.pendantSpeed = 1;
	 }
	
	if (r->sio.pendantFlags & PDNT_TICK_SET)
  	 { switch (r->sio.pendantSpeed)
	    { case 0: i = (TICK_ON_LOW>>16);
		      break;
	      case 1: i = (TICK_ON_MED>>16);
		      break;
	      case 2: i = (TICK_ON_HIGH>>16);
		      break;
	    }
	 }
 	else
  	 { switch (r->sio.pendantSpeed)
	    { case 0: i = (TICK_OFF_LOW>>16);
		      break;
	      case 1: i = (TICK_OFF_MED>>16);
		      break;
	      case 2: i = (TICK_OFF_HIGH>>16);
		      break;
	    }
	 }
	r->sio.pendantVBuf[1] = (r->sio.pendantVBuf[1] & 0xff) | i;
}

pendantReset(r)
RobotDesc *r;
{
	r->sio.pendantVBuf[0] = 0;
	r->sio.pendantVBuf[1] = (TICK_OFF_MED>>16);
	r->sio.pendantVBuf[2] = 0;
	r->sio.pendantFlags = 0;
	r->sio.pendantSpeed = 1;
	r->sio.pendantButPtr = 0;
}

pendantButtonRelease(r)
RobotDesc *r;
{
	/* simulate releasing the current pendant button */

	struct button *bp;

	if (bp = (struct button*)r->sio.pendantButPtr)
  	 { r->sio.pendantVBuf[bp->word] &= ~bp->code;
	   r->sio.pendantButPtr = 0;
	 }
}

pendantWord(r,j)
RobotDesc *r;
int j;
{
	return (r->sio.pendantVBuf[j]);
}
