/*
 * Remote Laboratory Instrumentation Server
 *
 * 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, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * (c) 2009 Timothy Pearson
 * Raptor Engineering
 * http://www.raptorengineeringinc.com
 */

#include <stdio.h>                /* perror() */
#include <stdlib.h>               /* atoi() */
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>               /* read() */
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <fcntl.h>
#include <glib.h>
#include <gtk/gtk.h>
#include <termios.h>
#include <unistd.h>
#include <sys/signal.h>
#include <sys/types.h>
#include <gpib/ib.h>
#include <time.h>
#include "parameters.h"
#include "scope_functions.h"
#include "signal_functions.h"
#include "commanalyzer_functions.h"

// Server variables
char *serverAddress;
unsigned short serverPort = 4000;
char *serialportDescription;
char *gpibServerString;

// Scope parameters
char *scopeBoard;
char *scopeDevice;
char *scopeType;
char scopeFound = 0;
int scope_board_device;

// Function generator parameters
char *funcgenBoard;
char *funcgenDevice;
char *funcgenType;
char funcgenFound = 0;
int funcgen_board_device;

// Communications analyzer parameters
char *commanalyzerBoard;
char *commanalyzerDevice;
char *commanalyzerType;
char commanalyzerFound = 0;
int commanalyzer_board_device;

// Serial port parameters
char *serialDevice;
long serialBaud = 0;

// Serial stuff
int tty;
struct termios oldtio, newtio;       //place for old and new port settings for serial port
struct termios oldkey, newkey;       //place tor old and new port settings for keyboard teletype
struct sigaction saio;               //definition of signal action
char buf[256];                       //buffer for where data is put
int fd_tty;
int wait_flag=TRUE;                  //TRUE while no signal received
int server_fd_with_serial = -1;

// Network variables
int clientSocket;
int status = 0;
int last_command_acked = 0;
struct hostent *hostPtr = NULL;
struct sockaddr_in serverName = { 0 };
int authentication_timer_check(void);
long optval;
int return_status;

// Timing variables
unsigned int authentication_timer;
unsigned char enable_authentication_timer;

unsigned char buffer[100000];

// Generic server stuff
#define QLEN 100000
u_short portbase = 0;
struct timeval server_multiplexer;

// Query server stuff
char *queryservice_port = "4003";
struct sockaddr_in fsin_query;
int msock_query;
fd_set rfds_query;
fd_set afds_query;
int alen_query;
int fd_query;
int nfds_query;

// Main server stuff
char *mainservice_port = "4002";
struct sockaddr_in fsin_mainserver;
int msock_mainserver;
fd_set rfds_mainserver;
fd_set afds_mainserver;
int alen_mainserver;
int fd_mainserver;
int nfds_mainserver;
int ssock_mainserver;
//char main_server_in_use;
//char main_server_fd;
char main_server_state[65535];
char auth_char_pos;
char auth_string[40];
int k;
int m;

// Configuration stuff
static const char filename[] = "remotefpga_gpib.conf";
char linedata [256];

// Shut up GCC
void quiet_write(int fd, const void *buf, size_t count) {
	int retcode = write(fd, buf, count);
	if (retcode < 0) {
		printf("[WARN] Network error\n\r");
	}
}

void getMyIP (void)
{
	char Buf [256];
	struct hostent* Host;
	gethostname (Buf, 256);
	Host=(struct hostent *) gethostbyname (Buf);
	serverAddress=strdup(inet_ntoa(*((struct in_addr *)Host->h_addr)));
	//serverAddress=strdup(Buf);
}

int msleep(unsigned long milisec)  {
	struct timespec req={0};
	time_t sec=(int)(milisec/1000);
	milisec=milisec-(sec*1000);
	req.tv_sec=sec;
	req.tv_nsec=milisec*1000000L;
	while(nanosleep(&req,&req)==-1)
		continue;
	return 1;
}

int musleep(unsigned long milisec)  {
	struct timespec req={0};
	time_t sec=(int)(milisec/1000);
	milisec=milisec-(sec*1000);
	req.tv_sec=sec;
	req.tv_nsec=milisec*1000L;
	while(nanosleep(&req,&req)==-1)
		continue;
	return 1;
}

void signal_handler_IO (int status) {
	wait_flag = FALSE;
}

int setupSerial(void) {
	struct termios oldtio,newtio;
	
	fd_tty = open(serialDevice, O_RDWR | O_NOCTTY | O_NONBLOCK | O_APPEND);
	if (fd_tty < 0) {
		printf("[FAIL] Unable to open serial device %s\n\r", serialDevice);
		return 1;
	}
	
	tcgetattr(fd_tty,&oldtio);	// Save current port settings
	
	bzero(&newtio, sizeof(newtio));
	newtio.c_cflag = serialBaud | CS8 | CLOCAL | CREAD;
	newtio.c_iflag = IGNPAR;
	newtio.c_oflag = 0;
	
	// Set input mode (non-canonical, no echo,...)
	newtio.c_lflag = 0;
	
	newtio.c_cc[VTIME]    = 0;	// Inter-character timer unused
	newtio.c_cc[VMIN]     = 0;	// Blocking read unused
	
	tcflush(fd_tty, TCIFLUSH);
	tcsetattr(fd_tty,TCSANOW,&newtio);

	return 0;
}

int getConfig(char *parameter, char *line) {
	int i;

	if (strstr(line, parameter) != NULL) {
		for (i=0; i<(strlen(line)-strlen(parameter));i++) {
			linedata[i] = line[i+strlen(parameter)];
		}
		linedata[i-1]=0;

		return 0;
	}
	else {
		return 1;
	}
}

void concatenateStrings(char * str1, char * str2) {
	char *str3;
	
	str3 = (char *)calloc(strlen(str1) + strlen(str2) + 1, sizeof(char));
	strcpy(str3, str1);
	strcat(str3, str2);
	
	str1 = (char *)calloc(strlen(str3) + 1, sizeof(char));
	
	strcpy(str1, str3);
	
	free(str1);
	free(str3);
}

/* returns a device descriptor after prompting user for primary address */
int open_gpib_device(int minor, int pad)
{
	int ud;
	const int sad = 0;
	const int send_eoi = 1;
	const int eos_mode = 0;
	const int timeout = T1s;

	printf("[INFO] Trying to open GPIB device %i on board /dev/gpib%i...\n\r", pad, minor);
	ud = ibdev(minor, pad, sad, timeout, send_eoi, eos_mode);
	if(ud < 0)
	{
		printf("[WARN] GPIB interface error\n\r");
		return -1;
	}

	return ud;
}

char * scopeLongDescription (char * scopeType) {
	if (strcmp("HP54600OS", scopeType) == 0) {
		return "Hewlett Packard 54600 series";
	}
	else if (strcmp("TDS744AOS", scopeType) == 0) {
		return "Tektronix 744A series";
	}
	else {
		return "UNKNOWN";
	}
}

char * commanalyzerLongDescription (char * scopeType) {
	if (strcmp("HP8924C", commanalyzerType) == 0) {
		return "Hewlett Packard 8924 series";
	}
	else {
		return "UNKNOWN";
	}
}

char * funcgenLongDescription (char * funcgenType) {
	if (strcmp("AG33250A", funcgenType) == 0) {
		return "Agilent AG33250A";
	}
	else {
		return "UNKNOWN";
	}
}

int readConfig(void) {
	int i;

	FILE *file = fopen ( filename, "r" );
	if ( file != NULL ) {
		char line [256];	// or other suitable maximum line size
		// read a line
		while ( fgets ( line, sizeof line, file ) != NULL ) {
			// Parse the line and update global variables (current line in variable "line")
			if (getConfig("SERVER_DESCRIPTION:", line) == 0) {
				gpibServerString = strdup(linedata);
				printf("[INFO] Server Description: %s\n\r", gpibServerString);
			}
			if (getConfig("SERIAL_PORT:", line) == 0) {
				serialDevice = strdup(linedata);
				printf("[INFO] Serial Port: %s\n\r", serialDevice);
			}
			if (getConfig("BAUD_RATE:", line) == 0) {
				if (strcmp(linedata, "9600") == 0) serialBaud = B9600;
				if (strcmp(linedata, "115200") == 0) serialBaud = B115200;
				//serialBaud = B9600;
				printf("[INFO] Baud Rate: %s [%ld]\n\r", linedata, serialBaud);
				serialportDescription = strdup(linedata);
			}
			if (getConfig("SCOPE_BOARD:", line) == 0) {
				scopeBoard = strdup(linedata);
				scopeFound++;
			}
			if (getConfig("SCOPE_DEVICE:", line) == 0) {
				scopeDevice = strdup(linedata);
				scopeFound++;
			}
			if (getConfig("SCOPE_TYPE:", line) == 0) {
				scopeType = strdup(linedata);
				scopeFound++;
			}
			if (getConfig("FUNCTION_GENERATOR_BOARD:", line) == 0) {
				funcgenBoard = strdup(linedata);
				funcgenFound++;
			}
			if (getConfig("FUNCTION_GENERATOR_DEVICE:", line) == 0) {
				funcgenDevice = strdup(linedata);
				funcgenFound++;
			}
			if (getConfig("FUNCTION_GENERATOR_TYPE:", line) == 0) {
				funcgenType = strdup(linedata);
				funcgenFound++;
			}
			if (getConfig("COMMANALYZER_BOARD:", line) == 0) {
				commanalyzerBoard = strdup(linedata);
				commanalyzerFound++;
			}
			if (getConfig("COMMANALYZER_DEVICE:", line) == 0) {
				commanalyzerDevice = strdup(linedata);
				commanalyzerFound++;
			}
			if (getConfig("COMMANALYZER_TYPE:", line) == 0) {
				commanalyzerType = strdup(linedata);
				commanalyzerFound++;
			}
		}
		fclose ( file );
		if (scopeFound == 3) {
			printf("[INFO] Oscilloscope conjectured to be on GPIB address %s:%s\n\r", scopeBoard, scopeDevice);
			scope_board_device = open_gpib_device(atoi(scopeBoard), atoi(scopeDevice));
			if (scope_board_device < 0) {
				//return 1;
			}
			else {
				time_t rawtime;
				struct tm * timeinfo;
				char datebuffer [80];
				char timebuffer [80];
				time ( &rawtime );
				timeinfo = localtime ( &rawtime );
				strftime(timebuffer,80,"TIME \"%H:%M:%S\"",timeinfo);
				strftime(datebuffer,80,"DATE \"%Y-%m-%d\"",timeinfo);
				printf("[INFO] Configuring %s oscilloscope\n\r", scopeLongDescription(scopeType));
				printf("[INFO] %s\n\r", datebuffer);
				printf("[INFO] %s\n\r", timebuffer);
				if (gpib_write(scope_board_device, timebuffer) == 0) {
					gpib_write(scope_board_device, datebuffer);
					printf("[INFO] Communication verified\n\r");
				}
				else {
					printf("[WARN] Communication failed!\n\r");
				}
			}
		}
		if (funcgenFound == 3) {
			printf("[INFO] Function generator conjectured to be on GPIB address %s:%s\n\r", funcgenBoard, funcgenDevice);
			funcgen_board_device = open_gpib_device(atoi(funcgenBoard), atoi(funcgenDevice));
			if (funcgen_board_device < 0) {
				//return 1;
			}
			else {
				printf("[INFO] Configuring %s function generator\n\r", funcgenLongDescription(funcgenType));
				if (gpib_write(funcgen_board_device, "RESET") == 0) {
					printf("[INFO] Communication verified\n\r");
				}
				else {
					printf("[WARN] Communication failed!\n\r");
				}
			}
		}
		if (commanalyzerFound == 3) {
			printf("[INFO] Communications analyzer conjectured to be on GPIB address %s:%s\n\r", commanalyzerBoard, commanalyzerDevice);
			commanalyzer_board_device = open_gpib_device(atoi(commanalyzerBoard), atoi(commanalyzerDevice));
			if (commanalyzer_board_device < 0) {
				//return 1;
			}
			else {
				time_t rawtime;
				struct tm * timeinfo;
				time ( &rawtime );
				timeinfo = localtime ( &rawtime );
				printf("[INFO] Configuring %s communications analyzer\n\r", commanalyzerLongDescription(commanalyzerType));
				if (commanalyzer_set_time(timeinfo, commanalyzerType, commanalyzer_board_device) == 0) {
					commanalyzer_set_date(timeinfo, commanalyzerType, commanalyzer_board_device);
					printf("[INFO] Communication verified\n\r");
				}
				else {
					printf("[WARN] Communication failed!\n\r");
				}
			}
		}
	}
	else
	{
		printf("[WARN] Unable to open configuration file %s\n\r", filename);
		return 1;
	}

	return 0;
}

int authentication_timer_check(void) {
	authentication_timer++;

	return enable_authentication_timer;
}

int passivesock(const char *service, const char *transport, int qlen) {
	struct servent  *pse;
	struct protoent *ppe;
	struct sockaddr_in sin;
	int s;
	int type;
	
	memset(&sin, 0, sizeof(sin));
	sin.sin_family = AF_INET;
	sin.sin_addr.s_addr = INADDR_ANY;
	
	// Map service name to port number
	if(pse = getservbyname(service, transport)) {
		sin.sin_port = htons(ntohs((u_short)pse->s_port) + portbase);
	}
	else if((sin.sin_port = htons((u_short)atoi(service))) == 0) {
		printf("[FAIL] Query Server Service Entry %s\n\r", service);
		return -1;
	}
	
	// Map protocol name to protocol number
	if((ppe = getprotobyname(transport)) == 0) {
		printf("[FAIL] Query Server Protocol Entry %s\n\r", transport);
		return -1;
	}
	
	// Use protocol to choose a socket type
	if(strcmp(transport, "udp") == 0) {
		type = SOCK_DGRAM;
	}
	else {
		type = SOCK_STREAM;
	}
	
	// Allocate a socket
	s = socket(PF_INET, type, ppe->p_proto);
	
	if(s < 0) {
		printf("[FAIL] Socket Error\n\r");
		return -1;
	}
	
	// Bind the socket
	if(bind(s, (struct sockaddr *)&sin, sizeof(sin)) < 0) {
		printf("[FAIL] Cannot bind to port %s\n\r", service);
		return -1;
	}
	
	if(type == SOCK_STREAM && listen(s, qlen) < 0) {
		printf("[FAIL] Cannot bind to port %s\n\r", service);
		return -1;
	}
	return s;
}

int passiveTCP(const char *service, int qlen) {
	return passivesock(service, "tcp", qlen);
}

int queryserver(int fd) {
	return 0;
}

void buffer_lookfor_two_termdegree (unsigned char * array, unsigned char * array2, int termination_count) {
	int i;
	int k;
	char degree_found;

	degree_found = 0;
	k=0;
	for (i=0;i<termination_count;i++) {
		if (array[i+1] == 176) {
			if (degree_found == 0) {
				degree_found = 1;
			}
			else {
				degree_found = 3;
			}
		}
		if (degree_found == 2) {
			array2[k] = array[i+1];
			k++;
		}
		if (degree_found == 1) {
			array[i] = 0;
			degree_found = 2;
			k=0;
		}
		else {
			array[i] = array[i+1];
		}
	}
	array[i] = 0;
	array2[k] = 0;
}

void buffer_lookfor_termdegree (unsigned char * array, int termination_count) {
	int i;
	char degree_found;

	degree_found = 0;
	for (i=0;i<termination_count;i++) {
		if (array[i+1] == 176) {
			degree_found = 1;
		}
		if (degree_found == 1) {
			array[i] = 0;
		}
		else {
			array[i] = array[i+1];
		}
	}
	array[i] = 0;
}

int mainserver(int fd) {
	char readbuf[100000];
	char readbuf2[100000];
	char writebuf[100000];
	int cc;
	int z;
	long bytestosend;
	char errorbuf[1000];

	cc = 1;
	switch (main_server_state[fd]) {
		case 1:		// Check the command code
				cc = read(fd, readbuf, 1);
				if (cc > 0) {
					// Got one!
					#ifdef ENABLE_EXTRA_DEBUGGING
					printf("[DEBG] Got command %d on server fd %d\n\r", readbuf[0], fd);
					#endif

					// What is it?
					if (readbuf[0] == 31) {
						// Serial channel open request
						// Open the serial port
						printf("[INFO] Opening serial port for fd %d...\n\r", fd);
						if (serialBaud != 0) {
							if (setupSerial() != 0) {
								printf("[FAIL] Cannot open serial port\n\r");
								quiet_write(fd, "NCK\r", strlen("NCK\r"));
							}
							else {
								quiet_write(fd, "ACK\r", strlen("ACK\r"));
								main_server_state[fd] = 4;
								server_fd_with_serial = fd;
								printf("[INFO] Entering state 4 on server fd %d\n\r", fd);
							}
						}
						else {
							printf("[FAIL] Serial port not set up!\n\r");
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 21) {
						// Scope request
						if (scope_board_device < 0) {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
						else {
							if (strcmp("HP54600OS", scopeType) == 0) {
								quiet_write(fd, "546\r", strlen("546\r"));
								main_server_state[fd] = 5;
							}
							else if (strcmp("TDS744AOS", scopeType) == 0) {
								quiet_write(fd, "744\r", strlen("744\r"));
								main_server_state[fd] = 5;
							}
							else {
								// Scope not recognized, apparently
								quiet_write(fd, "NCK\r", strlen("NCK\r"));
							}
							printf("[INFO] Entering state %d on server fd %d\n\r", main_server_state[fd], fd);
						}
					}
					else if (readbuf[0] == 40) {
						// Function generator request
						if (funcgen_board_device < 0) {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
						else {
							if (strcmp("AG33250A", funcgenType) == 0) {
								//if (signal_reset(funcgenType, funcgen_board_device, errorbuf) == 0) {
									quiet_write(fd, "ACK\r", strlen("ACK\r"));
									main_server_state[fd] = 6;
								//}
								// Function generator seems to have failed
								//write(fd, "NCK\r", strlen("NCK\r"));
							}
							else {
								// Function generator not recognized, apparently
								quiet_write(fd, "NCK\r", strlen("NCK\r"));
							}
							printf("[INFO] Entering state %d on server fd %d\n\r", main_server_state[fd], fd);
						}
					}
					else if (readbuf[0] == 41) {
						// Communications analyzer request
						if (commanalyzer_board_device < 0) {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
						else {
							if (strcmp("HP8924C", commanalyzerType) == 0) {
								quiet_write(fd, "892\r", strlen("892\r"));
								main_server_state[fd] = 7;
							}
							else {
								// Communications analyzer not recognized, apparently
								quiet_write(fd, "NCK\r", strlen("NCK\r"));
							}
							printf("[INFO] Entering state %d on server fd %d\n\r", main_server_state[fd], fd);
						}
					}
				}
				break;

		case 4:		// Process serial port transfers
				cc = read(fd_tty, readbuf, 100000);
				if (cc > 0) {
					quiet_write(fd, readbuf, cc);
					fsync(fd_tty);
					#ifdef ENABLE_EXTRA_DEBUGGING
					printf("[DEBG] Got %d bytes from the serial port\n\r", cc);
					#endif
				}
				cc = read(fd, writebuf, 100000);
				if (cc > 0) {
					quiet_write(fd_tty, writebuf, cc);
					fsync(fd);
					#ifdef ENABLE_EXTRA_DEBUGGING
					printf("[DEBG] Got %d bytes from the network interface\n\r", cc);
					#endif
				}
				break;

		case 5:		// Process scope interface commands
				cc = read(fd, readbuf, 25);
				if (cc > 0) {
					// Got one!
					#ifdef ENABLE_EXTRA_DEBUGGING
					printf("[DEBG] Got command %d on server fd %d\n\r", readbuf[0], fd);
					#endif
					if ((readbuf[0] == 20) || (readbuf[0] == 29)) {		// Want scope screenshot!
						quiet_write(fd, "ACK", strlen("ACK"));
						fsync(fd);
						if (scope_get_screenshot(scopeType, scope_board_device) == 0) {
							sleep (5);
							scope_board_device = open_gpib_device(atoi(scopeBoard), atoi(scopeDevice));
							if (scope_get_screenshot_stage2(scopeType, scope_board_device) == 0) {
								// Send the data, all of it!
								quiet_write(fd, "ACK", strlen("ACK"));

								bytestosend = scopeScreenSize(scopeType);
								k=0;
								while (bytestosend > 0) {
									return_status = write(fd, scope_raw_screenshot_data+k, 1);
									if (return_status > 0) {
										bytestosend = bytestosend - return_status;
										k++;
									}
									else {
										bytestosend = 0;
									}
								}
							}
							else {
								quiet_write(fd, "NCK", strlen("NCK"));
							}
						}
						else {
							quiet_write(fd, "NCK", strlen("NCK"));
						}
					}
					else if (readbuf[0] == 22) {		// Want to change horizontal timebase
						buffer_lookfor_termdegree(readbuf, 25);
						if (scope_set_timebase(atof(readbuf), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 23) {		// Want to change volts per division
						buffer_lookfor_two_termdegree(readbuf, readbuf2, 25);
						if (scope_set_volts_div(atoi(readbuf), atof(readbuf2), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 24) {		// Want to change run status
						buffer_lookfor_termdegree(readbuf, 25);
						if (scope_set_acquisition(atoi(readbuf), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 25) {		// Want to change channel enable
						buffer_lookfor_two_termdegree(readbuf, readbuf2, 25);
						if (scope_set_channel_state(atoi(readbuf), atoi(readbuf2), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 26) {		// Want to change trigger channel
						buffer_lookfor_termdegree(readbuf, 25);
						if (scope_set_trigger_channel(atoi(readbuf), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 27) {		// Want to change trigger level
						buffer_lookfor_termdegree(readbuf, 25);
						if (scope_set_trigger_level(atof(readbuf), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 28) {		// Want to change channel vertical position
						buffer_lookfor_two_termdegree(readbuf, readbuf2, 25);
						if (scope_set_channel_position(atoi(readbuf), atof(readbuf2), scopeType, scope_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
				}
				break;
		case 6:		// Process function generator interface commands
				cc = read(fd, readbuf, 25);
				if (cc > 0) {
					// Got one!
					#ifdef ENABLE_EXTRA_DEBUGGING
					printf("[DEBG] Got command %d on server fd %d\n\r", readbuf[0], fd);
					#endif
					if (readbuf[0] == 40) {		// Want to reset function generator
						if (signal_reset(funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 41) {		// Is function generator configured?
						// If I'm in state 6 it had *better* be available!!!
						quiet_write(fd, "ACK\r", strlen("ACK\r"));
					}
					else if (readbuf[0] == 42) {		// Want to change frequency
						buffer_lookfor_termdegree(readbuf, 25);
						if (signal_set_frequency(atof(readbuf), funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 43) {		// Want to change duty cycle
						buffer_lookfor_termdegree(readbuf, 25);
						if (signal_set_duty_cycle(atof(readbuf), funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 44) {		// Want to set square wave
						if (signal_set_waveform("SQUARE", funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 45) {		// Want to set sine wave
						if (signal_set_waveform("SINUSOID", funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 46) {		// Want to set triangle wave
						if (signal_set_waveform("RAMP", funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 47) {		// Want to set noise wave
						if (signal_set_waveform("NOISE", funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 48) {		// Want to change P-P voltage
						buffer_lookfor_termdegree(readbuf, 25);
						if (signal_set_peak_peak_voltage(atof(readbuf), funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
					else if (readbuf[0] == 49) {		// Want to change offset voltage
						buffer_lookfor_termdegree(readbuf, 25);
						if (signal_set_offset_voltage(atof(readbuf), funcgenType, funcgen_board_device, errorbuf) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, errorbuf, strlen(errorbuf));
						}
					}
				}
				break;
		case 7:		// Process communications analyzer interface commands
				cc = read(fd, readbuf, 25);
				if (cc > 0) {
					// Got one!
					#ifdef ENABLE_EXTRA_DEBUGGING
					printf("[DEBG] Got command %d on server fd %d\n\r", readbuf[0], fd);
					#endif
					if (readbuf[0] == 40) {		// Want to set SA mode
						if (commanalyzer_switch_to_spectrum_analyzer_mode(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 41) {		// Is communications analyzer configured?
						// If I'm in state 7 it had *better* be available!!!
						quiet_write(fd, "ACK\r", strlen("ACK\r"));
					}
					else if ((readbuf[0] == 42)) {		// Want SA trace
						quiet_write(fd, "ACK\r", strlen("ACK\r"));
						fsync(fd);
						if (commanalyzer_get_spectrum_analyzer_trace(commanalyzerType, commanalyzer_board_device) == 0) {
							bytestosend = commanalyzerTraceLength(commanalyzerType)*sizeof(double);
							int16_t numbytes = bytestosend;
							quiet_write(fd, &numbytes, 2);
							quiet_write(fd, "\r", 1);
							fsync(fd);
							k=0;
							while (bytestosend > 0) {
								return_status = write(fd, ((uint8_t*)commanalyzer_raw_trace_data)+k, 1);
								if (return_status > 0) {
									bytestosend = bytestosend - return_status;
									k++;
								}
								else {
									bytestosend = 0;
								}
							}
							fsync(fd);
						}
						else {
							quiet_write(fd, "NCK", strlen("NCK"));
						}
					}
					else if (readbuf[0] == 43) {		// Want to lock screen
						if (commanalyzer_lock_screen(commanalyzerType, commanalyzer_board_device) == 0) {
							commanalyzer_set_display_brightness(0, commanalyzerType, commanalyzer_board_device);	// Don't burn in the screen
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 44) {		// Want to set generator to tracking mode
						if (commanalyzer_spectrum_analyzer_set_generator_mode_tracking(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 45) {		// Want to set generator to fixed mode
						if (commanalyzer_spectrum_analyzer_set_generator_mode_fixed(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 46) {		// Want to change center frequency
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_center_frequency(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 47) {		// Want to change frequency span
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_frequency_span(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 48) {		// Want to set RF input to dedicated connector
						if (commanalyzer_spectrum_analyzer_set_rf_input_dedicated(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 49) {		// Want to set RF input to multiplexed connector
						if (commanalyzer_spectrum_analyzer_set_rf_input_muxed(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 50) {		// Want to set generator output to dedicated connector
						if (commanalyzer_spectrum_analyzer_set_generator_output_dedicated(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 51) {		// Want to set generator output to multiplexed connector
						if (commanalyzer_spectrum_analyzer_set_generator_output_muxed(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 52) {		// Want to change input attenuation
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_input_attenuation(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 53) {		// Want to change scale
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_scale(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 54) {		// Want to set RF input attenuator mode to automatic
						if (commanalyzer_set_spectrum_analyzer_input_attenuator_mode_auto(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 55) {		// Want to set RF input attenuator mode to fixed
						if (commanalyzer_set_spectrum_analyzer_input_attenuator_mode_fixed(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 56) {		// Want to change generator output power
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_generator_power(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 57) {		// Want to change generator output frequency
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_generator_frequency(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 58) {		// Want to set generator sweep to ascending
						if (commanalyzer_spectrum_analyzer_set_generator_sweep_ascending(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 59) {		// Want to set generator sweep to descending
						if (commanalyzer_spectrum_analyzer_set_generator_sweep_descending(commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 60) {		// Want to set trace averaging
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_trace_averaging(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 61) {		// Want to set reference power level
						buffer_lookfor_termdegree(readbuf, 25);
						if (commanalyzer_set_spectrum_analyzer_reference_power_level(atof(readbuf), commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 62) {		// Want the number of vertical divisions available
						int16_t divisions = commanalyzer_get_spectrum_analyzer_number_of_vertical_divisions(commanalyzerType, commanalyzer_board_device);
						if (divisions >= 0) {
							quiet_write(fd, &divisions, 2);
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 63) {		// Want the number of samples in a trace
						int16_t divisions = commanalyzerTraceLength(commanalyzerType);
						if (divisions >= 0) {
							quiet_write(fd, &divisions, 2);
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, &divisions, 2);
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 64) {		// Want the number of horizontal divisions available
						int16_t divisions = commanalyzer_get_spectrum_analyzer_number_of_horizontal_divisions(commanalyzerType, commanalyzer_board_device);
						if (divisions >= 0) {
							quiet_write(fd, &divisions, 2);
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, &divisions, 2);
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 65) {		// Want the reference power level
						double rpower;
						if (commanalyzer_get_spectrum_analyzer_reference_power_level(&rpower, commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, &rpower, sizeof(double));
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, &rpower, sizeof(double));
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 66) {		// Want the vertical division scale
						double scale;
						if (commanalyzer_get_spectrum_analyzer_scale(&scale, commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, &scale, sizeof(double));
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, &scale, sizeof(double));
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 67) {		// Want to get the center frequency
						double freq;
						if (commanalyzer_get_spectrum_analyzer_center_frequency(&freq, commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, &freq, sizeof(double));
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, &freq, sizeof(double));
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
					else if (readbuf[0] == 68) {		// Want to get the frequency span
						double freq;
						if (commanalyzer_get_spectrum_analyzer_span(&freq, commanalyzerType, commanalyzer_board_device) == 0) {
							quiet_write(fd, &freq, sizeof(double));
							quiet_write(fd, "ACK\r", strlen("ACK\r"));
						}
						else {
							quiet_write(fd, &freq, sizeof(double));
							quiet_write(fd, "NCK\r", strlen("NCK\r"));
						}
					}
				}
				break;
		case 2:		// Open the serial port
				printf("[INFO] Opening serial port...\n\r");
				if (setupSerial() != 0) {
					printf("[FAIL] Cannot open serial port\n\r");
					cc = 0;
				}
				main_server_state[fd] = 1;
				break;
		case 127:	sleep(1);
				cc = 0;
				break;
	}

 	return cc;
}

int RunQueryServer() {
	msock_query = passiveTCP(queryservice_port, QLEN);
	if (msock_query == -1) {
		return -1;
	}
	nfds_query = getdtablesize();
	FD_ZERO(&afds_query);
	FD_SET(msock_query, &afds_query);
}

int RunMainServer() {
	//main_server_in_use = 0;
	//main_server_fd = -1;
	msock_mainserver = passiveTCP(mainservice_port, QLEN);
	if (msock_mainserver == -1) {
		return -1;
	}
	nfds_mainserver = getdtablesize();;
	FD_ZERO(&afds_mainserver);
	FD_SET(msock_mainserver, &afds_mainserver);
}

int main(int argc, char *argv[])
{
	char successful = 1;

	server_multiplexer.tv_sec = 0;
	server_multiplexer.tv_usec = 10;

	// Register signal handler
	signal(SIGPIPE, signal_handler_IO);

	printf("RemoteFPGA GPIB Server v%s.%s%s\n\r", SERVER_MAJOR, SERVER_MINOR, SERVER_REVISION);
	printf("(c) %s Timothy Pearson\n\r", COPYRIGHT_DATE);
	printf("(c) %s Remote Laboratory FOSS Contributors\n\r", FOSS_COPYRIGHT_DATE);

	getMyIP();

	printf("[INFO] Reading configuration from %s...\n\r", filename);
	readConfig();

	if (serialBaud != 0) {
		printf("[INFO] Opening serial port...\n\r");
		if (setupSerial() != 0) {
			successful = 0;
		}
		printf("[INFO] Closing serial port...\n\r");
		close(fd_tty);
	}

	if (successful == 1) {
		// Open query port 4001
		if (RunQueryServer() == -1) {
			successful = 0;
		}
		else {
			printf("[INFO] Query process started on %s:%s\n\r", serverAddress, queryservice_port);
		}
	}

	if (successful == 1) {
		// Open main port 4000
		if (RunMainServer() == -1) {
			successful = 0;
		}
		else {
			printf("[INFO] Main port opened on %s:%s\n\r", serverAddress, mainservice_port);
		}
	}

	if (successful == 1) {
		while (1) {
			//musleep(1);
			musleep(10);
			//musleep(50);
			//musleep(100);
			//musleep(250);
			//musleep(1000);

			// Execute Query Server
			memcpy(&rfds_query, &afds_query, sizeof(rfds_query));
			if (select(nfds_query, &rfds_query, (fd_set *)0, (fd_set *)0, &server_multiplexer) < 0) {
				//errexit("select: %s\n\r", strerror(errno));
			}
			if (FD_ISSET(msock_query, &rfds_query)) {
				int ssock_query;
				alen_query = sizeof(fsin_query);
				ssock_query = accept(msock_query, (struct sockaddr *)&fsin_query, &alen_query);
				if (ssock_query >= 0) {
					printf("[INFO] Query received from %s\n\r", inet_ntoa(fsin_query.sin_addr));
				
					FD_SET(ssock_query, &afds_query);
					if (strlen(gpibServerString) > 0) {
						quiet_write(ssock_query, gpibServerString, strlen(gpibServerString));
						quiet_write(ssock_query, "\r", strlen("\r"));
					}
					if (serialBaud != 0) {
						quiet_write(ssock_query, "Auxiliary serial port: ", strlen("Auxiliary serial port: "));
						quiet_write(ssock_query, serialportDescription, strlen(serialportDescription));
						quiet_write(ssock_query, " baud\r", strlen(" baud\r"));
					}
					if (scopeFound == 3) {
						quiet_write(ssock_query, scopeLongDescription(scopeType), strlen(scopeLongDescription(scopeType)));
						quiet_write(ssock_query, " oscilloscope\r", strlen(" oscilloscope\r"));
					}
					if (funcgenFound == 3) {
						quiet_write(ssock_query, funcgenLongDescription(funcgenType), strlen(funcgenLongDescription(funcgenType)));
						quiet_write(ssock_query, " signal generator\r", strlen(" signal generator\r"));
					}
					if (commanalyzerFound == 3) {
						quiet_write(ssock_query, commanalyzerLongDescription(commanalyzerType), strlen(commanalyzerLongDescription(commanalyzerType)));
						quiet_write(ssock_query, " communications analyzer\r", strlen(" communications analyzer\r"));
					}
				}
				else {
					printf("[WARN] Unable to accept query connection\n\r");
				}
			}
 			for (fd_query=0; fd_query < nfds_query; fd_query++) {
 				if(fd_query != msock_query && FD_ISSET(fd_query, &rfds_query)) {
 					if(queryserver(fd_query) == 0) {
 						(void) close(fd_query);
 						FD_CLR(fd_query, &afds_query);
 					}
 				}
 			}
	
			// Execute Main Server
			memcpy(&rfds_mainserver, &afds_mainserver, sizeof(rfds_mainserver));
			if (select(nfds_mainserver, &rfds_mainserver, (fd_set *)0, (fd_set *)0, &server_multiplexer) < 0) {
				//errexit("select: %s\n\r", strerror(errno));
			}
			if (FD_ISSET(msock_mainserver, &rfds_mainserver)) {
				int ssock_mainserver;
				alen_mainserver = sizeof(fsin_mainserver);
				ssock_mainserver = accept(msock_mainserver, (struct sockaddr *)&fsin_mainserver, &alen_mainserver);
				
				optval = 4194304;
				status = setsockopt(ssock_mainserver, SOL_SOCKET, SO_SNDBUF, &optval, sizeof(optval));
				printf("[INFO] Socket send buffer size set to %ld bytes\n", optval, status);

				if (ssock_mainserver >= 0) {
					//printf("[INFO] Connection established with %s\n\r", &fsin_mainserver.sin_addr);
				}
				FD_SET(ssock_mainserver, &afds_mainserver);

				fcntl(ssock_mainserver, F_SETFL, (fcntl(ssock_mainserver, F_GETFL) | O_NONBLOCK));
				//int sockbufsize = 0;
				//int size = sizeof(int);
				//getsockopt(ssock_mainserver, SOL_SOCKET, SO_RCVBUF, (char *)&sockbufsize, &size);
				//printf("[DEBG] SO_RCVBUF: %d\n\r", sockbufsize);
				main_server_state[ssock_mainserver] = 1;
				//write(ssock_mainserver, "OPENA", strlen("OPENA"));

				printf("[INFO] Connection established with client %s\n\r", inet_ntoa(fsin_mainserver.sin_addr));
			}
			for (fd_mainserver=0; fd_mainserver < nfds_mainserver; ++fd_mainserver) {
				if (fd_mainserver != msock_mainserver && FD_ISSET(fd_mainserver, &rfds_mainserver)) {
					if (mainserver(fd_mainserver) == 0) {
						(void) close(fd_mainserver);
						FD_CLR(fd_mainserver, &afds_mainserver);
						if (server_fd_with_serial != -1) {
							printf("[INFO] Closing serial port...\n\r");
							close(fd_tty);
						}
						printf("[INFO] Connection with client terminated\n\r");
					}
				}
			}
			if (server_fd_with_serial != -1) {
				mainserver(server_fd_with_serial);
			}
		}
	}

	return EXIT_SUCCESS;
}