/** @file TCP 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>
#include <fupa.h>

#define ERRLEN 128
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 {
  /* frexp -> 0.f * 2^23 */
  /* 2^24 * fract instead of 2^23  because we get 0.(f/2) not 1.f from frexp and exponent = e + 1*/
    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(unsigned char ieee[4]) {

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

  long mantissa;
  double output, norm = 1;
  int exponent, bias = 127;

  mantissa = ((ieee[1] << 16) & 0x7F0000)
           | ((ieee[2] <<  8) & 0xFF00)
           | ((ieee[3]      ) & 0xFF);

  exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
  if (exponent == 255) {
    if (mantissa == 0) {
      /*TODO INF */
    } else {
      /*TODO NAN */
    }
  }
  if (exponent == 0) {
    if (mantissa == 0) {
      return 0.0;
    } else {
      bias = 126;
      norm = 0;
    }
  }
  output = ldexp(mantissa, -23) + norm;
  if (ieee[0] & 0x80) {
    output = -output;
  }
  return output * ldexp(1, exponent - bias);
}
/*-------------------------------------------------------------------------*/


/*
  NumVal = number of values to read or write, ie number of coils, input/outputs or registers.
  Number of bytes = NumVal * (number of bytes in Dtype)
*/
enum mbDtypes {U16, U32, F32};
struct cmdPar {
  unsigned int MBAddr, Fcode, SAddr, NumVal, NumBytes, NumRd, NumWr;
  enum mbDtypes Dtype;
  double MBdata[2000];
};

enum MBFCODES { RdCoil=1, RdInp=2, RdHldReg=3, RdInpReg=4, WrCoil=5, WrReg=6, WrMCoils=15, WrMRegs=16 };
static struct cmdPar MBcmd;
static int BO[4];

/** @brief Parse a command string of the form "MBAddr:Fcode:SAddr:Dtype:MBdata"
Dtype U16, U32, F32 with optional byte order array eg U32[3,2,1,0]
*/
void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
  int len;
  unsigned int i;
  char *nextPar, dataType[4];
  double dVal;

  MBC->MBAddr = strtoul(cmdLine, &nextPar, 0);
  cmdLine = nextPar+1;
  MBC->Fcode = strtoul(cmdLine, &nextPar, 0);
  cmdLine = nextPar+1;
  MBC->SAddr = strtoul(cmdLine, &nextPar, 0);
  cmdLine = nextPar+1;
  if (MBC->Fcode != WrCoil) {
    MBC->NumVal = strtod(cmdLine, &nextPar);
    cmdLine = nextPar + 1;
    strncpy(dataType,cmdLine,3);
    dataType[3] = '\0';
    cmdLine += 3;
    nextPar = cmdLine;
    if (strcmp(dataType, "U16") == 0) {
      MBC->Dtype = U16;
    } else if (strcmp(dataType, "U32") == 0) {
      MBC->Dtype = U32;
    } else if (strcmp(dataType, "F32") == 0) {
      MBC->Dtype = F32;
    }
    if (*cmdLine == '[') {
      cmdLine++;
      BO[0] = strtol(cmdLine, &nextPar, 10);
      cmdLine = nextPar + 1;
      BO[1] = strtol(cmdLine, &nextPar, 10);
      cmdLine = nextPar + 1;
      BO[2] = strtol(cmdLine, &nextPar, 10);
      cmdLine = nextPar + 1;
      BO[3] = strtol(cmdLine, &nextPar, 10);
    }
    cmdLine = nextPar + 1;
  }
  switch (MBC->Fcode) {
    case RdCoil:
    /* Coils are numbered from 1 but addressed from 0 */
      MBC->SAddr--;
      MBC->NumBytes = MBC->NumVal;
      MBC->NumRd = MBC->NumVal;
    break;
    case WrCoil:
      MBC->SAddr--;
      dVal = strtod(cmdLine, &nextPar);
      if (dVal == 1) {
        MBC->MBdata[0] = 255;
      } else if (dVal == 0) {
        MBC->MBdata[0] = 0;
      } else {
        /* TODO ERROR */
      }
    break;
    case RdHldReg:
      if (MBC->Dtype == F32 || MBC->Dtype == U32) {
        MBC->NumRd = MBC->NumVal * 2;
      } else if (MBC->Dtype == U16) {
        MBC->NumRd = MBC->NumVal;
      } else {
        /* TODO ERROR */
      }
    break;
    case WrMRegs:
      for (i=0; i < MBC->NumVal; i++) {
        MBC->MBdata[i] = strtod(cmdLine, &nextPar);
        cmdLine = nextPar + 1;
      }
      if (MBC->Dtype == F32 || MBC->Dtype == U32) {
        MBC->NumWr = MBC->NumVal * 2;
        MBC->NumBytes = MBC->NumVal * 4;
      } else if (MBC->Dtype == U16) {
        MBC->NumWr = MBC->NumVal;
        MBC->NumBytes = MBC->NumVal * 2;
      }
    break;
  }
}
/** @brief encode modbus request
*/
int TCPMBWriteStart(Ascon *a) {
  int ADUlen, PDUlenPlusUID;
  unsigned int i, j;
  char *cmdLine;
  unsigned char ADU[32], ieee[4];


  cmdLine = GetCharArray(a->wrBuffer);
  for (i=0; i < 4; i++) BO[i] = i;
  parseCmd(cmdLine, &MBcmd, BO);

  ADU[0] = 0;
  ADU[1] = 0;
  ADU[2] = 0;
  ADU[3] = 0;
  ADU[6] = MBcmd.MBAddr;
  ADU[7] = MBcmd.Fcode;
  switch (MBcmd.Fcode) {
    case RdCoil:
    case RdHldReg:
    /* TODO Max NumRd = 125 */
      ADU[8]  = (MBcmd.SAddr & 0xFF00) >> 8;
      ADU[9]  =  MBcmd.SAddr & 0xFF;
      ADU[10] = (MBcmd.NumRd & 0xFF00) >> 8;
      ADU[11] =  MBcmd.NumRd & 0xFF;
      ADUlen  = 12;
    break;
    case WrCoil:
      ADU[8]  = (MBcmd.SAddr & 0xFF00) >> 8;
      ADU[9]  =  MBcmd.SAddr & 0xFF;
      ADU[10] = MBcmd.MBdata[0];
      ADU[11] = 0;
      ADUlen  = 12;
    break;
    case WrMRegs:
      ADU[8]  = (MBcmd.SAddr & 0xFF00) >> 8;
      ADU[9]  =  MBcmd.SAddr & 0xFF;
      ADU[10] = (MBcmd.NumWr & 0xFF00) >> 8;
      ADU[11] =  MBcmd.NumWr & 0xFF;
      ADU[12] =  MBcmd.NumBytes;
      switch (MBcmd.Dtype) {
        case U16:
          for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
            ADU[j + BO[0]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF00)     >>  8;
            ADU[j + BO[1]] =  (unsigned int)MBcmd.MBdata[i] & 0xFF;
          }
        break;
        case U32:
          for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
            ADU[j + BO[0]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF000000) >> 24;
            ADU[j + BO[1]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF0000)   >> 16;
            ADU[j + BO[2]] = ((unsigned int)MBcmd.MBdata[i] & 0xFF00)     >>  8;
            ADU[j + BO[3]] =  (unsigned int)MBcmd.MBdata[i] & 0xFF;
          }
        break;
        case F32:
          for (i=0, j=13; i < MBcmd.NumVal; i++, j+=4) {
            double2ieee(MBcmd.MBdata[i], ieee);
            ADU[j + BO[0]] = ieee[0];
            ADU[j + BO[1]] = ieee[1];
            ADU[j + BO[2]] = ieee[2];
            ADU[j + BO[3]] = ieee[3];
          }
        break;
      }
      ADUlen  = 13 + MBcmd.NumBytes;
    break;
    default:
    // Not Implemented
    break;
  }
  /* Length field in MBAP includes byte 7 of the MBAP, so Length = ADU length - 6. */
  PDUlenPlusUID = ADUlen - 6;
  ADU[4]  = (PDUlenPlusUID & 0xFF00) >> 8;
  ADU[5]  =  PDUlenPlusUID & 0xFF;
  DynStringClear(a->wrBuffer);
  DynStringConcatBytes(a->wrBuffer, ADU, ADUlen);
  a->state = AsconWriting;
  a->wrPos = 0;
  return 1;
}


/** @brief decode modbus response
*/
/* ADU[4] = MSB of DatLen, ADU[5] = LSB of DatLen */
static  int aduLen = 0, RespLen = 0, pduLen = 0, DatLen = 0;
  /* NOTE: Min aduLen is 9. The header (7 bytes) + an Error code and Exception code */
  #define ADUSIZE 17
  static const int TIDidx=0, PIDidx=2, LENidx=4, UIDidx=6, CODEidx=7, DATLENidx=8, DATAidx=9;
static unsigned char ADU[ADUSIZE]; /* Allows upto 8 bytes if data */
int TCPMBReading(Ascon *a) {
  const int MBAPLen = 7, UIDpos=6;
  int i, ret, rlen, byteCnt;
  char chr, temp[64], errMsg[ERRLEN];
  unsigned char ieee[4];
  double dval;
  long int lival;

  ret = AsconReadChar(a->fd, &chr);
  while (ret > 0) {
    a->start = DoubleTime();
    ADU[aduLen++] = chr;
    ret = AsconReadChar(a->fd, &chr);
  }

  if (RespLen == 0 && aduLen > CODEidx) {
    RespLen = (ADU[LENidx] << 8) + ADU[LENidx+1];
  }

  if (aduLen < (UIDpos + RespLen)) {
    if (ret == 0) {
      if (a->timeout > 0) {
        if (DoubleTime() - a->start > a->timeout) {
          AsconError(a, "read timeout", 0);
          a->state = AsconTimeout;
        }
      }
    } else if (ret < 0) {
      AsconError(a, "AsconReadChar failed:", errno);
    }
    return 0;
  }

  if (ADU[CODEidx] > 0x80) {
    a->state = AsconReadDone;
    snprintf(errMsg, ERRLEN, "TCPMODBUS: Function code %d exception %d:", ADU[CODEidx] & 0x0F, ADU[8] &0x0F);
    AsconError(a, errMsg, 0);
    /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */
    a->lastReconnect = DoubleTime();
  } else if (ADU[CODEidx] != MBcmd.Fcode) {
    a->state = AsconReadDone;
    snprintf(errMsg, ERRLEN, "TCPMODBUS: Requested function %d but got response for %d:", MBcmd.Fcode, ADU[CODEidx]);
    AsconError(a, errMsg, 0);
    /*TODO This hack stops ascon.c:AsconTask() from needlessly closing the connection. Remove this when it's no longer needed */
    a->lastReconnect = DoubleTime();
  } else {
    DynStringClear(a->rdBuffer);
    switch (MBcmd.Fcode) {
      case RdCoil:
        byteCnt = ADU[8];
        for (i=0; i < byteCnt; i++) {
          rlen = snprintf(temp, 64, "%ld", ADU[9+i]);
          DynStringConcat(a->rdBuffer, temp);
          DynStringConcatChar(a->rdBuffer, ' ');
        }
        a->state = AsconReadDone;
        break;
      case RdHldReg:
        byteCnt = ADU[8];
        switch (MBcmd.Dtype) {
          case U16:
            for (i=0; i < byteCnt/2; i++) {
              ieee[ BO[0] ] = ADU[9+i*2];
              ieee[ BO[1] ] = ADU[10+i*2];
              lival = (ieee[0] << 8) | ieee[1];
              rlen = snprintf(temp, 64, "%ld", lival);
              DynStringConcat(a->rdBuffer, temp);
              DynStringConcatChar(a->rdBuffer, ' ');
            }
            break;
          case U32:
          case F32:
            for (i=0; i < byteCnt/4; i++) {
              ieee[ BO[0] ] = ADU[9+i*4];
              ieee[ BO[1] ] = ADU[10+i*4];
              ieee[ BO[2] ] = ADU[11+i*4];
              ieee[ BO[3] ] = ADU[12+i*4];
              if (MBcmd.Dtype == F32) {
                dval = ieee2double(ieee);
                rlen = snprintf(temp, 64, "%g", dval);
              } else if (MBcmd.Dtype == U32) {
                lival = (ieee[0] << 24) | (ieee[1] << 16) | (ieee[2] << 8) | ieee[3];
                rlen = snprintf(temp, 64, "%ld", lival);
              }
              DynStringConcat(a->rdBuffer, temp);
              DynStringConcatChar(a->rdBuffer, ' ');
            }
            break;
          default:
            break;
        }
        a->state = AsconReadDone;
        break;
      case WrCoil:
        if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr &&
            ADU[10] == MBcmd.MBdata[0]) {
          DynStringReplace(a->rdBuffer, "OK", 0);
        } else {
          DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0);
        }
        a->state = AsconReadDone;
        break;
      case WrMRegs:
        if (ADU[7] == MBcmd.Fcode && (ADU[8] << 8 | ADU[9]) == MBcmd.SAddr &&
            (ADU[10] << 8 | ADU[11]) == MBcmd.NumWr) {
          DynStringReplace(a->rdBuffer, "OK", 0);
        } else {
          DynStringReplace(a->rdBuffer, "ASCERR: Response doesn't match set request", 0);
        }
        a->state = AsconReadDone;
        break;
      default:
        break;
    }
  }
  aduLen = 0, DatLen = 0, RespLen = 0;
  memset(ADU, 0, ADUSIZE);
  return 0;
}

int TCPMBUtil(SConnection *pCon, SicsInterp *pSics, void *pData, int argc, char *argv[]) {
  char cmdLine[512], msg[512];
  unsigned long ul;
  unsigned char ieee[4];
  double dval;
  int iRet, iNumCmds = 3;
  FuPaResult PaRes;
  FuncTemplate CommandTemplate[] = {
    {"help",0,{0,0}},
    {"ieee2double",1,{FUPATEXT}},
    {"double2ieee",1,{FUPAFLOAT}},
    {NULL}
  };

  Arg2Text(argc, argv, cmdLine, 511);
  iRet = EvaluateFuPa((pFuncTemplate)&CommandTemplate,iNumCmds,argc-1,&argv[1],&PaRes);
  switch(iRet) {
    case 1:
      ul = 0xFFFFFFFF & strtoul(argv[2],NULL,16);
      ieee[3] = ul & 0xFF;
      ieee[2] = (ul & 0xFF00) >>  8;
      ieee[1] = (ul & 0xFF0000) >> 16;
      ieee[0] = (ul & 0xFF000000) >> 24;
      dval = ieee2double(ieee);
      snprintf(msg, 512, "%s -> %f", argv[1], dval);
      SCWrite(pCon, msg, eValue);
    break;
    case 2:
    break;
    case 0:
    default:
    break;
  }
  return 1;
}

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

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

void AddTCPMBProtocol(){
	AsconProtocol *prot = NULL;

  AddCommand(pServ->pSics, "fermi", TCPMBUtil, NULL, NULL);
	prot = calloc(sizeof(AsconProtocol), 1);
	prot->name = strdup("tcpmodbus");
	prot->init = AsconStdInit;
	prot->handler = TCPMBProtHandler;
	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/fermiskfMB.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;
}