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

	Implementation file for the drive command

        This version as Test for Device Executor.

	Mark Koennecke,    December 1996


	Labor fuer Neutronenstreuung
	Paul Scherrer Institut
	CH-5423 Villigen-PSI


        The authors hereby grant permission to use, copy, modify, distribute,
        and license this software and its documentation for any purpose, provided
        that existing copyright notices are retained in all copies and that this
        notice is included verbatim in any distributions. No written agreement,
        license, or royalty fee is required for any of the authorized uses.
        Modifications to this software may be copyrighted by their authors
        and need not follow the licensing terms described here, provided that
        the new terms are clearly indicated on the first page of each file where
        they apply.

        IN NO EVENT SHALL THE AUTHORS OR DISTRIBUTORS BE LIABLE TO ANY PARTY
        FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES
        ARISING OUT OF THE USE OF THIS SOFTWARE, ITS DOCUMENTATION, OR ANY
        DERIVATIVES THEREOF, EVEN IF THE AUTHORS HAVE BEEN ADVISED OF THE
        POSSIBILITY OF SUCH DAMAGE.

        THE AUTHORS AND DISTRIBUTORS SPECIFICALLY DISCLAIM ANY WARRANTIES,
        INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY,
        FITNESS FOR A PARTICULAR PURPOSE, AND NON-INFRINGEMENT.  THIS SOFTWARE
        IS PROVIDED ON AN "AS IS" BASIS, AND THE AUTHORS AND DISTRIBUTORS HAVE
        NO OBLIGATION TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR
        MODIFICATIONS.
------------------------------------------------------------------------------*/

#include <stdlib.h>
#include <assert.h>
#include <string.h>
#include <math.h>
#include "fortify.h"
#include "sics.h"
#include "drive.h"
#include "nserver.h"            /* for SicsWait */
#include "drive.h"
#include "splitter.h"
#include "status.h"
#include "devexec.h"
#include "motor.h"

/*---------------------------------------------------------------------------*/
int Drive(SConnection * pCon, SicsInterp * pInter, char *name, float fNew)
{
  CommandList *pObject = NULL;
  pObjectDescriptor pDes = NULL;
  pIDrivable pInt = NULL;
  Dummy *pDum;
  char pBueffel[512];
  int iRet;
  float fDelta, fPos;
  long lTime;

  assert(pCon);
  assert(pInter);

  /* check if user is allowed to drive */
  if (!SCMatchRights(pCon, usUser)) {
    snprintf(pBueffel, 511, "Insufficient Privilege to drive %s", name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* first try to find the thing to drive */
  pObject = FindCommand(pInter, name);
  if (!pObject) {
    snprintf(pBueffel,511, "Cannot find %s to drive ", name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* get the Descriptor, this RELIES on the descriptor being the first
     thing in the struct
   */
  pDum = (Dummy *) pObject->pData;
  pDes = pDum->pDescriptor;
  if (!pDes) {
    snprintf(pBueffel,511, "%s is NOT drivable!", pDes->name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* both scanable and drivable objects can be driven, check this 
     and act accordingly
   */

  pInt = pDes->GetInterface(pDum, DRIVEID);
  if (!pInt) {
    snprintf(pBueffel,511, "%s is NOT drivable!", pDes->name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
  if (pInt) {
    iRet = pInt->CheckLimits(pDum, fNew, pBueffel, 511);
    if (!iRet) {
      SCWrite(pCon, pBueffel, eLogError);
      SCSetInterrupt(pCon, eAbortOperation);
      return 0;
    }
    iRet = StartDevice(GetExecutor(), name, pDes, pDum, pCon, RUNDRIVE, fNew);
    if (!iRet) {
      snprintf(pBueffel,511, "ERROR: cannot start device %s", name);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }

    /* wait for finish */
    iRet = Wait4Success(GetExecutor());
    fPos = pInt->GetValue(pDum, pCon);
    if (iRet == DEVINT) {
      if (SCGetInterrupt(pCon) == eAbortOperation) {
        SCSetInterrupt(pCon, eContinue);
        snprintf(pBueffel, 511, "Driving %s aborted at %.6g", name, fPos);
        SCWrite(pCon, pBueffel, eError);
      }
      return 0;
    } else if (iRet == DEVDONE) {
      snprintf(pBueffel, 511, "Driving %s to %.6g done", name, fPos);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    } else {
      snprintf(pBueffel, 511,
              "Driving %s finished with problems, position: %.6g", name,
              fPos);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    }
  }
  snprintf(pBueffel, 511, "%s is NOT drivable", pDes->name);
  SCWrite(pCon, pBueffel, eError);
  return 0;
}

/*---------------------------------------------------------------------------*/
int Start2Run(SConnection * pCon, SicsInterp * pInter, char *name,
              int level, float fNew)
{
  CommandList *pObject = NULL;
  pObjectDescriptor pDes = NULL;
  pIDrivable pInt = NULL;
  Dummy *pDum;
  char pBueffel[512];
  int iRet;
  float fDelta;
  long lTime;
  float target;

  assert(pCon);
  assert(pInter);

  /* check if user is allowed to drive */
  if (!SCMatchRights(pCon, usUser)) {
    snprintf(pBueffel,511, "ERROR: Insuficient Privilege to drive %s", name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* first try to find the thing to drive */
  pObject = FindCommand(pInter, name);
  if (!pObject) {
    snprintf(pBueffel,511, "ERROR: Cannot find %s to drive ", name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* get the Descriptor, this RELIES on the descriptor being the first
     thing in the struct
   */
  pDum = (Dummy *) pObject->pData;
  pDes = pDum->pDescriptor;
  if (!pDes) {
    snprintf(pBueffel,511, "ERROR: %s is NOT drivable!", pDes->name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* Only drivable objects can be driven, check this 
     and act accordingly
   */
  pInt = pDes->GetInterface(pDum, DRIVEID);
  if (!pInt) {
    snprintf(pBueffel,511, "ERROR: %s is NOT drivable!", pDes->name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
  if (pInt) {
    iRet = pInt->CheckLimits(pDum, fNew, pBueffel, 511);
    if (!iRet) {
      SCPrintf(pCon, eError, "ERROR: %s", pBueffel);
      SCSetInterrupt(pCon, eAbortOperation);
      return 0;
    }
    iRet = StartDevice(GetExecutor(), name, pDes, pDum, pCon,level, fNew);
    if (!iRet) {
      return 0;
    } else {
      return 1;
    }
  } else {
    snprintf(pBueffel,511, "ERROR: %s is NOT drivable", pDes->name);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
}

/*-------------------------------------------------------------------------
  This is meant to be called specially from DriveWrapper at a stage when
  we already know, that name is a drivable motor. Thus no error checking
  is performed. Do not use this in any other context!!!!
*/
static float findPosition(SicsInterp * pSics, SConnection * pCon,
                          char *name)
{
  CommandList *pObject = NULL;
  pObjectDescriptor pDes = NULL;
  pIDrivable pInt = NULL;
  Dummy *pDum = NULL;
  pMotor pMot = NULL;
  float fPos;
  int iRet;
  char pBueffel[132];

  /* 
     treat motors separately in order to correct for zero points
     Sighh.........
   */
  pMot = FindMotor(pSics, name);
  if (pMot != NULL) {
    iRet = MotorGetSoftPosition(pMot, pCon, &fPos);
    if (iRet) {
      return fPos;
    } else {
      return -999.;
    }
  }

  pObject = FindCommand(pSics, name);
  if (pObject) {
    pDum = (Dummy *) pObject->pData;
    if (pDum) {
      pDes = pDum->pDescriptor;
      if (pDes) {
        pInt = pDes->GetInterface(pDum, DRIVEID);
        if (!pInt) {
          snprintf(pBueffel,131,
                  "ERROR: internal error in findPosition for %s", name);
          SCWrite(pCon, pBueffel, eError);
          return -999.99;
        } else {
          return pInt->GetValue(pDum, pCon);
        }
      }
    }
  }
  return -999.99;
}

/*--------------------------------------------------------------------------*/
int DriveWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
                 int argc, char *argv[])
{
  Tcl_Interp *tcl_interp;
  int iRet, i;
  double dTarget;
  char pBueffel[512];
  Status eOld;
  char *error = NULL;

  assert(pCon);
  assert(pSics);
  tcl_interp = InterpGetTcl(pSics);

  /* check Status */
  eOld = GetStatus();


  argtolower(argc, argv);
  /* check no of args */
  if (argc < 3) {
    snprintf(pBueffel, 511,"Insufficient number of args. Usage %s name val",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* check authorisation */
  if (!SCMatchRights(pCon, usUser)) {
    SCWrite(pCon,
            "ERROR: You are not authorized to use the drive command",
            eError);
    return 0;
  }

  /* interpret arguments as pairs (name, value) and try to start */
  for (i = 1; i < argc; i += 2) {
    if (argv[i + 1] == NULL) {
      snprintf(pBueffel, 511, "ERROR: no value found for driving %s", argv[i]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    iRet = Tcl_GetDouble(tcl_interp, argv[i + 1], &dTarget);
    if (iRet == TCL_ERROR) {
      SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
      return 0;
    } else if (!isfinite(dTarget)) {
      snprintf(pBueffel, 511,
              "ERROR: target %s value for %s is not a finite number",
              argv[i + 1], argv[i]);
      SCWrite(pCon, pBueffel, eError);
      StopExe(GetExecutor(), "ALL");
      return 0;
    }
    iRet = Start2Run(pCon, pSics, argv[i], RUNDRIVE, dTarget);
    if (!iRet) {
      error = Tcl_GetStringResult(tcl_interp);
      if(error != NULL) {
	snprintf(pBueffel, 511,"%s", error);
      }  else {
	snprintf(pBueffel, 511,"ERROR: cannot run %s to %s", argv[i],
              argv[i + 1]);
      }
      SCWrite(pCon, pBueffel, eError);
      StopExe(GetExecutor(), "ALL");
      return 0;
    }
  }

  /* wait for completion */
  iRet = Wait4Success(GetExecutor());

  /* print positions */
  for (i = 1; i < argc; i += 2) {
    snprintf(pBueffel,511, "New %s position: %.6g", argv[i],
            findPosition(pSics, pCon, argv[i]));
    SCWrite(pCon, pBueffel, eValue);
  }

  /* check the completion status */
  if (!(eOld == eScanning || eOld == eBatch)) {
    eOld = eEager;
  }
  if (iRet == DEVERROR) {
    sprintf(pBueffel, "Driving finished with problem");
    SCWrite(pCon, pBueffel, eError);
    ClearExecutor(GetExecutor());
    return 0;
  } else if (iRet == DEVINT) {
    sprintf(pBueffel, "ERROR: Driving Interrupted!");
    SCWrite(pCon, pBueffel, eError);
    ClearExecutor(GetExecutor());
    return 0;
  }
  SCWrite(pCon, "Driving finished successfully", eValue);
  return 1;
}

/*---------------------------------------------------------------------------*/
int RunWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
               int argc, char *argv[])
{
  Tcl_Interp *tcl_interp;
  int iRet, i;
  double dTarget;
  char pBueffel[512];
  Status eOld;

  assert(pCon);
  assert(pSics);
  tcl_interp = InterpGetTcl(pSics);
  /* check Status */
  eOld = GetStatus();


  /* check no of args */
  if (argc < 3) {
    snprintf(pBueffel,511, "Insufficient number of args. Usage %s name val",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* check authorisation */
  if (!SCMatchRights(pCon, usUser)) {
    SCWrite(pCon,
            "ERROR: You are not authorized to use the drive command",
            eError);
    return 0;
  }

  /* interpret arguments as pairs name value and try to start */
  for (i = 1; i < argc; i += 2) {
    if (argv[i + 1] == NULL) {
      snprintf(pBueffel,511, "ERROR: no value found for driving %s", argv[i]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    iRet = Tcl_GetDouble(tcl_interp, argv[i + 1], &dTarget);
    if (iRet == TCL_ERROR) {
      SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
      StopExe(GetExecutor(), "ALL");
      return 0;
    }
    iRet = Start2Run(pCon, pSics, argv[i], RUNRUN, dTarget);
    if (!iRet) {
      snprintf(pBueffel, 511, "ERROR: cannot run %s to %s", argv[i],
              argv[i + 1]);
      SCWrite(pCon, pBueffel, eError);
      StopExe(GetExecutor(), "ALL");
      return 0;
    }
  }
  return 1;
}
/*---------------------------------------------------------------------------*/
int MoveWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
               int argc, char *argv[])
{
  Tcl_Interp *tcl_interp;
  int iRet, i;
  double dTarget;
  float curPos;
  char pBueffel[512];
  Status eOld;
  long groupID, taskID;
  void *obj;

  assert(pCon);
  assert(pSics);
  tcl_interp = InterpGetTcl(pSics);
  /* check Status */
  eOld = GetStatus();


  /* check no of args */
  if (argc < 3) {
    snprintf(pBueffel,511, "Insufficient number of args. Usage %s name val",
            argv[0]);
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  /* check authorisation */
  if (!SCMatchRights(pCon, usUser)) {
    SCWrite(pCon,
            "ERROR: You are not authorized to use the mv command",
            eError);
    return 0;
  }
  groupID = GetTaskGroupID(pServ->pTasker);
  for (i = 1; i < argc; i += 2) {
    if (argv[i + 1] == NULL) {
      snprintf(pBueffel,511, "ERROR: no value found for driving %s", argv[i]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
    iRet = Tcl_GetDouble(tcl_interp, argv[i + 1], &dTarget);
    if (iRet == TCL_ERROR) {
      SCWrite(pCon, Tcl_GetStringResult(tcl_interp), eError);
      StopExe(GetExecutor(), "ALL");
      return 0;
    }
    obj = FindCommandData(pSics,argv[i],NULL);
    if(obj == NULL || GetDrivableInterface(obj) == NULL){
      SCPrintf(pCon,eError, "ERROR: %s not found, not started",argv[i]);
      break;
    }
    GetDrivablePosition(obj,pCon,&curPos);
    dTarget = curPos + dTarget;
    taskID = StartDriveTask(obj, pCon, argv[i], (float)dTarget);
    AddTaskToGroup(pServ->pTasker,taskID,groupID);
  }
  while(isTaskGroupRunning(pServ->pTasker,groupID)){
    TaskYield(pServ->pTasker);
  }
  SCSendOK(pCon);
  return 0;
}
/*---------------------------------------------------------------------------*/
int MakeDrive(SConnection * pCon, SicsInterp * pSics, void *pData,
              int argc, char *argv[])
{
  int iRet;
  char pBueffel[512];

  assert(pCon);
  assert(pSics);

  if (argc > 1) {
    iRet = AddCommand(pSics, argv[1], DriveWrapper, NULL, NULL);
  } else {
    iRet = AddCommand(pSics, "drive", DriveWrapper, NULL, NULL);
  }
  if (!iRet) {
    sprintf(pBueffel, "ERROR: duplicate command drive not created");
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }
  if (argc > 2) {
    iRet = AddCommand(pSics, argv[2], RunWrapper, NULL, NULL);
  } else {
    iRet = AddCommand(pSics, "run", RunWrapper, NULL, NULL);
  }
  if (!iRet) {
    sprintf(pBueffel, "ERROR: duplicate command run not created");
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  if (argc > 3) {
    iRet = AddCommand(pSics, argv[3], MoveWrapper, NULL, NULL);
  } else {
    iRet = AddCommand(pSics, "mv", MoveWrapper, NULL, NULL);
  }
  if (!iRet) {
    sprintf(pBueffel, "ERROR: duplicate command mv not created");
    SCWrite(pCon, pBueffel, eError);
    return 0;
  }

  return 1;
}