#include <assert.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <linux/phonet.h>

#define BUFFER_SIZE 4096

static
int sck;
static
struct sockaddr_pn sa;

static
void
sigShutdown(int signum) {
	printf("Shutdown signal received\n");
	assert(28 == sendto(sck, "\0\220\1\1\0\0\0\0\0\0\0\5\t\1\0\20\0\0\0\1\0\0\0\3\0\n\0\0", 28, 0, (struct sockaddr*)&sa, sizeof(sa)));
	assert(12 == sendto(sck, "\0\220\2\0\0\0\0\0\0\0\0\5", 12, 0, (struct sockaddr*)&sa, sizeof(sa)));
	exit(0);
}

int
main() {
	signal(SIGHUP, sigShutdown);
	signal(SIGINT, sigShutdown);
	signal(SIGQUIT, sigShutdown);
	signal(SIGPIPE, sigShutdown);
	
	sck = socket(AF_PHONET, SOCK_DGRAM, 0);
	assert(sck >= 0);
	bzero(&sa, sizeof(sa));
	sa.spn_family = AF_PHONET;
	sa.spn_obj = 0;
	sa.spn_dev = 0;
	sa.spn_resource = 255;
	assert(!bind(sck, (struct sockaddr*)&sa, sizeof(sa)));
	
	sa.spn_resource = 16;
	assert(4 == sendto(sck, "\0\20\1T", 4, 0, (struct sockaddr*)&sa, sizeof(sa)));
	sa.spn_resource = 84;
	assert(28 == sendto(sck, "\0\220\0\1\0\0\0\0\0\0\0\0\t\1\0\20\0\0\0\n\0\0\0\3\0\0\0\0", 28, 0, (struct sockaddr*)&sa, sizeof(sa)));
	
	char buf[BUFFER_SIZE];
	while (1)
	{
		puts("");
		ssize_t buflen = recv(sck, buf, sizeof(buf), 0);
		assert(buflen > 0);
		printf("Read %d bytes\n", buflen);
		
		if (buflen < 0x14) continue;
		uint16_t date_year = (buf[0x10] << 8) | buf[0x11];
		uint8_t date_month = buf[0x12];
		uint8_t date_day = buf[0x13];
		printf("Date: %04d-%02d-%02d\n", date_year, date_month, date_day);
		
		if (buflen < 0x1c) continue;
		uint8_t time_hour = buf[0x15];
		uint8_t time_minute = buf[0x16];
		uint16_t time_ms = (buf[0x18] << 8) | buf[0x19];
		uint8_t time_second = time_ms / 1000;
		time_ms %= 1000;
		uint16_t time_accuracy = (buf[0x1a] << 8) | buf[0x1b];
		printf("Time: %02d:%02d:%02d.%03d (accuracy: %d)\n", time_hour, time_minute, time_second, time_ms, time_accuracy);
		
		if (buflen < 0x23) continue;
		float lat = 360. * (((float)buf[0x20] / 0x100) + ((float)buf[0x21] / 0x10000) + ((float)buf[0x22] / 0x1000000));
		if (lat > 180.) lat -= 360.;
		printf("Latitude : %f\n", lat);
		
		if (buflen < 0x27) continue;
		float lon = 360. * (((float)buf[0x24] / 0x100) + ((float)buf[0x25] / 0x10000) + ((float)buf[0x26] / 0x1000000));
		if (lon > 180.) lon -= 360.;
		printf("Longitude: %f\n", lon);
		
		if (buflen < 0x30) continue;
		uint16_t eph = (buf[0x2e] << 8) | buf[0x2f];
		printf("eph: %dcm\n", eph);
		
		if (buflen < 0x36) continue;
		uint16_t altiA = (buf[0x32] << 8) | buf[0x33];
		uint16_t altiB = (buf[0x36] << 8) | buf[0x37];
		float alti = (float)(altiA - *((int16_t*)&altiB)) / 2;
		float epv2 = ((buf[0x34] << 8) | buf[0x35]) / 2;
		printf("Altitude: %.1fm (accurate to %.1f?)\n", alti, epv2);
		
		if (buflen < 0x40) continue;
		float track = ((buf[0x3c] << 8) | buf[0x3e]) / 100.;
		float epd = ((buf[0x3e] << 8) | buf[0x3f]) / 100.;
		printf("Direction of motion: %.2f degrees (accurate to %.2f degrees?)\n", track, epd);
		
		if (buflen < 0x46) continue;
		uint16_t speed = (buf[0x42] << 8) | buf[0x43];
		uint16_t eps = (buf[0x44] << 8) | buf[0x45];
		printf("Speed: %d cm/sec (accurate to %d cm/sec)\n", speed, eps);
		
		if (buflen < 0x4b) continue;
		uint16_t climb = (buf[0x46] << 8) | buf[0x47];
		uint16_t epc = (buf[0x49] << 8) | buf[0x4a];
		printf("Climb: %d cm/sec (accurate to %d cm/sec)\n", climb, epc);
	}
}