#include "../h/cdefs.h"
#include "manipIO.h"

#include "robotsim.h"

initRobotIO(r)
RobotDesc *r;
{
	r->rs.baseClkRate = 28;
	r->rs.servoRate = 9;
}

putj(r, command, value, j)
RobotDesc *r;
unsigned short command;
unsigned short value;
int j;
{
	switch (command)
	 { case POSET:
	    { r->rs.com[j] = POSITION;

	      if (r->rs.stat[j] & JFNDIXS)
	       { break;
	       }
	      r->rs.setp[j] = value;
	      if (r->rs.mode[j] & MLOOKZI)
	       { /* find distance to nearest Zindex positions on either side */

		 int deltaRef, deltaLow, deltaHigh, delta;

		 deltaRef = r->rs.enc[j] - (r->jls->encCalvec[j] - r->jls->encOffset[j]);
		 if (deltaRef > 0)
		  { deltaLow = deltaRef % r->jls->encWheelSize[j];
		    deltaHigh = r->jls->encWheelSize[j] - deltaLow;
		  }
		 else
		  { deltaHigh = (-deltaRef) % r->jls->encWheelSize[j];
		    deltaLow = r->jls->encWheelSize[j] - deltaHigh;
		  }

		 /* Now see if either Zindex mark has been crossed */

		 delta = value - r->rs.enc[j];

		 if (j==0)
		  { printf ("deltaHigh %06o  deltaLow %06o  delta %06o\n",
			     deltaHigh, deltaLow, delta);
		  }

		 if (delta > 0 && delta >= deltaHigh)
		  { r->rs.setp[j] = r->rs.enc[j] + deltaHigh;
		    r->rs.stat[j] |= JFNDIXS;
		  }
		 else if (delta < 0 && -delta >= deltaLow)
		  { r->rs.setp[j] = r->rs.enc[j] - deltaLow;
		    r->rs.stat[j] |= JFNDIXS;
		  }
		}
	      break;
	    }
	   case DACSET:
	    { r->rs.com[j] = CURRENT;
	      r->rs.setp[j] = value;
	      break;
	    }
	   case CALIB:
	   case STOPM:
	    { r->rs.com[j] = HOLD;
	      setOneEncoder (r, j, (int)value);
	      r->rs.stat[j] &= ~(JFNDIXS);
	      r->rs.mode[j] &= ~(MLOOKZI);
	      break;
	    }
	   case STOPP:
	    { r->rs.com[j] = HOLD;
	      break;
	    }
	   case SETPAR:
	    { if ((value & (0xff00)) == MODE)
	       { int mode = value & 0xff;
		r->rs.mode[j] = mode;
	       }
	      break;
	    }
	   case INTBND:
	   case TOLBND:
	    { break;
	    }
	 }
}

putmode (r, mode, j)
RobotDesc *r;
int mode, j;
{
	r->rs.mode[j] = mode & 0xff;
}

getj (r, com, val, j)
RobotDesc *r;
unsigned short com;
unsigned short *val;
int j;
{
	switch (com)
	 { case JSTAT:
	    { *val = r->rs.stat[j];
	      break;
	    }
	   case READ:
	    { *val = r->rs.enc[j];
	      break;
	    }
	   case READP:
	    { if (FABS(r->jls->potSlope[j]) > 0)
	       { *val = (r->rs.enc[j] - r->jls->potIntercept[j])/r->jls->potSlope[j];
	       }
	      else
	       { *val = 0;
	       }
	      break;
	    }
	 }
	return 0;
}

joint_ram_load(r)
RobotDesc *r;
{
	return (1);
}

setRobotRate (r, interval)
RobotDesc *r;
int interval;
{
	r->rs.commandRate = interval;
}

getJointRate(r)
RobotDesc *r;
{
	return (r->rs.servoRate);
}

clockSetup(r)
RobotDesc *r;
{
	return (r->rs.baseClkRate);
}

gettime(r)
RobotDesc *r;
{
	return (r->ss.realTime/r->rs.baseClkRate);
}

setOneEncoder (r, j, value)
RobotDesc *r;
int j;
int value;
{
	int realEnc[MAXJNTS];

	r->jls->angleToEncoder (realEnc, r->rs.angles, r->jls);
	r->rs.encOffsets[j] = realEnc[j] - value;
	r->rs.enc[j] = value;
}

powerOn(r)
RobotDesc *r;
{
	r->rs.power = 1;
}

powerOff(r)
RobotDesc *r;
{
	int j;
	r->rs.power = 0;
	for (j=0; j<r->jls->numJ; j++)
	 { r->rs.com [j] = HOLD;
	 }
}

armPowerOn(r)
RobotDesc *r;
{	
	return (r->rs.power);
}

setHandPos (r, value)
RobotDesc *r;
short value;
{
	if (value)
	 { r->rs.handPos = 1;
	 }
	else
	 { r->rs.handPos = 0;
	 }
}

getHandPos(r)
RobotDesc *r;
{
	return (r->rs.handPos);
}

setHandForce(r,value)
RobotDesc *r;
short value;
{
	r->rs.handForce = 0;
}

getHandForce(r)
RobotDesc *r;
{
	return (r->rs.handForce);
}

armError(r)
RobotDesc *r;
{	
	return (r->rs.errorCode);
}

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

setArmError (r, code)
RobotDesc *r;
{
	r->rs.errorCode = code;
}	
