/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd.			    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/

#include "../h/cdefs.h"
#include "../h/rciTask.h"
#include "../h/rciMacros.h"
#include "../h/robotControl.h"
#include "../h/chase.h"

#include "potcal.h"

#define PSET	"sysprogs"

static char *simulatorAddr;

setSimulator(name)
char *name;
{
	/* record the internet address of the simulator program.
	   WARNING: this routine and the next will not work unless
	   called on the arbiter CPU, since 'simulatorAddr' is not
	   shared across CPUs. */
	simulatorAddr = name;
}

RCI_DESC *createChaseTask (robotName, simflag)
char *robotName;
int simflag;
{
	RCI_DESC *td;
	int pset;
	int cpus;
	char *schedStr;
	int scheduling;
	int interval;
	int timeout = UNDEF;
	int leavePowerOn = 0;

	if ((pset = rciParameters (PSET, 0)) < 0)
	 { printErrors();
	   exit (-1);
	 }
	if (pset == 0)
	 { printf ("parameter set '%s' not found\n", PSET);
	   exit (-1);
	 }
	if (getparInteger (&cpus, "cpus", pset) < 1)
	 { printf ("integer 'cpus' not in parameters '%s'\n", PSET);
	   exit (-1);
	 }
	if (getparIdentifier (&schedStr, "scheduling", pset) < 1)
	 { printf ("string 'scheduling' not in parameters '%s'\n", PSET);
	   exit (-1);
	 }
	if (getparInteger (&timeout, "timeout", pset) == 1)
	 { if (timeout < 0)
	    { printf ("negative timeout parameter specified\n");
	      exit (-1);
	    }
	 }
	getparInteger (&leavePowerOn, "leavePowerOn", pset);

	if (strcmp (schedStr, "ON_TRIGGER") == 0)
	 { scheduling = ON_TRIGGER;
	 }
	else if (strcmp (schedStr, "ON_CLOCK") == 0)
	 { scheduling = ON_CLOCK;
	 }
	else if (strcmp (schedStr, "ON_FUNCTION") == 0)
	 { scheduling = ON_FUNCTION;
	 }
	else if (strcmp (schedStr, "ON_SIGNAL") == 0)
	 { scheduling = ON_SIGNAL;
	   simflag = 1;
	 }
	else
	 { printf ("bad 'scheduling' desc. %s in parameters '%s'\n",
		   schedStr, PSET);
	   exit (-1);
	 }
	if (getparInteger (&interval, "interval", pset) < 1)
	 { printf ("integer 'interval' not in parameters '%s'\n", PSET);
	   exit (-1);
	 }
	if (interval < 1)
	 { printf ("'interval' is less than 0 in parameters '%s'\n", PSET);
	   exit (-1);
	 }	
	if (simflag)
	 { cpus = 1;
	 }
	if ((td = rciCreate ("chase", cpus, simflag)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	if (simflag)
	 { if (simAttachRobot (td, robotName, simulatorAddr) < 0)
	    { printErrors();
	      exit (-1);
	    }
	   simScheduling (td);
	 }
	else
	 { if (rciAttachRobot (td, robotName, NULL) < 0)
	    { printErrors();
	      exit (-1);
	    }
	   if (rciScheduling (td, scheduling, TASKROBOT(td,0)->idev) < 0)
	    { printErrors();
	      exit (-1);
	    }
	 }
	if (rciRate (td, interval, 0) < 0)
	 { printErrors();
	   exit(-1);
	 }
	if (timeout != UNDEF)
	 { if (rciTimeout (td, timeout, UNDEF, UNDEF) < 0)
	    { printErrors();
	    }
	 }
	if (rciControlFxns (td, chaseStart, chaseDrive, chaseRelease, 0)<0)
	 { printErrors();
	   exit (-1);
	 }
	if (leavePowerOn)
	 { SET_POWER_ON_RELEASE (TASKROBOT(td, 0), 1);
	 }
	return (td);
}

CHASE_CTRL *chaseTaskMemory (td)
RCI_DESC *td;
{
	CHASE_CTRL *cb;

	if ((cb = (CHASE_CTRL*)rciSharedMemory(
		  "CHASE_CTRL", sizeof(CHASE_CTRL), td)) == NULL)
	 { printErrors();
	   exit (-1);
	 }
	chaseCtrlInit(cb);
	return (cb);
}


startChaseTask (td)
RCI_DESC *td;
{
	return (rciStart (td->taskCode));
}

powerUpChaseTask (td, block)
RCI_DESC *td;
int block;
{
	RCI_RBT *rbt = TASKROBOT(td, 0);

	SET_POWER_ON(rbt);
	rciBlock(td);	
	rciBlock(td);
	if (block && !rbt->how->power)
	 { printf ("Turn on arm power\n");
	   while (!rbt->how->power)
	    { nap (0.2);
	    }
	 }
}

float speedSet (cb, jls, speed)
CHASE_CTRL *cb;
JLS *jls;
float speed;
{
	float oldSpeed;
	register int j;

	oldSpeed = cb->angSpeed[0];
	for (j=0; j<jls->numJ; j++)
	 { cb->angSpeed[j] = speed;
	   cb->encSpeed[j] = speed * jls->averageAngtoenc;
	 }
	return (oldSpeed);
}

computeLine(m, b, d)
double *m, *b;
LINE_DATA *d;
{
	/* compute the slope and intercept for the line data collected
	   in the structure 'd'.
	 */

	double denom;
	
	denom = d->n*d->sumEncEnc - d->sumEnc*d->sumEnc;
	*m =   (d->n*d->sumEncPot -
		d->sumEnc*d->sumPot) / denom;
	*b =   (d->sumPot*d->sumEncEnc -
		d->sumEnc*d->sumEncPot) / denom;
}
