/************************************************************************
 * Includes
 ************************************************************************/
// FreeRTOS includes
#include "FreeRTOS.h"
#include "task.h"

// lwIP includes
#include "lwip/netif.h"
#include "lwip/sockets.h"

#include "string.h"
#include "print_funcs.h"


/************************************************************************
 * Imports
 ************************************************************************/


/************************************************************************
 * Prototypes
 ************************************************************************/
static void prvMulticastInitTask(void);

static void prvMulticastProcessReply_X(void);

static void prvMulticastProcessReply_Y(void);

static void prvMulticastProcessReply_Z(void);


/************************************************************************
 * Type definitions
 ************************************************************************/
typedef struct {
	int						socket;
	struct sockaddr_in		addr;
	U16						port;
	ip_mreq 				mreq;
} tmcast_socket;


/************************************************************************
 * Constants
 ************************************************************************/


/************************************************************************
 * Global variables
 ************************************************************************/


/************************************************************************
 * Task local variables
 ************************************************************************/
static U8 ucMulticastRecBuffer_X[multicastBUFFER_SIZE_X];
static U8* pucMulticastRecBuffer_X = &ucMulticastRecBuffer_X[0];

static U8 ucMulticastRecBuffer_Y[multicastBUFFER_SIZE_Y];
static U8* pucMulticastRecBuffer_Y = &ucMulticastRecBuffer_Y[0];

static U8 ucMulticastRecBuffer_Z[multicastBUFFER_SIZE_Z];
static U8* pucMulticastRecBuffer_Z = &ucMulticastRecBuffer_Z[0];


/************************************************************************
 * Methods
 ************************************************************************/
portTASK_FUNCTION(vMulticast, pvParameters) {
	// Descriptor set and structures for multicast sockets
	fd_set fd;
	tmcast_socket ntp_socket;
	tmcast_socket cfg_socket;
	tmcast_socket sn_socket;

	// Helpers
	int ret;
	socklen_t len;

	// The parameters are not used
	(void)pvParameters;

	// Initialize task members
	prvMulticastInitTask();

	// Setup socket X
	x_socket.port = multicastPORT_X;
	if (prucMulticastSetupSocket(&x_socket) == 0) {
		print_dbg("multicast.c: X socket creation failed.\n");
	}

	// Setup socket Y
	y_socket.port = multicastPORT_Y;
	if (prucMulticastSetupSocket(&y_socket) == 0) {
		print_dbg("multicast.c: Y socket creation failed.\n");
	}

	// Setup socket Z	
	z_socket.port = multicastPORT_Z;
	if (prucMulticastSetupSocket(&z_socket) == 0) {
		print_dbg("multicast.c: Z socket creation failed.\n");
	}

	// Task loop
	while (1) {
		// Clear descriptor set
		FD_ZERO(&fd);
		// Add sockets to descriptor set
		FD_SET(x_socket.socket, &fd);
		FD_SET(y_socket.socket, &fd);		
		FD_SET(z_socket.socket, &fd);
		
		// Check for received packets on configured sockets (blocking indefinite)
		ret = select(FD_SETSIZE, &fd, NULL, NULL, NULL);

		if (ret < 0) {
			print_dbg("multicast.c: Select failed.\n");
		}
		if (ret == 0) {
			print_dbg("multicast.c: Select timeout.\n");
		}
		if (ret > 0) {
			if (FD_ISSET(x_socket.socket, &fd)) {
				// Receive X packet
				len = sizeof(x_socket.addr);
				if (recvfrom(x_socket.socket, pucMulticastRecBuffer_X, multicastBUFFER_SIZE_X, 0, (struct sockaddr *)&(x_socket.addr), &len) >= 0) {
					prvMulticastProcessReply_X();
					print_dbg("Multicast.c: X packet received.\n");
				}
			}
			if (FD_ISSET(y_socket.socket, &fd)) {
				// Receive Y packet
				len = sizeof(y_socket.addr);
				if (recvfrom(y_socket.socket, pucMulticastRecBuffer_Y, multicastBUFFER_SIZE_Y, 0, (struct sockaddr *)&(y_socket.addr), &len) >= 0) {
					prvMulticastProcessReply_Y();
					print_dbg("Multicast.c: Y packet received.\n");
				}
			}
			if (FD_ISSET(z_socket.socket, &fd)) {
				// Receive Z packet
				len = sizeof(z_socket.addr);
				if (recvfrom(z_socket.socket, pucMulticastRecBuffer_Z, multicastBUFFER_SIZE_Z, 0, (struct sockaddr *)&(z_socket.addr), &len) >= 0) {
					prvMulticastProcessReply_Z();
					print_dbg("Multicast.c: Z packet received.\n");
				}
			}
		}
	}
	// Leave multicast group and close sockets (never executed)
	setsockopt(x_socket.socket, IPPROTO_IP, IP_DROP_MEMBERSHIP, &(x_socket.mreq), sizeof(ip_mreq));
	close(x_socket.socket);
	setsockopt(y_socket.socket, IPPROTO_IP, IP_DROP_MEMBERSHIP, &(y_socket.mreq), sizeof(ip_mreq));
	close(y_socket.socket);	
	setsockopt(z_socket.socket, IPPROTO_IP, IP_DROP_MEMBERSHIP, &(z_socket.mreq), sizeof(ip_mreq));
	close(z_socket.socket);
}

/**
 * Initialize task members.
 */
static void prvMulticastInitTask() {
	// Initialize buffers
	memset(&ucMulticastRecBuffer_X, 0, multicastBUFFER_SIZE_X);
	memset(&ucMulticastRecBuffer_Y, 0, multicastBUFFER_SIZE_Y);
	memset(&ucMulticastRecBuffer_Z, 0, multicastBUFFER_SIZE_Z);
}

/**
 * Process X reply.
 */
static void prvMulticastProcessReply_X(void) {
	// TODO
}

/**
 * Process Y reply.
 */
static void prvMulticastProcessReply_Y(void) {
	// TODO
}

/**
 * Process Z reply.
 */
static void prvMulticastProcessReply_Z(void) {
	// TODO
}

static U8 prucMulticastSetupSocket(void* mcast) {
	// Local pointer
	tmcast_socket *p_mcast = (tmcast_socket*)mcast;

	// Create socket (same as UDP socket)
	if ((p_mcast->socket = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
		return 0;
	}

	// Set up address
	memset(&(p_mcast->addr), 0, sizeof(struct sockaddr_in));
	p_mcast->addr.sin_family = AF_INET;
	p_mcast->addr.sin_addr.s_addr = htonl(INADDR_ANY);
	p_mcast->addr.sin_port = htons(p_mcast->port);

	// Bind to address
	if (bind(p_mcast->socket, (struct sockaddr*)&(p_mcast->addr), sizeof(struct sockaddr_in)) < 0) {
		return 0;
	}

	// Join multicast group
	p_mcast->mreq.imr_multiaddr.s_addr = inet_addr(pcConfctrlGetAddrPtr());
	p_mcast->mreq.imr_interface.s_addr = htonl(INADDR_ANY);
	if (setsockopt(p_mcast->socket, IPPROTO_IP, IP_ADD_MEMBERSHIP, &(p_mcast->mreq), sizeof(ip_mreq)) < 0) {
		return 0;
	}

	// Socket setup successful
	return 1;
}