/* MGS_ :Message from camera Get Status command
 */
#include <sics.h>
#include <obdes.h>
#include <countdriv.h>
#include <nwatch.h>
#include <asyncqueue.h>
#include "camera.h"

#define ERRLEN 256
#define MSGLEN 256

enum camstates {idle, acquiring, processing, saving};
#define CAMDRIV_ERRTABLE \
  TR(ENONE,     "OK")                           \
  TR(EFAIL,     "command failed")               \
  TR(EBUSYACQ,  "camera busy acquiring image")  \
  TR(EBUSYSAVE, "camera busy saving image")     \
  TE(EBUSYPROC, "camera busy processing image")

#define TR(a,b) a,
#define TE(a,b) a
enum errcodes {CAMDRIV_ERRTABLE};
#undef TR
#undef TE

#define TR(a,b) b,
#define TE(a,b) b
char *errmsg[] = {CAMDRIV_ERRTABLE};
#undef TR
#undef TE

// Camera get/set commands:  ['status', 'info', 'state', 'camera', 'meta', 'file']
#define ECMDSTART 0
#define EGETSTART 100
#define ESETSTART 200
enum CommandCodes {cmdTake = 1, cmdClear, cmdGet, cmdSet, cmdEnd};
enum GetCodes     {gCamera = 1, gFile, gMeta, gInfo, gStatus, gState, gEnd};
enum SetCodes     {sCamera = 1, sFile, sMeta, sInfo, sEnd};
char *CMD[13] = {"temp", "shspd", "ffmt", "binres", "imsz", "xstart", "ystart", "floc", "fname", "fcntr", "acq", "status", "shtime"};

struct __cameraObj {
  int debug;
  camdriv_t state_machine;
  pNWTimer state_timer;
  int status;
  enum errcodes camError;
  pAsyncUnit asyncUnit;
};
typedef struct __cameraObj CamObj;

/* Camera communications and protocol handlers */
static pAsyncProtocol CAM_Protocol = NULL;

static int CAM_Tx(pAsyncProtocol p, pAsyncTxn txn) {
  int ret = 1;

  if (txn) {
    txn->txn_status = ATX_ACTIVE;
    if (AsyncUnitWrite(txn->unit, txn->out_buf, txn->out_len) < 0) {
      if (AsyncUnitReconnect(txn->unit) < 0)
        ret = 0;
    }
    ret = 1;
  } else {
    ret = 0;
  }
  return ret;
}
int defaultHandleInput(pAsyncProtocol p, pAsyncTxn txn, int ch);
static int CAM_Rx(pAsyncProtocol p, pAsyncTxn txn, int ch) {
  int ret = 1;

  if (ch == '\r')
    ret = 1;
  else if (ch == '\n')
    ret = AQU_POP_CMD; 
  else if (txn->inp_idx < txn->inp_len)
    txn->inp_buf[txn->inp_idx++] = ch;
  else
    ret = AQU_POP_CMD;

  return ret;
}
/* TODO
static int CAM_Ev(pAsyncProtocol p, pAsyncTxn txn, int event) {
}
*/

void CameraInitProtocol(SicsInterp *pSics) {
  if (CAM_Protocol == NULL) {
    CAM_Protocol = AsyncProtocolCreate(pSics, "CAMERA", NULL, NULL);
    CAM_Protocol->sendCommand = CAM_Tx;
    CAM_Protocol->handleInput = CAM_Rx;
    CAM_Protocol->prepareTxn  = NULL;
    CAM_Protocol->killPrivate = NULL;
    #if 0
    CAM_Protocol->handleEvent = CAM_Ev;
    CAM_Protocol->sendTerminator = strdup("\r\n");
    CAM_Protocol->replyTerminator[0] = strdup("\r\n");
    #endif
  }
}

/* CounterDriver interface functions */
static int CamGetStatus(CounterDriver *cntrData, float *fControl) {
  CamObj *camdrv= (CamObj *)cntrData->pData;

  return camdrv->status;
}
static int replyOK(enum CommandCodes cc, char *reply) {
  return 1;
}

static void crank_state_machine(CamObj *self, enum event_codes ev_sym) {
  char sscur[SSLEN+1], ssnext[SSLEN+1], esout[ESLEN+1], message[MSGLEN+1];

  if (self->debug)
    strncpy(sscur, strstate(self->state_machine.Sc), SSLEN);
  camdriv_input(self, &self->state_machine, ev_sym);
  if (self->debug) {
    strncpy(ssnext, strstate(self->state_machine.Sc), SSLEN);
    strncpy(esout, strevent(self->state_machine.Eo), ESLEN);
    snprintf(message, MSGLEN,
        "DEBUG:(%s:%d) Scurr:%s Ein:%s|Snext:%s Eout:%s",__FILE__,__LINE__,
        sscur,event_names[ev_sym],ssnext,esout);
    SICSLogWrite(message, eLog);
  }
}
static int CamStart(CounterDriver *cntrData) {
  CamObj *self = NULL;
  enum event_codes cd_sym;

  self = cntrData->pData;

  if (self->state_machine.multi) {
    cd_sym = ECD_TK_MLTI;
  } else {
    cd_sym = ECD_TK_SHOT;
  }
  crank_state_machine(self, cd_sym);
  return 1;
}
static int CamPause(CounterDriver *cntrData) {
  return 1;
}
static int CamContinue(CounterDriver *cntrData) {
  return 1;
}
static int CamHalt(CounterDriver *cntrData) {
  return 1;
}
static int CamReadValues(CounterDriver *cntrData) {
  int status, iReplyLen=MSGLEN;
  char *cmd="get imsz", pReply[MSGLEN];
  CamObj *self = NULL;
  return 1;

  self = cntrData->pData;
 // fTime = [get shtime]
  status = AsyncUnitTransact(self->asyncUnit, cmd, strlen(cmd), pReply, &iReplyLen);
  return 1;
}
static int CamGetError(CounterDriver *cntrData, int *iCode, char *error, int iErrLen) {
  CamObj *camdrv=NULL;
  camdrv = (CamObj *) cntrData->pData;
  *iCode = camdrv->camError;

  camdrv->camError = ENONE;
  switch (*iCode) {
    case EBUSYACQ:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[EBUSYACQ]);
    break;
    case EBUSYSAVE:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[EBUSYSAVE]);
    break;
    case EBUSYPROC:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[EBUSYPROC]);
    break;
    case ENONE:
      snprintf(error, (size_t) iErrLen, "CAMERR: Can't complete operation, %s", errmsg[ENONE]);
    break;
    case EFAIL:
      snprintf(error, (size_t) iErrLen, "CAMERR: %s", errmsg[EFAIL]);
    break;
  }
  return 1;
}
static int CamTryAndFixIt(CounterDriver *cntrData, int iCode) {
  return COTERM;
}
static int CamSet(CounterDriver *cntrData, char *name, int iCter, float fVal) {
  return 1;
}
static int CamGet(CounterDriver *cntrData, char *name, int iCter, float *fVal) {
  return 1;
}
static int CamSend(CounterDriver *cntrData, char *pText, char *pReply, int iReplyLen) {
  int status;
  CamObj *self = NULL;

  self = cntrData->pData;

  status = AsyncUnitTransact(self->asyncUnit, pText, strlen(pText), pReply, &iReplyLen);
  return 1;
}

static int cb_shotcmd(pAsyncTxn txn) {
  CamObj *self = (CamObj *) txn->cntx;
  char *resp = txn->inp_buf, message[MSGLEN+1];
  enum event_codes cd_sym;

  if (strncmp(resp, "OK", 2) != 0) {
    self->camError = EFAIL;
    return 0;
  }
  return 1;
}

int camdriv_out(void *me, event_t Eo) {
  int len;
  char cmd[MSGLEN], logmsg[MSGLEN];
  CamObj *self = (CamObj *)me;

  if (Eo.ca) {
    /* send command to camera */
    switch (Eo.ca) {
      case ECA_TK_SHOT:
        len = strlen(event_signatures[ECA_TK_SHOT]);
        strncpy(cmd, event_signatures[ECA_TK_SHOT], len);
        break;
      case ECA_MLTI_ON:
        len = strlen(event_signatures[ECA_MLTI_ON]);
        strncpy(cmd, event_signatures[ECA_MLTI_ON], len);
        break;
    }
    AsyncUnitSendTxn(self->asyncUnit, cmd, len, cb_shotcmd, self, MSGLEN);
    if (self->debug) {
      snprintf(logmsg, MSGLEN, "DEBUG: camdriv_out: ev=%s, output=%s\n", event_names[Eo.ca], event_signatures[Eo.ca]);
      SICSLogWrite(logmsg, eLog);
    }
  }
  if (Eo.cm) {
    snprintf(logmsg, MSGLEN, "ERROR: NOT IMPLEMENTED, camdriv_out: ev=%s, output=%s\n", event_names[Eo.cm], event_signatures[Eo.cm]);
    SICSLogWrite(logmsg, eLogError);
  }
  if (Eo.cd) {
    snprintf(logmsg, MSGLEN, "ERROR: NOT IMPLEMENTED, camdriv_out: ev=%s, output=%s\n", event_names[Eo.cd], event_signatures[Eo.cd]);
    SICSLogWrite(logmsg, eLogError);
  }
  if (Eo.dr) {
    /* send msg to SICS */
    switch (Eo.dr) {
      case EDR_IDLE:
        self->status = HWIdle;
        break;
      case EDR_BUSY:
        self->status = HWBusy;
        break;
      case EDR_FAULT:
        self->status = HWFault;
        break;
    }
    if (self->debug) {
      snprintf(logmsg, MSGLEN, "DEBUG: camdriv_out: ev=%s, output=%s\n", event_names[Eo.dr], event_signatures[Eo.dr]);
      SICSLogWrite(logmsg, eLog);
    }
  } 
  return 1;
}
static int cb_getstate(pAsyncTxn txn);
static int cb_state_timer(void *ctx, int mode) {
  CamObj *self = (CamObj *) ctx;
  char errmsg[32]="";
  char cmd[MSGLEN];
  int len, status;

  len = strlen(event_signatures[ECA_GET_STATE]);
  strncpy(cmd, event_signatures[ECA_GET_STATE], len);
  status = AsyncUnitSendTxn(self->asyncUnit, cmd, len, cb_getstate, self, MSGLEN);

  if (status==1) {
    return 1;
  } else {
    snprintf(errmsg, 31, "ERROR:(%s) AsyncUnitTransact failed",__FILE__);
    SICSLogWrite(errmsg, eLogError);
    return 0;
  }
}

/* Input from camera state feedback */
static int cb_getstate(pAsyncTxn txn) {
  CamObj *self = (CamObj *) txn->cntx;
  char *resp = txn->inp_buf, message[MSGLEN+1];
  int len = txn->inp_idx, ret=1, time_rem, time_tot;
  enum event_codes ca_sym, cm_sym;


  if ( cam_parse_status(resp, &ca_sym, &time_rem, &time_tot) == -1) {
    snprintf(message, MSGLEN, "ERROR:(%s:%d) cam_parse_status failed on '%s'",__FILE__,__LINE__,resp);
    SICSLogWrite(message, eLogError);
    ret = 0;
  } else {
    cm_sym = camera_model(ca_sym);
    crank_state_machine(self, cm_sym);
  }
  if (self->state_timer) {
    NetWatchRemoveTimer(self->state_timer);
    self->state_timer=0;
  }
  NetWatchRegisterTimer(&self->state_timer, 500, cb_state_timer, self);
  return ret;
}

pCounterDriver CreateCam(SConnection *pCon, char *name, char *asynq) {
  char msg[ERRLEN], cmd[MSGLEN], reply[MSGLEN];
  int len, reply_len;
  state_t start_state = {.cl=SCL_RDY, .cm=SCM_IDLE, .dr=SDR_IDLE};

  pCounterDriver pCntDriv = NULL;
  CamObj *pNewCam = NULL;

  pNewCam = (CamObj *) malloc(sizeof(CamObj));
  memset(pNewCam, 0, sizeof(CamObj));
  STset(&pNewCam->state_machine.Sc, start_state);
  EVclr(&pNewCam->state_machine.Eo);
  pNewCam->state_machine.output_fn = camdriv_out;
  pNewCam->state_machine.multi = 0;
  pNewCam->state_timer = 0;
  pNewCam->status = HWIdle;
  pNewCam->camError = ENONE;
  pNewCam->debug = 1;

  if (!AsyncUnitCreate(asynq, &pNewCam->asyncUnit)) {
    snprintf(msg, ERRLEN, "CAMERR:AsyncQueue %s has not been defined", asynq);
    SCWrite(pCon, msg, eError);
    return NULL;
  }

  pCntDriv = CreateCounterDriver(name, "anstocamera");
  if (pCntDriv == NULL)
    return NULL;


  pCntDriv->GetStatus = CamGetStatus;
  pCntDriv->Start = CamStart;
  pCntDriv->Pause = CamPause;
  pCntDriv->Continue = CamContinue;
  pCntDriv->Halt = CamHalt;
  pCntDriv->ReadValues = CamReadValues;
  pCntDriv->GetError = CamGetError;
  pCntDriv->TryAndFixIt = CamTryAndFixIt;
  pCntDriv->Set = CamSet;
  pCntDriv->Get = CamGet;
  pCntDriv->Send = CamSend;

  pCntDriv->iNoOfMonitors = 1;
  pCntDriv->pData = pNewCam;

  len = strlen(event_signatures[ECA_GET_STATE]);
  strncpy(cmd, event_signatures[ECA_GET_STATE], len);
  AsyncUnitSendTxn(pNewCam->asyncUnit, cmd, len, cb_getstate, pNewCam, MSGLEN);
  return pCntDriv;
}
#if 0
int CameraAction(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
CamObj *pCam = (CamObj *) pData;
}
int CamMakeCmd(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
  CamObj *pNew = NULL;

  pNew = CreateCam(argv[1], argv[2]);
  if (pNew == NULL)
    return 0;
  AddCommand(pSics, argv[1], CameraAction, NULL, pNew);
  return 1;
}
#endif