/*----------------------------------------------------------------------------
  This is a single counter implemented on top of the Risoe ECB electronic

  copyright: see file COPYRIGHT

  Mark Koennecke, January-February 2003
  ---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <errno.h>
#include <tcl.h>
#include <math.h>
#include <unistd.h>
#include <fortify.h>
#include <sics.h>
#include <status.h>
#include "ecb.h"
#include <countdriv.h>

/*------------------ our private data structure ------------------------*/
typedef struct {
  pECB ecb; /* the ECB system we talk to */
  unsigned char prescaler[8]; /* an array for the prescaler values */
  int tfreq;        /* timer frequency */
  unsigned char control;      /* marks the control monitor */
  int state;                  /* current counting state */ 
}ECBCounter, *pECBCounter;

/*----------------- private defines ------------------------------------*/
#define STFRD    137
#define STREAD   138
#define STOPS    136 
#define STCLEA   134
#define PRELOA   139
#define STLOAD   156
#define STCPRE   133 
#define STARTS   135
#define SPCSTA   169

/*------------------ state codes --------------------------------------*/
#define IDLE 0
#define COUNT 2
#define NOBEAM 3
/*--------------------------------------------------------------------*/
#define MAX_COUNT 4294967295.0 
/*------------------ error codes --------------------------------------*/
#define COMMERROR        -300
#define TOMANYCOUNTS     -301
#define NOSEND           -302
#define INVALIDCOUNTER   -304
#define INVALIDPRESCALER -305
#define BADFREQ          -306
/*======================================================================*/
static int readScaler(pECBCounter pPriv, int scaler, int *count){
  int status;
  Z80_reg in, out;
  Ecb_pack data;
  
  in.c = (unsigned char)scaler;
  status = ecbExecute(pPriv->ecb,STREAD,in,&out);
  if(status != 1){
    return COMMERROR;
  }

  data.b.byt3 = out.c;
  data.b.byt2 = out.b;
  data.b.byt1 = out.d;
  data.b.byt0 = out.e;
  if(scaler == 0){
    *count = data.result/pPriv->tfreq;
  } else {
    *count = data.result;
  }

  return 1;
}
/*---------------------------------------------------------------------*/
static int check4Beam(struct __COUNTER *pCter, int *beam){
  Z80_reg in, out;
  pECBCounter self = NULL;
  int status;

  self = (pECBCounter)pCter->pData;
  assert(self);

  in.c = 1;
  status = ecbExecute(self->ecb,SPCSTA,in,&out);
  if(status != 1){
    pCter->iErrorCode = COMMERROR;
    return HWFault;
  }
  *beam = (int)out.d;
  return 1;
}
/*----------------------------------------------------------------------*/
static int stopScalers(pECBCounter self){
  int status;
  Z80_reg in, out;

  status = ecbExecute(self->ecb,STOPS,in,&out);
  if(status != 1){
    return COMMERROR;
  }
  return 1;
}
/*========================================================================
  These two functions currently rely on the idea that the ECB stops 
  and starts without clearing counters in between. The sequence of
  things necessary to start it, suggests this. If this is not the case then
  this will not work.
===========================================================================*/
static int ECBPause(struct __COUNTER *self){
  int status;
  pECBCounter pPriv = NULL;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  if((status = stopScalers(pPriv)) <= 0){
    self->iErrorCode = status;
    return HWFault;
  }
  return OKOK;
}
/*=======================================================================*/
static int ECBContinue(struct __COUNTER *self){
  int status;
  pECBCounter pPriv = NULL;
  Z80_reg in, out;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  status = ecbExecute(pPriv->ecb,STARTS,in,&out);
  if(status != 1){
    self->iErrorCode = status;
    return HWFault;
  }

  return OKOK;
}
/*-----------------------------------------------------------------------*/
static int ECBGetStatus(struct __COUNTER *self, float *fControl){
  pECBCounter pPriv = (pECBCounter)self->pData;
  int status, result, scaler;
  Z80_reg in, out;
  int count, beam;
  
  assert(pPriv);

  /*
    This can happen after a stop
  */
  if(pPriv->state == IDLE){
    return HWIdle;
  }

  /*
    read status bit
  */
  status = ecbExecute(pPriv->ecb,STFRD,in,&out);
  if(status != 1){
    self->iErrorCode = COMMERROR;
    pPriv->state = IDLE;
    return HWFault;
  }
  /*
    read beam status
  */
  status = check4Beam(self,&beam);
  if(status != 1){
    self->iErrorCode = COMMERROR;
    return HWFault;
  }
  beam &= 1;

  /* 
     sophisticated logic in order to keep track of the various states
     the thing can be in. Complicated by the fact that the status becomes 
     idle (out.d = 0) when the measurement is paused due to the lack of 
     beam.
  */
  if(pPriv->state == COUNT && beam == 1){
    ECBPause(self);
    pPriv->state = NOBEAM;
    SetStatus(eOutOfBeam);
    result = HWNoBeam;
  }
  if(pPriv->state == NOBEAM && beam == 0){
    ECBContinue(self);
    pPriv->state = COUNT;
    SetStatus(eCounting);
    return HWBusy;
  }
  if(pPriv->state == NOBEAM && beam == 1){
    return HWNoBeam;
  }
  if(out.d == 0 && pPriv->state == COUNT){
    result = HWIdle;
    pPriv->state = IDLE;
  } else {
    result = HWBusy;
  }


  /*
    select which scaler to read
  */
  if(self->eMode == eTimer){
    scaler = 0;
  }else {
    scaler = pPriv->control;
  }

  readScaler(pPriv,scaler,&count);
  /*
    ignore errors on this one
  */
  *fControl = (float)count;

  return result;
}
/*=====================================================================*/
static int clearScalers(pECBCounter self){
  int status;
  Z80_reg in, out;

  status = ecbExecute(self->ecb,STCLEA,in,&out);
  if(status != 1){
    return COMMERROR;
  }
  return 1;
}
/*----------------------------------------------------------------------*/
static int loadPrescalers(pECBCounter self){
  Z80_reg in, out;
  int status, i;

  for(i = 0; i < 8; i++){
    in.c = (unsigned char)i;
    in.d = self->prescaler[i];
    status = ecbExecute(self->ecb,PRELOA,in,&out);
    if(status != 1){
      return COMMERROR;
    }
  }
  return 1;
}
/*----------------------------------------------------------------------*/
static int loadPreset(pECBCounter self, int preset, unsigned char control){
  Z80_reg in, out;
  Ecb_pack data;
  int status, i;

  data.result = preset;
  
  in.c = data.b.byt3;
  in.b = data.b.byt2;
  in.e = data.b.byt1;
  in.d = data.b.byt0;
  status = ecbExecute(self->ecb,STLOAD,in,&out);
  if(status != 1){
    return COMMERROR;
  }

  in.b = data.b.byt2;
  in.e = data.b.byt1;
  in.d = data.b.byt0;
  in.c = 4*control;
  status = ecbExecute(self->ecb,STCPRE,in,&out);
  if(status != 1){
    return COMMERROR;
  }
  return 1;
}
/*-----------------------------------------------------------------------*/
static int ECBStart(struct __COUNTER *self){
  pECBCounter pPriv = NULL;
  int preset, status, controlUnit;
  Z80_reg in, out;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  /*
    check if the preset is permissible
  */
  preset = (int)rint(self->fPreset);
  if(preset > MAX_COUNT){
    self->iErrorCode = TOMANYCOUNTS;
    return HWFault;
  }
  if(self->eMode == eTimer){
    controlUnit = 0;
    preset *= pPriv->tfreq; 
    if(preset > MAX_COUNT){
      self->iErrorCode = TOMANYCOUNTS;
      return HWFault;
    }
  } else {
    controlUnit = pPriv->control;
  }

  if((status = stopScalers(pPriv)) <= 0){
    self->iErrorCode = status;
    return HWFault;
  }

  if((status = clearScalers(pPriv)) <= 0){
    self->iErrorCode = status;
    return HWFault;
  }

  if((status = loadPrescalers(pPriv)) <= 0){
    self->iErrorCode = status;
    return HWFault;
  }

  if((status = loadPreset(pPriv, preset,(unsigned char)controlUnit)) <= 0){
    self->iErrorCode = status;
    return HWFault;
  }

  status = ecbExecute(pPriv->ecb,STARTS,in,&out);
  if(status != 1){
    self->iErrorCode = status;
    return HWFault;
  }
  
  pPriv->state = COUNT;
  return OKOK;
}
/*=======================================================================*/
static int ECBHalt(struct __COUNTER *self){
  int status;
  pECBCounter pPriv = NULL;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  pPriv->state = IDLE;
  if((status = stopScalers(pPriv)) <= 0){
    self->iErrorCode = status;
    return HWFault;
  }
  return OKOK;
}
/*=======================================================================*/
static int ECBTransfer(struct __COUNTER *self){
  int status, count, i;
  pECBCounter pPriv = NULL;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  /*
    read time
  */
  status = readScaler(pPriv,0,&count);
  if(status <= 0){
    self->iErrorCode = COMMERROR;
    return HWFault;
  }
  self->fTime = (float)count;

  /*
    read other scalers
  */
  for(i = 1; i < 8; i++){
    status = readScaler(pPriv,i,&count);
    if(status <= 0){
      self->iErrorCode = COMMERROR;
      return HWFault;
    }
    self->lCounts[i-1] = count;
  }
  return OKOK;
}
/*======================================================================*/
static int ECBGetError(struct __COUNTER *self, int *iCode,
		       char *errorText, int errlen){
  char pBueffel[132];

  *iCode = self->iErrorCode;
  switch(self->iErrorCode){
  case COMMERROR:
    strncpy(errorText,"Communication error with ECB",errlen);
    break;
  case TOMANYCOUNTS:
    strncpy(errorText,"Preset is to high!",errlen);
    break;
  case NOSEND:
    strncpy(errorText,"Cannot send naked data to ECB",errlen);
    break;
  case UNKNOWNPAR:
    strncpy(errorText,"parameter unknown",errlen);
    break;
  case INVALIDCOUNTER:
    strncpy(errorText,"Invalid counter number requested, 0-7 allowed",
	    errlen);
    break;
  case INVALIDPRESCALER:
    strncpy(errorText,"Invalid prescaler value, allowed 1 or 10",
	    errlen);
    break;
  case BADFREQ:
    strncpy(errorText,"Bad timer frequency: 10 or 1000 allowed",errlen);
    break;
  default:
    sprintf(pBueffel,"Unknown error code %d", self->iErrorCode);
    strncpy(errorText,pBueffel,errlen);
    break;
  }
  return 1;
}
/*=======================================================================*/
static int ECBFixIt(struct __COUNTER *self, int iCode){
  return COTERM;
}
/*======================================================================*/

/*******************************************************************************
* Load the parameters 'dot' and 'divide' for a motor or an encoder.
* 'dot' specifies the placement of a punctuation mark on the display
* of f.ex a motor position. 'divide' specifies how many times the po-
* sition is to be divided by two before it is displayed.
******************************************************************************/
static void
Dot_divide (int device, int data, pECB ecb)
{
  int function, dot, divide;
  Z80_reg x_inreg, out;

  if (data == 0)			/* If zero, dont send dot/divide) */
    return;

  dot = 0;
  while ((data%10) == 0)
    {
      dot++;
      data /= 10;
    }
  divide = 0;
  while ((data%2) == 0)
    {
      divide++;
      data /= 2;
    }
  if (data != 1)			/* If != 1, not a binary No. */
    return;
  if (dot > 0)
    dot = 8 - dot;
  x_inreg.c = 0;				/* Specify input */
  x_inreg.b = (unsigned char) device;
  x_inreg.d = (unsigned char) dot;		/* Dot position */
  x_inreg.e = (unsigned char) divide;		/* No. of times to divide by 2 */

  ecbExecute(ecb,170,x_inreg,&out);
  return;
}
/*-----------------------------------------------------------------------*/
static int ECBSet(struct __COUNTER *self, char *name, 
		  int iCter, float fVal){
  pECBCounter pPriv = NULL;
  int iVal;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  iVal = (int)rint(fVal);

  if(strcmp(name,"prescaler") == 0){
    if(iCter < 0 || iCter > 7){
      self->iErrorCode = INVALIDCOUNTER;
      return HWFault;
    }
    if(iVal != 1 && iVal != 10){
      self->iErrorCode = INVALIDPRESCALER;
      return HWFault;
    }
    pPriv->prescaler[iCter] = (unsigned char)iVal;
    return OKOK;
  } else if(strcmp(name,"tfreq") == 0){
    if(fVal == 1000){
      pPriv->prescaler[0] = 1;
      pPriv->tfreq = 1000;
      Dot_divide(64,1000,pPriv->ecb);
      return OKOK;
    } else if(fVal == 10){
      pPriv->tfreq = 10;
      pPriv->prescaler[0] = 10;
      Dot_divide(64,10,pPriv->ecb);
      return OKOK;
    } else {
      self->iErrorCode = BADFREQ;
      return HWFault;
    }
  } else {
    self->iErrorCode = UNKNOWNPAR;
    return HWFault;
  }
}
/*===================================================================*/
static int ECBGet(struct __COUNTER *self, char *name, 
		  int iCter, float *fVal){
  pECBCounter pPriv = NULL;

  assert(self);
  pPriv = (pECBCounter)self->pData;
  assert(pPriv);

  if(strcmp(name,"prescaler") == 0){
    *fVal = (float)pPriv->prescaler[iCter];
    return OKOK;
  } else if(strcmp(name,"tfreq") == 0){
    *fVal = (float)pPriv->tfreq;
    return OKOK;
  } else{
    self->iErrorCode = UNKNOWNPAR;
    return HWFault;
  }
} 
/*=====================================================================*/
static int ECBSend(struct __COUNTER *self, char *text, 
		   char *reply, int replylen){
  strncpy(reply,"ECB does not feast on ASCII strings, refused!",
	  replylen);
  return OKOK;
}
/*====================================================================*/
pCounterDriver MakeECBCounter(char *ecb){
  pECBCounter pPriv = NULL;
  pCounterDriver self = NULL;
  int i;

  /*
    memory for everybody
  */
  self = CreateCounterDriver("ecb","ecb");
  pPriv = (pECBCounter)malloc(sizeof(ECBCounter));
  if(self == NULL || pPriv == NULL){
    return NULL;
  }
  memset(pPriv,0,sizeof(ECBCounter));

  /*
    initialize private data structure
  */
  pPriv->ecb = (pECB)FindCommandData(pServ->pSics,ecb,"ECB");
  if(pPriv->ecb == NULL){
    DeleteCounterDriver(self);
    free(pPriv);
    return NULL;
  }
  for(i = 0; i < 8; i++){
    pPriv->prescaler[i] = 1;
  }
  pPriv->tfreq = 1000;
  pPriv->control = 1;


  /*
    assign function pointers
  */
  self->GetStatus   = ECBGetStatus;
  self->Start       = ECBStart;
  self->Pause       = ECBPause;
  self->Continue    = ECBContinue;
  self->Halt        = ECBHalt;
  self->ReadValues  = ECBTransfer;
  self->GetError    = ECBGetError;
  self->TryAndFixIt = ECBFixIt;
  self->Set         = ECBSet;
  self->Get         = ECBGet;
  self->Send        = ECBSend;
  self->KillPrivate = NULL;
  self->iNoOfMonitors = 8;

  self->pData = pPriv;
  return self;
}
