/** @file Protocol handler for Aviatronic 35V 7A AC RF amplifier.
 * Ferdi Franceschini July 2009
 *
 * Create with 
 * makesctcontroller sct_rfamp ansrfamp IP:PORT
 *
 * TODO Handle control commands
 */
#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <dynstring.h>
#include <unistd.h>

#define STARTSIG 2
#define ENDSIG 3
#define ERRMSGLEN 127
#define MAXRETRY 3

/** @brief Enclose command with start and send signals
 *  before sending
 */
int AnsfrWriteStart(Ascon *a) {
  char startSignal[2] = {STARTSIG,'\0'}, endSignal = ENDSIG;
  
  DynStringInsert(a->wrBuffer, startSignal, 0);
  DynStringConcatChar(a->wrBuffer, endSignal);
  a->state = AsconWriting;
  a->wrPos = 0;
  return 1;
}

int AnsfrReading(Ascon *a) {
  int ret, i;
  static int retries = MAXRETRY;
  unsigned char reply[11];
  char chr, errmsg[ERRMSGLEN];
  char startSignal = STARTSIG, endSignal = ENDSIG;
  unsigned char OutputOn, C1on, C2on, C3on, OH, CV, CC;

  OutputOn = 1 << 7;
  C1on = 1 << 6;
  C2on = 1 << 5;
  C3on = 1 << 4;

  OH = 1 << 7;
  CV = 1 << 6;
  CC = 1 << 5;

  
  ret = AsconReadChar(a->fd, &chr);
  if (ret > 0 && chr != startSignal && retries > 0) {
    AsconReadGarbage(a->fd);
    a->state = AsconWriting;
    a->wrPos = 0;
    retries--;
    return 0;
  } else if (retries <= 0) {
    a->state = AsconReadDone;
    AsconError(a, "Failed to get start signal", 0);
    retries = MAXRETRY;
    return 0;
  }
  ret = AsconReadChar(a->fd, &chr);
  retries = MAXRETRY;
  for (i=0; ret > 0; i++) {
    a->start = DoubleTime();

    if (chr == endSignal) {
      a->state = AsconReadDone;
      break;
    } else {
      if (i >= 11) {
        a->state = AsconReadDone;
        AsconError(a, "Corrupt reply", ENOMEM);
        return 1;
      }
      reply[i] = chr;
      ret = AsconReadChar(a->fd, &chr);
    }
  }

  if (a->state != AsconReadDone) {
      if (a->timeout > 0) {
        if (DoubleTime() - a->start > a->timeout) {
          AsconError(a, "read timeout", 0);
          a->state = AsconTimeout;
          return 1;
        } else {
          return 1;
        }
      }
  }
  DynStringConcatChar(a->rdBuffer, reply[0]);
  DynStringConcatChar(a->rdBuffer, ' ');
  DynStringConcatChar(a->rdBuffer, reply[1]);
  DynStringConcatChar(a->rdBuffer, ' ');
  DynStringConcatChar(a->rdBuffer, reply[2]);
  DynStringConcatChar(a->rdBuffer, reply[3]);
  DynStringConcatChar(a->rdBuffer, ' ');
  DynStringConcatChar(a->rdBuffer, reply[4]);
  DynStringConcatChar(a->rdBuffer, reply[5]);
  DynStringConcatChar(a->rdBuffer, reply[6]);
  DynStringConcatChar(a->rdBuffer, ' ');
  DynStringConcatChar(a->rdBuffer, reply[7]);
  DynStringConcatChar(a->rdBuffer, reply[8]);
  DynStringConcatChar(a->rdBuffer, ' ');
  if (C3on & reply[9]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  if (C2on & reply[9]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  if (C1on & reply[9]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  if (OutputOn & reply[9]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  if (CC & reply[10]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  if (CV & reply[10]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  if (OH & reply[10]) {
    DynStringConcatChar(a->rdBuffer, '1');
    DynStringConcatChar(a->rdBuffer, ' ');
  } else {
    DynStringConcatChar(a->rdBuffer, '0');
    DynStringConcatChar(a->rdBuffer, ' ');
  }
  return 1;
}

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

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

void AddAnsfrProtocol(){
  AsconProtocol *prot = NULL;

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