#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"

#include <UDPSocket.h>
#include "SocketAddress.h"

EthernetInterface gEth;
#define SERVER_ADDRESS "www.google.com"
#define SERVER_PORT 5683

static bool DeviceIpAddrGet()
{
	int retries = 5;

	while (retries--) {
		const char *ip = gEth.get_ip_address();
		if (ip != NULL)	{
			printf("Device IP address: %s\n", ip);
			return true;
		}
		else {
			wait(0.2);
		}
	}

	printf("Failed retrieving device IP address\n");
	return false; /* failed to retrieve a valid IP address */
}


int main(int argc, char *argv[])
{
	char messageOut[] = "PING";
	char messageIn[100];

	int rc = gEth.connect();
	if (rc != 0) {
		printf("Failed connecting to ethernet interface!\n");
		return EXIT_FAILURE;
	}
	
	// Get device IP address - this should yield a factory IP address
	DeviceIpAddrGet();
	
	UDPSocket *_socket;
	SocketAddress *_socketAddress;
	int sentBytes;
	int recievedBytes;

	_socketAddress = new SocketAddress((NetworkInterface*)&gEth, SERVER_ADDRESS, SERVER_PORT);
	_socket = new UDPSocket((NetworkInterface*)&gEth);

	_socket->set_blocking(true);
	_socket->set_timeout(3000);

	recievedBytes = _socket->recvfrom(_socketAddress, messageIn, sizeof(messageIn));
	if (recievedBytes == NSAPI_ERROR_WOULD_BLOCK){
		printf("Failed to receive data from the socket %i \n", recievedBytes); //should be warn level
	}
	else if (recievedBytes < 0) {
		printf("Failed to receive data from the socket %i \n", recievedBytes);
	}

	sentBytes = _socket->sendto(*_socketAddress, messageOut, strlen(messageOut));
	if (sentBytes == NSAPI_ERROR_WOULD_BLOCK) {
		printf("Timeout error  %i \n", sentBytes);
	}
	else if (sentBytes < 0) {
		printf("Failed sending data to the socket %i \n", sentBytes);
	}
	else if ((unsigned)sentBytes != strlen(messageOut)) {
		printf("Error data size was send , packet size %i, sent bytes messageSize %i \n", strlen(messageOut), sentBytes);
	}
	


	return 0;
}