/*---------------------------------------------------------------------------*\
    FILE....: cid_jp.cpp
    TYPE....: WIN32 C++ Function
    AUTHOR..: John Kostogiannis
    DATE....: 14/11/97

    Japan DID demodulate and Decode functions


	Voicetronix Voice Processing Board (VPB) Software
	Copyright (C) 1999-2007 Voicetronix www.voicetronix.com.au

	This library is free software; you can redistribute it and/or
	modify it under the terms of the GNU Lesser General Public
	License as published by the Free Software Foundation; either
	version 2.1 of the License, or (at your option) any later version.

	This library 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
	Lesser General Public License for more details.
	You should have received a copy of the GNU Lesser General Public
	License along with this library; if not, write to the Free Software
	Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
	MA  02110-1301  USA

\*-------------------------------------------------------------------------*/

#include <assert.h>
#include <cmath>
#include <cstdio>
#include <cstring>
#include <cstdlib>

#include "apifunc.h"
#include "modulate.h"
#include "wobbly.h"
#include "mess.h"


#define	JP_RAW		128	// Max byte count Japan DID/BIN
#define	OTN_L		21	// Max char for CN EXPansion data
//#define OTNRA_L	31	// Max char for CN EXPansion data
//#define OTNEXP_L	4	// Max char for CN EXPansion data
//#define CND_L		21	// Max char for CN EXPansion data
//#define CNEXP_L	4	// Max char for CN EXPansion data

	// parity control
#define PAR_8N	0
#define PAR_7E  1
#define PAR_7O  2

	// Decode frequencies for FSK bit stream
#define JP_F1	1300
#define JP_F2	2100
#define FS      8000    // sampling rate
#define NCOEF   7       // number of filter coefficients
#define NBASCII   8     // nunber of bits in each ASCII processing block
#define NAV     6       // number of samples to average energy over
#define MARKB   400     // number of sam. for MARK BITs header (50ms) 
#define NDCTMP  40      // number of sam. of CSS signal used for temp DC removal

#define MAX_JPRAW	(JP_RAW+10)

#define MAXDIDWAV	(10240) // Size required for wave buffer on DID TX

	// Header and Control bytes
#define	DLE	0x10
#define	SOH	0x01
#define	STX	0x02
#define	ETX	0x03
#define	SI	0x0f
#define	SO	0x0e

#define	H_BIN	0x07
#define	T_BIN	0x40

#define BIN_OTN 	0x02
#define DID_CND 	0x09
#define BIN_OTNRA 	0x04
/*	Byte +3 Reasons for not informing of the origenators Phone #
	'P'	0x50	// User Denial
	'O'	0x4f	// Unable to Offer Service
	'C'	0x43	// Origenated From Public Telephone
	'S'	0x53	// Service Conflict

*/
#define BIN_OTNEXP	0x21
#define DID_CNDEXP 	0x22
/*	Byte +3 Type of Number (TON) Expantion Signal 
	'0'	0x30	// Uncertain
	'1'	0x31	// International Number
	'2'	0x32	// Domestic Number
	'3'	0x33	// Network's Own Number
	'4'	0x34	// Local Number
	'6'	0x36	// Abbreviated Number
	'7'	0x37	// Resurved for extention
			// Others reserved

	bytes +4 & +5 Number Plan Identifier (NPI)
	'0','0'	0x30, 0x30	// Uncertain
	'0','1'	0x30, 0x31	// ISDN/telephone numbering plan E.164
	'0','3'	0x30, 0x33	// Data numbering plan X.121
	'0','4'	0x30, 0x34	// Telix numbering plan F.69
	'0','8'	0x30, 0x38	// Domestic numbering plan
	'0','9'	0x30, 0x39	// Private numbering plan
	'1','5'	0x31, 0x35	// resurved for extention
				// Others reserved
*/


//static FILE *fcd = NULL;

// FIR filter coefficients

static float coef[] = {
	(float)1.0000000e+000,
	(float)1.0515795e+000,
	(float)6.5481993e-001,
	(float)4.8815834e-001,
	(float)6.5481993e-001,
	(float)1.0515795e+000,
	(float)1.0000000e+000
};

// filter coeffs for averaging

static float avcoef[] = {
	(float)(1.0/6.0),
	(float)(1.0/6.0),
	(float)(1.0/6.0),
	(float)(1.0/6.0),
	(float)(1.0/6.0),
        (float)(1.0/6.0),
};



void demodulate_jp(float *demod, short *s, int n, float f1, float f2);

int extract_jp_pres(
		char    	*pres,  	// char array for bytes
                int     	*npres, 	// byte count of pres
                float   	*demod, 	// bit streem data
                int     	n       	// word count of bit stream
                );

int bit_sync_ascii(int *bits_out, float *demod, int idx, int *valid);

float filter(
                float in[],
                float coeff[],
                int   ncoeff
                );

int sgn(float x);

int crc(unsigned long *crc_acc, char data);

int byte_assemble(      float *demod,   // Demodulated buffer
                        int n,          // Sample count in demod
                        int *idx,       // Current index into demod
                        int parity,     // parity check + data mask option
                        char *byte,     // returned byte
			unsigned long	*cksumb);	// CRC WORD

int  bit_parity(int bits[]);
char parity_gen(char bytein, int mode);

/*--------------------------------------------------------------------------*\
		Functions for JP CID encode
\*--------------------------------------------------------------------------*/

//int WINAPI vpb_cid_jp_compose_dlp(VPB_CID *message, char *dlp);

//int WINAPI vpb_cid_jp_set(VPB_CID *cid_struct, int type, void *value);

//int WINAPI vpb_cid_jp_compose_wav(char *dlp, int dlp_len, short *buff_wav, int *buff_cnt);

/*----------------------------------------------------------------------------\
		CID / DID JAPAN
This structure is in the vpbapi.h file to allow common access

typedef struct {
	char    raw[130];               // Raw bytes decoded, inc parity
	int     raw_size;               // Raw data byte count
	char    otn[21];                // Origenators Telephone Number
	char    otnra[31];              // OTA Reason for Absence
	char    otnexp[4];              // OTA Expansion Data
	char    cnd[21];                // DID Called Number Data
	char    cndexp[4];              // DID Expansion Data
} VPB_CID_JP;

For each field above from otn,
\----------------------------------------------------------------------------*/


/*---------------------------------------------------------------------------*\
	FUNCTION: vpb_cid_jp_set(VPB_CID *cid_struct, int type, void *value)
	AUTHOR..: Peter Wintulich
	DATE....: 6 MAY 2005

Function:
	Validate size of value field and if ok copy into 'cid_struct'
	Note this dose not verify content.
Return:
	0 for sucess, -n for fail.
\*---------------------------------------------------------------------------*/
int WINAPI vpb_cid_jp_set(VPB_CID_JP *cid_struct, int type, void *value)
{       //      *cid            Structure containing cid fields
        //      type            cid Field label
        //      *value          Pointer to value

//	int     x;
	int     len;
//	int     valid;
	VPB_CID_JP	*cid;

	cid = cid_struct;
	if(cid != NULL)       // check pointer is valid
        {
	    switch (type)
	    {
	        case VPB_CID_EMPTY:             // Initalize all fields 	    
			cid->raw[0] = 0;
			cid->raw_size = 0;
			cid->otn[0] = 0;
			cid->otnra[0] = 0;
			cid->otnexp[0] = 0;
			cid->cnd[0] = 0;
			cid->cndexp[0] = 0;
			break;
		case VPB_DID_OTN:
			len = strlen((char*)value);
			if((len > 0) && (len <= 20))
				strcpy(cid->otn, (char *)value);
			else
				return -2;
			break;
		case VPB_DID_OTNRA:
			len = strlen((char*)value);
			if((len > 0) && (len <= 30))
				strcpy(cid->otnra, (char *)value);
			else
				return -2;
			break;
		case VPB_DID_OTNEXP:
			len = strlen((char*)value);
			if((len > 0) && (len <= 3))
				strcpy(cid->otnexp, (char *)value);
			else
				return -2;
			break;
		case VPB_DID_CND:
			len = strlen((char*)value);
			if((len > 0) && (len <= 20))
				strcpy(cid->cnd, (char *)value);
			else
				return -2;
			break;
		case VPB_DID_CNDEXP:
			len = strlen((char*)value);
			if((len > 0) && (len <= 3))
				strcpy(cid->cndexp, (char *)value);
			else
				return -2;
			break;
	    }
	}
	else
		return -1;
	return 0;
}

/*---------------------------------------------------------------------------*\
	FUNCTION: vpb_cid_jp_compose_dlp(VPB_CID *cid_struct);
	AUTHOR..: Peter Wintulich
	DATE....: 6 MAY 2005

Function:
	translate data fields in '*cid_struct' and build byte streem in ->raw,
	also fill in the size byte count.
Return:
	0 for sucess, -n for fail.
\*---------------------------------------------------------------------------*/
int WINAPI vpb_cid_jp_compose_dlp(VPB_CID_JP *cid_struct)
{
	int     i=0, x;
	int     len,base;
//	int     valid;
	VPB_CID_JP	*cid;
	unsigned long crc_acc = 0x0000;
	unsigned long crc_acc2 = 0x0000;
//	char	byte;

//	char	DLE = 0x10;
//	char	SOH = 1;
//	char	HEADER = 7;
//	char	STX = 2;
//	char	BIN = 0x40;
//	char	ETX = 3;

	cid = cid_struct;
	// build the header
	cid->raw[i++]=parity_gen(DLE,2);	// not in CRC
	cid->raw[i++]=parity_gen(SOH,2);	// not in CRC
	cid->raw[i++]=parity_gen(H_BIN,2);	// CRC starts here
	cid->raw[i++]=parity_gen(DLE,2);
	cid->raw[i++]=parity_gen(STX,2);
	cid->raw[i++]=parity_gen(T_BIN,2);	//Service type BIN display
	base=i;	// use to byte count for length field & special case
	i++;	//allow 1 byte for message length
	// assemble in the fields
	len=strlen(cid->otn);			// Origenating Telephone Number
	if(len>0)
	{
		printf("OTN = %d",len);
		cid->raw[i++] = parity_gen(VPB_DID_OTN,2);
		cid->raw[i++] = parity_gen(len,2);
		for(x=0;x<len;x++)
			cid->raw[i++] = parity_gen(cid->otn[x],2);
	}
	len=strlen(cid->otnra);			// Origenating Telephone Number
	if(len>0)				// Reason for not informing
	{
		printf("OTNra = %d",len);
		cid->raw[i++] = parity_gen(VPB_DID_OTNRA,2);
		cid->raw[i++] = parity_gen(len,2);
		for(x=0;x<len;x++)
			cid->raw[i++] = parity_gen(cid->otnra[x],2);
	}
	len=strlen(cid->otnexp);		// Origenating Telephone Number
	if(len>0)				// Expansion
	{
		printf("OTNexp = %d",len);
		cid->raw[i++] = parity_gen(VPB_DID_OTNEXP,2);
		cid->raw[i++] = parity_gen(len,2);
		for(x=0;x<len;x++)
			cid->raw[i++] = parity_gen(cid->otnexp[x],2);
	}
	len=strlen(cid->cnd);			// Calling Number Display
	if(len>0)
	{
		printf("CND = %d",len);
		cid->raw[i++] = parity_gen(VPB_DID_CND,2);
		cid->raw[i++] = parity_gen(len,2);
		for(x=0;x<len;x++)
			cid->raw[i++] = parity_gen(cid->cnd[x],2);
	}
	len=strlen(cid->cndexp);		// Calling Number Display Expansion
	if(len>0)
	{
		printf("CNDexp = %d",len);
		cid->raw[i++] = parity_gen(VPB_DID_CNDEXP,2);
		cid->raw[i++] = parity_gen(len,2);
		for(x=0;x<len;x++)
			cid->raw[i++] = parity_gen(cid->cndexp[x],2);
	}
	// calculate and save presentation size
	len = i-base-1;
	cid->raw[base] = len;
	// Special case Insert DLE before len if len == 16 dec.
	if(len == 16)
	{
		for(x=base+len; x>(base-1); x++)
			cid->raw[x] = cid->raw[x-1];
		i++;
		cid->raw[base-1] = parity_gen(DLE,2);
	}
	// build the footer
	cid->raw[i++]=parity_gen(DLE,2);
	cid->raw[i++]=parity_gen(ETX,2);	// CRC ends here
	// Apply crc to all relivent bytes
	for (x=2;x<i;x++)
		crc(&crc_acc, cid->raw[x]);
	crc(&crc_acc, 0x00);	// push in two 0x00 bytes to get crc in acc
	crc(&crc_acc, 0x00);
	// bit reverse the crc16 and put it into the frame.
	for (x=0; x<16; x++)
		if((crc_acc & (1<<x)) !=0)
			crc_acc2 = crc_acc2 | (1<<(15-x));

	// assemble in the CRC bytes fron crc_acc
	cid->raw[i++]= crc_acc2 & 0x00ff;	// upper byte (bit reversed)
	cid->raw[i++]= (crc_acc2 >>8) & 0x00ff;	// lower byte (bit reversed)
	// check its all under 129 bytes in length
	if(i>128)
		return -1;
	cid->raw_size = i;
	return 0;
}

/*---------------------------------------------------------------------------*\
	FUNCTION: vpb_cid_jp_compose_wav(char *dlp,
       					 int dlp_len,
				       	 short *buff_wav,
				       	 int *buff_cnt)
	AUTHOR..: Peter Wintulich
	DATE....: 6 MAY 2005

Function:
	modulate the raw byte streem to 16 bit linear.

\*---------------------------------------------------------------------------*/
int WINAPI vpb_cid_jp_compose_wav(VPB_CID_JP *cid_struct, short *buff_wav, int *buff_cnt)
{
	static const Country *country = vpb_get_country_data(81); // JAPAN

	int     MSS = 100;      // MSS bit count 100 bits = 83ms, 60ms is min. req.
	int     sc;             // sample count returned by f(n).
	int     i;
	int     accs = 0;       // Accumilated samples in buff_wav
	VPB_CID_JP	*cid;
	short	*wave;
	//  These are required to keep phase and bit timing in sync.
	int     bit_time =0;    // used by bit_encode to track samples/bit
	double  sample_time =0; // used by bit_encode to track sinewave cycles

	cid = cid_struct;

	if(buff_wav == NULL)	// if we were passed an empty pointer make a space
		wave = (short*) malloc(MAXDIDWAV);
	else
		wave = buff_wav;
	// assert(wave);

	for (i=0; i < MSS; i++) // Compose MSS (all 1's)
	{
		bit_encode(1, &sc, &wave[accs], &sample_time, &bit_time, country);
		accs +=sc;
	}

	for (i=0; i < cid->raw_size; i++)     // Compose data frame
		byte_encode(cid->raw[i], &accs, wave, &sample_time, &bit_time, country);

	for (i=0; i < 5; i++)   // Trail some bits at the end (not required)
	{
		bit_encode(1, &sc, &wave[accs], &sample_time, &bit_time, country);
		accs +=sc;
	}
        *buff_cnt = accs;       // Accumulated sample count
        return 0;
}


/*---------------------------------------------------------------------------*\
	FUNCTION: cid_jp_decode()
	AUTHOR..: Peter Wintulich
	DATE....: 22 MAR 2004

	Takes Raw Audio sample and decodes to DID and BIN information
	Valid data if filled into VPB_JP_CID structure passed to the Fn.

	return()
		Return VPB_OK if sucess.
		Returns error code if fail to decode (use to identify point
		of fail in debug and test). Error will normaly indicate absence
		of DID and BIN information.

	error ==0	No error
	      > 0	error from extract_jp_pres()
      	      < 0	error from cid_jp_decode ()
\*---------------------------------------------------------------------------*/
static int cid_jp_decode ( VPB_CID_JP *cid,           // JP Caller ID struct
                           short      *in,            // Wave input
                           int        n )             // Sample count
{
	char	pres[MAX_JPRAW];
	int	npres, bc=0;
	int	error = 0;
	int	i =0;
	int	state= 1;

	float *demod = new float[n]; 		// allocate space to decode

	// Demodulate audiosample to bit stream
	demodulate_jp(demod, in, n, JP_F1, JP_F2 );

	// display demodulated signal
	//for(i=0; i<(n/2); i++)
	//{
	//	printf("%1d,",sgn(demod[i]));
	//}	


	// Convert bit stream to byte stream of presentation layer only
	// and return byte count	
	error = extract_jp_pres(pres, &npres, demod, n);
		//errors returned range 1..20 indicating point of fail

	// clean up temp varible, 
	delete [] demod;

	//printf(" \nerror= %d\n",error);
	//printf("npres=%d\n",npres);
 	//for(i=0; i<npres; i++)
	//         {
	//		printf("%2x,",pres[i]);
	//         }

	memcpy(cid->raw, pres, npres);
        cid->raw_size = npres;

	//	Empty any old data before proceeding
	cid->cnd[0] = 0;
	cid->cndexp[0] = 0;
	cid->otn[0] = 0;
	cid->otnra[0] = 0;
	cid->otnexp[0] = 0;

	//printf(" \nenter while\n");
	i=0;

	state = 1;	// go till =0 or error
	while((state == 1) && (0 == error))
	{
	    	switch(pres[i])
	        {
		    case DID_CND:	// Called Number Data 
			if((bc= pres[i+1]) < OTN_L )
			{
			    memcpy(cid->cnd, &pres[i+2], bc);
			    cid->cnd[bc] = 0; // Null Terminate
			}
			else
			{
			    cid->cnd[0] = 0; // Zero length string
			    error = -2;
			}
			break;

		    case DID_CNDEXP:	// Called Number Data Expansion
			if((bc=pres[i+1]) == 3)
			{
			    memcpy(cid->cndexp, &pres[i+2], bc);
			    cid->cndexp[bc] = 0; // Null Terminate
			}
			else
			{
			    cid->cndexp[0] = 0;	// Zero length string
			    error = -3;		// Inconsistent	
			}
			break;

		    case BIN_OTN:	// Origenators Tel. Number
			if((bc=pres[i+1]) <= 20)
			{
			    memcpy(cid->otn, &pres[i+2], bc);
			    cid->otn[bc] = 0;	// Null Term.
			}
			break;

		    case BIN_OTNRA:	// OTN Reason for Absence
			if((bc=pres[i+1]) == 1)
			{
			    memcpy(cid->otnra, &pres[i+2], bc);
			    cid->otnra[bc] = 0;	// Null Term.
			}
			else
			{
			    cid->otnra[0] = 0;	// Zero length string
			    error = -4;		// Inconsistent	
			}
			break;

		    case BIN_OTNEXP:	// OTN Expansion Signal
			if((bc=pres[i+1]) == 3)
			{
			    memcpy(cid->otnexp, &pres[i+2], pres[i+1]);
			    cid->otnexp[(int)(pres[i+1])] = 0;	// Null Term.
			}
			else
			{
			    cid->otnexp[0] = 0;	// Zero length string
			    error = -5;		// Inconsistent	
			}
			break;
	        } // end: switch(pres[i])
	        i += bc + 2;
	        if( npres <= i)
			state = 0;	// we are done
	}	// end: while((state == 1) && (0 == error))

	if(error != 0)
		return(error);
	else
		return(VPB_OK);	// VPB_OK = No error if all ok
}

/*---------------------------------------------------------------------------*\
	FUNCTION: vpb_cid_jp_decode()
	AUTHOR..:
	DATE....:

\*---------------------------------------------------------------------------*/
int WINAPI vpb_cid_jp_decode( VPB_CID_JP *cid,		// JP Caller ID struct
                              short	 *in,		// Wave input
                              int	 n )		// Sample count
{
	// Check if there is any big problems
	assert( cid != NULL );
	assert( in != NULL );
	assert( n > 1 );
	// OK lets go
	return cid_jp_decode(cid, in, n);
}

/*---------------------------------------------------------------------------*\
	FUNCTION: 	extract_jp_pres()
	AUTHOR..:	Peter Wintulich
	DATE....:	22/APR/2004

	Convert Demodulated stream to bytes, Verify header, 

	Error numbers 1..14 used, resurved to 20
	0 == no error (VPB_OK)

\*---------------------------------------------------------------------------*/
int extract_jp_pres( char 	*pres,	// char array for bytes
		     int	*npres, // byte count of pres
		     float	*demod, // bit stream data
		     int	n )	// Nuimber of samples in bit stream demod[]
{

	#define	SMARK	2
	#define VMARK	3
	#define SSTART	4
	#define	EXITS	10
	#define	S2	12
	#define	S3	13
	#define	S4	14
	#define	EPL	15

	int	idx;
	int	error = 0;
	int	state;
	int	i = 0, j, k;
//	int	tos;
	int	tos_l = 0;
	char	crc_l, crc_h;
	unsigned long	cksum;
	int	dle_trail=0;
	char	ascii_char;
        float           low=0.0, hi=0.0, dc=0.0;
        int             lc=0, hc=0;

	cksum = 0;

	mprintf("extract_jp_pres(%p,%p,%p,%d)\n",pres, npres, demod, n);
	*npres = 0;
	idx = 0;
	state=SMARK;
	do{
	    switch(state)
	    {
		case	SMARK:  // Search for beginning of MARK
			mprintf("SMARK idx=%d\n",idx);
			i=idx;
			while(sgn(demod[i]) == 0 && state == EXITS)
			{
				printf("%1d,",sgn(demod[i]));
				i++;
				if(i >= n)		// More samples ? 
				{
					error=1;
					state=EXITS;
					mprintf("(SMARK) i=%d\n",i);
				}
			}

			if(error == 0)			// If no error then
			{
				idx=i;		// save current place were upto
				i=0;		// reset 
				state=VMARK;	// now measer mark len
			}
			break;

		case	VMARK:	// Measer (verify) Mark lead in.
			mprintf("VMARK idx=%d, i=%d, error=%d\n",idx,i,error);
			while(state == VMARK && (i + idx) < n)
			{
			    if(sgn(demod[i+idx]) ==1)
			    	i++;
			    else
			    {
				if(i > MARKB)
				{	// Test if there are more than 3 zero's
					j=0;
					for(k=0; k<6; k++)
	                                	j += sgn(demod[k+idx+i]);
					if(j < 3)
					{
						state = SSTART;
						idx += (i-1);	// leave 1 for zcross det
					}
					else
						i++;
				}
				else
				{
					idx +=i+1;
					i=0;
				}
			    }

			    if((i+idx) >= n)
			    {
				state=EXITS;
				error=2;
			    }
			}
			break;

		case	SSTART:	// dle
			mprintf("DLE idx=%d\n",idx);
			// First do the DC offset average over next 1000 samples
			low=0.0;// Accumilated LOW samples
			hi=0.0;	// Accumilated HIGH samples
			dc=0.0;	// Final DC shift to apply
			lc=0;	// count of LOW (neg.) sig. samples
			hc=0;	// count of HIGH (pos.) sig. samples
		        for(i=idx; i<(idx+1000); i++)
		        {	// only calculate with samples of strong signal
				if(demod[i] > 100000.0)
				{
					hi+= demod[i];
					hc++;
				}
				if(demod[i] < -100000.0)
				{
					low+= demod[i];
					lc++;
				}
			}
			mprintf("hi=%f, low=%f\n",hi/(float)hc, low/(float)lc);
			dc= (low/(float)lc)+(((hi/(float)hc)-(low/(float)lc))/2);
			mprintf("DC=%f\n", dc);
			// don't apply offset yet, first check that it is realy data we
			// are reading and not noise from off hook physical event.

			// now try reading data
			if(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) !=0)
			{
				if(ascii_char == DLE)	// If good apply offset & continue
		       		{
					state = S2;
					// correct DC offset across entire signal
					for(i=0; i<n; i++)
						demod[i] -= dc;
				}
				else
				{
				        //error= 3;
					cksum =0;
					state=SMARK;
				}
			}
			else
			{
				if(idx < n)
				{
					cksum =0;
					state=SMARK;
				}
				else
				{
					state=EXITS;
				        error= 4;
				}
			}
			break;

		case	S2:	// soh
			mprintf("SOH idx=%d\n",idx);
			if(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) !=0)
			{
				if(ascii_char == SOH)
				{
					state= S3;
				}
				else
					error= 5;
			}
			else
                        {
                                state=EXITS;
                                error= 6;
                        }
			break;

		case	S3:	// HEADER NOTE: CRC starts from here
			mprintf("HEADER idx=%d\n",idx);
			cksum =0;	// Resst CRC accumilator, first valid byte.

			if(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) !=0)
			{
				if(ascii_char == H_BIN)
				{
					state= S4;
				}
				else
					error= 7;
			}
			else
			{
				state=EXITS;
				error= 8;
			}
			break;

		case	S4:	// DLE & STX
			mprintf("S4 idx=%d\n",idx);
			byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum);

			if(ascii_char == DLE)
			{
				dle_trail=1;
				byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum);
			}
			if(ascii_char == STX)
			{
				state= EPL;
			}
			else
				error= 9;
			break;

		case	EPL:	// Extract presentation layer
			mprintf("EPL idx=%d\n",idx);

			// Get Type Of Service
			if(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) ==0)
			{
				state= EXITS;
				error= 10;		// bad parity
			}
			else
			{
				// tos= (int) ascii_char;
				//printf("tos=%2d\n",tos);
			}

			// Get Length
			if((error ==0)&&(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) ==0))
			{
			        state= EXITS;
                                error= 11;               // bad parity
                        }
			else
			{
			        tos_l= (int) ascii_char;
				//printf("tos_l=%2d\n",tos_l);
			}

			// read presentation layer into buffer
			*npres = tos_l;
			for(i=0; i<tos_l; i++)
			{
				if(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) !=0)
				{
	                        	pres[i]= ascii_char;
					//printf("pres[%d]=%d,'%c'\n",i,pres[i],pres[i]);
				}
				else
				{
				        state= EXITS;
					error= 12;               // bad parity
				}
			}
			if((error == 0) &&(dle_trail == 1))
			{
				if(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) !=0)
				{
					if(ascii_char == DLE)
					{
						mprintf("DLE,");
					}
				}
				else
				{
				        state= EXITS;
				        error= 13;               // bad Parity on DLE
				}
			}

			// check for ETX  NOTE: this is the last byte to the CRC 
			if((error ==0)&&(byte_assemble(demod, n, &idx, PAR_7E, &ascii_char, &cksum) !=0))
			{
				if(ascii_char == ETX)
				{
					mprintf("ETX\n");
				}
				else
					error =14;	       // NOT ETX
			}

			if(error ==0)	// if ok then read CRC bytes note 8 bit data
			{

				byte_assemble(demod, n, &idx, PAR_8N, &crc_h, &cksum);
				byte_assemble(demod, n, &idx, PAR_8N, &crc_l, &cksum);
				mprintf("crc16 = %8lx\n",cksum);
				if(cksum != 0)
					error = 15;		// bad crc calculation
			}

			state= EXITS;
			break;

		default:
			state= EXITS;
			break;
	    }
	    if(error !=0)
		    state= EXITS;

	}while(state != EXITS);

	return error;
}

/*---------------------------------------------------------------------------*\
	FUNCTION:	byte_assemble() 
	AUTHOR..:	Peter Wintulich
	DATE....:	20/APR/2004

	Assemble byte from demodulated string & current pointer,
       	Apply Parity check. update pointer.

\*---------------------------------------------------------------------------*/
int byte_assemble( float *demod,	// Demodulated buffer
	       	   int n,		// Sample count in demod
		   int *idx, 	        // Current index into demod
		   int parity,	        // parity check + data mask option
		   char *byte,	        // returned byte
		   unsigned long *cksumb) // Checksum word
{
	(void)n;  //XXX is this really checked anywhere?

	int	valid =1;
	int	bits_out[10];
	int	i;
	int	dc=0;
	char	chr;

	*idx = bit_sync_ascii(bits_out, demod, *idx, &valid);	// demod to data bits

	// Debug assist
	mprintf("         |          0      1      2      3      4     5      6      7\n");
	mprintf("IDX=%05d:",*idx);
	for(i=0; i<100; i++)
	{
		mprintf("%c",(sgn(dc + demod[-63 + i + *idx]) == 1 ?'1':'0'));
	}

	if(valid != 0)
	{
	    chr = (char)  bits2char(bits_out);
	    switch(parity)
	    {
		case 	PAR_7O:		// 7 databit odd parity
			if(bit_parity(bits_out) != 1)
				valid = 0;	// Not valid
			*byte = chr & 0x7f;	// Remove Parity bit
			crc(cksumb, chr);      // Do CRC all 8 bits
			break;

		case 	PAR_7E:         // 7 databit even parity
			if(bit_parity(bits_out) != 0)
				valid = 0;	// Not valid
			*byte = chr & 0x007f;	// Remove Parity bit 
			crc(cksumb, chr); 	// Do CRC all 8 bits
			break;

		case	PAR_8N:		// 8 databit no patity
			*byte = chr;
			crc(cksumb, chr);	// Do CRC
			break;
		default:
			valid = 0;      // Not valid	
	    }
	}
	mprintf("=%02x,%01d\n",*byte,valid);
	return (valid);
}

/*---------------------------------------------------------------------------*\
	FUNCTION:	crc(crc_acc, data) 
	AUTHOR..:	Peter Wintulich
	DATE....:	9 JULY 2004

	CRC16 checking function 
	Need X16+X12+X5+1. Bits enter CRC checker in the order they are recived
	from the raw data streem. So 7 bits and the parity bit. crc_acc should be
	loaded with 0 before start. 

\*---------------------------------------------------------------------------*/
int	crc(unsigned long *crc_acc, char data )
{
	int	i, bit;
	unsigned long	genpoly= 0x00011021;
	// 		 bin 0001 0001 0000 0010 0001 = (2^16+2^12+2^5+2^0)

	for(i=0; i<8; i++)	// go from bit 
	{
		bit=(data & (1<<i) ?1:0);	// sample bit
		*crc_acc= (*crc_acc <<1) + bit; // shift bit into CRC acc
		if((*crc_acc & (1<<16))!=0)	// is overflow 1 from 16th bit
		{
			*crc_acc= (*crc_acc) ^ genpoly;	// xor
		}
	}

	return((*crc_acc) & 0xffff);
}

/*---------------------------------------------------------------------------*\
	FUNCTION:	bit_parity(int bits[]) 
	AUTHOR..:	Peter Wintulich
	DATE....:	22/APR/2004

	check parity of a bits stored in char array.

\*---------------------------------------------------------------------------*/
int	bit_parity(int bits[])
{
	int 	i;
	int	j = 0;

	for(i=0; i<8; i++)
	{
		j+= bits[i];
	}

	return(j & 0x0001);
}

/*---------------------------------------------------------------------------*\
	FUNCTION:	bit_parity(int bits[]) 
	AUTHOR..:	Peter Wintulich
	DATE....:	06/may/2005

	calculate parity and if mode == 1 apply odd parity to byte
				mode == 2 apply even parity to byte
				mode == other return count of 1 bits.
\*---------------------------------------------------------------------------*/
char parity_gen(char bytein, int mode)
{
	int     i;
	int     j = 0;
	char	byteout;

	for(i=0; i<7; i++)
	{
		if(bytein & (1<<i))
			j++;
	}
	if(mode == 1)
	{
		byteout = (bytein & 0x7f) | ((j & 0x01)==1? 0x00: 0x80);
		return byteout;
	}
	if(mode == 2)
	{
		byteout = (bytein & 0x7f) | ((j & 0x01)==1? 0x80: 0x00);
		return byteout;
	}
	return (char) j & 0x01;
}


/*---------------------------------------------------------------------------*\
	FUNCTION: 	demodulate_jp()
	AUTHOR..:	David Rowe + 
	DATE....:	

	Demodulate FSK encoded signal.

\*---------------------------------------------------------------------------*/
void demodulate_jp(
	   float *demod,		// output demodlated samples	
	   short *s,			// input samples
	   int   n,			// length of s and demod
	   float f1,			// low freq. fsk
	   float f2			// high freq. fsk
	)
{
	int   i,j;
	float pi = (float)4.0*(float)atan((float)1.0);
	float w1 = 2*pi*f1/FS;
	float w2 = 2*pi*f2/FS;
	float x_sinF1[NCOEF], x_cosF1[NCOEF], x_sinF2[NCOEF], x_cosF2[NCOEF];
	float xlpsf1, xlpcf1, xlpsf2, xlpcf2;
	float ef1[NAV], ef2[NAV];
	float avef1, avef2;

	//printf("demodulate_jp(%x,%x,%d,%f,%f)\n",demod, s, n, f1, f2 );
	// initialise memories

	for(i=0; i<NCOEF; i++) {
		x_sinF1[i] = (float)0.0;
		x_cosF1[i] = (float)0.0;
		x_sinF2[i] = (float)0.0;
		x_cosF2[i] = (float)0.0;
	}

	for(i=0; i<NAV; i++) {
		ef1[i] = (float)0.0;
		ef2[i] = (float)0.0;
	}

	for(i=0; i<n; i++) {
		// subject signal to mixers
		x_sinF1[NCOEF-1] = s[i]*(float)sin(w1*i);
		x_cosF1[NCOEF-1] = s[i]*(float)cos(w1*i);
		x_sinF2[NCOEF-1] = s[i]*(float)sin(w2*i);
		x_cosF2[NCOEF-1] = s[i]*(float)cos(w2*i);

		// low pass filter
		xlpsf1 = filter(&x_sinF1[NCOEF-1], coef, NCOEF);
		xlpcf1 = filter(&x_cosF1[NCOEF-1], coef, NCOEF);
		xlpsf2 = filter(&x_sinF2[NCOEF-1], coef, NCOEF);
		xlpcf2 = filter(&x_cosF2[NCOEF-1], coef, NCOEF);

		// sum the energies for each arm
		ef1[NAV-1] = xlpsf1*xlpsf1 + xlpcf1*xlpcf1;
		ef2[NAV-1] = xlpsf2*xlpsf2 + xlpcf2*xlpcf2;

		// average energies over past 6 samples
		avef1 = filter(&ef1[NAV-1], avcoef, NAV);
		avef2 = filter(&ef2[NAV-1], avcoef, NAV);

		// output is difference of upper and lower arms
		demod[i] = avef1 - avef2;

		// update memories
		for(j=0; j<NCOEF-1; j++) {
			x_sinF1[j] = x_sinF1[j+1];
			x_cosF1[j] = x_cosF1[j+1];
			x_sinF2[j] = x_sinF2[j+1];
			x_cosF2[j] = x_cosF2[j+1];
		}
		for(j=0; j<NAV-1; j++) {
			ef1[j] = ef1[j+1];
			ef2[j] = ef2[j+1];
		}
	}
}

