/*-------------------------------------------------------------------
  diffscan is an operator which can perform a fast differential scan 
  while a motor is running. 

  copyright: see file COPYRIGHT

  Mark Koennecke, November 2004
---------------------------------------------------------------------*/
#include <stdio.h>
#include <assert.h>
#include <tcl.h>
#include "fortify.h"
#include "sics.h"
#include "diffscan.h"
#include "drive.h"
#include "counter.h"

extern double DoubleTime(void);

#define DIFFMONITOR 0
#define SKIP 1

/*-------------------------------------------------------------------*/
static void KillDiffScan(void *data)
{
  pDiffScan self = (pDiffScan) data;

  if (self == NULL) {
    return;
  }

  if (self->pDes != NULL) {
    DeleteDescriptor(self->pDes);
  }
  if (self->parArray != NULL) {
    ObParDelete(self->parArray);
  }
  free(self);
}

/*---------------------------------------------------------------------*/
static int SaveDiffScan(void *data, char *name, FILE * fd)
{
  pDiffScan self = (pDiffScan) data;
  if (self == NULL) {
    return 0;
  }
  fprintf(fd, "%s monitor %f\n", name, ObVal(self->parArray, DIFFMONITOR));
  fprintf(fd, "%s skip %f\n", name, ObVal(self->parArray, SKIP));
  return 1;
}

/*----------------------------------------------------------------------*/
int MakeDiffScan(SConnection * pCon, SicsInterp * pSics, void *pData,
                 int argc, char *argv[])
{
  pDiffScan pNew = NULL;
  int status;

  pNew = (pDiffScan) malloc(sizeof(DiffScan));
  if (pNew == NULL) {
    SCWrite(pCon, "ERROR: out of memory creating differential scan",
            eError);
    return 0;
  }
  memset(pNew, 0, sizeof(DiffScan));
  pNew->pDes = CreateDescriptor("DiffScan");
  pNew->parArray = ObParCreate(2);
  if (!pNew->pDes || !pNew->parArray) {
    SCWrite(pCon, "ERROR: out of memory creating differential scan",
            eError);
    KillDiffScan(pNew);
    return 0;
  }
  ObParInit(pNew->parArray, DIFFMONITOR, "monitor", 4.0, usUser);
  ObParInit(pNew->parArray, SKIP, "skip", .0, usUser);
  pNew->pDes->SaveStatus = SaveDiffScan;

  if (argc > 1) {
    status = AddCommand(pSics, argv[1],
                        DiffScanWrapper, KillDiffScan, pNew);

  } else {
    status = AddCommand(pSics, "diffscan",
                        DiffScanWrapper, KillDiffScan, pNew);

  }
  if (status != 1) {
    SCWrite(pCon, "ERROR: duplicate diffscan not created", eError);
    return 0;
  }
  return 1;
}

/*----------------------------------------------------------------------*/
int DiffScanWrapper(SConnection * pCon, SicsInterp * pSics, void *pData,
                    int argc, char *argv[])
{
  pDiffScan self = NULL;
  pScanData pScan = NULL;
  ObPar *par = NULL;
  char pBueffel[255];
  int status;

  self = (pDiffScan) pData;
  assert(self);

  if (argc < 2) {
    SCWrite(pCon, "ERROR: need arguments to diffscan", eError);
    return 0;
  }
  if (!SCMatchRights(pCon, usUser)) {
    return 0;
  }

  /*
     first try to find a scan object and to run
   */
  strtolower(argv[1]);
  pScan = (pScanData) FindCommandData(pSics, argv[1], "ScanObject");
  if (pScan != NULL && argc > 2) {
    status = RunDiffScan(self, pScan, pCon, atof(argv[2]));
    return status;
  }

  if (strcasecmp(argv[1], "sicsvar") == 0) {
    if (argc > 2) {
      if (strcasecmp(argv[2], "none") == 0) {
        self->sicsvar = NULL;
        return 1;
      } else {
        self->sicsvar = FindVariable(pServ->pSics, argv[2]);
        if (self->sicsvar == NULL) {
          SCPrintf(pCon, eError, "Cannot find SICS Variable: %s\n", argv[2]);
          return 0;
        }
        else if (self->sicsvar->eType != veText) {
          SCPrintf(pCon, eError, "SICS Variable must be TEXT not %s\n",
              self->sicsvar->eType == veInt ? "INT" :
              self->sicsvar->eType == veFloat ? "FLOAT" : "unknown");
          self->sicsvar = NULL;
          return 0;
        }
      }
      return 1;
    } else {
      SCPrintf(pCon, eValue, "sicsvar = %s\n", self->sicsvar ? self->sicsvar->name : "none");
      return 1;
    }
  }
  if (strcasecmp(argv[1], "hdbnode") == 0) {
    if (argc > 2) {
      if (strcasecmp(argv[2], "none") == 0) {
        self->hdbnode = NULL;
        return 1;
      } else {
        self->hdbnode = FindHdbNode(NULL, argv[2], pCon);
        if (self->hdbnode == NULL) {
          SCPrintf(pCon, eError, "Cannot find HDB Node: %s\n", argv[2]);
          return 0;
        }
        else if (self->hdbnode->value.dataType != HIPTEXT) {
          SCPrintf(pCon, eError, "HDB Node %s must be TEXT\n", argv[2]);
          self->hdbnode = NULL;
          return 0;
        }
      }
      return 1;
    } else {
      char *path = NULL;
      if (self->hdbnode != NULL)
        path = GetHipadabaPath(self->hdbnode);
      SCPrintf(pCon, eValue, "hdbnode = %s\n", path ? path : "none");
      return 1;
    }
  }
  /*
     if we end here we are treating variables
   */
  if (argc > 2) {
    /*
       set case
     */
    return ObParSet(self->parArray, argv[0], argv[1], atof(argv[2]), pCon);
  } else {
    /*
       get case
     */
    par = ObParFind(self->parArray, argv[1]);
    if (par != NULL) {
      snprintf(pBueffel, 255, "%s.%s = %f", argv[0], argv[1], par->fVal);
      SCWrite(pCon, pBueffel, eValue);
      return 1;
    } else {
      snprintf(pBueffel, 255, "ERROR: parameter %s not found", argv[1]);
      SCWrite(pCon, pBueffel, eError);
      return 0;
    }
  }
  return 1;
}

/*--------------------------------------------------------------------*/
static int StartDiffScan(pDiffScan self, pScanData pScan,
                         SConnection * pCon, float fEnd)
{
  pVarEntry pVar = NULL;
  void *pPtr = NULL;
  pCounter pCount = NULL;
  char pBueffel[255];
  int status;

  /*
     error checks
   */
  if (pScan->iScanVar < 1) {
    SCWrite(pCon, "ERROR: no scan variable to diffscan", eError);
    return 0;
  }
  if (pScan->iScanVar > 1) {
    snprintf(pBueffel, 255,
             "WARNING: diffscan handles only first scan variable, %d %s",
             pScan->iScanVar - 1, "scan variables ignored");
    SCWrite(pCon, pBueffel, eWarning);
  }

  /*
     initialize data structure
   */
  self->scanObject = pScan;
  self->scanObject->pCon = pCon;
  self->skip = (int) ObVal(self->parArray, SKIP);
  self->scaleMonitor = (int) ObVal(self->parArray, DIFFMONITOR);
  self->normalizationScale = -1;
  pScan->iCounts = 0;
  pScan->iNP = 0;

  /*
     get variable
   */
  DynarGet(pScan->pScanVar, 0, &pPtr);
  pVar = (pVarEntry) pPtr;
  if (pVar == NULL) {
    SCWrite(pCon, "ERROR: cannot access scan variable", eError);
    return 0;
  }
  InitScanVar(pVar);

  /*
     drive to start position
   */
  status =
      Drive(pCon, pServ->pSics, ScanVarName(pVar), ScanVarStart(pVar));
  if (status != 1) {
    return 0;
  }

  /*
     Configure counter. We operate in timer mode with a very long 
     preset mode. Stopping is done explicitly in the diffscan task
   */
  SetCounterMode(pScan->pCounterData, eTimer);
  SetCounterPreset(pScan->pCounterData, 3600.);

  /*
     start motor and counter
   */
  status = pVar->pInter->SetValue(pVar->pObject, pCon, fEnd);
  if (status != OKOK) {
    /*
       errors will already have been reported in SetValue
     */
    return 0;
  }
  pCount = (pCounter) pScan->pCounterData;
  assert(pCount);
  status = pCount->pCountInt->StartCount(pCount, pCon);
  if (status != OKOK) {
    return 0;
  }
  pCount->pCountInt->lastStatus = HWBusy;
  return 1;
}

/*--------------------------------------------------------------------*/
static float normalizeEntry(pCountEntry pCount, pCountEntry last,
                            long monValue, int scaleMon)
{
  int i;
  float fScale;
  float value;
  long diff;

  /*
     calculate scale
   */
  diff = pCount->Monitors[scaleMon - 1] - last->Monitors[scaleMon - 1];
  if (diff > 0) {
    fScale = (float) monValue / (float) diff;
  } else {
    fScale = 0.;
  }
  value = ((float) (pCount->lCount - last->lCount)) * fScale;
  pCount->lCount = (long) value;
  for (i = 0; i < 10; i++) {
    pCount->Monitors[i] =
        (pCount->Monitors[i] - last->Monitors[i]) * fScale;
  }
  fScale = pCount->fTime - last->fTime;
  if (fScale > 0)
    value /= fScale;
  return value;
}

/*--------------------------------------------------------------------*/
static void copyCountData(pCountEntry last, pCountEntry pCount)
{
  memcpy(last, pCount, sizeof(CountEntry));
}

/*---------------------------------------------------------------------*/
static int DiffScanTask(void *pData)
{
  pCounter pCount = NULL;
  pCountEntry data;
  pVarEntry pVar = NULL;
  void *pPtr = NULL;
  float fPos, countValue;
  int status, finish = 1, count;
  char pBueffel[255];
  long rawCount, rawMon;
  CountEntry rawCopy;
  double now;

  pDiffScan self = (pDiffScan) pData;

  /*
     manage skip
   */
#if 1
  now = DoubleTime();
  if (self->skip > 0) {
    if (now - self->last_report_time < 0.001 * self->skip) {
      return 1;
    }
  } else {
    if (now - self->last_report_time < 0.1) {
      return 1;
    }
  }
  self->last_report_time = now;
#else
  if (self->skip > 0) {
    if (self->skipCount > self->skip) {
      self->skipCount = 0;
    } else {
      self->skipCount++;
      return 1;
    }
  }
#endif

  /*
     read motor status
   */
  DynarGet(self->scanObject->pScanVar, 0, &pPtr);
  pVar = (pVarEntry) pPtr;
  status =
      pVar->pInter->CheckStatus(pVar->pObject, self->scanObject->pCon);
  if (status != HWBusy) {
    finish = 0;
  }

  /*
     read values
   */
  status = GetDrivablePosition(pVar->pObject, self->scanObject->pCon,
                               &fPos);
  if (status == 0) {
    ReleaseCountLock(pCount->pCountInt);
    return finish;
  }
  AppendScanVar(pVar, fPos);
  pCount = (pCounter) self->scanObject->pCounterData;
  pCount->pCountInt->TransferData(pCount, self->scanObject->pCon);
  CollectCounterData(self->scanObject);

  /*
     normalize (or not)
   */
  DynarGet(self->scanObject->pCounts, self->scanObject->iCounts - 1,
           &pPtr);
  data = (pCountEntry) pPtr;
  copyCountData(&rawCopy, data);
  if (self->normalizationScale < 0) {
    countValue = (float) data->lCount;
    rawCount = data->lCount;
    rawMon = data->Monitors[self->scaleMonitor - 1];
    if(rawMon > 100){
      self->normalizationScale = rawMon;
      traceSys("diffscan","START:normalizing on %d, scale monitor is %d",
          self->normalizationScale, self->scaleMonitor);
    } else {
      traceSys("diffscan","START:normalization count  %ld, on scale monitor is %d and is to low",
          rawMon, self->scaleMonitor);
      SCWrite(self->scanObject->pCon,"WARNING: Skipping first point because of low count rate", eWarning);
      return finish;
    }
  } else {
    if (data->Monitors[self->scaleMonitor - 1] -
        self->last.Monitors[self->scaleMonitor - 1] < 5) {
      SCWrite(self->scanObject->pCon, "WARNING: low count rate", eLog);
      traceSys("diffscan","RUN:low monitor difference from %ld to %ld",data->Monitors[self->scaleMonitor-1],
          self->last.Monitors[self->scaleMonitor -1]);
    }
    rawCount = data->lCount;
    rawMon = data->Monitors[self->scaleMonitor - 1];
    countValue = normalizeEntry(data, &self->last,
                                self->normalizationScale,
                                self->scaleMonitor);
  }
  copyCountData(&self->last, &rawCopy);

  /*
     print progress
   */
  snprintf(pBueffel, 255, "%5d %12.4f %12.4f RAW: %10ld %10ld %10.3f",
           self->scanObject->iCounts - 1,
           fPos, countValue, rawCount, rawMon, data->fTime);
  SCWrite(self->scanObject->pCon, pBueffel, eLog);
  if (self->sicsvar != NULL && self->sicsvar->eType == veText)
    VarSetText(self->sicsvar, pBueffel, usInternal);
  if (self->hdbnode != NULL && self->hdbnode->value.dataType == HIPTEXT) {
    hdbValue v;
    char error[512];
    cloneHdbValue(&self->hdbnode->value, &v);
    if (!readHdbValue(&v, pBueffel, error, 512)) {
      SCWrite(self->scanObject->pCon, error, eError);
    } else {
      UpdateHipadabaPar(self->hdbnode, v, self->scanObject->pCon);
    }
    ReleaseHdbValue(&v);
  }
  InvokeCallBack(self->scanObject->pCall, SCANPOINT, self->scanObject);
  traceSys("diffscan","RUN: pos, count, rawcount, rawmon: %f, %f, %ld, %ld",
      fPos, countValue, rawCount, rawMon);

  /*
     check for interrupt
   */
  if (SCGetInterrupt(self->scanObject->pCon) >= eAbortScan) {
    pCount->pCountInt->Halt(pCount);
    pVar->pInter->Halt(pVar->pObject);
    SicsWait(1);
    finish = 0;
  }

  if(finish == 0) {
    ReleaseCountLock(pCount->pCountInt);
  }
  
  return finish;
}

/*----------------------------------------------------------------------*/
int RunDiffScan(pDiffScan self, pScanData pScan,
                SConnection * pCon, float fEnd)
{
  long lID;
  pCounter pCount = NULL;

  if (StartDiffScan(self, pScan, pCon, fEnd) != 1) {
    return 0;
  }
  self->last_report_time = DoubleTime();
  InvokeCallBack(self->scanObject->pCall, SCANSTART, self->scanObject);


  lID = TaskRegisterN(pServ->pTasker,"diffscan", DiffScanTask, NULL, NULL, self, TASK_PRIO_HIGH);
  TaskWait(pServ->pTasker, lID);


  pCount = (pCounter) self->scanObject->pCounterData;
  pCount->pCountInt->Halt(pCount);
  InvokeCallBack(self->scanObject->pCall, SCANEND, self->scanObject);
  pCount->pCountInt->lastStatus = HWIdle;

  return 1;
}