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

  Another driver for a Dornier velocity selector. This is for a newer 
  version of the velocity selector driver as delivered with SANS-2. It
  also uses a direct connection to the terminal server without David Maden's
  SerPortServer program in between. 

  I believe this is for Dornier software version: NGS037 of 2002.

  The protocoll is inconsistent: status messages come back with a <cr>,
  command responses tend to come back with a \ and no <cr>!

  There is a scheme here: while waiting for status reponses during driving,
  the last status read is used for any requests.

  copyright: see file COPYRIGHT

  Mark Koennecke, July 2003
  ---------------------------------------------------------------------------*/
#include <sics.h>
#include <string.h>
#include <math.h>
#include <tcl.h>
#include <time.h>
#include <fortify.h>
#include <rs232controller.h>
typedef struct __VelSelDriv *pVelSelDriv;

#include <velodriv.h>
#include "velodorn.h"

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


/* start speed */
#define STARTSPEED 3100

/*--------- special Dornier conditions*/
#define STARTED    -88
#define HALTREQ    -77
/* INVALIDSTATUS is defined in velodorn.h */
#define TARGETREJECTED -7001
#define NOSTATUS       -7002

/*---------- DORNIER status modes */
#define STATSEND 1
#define STATREAD 2
/*----------------------------- The private data structure ---------------*/
 typedef struct{
                prs232 controller;
                int iTimeOut;
                int iLastError;
                time_t  t_End;
                time_t  t_timeout;
                float fTarget;
                float fLastRPM;
                int statusMode;
                DornierStatus lastStatus;
                int minRPM; /* the minimum control speed of the thing*/
                int haltCount;
                int rejectCount;
                int noStatus; /* flag which indicates that no valid status
                                 has yet been read. Solves a starting
                                 problem
			      */
                 int firstStatus; /* at times the nvs does not send
                                     the reply to the first status 
                                     request after starting. This flag
                                     helps to suppress an error message
                                     which may be confusing to loosers
				  */
                } Dornier, *pDornier;
/*------------------------------------------------------------------*/
static int requestDornierStatus(pDornier pDorn){
  int status;

  status = writeRS232(pDorn->controller,"???\n",4);
  if(status < 0){
    pDorn->iLastError = status;
    return 0;
  }
  return 1;
}
/*------------------------------------------------------------------*/
static int readAndInterpretStatus(pDornier pDorn, DornierStatus *DStatus){
  int status, datalen;
  char reply[512];

  datalen = 512;
  status = readRS232TillTerm(pDorn->controller,reply,&datalen);
  if(status < 0){
    pDorn->iLastError = status;
    return 0;
  }
  if(strlen(reply) < 80){
    pDorn->iLastError = INVALIDSTATUS;
    pDorn->statusMode = STATSEND;
    return 0;
  }

  DecodeNewDornierStatus(reply,DStatus);
  if(pDorn->noStatus == 1){
    pDorn->noStatus = 0;
  }
  return 1;
}
/*-----------------------------------------------------------------*/
static int takeControl(pDornier pDorn){
  int iRet;
  char pError[80];
  setRS232ReplyTerminator(pDorn->controller,"\\");
  iRet = transactRS232(pDorn->controller,"REM\n",4,pError,79);
  setRS232ReplyTerminator(pDorn->controller,"\n");
  return iRet;
}
/*--------------------------------------------------------------------*/
  static int GetDornierPos(pVelSelDriv self, float *fPos)
  {
    pDornier pDorn = NULL;
    DornierStatus DStatus;
    int status;
  
    assert(self);
    pDorn = (pDornier)self->pPrivate;
       
    if(pDorn->statusMode == STATSEND){
      if(!requestDornierStatus(pDorn)){
	   *fPos = -9999.;
	   return 0;
      }
      if(!readAndInterpretStatus(pDorn,&DStatus)){
	    *fPos = -9999.;
	     return 0;
      }
      pDorn->lastStatus = DStatus;
    }
    
    *fPos = pDorn->lastStatus.cur_rpm;
    pDorn->fLastRPM = pDorn->lastStatus.cur_rpm;
    return 1;
  } 
/*--------------------------------------------------------------------------*/
  static int DornierHalt(pVelSelDriv self)
  {
        pDornier pDorn = NULL;
        int iRet;
        char pCom[50];
        char pAnswer[80];
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;

	snprintf(pCom,49,"SDR %d\n",pDorn->minRPM);
	iRet = transactRS232(pDorn->controller,pCom,strlen(pCom),
			     pAnswer,79);
        if(iRet < 1)
        {
          pDorn->iLastError = iRet;
          return 0;
        }
        return 1;
  }
/*----------------------------------------------------------------------*/
  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;

	/*
	  use cached status while waiting for reply during drive
	*/
	if(pDorn->statusMode == STATSEND){
	  if(!requestDornierStatus(pDorn)){
	    return 0;
	  }
	  if(!readAndInterpretStatus(pDorn,&sStatus)){
	    return 0;
	  }
	  pDorn->lastStatus = sStatus;
	} else {
	  sStatus = pDorn->lastStatus;
	}

         /* 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 DornierRun(pVelSelDriv self, float fVal)
  {
      int iRet;
      char pCommand[50], pAnswer[50], pText[132];
      pDornier pDorn = NULL;
      int startFlag = 0;
      int i;
      DornierStatus sStatus;

      assert(self);
      pDorn = (pDornier)self->pPrivate;    

      /* 
       make sure that a status was read before we do anything here,
       otherwise we may be in deep trouble
      */
      if(pDorn->statusMode == STATSEND){
	iRet = requestDornierStatus(pDorn);
	if(iRet == 0){
	  return 0;
	}
      }
      iRet = readAndInterpretStatus(pDorn,&sStatus);
      if(iRet == 0){
	return 0;
      }
      pDorn->lastStatus = sStatus;

      /* 
	 less then STARTSPEED, means halt in this case.
	 Accept this only after three times, see code in GetError as well.
      */
      if(fVal < 0){
	fVal = - fVal;
      }
      memset(pCommand,0,50);
      pDorn->rejectCount = 0;

      if(fVal < STARTSPEED - 3*self->fTolerance)
      {
	if(pDorn->haltCount < 3){
	  pDorn->iLastError = HALTREQ;
	  return 0;
	}
	strcpy(pCommand,"HAL\n");
	setRS232ReplyTerminator(pDorn->controller,"\r");
	pDorn->haltCount = 0;
	pDorn->fTarget = fVal;
      } else {
	if(pDorn->lastStatus.cur_rpm < STARTSPEED - 3 * self->fTolerance){
	  strcpy(pCommand,"SST\n");
	  startFlag = 1;
	  pDorn->fTarget = STARTSPEED;
	  setRS232ReplyTerminator(pDorn->controller,"\r");
	} else {
	  setRS232ReplyTerminator(pDorn->controller,"\r");
	  sprintf(pCommand,"SDR %d\n",(int)fVal);
	  pDorn->fTarget = fVal;
	}
      }
      
      iRet = transactRS232(pDorn->controller,pCommand,strlen(pCommand),
			   pAnswer,49);
      setRS232ReplyTerminator(pDorn->controller,"\n");
      pDorn->firstStatus = 1;
      if(iRet < 1)
      {
	if(iRet != INCOMPLETE){
	  
	  pDorn->iLastError = iRet;
	  return 0;
	} 
      }
      pDorn->statusMode = STATSEND;
      if(startFlag){
	pDorn->iLastError = STARTED;
	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 HALTREQ:
	strncpy(error,"Repeat command if you really want to HALT selector",
		 iErrLen);
	pDorn->haltCount++;
	break;
      case STARTED:
	strncpy(error,
		"Started selector, standby and check manually when ready",
		iErrLen);
	break;
      case INVALIDSTATUS:
	strncpy(error,"Received invalid status reply",iErrLen);
	break;
      case TARGETREJECTED:
	strncpy(error,"VS in local mode or target out of range", iErrLen);
	break;
      case NOSTATUS:
        strncpy(error,"No successfull status request after 3 tries",
		iErrLen);
	break;
      default:
	getRS232Error(pDorn->iLastError,error,iErrLen);
	break;
      }
      return 1;
}
/*-------------------------------------------------------------------*/
static int DornierFixIt(pVelSelDriv self, int iCode){
      pDornier pDorn = NULL;
      int status, oldReject;

      assert(self);
      pDorn = (pDornier)self->pPrivate;    

      switch(iCode){
      case NOTCONNECTED:
	status = initRS232(pDorn->controller);
	if(status){
	  return VELOREDO;
	} else {
	  return VELOFAIL;
	}
	break;
      case TIMEOUT:
      case INCOMPLETE:
      case INVALIDSTATUS:
	return VELOREDO;
	break;
      case TARGETREJECTED:
        if(pDorn->rejectCount >= 3){
	  pDorn->rejectCount = 0;
	  return VELOFAIL;
	}
	oldReject = pDorn->rejectCount;
	status = takeControl(pDorn);
	if(status >= 1){
	  DornierRun(self,pDorn->fTarget);
	  pDorn->rejectCount = oldReject + 1;
	  return VELOREDO;
	}
	return VELOFAIL;
	break;
      default:
	return VELOFAIL;
      }
} 
/*---------------------------------------------------------------------*/
static int statusSendHandler(pDornier pDorn){
  int status;

  if(!requestDornierStatus(pDorn)){
    return VSFAIL;
  }
  pDorn->t_timeout = time(NULL) + pDorn->iTimeOut/1000;
  pDorn->statusMode = STATREAD;
  return VSACCEL;
}
/*------------------------------------------------------------------*/
static int evaluateStatus(pVelSelDriv self, int *iCode){
  int status;
  DornierStatus sStatus;
  char pCommand[80];
  char pAnswer[80];
  float fDelta;
  static int iCount = 0;
  pDornier pDorn = NULL;

  pDorn = (pDornier)self->pPrivate;

  pDorn->statusMode = STATSEND;
  status = readAndInterpretStatus(pDorn,&sStatus);
  if(!status){
    if(pDorn->firstStatus == 1){
      pDorn->firstStatus = 0;
      return VSACCEL;
    }
    return VELOFAIL;
  }

  *iCode = ROTMOVE;

  /*
    sometimes the velocity selector does not accept a new target:
    Two reasons: a) it is local, b) out of range
    Check for this here as it is the only place appropriate.
  */
  fDelta = sStatus.nom_rpm - pDorn->fTarget;
  if(fDelta < 0){
    fDelta = - fDelta;
  }
  if(fDelta > self->fTolerance){
    pDorn->iLastError = TARGETREJECTED;
    return VSFAIL;
  }

  /*
    This code considers the velocity selector arrived if it reads 
    four times a difference between requested speed and actual speed
    below difference
  */
  pDorn->fLastRPM = sStatus.cur_rpm;
  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;
    }   
  }
}
/*---------------------------------------------------------------------*/
static int statusReceiveHandler(pVelSelDriv self, int *iCode){
  int status;
  pDornier pDorn = NULL;

  pDorn = (pDornier)self->pPrivate;

  status = availableRS232(pDorn->controller);
  if(!status){
    if(time(NULL) > pDorn->t_timeout){
      pDorn->iLastError = TIMEOUT;
      pDorn->statusMode = STATSEND;
      return VELOFAIL;
    } else {
      return VSACCEL;
    }
  }
  
  return evaluateStatus(self, iCode);
}
/*--------------------------------------------------------------------------
  The Dornier takes a long time to answer a status message. In order to keep 
  SICS responsive the following state machine is implemented:
  - a status request is sent.
  - next data availability will be checked, if available: process!
  ---------------------------------------------------------------------------*/
static int DornierStatNew(pVelSelDriv self, int *iCode, float *fCur){
  pDornier pDorn = NULL;
  int status;
  
  assert(self);
  pDorn = (pDornier)self->pPrivate;

  if(pDorn->statusMode == STATSEND){
    return statusSendHandler(pDorn);
  } else {
    status = statusReceiveHandler(self,iCode);
    *fCur = pDorn->fLastRPM;
    return status;
  }
}
/*------------------------------------------------------------------------*/
  static int DornierLoss(pVelSelDriv self, float *fLoss)
  {
        pDornier pDorn = NULL;
        int iRet, iErrStat, iDelta;
        DornierStatus DStatus;
        char pCommand[] = {"BRE\n"};
        char pAnswer[80];
        static int iCount;
        static int iError;
	int i;
        
        assert(self);
        pDorn = (pDornier)self->pPrivate;

        /* send a command */
        iRet = transactRS232(pDorn->controller,pCommand,strlen(pCommand),
			     pAnswer,79);
        if(iRet < 1)
        {
          pDorn->iLastError = iRet;
          return 0;
        }
  
        /* wait 10 seconds before doing anything */
        SicsWait(10);       

        /* loop until back to speed again */
        for(i = 0; i < 100; i++ )
        {
	  if(!requestDornierStatus(pDorn)){
	    return 0;
	  }
	  if(!readAndInterpretStatus(pDorn,&DStatus)){
	    return 0;
	  }
          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;
           }
        }
        *fLoss = DStatus.pwr;
        return 1;
  }
/*-------------------------------------------------------------------------*/
  static void DornierKill(void *pData)
  {
     pDornier pDorn = NULL;
     
     pDorn = (pDornier)pData;
     assert(pDorn);
     
     writeRS232(pDorn->controller,"TTY\n",4);
     KillRS232(pDorn->controller);
     free(pDorn);
  }
/*------------------------------------------------------------------------*/
  static int DornierInit(pVelSelDriv self, SConnection *pCon)
  {
      pDornier pDorn = NULL;
      int iRet, iError;
      float fRot;
      char pError[80], pBueffel[256];
      
      assert(self);
      pDorn = (pDornier)self->pPrivate;
      assert(pDorn);
      
      iRet = initRS232(pDorn->controller);
      if(iRet < 0){
	return 1;
      }      
      setRS232SendTerminator(pDorn->controller,"\n");
      setRS232Timeout(pDorn->controller,pDorn->iTimeOut);
      setRS232Debug(pDorn->controller,0);

      /* 
	 tell him that we want control.
	 Funny enough no <cr> or <nl> is sent in the reply to this.
      */
      iRet = takeControl(pDorn);
      if(iRet <= 1)
      {
        sprintf(pBueffel,
          "ERROR: %s while switching velocity selector to remote",
                pError);
        SCWrite(pCon,pBueffel,eError);
      }
      /*
	check which status the velo is in
      */
      pDorn->statusMode = STATSEND;
      return 1;   
  }    
/*-------------------------------------------------------------------------*/
    pVelSelDriv  VSCreateDornier2003(char *name, Tcl_Interp *pTcl)
    {
      pVelSelDriv pNew = NULL;
      pDornier pDorn = NULL;
      char *pPtr = NULL;
      int iVal, iRet, iPort;
      char pHost[132];


      /* 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 = (char *)Tcl_GetVar2(pTcl,name,"Host",TCL_GLOBAL_ONLY);
      if(!pPtr)
      {
        Tcl_AppendResult(pTcl,"ERROR: no hostname found in",name,NULL);
        free(pDorn);
        return NULL;
      }
      strncpy(pHost,pPtr,131);
      
      /* port number */
      pPtr = (char *)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,&iPort);
      if(iRet != TCL_OK)
      {
        free(pDorn);
        return NULL;
      }
      
      /* time out. This one gets defaulted when not specified */
      pPtr = (char *)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;
       }

      /* minimum control speed */
      pPtr = (char *)Tcl_GetVar2(pTcl,name,"MinControl",TCL_GLOBAL_ONLY);
      if(!pPtr)
      {
         pDorn->minRPM = 3100;
      }
      else
      {
         iRet = Tcl_GetInt(pTcl,pPtr,&iVal);
         if(iRet != TCL_OK)
         {
           pDorn->minRPM = 3100;
         }
         pDorn->minRPM = 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;
      pDorn->controller = createRS232(pHost,iPort);
      if(!pDorn->controller){
	DornierKill(pNew->pPrivate);
	free(pNew);
	return NULL;
      }
      pDorn->noStatus = 1;
      
      /* initialise function pointers */
      pNew->DeletePrivate = DornierKill;
      pNew->Halt = DornierHalt;
      pNew->GetError = DornierError;
      pNew->TryAndFixIt = DornierFixIt;
      pNew->GetRotation = GetDornierPos;
      pNew->SetRotation = DornierRun;
      pNew->GetStatus   = DornierStatNew;
      pNew->GetDriverText = DornierText;
      pNew->GetLossCurrent = DornierLoss;
      pNew->Init           = DornierInit;
      
      /* done it */
      return pNew;
    }     







