/******************************************************************************
* Process data from PBLM type SIM900 modules                                  *
*                                                                             *
* Message format:                                                             *
* MSG N,#LCCCAAA...AAA\r\n                                                    *
* ...                                                                         *
* MSG N,#LCCCAAA...AAA\r\n\r\n                                                *
*                                                                             *
* where N is the chassis slot number                                          *
*       L is the number of digits of C                                        *
*       C is the number of characters to the end of the current line          *
*       AAA...AAA is the C characters of data, same format as LBLM data       *
* There are two or more MSG... lines with the 224 character BLM data. Each    *
* line ends with a <CR><LF>, and an extra <CR><LF> on the last line.          *
*                                                                             *
* The characters in the AAA.AAA portion of each message are concatenated as   *
* they arrive. After the last message in a series has been added, the         *
* string of concatenated messages is parsed, and the values pushed to outputs.*
*                                                                             *
* The data consists of 28 values. There are only 20 aSub outputs, so the 17   *
* floating point values are put in an array, to be directed to corresponding  *
* ai records by a transform record.                                           *
*                                                                             *
*	Register name			byte offset	byte width	numeric type              *
*	FW_VERSION				0			2			uint16                    *
*	HW_REVISION				2			2			uint16                    *
*	COMMIT_SHA				4			4			uint32                    *
*	UP_TIME_CNT				8			4			uint32                    *
*	UP_TIME_CNT_L			12			4			uint32                    *
*	BLM_STS_REG				16			4			uint32                    *
*	BLM_STS_REG_L			20			4			uint32                    *
*	LBLM_STS_REG			24			4			uint32                    *
*	LBLM_STS_REG_L			28			4			uint32                    *
*	DSP_TEMP_C				32			4			float                     *
*	OUTPUT_FREQ_DAC			36			4			float                     *
*	OPAMP_GAIN				40			4			float                     *
*	ADC_BUF_OFFSET			40			4			float                     *
*	ADC_BUF_VPP				44			4			float                     *
*	WAVEFORM_OFFSET_DAC		48			4			float                     *
*	WAVEFORM_GAIN_DAC		52			4			float                     *
*	PHASE_DET				56			4			float                     *
*	SELF_CHECK_HIGH			60			4			float                     *
*	SELF_CHECK_LOW			64			4			float                     *
*	DSP_SP					68			4			float                     *
*	DSP_MON_HV				72			4			float                     *
*	DSP_SET_HV				72			4			float                     *
*	LOCK_IN_AMP_FIR			76			4			float                     *
*	LOCK_IN_AMP_SMM			80			4			float                     *
*	LOCK_IN_AMP_IIR			84			4			float                     *
*	LOCK_IN_AMP_GAIN		84			4			float                     *
*	LOCK_IN_PHASE_FIR		88			4			float                     *
*	LOCK_IN_PHASE_SMM		92			4			float                     *
*	LOCK_IN_PHASE_IIR		96			4			float                     *
*	PMT_GAIN_CALIB_FACTOR	96			4			float                     *
*                                                                             *
* NOTE: floats are an ASCII representation of the IEEE 32 bit floating point  *
* format, e.g. the raw data for a value of 28.0 is                            *
*                                                                             *
* 0x34 0x31 0x45 0x30 0x30 0x30 0x30 0x30                                     *
*                                                                             *
* Which is the ASCII code for                                                 *
*                                                                             *
* 41E00000                                                                    *
*                                                                             *
* Which is the IEEE-754 floating point representation of 28.0.                *
*                                                                             *
******************************************************************************/

#include <epicsStdioRedirect.h>
#include <epicsStdlib.h>
#include <stdint.h>
#include <string.h>

#include <epicsExport.h>
#include <registry.h>
#include <registryFunction.h>
#include <aSubRecord.h>
#include <recSup.h>
#include <recGbl.h>
#include <epicsTypes.h>
#include <alarm.h>
#include <errlog.h>
#include <epicsTime.h>

#include <iostream>
#include <string>
#include <sstream>
#include <iomanip>
using namespace std;

#undef DEBUG

static char tsbuffer[64];
static char *timestamp() {
   epicsTimeStamp ts_current;
   epicsTimeGetCurrent(&ts_current);
   epicsTimeToStrftime(tsbuffer, sizeof(tsbuffer), "%Y%m%d_%H:%M:%S.%06f", &ts_current);
   return tsbuffer;
   }


static long blmSim900Init(aSubRecord *prec)
{
	short debug = (prec->tpro > 1) ? 1 : 0;
	if (debug)
		printf("Record %s called myAsubInit(%p)\n",
			prec->name, (void*) prec);

	string *sRawRegBuff = new string();
	prec->dpvt = (void *) sRawRegBuff;
	return 0;
}

// Extract a block of register data from an SCPI style data buffer like #abbbxxx... where
// a is the number of characters in b, and b is the number of characters in the
// data buffer.
// The register data is removed from the original message string, and put in another string
//
// Arguments: sMsgLine - a string containing an SCPI style data buffer. Anything past that is
//            left in the string.
//
//            sRawRegPart - a string where the extracted register data is put.
//
// Returns: 0 if data was extracted, -1 if sMsgLine is corrupted, e.g. fewer than bbb characters
int extractRegBuff(struct aSubRecord *prec, string &sMsgLine, string &sRawRegPart)
{
	short tpro = (prec->tpro > 1) ? 1 : 0;
	unsigned int lDigits, msgLength, nCharCnt;
	char formatStr[16] = "#%1u";
	// The number after the # indicates the number of digits in the next number. Assign to lDigits

	// Do some error checking for slot and lDigits non-0
	if ((sscanf(sMsgLine.c_str(), formatStr, &lDigits) != 1) || (lDigits < 1))
	{
		if(tpro)
		{
			printf("[%s] %s: %s expected SCPI style data buffer like #abbbxxx... where a>0, but read %s\n", timestamp(), prec->name, formatStr, sMsgLine.c_str());
		}
		return -1;
	}

	// Update formatStr now that we know there are lDigits characters in the data block.
	// e.g. if lDigits is 3, formatStr becomes "#%1u%3u%n
	sprintf(formatStr,"%s%%%uu%%n", formatStr, lDigits);

	// test that sMsgLine is long enough to hold the expected number of characters
	// Note: For the last line of a set of register data lines, the number of
	// characters in the rest of the string (the bbb portion of #abbbxxxx...)
	// includes the end of the line \r\n that is stripped off by streamdevice.
	if((sscanf(sMsgLine.c_str(), formatStr, &lDigits, &msgLength, &nCharCnt) != 2) || (nCharCnt + msgLength - strlen("\r\n") > (unsigned int) sMsgLine.length()))
	{
		if (tpro) {
			printf("[%s] %s: %s expected at least %u + %u - %u more characters, but there are only %zu in %s\n",
					timestamp(), prec->name, formatStr, nCharCnt, msgLength, strlen("\r\n"), sMsgLine.length(), sMsgLine.c_str());
		}
		// Data is corrupted. Clear the buffer and start over when the next message comes in.
		return -1;
	}

	if (tpro) printf("prec->nea=%d characters received, nCharCnt=%d characters processed, msgLength=%d caracters of data follow, lDigits=%d digits in msgLength, sMsgLine.length()=%zu\n",
			prec->nea, nCharCnt, msgLength, lDigits, sMsgLine.length());

	if (tpro) {
		cout << prec->name << ": sMsgLine=" << sMsgLine << endl;
	}

	sRawRegPart = sMsgLine.substr(nCharCnt, msgLength);
	sMsgLine.erase(0, nCharCnt+msgLength);
	return 0;
}

/* Build bit mask from individual bits. */
static long blmSim900Process(struct aSubRecord *prec)
{
    const size_t sRawRegBuffExpectedLen = 225;
	short tpro = (prec->tpro > 1) ? 1 : 0;

	// String read from hardware contains white spaces and newlines, so we
	// need to specify the number of characters (nea)
	string sMsgLine((const char*) prec->a, prec->nea);

	// Use canned message for simulation if input S != 0
	if(((unsigned char *)prec->s)[0] != 0)
	{
		sMsgLine = "#3117000b0C01f42cb969000EDD9F000EB7720000051B00000518000000000000002741F400003F4CCCCD41A0F5C3404D1400416CD1004480000040400\r\nMSG 1,#31090000000000041A000004040000043BD3580447EF970447EF97040EA600040EA600040EF420043B9CE2AC1A961A8C1A9DCB2C1A9EE84";
	}

	// String where register data is accumulated until complete
	string *sRawRegBuff = (string *)prec->dpvt;

    int* slot = (int*)(prec->b);

	// Registers
	char *FwVersion= (char *) prec->vala;
	static const unsigned int FwVersionSize = 4;
	char *HwRevision= (char *) prec->valb;
	char *CommitSha= (char *) prec->valc;
	static const unsigned int CommitShaEnd = 16;
	uint32_t UpTimeCnt;
	uint32_t UpTimeCntL;
	uint32_t BlmStsReg;
	uint32_t BlmStsRegL;
	uint32_t LblmStsReg;
	uint32_t LblmStsRegL;
	unsigned int i = 0;
	union {
		uint32_t H;
		float F;
	} hexfloat;
	uint32_t DspTempC;
	uint32_t OutputFreqDac;
	uint32_t OpAmpGain;
	uint32_t AdcBufOffset;
	uint32_t AdcBufVpp;
	uint32_t WaveformOffsetDac;
	uint32_t WaveformGainDac;
	uint32_t PhaseDet;
	uint32_t SelfCheckHigh;
	uint32_t SelfCheckLow;
	uint32_t DspSp;
	uint32_t DspMonHv;
	uint32_t DspSetHv;
	uint32_t LockInAmpFir;
	uint32_t LockInAmpSmm;
	uint32_t LockInAmpIir;
	uint32_t LockInAmpGain;
	uint32_t LockInPhaseFir;
	uint32_t LockInPhaseSmm;
	uint32_t LockInPhaseIir;

	/* input */
	if(tpro)
	{
		cout << endl << endl << prec->name << " received serial message: " << sMsgLine << endl;
	}

	// Sometimes this device interlaces partial messages from different slots.
	// Valid responses have between 243 and 255 chars.
	// If message length < 243, we only got a partial message and have to throw it away.
	// If message length > 255, we got another slot's partial message and will discard it below.
	do
	{
		string sRawRegPart;
		if (extractRegBuff(prec, sMsgLine, sRawRegPart) == -1)
		{
			// Can't parse, abort and start fresh with the next message
			if (tpro)
			{
				cout << prec->name << " got unexpected data ";
				cout << ", sMsgLine = " << sMsgLine << ", sRawRegBuff = " << *sRawRegBuff;
				cout << ", input A = " << (const char*) prec->a << endl;
			}
			sRawRegBuff->clear();
			return -1;
		}

		// The hex digits in firmware version and commit sha are lower case, the rest upper case.
		// Check for a lower case hex value with a-f in firmware version, and only upper case hex after the sha
		if ((sRawRegPart.find_first_of("abcdef") < FwVersionSize)
				&& (sRawRegPart.find_first_not_of("0123456789ABCDEF\r\n", CommitShaEnd)
						== string::npos))
		{
			*sRawRegBuff = sRawRegPart;
			if (tpro)
			{
				cout << prec->name
						<< ": first part of register data block found: "
						<< *sRawRegBuff << endl;
			}
		} // If there's already partial register data, and new data is all upper case hex
		else
		{
			*sRawRegBuff += sRawRegPart;
			if (tpro)
			{
				cout << prec->name << ": more of register data block found: "
						<< *sRawRegBuff << ", sRawRegBuff->empty() = " << sRawRegBuff->empty();
				cout << ", sRawRegPart = " << sRawRegPart << " sRawRegPart.find_first_not_of(\"0123456789ABCDEF\") = " << sRawRegPart.find_first_not_of("0123456789ABCDEF") << ", string::npos = " << string::npos;
				cout << ", sMsgLine = " << sMsgLine<< endl;
			}
		}

		// Look for our slot number in message.
		// If it's not there, streamdevice has already swallowed it. Normally it's not, but it's something we can handle.
		if (sMsgLine[0] != '#')
		{
			char myMsgNum[8];
			sprintf(myMsgNum, "MSG %u,", *slot);
			std::size_t slotPos = sMsgLine.find(myMsgNum);
			if (slotPos == std::string::npos)
			{
				if (tpro)
					cout << prec->name << ": \"" << myMsgNum << "\" not found, end of data for slot " << *slot << ". Remaining text in sMsgLine="
							<< sMsgLine << endl;
			}
			else
			{
				// If slot number from line matches our value from record input, start sMsgLine at the next block of data for the next iteration of this loop.
				sMsgLine.erase(0, slotPos + strlen(myMsgNum));

				if (tpro)
				{
					std::cout << "Found more data for slot " << *slot << ": " << sMsgLine << std::endl;
				}
			}
		}

		if (tpro)
		{
			cout << prec->name << ": sRawRegBuff=" << *sRawRegBuff << endl;
		}

	} while (sMsgLine.find('#') != std::string::npos);

	if(tpro)
	{
		cout << "Concatenated message with " << sRawRegBuff->size() << " characters: " << *sRawRegBuff<< endl;
	}
	// If we don't have the full register data yet, stop here and wait for more data
	if(sRawRegBuff->length() < sRawRegBuffExpectedLen)
	{
		if (tpro) {
			printf("%s waiting for %zu hex characters, received %zu so far: %s\n",
					prec->name, sRawRegBuffExpectedLen, sRawRegBuff->length(), sRawRegBuff->c_str());
		}
		return -1;
	}// If there's more data than expected, something is wrong. Erase and start over.
	else if(sRawRegBuff->length() > sRawRegBuffExpectedLen + 1)
	{
		if (tpro) {
			printf("%s expected %zu hex characters, but there are %zu extra in %s\n",
					prec->name, sRawRegBuffExpectedLen, sRawRegBuff->length() - sRawRegBuffExpectedLen, sRawRegBuff->c_str());
		}
		sRawRegBuff->clear();
		return -1;
	}

	sscanf(sRawRegBuff->c_str(), "%4s%4s%8s%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x%8x",
			FwVersion, HwRevision, CommitSha,
			(unsigned int*) &UpTimeCnt,
			(unsigned int*) &UpTimeCntL,
			(unsigned int*) &BlmStsReg,
			(unsigned int*) &BlmStsRegL,
			(unsigned int*) &LblmStsReg,
			(unsigned int*) &LblmStsRegL,
			(unsigned int*) &DspTempC,
			(unsigned int*) &OutputFreqDac,
			(unsigned int*) &OpAmpGain,
			(unsigned int*) &AdcBufOffset,
			(unsigned int*) &AdcBufVpp,
			(unsigned int*) &WaveformOffsetDac,
			(unsigned int*) &WaveformGainDac,
			(unsigned int*) &PhaseDet,
			(unsigned int*) &SelfCheckHigh,
			(unsigned int*) &SelfCheckLow,
			(unsigned int*) &DspSp,
			(unsigned int*) &DspMonHv,
			(unsigned int*) &DspSetHv,
			(unsigned int*) &LockInAmpFir,
			(unsigned int*) &LockInAmpSmm,
			(unsigned int*) &LockInAmpIir,
			(unsigned int*) &LockInAmpGain,
			(unsigned int*) &LockInPhaseFir,
			(unsigned int*) &LockInPhaseSmm,
			(unsigned int*) &LockInPhaseIir);

	if(tpro)
	{
		cout << "Parsed hex values," << std::hex << setfill('0') << endl;
		cout << "FwVersion =	     0x" << std::setw(4) << FwVersion << "," << endl;
		cout << "HwRevision =	     0x" << std::setw(4) << HwRevision << "," << endl;
		cout << "CommitSha =	     0x" << std::setw(8) << CommitSha << "," << endl;
		cout << "UpTimeCnt =	     0x" << std::setw(8) << UpTimeCnt << "," << endl;
		cout << "UpTimeCntL =	     0x" << std::setw(8) << UpTimeCntL << "," << endl;
		cout << "BlmStsReg =	     0x" << std::setw(8) << BlmStsReg << "," << endl;
		cout << "BlmStsRegL =	     0x" << std::setw(8) << BlmStsRegL << "," << endl;
		cout << "LblmStsReg =	     0x" << std::setw(8) << LblmStsReg << "," << endl;
		cout << "LblmStsRegL =	     0x" << std::setw(8) << LblmStsRegL << "," << endl;
		cout << "DspTempC =	         0x" << std::setw(8) << DspTempC << "," << endl;
		cout << "OutputFreqDac =     0x" << std::setw(8) << OutputFreqDac << "," << endl;
		cout << "OpAmpGain =	     0x" << std::setw(8) << OpAmpGain << "," << endl;
		cout << "AdcBufOffset =	     0x" << std::setw(8) << AdcBufOffset << "," << endl;
		cout << "AdcBufVpp =	     0x" << std::setw(8) << AdcBufVpp << "," << endl;
		cout << "WaveformOffsetDac = 0x" << std::setw(8) << WaveformOffsetDac << "," << endl;
		cout << "WaveformGainDac =	 0x" << std::setw(8) << WaveformGainDac << "," << endl;
		cout << "PhaseDet =	         0x" << std::setw(8) << PhaseDet << "," << endl;
		cout << "SelfCheckHigh =	 0x" << std::setw(8) << SelfCheckHigh << "," << endl;
		cout << "SelfCheckLow =	     0x" << std::setw(8) << SelfCheckLow << "," << endl;
		cout << "DspSp =	         0x" << std::setw(8) << DspSp << "," << endl;
		cout << "DspMonHv =	         0x" << std::setw(8) << DspMonHv << "," << endl;
		cout << "DspSetHv =	         0x" << std::setw(8) << DspSetHv << "," << endl;
		cout << "LockInAmpFir =	     0x" << std::setw(8) << LockInAmpFir << "," << endl;
		cout << "LockInAmpSmm =	     0x" << std::setw(8) << LockInAmpSmm << "," << endl;
		cout << "LockInAmpIir =	     0x" << std::setw(8) << LockInAmpIir << "," << endl;
		cout << "LockInAmpGain =	 0x" << std::setw(8) << LockInAmpGain << "," << endl;
		cout << "LockInPhaseFir =	 0x" << std::setw(8) << LockInPhaseFir << "," << endl;
		cout << "LockInPhaseSmm =	 0x" << std::setw(8) << LockInPhaseSmm << "," << endl;
		cout << "LockInPhaseIir =	 0x" << std::setw(8) << LockInPhaseIir << std::dec << endl;
	}

	// bit fields as unsigned integers
	*((epicsUInt32 *)prec->vald) = UpTimeCnt;
	*((epicsUInt32 *)prec->vale) = UpTimeCntL;
	*((epicsUInt32 *)prec->valf) = BlmStsReg;
	*((epicsUInt32 *)prec->valg) = BlmStsRegL;
	*((epicsUInt32 *)prec->valh) = LblmStsReg;
	*((epicsUInt32 *)prec->vali) = LblmStsRegL;

	// floats as hex representation
	hexfloat.H = DspTempC;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = OutputFreqDac;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = OpAmpGain;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = AdcBufOffset;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = AdcBufVpp;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = WaveformOffsetDac;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = WaveformGainDac;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = PhaseDet;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = SelfCheckHigh;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = SelfCheckLow;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = DspSp;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = DspMonHv;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = DspSetHv;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInAmpFir;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInAmpSmm;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInAmpIir;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInAmpGain;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInPhaseFir;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInPhaseSmm;
	((epicsFloat32 *)prec->valj)[i++] = hexfloat.F;
	hexfloat.H = LockInPhaseIir;
	((epicsFloat32 *)prec->valj)[i] = hexfloat.F;

	if(tpro)
	{
		i = 0;
		cout << "As float values"
				<< ",\nDspTempC =	" << ((epicsFloat32 *)prec->valj)[i++] << endl;
		cout << "OutputFreqDac =	" << ((epicsFloat32 *)prec->valj)[i++] << "," << endl;
		cout << "OpAmpGain =	" << ((epicsFloat32 *)prec->valj)[i++] << "," << endl;
		cout << "AdcBufOffset =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "AdcBufVpp =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "WaveformOffsetDac =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "WaveformGainDac =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "PhaseDet =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "SelfCheckHigh =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "SelfCheckLow =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "DspSp =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "DspMonHv =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "DspSetHv =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInAmpFir =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInAmpSmm =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInAmpIir =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInAmpGain =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInPhaseFir =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInPhaseSmm =	" << ((epicsFloat32 *)prec->valj)[i++] <<  "," << endl;
		cout << "LockInPhaseIir =	" << ((epicsFloat32 *)prec->valj)[i] << endl;
	}
	sRawRegBuff->clear();
	return 0;
}


epicsRegisterFunction(blmSim900Init);
epicsRegisterFunction(blmSim900Process);
