/*---------------------------------------------------------------------------
  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 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 < 0. || 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;
	
	status = LLDnodePtr2First(self->motorTable);
	while(status != 0){
		LLDnodeDataTo(self->motorTable,moti);
		if(strcmp(moti->motorName,self->orientMotor) == 0){
			return 1;
		}
		status = LLDnodePtr2Next(self->motorTable);
	}
	return 0;
}
/*-------------------------------------------------------------------------*/
static float interpolateLocate(tdEntry lower, tdEntry upper, 
		float value, int count){
	float diff, result = count -1;
	
	result = count;
	diff = upper.position - lower.position;
	value -= lower.position;
	if(ABS(diff) > .00001){
		result += value/diff;
	}
	return result;
}
/*-------------------------------------------------------------------------*/
static float locatePosition(tdMotor moti, SConnection *pCon){
	int status, count = 0;
	float value;
	tdEntry entry, back;
	
	status = MotorGetSoftPosition(moti.pMot,pCon,&value);
	if(!status) {
		return -9999.99;
	}
	status = LLDnodePtr2First(moti.table);
	while(status != 0){
		LLDnodeDataTo(moti.table,&entry);
		if(entry.position > value){
			if(count == 0){
				return 1.;
			}
			LLDnodePtr2Prev(moti.table);
			LLDnodeDataTo(moti.table,&back);
			return interpolateLocate(back,entry,value, count);			
		}
		count++;
		status = LLDnodePtr2Next(moti.table);
	}
	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);
	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];
	
	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;
}
/*------------------------------------------------------------------------
 * 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);
		moti.pMot->ParArray[SLOW].fVal = lower;
		moti.pMot->ParArray[SUPP].fVal = upper;
		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;
		moti.pMot->ParArray[SLOW].fVal = lower;
		moti.pMot->ParArray[SUPP].fVal = upper;
		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);
    }
}
/*-------------------------------------------------------------------------*/
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);
			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);
				return status;
			}
			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;
	
	if(!loadTable(pNew,argv[2],pCon)){
		killTableDrive(pNew);
		return 0;
	}
    
    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;    
                        
    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],"orient") == 0){
    		if(argc > 2){
    			if(!SCMatchRights(pCon,usMugger)){
    				return 0;
    			}
	    		strncpy(self->orientMotor,argv[2],80);
	    		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;
}