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

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

 Mark Koennecke, October 2001 

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

  typedef struct __EVDriver  *pEVDriver;

#include "evdriver.i"  
#include "hardsup/el734_def.h"
#include "hardsup/el734fix.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 iRet;
  }  
/*--------------------------------------------------------------------------*/
  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; 
  }