/**
 * This is the implementation of a SICS access module for the POLDI
 * pull machine for testing mechanical samples. This thing is a one 
 * of a kind; therefore its implementation does not follow the usual
 * SICS distinction between hardware object and driver. The thing can 
 * operate in two modes: either a force is driven or a position. 
 * 
 * copyright: see file COPYRIGHT
 * 
 * Mark Koennecke, October 2006(started) 
 *                 - May 2007(The device was actually available)
 */
#include <sics.h>
#include <rs232controller.h>
/*===============================================================*/
#define FORCE  0
#define ZUGPOS 1
#define REGEL  10
#define OFF    20
/*---------------------------------------------------------------*/
typedef struct {
  pObjectDescriptor pDes;
  pIDrivable pDriv;
  prs232 controller;
  int iMode;
  float maxForce, minForce;
  float maxPos, minPos;
  int velocity;
} PZR, *pPZR;
/*--------------------------------------------------------------*/
static int PZRCommand(prs232 controller, SConnection * pCon,
                      char *command, char *reply, int replyLen)
{
  int status, i;
  char buffer[512], error[132];

  /*
   * try three times to get our message through...
   */
  for (i = 0; i < 3; i++) {
    status = transactRS232(controller, command, strlen(command),
                           reply, replyLen);
    if (status > 0) {
      if (strstr(reply, "ERR") != NULL) {
        if (pCon != NULL) {
          snprintf(buffer, 511, "ERROR: device reported %s", reply);
          SCWrite(pCon, buffer, eError);
          return HWFault;
        }
      }
      return 1;
    }
    /*
     * error processing
     */
    memset(error, 0, 132);
    getRS232Error(status, error, 131);
    status = fixRS232Error(controller, status);
    if (pCon != NULL) {
      if (status == 1) {
        snprintf(buffer, 511, "WARNING: trying to fix: %s", error);
        SCWrite(pCon, buffer, eWarning);
      } else {
        snprintf(buffer, 511, "ERROR: %s", error);
        SCWrite(pCon, buffer, eError);
      }
    }
    if (status == 0) {
      return HWFault;
    }
  }
  return HWFault;
}

/*===============================================================*/
static void *PZRGetInterface(void *data, int iD)
{
  pPZR self = NULL;

  self = (pPZR) data;
  if (self != NULL && iD == DRIVEID) {
    return self->pDriv;
  } else {
    return NULL;
  }
  return NULL;
}

/*----------------------------------------------------------------
 This routine can return either OKOK or HWFault when thing
 go wrong. However, the return value of Halt is usually ignored!
------------------------------------------------------------------*/
static int PZRHalt(void *data)
{
  pPZR self = NULL;
  char buffer[80];

  self = (pPZR) data;

  PZRCommand(self->controller, NULL, "stop\r\n", buffer, 79);

  return OKOK;
}

/*----------------------------------------------------------------
 This routine can return either 1 or 0. 1 means the position can 
 be reached, 0 NOT
 If 0, error shall contain up to errlen characters of information
 about which limit was violated
------------------------------------------------------------------*/
static int PZRCheckLimits(void *data, float val, char *error, int errlen)
{
  pPZR self = NULL;

  self = (pPZR) data;
  if (self->iMode == FORCE) {
    if (val < self->minForce || val > self->maxForce) {
      snprintf(error, errlen, "%f is not within limits %f to %f",
               val, self->minForce, self->maxForce);
      return 0;
    }
  } else {
    if (val < self->minPos || val > self->maxPos) {
      snprintf(error, errlen, "%f is not within limits %f to %f",
               val, self->minPos, self->maxPos);
      return 0;
    }
  }
  return 1;
}

/*----------------------------------------------------------------
 This routine can return 0 when a limit problem occurred 
 OKOK when the motor was successfully started 
 HWFault when a problem occured starting the device
 Possible errors shall be printed to pCon
 For real motors, this is supposed to try at least three times
 to start the motor in question
 val is the value to drive the motor too
------------------------------------------------------------------*/
static long PZRSetValue(void *data, SConnection * pCon, float val)
{
  pPZR self = NULL;
  char command[80], reply[80];
  int status;

  self = (pPZR) data;

  switch (self->iMode) {
  case FORCE:
      snprintf(command, 79, "wr vel %d\r\n", self->velocity);
      status = PZRCommand(self->controller, pCon, command, reply, 79);
      if (status != 1) {
        return HWFault;
      }
      snprintf(command, 79, "wr force %f\r\n", val);
      status = PZRCommand(self->controller, pCon, command, reply, 79);
      if (status != 1) {
        return HWFault;
      }
      status =
          PZRCommand(self->controller, pCon, "wr mode 1\r\n", reply, 79);
      if (status != 1) {
        return HWFault;
      }
      return OKOK;
    break;
  case ZUGPOS:
    snprintf(command, 79, "wr vel %d\r\n", self->velocity);
    status = PZRCommand(self->controller, pCon, command, reply, 79);
    if (status != 1) {
      return HWFault;
    }
    snprintf(command, 79, "wr pos %f\r\n", val);
    status = PZRCommand(self->controller, pCon, command, reply, 79);
    if (status != 1) {
      return HWFault;
    }
    status =
        PZRCommand(self->controller, pCon, "wr mode 2\r\n", reply, 79);
    if (status != 1) {
      return HWFault;
    }
    return OKOK;
    break;
  }
  return OKOK;
}

/*----------------------------------------------------------------
 Checks the status of a running motor. Possible return values
   HWBusy The motor is still running
   OKOK or HWIdle when the motor finished driving
   HWFault when a hardware problem ocurred
   HWPosFault when the hardware cannot reach a position
 Errors are duly to be printed to pCon
 For real motors CheckStatus again shall try hard to fix any 
 issues with the motor 
------------------------------------------------------------------*/
static int BitSet(char d, int n)
{
  unsigned char e;

  e = d & (1 << n);
  if (e == 0) {
    return 0;
  } else {
    return 1;
  }
}

/*---------------------------------------------------------------*/
static int PZRCheckStatus(void *data, SConnection * pCon)
{
  pPZR self = NULL;
  int status, code, i;
  char command[80], reply[80], bits;

  self = (pPZR) data;
  snprintf(command, 79, "rd stat\r\n");
  status = PZRCommand(self->controller, pCon, command, reply, 79);
  if (status != 1) {
    return HWFault;
  }
  sscanf(reply, "%x", &code);
  bits = (char) code;

  /*
     memset(command,0,80);
     for(i = 0; i < 8; i++){
     if(BitSet(bits,i)){
     command[i] = '1';
     } else {
     command[i] = '0';
     }    
     }
     snprintf(reply,79,"code = %d",code);
     strcat(command,reply);
     SCWrite(pCon,command,eValue);
   */

  /*
   * check for errors
   */
  if (BitSet(bits, 4)) {
    SCWrite(pCon, "ERROR: hit positive limit switch", eError);
    return HWFault;
  }
  if (BitSet(bits, 5)) {
    SCWrite(pCon, "ERROR: hit negative limit switch", eError);
    return HWFault;
  }
  if (BitSet(bits, 6)) {
    SCWrite(pCon, "ERROR: max force exceed, manual reset REQUIRED!",
            eError);
    return HWFault;
  }
  if (BitSet(bits, 7)) {
    SCWrite(pCon, "ERROR: bit 7 error, ask Marcel what this means",
            eError);
    return HWFault;
  }

  switch (self->iMode) {
  case FORCE:
    if (BitSet(bits, 2)) {
      return HWIdle;
    } else {
      return HWBusy;
    }
    break;
  case ZUGPOS:
    if (BitSet(bits, 3)) {
      return HWIdle;
    } else {
      return HWBusy;
    }
    break;
  }

  return HWFault;
}

/*----------------------------------------------------------------
 GetValue is supposed to read a motor position
 On errors, -99999999.99 is returned and messages printed to pCon
------------------------------------------------------------------*/
static float PZRGetValue(void *data, SConnection * pCon)
{
  pPZR self = NULL;
  float val = -9999.99;
  int status;
  char command[80], reply[80];

  self = (pPZR) data;
  if (self->iMode == FORCE) {
    snprintf(command, 79, "rd force\r\n");
  } else {
    snprintf(command, 79, "rd pos\r\n");
  }
  status = PZRCommand(self->controller, pCon, command, reply, 79);
  if (status != 1) {
    return HWFault;
  }
  sscanf(reply, "%f", &val);
  return val;
}

/*---------------------------------------------------------------*/
static int PZRConnect(pPZR self, SConnection * pCon)
{
  char error[132], buffer[356];
  int status;

  closeRS232(self->controller);
  status = initRS232(self->controller);
  if (status < 0) {
    getRS232Error(status, error, 131);
    snprintf(buffer, 255, "ERROR: failed to reconnect with: %s", error);
    SCWrite(pCon, buffer, eError);
    return 0;
  }
  SCSendOK(pCon);
  return 1;
}

/*----------------------------------------------------------------
 returns NULL on failure, a new datastructure else
------------------------------------------------------------------*/
static pPZR PZRMakeObject(char *host, int port)
{
  pPZR self = NULL;

  self = malloc(sizeof(PZR));
  if (self == NULL) {
    return NULL;
  }
  self->pDes = CreateDescriptor("PoldiReiss");
  self->pDriv = CreateDrivableInterface();
  self->controller = createRS232(host, port);
  if (self->pDes == NULL || self->pDriv == NULL
      || self->controller == NULL) {
    free(self);
    return NULL;
  }
  initRS232(self->controller);
  setRS232SendTerminator(self->controller, "\n");
  setRS232ReplyTerminator(self->controller, "\n");
  setRS232Timeout(self->controller, 10000);
  setRS232Debug(self->controller, 1);


  self->pDes->GetInterface = PZRGetInterface;
  self->pDriv->Halt = PZRHalt;
  self->pDriv->CheckLimits = PZRCheckLimits;
  self->pDriv->SetValue = PZRSetValue;
  self->pDriv->CheckStatus = PZRCheckStatus;
  self->pDriv->GetValue = PZRGetValue;

  return self;
}

/*--------------------------------------------------------------------------*/
static void killPoldiZug(void *data)
{
  pPZR self = (pPZR) data;

  if (self == NULL) {
    return;
  }
  if (self->pDes != NULL) {
    DeleteDescriptor(self->pDes);
  }
  if (self->controller != NULL) {
    closeRS232(self->controller);
    KillRS232(self->controller);
  }
  free(self);
}

/*============================================================================*/
static int PoldiReissAction(SConnection * pCon, SicsInterp * pSics,
                            void *pData, int argc, char *argv[])
{
  pPZR self = NULL;
  char command[80], reply[80];
  float val;
  int status;

  self = (pPZR) pData;
  assert(self != NULL);

  if (argc > 1) {
    strtolower(argv[1]);
    if (strcmp(argv[1], "mode") == 0) {
      if (argc > 2) {
        if (!SCMatchRights(pCon, usUser)) {
          return 0;
        }
        strtolower(argv[2]);
        if (strcmp(argv[2], "force") == 0) {
          self->iMode = FORCE;
          SCSendOK(pCon);
          return 1;
        } else if (strcmp(argv[2], "pos") == 0) {
          self->iMode = ZUGPOS;
          SCSendOK(pCon);
          return 1;
        } else {
          SCWrite(pCon, "ERROR: zug mode unknown, allowed force, pos",
                  eError);
          return 0;
        }
      } else {
        if (self->iMode == FORCE) {
          snprintf(command, 79, "%s.mode = force", argv[0]);
        } else {
          snprintf(command, 79, "%s.mode = pos", argv[0]);
        }
        SCWrite(pCon, command, eValue);
        return 1;
      }
    } else if (strcmp(argv[1], "velocity") == 0) {
      if (argc > 2) {
        if (!SCMatchRights(pCon, usUser)) {
          return 0;
        }
        self->velocity = atoi(argv[2]);
        SCSendOK(pCon);
        return 1;
      } else {
        snprintf(command, 70, "%s.velocity = %d", argv[0], self->velocity);
        SCWrite(pCon, command, eValue);
        return 1;
      }
    } else if (strcmp(argv[1], "connect") == 0) {
      return PZRConnect(self, pCon);
    } else if (strcmp(argv[1], "off") == 0) {
      status =
          PZRCommand(self->controller, pCon, "wr mode 0\r\n", reply, 79);
      if (status == 1) {
        SCSendOK(pCon);
        return 1;
      } else {
        return 0;
      }
    } else if (strcmp(argv[1], "poslimits") == 0) {
      if (argc > 3) {
        if (!SCMatchRights(pCon, usMugger)) {
          return 0;
        }
        self->minPos = atof(argv[2]);
        self->maxPos = atof(argv[3]);
        SCSendOK(pCon);
        return 1;
      } else {
        snprintf(command, 80, "pzr.poslimits = %f,%f", self->minPos,
                 self->maxPos);
        SCWrite(pCon, command, eValue);
        return 1;
      }
    } else if (strcmp(argv[1], "forcelimits") == 0) {
      if (argc > 3) {
        if (!SCMatchRights(pCon, usMugger)) {
          return 0;
        }
        self->minForce = atof(argv[2]);
        self->maxForce = atof(argv[3]);
        SCSendOK(pCon);
        return 1;
      } else {
        snprintf(command, 80, "pzr.forcelimits = %f,%f", self->minForce,
                 self->maxForce);
        SCWrite(pCon, command, eValue);
        return 1;
      }
    }
  }
  val = self->pDriv->GetValue(self, pCon);
  snprintf(command, 79, "%s = %f", argv[0], val);
  SCWrite(pCon, command, eValue);
  return 1;
}

/*----------------------------------------------------------------------------
 * MakePoldiReiss name host port minForce maxForce minPos maxPos
 * ---------------------------------------------------------------------------*/
int MakePoldiReiss(SConnection * pCon, SicsInterp * pSics,
                   void *pData, int argc, char *argv[])
{
  pPZR self = NULL;
  char reply[80];

  if (argc < 8) {
    SCWrite(pCon, "ERROR: not enough arguments to MakePoldiReiss", eError);
    return 0;
  }
  self = PZRMakeObject(argv[2], atoi(argv[3]));
  if (self == NULL) {
    SCWrite(pCon, "ERROR: out of memory making PoldiReiss", eError);
    return 0;
  }
  self->minForce = atof(argv[4]);
  self->maxForce = atof(argv[5]);
  self->minPos = atof(argv[6]);
  self->maxPos = atof(argv[7]);
  self->iMode = FORCE;
  self->velocity = 2;
  PZRCommand(self->controller, pCon, "wr mode 0\r\n", reply, 79);

  AddCommand(pSics, argv[1], PoldiReissAction, killPoldiZug, self);
  return 1;
}
