 /*--------------------------------------------------------------------------
                     S L S M A G N E T

 This file contains the driver for the PSI-DSP magnet controller as
 aquired from SLS.

 This device runs a binary protocoll on its RS232 interface. And the lowest
 speed this device understands is 115KB! This is why this driver accesses 
 the terminal server port directly through the network without David's
 SerPortServer program in between. Also make sure that the port runs at
 115KB when using this device. 

 Mark Koennecke, October 2001 

 Copyright: see copyright.h
----------------------------------------------------------------------------*/
#include <string.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <assert.h>
#include <unistd.h>
#include <fortify.h>
#include <conman.h>
#include <servlog.h>
#include <fortify.h>

#include "evdriver.h"
#include <network.h>
#include <rs232controller.h>

/*
  error codes
*/
#define BADECHO -5100
/* 
#define NOTCONNECTED -5200 
#define TIMEOUT      -5300
*/

/*
  when waiting for results, the code loops at max MAXLOOP times 
  before doing a timeout. 100 corresponds to one second
*/
#define MAXLOOP 100

/*
  packet header codes
*/
#define DSPWRITE 0x80
#define DSPREAD 0x0

/*-----------------------------------------------------------------------*/
  typedef struct {
                   mkChannel *pSock;
                   char *pHost;
                   int iPort;
                   int iError;
                  } SLSDriv, *pSLSDriv;
/*-------------------------------------------------------------------------
 Code for data conversion from Lukas Tanner. The ULONG is system dependent,
 it MUST describe a 32 bit value.  
 -------------------------------------------------------------------------*/
#define ULONG int
/***********************************************************************/
static ULONG double2DSPfloat( double input )
/***********************************************************************/
/* Konvertiert eine normale double Variable in ein 32bit DSP Float Format. */
{
	ULONG output;
	double mantissa;
	int exponent;

	if (input == 0) {
		output = 0;
	} else {
		mantissa = 2 * (frexp(fabs(input), &exponent));
		mantissa = ldexp(mantissa, 23);
		exponent =exponent - 1 + 127;
		if (exponent < 0) {
			exponent = 0;
		} else if (exponent > 0xFE) {
			exponent = 0xFE;
		}
		exponent = exponent << 23;
		output = ((ULONG) mantissa) & 0x7FFFFF;
		output = output | ((ULONG) (exponent));
		if (input < 0) {
			output = output | 0x80000000;
		}
	}
    return output;
}

/***********************************************************************/
static double DSPfloat2double( ULONG input )
/***********************************************************************/
/* Konvertiert eine Variable inder ein 32bit DSP Float Wert abgelegt ist,
   in ein normales double Format
   32bit IEEE Float => SEEEEEEEEMMMMMMMMMMMMMMMMMMMMMMM;	S = Sign Bit
															E = Exponent
												   			M = Mantissa (23)*/
{
	double output;
	if ((input & 0x7FFFFF) == 0 && ((input >> 23) & 0xFF) == 0) {
		output = 0;
	} else {
		output = ldexp(input & 0x7FFFFF,-23) + 1;
		output = output * pow(-1, ((input >>31) & 1));
		output = output * ldexp(1, (((input >>23) & 0xFF) - 127));
	}
    return output;
}
/*-------------------------------------------------------------------------*/
static int communicateSLS(mkChannel *pSock, char msg[6], char reply[6])
{
  long lVal = 0;
  int iRet, i;

  if(!pSock)
    return NOTCONNECTED;

  iRet = NETWrite(pSock,msg,6);
  if(iRet < 0)
  {
    return iRet;
  }
  for(i = 0; i < MAXLOOP; i++)
  {
    iRet = NETAvailable(pSock,10);
    if(iRet < 0)
    {
      return iRet;
    }
    else if(iRet == 1)
    {
      lVal += NETRead(pSock,reply+lVal,6-lVal,-10);
      if(lVal >= 6)
      {
        return (int)lVal;      
      }
      else
      {
	continue;
      }
    }
  }
  return TIMEOUT;
}
/*---------------------------------------------------------------------------*/
  static int GetSLSPos(pEVDriver  self, float *fPos)
  {
     pSLSDriv pMe = NULL;
     int iRet, ival;
     double dval;
     char msg[6], reply[6];
     long lVal;

     assert(self);
     pMe = (pSLSDriv)self->pPrivate;
     assert(pMe);
     
     msg[0] = DSPREAD; /* read request */
     msg[1] = 0x92; /* address of mag current */
     iRet = communicateSLS(pMe->pSock,msg,reply);
     if(iRet < 0)
     {
       pMe->iError = iRet;
       return iRet;
     }
     memcpy(&ival,reply+2,4);
     dval = DSPfloat2double(ival);
     *fPos = (float)dval;
     pMe->iError = 0;

     return 1;
  } 
/*----------------------------------------------------------------------------*/
  static int SLSRun(pEVDriver self, float fVal)
  {
     pSLSDriv pMe = NULL;
     int iRet, ival,i;
     char msg[6], reply[6];
     
     assert(self);
     pMe = (pSLSDriv )self->pPrivate;
     assert(pMe);

     msg[0] = DSPWRITE;
     msg[1] = 0x90;
     ival = double2DSPfloat((double)fVal);
     memcpy(msg+2, &ival,4);
     iRet = communicateSLS(pMe->pSock,msg,reply);
     if(iRet <= 0)
     {
       pMe->iError = iRet;
       return iRet;
     }
     for(i = 1; i < 6; i++)
     {
       if(msg[i] != reply[i])
       {
	 pMe->iError = BADECHO;
         return BADECHO;
       }
     }
     return 1;
 }
/*--------------------------------------------------------------------------*/
  static int SLSError(pEVDriver self, int *iCode, char *error, int iErrLen)
  {
     pSLSDriv pMe = NULL;
     char *pPtr = NULL;
     int i1, i2;
     char pBueffel[132];
     
     assert(self);
     pMe = (pSLSDriv)self->pPrivate;
     assert(pMe);

     *iCode = pMe->iError;
     switch(*iCode)
     {
     case BADECHO:
       strncpy(error,"message sent and reply did not match", iErrLen);
       break;
     case NOTCONNECTED:
       strncpy(error,"Not connected to device", iErrLen);
       break;
     case TIMEOUT:
       strncpy(error,"Timeout waiting for response", iErrLen);
       break;
     default:
       getRS232Error(*iCode,error,iErrLen);
       break;
     }  
    
    return 1;
  }
/*-------------------------------------------------------------------------*/
  static int SLSSend(pEVDriver self, char *pCommand, char *pReply, int iLen)
  {
    strncpy(pReply,"PSI-DSP understands only binary, send disabled",iLen);
    return 0;
  }
/*--------------------------------------------------------------------------*/
  static int SLSInit(pEVDriver self)
  {
     pSLSDriv pMe = NULL;
     int iRet, ival, i;
     char msg[6], reply[6];
     
     assert(self);
     pMe = (pSLSDriv )self->pPrivate;
     assert(pMe);

     pMe->pSock = NULL;
     pMe->pSock = NETConnect(pMe->pHost,pMe->iPort);
     if(!pMe->pSock)
     {
       return 0;
     }

     sleep(1); 

     /*
       try to switch device on
     */
     msg[0] = DSPWRITE;
     msg[1] = 0x31;
     ival = 1;
     memcpy(msg+2, &ival,4);
     iRet = communicateSLS(pMe->pSock,msg,reply);
     if(iRet <= 0)
     {
       pMe->iError = iRet;
       return iRet;
     }
     for(i = 1; i < 6; i++)
     {
       if(msg[i] != reply[i])
       {
	 pMe->iError = BADECHO;
         return BADECHO;
       }
     }

     return 1;
  }  
/*--------------------------------------------------------------------------*/
  static int SLSClose(pEVDriver self)
  {
     pSLSDriv pMe = NULL;
     int iRet;
     
     assert(self);
     pMe = (pSLSDriv )self->pPrivate;
     assert(pMe);

     NETClosePort(pMe->pSock);
     pMe->pSock = NULL;
     return 1;
  }  
/*---------------------------------------------------------------------------*/
  static int SLSFix(pEVDriver self, int iError)
  {
     pSLSDriv pMe = NULL;
     int iRet;
     
     assert(self);
     pMe = (pSLSDriv )self->pPrivate;
     assert(pMe);

     switch(iError)
     {
            case BADECHO:
            case TIMEOUT:
	        return DEVREDO;
            default:
                      SLSClose(self);
                      iRet = SLSInit(self);
                      if(iRet)
                      {
                        return DEVREDO;
                      }                 
                      else
                      {
                        return DEVFAULT;
                      }                 
                      break;
     }
  }
     
/*--------------------------------------------------------------------------*/
  static int SLSHalt(pEVDriver *self)
  {
     assert(self);
     
     return 1;
  }
/*------------------------------------------------------------------------*/
  void KillSLS(void *pData)
  {
     pSLSDriv pMe = NULL;
     
     pMe = (pSLSDriv)pData;
     assert(pMe);
     
     if(pMe->pHost)
     {
        free(pMe->pHost);
     }
     free(pMe);
  }  
/*------------------------------------------------------------------------*/
  pEVDriver CreateSLSDriv(int argc, char *argv[])
  {
     pEVDriver pNew = NULL;
     pSLSDriv pSim = NULL;
     
     /* check for arguments */
     if(argc < 2)
     {
       return NULL;
     }
     
     pNew = CreateEVDriver(argc,argv);
     pSim = (pSLSDriv)malloc(sizeof(SLSDriv));
     memset(pSim,0,sizeof(SLSDriv));
     if(!pNew || !pSim)
     {
       return NULL;
     }
     pNew->pPrivate = pSim;
     pNew->KillPrivate = KillSLS;
     
     pSim->pHost = strdup(argv[0]);
     pSim->iPort = atoi(argv[1]);
     
     /* initialise function pointers */
     pNew->SetValue = SLSRun;
     pNew->GetValue = GetSLSPos;
     pNew->Send     = SLSSend;
     pNew->GetError = SLSError;
     pNew->TryFixIt = SLSFix;
     pNew->Init     = SLSInit;
     pNew->Close    = SLSClose;
     
     return pNew; 
  }	
  


