/** @file OrdHVPS protocol handler for script-context based controllers.
 * Author: Ferdi Franceschini
 *
 * ffr This is a replacement for the orhvps.c controller which suffered from state lockups
 *     The new driver will resend a command if it receives a NAK.
 * MJL 17/9/08 implement a special command mode to make life easier
 *
 * Examples
    sct_dhv1 transact "P x4"
    x4 6
    sct_dhv1 transact v
    ORDELA 2.10 07/24/00
    sct_dhv1 transact H
    56
    set msg [sct_dhv1 transact {h 57}]
    ACK
    sct_dhv1 transact H
    57

 * TODO Remove globals and statics if we want to allow more controllers.
 */
#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <dynstring.h>
#include <unistd.h>

#define ACK 6
#define NAK 21
#define MAXARGS 5
#define MAXARGLEN 32
#define ERRMSGLEN 127
#define MAXRETRY 3
static char cmdrspfmt[MAXARGLEN];
static char *pcmdrspfmt=cmdrspfmt;
static char cmdArgv[MAXARGS][MAXARGLEN];
/** @brief Format command before sending
 */
int OrdHVPSWriteStart(Ascon *a) {
  char cmd[MAXARGLEN];
  int cmd_len, ci,cj,bi;
  char *wrBArray, ch;

//  strcpy(wrBArray, GetCharArray(a->wrBuffer));
  wrBArray = GetCharArray(a->wrBuffer);
  cmd_len = GetDynStringLength(a->wrBuffer);
  for (ci=2, cj=0, bi=0; ci<MAXARGS && bi<cmd_len && cj < MAXARGLEN; ci++) {
    for (cj=0; (ch = wrBArray[bi]) != ' ' && bi<cmd_len; bi++, cj++)
      cmdArgv[ci][cj] = ch;
    cmdArgv[ci][cj] = '\0';
    while (wrBArray[bi] == ' ' && bi < cmd_len)
      bi++;
  }
  if (cj >= MAXARGLEN) {
    a->state = AsconIdle;
    AsconError(a, "Command or argument exceeds maximum length", 0);
    return 0;
  }

  pcmdrspfmt=cmdrspfmt;
  // In command mode, the third argument is normally a single letter command
  // and the remaining arguments are command parameters.  For future expansion,
  // we also allow the third argument to be an explicit command/response format.
  // This is in format "<cmd>-<resp>" where the command and response fields
  // are format specifiers containing the required/expected ASCII letters
  // in the command, and the format specifiers '%s' (string),
  // '%d' (integer 0-255), , '%B' (a board name e.g. x0-y15),
  // '%P' (a pot name e.g. x0-y191), or '%A' (ack code as 'ACK'/'NAK').
  // Formats '%c' (a character) and '%x' (char as 2 hex digits) could be
  // added to the code if needed.
  // Note '%P' translates to/from a two-byte board number plus pot number.
  // Code '%A' only useable in response format.
  if (strlen(cmdArgv[2])==1)
    switch(*cmdArgv[2])
    {
      // All the known commands for the Ordela 21000N, at 9/08.
      // If more become available, add them here, or alternately
      // supply an appropriate command format string explicitly.
      case 'v':  pcmdrspfmt="vz-%sz"; break;     // Get firmware version
      case 'p':  pcmdrspfmt="p%P%dz-%A"; break;   // Set a pot value
      case 'P':  pcmdrspfmt="P%Pz-P%P%dz"; break; // Get a pot value
      case 'h':  pcmdrspfmt="h%dz-%A"; break;     // Set HV voltage (also settable directly via the orhvps object)
      case 'H':  pcmdrspfmt="Hz-H%dz"; break;     // Get HV setting
      case 'd':  pcmdrspfmt="dz-%A"; break;       // Enter diagnostic mode (disables coincidence detection etc.)
      case 'D':  pcmdrspfmt="Dz-D%dz"; break;     // Check if in diagnostic mode
      case 'n':  pcmdrspfmt="nz-%A"; break;       // Exit diagnostic mode
      case 'b':  pcmdrspfmt="b%Bz-%A"; break;     // Disable a board
      case 'B':  pcmdrspfmt="B%Bz-B%B%dz"; break; // Query if board disabled
      case 'l':  pcmdrspfmt="l%Bz-%A"; break;     // Re-enable a board
      case 'J':  pcmdrspfmt="Jz-J%dz"; break;     // Check jumper settings
      default:
        a->state = AsconIdle;
        AsconError(a, "Unknown command", 0);
        return 0;
        break;
    }
  // Prepare the command string
  char *pcmd=cmd;
  int nfmtspec=0;
  while(*pcmdrspfmt!='-')
  {
    if (*pcmdrspfmt=='\0')
    {
      a->state = AsconIdle;
      AsconError(a, "Missing '-' separator in format string", 0);
      return 0;
    }
    if (*pcmdrspfmt=='%') // format specifier
    {
      pcmdrspfmt++;
      if (++nfmtspec>3)
      {
        a->state = AsconIdle;
        AsconError(a, "Not enough arguments supplied for cmd", 0);
        return 0;
      }

      int v1,v2;
      switch(*pcmdrspfmt++)
      {
        case 's': // probably never used
          pcmd+=sprintf(pcmd,"%s",cmdArgv[nfmtspec+2]);
          break;
        case 'd':
          sscanf(cmdArgv[nfmtspec+2],"%d",&v1);
          pcmd+=sprintf(pcmd,"%c",(char)v1);
          break;
        case 'B':
          sscanf(cmdArgv[nfmtspec+2]+1,"%d",&v1);
          v1+=(v1>=8)*8; // 0-15-> 0-7 and 16-23
          v1+=(*cmdArgv[nfmtspec+2]=='y')*8; // y at 8-15 and 24-31
          pcmd+=sprintf(pcmd,"%c",v1);
          break;
        case 'P':
          sscanf(cmdArgv[nfmtspec+2]+1,"%d",&v1);
          v1^=0x3;
          v2=v1&0xF;
          v1>>=4;
          v1+=(v1>=8)*8;
          v1+=(*cmdArgv[nfmtspec+2]=='y')*8;
          pcmd+=sprintf(pcmd,"%c%c",v1,v2);
          break;
          // case 'A': is NOT supported - responses only!
        default:
          a->state = AsconIdle;
          AsconError(a, "Unknown % specification in command format", 0);
          return 0;
      }
    }
    else
      *pcmd++=*pcmdrspfmt++; // Simple ASCII character that is part of command
  }
  pcmdrspfmt++; // skip over the '-' separator into the response format
  cmd_len=pcmd-cmd;
  cmd[cmd_len] = '\0';
  DynStringClear(a->wrBuffer);
  DynStringConcatBytes(a->wrBuffer, cmd, cmd_len);
  a->state = AsconWriting;
  a->wrPos = 0;
  return 1;
}


int OrdFmtReply(char *rdBArray, int rsp_len, char *response, char *errmsg) {
  int ret, i, idx, bnum;
  char chr, ch[2];
  char *rsp;
  int error_in_cmd_rsp_parse=0;
//int rsp_len;
  // Start parsing the response according to the format string.
  // If we don't get what we expect, set error_in_cmd_rsp_parse
  // so that the response is output in 'send' format.
  // Only output the parameters in the response format, not ASCII's
  char *pcmd;
  char *prsp;
  int got_fmt_rsp=0;
  #if 0
  AsconStdHandler(a);
  /*
  ffr If AsconStdHandler doesn't see a '\r' or '\n' and there are no more
  chars to read, it leaves state=AsconReading and the rdBuffer char array
  without a null terminator.  So we assume that higher level code can tell
  if the response is OK and set the state and terminator.
  */
  DynStringConcatChar(a->rdBuffer, '\0');
  a->state = AsconReadDone;
  #else
//if (0 == OrdReadChars(a)) return 0;
  #endif
  pcmd = response;
  rsp=rdBArray;
//rsp_len = GetDynStringLength(a->rdBuffer);
  prsp = rsp;
  while (*pcmdrspfmt&&!error_in_cmd_rsp_parse)
  {
    if (prsp-rsp>=rsp_len)
      error_in_cmd_rsp_parse=1; // response is too short - no bytes left to parse
    else if (*pcmdrspfmt=='%') // Parse byte(s)
    {
      pcmdrspfmt++;
      if (got_fmt_rsp)
        *pcmd++=' '; // separate response fields with spaces (Tcl list can separate these)
      int slen;
      switch(*pcmdrspfmt++)
      {
        case 's':
          pcmd+=(slen=sprintf(pcmd,"%s",prsp));
          *--pcmd='\0'; // string should have been terminated (with a 'z') but that gets checked later
          prsp+=slen-1;
          break;
        case 'd':
          pcmd+=sprintf(pcmd,"%d",*prsp++);
          break;
        case 'B':
          pcmd+=sprintf(pcmd,"%c%d",(prsp[0]&0x8)?'y':'x',(prsp[0]&0x7)+((prsp[0]&0x10)>>1));
          prsp++;
          break;
        case 'P':
          if (rsp_len-(prsp-rsp)<2)
            error_in_cmd_rsp_parse=1;
          else
          {
            bnum =0x3^( (prsp[0]&0x7)*0x10+(prsp[0]&0x10)*0x8+prsp[1] );
            pcmd+=sprintf(pcmd,"%c%d",(prsp[0]&0x8)?'y':'x',bnum);
            prsp+=2;
          }
          break;
        case 'A':
          error_in_cmd_rsp_parse|=(prsp[0]!=0x06&&prsp[0]!=0x15);
          pcmd+=sprintf(pcmd,"%s",(prsp[0]==0x06)?"ACK":"NAK");
          prsp++;
          break;
        default:
//        a->state = AsconReadDone;
//        AsconError(a, "Unknown % specification in response format", 0);
          sprintf(errmsg, "Unknown specification for response format");
          return -1;
      }
      got_fmt_rsp=1;
    }
    else // expected ASCII characters in response have to match response format
      error_in_cmd_rsp_parse|=(*prsp++!=*pcmdrspfmt++);
  }
  if (error_in_cmd_rsp_parse) {
    idx=0;
    idx=sprintf(errmsg,"Bad response format: ");
    for (i = 0; i < rsp_len && idx < 31; ++i) {
      if (response[i] < 32 || response[i] > 126)
        idx+=sprintf(&errmsg[idx], "%02Xh", response[i]);
      else
        errmsg[idx++] = response[i];
    }
//  if (idx < 31)
      errmsg[idx++] = '\0';

//    a->state = AsconReadDone;
//    AsconError(a, errmsg, 0);
    return -1;
  } else {
//  a->state = AsconReadDone;
//  DynStringReplace(a->rdBuffer,response,0);
    return 1;
  }
}

int OrdHVPSReading(Ascon *a) {
  int ret, rsp_len;
  static int retries = MAXRETRY;
  char chr, errmsg[ERRMSGLEN], response[MAXARGLEN];

  ret = AsconReadChar(a->fd, &chr);
  if (ret > 0 && chr == NAK && retries > 0) {
    AsconReadGarbage(a->fd);
    a->state = AsconWriting;
    a->wrPos = 0;
    retries--;
    return 0;
  } else if (retries <= 0) {
    a->state = AsconReadDone;
    AsconError(a, "Received  too many NAKs", 0);
    retries = MAXRETRY;
    return 0;
  }
  retries = MAXRETRY;
  while (ret > 0) {
    a->start = DoubleTime();

    if (ACK == chr) {
      a->state = AsconReadDone;
      DynStringConcatChar(a->rdBuffer, chr);
    } else if (chr == 'z') {
      a->state = AsconReadDone;
      DynStringConcatChar(a->rdBuffer, 'z');
      break;
    } else {
      if (DynStringConcatChar(a->rdBuffer, chr) == 0) {
        a->state = AsconReadDone;
        AsconError(a, "DynStringConcatChar failed:", ENOMEM);
        return 0;
        break;
      }
    }
    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 0;
        } else {
          return 0;
        }
      }
  }
  rsp_len = GetDynStringLength(a->rdBuffer);
  if (OrdFmtReply(GetCharArray(a->rdBuffer), rsp_len, response, errmsg) >= 0) {
    DynStringCopy(a->rdBuffer,response);
    return 0;
  } else {
    AsconError(a, errmsg, 0);
    return 0;
  }
}

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

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

void AddOrdHVPSProtocoll(){
  AsconProtocol *prot = NULL;

  pcmdrspfmt = (char *) malloc(128);
  prot = calloc(sizeof(AsconProtocol), 1);
  prot->name = strdup("ordhvps");
  prot->init = AsconStdInit;
  prot->handler = OrdHVPSProtHandler;
  AsconInsertProtocol(prot);
}