/*--------------------------------------------------------------------------
  This is one implementation file for the TASMAD simulation module for
  SICS. The requirement is to make SICS look as much as TASMAD as 
  possible. This includes:
  - TASMAD is variable driven
  - Sometimes variables are accessed in storage order.
  - A complicated calculation has to be done for getting the instruments
    settings right. The appropriate F77 routine from TASMAD will be 
    reused.
  - The scan logic is different.
  - Output of ILL-formatted data files is required. 

  This file implements the MAD dr command for driving.

  Mark Koennecke, November 2000

  Polarisation support added.

  Mark Koennecke, April 2002
---------------------------------------------------------------------------*/
#include <stdlib.h>
#include <assert.h>
#include <fortify.h>
#include <sics.h>
#include <sicsvar.h>
#include <counter.h>
#include <motor.h>
#include <scan.h>
#include <splitter.h>
#include "tas.h"
#include "tasu.h"


/* a token break function, implemented in stptok.c */
extern char *stptok(const char *s, char *tok, size_t toklen, char *brk);

#define VAR 1
#define VALUE 2

/*----------------------------------------------------------------------
  TASDrive has to do an interesting parsing job: The normal syntax is
  par=val. However, it is possible that the we get par = val, val, val 
  which means that the motors following par in storage order are
  driven. Additionally we have to check for special energy or Q variables
  which require a triple axis calculation.
  
  M.Z. June 07: storage order mode only allowed for QH,QK,QL,EN.

  It helps if one understands some fundamental things used in the code 
  below:
  - motorMask holds a value for each motor in the motor list. The
    value can be either 0 for not to drive or 1 for drive. After
    successfull parsing these motors will be started. The mask will be built 
    during parsing. 
  - newPositions holds the new positions for the normal motors.
  - tasMask is a mask which indicates which triple axis special variable
    (Energy or Q) is driven.
  - tasTargetMask will be set by the TAS calculation and will indicate
    which motors of the range A1-A6, curvature and currents need to be
    driven. 
  - tasTargets holds after the TAS calculation the target values for the
    A1-A6, curvature and currents  motors.


-------------------------------------------------------------------------*/
#define NUM 1
#define TXT 2

int TASDrive(SConnection * pCon, SicsInterp * pSics, void *pData,
             int argc, char *argv[])
{
  pTASdata self = NULL;
  int iTAS = 0;
  float tasTargets[20], oldPos, oldEnergy[MAXEVAR];
  unsigned char tasTargetMask[20], tasMask[MAXEVAR];
  char *pPtr, pToken[20], pLine[256];
  int varPointer, i, motorPointer, status, rStatus, lastToken;
  char pBueffel[256];
  unsigned char motorMask[MAXMOT];
  float newPositions[MAXMOT];
  pMotor pMot;
  int qhPars;


  assert(pCon);
  assert(pSics);
  self = (pTASdata) pData;
  assert(self);

  /* 
     check authorization
   */
  if (!SCMatchRights(pCon, usUser))
    return 0;


  /* Initialize */
  Arg2Text(argc, argv, pLine, 255);
  strtolower(pLine);
  lastToken = NUM;
  pPtr = pLine + strlen(argv[0]);       /* step over command */
  for (i = 0; i < 10; i++) {
    tasMask[i] = 0;
    motorMask[i] = 0;
    motorMask[10 + i] = 0;
    tasTargets[i] = .0;
    tasTargets[i + 10] = .0;
    oldEnergy[i] = .0;
  }
  tasMask[10] = 0;
  tasMask[11] = 0;

  for (i = 0; i < MAXMOT; i++) {
    motorMask[i] = 0;
  }
  varPointer = -1;
  motorPointer = -1;
  rStatus = 1;
  qhPars = 0;

  /* parse loop */
  while (pPtr != NULL) {
    pPtr = stptok(pPtr, pToken, 20, " ,=");
    if (strlen(pToken) < 1 || pPtr == NULL)
      continue;

    if (tasNumeric(pToken)) {   /* numbers */
      if (lastToken == NUM) {
        /* handle storage order logic */
        if (varPointer > -1 && qhPars > 1) {
          varPointer++;
          qhPars--;
        } else {
          sprintf(pBueffel, "ERROR: parse error at %s, %s",
                  pToken, "need parameter to drive");
          SCWrite(pCon, pBueffel, eError);
          return 0;
        }
      }
      /* enter the parameter to drive into the appropriate mask */
      if (motorPointer >= 0) {
        motorMask[motorPointer] = 1;
        newPositions[motorPointer] = atof(pToken);
      } else if (varPointer >= 0) {
        tasMask[varPointer] = 1;
        oldEnergy[varPointer] = self->tasPar[EMIN + varPointer]->fVal;
        self->tasPar[EMIN + varPointer]->fVal = atof(pToken);
        self->tasPar[ETARGET + varPointer]->fVal = atof(pToken);
      } else {
        sprintf(pBueffel, "ERROR: parse error at %s, %s",
                pToken, "need parameter to drive");
        SCWrite(pCon, pBueffel, eError);
        return 0;
      }
      lastToken = NUM;
    } else {                    /* text tokens */

      lastToken = TXT;
      if ((status = isTASEnergy(pToken)) > -1) {        /* Ei, KI, EF, KF, Q.... */
        iTAS = 1;
        motorPointer = -1;
        varPointer = status;
        if (EMIN + varPointer == QH) {
          qhPars = 4;
        }
      } else if ((status = isTASMotor(pToken)) > -1) {
        motorPointer = status;
        varPointer = -1;
      } else {
        sprintf(pBueffel, "ERROR: cannot drive %s", pToken);
        SCWrite(pCon, pBueffel, eError);
        rStatus = 0;
        break;
      }
    }
  }                             /* end of parse loop */

  if (rStatus != 1)
    return rStatus;

  /*
     store SRO motor value
   */
  getSRO(pCon, &self->oldSRO);


  /* having done this, we can start the motors */
  for (i = 0; i < MAXMOT; i++) {
    if (motorMask[i] > 0) {
      pMot = FindMotor(pSics, tasMotorOrder[i]);
      if (pMot) {
        MotorGetSoftPosition(pMot, pCon, &oldPos);
        sprintf(pBueffel, "Driving %s from %f to %f",
                tasMotorOrder[i], oldPos, newPositions[i]);
      } else {
        sprintf(pBueffel, "Driving %s to %f",
                tasMotorOrder[i], newPositions[i]);
      }
      SCWrite(pCon, pBueffel, eWarning);
      status = StartMotor(pServ->pExecutor, pSics, pCon,
                          tasMotorOrder[i], RUNDRIVE, newPositions[i]);
      if (status == 0) {
        /* error should already have been reported by StartMotor */
        rStatus = 0;
      }
    }
  }

  /* 
     if in TAS mode do the TAS calculation and start the appropriate   
     motors.
   */
  if (iTAS > 0) {
    if (qhPars == 3) {
      SCWrite(pCon, "ERROR: QH cannot have 2 values", eError);
      return 0;
    }

    status = TASCalc(self, pCon, tasMask, tasTargets, tasTargetMask);
    if (status) {
      /*
         do output, first Q-E variables
       */
      for (i = 0; i < 12; i++) {
        if (tasMask[i]) {
          sprintf(pBueffel, "Driving %s from %f to %f",
                  tasVariableOrder[EI + i], oldEnergy[i],
                  self->tasPar[EI + i]->fVal);
          SCWrite(pCon, pBueffel, eWarning);
        }
      }
      /* 
         more output: the motor positions
       */
      for (i = 0; i < 9; i++) {
        if (tasTargetMask[i]) {
          pMot = FindMotor(pSics, tasMotorOrder[i]);
          if (pMot) {
            MotorGetSoftPosition(pMot, pCon, &oldPos);
          } else {
            oldPos = -9999.;
          }
          sprintf(pBueffel, "Driving %s from %f to %f", tasMotorOrder[i],
                  oldPos, tasTargets[i]);
          SCWrite(pCon, pBueffel, eWarning);
        }
      }
      /*
         output for magnet currents 
       */
      for (i = 0; i < 8; i++) {
        if (tasTargetMask[9 + i]) {
          oldPos = readDrivable(tasMotorOrder[CURMOT + i], pCon);
          sprintf(pBueffel, "Driving %s from %f to %f",
                  tasMotorOrder[CURMOT + i], oldPos, tasTargets[9 + i]);
          SCWrite(pCon, pBueffel, eWarning);
        }
      }

      status = TASStart(self, pCon, pSics, tasTargets, tasTargetMask);
      if (status == 0) {
        rStatus = 0;
      }
    } else {
      rStatus = 0;
    }
  }


  /*
     wait till we are finished
   */
  status = Wait4Success(GetExecutor());
  TASUpdate(self, pCon);

  /*
     handle interrupts
   */
  if (status == DEVINT) {
    if (SCGetInterrupt(pCon) == eAbortOperation) {
      SCSetInterrupt(pCon, eContinue);
      sprintf(pBueffel, "Driving aborted");
      SCWrite(pCon, pBueffel, eError);
    }
    return 0;
  } else if (status == DEVDONE) {
    sprintf(pBueffel, "Driving done");
    SCWrite(pCon, pBueffel, eValue);
  } else {
    sprintf(pBueffel, "Driving finished");
    SCWrite(pCon, pBueffel, eValue);
  }
  return rStatus;
}
