/*
	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/>.
*/

#include <stdio.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <pthread.h>
#include <libusb-1.0/libusb.h>
#include <netdb.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>


#define S_RATE 122880000
#define S_RATE_S1 61440000


typedef struct _cbdata_t {
	int obj;
	struct libusb_transfer *transfer;
	float *outbuf;
	long *freq;
	int *atten;
	int *filter;
	char *sampling;
	int sockfd;
	socklen_t salen;
	struct sockaddr *sa;
	int *bytes_per_sample;
	int rescale;
} cbdata_t, *cbdata_p;

typedef struct _threaddata_t {
	long *freq;
	int *atten;
	int *filter;
	char *sampling;
	int port;
	int *bytes_per_sample;
	int rescale;
} threaddata_t, *threaddata_p;

libusb_device_handle *dev_handle=NULL; 
static int isDuo = 0;
static int isS1 = 0;
static int isS2 = 0;
static float globalOffset, lpOffset, attOffset;
static float recalc, rescale;

void cb_in( struct libusb_transfer * transfer ) {
	cbdata_p cbd;
	int res;
	int j, k, y;
	float c;
	short rs;
	int ri;
	short filler;
	// struct timeval start;
	// struct timeval stop;
	static char buffer[1034];
	static long counter=0;
	struct libusb_transfer * transfer_in;
	cbd = (cbdata_p)transfer->user_data;
	// gettimeofday( &(start), NULL );
	switch(transfer->status) {
		case LIBUSB_TRANSFER_COMPLETED:
			// write( 1, transfer->buffer, transfer->actual_length );
                        if(*cbd->bytes_per_sample==2) {
                                c=recalc/32.0/1024.0;
                                for( j=0; j<transfer->actual_length/sizeof(short); j++ ) {
                                        rs=(short)((uint16_t *)(transfer->buffer))[j];
                                        cbd->outbuf[j]=rs*c;
                                }
                        } else {
                                c=recalc/2.0/1024.0/1024.0/1024.0;
                                for( j=0; j<transfer->actual_length/sizeof(int); j++ ) {
                                        ri=(int)((uint32_t *)(transfer->buffer))[j];
                                        cbd->outbuf[j]=ri*c;
                                }
                        }
			// write( 1, cbd->outbuf, j*sizeof(float) );
			// write msg to remote system
			y=j*sizeof(float);
			for( k=0; k<y; k+=1024 ) {
				counter++;
				*(long *)buffer = htonl( counter );
				*(long *)(buffer+4) = htonl( *cbd->freq );
				filler=(*cbd->atten) |  (*cbd->filter)<<1 | (*cbd->sampling)<<4;
				*(short *)(buffer+8) = htons( filler );
				memcpy( buffer+10, ((char *)(cbd->outbuf))+k, y-k<1024?y-k:1024 );
				res = sendto( cbd->sockfd, buffer, y-k<1024?y-k+4:1034, 0, cbd->sa, cbd->salen );
				if( res < 0 ) {
					fprintf( stderr, "CB UDP error (%d)\n", res  );
				}
			}
			break;
		case LIBUSB_TRANSFER_CANCELLED:
			fprintf( stderr, "CB result: Cancelled %d\n", cbd->obj );
			break;
		case LIBUSB_TRANSFER_NO_DEVICE:
			fprintf( stderr, "CB result: Nodevice %d\n", cbd->obj );
			break;
		case LIBUSB_TRANSFER_TIMED_OUT:
			fprintf( stderr, "CB result: Timedout %d\n", cbd->obj );
			break;
		case LIBUSB_TRANSFER_ERROR:
			fprintf( stderr, "CB result: Error %d\n", cbd->obj );
			break;
		case LIBUSB_TRANSFER_STALL:
			fprintf( stderr, "CB result: Stalled %d\n", cbd->obj );
			break;
		case LIBUSB_TRANSFER_OVERFLOW:
			fprintf( stderr, "CB result: Overflowed %d\n", cbd->obj );
			break;
	}
	fflush( stderr );
	transfer_in = cbd->transfer;
	res = libusb_submit_transfer( transfer_in );
	if( res ) {
		fprintf( stderr, "libusb_submit_transfer failed (%d)\n", res ); 
	}
	// gettimeofday( &(stop), NULL );
	// fprintf( stderr, "hf;%f;time= %f mSec\n",start.tv_sec*1000+0.001*start.tv_usec,(stop.tv_sec-start.tv_sec)*1000+0.001*(stop.tv_usec-start.tv_usec) );
}

void set_FREQ( long LOfreq ) {
	int res;
	unsigned char buffer[1024];
	int j;
	double tuningFreq;
	int sampleRateCorr;
	unsigned int tuningWordHex, twLS, twMS;

	// read global offset
	memset( buffer, 0, sizeof( buffer ) );
	res = libusb_control_transfer( dev_handle, 0xc0, 0xA2, 0x4028, 0x0151, buffer, 4, 0 );
	if( res==4  ){
		memcpy( &globalOffset, buffer, 4 );
		fprintf( stderr, "Global Offset: %f\n", globalOffset ); 
	} else {
		sampleRateCorr = 0;
		fprintf( stderr, "Global Offset failed (%d)\n", res ); 
		return;
	}

	// read lp offset
	memset( buffer, 0, sizeof( buffer ) );
	res = libusb_control_transfer( dev_handle, 0xc0, 0xA2, 0x402c, 0x0151, buffer, 4, 0 );
	if( res==4  ){
		memcpy( &lpOffset, buffer, 4 );
		fprintf( stderr, "LP Offset: %f\n", lpOffset ); 
	} else {
		sampleRateCorr = 0;
		fprintf( stderr, "LP Offset failed (%d)\n", res ); 
		return;
	}

	// read att offset
	memset( buffer, 0, sizeof( buffer ) );
	res = libusb_control_transfer( dev_handle, 0xc0, 0xA2, 0x4030, 0x0151, buffer, 4, 0 );
	if( res==4  ){
		memcpy( &attOffset, buffer, 4 );
		fprintf( stderr, "ATT Offset: %f\n", attOffset ); 
	} else {
		sampleRateCorr = 0;
		fprintf( stderr, "ATT Offset failed (%d)\n", res ); 
		return;
	}

	// read sample rate correction
	memset( buffer, 0, sizeof( buffer ) );
	res = libusb_control_transfer( dev_handle, 0xc0, 0xA2, 0x4024, 0x0151, buffer, 4, 0 );
	if( res==4  ){
		memcpy( &sampleRateCorr, buffer, 4 );
		fprintf( stderr, "Sample Rate Correction: %d\n", sampleRateCorr ); 
	} else {
		sampleRateCorr = 0;
		fprintf( stderr, "Sample rare correction failed (%d)\n", res ); 
		return;
	}

	if( isDuo ) {
		// set tuning frequency into FPGA
		memset( buffer, 0, sizeof( buffer ) );
		tuningFreq = LOfreq - (floor(LOfreq/(S_RATE+sampleRateCorr)) * (S_RATE+sampleRateCorr));
		tuningWordHex = (unsigned int)((4294967296.0*tuningFreq)/(S_RATE+sampleRateCorr));
		fprintf( stderr, "Tuning TuningHEX=%08X ", tuningWordHex );
		twLS = tuningWordHex&0x0000FFFF;
		tuningWordHex>>=16;
		twMS = 0xF2;
		twMS <<= 8;
		twMS |= (tuningWordHex&0x000000FF);
		tuningWordHex>>=8;
		buffer[0] = tuningWordHex&0x000000FF;
		buffer[1] = 0;
		// fprintf( stderr, "twMS=%04X twLS=%04X Buffer[0]=%02X Buffer[1]=%02X\n", twMS, twLS, buffer[0], buffer[1] );
		res = libusb_control_transfer( dev_handle, 0x40, 0xE1, twLS, twMS, buffer, 2, 0 );
		if( res == 2 ) {
			// printf( "Xilinx frequency set\n" );
		} else {
			fprintf( stderr, "Xilinx Frequency set failed\n" ); 
			return;
		}

		// verify that CAT buffer is free from previous commands
		memset( buffer, 0, sizeof( buffer ) );
		for( j=0; ; j++ ) {
			res = libusb_control_transfer( dev_handle, 0xc0, 0xE1, 0x00, 0x0FC<<8, buffer, 3, 0 );
			if( res != 3 || ((buffer[2]&0x04)!=0x04) || j==200 ) {
				break;
			}
			usleep( 10000 );
		} 
		if( res != 3 ) {
			fprintf( stderr, "CAT buffer waiting error\n" ); 
			return;
		}
		if ((j==200)&&(buffer[2]&0x04)==0x04) {
			fprintf( stderr, "CAT buffer waiting timeout\n" ); 
			return;
		}
		// fprintf( stderr, "CAT buffer waiting ended (%d)\n", j ); 

		// set tuning frequency to ibe delivered to CAT
		memset( buffer, 0, sizeof( buffer ) );
		sprintf( (char *)buffer, "CF%11ld;", LOfreq );
		res = libusb_control_transfer( dev_handle, 0x40, 0xE1, 16, 0xF1<<8, buffer, 16, 0 );
		// printf( "End setting cat frequency\n" );
		printf( "Duo set freq %ld\n", LOfreq );
	} else {
		// set tuning frequency into FPGA
		memset( buffer, 0, sizeof( buffer ) );
		tuningFreq = LOfreq - (floor(LOfreq/((isS1?S_RATE_S1:S_RATE)+sampleRateCorr)) * ((isS1?S_RATE_S1:S_RATE)+sampleRateCorr));
		tuningWordHex = (unsigned int)((4294967296.0*tuningFreq)/((isS1?S_RATE_S1:S_RATE)+sampleRateCorr));
		// fprintf( stderr, "Tuning TuningHEX=%08X ", tuningWordHex );
		twLS = tuningWordHex&0x0000FFFF;
		tuningWordHex>>=16;
		twMS = tuningWordHex&0x0000FFFF;
		buffer[0] = tuningWordHex&0x000000FF;
		buffer[1] = 0;
		res = libusb_control_transfer( dev_handle, 0x40, 0xF2, twLS, twMS, buffer, 2, 0 );
		if( res == 2 ) {
			// printf( "Xilinx frequency set\n" );
		} else {
			fprintf( stderr, "Xilinx Frequency set failed\n" ); 
		}
		printf( "S1/S2 set freq %ld\n", LOfreq );
	}
}

void set_filter( long freq, int filter, int atten ) {
	int j;
	int res;
	unsigned char buffer[1024];
	unsigned int fl;
	if( isS1 ) {
		res = libusb_control_transfer( dev_handle, 0xc0, 0xF7, (unsigned int)filter, 0x0002, buffer, 1, 0 );
		if( res != 1 || buffer[0] != 0xF7 ) {
			fprintf( stderr, "Set filters failed (%d %02X)\n", res, buffer[0] ); 
		}
		fprintf( stderr, "S1 Filters set (%d)\n", filter );
		res = libusb_control_transfer( dev_handle, 0xc0, 0xF7, (unsigned int)atten, 0x0003, buffer, 1, 0 );
		if( res != 1 || buffer[0] != 0xF7 ) {
			fprintf( stderr, "Set attenuator failed (%d %02X)\n", res, buffer[0] ); 
		}
		fprintf( stderr, "S1 Attenuation set (%d)\n", atten );
	}
	if( isS2 ) {
		if( freq < 61440000 ) {
			fl=0x03;
		} else if( freq < 122880000 ) {
			fl=0x02;
		} else {
			fl=0x04;
		}
		if( !filter ) {
			fl = 0x01;
		}
		if( atten ) {
			fl |=0x08;
		}
		res = libusb_control_transfer( dev_handle, 0xc0, 0xF7, fl, 0x0002, buffer, 1, 0 );
		if( res != 1 || buffer[0] != 0xF7 ) {
			fprintf( stderr, "Set filters failed (%d %02X)\n", res, buffer[0] ); 
		}
		fprintf( stderr, "S2 Filters set (%d)\n", filter );
	}
	if( isDuo ) {
		// verify that CAT buffer is free from previous commands
		memset( buffer, 0, sizeof( buffer ) );
		for( j=0; ; j++ ) {
			res = libusb_control_transfer( dev_handle, 0xc0, 0xE1, 0x00, 0x0FC<<8, buffer, 3, 0 );
			if( res != 3 || ((buffer[2]&0x04)!=0x04) || j==200 ) {
				break;
			}
			usleep( 10000 );
		} 
		if( res != 3 ) {
			fprintf( stderr, "CAT buffer waiting error\n" ); 
			return;
		}
		if ((j==200)&&(buffer[2]&0x04)==0x04) {
			fprintf( stderr, "CAT buffer waiting timeout\n" ); 
			return;
		}
		// fprintf( stderr, "CAT buffer waiting ended (%d)\n", j ); 

		// set filter to be delivered to CAT
		memset( buffer, 0, sizeof( buffer ) );
		sprintf( (char *)buffer, "LP%1d;", filter );
		res = libusb_control_transfer( dev_handle, 0x40, 0xE1, 16, 0xF1<<8, buffer, 16, 0 );
		fprintf( stderr, "DUO LPF set (%d - %s -> %d)\n", filter, buffer, res );

		// set atten to be delivered to CAT
		memset( buffer, 0, sizeof( buffer ) );
		sprintf( (char *)buffer, "AT%1d;", atten );
		res = libusb_control_transfer( dev_handle, 0x40, 0xE1, 16, 0xF1<<8, buffer, 16, 0 );
		fprintf( stderr, "DUO Attenuation set (%d - %s -> %d)\n", atten, buffer, res );
	}
	recalc = pow( 10.0,((isDuo?21.4:0)+rescale+globalOffset+(filter?lpOffset:0)+(atten?attOffset:0))/20.0);
}

char *manageMessage( char * message, int len, threaddata_p payload ) {
	static char retbuf[1024];
	fprintf( stderr, "message %s (%d)\n", message, len );
	if( len>=3 && !strncmp( message, "AT", 2 ) ) {
		if( message[2] != '0' ) {
			*payload->atten = 1;
		} else {
			*payload->atten = 0;
		}
		set_filter( *payload->freq, *payload->filter, *payload->atten );
		sprintf( retbuf, "AT%1d;", *payload->atten );
	} else if( len>=3 && !strncmp( message, "LP", 2 ) ) {
		if( message[2] != '0' ) {
			*payload->filter = 1;
		} else {
			*payload->filter = 0;
		}
		set_filter( *payload->freq, *payload->filter, *payload->atten );
		sprintf( retbuf, "LP%1d;", *payload->filter );
	} else if( len>=4 && !strncmp( message, "FC", 2 ) ) {
		*payload->freq=atol( message+2 );
		fprintf( stderr, "request %ld Hz frequency set\n", *payload->freq );
		set_FREQ( *payload->freq );
		sprintf( retbuf, "CF%011ld;", *payload->freq );
	} else if( message[0]=='F' ) {
		*payload->freq=atol( message+1 );
		fprintf( stderr, "request %ld Hz frequency set\n", *payload->freq );
		set_FREQ( *payload->freq );
		sprintf( retbuf, "CF%011ld;", *payload->freq );
	} else if( message[0]=='A' ) {
		*payload->atten=*payload->filter=0;
		if( message[1] != '0' ) {
			*payload->atten = 1;
		} else {
			*payload->atten = 0;
		}
		if( message[2] != '0' ) {
			*payload->filter = 1;
		} else {
			*payload->filter = 0;
		}
		set_filter( *payload->freq, *payload->filter, *payload->atten );
		sprintf( retbuf, "AT%1d;LP%1d;", *payload->atten, *payload->filter );
	}
	return( retbuf );
}

void *tcpManage( void *payload ) {
	int pfd;
	struct sockaddr_in saddr;
	int chfd;
	struct hostent *host;
	struct sockaddr_in cliaddr;
	unsigned int clilun;
	char buf[1024];
	char *hp;
	int j, n;
	threaddata_p payl =(threaddata_p)payload;
	int port = payl->port;
	int loop;
	char *retbuf;

	// socket: create the socket 
	pfd = socket( AF_INET, SOCK_STREAM, 0 );
	if( pfd == -1 ) {
		fprintf( stderr, "Socket open error\n" );
		exit( 1 );
	}
	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( (unsigned short)port );

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

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

	clilun = sizeof( cliaddr );

	for( ;; ) {

		// wait for a connection request 
		chfd = accept( pfd, (struct sockaddr *) &cliaddr, &clilun );
		if( chfd == -1 ) {
			fprintf( stderr, "Accept-ing Error\n" );
			exit( 4 );
		}

		// determine the message  sender
		host = gethostbyaddr( (const char *)&cliaddr.sin_addr.s_addr, sizeof(cliaddr.sin_addr.s_addr), AF_INET );
		if( !host ) {
			fprintf( stderr, "GetHostByName Error\n" );
			exit( 5 );
		}
		hp = inet_ntoa(cliaddr.sin_addr);
		if( !hp ) {
			fprintf( stderr, "inet_ntoa Error\n" );
			exit( 6 );
		} else {
			fprintf( stderr, "Connected by %s\n", hp );
		}
    
		for( loop=0;; ) {
			fprintf( stderr, "TCP loop\n" );
			memset( buf, 0, sizeof( buf ) );
			for( j=0, n=0; j<sizeof( buf )-1 ; j+=n ) {
				n = read( chfd, buf+j, 1 );
				if( n == -1 )  {
					fprintf( stderr, "TCP read Error\n" );
					break;
				}
				if( n == 0 ) {
					fprintf( stderr, "TCP read 0 bytes\n" );
					loop = 1;
					break;
				}
				if( buf[j]=='\n' ) {
					n = write( chfd, "\n", 1 );
					if( n == -1 ) {
						fprintf( stderr, "TCP write Error\n" );
						exit( 8 );
					}
					fprintf( stderr, "TCP write closure\n" );
					break;
				}
				if( buf[j]==';' ) {
					fprintf( stderr, "TCP message received: %s\n", buf );
					retbuf = manageMessage( buf, j+1, payl );
					fprintf( stderr, "TCP message answer: %s\n", retbuf );
					n = write( chfd, retbuf, strlen( retbuf ) );
					if( n == -1 ) {
						fprintf( stderr, "TCP write Error\n" );
						exit( 8 );
					}
					break;
				}
			}
			if( loop > 0 ) {
				break;
			}
			if( n == -1 ) {
				break;
			}
		}
		n = close( chfd );
		if( n == -1 ) {
			fprintf( stderr, "TCP close Error\n" );
			exit( 9 );
		}
		fprintf( stderr, "TCP closed\n" );
	}
}

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

        libusb_context *ctx;
        libusb_device **devs;
        struct libusb_device_descriptor desc;
        int cnt, vid, pid;
        int j,k;

	int res;
	unsigned char buffer[1024];

	unsigned char pBuffer1[512*24];
	unsigned char pBuffer2[512*24];
	unsigned char pBuffer3[512*24];
	unsigned char pBuffer4[512*24];
	struct libusb_transfer *transfer_in1;
	struct libusb_transfer *transfer_in2;
	struct libusb_transfer *transfer_in3;
	struct libusb_transfer *transfer_in4;
	float pBuffer1a[512*12];
	float pBuffer2a[512*12];
	float pBuffer3a[512*12];
	float pBuffer4a[512*12];

	cbdata_t cbdata1;
	cbdata_t cbdata2;
	cbdata_t cbdata3;
	cbdata_t cbdata4;

	threaddata_t threaddata0;
	pthread_t pthread0;
	pthread_attr_t attr0;

	long LOfreq;
	static int atten;
	static int filter;
	static char sampling;
	long resampling;
	static int bytes_per_sample=4;

	static int sockfd;
	static socklen_t salen;
	static struct sockaddr *sa;


	struct addrinfo hints, *rlist, *rlistsave;
	char *host;
	int port;
	char *ports;

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

	// Read Central Frequency From Argument, if not 14200000
	if( ac<3 ) {
		LOfreq = 14200000;
	} else {
		sscanf( av[2], "%ld", &LOfreq );
	}
	fprintf( stderr, "Central Frequency: %10ld\n", LOfreq );

	// Read Filter + atten From Argument, if not 1/0
	if( ac<4 ) {
		filter = 1;
		atten = 0;
	} else {
		filter=av[3][0]-'0';
		atten=av[3][1]-'0';
	}
	fprintf( stderr, "filter: %d atten: %d\n", filter, atten );

	// Read samplerate From Argument, if not 192000
	if( ac<5 ) {
		resampling = 192000;
	} else {
		sscanf( av[4], "%ld", &resampling );
	}
	fprintf( stderr, "resampling: %ld\n", resampling );

	// Read host from Argument, if not localhost
	if( ac<6 ) {
		host = "localhost";
	} else {
		host = av[5];
	}
	fprintf( stderr, "host: %s\n", host );

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

	switch(resampling) {
		case 6144000:
			bytes_per_sample=2;
			rescale=-0.7;
			sampling=6;
			break;
		case 3072000:
			bytes_per_sample=4;
			rescale=5.4;
			sampling=5;
			break;
		case 1536000:
			bytes_per_sample=4;
			rescale=5.4;
			sampling=4;
			break;
		case 768000:
			bytes_per_sample=4;
			rescale=6;
			sampling=3;
			break;
		case 384000:
			bytes_per_sample=4;
			rescale=6;
			sampling=2;
			break;
		case 192000:
			bytes_per_sample=4;
			rescale=0;
			sampling=1;
			break;
		default:
			bytes_per_sample=4;
			rescale=0;
			sampling=1;
			break;
	}

	threaddata0.port = port;
	threaddata0.freq = &LOfreq;
	threaddata0.sampling = &sampling;
	threaddata0.atten = &atten;
	threaddata0.filter = &filter;
	threaddata0.bytes_per_sample = &bytes_per_sample;
	threaddata0.rescale = rescale;

	// Initialize libusb-1.0
	res = libusb_init(&ctx);
	if( res < 0 ){
		fprintf( stderr, "Init Error %d\n", res );
		return 1;
	}

	// set verbosity level to 3, as suggested in the documentation
	libusb_set_debug( ctx, 3 );

	cnt = libusb_get_device_list(ctx, &devs);
	if(cnt < 0) {
		fprintf( stderr, "Get Device Error\n" ); 
		return 3;
	}
	for( j=0,k=0; j<cnt;j++ ) {
        	res = libusb_get_device_descriptor( devs[j], &desc );
		if( res != 0 ){
			fprintf( stderr, "Get Device Descriptor %d %d\n", j, res ); 
			return 4;
		}
		vid=desc.idVendor;
		pid=desc.idProduct;
		fprintf( stderr, "%d vid %04x pid %04x %d\n", j, vid, pid, k );
		if( vid==0x1721 ) {
			res = libusb_open( devs[j], &dev_handle );
			if( res ) {
				fprintf( stderr, "Cannot open FDM Device\n" ); 
				return 6;
			} else {
				fprintf( stderr, "FDM Device Opened\n" ); 
			}

			// detach kernel drivers
			if( libusb_kernel_driver_active( dev_handle, 0 ) == 1 ) {
				fprintf( stderr, "Kernel Driver Active\n" ); 
				if( libusb_detach_kernel_driver( dev_handle, 0 ) == 0 ) {
					fprintf( stderr, "Kernel Driver Detached\n" ); 
				}
			}

			// claim interface
			if( libusb_claim_interface( dev_handle, 0 ) < 0 ) {
				fprintf( stderr, "Cannot claim Interface\n" ); 
				return 7;
			}
			fprintf( stderr, "Interface claimed\n" ); 

			// read USB driver version
			memset( buffer, 0, sizeof( buffer ) );
			res = libusb_control_transfer( dev_handle, 0xc0, 0xFF, 0x0000, 0x0000, buffer, 2, 0 );
			if( res  ){
				fprintf( stderr, "USB Driver Version: %d.%d\n", buffer[0], buffer[1] );
			} else {
				fprintf( stderr, "USB Driver Version read failed\n" );
			}

			// read HW version
			memset( buffer, 0, sizeof( buffer ) );
			res = libusb_control_transfer( dev_handle, 0xc0, 0xA2, 0x404C, 0x0151, buffer, 2, 0 );
			if( res==2  ){
				fprintf( stderr, "HW Version: %d.%d\n", buffer[0], buffer[1] );
			} else {
				fprintf( stderr, "HW Version read failed\n" );
			}

			// read serial number
			memset( buffer, 0, sizeof( buffer ) );
			res = libusb_control_transfer( dev_handle, 0xc0, 0xA2, 0x4000, 0x0151, buffer, 32, 0 );
			if( res==32  ){
				if( pid==0x061a ) {
					fprintf( stderr, "FDM DUO " ); 
					isDuo=1;
					isS1=0;
					isS2=0;
				}
				if( pid==0x061c ) {
					fprintf( stderr, "FDM S2 " ); 
					isDuo=0;
					isS1=0;
					isS2=1;
				}
				if( pid==0x0610 ) {
					fprintf( stderr, "FDM S1 " ); 
					isDuo=0;
					isS1=1;
					isS2=0;
				}
				fprintf( stderr, "Serial: %-s\n", buffer ); 
			} else {
				fprintf( stderr, "Serial read failed (%d)\n", res ); 
				return 10;
			}
			if( ac<2 || !strcmp( (const char *)buffer, (const char *)av[1] ) || av[1][0]=='+' ) {
				fprintf( stderr, "Device caught\n" ); 
				libusb_free_device_list( devs, 1 );
				fprintf( stderr, "libusb devices list freed\n" ); 
				break;
			} else {
				fprintf( stderr, "Get >%s<  Need >%s<\n", buffer, av[1] );
				libusb_close( dev_handle );
				dev_handle = NULL;
			}			
		}
		k++;
	}
	if( j==cnt ) {
		fprintf( stderr, "Cannot find FDM Device\n" ); 
		return 5;
	}
	if( !dev_handle ) {
		res = libusb_open( devs[j], &dev_handle );
		if( res ) {
			fprintf( stderr, "Cannot open FDM Device\n" ); 
			return 6;
		} else {
			fprintf( stderr, "FDM Device Opened\n" ); 
		}

		libusb_free_device_list( devs, 1 );
		fprintf( stderr, "libusb devices list freed\n" ); 

		// detach kernel drivers
		if( libusb_kernel_driver_active( dev_handle, 0 ) == 1 ) {
			fprintf( stderr, "Kernel Driver Active\n" ); 
			if( libusb_detach_kernel_driver( dev_handle, 0 ) == 0 ) {
				fprintf( stderr, "Kernel Driver Detached\n" ); 
			}
		}

		// claim interface
		if( libusb_claim_interface( dev_handle, 0 ) < 0 ) {
			fprintf( stderr, "Cannot claim Interface\n" ); 
			return 7;
		}
		fprintf( stderr, "Interface claimed\n" ); 
	}

	if( isDuo ) {

		// stop FIFO
		memset( buffer, 0, sizeof( buffer ) );
		res = libusb_control_transfer( dev_handle, 0xc0, 0xE1, 0x0000, 0xE9<<8, buffer, 1, 0 );
		// res = control_transfer( localDeviceInfo.winUsbHandle, 0xc0, 0xE1, 0x0000, 0x00E9<<8, buffer, 1, 0 );
		if( res==1  ){
			fprintf( stderr, "Stop FIFO\n" );
		} else {
			fprintf( stderr, "Stop FIFO failed (%d)\n", res );
		}

		// imposta la FIFO di EP6 del CY-USB in modalità slave...
		memset( buffer, 0, sizeof( buffer ) );
		res = libusb_control_transfer( dev_handle, 0xc0, 0xE1, 0x0000, 0xE8<<8, buffer, 1, 0 );
		if( res==1  ){
			fprintf( stderr, "Init FIFO\n" );
		} else {
			fprintf( stderr, "Init FIFO failed (%d)\n", res );
		}

		// set tuning frequency 
		sprintf( (char *)buffer, ";F%ld", LOfreq );
		manageMessage( (char *)buffer, strlen( (char *)buffer ), &threaddata0 );
		sprintf( (char *)buffer, ";A%1d%1d", filter, atten );
		manageMessage( (char *)buffer, strlen( (char *)buffer ), &threaddata0 );

		// start FIFO
		memset( buffer, 0, sizeof( buffer ) );
		res = libusb_control_transfer( dev_handle, 0xc0, 0xE1, 0x0001, 0x0E9<<8, buffer, 1, 0 );
		if( res != 1 || buffer[0] != 0xE9 ) {
			fprintf( stderr, "Enable SYN failed (%d %02X)\n", res, buffer[0] );
		}
		fprintf( stderr, "SYN Enabled\n" );
	} else {

		// set tuning frequency 
		sprintf( (char *)buffer, "F%ld\n", LOfreq );
		manageMessage( (char *)buffer, strlen( (char *)buffer ), &threaddata0 );
		sprintf( (char *)buffer, "A%1d%1d\n", filter, atten );
		manageMessage( (char *)buffer, strlen( (char *)buffer ), &threaddata0 );

		// start FIFO
		memset( buffer, 0, sizeof( buffer ) );
		res = libusb_control_transfer( dev_handle, 0xc0, 0xE9, 0x0001, 0x0000, buffer, 1, 0 );
		if( res != 1 || buffer[0] != 0xE9 ) {
			fprintf( stderr, "Enable SYN failed (%d %02X)\n", res, buffer[0] ); 
			return 13;
		}
		fprintf( stderr, "SYN Enabled\n" );
	}

	// initilize addrinfo structure
	memset( &hints, 0, sizeof(struct addrinfo) );
	hints.ai_family=AF_UNSPEC;
	hints.ai_socktype=SOCK_DGRAM;
	hints.ai_protocol=IPPROTO_UDP;

	res = getaddrinfo( host, ports, &hints, &rlist );
	if( res == -1 ){
		fprintf( stderr, "udp error for %s, %s: %s", host, ports, gai_strerror( res ) );
		return 19;
	}

	rlistsave = rlist;
	for( j=0; ; rlist=rlist->ai_next ) {
		sockfd=socket( rlist->ai_family, rlist->ai_socktype, rlist->ai_protocol );
		if( sockfd>= 0 ){
			break; 
		}
	}

	sa=malloc( rlist->ai_addrlen );
	memcpy(sa, rlist->ai_addr, rlist->ai_addrlen );
	salen=rlist->ai_addrlen;
	freeaddrinfo( rlistsave );
	fprintf( stderr, "UDP out prepared\n" );

	// prepare async transfer
	transfer_in1 = libusb_alloc_transfer(0);
	if( !transfer_in1 ) {
		fprintf( stderr, "libusb_alloc_transfer failed\n" ); 
		return 14;
	}
	transfer_in2 = libusb_alloc_transfer(0);
	if( !transfer_in2 ) {
		fprintf( stderr, "libusb_alloc_transfer failed\n" ); 
		return 15;
	}
	transfer_in3 = libusb_alloc_transfer(0);
	if( !transfer_in3 ) {
		fprintf( stderr, "libusb_alloc_transfer failed\n" ); 
		return 16;
	}
	transfer_in4 = libusb_alloc_transfer(0);
	if( !transfer_in4 ) {
		fprintf( stderr, "libusb_alloc_transfer failed\n" ); 
		return 17;
	}
	fprintf( stderr, "libusb_alloc_transfer succedeed\n" ); 

	// create thread that reads commands from tcp
	pthread_attr_init( &attr0 );
	pthread_create( &pthread0, &attr0, tcpManage, (void *)&threaddata0 );
	fprintf( stderr, "Tcp manage thread created\n" ); 

	cbdata1.obj=0;
	cbdata1.transfer=transfer_in1;
	cbdata1.outbuf=pBuffer1a;
	cbdata1.freq = &LOfreq;
	cbdata1.sampling = &sampling;
	cbdata1.atten = &atten;
	cbdata1.filter = &filter;
	cbdata1.sockfd=sockfd;
	cbdata1.salen=salen;
	cbdata1.sa=sa;
	cbdata1.bytes_per_sample = &bytes_per_sample;
	cbdata1.rescale = rescale;
	cbdata2.obj=1;
	cbdata2.transfer=transfer_in2;
	cbdata2.outbuf=pBuffer2a;
	cbdata2.freq = &LOfreq;
	cbdata2.sampling = &sampling;
	cbdata2.atten = &atten;
	cbdata2.filter = &filter;
	cbdata2.sockfd=sockfd;
	cbdata2.salen=salen;
	cbdata2.sa=sa;
	cbdata2.bytes_per_sample = &bytes_per_sample;
	cbdata2.rescale = rescale;
	cbdata3.obj=2;
	cbdata3.transfer=transfer_in3;
	cbdata3.outbuf=pBuffer3a;
	cbdata3.freq = &LOfreq;
	cbdata3.sampling = &sampling;
	cbdata3.atten = &atten;
	cbdata3.filter = &filter;
	cbdata3.sockfd=sockfd;
	cbdata3.salen=salen;
	cbdata3.sa=sa;
	cbdata3.bytes_per_sample = &bytes_per_sample;
	cbdata3.rescale = rescale;
	cbdata4.obj=3;
	cbdata4.transfer=transfer_in4;
	cbdata4.outbuf=pBuffer4a;
	cbdata4.freq = &LOfreq;
	cbdata4.sampling = &sampling;
	cbdata4.atten = &atten;
	cbdata4.filter = &filter;
	cbdata4.sockfd=sockfd;
	cbdata4.salen=salen;
	cbdata4.sa=sa;
	cbdata4.bytes_per_sample = &bytes_per_sample;
	cbdata4.rescale = rescale;

	libusb_fill_bulk_transfer( transfer_in1, dev_handle, 0x86, pBuffer1, 512*24, cb_in, &cbdata1, 2000);
	libusb_fill_bulk_transfer( transfer_in2, dev_handle, 0x86, pBuffer2, 512*24, cb_in, &cbdata2, 2000);
	libusb_fill_bulk_transfer( transfer_in3, dev_handle, 0x86, pBuffer3, 512*24, cb_in, &cbdata3, 2000);
	libusb_fill_bulk_transfer( transfer_in4, dev_handle, 0x86, pBuffer4, 512*24, cb_in, &cbdata4, 2000);
 
	res = libusb_submit_transfer( transfer_in1 );
	if( res ) {
		fprintf( stderr, "libusb_submit_transfer failed (%d)\n", res ); 
		return 18;
	}
	res = libusb_submit_transfer( transfer_in2 );
	if( res ) {
		fprintf( stderr, "libusb_submit_transfer failed (%d)\n", res ); 
		return 19;
	}
	res = libusb_submit_transfer( transfer_in3 );
	if( res ) {
		fprintf( stderr, "libusb_submit_transfer failed (%d)\n", res ); 
		return 20;
	}
	res = libusb_submit_transfer( transfer_in4 );
	if( res ) {
		fprintf( stderr, "libusb_submit_transfer failed (%d)\n", res ); 
		return 21;
	}
	fprintf( stderr, "libusb_submit_transfer succedeed\n" ); 

	fprintf( stderr, "SYN transfer starting\n" );

	// main loop 
	for( j=0, res=0; res==0 ; j++) {
		res = libusb_handle_events_completed( ctx, NULL );
	}
	fprintf( stderr, "libusb_handle events completed (%d)\n", res );

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

	return 0;
}