/** MonitorModuleFMS.cpp
 *
 *      This file is part of MyMonitor / BOSAssistent / monitor 2.0
 *
 *		Copyright (C) 1996
 *          Thomas Sailer (sailer@ife.ee.ethz.ch, hb9jnx@hb9w.che.eu)
 *
 *      Copyright (C) 1998-2002
 *          Markus Grohmann (markus_grohmann@gmx.de)
 *
 *      Copyright (c) 2002
 *          Stephan Effertz (info@stephan-effertz.de)
 *
 *
 *		(Demodulation parts taken from monitor (c) Markus Grohmann, Thomas Sailor)
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 2 of the License, or
 *      (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* ---------------------------------------------------------------------- */

#include "MonitorModuleFMS.h"
#include <math.h>
#include <algorithm>
#include <sstream>
#include <stdlib.h>
#include "stdio.h"
#include "convert.h"
#include "time.h"
#include "MonitorLogging.h"

#include <iostream>
using namespace std;

#include "memlock.h"
#include "base64.h"
#include "MonitorConfiguration.h"

#ifdef _DEBUG
#undef THIS_FILE
//static char THIS_FILE[]=__FILE__;
#endif

#define MINPREAMBEL 8
#define READ_AHEAD 80*CORRLEN

// Wenn die nachfolgende Variable Definiert ist, wird der mrtty decode f?r FMS genutzt
// Bei MRTTY sollte SUBSAMP auf 1 definiert sein ! Sonst geht NIX !
//#define MRTTY 0

#ifdef WIN32
//#define MON_DEBUG
#endif

//////////////////////////////////////////////////////////////////////
// Konstruktion/Destruktion
//////////////////////////////////////////////////////////////////////

MonitorModuleFMS::MonitorModuleFMS(unsigned int sampleRate,XMLNode* pConfig)
{
	int signallevel=10000 ;

	int vorlaufbits=getNodeInt(*pConfig,"syncbits",8) ;
	int crccheck=getNodeInt(*pConfig,"crc-check",1) ;
	int algorithm=getNodeInt(*pConfig,"algorithmus",0) ;

	initialize(	sampleRate,
						vorlaufbits,
						crccheck,
						(((float)(signallevel))/65535.0),
						false,
						false,
						algorithm,
						false,
						false);
}


MonitorModuleFMS::MonitorModuleFMS(int sampleRate, int vorlaufbits, bool crccheck, float signallevel, bool ignore_Wiederholung, bool ignore_Quittung, int algorithm, bool error_correction, bool force_preambel)
{
	initialize(sampleRate, vorlaufbits, crccheck,  signallevel, ignore_Wiederholung, ignore_Quittung, algorithm, error_correction,  force_preambel);
	}

void MonitorModuleFMS::initialize(int sampleRate, int vorlaufbits, bool crccheck, float signallevel, bool ignore_Wiederholung, bool ignore_Quittung, int algorithm, bool error_correction, bool force_preambel)
{
	float f;
	int i;
	LOG_DEBUG( "FMS Parameter")
	LOG_DEBUG( "sample : " << sampleRate)
	LOG_DEBUG( "syncbits: " << vorlaufbits)
	LOG_DEBUG( "crc    : " << crccheck)
	LOG_DEBUG( "level  : " << signallevel)
	LOG_DEBUG( "Algo   : " << algorithm)
	LOG_DEBUG( "ignW   : " << ignore_Wiederholung)
	LOG_DEBUG( "ignQ   : " << ignore_Quittung)
	LOG_DEBUG( "ErrC   : " << error_correction)
	LOG_DEBUG( "force   : " << force_preambel)

	m_bErrorCorrection=error_correction ;
	m_iAlgorithmus=algorithm ;
	rxstate=0 ;
	distCounter=0 ;


	bit_phase=0 ;
	m_iLastSyncState=0 ;

	summe=0 ;

	FREQ_SAMP=sampleRate ;

	CORRLEN=(int) (FREQ_SAMP / BAUD) ;
	SPHASEINC=(0x10000u * BAUD * SUBSAMP / FREQ_SAMP) ;
	SPHASEINC_BASE=(0x10000u * BAUD * SUBSAMP / FREQ_SAMP) ;

	m_fSignallevel=signallevel ;

	m_lpszStatusTabelle="status" ;
	memset(&fms,0,sizeof(FMS)) ;
	memset(&lasts,0,sizeof(unsigned int[11])) ;
	memset(&m_LastMessage,0,sizeof(FMS)) ;

	lastout=0 ;
	quittung=0 ;
	subsamp=0 ;
	sphase=0 ;
	//
	memset(&vals, 0, sizeof(VALS));

	rxstate = 0;

	for (f = 0, i = 0; i < CORRLEN; i++) {
		corr_mark_i[i] = cos(f);
		corr_mark_q[i] = sin(f);
		//int_corr_mark_i[i] = (int) corr_mark_i[i]*32768 ;
		//int_corr_mark_q[i] = (int) corr_mark_q[i]*32768 ;

		f += 2.0 * M_PI * (float) FREQ_MARK / (float) FREQ_SAMP;
	}

	for (f = 0, i = 0; i < CORRLEN; i++) {
		corr_space_i[i] = cos(f);
		corr_space_q[i] = sin(f);
		//int_corr_space_i[i] = (int) corr_space_i[i]*32768 ;
		//int_corr_space_q[i] = (int) corr_space_q[i]*32768 ;

		f += 2.0 * M_PI * (float) FREQ_SPACE / (float) FREQ_SAMP;
		m_fZwPuffer[i]=0.0 ;
		m_fZwPuffer[CORRLEN+i]=0.0 ;
	}


	vals.quality=vorlaufbits ;
	vals.ctrl=0 ;

	if (crccheck) // Wenn CRC-Check, dann um 20 erh?hen
		vals.quality+=20 ;

	m_lpszName="FMS" ;
	//m_bSuppressQuittung=true ;
	//m_bSuppressDupes=true ;



	set_filters(FREQ_SPACE,FREQ_MARK,BAUD);


	#ifdef FMSDEBUG
		debugFile1 = fopen("fms_in.raw", "wb");
		debugFile2 = fopen("fms_takt.raw", "wb");
		debugFile3 = fopen("fms_pfd.raw", "wb");
		debugFile4 = fopen("fms_4.raw", "wb");
	#endif
}


MonitorModuleFMS::~MonitorModuleFMS()
{
	#ifdef FMSDEBUG
		fclose(debugFile1) ;
		fclose(debugFile2) ;
		fclose(debugFile3) ;
		fclose(debugFile4) ;
	#endif
}

void MonitorModuleFMS::parseConfigData()
{

}

////////////////////////////////////////////////////////////////////
// Sucht im Bereitgestellten Datenpuffer (*buffer) nach enthaltenen
// Bits gem. FMS Codierung
// Jedes einzelne Bit wird an rxbit ?bergeben und dort im Kontext
// analysiert
//

int counter =0 ;


void MonitorModuleFMS::demod(float *buffer, int length)
{
	m_fSignallevel=0.3 ;

	if (m_iAlgorithmus==1)
	{
		demod_se(buffer,length) ;
		return ;
	} ;

	float f, ampl_mark, ampl_space ;

	if (subsamp) {
		int numfill = SUBSAMP - subsamp;
		if (length < numfill) {
			subsamp += length;
			return;
		}
		buffer += numfill;
		length -= numfill;
		subsamp = 0;
	}

 	for (; length >= max(SUBSAMP, CORRLEN) ; length -= SUBSAMP, buffer += SUBSAMP)
	{
		ampl_mark=	fsqr(mac(buffer, corr_mark_i, CORRLEN)) +
					fsqr(mac(buffer, corr_mark_q, CORRLEN)) ;
		ampl_space=	fsqr(mac(buffer, corr_space_i, CORRLEN)) +
					fsqr(mac(buffer, corr_space_q, CORRLEN));

		f =  ampl_mark - ampl_space ;

		dcd_shreg <<= 1;
		dcd_shreg |= (f > 0);

		// check if transition
		//
		if ((dcd_shreg ^ (dcd_shreg >> 1)) & 1) {
			if (sphase < (0x8000u - (SPHASEINC / 2)))
				sphase += SPHASEINC / 8;
			else
				sphase -= SPHASEINC / 8;
		}
		sphase += SPHASEINC;


		summe=summe + ( (f > 0) ? 1 : -1) ;

		if (sphase>=0x10000u)
		{
			sphase &= 0xffffu;
			// curbit = dcd_shreg & 1;

			if ( (summe >= (int) (m_fSignallevel * ((float) (FREQ_SAMP/SPHASEINC_BASE))) )  || (-summe >= (int) (m_fSignallevel * ((float) (FREQ_SAMP/SPHASEINC_BASE)) ) ))
			//(f >= m_fSignallevel) || (f <=-m_fSignallevel)) // Es muss schon ein wenig dahinterstehen ;-)
			{
				// TRACE1("summe: %d\n",summe) ;
				int bit=(summe >= 0) ? 1 : 0 ;
				rxbit(bit);

			} else {
				// ggf. laufende Auswertung abbrechen, da Signal zu schwach
				//
				//TRACE2("FMS: Low Power Signal %d < %d \n",summe,(int) (m_fSignallevel * ((float)(FREQ_SAMP/SPHASEINC_BASE)) )) ;
				if (rxstate) rxstate=0 ;
			} ;
			summe=0 ;
		}
	}

	subsamp = length;
}

///////////////////////////////////////////////////////////
// Verarbeiten des Empfangenen Bits und Untersuchung
// im Kontext bisher empfangener Daten
//

bool MonitorModuleFMS::rxbit(unsigned char bit, bool test)
{
	// #define ITEMLEN 80
	bool returnValue=false ;

	unsigned int	i = 0, j = 0;
	unsigned int filter= 0xffff >> (15 - (vals.quality >= 20 ? vals.quality - 20 : vals.quality));
	unsigned int sync  = 0xff1a & filter;
	char	c[ITEMLEN], *p;
	char	kontrollString[ITEMLEN] ;


	/*	F?llen des Vorlaufstromes aus dem Overflow vom Rxbitstream und Schieben	*/
	for(;i < 10; i++){
		lasts[i] <<= 1;
		lasts[i] |= lasts[i + 1] >> 31;
	}
	lasts[10] <<= 1;
	lasts[10] |= rxbitstream >> 31;
	rxbitstream <<= 1;
	rxbitstream |= !!bit;

	// CString outstring ;
	// itoa(rxbitstream, outstring.GetBuffer(20),16) ;
	// TRACE(outstring) ;

	unsigned int testen=(rxbitstream & filter) ;
	if (!rxstate && (testen==sync)  ) {
		/*	ein Syncwort wurde gefunden	*/
		// TRACE0 ("->SYNC<-") ;
		if (m_iLastSyncState>0)
		{
			m_bPrintError=true ;
			//TRACE0 ("->SYNC<-") ;
			// //DebugMessage("-> SYNC-WORT <- gefunden") ;
		} else {
			m_bPrintError=false ;
			// //DebugMessage("Ohne Pr?ambel: -> SYNC-WORT <- gefunden") ;
		}
		// counter = -1 ;

		rxstate = 1; // In den Empfangsmodus schalten
		rxptr = rxbuf; // und das nachfolgende Auswerten
		return false ;
	}

	if (rxstate)
	{
		if (rxstate++ <= 48)
		{
			rxbitbuf |= !!bit;
			*rxptr++ = !!bit;
			if (rxstate == 49)
			{
				*fms.stat = (unsigned short) 0;

				/*	Dekodiere das empfangene Codewort	*/
				i = decode(test);

				if (i) returnValue=true ;

				if (!i)
				{
					if (m_iLastSyncState>0)
					{
						ErrorOut() ;
						m_iLastSyncState=0 ;
					}
					/*
					 *	CRC-Fehler
					 *	Zur?ckschieben des Buffers f?r Neutests
					 */
					rxbitstream = lasts[9];
					rxbitstream <<= 16;
					rxbitstream |= lasts[10] >> 16;

					for (i = 10; i > 1; i--) {
						lasts[i] = lasts[i - 2];
						lasts[i] <<= 16;
						lasts[i] |= lasts[i - 1] >> 16;
					}
					lasts[1] = lasts[0] >> 16;
					rxstate--;

					while (rxstate--) {

						for(i = 0; i < 10; i++){
							lasts[i] <<= 1;
							lasts[i] |= lasts[i + 1] >> 31;
						}
						lasts[10] <<= 1;
						lasts[10] |= rxbitstream >> 31;
						p = (char*) rxbuf;
						rxbitstream <<= 1;
						rxbitstream |= *p;
						for (i = 0; i < rxstate; i++, p++) *p = *(p + 1);
						rxptr--;

						if ((rxbitstream & filter) == sync) {

							/*	ein Syncwort wurde gefunden	*/
							p = (char*) rxbuf;
							for (; j < rxstate; j++) {
								for(i = 0; i < 10; i++){
									lasts[i] <<= 1;
									lasts[i] |= lasts[i + 1] >> 31;
								}
								lasts[10] <<= 1;
								lasts[10] |= rxbitstream >> 31;
								rxbitstream <<= 1;
								rxbitstream |= *p++;
							}
							rxstate++;
							return false;
						}
					}
					rxstate = 0;
				}
				else
				{
					returnValue=true ;
					if (i < 2) {
						/*
						 *	Telegramm ok, wurde ausgegeben
						 */
						rxstate = 0;
					}
					else {	/*	i == 2 Text?bertragung	*/
						/*	Buffer vorbereiten	*/
						txtbuf = (Line *) malloc (sizeof(Line));
						memset(txtbuf, 0, sizeof(Line));
						txtbuf->next = (Line *) malloc (sizeof(Line));
						memset(txtbuf->next, 0, sizeof(Line));
					}
				}
				return returnValue ;
			}
		}
		else
		{
			/*	Text?bertragung	*/
			Line *txt;

			*rxptr++ = !!bit;

			if (rxstate == 97)
			{
				memset(c, 0, 5);
				memset(c, 46, 4);
				txt = txtbuf->next;

				while (txt->next != NULL) txt = txt->next;

				if (!vals.txtinc)
				{
					memcpy (kontrollString,c,ITEMLEN) ;

					if (!decode(test))
					{
						rxstate = 0;
						return false ;
					}

					returnValue=true ;
					if (fms.dir[0]==0) // Bein Fz->Lst keine ?bersetzung von Sonderzeichen !
					{
						m_bTranslate=false ; // Fz-Daten nicht ?bersetzen !
						// if (!crc_check(48))
						//{
							fms_bin(48,c);

							fms.txtnr=2 ; // Ausgedachter Wert 0xb schlie?t es ja ab
						//}
					} else {
						if (!crc_check(48))
						{
							m_bTranslate=false ;
							fms_txt(48,kontrollString);
							m_bTranslate=true ;
							fms_txt(48,c);

							fms.txtnr= kontrollString[0];
							//TRACE1("L?nge: %d\n",fms.txtnr) ;
							// *c = ' ';
						}
					} ;

					vals.txtinc = 1;
				}
				else
				{
					if (fms.dir[0]==0)
					{
						fms_bin(0,c) ;
					}else {
						if (!crc_check(0))
						{
							fms_txt(0, c);
						}
					} ;

				} ;

				if (fms.dir[0]==0)
				{
					char tempString [20]="" ;

					char rotateByte[4] ; // =((c[i]>>4) & 0xf) + ((c[i] & 0xf) << 4) ;

					/*
					rotateByte[0]= (((c[3]>>7) &0x1)<<0) + (((c[3]>>6) &0x1)<<1) + (((c[3]>>5) &0x1)<<2) + (((c[3]>>4) &0x1)<<3) + (((c[3]>>3) &0x1)<<4) + (((c[3]>>2) &0x1)<<5) + (((c[3]>>1) &0x1)<<6) + (((c[3]>>0) &0x1)<<7) ;
					rotateByte[1]= (((c[2]>>7) &0x1)<<0) + (((c[2]>>6) &0x1)<<1) + (((c[2]>>5) &0x1)<<2) + (((c[2]>>4) &0x1)<<3) + (((c[2]>>3) &0x1)<<4) + (((c[2]>>2) &0x1)<<5) + (((c[2]>>1) &0x1)<<6) + (((c[2]>>0) &0x1)<<7) ;
					rotateByte[2]= (((c[1]>>7) &0x1)<<0) + (((c[1]>>6) &0x1)<<1) + (((c[1]>>5) &0x1)<<2) + (((c[1]>>4) &0x1)<<3) + (((c[1]>>3) &0x1)<<4) + (((c[1]>>2) &0x1)<<5) + (((c[1]>>1) &0x1)<<6) + (((c[1]>>0) &0x1)<<7) ;
					rotateByte[3]= (((c[0]>>7) &0x1)<<0) + (((c[0]>>6) &0x1)<<1) + (((c[0]>>5) &0x1)<<2) + (((c[0]>>4) &0x1)<<3) + (((c[0]>>3) &0x1)<<4) + (((c[0]>>2) &0x1)<<5) + (((c[0]>>1) &0x1)<<6) + (((c[0]>>0) &0x1)<<7) ;

					int test=(c[0]>>7) &0x1 ;
					test=c[0]>>0 ;
*/
					for (i=0;i<4;i++)
					{
						// Oberes und unteres Nibble vertauschen
						rotateByte[i]=((c[i]>>4) & 0xf) + ((c[i] & 0xf) << 4) ;

						// Nibbles nicht tauschen
						//
						//rotateByte[i]=c[i] & 0xff ;

						sprintf(tempString,"%02x",(unsigned char) rotateByte[i]) ;
						strcat (txt->string,tempString) ;
					} ;
				}
				else
				{
					strcat(txt->string, c);
				} ;

				rxptr = rxbuf;
				fms.txtnr -= (4 >> 1);
				// fms.txtnr -= 4 ;
				rxstate = 49;

				if (vals.txtinc)
				{
					if (rxbuf[32] || (fms.txtnr<=0))
					{
						/*	Status b: Abschluss (bisher Status a,
						 * unterscheidet sich in Bit 32)
						 * txtnr: 0
						 * idealerweise trifft beides zu	*/

						// txt_change(txt->string, s->l1.fms.subst);
						// txt_break(txt, MAXROW);

						txt = txtbuf;
						i = 0;
						std::string outString ;

						while (txt != NULL)
						{
							outString +=txt->string ;
							txt = txt->next;
						}

						if (!test)
						{
							DisplayResult(outString.c_str()) ;
							StoreForDupeCheck() ; // F?r sp?ter sichen, um zu pr?fen, ob was doppelt ist
						}
						vals.txtinc = 0;
						rxstate = 0;
						fms.stat[0]=0xb ;
						returnValue=true ;
					}
				}
			}
		}
	}
	return returnValue ;
}

int MonitorModuleFMS::decode(bool test)
{
	int i = 0, rules_;
	short bit = 0;

	if (error_correction())
	{
		return 0 ;
	}


	// if (crc_check(0)) return 0;

	/*	falls keine laufende Text?bertragung	*/
	if (fms.stat[0] != 0xa) {

		fms.bos[0]	= rxbuf[3] << 3 | rxbuf[2] << 2 | rxbuf[1] << 1 | rxbuf[0];

		fms.land[0]	= rxbuf[7] << 3 | rxbuf[6] << 2 | rxbuf[5] << 1 | rxbuf[4];

		fms.ort[0]  = rxbuf[11] << 3 | rxbuf[10] << 2 | rxbuf[9] << 1 | rxbuf[8];
		fms.ort[1]  = rxbuf[15] << 3 | rxbuf[14] << 2 | rxbuf[13] << 1 | rxbuf[12];

		fms.kfz[0] = rxbuf[19] << 3 | rxbuf[18] << 2 | rxbuf[17] << 1 | rxbuf[16];
		fms.kfz[1] = rxbuf[23] << 3 | rxbuf[22] << 2 | rxbuf[21] << 1 | rxbuf[20];
		fms.kfz[2] = rxbuf[27] << 3 | rxbuf[26] << 2 | rxbuf[25] << 1 | rxbuf[24];
		fms.kfz[3] = rxbuf[31] << 3 | rxbuf[30] << 2 | rxbuf[29] << 1 | rxbuf[28];

		fms.stat[0]	= rxbuf[35] << 3 | rxbuf[34] << 2 | rxbuf[33] << 1 | rxbuf[32];

		fms.bst[0]	= rxbuf[36];
		fms.dir[0]	= rxbuf[37];
		fms.tki[0]	= rxbuf[38] << 1 | rxbuf[39];

		for (i = 0; i < 7; i++) fms.crc[i] = rxbuf[40 + i];

		rules_ = rules();

		if (!(rules_ )) {
			bit = vals.bit;
			if (bit < 49 && bit) rxbuf[bit - 1] ^= 1;
			return 0;
		}
	}
		/*	noch keine Ausgabe falls Text?bertragung (sp?ter mit Zeichennzahl))	*/
	if (fms.stat[0] == 0xa) return 2;

	bit = vals.bit;

	// if(!rxbuf[47]) 	/*	Stoppbit == 0	*/
	{
		if (!test)
		{
			DisplayResult() ; /* Auf dem Bildschirm ausgeben */
			StoreForDupeCheck() ; // F?r sp?ter sichen, um zu pr?fen, ob was doppelt ist
		}

	}
	if (bit < 49 && bit) rxbuf[bit - 1] ^= 1;
	return !rxbuf[47] ;
}



int MonitorModuleFMS::crc_check(int offset)
{
/*****************************************************************************
 *	CRC-Pr?froutine und teilweise blindes Korrigieren, offset=[0, 48]
 *****************************************************************************/

	#define	CODE	47

	unsigned char	g[] = {1,0,0,0,1,0,1};
	unsigned char	r[] = {0,0,0,0,0,0,0,0};
	char *p;
	unsigned int	i = 0, j;

	// rxbuf,fms.vals.quality,0, &vals.bit


	/*	CRC-Pr?froutine Begin	*/
	for (i = 0; i < CODE; i++) {
		vals.bit = rxbuf[i + offset] ^ r[0];
		p = (char*) r;
		for (j = 0; j < 7; j++) *p++ = (vals.bit & g[j]) ^ r[j + 1];
	}
	/*	CRC-Pr?froutine Ende	*/

	/*	Pr?fen des Rest-Polynoms	*/
	vals.bit = 0;
	for (i = 0; i < 7; i++) {
		vals.bit <<= 1;
		vals.bit |= r[i];
	}
	/*	ok, falls kein Rest (*bit=0)	*/

	// TRACE1("CRC-Check: %d\n",vals.bit) ;
	if (vals.quality >= 20) return vals.bit;	/*	crc_check on	*/
	else{
		if (vals.bit) {
			vals.bit = crc_table[vals.bit];
			if (vals.bit < 48) rxbuf[(vals.bit)++] ^= 1;
			else return vals.bit;
		}
	}
	return 0;
}

int MonitorModuleFMS::rules()
{
	// ung?ltige Telegramme:
	// Ort : > 9* && > *9
	// BOS : 0 ;
	// SE: geändert auf Ort bis F* und *F, da in Bayern so gebräuchlich

	if (fms.ort[0] > 0xf) return 0 ;
	if (fms.ort[1] > 0xf) return 0 ;
	if (fms.bos==0) return 0 ;
	return 1 ;
}

void MonitorModuleFMS::DisplayResult(std::string input)
{
	//

	// TRACE2("Ergebnisse: Dupes: %d - Quittung: %d\n", (int) CheckForDupes(), (int) CheckForQuittung()) ;
	if (!CheckForDupes()  && !CheckForQuittung() )
	{
		char statusString[5] ; // war: traceString
		char kfzString[10] ; // war: traceString1
		char ortString[10] ; // war: traceString2
		char bosString[3] =" " ;
		char landString[3]=" " ;
		char richtungString[3]=" " ;
		char tkiString[3] = " " ;
		char baustufeString[3] = " " ;
		char dateStr[9];
		char timeStr[9];

		std::string fahrzeugKennung;
		std::string bosDezimalString ;
		std::string landDezimalString ;
		std::string statusDezimalString ;
		std::string jetzt ;

		sprintf(statusString,"%01x",fms.stat[0]) ;
		sprintf(kfzString, "%04x", fms.kfz[0]*0x1000+fms.kfz[1]*0x100+fms.kfz[2]*0x10+fms.kfz[3]*0x1) ;
		sprintf(ortString, "%02x", fms.ort[0]*0x10+fms.ort[1]*0x1) ;


      	// kfzString in Uppercase umwandeln (ist immer 4 stellig)
      	kfzString[0]=toupper( kfzString[0] );
      	kfzString[1]=toupper( kfzString[1] );
      	kfzString[2]=toupper( kfzString[2] );
      	kfzString[3]=toupper( kfzString[3] );

      	// ortString
      	ortString[0]=toupper( ortString[0] );
      	ortString[1]=toupper( ortString[1] );

      	// das gleiche für den StatusString
   		statusString[0]=toupper(statusString[0]) ;

		bosDezimalString=convertIntToString(fms.bos[0]) ;
		landDezimalString=convertIntToString(fms.land[0]) ;
		statusDezimalString=convertIntToString(fms.stat[0]) ;

		richtungString[0] = '0'+fms.dir[0] ;
		tkiString[0] = '0'+fms.tki[0] ;
		baustufeString[0]= '0' + fms.bst[0] ;

		bosString[0]= fms.bos[0]<=9 ? '0'+fms.bos[0] : ('A'+fms.bos[0]-10) ;
		landString[0]= fms.land[0]<=9 ? '0'+fms.land[0] : ('A'+fms.land[0]-10) ;
		fahrzeugKennung=std::string(bosString)+landString+ortString+kfzString ;

		currentTime(jetzt) ; // aktuelle Uhrzeit holen
		struct tm* tm_time= localtime(&m_time) ;
		strftime(dateStr,9,"%d.%m.%y" ,tm_time) ;
		strftime(timeStr,9,"%H:%M:%S" ,tm_time) ;

		ModuleResultBase *pRes =new ModuleResultBase() ;

		pRes->set("timestamp",jetzt);
		pRes->set("uhrzeit",timeStr) ;
		pRes->set("datum",dateStr) ;
		pRes->set("servernamehex",m_serverNameHex);
		pRes->set("channelnamehex",m_channelNameHex);
		pRes->set("channelnum",convertIntToString(m_iChannelNum));

		pRes->set("typ","fms");
		pRes->set("fmskennung",fahrzeugKennung);
		pRes->set("status",statusString);
		pRes->set("baustufe",baustufeString);
		pRes->set("timestamp",jetzt);
		pRes->set("richtung",richtungString);
		pRes->set("tki",tkiString);
		pRes->set("bosdezimal",bosDezimalString) ;
		pRes->set("landdezimal",landDezimalString) ;
		pRes->set("statusdezimal",statusDezimalString) ;
		pRes->set("bos",bosString) ;
		pRes->set("land",landString) ;
		pRes->set("ort",ortString) ;
		pRes->set("kfz",kfzString) ;
		pRes->set("textuebertragung",input) ;

		pRes->Dump(); 
		GlobalDispatcher->addResult(pRes) ;
	} ;
}

void MonitorModuleFMS::fms_txt(short offset, char *c)
{
	int j, letters = 4, tlen;
	char	*str = c, *bpp;
	const char * tstr;

	bpp = (char*) &rxbuf[offset + 6];

	for (; letters; letters--) {
		*str = 0x20 ;
		for (j = 7; j; j--) {
			*str <<= 1;
			*str |= *bpp--;
		}

		if (m_bTranslate) tstr=translate_alpha(*str) ;
		else tstr=(const char *)NULL ;

		if (tstr != NULL) {
			/*	Steuerzeichen ausgeben?	*/
			if (vals.ctrl
			/*	Umlaute	*/
			|| ((*str & 0x58) == 0x58 && (*str & 0x7) >= 3 && (*str && 0x7) <= 5)
			/* ?	*/
			|| *str == 0x7e) {
				tlen= strlen(tstr);
				memcpy(str, tstr, tlen);
				str += tlen - 1;
			}
			else
			{
				/*	Enter	*/
				if (*str == 13 || *str == 10) {
					*str = '\n';
				} else {
					str--;
				}
			}

		} ;
		str++;
		bpp += 15;
	} ;
	*str = 0;
	/*	fms_txt*/
}


void MonitorModuleFMS::fms_bin(short offset, char *c)
{
	int j, letters = 4 ;
	char	*str = c, *bpp ;

	bpp = (char*) &rxbuf[offset + 7];

	for (; letters; letters--) {
		*str = 0x00 ;
		for (j = 8; j; j--) {
			*str <<= 1;
			*str |= *bpp--;
		}


		str++;
		bpp += 16; // ?????????
	} ;
	*str = 0;
	/*	fms_txt*/
}

void MonitorModuleFMS::StoreForDupeCheck()
{
	if (!CheckForDupes()  && !CheckForQuittung() )
	{
		memcpy (&m_LastMessage,&fms,sizeof(FMS)) ;
		// TRACE1("Gespeichert als letzte Nachricht %d\n",fms.bos[0]) ;
	}
	StoreForReverseCheck() ;
}

bool MonitorModuleFMS::CheckForDupes()
{
	if (	(m_LastMessage.bos[0]==fms.bos[0]) &&
			(m_LastMessage.bst[0]==fms.bst[0]) &&
			(m_LastMessage.tki[0]==fms.tki[0]) &&
			(m_LastMessage.dir[0]==fms.dir[0]) &&
			(m_LastMessage.kfz[0]==fms.kfz[0]) &&
			(m_LastMessage.kfz[1]==fms.kfz[1]) &&
			(m_LastMessage.kfz[2]==fms.kfz[2]) &&
			(m_LastMessage.kfz[3]==fms.kfz[3]) &&
			(m_LastMessage.land[0]==fms.land[0]) &&
			(m_LastMessage.ort[0]==fms.ort[0]) &&
			(m_LastMessage.ort[1]==fms.ort[1]) &&
			(m_LastMessage.stat[0]==fms.stat[0])
		)
	{
		return false ; /* Doppelte nicht verwerfen */
	}

	return false ;
}

bool MonitorModuleFMS::CheckForQuittung()
{
	if (	(m_LastMessage.bos[0]==fms.bos[0]) &&
			(m_LastMessage.bst[0]==fms.bst[0]) &&
			(m_LastMessage.dir[0]==(1-fms.dir[0])) &&
			(m_LastMessage.kfz[0]==fms.kfz[0]) &&
			(m_LastMessage.kfz[1]==fms.kfz[1]) &&
			(m_LastMessage.kfz[2]==fms.kfz[2]) &&
			(m_LastMessage.kfz[3]==fms.kfz[3]) &&
			(m_LastMessage.land[0]==fms.land[0]) &&
			(m_LastMessage.ort[0]==fms.ort[0]) &&
			(m_LastMessage.ort[1]==fms.ort[1]) &&
			(fms.stat[0]==15)
		)
	{
		/*if (m_bSuppressQuittung)
		{
			//TRACE0 ("Quittung raus raus !\n") ;
		} ;
		return m_bSuppressQuittung ;*/
	}

	return false ;
}


// *********************************************************
// ** IIR Filter
// *********************************************************

inline float MonitorModuleFMS::biq_lp(float x, float *pcoef, float *buf)
{
	float y;
	y =(*pcoef++)*(x+buf[0]+buf[0]+buf[1]);
	y+=(*pcoef++)*buf[2];
	y+=(*pcoef)*buf[3];

	buf[1]=buf[0]; buf[0]=x;
	buf[3]=buf[2]; buf[2]=y;

	return y;
}

inline float MonitorModuleFMS::biq_bp(float x, float *pcoef, float *buf)
{
	float y;
	y =(*pcoef++)*(x-buf[1]);
	y+=(*pcoef++)*buf[2];
	y+=(*pcoef)*buf[3];

	buf[1]=buf[0]; buf[0]=x;
	buf[3]=buf[2]; buf[2]=y;

	return y;
}

inline float MonitorModuleFMS::biq_hp(float x, float *pcoef, float *buf)
{
	float y;
	y =(*pcoef++)*(x-buf[0]-buf[0]+buf[1]);
	y+=(*pcoef++)*buf[2];
	y+=(*pcoef)*buf[3];

	buf[1]=buf[0]; buf[0]=x;
	buf[3]=buf[2]; buf[2]=y;

	return y;
}

void MonitorModuleFMS::gen_coef(int tipo, float f0, float Q, float *pcoef)
{
	float w0,a,d;

	w0=2.*M_PI*f0;

	//Prewharping
	w0=2.0*FREQ_SAMP*tan(w0/FREQ_SAMP/2.0);

	a=FREQ_SAMP/w0;
	d=4.*a*a +2.*a/Q +1.;

	switch(tipo){
	case 0:	(*pcoef++)=1.0/d;	break;
	case 1: (*pcoef++)=2.*a/Q/d;	break;
	case 2: (*pcoef++)=4.*a*a/d;	break;
	}

	(*pcoef++)=(8.*a*a -2.)/d;
	(*pcoef++)=-(4.*a*a -2.*a/Q +1.)/d;
}

void MonitorModuleFMS::set_filters(float f0, float f1, float dr)
{
	gen_coef(1,f0,f0/dr/2.0,bp0_c);	/* Space filter */
	gen_coef(1,f1,f1/dr/2.0,bp1_c);	/* Mark filter  */
	gen_coef(0,dr,0.5412,lp1_c);	/* Low-Pass order-4 Butt. filter */
	gen_coef(0,dr,1.3066,lp2_c);

	for (int i=0;i<4;i++) lp1_b[i]=lp2_b[i]=bp0_b[i]=bp1_b[i]=0.0;
}

void MonitorModuleFMS::demod_se(float *buffer, int length)
{
	float xs,xm,y, sample;

	/* This function implements a digital PLL for data recovery */
	static int x0;
	static int x_se ;

	static int x,f=0;


	#ifdef FMSDEBUG
		// Startwerte
		fwrite(buffer, sizeof(float), length,debugFile1);
		pBuffer1=fileBuffer1 ;
		pBuffer2=fileBuffer2 ;
		pBuffer3=fileBuffer3 ;
		bufferLen1=0 ;
		bufferLen2=0 ;
		bufferLen3=0 ;
	#endif


	for (; length >= SUBSAMP; length -= SUBSAMP, buffer += SUBSAMP)
	{
		// rtty (2) decoder
		//
		sample=(*buffer)*128 ;

		xs=biq_bp(sample,bp0_c,bp0_b);
		xm=biq_bp(sample,bp1_c,bp1_b);
		xs*=xs;		// xs RMS
		xm*=xm;		// xm RMS
		y=biq_lp(biq_lp(xm-xs,lp1_c,lp1_b),lp2_c,lp2_b);


		x=(y>0.0)?1:0 ;

		if ((x^x0)) 	// Data Change
		{
			if (!f)
			{
				if (bit_phase>0x8000)
				{ // Late
					bit_phase+=SPHASEINC_BASE*1.5; // war: /8
				} else 			// Early
				{
					bit_phase-=SPHASEINC_BASE*1.5 ;// war: /8
				}

				f=1;
			}
		}

		x0=x;

		x_se=bit_phase;

		bit_phase+=SPHASEINC_BASE ;


		#ifdef FMSDEBUG
			/*
			debugFile1 = fopen("fms_in.raw", "wb");
			debugFile2 = fopen("fms_takt.raw", "wb");
			debugFile3 = fopen("fms_pfd.raw", "wb");
			debugFile4 = fopen("fms_4.raw", "wb");

			pBuffer1=debugFile2 ;
			pBuffer2=debugFile3
			pBuffer3=debugFile4 ;
			*/

			*pBuffer1=(bit_phase <0x8000)?0.7:-0.7 ;
			*pBuffer2=y ;
			*pBuffer3=(Ud*0.75)/0x8000 ;

			pBuffer1++ ;
			pBuffer2++ ;
			pBuffer3++ ;
			bufferLen1++ ;
			bufferLen2++ ;
			bufferLen3++ ;
		#endif

			if (((bit_phase & 0x8000)>0) && ((x_se & 0x8000)==0)) // in der "Mitte" bit_phase=0x8000 wird ein einzelnes Bit "empfangen", wichtig wg. der early/late - nicht erst am Ende !
			{
				f=0 ;

				//if ( (!m_bForcePreambel) || rxstate  ) // rxstate bei lfd. Text?bertragung
				{
					#ifdef FMSDEBUG
					pBuffer3-- ;
					*pBuffer3= x0==1 ? 0.7:-0.7 ;
					pBuffer3++ ;
					#endif

					if (rxbit(x0<<7)) // Gibt true zur?ck, wenn ein korrektes Wort empfangen worden ist
					{
						// Daten korrekt empfagen
						ErrorOut() ;
					}
				}

				// This function returns in the middle of a bit
			} ;
		}
	#ifdef FMSDEBUG
		fwrite(fileBuffer1, sizeof(float), bufferLen1,debugFile2);
		fwrite(fileBuffer2, sizeof(float), bufferLen2,debugFile3);
		fwrite(fileBuffer3, sizeof(float), bufferLen3,debugFile4);
	#endif

}

void MonitorModuleFMS::ErrorOut()
{
	if (fms.stat[0] != 0xa) {

		fms.bos[0]	= rxbuf[3] << 3 | rxbuf[2] << 2 | rxbuf[1] << 1 | rxbuf[0];

		fms.land[0]	= rxbuf[7] << 3 | rxbuf[6] << 2 | rxbuf[5] << 1 | rxbuf[4];

		fms.ort[0]  = rxbuf[11] << 3 | rxbuf[10] << 2 | rxbuf[9] << 1 | rxbuf[8];
		fms.ort[1]  = rxbuf[15] << 3 | rxbuf[14] << 2 | rxbuf[13] << 1 | rxbuf[12];

		fms.kfz[0] = rxbuf[19] << 3 | rxbuf[18] << 2 | rxbuf[17] << 1 | rxbuf[16];
		fms.kfz[1] = rxbuf[23] << 3 | rxbuf[22] << 2 | rxbuf[21] << 1 | rxbuf[20];
		fms.kfz[2] = rxbuf[27] << 3 | rxbuf[26] << 2 | rxbuf[25] << 1 | rxbuf[24];
		fms.kfz[3] = rxbuf[31] << 3 | rxbuf[30] << 2 | rxbuf[29] << 1 | rxbuf[28];

		fms.stat[0]	= rxbuf[35] << 3 | rxbuf[34] << 2 | rxbuf[33] << 1 | rxbuf[32];

		fms.bst[0]	= rxbuf[36];
		fms.dir[0]	= rxbuf[37];
		fms.tki[0]	= rxbuf[38] << 1 | rxbuf[39];

		for (int i = 0; i < 7; i++) fms.crc[i] = rxbuf[40 + i];

	}

	char traceString [5] ;
	// itoa( fms.stat[0], traceString,16) ;
	sprintf(traceString,"%01x",fms.stat[0]) ;

	char traceString1[10] ;
	sprintf(traceString1, "%04x", fms.kfz[0]*0x1000+fms.kfz[1]*0x100+fms.kfz[2]*0x10+fms.kfz[3]*0x1) ;

	char traceString2[10] ;
	sprintf(traceString2, "%02x", fms.ort[0]*0x10+fms.ort[1]*0x1) ;

#ifdef MON_DEBUG_ERR
	TRACE0("KFZ: ") ;
	TRACE0(traceString1) ;
	TRACE0 (" ") ;

	TRACE0("Ort: ") ;
	TRACE0(traceString2) ;
	TRACE0(" ") ;

	TRACE0("Status: ") ;
	TRACE0(traceString) ;
	TRACE0("\n") ;
#endif
/*
	COleDateTime now ;
	now=COleDateTime::GetCurrentTime() ;

	CString outString=now.Format("%H:%M:%S - ") ;
	*/
	std::string outString ;
	outString+="received: " ;

	char tempString[10]=" " ;
	tempString[0]= fms.bos[0]<=9 ? '0'+fms.bos[0] : ('A'+fms.bos[0]-10) ;
	//	itoa(fms.bos[0], tempString,16) ; // BOS
	outString += tempString ;
	outString +=" " ;

	tempString[0]= fms.land[0]<=9 ? '0'+fms.land[0] : ('A'+fms.land[0]-10) ;
	//itoa(fms.land[0], tempString,16) ; // LAND
	outString += tempString ;

	outString += traceString2 ; // ORT
	outString +=" " ;
	outString += traceString1 ; // KFZ

	std::transform (outString.begin(), outString.end(), outString.begin(),
               (int(*)(int)) toupper);

	outString +=" : " ;
	outString += traceString ; // Status ;

	outString += " - " ;
	tempString[0]= fms.bst[0]<=9 ? '0'+fms.bst[0] : ('A'+fms.bst[0]-10) ;
	//	itoa(fms.bst[0], tempString,16) ; // BST
	outString += tempString ;

	outString += "/" ;
	if (fms.dir[0]==0)
		outString += "Kfz->Lst" ;
	else
		outString += "Lst->Kfz" ;

	// itoa(fms.dir[0], tempString,16) ; // DIR
	// outString += tempString ;

	outString += "/" ;
	tempString[0]= fms.tki[0]<=9 ? '0'+fms.tki[0] : ('A'+fms.tki[0]-10) ;
	//	itoa(fms.tki[0], tempString,16) ; // TKI
	outString += tempString ;


	////DebugMessage(outString) ;

	//TRACE1("%s\n",outString) ;
}

void MonitorModuleFMS::StoreForReverseCheck()
{

	memcpy (&m_ReverseCheckMessage,&fms,sizeof(FMS)) ;
			// TRACE1("Gespeichert als letzte Nachricht %d\n",fms.bos[0]) ;

}

bool MonitorModuleFMS::IsRelatedMessage()
{
	if (	(m_ReverseCheckMessage.bos[0]==fms.bos[0]) &&
			(m_ReverseCheckMessage.kfz[0]==fms.kfz[0]) &&
			(m_ReverseCheckMessage.kfz[1]==fms.kfz[1]) &&
			(m_ReverseCheckMessage.kfz[2]==fms.kfz[2]) &&
			(m_ReverseCheckMessage.kfz[3]==fms.kfz[3]) &&
			(m_ReverseCheckMessage.land[0]==fms.land[0]) &&
			(m_ReverseCheckMessage.ort[0]==fms.ort[0]) &&
			(m_ReverseCheckMessage.ort[1]==fms.ort[1])
		)
	{
		return true ;
	} else
	{
		return false ;
	}
}

bool MonitorModuleFMS::error_correction()
{
	#define	CODE	47
	// Gibt true zur?ck, wenn keine Fehlerkorrektur o.?. helfen konnte
	//
	if (crc_check(0))
	{
		//
		if (m_bPrintError && m_bErrorCorrection)
		{
		//	TRACE0("CRC Check failed...\n") ;
			// CRC-Check war nicht ok ...
			// Deswegen

			// Ein Bit Fehler versuchen zu korrigieren
			//
			// gr?ssere Hamming-Dist m?glich ? (48 Bit, davon 7 Bit CRC = 42 Bit Nutzdaten)
			//
			int pos1=0 ;
			do
			{
				rxbuf[pos1] = (1-rxbuf[pos1]) ; // Bit invertieren
				if (!crc_check(0)  )
				{
					if (RMExists())
					{
						//std::string outString ;
						//outString.Format("1) CRC corrected at %d.",pos1) ;
						////DebugMessage(outString) ;
						ErrorOut() ;

						return false ;
					}
				}
				rxbuf[pos1] = (1-rxbuf[pos1]) ; // Bit invertieren
				pos1++ ;
			} while (pos1<(CODE)) ;
		}
		return true ;
	} else
	{
		return false  ;
	}
}

bool MonitorModuleFMS::RMExists()
{
	return true ;
}

bool MonitorModuleFMS::IsSyncWord(unsigned long rxbitstream)
{
	int minbits= vals.quality >= 20 ? vals.quality - 20 : vals.quality;
	int i,bitcounter=0 ;

	unsigned int sync  = 0xfff1a ;

	for (i=0;i<20;i++)
	{
		unsigned int bit1=rxbitstream & 0x1 ;
		unsigned int bit2=sync & 0x1 ;
		if (bit1==bit2)
		{
			bitcounter++ ;
		}
		rxbitstream>>=1 ;
		sync>>=1 ;
	}

	return (bitcounter >= minbits) ;
}


void MonitorModuleFMS::demod_neu(float *buffer, int length)
{

}


bool MonitorModuleFMS::IsValid()
{
	return !(fms.bos[0]==0) ;
}
