//	ARDOP Modem Decode Sound Samples

#include <math.h>
#include "ARDOPC.h"

#pragma warning(disable : 4244)		// Code does lots of float to int

#ifndef TEENSY
#define MEMORYARQ
#endif

#undef PLOTWATERFALL

#ifdef PLOTWATERFALL
#define WHITE  0xffff
#define Tomato 0xffff
#define Orange 0xffff
#define Khaki 0xffff
#define Cyan 0xffff
#define DeepSkyBlue 0
#define RoyalBlue 0 
#define Navy 0
#define Black 0
#endif

#ifdef TEENSY
#define PKTLED LED3		// flash when packet received
extern unsigned int PKTLEDTimer;
#endif

//#define max(x, y) ((x) > (y) ? (x) : (y))
//#define min(x, y) ((x) < (y) ? (x) : (y))

void SendFrametoHost(unsigned char *data, unsigned dlen);

void CheckandAdjustRXLevel(int maxlevel, int minlevel, BOOL Force);
void mySetPixel(unsigned char x, unsigned char y, unsigned int Colour);
void clearDisplay();
void updateDisplay();
VOID L2Routine(UCHAR * Packet, int Length, int FrameQuality, int totalRSErrors, int NumCar, int pktRXMode);
void RemoveProcessedOFDMData();
BOOL  CheckCRC16(unsigned char * Data, int Length);

void DrawAxes(int Qual, const char * Frametype, char * Mode);

extern int lastmax, lastmin;		// Sample Levels

char strRcvFrameTag[32];

BOOL blnLeaderFound = FALSE;

int intLeaderRcvdMs = 1000;		// Leader length??

extern int intLastRcvdFrameQuality;
extern int intReceivedLeaderLen;
extern UCHAR bytLastReceivedDataFrameType;
extern int NErrors;
extern BOOL blnBREAKCmd;
extern UCHAR bytLastACKedDataFrameType;
extern int intARQDefaultDlyMs;
unsigned int tmrFinalID;
extern BOOL PKTCONNECTED;
extern int LastDemodType;

extern int pktRXMode;
extern int RXOFDMMode;

extern BOOL blnBusyStatus;
BOOL blnLastBusyStatus;
int BusyCount;

short intPriorMixedSamples[120];  // a buffer of 120 samples to hold the prior samples used in the filter
int	intPriorMixedSamplesLength = 120;  // size of Prior sample buffer

// While searching for leader we must save unprocessed samples
// We may have up to 720 left, so need 1920 

short rawSamples[2400];	// Get Frame Type need 2400 and we may add 1200
extern int rawSamplesLength;
int maxrawSamplesLength;

short intFilteredMixedSamples[3500];	// Get Frame Type need 2400 and we may add 1200
int intFilteredMixedSamplesLength = 0;
int MaxFilteredMixedSamplesLength = 0;

int intFrameType= 0;				// Type we are decoding
int LastDataFrameType = 0;			// Last data frame processed (for Memory ARQ, etc)

char strDecodeCapture[256];

//	Frame type parameters

int intCenterFreq = 1500;
float floatCarFreq;			//(was int)	// Are these the same ??
int intNumCar;
int intSampPerSym;
int intBaud;
int intDataLen;
int intRSLen;
int intSampleLen;
int DataRate = 0;				// For SCS Reporting
int intDataPtr;
int intDataBytesPerCar;
BOOL blnOdd;
char strType[18] = "";
char strMod[16] = "";
UCHAR bytMinQualThresh;
int intPSKMode;
int intSymbolsPerByte = 4;

// ARDOP V2 has max 10 carriers and 160 (120 + 40RS) per carrier

#define MAX_RAW_LENGTH	163     // Len Byte + Data + RS  + CRC I think!
#define MAX_RAW_LENGTH_FSK	43	// MAX FSK 32 data + 8 RS
// OFDM is MAXCAR * 100
// 10 carrier 16QAM id 10 * 160

#define MAX_DATA_LENGTH	MAXCAR * 100 // I think!  (OFDM 16QAM)

// intToneMags should be an array with one row per carrier.
// and 16 * max bytes data (2 bits per symbol, 4 samples per symbol in 4FSK.

// but as 600 Baud frames are very long (750 bytes), but only one carrier
// may be better to store as scalar and calculate offsets into it for each carrier
// treat 600 as 3 * 200, but scalar still may be better

// Needs 64K if ints + another 64 for MEM ARQ. (maybe able to store as shorts)
// 48K would do if we use a scalar (600 baud, 750 bytes)
// Max is 4 carrier, 83 bytes or 1 carrier 762 (or treat as 3 * 253)

// Could just about do this on Teensy 3.6 or Nucleo F7

// looks like we have 4 samples for each 2 bits, which means 16 samples per byte.

// ARDOP 2 only has one and two carrier FSK modes

// Teensy is rather short of RAM, but as we never receive FSK and PSK
// at the same time we can use same data area (saves about 20K)

int intToneMagsIndex[2];

// Same here

int intSumCounts[MAXCAR];			// number in above arrays

int intToneMagsLength;

unsigned char goodCarriers = 0;	// Carriers we have already decoded

//	We always collect all phases for PSK and QAM so we can do phase correction

// Max PSK frame is 83, 4 samples per byte  = 332
// Max 16QAM frame is 163, 2 samples per byte = 326

// OFDM frames are shorter, so carriers 11 - 17 could have smaller sample buffers

// This is a bit complicated, but allows smaller buffers for the OFDM carriers (Needed for Teensy)

//short intPhases[MAXCAR][332] = {0};	

short QAMPhases[10][332];				// 6640 bytes
short OFDMPhases[MAXCAR - 10][232];		// Need 232 = (PSK2 8 * 29); 15312		

short * Phaseptrs[MAXCAR] = 
	{&QAMPhases[0][0], &QAMPhases[1][0], &QAMPhases[2][0], &QAMPhases[3][0], &QAMPhases[4][0], 
	&QAMPhases[5][0], &QAMPhases[6][0], &QAMPhases[7][0], &QAMPhases[8][0], &QAMPhases[9][0],
	&OFDMPhases[0][0], &OFDMPhases[1][0], &OFDMPhases[2][0], &OFDMPhases[3][0], &OFDMPhases[4][0], 
	&OFDMPhases[5][0], &OFDMPhases[6][0], &OFDMPhases[7][0], &OFDMPhases[8][0], &OFDMPhases[9][0],
	&OFDMPhases[10][0], &OFDMPhases[11][0], &OFDMPhases[12][0], &OFDMPhases[13][0], &OFDMPhases[14][0], 
	&OFDMPhases[15][0], &OFDMPhases[16][0], &OFDMPhases[17][0], &OFDMPhases[18][0], &OFDMPhases[19][0],
	&OFDMPhases[20][0], &OFDMPhases[21][0], &OFDMPhases[22][0], &OFDMPhases[23][0], &OFDMPhases[24][0], 
	&OFDMPhases[25][0], &OFDMPhases[26][0], &OFDMPhases[27][0], &OFDMPhases[28][0], &OFDMPhases[29][0],
	&OFDMPhases[30][0], &OFDMPhases[31][0], &OFDMPhases[32][0]};

short ** intPhases = &Phaseptrs[0];

short QAMMags[10][332];
short OFDMMags[MAXCAR - 10][232];

short * Magptrs[MAXCAR] = 
	{&QAMMags[0][0], &QAMMags[1][0], &QAMMags[2][0], &QAMMags[3][0], &QAMMags[4][0], 
	&QAMMags[5][0], &QAMMags[6][0], &QAMMags[7][0], &QAMMags[8][0], &QAMMags[9][0],
	&OFDMMags[0][0], &OFDMMags[1][0], &OFDMMags[2][0], &OFDMMags[3][0], &OFDMMags[4][0], 
	&OFDMMags[5][0], &OFDMMags[6][0], &OFDMMags[7][0], &OFDMMags[8][0], &OFDMMags[9][0],
	&OFDMMags[10][0], &OFDMMags[11][0], &OFDMMags[12][0], &OFDMMags[13][0], &OFDMMags[14][0], 
	&OFDMMags[15][0], &OFDMMags[16][0], &OFDMMags[17][0], &OFDMMags[18][0], &OFDMMags[19][0],
	&OFDMMags[20][0], &OFDMMags[21][0], &OFDMMags[22][0], &OFDMMags[23][0], &OFDMMags[24][0], 
	&OFDMMags[25][0], &OFDMMags[26][0], &OFDMMags[27][0], &OFDMMags[28][0], &OFDMMags[29][0],
	&OFDMMags[30][0], &OFDMMags[31][0], &OFDMMags[32][0]};

short ** intMags = &Magptrs[0];



//int Tones[2][16 * MAX_RAW_LENGTH_FSK];

int intToneMags[4][16 * MAX_RAW_LENGTH_FSK] = {0};	// Need one per carrier

// We need 5504 bytes for FSK but can overlay on PSK data areas

//int * Toneptrs[2] = {(int *)&Tones[0][0], (int *)&Tones[1][0]};

//int ** intToneMags = &Toneptrs[0];


#ifdef MEMORYARQ

// Enough RAM for memory ARQ so keep all samples for FSK and a copy of tones or phase/amplitude

int  intToneMagsAvg[2][332];	//???? FSK Tone averages

short intCarPhaseAvg[MAXCAR][332];	// array to accumulate phases for averaging (Memory ARQ)
short intCarMagAvg[MAXCAR][332];		// array to accumulate mags for averaging (Memory ARQ) 
 
#endif




//219 /3 * 8= 73 * 8 =  584
//163 * 4 = 652

//	If we do Mem ARQ we will need a fair amount of RAM

int intPhasesLen;

// Received Frame

UCHAR bytData[128 * 80];		// Max OFDM Window
int frameLen;

int totalRSErrors;

// for comparing with CarrierOK
const char Good[MAXCAR] = {1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1};		// All Good
const char Bad[MAXCAR] = {0};	// All bad

// We need one raw buffer per carrier

// This can be optimized quite a bit to save space
// We can probably overlay on bytData

// If we still have 600 baud modes may need a lot more for first

// Note OFDM doesn't need one per carrier so only need 10

UCHAR bytFrameData[10][MAX_RAW_LENGTH + 10];		// Received chars

char CarrierOk[MAXCAR];		// RS OK Flags per carrier
int RepeatedFrame = 0;		// set if this dats frame is a repeat

int charIndex = 0;			// Index into received chars

int SymbolsLeft;			// number still to decode

int DummyCarrier = 0;	// pseudo carrier used for long 600 baud frames
UCHAR * Decode600Buffer;

BOOL PSKInitDone = FALSE;

BOOL blnSymbolSyncFound, blnFrameSyncFound;

extern UCHAR bytLastARQSessionID;
extern UCHAR bytCurrentFrameType;
extern int intShiftUpDn;
extern const char ARQSubStates[10][11];
extern int intLastARQDataFrameToHost;

// dont think I need it short intRcvdSamples[12000];		// 1 second. May need to optimise

float dblOffsetLastGoodDecode = 0;
int dttLastGoodFrameTypeDecode = -20000;

float dblOffsetHz = 0;;
int dttLastLeaderDetect;

extern int intRmtLeaderMeasure;

extern BOOL blnARQConnected;


extern BOOL blnPending;
extern UCHAR bytPendingSessionID;
extern UCHAR bytSessionID;

int dttLastGoodFrameTypeDecod;
int dttStartRmtLeaderMeasure;

char lastGoodID[11] = "";

int GotBitSyncTicks;

int intARQRTmeasuredMs;

float dbl2Pi = 2 * M_PI; 

float dblSNdBPwr;
float dblNCOFreq = 3000;	 // nominal NC) frequency
float dblNCOPhase = 0;
float dblNCOPhaseInc = 2 * M_PI * 3000 / 12000;  // was dblNCOFreq
float dblPwrSNPower_dBPrior = 0;
float dblPhaseDiff1_2Avg;  // an initial value of -10 causes initialization in AcquireFrameSyncRSBAvg


int	intMFSReadPtr = 0;				// reset the MFSReadPtr offset 30 to accomodate the filter delay

int RcvdSamplesLen = 0;				// Samples in RX buffer

float dblPhaseDiff1_2Avg;
int intPhaseError = 0;


BOOL Acquire2ToneLeaderSymbolFraming();
BOOL SearchFor2ToneLeader4(short * intNewSamples, int Length, float * dblOffsetHz, int * intSN);
BOOL AcquireFrameSyncRSB();
BOOL AcquireFrameSyncRSBAvg();
int Acquire4FSKFrameType();

void DemodulateFrame(int intFrameType);
void Demod1Car4FSKChar(int Start, UCHAR * Decoded, int Carrier);
VOID Track1Car4FSK(short * intSamples, int * intPtr, int intSampPerSymbol, float intSearchFreq, int intBaud, UCHAR * bytSymHistory);
VOID Decode1CarPSK(int Carrier, BOOL OFDM);
int EnvelopeCorrelator();
int EnvelopeCorrelatorNew();
BOOL DecodeFrame(int chan, int intFrameType, UCHAR * bytData);

void Update4FSKConstellation(int * intToneMags, int * intQuality);
void Update16FSKConstellation(int * intToneMags, int * intQuality);
void Update8FSKConstellation(int * intToneMags, int * intQuality);
void ProcessPingFrame(char * bytData);
int Compute4FSKSN();

void DemodPSK();
BOOL DemodQAM();
BOOL DemodOFDM();
BOOL Decode4FSKOFDMACK();


void PrintCarrierFlags()
{
	char Msg[128];

	if (intNumCar == 1)
		Debugprintf("MEMARQ Flags %d", CarrierOk[0]);
	else if (intNumCar == 2)
		Debugprintf("MEMARQ Flags %d %d", CarrierOk[0], CarrierOk[1]);

	else
	{
		sprintf(Msg, "MEMARQ Flags %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d",
			CarrierOk[0], CarrierOk[1], CarrierOk[2], CarrierOk[3], CarrierOk[4], CarrierOk[5], CarrierOk[6], CarrierOk[7], CarrierOk[8], CarrierOk[9],
			CarrierOk[10], CarrierOk[11], CarrierOk[12], CarrierOk[13], CarrierOk[14], CarrierOk[15], CarrierOk[16], CarrierOk[17], CarrierOk[18], CarrierOk[19],
			CarrierOk[20], CarrierOk[21], CarrierOk[22], CarrierOk[23], CarrierOk[24], CarrierOk[25], CarrierOk[26], CarrierOk[27], CarrierOk[28], CarrierOk[29],
			CarrierOk[30], CarrierOk[31], CarrierOk[32], CarrierOk[33], CarrierOk[34], CarrierOk[35], CarrierOk[36], CarrierOk[37], CarrierOk[38], CarrierOk[39],
			CarrierOk[40], CarrierOk[41], CarrierOk[42]);
	
		Msg[12 + 2 * intNumCar] = 0; 
		Debugprintf(Msg);
	}

}


// Function to determine if frame type is short control frame
  
BOOL IsShortControlFrame(UCHAR bytType)
{
	switch (intFrameType)
	{
	case DataNAK:
	case DataNAKLoQ:
	case ConRejBusy:
	case ConRejBW:
	case ConAck:
	case DISCFRAME:
	case BREAK:
	case END:
	case IDLEFRAME:
	case DataACK:
	case DataACKHiQ:

		return TRUE;
	}

	return FALSE;
}

BOOL IsConReqFrame(UCHAR bytType)
{
	switch (bytType)
	{
	case ConReq200:
	case ConReq500:
	case ConReq2500:
	case OConReq500:
	case OConReq2500:

		return TRUE;
	}
	return FALSE;
}
 


//	 Function to determine if it is a data frame (Even OR Odd) 

BOOL IsDataFrame(UCHAR intFrameType)
{
	const char * String = Name(intFrameType);

	if (intFrameType == PktFrameHeader)
		return TRUE;
	
	if (String == NULL || String[0] == 0)
		return FALSE;

	if (strstr(String, ".E") || strstr(String, ".O"))
		return TRUE;

	return FALSE;
}

//    Subroutine to clear all mixed samples 

void ClearAllMixedSamples()
{
	intFilteredMixedSamplesLength = 0;
	intMFSReadPtr = 0;
	rawSamplesLength = 0;	// Clear saved
}

//  Subroutine to Initialize mixed samples

void InitializeMixedSamples()
{
	// Measure the time from release of PTT to leader detection of reply.

	intARQRTmeasuredMs = min(10000, Now - dttStartRTMeasure); //?????? needs work
	intPriorMixedSamplesLength = 120;  // zero out prior samples in Prior sample buffer
	intFilteredMixedSamplesLength = 0;	// zero out the FilteredMixedSamples array
	intMFSReadPtr = 0;				// reset the MFSReadPtr offset 30 to accomodate the filter delay
}

//	Subroutine to discard all sampled prior to current intRcvdSamplesRPtr

void DiscardOldSamples()
{
	// This restructures the intRcvdSamples array discarding all samples prior to intRcvdSamplesRPtr
 
	//not sure why we need this !!
/*
	if (RcvdSamplesLen - intRcvdSamplesRPtr <= 0)
		RcvdSamplesLen = intRcvdSamplesRPtr = 0;
	else
	{
		// This is rather slow. I'd prefer a cyclic buffer. Lets see....
		
		memmove(intRcvdSamples, &intRcvdSamples[intRcvdSamplesRPtr], (RcvdSamplesLen - intRcvdSamplesRPtr)* 2);
		RcvdSamplesLen -= intRcvdSamplesRPtr;
		intRcvdSamplesRPtr = 0;
	}
*/
}

//	Subroutine to apply 2000 Hz filter to mixed samples 

float xdblZin_1 = 0, xdblZin_2 = 0, xdblZComb= 0;  // Used in the comb generator

	// The resonators 
      
float xdblZout_0[29] = {0.0f};	// resonator outputs
float xdblZout_1[29] = {0.0f};	// resonator outputs delayed one sample
float xdblZout_2[29] = {0.0f};	// resonator outputs delayed two samples
float xdblCoef[29] = {0.0};		// the coefficients
float xdblR = 0.9995f;			// insures stability (must be < 1.0) (Value .9995 7/8/2013 gives good results)
int xintN = 120;				//Length of filter 12000/100


void FSMixFilter2500Hz(short * intMixedSamples, int intMixedSamplesLength)
{
	// assumes sample rate of 12000
	// implements  27 100 Hz wide sections   (~2500 Hz wide @ - 30dB centered on 1500 Hz)

	// FSF (Frequency Selective Filter) variables

	// This works on intMixedSamples, len intMixedSamplesLength;

	// Filtered data is appended to intFilteredMixedSamples

	float dblRn;
	float dblR2;

	float dblZin = 0;
      
	int i, j;

	float intFilteredSample = 0;			//  Filtered sample

	if (intFilteredMixedSamplesLength < 0)
		Debugprintf("Corrupt intFilteredMixedSamplesLength");

	dblRn = powf(xdblR, xintN);

	dblR2 = powf(xdblR, 2);

	// Initialize the coefficients
    
	if (xdblCoef[28] == 0)
	{
		for (i = 2; i <= 28; i++)
		{
			xdblCoef[i] = 2 * xdblR * cosf(2 * M_PI * i / xintN);  // For Frequency = bin i
		}
	}

	for (i = 0; i < intMixedSamplesLength; i++)
	{
		intFilteredSample = 0;

		if (i < xintN)
			dblZin = intMixedSamples[i] - dblRn * intPriorMixedSamples[i];
		else 
			dblZin = intMixedSamples[i] - dblRn * intMixedSamples[i - xintN];
 
		//Compute the Comb

		xdblZComb = dblZin - xdblZin_2 * dblR2;
		xdblZin_2 = xdblZin_1;
		xdblZin_1 = dblZin;

		// Now the resonators
		for (j = 2; j <= 28; j++)	   // calculate output for 3 resonators 
		{
			xdblZout_0[j] = xdblZComb + xdblCoef[j] * xdblZout_1[j] - dblR2 * xdblZout_2[j];
			xdblZout_2[j] = xdblZout_1[j];
			xdblZout_1[j] = xdblZout_0[j];

			//' scale each by transition coeff and + (Even) or - (Odd) 
			//' Resonators 2 and 13 scaled by .389 get best shape and side lobe supression 
			//' Scaling also accomodates for the filter "gain" of approx 60. 
 
			if (j == 2 || j == 28)
				intFilteredSample += 0.389f * xdblZout_0[j];
			else if ((j & 1) == 0)
				intFilteredSample += xdblZout_0[j];
			else
				intFilteredSample -= xdblZout_0[j];
		}

		intFilteredSample = intFilteredSample * 0.00833333333f;
		intFilteredMixedSamples[intFilteredMixedSamplesLength++] = intFilteredSample;  // rescales for gain of filter
	}
	
	// update the prior intPriorMixedSamples array for the next filter call 
   
	memmove(intPriorMixedSamples, &intMixedSamples[intMixedSamplesLength - xintN], intPriorMixedSamplesLength * 2);		 

	if (intFilteredMixedSamplesLength > MaxFilteredMixedSamplesLength)
		MaxFilteredMixedSamplesLength = intFilteredMixedSamplesLength;

	if (intFilteredMixedSamplesLength > 3500)
		Debugprintf("Corrupt intFilteredMixedSamplesLength %d", intFilteredMixedSamplesLength);

}

//	Function to apply 150Hz filter used in Envelope correlator

void Filter150Hz(short * intFilterOut)
{
	// assumes sample rate of 12000
	// implements  3 100 Hz wide sections   (~150 Hz wide @ - 30dB centered on 1500 Hz)

	// FSF (Frequency Selective Filter) variables

	static float dblR = 0.9995f;		// insures stability (must be < 1.0) (Value .9995 7/8/2013 gives good results)
	static int intN = 120;				//Length of filter 12000/100
	static float dblRn;
	static float dblR2;
	static float dblCoef[17] = {0.0};			// the coefficients
	float dblZin = 0, dblZin_1 = 0, dblZin_2 = 0, dblZComb= 0;  // Used in the comb generator
	// The resonators 
      
	float dblZout_0[17] = {0.0};	// resonator outputs
	float dblZout_1[17] = {0.0};	// resonator outputs delayed one sample
	float dblZout_2[17] = {0.0};	// resonator outputs delayed two samples

	int i, j;

	float FilterOut = 0;			//  Filtered sample
	float largest = 0;

	dblRn = powf(dblR, intN);

	dblR2 = powf(dblR, 2);

	// Initialize the coefficients
    
	if (dblCoef[17] == 0)
	{
		for (i = 14; i <= 16; i++)
		{
			dblCoef[i] = 2 * dblR * cosf(2 * M_PI * i / intN);  // For Frequency = bin i
		}
	}

	for (i = 0; i < 480; i++)
	{
		if (i < intN)
			dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * 0;	// no prior mixed samples
		else
			dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * intFilteredMixedSamples[intMFSReadPtr + i - intN];

		// Compute the Comb
		
		dblZComb = dblZin - dblZin_2 * dblR2;
		dblZin_2 = dblZin_1;
		dblZin_1 = dblZin;

		// Now the resonators

		for (j = 14; j <= 16; j++)		   // calculate output for 3 resonators 
		{
			dblZout_0[j] = dblZComb + dblCoef[j] * dblZout_1[j] - dblR2 * dblZout_2[j];
			dblZout_2[j] = dblZout_1[j];
			dblZout_1[j] = dblZout_0[j];
	
			//	scale each by transition coeff and + (Even) or - (Odd) 

			// Scaling also accomodates for the filter "gain" of approx 120. 
			// These transition coefficients fairly close to optimum for WGN 0db PSK4, 100 baud (yield highest average quality) 5/24/2014
 
			if (j == 14 || j == 16)
				FilterOut = 0.2f * dblZout_0[j];	 // this transisiton minimizes ringing and peaks
			else
				FilterOut -= dblZout_0[j];
		}
		intFilterOut[i] = (int)ceil(FilterOut * 0.00833333333);	 // rescales for gain of filter
	}

}

//	Function to apply 75Hz filter used in Envelope correlator

void Filter75Hz(short * intFilterOut, BOOL blnInitialise, int intSamplesToFilter)
{
	// assumes sample rate of 12000
	// implements  3 100 Hz wide sections   (~150 Hz wide @ - 30dB centered on 1500 Hz)

	// FSF (Frequency Selective Filter) variables

	static float dblR = 0.9995f;		// insures stability (must be < 1.0) (Value .9995 7/8/2013 gives good results)
	static int intN = 240;				//Length of filter 12000/50 - delays output 120 samples from input
	static float dblRn;
	static float dblR2;
	static float dblCoef[3] = {0.0};			// the coefficients
	float dblZin = 0, dblZin_1 = 0, dblZin_2 = 0, dblZComb= 0;  // Used in the comb generator
	// The resonators 
      
	float dblZout_0[3] = {0.0};	// resonator outputs
	float dblZout_1[3] = {0.0};	// resonator outputs delayed one sample
	float dblZout_2[3] = {0.0};	// resonator outputs delayed two samples

	int i, j;

	float FilterOut = 0;			//  Filtered sample
	float largest = 0;

	dblRn = powf(dblR, intN);

	dblR2 = powf(dblR, 2);

	// Initialize the coefficients
    
	if (dblCoef[2] == 0)
	{
		for (i = 0; i <= 3; i++)
		{
			dblCoef[i] = 2 * dblR * cosf(2 * M_PI * (29 + i)/ intN);  // For Frequency = bin 29, 30, 31
		}
	}

	for (i = 0; i < intSamplesToFilter; i++)
	{
		if (i < intN)
			dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * 0;	// no prior mixed samples
		else
			dblZin = intFilteredMixedSamples[intMFSReadPtr + i] - dblRn * intFilteredMixedSamples[intMFSReadPtr + i - intN];

		// Compute the Comb
		
		dblZComb = dblZin - dblZin_2 * dblR2;
		dblZin_2 = dblZin_1;
		dblZin_1 = dblZin;

		// Now the resonators

		for (j = 0; j < 3; j++)		   // calculate output for 3 resonators 
		{
			dblZout_0[j] = dblZComb + dblCoef[j] * dblZout_1[j] - dblR2 * dblZout_2[j];
			dblZout_2[j] = dblZout_1[j];
			dblZout_1[j] = dblZout_0[j];
	
			//	scale each by transition coeff and + (Even) or - (Odd) 

			// Scaling also accomodates for the filter "gain" of approx 120. 
			// These transition coefficients fairly close to optimum for WGN 0db PSK4, 100 baud (yield highest average quality) 5/24/2014
 
			if (j == 0 || j == 2)
				FilterOut -= 0.39811f * dblZout_0[j];	 // this transisiton minimizes ringing and peaks
			else
				FilterOut += dblZout_0[j];
		}
		intFilterOut[i] = (int)ceil(FilterOut * 0.0041f);	 // rescales for gain of filter
	}
}

// Subroutine to Mix new samples with NCO to tune to nominal 1500 Hz center with reversed sideband and filter. 

void MixNCOFilter(short * intNewSamples, int Length, float dblOffsetHz)
{
	// Correct the dimension of intPriorMixedSamples if needed (should only happen after a bandwidth setting change). 

	int i;
	short intMixedSamples[2400];	// All we need at once ( I hope!)		// may need to be int
	int	intMixedSamplesLength = 0;		//size of intMixedSamples

	if (Length == 0)
		return;

	// Nominal NCO freq is 3000 Hz  to downmix intNewSamples  (NCO - Fnew) to center of 1500 Hz (invertes the sideband too) 

	dblNCOFreq = 3000 + dblOffsetHz;
	dblNCOPhaseInc = dblNCOFreq * dbl2Pi / 12000;

	intMixedSamplesLength = Length;

	for (i = 0; i < Length; i++)
	{
		intMixedSamples[i] = (int)ceilf(intNewSamples[i] * cosf(dblNCOPhase));  // later may want a lower "cost" implementation of "Cos"
		dblNCOPhase += dblNCOPhaseInc;
		if (dblNCOPhase > dbl2Pi)
			dblNCOPhase -= dbl2Pi;
	}

	
	
	// showed no significant difference if the 2000 Hz filer used for all bandwidths.
//	printtick("Start Filter");
	FSMixFilter2500Hz(intMixedSamples, intMixedSamplesLength);   // filter through the FS filter (required to reject image from Local oscillator)
//	printtick("Done Filter");

	// save for analysys

//	WriteSamples(&intFilteredMixedSamples[oldlen], Length);
//	WriteSamples(intMixedSamples, Length);

}

//	Function to Correct Raw demodulated data with Reed Solomon FEC 

int CorrectRawDataWithRS(UCHAR * bytRawData, UCHAR * bytCorrectedData, int intDataLen, int intRSLen, int bytFrameType, int Carrier)
{
	BOOL blnRSOK;
	BOOL FrameOK;
	BOOL OK;

	//Dim bytNoRS(1 + intDataLen + 2 - 1) As Byte  ' 1 byte byte Count, Data, 2 byte CRC 
	//Array.Copy(bytRawData, 0, bytNoRS, 0, bytNoRS.Length)

	if (CarrierOk[Carrier])	// Already decoded this carrier?
	{
		// Athough we have already checked the data, it may be in the wrong place
		// in the buffer if another carrier was decoded wrong.

		memcpy(bytCorrectedData, &bytRawData[1], bytRawData[0] + 1);    // Extra char in case OFDM

		if (strFrameType[LastDataFrameType][0] == 'O')
			Debugprintf("[CorrectRawDataWithRS] Carrier %d already decoded Block %d, Len %d", Carrier, bytRawData[1], bytRawData[0]);
		else
			Debugprintf("[CorrectRawDataWithRS] Carrier %d already decoded Len %d", Carrier, bytRawData[0]);
		return bytRawData[0];			// don't do it again
	}

	if (strFrameType[intFrameType][0] == 'O')
		OK = CheckCRC16(bytRawData, intDataLen + 1);
	else
		OK = CheckCRC16FrameType(bytRawData, intDataLen + 1, bytFrameType);

	// As crc can fail also check returned lenght is reasonable

	if (OK && bytRawData[0] <= intDataLen) // No RS correction needed	// return the actual data
	{
		memcpy(bytCorrectedData, &bytRawData[1], bytRawData[0] + 1);    
		if (strFrameType[intFrameType][0] == 'O')
			Debugprintf("[CorrectRawDataWithRS] Carrier %d OK without RS, Block %d Len = %d", Carrier, bytRawData[1], bytRawData[0]);
		else
			Debugprintf("[CorrectRawDataWithRS] Carrier %d OK without RS, Len = %d", Carrier, bytRawData[0]);

		CarrierOk[Carrier] = TRUE;
		return bytRawData[0];
	}
	
	// Try correcting with RS Parity

	FrameOK = RSDecode(bytRawData, intDataLen + 3 + intRSLen, intRSLen, &blnRSOK);

	if (blnRSOK)
	{}
//		Debugprintf("RS Says OK without correction");
	else
	if (FrameOK)
	{}
//		Debugprintf("RS Says OK after %d correction(s)", NErrors);
	else
	{
		Debugprintf("[intFrameType] RS Says Can't Correct");
		goto returnBad;
	}

    if (FrameOK)
	{
		if (strFrameType[intFrameType][0] == 'O')
			OK = CheckCRC16(bytRawData, intDataLen + 1);
		else
			OK = CheckCRC16FrameType(bytRawData, intDataLen + 1, bytFrameType);

		if (OK && bytRawData[0] <= intDataLen) // Now OK -  return the actual data
		{
			int intFailedByteCnt = 0;

			if (strFrameType[intFrameType][0] == 'O')
				Debugprintf("[CorrectRawDataWithRS] Carrier %d OK with RS %d corrections, Block %d, Len = %d", Carrier, NErrors, bytRawData[1], bytRawData[0]);
			else
				Debugprintf("[CorrectRawDataWithRS] Carrier %d OK with RS %d corrections, Len = %d", Carrier, NErrors, bytRawData[0]);
			
			totalRSErrors += NErrors;
 
			memcpy(bytCorrectedData, &bytRawData[1], bytRawData[0] + 1);  
			CarrierOk[Carrier] = TRUE;
			return bytRawData[0];
		}
        Debugprintf("[CorrectRawDataWithRS] Carrier %d RS says ok but CRC still bad", Carrier);
	}
	// return uncorrected data without byte count or RS Parity

returnBad:

	memcpy(bytCorrectedData, &bytRawData[1], intDataLen + 1);    
     
	CarrierOk[Carrier] = FALSE;
	return intDataLen;
}



// Subroutine to process new samples as received from the sound card via Main.ProcessCapturedData
// Only called when not transmitting

double dblPhaseInc;  // in milliradians
short intNforGoertzel[MAXCAR];
short intPSKPhase_1[MAXCAR], intPSKPhase_0[MAXCAR];
short intCP[MAXCAR];	  // Cyclic prefix offset 
float dblFreqBin[MAXCAR];

BOOL CheckFrameTypeParity(int intTonePtr, int * intToneMags);

void ARDOPProcessNewSamples(int chan, short * Samples, int nSamples)
{
	BOOL blnFrameDecodedOK = FALSE;

//	LookforUZ7HOLeader(Samples, nSamples);

//	printtick("Start afsk");
//	DemodAFSK(Samples, nSamples);
//	printtick("End afsk");

//	return;


	if (ProtocolState == FECSend)
		return;

	// Append new data to anything in rawSamples

	memcpy(&rawSamples[rawSamplesLength], Samples, nSamples * 2);
	rawSamplesLength += nSamples;

	if (rawSamplesLength > maxrawSamplesLength)
		maxrawSamplesLength = rawSamplesLength;

	if (rawSamplesLength >= 2400)
		Debugprintf("Corrupt rawSamplesLength %d", rawSamplesLength);


	nSamples = rawSamplesLength;
	Samples = rawSamples;
	
	rawSamplesLength = 0;

//	printtick("Start Busy");
	if (nSamples >= 1024)
		UpdateBusyDetector(Samples);
//	printtick("Done Busy");

	// it seems that searchforleader runs on unmixed and unfilered samples

	// Searching for leader

	if (State == SearchingForLeader)
	{
		// Search for leader as long as 960 samples (8  symbols) available

//		printtick("Start Leader Search");

		if (nSamples >= 1200)
		{		
			if (ProtocolState == FECSend)
					return;
		}
		while (State == SearchingForLeader && nSamples >= 1200)
		{
			int intSN;
			
			blnLeaderFound = SearchFor2ToneLeader4(Samples, nSamples, &dblOffsetHz, &intSN);
//			blnLeaderFound = SearchFor2ToneLeader2(Samples, nSamples, &dblOffsetHz, &intSN);
		
			if (blnLeaderFound)
			{
//				Debugprintf("Got Leader");

				dttLastLeaderDetect = Now;

				nSamples -= 480;
				Samples += 480;		// !!!! needs attention !!!

				InitializeMixedSamples();
				State = AcquireSymbolSync;
			}
			else
			{
				if (SlowCPU)
				{
					nSamples -= 480;
					Samples += 480;		 // advance pointer 2 symbols (40 ms) ' reduce CPU loading
				}
				else
				{
					nSamples -= 240;
					Samples += 240;		// !!!! needs attention !!!
				}
			}
		}
		if (State == SearchingForLeader)
		{
			// Save unused samples

			memmove(rawSamples, Samples, nSamples * 2);
			rawSamplesLength = nSamples;

//			printtick("End Leader Search");

			return;
		}
	}


	// Got leader

	//	At this point samples haven't been processed, and are in Samples, len nSamples

	// I'm going to filter all samples into intFilteredMixedSamples.

//	printtick("Start Mix");

	MixNCOFilter(Samples, nSamples, dblOffsetHz); // Mix and filter new samples (Mixing consumes all intRcvdSamples)
	nSamples = 0;	//	all used

//	printtick("Done Mix Samples");

	// Acquire Symbol Sync 

    if (State == AcquireSymbolSync)
	{
		if ((intFilteredMixedSamplesLength - intMFSReadPtr) > 960)
		{
			blnSymbolSyncFound = Acquire2ToneLeaderSymbolFraming();  // adjust the pointer to the nominal symbol start based on phase
			if (blnSymbolSyncFound)
				State = AcquireFrameSync;
			else
			{
				// Rick's Latest code (2.0.3) advances pointer instead of clearing samples
//				DiscardOldSamples();
//				ClearAllMixedSamples();
				intMFSReadPtr += 240; //  advance the MFSReadPointer one symbol and try to search for leader again. 
				State = SearchingForLeader;
				return;
			}
//			printtick("Got Sym Sync");
		}
	}
	
	//	Acquire Frame Sync
	
	if (State == AcquireFrameSync)
	{
		blnFrameSyncFound = AcquireFrameSyncRSB();

		// Remove used samples

		intFilteredMixedSamplesLength -= intMFSReadPtr;

		if (intFilteredMixedSamplesLength < 0)
			Debugprintf("Corrupt intFilteredMixedSamplesLength");

		memmove(intFilteredMixedSamples,
			&intFilteredMixedSamples[intMFSReadPtr], intFilteredMixedSamplesLength * 2);

		intMFSReadPtr = 0;

	
		if (blnFrameSyncFound)
		{
			State = AcquireFrameType;
				
			//	Have frame Sync. Remove used samples from buffer

			printtick("Got Frame Sync");

		}
		else if ((Now - dttLastLeaderDetect) > 1000)		 // no Frame sync within 1000 ms (may want to make this limit a funciton of Mode and leaders)
		{
			DiscardOldSamples();
			ClearAllMixedSamples();
			State = SearchingForLeader;
			printtick("frame sync timeout");
		}
/*
else if (intPhaseError > 2)
		{
			DiscardOldSamples();
			ClearAllMixedSamples();
			State = SearchingForLeader;
			printtick("frame sync timeout");
		}
//		else
//			printtick("no frame sync");
*/


	}
	
	//	Acquire Frame Type

	if (State == AcquireFrameType)
	{
//		printtick("getting frame type");

		intFrameType = Acquire4FSKFrameType();
		if (intFrameType == -2)
		{
//			sprintf(Msg, "not enough %d %d", intFilteredMixedSamplesLength, intMFSReadPtr);
//			printtick(Msg);
			return;		//  insufficient samples
		}

		if (intFrameType == -1)		  // poor decode quality (large decode distance)
		{
			State = SearchingForLeader;
			ClearAllMixedSamples();
			DiscardOldSamples();
			Debugprintf("poor frame type decode");

			// stcStatus.BackColor = SystemColors.Control
			// stcStatus.Text = ""
			// stcStatus.ControlName = "lblRcvFrame"
			// queTNCStatus.Enqueue(stcStatus)
		}
		else
		{
			//	Get Frame info and Initialise Demodulate variables

			// We've used intMFSReadPtr samples, so remove from Buffer

//			sprintf(Msg, "Got Frame Type %x", intFrameType);
//			printtick(Msg);

			blnLastBusyStatus = 1;
			blnBusyStatus = 1; 
			BusyCount = 10;

			intFilteredMixedSamplesLength -= intMFSReadPtr;
	
			if (intFilteredMixedSamplesLength < 0)
				Debugprintf("Corrupt intFilteredMixedSamplesLength");
	
			memmove(intFilteredMixedSamples,
				&intFilteredMixedSamples[intMFSReadPtr], intFilteredMixedSamplesLength * 2); 

			intMFSReadPtr = 0;

			if (!FrameInfo(intFrameType, &blnOdd, &intNumCar, strMod, &intBaud, &intDataLen, &intRSLen, &bytMinQualThresh, strType))
			{
				printtick("bad frame type");
				State = SearchingForLeader;
				ClearAllMixedSamples();
				DiscardOldSamples();
				return;
			}

			if (IsShortControlFrame(intFrameType))
			{
				// Frame has no data so is now complete

				DrawRXFrame(1, Name(intFrameType));

				// See if IRStoISS shortcut can be invoked
			// prepare for next

				DiscardOldSamples();
				ClearAllMixedSamples();
				State = SearchingForLeader;
				blnFrameDecodedOK = TRUE;
				Debugprintf("[DecodeFrame] Frame: %s ", Name(intFrameType));

				DecodeCompleteTime = Now;

				goto ProcessFrame;
			}

			DrawRXFrame(0, Name(intFrameType));

			if (intBaud == 25)
				intSampPerSym = 480;
			else if (intBaud == 50)
				intSampPerSym = 240;
			else if (intBaud == 55)
				intSampPerSym = 216;
			else if (intBaud == 100)
				intSampPerSym = 120;
			else if (intBaud == 167)
				intSampPerSym = 72;
			else if (intBaud == 600)
				intSampPerSym = 20;

			if (IsDataFrame(intFrameType))
				SymbolsLeft = intDataLen + intRSLen + 3; // Data has crc + length byte
			else if (intFrameType == OFDMACK)
				SymbolsLeft = intDataLen + intRSLen + 2;	// CRC but no len
			else
				SymbolsLeft = intDataLen + intRSLen;	// No CRC

			if (intDataLen == 600)
				SymbolsLeft += 6;		// 600 baud has 3 * RS Blocks

			// Save data rate for PTC reporting

			if (Rate[intFrameType] > 0)
				DataRate = Rate[intFrameType];

			intToneMagsLength = 16 * SymbolsLeft;	// 4 tones, 2 bits per set
			
			memset(intToneMagsIndex, 0, sizeof(intToneMagsIndex));
			
			charIndex = 0;	
			PSKInitDone = 0;
			
			frameLen = 0;
			totalRSErrors = 0;

			DummyCarrier = 0;	// pseudo carrier used for long 600 baud frames
			Decode600Buffer = &bytFrameData[0][0];

			State = AcquireFrame;
			
			// if a data frame, and not the same frame type as last, reinitialise 
			// correctly received carriers byte and memory ARQ fields

//			if (IsDataFrame(intFrameType) && LastDataFrameType != intFrameType)

			if (intFrameType == PktFrameHeader || intFrameType == PktFrameData)
			{
				memset(CarrierOk, 0, sizeof(CarrierOk));
				memset(intSumCounts, 0, sizeof(intSumCounts));
#ifdef MEMORYARQ
				memset(intToneMagsAvg, 0, sizeof(intToneMagsAvg));
				memset(intCarPhaseAvg, 0, sizeof(intCarPhaseAvg));
				memset(intCarMagAvg, 0, sizeof(intCarMagAvg));
#endif
				LastDataFrameType = intFrameType;
			}
			else if (LastDataFrameType != intFrameType)
			{
				if (strFrameType[LastDataFrameType][0] == 'O')
				{
					// OFDM Frame, We know the ISS received the last ack, so can remove any data passed to host.
					// We need to do that, as new frame block numbers will start from first unacked block.

					if (intFrameType == OFDMACK)
						RepeatedFrame = FALSE;

					RepeatedFrame = FALSE;
					RemoveProcessedOFDMData();	
				}

				Debugprintf("New frame type - MEMARQ flags reset");
				memset(CarrierOk, 0, sizeof(CarrierOk));
	
				if (IsDataFrame(intFrameType))
					LastDataFrameType = intFrameType;

				// note that although we only do mem arq if enough RAM we
				// still skip decoding carriers that have been received;

#ifdef MEMORYARQ
				memset(intSumCounts, 0, sizeof(intSumCounts));
				memset(intToneMagsAvg, 0, sizeof(intToneMagsAvg));
				memset(intCarPhaseAvg, 0, sizeof(intCarPhaseAvg));
				memset(intCarMagAvg, 0, sizeof(intCarMagAvg));
#endif
			}
			else
			{
				// Repeated frame. OFDM needs to know, as we may have passed data to host.

				if (IsDataFrame(intFrameType))
					RepeatedFrame = TRUE;

			}

			PrintCarrierFlags();
		}
	}
	// Acquire Frame

	if (State == AcquireFrame)
	{
		// Call DemodulateFrame for each set of samples

		DemodulateFrame(intFrameType);

		if (State == AcquireFrame)

			// We haven't got it all yet so wait for more samples	
			return;	

		//	We have the whole frame, so process it


//		printtick("got whole frame");

		LastDemodType = intFrameType;

		if (strcmp (strMod, "4FSK") == 0)
			Update4FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
		else if (strcmp (strMod, "16FSK") == 0)
			Update16FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
		else if (strcmp (strMod, "8FSK") == 0)
			Update8FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);

		// PSK and QAM quality done in Decode routines

		Debugprintf("Qual = %d", intLastRcvdFrameQuality);

		// This mechanism is to skip actual decoding and reply/change state...no need to decode 

     
		blnFrameDecodedOK = DecodeFrame(chan, intFrameType, bytData);

ProcessFrame:	

		if (!blnFrameDecodedOK)
			DrawRXFrame(2, Name(intFrameType));

		if (intFrameType == PktFrameData)
		{
#ifdef TEENSY
			SetLED(PKTLED, TRUE);		// Flash LED
			PKTLEDTimer = Now + 200;	// for 200 mS
#endif
			return;
		}

		if (blnFrameDecodedOK)
		{
			// Set input level if supported
			
#ifdef HASPOTS
			CheckandAdjustRXLevel(lastmax, lastmin, TRUE);
#endif
			if (AccumulateStats)
				if (IsDataFrame(intFrameType))
					if (strstr (strMod, "PSK"))
						intGoodPSKFrameDataDecodes++;
					else if (strstr (strMod, "QAM"))
						intGoodQAMFrameDataDecodes++;
					else if (strstr (strMod, "OFDM"))
						intGoodOFDMFrameDataDecodes++;
					else	
						intGoodFSKFrameDataDecodes++;

#ifdef TEENSY
			if (IsDataFrame(intFrameType))
			{
				SetLED(PKTLED, TRUE);		// Flash LED
				PKTLEDTimer = Now + 400;	// For 400 Ms
			}
#endif			
		}
		else
		{
			//	Bad decode

			if (AccumulateStats)
				if (IsDataFrame(intFrameType))
					if (strstr(strMod, "PSK"))
						intFailedPSKFrameDataDecodes++;
					else if (strstr(strMod, "QAM"))
						intFailedQAMFrameDataDecodes++;
					else if (strstr(strMod, "OFDM"))
						intFailedOFDMFrameDataDecodes++;
					else
						intFailedFSKFrameDataDecodes++;


			// Debug.WriteLine("[DecodePSKData2] bytPass = " & Format(bytPass, "X"))

		}
skipDecode:			
		State = SearchingForLeader;
		ClearAllMixedSamples();
		DiscardOldSamples();
		return;

	}
}
// Subroutine to compute Goertzel algorithm and return Real and Imag components for a single frequency bin

void GoertzelRealImag(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
	// intRealIn is a buffer at least intPtr + N in length
	// N need not be a power of 2
	// m need not be an integer
	// Computes the Real and Imaginary Freq values for bin m
	// Verified to = FFT results for at least 10 significant digits
	// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
	//        GoertzelRealImag .015 ms   Normal FFT (.5 ms)
	//  assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
	//  FFT:Goertzel time  ratio ~ 3.3 Log2(N)

	//  Sanity check

	//if (intPtr < 0 Or (intRealIn.Length - intPtr) < N Then
    //        dblReal = 0 : dblImag = 0 : Exit Sub
     //   End If

	float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
	float dblCoeff = 2 * cosf(2 * M_PI * m / N);
	int i;

	for (i = 0; i <= N; i++)
	{
		if (i == N)
			dblW = dblZ_1 * dblCoeff - dblZ_2;
		else
			dblW = intRealIn[intPtr] + dblZ_1 * dblCoeff - dblZ_2;

		dblZ_2 = dblZ_1;
		dblZ_1 = dblW;
		intPtr++;
	}
	*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2
	*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2   (this sign agrees with Scope DSP phase values) 
}

// Subroutine to compute Goertzel algorithm and return Real and Imag components for a single frequency bin with a Hanning Window function

float dblHanWin[120];
float dblHanAng;
int HanWinLen = 0;

float dblHannWin[480];
float dblHannAng;

// Subroutine to compute Goertzel algorithm and return Real and Imag components for a single frequency bin with a Hann Window function for N a multiple of 120

void GoertzelRealImagHann120(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
    // This version precomputes the raised cosine (Hann or Hanning) window and uses it for any length that is a multiple of 120 samples
	// intRealIn is a buffer at least intPtr + N in length
    // N must be 960 to use this function
    // Hann coefficients are approximated for N>120 but should be close
	// m need not be an integer
	// Computes the Real and Imaginary Freq values for bin m
	// Verified to = FFT results for at least 10 significant digits
	// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
	//        GoertzelRealImag .015 ms   Normal FFT (.5 ms)
	//  assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
	//  FFT:Goertzel time  ratio ~ 3.3 Log2(N)

 
  	float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
	float dblCoeff = 2 * cosf(2 * M_PI * m / N);

	int i;
	int intM = N / 120;	// No if 120 sample blocks

	if (HanWinLen != N)  //if there is any change in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
	{
		HanWinLen = N;

		dblHanAng = 2 * M_PI / 120;

		for (i = 0; i < 60; i++)
		{
			dblHanWin[i] = 0.5 - 0.5 * cosf(i * dblHanAng + dblHanAng);
		}
	}

	for (i = 0; i <= N; i++)
	{
		if (i == N)
			dblW = dblZ_1 * dblCoeff - dblZ_2;
		
		else if (i < (N / 2))	// ist half of 120 sample block
				// looks like we use values 0 ti 59 then 59 down to 0
			dblW = intRealIn[intPtr] * dblHanWin[(i /intM) % 60] + dblZ_1 * dblCoeff - dblZ_2;
		else
			dblW = intRealIn[intPtr]  * dblHanWin[59 - ((i /intM) % 60)] + dblZ_1 * dblCoeff - dblZ_2;

		dblZ_2 = dblZ_1;
		dblZ_1 = dblW;
		intPtr++;
	}
	
	*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2
	*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2   (this sign agrees with Scope DSP phase values) 

}




void GoertzelRealImagHann960(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
    // This version precomputes the raised cosine (Hann or Hanning) window and uses it for any length that is a multiple of 120 samples
	// intRealIn is a buffer at least intPtr + N in length
    // N must be a multiple of 120 to use this function
    // Hann coefficients are approximated for N>120 but should be close
	// m need not be an integer
	// Computes the Real and Imaginary Freq values for bin m
	// Verified to = FFT results for at least 10 significant digits
	// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
	//        GoertzelRealImag .015 ms   Normal FFT (.5 ms)
	//  assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
	//  FFT:Goertzel time  ratio ~ 3.3 Log2(N)

 
  	float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
	float dblCoeff = 2 * cosf(2 * M_PI * m / N);

	int i;
	int intM = N / 120;	// No if 120 sample blocks

	if (dblHannWin[479] < 0.5)  //if there is any change in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
	{
		dblHannAng = 2 * M_PI / 960;

		for (i = 0; i < 480; i++)
		{
			dblHannWin[i] = 0.5 - 0.5 * cosf(i * dblHannAng + dblHannAng);
		}
	}

	for (i = 0; i <= N; i++)
	{
		if (i == N)
			dblW = dblZ_1 * dblCoeff - dblZ_2;
		
		else if (i < (N / 2))	// ist half of 120 sample block
				// looks like we use values 0 ti 59 then 59 down to 0
			dblW = intRealIn[intPtr] * dblHannWin[(i /intM) % 60] + dblZ_1 * dblCoeff - dblZ_2;
		else
			dblW = intRealIn[intPtr]  * dblHannWin[479 - ((i /intM) % 60)] + dblZ_1 * dblCoeff - dblZ_2;

		dblZ_2 = dblZ_1;
		dblZ_1 = dblW;
		intPtr++;
	}
	
	*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2
	*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2   (this sign agrees with Scope DSP phase values) 

}





void GoertzelRealImagHanning(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
	// intRealIn is a buffer at least intPtr + N in length
	// N need not be a power of 2
	// m need not be an integer
	// Computes the Real and Imaginary Freq values for bin m
	// Verified to = FFT results for at least 10 significant digits
	// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
	//        GoertzelRealImag .015 ms   Normal FFT (.5 ms)
	//  assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
	//  FFT:Goertzel time  ratio ~ 3.3 Log2(N)

	//  Sanity check
 
  	float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
	float dblCoeff = 2 * cosf(2 * M_PI * m / N);

	int i;

	if (HanWinLen != N)  //if there is any change in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
	{
		HanWinLen = N;

		dblHanAng = 2 * M_PI / (N - 1);

		for (i = 0; i < N; i++)
		{
			dblHanWin[i] = 0.5 - 0.5 * cosf(i * dblHanAng);
		}
	}

	for (i = 0; i <= N; i++)
	{
		if (i == N)
			dblW = dblZ_1 * dblCoeff - dblZ_2;
		else
			dblW = intRealIn[intPtr]  * dblHanWin[i] + dblZ_1 * dblCoeff - dblZ_2;

		dblZ_2 = dblZ_1;
		dblZ_1 = dblW;
		intPtr++;
	}
	
	*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2
	*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2   (this sign agrees with Scope DSP phase values) 
}

float dblHamWin[1200];
float dblHamAng;
int HamWinLen = 0;

void GoertzelRealImagHamming(short intRealIn[], int intPtr, int N, float m, float * dblReal, float * dblImag)
{
	// intRealIn is a buffer at least intPtr + N in length
	// N need not be a power of 2
	// m need not be an integer
	// Computes the Real and Imaginary Freq values for bin m
	// Verified to = FFT results for at least 10 significant digits
	// Timings for 1024 Point on Laptop (64 bit Core Duo 2.2 Ghz)
	//        GoertzelRealImag .015 ms   Normal FFT (.5 ms)
	//  assuming Goertzel is proportional to N and FFT time proportional to Nlog2N
	//  FFT:Goertzel time  ratio ~ 3.3 Log2(N)

	//  Sanity check
 
  	float dblZ_1 = 0.0f, dblZ_2 = 0.0f, dblW = 0.0f;
	float dblCoeff = 2 * cosf(2 * M_PI * m / N);

	int i;

	if (HamWinLen != N)  //if there is any cHamge in N this is then recalculate the Hanning Window...this mechanism reduces use of Cos
	{
		HamWinLen = N;

		dblHamAng = 2 * M_PI / (N - 1);

		for (i = 0; i < N; i++)
		{
			dblHamWin[i] = 0.54f - 0.46f * cosf(i * dblHamAng);
		}
	}

	for (i = 0; i <= N; i++)
	{
		if (i == N)
			dblW = dblZ_1 * dblCoeff - dblZ_2;
		else
			dblW = intRealIn[intPtr]  * dblHamWin[i] + dblZ_1 * dblCoeff - dblZ_2;

		dblZ_2 = dblZ_1;
		dblZ_1 = dblW;
		intPtr++;
	}
	
	*dblReal = 2 * (dblW - cosf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2
	*dblImag = 2 * (sinf(2 * M_PI * m / N) * dblZ_2) / N;  // scale results by N/2   (this sign agrees with Scope DSP phase values) 
}

// Function to interpolate spectrum peak using Quinn algorithm

float QuinnSpectralPeakLocator(float XkM1Re, float XkM1Im, float XkRe, float XkIm, float XkP1Re, float XkP1Im)
{
	// based on the Quinn algorithm in Streamlining Digital Processing page 139
	// Alpha1 = Re(Xk-1/Xk)
	// Alpha2 = Re(Xk+1/Xk)
	//Delta1 = Alpha1/(1 - Alpha1)
	//'Delta2 = Alpha2/(1 - Alpha2)
	// if Delta1 > 0 and Delta2 > 0 then Delta = Delta2 else Delta = Delta1
	// should be within .1 bin for S:N > 2 dB

	float dblDenom = powf(XkRe, 2) + powf(XkIm, 2);
	float dblAlpha1;
	float dblAlpha2;
	float dblDelta1;
	float dblDelta2;

	dblAlpha1 = ((XkM1Re * XkRe) + (XkM1Im * XkIm)) / dblDenom;
	dblAlpha2 = ((XkP1Re * XkRe) + (XkP1Im * XkIm)) / dblDenom;
	dblDelta1 = dblAlpha1 / (1 - dblAlpha1);
	dblDelta2 = dblAlpha2 / (1 - dblAlpha2);

	if (dblDelta1 > 0 &&  dblDelta2 > 0)
		return dblDelta2;
	else
		return dblDelta1;
}

// Function to interpolate spectrum peak using simple interpolation 

float SpectralPeakLocator(float XkM1Re, float XkM1Im, float XkRe, float XkIm, float XkP1Re, float XkP1Im, float * dblCentMag, char * Win)
{
	// Use this for Windowed samples instead of QuinnSpectralPeakLocator

	float dblLeftMag, dblRightMag;
	*dblCentMag = sqrtf(powf(XkRe, 2) + powf(XkIm, 2));

	dblLeftMag  = sqrtf(powf(XkM1Re, 2) + powf(XkM1Im, 2));
	dblRightMag  = sqrtf(powf(XkP1Re, 2) + powf(XkP1Im, 2));

	//Factor 1.22 empirically determine optimum for Hamming window
	// For Hanning Window use factor of 1.36
	// For Blackman Window use factor of  1.75
    
	if (strcmp(Win, "Blackman"))
		return 1.75 * (dblRightMag - dblLeftMag) / (dblLeftMag + *dblCentMag + dblRightMag);  // Optimized for Hamming Window
	if (strcmp(Win, "Hann"))
		return 1.36 * (dblRightMag - dblLeftMag) / (dblLeftMag + *dblCentMag + dblRightMag);  // Optimized for Hamming Window
	if (strcmp(Win, "Hamming"))
		return 1.22 * (dblRightMag - dblLeftMag) / (dblLeftMag + *dblCentMag + dblRightMag);  // Optimized for Hamming Window

	return 0;
}

// Function to detect and tune the 50 baud 2 tone leader (for all bandwidths) Updated version of SearchFor2ToneLeader2 

float dblPriorFineOffset = 1000.0f;


BOOL SearchFor2ToneLeader3(short * intNewSamples, int Length, float * dblOffsetHz, int * intSN)
{
	// This version uses 10Hz bin spacing. Hamming window on Goertzel, and simple spectral peak interpolator
	// It requires about 50% more CPU time when running but produces more sensive leader detection and more accurate tuning
	// search through the samples looking for the telltail 50 baud 2 tone pattern (nominal tones 1475, 1525 Hz)
	// Find the offset in Hz (due to missmatch in transmitter - receiver tuning
	// Finds the S:N (power ratio of the tones 1475 and 1525 ratioed to "noise" averaged from bins at 1425, 1450, 1550, and 1575Hz)
 
	float dblGoertzelReal[56];
	float dblGoertzelImag[56];
	float dblMag[56];
	float dblPower, dblLeftMag, dblRightMag;
	float dblMaxPeak = 0.0, dblMaxPeakSN = 0.0, dblBinAdj;
	int intInterpCnt = 0;  // the count 0 to 3 of the interpolations that were < +/- .5 bin
	int  intIatMaxPeak = 0;
	float dblAlpha = 0.3f;  // Works well possibly some room for optimization Changed from .5 to .3 on Rev 0.1.5.3
	float dblInterpretThreshold= 1.0f; // Good results June 6, 2014 (was .4)  ' Works well possibly some room for optimization
	float dblFilteredMaxPeak = 0;
	int intStartBin, intStopBin;
	float dblLeftCar, dblRightCar, dblBinInterpLeft, dblBinInterpRight, dblCtrR, dblCtrI, dblLeftP, dblRightP;
	float dblLeftR[3], dblLeftI[3], dblRightR[3], dblRightI[3];
	int i;
	int Ptr = 0;
	float dblAvgNoisePerBin, dblCoarsePwrSN, dblBinAdj1475, dblBinAdj1525, dblCoarseOffset = 1000;
	float dblTrialOffset, dblPowerEarly, dblSNdBPwrEarly;

	if ((Length) < 1200)
		return FALSE;		// ensure there are at least 1200 samples (5 symbols of 240 samples)

//	if ((Now - dttLastGoodFrameTypeDecode > 20000) && TuningRange > 0)
	{
		// this is the full search over the full tuning range selected.  Uses more CPU time and with possibly larger deviation once connected. 
		
		intStartBin = ((200 - TuningRange) / 10);
		intStopBin = 55 - intStartBin;

		dblMaxPeak = 0;

		// Generate the Power magnitudes for up to 56 10 Hz bins (a function of MCB.TuningRange) 
  
		for (i = intStartBin; i <= intStopBin; i++)
		{
            // note hamming window reduces end effect caused by 1200 samples (not an even multiple of 240)  but spreads response peaks
		
			GoertzelRealImagHamming(intNewSamples, Ptr, 1200, i + 122.5f, &dblGoertzelReal[i], &dblGoertzelImag[i]);
			dblMag[i] = powf(dblGoertzelReal[i], 2) + powf(dblGoertzelImag[i], 2); // dblMag(i) in units of power (V^2)
 		}

		// Search the bins to locate the max S:N in the two tone signal/avg noise.  

 		for (i = intStartBin + 5; i <= intStopBin - 10; i++)	// ' +/- MCB.TuningRange from nominal 
		{
			dblPower = sqrtf(dblMag[i] * dblMag[i + 5]); // using the product to minimize sensitivity to one strong carrier vs the two tone
			// sqrt converts back to units of power from Power ^2
			// don't use center noise bin as too easily corrupted by adjacent carriers

			dblAvgNoisePerBin = (dblMag[i - 5] + dblMag[i - 3] + dblMag[i + 8] + dblMag[i + 10]) / 4;  // Simple average
			dblMaxPeak = dblPower / dblAvgNoisePerBin;
			if (dblMaxPeak > dblMaxPeakSN)
			{
				dblMaxPeakSN = dblMaxPeak;
				dblCoarsePwrSN = 10 * log10f(dblMaxPeak);
				intIatMaxPeak = i + 122;
			}
		}
		// Do the interpolation based on the two carriers at nominal 1475 and 1525Hz

		if (((intIatMaxPeak - 123) >= intStartBin) && ((intIatMaxPeak - 118) <= intStopBin)) // check to ensure no index errors
		{
			// Interpolate the adjacent bins using QuinnSpectralPeakLocator

			dblBinAdj1475 = SpectralPeakLocator(
				dblGoertzelReal[intIatMaxPeak - 123], dblGoertzelImag[intIatMaxPeak - 123],
				dblGoertzelReal[intIatMaxPeak - 122], dblGoertzelImag[intIatMaxPeak - 122], 
				dblGoertzelReal[intIatMaxPeak - 121], dblGoertzelImag[intIatMaxPeak - 121], &dblLeftMag, "Hamming");

			if (dblBinAdj1475 < dblInterpretThreshold && dblBinAdj1475 > -dblInterpretThreshold)
			{
				dblBinAdj = dblBinAdj1475;
				intInterpCnt += 1;
			} 

			dblBinAdj1525 = SpectralPeakLocator(
				dblGoertzelReal[intIatMaxPeak - 118], dblGoertzelImag[intIatMaxPeak - 118], 
				dblGoertzelReal[intIatMaxPeak - 117], dblGoertzelImag[intIatMaxPeak - 117], 
				dblGoertzelReal[intIatMaxPeak - 116], dblGoertzelImag[intIatMaxPeak - 116], &dblRightMag, "Hamming");

			if (dblBinAdj1525 < dblInterpretThreshold && dblBinAdj1525 > -dblInterpretThreshold)
			{
				dblBinAdj += dblBinAdj1525;
        		intInterpCnt += 1;
			}
			if (intInterpCnt == 0)					
			{
				dblPriorFineOffset = 1000.0f;
				return FALSE;
			}
			else
			{	
				dblBinAdj = dblBinAdj / intInterpCnt;	 // average the offsets that are within 1 bin
				dblCoarseOffset = 10.0f * (intIatMaxPeak + dblBinAdj - 147); // compute the Coarse tuning offset in Hz
			}
		}
		else
		{
			dblPriorFineOffset = 1000.0f;
			return FALSE;
		}
	}
	
	// Drop into Narrow Search
  
           
	if (dblCoarseOffset < 999)
		dblTrialOffset = dblCoarseOffset;  // use the CoarseOffset calculation from above
	else
		dblTrialOffset = *dblOffsetHz; // use the prior offset value
	
    if (fabsf(dblTrialOffset) > TuningRange && TuningRange > 0)
	{
		dblPriorFineOffset = 1000.0f;	
		return False;
	}

	dblLeftCar = 147.5f + dblTrialOffset / 10.0f;  // the nominal positions of the two tone carriers based on the last computerd dblOffsetHz
	dblRightCar = 152.5f + dblTrialOffset / 10.0f;

	// Calculate 4 bins total for Noise values in S/N computation (calculate average noise)  ' Simple average of noise bins      
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 142.5f + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI);  // nominal center -75 Hz
	dblAvgNoisePerBin = powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 145.0f + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI); // center - 50 Hz
	dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 155.0 + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI); // center + 50 Hz
	dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, 157.5 + dblTrialOffset / 10.0f, &dblCtrR, &dblCtrI);  // center + 75 Hz
	dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
	dblAvgNoisePerBin = dblAvgNoisePerBin * 0.25f; // simple average,  now units of power
  
	// Calculate one bin above and below the two nominal 2 tone positions for Quinn Spectral Peak locator
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblLeftCar - 1, &dblLeftR[0], &dblLeftI[0]);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblLeftCar, &dblLeftR[1], &dblLeftI[1]);
	dblLeftP = powf(dblLeftR[1], 2) + powf(dblLeftI[1],  2);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblLeftCar + 1, &dblLeftR[2], &dblLeftI[2]);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblRightCar - 1, &dblRightR[0], &dblRightI[0]);
	GoertzelRealImagHamming(intNewSamples, Ptr, 1200, dblRightCar, &dblRightR[1], &dblRightI[1]);
	dblRightP = powf(dblRightR[1], 2) + powf(dblRightI[1], 2);
	GoertzelRealImag(intNewSamples, Ptr, 1200, dblRightCar + 1, &dblRightR[2], &dblRightI[2]);

	// Calculate the total power in the two tones 
	// This mechanism designed to reject single carrier but average both carriers if ratios is less than 4:1

	if (dblLeftP > 4 * dblRightP)
		dblPower = dblRightP;
	else if (dblRightP > 4 * dblLeftP)
		dblPower = dblLeftP;
	else
		dblPower = sqrtf(dblLeftP * dblRightP);
 
	dblSNdBPwr = 10 * log10f(dblPower / dblAvgNoisePerBin);

	// Early leader detect code to calculate S:N on the first 2 symbols)
	//  concept is to allow more accurate framing and sync detection and reduce false leader detects

	GoertzelRealImag(intNewSamples, Ptr, 480, 57.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); //  nominal center -75 Hz
	dblAvgNoisePerBin = powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImag(intNewSamples, Ptr, 480, 58.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); //  nominal center -75 Hz
	dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImag(intNewSamples, Ptr, 480, 62.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); //  nominal center -75 Hz
	dblAvgNoisePerBin += powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImag(intNewSamples, Ptr, 480, 63.0f + dblTrialOffset / 25.0f, &dblCtrR, &dblCtrI); //  nominal center -75 Hz 
	dblAvgNoisePerBin = max(1000.0f, 0.25 * (dblAvgNoisePerBin + powf(dblCtrR, 2) + powf(dblCtrI, 2))); // average of 4 noise bins
	dblLeftCar = 59 + dblTrialOffset / 25;  // the nominal positions of the two tone carriers based on the last computerd dblOffsetHz
	dblRightCar = 61 + dblTrialOffset / 25;

	GoertzelRealImag(intNewSamples, Ptr, 480, dblLeftCar, &dblCtrR, &dblCtrI); // LEFT carrier
	dblLeftP = powf(dblCtrR, 2) + powf(dblCtrI, 2);
	GoertzelRealImag(intNewSamples, Ptr, 480, dblRightCar, &dblCtrR, &dblCtrI); // Right carrier
	dblRightP = powf(dblCtrR, 2) + powf(dblCtrI, 2);

	// the following rejects a single tone carrier but averages the two tones if ratio is < 4:1

	if (dblLeftP > 4 * dblRightP)
		dblPowerEarly = dblRightP;
	else if (dblRightP > 4 * dblLeftP)
		dblPowerEarly = dblLeftP;
	else
		dblPowerEarly = sqrtf(dblLeftP * dblRightP);

	dblSNdBPwrEarly = 10 * log10f(dblPowerEarly / dblAvgNoisePerBin);

	// End of Early leader detect test code 
  
	if (dblSNdBPwr > (4 + Squelch) && dblSNdBPwrEarly > Squelch && (dblAvgNoisePerBin > 100.0f || dblPriorFineOffset != 1000.0f)) // making early threshold = lower (after 3 dB compensation for bandwidth)
	{
//		Debugprintf("Fine Search S:N= %f dB, Early S:N= %f dblAvgNoisePerBin %f ", dblSNdBPwr, dblSNdBPwrEarly, dblAvgNoisePerBin);

		// Calculate the interpolation based on the left of the two tones

		dblBinInterpLeft = SpectralPeakLocator(dblLeftR[0], dblLeftI[0], dblLeftR[1], dblLeftI[1], dblLeftR[2], dblLeftI[2], &dblLeftMag, "Hamming");
		
		// And the right of the two tones

		dblBinInterpRight = SpectralPeakLocator(dblRightR[0], dblRightI[0], dblRightR[1], dblRightI[1], dblRightR[2], dblRightI[2], &dblRightMag, "Hamming");

		// Weight the interpolated values in proportion to their magnitudes
		
		dblBinInterpLeft = dblBinInterpLeft * dblLeftMag / (dblLeftMag + dblRightMag);
		dblBinInterpRight = dblBinInterpRight * dblRightMag / (dblLeftMag + dblRightMag);
	
#ifdef ARMLINUX
		{
			int x = round(dblBinInterpLeft);	// odd, but PI doesnt print floats properly 
			int y = round(dblBinInterpRight);
		
//			Debugprintf(" SPL Left= %d  SPL Right= %d Offset %f, LeftMag %f RightMag %f", x, y, *dblOffsetHz, dblLeftMag, dblRightMag);
		}
#else
//		Debugprintf(" SPL Left= %f  SPL Right= %f, Offset %f, LeftMag %f RightMag %f",
//			dblBinInterpLeft, dblBinInterpRight, *dblOffsetHz, dblLeftMag, dblRightMag);
#endif    
		if (fabsf(dblBinInterpLeft + dblBinInterpRight) < 1.0) // sanity check for the interpolators 
		{
			if (dblBinInterpLeft + dblBinInterpRight > 0)  // consider different bounding below
				*dblOffsetHz = dblTrialOffset + min((dblBinInterpLeft + dblBinInterpRight) * 10.0f, 3); // average left and right, adjustment bounded to +/- 3Hz max
			else
				*dblOffsetHz = dblTrialOffset + max((dblBinInterpLeft + dblBinInterpRight) * 10.0f, -3);

			// Note the addition of requiring a second detect with small offset dramatically reduces false triggering even at Squelch values of 3
			// The following demonstrated good detection down to -10 dB S:N with squelch = 3 and minimal false triggering. 
			// Added rev 0.8.2.2 11/6/2016 RM

			if (abs(dblPriorFineOffset - *dblOffsetHz) < 2.9f)
			{
				Debugprintf("Prior-Offset= %f", (dblPriorFineOffset - *dblOffsetHz));
                   		
				// Capture power for debugging ...note: convert to 3 KHz noise bandwidth from 25Hz or 12.Hz for reporting consistancy.
	
				Debugprintf("Ldr; S:N(3KHz) Early= %f dB, Full %f dB, Offset= %f Hz: ", dblSNdBPwrEarly - 20.8f, dblSNdBPwr  - 24.77f, *dblOffsetHz);
				dttStartRmtLeaderMeasure = Now;


				if (AccumulateStats)
				{              
					dblLeaderSNAvg = ((dblLeaderSNAvg * intLeaderDetects) + dblSNdBPwr) / (1 + intLeaderDetects); 
					intLeaderDetects++;
				}

				dblNCOFreq = 3000 + *dblOffsetHz; // Set the NCO frequency and phase inc for mixing         
				dblNCOPhaseInc = dbl2Pi * dblNCOFreq / 12000;
				dttLastLeaderDetect = dttStartRmtLeaderMeasure = Now;
    
				State = AcquireSymbolSync;
				*intSN = dblSNdBPwr - 24.77; // 23.8dB accomodates ratio of 3Kz BW:10 Hz BW (10Log 3000/10 = 24.77)

				// don't advance the pointer here
              
				dblPriorFineOffset = 1000.0f;
				return TRUE;
			}
			else
				dblPriorFineOffset = *dblOffsetHz;

			// always use 1 symbol inc when looking for next minimal offset
		}
	}
	return FALSE;
}	




BOOL SearchFor2ToneLeader4(short * intNewSamples, int Length, float * dblOffsetHz, int * intSN)
{
    // This version uses 12.5 Hz bin spacing. Blackman window on Goertzel, and simple spectral peak interpolator optimized for Blackman
    // Blackman selected for maximum rejection (about 60 dB) of the other two-tone bin 50 Hz (4 x 12.5 Hz bins) away. 
	// search through the samples looking for the telltail 50 baud 2 tone pattern (nominal tones 1475, 1525 Hz)
	// Find the offset in Hz (due to missmatch in transmitter - receiver tuning
	// Finds the S:N (power ratio of the tones 1475 and 1525 ratioed to "noise" averaged from bins at 1425, 1450, 1550, and 1575Hz)
 
	float dblGoertzelReal[45];
	float dblGoertzelImag[45];
	float dblMag[45];
	float dblPower, dblPwrSNdB, dblLeftMag, dblRightMag, dblAvgNoisePerBinAtPeak;
	float dblRealL, dblRealR, dblImagL, dblImagR;
	float dblMaxPeak = 0.0, dblMaxPeakSN = 0.0, dblMagWindow;
	int intInterpCnt = 0;  // the count 0 to 3 of the interpolations that were < +/- .5 bin
	int  intIatMaxPeak = 0;
	float dblAlpha = 0.3f;  // Works well possibly some room for optimization Changed from .5 to .3 on Rev 0.1.5.3
	float dblInterpretThreshold= 1.0f; // Good results June 6, 2014 (was .4)  ' Works well possibly some room for optimization
	float dblFilteredMaxPeak = 0;
	int intStartBin, intStopBin;
	int i;
	int Ptr = 0;
	float dblAvgNoisePerBin, dblBinAdj1475, dblBinAdj1525, dblCoarseOffset = 1000;
	float dblOffset = 1000; //  initialize to impossible value

	// This should allow tunning from nominal bins at 1425Hz to 1575Hz +/- 200 Hz tuning range

	if ((Length) < 1200)
		return FALSE;		// ensure there are at least 1200 samples (5 symbols of 240 samples)

//	if ((Now - dttLastGoodFrameTypeDecode > 20000) && TuningRange > 0)
//	{
//		// this is the full search over the full tuning range selected.  Uses more CPU time and with possibly larger deviation once connected. 
		
	intStartBin = ((200 - TuningRange) / 12.5);
	intStopBin = 44 - intStartBin;

	dblMaxPeak = 0;
	dblMagWindow = 0;
	dblMaxPeakSN = -100;
    
	// Generate the Power magnitudes for up to 56 10 Hz bins (a function of MCB.TuningRange) 
  
	for (i = intStartBin; i <= intStopBin; i++)
	{
		// note Blackman window reduced end effect but looses sensitivity so sticking with Hann window
		// Test of 4/22/2018 indicated accurate Hann window (960) gives about 1-2 dB more sensitivity than Blackman window
		
		GoertzelRealImagHann960(intNewSamples, Ptr, 960, i + 98, &dblGoertzelReal[i], &dblGoertzelImag[i]);
		dblMag[i] = powf(dblGoertzelReal[i], 2) + powf(dblGoertzelImag[i], 2); // dblMag(i) in units of power (V^2)
		dblMagWindow += dblMag[i];
	}

	// Search the bins to locate the max S:N in the two tone signal/avg noise.  

 	for (i = intStartBin + 4; i <= intStopBin - 8; i++)	// ' +/- MCB.TuningRange from nominal 
	{
		dblPower = sqrtf(dblMag[i] * dblMag[i + 4]); // using the product to minimize sensitivity to one strong carrier vs the two tone
		// sqrt converts back to units of power from Power ^2
		// don't use center 7 noise bins as too easily corrupted by adjacent two-tone carriers
       
		dblAvgNoisePerBin = (dblMagWindow - (dblMag[i - 1] + dblMag[i] + dblMag[i + 1] + dblMag[i + 2] + dblMag[i + 3] + dblMag[i + 4] + dblMag[i + 5])) / (intStopBin - (intStartBin + 7));
		dblMaxPeak = dblPower / dblAvgNoisePerBin;

		if (dblMaxPeak > dblMaxPeakSN)
		{
			dblMaxPeakSN = dblMaxPeak;
			dblAvgNoisePerBinAtPeak = max(dblAvgNoisePerBin, 1000.0f);
			intIatMaxPeak = i + 98;
		}
	}
		
	dblMaxPeakSN = (dblMag[intIatMaxPeak - 98] + dblMag[intIatMaxPeak - 94]) / dblAvgNoisePerBinAtPeak;
	dblPwrSNdB = 10.0f * log10f(dblMaxPeakSN);
 
	// Check aquelch

	if ((dblPwrSNdB > (3 * Squelch)) && dblPwrSNPower_dBPrior > (3 * Squelch))
	{

		// Do the interpolation based on the two carriers at nominal 1475 and 1525Hz

		if (((intIatMaxPeak - 99) >= intStartBin) && ((intIatMaxPeak - 103) <= intStopBin)) // check to ensure no index errors
		{
			// Interpolate the adjacent bins using QuinnSpectralPeakLocator

			dblBinAdj1475 = SpectralPeakLocator(
				dblGoertzelReal[intIatMaxPeak - 99], dblGoertzelImag[intIatMaxPeak - 99],
				dblGoertzelReal[intIatMaxPeak - 98], dblGoertzelImag[intIatMaxPeak - 98], 
				dblGoertzelReal[intIatMaxPeak - 97], dblGoertzelImag[intIatMaxPeak - 97], &dblLeftMag, "Hann");

			dblBinAdj1525 = SpectralPeakLocator(
				dblGoertzelReal[intIatMaxPeak - 95], dblGoertzelImag[intIatMaxPeak - 95], 
				dblGoertzelReal[intIatMaxPeak - 94], dblGoertzelImag[intIatMaxPeak - 94], 
				dblGoertzelReal[intIatMaxPeak - 93], dblGoertzelImag[intIatMaxPeak - 93], &dblRightMag, "Hann");

			// Weight the offset calculation by the magnitude of the dblLeftMag and dblRightMag carriers 
			
			dblOffset = 12.5 * (intIatMaxPeak + dblBinAdj1475 * dblLeftMag / (dblLeftMag + dblRightMag) + dblBinAdj1525 * dblRightMag / (dblLeftMag + dblRightMag) - 118);  // compute the Coarse tuning offset in Hz
				
			if (fabsf(dblOffset) > (TuningRange + 7))		// Was 7 caused tuning problems
			{
				dblPwrSNPower_dBPrior = dblPwrSNdB;
				return False;
			}
			
			// recompute the S:N based on the interpolated bins and average with computation 1 and 2 symbols in the future 
			//  Use of Hann window increases sensitivity slightly (1-2 dB)

			GoertzelRealImagHann120(intNewSamples, 0, 960, intIatMaxPeak + dblOffset / 12.5, &dblRealL, &dblImagL);
			GoertzelRealImagHann120(intNewSamples, 0, 960, intIatMaxPeak + 4 + dblOffset / 12.5, &dblRealR, &dblImagR);
			dblMaxPeakSN = (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
			// now compute for 120 samples later
			GoertzelRealImagHann120(intNewSamples, 120, 960, intIatMaxPeak + dblOffset / 12.5, &dblRealL, &dblImagL);
			GoertzelRealImagHann120(intNewSamples, 120, 960, intIatMaxPeak + 4 + dblOffset / 12.5, &dblRealR, &dblImagR);
			dblMaxPeakSN += (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
			//  and a third 240 samples later
			GoertzelRealImagHann120(intNewSamples, 240, 960, intIatMaxPeak + dblOffset / 12.5, &dblRealL, &dblImagL);
			GoertzelRealImagHann120(intNewSamples, 240, 960, intIatMaxPeak + 4 + dblOffset / 12.5, &dblRealR, &dblImagR);
			dblMaxPeakSN += (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
 
			dblMaxPeakSN = dblMaxPeakSN / 3;  // average the dblMaxPeakSN over the three calculations
	// ???? Calc Twice ????
 			dblMaxPeakSN = (powf(dblRealL, 2) + powf(dblImagL, 2) + powf(dblRealR, 2) + powf(dblImagR, 2)) / dblAvgNoisePerBinAtPeak;
      
			
			dblPwrSNdB = 10 * log10f(dblMaxPeakSN);
			
			if (dblPwrSNdB > 3 * Squelch)	// This average power now includes two samples from symbols +120 and + 240 samples 
			{
				//strDecodeCapture = "Ldr; S:N(3KHz) Prior=" & Format(dblPwrSNPower_dBPrior, "#.0") & "dB, Current=" & Format(dblPwrSNdB, "#.0") & "dB, Offset=" & Format(dblOffset, "##0.00") & "Hz "
  				Debugprintf("Ldr; S:N(3KHz) Avg= %f dB, Offset== %f Hz", dblPwrSNdB, dblOffset);
				dttStartRmtLeaderMeasure = Now;
				if (AccumulateStats)
				{
					dblLeaderSNAvg = ((dblLeaderSNAvg * intLeaderDetects) + dblPwrSNdB) / (1 + intLeaderDetects);
					intLeaderDetects += 1;
				}
				*dblOffsetHz = dblOffset;
				dblNCOFreq = 3000 + *dblOffsetHz;	// Set the NCO frequency and phase inc for mixing 
				dblNCOPhaseInc = dbl2Pi * dblNCOFreq / 12000;
				// don't advance the pointer here
				State = AcquireSymbolSync;
				dttLastLeaderDetect = Now;
				dblPhaseDiff1_2Avg = 10; //  initialize to 10 to cause initialization of exponential averager in AcquireFrameSyncRSBAvg 
				*intSN = round(dblPwrSNdB - 20.8); // 20.8dB accomodates ratio of 3Kz BW: (effective Blackman Window bandwidth  of ~25 Hz) 
				return True;
			}
			else
			{
				return False;
			}
		}
	}
	
	dblPwrSNPower_dBPrior = dblPwrSNdB;

	return FALSE;
}	




//	Function to look at the 2 tone leader and establishes the Symbol framing using envelope search and minimal phase error. 

BOOL Acquire2ToneLeaderSymbolFraming()
{
	float dblCarPh;
	float dblReal, dblImag;
	int intLocalPtr = intMFSReadPtr;  // try advancing one symbol to minimize initial startup errors 
	float dblAbsPhErr;
	float dblMinAbsPhErr = 5000;	 // initialize to an excessive value
	int intIatMinErr;
	float dblPhaseAtMinErr;
	int intAbsPeak = 0;
	int intJatPeak = 0;
	int i;

	// Use Phase of 1500 Hz leader  to establish symbol framing. Nominal phase is 0 or 180 degrees

	if ((intFilteredMixedSamplesLength - intLocalPtr) < 960)
		return FALSE;			// not enough
	
	intLocalPtr = intMFSReadPtr + EnvelopeCorrelatorNew(); // should position the pointer at the symbol boundary

	if (intLocalPtr < intMFSReadPtr)
		return False; // use negative value of EnvelopeCorrelator to indicate insufficient correlation. 


	// Check 2 samples either side of the intLocalPtr for minimum phase error.(closest to Pi or -Pi) 
	// Could be as much as .4 Radians (~70 degrees) depending on sampling positions.
   
	for (i = -2; i <= 2; i++)	 // 0 To 0 '  -2 To 2 ' for just 5 samples
	{
		// using the full symbol seemed to work best on weak Signals (0 to -5 dB S/N) June 15, 2015
	
		GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr + i, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning 
		dblCarPh = atan2f(dblImag, dblReal);
		dblAbsPhErr = fabsf(dblCarPh - (ceil(dblCarPh / M_PI) * M_PI));
		if (dblAbsPhErr < dblMinAbsPhErr)
		{
			dblMinAbsPhErr = dblAbsPhErr;
			intIatMinErr = i;
			dblPhaseAtMinErr = dblCarPh;
		}     
	}

	intMFSReadPtr = intLocalPtr + intIatMinErr;
	Debugprintf("[Acquire2ToneLeaderSymbolFraming] intIatMinError= %d, Leader Length %d mS", intIatMinErr, Now - dttLastLeaderDetect);
	State = AcquireFrameSync;

	if (AccumulateStats)
		intLeaderSyncs++;

	//Debug.WriteLine("   [Acquire2ToneLeaderSymbolSync] iAtMinError = " & intIatMinErr.ToString & "   Ptr = " & intMFSReadPtr.ToString & "  MinAbsPhErr = " & Format(dblMinAbsPhErr, "#.00"))
	//Debug.WriteLine("   [Acquire2ToneLeaderSymbolSync]      Ph1500 @ MinErr = " & Format(dblPhaseAtMinErr, "#.000"))
        
	//strDecodeCapture &= "Framing; iAtMinErr=" & intIatMinErr.ToString & ", Ptr=" & intMFSReadPtr.ToString & ", MinAbsPhErr=" & Format(dblMinAbsPhErr, "#.00") & ": "
     intPhaseError = 0;
	return TRUE;
}

// Function to establish symbol sync 
int EnvelopeCorrelator()
{
	// Compute the two symbol correlation with the Two tone leader template.
	// slide the correlation one sample and repeat up to 240 steps 
	// keep the point of maximum or minimum correlation...and use this to identify the the symbol start. 

	float dblCorMax  = -1000000.0f;		//  Preset to excessive values
	float dblCorMin  = 1000000.0f;
	int intJatMax = 0, intJatMin = 0;
	float dblCorSum, dblCorProduct, dblCorMaxProduct = 0.0;
	int i,j;
	short int75HzFiltered[720];

	if (intFilteredMixedSamplesLength < intMFSReadPtr + 720)
		return -1;
	
	Filter75Hz(int75HzFiltered, TRUE, 720); // This filter appears to help reduce avg decode distance (10 frames) by about 14%-19% at WGN-5 May 3, 2015
	
	for (j = 0; j < 360; j++)		// Over 1.5 symbols
	{
		dblCorSum = 0;
		for (i = 0; i < 240; i++)	 // over 1 50 baud symbol (may be able to reduce to 1 symbol)
		{
			dblCorProduct = int50BaudTwoToneLeaderTemplate[i] * int75HzFiltered[120 + i + j]; // note 120 accomdates filter delay of 120 samples
			dblCorSum += dblCorProduct;
            if (fabsf(dblCorProduct) > dblCorMaxProduct)
				dblCorMaxProduct = fabsf(dblCorProduct);
		}

		if (fabsf(dblCorSum) > dblCorMax)
		{
			dblCorMax = fabsf(dblCorSum);
			intJatMax = j;
		}		
	}
	
	if (AccumulateStats)
	{
		dblAvgCorMaxToMaxProduct = (dblAvgCorMaxToMaxProduct * intEnvelopeCors + (dblCorMax / dblCorMaxProduct)) / (intEnvelopeCors + 1);
		intEnvelopeCors++;
	}
 
//	if (dblCorMax > 40 * dblCorMaxProduct)
	{
		Debugprintf("EnvelopeCorrelator CorMax:MaxProd= %f  J= %d", dblCorMax / dblCorMaxProduct, intJatMax);
		return intJatMax;
	}
//	else
//		return -1;
}
 

int EnvelopeCorrelatorNew()
{
	// Compute the two symbol correlation with the Two tone leader template.
	// slide the correlation one sample and repeat up to 240 steps 
	// keep the point of maximum or minimum correlation...and use this to identify the the symbol start. 

	float dblCorMax  = -1000000.0f;		//  Preset to excessive values
	float dblCorMin  = 1000000.0f;
	int intJatMax = 0, intJatMin = 0;
	float dblCorSum, dblCorProduct, dblCorMaxProduct = 0.0;
	int i,j;
	short int75HzFiltered[960];

	if (intFilteredMixedSamplesLength < intMFSReadPtr + 960)
		return -1;
	
	Filter75Hz(int75HzFiltered, TRUE, 960); // This filter appears to help reduce avg decode distance (10 frames) by about 14%-19% at WGN-5 May 3, 2015
	
	for (j = 360; j < 600; j++)		// Over 2 symbols
	{
		dblCorSum = 0;
		for (i = 0; i < 240; i++)	 // over 1 50 baud symbol (may be able to reduce to 1 symbol)
		{
			dblCorProduct = int50BaudTwoToneLeaderTemplate[i] * int75HzFiltered[120 + i + j]; // note 120 accomdates filter delay of 120 samples
			dblCorSum += dblCorProduct;
            if (fabsf(dblCorProduct) > dblCorMaxProduct)
				dblCorMaxProduct = fabsf(dblCorProduct);
		}

		if (fabsf(dblCorSum) > dblCorMax)
		{
			dblCorMax = fabsf(dblCorSum);
			intJatMax = j;
		}		
	}
	
	if (AccumulateStats)
	{
		dblAvgCorMaxToMaxProduct = (dblAvgCorMaxToMaxProduct * intEnvelopeCors + (dblCorMax / dblCorMaxProduct)) / (intEnvelopeCors + 1);
		intEnvelopeCors++;
	}
 
	if (dblCorMax > 40 * dblCorMaxProduct)
	{
		Debugprintf("EnvelopeCorrelator CorMax:MaxProd= %f  J= %d", dblCorMax / dblCorMaxProduct, intJatMax);
		return intJatMax;
	}
		
	Debugprintf("EnvelopeCorrelator failed %d",  dblCorMax / dblCorMaxProduct);
	
	return -1;
}
 

//	Function to acquire the Frame Sync for all Frames 

BOOL AcquireFrameSyncRSB()
{
	// Two improvements could be incorporated into this function:
	//    1) Provide symbol tracking until the frame sync is found (small corrections should be less than 1 sample per 4 symbols ~2000 ppm)
	//    2) Ability to more accurately locate the symbol center (could be handled by symbol tracking 1) above. 

	//  This is for acquiring FSKFrameSync After Mixing Tones Mirrored around 1500 Hz. e.g. Reversed Sideband
	//  Frequency offset should be near 0 (normally within +/- 1 Hz)  
	//  Locate the sync Symbol which has no phase change from the prior symbol (BPSK leader @ 1500 Hz)   

	int intLocalPtr = intMFSReadPtr;
	int intAvailableSymbols = (intFilteredMixedSamplesLength - intMFSReadPtr) / 240;
	float dblPhaseSym1;	//' phase of the first symbol 
	float dblPhaseSym2;	//' phase of the second symbol 
	float dblPhaseSym3;	//' phase of the third symbol

	float dblReal, dblImag;
	float dblPhaseDiff12, dblPhaseDiff23;

	int i;

	if (intAvailableSymbols < 3)
		return FALSE;				// must have at least 360 samples to search
 
	// Calculate the Phase for the First symbol 
	
	GoertzelRealImag(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
	dblPhaseSym1 = atan2f(dblImag, dblReal);
	intLocalPtr += 240;	// advance one symbol
	GoertzelRealImag(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
	dblPhaseSym2 = atan2f(dblImag, dblReal);
	intLocalPtr += 240;		// advance one symbol

	for (i = 0; i <=  intAvailableSymbols - 3; i++)
	{
		// Compute the phase of the next symbol  
	
		GoertzelRealImag(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
		dblPhaseSym3 = atan2f(dblImag, dblReal);
		// Compute the phase differences between sym1-sym2, sym2-sym3
		dblPhaseDiff12 = dblPhaseSym1 - dblPhaseSym2;
		if (dblPhaseDiff12 > M_PI)		// bound phase diff to +/- Pi
			dblPhaseDiff12 -= dbl2Pi;
		else if (dblPhaseDiff12 < -M_PI)
			dblPhaseDiff12 += dbl2Pi;

		dblPhaseDiff23 = dblPhaseSym2 - dblPhaseSym3;
		if (dblPhaseDiff23 > M_PI)		//  bound phase diff to +/- Pi
			dblPhaseDiff23 -= dbl2Pi;
		else if (dblPhaseDiff23 < -M_PI)
			dblPhaseDiff23 += dbl2Pi;

		if (fabsf(dblPhaseDiff12) > 0.6667f * M_PI && fabsf(dblPhaseDiff23) < 0.3333f * M_PI)  // Tighten the margin to 60 degrees
		{
//			intPSKRefPhase = (short)dblPhaseSym3 * 1000;

			intLeaderRcvdMs = (int)ceil((intLocalPtr - 30) / 12);	 // 30 is to accomodate offset of inital pointer for filter length. 
			intMFSReadPtr = intLocalPtr + 240;		 // Position read pointer to start of the symbol following reference symbol 
		
			if (AccumulateStats)
				intFrameSyncs += 1;		 // accumulate tuning stats
	
			//strDecodeCapture &= "Sync; Phase1>2=" & Format(dblPhaseDiff12, "0.00") & " Phase2>3=" & Format(dblPhaseDiff23, "0.00") & ": "
	
			return TRUE;	 // pointer is pointing to first 4FSK data symbol. (first symbol of frame type)
		}
		else
		{
			dblPhaseSym1 = dblPhaseSym2;           
			dblPhaseSym2 = dblPhaseSym3;
			intLocalPtr += 240;			// advance one symbol 
		}
	}

	intMFSReadPtr = intLocalPtr - 480;		 // back up 2 symbols for next attempt (Current Sym2 will become new Sym1)
	return FALSE;	
}



// Function to acquire the Frame Sync for all Frames using exponential averaging 
int AcquireFrameSyncRSBAvg()
{
	//	This new routine uses exponential averaging on the ptr reference leader phases to minimize noise contribution
	//	Needs optimization of filter values and decision thresholds with actual simulator at low S:N and multipath. 

	//	This is for acquiring FSKFrameSync After Mixing Tones Mirrored around 1500 Hz. e.g. Reversed Sideband
	//	Frequency offset should be near 0 (normally within +/- 1 Hz)  
	//	Locate the sync Symbol which has no phase change from the prior symbol (50 baud BPSK leader @ 1500 Hz)   

	int intLocalPtr = intMFSReadPtr;
	int intAvailableSymbols = (intFilteredMixedSamplesLength - intMFSReadPtr) / 240;
	float dblPhaseSym1;	//' phase of the first symbol 
	float dblPhaseSym2;	//' phase of the second symbol 
	float dblPhaseSym3;	//' phase of the third symbol

	float dblReal, dblImag;
	float dblPhaseDiff12, dblPhaseDiff23;

	int i;

	if (intAvailableSymbols < 3)
		return FALSE;				// must have at least 360 samples to search

	// Calculate the Phase for the First symbol 
	
	GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
	dblPhaseSym1 = atan2f(dblImag, dblReal);
	intLocalPtr += 240;	// advance one symbol
	GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
	dblPhaseSym2 = atan2f(dblImag, dblReal);
	intLocalPtr += 240;		// advance one symbol

 	for (i = 0; i <=  intAvailableSymbols - 3; i++)
	{
 		// Compute the phase of the next symbol  
	
		GoertzelRealImagHann120(intFilteredMixedSamples, intLocalPtr, 240, 30, &dblReal, &dblImag); // Carrier at 1500 Hz nominal Positioning with no cyclic prefix
		dblPhaseSym3 = atan2f(dblImag, dblReal);
		// Compute the phase differences between sym1-sym2, sym2-sym3
		dblPhaseDiff12 = dblPhaseSym1 - dblPhaseSym2;
		if (dblPhaseDiff12 > M_PI)		// bound phase diff to +/- Pi
			dblPhaseDiff12 -= dbl2Pi;
		else if (dblPhaseDiff12 < -M_PI)
			dblPhaseDiff12 += dbl2Pi;

		if (dblPhaseDiff1_2Avg > 9)
			dblPhaseDiff1_2Avg = fabsf(dblPhaseDiff12); // initialize the difference average after a prior detect
		else
			dblPhaseDiff1_2Avg = 0.75 * dblPhaseDiff1_2Avg + 0.25 * fabsf(dblPhaseDiff12);  // exponential average 
    
 		
		dblPhaseDiff23 = dblPhaseSym2 - dblPhaseSym3;
		if (dblPhaseDiff23 > M_PI)		//  bound phase diff to +/- Pi
			dblPhaseDiff23 -= dbl2Pi;
		else if (dblPhaseDiff23 < -M_PI)
			dblPhaseDiff23 += dbl2Pi;
			
             

		if (fabsf(dblPhaseDiff1_2Avg ) > (0.83333 * M_PI) && fabsf(dblPhaseDiff23) < (0.25f * M_PI))  // Margin ~30 deg and 45 degrees
		{
			intLeaderRcvdMs = (int)ceil((intLocalPtr - 30) / 12);	 // 30 is to accomodate offset of inital pointer for filter length. 
			intMFSReadPtr = intLocalPtr + 240;		 // Position read pointer to start of the symbol following reference symbol 
		
			if (AccumulateStats)
				intFrameSyncs += 1;		 // accumulate tuning stats
	
			//strDecodeCapture &= "Sync; Phase1>2=" & Format(dblPhaseDiff12, "0.00") & " Phase2>3=" & Format(dblPhaseDiff23, "0.00") & ": "
//			dttLastLeaderSync = Now;
			dblPwrSNPower_dBPrior = -1000;  // Reset the prior Leader power to small value to insure minimum of two symbol passes on next leader detect. 
			return TRUE;	 // pointer is pointing to first 4FSK data symbol. (first symbol of frame type)
		}
		//	The following looks for phase errors (which should nomimally be Pi or 180 deg) and counts errors 
		//	abandoning search on the second error, Then advancing the main intMFSReadPtr one symbol (240 samples) and returning to SearchingForLeader state.
		    
		if (fabsf(dblPhaseDiff1_2Avg) < (0.6667 * M_PI) || fabsf(dblPhaseDiff23) < (0.6667 * M_PI)) // Margin 60 deg 
		{
			intPhaseError += 1;
			dblPhaseSym1 = dblPhaseSym2;           
			dblPhaseSym2 = dblPhaseSym3;
			intLocalPtr += 240;			// advance one symbol 
	 
//			if (intPhaseError > 1) // This bailout mechanism for sync failure is superior and doesn't make any assumptions about leader length
//			{
//				intMFSReadPtr += 240; // advance the MFSReadPointer one symbol and try to search for leader again. 
//				State = SearchingForLeader;
//				return False;
//			}
		}
		else
		{
			//	keep searching available samples

			dblPhaseSym1 = dblPhaseSym2;           
			dblPhaseSym2 = dblPhaseSym3;
			intLocalPtr += 240;			// advance one symbol 
		}
	}
 
	intMFSReadPtr = intLocalPtr - 480;		 // back up 2 symbols for next attempt (Current Sym2 will become new Sym1)
	return FALSE;

}
//	 Function to Demod FrameType4FSK

BOOL DemodFrameType4FSK(int intPtr, short * intSamples, int * intToneMags)
{
	float dblReal, dblImag;
	int i;

	if ((intFilteredMixedSamplesLength - intPtr) < 1920) // 8 symbols
		return FALSE;

	intToneMagsLength = 8;

	for (i = 0; i < 8; i++)
	{
		GoertzelRealImagHann120(intSamples, intPtr, 240, 1350 / 50.0f, &dblReal, &dblImag);
		intToneMags[4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
		GoertzelRealImagHann120(intSamples, intPtr, 240, 1450 / 50.0f, &dblReal, &dblImag);
		intToneMags[1 + 4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
		GoertzelRealImagHann120(intSamples, intPtr, 240, 1550 / 50.0f, &dblReal, &dblImag);
		intToneMags[2 + 4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
		GoertzelRealImagHann120(intSamples, intPtr, 240, 1650 / 50.0f, &dblReal, &dblImag);
		intToneMags[3 + 4 * i] = (int)powf(dblReal, 2) + powf(dblImag, 2);
		intPtr += 240;
	}
	
	return TRUE;
}

// Function to compute the "distance" from a specific bytFrame Xored by bytID using 1 symbol parity 

float ComputeDecodeDistance(int intTonePtr, int * intToneMags, UCHAR bytFrameType, UCHAR bytID)
{
	// intTonePtr is the offset into the Frame type symbols. 0 for first Frame byte 16 = (4 x 4) for second frame byte 

	float dblDistance = 0;
	int int4ToneSum;
	int intToneIndex;
	UCHAR bytMask = 0x30;
	int j, k;

	for (j = 0; j <= 3; j++)		//  over 4 symbols
	{
		int4ToneSum = 0;
		for (k = 0; k <=3; k++)
		{
			int4ToneSum += intToneMags[intTonePtr + (4 * j) + k];
		}
		if (int4ToneSum == 0)
			int4ToneSum = 1;		//  protects against possible overflow
		if (j < 3)
		    intToneIndex = ((bytFrameType ^ bytID) & bytMask) >> (4 - 2 * j);
		else
			intToneIndex = ComputeTypeParity(bytFrameType ^ bytID);

		dblDistance += 1.0f - ((1.0f * intToneMags[intTonePtr + (4 * j) + (3 - intToneIndex)]) / (1.0f * int4ToneSum));
		bytMask = bytMask >> 2;
	}
	
	dblDistance = dblDistance / 4;		// normalize back to 0 to 1 range 
	return dblDistance;
}

//	A function to check the parity symbol used in the frame type decoding

BOOL CheckTypeParity(UCHAR bytFrameType)
{
	// Returns True if Parity OK

	UCHAR bytMask = 0x30;	 // Look at only 6 bits of data (values only 0 to 63)
	UCHAR bytParitySum = 3;
	UCHAR bytSym = 0;
	int k;

	for (k = 0; k < 3; k++)
	{
		bytSym = (bytMask & bytFrameType) >> (2 * (2 - k));
		bytParitySum = bytParitySum ^ bytSym;
		bytMask = bytMask >> 2;
	}

	return bytParitySum == ((bytFrameType & 0x0C0) >> 6);
 }


// Function to check Parity of frame type bytes

UCHAR GetFrameTypeByte(int intTonePtr, int * intToneMags)
{
	// Demodulate the byte pointed to postion of tone PTR and return it
	 
	UCHAR bytData = 0, bytParity, bytSym;
    int intIndex = intTonePtr;
	int j;

	for (j = 0; j < 4; j++)
	{
		// for each 4FSK symbol (2 bits) in a byte

		if (intToneMags[intIndex] > intToneMags[intIndex + 1] && intToneMags[intIndex] > intToneMags[intIndex + 2] && intToneMags[intIndex] > intToneMags[intIndex + 3])
			bytSym = 3;
		else if (intToneMags[intIndex + 1] > intToneMags[intIndex] && intToneMags[intIndex + 1] > intToneMags[intIndex + 2] && intToneMags[intIndex + 1] > intToneMags[intIndex + 3])
			bytSym = 2;
		else if (intToneMags[intIndex + 2] > intToneMags[intIndex] && intToneMags[intIndex + 2] > intToneMags[intIndex + 1] && intToneMags[intIndex + 2] > intToneMags[intIndex + 3])
			bytSym = 1;
		else
			bytSym = 0;

		if (j < 3)
			bytData = (bytData << 2) + bytSym;
		else
			bytParity = bytSym << 6;
		
		intIndex += 4;
	}
	return bytData | bytParity;
} 


BOOL CheckFrameTypeParity(int intTonePtr, int * intToneMags)
{
	// Demodulate the byte pointed to postion of tone PTR and check Parity Return True if OK

	UCHAR bytData = GetFrameTypeByte(intTonePtr, intToneMags);

	return CheckTypeParity(bytData);
} 
//	Function to compute the frame type by selecting the minimal distance from all valid frame types.

int MinimalDistanceFrameType(int * intToneMags, UCHAR bytSessionID)
{
	float dblMinDistance1 = 5; // minimal distance for the first byte initialize to large value
	float dblMinDistance2 = 5; // minimal distance for the second byte initialize to large value
	float dblMinDistance3 = 5; // minimal distance for the second byte under exceptional cases initialize to large value
	int intIatMinDistance1, intIatMinDistance2, intIatMinDistance3;
	float dblDistance1, dblDistance2, dblDistance3;
	int i;

	strDecodeCapture[0] = 0;

	if (ProtocolState == ISS)
	{
		bytValidFrameTypes = bytValidFrameTypesISS;
		bytValidFrameTypesLength = bytValidFrameTypesLengthISS;
	}
	else
	{
		bytValidFrameTypes = bytValidFrameTypesALL;
		bytValidFrameTypesLength = bytValidFrameTypesLengthALL;
	}

	// Search through all the valid frame types finding the minimal distance 
	// This looks like a lot of computation but measured < 1 ms for 135 iterations....RM 11/1/2016

	for (i = 0; i < bytValidFrameTypesLength; i++)
	{
		dblDistance1 = ComputeDecodeDistance(0, intToneMags, bytValidFrameTypes[i], 0);
		dblDistance2 = ComputeDecodeDistance(16, intToneMags, bytValidFrameTypes[i], bytSessionID);

		if (blnPending)
		    dblDistance3 = ComputeDecodeDistance(16, intToneMags, bytValidFrameTypes[i], 0x3F);
		else
			dblDistance3 = ComputeDecodeDistance(16, intToneMags, bytValidFrameTypes[i], bytLastARQSessionID);

		if (dblDistance1 < dblMinDistance1)
		{
			dblMinDistance1 = dblDistance1;
			intIatMinDistance1 = bytValidFrameTypes[i];
		}
		if (dblDistance2 < dblMinDistance2)
		{
			dblMinDistance2 = dblDistance2;
			intIatMinDistance2 = bytValidFrameTypes[i];
		}
		if (dblDistance3 < dblMinDistance3)
		{
			dblMinDistance3 = dblDistance3;
			intIatMinDistance3 = bytValidFrameTypes[i];
		}
	}

	Debugprintf("Frame Decode type %x %x %x Dist %.2f %.2f %.2f Sess %x pend %d conn %d lastsess %d",
		intIatMinDistance1, intIatMinDistance2, intIatMinDistance3, 
		dblMinDistance1, dblMinDistance2, dblMinDistance3, 
		bytSessionID, blnPending, blnARQConnected, bytLastARQSessionID); 
	
	if (bytSessionID == 0x3F)		// ' we are in a FEC QSO, monitoring an ARQ session or have not yet reached the ARQ Pending or Connected status 
	{
		if (intIatMinDistance1 == intIatMinDistance2 && ((dblMinDistance1 < 0.3) || (dblMinDistance2 < 0.3)))
		{
			sprintf(strDecodeCapture, "%s MD Decode;2 ID=H%X, Type=H%X:%s, D1= %.2f, D2= %.2f",
				 strDecodeCapture, bytSessionID, intIatMinDistance1, Name(intIatMinDistance1), dblMinDistance1, dblMinDistance2);
			Debugprintf("[Frame Type Decode OK  ] %s", strDecodeCapture);
			dblOffsetLastGoodDecode = dblOffsetHz;

			return intIatMinDistance1;
		}
  

		if ((dblMinDistance1 < 0.3) && CheckFrameTypeParity(0, intToneMags)  && IsDataFrame(intIatMinDistance1) )	//  this would handle the case of monitoring an ARQ connection where the SessionID is not 0x3F
		{
			sprintf(strDecodeCapture, "%s MD Decode;3 ID=H%X, Type=H%X:%s, D1= %.2f, D2= %.2f",
				 strDecodeCapture, bytSessionID, intIatMinDistance1, Name(intIatMinDistance1), dblMinDistance1, dblMinDistance2);
			Debugprintf("[Frame Type Decode OK  ] %s", strDecodeCapture);
			
			return intIatMinDistance1;
		}

		if ((dblMinDistance2 < 0.3) &&  CheckFrameTypeParity(16, intToneMags) && IsDataFrame(intIatMinDistance2))  // this would handle the case of monitoring an FEC transmission that failed above when the session ID is = 03F
 		{
			sprintf(strDecodeCapture, "%s MD Decode;4 ID=H%X, Type=H%X:%s, D1= %.2f, D2= %.2f",
				 strDecodeCapture, bytSessionID, intIatMinDistance1, Name(intIatMinDistance2), dblMinDistance1, dblMinDistance2);
			Debugprintf("[Frame Type Decode OK  ] %s", strDecodeCapture);

			return intIatMinDistance2;
		}
		return -1;		// indicates poor quality decode so  don't use

	}

	sprintf(strDecodeCapture, "%s MD Decode;12  Type1=H%X: Type2=H%X: , D1= %.2f, D2= %.2f",
		strDecodeCapture, intIatMinDistance1 , intIatMinDistance2, dblMinDistance1, dblMinDistance2);
	Debugprintf("[Frame Type Decode Fail] %s", strDecodeCapture);
	return -1; // indicates poor quality decode so  don't use
}


//	Function to acquire the 4FSK frame type

int Acquire4FSKFrameType()
{
	// intMFSReadPtr is pointing to start of first symbol of Frame Type (total of 8 4FSK symbols in frame type (2 bytes) + 1 parity symbol per byte 
	// returns -1 if minimal distance decoding is below threshold (low likelyhood of being correct)
	// returns -2 if insufficient samples 
	// Else returns frame type 0-255

	int NewType = 0;
	char Offset[32];

	if ((intFilteredMixedSamplesLength - intMFSReadPtr) < (240 * 8))
		return -2;		//  Check for 8 available 4FSK Symbols 
	

	if (!DemodFrameType4FSK(intMFSReadPtr, intFilteredMixedSamples, &intToneMags[0][0]))
	{
		Update4FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);
		intMFSReadPtr += (240 * 8);
		return -1;
	}
	
	intRmtLeaderMeasure = (Now - dttStartRmtLeaderMeasure);
	dttLastGoodFrameTypeDecode = Now;

	// Now do check received  Tone array for testing minimum distance decoder

	if (blnPending)			// If we have a pending connection (btween the IRS first decode of ConReq until it receives a ConAck from the iSS)  
		NewType = MinimalDistanceFrameType(&intToneMags[0][0], bytPendingSessionID);		 // The pending session ID will become the session ID once connected) 
	else if (blnARQConnected)		// If we are connected then just use the stcConnection.bytSessionID
		NewType = MinimalDistanceFrameType(&intToneMags[0][0], bytSessionID);
	else					// not connected and not pending so use &FF (FEC or ARQ unconnected session ID
		NewType = MinimalDistanceFrameType(&intToneMags[0][0], 0x3F);
  

	sprintf(Offset, "Offset %5.1f", dblOffsetHz);
	SendtoGUI('O', Offset, strlen(Offset));

	if (NewType >= 0 &&  IsShortControlFrame(NewType))		// update the constellation if a short frame (no data to follow)
		Update4FSKConstellation(&intToneMags[0][0], &intLastRcvdFrameQuality);

	if (AccumulateStats)
		if (NewType >= 0)
			intGoodFSKFrameTypes++;
		else
			intFailedFSKFrameTypes++;
	
	intMFSReadPtr += (240 * 8);			 // advance to read pointer to the next symbol (if there is one)
	
	return NewType;
}


//	Demodulate Functions. These are called repeatedly as samples arrive
//	and buld a frame in static array  bytFrameData

// Function to demodulate one carrier for all low baud rate 4FSK frame types
 
//	Is called repeatedly to decode multitone modes
int Corrections = 0;

BOOL Demod1Car4FSK()
{
	int Start = 0;
	
	// We can't wait for the full frame as we don't have enough ram, so
	// we do one character at a time, until we run out or end of frame

	// Only continue if we have more than intSampPerSym * 4 chars

	while (State == AcquireFrame)
	{
		if (intFilteredMixedSamplesLength < ((intSampPerSym * 4) + 20)) // allow for correcrions
		{
			// Move any unprocessessed data down buffer

			//	(while checking process - will use cyclic buffer eventually

//			Debugprintf("Corrections %d", Corrections);

			// If corrections is non-zero, we have to adjust
			//	number left

			intFilteredMixedSamplesLength -= Corrections;
	
			if (intFilteredMixedSamplesLength < 0)
				Debugprintf("Corrupt intFilteredMixedSamplesLength");

			Corrections = 0;

			if (intFilteredMixedSamplesLength > 0)
				memmove(intFilteredMixedSamples,
					&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2); 

			return FALSE;
		}

		// If this is a multicarrier mode, we must call the
		// decode char routing for each carrier
	
		switch (intNumCar)
		{
		case 1:

			intCenterFreq = 1500;
			if (CarrierOk[0] == FALSE)		// Don't redo if already decoded
				Demod1Car4FSKChar(Start, bytFrameData[0], 0);
			break;

		case 2:

			intCenterFreq = 1750;
			if (CarrierOk[0] == FALSE)
				Demod1Car4FSKChar(Start, bytFrameData[0], 0);
			intCenterFreq = 1250;
			if (CarrierOk[1] == FALSE)
				Demod1Car4FSKChar(Start, bytFrameData[1], 1);
			break;

/*		case 4:

			intCenterFreq = 2250;
			if (CarrierOk[0] == FALSE)
				Demod1Car4FSKChar(Start, bytFrameData1, 0);
			intCenterFreq = 1750;
			if (CarrierOk[1] == FALSE)
				Demod1Car4FSKChar(Start, bytFrameData2, 1);
			intCenterFreq = 1250;
			if (CarrierOk[2] == FALSE)
				Demod1Car4FSKChar(Start, bytFrameData3, 2);
			intCenterFreq = 750;
			if (CarrierOk[3] == FALSE)
				Demod1Car4FSKChar(Start, bytFrameData4, 3);
			break;
*/
		}

		charIndex++;			// Index into received chars
		SymbolsLeft--;			// number still to decode
		Start += intSampPerSym * 4;	// 4 FSK bit pairs per byte
		intFilteredMixedSamplesLength -= intSampPerSym * 4;
	
		if (intFilteredMixedSamplesLength < 0)
			Debugprintf("Corrupt intFilteredMixedSamplesLength");

		if (SymbolsLeft == 0)	
		{	
			//- prepare for next

			// If variable length packet frame header we only have header - leave rx running
		
			if (intFrameType == PktFrameHeader)
			{
				State = SearchingForLeader;
			
				// Save any unused samples
			
				if (intFilteredMixedSamplesLength > 0 && Start > 0)
					memmove(intFilteredMixedSamples,
						&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2); 

				return TRUE;
			}

			DecodeCompleteTime = Now;
			DiscardOldSamples();
			ClearAllMixedSamples();
			State = SearchingForLeader;
		}
	}
	return TRUE;
}

// Function to demodulate one carrier for all low baud rate 4FSK frame types
 
void Demod1Car4FSKChar(int Start, UCHAR * Decoded, int Carrier)
{
	// Converts intSamples to an array of bytes demodulating the 4FSK symbols with center freq intCenterFreq
	// intPtr should be pointing to the approximate start of the first data symbol  
	// Updates bytData() with demodulated bytes
	// Updates bytMinSymQuality with the minimum (range is 25 to 100) symbol making up each byte.

	float dblReal, dblImag;
	float dblSearchFreq;
	float dblMagSum = 0;
	float  dblMag[4];	// The magnitude for each of the 4FSK frequency bins
	UCHAR bytSym;
	static UCHAR bytSymHistory[3];
	int j;
	UCHAR bytData = 0;

	int * intToneMagsptr = &intToneMags[Carrier][intToneMagsIndex[Carrier]];
	   
	intToneMagsIndex[Carrier] += 16;

	//	ReDim intToneMags(4 * intNumOfSymbols - 1)
    //    ReDim bytData(intNumOfSymbols \ 4 - 1)

	if (intBaud == 100)
		dblSearchFreq = intCenterFreq + (1.5f * intBaud);	// the highest freq (equiv to lowest sent freq because of sideband reversal)
	else
		dblSearchFreq = intCenterFreq + (3.0f * intBaud);	// the highest freq (equiv to lowest sent freq because of sideband reversal)


	// Do one symbol

	for (j = 0; j < 4; j++)		// for each 4FSK symbol (2 bits) in a byte
	{
		dblMagSum = 0;
		if (intBaud == 100)
		{
		GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, dblSearchFreq / intBaud, &dblReal, &dblImag);
		dblMag[0] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[0];

        GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - intBaud) / intBaud, &dblReal, &dblImag);
		dblMag[1] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[1];

		GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 2 * intBaud) / intBaud, &dblReal, &dblImag);
		dblMag[2] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[2];
			
		GoertzelRealImag(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 3 * intBaud) / intBaud, &dblReal,& dblImag);
		dblMag[3] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[3];
		}
		else
		{
		dblMagSum = 0;
		GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, dblSearchFreq / intBaud, &dblReal, &dblImag);
		dblMag[0] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[0];

		GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 2 * intBaud) / intBaud, &dblReal, &dblImag);
		dblMag[1] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[1];

		GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 4 * intBaud) / intBaud, &dblReal, &dblImag);
		dblMag[2] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[2];
			
		GoertzelRealImagHann120(intFilteredMixedSamples, Start, intSampPerSym, (dblSearchFreq - 6 * intBaud) / intBaud, &dblReal,& dblImag);
		dblMag[3] = powf(dblReal,2) + powf(dblImag, 2);
		dblMagSum += dblMag[3];
		
		}
		
		if (dblMag[0] > dblMag[1] && dblMag[0] > dblMag[2] && dblMag[0] > dblMag[3])
			bytSym = 0;
		else if (dblMag[1] > dblMag[0] && dblMag[1] > dblMag[2] && dblMag[1] > dblMag[3])
			bytSym = 1;
		else if (dblMag[2] > dblMag[0] && dblMag[2] > dblMag[1] && dblMag[2] > dblMag[3])
			bytSym = 2;
		else
			bytSym = 3;

		bytData = (bytData << 2) + bytSym;

		// !!!!!!! this needs attention !!!!!!!!

		*intToneMagsptr++ = dblMag[0];
		*intToneMagsptr++ = dblMag[1];
		*intToneMagsptr++ = dblMag[2];
		*intToneMagsptr++ = dblMag[3];
		bytSymHistory[0] = bytSymHistory[1];
		bytSymHistory[1] = bytSymHistory[2];
		bytSymHistory[2] = bytSym;

//		if ((bytSymHistory[0] != bytSymHistory[1]) && (bytSymHistory[1] != bytSymHistory[2]))
		{
			// only track when adjacent symbols are different (statistically about 56% of the time) 
			// this should allow tracking over 2000 ppm sampling rate error	
//			if (Start > intSampPerSym + 2)
//				Track1Car4FSK(intFilteredMixedSamples, &Start, intSampPerSym, dblSearchFreq, intBaud, bytSymHistory);
		}
		Start += intSampPerSym; // advance the pointer one symbol
	}

	if (AccumulateStats)
		intFSKSymbolCnt += 4;
 
	Decoded[charIndex] = bytData;	
	return;
}

extern int intBW;
 
//  Function to Demodulate Frame based on frame type
//	Will be called repeatedly as new samples arrive

void DemodulateFrame(int intFrameType)
{
 //       Dim stcStatus As Status = Nothing

	int intConstellationQuality = 0;

 //       ReDim bytData(-1)

	strRcvFrameTag[0] = 0;


	switch (intFrameType)
	{
	case ConReq200:
	case ConReq500:
	case ConReq2500:
	case OConReq500:
	case OConReq2500:
	case PING:
	case IDFRAME:
	case PINGACK:
	case CQ_de:
	case PktFrameHeader:	// Experimental Variable Length Frame 
	case OFDMACK:

		Demod1Car4FSK();
		return;

	case PktFrameData:	// Experimantal Variable Length Frame 
			
		if (strcmp(strMod, "4FSK") == 0)
			Demod1Car4FSK();
		else if (strcmp(strMod, "16QAM") == 0)
			DemodQAM();
		else
			DemodPSK();
		return;
	}

	switch (intFrameType & 0xFE)		// Others are even/odd data frames
	{
		case D4FSK_500_50_E:
		case D4FSK_1000_50_E:

			Demod1Car4FSK();
			break;

		case D4PSK_200_50_E:
		case D4PSK_200_100_E:
		case D4PSK_500_50_E:
		case D4PSK_500_100_E:
		case D4PSKR_2500_50_E:
		case D4PSK_2500_50_E:
		case D4PSK_2500_100_E:

			DemodPSK();
			break;

		case D16QAM_200_100_E:
		case D16QAMR_500_100_E:
		case D16QAM_500_100_E:
		case D16QAMR_2500_100_E:
		case D16QAM_2500_100_E:

			DemodQAM();
			break;

		case DOFDM_200_55_E:
		case DOFDM_500_55_E:
		case DOFDM_2500_55_E:

			DemodOFDM();
			break;

		default:

			Debugprintf("Unsupported frame type %x", intFrameType);
			DiscardOldSamples();
			ClearAllMixedSamples();
			State = SearchingForLeader;

			intFilteredMixedSamplesLength = 0;	// Testing
	}
}


int intSNdB = 0, intQuality = 0;


BOOL DecodeFrame(int chan, int xxx, UCHAR * bytData)
{
	BOOL blnDecodeOK = FALSE;
	char strCallerCallsign[10] = "";
	char strTargetCallsign[10] = "";
	char strIDCallSign[11] = "";
	char strGridSQ[20] = "";
	char Reply[80];
	
	strRcvFrameTag[0] = 0;

	//DataACK/NAK and short control frames 

	if (IsShortControlFrame(intFrameType)) // Short Control Frames
	{
		blnDecodeOK = TRUE;
		DrawRXFrame(1, Name(intFrameType));

		goto returnframe;
	}

	totalRSErrors = 0;
			
	if (IsDataFrame(intFrameType))
		PrintCarrierFlags();


	switch (intFrameType)
	{

	case PktFrameHeader:
	{
		// Variable Length Packet Frame Header
		// 6 bits Type 10 Bits Len

		int Len;
		int pktNumCar;
		int pktDataLen;
		int pktRSLen;

		frameLen = CorrectRawDataWithRS(&bytFrameData[0][0], bytData, intDataLen, intRSLen, intFrameType, 0);

		if (CarrierOk[0])
		{
			pktRXMode = bytFrameData[0][1] >> 2;
			pktNumCar = pktCarriers[pktRXMode];

			Len = ((bytFrameData[0][1] & 0x3) << 8) | bytFrameData[0][2];
		}
		//	Now only using one carrier

		//				else if (CarrierOk[1])
		//				{
		//					pktRXMode = bytFrameData2[1] >> 5;
		//					pktNumCar = ((bytFrameData2[1] & 0x1c) >> 2) + 1;
		//					Len =  ((bytFrameData2[1] & 0x3) << 8) | bytFrameData2[2];
		//				}
		else
		{
			// Cant decode

			DiscardOldSamples();
			ClearAllMixedSamples();
			break;
		}

		strcpy(strMod, &pktMod[pktRXMode][0]);

		// Reset to receive rest of frame

		pktDataLen = (Len + (pktNumCar - 1)) / pktNumCar; // Round up

		// This must match the encode settings

		pktRSLen = pktDataLen >> 2;			// Try 25% for now
		if (pktRSLen & 1)
			pktRSLen++;						// Odd RS bytes no use

		if (pktRSLen < 4)
			pktRSLen = 4;					// At least 4

		SymbolsLeft = pktDataLen + pktRSLen + 3; // Data has crc + length byte
		State = AcquireFrame;
		intFrameType = PktFrameData;
		CarrierOk[1] = CarrierOk[0] = 0;
		charIndex = 0;
		frameLen = 0;
		intPhasesLen = 0;
		memset(intToneMagsIndex, 0, sizeof(intToneMagsIndex));
		intDataLen = pktDataLen;
		intRSLen = pktRSLen;
		intNumCar = pktNumCar;
		PSKInitDone = 0;

		Debugprintf("Pkt Frame Header Type %s Len %d", strMod, Len);
		strlop(strMod, '/');
		blnDecodeOK = TRUE;

		return 0;
	}


	case PktFrameData:
	{
		if (pktFSK[pktRXMode])
		{
			// Need to Check RS

			frameLen = CorrectRawDataWithRS(&bytFrameData[0][0], bytData, intDataLen, intRSLen, intFrameType, 0);
			if (intNumCar > 1)
				frameLen += CorrectRawDataWithRS(&bytFrameData[1][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 1);

			if (intNumCar > 2)
			{
				frameLen += CorrectRawDataWithRS(&bytFrameData[2][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 2);
				frameLen += CorrectRawDataWithRS(&bytFrameData[3][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 3);
			}
		}

		if (memcmp(CarrierOk, Good, intNumCar) == 0)
		{
			blnDecodeOK = TRUE;

			// Packet Data  - if KISS interface ias active
			// Pass to Host as KISS frame, else pass to
			// Session code

			// Data in bytData  len in frameLen

			ProcessPktFrame(chan, bytData, frameLen);
//				else
//			L2Routine(bytData, frameLen, intLastRcvdFrameQuality, totalRSErrors, intNumCar, pktRXMode);

		}
		break;
	}


	default:

		Debugprintf("Unrecognised frame type");

	}

	if (blnDecodeOK)
	{
		Debugprintf("[DecodeFrame] Frame: %s Decode PASS,   Constellation Quality= %d", Name(intFrameType),  intLastRcvdFrameQuality);
#ifdef PLOTCONSTELLATION
		if (intFrameType >= 0x30 && intFrameType <= 0x38)
			DrawDecode(lastGoodID);		// ID or CONREQ
		else
			DrawDecode("PASS");
		updateDisplay();
#endif
	}

	else
	{
		Debugprintf("[DecodeFrame] Frame: %s Decode FAIL,   Constellation Quality= %d", Name(intFrameType),  intLastRcvdFrameQuality);
#ifdef PLOTCONSTELLATION
		DrawDecode("FAIL");
		updateDisplay();
#endif
	}


returnframe:

	if (blnDecodeOK && IsDataFrame(intFrameType))
		bytLastReceivedDataFrameType = intFrameType;
	
//	if (DebugLog)
//		if (blnDecodeOK)
//			Debugprintf("[DecodeFrame] Frame: %s Decode PASS, Constellation Quality= %d", Name(intFrameType), intLastRcvdFrameQuality);
//		else
//			Debugprintf("[DecodeFrame] Frame: %s Decode FAIL, Constellation Quality= %d", Name(intFrameType), intLastRcvdFrameQuality);

	return blnDecodeOK;
}

// Subroutine to update the 4FSK Constellation

void drawFastVLine(int x0, int y0, int length, int color);
void drawFastHLine(int x0, int y0, int length, int color);

void Update4FSKConstellation(int * intToneMags, int * intQuality)
{
	// Subroutine to update bmpConstellation plot for 4FSK modes...
        
	int intToneSum = 0;
	int intMagMax = 0;
	float dblPi4  = 0.25 * M_PI;
	float dblDistanceSum = 0;
	int intRad = 0;
	int i, x, y;
	int yCenter = 0;
	int xCenter = 0;

#ifdef PLOTCONSTELLATION

	int clrPixel;
	int yCenter = (ConstellationHeight)/ 2;
	int xCenter = (ConstellationWidth) / 2;

	clearDisplay();
#endif

	for (i = 0; i < intToneMagsLength; i += 4)  // for the number of symbols represented by intToneMags
	{
		intToneSum = intToneMags[i] + intToneMags[i + 1] + intToneMags[i + 2] + intToneMags[i + 3];
        
		if (intToneMags[i] > intToneMags[i + 1] && intToneMags[i] > intToneMags[i + 2] && intToneMags[i] > intToneMags[i + 3])
		{
			if (intToneSum > 0)
				intRad = max(5, 42 - 80 * (intToneMags[i + 1] + intToneMags[i + 2] + intToneMags[i + 3]) / intToneSum);

			dblDistanceSum += (42 - intRad);
			intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees 
			x = xCenter + intRad;
			y = yCenter + intRad;
		}
		else if (intToneMags[i + 1] > intToneMags[i] && intToneMags[i + 1] > intToneMags[i + 2] && intToneMags[i + 1] > intToneMags[i + 3])
		{
			if (intToneSum > 0)
				intRad = max(5, 42 - 80 * (intToneMags[i] + intToneMags[i + 2] + intToneMags[i + 3]) / intToneSum);

			dblDistanceSum += (42 - intRad);
			intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees 
			x = xCenter + intRad;
			y = yCenter - intRad;
		}
		else if (intToneMags[i + 2] > intToneMags[i] && intToneMags[i + 2] > intToneMags[i + 1] && intToneMags[i + 2] > intToneMags[i + 3]) 
		{
            if (intToneSum > 0)
				intRad = max(5, 42 - 80 * (intToneMags[i + 1] + intToneMags[i] + intToneMags[i + 3]) / intToneSum);

			dblDistanceSum += (42 - intRad);
			intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees 
			x = xCenter - intRad;
			y = yCenter - intRad;
		}
		else if (intToneSum > 0)
		{
			intRad = max(5, 42 - 80 * (intToneMags[i + 1] + intToneMags[i + 2] + intToneMags[i]) / intToneSum);	

			dblDistanceSum += (42 - intRad);
			intRad = (intRad * PLOTRADIUS) / 50; // rescale for OLED (50 instead of 42 as we rotate constellation 35 degrees 
			x = xCenter - intRad;
			y = yCenter + intRad;
		}

#ifdef PLOTCONSTELLATION
		if (intRad < 15)
			clrPixel = Tomato;
		else if (intRad < 30)
			clrPixel = Gold;
		else
			clrPixel = Lime;

		mySetPixel(x, y, clrPixel);
#endif

	}

	*intQuality = 100 - (2.7f * (dblDistanceSum / (intToneMagsLength / 4))); // ' factor 2.7 emperically chosen for calibration (Qual range 25 to 100)

	if (*intQuality < 0)
		*intQuality = 0;
	else if (*intQuality > 100)
		*intQuality = 100;

	if (AccumulateStats)
	{
		int4FSKQualityCnts += 1;
		int4FSKQuality += *intQuality;
	}

#ifdef PLOTCONSTELLATION
	DrawAxes(*intQuality, shortName(intFrameType), strMod);
#endif

	return;
}



// Subroutine to update the 16FSK constallation

void Update16FSKConstellation(int * intToneMags, int * intQuality)
{
	//	Subroutine to update bmpConstellation plot for 16FSK modes...


	int	intToneSum = 0;
	float intMagMax = 0;
	float dblDistanceSum = 0;
	float dblPlotRotation = 0;
//            Dim stcStatus As Status
	int	intRad;
//            Dim clrPixel As System.Drawing.Color
	int	intJatMaxMag;
	int i, j;

#ifdef PLOTCONSTELLATION

	float dblRad;
	float dblAng;
	int x, y,clrPixel;
	int yCenter = (ConstellationHeight - 2)/ 2;
	int xCenter = (ConstellationWidth - 2) / 2;

	clearDisplay();
#endif


	for (i = 0; i< intToneMagsLength; i += 16)  // for the number of symbols represented by intToneMags
	{
		intToneSum = 0;
		intMagMax = 0;

		for (j = 0; j < 16; j++)
		{
			if (intToneMags[i + j] > intMagMax)
			{
				intMagMax = intToneMags[i + j];
				intJatMaxMag = j;
			}
			intToneSum += intToneMags[i + j];
		}
		intRad = max(5, 42 - 40 * (intToneSum - intMagMax) / intToneSum);
		dblDistanceSum += (43 - intRad);

#ifdef PLOTCONSTELLATION		
		if (intRad < 15)
			clrPixel = Tomato;
		else if (intRad < 30)
			clrPixel = Gold;
		else
			clrPixel = Lime;

		// plot the symbols rotated to avoid the axis

		intRad = (intRad * PLOTRADIUS) /42; // rescale for OLED
		dblAng = M_PI / 16.0f + (intJatMaxMag * M_PI / 8);
  
		x = xCenter + intRad * cosf(dblAng);
		y = yCenter + intRad * sinf(dblAng);
		mySetPixel(x, y, clrPixel);    
#endif

	}
		
	*intQuality = max(0, (100 - 2.2 * (dblDistanceSum / (intToneMagsLength / 16))));	 // factor 2.2 emperically chosen for calibration (Qual range 25 to 100)
//	*intQuality = max(0, (100 - 1.0 * (dblDistanceSum / (intToneMagsLength / 16))));	 // factor 2.2 emperically chosen for calibration (Qual range 25 to 100)

	if(AccumulateStats)
	{
		int16FSKQualityCnts++;
		int16FSKQuality += *intQuality;
	}
#ifdef PLOTCONSTELLATION
	DrawAxes(*intQuality, shortName(intFrameType), strMod);
#endif
}

//	Subroutine to udpate the 8FSK Constellation

void Update8FSKConstellation(int * intToneMags, int * intQuality)
{
	//	Subroutine to update bmpConstellation plot for 8FSK modes...
         
	int intToneSum = 0;
	int intMagMax = 0;
	float dblPi4  = 0.25 * M_PI;
	float dblDistanceSum = 0;
	int intRad = 0;
	int i, j, intJatMaxMag;

#ifdef PLOTCONSTELLATION

	float dblAng;
	int yCenter = (ConstellationHeight - 2)/ 2;
	int xCenter = (ConstellationWidth - 2) / 2;
	unsigned short clrPixel = WHITE;
	unsigned short x, y;

	clearDisplay();
#endif

	for (i = 0; i < intToneMagsLength; i += 8)  // for the number of symbols represented by intToneMags
	{
		intToneSum = 0;
		intMagMax = 0;

 		for (j = 0; j < 8; j++)
		{
			if (intToneMags[i + j] > intMagMax)
			{
				intMagMax = intToneMags[i + j];
				intJatMaxMag = j;
			}
			intToneSum += intToneMags[i + j];
		}

		intRad = max(5, 42 - 40 * (intToneSum - intMagMax) / intToneSum);
		dblDistanceSum += (43 - intRad);
								
#ifdef PLOTCONSTELLATION		
		if (intRad < 15)
			clrPixel = Tomato;
		else if (intRad < 30)
			clrPixel = Gold;
		else
			clrPixel = Lime;

		// plot the symbols rotated to avoid the axis

		intRad = (intRad * PLOTRADIUS) /42; // rescale for OLED

		dblAng = M_PI / 9.0f + (intJatMaxMag * M_PI / 4);
  
		x = xCenter + intRad * cosf(dblAng);
		y = yCenter + intRad * sinf(dblAng);
		mySetPixel(x, y, clrPixel);    
#endif
	}
		
	*intQuality = max(0, (100 - 2.0 * (dblDistanceSum / (intToneMagsLength / 8))));	 // factor 2.0 emperically chosen for calibration (Qual range 25 to 100)

	if(AccumulateStats)
	{
		int8FSKQualityCnts++;
		int8FSKQuality += *intQuality;
	}
#ifdef PLOTCONSTELLATION
	DrawAxes(*intQuality, shortName(intFrameType), strMod);
#endif
	return;
}



//	Subroutine to Update the PhaseConstellation

int UpdatePhaseConstellation(short * intPhases, short * intMag, int intPSKPhase, BOOL blnQAM, BOOL OFDM)
{
	// Subroutine to update bmpConstellation plot for PSK modes...
	// Skip plotting and calculations of intPSKPhase(0) as this is a reference phase (9/30/2014)

	float dblPhaseError; 
	float dblPhaseErrorSum = 0;
	int intPSKIndex;
	float intP = 0;
	float dblRad = 0;
	float dblAvgRad = 0;
	float intMagMax = 0;
	float dblPi4 = 0.25 * M_PI;
	float dbPhaseStep;
	float dblRadError = 0;
	float dblPlotRotation = 0;
	int intRadInner = 0, intRadOuter = 0;
	float dblAvgRadOuter = 0, dblAvgRadInner = 0, dblRadErrorInner = 0, dblRadErrorOuter = 0;
 
	int i,j, k, intQuality;

#ifdef PLOTCONSTELLATION

	int intX, intY;
	int yCenter = (ConstellationHeight - 2)/ 2;
	int xCenter = (ConstellationWidth - 2) / 2;

	unsigned short clrPixel = WHITE;

	clearDisplay();
#endif

	if (intPSKPhase == 4)
		intPSKIndex = 0;
	else
		intPSKIndex = 1;

	if (blnQAM)
	{
		intPSKPhase = 8;
		intPSKIndex = 1;
		dbPhaseStep  = 2 * M_PI / intPSKPhase;
		for (j = 1; j < intPhasesLen; j++)   // skip the magnitude of the reference in calculation
		{
			intMagMax = max(intMagMax, intMag[j]); // find the max magnitude to auto scale
		}

		for (k = 1; k < intPhasesLen; k++)
		{
			if (intMag[k] < 0.75f * intMagMax)
			{
				dblAvgRadInner += intMag[k];
				intRadInner++;
			}
			else
			{
				dblAvgRadOuter += intMag[k];
				intRadOuter++;
			}
		}

		dblAvgRadInner = dblAvgRadInner / intRadInner;
		dblAvgRadOuter = dblAvgRadOuter / intRadOuter;
	}
	else
	{
		dbPhaseStep  = 2 * M_PI / intPSKPhase;
		for (j = 1; j < intPhasesLen; j++)   // skip the magnitude of the reference in calculation
		{
			intMagMax = max(intMagMax, intMag[j]); // find the max magnitude to auto scale
            dblAvgRad += intMag[j];	
		}
	}
           
	dblAvgRad = dblAvgRad / (intPhasesLen - 1); // the average radius

	for (i = 1; i <  intPhasesLen; i++)  // Don't plot the first phase (reference)
	{
		intP = round((0.001f * intPhases[i]) / dbPhaseStep);

		// compute the Phase and Radius errors
 
		if (intMag[i] > (dblAvgRadInner + dblAvgRadOuter) / 2) 
			dblRadErrorOuter += fabsf(dblAvgRadOuter - intMag[i]);
		else
			dblRadErrorInner += fabsf(dblAvgRadInner - intMag[i]);

		dblPhaseError = fabsf(((0.001 * intPhases[i]) - intP * dbPhaseStep)); // always positive and < .5 *  dblPhaseStep
		dblPhaseErrorSum += dblPhaseError;

#ifdef PLOTCONSTELLATION
		dblRad = PLOTRADIUS * intMag[i] / intMagMax; //  scale the radius dblRad based on intMagMax
		intX = xCenter + dblRad * cosf(dblPlotRotation + intPhases[i] / 1000.0f);
		intY = yCenter + dblRad * sinf(dblPlotRotation + intPhases[i] / 1000.0f);
    
		
		if (intX > 0 && intY > 0)
			if (intX != xCenter && intY != yCenter)
				mySetPixel(intX, intY, Yellow); // don't plot on top of axis
#endif
	}

	if (blnQAM) 
	{
//		intQuality = max(0, ((100 - 200 * (dblPhaseErrorSum / (intPhasesLen)) / dbPhaseStep))); // ignore radius error for (PSK) but include for QAM
		intQuality = max(0, (1 - (dblRadErrorInner / (intRadInner * dblAvgRadInner) + dblRadErrorOuter / (intRadOuter * dblAvgRadOuter))) * (100 - 200 * (dblPhaseErrorSum / intPhasesLen) / dbPhaseStep));

//		intQuality = max(0, ((100 - 200 * (dblPhaseErrorSum / (intPhasesLen)) / dbPhaseStep))); // ignore radius error for (PSK) but include for QAM
		
		if (AccumulateStats)
		{
			if (OFDM)
			{
				intOFDMQualityCnts[RXOFDMMode] ++;
				intOFDMQuality[RXOFDMMode] += intQuality;
				intOFDMSymbolsDecoded += intPhasesLen;
			}
			else
			{
				intQAMQualityCnts += 1;
				intQAMQuality += intQuality;
				intQAMSymbolsDecoded += intPhasesLen;
			}
		}
	}
	else
	{
		intQuality =  max(0, ((100 - 200 * (dblPhaseErrorSum / (intPhasesLen)) / dbPhaseStep))); // ignore radius error for (PSK) but include for QAM
	
		if (AccumulateStats)
		{
			if (OFDM)
			{
				intOFDMQualityCnts[RXOFDMMode] ++;
				intOFDMQuality[RXOFDMMode] += intQuality;
				intOFDMSymbolsDecoded += intPhasesLen;
			}
			else
			{
				intPSKQualityCnts[intPSKIndex]++;
				intPSKQuality[intPSKIndex] += intQuality;
				intPSKSymbolsDecoded += intPhasesLen;
			}

		}
	}	
#ifdef PLOTCONSTELLATION
	DrawAxes(intQuality, shortName(intFrameType), strMod);
#endif
	return intQuality;

}


// Subroutine to track 1 carrier 4FSK. Used for both single and multiple simultaneous carrier 4FSK modes.


VOID Track1Car4FSK(short * intSamples, int * intPtr, int intSampPerSymbol, float dblSearchFreq, int intBaud, UCHAR * bytSymHistory)
{
	// look at magnitude of the tone for bytHistory(1)  2 sample2 earlier and 2 samples later.  and pick the maximum adjusting intPtr + or - 1
	// this seems to work fine on test Mar 16, 2015. This should handle sample rate offsets (sender to receiver) up to about 2000 ppm

	float dblReal, dblImag, dblMagEarly, dblMag, dblMagLate;
	float dblBinToSearch = (dblSearchFreq - (intBaud * bytSymHistory[1])) / intBaud; //  select the 2nd last symbol for magnitude comparison


	GoertzelRealImag(intSamples, (*intPtr - intSampPerSymbol - 2), intSampPerSymbol, dblBinToSearch, &dblReal, &dblImag);
	dblMagEarly = powf(dblReal, 2) + powf(dblImag, 2);
	GoertzelRealImag(intSamples, (*intPtr - intSampPerSymbol), intSampPerSymbol, dblBinToSearch, &dblReal, &dblImag);
	dblMag = powf(dblReal, 2) + powf(dblImag, 2);
	GoertzelRealImag(intSamples, (*intPtr - intSampPerSymbol + 2), intSampPerSymbol, dblBinToSearch, &dblReal, &dblImag);
	dblMagLate = powf(dblReal, 2) + powf(dblImag, 2);

	if (dblMagEarly > dblMag && dblMagEarly > dblMagLate)
	{
		*intPtr --;
		Corrections--;
		if (AccumulateStats)
			intAccumFSKTracking--;
	}
	else if (dblMagLate > dblMag && dblMagLate > dblMagEarly)
	{
		*intPtr ++;
		Corrections++;
		if (AccumulateStats)
			intAccumFSKTracking++;
	}
}

//	Function to Decode one Carrier of PSK modulation 

//	Ideally want to be able to call on for each symbol, as I don't have the
//	RAM to build whole frame

//	Call for each set of 4 or 8 Phase Values

int pskStart = 0;


VOID Decode1CarPSK(int Carrier, BOOL OFDM)
{
	unsigned int int24Bits;
	UCHAR bytRawData;
	int k;
	int Len = intPhasesLen;
	UCHAR * Decoded;
	
	if (OFDM)
		Decoded = &bytFrameData[0][0];		// Always uses same buffer
	else
	{
		if (CarrierOk[Carrier])
			return;							// don't do it again
	
		Decoded = &bytFrameData[Carrier][0];
	}

	pskStart = 0;
	charIndex = 0;

    	
	while (Len >= 0)
	{

		// Phase Samples are in intPhases

		switch (intPSKMode)
		{
			case 2:		// process 8 sequential phases per byte (1 bits per phase)

			for (k = 0; k < 8; k++)
			{
				if (k == 0)
					bytRawData = 0;
				else
					bytRawData <<= 1;
				
				if (intPhases[Carrier][pskStart] >= 1572 || intPhases[Carrier][pskStart]<= -1572)
					bytRawData += 1;
			
				pskStart++;
			}

			Decoded[charIndex++] = bytRawData;
			Len -= 8;
			break;

		case 4:		// process 4 sequential phases per byte (2 bits per phase)

			for (k = 0; k < 4; k++)
			{
				if (k == 0)
					bytRawData = 0;
				else
					bytRawData <<= 2;
				
				if (intPhases[Carrier][pskStart] < 786 && intPhases[Carrier][pskStart] > -786)
				{
				}		// Zero so no need to do anything
				else if (intPhases[Carrier][pskStart] >= 786 && intPhases[Carrier][pskStart] < 2356)
					bytRawData += 1;
				else if (intPhases[Carrier][pskStart] >= 2356 || intPhases[Carrier][pskStart] <= -2356)
					bytRawData += 2;
				else
					bytRawData += 3;

				pskStart++;
			}

			Decoded[charIndex++] = bytRawData;
			Len -= 4;
			break;

		case 8: // Process 8 sequential phases (3 bits per phase)  for 24 bits or 3 bytes  

			//	Status verified on 1 Carrier 8PSK with no RS needed for High S/N

			//	Assume we check for 8 available phase samples before being called

			int24Bits = 0;

			for (k = 0; k < 8; k++)
			{
				int24Bits <<= 3;

				if (intPhases[Carrier][pskStart] < 393 && intPhases[Carrier][pskStart] > -393)
				{
				}		// Zero so no need to do anything
				else if (intPhases[Carrier][pskStart] >= 393 && intPhases[Carrier][pskStart] < 1179)
					int24Bits += 1;
				else if (intPhases[Carrier][pskStart] >= 1179 && intPhases[Carrier][pskStart] < 1965)
					int24Bits += 2;
				else if (intPhases[Carrier][pskStart] >= 1965 && intPhases[Carrier][pskStart] < 2751)
				int24Bits += 3;
				else if (intPhases[Carrier][pskStart] >= 2751 || intPhases[Carrier][pskStart] < -2751)
					int24Bits += 4;
				else if (intPhases[Carrier][pskStart] >= -2751 && intPhases[Carrier][pskStart] < -1965)
				int24Bits += 5;
				else if (intPhases[Carrier][pskStart] >= -1965 && intPhases[Carrier][pskStart] <= -1179)
					int24Bits += 6;
				else 
					int24Bits += 7;

				pskStart ++;
	
			}
			Decoded[charIndex++] = int24Bits >> 16;
			Decoded[charIndex++] = int24Bits >> 8;
			Decoded[charIndex++] = int24Bits;

			Len -= 8;
			break;
	
		case 16: // Process 2 sequential phases (4 bits per phase) for 1 bytes  


			for (k = 0; k < 2; k++)
			{
				if (k == 0)
					bytRawData = 0;
				else
					bytRawData <<= 4;
				
				if (intPhases[Carrier][pskStart] < 196 && intPhases[Carrier][pskStart] > -196)
				{
				}		// Zero so no need to do anything
				else if (intPhases[Carrier][pskStart] >= 196 && intPhases[Carrier][pskStart] < 589)
					bytRawData += 1;
				else if (intPhases[Carrier][pskStart] >= 589 && intPhases[Carrier][pskStart] < 981)
					bytRawData += 2;
				else if (intPhases[Carrier][pskStart] >= 981 && intPhases[Carrier][pskStart] < 1374)
					bytRawData += 3;
				else if (intPhases[Carrier][pskStart] >= 1374 && intPhases[Carrier][pskStart] < 1766)
					bytRawData += 4;
				else if (intPhases[Carrier][pskStart] >= 1766 && intPhases[Carrier][pskStart] < 2159)
					bytRawData += 5;
				else if (intPhases[Carrier][pskStart] >= 2159 && intPhases[Carrier][pskStart] < 2551)
					bytRawData += 6;
				else if (intPhases[Carrier][pskStart] >= 2551 && intPhases[Carrier][pskStart] < 2944)
					bytRawData += 7;

				else if (intPhases[Carrier][pskStart] >= 2944 || intPhases[Carrier][pskStart] < -2944)
					bytRawData += 8;
				else if (intPhases[Carrier][pskStart] >= -2944 && intPhases[Carrier][pskStart] < -2551)
					bytRawData += 9;
				else if (intPhases[Carrier][pskStart] >= -2551 && intPhases[Carrier][pskStart] < -2159)
					bytRawData += 10;
				else if (intPhases[Carrier][pskStart] >= -2159 && intPhases[Carrier][pskStart] < -1766)
					bytRawData += 11;
				else if (intPhases[Carrier][pskStart] >= -1766 && intPhases[Carrier][pskStart] < -1374)
					bytRawData += 12;
				else if (intPhases[Carrier][pskStart] >= -1374 && intPhases[Carrier][pskStart] < -981)
					bytRawData += 13;
				else if (intPhases[Carrier][pskStart] >= -981 && intPhases[Carrier][pskStart] < -589)
					bytRawData += 14;
				else 
					bytRawData += 15;

				pskStart ++;
	
			}
			Decoded[charIndex++] = bytRawData;

			Len -= 2;
			break;

		default:
			return; //????
		}
	}
	return;
}

//	Function to compute PSK symbol tracking (all PSK modes, used for single or multiple carrier modes) 

int Track1CarPSK(int floatCarFreq, int PSKMode, BOOL QAM, BOOL OFDM, float dblUnfilteredPhase, BOOL blnInit)
{
	// This routine initializes and tracks the phase offset per symbol and adjust intPtr +/-1 when the offset creeps to a threshold value.
	// adjusts (by Ref) intPtr 0, -1 or +1 based on a filtering of phase offset. 
	// this seems to work fine on test Mar 21, 2015. May need optimization after testing with higher sample rate errors. 
	// This should handle sample rate offsets (sender to receiver) up to about 2000 ppm

	float dblAlpha = 0.3f; // low pass filter constant  may want to optimize value after testing with large sample rate error. 
		// (Affects how much averaging is done) lower values of dblAlpha will minimize adjustments but track more slugishly.

	float dblPhaseOffset;

	static float dblTrackingPhase = 0;
	static float dblModFactor;
	static float dblRadiansPerSample;  // range is .4188 @ car freq = 800 to 1.1195 @ car freq 2200
	static float dblPhaseAtLastTrack;
	static int intCountAtLastTrack;
	static float dblFilteredPhaseOffset;

	if (blnInit)
	{
		// dblFilterredPhase = dblUnfilteredPhase;
		dblTrackingPhase = dblUnfilteredPhase;
		
		if (PSKMode == 16)
			dblModFactor = M_PI / 8;
		else if (PSKMode == 8)
			dblModFactor = M_PI / 4;
		else if (PSKMode == 4)
			dblModFactor = M_PI / 2;
		else
			dblModFactor = M_PI;	// 2PSK

		dblRadiansPerSample = (floatCarFreq * dbl2Pi) / 12000.0f;
		dblPhaseOffset = dblUnfilteredPhase - dblModFactor * round(dblUnfilteredPhase / dblModFactor);
		dblPhaseAtLastTrack = dblPhaseOffset;
		dblFilteredPhaseOffset = dblPhaseOffset;
		intCountAtLastTrack = 0;
		return 0;
	}

	intCountAtLastTrack += 1;
	dblPhaseOffset = dblUnfilteredPhase - dblModFactor * round(dblUnfilteredPhase / dblModFactor);
	dblFilteredPhaseOffset = (1 - dblAlpha) * dblFilteredPhaseOffset + dblAlpha * dblPhaseOffset;

	if ((dblFilteredPhaseOffset - dblPhaseAtLastTrack) > dblRadiansPerSample)
	{
		//Debug.WriteLine("Filtered>LastTrack: Cnt=" & intCountAtLastTrack.ToString & "  Filtered = " & Format(dblFilteredPhaseOffset, "00.000") & "  Offset = " & Format(dblPhaseOffset, "00.000") & "  Unfiltered = " & Format(dblUnfilteredPhase, "00.000"))
		dblFilteredPhaseOffset = dblPhaseOffset - dblRadiansPerSample;
		dblPhaseAtLastTrack = dblFilteredPhaseOffset;
	
		if (AccumulateStats)
		{
			if (OFDM)	
			{ 
				intOFDMTrackAttempts++;
				intAccumOFDMTracking--;
			}
			else
			if (QAM)
			{ 
				intQAMTrackAttempts++;
				intAccumQAMTracking--;
			}
			else
			{
				intPSKTrackAttempts++;
				intAccumPSKTracking--;
			}
		}
		return -1;
	}

	if ((dblPhaseAtLastTrack - dblFilteredPhaseOffset) > dblRadiansPerSample)
	{
		//'Debug.WriteLine("Filtered<LastTrack: Cnt=" & intCountAtLastTrack.ToString & "  Filtered = " & Format(dblFilteredPhaseOffset, "00.000") & "  Offset = " & Format(dblPhaseOffset, "00.000") & "  Unfiltered = " & Format(dblUnfilteredPhase, "00.000"))
		dblFilteredPhaseOffset = dblPhaseOffset + dblRadiansPerSample;
		dblPhaseAtLastTrack = dblFilteredPhaseOffset;

		if (AccumulateStats)
		{
			if (OFDM)	
			{ 
				intOFDMTrackAttempts++;
				intAccumOFDMTracking++;
			}
			else
			if (QAM)	// 16QAM" Then
			{ 
				intQAMTrackAttempts++;
				intAccumQAMTracking++;
			}
			else
			{
				intPSKTrackAttempts++;
				intAccumPSKTracking++;
			}
		}
		return 1;
	}
	// 'Debug.WriteLine("Filtered Phase = " & Format(dblFilteredPhaseOffset, "00.000") & "  Offset = " & Format(dblPhaseOffset, "00.000") & "  Unfiltered = " & Format(dblUnfilteredPhase, "00.000"))

	return 0;
}
 
// Function to compute the differenc of two angles 

int ComputeAng1_Ang2(int intAng1, int intAng2)
{
	// do an angle subtraction intAng1 minus intAng2 (in milliradians) 
	// Results always between -3142 and 3142 (+/- Pi)

	int intDiff;

	intDiff = intAng1 - intAng2;

	if (intDiff < -3142)
		intDiff += 6284;
	else if (intDiff > 3142 )
		intDiff -= 6284;

	return intDiff;
}

// Subroutine to "rotate" the phases to try and set the average offset to 0. 

void CorrectPhaseForTuningOffset(short * intPhase, int intPhaseLength, int intPSKMode)
{
	// A tunning error of -1 Hz will rotate the phase calculation Clockwise ~ 64 milliradians (~4 degrees)
	//   This corrects for:
	// 1) Small tuning errors which result in a phase bias (rotation) of then entire constellation
	// 2) Small Transmitter/receiver drift during the frame by averaging and adjusting to constellation to the average. 
	//   It only processes phase values close to the nominal to avoid generating too large of a correction from outliers: +/- 30 deg for 4PSK, +/- 15 deg for 8PSK
	//  Is very affective in handling initial tuning error.  

	short intPhaseMargin  = 2095 / intPSKMode; // Compute the acceptable phase correction range (+/-30 degrees for 4 PSK)
	short intPhaseInc = 6284 / intPSKMode;
	int intTest;
	int i;
	int intOffset, intAvgOffset, intAvgOffsetBeginning, intAvgOffsetEnd;
	int intAccOffsetCnt = 0, intAccOffsetCntBeginning = 0, intAccOffsetCntEnd = 0;
	int	intAccOffsetBeginning = 0, intAccOffsetEnd = 0, intAccOffset = 0;

      
	// Note Rev 0.6.2.4 The following phase margin value increased from 2095 (120 deg) to 2793 (160 deg) yielded an improvement in decode at low S:N

	intPhaseMargin  = 2793 / intPSKMode; // Compute the acceptable phase correction range (+/-30 degrees for 4 PSK)
	intPhaseInc = 6284 / intPSKMode;

	// Compute the average offset (rotation) for all symbols within +/- intPhaseMargin of nominal
            
	for (i = 0; i <  intPhaseLength; i++)
	{
		intTest = (intPhase[i] / intPhaseInc);
		intOffset = intPhase[i] - intTest * intPhaseInc;

		if ((intOffset >= 0 && intOffset <= intPhaseMargin) || (intOffset < 0 && intOffset >= -intPhaseMargin))
		{
			intAccOffsetCnt += 1;
			intAccOffset += intOffset;
			
			if (i <= intPhaseLength / 4)
			{
				intAccOffsetCntBeginning += 1;
				intAccOffsetBeginning += intOffset;
			}
			else if (i >= (3 * intPhaseLength) / 4)
			{
				intAccOffsetCntEnd += 1;
				intAccOffsetEnd += intOffset;
			}
		}
	}
	
	if (intAccOffsetCnt > 0)
		intAvgOffset = (intAccOffset / intAccOffsetCnt);
	if (intAccOffsetCntBeginning > 0)
		intAvgOffsetBeginning = (intAccOffsetBeginning / intAccOffsetCntBeginning);
	if (intAccOffsetCntEnd > 0)
		intAvgOffsetEnd = (intAccOffsetEnd / intAccOffsetCntEnd);
     
	//Debugprintf("[CorrectPhaseForOffset] Beginning: %d End: %d Total: %d",
		//intAvgOffsetBeginning, intAvgOffsetEnd, intAvgOffset);

	if ((intAccOffsetCntBeginning > intPhaseLength / 8) && (intAccOffsetCntEnd > intPhaseLength / 8))
	{
		for (i = 0; i < intPhaseLength; i++)
		{
			intPhase[i] = intPhase[i] - ((intAvgOffsetBeginning * (intPhaseLength - i) / intPhaseLength) + (intAvgOffsetEnd * i / intPhaseLength));
			if (intPhase[i] > 3142)
				intPhase[i] -= 6284;
			else if (intPhase[i] < -3142)
				intPhase[i] += 6284;
		}
		Debugprintf("[CorrectPhaseForTuningOffset] AvgOffsetBeginning=%d AvgOffsetEnd=%d AccOffsetCnt=%d/%d",
				intAvgOffsetBeginning, intAvgOffsetEnd, intAccOffsetCnt, intPhaseLength);
	}
	else if (intAccOffsetCnt > intPhaseLength / 2)
	{
		for (i = 0; i < intPhaseLength; i++)
		{
			intPhase[i] -= intAvgOffset;
			if (intPhase[i] > 3142)
				intPhase[i] -= 6284;
			else if (intPhase[i] < -3142)
				intPhase[i] += 6284;
		}
		Debugprintf("[CorrectPhaseForTuningOffset] AvgOffset=%d AccOffsetCnt=%d/%d",
				intAvgOffset, intAccOffsetCnt, intPhaseLength);

	}
}

// Function to Decode one Carrier of 16QAM modulation 

//	Call for each set of 4 or 8 Phase Values

short intCarMagThreshold[MAXCAR] = {0};


VOID Decode1CarQAM(int Carrier)
{
	unsigned int intData;
	int k;
	float dblAlpha = 0.1f; // this determins how quickly the rolling average dblTrackingThreshold responds.

	// dblAlpha value of .1 seems to work well...needs to be tested on fading channel (e.g. Multipath)
	
	int Threshold = intCarMagThreshold[Carrier];
	int Len = intPhasesLen;

	UCHAR * Decoded = bytFrameData[Carrier];

	if (CarrierOk[Carrier])
		return;	// don't do it again
	pskStart = 0;
	charIndex = 0;

	// We calculated initial mag from reference symbol

	// use filtered tracking of refernce phase amplitude
	// (should be full amplitude value)
     
	// On WGN this appears to improve decoding threshold about 1 dB 9/3/2016
    	
	while (Len >= 0)
	{
		// Phase Samples are in intPhases

		intData = 0;

		for (k = 0; k < 2; k++)
		{
			intData <<= 4;

			if (intPhases[Carrier][pskStart] < 393 && intPhases[Carrier][pskStart] > -393)
			{
			}		// Zero so no need to do anything
			else if (intPhases[Carrier][pskStart] >= 393 && intPhases[Carrier][pskStart] < 1179)
				intData += 1;
			else if (intPhases[Carrier][pskStart] >= 1179 && intPhases[Carrier][pskStart] < 1965)
				intData += 2;
			else if (intPhases[Carrier][pskStart] >= 1965 && intPhases[Carrier][pskStart] < 2751)
				intData += 3;
			else if (intPhases[Carrier][pskStart] >= 2751 || intPhases[Carrier][pskStart] < -2751)
				intData += 4;
			else if (intPhases[Carrier][pskStart] >= -2751 && intPhases[Carrier][pskStart] < -1965)
				intData += 5;
			else if (intPhases[Carrier][pskStart] >= -1965 && intPhases[Carrier][pskStart] <= -1179)
				intData += 6;
			else 
				intData += 7;

			if (intMags[Carrier][pskStart] < Threshold)
			{
				intData += 8;		//  add 8 to "inner circle" symbols. 
				Threshold = (Threshold * 900 + intMags[Carrier][pskStart] * 150) / 1000;
			}
			else
			{
				Threshold = ( Threshold * 900 + intMags[Carrier][pskStart] * 75) / 1000;
			}
		
			intCarMagThreshold[Carrier] = Threshold;
			pskStart++;
		}
		Decoded[charIndex++] = intData;
		Len -=2;
	}
}
//	Functions to demod all PSKData frames single or multiple carriers 


VOID InitDemodPSK()
{
	// Called at start of frame

	int i;
	float dblPhase, dblReal, dblImag;

	intPSKMode = strMod[0] - '0';
	PSKInitDone = TRUE;
	intPhasesLen = 0;

	if (intPSKMode == 8)
		dblPhaseInc = 2 * M_PI * 1000 / 8;
	else
		dblPhaseInc = 2 * M_PI * 1000 / 4;

	if (intBaud == 50)
		intSampPerSym = 240;
	else
		intSampPerSym = 120;

	if (intNumCar == 1)
		floatCarFreq = 1500;
	else
		floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing
  
	for (i= 0; i < intNumCar; i++)
	{
		if (intBaud == 50)
		{
			intCP[i] = 0;
			intNforGoertzel[i] = 240;
            dblFreqBin[i] = floatCarFreq / 50;
		}
		else if (intBaud == 100)
		{
			//Experimental use of Hanning Windowing
				
            intNforGoertzel[i] = 120;
            dblFreqBin[i] = floatCarFreq / 100;
            intCP[i] = 0;
		}
 
/*		if (intBaud == 100 && floatCarFreq == 1500) 
		{
		intCP[i] = 20;  //  These values selected for best decode percentage (92%) and best average 4PSK Quality (82) on MPP0dB channel
		dblFreqBin[i] = floatCarFreq / 150;
		intNforGoertzel[i] = 80;
		}
		else if (intBaud == 100)
		{
			intCP[i] = 28; // This value selected for best decoding percentage (56%) and best Averag 4PSK Quality (77) on mpg +5 dB
			intNforGoertzel[i] = 60;
			dblFreqBin[i] = floatCarFreq / 200;
		}
		else if (intBaud == 167)
		{
			intCP[i] = 6;  // Need to optimize (little difference between 6 and 12 @ wgn5, 2 Car 500 Hz)
			intNforGoertzel[i] = 60;
			dblFreqBin[i] = floatCarFreq / 200;
		}
*/	
		// Get initial Reference Phase		

		GoertzelRealImagHann120(intFilteredMixedSamples, 0, intNforGoertzel[i], dblFreqBin[i], &dblReal, &dblImag);
            
		dblPhase = atan2f(dblImag, dblReal);
		Track1CarPSK(floatCarFreq, strMod[0] - '0', FALSE, FALSE, dblPhase, TRUE);
		intPSKPhase_1[i] = -1000 * dblPhase;  // negative sign compensates for phase reverse after mixing

		// Set initial mag from Reference Phase (which should be full power)
		// Done here as well as in initQAM for pkt where we may switch mode midpacket

		intCarMagThreshold[i] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
		intCarMagThreshold[i] *= 0.75;

		floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
	}
}

int Demod1CarPSKChar(int Start, int Carrier);
void SavePSKSamples(int i);


short WeightedAngleAvg(short intAng1, short intAng2);

int CheckCarrierPairPSK(int Base, int Dup, int frameLen)
{
	int i, Len;
	
	Debugprintf("DemodPSK Carriers %d and %d", Base, Dup);

	Decode1CarPSK(Base, FALSE);
	Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);

	if (CarrierOk[Base])
	{
		// No need to decode 2nd

		CarrierOk[Dup] = 1;		// So FrameOk test is passed
		return Len + frameLen;
	}
	
	Debugprintf("DemodPSK Carrier %d bad, trying %d", Base, Dup);

	Decode1CarPSK(Dup, FALSE);			// Decode Dup carrier
	Len =  CorrectRawDataWithRS(&bytFrameData[Dup][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);	// Save as carrier 1
	
	if (CarrierOk[Base])
	{
		CarrierOk[Dup] = 1;		// So FrameOk test is passed
		bytFrameData[Base][0] = Len;
		memcpy(&bytFrameData[Base][1], &bytData[frameLen], Len);		// Any retry will use first copy without new decode
		return Len + frameLen;
	}


	// Try to average phases for the two carriers

	Debugprintf("DemodPSK both bad, trying average");

	for (i = 0; i <intPhasesLen; i++)
	{
		intPhases[Base][i] = WeightedAngleAvg(intPhases[Base][i], intPhases[Dup][i]);
	}
	
	// Try decode again on averages

	Decode1CarPSK(Base, FALSE);
	Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);

	if (CarrierOk[Base])
		CarrierOk[Dup] = 1;		// So FrameOk test is passed
		
	return Len + frameLen;
}


void DemodPSK()
{
	int Used[MAXCAR] = {0}, Carrier;
	int Start = 0, i;
	int MemARQRetries = 0;

	// We can't wait for the full frame as we don't have enough RAM, so
	// we do one DMA Buffer at a time, until we run out or end of frame

	// Only continue if we have enough samples
	
	intPSKMode = strMod[0] - '0';

	while (State == AcquireFrame)
	{
		if (intFilteredMixedSamplesLength < intPSKMode * intSampPerSym + 10) // allow for a few phase corrections
		{
			// Move any unprocessessed data down buffer

			//	(while checking process - will use cyclic buffer eventually

			if (intFilteredMixedSamplesLength > 0 && Start > 0)
				memmove(intFilteredMixedSamples,
					&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2); 

			return;
		}
		

		if (PSKInitDone == 0)		// First time through
		{	
			if (intFilteredMixedSamplesLength < 2 * intPSKMode * intSampPerSym + 10) 
				return;				// Wait for at least 2 chars worth

			InitDemodPSK();
			intFilteredMixedSamplesLength -= intSampPerSym;
			if (intFilteredMixedSamplesLength < 0)
				Debugprintf("Corrupt intFilteredMixedSamplesLength");

			Start += intSampPerSym;	
		}

		// If this is a multicarrier mode, we must call the
		// decode char routing for each carrier

		if (intNumCar == 1)
			floatCarFreq = 1500;
		else
			floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing

		Used[0] = Demod1CarPSKChar(Start, 0);

		if (intNumCar > 1)
		{
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[1] = Demod1CarPSKChar(Start, 1);
		}

		if (intNumCar > 2)
		{
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Used[2] = Demod1CarPSKChar(Start, 2);
			
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Used[3] = Demod1CarPSKChar(Start, 3);
		}

		if (intNumCar > 4)
		{
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[4] = Demod1CarPSKChar(Start, 4);
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[5] = Demod1CarPSKChar(Start, 5);
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[6] = Demod1CarPSKChar(Start, 6);
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[7] = Demod1CarPSKChar(Start, 7);
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[8] = Demod1CarPSKChar(Start, 8);
			intPhasesLen -= intPSKMode;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 

			Used[9] = Demod1CarPSKChar(Start, 9);	
		}

		if (intPSKMode == 4)
			SymbolsLeft--;		// number still to decode
		else
			SymbolsLeft -=3;

		// If/when we reenable phase correstion we can take average of Used values.
		// ?? Should be also keep start value per carrier ??

		Start += Used[0];
		intFilteredMixedSamplesLength -= Used[0];

		if (intFilteredMixedSamplesLength < 0)
			Debugprintf("Corrupt intFilteredMixedSamplesLength");

		if (SymbolsLeft > 0)
			continue;	

		// Decode the phases

		DecodeCompleteTime = Now;

//		CorrectPhaseForTuningOffset(&intPhases[0][0], intPhasesLen, strMod);
			
//		if (intNumCar > 1)
//			CorrectPhaseForTuningOffset(&intPhases[1][0], intPhasesLen, strMod);
			
		if (intNumCar > 2)
		{
//			CorrectPhaseForTuningOffset(&intPhases[2][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[3][0], intPhasesLen, strMod);
		}
		if (intNumCar > 4)
		{
//			CorrectPhaseForTuningOffset(&intPhases[4][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[5][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[6][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[7][0], intPhasesLen, strMod);
		}

		// Rick uses the last carier for Quality
		intLastRcvdFrameQuality = UpdatePhaseConstellation(&intPhases[intNumCar - 1][0], &intMags[intNumCar - 1][0], strMod[0] - '0', FALSE, FALSE);

		// prepare for next

		State = SearchingForLeader;
		DiscardOldSamples();
		ClearAllMixedSamples();

		if (strchr(strMod, 'R'))
		{
			// Robust Mode - data is repeated (1-2 or 1-6, 2-7, etc

			if (intNumCar == 2)
			{
				frameLen = CheckCarrierPairPSK(0, 1, 0);
				return;
			}

			//Only have 2 or 10 (500 or 2500 modes)


			frameLen = CheckCarrierPairPSK(0, 5, 0);
			frameLen = CheckCarrierPairPSK(1, 6, frameLen);
			frameLen = CheckCarrierPairPSK(2, 7, frameLen);
			frameLen = CheckCarrierPairPSK(3, 8, frameLen);
			frameLen = CheckCarrierPairPSK(4, 9, frameLen);

			return;
		}

		// Non -robust

		frameLen = 0;

		for (i = 0; i < intNumCar; i++)	
		{
			Decode1CarPSK(i, FALSE);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[i][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, i);
		}

		// If variable length packet frame header we only have header - leave rx running
		
		if (intFrameType == PktFrameHeader)
		{
			State = SearchingForLeader;
			
			// Save any unused samples
			
			if (intFilteredMixedSamplesLength > 0 && Start > 0)
				memmove(intFilteredMixedSamples,
					&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2); 

			return;
		}
	
#ifdef MEMORYARQ

		for (Carrier = 0; Carrier < intNumCar; Carrier++)
		{
			if (!CarrierOk[Carrier])
			{
				// Decode error - save data for MEM ARQ

				SavePSKSamples(Carrier);
				
				if (intSumCounts[Carrier] > 1)
				{
					Decode1CarQAM(Carrier); // try to decode based on the WeightedAveragePhases
					MemARQRetries++;
				}
			}
		}

		if (MemARQRetries)
		{
			// We've retryed to decode - see if ok now

			int OKNow = TRUE;

			Debugprintf("DemodPSK retry RS on MEM ARQ Corrected frames");
			frameLen = 0;
	
			for (Carrier = 0; Carrier < intNumCar; Carrier++)
			{
				frameLen += CorrectRawDataWithRS(bytFrameData[Carrier], bytData, intDataLen, intRSLen, intFrameType, Carrier);
				if (CarrierOk[Carrier] == 0)
					OKNow = FALSE;
			}

			if (OKNow && AccumulateStats) 
				intGoodPSKSummationDecodes++;
		}
#endif
	}
	return;
}

// Function to demodulate one carrier for all PSK frame types
int Demod1CarPSKChar(int Start, int Carrier)
{
	// Converts intSample to an array of differential phase and magnitude values for the Specific Carrier Freq
	// intPtr should be pointing to the approximate start of the first reference/training symbol (1 of 3) 
	// intPhase() is an array of phase values (in milliradians range of 0 to 6283) for each symbol 
	// intMag() is an array of Magnitude values (not used in PSK decoding but for constellation plotting or QAM decoding)
	// Objective is to use Minimum Phase Error Tracking to maintain optimum pointer position

	//	This is called for one DMA buffer of samples (normally 1200)

	float dblReal, dblImag;
	int intMiliRadPerSample = floatCarFreq * M_PI / 6;
	int i;
	int intNumOfSymbols = intPSKMode;
	int origStart = Start;;

	if (CarrierOk[Carrier])		// Already decoded this carrier?
	{
		intPhasesLen += intNumOfSymbols;
		return intSampPerSym * intNumOfSymbols;
	}

	for (i = 0; i <  intNumOfSymbols; i++)
	{
		GoertzelRealImag(intFilteredMixedSamples, Start, intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
//		GoertzelRealImagHann120(intFilteredMixedSamples, Start, intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
	
		intMags[Carrier][intPhasesLen] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
		intPSKPhase_0[Carrier] = -1000 * atan2f(dblImag, dblReal);
		intPhases[Carrier][intPhasesLen] = (ComputeAng1_Ang2(intPSKPhase_0[Carrier], intPSKPhase_1[Carrier]));

/*
		if (Carrier == 0)
		{
			Corrections = Track1CarPSK(floatCarFreq, strMod, atan2f(dblImag, dblReal), FALSE);

			if (Corrections != 0)
			{
				Start += Corrections;

				if (intCP[i] == 0)
					GoertzelRealImagHanning(intFilteredMixedSamples, Start, intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
				else
					GoertzelRealImag(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);

				intPSKPhase_0[Carrier] = 1000 * atan2f(dblImag, dblReal);
			}
		}
*/
		intPSKPhase_1[Carrier] = intPSKPhase_0[Carrier];
		intPhasesLen++;
		Start += intSampPerSym;
	
	}
	if (AccumulateStats)
		intPSKSymbolCnt += intNumOfSymbols;

	return (Start - origStart);	// Symbols we've consumed
}

VOID InitDemodQAM()
{
	// Called at start of frame

	int i;
	float dblPhase, dblReal, dblImag;

	intPSKMode = 8;				// 16QAM uses 8 PSK
	dblPhaseInc = 2 * M_PI * 1000 / 8;
	intPhasesLen = 0;

	PSKInitDone = TRUE;

	intSampPerSym = 120;

	if (intNumCar == 1)
		floatCarFreq = 1500;
	else
		floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing
  
	for (i= 0; i < intNumCar; i++)
	{
		// Only 100 Hz for QAM
						
		intCP[i] = 0;
		intNforGoertzel[i] = 120;
		dblFreqBin[i] = floatCarFreq / 100;
	
		// Get initial Reference Phase
		
		GoertzelRealImagHanning(intFilteredMixedSamples, intCP[i], intNforGoertzel[i], dblFreqBin[i], &dblReal, &dblImag);
		dblPhase = atan2f(dblImag, dblReal);

		// Set initial mag from Reference Phase (which should be full power)

		intCarMagThreshold[i] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
		intCarMagThreshold[i] *= 0.75;

		Track1CarPSK(floatCarFreq, 8, TRUE, FALSE, dblPhase, TRUE);
		intPSKPhase_1[i] = 1000 * dblPhase;
		floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
	}
}

int Demod1CarQAMChar(int Start, int Carrier);


//	Function to average two angles using magnitude weighting

short WeightedAngleAvg(short intAng1, short intAng2)
{
	// Ang1 and Ang 2 are in the range of -3142 to + 3142 (miliradians)
	// works but should come up with a routine that avoids Sin, Cos, Atan2
    // Modified in Rev 0.3.5.1 to "weight" averaging by intMag1 and intMag2 (why!!!)

	float dblSumX, dblSumY;

	dblSumX = cosf(intAng1 / 1000.0) + cosf(intAng2 / 1000.0);
	dblSumY = sinf(intAng1 / 1000.0) + sinf(intAng2 / 1000.0);
        
	return (1000 * atan2f(dblSumY, dblSumX));
}

#ifdef MEMORYARQ

void SaveQAMSamples(int i)
{
	int m;

	if (intSumCounts[i] == 0)
	{
		// First try - initialize Sum counts Phase average and Mag Average 

		for (m = 0; m < intPhasesLen; m++)
		{
			intCarPhaseAvg[i][m] = intPhases[i][m];
			intCarMagAvg[i][m] = intMags[i][m];
		}
	}
	else
	{
		for (m = 0; m < intPhasesLen; m++)
		{
			intCarPhaseAvg[i][m] = WeightedAngleAvg(intCarPhaseAvg[i][m], intPhases[i][m]);
			intPhases[i][m] = intCarPhaseAvg[i][m];
			// Use simple weighted average for Mags 
			intCarMagAvg[i][m] = (intCarMagAvg[i][m] * intSumCounts[i] + intMags[i][m]) / (intSumCounts[i] + 1);		
			intMags[i][m] = intCarMagAvg[i][m];
		}
	}
	intSumCounts[i]++;
}

void SavePSKSamples(int i)
{
	int m;

	if (intSumCounts[i] == 0)
	{
		// First try - initialize Sum counts Phase average and Mag Average 

		for (m = 0; m < intPhasesLen; m++)
		{
			intCarPhaseAvg[i][m] = intPhases[i][m];
		}
	}
	else
	{
		for (m = 0; m < intPhasesLen; m++)
		{
			intCarPhaseAvg[i][m] = WeightedAngleAvg(intCarPhaseAvg[i][m], intPhases[i][m]);
			intPhases[i][m] = intCarPhaseAvg[i][m];
		}
	}
	intSumCounts[i]++;
}			

#endif

int CheckCarrierPair(int Base, int Dup, int frameLen)
{
	int i, Len;
	
	Debugprintf("DemodQAMR Carriers %d and %d", Base, Dup);

	Decode1CarQAM(Base);
	Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);

	if (CarrierOk[Base])
	{
		// No need to decode 2nd

		CarrierOk[Dup] = 1;		// So FrameOk test is passed
		Debugprintf("DemodQAMR Returning Len %d", Len);
		return Len + frameLen;
	}
	
	Debugprintf("DemodQAMR Carrier %d bad, trying %d", Base, Dup);

	Decode1CarQAM(Dup);			// Decode Dup carrier
	Len =  CorrectRawDataWithRS(&bytFrameData[Dup][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);	// Save as carrier 1
	
	if (CarrierOk[Base])
	{
		CarrierOk[Dup] = 1;		// So FrameOk test is passed
		bytFrameData[Base][0] = Len;
		memcpy(&bytFrameData[Base][1], &bytData[frameLen], Len);		// Any retry will use first copy without new decode
		Debugprintf("DemodQAMR Returning Len %d", Len);
		return Len + frameLen;
	}
	
	// Try to average phases for the two carriers

	Debugprintf("DemodQAMR both bad, trying average");

	for (i = 0; i <intPhasesLen; i++)
	{
		intPhases[Base][i] = WeightedAngleAvg(intPhases[Base][i], intPhases[Dup][i]);
		intMags[Base][i] = (intMags[Base][i] + intMags[Dup][i]) / 2;
	}
	
	// Try decode again on averages

	Decode1CarQAM(Base);
	Len = CorrectRawDataWithRS(&bytFrameData[Base][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, Base);

	if (CarrierOk[Base])
		CarrierOk[Dup] = 1;		// So FrameOk test is passed

	Debugprintf("DemodQAMR Returning Len %d", Len);
		
	return Len + frameLen;
}

BOOL DemodQAM()
{
	int Used = 0;
	int Start = 0;
	int i, MemARQOk = 0;

	// We can't wait for the full frame as we don't have enough RAM, so
	// we do one DMA Buffer at a time, until we run out or end of frame

	// Only continue if we have enough samples

	while (State == AcquireFrame)
	{
		if (intFilteredMixedSamplesLength < 8 * intSampPerSym + 10) // allow for a few phase corrections
		{
			// Move any unprocessessed data down buffer

			//	(while checking process - will use cyclic buffer eventually

			if (intFilteredMixedSamplesLength > 0)
				memmove(intFilteredMixedSamples,
					&intFilteredMixedSamples[Start], intFilteredMixedSamplesLength * 2); 

			return FALSE;
		}
		
		if (PSKInitDone == 0)		// First time through
		{	
			if (intFilteredMixedSamplesLength < 9 * intSampPerSym + 10) 
				return FALSE;				// Wait for at least 2 chars worth

			InitDemodQAM();
			intFilteredMixedSamplesLength -= intSampPerSym;

			Start += intSampPerSym;	
		}

		// If this is a multicarrier mode, we must call the
		// decode char routine for each carrier

		if (intNumCar == 1)
			floatCarFreq = 1500;
		else
			floatCarFreq = 1400 + (intNumCar / 2) * 200; // start at the highest carrier freq which is actually the lowest transmitted carrier due to Reverse sideband mixing

	
		Used = Demod1CarQAMChar(Start, 0);		// demods 2 phase values - enough for one char
	
		if (intNumCar > 1)
		{
			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 1);
		}

		if (intNumCar > 2)
		{
			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 2);
			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 3);
		}

		if (intNumCar > 4)
		{
			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 4);
	
			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 5);

			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 6);

			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 7);	

			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 8);	

			intPhasesLen -= 2;
			floatCarFreq -= 200;  // Step through each carrier Highest to lowest which is equivalent to lowest to highest before RSB mixing. 
			Demod1CarQAMChar(Start, 9);	
		}

		SymbolsLeft--;		// number still to decode - we've done one
	
		Start += Used;
		intFilteredMixedSamplesLength -= Used;

		if (SymbolsLeft <= 0)	
		{
			// Frame complete - decode it

			DecodeCompleteTime = Now;

//		CorrectPhaseForTuningOffset(&intPhases[0][0], intPhasesLen, strMod);
			
//		if (intNumCar > 1)
//			CorrectPhaseForTuningOffset(&intPhases[1][0], intPhasesLen, strMod);
			
		if (intNumCar > 2)
		{
//			CorrectPhaseForTuningOffset(&intPhases[2][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[3][0], intPhasesLen, strMod);
		}
		if (intNumCar > 4)
		{
//			CorrectPhaseForTuningOffset(&intPhases[4][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[5][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[6][0], intPhasesLen, strMod);
//			CorrectPhaseForTuningOffset(&intPhases[7][0], intPhasesLen, strMod);
		}

			intLastRcvdFrameQuality = UpdatePhaseConstellation(&intPhases[intNumCar - 1][0], &intMags[intNumCar - 1][0], 8, TRUE, FALSE);

			// prepare for next so we can exit when we have finished decode

			DiscardOldSamples();
			ClearAllMixedSamples();
			State = SearchingForLeader;

			if (strchr(strMod, 'R'))
			{
				// Robust Mode - data is repeated (1-2 or 1-6, 2-7, etc

				if (intNumCar == 2)
				{
					frameLen = CheckCarrierPair(0, 1, 0);
					return TRUE;
				}

				//Only have 2 or 10 (500 or 2500 modes)


				frameLen = CheckCarrierPair(0, 5, 0);
				frameLen = CheckCarrierPair(1, 6, frameLen);
				frameLen = CheckCarrierPair(2, 7, frameLen);
				frameLen = CheckCarrierPair(3, 8, frameLen);
				frameLen = CheckCarrierPair(4, 9, frameLen);

				return TRUE;
			}

			// Non -robust

			Decode1CarQAM(0);
			frameLen = CorrectRawDataWithRS(&bytFrameData[0][0], bytData, intDataLen, intRSLen, intFrameType, 0);


			if (intNumCar > 1)
			{
				Decode1CarQAM(1);
				frameLen +=  CorrectRawDataWithRS(&bytFrameData[1][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 1);

			}


		if (intNumCar > 2)
		{
			Decode1CarQAM(2);
			Decode1CarQAM(3);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[2][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 2);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[3][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 3);

		}


		if (intNumCar > 4)
		{
			Decode1CarQAM(4);
			Decode1CarQAM(5);
			Decode1CarQAM(6);
			Decode1CarQAM(7);
			Decode1CarQAM(8);
			Decode1CarQAM(9);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[4][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 4);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[5][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 5);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[6][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 6);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[7][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 7);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[8][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 8);
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[9][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, 9);

		}

		// Check Data

		if (memcmp(CarrierOk, Good, intNumCar) == 0)
			return TRUE;

		// Bad decode if we have Memory ARQ try it

#ifdef MEMORYARQ

		for (i = 0; i < intNumCar; i++)
		{
			if (!CarrierOk[i] && intFrameType != PktFrameHeader)
			{
				// Decode error - save data for MEM ARQ
					
				SaveQAMSamples(i);

				if (intSumCounts[0] > 1)
				{
					MemARQOk = 1;
					Decode1CarQAM(i); // try to decode based on the WeightedAveragePhases
				}
			}
		}

		if (MemARQOk == 0)			// Havent averaged yet
			return TRUE;

		// We've tried to correct - see if it worked

		Debugprintf("DemodQAM Trying MEM ARQ");

		// Non -robust

		frameLen = 0;
		
		for (i = 0; i < intNumCar; i++)
		{
			frameLen +=  CorrectRawDataWithRS(&bytFrameData[i][0], &bytData[frameLen], intDataLen, intRSLen, intFrameType, i);
		}

		// Check Data

		if (memcmp(CarrierOk, Good, intNumCar) == 0)
		{
			Debugprintf("DemodQAM MEM ARQ Corrected frame");
			intGoodQAMSummationDecodes++;
		}
#endif
		}
	}
	return TRUE;
}

int Demod1CarQAMChar(int Start, int Carrier)
{
	// Converts intSample to an array of differential phase and magnitude values for the Specific Carrier Freq
	// intPtr should be pointing to the approximate start of the first reference/training symbol (1 of 3) 
	// intPhase() is an array of phase values (in milliradians range of 0 to 6283) for each symbol 
	// intMag() is an array of Magnitude values (not used in PSK decoding but for constellation plotting or QAM decoding)
	// Objective is to use Minimum Phase Error Tracking to maintain optimum pointer position

	//	This is called for one DMA buffer of samples (normally 1200)

	float dblReal, dblImag;
	int intMiliRadPerSample = floatCarFreq * M_PI / 6;
	int i;
	int intNumOfSymbols = 2;
	int origStart = Start;;

	if (CarrierOk[Carrier])		// Already decoded this carrier?
	{
		intPhasesLen += intNumOfSymbols;
		return intSampPerSym * intNumOfSymbols;
	}

	for (i = 0; i <  intNumOfSymbols; i++)
	{
	//	GoertzelRealImag(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
		GoertzelRealImagHanning(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
		intMags[Carrier][intPhasesLen] = sqrtf(powf(dblReal, 2) + powf(dblImag, 2));
		intPSKPhase_0[Carrier] = 1000 * atan2f(dblImag, dblReal);
		intPhases[Carrier][intPhasesLen] = -(ComputeAng1_Ang2(intPSKPhase_0[Carrier], intPSKPhase_1[Carrier]));


/*
		if (Carrier == 0)
		{
			Corrections = Track1CarPSK(floatCarFreq, strMod, atan2f(dblImag, dblReal), FALSE);

			if (Corrections != 0)
			{
				Start += Corrections;

		//	GoertzelRealImag(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
				GoertzelRealImagHanning(intFilteredMixedSamples, Start + intCP[Carrier], intNforGoertzel[Carrier], dblFreqBin[Carrier], &dblReal, &dblImag);
				intPSKPhase_0[Carrier] = 1000 * atan2f(dblImag, dblReal);
			}
		}
*/
		intPSKPhase_1[Carrier] = intPSKPhase_0[Carrier];
		intPhasesLen++;
		Start += intSampPerSym;
	}
       
	if (AccumulateStats)
		intQAMSymbolCnt += intNumOfSymbols;

	return (Start - origStart);	// Symbols we've consumed
}


extern int bytQDataInProcessLen;


//	function to decode one carrier from tones (used to decode from Averaged intToneMags) 

BOOL Decode1Car4FSKFromTones(UCHAR * bytData, int intToneMags)
{
	//	Decodes intToneMags() to an array of bytes   
    //	Updates bytData() with decoded 

/*
	UCHAR bytSym;
	int intIndex;

        ReDim bytData(intToneMags.Length \ 16 - 1)

        For i As Integer = 0 To bytData.Length - 1 ' For each data byte
            intIndex = 16 * i
            For j As Integer = 0 To 3 ' for each 4FSK symbol (2 bits) in a byte
                If intToneMags(intIndex) > intToneMags(intIndex + 1) And intToneMags(intIndex) > intToneMags(intIndex + 2) And intToneMags(intIndex) > intToneMags(intIndex + 3) Then
                    bytSym = 0
                ElseIf intToneMags(intIndex + 1) > intToneMags(intIndex) And intToneMags(intIndex + 1) > intToneMags(intIndex + 2) And intToneMags(intIndex + 1) > intToneMags(intIndex + 3) Then
                    bytSym = 1
                ElseIf intToneMags(intIndex + 2) > intToneMags(intIndex) And intToneMags(intIndex + 2) > intToneMags(intIndex + 1) And intToneMags(intIndex + 2) > intToneMags(intIndex + 3) Then
                    bytSym = 2
                Else
                    bytSym = 3
                End If
                bytData(i) = (bytData(i) << 2) + bytSym
                intIndex += 4
            Next j
        Next i
        Return True
    End Function  '  Decode1Car4FSKFromTones
*/
	return TRUE;
}

/*    ' Function to decode one carrier from tones (used to decode from Averaged intToneMags) 
    Private Function Decode1Car8FSKFromTones(ByRef bytData() As Byte, ByRef intToneMags() As Int32) As Boolean
        ' Decodes intToneMags() to an array of bytes   
        ' Updates bytData() with decoded 

        Dim bytSym As Byte
        Dim intThreeBytes As Int32
        ReDim bytData(3 * intToneMags.Length \ 64 - 1)
        Dim intMaxMag As Int32
        For i As Integer = 0 To (bytData.Length \ 3) - 1   ' For each group of 3 bytes data byte
            intThreeBytes = 0
            For j As Integer = 0 To 7 ' for each group of 8 symbols (24 bits) 
                intMaxMag = 0
                For k As Integer = 0 To 7 ' for each of 8 possible tones per symbol
                    If intToneMags((i * 64) + 8 * j + k) > intMaxMag Then
                        intMaxMag = intToneMags((i * 64) + 8 * j + k)
                        bytSym = k
                    End If
                Next k
                intThreeBytes = (intThreeBytes << 3) + bytSym
            Next j
            bytData(3 * i) = (intThreeBytes And &HFF0000) >> 16
            bytData(3 * i + 1) = (intThreeBytes And &HFF00) >> 8
            bytData(3 * i + 2) = (intThreeBytes And &HFF)
        Next i
        Return True
    End Function  '  Decode1Car8FSKFromTones

    ' Function to decode one carrier from tones (used to decode from Averaged intToneMags) 
    Private Function Decode1Car16FSKFromTones(ByRef bytData() As Byte, ByRef intToneMags() As Int32) As Boolean
        ' Decodes intToneMags() to an array of bytes   
        ' Updates bytData() with decoded tones 

        Dim bytSym As Byte
        Dim intMaxMag As Int32
        ReDim bytData(intToneMags.Length \ 32 - 1)
        For i As Integer = 0 To bytData.Length - 1 ' For each data byte
            For j As Integer = 0 To 1 ' for each 16FSK symbol (4 bits) in a byte
                intMaxMag = 0
                For k As Integer = 0 To 15
                    If intToneMags(i * 32 + 16 * j + k) > intMaxMag Then
                        intMaxMag = intToneMags(i * 32 + 16 * j + k)
                        bytSym = k
                    End If
                Next k
                bytData(i) = (bytData(i) << 4) + bytSym
            Next j
        Next i
        Return True
    End Function  '  Decode1Car16FSKFromTones

*/



//	Subroutine to update the Busy detector when not displaying Spectrum or Waterfall (graphics disabled)
 		
extern int LastBusyCheck;

extern BOOL blnBusyStatus;

int intWaterfallRow = 0;



void UpdateBusyDetector(short * bytNewSamples)
{
	float dblReF[1024];
	float dblImF[1024];
	float dblMag[206];

	float dblMagAvg = 0;
	int intTuneLineLow, intTuneLineHi, intDelta;
	int i;
	int BusyFlag;


	if (ProtocolState > DISC)		// ' Only process busy when in DISC state
		return;

	if (State != SearchingForLeader)
		return;						// only when looking for leader

	if (Now - LastBusyCheck < 100)
		return;

	LastBusyCheck = Now;

	FourierTransform(1024, bytNewSamples, &dblReF[0], &dblImF[0], FALSE);

	for (i = 0; i < 206; i++)
	{
		//	starting at ~300 Hz to ~2700 Hz Which puts the center of the signal in the center of the window (~1500Hz)

		dblMag[i] = powf(dblReF[i + 25], 2) + powf(dblImF[i + 25], 2);	 // first pass 
		dblMagAvg += dblMag[i];
	}

	// Not sure about this as we use variable bandwidth frames. For now use 500

	intDelta = (500 / 2 + TuningRange) / 11.719f;

	intTuneLineLow = max((103 - intDelta), 3);
	intTuneLineHi = min((103 + intDelta), 203);

	// At the moment we only get here what seaching for leader,
	// but if we want to plot spectrum we should call
	// it always

	BusyFlag = BusyDetect3(dblMag, intTuneLineLow, intTuneLineHi);

	if (BusyFlag == 0)
	{
		if (BusyCount == 0)
			blnBusyStatus = 0;
		else
			BusyCount--;
	}
	else
	{
		blnBusyStatus = 1;
		BusyCount = 10;			// Try delaying busy off a bit
	}
 
	if (blnBusyStatus && !blnLastBusyStatus)
	{
		Debugprintf("BUSY TRUE");
	}
	//    stcStatus.Text = "True"
		//    queTNCStatus.Enqueue(stcStatus)
		//    'Debug.WriteLine("BUSY TRUE @ " & Format(DateTime.UtcNow, "HH:mm:ss"))

	else if (blnLastBusyStatus && !blnBusyStatus)
	{
		Debugprintf("BUSY FALSE");
	}

	blnLastBusyStatus = blnBusyStatus;

}