/*--------------------------------------------------------------------------
              		       V E L O D O R N I E R
              		       
     A driver for a Dornier velocity selector, connected to our world
     via a terminal server and TCP/IP. Please note, that the protocoll
     implemented by the velocity selector PC has been changed in the following 
     ways from the standard as supplied by Dornier:
     - no messages we have not asked for.
     - The whole response will be concatenated in a string, each item 
       separated by a /. At the end is a single <CR><LF>. This is because
       our terminal server reads only up to the first terminator.      
   
     
     Author: Mark Koennecke, Juli 1997
     Thoroughly revised: October 1997 Mark Koennecke
     
     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 "sics.h"
#include <string.h>
#include <math.h>
#include <tcl.h>
#include <time.h>
#include <fortify.h>

typedef struct __VelSelDriv *pVelSelDriv;

#include <velodriv.h>
#include "velodorn.h"
#include "hardsup/serialsinq.h"
#include "hardsup/el734_def.h"
#include "hardsup/el734fix.h"
#include <serialwait.h>

/* VELO* MUST be the same as in velo.i!*/
#define VELOREDO -1
#define VELOFAIL  0
#define VELOOK    1
#define VSNOCON 0
#define VSOK    1
#define VSACCEL -7
#define VSFAIL -2

typedef enum {eStart, eRegel, eHalted} eVeloMode;
#define RPMALIFE 3090

/*----------------------------- The private data structure ---------------*/
 typedef struct{
                char *pComputer;
                int iPort;
                int iChannel;
                int iTimeOut;
                int iLastError;
                void *pData;
                eVeloMode eVelo;
                time_t  t_End;
                float fTarget;
                int iBusy;
                float fLastRPM;
                } Dornier, *pDornier;
/*----------------------------------------------------------------------------*/

  static int GetDornierPos(pVelSelDriv self, float *fPos)
  {
    pDornier pDorn = NULL;
    DornierStatus DStatus;
    int iRet;
  
    assert(self);
    pDorn = (pDornier)self->pPrivate;
    
    if(pDorn->iBusy)
    {
       *fPos = pDorn->fLastRPM;
       return 1;
    }
    
    pDorn->iBusy = 1;
    iRet = GetDornierStatus(&pDorn->pData, &DStatus);
    pDorn->iBusy = 0;
    if(iRet < 0)
    {
      *fPos = -9999.;
      return 0;
    }
    *fPos = DStatus.cur_rpm;
    pDorn->fLastRPM = DStatus.cur_rpm;
    return 1;
  } 
/*--------------------------------------------------------------------------*/
  static int DornierHalt(pVelSelDriv self)
  {
        pDornier pDorn = NULL;
        int iRet;
        char pCom[] = {"HAL"};
        char pAnswer[80];
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;

        iRet = SerialWriteRead(&pDorn->pData, pCom,pAnswer,79);
/*        iRet = SerialSicsExecute(&pDorn->pData, pCom,pAnswer,79);
*/
        pDorn->eVelo = eHalted;
        if(iRet != 1)
        {
          pDorn->iLastError = iRet;
          return 0;
        }
        return 1;
  }
/*----------------------------------------------------------------------------*/
  static int DornierRun(pVelSelDriv self, float fVal)
  {
      int iRet;
      char pCommand[50], pAnswer[50];
      pDornier pDorn = NULL;
      
      assert(self);
      pDorn = (pDornier)self->pPrivate;    

      /* less then zero, means halt in this case */
      if(fVal < RPMALIFE)
      {
         DornierHalt(self);
         return 1;
      }
      
      /* if busy, wait until free */
      while(pDorn->iBusy)
      {
         SicsWait(1);
      }
      
      switch(pDorn->eVelo)
      {
        case eHalted:
             strcpy(pCommand,"SST");
             pDorn->fTarget = fVal;
             pDorn->eVelo = eStart;
             pDorn->t_End = time(NULL) + 1800; /* start time + 30 min */
             break;
        case eRegel:
              sprintf(pCommand,"SDR %5u",(int)fVal);
              pDorn->fTarget = fVal;
              break;
      }

      pDorn->iBusy = 1;
      iRet = SerialWriteRead(&pDorn->pData,pCommand,pAnswer,49);

/*      iRet = SerialSicsExecute(&pDorn->pData,pCommand,pAnswer,49);
*/
      pDorn->iBusy = 0;
      
      if(iRet != 1)
      {
         pDorn->iLastError = iRet;
         return 0;
      }
      return 1;
  }
/*--------------------------------------------------------------------------*/
  static int DornierError(pVelSelDriv self, int *iCode, char *error, int iErrLen)
  {
      pDornier pDorn = NULL;
      
      assert(self);
      pDorn = (pDornier)self->pPrivate;    

      *iCode = pDorn->iLastError;
      
      switch(pDorn->iLastError)
      {
        case NOCOMMAND:
                       strncpy(error,"No command was specified, internal error",iErrLen);
                       break;
        case ECHOMISSING:
                       strncpy(error,"No echo received, may be busy",iErrLen);
                       break;
        case BADANALYSIS:
                       strncpy(error,"Error analysing status messge",iErrLen);
                       break;
        case STARTTIMEOUT:
                       strncpy(error,"Velocity Selector failed to start",iErrLen);
                       break;               
        default:
                       SerialError(pDorn->iLastError,error,iErrLen);
                       break;                                                     
      }
      return 1;
  }
/*---------------------------------------------------------------------------*/
  static int DornierFixIt(pVelSelDriv self, int iError)
  {
        pDornier pDorn = NULL;
        int iRet;
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;
        
        switch(iError)
        {
            /* network errors */
            case NOCONNECTION:
            case EL734__BAD_FLUSH:
            case EL734__BAD_RECV:
            case EL734__BAD_RECV_NET:
            case EL734__BAD_RECV_UNKN:
            case EL734__BAD_RECVLEN:
            case EL734__BAD_RECV1:
            case EL734__BAD_RECV1_PIPE:
            case EL734__BAD_RNG:
            case EL734__BAD_SEND:
            case EL734__BAD_SEND_PIPE:
            case EL734__BAD_SEND_NET:
            case EL734__BAD_SEND_UNKN:
            case EL734__BAD_SENDLEN:
                      SerialClose(&pDorn->pData);
                      iRet = SerialForceOpen(&pDorn->pData,pDorn->pComputer,
                                        pDorn->iPort, pDorn->iChannel);
                      if(iRet != 1)
                      {
                        return VELOREDO;
                      }                 
                      else
                      {
                        return VELOFAIL;
                      }                 
                      break;
            /* handable protocoll errors */
            case ECHOMISSING:
            case EL734__BAD_TMO:
            case -1:
            case TIMEOUT:
                      return VELOREDO;
                      break;
            case STARTTIMEOUT:
                      return VELOFAIL;
                      break;
            case INVALIDSTATUS:
                      return VELOREDO;
                      break;
            default:
                      return VELOFAIL;
                      break;                       
        }
        return VELOFAIL;
  }
/*--------------------------------------------------------------------------*/
  static int DornierStat(pVelSelDriv self, int *iCode, float *fCur)
  {
        pDornier pDorn = NULL;
        int iRet, iErrStat;
        DornierStatus sStatus;
        char pCommand[80];
        char pAnswer[80];
        float fDelta;
        static int iCount = 0;
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;

        /* if busy, return VSACCEL */
        if(pDorn->iBusy)
        {
           *fCur = pDorn->fLastRPM;
           return VSACCEL;
        }

        *iCode = ROTMOVE;
        /* get the status */
        pDorn->iBusy = 1;
        iRet =  GetDornierStatus(&pDorn->pData,&sStatus);
        pDorn->iBusy = 0;
        if( iRet < 0)
        {
           iErrStat = DornierFixIt(self,iRet);
           *iCode = 9384; /* ignore this one */
           if(iErrStat == VELOREDO)
           {
             /* the velcocity selector will not respond when busy.
                Therefore such cases are interpreted as busy!
             */   
             return VSACCEL;
           }
           else
           {
             /* this is what we got if there is network trouble */
             return VELOFAIL;
           }
        }
        
             
        /* some serious logic because of multi - modes */ 
        switch(pDorn->eVelo)
        {
           case eStart:
                       *iCode = ROTSTART;
                       *fCur = 0.;
                       if(sStatus.cur_rpm >= RPMALIFE)
                       {
                          sprintf(pCommand,"SDR %5u",(int)pDorn->fTarget);
                          pDorn->iBusy = 1;
                          iRet = SerialWriteRead(&pDorn->pData,pCommand,pAnswer,49);

/*                          iRet = SerialSicsExecute(&pDorn->pData,pCommand,pAnswer,49);
*/
                          pDorn->iBusy = 0;
                          pDorn->eVelo = eRegel;
                          iCount = 0;
                          if(iRet != 1)
                          {
                            pDorn->iLastError = iRet;
                            return VELOFAIL;
                          }
                          return VSACCEL;
                        }
                        else
                        {
                          if(time(NULL) > pDorn->t_End)
                          {
                            pDorn->iLastError = STARTTIMEOUT;
                            return VELOFAIL;
                          }
                          return VSACCEL;
                        }
                        break;
           case eRegel:
                       *iCode = ROTMOVE;
                       *fCur  = (float)sStatus.cur_rpm;
                       pDorn->fLastRPM = *fCur;
                       fDelta = sStatus.cur_rpm - sStatus.nom_rpm;
                       if(fDelta < 0)
                       { 
                         fDelta = - fDelta;
                       }
                       if(fDelta > self->fTolerance)
                       {
                         iCount = 0;
                         return VSACCEL;
                       }
                       else
                       {
                         iCount++;
                         if(iCount > 4)
                         {
                           return VSOK;
                         }
                         else
                         {
                            return VSACCEL;
                         }   
                       }
                       
           case eHalted:
                       if(sStatus.iHz > 5)
                       {
                         *iCode = ROTMOVE;
                         *fCur = (float)sStatus.cur_rpm;
                         pDorn->fLastRPM = *fCur;
                         return VSACCEL;
                       }
                       else
                       {
                         return VSOK;
                       }
                       break;             
        }
        return VELOOK;
  }
/*-------------------------------------------------------------------------*/
  static int DornierText(pVelSelDriv self, char *pText, int iTextLen)
  {
        pDornier pDorn = NULL;
        int iRet, iErrStat;
        DornierStatus sStatus;
        char pBueffel[1024];
        char pHelp[80];
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;

        /* get the status */
        iRet =  GetDornierStatus(&pDorn->pData,&sStatus);
        if(iRet < 0)
        {
           pDorn->iLastError = iRet;
           return 0;
        }

         /* format it to a string */
         sprintf(pHelp,"RPM: %d , should %d\n",sStatus.cur_rpm,sStatus.nom_rpm);
         strcpy(pBueffel,pHelp);
         sprintf(pHelp,"State:  %s\n",sStatus.rm);
         strcat(pBueffel,pHelp);
         sprintf(pHelp,"Current: %d\n",sStatus.pwr);
         strcat(pBueffel,pHelp);
         sprintf(pHelp,"Rotor T: %d, Housing T: %d\n",sStatus.rot_temp,
                 sStatus.cont_temp);
         strcat(pBueffel,pHelp);
         sprintf(pHelp,"Cooling: In-T: %d, Out-T: %d, Flow: %f\n",
                 sStatus.inl_temp,sStatus.outl_temp,sStatus.cool_wat);
         strcat(pBueffel,pHelp);
         sprintf(pHelp,"Vaccum: %f, Accel: %f",sStatus.vacuum, sStatus.accel);
         strcat(pBueffel,pHelp);
         
         strncpy(pText,pBueffel, iTextLen);
         return 1;                
  }
/*------------------------------------------------------------------------*/
  static int SimSetRot(pVelSelDriv self, float fNew)
  {
    return 1;
  }  
/*------------------------------------------------------------------------*/
  static int DornierLoss(pVelSelDriv self, float *fLoss)
  {
        pDornier pDorn = NULL;
        int iRet, iErrStat, iDelta;
        DornierStatus DStatus;
        char pCommand[] = {"BRE"};
        char pAnswer[80];
        static int iCount;
        static int iError;
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;

        /* wait until not busy, to do it */
        while(pDorn->iBusy)
        {
          SicsWait(1);
        }

        /* send a command */
        pDorn->iBusy = 1;
        iRet = SerialWriteRead(&pDorn->pData,pCommand,pAnswer,79);
        pDorn->iBusy = 0;
        if(iRet != 1)
        {
          pDorn->iLastError = iRet;
          return 0;
        }
  
        /* wait 10 seconds before doing anything */
        SicsWait(10);       

        /* loop until back to speed again */
        for( ; ; )
        {
           iRet = GetDornierStatus(&pDorn->pData, &DStatus);
           if(iRet)
           {
             iError = 0;
             iDelta = DStatus.cur_rpm - DStatus.nom_rpm;
             if(iDelta < 0)
             {
               iDelta = -iDelta;
             }
             if(iDelta < 15)
             {
               iCount++;
               if(iCount > 4)
               {
                 break;
               }
             }
             else
             {
               iCount = 0;
             }
           }
           else
           {
             iError++;
             if(iError > 10)
             {
               break;
             }
           }
        }
       
             
        *fLoss = DStatus.pwr;
        return 1;  
  }
/*-------------------------------------------------------------------------*/
  static void DornierKill(void *pData)
  {
     pDornier pDorn = NULL;
     
     pDorn = (pDornier)pData;
     assert(pDorn);
     
     SerialSend(&pDorn->pData,"TTY");
     SerialClose(&pDorn->pData);
     if(pDorn->pComputer)
     {
       free(pDorn->pComputer);
     }
     free(pDorn);
  }
/*------------------------------------------------------------------------*/
  static int DornierInit(pVelSelDriv self, SConnection *pCon)
  {
      pDornier pDorn = NULL;
      int iRet, iError;
      char pError[80], pBueffel[256];
      
      assert(self);
      pDorn = (pDornier)self->pPrivate;
      assert(pDorn);
      
      /* if there is no connection, open it */
      if(!pDorn->pData)
      {
         iRet = SerialForceOpen(&pDorn->pData, pDorn->pComputer, 
                           pDorn->iPort, pDorn->iChannel);
         if(iRet != 1)
         {
             SerialError(iRet,pError,79);
             sprintf(pBueffel,"ERROR: %s when initialising Velocity selector",
                     pError);
             SCWrite(pCon,pBueffel,eError);
             return 0;           
         } 
         SerialConfig(&pDorn->pData,pDorn->iTimeOut);           
         SerialATerm(&pDorn->pData,"1\r");
         SerialSendTerm(&pDorn->pData,"\n");
      }

      /* tell him that we want control */
      iRet = DornierSend(&pDorn->pData,"REM",pError,79);
      if(iRet != 1)
      {
        sprintf(pBueffel,"ERROR: %s while switching velocity selector to remote",
                pError);
        SCWrite(pCon,pBueffel,eError);
        return 1;        
      }
      pDorn->eVelo = eHalted;
      pDorn->iBusy = 0;
      
      return 1;   
  }    
/*-------------------------------------------------------------------------*/
    pVelSelDriv  VSCreateDornierSINQ(char *name, Tcl_Interp *pTcl)
    {
      pVelSelDriv pNew = NULL;
      pDornier pDorn = NULL;
      char *pPtr = NULL;
      int iVal, iRet;
      
      /* the most likely error is the parameters specified are wrong!
         So check this first. We''ll use Tcl's result for error reporting.
         name is the name of an Tcl array which should hold the info
         necessary 
      */       
      
      /* allocate a Dornier structure */
      pDorn = (pDornier)malloc(sizeof(Dornier));
      if(!pDorn)
      {
        return NULL;
      }
      memset(pDorn,0,sizeof(Dornier));
      
      /* host name */
      pPtr = Tcl_GetVar2(pTcl,name,"Host",TCL_GLOBAL_ONLY);
      if(!pPtr)
      {
        Tcl_AppendResult(pTcl,"ERROR: no hostname found in",name,NULL);
        free(pDorn);
        return NULL;
      }
      pDorn->pComputer = strdup(pPtr);
      
      /* port number */
      pPtr = Tcl_GetVar2(pTcl,name,"Port",TCL_GLOBAL_ONLY);
      if(!pPtr)
      {
        Tcl_AppendResult(pTcl,"ERROR: no port number found in",name,NULL);
        free(pDorn);
        return NULL;
      }
      iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
      if(iRet != TCL_OK)
      {
        free(pDorn->pComputer);
        free(pDorn);
        return NULL;
      }
      pDorn->iPort = iVal;
      
      /* channel number */
      pPtr = Tcl_GetVar2(pTcl,name,"Channel",TCL_GLOBAL_ONLY);
      if(!pPtr)
      {
        Tcl_AppendResult(pTcl,"ERROR: no channel number found in",name,NULL);
        free(pDorn);
        return NULL;
      }
      iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
      if(iRet != TCL_OK)
      {
        free(pDorn->pComputer);
        free(pDorn);
        return NULL;
      }
      pDorn->iChannel = iVal;
      
      /* time out. This one gets defaulted when not specified */
      pPtr = Tcl_GetVar2(pTcl,name,"Timeout",TCL_GLOBAL_ONLY);
      if(!pPtr)
      {
         pDorn->iTimeOut = 1000;
      }
      else
      {
         iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
         if(iRet != TCL_OK)
         {
           pDorn->iTimeOut = 1000;
         }
         pDorn->iTimeOut = iVal;
       }
      
      /* business as usual: allocate memory */
      pNew = (pVelSelDriv)malloc(sizeof(VelSelDriv));
      if(!pNew)
      {
        return NULL;
      }

      /* zero the world */
      memset(pNew,0,sizeof(VelSelDriv));
      pNew->pPrivate = pDorn;
      
      /* initialise function pointers */
      pNew->DeletePrivate = DornierKill;
      pNew->Halt = DornierHalt;
      pNew->GetError = DornierError;
      pNew->TryAndFixIt = DornierFixIt;
      pNew->GetRotation = GetDornierPos;
      pNew->SetRotation = DornierRun;
      pNew->GetStatus   = DornierStat;
      pNew->GetDriverText = DornierText;
      pNew->GetLossCurrent = DornierLoss;
      pNew->Init           = DornierInit;
      
      /* done it */
      return pNew;
    }     

