#include "p_serial_bus.h"
#include "p_serial_packet.h"
#include "p_util.h"
#include <iostream>
#include <signal.h>
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <zmq.hpp>

#ifdef RUN_TESTS
#include "tests.h"
#endif

#define SERIAL_DEV ("/dev/ttyAMA1")
#define ZMQ_IPC_ADDR ("ipc:///penguinator/pubsub")

int x = 0;
int y = 0;

bool b_quit = false;

void sigint_handler(int param)
{
	(void)param;
	b_quit = true;
	std::cout << "Exiting..." << std::endl;
	return;
}

int main()
{
#ifdef RUN_TESTS
	run_tests();
	return 0;
#endif
	signal(SIGINT, sigint_handler);

	ps_bus sbus(SERIAL_DEV);
	zmq::context_t ctx;
	zmq::socket_t sub(ctx, ZMQ_SUB);
	sub.connect(ZMQ_IPC_ADDR);
	sub.setsockopt(ZMQ_SUBSCRIBE, "", 0);

	while (!b_quit)
	{
		zmq::message_t msg_recv;
		int32_t x = 0;
		int32_t y = 0;
		try
		{
			if (sub.recv(msg_recv, zmq::recv_flags(0)))
			{
				uint8_t xy[64];
				memset(xy, 0, 64);
				memcpy(xy, msg_recv.data(), msg_recv.size());
				sscanf((const char*)xy, "X:%" PRId32 " Y:%" PRId32 "\n", &x,
					   &y);
				ps_packet xy_packet = ps_packet::from_xy(x, y);
				xy_packet.encode();
				// xy_packet.show_packet();
				if (!sbus.send_pkt(&xy_packet))
				{
					printf("Some failure on transmission\n");
				}
			}
		}
		catch (zmq::error_t& e)
		{

			std::cout << e.what() << std::endl;
		}
	}

	return 0;
}