/*
	Copyright 2016 Giovanni Franza - HB9EIK 

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

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.

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

	This program is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <pthread.h>
#include <sched.h>
#include <netinet/in.h>
#include <netdb.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <errno.h>
#include <sys/shm.h>
#include <math.h>
#include <openssl/sha.h>
#include <glib.h>

#define BUFFERSIZE 512*24

typedef struct _payload {
	pthread_cond_t *con_a;
	pthread_cond_t *con_b;
	pthread_cond_t *con_c;
	pthread_cond_t *con;
	pthread_mutex_t *mut;
	pthread_mutex_t *mut_a;
	pthread_mutex_t *mut_b;
	pthread_mutex_t *mut_c;
	char *buf;
	int *lung;
	char *buftemp;
	char *bufmed;
	char *shmaddr;
	struct timeval start;
	struct timeval stop;
	int *outfile;
	void *payload;
	char *passphrase;
	int *freq;
	int *bw;
	int *mod;
	long *tune;
	long *bfo;
	short *att_lowpass_rate;
} payload, *payloadp;

void bufferMixer( float *in, float *out, int howMany, float *osci, float *oscq ) {
	int hm=howMany/4;
	int j,k;
	float inf, qua;

	for( j=0,k=0; j<hm; j+=2,k++ ) {
		inf=in[j];
		qua=in[j+1];
		out[j]=-qua*oscq[k]+inf*osci[k];
		out[j+1]=inf*oscq[k]+qua*osci[k];
	}
}

static float inphase[512*3]={0};
static float quadrature[512*3]={0};

void bufferLO( int freq, float *inphase, float *quadrature, int howMany ) {

	static int isFirst=1;
	static double amplitude=1;
	static int myfreq=0;
	static double phi=0;
	static double cosphi=1;
	static double sinphi=0;
	static double costmp=1;
	static double sintmp=0;
	static double cossav=1;
	static double sinsav=0;
	int j;
	if( isFirst ) {
		inphase[howMany-1]=0;
		quadrature[howMany-1]=1;
	}
	if( myfreq != freq ) {
		myfreq=freq;
		phi=-freq*2*3.1415926535897932384/768000;
		cosphi=cos( phi );
		sinphi=sin( phi );
	}
	amplitude=1.0d+(1.0d-sqrt(sintmp*sintmp+costmp*costmp))/howMany;

	for( j=0; j<howMany; j++ ) {
		sinsav=sintmp*cosphi+costmp*sinphi;
		cossav=costmp*cosphi-sintmp*sinphi;
		sintmp=amplitude*sinsav;
		costmp=amplitude*cossav;
		inphase[j]=costmp;
		quadrature[j]=sintmp;
	}
}

static float BFOinphase[512*3]={0};
static float BFOquadrature[512*3]={0};

void bufferBFO( int freq, float *inphase, float *quadrature, int howMany ) {

	static int isFirst=1;
	static double amplitude=1;
	static int myfreq=0;
	static double phi=0;
	static double cosphi=1;
	static double sinphi=0;
	static double costmp=1;
	static double sintmp=0;
	static double cossav=1;
	static double sinsav=0;
	int j;
	if( isFirst ) {
		inphase[howMany-1]=0;
		quadrature[howMany-1]=1;
	}
	if( myfreq != freq ) {
		myfreq=freq;
		phi=-freq*2*3.1415926535897932384/48000;
		cosphi=cos( phi );
		sinphi=sin( phi );
	}
	amplitude=1.0d+(1.0d-sqrt(sintmp*sintmp+costmp*costmp))/howMany;

	for( j=0; j<howMany; j++ ) {
		sinsav=sintmp*cosphi+costmp*sinphi;
		cossav=costmp*cosphi-sintmp*sinphi;
		sintmp=amplitude*sinsav;
		costmp=amplitude*cossav;
		inphase[j]=costmp;
		quadrature[j]=sintmp;
	}
}

static float BFO1inphase[512*3]={0};
static float BFO1quadrature[512*3]={0};

void bufferBFO1( int freq, float *inphase, float *quadrature, int howMany ) {

	static int isFirst=1;
	static double amplitude=1;
	static int myfreq=0;
	static double phi=0;
	static double cosphi=1;
	static double sinphi=0;
	static double costmp=1;
	static double sintmp=0;
	static double cossav=1;
	static double sinsav=0;
	int j;
	if( isFirst ) {
		inphase[howMany-1]=0;
		quadrature[howMany-1]=1;
	}
	if( myfreq != freq ) {
		myfreq=freq;
		phi=-freq*2*3.1415926535897932384/24000;
		cosphi=cos( phi );
		sinphi=sin( phi );
	}
	amplitude=1.0d+(1.0d-sqrt(sintmp*sintmp+costmp*costmp))/howMany;

	for( j=0; j<howMany; j++ ) {
		sinsav=sintmp*cosphi+costmp*sinphi;
		cossav=costmp*cosphi-sintmp*sinphi;
		sintmp=amplitude*sinsav;
		costmp=amplitude*cossav;
		inphase[j]=costmp;
		quadrature[j]=sintmp;
	}
}

#ifndef XtNumber 
#define XtNumber(x) sizeof(x)/sizeof(x[0])
#endif

static double coeff0[]={0.0005682363843196175, 0.003977628147141972, 0.014915836118325287, 0.0387805695219929,
		0.07706302235338547, 0.1228026248638052, 0.16102227904707134, 0.1760082348590731,
		0.16102227904707134, 0.1228026248638052, 0.07706302235338547, 0.0387805695219929,
		0.014915836118325287, 0.003977628147141972, 0.0005682363843196175 };

static double coeff1[]={0.0005682363843196175, 0.003977628147141972, 0.014915836118325287, 0.0387805695219929,
		0.07706302235338547, 0.1228026248638052, 0.16102227904707134, 0.1760082348590731,
		0.16102227904707134, 0.1228026248638052, 0.07706302235338547, 0.0387805695219929,
		0.014915836118325287, 0.003977628147141972, 0.0005682363843196175 };

static double coeff2[]={-0.00024070095796718734, -0.0016601253102937602, -0.005854667699670814, -0.013317414278153432,
		-0.020062118534461132, -0.01569053558305392, 0.012765160512214234, 0.07076145541162247,
		0.14668804652489328, 0.212680055050458, 0.23895244966549228, 0.212680055050458,
		0.14668804652489328, 0.07076145541162247, 0.012765160512214234, -0.01569053558305392,
		-0.020062118534461132, -0.013317414278153432, -0.005854667699670814, -0.0016601253102937602,
		-0.00024070095796718734};

static double coeff3[]={-0.00024070095796718734, -0.0016601253102937602, -0.005854667699670814, -0.013317414278153432,
		-0.020062118534461132, -0.01569053558305392, 0.012765160512214234, 0.07076145541162247,
		0.14668804652489328, 0.212680055050458, 0.23895244966549228, 0.212680055050458,
		0.14668804652489328, 0.07076145541162247, 0.012765160512214234, -0.01569053558305392,
		-0.020062118534461132, -0.013317414278153432, -0.005854667699670814, -0.0016601253102937602,
		-0.00024070095796718734};

static float historyi0[XtNumber( coeff0 )]={0};
static float historyq0[XtNumber( coeff0 )]={0};
	
static float historyi1[XtNumber( coeff1 )]={0};
static float historyq1[XtNumber( coeff1 )]={0};
	
static float historyi2[XtNumber( coeff2 )]={0};
static float historyq2[XtNumber( coeff2 )]={0};
	
static float historyi3[XtNumber( coeff3 )]={0};
static float historyq3[XtNumber( coeff3 )]={0};
	
static int coeff_lung0=XtNumber(coeff0);
static int coeff_lung1=XtNumber(coeff1);
static int coeff_lung2=XtNumber(coeff2);
static int coeff_lung3=XtNumber(coeff3);

static int coeff_punt0=0;
static int coeff_punt1=0;
static int coeff_punt2=0;
static int coeff_punt3=0;

void bufferFilter( float* in, float*out, int howMany, double *coeff, float *historyi, float *historyq, int lung, int *punt ) {
	int j,k,y;
	int hm=howMany/4;

	for( j=0,y=0; j<hm; j+=4,y+=2 ) {
		historyi[(++*punt)%lung]=in[j];
		historyq[(*punt)%lung]=in[j+1];
		historyi[(++*punt)%lung]=in[j+2];
		historyq[(*punt)%lung]=in[j+3];
		out[y]=0;
		out[y+1]=0;
		for( k=0; k<lung; k++ ) {
			out[y]+=historyi[(k+*punt)%lung]*coeff[k];
			out[y+1]+=historyq[(k+*punt)%lung]*coeff[k];
		}
	}
}
 	

static double denom1[]={ 0, -3.419, 4.42, -2.557, 0.5583 };
static double numer1[]={ 0.0001164, 0.0004657, 0.0006985, 0.0004657, 0.0001164 };

// static double numer1[] = { 1.677e-14, 2.684e-13, 2.013e-12, 9.393e-12, 3.053e-11, 7.326e-11, 1.343e-10, 1.919e-10, 2.159e-10, 1.919e-10, 1.343e-10, 7.326e-11, 3.053e-11, 9.393e-12, 2.013e-12, 2.684e-13, 1.677e-14 };
	 
// static double denom1[] = { 1, -12.93, 78.6, -298.2, 790.3778, -1551, 2331.8632, -2739, 2539.4189, -1865, 1081.2488, -489.6, 169.7186, -43.54, 7.796, -0.8703, 0.04564 };

static double histii0[5]={0.0};
static double histiq0[5]={0.0};
static double histui0[5]={0.0};
static double histuq0[5]={0.0};
static double histii1[5]={0.0};
static double histiq1[5]={0.0};
static double histui1[5]={0.0};
static double histuq1[5]={0.0};
static double histii2[5]={0.0};
static double histiq2[5]={0.0};
static double histui2[5]={0.0};
static double histuq2[5]={0.0};
static double histii3[5]={0.0};
static double histiq3[5]={0.0};
static double histui3[5]={0.0};
static double histuq3[5]={0.0};

void audioFilter( float *in, float *out, int howMany, int order, double *denom, double*numer, double *histii, double *histiq, double *histui, double *histuq, int divide ) {
	int j,k;
	long double acci, accq;
	for( j=0; j<howMany; j+=2 ) {
		for( k=order; k>0; k-- ) {
			histii[k]=histii[k-1];
			histiq[k]=histiq[k-1];
			histui[k]=histui[k-1];
			histuq[k]=histuq[k-1];
		}
		histii[0]=in[j];
		histiq[0]=in[j+1];
		acci=0.0;
		accq=0.0;
		for( k=0; k<=order; k++ ) {
			acci+=histii[k]*numer[k];
			accq+=histiq[k]*numer[k];
		}
		for( k=1; k<=order; k++ ) {
			acci-=histui[k]*denom[k];
			accq-=histuq[k]*denom[k];
		}
		histui[0]=acci;
		histuq[0]=accq;
		out[j]=acci;
		out[j+1]=accq;
	}

	if( divide > 1 ) {
		for( j=divide*2, k=2; j<howMany; j+=divide*2,k+=2){
			out[k]=out[j];
			out[k+1]=out[j+1];
		}
	}
}

void *hfThread( void *pay ) {

	payloadp payl=(payloadp)pay;
//	static float howMany=512*3;
	char buf[1024];
	static int sect=0;
	static int sect1=0;
	short lpr;
	short rate;
	int j,k;
	int divider=1;

	for( ;; ) {
		pthread_cond_wait( payl->con, payl->mut);
		// signal to fft
		if( payl->payload ) {
			pthread_cond_signal( payl->con_a );
		}
		memcpy( (char *)payl->tune, payl->shmaddr-6, 4 );
		memcpy( (char *)payl->att_lowpass_rate, payl->shmaddr-2, 2 );
		lpr=ntohs(*payl->att_lowpass_rate);
		rate=lpr>>4;
		divider=1;
		// gettimeofday( &(payl->start), NULL );
		bufferLO( *payl->freq, inphase, quadrature, BUFFERSIZE/8 );
		bufferMixer( (float *)payl->shmaddr, (float *)payl->buf, BUFFERSIZE, inphase, quadrature );
		if( rate == 3 ) {
			bufferFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE, coeff0, historyi0, historyq0, coeff_lung0, &coeff_punt0 );
			divider<<=1;
		}
		if( rate > 1 ) {
			bufferFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider, coeff1, historyi1, historyq1, coeff_lung1, &coeff_punt1 );
			divider<<=1;
		}
		memcpy( payl->bufmed+sect1, payl->buf, BUFFERSIZE/divider );
		sect1+=BUFFERSIZE/divider;
		if( sect1 >1024*16 ) { // 4+4 bytes(I+Q)*2(because FFT decimate)
			sect1=0;
			pthread_cond_signal( payl->con_c );
		}
		bufferFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider, coeff2, historyi2, historyq2, coeff_lung2, &coeff_punt2 );
		divider<<=1;
		bufferFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider, coeff3, historyi3, historyq3, coeff_lung3, &coeff_punt3 );
		divider<<=1;
		bufferBFO( *payl->bfo, BFOinphase, BFOquadrature, BUFFERSIZE/divider/8 );
		bufferMixer( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider, BFOinphase, BFOquadrature );
		audioFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider/4, 4, denom1, numer1, histii0, histiq0, histui0, histuq0, 1 );
		audioFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider/4, 4, denom1, numer1, histii1, histiq1, histui1, histuq1, 1 );
		audioFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider/4, 4, denom1, numer1, histii2, histiq2, histui2, histuq2, 1 );
		audioFilter( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider/4, 4, denom1, numer1, histii3, histiq3, histui3, histuq3, 2 );
		divider<<=1;
		memcpy( payl->buftemp+sect, payl->buf, BUFFERSIZE/divider );
		sect+=BUFFERSIZE/divider;
		if( sect >1024*16 ) { // 4+4 bytes(I+Q)*2(because FFT decimate)
			sect=0;
			pthread_cond_signal( payl->con_b );
		}
		// gettimeofday( &(payl->stop), NULL );
		// fprintf( stderr, "hf time= %f mSec\n", (payl->stop.tv_sec-payl->start.tv_sec)*1000+0.001*(payl->stop.tv_usec-payl->start.tv_usec) );

		bufferBFO1( -*payl->bfo, BFO1inphase, BFO1quadrature, BUFFERSIZE/divider/8 );
		bufferMixer( (float *)payl->buf, (float *)payl->buf, BUFFERSIZE/divider, BFO1inphase, BFO1quadrature );
		buf[0]=0x82;
		buf[1]=0x7e;
		buf[2]=0x00;
		buf[3]=0xC4;
		buf[4]=0x04; //audio
		buf[5]='A';
		buf[6]='F';
		buf[7]=0x01;
		for( j=0,k=8; j<BUFFERSIZE/32; j+=8,k+=4 ){
			memcpy( buf+k, payl->buf+j, 4 );
		}
		if( *payl->outfile > 0 ) {
			k=write( *payl->outfile, buf, 200 );
			if( k==-1 ) {
				fprintf( stderr, "Error writing audio stream error %d - %s\n", errno, strerror(errno) );
			}
		}
	}
	return NULL;
}

void *FFTSend( void *pay ) {

	double *co = NULL;
	double *si = NULL;
	double *fftr = NULL;
	double *ffti = NULL;
	double *coeff = NULL;
	unsigned char *fftout = NULL;
	int N;
	double co1, si1;
	double pi=3.141592653589793238462643383279502884197169399375105820974944592;
	double fftdr, fftdi;
	int j, k; 
	int m, p, md, ng, ig, i, ind, i1, i2, jd, kp;
	long val;
	char buf[1050];
	int n;
	int flipflop=0;
	long freq;

	payloadp payl=(payloadp)pay;
	
	p=10;
	N=1<<p;

	fftr = calloc( N, sizeof( double ) );
	if( fftr == NULL ) {
		fprintf( stderr, "error allocating fftr buffer" );
		exit( 1 );
	}
	ffti = calloc( N, sizeof( double ) );
	if( ffti == NULL ) {
		fprintf( stderr, "error allocating ffti buffer" );
		exit( 2 );
	}
	fftout = calloc( N, sizeof( char ) );
	if( fftout == NULL ) {
		fprintf( stderr, "error allocating fftout buffer" );
		exit( 3 );
	}
	co = calloc( N/2, sizeof( double ) );
	if( co == NULL ) {
		fprintf( stderr, "error allocating co buffer" );
		exit( 4 );
	}
	si = calloc( N/2, sizeof( double ) );
	if( si == NULL ) {
		fprintf( stderr, "error allocating si buffer" );
		exit( 5 );
	}
	coeff = calloc( N, sizeof( double ) );
	if( coeff == NULL ) {
		fprintf( stderr, "error allocating coeff buffer" );
		exit( 6 );
	}

	// coeff table initialization
	for( j=1; j<N; j++ ) {
		coeff[j]=0.35875-0.48829*cos(2*pi*j/(N-1))+0.14128*cos(4*pi*j/(N-1))-0.01168*cos(6*pi*j/(N-1));
	}

	// sin table initialization
	m = N/2;
	p = 10;
	co1 = cos( pi/m );
	si1 = -sin( pi/m );
	co[0] = 1;
	si[0] = 0;
	for( j=1; j<m; j++ ) {
		co[j] = co1 * co[j-1] - si1 * si[j-1];
		si[j] = si1 * co[j-1] + co1 * si[j-1];
	}

	for( ;; ) {
		flipflop=(flipflop+1)%3;
		if( flipflop==0 ) {
			pthread_cond_wait(payl->con_b, payl->mut_b);
		} else if( flipflop==1 ) {
			pthread_cond_wait(payl->con_a, payl->mut_a);
		} else {
			pthread_cond_wait(payl->con_c, payl->mut_c);
		}

		// Time measurement
		gettimeofday( &(payl->start), NULL );
		
		// Read input data 
		if( flipflop==0 ) {
			for( j=0, k=0; k<N; j+=4,k++ ) {
				fftr[k]=((float *)payl->buftemp)[j]*coeff[k];
				ffti[k]=((float *)payl->buftemp)[j+1]*coeff[k];
			}
		} else if( flipflop==1 ) {
			for( j=0, k=0; k<N; j+=2,k++ ) {
				fftr[k]=((float *)payl->buf)[j]*coeff[k];
				ffti[k]=((float *)payl->buf)[j+1]*coeff[k];
			}
		} else {
			for( j=0, k=0; k<N; j+=4,k++ ) {
				fftr[k]=((float *)payl->bufmed)[j]*coeff[k];
				ffti[k]=((float *)payl->bufmed)[j+1]*coeff[k];
			}
		}

		m = N/2;
		p = 10;
			
		// FFT Compute
		for( j=0,ng=1,md=m; j<p; j++ ) {
			for( ind=0, ig=0; ig<ng; ig++ ) {
				for( i=0, k=0; i<md; i++ ) {
					i1=ind+i;
					i2=i1+md;
					fftdr = fftr[i1] - fftr[i2];
					fftdi = ffti[i1] - ffti[i2];
					fftr[i1] = fftr[i1] + fftr[i2];
					ffti[i1] = ffti[i1] + ffti[i2];
					fftr[i2] = fftdr * co[k] - fftdi * si[k];
					ffti[i2] = fftdr * si[k] + fftdi * co[k];
					k += ng;
				}
				ind += md*2;
			}
			md >>= 1;
			ng <<= 1;
		}

		// Coefficients reordering
		for( j=0; j<N; j++ ) {
			jd=j;
			k=0;
			kp=m;
			for( i=1; i<=p; i++ ) {
				k = k + (jd-(jd/2)*2)*kp;
				jd >>= 1;
				kp >>= 1;
			}
			if( k>j ) {
				fftdr = fftr[j];
				fftdi = ffti[j];
				fftr[j] = fftr[k];
				ffti[j] = ffti[k];
				fftr[k] = fftdr;
				ffti[k] = fftdi;
			}
		}

		// Absolute value
		for( j=0; j<N; j++ ) {
			fftr[j] = fftr[j]/m;
			ffti[j] = ffti[j]/m;
			fftr[j] = sqrt( fftr[j] * fftr[j] + ffti[j] * ffti[j] );
			val= 20*log10(fftr[j]);
			if( val< -200 ) {
				val=-200;
			}
			if( val> 50 ) {
				val=50;
			}
			fftout[j]=200+(char)val;
			// fprintf( stderr, "%04d ", fftout[j] );
		}

		// time measurement
		gettimeofday( &(payl->stop), NULL );
		// fprintf( stderr, "FFT time= %f mSec\n", (payl->stop.tv_sec-payl->start.tv_sec)*1000+0.001*(payl->stop.tv_usec-payl->start.tv_usec) );

		// FFT write out
		if( *payl->outfile >0 ) {
			buf[0]=0x82;
			buf[1]=0x7e;
			if( flipflop==0 ) {
				buf[2]=0x02;
				buf[3]=0x10;
				buf[4]=0x01; //fft type
				buf[5]='B';
				buf[6]='F';
				buf[7]=0x01;
				memcpy( buf+8, (char *)payl->tune, 4 );
				memcpy( buf+12, (char *)payl->att_lowpass_rate, 2 );
				freq = htonl( *payl->freq );
				memcpy( buf+14, &freq, 4 );
				memcpy( buf+20, fftout+512+256, 256 );
				memcpy( buf+20+256, fftout, 256 );
				n=write( *payl->outfile, buf, 512+20 );
			} else if( flipflop==1 ) {
				buf[2]=0x03;
				buf[3]=0x90;
				buf[4]=0x02; //fft type
				buf[5]='H';
				buf[6]='F';
				buf[7]=0x01;
				memcpy( buf+8, (char *)payl->tune, 4 );
				memcpy( buf+12, (char *)payl->att_lowpass_rate, 2 );
				freq = htonl( *payl->freq );
				memcpy( buf+14, &freq, 4 );
				memcpy( buf+20, fftout+512+64, 512-64 );
				memcpy( buf+512+20-64, fftout, 512-64 );
				n=write( *payl->outfile, buf, 1024+20-128 );
			} else {
				buf[2]=0x03;
				buf[3]=0x90;
				buf[4]=0x03; //fft type
				buf[5]='M';
				buf[6]='F';
				buf[7]=0x01;
				memcpy( buf+8, (char *)payl->tune, 4 );
				memcpy( buf+12, (char *)payl->att_lowpass_rate, 2 );
				freq = htonl( *payl->freq );
				memcpy( buf+14, &freq, 4 );
				memcpy( buf+20, fftout+512+64, 512-64 );
				memcpy( buf+512+20-64, fftout, 512-64 );
				n=write( *payl->outfile, buf, 1024+20-128 );
			}
			if( n<0 ) {
				fprintf( stderr, "sent %d bytes %d %s\n", n, errno, strerror(errno) );fflush( stderr );
			}
		}
		usleep( 30000 );
		// fprintf( stderr, "post Round\n" );fflush( stderr );
	}
	return NULL;
}

static char *requestHeader[] = {
	"GET ",
	"Host: ",
	"Sec-WebSocket-Version: ",
	"Sec-WebSocket-Protocol: ",
	"Sec-WebSocket-Key: ",
	"Connection: keep-alive, Upgrade",
	"Upgrade: websocket"
};
static char *keyHeader="Sec-WebSocket-Key: ";

void *tcpManage( void *payload ) {
	int chfd;
	FILE *bchfd;
	char buf[1024];
	char *hp, *pp;
	struct sockaddr_in cliaddr;
	int j, n, k;
	payloadp payl = (payloadp)payload;
	int status;
	char keys[100];
	int isFirst;
	int pid;

	pid = getpid();
	chfd = *payl->outfile;
	fprintf( stderr, "Receiving websocket on %d\n", chfd );
	bchfd=fdopen( chfd, "r+" );
	if( !bchfd ) {
		fprintf( stderr, "Reopen-ing Error\n" );
		exit( 12 );
	}
	hp = inet_ntoa(cliaddr.sin_addr);
	if( !hp ) {
		fprintf( stderr, "inet_ntoa Error\n" );
		exit( 14 );
	} else {
		fprintf( stderr, "Connected by %s\n", hp );
	}
	for( status=0;; ) {
		fprintf( stderr, "%d TCP loop\n", pid );
		memset( buf, 0, sizeof( buf ) );
		pp = fgets( buf, sizeof(buf), bchfd );
		if( !pp && errno )  {
			fprintf( stderr, "%d TCP read Error\n", pid );
			exit( 15 );
		}
		if( !pp ) {
			fprintf( stderr, "%d TCP read 0 bytes \n", pid );
			continue;
		}
		if( buf[strlen(buf)-1]=='\n' ) {
			fprintf( stderr, "%d Received %d bytes %s", pid, (int)strlen( buf ), buf );
			for( j=0; j<XtNumber(requestHeader); j++ ) {
				if( !strncmp( buf, requestHeader[j], strlen(requestHeader[j] ) ) ) {
					status++;
					fprintf( stderr, "Caught %s go to status %d/%ld\n", requestHeader[j], status, XtNumber(requestHeader) );
					if( !strncmp( buf, keyHeader, strlen(keyHeader ) ) ) {
						fprintf( stderr, "Caught key %s\n", buf+strlen(keyHeader) );
						strcpy( keys, buf+strlen(keyHeader) );
						if( keys[strlen(keys)-1]=='\r' || keys[strlen(keys)-1]=='\n'  ){
							keys[strlen(keys)-1]=0;
						}
						if( keys[strlen(keys)-1]=='\r' || keys[strlen(keys)-1]=='\n'  ){
							keys[strlen(keys)-1]=0;
						}
						fprintf( stderr, "Caught key %s\n", keys );
					}
				}
			}
			if( strlen(buf)<=2 ) {
				status++;
				fprintf( stderr, "status advanced %d\n", status );
			}
		}
		if( status >= XtNumber(requestHeader)+1 ) {
			break;
		}
	}
	n=fprintf( bchfd, "HTTP/1.1 101 Switching Protocols\r\n" );
	fprintf( stderr, "sent %d bytes error %d\n", n, errno );
	fprintf( stderr, "HTTP/1.1 101 Switching Protocols\r\n" );
	n=fprintf( bchfd, "Upgrade: websocket\r\n" );
	fprintf( stderr, "sent %d bytes error %d\n", n, errno );
	fprintf( stderr, "Upgrade: websocket\r\n" );
	n=fprintf( bchfd, "Connection: Upgrade\r\n" );
	fprintf( stderr, "sent %d bytes error %d\n", n, errno );
	fprintf( stderr, "Connection: Upgrade\r\n" );
	// strcpy( keys, (const char *)"dGhlIHNhbXBsZSBub25jZQ==" );
	sprintf( buf, "%s%s", keys, "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" ); 
	memset( keys, 0, sizeof( keys ) );
	SHA1( (unsigned const char *)buf, strlen(buf), (unsigned char *)keys );
	pp=g_base64_encode( (const guchar *)keys, strlen(keys) );
	memset( keys, 0, sizeof( keys ) );
	strcpy( keys, (const char *)pp );
	g_free( pp );
	n=fprintf( bchfd, "Sec-WebSocket-Accept: %s\r\n", keys ); 
	fprintf( stderr, "Sec-WebSocket-Accept: %s\r\n", keys ); 
	n=fprintf( bchfd, "Sec-WebSocket-Protocol: chat\r\n\r\n" );
	fprintf( stderr, "Sec-WebSocket-Protocol: chat\r\n\r\n" );
	fprintf( stderr, "sent %d bytes error %d\n", n, errno );
	fflush( bchfd );
	for( isFirst=1;; ) {
		fprintf( stderr, "%d TCP rec cmd loop\n", pid );
		fflush( stderr );
		memset( buf, 0, sizeof( buf ) );
		n = read( chfd, buf, sizeof(buf) );
		if( n==-1  )  {
			fprintf( stderr, "%d TCP read Error %d - %s on %d\n", pid, errno, strerror(errno), chfd );
			exit( 15 );
		}
		if( !n ) {
			fprintf( stderr, "%d TCP read 0 bytes \n", pid );
			continue;
		}
		fprintf( stderr, "%d Received %d bytes : ", pid, n);
		for( j=0; j<n; j++ ) {
			fprintf( stderr, "%02x ", (unsigned char)buf[j] );
		}
		fprintf( stderr, "\n" );
		fflush( stderr );
		// We only manage a final, text, masked, short (less than 126bit) message
		if( (unsigned char)buf[0]==0x81 ) {
			j=buf[1]&0x7F;
			if( j==n-6 ) {
				for( n=0; n<j; ) {
					for( k=0; k<4 && n<j; k++ ) {
						buf[n+6]^=buf[k+2];
						n++;
					}
				}
				if( isFirst==1 ) {
					isFirst++;
					if( strcmp( buf+6, payl->passphrase ) ) {
						fprintf( stderr, "required >%s< differ from >%s<\n", payl->passphrase, buf+6 );
						break;
					}
				} else {
					strcpy( payl->buf, buf+6 );
					fprintf( stderr, "%s\n", buf+6 );
					for( j=0,pp=payl->buf,n=strlen(payl->buf); j<=n; j++ ) {
						if( payl->buf[j]=='\n' ) {
							payl->buf[j]=0;
						}
						if( payl->buf[j]==0 ) {
							if( !strncmp( pp, "MODE=", 5 ) ) {
								if( !strncmp( pp+5, "USB", 3 ) ) {
									*payl->mod=0;
									*payl->bfo=1850;
								} else if( !strncmp( pp+5, "LSB", 3 ) ) {
									*payl->mod=1;
									*payl->bfo=-1850;
								} else if( !strncmp( pp+5, "CW", 2 ) ) {
									*payl->mod=2;
									*payl->bfo=700;
								} else {
									*payl->mod=-1;
									*payl->bfo=0;
								}
								fprintf( stderr, "MODE=%d\n", *payl->mod );
							}
							if( !strncmp( pp, "BW=", 3 ) ) {
								if( !strncmp( pp+3, "NARROW", 6 ) ) {
									*payl->bw=0;
								} else if( !strncmp( pp+3, "MEDIUM", 6 ) ) {
									*payl->bw=1;
								} else if( !strncmp( pp+3, "WIDE", 4 ) ) {
									*payl->bw=2;
								} else {
									*payl->bw=-1;
								}
								fprintf( stderr, "BW=%d\n", *payl->bw );
							}
							if( !strncmp( pp, "FC=", 3 ) ) {
								sscanf( pp+3, "%d", payl->freq );
								fprintf( stderr, "FC=%d\n", *payl->freq );
							}
							pp=payl->buf+j+1;
						}
					}
				}
			} else {
				fprintf( stderr, "wrong message length: is %d instead of %d\n", j, n-6 );
				break;
			}
		} else {
			fprintf( stderr, "wrong message type: %x\n", buf[0] );
			break;
		}
	}
	return( NULL );
}

int main( int ac, char *av[], char **ev ) {

	int res;
	int port;
	int outfile=0;

	static int freq=0;
	static int mod;
	static int bw;
	char *passphrase;
	long tune;
	long bfo;
	short att_lowpass_rate;

	int pfd;
	struct sockaddr_in saddr;
	struct sockaddr_in cliaddr;
	unsigned int clilun;
	int pid;

	payload payl0;
        pthread_t pthread0;
        pthread_attr_t attr0;
	struct sched_param prio0;
	payload payl1;
        pthread_t pthread1;
        pthread_attr_t attr1;
	struct sched_param prio1;
	payload payl2;
        pthread_t pthread2;
        pthread_attr_t attr2;
	struct sched_param prio2;
	payload payl3;
        pthread_t pthread3;
        pthread_attr_t attr3;
	struct sched_param prio3;

	key_t shm_key;
	int shm_id;
        char* shmaddr1;
        char* shmaddr2;
        char* shm_cond1;
        char* shm_cond2;

	pthread_cond_t con_a = PTHREAD_COND_INITIALIZER;
	pthread_cond_t con_b = PTHREAD_COND_INITIALIZER;
	pthread_cond_t con_c = PTHREAD_COND_INITIALIZER;
	pthread_mutex_t mut_a = PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_t mut_b = PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_t mut_c = PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_t mut_ta = PTHREAD_MUTEX_INITIALIZER;
	pthread_mutex_t mut_tb = PTHREAD_MUTEX_INITIALIZER;

	static char buf_a[BUFFERSIZE];
	static char buf_b[BUFFERSIZE];
	static char buf_t[BUFFERSIZE*2];
	static char buf_m[BUFFERSIZE*2];
	static int lung_t;

        fprintf( stderr, "Operations init elad-server version 1.0\n" );

	// Read portnumber from Argument, if not 7777
	if( ac<2 ) {
		port = 7777;
	} else {
		port = atoi( av[1] );
	}
	fprintf( stderr, "portnumber: %d\n", port );

	// Read shared memory from Argument, if not 6166530
	if( ac<3 ) {
		shm_key = 6166530;
	} else {
		shm_key = atol( av[2] );
	}

	if( ac<4 ) {
		passphrase="accattatevillo";
	} else {
		passphrase = av[3];
	}

	// socket: create the socket 
	pfd = socket( AF_INET, SOCK_STREAM, 0 );
	if( pfd == -1 ) {
		fprintf( stderr, "Socket open error\n" );
		exit( 7 );
	}
	int opt = 1;
	setsockopt( pfd, SOL_SOCKET, SO_REUSEADDR, (const void *)&opt , sizeof(int) );

	memset( &saddr, 0, sizeof(saddr) );
	saddr.sin_family = AF_INET;
	saddr.sin_addr.s_addr = htonl( INADDR_ANY );
	saddr.sin_port = htons( port );

	if( bind( pfd, (struct sockaddr *) &saddr, sizeof( saddr ) ) == -1 )  {
		fprintf( stderr, "Binding Error\n" );
		exit( 8 );
	}

	if( listen( pfd, 1 ) == -1 ) {
		fprintf( stderr, "Listening Error\n" );
		exit( 10 );
	}
	fprintf( stderr, "Listening on port %d\n", port );

	clilun = sizeof( cliaddr );

	for( ;; ) {

		// wait for a connection request 
		fprintf( stderr, "accepting\n" );
		outfile = accept( pfd, (struct sockaddr *) &cliaddr, &clilun );
		if( outfile == -1 ) {
			fprintf( stderr, "Accept-ing Error\n" );
			exit( 11 );
		}
		pid = fork( );
		if( pid == -1 ) {
			fprintf( stderr, "Accept-ing Error\n" );
			exit( 11 );
		}
		if( pid > 0 ) {
			fprintf( stderr, "%d child - operate on %d\n", pid, outfile );
			break;
		}
		fprintf( stderr, "Father - continue\n" );
	}

	// create thread that manages websocket creation
	memset( &payl0, 0, sizeof( payl1 ) );
	payl0.outfile=&outfile;
	payl0.buf=buf_t;
	payl0.freq=&freq;
	payl0.mod=&mod;
	payl0.bw=&bw;
	payl0.passphrase=passphrase;
	payl0.tune=&tune;
	payl0.bfo=&bfo;
	payl0.att_lowpass_rate=&att_lowpass_rate;
	prio0.sched_priority=50;
	res=pthread_attr_init( &attr0 );
	if( res ) { fprintf( stderr, "pthread_attr_init attr0 failed\n" ); }
	res=pthread_create( &pthread0, &attr0, tcpManage, (void *)&payl0 );
	if( res ) { fprintf( stderr, "pthread_create pthread 0 failed\n" ); }
	res=pthread_setschedparam( pthread0, SCHED_RR, &prio0 );
	if( res ) { fprintf( stderr, "pthread_setschedparam pthread 0 failed\n" ); }
	fprintf( stderr, "websocket Processing thread a created\n" ); 

	// Allocate and attach a shared memory segment
	shm_id = shmget ( shm_key, (BUFFERSIZE+sizeof( pthread_cond_t )+6)*2, IPC_CREAT | S_IRUSR | S_IWUSR | 0666 );
	if( shm_id==-1 ) {
		fprintf( stderr, "erro allocating shmem %d %s\n", errno, strerror(errno) );fflush( stderr );
	}
	shm_cond1 = (char*) shmat (shm_id, 0, 0);
	shmaddr1 = shm_cond1 + sizeof( pthread_cond_t );
	shm_cond2 = shmaddr1 + BUFFERSIZE + 6;
	shmaddr2 = shm_cond2 + sizeof( pthread_cond_t );
	fprintf( stderr, "Shared %x memory attached at address %p\n", shm_id, shm_cond1 );
	
	memset( &payl3, 0, sizeof( payl3 ) );

	// create thread that receives and manages the data from shared memory
	memset( &payl1, 0, sizeof( payl1 ) );
	payl1.con=(pthread_cond_t *)shm_cond1;
	payl1.mut=&mut_ta;
	payl1.con_a=&con_a;
	payl1.con_b=&con_b;
	payl1.con_c=&con_c;
	payl1.payload=(void *)&payl3;
	payl1.buf=buf_a;
	payl1.buftemp=buf_t;
	payl1.bufmed=buf_m;
	payl1.outfile=&outfile;
	payl1.lung=&lung_t;
	payl1.shmaddr=shmaddr1+6;
	payl1.freq=&freq;
	payl1.mod=&mod;
	payl1.bw=&bw;
	payl1.tune=&tune;
	payl1.bfo=&bfo;
	payl1.att_lowpass_rate=&att_lowpass_rate;
	prio1.sched_priority=50;
	res=pthread_attr_init( &attr1 );
	if( res ) { fprintf( stderr, "pthread_attr_init attr1 failed\n" ); }
	res=pthread_create( &pthread1, &attr1, hfThread, (void *)&payl1 );
	if( res ) { fprintf( stderr, "pthread_create pthread 1 failed\n" ); }
	res=pthread_setschedparam( pthread1, SCHED_RR, &prio1 );
	if( res ) { fprintf( stderr, "pthread_setschedparam pthread 1 failed\n" ); }
	fprintf( stderr, "HF Processing thread a created\n" ); 

	// create thread that sends data to the websocket
	memset( &payl2, 0, sizeof( payl2 ) );
	payl2.con=(pthread_cond_t *)shm_cond2;
	payl2.mut=&mut_tb;
	payl2.con_a=&con_a;
	payl2.con_b=&con_b;
	payl2.con_c=&con_c;
	payl2.buf=buf_b;
	payl2.buftemp=buf_t;
	payl2.bufmed=buf_m;
	payl2.outfile=&outfile;
	payl2.lung=&lung_t;
	payl2.shmaddr=shmaddr2+6;
	payl2.freq=&freq;
	payl2.mod=&mod;
	payl2.bw=&bw;
	payl2.tune=&tune;
	payl2.bfo=&bfo;
	payl2.att_lowpass_rate=&att_lowpass_rate;
	prio2.sched_priority=40;
	res=pthread_attr_init( &attr2 );
	if( res ) { fprintf( stderr, "pthread_attr_init attr2 failed\n" ); }
	res=pthread_create( &pthread2, &attr2, hfThread, (void *)&payl2 );
	if( res ) { fprintf( stderr, "pthread_create pthread 2 failed\n" ); }
	res=pthread_setschedparam( pthread2, SCHED_RR, &prio2 );
	if( res ) { fprintf( stderr, "pthread_setschedparam pthread 2 failed\n" ); }
	fprintf( stderr, "HF Processing thread b created\n" ); 

	// create thread that sends FFT to the websocket
	payl3.con_a=&con_a;
	payl3.mut_a=&mut_a;
	payl3.con_b=&con_b;
	payl3.mut_b=&mut_b;
	payl3.con_c=&con_c;
	payl3.mut_c=&mut_c;
	payl3.buftemp=buf_t;
	payl3.bufmed=buf_m;
	payl3.buf=shmaddr1+6;
	payl3.outfile=&outfile;
	payl3.freq=&freq;
	payl3.mod=&mod;
	payl3.bw=&bw;
	payl3.tune=&tune;
	payl3.att_lowpass_rate=&att_lowpass_rate;
	prio3.sched_priority=80;
	res=pthread_attr_init( &attr3 );
	if( res ) { fprintf( stderr, "pthread_attr_init attr3 failed\n" ); }
	res=pthread_create( &pthread3, &attr3, FFTSend, (void *)&payl3 );
	if( res ) { fprintf( stderr, "pthread_create pthread 3 failed\n" ); }
	res=pthread_setschedparam( pthread3, SCHED_RR, &prio3 );
	if( res ) { fprintf( stderr, "pthread_setschedparam pthread 3 failed\n" ); }
	fprintf( stderr, "Send FFT thread created\n" ); 

	// main loop 
	for( ;; ) {
		sleep( 10 );
	}

	// dirty exit
	fprintf( stderr, "Operations end\n" );

	return 0;
}