/*---------------------------------------------------------------------------
  SICS object for driving a couple of motors along a tabulated given path.

  copyright: see file COPYRIGHT

  Mark Koennecke, July 2005
---------------------------------------------------------------------------*/
#include <stdio.h>
#include <math.h>
#include "../lld.h"
#include "tabledrive.h"
/*--------------------------------------------------------------------------*/
#define OUTOFSYNC   100
#define STARTING    101
#define STEPPING    102
#define WAITING     103
#define WAITFINISH  104
#define IDLE        105

#define ABS(x) (x < 0 ? -(x) : (x))    
#define SIGN(x) (x < .0 ? (-1) : (1))
extern char *trim(char *txt);		
/*
 * copied from motor.c!!
 */
#define SLOW 0
#define SUPP 1
	
/*=================== the drivable interface functions =====================*/
static int TableDriveHalt(void *pData){
	pTableDrive self = (pTableDrive)pData;
	int status;
	tdMotor moti;
	
	if(self == NULL || self->motorTable < 0){
			return 0;
	}
			
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,&moti);
		if(moti.pMot != NULL){
			moti.pMot->pDrivInt->Halt(moti.pMot);
		}
		status = LLDnodePtr2Next(self->motorTable);	
	}
	self->state = OUTOFSYNC;
	return 1;
}
/*-------------------------------------------------------------------------*/
static int TableDriveCheckLimits(void *pData, float fVal, char *error,
	int iErrLen){
	pTableDrive self = (pTableDrive)pData;
    tdMotor moti;

	if(self == NULL || self->motorTable < 0){
		strncpy(error,"Path Table Not Defined!",25);
		return 0;
	}		
	if(fVal < 1. || fVal > self->tableLength){
		strncpy(error,"Out of Range",25);
		return 0;
	}	
	return 1;
}
/*--------------------------------------------------------------------------*/
static long TableDriveSetValue(void *pData, SConnection *pCon, float fVal){
	
	pTableDrive self = (pTableDrive)pData;
	char pError[132];
	
	if(self == NULL || self->motorTable < 0){
		SCWrite(pCon,"ERROR: no path  defined",eError);
		return HWFault;
	}
	strcpy(pError,"ERROR:");
	if(!TableDriveCheckLimits(self,fVal,&pError[6],120)){
		SCWrite(pCon,pError,eError);
		return HWFault;
	}
	self->state = STARTING;
	self->targetPosition = fVal;
	return OKOK;
}
/*-------------------------------------------------------------------------*/
static int findOrientingMotor(pTableDrive self, ptdMotor moti){
	int status;
	
    if(!self->oriInvalid){
        *moti = self->oriMotor;
        return 1;
    }
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,moti);
		if(strcmp(moti->motorName,self->orientMotor) == 0){
            self->oriMotor = *moti;
            self->oriInvalid = 0;
			return 1;
		}
		status = LLDnodePtr2Next(self->motorTable);
	}
	return 0;
}
/*-------------------------------------------------------------------------*/
static float locatePosition(tdMotor moti, SConnection *pCon){
	int status, begin = 1;
	float value;
        double diff;
	tdEntry lower, upper;
	
	status = MotorGetSoftPosition(moti.pMot,pCon,&value);
	if(!status) {
		return -9999.99;
	}
	status = LLDnodePtr2First(moti.table);
	while(status != 0){
		LLDnodeDataTo(moti.table,&lower);
		status = LLDnodePtr2Next(moti.table);
		if(status != 0) {
		  LLDnodeDataTo(moti.table,&upper);
		} else {
		  /*
		   * end of table
		   */
		  return lower.tablePos;
		}
		/**
		 * we have to deal with ascending and descending tables. This is
		 * why we have to have some funny logic here
		 */
		if(upper.position > lower.position){
		  /*
		   * ascending table 
		   */
		  if(value >= lower.position && value < upper.position){
		    diff = upper.position - lower.position;
		    return lower.tablePos + (value - lower.position)/diff;
		  }
                  if(begin == 1 && value < lower.position) {
		    return lower.tablePos;
                  }
		} else {
		  /*
		   * descending table
		   */
		  if(value <= lower.position && value > upper.position){
		    diff = lower.position - upper.position;
		    return lower.tablePos + ABS(value - lower.position)/diff;
		  } 
                  if(begin == 1 && value > lower.position){
		    return lower.tablePos;
                  }
		}
		begin = 0;
	}
	return -999.99;
}
/*-------------------------------------------------------------------------*/
static void findBracket(tdMotor moti, float value, ptdEntry upper,
						ptdEntry lower){
	int status, targetCount;
    float diff;

	targetCount = (int)floor(value);
	status = LLDnodePtr2First(moti.table);
	while(status != 0){
		LLDnodeDataTo(moti.table,lower);
		if(lower->tablePos == targetCount){
			LLDnodeDataTo(moti.table,upper);
			if(LLDnodePtr2Next(moti.table)){
				LLDnodeDataTo(moti.table,upper);
			}	
			return;						
		}
		status = LLDnodePtr2Next(moti.table);
	}
}
/*-------------------------------------------------------------------------*/
static float findTarget(tdMotor moti, float value){
	int targetCount;
	tdEntry lower, upper;
    float diff;

	targetCount = (int)floor(value);
	findBracket(moti,value,&upper,&lower);
    if(ABS(targetCount - value) < .00001){
        return lower.position;
    } else {
       diff = upper.position - lower.position;
	   return (float)lower.position + (value - targetCount)*diff;
    }
}
/*-------------------------------------------------------------------------*/
static void checkSync(pTableDrive self, SConnection *pCon, float value){
	int status, test;
	float motorPosition, targetPosition, tolerance, diff;
	tdMotor moti;
	char pBueffel[256];

	if(self->state != IDLE){
	  return;
	}
	
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,&moti);
		test = MotorGetSoftPosition(moti.pMot,pCon,&motorPosition);
		if(!test){
			snprintf(pBueffel,255,"ERROR: failed to read motor %s",
				moti.motorName);
			SCWrite(pCon,pBueffel,eError);
		}	else {
			MotorGetPar(moti.pMot,"precision",&tolerance);
			targetPosition = findTarget(moti,value);
			if(ABS(targetPosition - motorPosition) > tolerance){
				snprintf(pBueffel,256,
					"WARNING: motor %s out of sync by %f",
					moti.motorName, ABS(targetPosition - motorPosition));
				SCWrite(pCon,pBueffel,eWarning);
			}
		}
		status = LLDnodePtr2Next(self->motorTable);
	} 
}
/*--------------------------------------------------------------------------*/
static float TableDriveGetValue(void *pData, SConnection *pCon){
	pTableDrive self = (pTableDrive)pData;
	tdMotor orient;
	float value;

	if(self == NULL || self->motorTable < 0){
		SCWrite(pCon,"ERROR: no path  defined",eError);
		return -9999.99;
	}
	if(!findOrientingMotor(self,&orient)){
		SCWrite(pCon,"ERROR: table corrupted or orienting motor not found",
			eError);
		return -9999.99;
	}
	value = locatePosition(orient,pCon);
	checkSync(self,pCon, value);

    self->currentPosition = value;
	return value;
}
/*-------------------------------------------------------------------------*/
static void tableInfo(pTableDrive self, SConnection *pCon){
    int status, test;
    float motorPosition, targetPosition, tolerance, diff, value;
    tdMotor moti;
    char pBueffel[256];
    
    value = TableDriveGetValue(self,pCon);
    snprintf(pBueffel,255," Triffid Position: %8.2f", value);
    SCWrite(pCon,pBueffel,eValue);
    
    status = LLDnodePtr2First(self->motorTable);
    while(status != 0){
        LLDnodeDataTo(self->motorTable,&moti);
        test = MotorGetSoftPosition(moti.pMot,pCon,&motorPosition);
        if(!test){
            snprintf(pBueffel,255,"ERROR: failed to read motor %s",
                moti.motorName);
            SCWrite(pCon,pBueffel,eError);
        }   else {
            MotorGetPar(moti.pMot,"precision",&tolerance);
            targetPosition = findTarget(moti,value);
            snprintf(pBueffel,256,"Motor %10s, should: %8.2f, is %8.2f, diff = %8.2f",
            moti.motorName, targetPosition, motorPosition, 
                ABS(targetPosition - motorPosition));
            SCWrite(pCon,pBueffel,eValue);
        }
        status = LLDnodePtr2Next(self->motorTable);
    } 
}
/*------------------------------------------------------------------------*/
static void tableSetPar(pMotor pMot, char *name, float value){
  ObPar *ob = NULL;

  ob = ObParFind(pMot->ParArray,name);
  assert(ob != NULL);
  ob->fVal = value;
}
/*------------------------------------------------------------------------
 * Before I can drive the motors, I have to release the software limits..
 -------------------------------------------------------------------------*/
static void liberateMotors(pTableDrive self, SConnection *pCon){
	int status;
	tdMotor moti;
	float upper, lower;
	
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,&moti);
		MotorGetPar(moti.pMot,"hardupperlim",&upper);
		MotorGetPar(moti.pMot,"hardlowerlim",&lower);
		
		tableSetPar(moti.pMot,"softupperlim",upper);
		tableSetPar(moti.pMot,"softlowerlim",lower);
		status = LLDnodePtr2Next(self->motorTable);
	}
}
/*------------------------------------------------------------------------
 * after driving the motors, we have to set the software limits in order
 * to prevent the user from manually crashing components 
------------------------------------------------------------------------ */
static void closeMotors(pTableDrive self, SConnection *pCon){
	int status;
	tdMotor moti;
	tdEntry tdLower, tdUpper;
	float upper, lower, diff, targetCount;
	
	targetCount = floor(self->currentPosition);
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,&moti);
		findBracket(moti,self->currentPosition,&tdUpper,&tdLower);
		diff = tdUpper.upper - tdLower.upper;
		upper = tdLower.upper + (self->currentPosition - targetCount)*diff;
		diff = tdUpper.lower - tdLower.lower;
		lower = tdLower.lower + (self->currentPosition - targetCount)*diff;
		tableSetPar(moti.pMot,"softupperlim",upper);
		tableSetPar(moti.pMot,"softlowerlim",lower);
		status = LLDnodePtr2Next(self->motorTable);
	}
}
/*------------------------------------------------------------------------*/
static float calculateNextStep(pTableDrive self){
	float step;
	
	step = self->targetPosition - self->currentPosition;
	if(ABS(step) > 1.){
		step = SIGN(step)*1.0;				
	}
	return step;
}
/*------------------------------------------------------------------------*/
static int runToNextStep(pTableDrive self, SConnection *pCon, 
	float value){
	int status, test;
	tdMotor moti;
	float target;
	
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,&moti);
		target = findTarget(moti,value);
        test = MotorRun(moti.pMot,pCon,target);
        if(test != OKOK){
            return test;
        }
		status = LLDnodePtr2Next(self->motorTable);
	}	
	return 1;
}
/*------------------------------------------------------------------------*/
static int checkRunning(pTableDrive self, SConnection *pCon){
		int status;
		int test;
		tdMotor moti;
		
		status = LLDnodePtr2First(self->motorTable);
		while(status != 0){
			LLDnodeDataTo(self->motorTable,&moti);
			test = moti.pMot->pDrivInt->CheckStatus(moti.pMot,pCon);
			if(test != HWIdle && test != OKOK){
				return test;
			}
			status = LLDnodePtr2Next(self->motorTable);
		}
		return HWIdle;
}
/*------------------------------------------------------------------------*/
static void showPositions(pTableDrive self, SConnection *pCon,
							char *pBueffel, int buffLen){
	float value;
	int status, test, len;
	tdMotor moti;
	
    value = TableDriveGetValue(self,pCon);
    snprintf(pBueffel,buffLen," %8.2f : ", value);
    
    status = LLDnodePtr2First(self->motorTable);
    while(status != 0){
    	LLDnodeDataTo(self->motorTable,&moti);
    	test = MotorGetSoftPosition(moti.pMot,pCon,&value);
    	len = strlen(pBueffel);
    	snprintf(&pBueffel[len],buffLen - len,
    			"%s = %8.2f ", moti.motorName, value);
    	status = LLDnodePtr2Next(self->motorTable);
    }
}
/*-------------------------------------------------------------------------
 * This is the old scheme: drive table positions tablestep by tablestep.
 * I leave this code here: may be it is useful in another scenario then
 * MARS. Then some means must be found to select the table drive 
 * algorithm.
 * -----------------------------------------------------------------------*/
static int TableDriveCheckStatusOld(void *pData, SConnection *pCon){
	pTableDrive self = (pTableDrive)pData;
	int status;
	float step;
	char pBueffel[1024];

	switch(self->state){
		case STARTING:
			/*
			 * make sure that our current position is up-to-date
			 */
			TableDriveGetValue(self,pCon);
			liberateMotors(self,pCon);
			self->state = STEPPING;
			return HWBusy;
			break;
		case STEPPING:
			step = calculateNextStep(self);
			status = runToNextStep(self,pCon, self->currentPosition + step);
			if(status == OKOK){
				if(ABS(step) < 1.){
					self->state = WAITFINISH;
				} else {
					self->state = WAITING;
				}
				return HWBusy;
			} else {
				TableDriveHalt(self);
				return HWFault;
			}
			break;
		case WAITING:
			status = checkRunning(self,pCon);
			switch(status){
				case HWIdle:
				case OKOK:
					self->state = STEPPING;
					if(self->debug > 0){
						showPositions(self,pCon,pBueffel,1023);
						SCWrite(pCon,pBueffel,eValue);
					} else {
						self->currentPosition += calculateNextStep(self);
					} 
					return HWBusy;
					break;
				case HWBusy:
					return HWBusy;
					break;
				default:
					TableDriveHalt(self);
					self->state = WAITFINISH;
					return HWBusy;
					break;								
			}
		case WAITFINISH:
			status = checkRunning(self,pCon);
			if(status != HWBusy){
				TableDriveGetValue(self,pCon);
				closeMotors(self,pCon);
                                self->state = IDLE;
				return status;
			} else {
			  return HWBusy;
			}
			break;
        case OUTOFSYNC:
            SCWrite(pCon,"WARNING: tabledrive out of sync",eWarning);
            self->state = IDLE;
            return HWFault;
            break;
		default:
			SCWrite(pCon,
			"ERROR: programming error in tabledrive, invalid state",
			eError);
			return HWFault;
			break;
	}	
	return HWFault; 
}
/*===========================================================================
 * This is code for the new scheme of driving MARS tables:
 * 1) drive ds and as to 0
 * 2) drive all other motors to targets directly
 * 3) drive ds and as to desired positions.
 * =========================================================================*/
 #define WAITDSAS  110
 #define WAITOTHER 111
 #define ASDSTO0      200
 #define ASDSTOTARGET 201
/*-------------------------------------------------------------------------*/
static int startASDS(pTableDrive self, SConnection *pCon, int target){
    tdMotor moti;
    int status;
    char name[132];
    float targetValue;
    
    if(self == NULL || self->motorTable < 0){
            return 0;
    }
            
    status = LLDnodePtr2First(self->motorTable);
    while(status != 0){
        LLDnodeDataTo(self->motorTable,&moti);
        strncpy(name,moti.motorName,131);
        strtolower(name);
        if(strstr(name,"ds") != NULL || strstr(name,"as") != NULL){
            if(target == ASDSTO0){
                targetValue = 0.0;
            } else  {
                targetValue = findTarget(moti, self->targetPosition);
            }
            status  = MotorRun(moti.pMot,pCon,targetValue);
            if(status != OKOK){
                return status;
            }
         }
        status = LLDnodePtr2Next(self->motorTable); 
    }
    return OKOK;
}
/*-------------------------------------------------------------------------*/
static int startOthers(pTableDrive self, SConnection *pCon){
    tdMotor moti;
    int status;
    char name[132];
    float targetValue;
    
    if(self == NULL || self->motorTable < 0){
            return 0;
    }
            
    status = LLDnodePtr2First(self->motorTable);
    while(status != 0){
        LLDnodeDataTo(self->motorTable,&moti);
        strncpy(name,moti.motorName,131);
        strtolower(name);
        if(strstr(name,"ds") == NULL && strstr(name,"as") == NULL){
            targetValue = findTarget(moti, self->targetPosition);
            status  = MotorRun(moti.pMot,pCon,targetValue);
            if(status != OKOK){
                return status;
            }
         }
        status = LLDnodePtr2Next(self->motorTable); 
    }
    return OKOK;
}
/*-------------------------------------------------------------------------*/
static int TableDriveCheckStatus(void *pData, SConnection *pCon){
    pTableDrive self = (pTableDrive)pData;
    int status;
    float step;
    char pBueffel[1024];

    switch(self->state){
        case STARTING:
            /*
             * make sure that our current position is up-to-date
             */
            TableDriveGetValue(self,pCon);
            liberateMotors(self,pCon);
            status = startASDS(self,pCon,ASDSTO0);
            if(status != OKOK){
	      TableDriveHalt(self);
	      return HWFault;
            }
            self->state = WAITDSAS;    
            return HWBusy;
            break;
         case WAITDSAS:
            status = checkRunning(self,pCon);
            switch(status){
                case HWIdle:
                case OKOK:
                    if(self->debug > 0){
                        showPositions(self,pCon,pBueffel,1023);
                        SCWrite(pCon,pBueffel,eValue);
                    }
                    status = startOthers(self,pCon);
                    if(status != OKOK){
		      TableDriveHalt(self);
		      return HWFault;
                    }
                    self->state = WAITOTHER;
                    return HWBusy;
                    break;
                case HWBusy:
                    return HWBusy;
                    break;
                default:
                    TableDriveHalt(self);
                    self->state = WAITFINISH;
                    return HWBusy;
                    break;                              
            }
            break;
         case WAITOTHER:
            status = checkRunning(self,pCon);
            switch(status){
                case HWIdle:
                case OKOK:
                    if(self->debug > 0){
                        showPositions(self,pCon,pBueffel,1023);
                        SCWrite(pCon,pBueffel,eValue);
                    }
                    status = startASDS(self,pCon,ASDSTOTARGET);
                    if(status != OKOK){
		      TableDriveHalt(self);
		      return HWFault;
                    }
                    self->state = WAITFINISH;
                    return HWBusy;
                    break;
                case HWBusy:
                    return HWBusy;
                    break;
                default:
                    TableDriveHalt(self);
                    self->state = WAITFINISH;
                    return HWBusy;
                    break;                              
            }
            break;
        case WAITFINISH:
            status = checkRunning(self,pCon);
            if(status != HWBusy){
                if(self->debug > 0){
                    showPositions(self,pCon,pBueffel,1023);
                    SCWrite(pCon,pBueffel,eValue);
                }
                TableDriveGetValue(self,pCon);
                closeMotors(self,pCon);
                self->state = IDLE;
                return status;
            } else {
              return HWBusy;
            }
            break;
        case OUTOFSYNC:
            SCWrite(pCon,"WARNING: tabledrive out of sync",eWarning);
            self->state = IDLE;
            return HWFault;
            break;
        default:
            SCWrite(pCon,
            "ERROR: programming error in tabledrive, invalid state",
            eError);
            return HWFault;
            break;
    }
    return HWFault;
}  
/*================== live and death ========================================*/
static void *TableDriveInterface(void *pData, int ID){
	pTableDrive self = (pTableDrive)pData;
	if(self == NULL){
		return NULL;
	}
	
	if(ID == DRIVEID){
		return self->pDriv;
	} else {
		return NULL;
	}
}
/*--------------------------------------------------------------------------*/
static void clearTable(int table){
	int status;
	tdMotor moti;
	
	status = LLDnodePtr2First(table);
	while(status != 0){
		LLDnodeDataTo(table,&moti);
		LLDdelete(moti.table);
		status = LLDnodePtr2Next(table);	
	}
}
/*------------------------------------------------------------------------*/
static int loadTable(pTableDrive self, char *filename, SConnection *pCon){
	FILE *fd = NULL;
	tdMotor moti;
	tdEntry entry;
	char pBueffel[512];
	int lineCount, tableCount = -1, read; 
	
	if(self->motorTable >= 0){
		clearTable(self->motorTable);
	}
	fd = fopen(filename,"r");
	if(fd == NULL){
		snprintf(pBueffel,511,"ERROR: failed to open %s for reading",
				filename);
		SCWrite(pCon,pBueffel,eError);
		return 0;	 
	}
	self->motorTable = LLDcreate(sizeof(tdMotor));
	if(self->motorTable < 0){
		SCWrite(pCon,"ERROR: failed to allocate motor table",eError);
		fclose(fd);
		return 0;
	}
	
	while(fgets(pBueffel,511,fd) != NULL){
		if(pBueffel[0] == '#'){
			strcpy(moti.motorName,trim(&pBueffel[1]));
			moti.pMot = FindCommandData(pServ->pSics,
						    moti.motorName,
						    "Motor");
			if(moti.pMot == NULL){
				snprintf(pBueffel,511,"ERROR: motor %s NOT found!",
					moti.motorName);
				SCWrite(pCon,pBueffel,eError);
				fclose(fd);
				return 0;
			}
			moti.table = LLDcreate(sizeof(tdEntry));
			if(moti.table < 0){
				SCWrite(pCon,"ERROR: failed to allocate table data",eError);
				fclose(fd);
				return 0;
			}
			lineCount = 1;
		} else {
			read = sscanf(pBueffel,"%lf %lf %lf", &entry.lower,&entry.position,
						&entry.upper);
			if(read >= 3){
				entry.tablePos = lineCount;
				LLDnodeAppendFrom(moti.table,&entry);
				lineCount++;	
			} else {
				/*
			 	* we are on a break line
			 	*/
			 	if(tableCount < 0){
			 		tableCount = lineCount;
			 	} else {
			 		if(lineCount != tableCount){
			 			SCWrite(pCon,
			 				"ERROR: bad table file, table length mismatch",
			 				eError);
			 			fclose(fd);
			 			return 0;
			 		}
			 	}
			 	LLDnodeAppendFrom(self->motorTable,&moti);
			}
		}
	}
	self->tableLength = tableCount-1;
	fclose(fd);
	return 1;
}
/*-------------------------------------------------------------------------*/
static void killTableDrive(void *pData){
	pTableDrive self = (pTableDrive)pData;
	if(self == NULL){
		return;
	}
	if(self->pDes != NULL){
		DeleteDescriptor(self->pDes);
	}
	if(self->motorTable >= 0){
		clearTable(self->motorTable);
		LLDdelete(self->motorTable);
		self->motorTable = -1;
	}
	if(self->pDriv != NULL){
		free(self->pDriv);
	}
	free(self);
}
/*-------------------------------------------------------------------------*/
int TableDriveFactory(SConnection *pCon, SicsInterp *pSics, void *pData,
                        int argc, char *argv[]){
    pTableDrive pNew = NULL;
    char pBueffel[256];
    int status;
    
    if(argc < 3){
    	SCWrite(pCon,"ERROR: insufficient parameters to TableDriveFactory",
    			eError);
    	return 0;
    }
    
    pNew = (pTableDrive)malloc(sizeof(TableDrive));
    if(pNew == NULL){
    	SCWrite(pCon,"ERROT: out of memory creating table drive",eError);
    	return 0;
    }
    memset(pNew,0,sizeof(TableDrive));
    
    pNew->pDes = CreateDescriptor("TableDrive");
    pNew->pDriv = CreateDrivableInterface();
    if(pNew->pDes == NULL || pNew->pDriv == NULL){
    	SCWrite(pCon,"ERROT: out of memory creating table drive",eError);
    	return 0;
    }
    pNew->pDes->GetInterface = TableDriveInterface;
	pNew->pDriv->CheckLimits = TableDriveCheckLimits;
	pNew->pDriv->CheckStatus = TableDriveCheckStatus;
	pNew->pDriv->GetValue    = TableDriveGetValue;
	pNew->pDriv->Halt        = TableDriveHalt;
	pNew->pDriv->SetValue    = TableDriveSetValue;
	pNew->motorTable         = -1;
	pNew->state              = OUTOFSYNC;
	pNew->tableLength        = 0;
	pNew->targetPosition     = 0;
	pNew->oriInvalid = 1;
    
	if(!loadTable(pNew,argv[2],pCon)){
		killTableDrive(pNew);
		return 0;
	}
	pNew->state = IDLE;
    
    status = AddCommand(pSics,argv[1],
    					TableDriveAction,
    					killTableDrive,
    					pNew);
    if(!status){
    	snprintf(pBueffel,256,"ERROR: duplicate command %s not created",
    			argv[1]);
    	SCWrite(pCon,pBueffel,eError);
    	killTableDrive(pNew);
    	return 0;		
    }					
	return 1;
}
/*====================== interpreter interface ============================*/
int TableDriveAction(SConnection *pCon, SicsInterp *pSics, void *pData,
                        int argc, char *argv[]){
    pTableDrive self = (pTableDrive)pData;
    int status;
    char pBueffel[1024];
    float value;
    pDynString print = NULL;    
                        
    if(argc < 2){
    	value = TableDriveGetValue(self,pCon);
    	snprintf(pBueffel,255,"%s = %f", argv[0], value);
    	SCWrite(pCon,pBueffel,eValue);
    	if(value > -999){
    		return 1;
    	} else {
    		return 0;
    	}
    } else {
    	strtolower(argv[1]);
    	if(strcmp(argv[1],"load") == 0){
    		if(!SCMatchRights(pCon,usMugger)){
    			return 0;
    		}
    		if(argc < 3){
    			SCWrite(pCon,"ERROR: require filename to load",
    				eError);
    			return 0;
    		}
    		status = loadTable(self,argv[2],pCon);
    		if(status == 1){
    			SCSendOK(pCon);
    		}
    		return status;
        }else if(strcmp(argv[1],"show") == 0){
            showPositions(self,pCon,pBueffel,1023);
            SCWrite(pCon,pBueffel,eValue);
            return 1;
        }else if(strcmp(argv[1],"info") == 0){
            SCStartBuffering(pCon);
            tableInfo(self,pCon);
            print = SCEndBuffering(pCon);
            if(print != NULL){
                SCWrite(pCon,GetCharArray(print), eValue);
            }
            return 1;
    	}else if(strcmp(argv[1],"orient") == 0){
    		if(argc > 2){
    			if(!SCMatchRights(pCon,usMugger)){
    				return 0;
    			}
	    		strncpy(self->orientMotor,argv[2],80);
                self->oriInvalid = 1;
	    		SCSendOK(pCon);
	    		return 1;
    		} else {
    			snprintf(pBueffel,255,"%s.orient = %s",
    				argv[0],self->orientMotor);
    			SCWrite(pCon,pBueffel,eValue);
    			return 1;
    		}
    	}else if(strcmp(argv[1],"debug") == 0){
    		if(self->debug == 0){
    			self->debug = 1;
    		} else {
    			self->debug = 0;
    		}
    		SCSendOK(pCon);
    		return 1;
    	} else {
    		SCWrite(pCon,"ERROR: sub command not understood",eError);
    		return 0;
    	}
    }                  	
	return 1;
}