/*
@file beamstopaction.c
@brief Proof of concept action object for Quokka beamstops.  
*/
#include <stdlib.h>
#include <stdio.h>
#include <sics.h>
#include <asyncqueue.h>
#include <nwatch.h>
#include "beamstopaction.h"

#define NOACTION -1
#define DEFAULTACTION 0
#define LASTACTION 1
#define AO_CMDLEN 256
#define SEQLEN 16
#define ACTIONS 32

extern int DMC2280MotionControl;

typedef enum {
  ActID,
  Args,
  DevStatus,
  DEBUG
} submcd;

char *subcmdName[] = {"act", "args", "status", "debug", NULL};
char *validActions[LASTACTION+1] = {"up", "down"};
/* Each action takes two arguments, an axis label and the jogspeed */
char *dfltActionSequence[ACTIONS][SEQLEN] = { {"SH%c","JG%c=%d", "BG%c", NULL}, {"SH%c","JG%c=(-1.0)*(%d)", "BG%c", NULL} };
int dfltactArgs[ACTIONS][SEQLEN] = { {1,2,1}, {1,2,1} };
char *dfltStatusCheck[ACTIONS][SEQLEN] = {{"TS%c", NULL}, {"TS%c", NULL}};
char *dfltDevStatusMsg[] = {"up", "down", "inbetween", "FAULT: Are the limit switches connected?"};
int numactions = 2;
#define NUM_DEVSTATES 4
// Mask TS reply to get limswitch status
#define LIMSWI_MASK 12
int dfltDevStatMask[NUM_DEVSTATES];
enum devicestatus {DEVUP, DEVDOWN, DEVBETWEEN, DEVFLT};

typedef struct __Action {
  pObjectDescriptor pDes;
  pIDrivable pDrivInt;
  pAsyncUnit asyncUnit;
  char objname[64];
  int actionID;
  char *(*actionSequence)[ACTIONS][SEQLEN];
  int (*actionArgs)[ACTIONS][SEQLEN];
  char *(*statusCheck)[ACTIONS][SEQLEN];
  int devStatusID;
  char **devStatusMsg;
  char args[256];
  char cf_axis;
  int cf_jogspeed;
  int cf_mask[NUM_DEVSTATES];
  int status;
  float fTarget;
  int debug;
} Action, *pAction;

void debugmsg(SConnection *pCon, pAction self, char *msg) {
    if (self->debug)
      SCWrite(pCon, msg, eValue);
}

/**
 * @brief Get the action ID corresponding to the given action name
 *
 * @par actnm action name
 * @par actlist array of actions ordered by action ID
 * @par numact number of available actions
 * @return action ID or -1 (not found)
 */
int ActNm2ID(char *actnm, char *actlist[], int numact) {
  int id;
  for (id=0; id < numact; id++) {
    if (strcasecmp(actlist[id], actnm) == 0) {
      return id;
    }
  }
  return -1;
}

static void *AO_GetInterface(void *pData, int iID) {
  pAction self = NULL;
       
  self = (pAction)pData;
  assert(self);

  if(iID == DRIVEID){
         return self->pDrivInt;
  }
  return NULL;  
} 

int AO_Wrapper(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
  char msg[128]="No message", output[256];
  int actID, status, i;
  OutCode msgType;
  pAction self = (pAction) pData;

  if (argc == 1) {
    sprintf(msg, "%d", self->status);
    msgType = eValue;
  } else if (argc == 2) { /* GET */
    if (strcasecmp(argv[1], "list") == 0) {
      for (i=0; subcmdName[i]; i++) {
        sprintf(msg, "%s", subcmdName[i]);
        SCWrite(pCon,msg,eValue);
      }
      return OKOK;
    } else if (strcasecmp(argv[1], subcmdName[ActID]) == 0) {
      sprintf(msg, "%s", validActions[self->actionID]);
      msgType = eValue;
    } else if (strcasecmp(argv[1], subcmdName[Args]) == 0) {
      sprintf(msg, "%s", self->args);
      msgType = eValue;
    } else if (strcasecmp(argv[1], subcmdName[DevStatus]) == 0) {
      if ((status = AO_CheckStatus(pData, pCon)) == HWFault) {
        return 0;
      }
      sprintf(msg, "%s", self->devStatusMsg[self->devStatusID]);
      msgType = eValue;
    } else if (strcasecmp(argv[1], subcmdName[DEBUG]) == 0) {
      sprintf(msg, "%d", self->debug);
      msgType = eValue;
    }
  } else if (argc == 3) { /* SET */
    if (strcasecmp(argv[1], subcmdName[ActID]) == 0) {
      if ( (actID = ActNm2ID(argv[2], validActions, numactions)) != -1) {
        sprintf(msg, "%s setting %s = %s", argv[0], argv[1], argv[2]);
        msgType = -1;
        self->actionID = actID;
      }
    } else if (strcasecmp(argv[1], subcmdName[Args]) == 0) {
    // TODO Check that args is a comma separated list a1,a2,a3 and type-check
      strcpy(self->args, argv[2]);
      sprintf(msg, "%s setting %s = %s", argv[0], argv[1], argv[2]);
      msgType = -1;
    } else if (strcasecmp(argv[1], subcmdName[DEBUG]) == 0) {
      self->debug = atoi(argv[2]);
    }
  }
  /* TODO allow plain output */
  switch (msgType) {
    case eValue: 
      sprintf(output, "%s = %s", argv[0], msg);
      SCWrite(pCon, output, eValue);
      break;
    default:
      strcpy(output, msg);
      debugmsg(pCon, self, output);
  }
  return OKOK;

}


static int AO_Halt(void *pData) {
  pAction self = (pAction) pData;
  char cmd[AO_CMDLEN], reply[AO_CMDLEN];
  int comStatus, cmd_len;

  sprintf(cmd, "ST%c", self->cf_axis);
  cmd_len = AO_CMDLEN;
  comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len);
  sprintf(cmd, "MO%c", self->cf_axis);
  cmd_len = AO_CMDLEN;
  comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len);
  return 1;
}

static int AO_CheckLimits(void *pData, float fVal, char *pError, int iErrLen) {
  return 1;
}
static long AO_StartAction(void *pData, SConnection *pCon, float fVal) {
/*
if fVal == start then
  get actionsequence which matches the actionID
  send sequence
  set currentAction = actionID, used by status 
*/
  char msg[128];
  char cmd[AO_CMDLEN], reply[AO_CMDLEN];
  int i, cmd_len, comStatus;

  if (DMC2280MotionControl != 1) {
    AO_Halt(pData);
    if (DMC2280MotionControl == 0) {
      SCWrite(pCon, "ERROR: Motion control is off", eError);
      return 0;
    } else {
      SCWrite(pCon, "ERROR: Motion control state is unknown", eError);
      return 0;
    }
  }
  pAction self = (pAction) pData;
  if (self->actionID < NOACTION || self->actionID > LASTACTION) {
    sprintf(msg, "ERROR: Programmer error, invalid action ID %d for %s. Alert a SICS programmer", self->actionID, self->objname);
    SCWrite(pCon, msg, eError);
    return 0;
  }
  if (self->actionID == NOACTION) {
    sprintf(msg, "ERROR: You must select an action before driving %s", self->objname);
    SCWrite(pCon, msg, eError);
    return 0;
  }
  for (i = 0; (*self->actionSequence)[self->actionID][i]; i++) {
    switch ( (*self->actionArgs)[self->actionID][i]) {
      case 1:
        sprintf(cmd, (*self->actionSequence)[self->actionID][i], self->cf_axis);
        break;
      case 2:
        sprintf(cmd, (*self->actionSequence)[self->actionID][i], self->cf_axis, self->cf_jogspeed);
        break;
    }
    debugmsg(pCon, self, cmd);
    cmd_len = AO_CMDLEN;
    comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len);
  }
  debugmsg(pCon, self, "Start action");
  self->fTarget = fVal;
  return OKOK;
} 
static int AO_CheckStatus(void *pData, SConnection *pCon) {
  pAction self =(pAction) pData;
  char cmd[AO_CMDLEN], reply[AO_CMDLEN]= " 0000000113 0007831721 0000000047 0000000000 0000000001\n:";
  int devStatus, sicsStatus, comStatus, limswi, cmd_len;
  int iSteps, iCounts, iFlags, iBG, iStopCode;
  char msg[128];
  
  sprintf(cmd, "MG {F10.0} _TD%c,_TP%c,_TS%c,_BG%c,_SC%c", self->cf_axis,self->cf_axis,self->cf_axis,self->cf_axis,self->cf_axis);
  debugmsg(pCon, self, cmd);
//  comStatus = 1;
cmd_len = AO_CMDLEN;
  comStatus = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), reply, &cmd_len);
  if (comStatus != 1) {
      SCWrite(pCon, "Command failed", eError);
    sicsStatus = HWFault;
  } else {
  debugmsg(pCon, self, reply);
    if ((reply[0] != ' ') & (reply[0] != '-')) {
      SCWrite(pCon, "ERROR: Invalid status reply from device", eError);
      sicsStatus = HWFault;
    } else {
      if ( 5 != sscanf(reply, "%d %d %d %d %d", &iSteps, &iCounts, &iFlags, &iBG, &iStopCode) ) {
      SCWrite(pCon, "Failed to scan reply", eError);
      sicsStatus = HWFault;
      } else {
        limswi = LIMSWI_MASK & iFlags;
        for (devStatus = 0; devStatus < NUM_DEVSTATES; devStatus++) {
          if (self->cf_mask[devStatus] == limswi) {
            self->devStatusID = devStatus;
            break;
          }
        }
        if (iBG == 1) {
          debugmsg(pCon, self, "BUSY");
          sicsStatus = HWBusy;
        } else {
          debugmsg(pCon, self, "IDLE");
          if (self->status == HWBusy) {
            AO_Halt(pData);
          }
          sicsStatus = HWIdle;
        }
      }
    }
  }
  self->status = sicsStatus;
  return sicsStatus;
}

/* @brief Check the device status and return the SICS status code 
*/
static float AO_GetValue(void *pData, SConnection *pCon) {
  pAction self = (pAction) pData;
  int status;
  status = AO_CheckStatus(pData, pCon);
  return status;
}

static void AO_Notify(void *context, int event) {
  return;
}
pAction AO_Create(SConnection *pCon, const char *asyncName) {
  pAction self = NULL;
  char msg[128];

  self = (pAction) malloc(sizeof(Action));
  if (self == NULL)
    return NULL;

  if (!AsyncUnitCreate(asyncName, &self->asyncUnit)) {
    sprintf(msg, "ERROR: Failed to find asyncqueue %s", asyncName);
    SCWrite(pCon, msg, eError);
    free(self);
    return NULL;
  }
  AsyncUnitSetNotify(self->asyncUnit, self, AO_Notify);

  self->pDrivInt = CreateDrivableInterface();
  if(!self->pDrivInt) {
    SCWrite(pCon, "ERROR: Failed to create drivable interface", eError);
    free(self);
    return NULL;
  }
  self->pDrivInt->Halt = AO_Halt;
  self->pDrivInt->CheckLimits = AO_CheckLimits;
  self->pDrivInt->SetValue = AO_StartAction;
  self->pDrivInt->CheckStatus = AO_CheckStatus;
  self->pDrivInt->GetValue = AO_GetValue;
  self->pDes = CreateDescriptor("ActionObject");
  self->pDes->GetInterface = AO_GetInterface;
  return self;
}

void AO_Kill(void *pData) {
  pAction self = (pAction) pData;
  if (self) {
    if (self->pDes) {
      free(self->pDes);
    }
    if (self->pDrivInt) {
      free(self->pDrivInt);
    }
    free(self);
  }
}

/* MakeActionObject objname asyncName jogspeed upmask downmask axis */
int ActionObjectFactory(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[])
{
  char msg[128], pError[128];
  int i, iRet;
  pAction pNew;

  if (argc < 7) {
    SCWrite(pCon, "You must provide the following four arguments, objname asyncName jogspeed upmask axis", eError);
    return 0;
  }
  printf("Create the %s action object on asyncqueue %s\n", argv[1], argv[2]);
  

  pNew = AO_Create(pCon, argv[2]);
  if (pNew == NULL) {
    sprintf(pError, "ERROR: Failed to create %s", argv[1]);
    SCWrite(pCon,pError,eError);
    return 0;
  }
  strcpy(pNew->objname,argv[1]);
  pNew->actionID = DEFAULTACTION;
  pNew->debug = 0;
  (pNew->args)[0] = '\0';
  pNew->actionSequence = dfltActionSequence;
  pNew->actionArgs = dfltactArgs;
  pNew->statusCheck = dfltStatusCheck;
  pNew->devStatusMsg = dfltDevStatusMsg;
  pNew->cf_jogspeed = atoi(argv[3]);
  pNew->cf_mask[DEVUP] = atoi(argv[4]);
  pNew->cf_mask[DEVDOWN] = atoi(argv[5]);
  pNew->cf_mask[DEVBETWEEN] = 12; // REVSWI | FWDSWI
  pNew->cf_mask[DEVFLT] = 0; // REV & FWD low
  pNew->cf_axis = argv[6][0];
  printf("axis = %c\n", pNew->cf_axis);
  printf("jogspeed = %d\n", pNew->cf_jogspeed);
  for (i=0; i< NUM_DEVSTATES; i++)
    printf("mask[%d] = %d\n",i, pNew->cf_mask[i]);
  iRet = AddCommand(pSics, argv[1], AO_Wrapper, AO_Kill, pNew);
  if(!iRet) {
        sprintf(pError,"ERROR: duplicate command %s not created [%d]", argv[1], iRet);
        SCWrite(pCon,pError,eError);
        AO_Kill(pNew);
        return 0;
  }
  SCSendOK(pCon);
  printf("Created action object %s\n", pNew->objname);
  AO_CheckStatus(pNew, pCon);
  return 1;
}