/*-=-=-=-=-=-=-=-=-=-=-=- RCCL/RCI Notice of Copyright -=-=-=-=-=-=-=-=-=-=-*
 *									    *
 *	Copyright (C) 1987, 1991 by John E. Lloyd and Vincent Hayward.	    *
 *									    *
 *	Information on copyright permissions, as well as a complete	    *
 *	disclaimer of warranty, is given in the file COPYRIGHT.		    *
 *									    *
 *-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=*/
/*---------------------------------------------------------------------------
 *		
 * MODULE NAME:	copyPosition
 *
 * DESCRIPTION:	Routines to reference and copy position equation 
 *		structures. These routines can be called from any part
 *		of the RCCL program.
 *
 * AUTHOR/DATE:	John Lloyd, Fall 1988
 *
 * MODIFICATION HISTORY:
 *
 *--------------------------------------------------------------------------*/

#include "../h/rcclDefs.h"
#include "../h/errorCodes.h"

#include "../rccl/transform.h"
#include "../rccl/rcclAutoAbort.h"
#include "../rccl/rcclErrorMsgs.h"

#include "../rci/hashTable.h"

/* ----- Error Messages. Defined externally to save space ----- */

static char *cantFindTransMsg		= "%s can't find trans 0x%x\n";

POS *getPositionByName (name)
const char *name;
{
	/* Find the address of a global position using its name as a key */

	return ((POS*)getMemByName (name));
}

POS *getPositionByAddr (pos)
POS *pos;
{
	/* Find the address of a global position using the planning leve;
	   address as a key */

	return ((POS*)getMemByAddr((void*)pos));
}

/* ----------------------------------------------------------------------

	Position equation structures are copied from the planning
	level to the control level. When this is done, the base
	address of the data structure changes, invalidating the 
	pointers. When necessary, these routines adjust the pointer 
	values.

  ------------------------------------------------------------------------ */

POS *copyPosition (pos)
POS *pos;
{
	/* Make a local copy of a position equation, but not its transforms */

	char *fxnName = "copyPosition()";

	POS *new;
	POS_TERM *newterm;
	POS_TERM *posterm;
	int offset;
	int k;

	if ((new = (POS*)rciMalloc (  sizeof(POS_TERM)*pos->numTerms
				     + sizeof(POS))) == NULL)
	 { return (NULL);
	 }

	/* Copy the position and the terms */
	*new = *pos;
	newterm = (POS_TERM*)&new[1];
	posterm = (POS_TERM*)&pos[1];
	for (k=0; k<pos->numTerms; k++)
	 { newterm[k] = posterm[k];
	 }

	/* Go through and adjust the pointer offsets */

#define OFFSET_TERM(n,offset)	(n)=(POS_TERM*)((char*)(n) + (offset))

	offset = (int)new - (int)pos->addr;	/* XXX */

	OFFSET_TERM(new->startTerm, offset);
	if (new->toolTerm)
	 { OFFSET_TERM(new->toolTerm, offset);
	 }
	if (new->t6Term)
	 { OFFSET_TERM(new->t6Term, offset);
	 }

	newterm = new->startTerm;
	do
	 { if (newterm != new->t6Term)
	    { if ((newterm->trsf = getTransByAddr (newterm->trsf)) == NULL)
	       { rciFree (new);
		 rciAbort(EInternal,rcclCantGetTrsfMsg,fxnName,newterm->trsf);
		 return (NULL);
	       }
	    }
	   OFFSET_TERM(newterm->next, offset);
	   OFFSET_TERM(newterm->prev, offset);
	   newterm = newterm->prev;
	 }
	while (newterm != new->startTerm);

	new->global = NO;
	new->addr = new;
	return (new);
}

freePosition(pos)
POS *pos;
{
	/* Free the memory used to allocate a position equation */

	char *fxnName = "freePosition()";
	extern int rciTaskId;

	if (pos != pos->addr)
	 { return (-1);
	 }
	if (pos->global)			
	 { if (rciTaskId != 0)
	    { return (-1);
	    }
	   else
	    { return (freeMemId ((void*)pos, POS_HASH_ID));
	    }
	 }
	else
	 { return (rciFree ((void*)pos));	/* local copy */
	 }
}

relocatePosition (pos, offset)
POS *pos;
int offset;
{
	/* A routine used internally by the trajectory generator.

	   Goes through a position equation and adds the indicated
	   offset to all the pointers. It is assumed that the transforms
	   are local copies which have been allocated in memory contiguous
	   which the memory containing the positions and terms. This
	   is the case for position equations that are copied into the
	   motion queue. */

	POS_TERM *term;

#define OFFSET_TRSF(n,offset)	(n)=(TRSF*)((char*)(n) + (offset))

	term = pos->startTerm;
	do
	 { term = term->next;
	   OFFSET_TRSF(term->trsf, offset);
	   OFFSET_TERM(term->prev->next, offset);
	   OFFSET_TERM(term->prev, offset);
	 }
	while (term != pos->startTerm);

	if (pos->t6Term)
	 { OFFSET_TERM(pos->t6Term, offset);
	 }
	if (pos->toolTerm)
	 { OFFSET_TERM(pos->toolTerm, offset);
	 }
}

delocatePosition (pos, offset)
POS *pos;
int offset;
{
	/* The oppposite of relocatePosition():
	   Assumes that a position equation's pointers are offset
	   and they must be corrected by subtracting the indicated
	   offset from all the pointers.
	*/

	POS_TERM *term;

	OFFSET_TERM(pos->startTerm, -offset);
	if (pos->toolTerm)
	 { OFFSET_TERM(pos->toolTerm, -offset);
	 }
	if (pos->t6Term)
	 { OFFSET_TERM(pos->t6Term, -offset);
	 }

	term = pos->startTerm;
	do
	 { OFFSET_TRSF(term->trsf, -offset);
	   OFFSET_TERM(term->next, -offset);
	   OFFSET_TERM(term->prev, -offset);
	   term = term->prev;
	 }
	while (term != pos->startTerm);
}
