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

	Implementation file for the drive command
	
        This version as Test for Device Executor.

	Mark Koennecke,    December 1996

	Copyright:

	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 "fortify.h"
#include "drive.h"
#include "obdes.h"
#include "interface.h"
#include "SCinter.h"
#include "conman.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))
    {
       sprintf(pBueffel,"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)
    {
        sprintf(pBueffel,"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)
    {
       sprintf(pBueffel,"%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)
    {
       sprintf(pBueffel,"%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,eError);
        SCSetInterrupt(pCon,eAbortOperation);
        return 0;
      }
      iRet = StartDevice(GetExecutor(),name,pDes,pDum,pCon,fNew);
      if(!iRet)
      {
        sprintf(pBueffel,"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);
          SCSetError(pCon,OKOK);
          sprintf(pBueffel,"Driving %s aborted at %9.3f",name, fPos);
          SCWrite(pCon,pBueffel,eStatus);
        }
        return 0;
      }
      else if(iRet == DEVDONE)
      {
           sprintf(pBueffel,"Driving %s to %9.3f done", name, fPos);
           SCWrite(pCon,pBueffel,eStatus);
           return 1;
      }
      else
      {
          sprintf(pBueffel,
           "Driving %s finished with problems, position: %9.3f",name,fPos);
          SCWrite(pCon,pBueffel,eStatus);
          return 1;
      }            
    }
    sprintf(pBueffel,"%s is NOT drivable",pDes->name);
    SCWrite(pCon,pBueffel,eError);
    return 0;
  }	
/*---------------------------------------------------------------------------*/
  int Start2Run(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;
    long lTime;
    
    assert(pCon);
    assert(pInter);
    
    /* check if user is allowed to drive */
    if(!SCMatchRights(pCon,usUser))
    {
       sprintf(pBueffel,"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)
    {
        sprintf(pBueffel,"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)
    {
       sprintf(pBueffel,"%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)
    {
       sprintf(pBueffel,"%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,eError);
        SCSetInterrupt(pCon,eAbortOperation);
        return 0;
      }
      iRet = StartDevice(GetExecutor(),name,pDes,pDum,pCon,fNew);
      if(!iRet)
      {
        return 0;
      }
      else
      {
        return 1;
      }
    }
    else
    {
      sprintf(pBueffel,"%s is NOT drivable",pDes->name);
      SCWrite(pCon,pBueffel,eError);
      return 0;
    }  
  }	
/*-------------------------------------------------------------------------
  This is meant to be called specially from DriveWrapper at at 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 separatetly 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)
            {
               sprintf(pBueffel,
                      "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[])
   {
     int iRet,i;
     char pBueffel[512];
     Status eOld;
     
     assert(pCon);
     assert(pSics);

     /* check Status */
     eOld = GetStatus();
     
     
     argtolower(argc,argv);
     /* check no of args */
     if(argc < 3)
     {
       sprintf(pBueffel,"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;
     }
     
     /* interprete arguments as pairs name value and try to start */
     SetStatus(eDriving);
     for(i = 1; i < argc; i+=2)
     {
        if(argv[i+1] == NULL)
        {
           sprintf(pBueffel,"ERROR: no value found for driving %s",
                   argv[i]);
           SCWrite(pCon,pBueffel,eError);
           SetStatus(eOld); 
           return 0;        
        }
        iRet = Start2Run(pCon,pSics,argv[i],atof(argv[i+1]));
        if(!iRet)
        {
          sprintf(pBueffel,"ERROR: cannot run %s to %s",argv[i],argv[i+1]);
          SCWrite(pCon,pBueffel,eError);
          StopExe(GetExecutor(),"ALL");
          SetStatus(eOld);
          return 0;
        }
     }     

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

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

      /* check the completion status */
      if(iRet == DEVERROR)
      {
        sprintf(pBueffel,"Driving finished with problem");
        SCWrite(pCon,pBueffel,eError);
        ClearExecutor(GetExecutor());
        SetStatus(eOld);
        return 0;
      }
      else if(iRet == DEVINT)
      {      
        sprintf(pBueffel,"Driving Interrupted!",argv[0]);
        SCWrite(pCon,pBueffel,eError);
        ClearExecutor(GetExecutor());
        SetStatus(eOld);
        return 0;
      }
      SCWrite(pCon,"Driving finished sucessfully",eStatus);
      SetStatus(eOld);
      return 1;          
   }
/*---------------------------------------------------------------------------*/   
   int RunWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
                                int argc, char *argv[])
   {
     int iRet, i;
     char pBueffel[512];
     Status eOld;
     
     assert(pCon);
     assert(pSics);

     /* check Status */
     eOld = GetStatus();
     
     
     /* check no of args */
     if(argc < 3)
     {
       sprintf(pBueffel,"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;
     }
     
     /* interprete arguments as pairs name value and try to start */
     SetStatus(eDriving);
     for(i = 1; i < argc; i+=2)
     {
        if(argv[i+1] == NULL)
        {
           sprintf(pBueffel,"ERROR: no value found for driving %s",
                   argv[i]);
           SCWrite(pCon,pBueffel,eError);
           SetStatus(eOld); 
           return 0;        
        }
        iRet = Start2Run(pCon,pSics,argv[i],atof(argv[i+1]));
        if(!iRet)
        {
          sprintf(pBueffel,"ERROR: cannot run %s to %s",argv[i],argv[i+1]);
          SCWrite(pCon,pBueffel,eError);
          StopExe(GetExecutor(),"ALL");
          SetStatus(eOld);
          return 0;
        }
     } 
     return 1;    
}
/*---------------------------------------------------------------------------*/
   int MakeDrive(SConnection *pCon, SicsInterp *pSics, void *pData,
                                int argc, char *argv[])
   {
       int iRet;
       char pBueffel[512];
       
       assert(pCon);
       assert(pSics);
       
       iRet = AddCommand(pSics,"drive", DriveWrapper,NULL,NULL);
       if(!iRet)
       {
         sprintf(pBueffel,"ERROR: duplicate command drive not created",argv[2]);
         SCWrite(pCon,pBueffel,eError);
         return 0;
       }
       iRet = AddCommand(pSics,"run", RunWrapper,NULL,NULL);
       if(!iRet)
       {
         sprintf(pBueffel,"ERROR: duplicate command run not created",argv[2]);
         SCWrite(pCon,pBueffel,eError);
         return 0;
       }
       return 1;
    }

