#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
#pragma GCC diagnostic ignored "-Wunused-variable"

#include <stdio.h>
#include <string.h>
#include <netinet/in.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <pthread.h>
#include <unistd.h>

typedef struct {
	int socket;
	struct sockaddr_in address;
} TData;

typedef struct {
	float x;
	float y;
	float z;
} TFloat;

void Error( int err );
void* dataProcessHandler( void * data );

int main( void ) {
	int e = 0;
	int i = 0;

        struct sockaddr_in myAddr;
        int sockMain;
        int port = 8080;
        socklen_t addrLen;

        int sock[2];
	struct sockaddr_in myAddrForClients[2];
        struct sockaddr_in clients[2];

	struct sockaddr_in tmp;

        myAddr.sin_family = AF_INET;
        myAddr.sin_port = htons(port);
        myAddr.sin_addr.s_addr = INADDR_ANY;

	myAddrForClients[0].sin_family = AF_INET;
	myAddrForClients[1].sin_family = AF_INET;

	myAddrForClients[0].sin_port = htons(60000);
	myAddrForClients[1].sin_port = htons(60000);

	myAddrForClients[0].sin_addr.s_addr = INADDR_ANY;
	myAddrForClients[1].sin_addr.s_addr = INADDR_ANY;

	for( i = 0; i < 2; i ++ ) {
		if( (sock[i] = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
			Error( e++ );
			return -1;
		}

		while( bind( sock[i], (struct sockaddr*)&myAddrForClients[i], sizeof( struct sockaddr_in) ) < 0 ) {
			myAddrForClients[i].sin_port = htons(ntohs(myAddrForClients[i].sin_port) + 1);
		}
	}

	if( (sockMain = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
		Error( e++ );
		return -1;
	}

        if( bind( sockMain, (struct sockaddr*)&myAddr, sizeof(struct sockaddr_in) ) < 0 ) {
                Error( e++ );
                return -1;
        }



        addrLen = sizeof( struct sockaddr_in );

        int flag = 2;
        int recvLen, sendLen;

	char buf[5];
	memset( buf, 0 , sizeof(buf));

        while( flag ) {
                recvLen = recvfrom( sockMain, buf, 5, 0, (struct sockaddr*)&tmp, &addrLen );

		if( buf[0] != 'U' || buf[1] != 'N' || buf[2] != 'I' || buf[3] != 'T' || buf[4] != 'Y' ) {
			Error( e++ );
			return -1;
		}
		clients[2-flag--] = tmp;
        }

	for( i = 0; i < (sizeof(myAddrForClients) / sizeof(myAddrForClients[0])); i++ ) {

		char port2char[8];
		sprintf(port2char, "%d\r\n", ntohs(myAddrForClients[i].sin_port) );
		sendLen = sendto(sockMain, port2char, sizeof(port2char), 0, (struct sockaddr*)&clients[i], sizeof( clients[i]) );
	}

	//Unity's got port number to connect
	//myAddrForClients[x] holds server addresses for each Unity player
	//clients[x] holds Unity's addresses

	pthread_t handlers[2];
	TData data[2];

	int size = ( sizeof(handlers) / sizeof(handlers[0]) );

	for( i = 0; i < size; i++ ) {
		data[i].socket = sock[i];
		data[i].address = clients[size-1-i];
	}

	for( i = 0; i < size; i++ ) {
		if( pthread_create(&handlers[i], NULL, dataProcessHandler, &data[i] ) ) {
			Error( e++ );
			return -1;
		}
	}

	for( i = 0; i < size; i++ ) {
		if( pthread_join( handlers[i], NULL )) {
			Error( e++ );
			return -1;
		}
	}

	close(sockMain);
	close(sock[0]);
	close(sock[1]);

        return 0;
}

void* dataProcessHandler( void * data ) {

	TData *info = (TData*)data;

	TFloat floats;
	memset(&floats,0,sizeof(floats));

	struct sockaddr_in tmp;
	socklen_t addrLen;

	printf("HANDLER STARTED..\n");
	printf("LOOKING AT SOCKET: %d, ON PORT: %d\n", info->socket, ntohs(info->address.sin_port) );

	while( 1 ) {
		if( recvfrom( info->socket, &floats, 12, 0, (struct sockaddr*)&tmp, &addrLen) != 12 ) continue;
		sendto( info->socket, &floats, 12, 0, (struct sockaddr*)&info->address, sizeof(info->address));
	}
	return NULL;
}

void Error( int err ) {
        printf("Error! Code: %d\n", err);
}