/**
 * This is the implementation file for a second generation velocity selector 
 * object. This is an extension to the drivable object. It mainly adds handling of 
 * the tilt motor to the system.
 * 
 * copyright: see file COPYRIGHT
 * 
 * Mark Koennecke, April 2009
 */
#include <stdlib.h>
#include <sics.h>
#include <motor.h>
#include <sicsobj.h>
#include <scriptcontext.h>
#include <sicshipadaba.h>
#include <drive.h>

#define ABS(x) (x < 0 ? -(x) : (x))
/*
 * from sctdriveobj.c 
 */
pSICSOBJ MakeSctDriveObj(pHdb node, char *class, SctController * c,
                         int doNotKillNode);
/*--------------------------------------------------------------------------*/
static hdbCallbackReturn tiltCallback(pHdb node, void *userData, 
		pHdbMessage message)
{
	pHdbDataMessage get = NULL, set = NULL;
	pMotor tilt = (pMotor)userData;
	float val, rot;
	char nvsName[80];
	SConnection *pCon, *mine;
	void *pNVS = NULL;
	int status;
	
	assert(tilt != NULL);
	
	if((get = GetHdbGetMessage(message)) != NULL){
		if(get->callData != NULL){
			val = tilt->pDrivInt->GetValue(tilt,(SConnection *)get->callData);
		} else {
			val = tilt->pDrivInt->GetValue(tilt,pServ->dummyCon);
		}
		node->value.v.doubleValue = val;
		return hdbContinue;
	}
	if((set = GetHdbSetMessage(message)) != NULL){
		if(set->callData != NULL){
			pCon = (SConnection *)set->callData;
		} else {
			pCon = pServ->dummyCon;
		}
		/*
		 * do we need to do anything?
		 */
		GetDrivablePosition(tilt,pCon,&val);
		if(ABS(val - set->v->v.doubleValue) < .01 ){
			SCWrite(pCon,"tilt already at position", eValue);
			return hdbContinue;
		}
		GetHdbProperty(node,"NVS", nvsName, 80);

		pNVS = FindCommandData(pServ->pSics,nvsName,"NVS");
		assert(pNVS != NULL);
		GetDrivablePosition(pNVS, pCon, &rot);
		
		SCWrite(pCon,"Wait for NVS to stop...", eLog);
		status = Drive(pCon, pServ->pSics, nvsName, .0);
		if(status != 1){
			SCWrite(pCon,"ERROR: failed to stop NVS, driving tilt aborted", eError);
			return hdbContinue;
		}
		
		SCWrite(pCon,"Actually driving tilt...", eLog);
		mine = SCCopyConnection(pCon);
		SCSetRights(mine, usInternal);
		status = Drive(pCon, pServ->pSics, tilt->name, (float)set->v->v.doubleValue);
		if(status != 1){
			SCWrite(pCon,"ERROR: failed to drive tilt, NVS stopped", eError);
			return hdbContinue;
		}
		SCDeleteConnection(mine);
		GetDrivablePosition(tilt,pCon,&val);
		UpdateHipadabaPar(node, MakeHdbFloat(val), pCon);
		
		SCWrite(pCon,"Wait even more for NVS to get to speed again...", eLog);
		status = Drive(pCon, pServ->pSics, nvsName, rot);
		if(status != 1){
			SCWrite(pCon,"ERROR: failed to run NVS to speed", eError);
			return hdbContinue;
		}
		SCWrite(pCon,"Done", eValue);
	}
	return hdbContinue;
}
/*--------------------------------------------------------------------------*/
int MakeSecNVS(SConnection * pCon, SicsInterp * pSics,
                       void *object, int argc, char *argv[])
{
	pMotor tilt = NULL;
	pSICSOBJ pNew = NULL;
	pSICSOBJ sct = NULL;
	pHdb node = NULL, tilli;
	int status;
	
	if(argc < 4){
		SCWrite(pCon,
		"ERROR: missing required parameters, Usage: MakeNVS name tiltmotorname sctcontroller", 
		eError);
		return 0;
	}
	
	tilt = (pMotor)FindCommandData(pSics,argv[2],"Motor");
	if(tilt == NULL){
		SCPrintf(pCon,eError, "ERROR: tilt motor %s not found", argv[2]);
		return 0;
	}
	
	sct =  (pSICSOBJ)FindCommandData(pSics, argv[3],"SctController");
	if(sct == NULL){
		SCPrintf(pCon,eError, "ERROR: scriptcontext %s not found", argv[3]);
		return 0;
	}
	
	node = MakeHipadabaNode(argv[1],HIPFLOAT,1);
	SetHdbProperty(node,"sicsdev",argv[1]);
	SetHdbProperty(node,"priv", "user");
	
	pNew = MakeSctDriveObj(node, "NVS", sct->pPrivate, 0);
	if(pNew == NULL || node == NULL){
		SCWrite(pCon,"ERROR: failed to create NVS, out of memory?", 
				eError);
		return 0;
	}
	
	
	tilli = MakeSICSHdbPar("tilt", usMugger, MakeHdbFloat(.0));
	if(tilli == NULL){
		KillSICSOBJ(pNew);
		SCWrite(pCon,"ERROR: failed to create tilt node, out-of-memory?", eError);
		return 0;
	}
	/*
	 * In the set callback I need the name of the NVS because I need to stop and 
	 * drive it up again. Rather then have another data structure I pass this name
	 * as a property to the tilt node.
	 */
	SetHdbProperty(tilli,"NVS", argv[1]);
	
	MotorSetPar(tilt,pCon,"accesscode", (float)usInternal);
	AppendHipadabaCallback(tilli, MakeHipadabaCallback(tiltCallback, tilt,NULL));
	AddHipadabaChild(node, tilli, pCon);
	
	status = AddCommand(pSics,
	                      argv[1], InterInvokeSICSOBJ, KillSICSOBJ, pNew);

	return status;
}