/******************************************************************************
Copyright (C) 2005 Matteo Lucarelli

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

#include "UdpLightMessaging.h"

// ulmSocket utility class - win/lin compatibility layer ///////////////////////


ulmSocket::ulmSocket(unsigned short port)
{
#ifdef WIN32
	WSADATA socketInfo;
	m_WsaInit =(WSAStartup (MAKEWORD(2,0), &socketInfo)==0);
#endif
	// create socket (if fails m_hSocket=INVALID_SOCKET) 
	m_hSocket = socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP);
	
	// address struct
	memset(&m_BindAddr,0,sizeof(m_BindAddr));
	m_BindAddr.sin_family = PF_INET;	
	m_BindAddr.sin_addr.s_addr = INADDR_ANY;
	m_BindAddr.sin_port = htons(port);

	// associate socket with port
	if (bind(m_hSocket,(const struct sockaddr*)&m_BindAddr,sizeof(m_BindAddr))!=0)
		fprintf(stderr,"ulm: Error binding socket to port %i\n",port);
}

ulmSocket::~ulmSocket()
{
#ifdef WIN32
	closesocket(m_hSocket);
	if (m_WsaInit) WSACleanup();
#else
	close(m_hSocket);
#endif
}

int ulmSocket::Send(const void* buff,long size,const char* host,unsigned short port)
{
	struct sockaddr_in saddr;
	struct hostent *h = NULL;

	// if host is a valid address use it
#ifdef WIN32
	saddr.sin_addr.s_addr=inet_addr("host");
	if (saddr.sin_addr.s_addr==INADDR_NONE){
#else
	if (inet_aton(host,&saddr.sin_addr)==0){
#endif		
		// else try to resolve it
		h = gethostbyname(host);
		if (h==NULL) {
			fprintf(stderr,"ulm: Error resolving %s\n",host);
			return (-2);
		}
		saddr.sin_addr = *((struct in_addr *) h->h_addr);
	}

	saddr.sin_family = PF_INET;
	saddr.sin_port = htons(port);

	if(sendto(m_hSocket,(char*)buff,size,0,(const sockaddr*)&saddr, sizeof(saddr))!=size){
		fprintf(stderr,"ulm: Error sending to %s\n",host);
		return (-1);
	}
	return 0;
}

int ulmSocket::Recv(void *buff,long size,char* fromAddr,unsigned short* fromPort,long msTimeout)
{
	int ret=0;
	struct sockaddr_in recvAddr;

#ifdef WIN32
	timeval timeOut;
	timeOut.tv_sec = (long)(msTimeout/1000);
	timeOut.tv_usec = (msTimeout%1000)*1000;
	fd_set readfds;
	FD_ZERO(&readfds);

	// verify socket status
	FD_SET(m_hSocket,&readfds);
	select(0, &readfds, NULL, NULL, &timeOut);

	// if socket has data
	if (FD_ISSET(m_hSocket,&readfds)) {
		int aDim = sizeof(recvAddr);
#else
	pollfd pfd;
	pfd.fd=m_hSocket;
	pfd.events=POLLIN;
	
	if (poll(&pfd,1,msTimeout)>0){
		socklen_t aDim = sizeof(recvAddr);
#endif
		ret = recvfrom(m_hSocket,(char*)buff,size,0,(struct sockaddr*)&recvAddr,&aDim);
	}
	if ((ret>0)&&(fromAddr!=NULL)) strncpy(fromAddr,inet_ntoa(recvAddr.sin_addr),16);
	if ((ret>0)&&(fromPort!=NULL)) *fromPort = ntohs(recvAddr.sin_port);

	return ret;
}

// ulmMessage - to send messages ///////////////////////////////////////////////

ulmMessage::ulmMessage(char ch1,char ch2,char ch3)
{
	m_ch1=ch1;
	m_ch2=ch2;
	m_ch3=ch3;
	
	Clear();
	m_binFlag=false;
}
ulmMessage::~ulmMessage(){}

// add a field/value couple to the message
// TODO!: don't use ESC (27) char (reserved for binary data!)
int ulmMessage::FieldAdd(const char* label, const char* value)
{
	if (m_binFlag) return -2;

	// control for max dim
	if ((strlen(label)+strlen(value)+msg_dim+2)>ULM_MAX_MSG) return -1;

	// add field name
	*(msg_buf+msg_dim+ULM_HEAD_DIM)=0;
	strcpy((msg_buf+msg_dim+ULM_HEAD_DIM),label);
	msg_dim += strlen(label) + 1;

	// add field value
	*(msg_buf+msg_dim+ULM_HEAD_DIM)=0;
	strcpy((msg_buf+msg_dim+ULM_HEAD_DIM),value);
	msg_dim += strlen(value) + 1;

	return 0;
}

// extract a field
int ulmMessage::FieldGet(const char* label, char* value, long valueDimBytes)
{
	*value=0;
	char *pN,*pV;

	pN=msg_buf+ULM_HEAD_DIM;
	pV=msg_buf+ULM_HEAD_DIM;

	// search first value
	while((pV<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pV!=0)&&(*pV!=27)) pV++;
	if (*pV==0) pV++;
	else return -1;

	while(pV<(msg_buf+msg_dim+ULM_HEAD_DIM)){

		// if name match return value
		if (!strcmp(pN,label)){
			strncpy(value,pV,valueDimBytes);
			return 0;
		}
		pN=pV;

		// search next name
		while((pN<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pN!=0)&&(*pN!=27)) pN++;
		if (*pN==0) pN++;
		else return -1;

		pV=pN;

		// search next value
		while((pV<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pV!=0)&&(*pN!=27)) pV++;
		if (*pV==0) pV++;
		else return -1;
	}
	return -1;
}

// add a binary field to message
// TODO: ESC ?
int ulmMessage::BinaryFieldAdd(const char* label,const void* data,unsigned short size)
{
	// control for max dim 
	if ((msg_dim+strlen(label)+4+size)>ULM_MAX_MSG) return -1;

	// data field is ^ESC + Label + NULL + 2BytesDataDim + Data
	*(msg_buf+msg_dim+ULM_HEAD_DIM)=27;		// ESC
	*(msg_buf+msg_dim+ULM_HEAD_DIM+1)=0;		// label start
	strcpy((msg_buf+msg_dim+ULM_HEAD_DIM+1),label);	// label
	*((unsigned short*)(msg_buf+msg_dim+ULM_HEAD_DIM+strlen(label)+2)) = size; // Data Dim
	memcpy((msg_buf+ULM_HEAD_DIM+msg_dim+strlen(label)+4),data,size);   // data

	// add new data to dim
	msg_dim += strlen(label)+size+4;

	m_binFlag = true;
	return 0;
}

// extract a binary field
// TODO: ESC ?
int ulmMessage::BinaryFieldGet(const char* label,void* data,unsigned short maxsize,unsigned short* size)
{
	char *pD;

	pD=msg_buf+ULM_HEAD_DIM;
	*size = 0;

	while(pD<(msg_buf+msg_dim+ULM_HEAD_DIM)){

		// search for ESC char
		while((pD<(msg_buf+msg_dim+ULM_HEAD_DIM))&&(*pD!=27)) pD++;
		if (*pD==27) pD++;
		else return -1;

		// get Data Dim
		*size = *((unsigned short*)(pD+strlen(pD)+1));

		if (!strcmp(label,pD)){
			memcpy(data,(pD+strlen(pD)+3),maxsize);
			return 0;
		}

		pD += strlen(label)+*size+3;
	}
	return -1;
}

int ulmMessage::Send(unsigned short lport,const char* host,unsigned short port)
{	
	ulmSocket OutSock(lport);

	// MESSAGE HEADER
	msg_buf[0]=m_ch1;
	msg_buf[1]=m_ch2;
	msg_buf[2]=m_ch3;

	// 2chars_msg_lenght
	msg_buf[3]=(unsigned char)(msg_dim%256);
	msg_buf[4]=(unsigned char)(msg_dim/256);
	
	// 6 bytes for future use
	msg_buf[5]=0;
	msg_buf[6]=0;
	msg_buf[7]=0;
	msg_buf[8]=0;
	msg_buf[9]=0;
	msg_buf[10]=0;

	// XOR chars 
	msg_buf[11]=computeXOR(msg_buf+ULM_HEAD_DIM,msg_dim);
	msg_buf[12]=computeXOR(msg_buf,ULM_HEAD_DIM-1);

	// send message
	return (OutSock.Send(msg_buf,msg_dim+ULM_HEAD_DIM,host,port));
}

// clear message buffer
void ulmMessage::Clear()

{
	msg_dim=0;
	memset(msg_buf,0,ULM_MAX_MSG+ULM_HEAD_DIM);
	m_binFlag = false;
}

char ulmMessage::computeXOR(char* buf,long dim)
{
	char ret=0;
	for ( int i=0 ; i<dim ; i++ ) ret=ret^*(buf+i);
	return ret;
}

// ulmReceiver /////////////////////////////////////////////////////////////////

ulmReceiver::ulmReceiver(void* pClient,ulmCallback* Cb,unsigned short LocalPort,
			char ch1,char ch2,char ch3)
{
	// fill thread parameters struct
	m_RTparms.pClient = pClient;
	m_RTparms.ClientCb = Cb;
	m_RTparms.Socket = new ulmSocket(LocalPort);
	m_RTparms.ch1=ch1;
	m_RTparms.ch2=ch2;
	m_RTparms.ch3=ch3;

	// start receiving thread
	if (pthread_create(&m_pRecvThread,NULL,&RecvThread,(void*)&m_RTparms)!=0)
		fprintf(stderr,"ulm: Error creating receiver thread\n");
	
	pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
}

ulmReceiver::~ulmReceiver()
{	
	// stop receiving thread
	if (pthread_cancel(m_pRecvThread)!=0)
		fprintf(stderr,"ulm: Error cancelling receiver thread\n");


	if (pthread_join(m_pRecvThread,NULL)!=0)
		fprintf(stderr,"ulm: Error joining receiver thread\n");
		
	delete m_RTparms.Socket;
}

void* ulmReceiver::RecvThread(void* Parms)
{
	ulmMessage InMsg;
	char fromAddr[16];
	unsigned short fromPort;

	// read params;
	ulmSocket* SockIn = ((struct RecvThreadParms*)Parms)->Socket;
	ulmCallback* ClientCb = ((struct RecvThreadParms*)Parms)->ClientCb;
	void* pClient = ((struct RecvThreadParms*)Parms)->pClient;
	
	char ch1 = ((struct RecvThreadParms*)Parms)->ch1;
	char ch2 = ((struct RecvThreadParms*)Parms)->ch2;
	char ch3 = ((struct RecvThreadParms*)Parms)->ch3;
	
	bool ignorehead=true;
	if (ch1|ch2|ch3 != 0) ignorehead=false;

	while (1){

		// try to receive message (1 sec timeout)
		if (SockIn->Recv(InMsg.msg_buf,ULM_MAX_MSG+ULM_HEAD_DIM,fromAddr,&fromPort)>0){

			// if header is correct
			if ((ignorehead)||
			    ((InMsg.msg_buf[0]==ch1)&&
			     (InMsg.msg_buf[1]==ch2)&&
			     (InMsg.msg_buf[2]==ch3))){

				// compute dim
				InMsg.msg_dim=(unsigned char)InMsg.msg_buf[3]+(unsigned char)InMsg.msg_buf[4]*256;

				// pass message to client
				ClientCb(InMsg,pClient);
			}
			// delete message
			InMsg.Clear();
		}
		pthread_testcancel();
	}
	return NULL;
}