/*
 * slsecho.c
 * This is a protocol adapter for yet another driver for the
 * SLS DPC magnet controller. This one is for the shiny, silvery
 * little box with TCP/IP, 8 fibre optic magnet couplers and
 * two wires for power.  The current assumption is that this
 * device communicates with 64 byte message going back and forth,
 * 8 bytes for each channel. Where the first two seem to be unused
 * and the last 6 being the DSP codes as described elsewhere.
 *
 * The expected input for this is:
 *
 *  channel:mode:code:val:convertflag
 *
 *  with mode being either r or w and convertflag being one of
 *  none,read,write
 *
 * There is an endianness issue here: swapDataBytes may or not be
 * required according to the platform used.
 *
 *  Created on: Mar 18, 2010
 *      Author: Mark Koennecke
 */
#include  <math.h>
#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <stptok.h>

#define NONE 0
#define READ 1
#define WRITE 2

typedef struct {
	char sendbuf[64];
	char readbuf[64];
	int convertflag; /* which conversions need to be done */
	int channel; /* magnet channel number, starting at 0 */
} SLSEcho, *pSLSEcho;

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

/*-------------------------------------------------------------------------
 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 void swapDataBytes(char *pMessage)
{
	char tmp[4];

	memcpy(tmp,pMessage+4,4);
	pMessage[4] = tmp[3];
	pMessage[5] = tmp[2];
	pMessage[6] = tmp[1];
	pMessage[7] = tmp[0];
}
/*-------------------------------------------------------------------------*/
static int EchoPack(Ascon *a)
{
	pSLSEcho echo = NULL;
	char *pMessage = NULL, *pPtr = NULL;
	char token[20], parseString[255];
	int code, val;
	double dval;

	echo = a->private;
	assert(echo != NULL);

	memset(echo->sendbuf,0,sizeof(echo->sendbuf));
	memset(echo->readbuf,0,sizeof(echo->readbuf));

    assert(GetDynStringLength(a->wrBuffer) < 255);
    memset(parseString,0,sizeof(parseString));
    strcpy(parseString,GetCharArray(a->wrBuffer));
    pPtr = parseString;

    /* read num */
    pPtr = stptok(pPtr,token,sizeof(token),":");
    echo->channel = atoi(token);
    pMessage = echo->sendbuf + echo->channel*8;

    /* read mode */
    pPtr = stptok(pPtr,token,sizeof(token),":");
    if(token[0] == 'r'){
    	pMessage[1] = DSPREAD;
    } else if(token[0] == 'w'){
    	pMessage[1] = DSPWRITE;
    }else if(token[0] == 's'){
    	pMessage[1] = 0x2;
    } else {
    	a->state = AsconFailed;
		DynStringConcat(a->errmsg,"ERROR: bad mode token ");
		DynStringConcat(a->errmsg,token);
    	return 0;
    }

    /* read code */
    pPtr = stptok(pPtr,token,sizeof(token),":");
    sscanf(token,"%x", &code);
    pMessage[3] = code;

    /* read value */
    pPtr = stptok(pPtr,token,sizeof(token),":");
    dval = atof(token);
    val = dval;

    /* read convertflag */
    pPtr = stptok(pPtr,token,sizeof(token),":");
    if(strcmp(token,"none") == 0){
    	echo->convertflag = NONE;
    } else if(strcmp(token,"read") == 0){
    	echo->convertflag = READ;
    } else if(strcmp(token,"write") == 0){
    	echo->convertflag = WRITE;
    } else {
    	a->state = AsconFailed;
		DynStringConcat(a->errmsg,"ERROR: bad convertflag ");
		DynStringConcat(a->errmsg,token);
    	return 0;
    }
    if(echo->convertflag == WRITE){
    	val = double2DSPfloat(dval);
    }
    memcpy(pMessage+4,&val,4);
    swapDataBytes(pMessage);

    /* printf("Sending code: 0x%x, val = %d\n", code, val); */
	return 1;
}
/*-------------------------------------------------------------------*/
static int EchoUnpack(Ascon *a)
{
	pSLSEcho echo = a->private;
	char *pMessage = NULL, printbuf[30];
	int val;
	unsigned char code = 0;
	double dval;


	DynStringClear(a->rdBuffer);
	pMessage = echo->readbuf + echo->channel*8;

	code = pMessage[3];
	snprintf(printbuf,sizeof(printbuf),"%x", code);
	DynStringConcat(a->rdBuffer,printbuf);
	DynStringConcatChar(a->rdBuffer,':');

	swapDataBytes(pMessage);
	memcpy(&val,pMessage+4, 4);
	dval = (double)val;

	if(echo->convertflag == READ){
		dval = DSPfloat2double(val);
	}
	snprintf(printbuf,sizeof(printbuf),"%f", dval);
	DynStringConcat(a->rdBuffer,printbuf);
    /* printf("Reading code: 0x%x, val = %d\n", code, val);*/
	return 1;
}
/*----------------------------------------------------------------------*/
static int SLSEchoHandler(Ascon *a)
{
	pSLSEcho echo = a->private;
	int written, read;
    char c;

	assert(echo != NULL);

	switch(a->state){
	case AsconWriteStart:
		EchoPack(a);
		a->wrPos = 0;
		a->state = AsconWriting;
		break;
	case AsconWriting:
	    AsconReadGarbage(a->fd);
	    written = send(a->fd, echo->sendbuf+a->wrPos,64-a->wrPos, 0);
	    if(written > 0){
	    	a->wrPos += written;
	    	if(a->wrPos >= 64){
	    		a->state = AsconWriteDone;
	    		break;
	    	}
	    } else if (written == 0) {
	    	errno = ECONNRESET;
	    	return ASCON_DISCONNECTED;
	    } else {
	    	return ASCON_SEND_ERROR;
	    }
	    break;
	case AsconReadStart:
		a->start = DoubleTime();
		a->state = AsconReading;
		a->wrPos = 0; /* reusing wrPos for character count received*/
		break;
	case AsconReading:
		read = AsconReadChar(a->fd,&c);
	    if (read <= 0) {
	      if (read < 0) {
	        AsconError(a, "ASC5", errno);
	        return 0;
	      }
	      if (a->timeout > 0) {
	        if (DoubleTime() - a->start > a->timeout) {
	          AsconError(a, "no response", 0);
	          a->state = AsconTimeout;
	        }
	      }
	      return 0;
	    }
	    echo->readbuf[a->wrPos] = c;
	    a->wrPos++;
	    if(a->wrPos >= 64){
	    	a->state = AsconReadDone;
	    	EchoUnpack(a);
	    }
		break;
	default:
	    return AsconStdHandler(a);
	}
	return 1;
}
/*------------------------------------------------------------------------*/
static int SLSEchoInit(Ascon * a, SConnection * con, int argc, char *argv[])
{
  a->fd = -1;
  a->state = AsconConnectStart;
  a->reconnectInterval = 10;
  a->hostport = strdup(argv[1]);
  if (argc > 2) {
    a->timeout = atof(argv[2]);
  } else {
    a->timeout = 2.0;           /* sec */
  }
  a->private = malloc(sizeof(SLSEcho));
  if(a->private == NULL){
	  return 0;
  }
  a->killPrivate = free;
  return 1;
}
/*------------------------------------------------------------------------*/
void AddSLSEchoProtocoll()
{
  AsconProtocol *prot = NULL;

  prot = calloc(sizeof(AsconProtocol), 1);
  prot->name = strdup("slsecho");
  prot->init = SLSEchoInit;
  prot->handler = SLSEchoHandler;
  AsconInsertProtocol(prot);
}