/*-=-=-=-=-=-=-=-=-=-=-=- 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 <stdio.h>
#include <ctype.h>

#include "../h/errorCodes.h"
#include "../h/robot_jls.h"
#include "../h/robotMath.h"

static FILE *stream;
static char *textp;
static char textBuf[256] = {'\0'};
static int lineNo = 0;
static int atEOF = 0;

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

static char *rbtCantGetJLSMsg	= "%s -- rbt %s: can't get JLS structure\n";
static char *noRCCLEnvMsg	= "%s -- environment variable RCCL not set\n";
static char *cantOpenFileMsg	= "%s -- can't open file %s\n";
static char *expectedItemMsg    = "%s -- file %s: %s expected, line %d\n";

static readInit ();
static endOfInput();
static lineNumber();
static nextToken();
static readDouble();
static readIdentifier();

static readInit (fp)
FILE *fp;
{
	stream = fp;
	textp = textBuf;
	textBuf[0] = '\0';
	lineNo = 0;
	atEOF = 0;
}

static endOfInput()
{
	return (atEOF);
}

static lineNumber()
{
	return (lineNo);
}

static nextToken()
{
	while (1)
	 { while (isspace(*textp))
	    { textp++;
	    }
	   if (*textp == '\0')
	    { textp = textBuf;
	      if (fgets (textBuf, 256, stream) == NULL)
	       { atEOF = 1;
		 *textp = '\0';
		 return;
	       }
	      else
	       { char *cp;
		 if (cp = (char*)index (textp, '#'))
		  { *cp = '\0';
		  }
		 lineNo++;
	       }
	    }
	   else
	    { return;
	    }
	 }
}

static readDouble(dp)
double *dp;
{
	char *cp;

	if (atEOF)
	 { return (0);
	 }
	nextToken();
	if (cp = (char*)scanDouble (dp, textp))
	 { textp = cp;
	   return (1);
	 }
	else
	 { return (0);
	 }
}

static readIdentifier(result, size)
char *result;
int size;
{
	char *cp;

	if (atEOF)
	 { return (0);
	 }
	nextToken();
	if (cp = (char*)scanIdentifier (result, size, textp))
	 { textp = cp;
	   return (1);
	 }
	else
	 { return (0);
	 }
}

getRobotPosition (jnts, name, robotName)
float *jnts;
const char *name, *robotName;
{
	char *rcclDir;
	char fileName[256];
	char ibuf[256];
	JLS *jls;
	FILE *fp;	       
	char *fxnName = "getRobotPosition()";
	int positionFound = 0;
	
	enum { READ_NAME,
	       READ_ANGLES } state;

	if (!planningLevelCheck(fxnName))
#ifdef LINUX_CFG
	 { return (-1);
#else
	 { return (NULL);
#endif
	 }
	if ((jls = getJls(robotName)) == NULL)
	 { errorMsg (getErrorCode(0) == EMemory ? EMemory : EBadAccess,
		     rbtCantGetJLSMsg, fxnName, robotName);
	   return (-1);
	 }
	if ((rcclDir = (char*)getenvRCCL()) == NULL)
	 { errorMsg (EEnviron, noRCCLEnvMsg, fxnName);
	   return (-1);
	 }
	sprintf (fileName, "%s/conf/%s.pos", rcclDir, robotName);
	if ((fp = fopen (fileName, "r")) == NULL)
	 { errorMsg (EBadAccess, cantOpenFileMsg, fxnName, fileName);
	   return (-1);
	 }

	readInit (fp);

	while (1)
	 { int j;
	   double dval;
	   float deg[MAXJNTS];

	   if (!readIdentifier (ibuf, 256))
	    { if (endOfInput())
	       { break;
	       }
	      else
	       { errorMsg (EBadObj, expectedItemMsg, fxnName, 
				    fileName, "position name", lineNumber());
		 fclose (fp);
		 return (-1);
	       }
	    }
	   for (j=0; j<jls->numJ; j++)
	    { 
	      if (!readDouble (&dval))
	       { errorMsg (EBadObj, expectedItemMsg, fxnName,
				    fileName, "angle values", lineNumber());
		 fclose (fp);
		 return (-1);
	       }
	      deg[j] = dval;
	    }
	   if (strcmp (ibuf, name) == 0)
	    { degreeToRadian (jnts, deg, jls);
	      positionFound = 1;
	    }
	 }
	fclose(fp);
	return (positionFound ? jls->numJ : 0);
}

#if TEST

char robotName[256];
char positionName[256];

main()
{
	JLS *jls;
	float jnts[MAXJNTS];
	int status;

	while (1)
	 { printf ("robot and position name> ");
	   scanf ("%s%s", robotName, positionName);
	   status = getRobotPosition (jnts, positionName, robotName);
	   jls = getJls(robotName);
	   if (status < 0)
	    { printErrors();
	      clearErrors();
	    }
	   else if (status == 0)
	    { printf ("no position defined\n");
	    }
	   else
	    { radianToDegree (jnts, jnts, jls);
	      printVf ("%10.4f\n", jnts, status);
	    }
	 }
}

#endif
