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

		M U L T I P L E   M O T O R S
		
	Code for maintaining a group of motors. This suuports
	 - aliases for motors
	 - named positions for motors
	 - a position history implemented as the special named position
	   back.


	Mark Koennecke,    December 1996
	
	heavily reworked and simplified, Mark Koennecke, June 1997

	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 <ctype.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "conman.h"
#include "devexec.h"
#include "motor.h"
#include "obdes.h"
#include "splitter.h"
#include "nserver.h"
#include "stringdict.h"
#include "mumo.h"
#include "mumo.i"
/*--------------------------------------------------------------------------*/
  pMulMot MakeMultiMotor(void)
  {
     pMulMot pNew = NULL;
     
     pNew = (pMulMot)malloc(sizeof(MulMot));
     if(!pNew)
     {
       return NULL;
     }
     memset(pNew,0,sizeof(MulMot));

     /* the string Dictionaries */
     pNew->pAlias = CreateStringDict();
     pNew->pNamPos = CreateStringDict(); 
     if( (!pNew->pAlias) || (!pNew->pNamPos))
     {
       free(pNew);
       return NULL;
     }
     
     pNew->pDes = CreateDescriptor("MulMot");
     if(!pNew->pDes)
     {
       DeleteStringDict(pNew->pAlias);
       DeleteStringDict(pNew->pNamPos);
       free(pNew);
       return NULL;
     }
     
     
     /* the parameter array */
     pNew->pParam = ObParCreate(1);
     ObParInit(pNew->pParam,ACCESS,"accesscode",usUser,usMugger);

     pNew->name = NULL;     
     return pNew;
  }
/*-------------------------------------------------------------------------*/
  void KillMultiMotor(void *pData)
  {
    pMulMot self;
    int i;
    
    assert(pData);
    
    self = (pMulMot)pData;
    assert(strcmp(self->pDes->name,"MulMot") == NULL);

    /* remove string dictionaries */
    if(self->pAlias)
    {
      DeleteStringDict(self->pAlias);
    }
    if(self->pNamPos)
    {
      DeleteStringDict(self->pNamPos);
    }
    
    /* free descriptor */
    if(self->pDes)
    {
      DeleteDescriptor(self->pDes);
    }
    
  /* kill parameter array */
     ObParDelete(self->pParam);  

  /* free name and myself */
    if(self->name)
    {
      free(self->name);
    }
    free(self);
  }
/*---------------------------------------------------------------------------
  Multiple Motors have a funny and flexible syntax. Analysing the argc, 
  argv[] set will not do. Therefore this is done by some recursive descent 
  parsing.
*/
  
  typedef struct __MYTOKEN {
                             char *pCommand;
                             char *pPtr;
                             int  iCurrentToken;
                             char Token[80];
                           } sParser, *psParser;
/* define some TokenTypes */
#define END 0
#define INCREMENT 1
#define DECREMENT 2
#define NAMPOS    3
#define ALIAS     4
#define SYMBOL    5
#define NUMBER    6
#define EQUALITY  7  
#define PLUS      8
#define MINUS     9
#define UNKNOWN   10
#define POSFIND   11
#define NAMALL    12
#define LIST      13
/*-------------------------------------------------------------------------*/  
   static int GetNextToken(psParser self, pMulMot pDings)
   {
     char *pPtr;
     int i;
     
     pPtr = self->pPtr;
     
     /* skip whitespace */
     while( (*pPtr == ' ') || (*pPtr == '\t') )
     {
       pPtr++;
     }

     /* check for end */ 
     if( (*pPtr == '\n') || (*pPtr == '\0') || (*pPtr == '\r') )
     {
         self->pPtr = pPtr;
         self->Token[0] = *pPtr;
         self->Token[1] = '\0';
         self->iCurrentToken = END;
         return END;
     }
     
     /* check equaL SIGN */
     if(*pPtr == '=')
     {
       strcpy(self->Token,"=");
       self->iCurrentToken = EQUALITY;
       self->pPtr = pPtr +1;
       return EQUALITY;
     }
     
     /* check + */
     if(*pPtr == '+')
     {
        if(*(pPtr+1) == '+') /* increment */
        {
          strcpy(self->Token,"++");
          self->iCurrentToken = INCREMENT;
          self->pPtr = pPtr+2;
          return INCREMENT;
        }
        else  /* silly plus */
        {
          strcpy(self->Token,"+");
          self->iCurrentToken = PLUS;
          self->pPtr = pPtr + 1;
          return PLUS;
        }
     }

     /* check - */
     if(*pPtr == '-')
     {
        if(*(pPtr+1) == '-') /* Decrement */
        {
          strcpy(self->Token,"--");
          self->iCurrentToken = DECREMENT;
          self->pPtr = pPtr+2;
          return DECREMENT;
        }
        else  /* silly minus */
        {
          strcpy(self->Token,"-");
          self->iCurrentToken = MINUS;
          self->pPtr = pPtr + 1;
          return MINUS;
        }
     }
     
     /* number */
     if( (isdigit((int)*pPtr)) || (*pPtr == '.') )
     {
       i = 0;
       while (isdigit((int)*pPtr) || (*pPtr == '.') )
       {
         self->Token[i] = *pPtr;
         i++;
         pPtr++;
       }
       self->Token[i] = '\0';
       self->iCurrentToken = NUMBER;
       self->pPtr = pPtr;
       return NUMBER;
     }
     
     /* a Symbol  ? */
     if(isalnum((int)*pPtr))
     {
       i = 0;
       while( (!isspace((int)*pPtr)) && (*pPtr != '=') 
             && (*pPtr != '+') && (*pPtr != '-') &&(*pPtr != '\0') 
             && (*pPtr != '\n') )
       {
         self->Token[i] = *pPtr;
         i++;
         pPtr++;
       }
       self->Token[i] = '\0';
       self->pPtr = pPtr;
       /* now it can be a named position, a motor alias or a keyword */
       if(StringDictExists(pDings->pNamPos,self->Token))
       {
         self->iCurrentToken = NAMPOS;
         return NAMPOS;
       }
       else if(StringDictExists(pDings->pAlias,self->Token))
       {
         self->iCurrentToken = ALIAS;
         return ALIAS;
       }
       else if(strcmp(self->Token,"all") == 0)
       {
         self->iCurrentToken = NAMALL;
         return NAMALL;
       }
       else if(strcmp(self->Token,"list") == 0)
       {
         self->iCurrentToken = LIST;
         return LIST;
       }
       else
       {
          self->iCurrentToken = SYMBOL;
          return SYMBOL;
       }
     }
     return UNKNOWN;
   } 
/*-------------------------------------------------------------------------*/
  static void ListMulMot(char *name, SConnection *pCon, pMulMot self)
  {
    int i, iRet;
    char pBueffel[512];
    float fVal;
    OutCode eOut = eValue;
    const char *pNam = NULL;
    char pMotName[132];
    pMotor pMot = NULL;
    
    sprintf(pBueffel,"Status listing for %s",name);
    SCWrite(pCon,pBueffel,eValue);

    /* scan through all aliases */
    pNam = StringDictGetNext(self->pAlias,pMotName, 131);
    while(pNam != NULL)
    {
      pMot = FindMotor(GetInterpreter(), pMotName);
      assert(pMot); /* has been tested on definition */
      iRet = MotorGetSoftPosition(pMot,pCon,&fVal);
      if(!iRet)
      {
        sprintf(pBueffel,"ERROR: Cannot read motor %s\n",
                pMotName);
        eOut = eError; 
      }
      else
      {
        eOut = eValue;
        sprintf(pBueffel,"%s.%s = %f\n",name,pNam, fVal);
      }
      SCWrite(pCon,pBueffel,eValue); 
      pNam = StringDictGetNext(self->pAlias, pMotName, 131);
    }
  }
/*--------------------------------------------------------------------------*/
  static int ParseAlias(psParser pParse, SConnection *pCon, 
                        pMulMot self)
  {
     int i, iToken, iRet;
     int iSign = 1;
     int iInc = 0;
     char pBueffel[132];
     char pCommand[1064];
     float fVal = 0, fIn = 0;
     int iComplete;
     pMotor pMot = NULL;
  
    /* Okay first find the alias, the current token must still be 
      its name 
    */
    strcpy(pCommand,"run ");
    iRet = StringDictGet(self->pAlias,pParse->Token,pBueffel,131);
    /* that it is alias has been tested earlier */
    assert(iRet == 1);
    strcat(pCommand, pBueffel);

  /* now find the value to handle */
    iToken = GetNextToken(pParse,self);
    iComplete = 0;
    while(iToken != END)
    {
      switch(iToken)
      {
        case PLUS:
                 iSign = 1;
                 break;
        case MINUS:
                 iSign = -1;
                 break;
        case EQUALITY:
                 break;
        case INCREMENT:
                 iInc = 1;
                 break;
        case DECREMENT:
                 iSign = -1;
                 iInc = 1;
                 break;
        case NUMBER:/* ey! Do some real work */
                 fIn = (float)atof(pParse->Token);
                 iComplete = 0;
                 if(iInc)
                 {
                    pMot = FindMotor(GetInterpreter(), pBueffel);
                    assert(pMot); /* existence shoul have been verified earlier */
                    MotorGetSoftPosition(pMot,pCon,&fVal);
                    fVal += iSign * fIn;
                    sprintf(pBueffel," %f ",fVal);
                    strcat(pCommand, pBueffel);
                    InterpExecute(GetInterpreter(),pCon,pCommand);
                    return 1; 
                 }
                 else
                 {
                    sprintf(pBueffel," %f ",iSign*fIn);
                    strcat(pCommand, pBueffel);
                    InterpExecute(GetInterpreter(),pCon,pCommand);
                    return 1; 
                 }
                 return 1;
                 break;
        default:        
               sprintf(pBueffel,"ERROR: Unexpected symbol %s",
               pParse->Token);
               SCWrite(pCon,pBueffel,eError);
               return 0;                   
      }
      iToken = GetNextToken(pParse,self);
    }
    if(!iComplete)
    {
      SCWrite(pCon,"ERROR: Incomplete command",eError);
      return 0;
    } 
  }  
    
/*--------------------------------------------------------------------------
*/
  static int MakeCurrentNamPos(char *name, SConnection *pCon, 
                        pMulMot self)
  {
     const char *pAlias = NULL;
     char pMotort[132];
     char pCommand[1064];
     pMotor pMot = NULL;
     float fVal;
     int iRet;

     assert(self);
     assert(pCon);
     
     pCommand[0] = '\0';
     pAlias = StringDictGetNext(self->pAlias, pMotort, 131);
     while(pAlias != NULL)
     {
       strcat(pCommand, pMotort);
       pMot = FindMotor(GetInterpreter(),pMotort);
       assert(pMot); /* validity of alias has already been checked */
       iRet = MotorGetSoftPosition(pMot,pCon,&fVal);
       if(!iRet)
       {
         sprintf(pCommand,"ERROR: failure to read motor %s, %s",
                 pMotort," named position not created");
         SCWrite(pCon,pCommand,eError);
         return 0;        
       }
       sprintf(pMotort,"  %f ", fVal);
       strcat(pCommand,pMotort);
       pAlias = StringDictGetNext(self->pAlias, pMotort, 131);
     }
     if(StringDictExists(self->pNamPos,name))
     {
       StringDictUpdate(self->pNamPos,name,pCommand);
     }
     else
     {
       StringDictAddPair(self->pNamPos,name,pCommand);
     }
     return 1;
  }  
/*--------------------------------------------------------------------------
  FindNamPos: checks if the current position is identical to a named
                position. The comparison is done by getting the motors
                current value and compare it with the stored ones for
                all currently stored named positions.

*/
 const char *FindNamPos(pMulMot self, SConnection *pCon)
 {
     int iRet, iTest;
     char pCurrCommand[1064], pTestCommand[1064];
     const char *pAlias;
     char *pName, *pVal;
     float f1, f2;
     pMotor pMot = NULL;
     
     /* scan named position list */
     StringDictKillScan(self->pNamPos);
     pAlias = StringDictGetNext(self->pNamPos,pTestCommand,1063);
     while(pAlias != NULL)
     {
        strcpy(pCurrCommand,pTestCommand);
        pName = strtok(pCurrCommand," ");
        iTest = 1;
        while(pName != NULL)
	{
          pVal = strtok(NULL," ");
          pMot = FindMotor(pServ->pSics,pName);
          if(pMot)
	  {
             iRet = MotorGetSoftPosition(pMot,pCon,&f1);
             if(!iRet)
	     {
               sprintf(pTestCommand,
                "ERROR: failed to get motor position for %s", pName);
               SCWrite(pCon,pTestCommand,eError);
               return NULL;
             }
             sscanf(pVal,"%f",&f2);
             f1 = f1 - f2;
             if(f1 < 0.)
                f1 = - f1;
             if(f1 > 0.03)
	     {
                iTest--;
                break;
             }
	  }
          else
	  {
             SCWrite(pCon,"ERROR: internal error in FindNamPos, no motor",
             eError);
             return NULL;
          }
          pName = strtok(NULL," ");
        }
        if(iTest == 1)
           return pAlias;

        pAlias = StringDictGetNext(self->pNamPos,pTestCommand,1063);
     }
     
     /* not found */
     return NULL;  
 }                
/*--------------------------------------------------------------------------*/
  static int ParseDropPos(psParser pParse, SConnection *pCon, 
                        pMulMot self)
  {
     int i, iToken, iRet;
     char pBueffel[256];
     float fVal = 0;
     char pName[80];
     char *pPtr = NULL;
  
    iToken = GetNextToken(pParse,self);
    while(iToken != END)
    {
      switch(iToken)
      {
        case EQUALITY:
                 break;
        case NAMPOS:
                 /* we never want back to be killed ..... */
                 if(strcmp(pParse->Token,"back") == 0)
                 {
                    SCWrite(pCon, 
                    "ERROR: You are NOT allowed to deleted system position BACK",
                    eError);
                    return 0; 
                 }
                 /* find NamPos and kill it */
                 iRet = StringDictDelete(self->pNamPos,pParse->Token);
                 return 1;
                 break;
        case NAMALL:
	         /* kill all named positions except back */
                 iRet = StringDictGet(self->pNamPos,"back",pBueffel,256);
                 DeleteStringDict(self->pNamPos);
                 self->pNamPos = CreateStringDict();
                 if(!self->pNamPos)
		 {
                     SCWrite(pCon,
                       "ERROR: severe! out of memory in mumo drop",
                     eError);
                     return 0;
                 }
                 if(iRet)
		 {
                     StringDictAddPair(self->pNamPos,"back",pBueffel);
                 }
                 return 1;
                 break;
        default:        
               sprintf(pBueffel,"ERROR: Unexpected symbol %s",
               pParse->Token);
               SCWrite(pCon,pBueffel,eError);
               return 0;                   
      }
    }
  }
/*-------------------------------------------------------------------------*/
  static int CheckPermission(SConnection *pCon, pMulMot self)
  {
     int iAccess;
     char pBueffel[132];
     
     iAccess = (int)ObVal(self->pParam,ACCESS);
     if(SCMatchRights(pCon, iAccess))
     {
       return 1;
     }
     else
     {
       sprintf(pBueffel,"ERROR: NO permission to drive %s",
               self->name);
       SCWrite(pCon,pBueffel,eError);
       return 0;
     }
  }  
/* ------------------------------------------------------------------------*/
  static int ParseNamPos(psParser pParse, SConnection *pCon, 
                         SicsInterp *pSics, pMulMot self)
  {
      int iToken, iRet;
      char pBueffel[256];
      char pCommand[1064];
      char *pPtr;

      /* is the user allowe dto do ANYTHING ? */
      iRet = CheckPermission(pCon,self);
      if(!iRet)
      {
         return 0;
      }
    
      strcpy(pCommand,"run    ");
      pPtr = pCommand + strlen("run ");
      iRet = StringDictGet(self->pNamPos,pParse->Token,pPtr, 1050);
      if(iRet)
      {
        InterpExecute(GetInterpreter(),pCon,pCommand);
        return 1;
      }
      else
      {
        sprintf(pCommand,"ERROR: named position %s NOT found",pParse->Token);
        SCWrite(pCon,pCommand,eError);
        return 0;
      }
      return 0; /* not reached */
  }

    
/*---------------------------------------------------------------------------
  MultiWrapper is the user interface to a multi motor unit. It supports the 
  following syntax, where DingsBums is the name of the unit:
  
  	DingsBums                     - lists position of all registered motors.
  	DingsBums  NamPos             - drives of to named position
  	DingsBums  [alias = num]      - drives alias to num. num supports 
  	                                decrement, increment and simple values.
        DingsBums  pos name           - makes the current position a named
                                        position with name name.
        DingsBums  drop name          - deletes the current named position
                                        name.            
        DingsBums  drop all           - drops all named positions (except back)
        DingsBums  list               - lists all named positions.                                                     
        
*/  	                                
     
  
  int MultiWrapper(SConnection *pCon, SicsInterp *pSics, void *pData,
                   int argc, char *argv[])
  {
    char pBueffel[512];
    char pError[132];
    sParser MyParser;
    int iToken;
    int iRet;
    pMulMot self;
    int iAlias = 0;
    const char *pPtr;
    Tcl_DString tString;
    
    assert(pCon);
    assert(pSics);
    assert(pData);
    
    self = (pMulMot)pData;
    assert(strcmp(self->pDes->name,"MulMot")== 0);
    
    /* setup */
    argtolower(argc,argv);
    iRet = Arg2Text(argc,argv,pBueffel,511);
    if(!iRet)
    {
      SCWrite(pCon,"Command line to long",eError);
      return 0;
    }
    MyParser.pPtr = &pBueffel[0];
    MyParser.pCommand = pBueffel;
  
    /* ignore first Token: that is my name ! */
    iToken = GetNextToken(&MyParser,self);
    
    /* now loop through my Tokens */
    iToken = GetNextToken(&MyParser,self);
    if(iToken == END)
    {
      ListMulMot(argv[0],pCon,self);
      return 1;
    }
    while( iToken != END)
    {
       switch(iToken)
       {
         case END: 
                 SCSendOK(pCon);
                 return 1;
         case NAMPOS:
                 /* do a named position, current token is it */
                 if(strcmp(MyParser.Token,"back")!= 0)
                 {
                    MakeCurrentNamPos("back",pCon,self);
                 }
                 iRet = ParseNamPos(&MyParser,pCon,pSics,self);
                 if(iRet)
                 {
                      SCSendOK(pCon);
                      return 1;
                 }
                 else
                 {
                   return 0;
                 }
                 break;
         case ALIAS:
                 iRet = CheckPermission(pCon, self);
                 if(iAlias == 0)
                 {
                    MakeCurrentNamPos("back",pCon,self);
                    iAlias = 1;
                 }
                 if(!iRet)
                 {
                   return 0;
                 }
                 iRet = ParseAlias(&MyParser,pCon,self);
                 if(!iRet)
                 {
                   return 0;
                 }
                 break;
         case LIST:
                   pPtr = NULL;
                   Tcl_DStringInit(&tString);
                   sprintf(pBueffel,"%s list of known named positions \n",
                           argv[0]);
                   Tcl_DStringAppend(&tString,pBueffel,strlen(pBueffel));
                   pPtr = StringDictGetNext(self->pNamPos,pError,131);
                   while(pPtr != NULL)
                   {
                      Tcl_DStringAppend(&tString,(char *)pPtr,-1);
                      Tcl_DStringAppend (&tString,"  \n",-1);
                      pPtr = StringDictGetNext(self->pNamPos,
                                                 pError,131);
                   }
                   SCWrite(pCon,Tcl_DStringValue(&tString),eValue);
                   Tcl_DStringFree(&tString);
                   return 1;
                   break;
         case SYMBOL:
                 if(strcmp(MyParser.Token,"pos") == 0)
                 {
                   iRet = CheckPermission(pCon,self);
                   if(!iRet)
                   {
                     return 0;
                   }
                   else
                   {
                     iToken = GetNextToken(&MyParser, self);
                     if((iToken == SYMBOL) || (iToken == NAMPOS))
                     {
                       MakeCurrentNamPos(MyParser.Token,pCon,self);
                       return 1;
                     }
                     else
                     {
                       sprintf(pError,"ERROR: %s no valid named position name",
                               MyParser.Token);
                       SCWrite(pCon,pError,eError);
                       return 0;        
                     }
                   } 
                 }        
                 if(strcmp(MyParser.Token,"drop") == 0)
                 {
                   iRet = CheckPermission(pCon,self);
                   if(!iRet)
                   {
                     return 0;
                   }
                   else
                   {
                       return ParseDropPos(&MyParser, pCon, self);
                   }
                 }
                 if(strcmp(MyParser.Token,"find") == 0)
                 {
                   pPtr = NULL;
                   pPtr = FindNamPos(self,pCon);
                   if(pPtr)
                   {
                      sprintf(pBueffel,"%s.namedposition = %s",argv[0],pPtr);
                   }  
                   else
                   {
                      sprintf(pBueffel,"%s.namedposition = UNKNOWN",argv[0]);
                   }
                   SCWrite(pCon,pBueffel,eValue);
                   return 1;
                 }        
                 sprintf(pError,"ERROR: Unknown Token %s",MyParser.Token);
                 SCWrite(pCon,pError,eError);
                 return 0;
         default:
                 SCWrite(pCon,"ERROR: Parse Error",eError);
                 return 0;
       }
       iToken = GetNextToken(&MyParser,self);  
    }
    return 1;
  }                            		   

