/*-=-=-=-=-=-=-=-=-=-=-=- 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>

#ifndef VXWORKS_CFG
#include <sys/types.h>
#include <sys/stat.h>
#endif

#include "../h/cdefs.h"
#include "../h/robot_jls.h"
#include "../h/robot_kynvar.h"
#include "../h/robotComfxns.h"
#include "../h/errorCodes.h"

#include "robotData.h"

static ROBOT_DATA_PTRS *robotDataPtrs;
static int tableSize;

void *malloc();

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

static char *noMemoryMsg	= "%s -- out of memory\n";
static char *noRCCLEnvMsg	= "%s -- environment variable RCCL not set\n";
static char *cantCreateMsg	= "%s -- can't create structure for '%s'\n";
static char *noCreateFxnMsg	= "%s -- no create function for '%s'\n";
static char *rbtNotDefMsg	= "%s -- rbt %s: not defined in RCI tables\n";
static char *noStatBuildMsg	= "%s -- can't stat or build '%s'\n";
static char *badFileMsg	  	= "%s -- error reading file '%s'\n";
static char *badChecksumMsg  	= "%s -- checksum error in file '%s'\n";
static char *fileTooLongMsg  	= "%s -- file '%s' is too long\n";

static JLS *getCompiledJls ();
static JLS *getAsciiJls ();

static initDone = 0;
static robotDataSetup()
{
	ROBOT_DATA_ENTRY *rde;
	ROBOT_DATA_PTRS *ptr;
	int i;

	if (robotDataPtrs)
	 { FREE(robotDataPtrs);
	 }
	tableSize = 0;
	for (rde=rciRobotDataTable; rde->name; rde++)
	 { tableSize++;
	 }
	if (tableSize > 0)
	 { ptr = (ROBOT_DATA_PTRS*)malloc(sizeof(ROBOT_DATA_PTRS)*tableSize);
	 }
	for (i=0; i<tableSize; i++)
	 { ptr[i].jls = NULL;
	   ptr[i].kyn = NULL;
	 }
	robotDataPtrs = ptr;
	initDone = 1;
	requestFlagInit(&initDone);
}

find_robot (name)
const char *name;
{
	register ROBOT_DATA_ENTRY *rde;
	register int i;

	for (rde=rciRobotDataTable,i=1; rde->name; rde++,i++)
	 { if (strcmp (name, rde->name) == 0)
	    { return (i);
	    }
	 }
 	return (0);
}

robot_class(name)
const char *name;
{
	register ROBOT_DATA_ENTRY *rde;

	for (rde=rciRobotDataTable; rde->name; rde++)
	 { if (strcmp (name, rde->name) == 0)
	    { return (rde->class);
	    }
	 }
 	return (0);
}

get_comfxns (fxns, name)
ROBOT_COMFXNS *fxns;
char *name;
{
	register ROBOT_DATA_ENTRY *rde;

	for (rde=rciRobotDataTable; rde->name; rde++)
	 { if (strcmp (name, rde->name) == 0)
	    { fxns->startup = rde->startupFxn;
	      fxns->input =   rde->inputFxn;
	      fxns->output =  rde->outputFxn;
	      fxns->release = rde->releaseFxn;
	      return (1);
	    }
	 }
	return (0);
}

KYN *doGetKyn (name, size, fxn)
const char *name;
int size;
int (*fxn)();
{
	char *fxnName = "doGetKyn()";

#define FNS 256
	char fileName[FNS+1];
	char buf[256];
	FILE *fp = NULL;
	char *rootdir;
        KYN *kyn = NULL;
	int j;

	if ((rootdir = (char *)getenvRCCL()) == NULL)
  	 { errorMsg (EEnviron, noRCCLEnvMsg, fxnName);
	   goto error;
	 }
	sprintf (fileName, "%s/conf/%s.kyn", rootdir, name);
	if ((fp = fopen (fileName, "r")) == NULL)
	 { sysErrorMsg (ENoFile, fxnName, "fopen on .kyn file");
	   goto error;
	 }
	if ((kyn = ALLOC(KYN)) == NULL)
	 { errorMsg (EMemory, noMemoryMsg, fxnName);
	   goto error;
	 }
	kyn->ext = NULL;
	kyn->gen = NULL;
	if ((kyn->ext = (void*)malloc(size)) == NULL ||
	    (kyn->gen = ALLOC(GEN_KYN)) == NULL)
	 { errorMsg (EMemory, noMemoryMsg, fxnName);
	   goto error;
	 }
	bzero(kyn->gen,sizeof(GEN_KYN));
	/* paranoid ... bzero doesn't guarantee to set floats to 0.0 */
	for (j=0; j<MAXJNTS; j++)
	 { kyn->gen->dh[j].theta = 0.0;
	   kyn->gen->dh[j].d = 0.0;
	   kyn->gen->dh[j].a = 0.0;
	   kyn->gen->dh[j].alpha = 0.0;
	 }

	/* call class specific routine to do the work */

	if ((*fxn)(kyn,fp) == -1)
	 { errorMsg (getErrorCode(0), badFileMsg, fxnName, fileName);
	   goto error;
	 }
  	fclose (fp);
 	return (kyn);
error:
	if (fp)
	 { fclose (fp);
	 }
	if (kyn)
	 { if (kyn->gen)
	    { FREE (kyn->gen);
	    }
	   if (kyn->ext)
	    { FREE (kyn->ext);
	    }
	   FREE (kyn);
	 }
	return (NULL);
}

KYN *getKyn (name, sizeptr)
const char *name;
int *sizeptr;
{
	char *fxnName = "getKyn()";
	register ROBOT_DATA_ENTRY *rde;
	register ROBOT_DATA_PTRS *ptr;

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if (!initDone)
	 { robotDataSetup();
	 }

	ptr=robotDataPtrs;
	for (rde=rciRobotDataTable; rde->name; rde++,ptr++)
	 { if (strcmp (name, rde->name) == 0)
	    { if (ptr->kyn)
	       { if (sizeptr)
		  { *sizeptr = rde->kynSize;
		  }
		 return (ptr->kyn);
	       }
	      else
	       { if (rde->getKyn)
		  { KYN *kyn;
		    if ((kyn=doGetKyn(name,rde->kynSize,rde->getKyn)) == NULL)
		     { errorMsg (EBadFxn, cantCreateMsg, fxnName, name);
		       return (NULL);
		     }
		    if (sizeptr)
		     { *sizeptr = rde->kynSize;
		     }
		    ptr->kyn = kyn;
		    return (kyn);
		  }
		 else
	          { errorMsg (EBadFxn, noCreateFxnMsg, fxnName, name);
		    return (NULL);
	          }
	       }
	    }
	 }
	errorMsg (ENotDefined, rbtNotDefMsg, fxnName, name);
	return (NULL);
}

int getVarSize (name)
const char *name;
{
	char *fxnName = "getVarSize()";
	register ROBOT_DATA_ENTRY *rde;

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if (!initDone)
	 { robotDataSetup();
	 }
	for (rde=rciRobotDataTable; rde->name; rde++)
	 { if (strcmp (name, rde->name) == 0)
	    { return (rde->varSize);
	    }
	 }
	errorMsg (ENotDefined, rbtNotDefMsg, fxnName, name);
	return (-1);
}

#ifndef VXWORKS_CFG

static JLS *getCompiledJls (name, fxn)
char *name;
int (*fxn)();
{
	char *fxnName = "getCompiledJls()";

#define FNS 256
	char fileName_JLS[FNS+1];
	char fileName_jls[FNS+1];
	FILE *fp = NULL;
	char *rootdir;
        JLS *jls = NULL;
	int checksum;
	struct stat stat_jls, stat_JLS;
	int statValid_jls, statValid_JLS;

	if ((jls = (JLS*)malloc(sizeof(JLS))) == NULL)
	 { errorMsg (EMemory, noMemoryMsg, fxnName);
	   goto errorReturn;
	 }
	if ((rootdir = (char *)getenvRCCL()) == NULL)
  	 { errorMsg (EEnviron, noRCCLEnvMsg, fxnName);
	   goto errorReturn;
	 }

	sprintf (fileName_jls, "%s/conf/%s.jls", rootdir, name);
	sprintf (fileName_JLS, "%s/conf/%s.JLS", rootdir, name);

	if (stat (fileName_JLS, &stat_JLS) < 0)
	 { statValid_JLS = 0;
	 }
	else
	 { statValid_JLS = 1;
	 }
	if (stat (fileName_jls, &stat_jls) < 0)
	 { statValid_jls = 0;
	 }
	else
	 { statValid_jls = 1;
	 }

	if (statValid_jls)
	 { if ((statValid_JLS && stat_jls.st_mtime > stat_JLS.st_mtime) ||
	       !statValid_JLS)
	    { char comstring[1024];
#ifdef sun
	      /* XXX do this because sunTouchMemoryHack() will bring
		 in rciSimOnly. */
	      extern int rciSimOnly;
	      int forceRef;

	      forceRef = rciSimOnly;
#endif
	      printf ("Rebuilding %s ...\n", fileName_JLS);
	      sprintf (comstring, "makejls %s %s", fileName_jls,
						     fileName_JLS);
	      printf ("%s\n", comstring);
	      if (system (comstring) != 0)
	       { sysErrorMsg (ENoFile, fxnName, "system");
		 goto errorReturn;
	       }
#ifdef sun
	      sunTouchMemoryHack();	/* XXXXXX !!!!!! */
#endif
	    }
	 }
	else if (!statValid_JLS)
	 { errorMsg (ENoFile, noStatBuildMsg, fxnName, fileName_JLS);
	   goto errorReturn;
	 }

	if ((fp = fopen (fileName_JLS, "r")) == NULL)
	 { sysErrorMsg (ENoFile, fxnName, "fopen on JLS file");
	   goto errorReturn;
	 }
	if ((fread (jls, sizeof(JLS), 1, fp) != 1) ||
	    (fread (&checksum, sizeof (long), 1, fp) != 1) )
	 { errorMsg (EBadFile, badFileMsg, fxnName, fileName_JLS);
	   goto errorReturn;
	 }

	/* ---- Compute checksum ---- */

	 { register int i, x = 0, *p = (int *) jls;

	   for (i=0; i<sizeof(JLS)/sizeof(int); i++)
	    { x += p[i];
	    }
	   if (x != checksum)
	    { errorMsg (EBadFile, badChecksumMsg, fileName_JLS);
	      goto errorReturn;
	    }
	 }

	/* ---- Now try to read just one more byte ---- */

	 { char c;
	   if (fread (jls, sizeof(char), 1, fp) == 1)
	    { errorMsg (EBadFile, fileTooLongMsg, fxnName, fileName_JLS);
	      goto errorReturn;
	    }
	 }

	fclose (fp);
 	return (jls);
errorReturn:
	if (fp)
	 { fclose (fp);
	 }
	if (jls)
	 { FREE(jls);
	 }
	return (NULL);
}

#endif

#ifdef VXWORKS_CFG

static JLS *getAsciiJls (name, fxn)
char *name;
int (*fxn)();
{
	char *fxnName = "getAsciiJls()";

#define FNS 256
	char fileName_jls[FNS+1];
	char *rootdir;
	FILE *fp;
        JLS *jls = NULL;

	if ((jls = (JLS*)malloc(sizeof(JLS))) == NULL)
	 { errorMsg (EMemory, noMemoryMsg, fxnName);
	   goto errorReturn;
	 }
	if ((rootdir = (char *)getenvRCCL()) == NULL)
  	 { errorMsg (EEnviron, noRCCLEnvMsg, fxnName);
	   goto errorReturn;
	 }
	sprintf (fileName_jls, "%s/conf/%s.jls", rootdir, name);

	if ((fp = fopen (fileName_jls, "r")) == NULL)
	 { sysErrorMsg (ENoFile, fxnName, "fopen on jls file");
	   goto errorReturn;
	 }
	if (readJls (jls, fp) < 0)
	 { errorMsg (EBadFile, badFileMsg, fxnName, fileName_jls);
	   goto errorReturn;
	 }
	
	fclose (fp);
 	return (jls);
errorReturn:
	if (fp)
	 { fclose (fp);
	 }
	if (jls)
	 { FREE(jls);
	 }
	return (NULL);
}

#endif

JLS *getJls (name)
const char *name;
{
#ifdef VXWORKS_CFG
#define GET_JLS_FXN	getAsciiJls
#else
#define GET_JLS_FXN	getCompiledJls
#endif

	char *fxnName = "getJls()";
	register ROBOT_DATA_ENTRY *rde;
	register ROBOT_DATA_PTRS *ptr;

	if (!planningLevelCheck(fxnName))
	 { return (NULL);
	 }
	if (!initDone)
	 { robotDataSetup();
	 }

	ptr=robotDataPtrs;
	for (rde=rciRobotDataTable; rde->name; rde++,ptr++)
	 { if (strcmp (name, rde->name) == 0)
	    { if (ptr->jls)
	       { return (ptr->jls);
	       }
	      else
	       { if (rde->getJls)
		  { JLS *jls;
		    if ((jls = GET_JLS_FXN(name)) == NULL)
		     { errorMsg (EBadFile, cantCreateMsg, fxnName, name);
		       return (NULL);
		     }
		    /* call class function to instantiate function pointers */
		    (*rde->getJls)(jls);

		    ptr->jls = jls;
		    return (jls);
		  }
		 else
	          { errorMsg (EBadFxn, noCreateFxnMsg, fxnName, name);
		    return (NULL);
	          }
	       }
	    }
	 }
	errorMsg (ENotDefined, rbtNotDefMsg, fxnName, name);
	return (NULL);
}
