/*
# VRPN iOS server 
# This server receives data from a iOS device by using a UDP connection
# written by Luis Ballesteros, AI3D Colivri Uniandes / May 2012 (Colombia)
# lg.ballesteros94@uniandes.edu.co
*/

#include <winsock2.h>
#include "vrpn_iOSDevice.h"
#pragma comment(lib,"ws2_32.lib")

#undef VERBOSE

const int SERVER_PORT=1111; /*the default serverport number*/

struct sockaddr_in serverAddr; /*server address*/
struct sockaddr_in clientAddr; /*client address*/

int serverSock; /*server socket, and client sock*/
const int bufSize = 13;
char buf[bufSize]; /*buffer the message send and receive*/
int len=sizeof(struct sockaddr);
char ip[15]; /*client address*/
int serverPort,clientPort; /*client and server address*/
int result;

__int16 currentButtons=-1;
__int16 pastButtons=-1;

__int16 currentXTranslationValue,currentYTranslationValue,currentZTranslationValue=-1;
__int16 pastXTranslationValue,pastYTranslationValue,pastZTranslationValue=-1;

__int16 currentXRotationValue,currentYRotationValue,currentZRotationValue=-1;
__int16 pastXRotationValue,pastYRotationValue,pastZRotationValue=-1;


WSADATA wsaData;


// Defines the modes in which the box can find itself.
vrpn_iOSDevice::vrpn_iOSDevice (const char * name, const char * port, vrpn_Connection * c) :
		vrpn_Analog(name, c),
		vrpn_Button(name, c)
{
	vrpn_Button::num_buttons = 0;  // Has 8 buttons
    vrpn_Analog::num_channel = 7;  // Has 6 Analogs
  
	printf("\nVRPN Server - iOS Device Connection over UDP\n\n");

	//	Socket Startup
	WSAStartup(0x0202,&wsaData); /*windows socket startup */

	memset((char*)&serverAddr,0,sizeof(serverAddr));
	serverAddr.sin_family=AF_INET; /*set server address protocol family*/
	serverAddr.sin_addr.s_addr=INADDR_ANY;

//	if (argc>1){
//		serverPort=atoi(argv[1]);
//	}
//	else
	{
		serverPort=SERVER_PORT;
	}

	if (serverPort>0)
		serverAddr.sin_port=htons((u_short)serverPort);/*set port*/
	else 
	{
		fprintf(stderr,"bad port number\n");
		exit(1);
	}

	serverSock=socket(PF_INET,SOCK_DGRAM,0);/*create a socket*/
	if (serverSock<0){
		fprintf(stderr,"socket creating failed\n");
		exit(1);
	}

	if (bind(serverSock,(LPSOCKADDR)&serverAddr,len)<0){/*bind a server address and port*/
		fprintf(stderr,"bind failed\n");
		exit(1);
	}

	// Set Non Blocking Mode, specified via last parameter
	// 0 Disabled 
	// !0 Enabled 
	unsigned long int nonBlockingMode = 1;
	result = ioctlsocket( serverSock, FIONBIO, &nonBlockingMode );

	if ( result )
	{
		fprintf(stderr,"ioctlsocket failed\n");
		exit(1);
	}

	//while(1)
	{

	}

}

void	vrpn_iOSDevice::clear_values(void)
{
	vrpn_int32	i;

	for (i = 0; i < num_buttons; i++) {
		vrpn_Button::buttons[i] = vrpn_Button::lastbuttons[i] = 0;
	}
	for (i = 0; i < num_channel; i++) {
		vrpn_Analog::channel[i] = vrpn_Analog::last[i] = 0;
	}
}

void	vrpn_iOSDevice::report_changes(vrpn_uint32 class_of_service)
{
	vrpn_Analog::timestamp = _timestamp;
	vrpn_Button::timestamp = _timestamp;

	vrpn_Analog::report_changes(class_of_service);
	vrpn_Button::report_changes();
}

void	vrpn_iOSDevice::report(vrpn_uint32 class_of_service)
{
	vrpn_Analog::timestamp = _timestamp;
	vrpn_Button::timestamp = _timestamp;

	vrpn_Analog::report(class_of_service);
	vrpn_Button::report_changes();
}


// This routine is called each time through the server's main loop. It will
// take an action depending on the current status of the iOS Device,
// either trying to reset it or trying to get a reading from it.
void	vrpn_iOSDevice::mainloop()
{
	// Call the generic server mainloop, since we are a server
	struct timeval reporttime;
	vrpn_gettimeofday(&reporttime, NULL);
    server_mainloop();

	// Setup the Client Address ( IP & port )
	struct sockaddr_in clientAddr;
	// this information will be filled in when recvfrom executes

	// Check for messages
	FD_SET set;
	timeval timeVal;

	FD_ZERO( &set );
	FD_SET( serverSock, &set );

	timeVal.tv_sec = 0;
	timeVal.tv_usec = 0;

	result = select( FD_SETSIZE, &set, NULL, NULL, &timeVal );

	if( result == 0 )
	{
		Sleep( 1 );
		//continue;
	}

	if( result == SOCKET_ERROR )
	{
		fprintf(stderr,"select error\n");
		exit(1);
	}

	memset(buf,0,sizeof(buf));
	recvfrom(serverSock,buf,sizeof(buf),0,(LPSOCKADDR)&clientAddr,&len);

	//Buttons
	currentButtons=((__int16)(0<<8) | (__int16)(buf[0] & 0x00FF));

	//Translations X, Y & Z
	currentXTranslationValue=((__int16)(buf[1]<<8) | (__int16)(buf[2] & 0x00FF));
	currentYTranslationValue=((__int16)(buf[3]<<8) | (__int16)(buf[4] & 0x00FF));
	currentZTranslationValue=((__int16)(buf[5]<<8) | (__int16)(buf[6] & 0x00FF));

	//Rotations X, Y Z
	currentXRotationValue=((__int16)(buf[7]<<8) | (__int16)(buf[8] & 0x00FF));
	currentYRotationValue=((__int16)(buf[9]<<8) | (__int16)(buf[10] & 0x00FF));
	currentZRotationValue=((__int16)(buf[11]<<8) | (__int16)(buf[12] & 0x00FF));


	//Debug bits
	//printf("%d",buf[0]);
	//printf(" ");
	//printbitssimple(yawValue);
	//printf("\n");

	//Filter data
	filterValue(&currentButtons,&pastButtons);
	filterValue(&currentXTranslationValue,&pastXTranslationValue);
	filterValue(&currentYTranslationValue,&pastYTranslationValue);
	filterValue(&currentZTranslationValue,&pastZTranslationValue);
	filterValue(&currentXRotationValue,&pastXRotationValue);
	filterValue(&currentYRotationValue,&pastYRotationValue);
	filterValue(&currentZRotationValue,&pastZRotationValue);

	//Send to VRPN
	vrpn_Analog::channel[0]=currentButtons;

	vrpn_Analog::channel[1]=currentXTranslationValue;
	vrpn_Analog::channel[2]=currentYTranslationValue;
	vrpn_Analog::channel[3]=currentZTranslationValue;

	vrpn_Analog::channel[4]=currentXRotationValue;
	vrpn_Analog::channel[5]=currentYRotationValue;
	vrpn_Analog::channel[6]=currentZRotationValue;

	// Send any changes out over the connection.
	_timestamp = reporttime;
	report_changes();
}

//#endif

/* Print n as a binary number */
void vrpn_iOSDevice::printbitssimple(__int16 n) {
	unsigned int i;
	i = 1<<(sizeof(n) * 8 - 1);

	while (i > 0) {
		if (n & i)
			printf("1");
		else
			printf("0");
		i >>= 1;
	}
}

void vrpn_iOSDevice::filterValue(__int16 *currentValue, __int16 *pastValue){
	if(*currentValue==0)
	{
		if(*pastValue!=0)
		{
			*currentValue=*pastValue;
		}
		else
		{
			//pastValue=currentValue;
		}
	}
	else
	{
		*pastValue=*currentValue;
	}
}
