/*---------------------------------------------------------------------------
  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) {
    strlcpy(error, "Path Table Not Defined!", 25);
    return 0;
  }
  if (fVal < 1. || fVal > self->tableLength) {
    strlcpy(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);
    strlcpy(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);
    strlcpy(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) {
      SCStartBuffering(pCon);	
      showPositions(self, pCon, pBueffel, 1023);
      SCWrite(pCon, pBueffel, eValue);
      print = SCEndBuffering(pCon);
      if(print != NULL){
    	  SCWrite(pCon,GetCharArray(print), 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;
        }
        strlcpy(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;
}