/** @file Modbus protocol handler for script-context based controllers.
*
*/
#include <errno.h>
#include <ascon.h>
#include <ascon.i>
#include <dynstring.h>
#include <stdbool.h>
#include <math.h>

static int dbgprintf(char* fmtstr, ...);
static int dbgprintx(const char* msg, const unsigned char* cp, int blen);
static unsigned int debug_modbus = 1;

/*-------------------------------------------------------------------------*/
/* IEEE code copied from psi/modbus.c */
static void double2ieee(double input, char ieee[4]) {

/* convert double to IEEE 32 bit floating number (denormalized numbers are considered as zero) */

  long mantissa;
  int exponent;

  if (input == 0) {
    ieee[0]= 0;
    ieee[1]= 0;
    ieee[2]= 0;
    ieee[3]= 0;
  } else {
    mantissa = 0x1000000 * (frexp(fabs(input), &exponent));
    exponent = exponent - 1 + 127;
    if (exponent < 0) {
      exponent = 0;
    } else if (exponent > 0xFE) {
      exponent = 0xFE;
    }
    if (input < 0) {
      ieee[0] = 0x80 | (exponent >> 1);
    } else {
      ieee[0] = exponent >> 1;
    }
    ieee[1] = (exponent & 1) << 7 | ((mantissa & 0x7F0000) >> 16);
    ieee[2] = (mantissa & 0xFF00) >> 8;
    ieee[3] = mantissa & 0xFF;
  }
  return;
}

/*-------------------------------------------------------------------------*/
static double ieee2double(char ieee[4]) {

/* IEEE 32 bit floating number to double (denormalized numbers are considered as zero) */

  long mantissa;
  double output;
  int exponent;
  
  mantissa = ((ieee[1] << 16) & 0x7FFFFF) 
           | ((ieee[2] <<  8) & 0xFF00)
           | ((ieee[3]      ) & 0xFF);
  
  exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
  if (exponent == 0 && mantissa == 0) {
    return 0.0;
  }
  output = ldexp(mantissa, -23) + 1.0;
  if (ieee[0] & 0x80) {
    output = -output;
  }
  return output * ldexp(1, exponent - 127);
}
/*-------------------------------------------------------------------------*/

/** @brief encode modbus request
* TODO: clean me up
*/
int ModbusWriteStart(Ascon *a) {
  unsigned int dev, cmd, reg;
  int len = GetDynStringLength(a->wrBuffer);
  char* buff = NULL;
  char temp[32];
  char* endp = NULL;
  int idx = 6;
  bool do_float = false;
  temp[0] = 0; /* transaction id */
  temp[1] = 0;
  temp[2] = 0; /* protocol id */
  temp[3] = 0;

  DynStringConcatChar(a->wrBuffer, 0);
  buff = GetCharArray(a->wrBuffer);
  dbgprintf("modbus-wr:%s\n", buff);
  dev = strtoul(buff, &endp, 10);
  if (endp == buff || dev < 1 || dev > 39) {
    dbgprintf("modbus-er: Bad device id: %d from %s\n", dev, buff);
    a->state = AsconIdle;
    AsconError(a, "Bad device id", 0);
    return 0;
  }
  temp[idx++] = dev;
  buff = endp + 1;
  cmd = strtoul(buff, &endp, 10);
  if (endp == buff || (cmd != 3 && cmd != 16 && cmd != 1003 && cmd != 1016)) { /* read/write registers */
    dbgprintf("modbus-er: Bad command id: %d from %s\n", cmd, buff);
    a->state = AsconIdle;
    AsconError(a, "Bad command id", 0);
    return 0;
  }
  if (cmd > 1000) {
    cmd %= 1000;
    do_float = true;
  } else {
    do_float = false;
  }
  temp[idx++] = cmd;
  buff = endp + 1;
  reg = strtoul(buff, &endp, 10);
  if (endp == buff || reg > 65535) {
    dbgprintf("modbus-er: Bad register id: %d from %s\n", reg, buff);
    a->state = AsconIdle;
    AsconError(a, "Bad register id", 0);
    return 0;
  }
  temp[idx++] = (reg >> 8) & 0xFF;
  temp[idx++] = reg & 0xFF;
  temp[idx++] = 0; /* word count msbyte */
  if (do_float)
    temp[idx++] = 2; /* word count lsbyte */
  else
    temp[idx++] = 1; /* word count lsbyte */
  if (cmd == 16) { /* write registers */
    buff = endp + 1;
    if (do_float) {
      struct { char v[4]; } ieee;        /* big endian */
      double val = strtod(buff, &endp);
      if (endp == buff) {
        dbgprintf("modbus-er: Bad value: %f from %s\n", val, buff);
        a->state = AsconIdle;
        AsconError(a, "Bad value", 0);
        return 0;
      }
      double2ieee(val, ieee.v);
      dbgprintf("strtod:%f\n", val);
      dbgprintx("double2ieee", (unsigned char*)ieee.v, 4);
      temp[idx++] = 4; /* byte count */
      temp[idx++] = ieee.v[2]; /* watlow RUI specific byte ordering */
      temp[idx++] = ieee.v[3];
      temp[idx++] = ieee.v[0];
      temp[idx++] = ieee.v[1];
    } else {
      unsigned int val;
      val = strtoul(buff, &endp, 10);
      if (endp == buff || val > 65535) {
        dbgprintf("modbus-er: Bad value: %d from %s\n", val, buff);
        a->state = AsconIdle;
        AsconError(a, "Bad value", 0);
        return 0;
      }
      temp[idx++] = 2; /* byte count */
      temp[idx++] = (val >> 8) & 0xFF;
      temp[idx++] = val & 0xFF;
    }
  }
  len = idx - 6;
  temp[4] = len >> 8;   /* length msbyte */
  temp[5] = len & 0xFF; /* length lsbyte */

  if (debug_modbus > 0) {
    dbgprintx("modbus-xo", (unsigned char*)temp, idx);
  }
  DynStringReplaceWithLen(a->wrBuffer, temp, 0, idx);
  a->state = AsconWriting;
  a->wrPos = 0;
  return 1;
}

/** @brief decode modbus response
* TODO: clean me up
*/
int ModbusReading(Ascon *a) {
  int ret, blen, rlen;
  char chr = '\0';
  unsigned char* cp = NULL;

  ret = AsconReadChar(a->fd, &chr);
  while (ret > 0) {
    a->start = DoubleTime();

    DynStringConcatChar(a->rdBuffer, chr);
    cp = (unsigned char*) GetCharArray(a->rdBuffer);
    blen = GetDynStringLength(a->rdBuffer);
    if (debug_modbus > 0) {
      dbgprintx("modbus-xi", cp, blen);
    }
    if (blen >= 6) {
      int mlen = (cp[4] << 8) + cp[5];
      if (blen - 6 >= mlen) {
        char temp[64];
        if (cp[7] == 3 && cp[8] == 2) {
          rlen = snprintf(temp, 64, "%d", (((unsigned int)cp[9]) << 8) + (unsigned int)cp[10]);
        }
        else if (cp[7] == 3 && cp[8] == 4) {
          struct { char v[4]; } ieee;
          double val;
          ieee.v[2] = cp[9]; /* watlow RUI specific byte ordering */
          ieee.v[3] = cp[10];
          ieee.v[0] = cp[11];
          ieee.v[1] = cp[12];
          val = ieee2double(ieee.v);
          dbgprintx("rawhex", (unsigned char*)&cp[9], 4);
          dbgprintf("ieee2double:%f\n", val);
          rlen = snprintf(temp, 64, "%g", val);
        }
        else if (cp[7] == 16 && cp[11] == 1) {
          rlen = snprintf(temp, 64, "OK int");
        }
        else if (cp[7] == 16 && cp[11] == 2) {
          rlen = snprintf(temp, 64, "OK float");
        }
        else if (((unsigned int)cp[7]) == 0x83) {
          rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);
        }
        else if (((unsigned int)cp[7]) == 0x90) {
          rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);
        }
        else {
          rlen = snprintf(temp, 64, "ASCERR:%02x:%d", cp[7], cp[8]);
        }
        if (debug_modbus > 0) {
          dbgprintx("modbus-xi", cp, blen);
        }
        dbgprintf("modbus-rd:%s\n", temp);
        DynStringReplaceWithLen(a->rdBuffer, temp, 0, rlen);
        a->state = AsconReadDone;
        return 1;
      }
    }
    ret = AsconReadChar(a->fd, &chr);
  }
  if (ret < 0) {
    AsconError(a, "AsconReadChar failed:", errno);
    return 0;
  }
  if (a->state == AsconReadDone) {
    DynStringConcatChar(a->rdBuffer, '\0');
  } else {
    if (a->timeout > 0) {
      if (DoubleTime() - a->start > a->timeout) {
        AsconError(a, "read timeout", 0);
        a->state = AsconTimeout;
      }
    }
  }
  return 0;
}

/** @brief Modbus TCP protocol handler.
* This handler encodes commands and decodes responses
* for the Modbus TCP protocol
*/
int ModbusProtHandler(Ascon *a) {
  int ret;

  switch(a->state){
    case AsconWriteStart:
      ret = ModbusWriteStart(a);
      return ret;
      break;
    case AsconReadStart:
      a->start = DoubleTime();
      ret = AsconStdHandler(a);
      return ret;
      break;
    case AsconReading:
      ret = ModbusReading(a);
      return ret;
      break;
    default:
      ret = AsconStdHandler(a);
      return ret;
      break;
  }
  return 1;
}

void AddModbusProtocoll(){
	AsconProtocol *prot = NULL;
	
	prot = calloc(sizeof(AsconProtocol), 1);
	prot->name = strdup("modbus");
	prot->init = AsconStdInit;
	prot->handler = ModbusProtHandler;
	AsconInsertProtocol(prot);
}

#include <stdio.h>
#include <stdarg.h>

static int dbgprintf(char* fmtstr, ...) {
  if (debug_modbus > 0) {
    FILE* fp = NULL;
    int ret = 0;
    fp = fopen("/tmp/modbus.txt", "a");
    if (fp != NULL) {
      va_list ap;
      va_start(ap, fmtstr);
      ret = vfprintf(fp, fmtstr, ap);
      va_end(ap);
      fclose(fp);
    }
    return ret;
  }
  return 0;
}
static int dbgprintx(const char* msg, const unsigned char* cp, int blen) {
  if (debug_modbus > 0) {
    char tmp[128];
    int i, j;
    const char hex[] = "0123456789ABCDEF";
    for (i = 0, j = 0; i < blen && j < 126; ++i) {
      tmp[j++] = hex[(cp[i] >> 4) & 0xF];
      tmp[j++] = hex[(cp[i]     ) & 0xF];
    }
    tmp[j++] = '\0';
    return dbgprintf("%s: %s\n", msg, tmp);
  }
  return 0;
}