/*--------------------------------------------------------------------------

	Some code to make EL734 motors as used at SINQ available in TCL.
	Just a wrapper around David Maden's motor control routines.
	
	You are free to use and modify this software for noncommercial
	usage.
	
	No warranties or liabilities of any kind taken by me or my employer
	
	Mark Koennecke June 1996
----------------------------------------------------------------------------*/
#include "sinq_prototypes.h"
/* #include <timers.h>*/
#include <tcl.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "rs232c_def.h"
#include "el734_def.h"

#define INACTIVE 999.8
#define MOTACURRACY 0.02
#define False 0
#define True 1

typedef struct {
  float fUpper;                 /* upper limit */
  float fLower;                 /* Lower Limit */
  int iFix;                     /* fixed, unfixed flag */
  float fSoftZero;              /* SW-zero point */
  float fSoftUpper;             /* software upper boundary */
  float fSoftLower;             /*    "      lower "      */
  int iLowFlag, iUpFlag, iZeroFlag;     /*activity flags */
  void *pData;                  /* EL734 open struct */
} EL734st;

EXTERN int EL734Action(ClientData pDat, Tcl_Interp * i, int a,
                       char *argv[]);
static void EL734Error2Text(char *pBuffer, int errcode);

/*---------------------------------------------------------------------------
	Tcl has a high niceness level. It deletes a command properly when
	exiting, reinitializing etc. I use this facility to kill off the
	motor initialised in EL734.
---------------------------------------------------------------------------*/
EXTERN void EL734Murder(ClientData pData)
{
  EL734st *pTa = (EL734st *) pData;
  EL734_Close(&(pTa->pData));
  free(pData);
}

/*----------------------------------------------------------------------------
	EL734 is the main entry point for this stuff. It connects to a motor
	and, on success, creates a new command with the name of the motor.
	Syntax:
		EL734 name host port channel index
---------------------------------------------------------------------------*/

int EL734(ClientData clientData, Tcl_Interp * interp,
          int argc, char *argv[])
{
  int iRet;
  EL734st *pEL734 = NULL;
  int iPort, iChannel, iMotor;
  char *pErr = NULL;
  char pBueffel[80];

  /* check arguments */
  if (argc < 6) {
    Tcl_AppendResult(interp,
                     " Insufficient arguments: EL734 name host port channel index",
                     (char *) NULL);
    return TCL_ERROR;
  }

  /* convert arguments */
  iRet = Tcl_GetInt(interp, argv[3], &iPort);
  if (iRet == TCL_ERROR) {
    Tcl_AppendResult(interp, "Need integer value for port", (char *) NULL);
    return iRet;
  }

  iRet = Tcl_GetInt(interp, argv[4], &iChannel);
  if (iRet == TCL_ERROR) {
    Tcl_AppendResult(interp, "Need integer value for channel",
                     (char *) NULL);
    return iRet;
  }

  iRet = Tcl_GetInt(interp, argv[5], &iMotor);
  if (iRet == TCL_ERROR) {
    Tcl_AppendResult(interp, "Need integer value for motor",
                     (char *) NULL);
    return iRet;
  }

  /* make a new pointer, initialise EL734st */
  pEL734 = (EL734st *) malloc(sizeof(EL734st));
  if (pEL734 == NULL) {
    Tcl_AppendResult(interp, "No memory in EL734", NULL);
    return TCL_ERROR;
  }
  pEL734->iFix = False;
  pEL734->fSoftZero = INACTIVE + 1;
  pEL734->fSoftUpper = INACTIVE + 1.;
  pEL734->fSoftLower = -INACTIVE - 1.;
  pEL734->iZeroFlag = False;
  pEL734->iLowFlag = False;
  pEL734->iUpFlag = False;

  /* open the motor, finally */
  iRet =
      EL734_Open(&(pEL734->pData), argv[2], iPort, iChannel, iMotor,
                 "STPMC EL734");
  if (iRet) {                   /* success */
    /* figure out motor limits */
    EL734_GetLimits(&(pEL734->pData), &(pEL734->fLower),
                    &(pEL734->fUpper));
    /* handle TCL, create new command: the motor */
    Tcl_CreateCommand(interp, strdup(argv[1]), EL734Action,
                      (ClientData) pEL734, EL734Murder);
    Tcl_AppendResult(interp, strdup(argv[1]), (char *) NULL);
    return TCL_OK;
  } else {
    EL734_ErrInfo(&pErr, &iPort, &iChannel, &iMotor);
    EL734Error2Text(pBueffel, iPort);
    Tcl_AppendResult(interp, pBueffel, (char *) NULL);
    free(pEL734);
    return TCL_ERROR;
  }
}

/*---------------------------------------------------------------------------
	CheckPos checks a position and converts it to a real position.
	Returns TCL_ERROR on mistake, TCL_OK else
---------------------------------------------------------------------------*/
static int CheckPos(Tcl_Interp * interp, EL734st * pData,
                    float fRequested, float *fDrive)
{
  float fPos;
  char pBueffel[132];

  /* fixed ? */
  if (pData->iFix) {
    Tcl_AppendResult(interp, "Motor fixed", NULL);
    return TCL_ERROR;
  }

  /* check against SW-boundaries */
  if (pData->iUpFlag) {
    if (fRequested > pData->fSoftUpper) {
      sprintf(pBueffel,
              "Requested position: %f violates SW-boundary %f",
              fRequested, pData->fSoftUpper);
      Tcl_AppendResult(interp, pBueffel, (char *) NULL);
      return TCL_ERROR;
    }
  }
  if (pData->iLowFlag) {
    if (fRequested < pData->fSoftLower) {
      sprintf(pBueffel,
              "Requested position: %f violates SW-boundary %f",
              fRequested, pData->fSoftLower);
      Tcl_AppendResult(interp, pBueffel, (char *) NULL);
      return TCL_ERROR;
    }
  }

  /* correct for zero point */
  if (pData->iZeroFlag) {
    fPos = fRequested - pData->fSoftZero;
  } else {
    fPos = fRequested;
  }

  /* check HW-boundaries */
  if ((fPos < pData->fLower) || (fPos > pData->fUpper)) {
    sprintf(pBueffel, " %f outside limits %f %f",
            fPos, pData->fLower, pData->fUpper);
    Tcl_AppendResult(interp, "Requested position: ",
                     pBueffel, (char *) NULL);
    return TCL_ERROR;
  }

  *fDrive = fPos;
  return TCL_OK;

}

/* -------------------------------------------------------------------------
	fucking standard library missing functionality!!!!!!!!!!!!!!!!
---------------------------------------------------------------------------*/
static float absf(float x)
{
  if (x < .0)
    return -x;
  else
    return x;
}

/*--------------------------------------------------------------------------

	EL734 Action is the routine where commands send to the motor will 
	end up.
	
	Syntax:
	motor lim               shows motor limits
        motor dr val            drives the motor to val
        motor run val           set the motor in motion, without waiting
                                for completion 
        motor pos               shows motor position
        motor fix               fixes a motor at a position 
        motor unfix             unfixes a fixed motor
        motor zero val          set a software zero point
        motor upper val         sets a software upper limit
        motor lower val         sets a software lower limit            
----------------------------------------------------------------------------*/
EXTERN int EL734Action(ClientData clientData, Tcl_Interp * interp,
                       int argc, char *argv[])
{
  EL734st *pData = (EL734st *) clientData;
  char pBueffel[80];
  char *pErr = NULL;
  int iRet, iMSR, iOMSR, iFPC, iFRC, iSS;
  float fPos, fNpos;
  double dPos;
  int i;
  struct RS__RplyStruct *pReply = NULL;


  /* check parameters */
  if (argc < 2) {
    Tcl_AppendResult(interp,
                     "Usage: motor and either dr, pos, hlim slim run zero up lo",
                     (char *) NULL);
    return TCL_ERROR;
  }
  if (pData == NULL) {
    Tcl_AppendResult(interp, "Motor data lost!!!!!!!!", (char *) NULL);
    return TCL_ERROR;
  }

  /* check for HW-lim */
  if (strcmp(argv[1], "hlim") == 0) {
    sprintf(pBueffel, " %f %f", pData->fLower, pData->fUpper);
    Tcl_AppendResult(interp, pBueffel, (char *) NULL);
    return TCL_OK;
  }

  /* check for SW-lim */
  if (strcmp(argv[1], "slim") == 0) {
    sprintf(pBueffel, " %f %f", pData->fSoftLower, pData->fSoftUpper);
    Tcl_AppendResult(interp, pBueffel, (char *) NULL);
    return TCL_OK;
  }

  /* fix & unfix */
  if (strcmp(argv[1], "fix") == 0) {
    pData->iFix = True;
    return TCL_OK;
  }
  if (strcmp(argv[1], "unfix") == 0) {
    pData->iFix = False;
    return TCL_OK;
  }

  /* reset */
  if (strcmp(argv[1], "reset") == 0) {
    pData->iFix = False;
    pData->iLowFlag = False;
    pData->iUpFlag = False;
    pData->iZeroFlag = False;
    return TCL_OK;
  }

  /* check for pos */
  if (strcmp(argv[1], "pos") == 0) {
    iRet = EL734_GetStatus(&(pData->pData),
                           &iMSR, &iOMSR, &iFPC, &iFRC, &iSS, &fPos);
    if (!iRet) {
      EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
      EL734Error2Text(pBueffel, iMSR);
      Tcl_AppendResult(interp, pBueffel, (char *) NULL);
      return TCL_ERROR;
    } else {
      if (pData->iZeroFlag) {
        fPos += pData->fSoftZero;
      }
      sprintf(pBueffel, " %f", fPos);
      Tcl_AppendResult(interp, pBueffel, NULL);
      return TCL_OK;
    }
  }

  /* zero point */
  if (strcmp(argv[1], "zero") == 0) {
    /* check for zero already been defined */
    if (pData->iZeroFlag) {
      Tcl_AppendResult(interp,
                       "Request to set new zero point rejected.",
                       " Use reset before new definition. ",
                       " I'll get confused otherwise ", NULL);
      return TCL_ERROR;
    }

    /* get the new position */
    if (argc < 3) {
      Tcl_AppendResult(interp, "Usage: motor zero val", NULL);
      return TCL_ERROR;
    }

    iRet = Tcl_GetDouble(interp, argv[2], &dPos);
    fNpos = dPos;
    if (iRet == TCL_ERROR) {
      Tcl_AppendResult(interp, "Need float value for new zeropint",
                       (char *) NULL);
      return iRet;
    }
    pData->fSoftZero = -fNpos;
    pData->iZeroFlag = True;
    return TCL_OK;
  }

  /* upper SW-limit   */
  if (strcmp(argv[1], "up") == 0) {
    /* get the new position */
    if (argc < 3) {
      Tcl_AppendResult(interp, "Usage: motor up val", NULL);
      return TCL_ERROR;
    }

    iRet = Tcl_GetDouble(interp, argv[2], &dPos);
    fNpos = dPos;
    if (iRet == TCL_ERROR) {
      Tcl_AppendResult(interp, "Need float value for new upper limit",
                       (char *) NULL);
      return iRet;
    }
    pData->fSoftUpper = fNpos;
    pData->iUpFlag = True;
    return TCL_OK;
  }

  /* lower SW-limit   */
  if (strcmp(argv[1], "lo") == 0) {
    /* get the new position */
    if (argc < 3) {
      Tcl_AppendResult(interp, "Usage: motor lo val", NULL);
      return TCL_ERROR;
    }

    iRet = Tcl_GetDouble(interp, argv[2], &dPos);
    fNpos = dPos;
    if (iRet == TCL_ERROR) {
      Tcl_AppendResult(interp, "Need float value for new lower limit",
                       (char *) NULL);
      return iRet;
    }
    pData->fSoftLower = fNpos;
    pData->iLowFlag = True;
    return TCL_OK;
  }



  /* this is most important: dr for Drive */
  if (strcmp(argv[1], "dr") == 0) {
    /* get the new position */
    if (argc < 3) {
      Tcl_AppendResult(interp, "Usage: motor dr val", NULL);
      return TCL_ERROR;
    }

    iRet = Tcl_GetDouble(interp, argv[2], &dPos);
    fNpos = dPos;
    if (iRet == TCL_ERROR) {
      Tcl_AppendResult(interp, "Need  value to drive to", (char *) NULL);
      return iRet;
    }

    /* check if compatible with limits */
    if (CheckPos(interp, pData, fNpos, &fPos) == TCL_ERROR)
      return TCL_ERROR;

    /* finally move */
    iRet = EL734_MoveWait(&(pData->pData), fPos, &iOMSR,
                          &iFPC, &iFRC, &fNpos);
    /* 99.99999999999% of all code is error checking */
    if (!iRet) {
      EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
      EL734Error2Text(pBueffel, iMSR);
      Tcl_AppendResult(interp, pBueffel, (char *) NULL);
      return TCL_ERROR;
    }

    /* check if driving has been done */
    if (absf(fPos - fNpos) > MOTACURRACY) {
      Tcl_AppendResult(interp,
                       " Motor error: inacurate driving!",
                       " Probably something serious is wrong ",
                       " Check the fucking hardware ", NULL);
      return TCL_ERROR;
    }
    return TCL_OK;
  }

  /* this is most important: run  for Driving without waiting */
  if (strcmp(argv[1], "run") == 0) {
    /* get the new position */
    if (argc < 3) {
      Tcl_AppendResult(interp, "Usage: motor run val", NULL);
      return TCL_ERROR;
    }

    iRet = Tcl_GetDouble(interp, argv[2], &dPos);
    fNpos = dPos;
    if (iRet == TCL_ERROR) {
      Tcl_AppendResult(interp, "Need value to run for", (char *) NULL);
      return iRet;
    }

    /* check if compatible with limits */
    if (CheckPos(interp, pData, fNpos, &fPos) == TCL_ERROR)
      return TCL_ERROR;

    /* finally move */
    iRet = EL734_MoveNoWait(&(pData->pData), fPos);

    /* 99.99999999999% of all code is error checking */
    if (!iRet) {
      EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
      EL734Error2Text(pBueffel, iMSR);
      Tcl_AppendResult(interp, pBueffel, (char *) NULL);
      return TCL_ERROR;
    }
    return TCL_OK;
  }

  /* the dangerous, undocumented expert command: com:
     sends something directly to the motor. All args following
     com will be concatenated in one string, closed with \r
     and send to the motor . A GetReply will be invoked in order
     to yield a return value. Usage by normal motor users strictly
     discouraged.
   */
  if (strcmp(argv[1], "com") == 0) {
    strcpy(pBueffel, argv[2]);
    for (i = 3; i < argc; i++) {
      strcat(pBueffel, " ");
      strcat(pBueffel, argv[i]);
    }
    sprintf(pBueffel, "%s\r", pBueffel);
    iRet = EL734_SendCmnds(&(pData->pData), pBueffel, NULL);
    if (!iRet) {
      EL734_ErrInfo(&pErr, &iMSR, &iOMSR, &iSS);
      EL734Error2Text(pBueffel, iMSR);
      Tcl_AppendResult(interp, pBueffel, (char *) NULL);
      return TCL_ERROR;
    }
    /* fetch reply */
    pReply =
        (struct RS__RplyStruct *) EL734_GetReply(&(pData->pData), NULL);
    while (pReply != NULL) {
      Tcl_AppendElement(interp, pReply->rply);
      pReply =
          (struct RS__RplyStruct *) EL734_GetReply(&(pData->pData),
                                                   pReply);
    }
    return TCL_OK;
  }

  /* if we end here an unknown command has been sent */
  Tcl_AppendResult(interp,
                   "Usage: motor and either dr, run,zero, pos, hlim"
                   "slim up low reset fix unfix", (char *) NULL);
  return TCL_ERROR;
}

/*---------------------------------------------------------------------------

	EL734Error2Text converts between an EL734 error code to text
-----------------------------------------------------------------------------*/
void EL734Error2Text(char *pBuffer, int iErr)
{
  switch (iErr) {
  case -28:
    strcpy(pBuffer, "EL734__BAD_ADR");
    break;
  case -8:
    strcpy(pBuffer, "EL734__BAD_BIND");
    break;
  case -30:
    strcpy(pBuffer, "EL734__BAD_BSY");
    break;
  case -3:
    strcpy(pBuffer, "EL734__BAD_CMD");
    break;
  case -9:
    strcpy(pBuffer, "EL734__BAD_CONNECT");
    break;
  case -23:
    strcpy(pBuffer, "EL734__BAD_FLUSH");
    break;
  case -6:
    strcpy(pBuffer, "EL734__BAD_HOST");
    break;
  case -10:
    strcpy(pBuffer, "EL734__BAD_ID");
    break;
  case -5:
    strcpy(pBuffer, "EL734__BAD_ILLG");
    break;
  case -2:
    strcpy(pBuffer, "EL734__BAD_LOC");
    break;
  case -11:
    strcpy(pBuffer, "EL734__BAD_MALLOC");
    break;
  case -21:
    strcpy(pBuffer, "EL734__BAD_NOT_BCD");
    break;
  case -4:
    strcpy(pBuffer, "EL734__BAD_OFL");
    break;
  case -29:
    strcpy(pBuffer, "EL734__BAD_PAR");
    break;

  case -17:
    strcpy(pBuffer, "EL734__BAD_RECV");
    break;
  case -19:
    strcpy(pBuffer, "EL734__BAD_RECV_NET");
    break;
  case -18:
    strcpy(pBuffer, "EL734__BAD_RECV_PIPE");
    break;
  case -20:
    strcpy(pBuffer, "EL734__BAD_RECV_UNKN");
    break;
  case -22:
    strcpy(pBuffer, "EL734__BAD_RECVLEN");
    break;
  case -24:
    strcpy(pBuffer, "EL734__BAD_RECV1");
    break;
  case -26:
    strcpy(pBuffer, "EL734__BAD_RECV1_NET");
    break;
  case -25:
    strcpy(pBuffer, "EL734__BAD_RECV1_PIPE");
    break;
  case -27:
    strcpy(pBuffer, "EL734__BAD_RNG");
    break;
  case -13:
    strcpy(pBuffer, "EL734__BAD_SEND");
    break;
  case -14:
    strcpy(pBuffer, "EL734__BAD_SEND_PIPE");
    break;
  case -15:
    strcpy(pBuffer, "EL734__BAD_SEND_NET");
    break;
  case -16:
    strcpy(pBuffer, "EL734__BAD_SEND_UNKN");
    break;
  case -12:
    strcpy(pBuffer, "EL734__BAD_SENDLEN");
    break;
  case -7:
    strcpy(pBuffer, "EL734__BAD_SOCKET");
    break;
  case -1:
    strcpy(pBuffer, "EL734__BAD_TMO");
    break;
  default:
    strcpy(pBuffer, "Unknown EL734 error");
    break;
  }
}
