/*---------------------------------------------------------------------------
linadriv.c

Driver for the Lock IN Amplifier SIGNAL RECOVERY Model 7265

Markus Zolliker, Oct 2006
----------------------------------------------------------------------------*/
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <sys/time.h>
#include <math.h>
#include <tcl.h>
#include <fortify.h>
#include <sics.h>
#include <splitter.h>
#include <obpar.h>
#include <devexec.h>
#include <nserver.h>
#include <interrupt.h>
#include <emon.h>
#include <evcontroller.h>
#include <evcontroller.i>
#include <servlog.h>
#include <sicsvar.h>
#include <evdriver.i>
#include <rs232controller.h>
#include "fsm.h"
#include "ease.h"
#include "initializer.h"

#define LINA_AMP 1
#define LINA_FREQ 2

typedef struct {
  EaseDriv d;
  float x, y;
  float amp, ampGet;
  float freq, freqGet;
} Lina;

static ParClass linaClass = { "LINA", sizeof(Lina) };

/*----------------------------------------------------------------------------*/
int LinaHandler(void *object)
{
  int iret, l;
  EaseBase *eab = EaseBaseCast(object);
  char *corr;

  if (eab->state < EASE_idle)
    goto quit;
  if (availableNetRS232(eab->ser) || availableRS232(eab->ser)) {
    eab->msg[0] = '\0';
    l = sizeof(eab->ans);
    iret = readRS232TillTerm(eab->ser, eab->ans, &l);
    if (eab->state != EASE_expect && eab->state != EASE_lost) {
      if (iret == 1) {
        ParPrintf(eab, eError, "unexpected answer: %s", eab->ans);
      }
      goto quit;
    }
    if (iret == 1) {
      ParPrintf(eab, -2, "ans: %s", eab->ans);
      if (eab->state == EASE_lost) {
        if (strcmp(eab->ans, "6,0") == 0) {
          EaseWrite(eab, "ID");
        }
        goto quit;
      } else if (strncmp(eab->cmd, "ID", 2) == 0) {
        if (strcmp(eab->ans, eab->version) == 0) {
          /* we are still connected with the same device */
        } else if (*eab->version == '\0' && strcmp(eab->ans, "7265") == 0) {
          strlcpy(eab->version, eab->ans, sizeof(eab->version));
        } else {                /* version (and therefore device) changed */
          eab->errCode = EASE_DEV_CHANGED;
          eab->state = EASE_idle;
          goto error;
        }
        eab->state = EASE_idle;
        goto quit;
      } else {
        eab->tmo = 10;
      }
    }
    if (iret != 1) {
      ParPrintf(eab, -2, "ans: %s?", eab->ans);
      eab->errCode = iret;
      eab->state = EASE_idle;
      goto error;
    }
    eab->state = EASE_read;
  } else if (eab->state == EASE_expect) {
    if (time(NULL) > eab->cmdtime + eab->tmo) {
      eab->state = EASE_lost;
    }
  } else if (eab->state == EASE_lost) {
    if (time(NULL) > eab->cmdtime) {
      EaseWrite(eab, "RS");
    }
  }
  goto quit;
error:
  /* EaseWriteError(eab); */
quit:
  return EaseHandler(eab);
}

/*----------------------------------------------------------------------------*/
static void LinaParDef(void *object)
{
  Lina *drv = ParCast(&linaClass, object);
  EaseBase *eab = object;

  ParName("");
  ParTail("units");
  ParFloat(&drv->x, PAR_NAN);

  ParName("x");
  ParTail("");
  ParFloat(&drv->x, PAR_NAN);

  ParName("y");
  ParTail("");
  ParFloat(&drv->y, PAR_NAN);

  ParName("ampGet");
  ParTail("");
  ParFloat(&drv->ampGet, PAR_NAN);

  ParName("freqGet");
  ParTail("");
  ParFloat(&drv->freqGet, PAR_NAN);

  ParName("amp");
  ParTail("V");
  EaseUpdate(LINA_AMP);
  ParFloat(&drv->amp, PAR_NAN);

  ParName("freq");
  ParTail("Hz");
  EaseUpdate(LINA_FREQ);
  ParFloat(&drv->freq, PAR_NAN);

  EaseBasePar(drv);
  EaseSendPar(drv);
/*
  EaseDrivPar(drv, "%.5g", "K");
*/
  ParStdDef();
  EaseMsgPar(drv);
}

/*----------------------------------------------------------------------------*/
static long LinaSetAmp(long pc, void *object) {
  Lina *drv = ParCast(&linaClass, object);
  EaseBase *eab = object;
  char buf[64];
  
  switch (pc) {
  default:
    snprintf(buf, sizeof buf, "OA.%g", drv->amp);
    EaseWrite(eab, buf);
    return __LINE__;
  case __LINE__:                 /**********************************/
    return 0;
  }
}
/*----------------------------------------------------------------------------*/
static long LinaSetFreq(long pc, void *object) {
  Lina *drv = ParCast(&linaClass, object);
  EaseBase *eab = object;
  char buf[64];

  switch (pc) {
  default:
    snprintf(buf, sizeof buf, "OF.%g", drv->freq);
    EaseWrite(eab, buf);
    return __LINE__;
  case __LINE__:                 /**********************************/
    return 0;
  }
}
/*----------------------------------------------------------------------------*/
static long LinaRead(long pc, void *object)
{
  Lina *drv = ParCast(&linaClass, object);
  EaseBase *eab = object;
  char *p, *q;

  switch (pc) {
  default:                     /* FSM BEGIN ****************************** */
    EasePchk(drv);
    EaseWrite(eab, "X.");
    return __LINE__;
  case __LINE__:                 /**********************************/
    if (1 != sscanf(eab->ans, "%f", &drv->x)) {
      ParPrintf(drv, eError, "illegal response %s", eab->ans);
    }
    EaseWrite(eab, "Y.");
    return __LINE__;
  case __LINE__:                 /**********************************/
    if (1 != sscanf(eab->ans, "%f", &drv->y)) {
      ParPrintf(drv, eError, "illegal response %s", eab->ans);
    }
    EaseWrite(eab, "OA.");
    return __LINE__;
  case __LINE__:                 /**********************************/
    if (1 != sscanf(eab->ans, "%f", &drv->ampGet)) {
      ParPrintf(drv, eError, "illegal response %s", eab->ans);
    }
    EaseWrite(eab, "OF.");
    return __LINE__;
  case __LINE__:                 /**********************************/
    if (1 != sscanf(eab->ans, "%f", &drv->freqGet)) {
      ParPrintf(drv, eError, "illegal response %s", eab->ans);
    }
    if (EaseGetUpdate(drv, LINA_FREQ)) {
      EaseSetUpdate(drv, LINA_FREQ, 0);
      FsmCall(LinaSetFreq);
    } else if (EaseGetUpdate(drv, LINA_AMP)) {
      EaseSetUpdate(drv, LINA_AMP, 0);
      FsmCall(LinaSetAmp);
    }
    ParLog(drv);
  fsm_quit:return 0;
  }                             /* FSM END ******************************** */
}

/*----------------------------------------------------------------------------*/
static long LinaStart(long pc, void *object)
{
  Lina *drv = ParCast(&linaClass, object);
  EaseBase *eab = object;
  char msg[256];
  
  switch (pc) {
  default:                     /* FSM BEGIN ****************************** */
    EasePchk(drv);
    EaseWrite(eab, "ID");
    return __LINE__;
  case __LINE__:                 /**********************************/
    if (0 != strncmp(eab->version, "7265", 4)) {
      snprintf(msg, sizeof msg,
               "unknown lock in amplifier version: %s", eab->version);
      EaseStop(eab, msg);
      goto quit;
    }
    ParPrintf(drv, eLog, "connected to %s", eab->version);
    eab->p.period = 1;
    FsmCall(LinaRead);
    return __LINE__;
  case __LINE__:                 /**********************************/

  quit:
    return 0;
  }                             /* FSM END ******************************************* */
}

/*----------------------------------------------------------------------------*/
static long LinaSet(long pc, void *object)
{
  Lina *drv = ParCast(&linaClass, object);
  EaseBase *eab = object;
  char cmd[32];
  int upd;

  switch (pc) {
  default:                     /* FSM BEGIN ****************************** */
    EasePchk(drv);
    /*
       snprintf(cmd, sizeof cmd, "SETP %.5g;SETP?", drv->d.targetValue);
       EaseWrite(eab, cmd);
     */
  quit:
    return 0;
  }                             /* FSM END ******************************************* */
}

/*----------------------------------------------------------------------------*/
static int LinaInit(SConnection * con, int argc, char *argv[], int dynamic)
{
  /* args:
     MakeObject objectname lina <rs232>
     MakeObject objectname lina <host> <port>
   */
  Lina *drv;

  drv = EaseMakeDriv(con, &linaClass, argc, argv, dynamic, 7,
                     LinaParDef, LinaHandler, LinaStart, NULL, LinaRead,
                     LinaSet);
  if (drv == NULL)
    return 0;
/*
  setRS232ReplyTerminator(drv->d.b.ser,"\n");
  setRS232SendTerminator(drv->d.b.ser,"\n");
*/
  return 1;
}

/*----------------------------------------------------------------------------*/
void LinaStartup(void)
{
  ParMakeClass(&linaClass, EaseDrivClass());
  MakeDriver("LINA", LinaInit, 0,
             "Lock in amplifier SIGNAL RECOVERY Model 7265");
}
