/** @file Galil protocol handler for script-context based controllers.
*
* If you 'send' commands to a galil controller using this protocol you
* will get one of three possible responses,
* 1. A value
*   eg
* 2. An acknowledgement (ie 'OK')
*   eg
*     sct_mc2 send "MOG"
*     OK
* 3. An error message
*   eg
*     sct_mc2 send "BGG"
*     ASCERR: 20 Begin not valid with motor off (during read finished)
*/
#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <dynstring.h>

/** @brief Set line terminator before sending command
*/
int GalilWriteStart(Ascon *a) {
  DynStringConcat(a->wrBuffer,"\r");
  a->state = AsconWriting;
  a->wrPos = 0;
  return 1;
}

/** @brief Map galil replies to OK, ASCERR: ..., value.
* You can use the first character to sort replies from a galil controller
* into four categories
* First character is,
* 'SPACE' This is followed by a value
* '?' Error, use 'TC 1' to get  error code and message.
* ':' Command received, in response to commands which don't request data.
* '1-9' First digit of error-code followed by error message. (in response to 'TC 1')
*/
int GalilReading(Ascon *a) {
  int ret;
  char chr, ch[2];

  ret = AsconReadChar(a->fd, &chr);
  if (ret > 0) {
    a->start = DoubleTime();
  } else if (ret < 0) {
    AsconError(a, "ASC5", errno);
    return 0;
  } else if (ret == 0) {
    if (a->timeout > 0) {
      if (DoubleTime() - a->start > a->timeout) {
        AsconError(a, "read timeout", 0);
        /* a->state = AsconTimeout; */
      }
    }
    return 0;
  }

  switch (chr) {
    case ' ':
      return AsconStdHandler(a);
      break;
    case '?':
      DynStringClear(a->wrBuffer);
      DynStringCopy(a->wrBuffer, "TC 1");
      a->noResponse = 0;
      a->state = AsconWriteStart;
      a->responseValid=0;
      a->wrPos = 0;
      return 0;
      break;
    case ':':
      DynStringReplace(a->rdBuffer, "OK",0);
      a->state = AsconReadDone;
      return 0;
      break;
    default:
      /* Check for first digit of errorcode followed by error message */
      if ('1' <= chr && chr <= '9') {
        ch[0] = chr;
        ch[1] = '\0';
        AsconStdHandler(a);
        DynStringInsert(a->rdBuffer, ch, 0);
        a->state = AsconReadDone;
        AsconError(a, GetCharArray(a->rdBuffer), 0);
        return 0;
      }
  }
  /* Return 1 to read next char befor progressing to next task */
  return 1;
}

/** brief Galil protocol handler.
* This handler formats commands (ie adds cr line terminator) and
* sorts replies into standard responses of
* <value>
* OK
* ASCERR: ...
*/
int GalilProtHandler(Ascon *a) {
  int ret;

  switch(a->state){
    case AsconWriteStart:
      ret = GalilWriteStart(a);
      return ret;
      break;
    case AsconReading:
      ret = GalilReading(a);
      return ret;
      break;
    default:
      return AsconStdHandler(a);
  }
  return 1;
}

void AddGalilProtocoll(){
	AsconProtocol *prot = NULL;

	prot = calloc(sizeof(AsconProtocol), 1);
	prot->name = strdup("galil");
	prot->init = AsconStdInit;
	prot->handler = GalilProtHandler;
	AsconInsertProtocol(prot);
}