#include "netif/xadapter.h"
#include "lwipopts.h"
#include "lwip/init.h"
#include "lwip/inet.h"
#include "lwip/tcp.h"
#include "lwip/ip_addr.h"
#include "lwip/sockets.h"
#include "lwip/sys.h"

#include "project.h"
#include "radar_manager_icd.h"
#include "novatel.h"

#define BUF_SIZE	1024

#define PWRPAK_PORT 3003

uint32_t utc_time;

void novatel_task()
{
	int sockfd;

	if ((sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
	{
	   xil_printf("Socket creation error\n");
	}

	struct timeval tv;
	tv.tv_sec = 2;
	tv.tv_usec = 0;
	setsockopt(sockfd, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);

	struct sockaddr_in serv_addr;
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_port = htons(PWRPAK_PORT);

	inet_pton(AF_INET, "0.0.0.0", &serv_addr.sin_addr);

	if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) == -1) {
	   xil_printf("Bind Failed\r\n");
	}

	xil_printf("Novatel Socket Opened\r\n");

	static uint8_t buffer[BUF_SIZE];
	NovatelLogShortHdrType * short_hdr = (NovatelLogShortHdrType *)buffer;
	NovatelLogHdrType * long_hdr = (NovatelLogHdrType *)buffer;
	int n;
	socklen_t len;
	struct sockaddr_in servaddr;

	while (1) {
		n = recvfrom(sockfd, buffer, BUF_SIZE, 0, (struct sockaddr *) &servaddr, &len);

		if (n < 0) {
			if (errno == EAGAIN) {
				// This was just a timeout
				continue;
			}
			xil_printf("Socket Read Error %d\n", errno);
		}

		int msg_length = 0;
		if (short_hdr->sync == 0x1244aa) {
			// This is a long header
//			xil_printf("long  ");
			msg_length = long_hdr->msg_length;
		}
		else if (short_hdr->sync == 0x1344aa) {
			// This is a short header
//			xil_printf("short ");
			msg_length = short_hdr->msg_length;
		}
		else {
			xil_printf("Bad Header Sync, 0x%x\r\n", short_hdr->sync);
			continue;
		}

//		xil_printf("PwrPak Msg ID - %d, length %d, %d, %d\r\n", short_hdr->msg_id, msg_length, sizeof(NovatelInspvasType), sizeof(NovatelTimeType));

		if (n >= (msg_length + 16)) {

			switch (short_hdr->msg_id)
			{
				case PWRPAK_MSG_ID_TIME:
					{
						NovatelTimeType * msg = (NovatelTimeType *)buffer;
//						printf("GPS Week        %d\r\n", msg->hdr.week);
//						printf("GPS ms          %lu\r\n", msg->hdr.ms);
//						printf("offset          %f\r\n", msg->offset);
//						printf("offset_std_dev  %f\r\n", msg->offset_std_dev);
//						printf("utc_offset      %f\r\n", msg->utc_offset);

						uint32_t gps_sec = (msg->hdr.week * 7 * 24 * 60 * 60) + (msg->hdr.ms / 1000);
						uint32_t utc_sec = gps_sec + 315964800 + msg->utc_offset;

						utc_time = utc_sec;

//						xil_printf("gps_week        %lu\r\n", msg->hdr.week);
//						xil_printf("gps_ms          %lu\r\n", msg->hdr.ms);
//						xil_printf("utc_time        %lu\r\n", utc_sec);
					}
					break;

				case PWRPAK_MSG_ID_INSPVAS:
					{
						NovatelInspvasType * msg = (NovatelInspvasType *)buffer;

					}
					break;

				default:
					xil_printf("Unhandled PwrPak Msg ID - %d\r\n", short_hdr->msg_id);
					break;
			}
		}
	}
}

