/*---------------------------------------------------------------------------
ipsdriv.c

Driver for the Oxford Instruments IPS/PS magnet power supply. 

Markus Zolliker, Sept 2004
----------------------------------------------------------------------------*/
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <sys/time.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 "oicom.h"
#include "fsm.h"

typedef struct {
  Eve eve;
  float current; /* current (in Tesla) */
  float persfield; /* persistent field from IPS (in Tesla) */
  float lastfield; /* persistent field from last drive */
  float ramp; /* actual ramp rate (Telsa/min) */
  int persmode; /* 0: leave switch on, 1: go to persistant mode */
  int perswitch; /* state of switch */
  int remote; /* 0: local, 1: remote, do not check, 2: remote, check */
  int heaterFault;
  int force; /* force = 2: put heater switch even when stored field does not match */
  time_t swtim; /* time when last switching the heater */
  time_t tim;
} IpsDriv;

/*----------------------------------------------------------------------------*/
static int IpsOk(IpsDriv *me, SConnection *pCon) {
  float dif;
  Eve *eve=&me->eve;
  
  if (!pCon) pCon = SCLoad(&eve->evc->conn);
  if (me->perswitch) return 1;
  if (fabs(me->persfield - me->lastfield) < 1e-5) {
    return 1;
  }
  if (me->force == 2) {
    return 1;
  }
  if (fabs(me->persfield - me->lastfield) < 1e-5
      || me->force == 2) return 1;
  eve->errCode = EVE_FAULT;
  SCPrintf(pCon, eWarning,
        "\nit is not sure which field is in the magnet\n"
        "value stored in power supply: %f\n"
        "                 in software: %f\n"
        "use command\n \n  %s confirm ...\n \n"
        "to specify the persistent field\n \n"
        , me->persfield, me->lastfield, eve->evc->pName);
  me->force = 0;
  return 0;
}
/*----------------------------------------------------------------------------*/
#define A EVE_ACTPAR
#define L EVE_LOGPAR
#define S EVE_SAVEPAR

void IpsPars(IpsDriv *me, EveParArg *arg) {
  IpsOk(me, EveArgConn(arg));
  EveIntPar(arg, "persmode", &me->persmode, usUser, A+S);
  EveIntPar(arg, "perswitch", &me->perswitch, usInternal, A);
  EveObPar(arg, UPLIMIT, "%.5g\tT", A+S);
  EveFloatPar(arg, "ramp", &me->ramp, "%.5g\tT/min", usUser, A+S);
  EveStdParEnd(arg, "%.5g\tT", A+L+S);
}
/*----------------------------------------------------------------------------*/
void IpsStatus(IpsDriv *me) {
  char *ans;
  int *code;
  Eve *eve=&me->eve;
  int swi;
  
  if (eve->state != readState) return;
  ans=eve->ans;
  code=&eve->errCode;
  if (ans[0] != 'X' ||
      ans[3] != 'A' ||
      ans[5] != 'C' ||
      ans[7] != 'H' ||
      ans[9] != 'M') {
    EvePrintf(eve, eError, "illegal status response");
    *code = EVE_FAULT;
    return;
  }
  switch (ans[1]) {
  case '0': break;
  case '1': EvePrintf(eve, eError, "magnet quenched"); *code = EVE_FAULT; return;
  case '2': EvePrintf(eve, eError, "IPS overheated"); *code = EVE_FAULT; return;
  case '4': EvePrintf(eve, eError, "IPS warming up"); *code = EVE_FAULT; return;
  case '8': EvePrintf(eve, eError, "IPS fault"); *code = EVE_FAULT; return;
  default:
    EvePrintf(eve, eError, "illegal status response");
    *code = EVE_FAULT;
    return;
  }
  if (ans[6] != '3' && me->remote == 2) {
    EvePrintf(eve, eError, "IPS switched to local");
    *code = EVE_FAULT;
    me->remote = 1;
    return;
  }
  if (ans[8] == '5') {
    me->heaterFault = 1;
    return;
  }
  if (ans[8] == '1') {
    swi = 1;
  } else {
    swi = 0;
  }
  if (swi != me->perswitch || me->swtim == 0) {
    me->swtim = time(NULL);
    me->perswitch = swi;
  }
}
/*----------------------------------------------------------------------------*/
static void IpsSetField(IpsDriv *me, float val) {
  char buf[128];
  if (me->eve.value != val && me->eve.evc != NULL) {
    snprintf(buf, sizeof(buf), "%s = %g", me->eve.evc->pName, val);
    InvokeCallBack(me->eve.evc->pCall, VALUECHANGE, buf);
  }
  if (me->perswitch) {
    me->current = val;
    me->lastfield = val;
    me->persfield = val;
  } else if (me->force == 2) {
    me->lastfield = val;
  } else {
    me->persfield = val;
  }
  me->eve.value = val;
}
/*----------------------------------------------------------------------------*/
static int IpsRead(long pc, IpsDriv *me) {
  Eve *eve=&me->eve;
  
  FSM_BEGIN
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me); /* check for errors and get perswitch */
    if (!me->remote) goto rd;
    EveWrite(eve, "C0");
    me->remote = 0;
  FSM_NEXT
  
  rd:
    EveWrite(eve, "R7"); /* read current (in Tesla) */
  FSM_NEXT
    me->current = OiGetFlt(eve, 3, NULL);
    if (me->perswitch) {
      IpsSetField(me, me->current);
      goto quit;
    }
    EveWrite(eve, "R18"); /* read persistant field (in Tesla) */
  FSM_NEXT
    IpsSetField(me, OiGetFlt(eve, 3, NULL));
    
  quit:
  FSM_END
}
/*----------------------------------------------------------------------------*/
static int IpsStart(long pc, IpsDriv *me) {
  Eve *eve=&me->eve;
  
  FSM_BEGIN
    EveWrite(eve, "V");
  FSM_NEXT
    if (0 == strncmp(eve->version, "IPS120", 6)) {
      me->eve.syntax = 1;
    } else if (0 == strncmp(eve->version, "PS", 2)) {
      me->eve.syntax = 0;
    } else {
      EvePrintf(eve, eError, "unknown power supply version: %s", eve->version);
      goto quit;
    }
    EvePrintf(eve, eStatus, "connected to %s", eve->version);
  FSM_CALL(IpsRead);
    
  quit:
  FSM_END
}
/*----------------------------------------------------------------------------*/
static int IpsChangeField(long pc, IpsDriv *me) {
  Eve *eve=&me->eve;
  pEVControl evc=eve->evc;
  float fld;
  float step;
  float ramp;
  time_t delay;
  
  FSM_BEGIN
    EveWrite(eve, "C3");
    me->remote = 1;
  FSM_NEXT
    EveWrite(eve, "F7"); /* switch to tesla on display */
  FSM_NEXT
    EveWrite(eve, "A0"); /* hold */
  FSM_NEXT
  FSM_CALL(IpsRead);
    me->remote = 2;
    if (!IpsOk(me, NULL)) goto finish;
    if (fabs(evc->fTarget - me->lastfield) < 1e-5) {
      EvePrintf(eve, -1, "IPS: we are already at field %f", me->lastfield);
      if (me->persmode) {
        if (!me->perswitch) goto finish;
        goto target_reached;
      } else {
        if (me->perswitch) goto finish;
      }
    }
    if (fabs(me->current - me->lastfield) < 1e-5) {
      goto switch_on;
    }
    OiSet(eve, "J", me->lastfield, 3); /* set point */
  FSM_NEXT
    EveWrite(eve, "A1");
    EvePrintf(eve, -1, "IPS: ramp to current for %f Tesla", me->lastfield);
  FSM_NEXT
    me->tim = time(NULL);
    
  stab1:
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me); /* just check for errors */
    EveWrite(eve, "R7"); /* read current (in Tesla) */
  FSM_NEXT
    me->current = OiGetFlt(eve, 3, NULL);
    if (fabs(me->current - me->lastfield) > 1e-5) goto stab1;
    
  stab2:
  FSM_WAIT(1)
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me);
    if (time(NULL) < me->tim + 3) goto stab2; /* stabilize */
    
  switch_on:
    if (me->perswitch) goto wait_open;
    if (me->force == 2) {
      EveWrite(eve, "H2");
    } else {
      EveWrite(eve, "H1");
    }
    me->force = 0;
  FSM_NEXT
    me->perswitch = 1;
    me->swtim = time(NULL);
    
  wait_open:
    delay = me->swtim + 30 - time(NULL);
    if (delay > 0)
      EvePrintf(eve, -1, "IPS: wait %d sec to open switch", delay);
    
  start_ramp:
  FSM_WAIT(1)
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me); /* check for errors */
    if (me->heaterFault) {
      if (time(NULL) > me->swtim + 3) {
        EvePrintf(eve, eError, "IPS heater fault");
        eve->errCode = EVE_FAULT;
        goto off_finish;
      }
      me->heaterFault = 0;
    }
    if (time(NULL) < me->swtim + 30) goto start_ramp; /* wait */
    OiSet(eve, "T", me->ramp, 3);
  FSM_NEXT
    OiSet(eve, "J", me->current, 3); /* put set point to actual value */
  FSM_NEXT
    EveWrite(eve, "A1"); /* go to setpoint (do not yet run) */ 
  FSM_NEXT
    EvePrintf(eve, -1, "IPS: ramp to %f Tesla", evc->fTarget);
    
  ramping:
  FSM_WAIT(1)
    EveWrite(eve, "R7"); /* read "current" in Tesla */
  FSM_NEXT
    IpsSetField(me, OiGetFlt(eve, 3, NULL)); /* set me->current and callback */
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me); /* just check for errors */
    EveWrite(eve, "R9"); /* read back ramp rate (may be sweep limited) */
  FSM_NEXT
    ramp=OiGetFlt(eve, 3, NULL);
    step=ramp/20; /* step = ramp * 3sec */
    if (step < 0.001) step=0.001;
    if (evc->fTarget > me->current + step) {
      fld=me->current + step;
    } else if (evc->fTarget < me->current - step) {
      fld=me->current - step;
    } else {
      fld=evc->fTarget;
      if (fabs(me->current - evc->fTarget) < 1e-5) goto target_reached;
    }
    OiSet(eve, "J", fld, 3);
  FSM_NEXT
    goto ramping;
    
  target_reached:
    eve->hwstate = HWIdle;
    EveWrite(eve, "A0"); /* hold */
  FSM_NEXT
    evc->eMode = EVMonitor; /* we are at field, drive has finished */
    if (!me->persmode) goto hold_finish;
    /* but we continue in the background */
    me->tim = time(NULL);
  
  stab3:
  FSM_WAIT(1)
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me); /* just check for errors */
    if (time(NULL) < me->tim + 3) goto stab3; /* stabilize */
    
    EveWrite(eve, "H0");
    me->perswitch = 0;
    me->swtim = time(NULL);
    me->lastfield = me->current;
  FSM_NEXT
    EvePrintf(eve, -1, "IPS: wait 30 sec to close switch");
    
  wait_closed:
  FSM_WAIT(1)
    EveWrite(eve, "X");
  FSM_NEXT
    IpsStatus(me);
    if (time(NULL) < me->swtim + 30) goto wait_closed; /* wait */
  
    if (me->current == 0) goto finish;
    EveWrite(eve, "A2"); /* goto zero */
    EvePrintf(eve, -1, "IPS: ramp current to 0");
  FSM_NEXT
    goto finish;

  hold_finish:
    EveWrite(eve, "A0");
  FSM_NEXT
    goto finish;
    
  off_finish:
    if (me->perswitch) {
      me->lastfield = me->current;
      me->swtim = time(NULL);
    }
    EveWrite(eve, "H0");
  FSM_NEXT
  
  finish:
    EveWrite(eve, "C0");
    me->remote = 0;
    eve->hwstate = HWIdle;
  FSM_NEXT
    
  FSM_END
}
/*----------------------------------------------------------------------------*/
int IpsConfirm(SConnection *pCon, pEVControl evc, int argc, char *argv[]) {
  float fld;
  IpsDriv *me;
  
  me=evc->pDriv->pPrivate; assert(me);
  
  if (argc > 1) {
    if (argc > 2) {
      SCPrintf(pCon, eError, "Too many arguments");
      return -1;
    }
    fld=atof(argv[1]);
    if (fld < ObVal(evc->pParam, UPLIMIT)) {
      SCPrintf(pCon, eError, "Field outside limit");
      return -1;
    }
    if (me->perswitch) {
      SCPrintf(pCon, eWarning, "switch heater is on - field is %f", me->current);
      return -1;
    }
    if (fabs(fld - me->persfield) > 1e-5 && fabs(fld - me->lastfield) > 1e-5) {
      SCPrintf(pCon, eWarning, "Be aware that this does neither match the field"
        " stored in software\nnor the field stored in power supply.");
      me->force = 0;
    }
    if (me->force == 0) {
      SCPrintf(pCon, eWarning, "Please repeat this command, to confirm again"
        " the persistent field of\n    %f Tesla.", fld);
      IpsSetField(me, fld);
      me->force=1;
    } else {
      me->force=2;
      IpsSetField(me, fld);
      SCPrintf(pCon, eValue, "%s.%s = %f", evc->pName, argv[1], fld);
    }
  } else {
    SCPrintf(pCon, eValue, "%s.%s = %f", evc->pName, argv[1], me->eve.value);
  }
  return 1;
}
/*------------------------------------------------------------------------*/
static int IpsIsOk(void *data) {
  /* always in tolerance (here we may implement what to do in case of a quench) */ 
  return 1;
}
/*------------------------------------------------------------------------*/
pEVControl IpsMakeEVC(SConnection *pCon, int argc, char *argv[]) {
  /* args:
     <objectname> ips <rs232>
     <objectname> ips <host> <port>
   */ 
  IpsDriv *me;
  Eve *eve;
  pEVControl evc;  

  evc = MakeEveEVC(argc, argv, calloc(1, sizeof *me), pCon);
  if (!evc) return NULL;
  
  me = evc->pDriv->pPrivate;
  eve=&me->eve;
  
  me->current = 0;
  me->persfield = 0;
  me->lastfield = 0;
  me->persmode = 1;
  me->perswitch = 0;
  me->force = 0;
  me->tim = 0;
  me->swtim = 0;
  me->ramp=0.1;
  
  eve->run = (FsmFunc)IpsChangeField;
  eve->read = (FsmFunc)IpsRead;
  eve->pardef = (EveParDef)IpsPars;
  eve->todo = (FsmFunc)IpsStart;
  eve->task = FsmStartTask(me, (FsmHandler)OiHandler, (FsmFunc)EveIdle);
  
  evc->pEnvir->IsInTolerance = IpsIsOk;
  EVCSetPar(evc,"upperlimit",14.9,pCon);
  EVCSetPar(evc,"lowerlimit",-14.9,pCon);
  
  return evc;
}