/* -----------------------------------------------------------------------------
 * 
 *
 *  ____                   _____     _      
 * / ___|  __ _ _ __  _ __|__  /  __| | ___ 
 * \___ \ / _` | '_ \| '_ \ / /  / _` |/ _ \
 *  ___) | (_| | |_) | |_) / /_ | (_| |  __/
 * |____/ \__,_| .__/| .__/____(_)__,_|\___|
 *             |_|   |_| 
 * 
 *
 * 
 * Author: Andreas Gschossmann
 * Email:  andreas.gschossmann@hs-regensburg.de
 * 
 * mavlink_binary.c - Sample Code to test serialize and deserialize a MAVlink
 * message.
 * 1. pack attitude message
 * 2. save binary to a file
 * 3. read binary from the same file
 * 4. decode attitude message  
 *
 * Instead of saving the binary to a file, it could also be sent serially over 
 * an I/O (websocket, serial port, ...).
 * 
 * ----------------------------------------------------------------------------*/

// TODO: In Makefile automatisch Mavlink-Headers mit Konsolen-Tool mavgen erweitern

#include <stdio.h>

#include <mavlink/common/mavlink.h>

#if (defined __QNX__) | (defined __QNXNTO__)
/* QNX specific headers */
#include <unix.h>
#else
/* Linux / MacOS POSIX timer headers */
#include <sys/time.h>
#include <time.h>
#include <arpa/inet.h>
#endif 

#define BUFFER_LENGTH 108 // minimum buffer size that can be used with qnx (I don't know why)

uint64_t microsSinceEpoch();

void main(void)
{
	int i = 0;
	uint16_t len=0;

	mavlink_message_t msg;
	mavlink_status_t status;

	uint8_t buf[BUFFER_LENGTH];

  ////////////////
  // write to file
	//
	FILE *fp = fopen("test.bin", "wb");
	if (fp == NULL) {
		printf("File does not exist, please check!\n");
	}

	// pack attitude
	mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
	len = mavlink_msg_to_send_buffer(buf, &msg);
	fwrite(buf, 1, len, fp);
	fclose(fp);

	fp = fopen("test.bin", "ab");
	mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 2.2, 2.7, 4.14, 0.01, 0.02, 0.03);
  len = mavlink_msg_to_send_buffer(buf, &msg);
	fwrite(buf, 1, len, fp);
	fclose(fp);

  fp = fopen("test.bin", "ab");
  mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 3.2, 3.7, 5.14, 0.01, 0.02, 0.03);
  len = mavlink_msg_to_send_buffer(buf, &msg);
  fwrite(buf, 1, len, fp);
	fclose(fp);	

	/////////////////
	// read from file
	//
	fp = fopen("test.bin", "rb");

	// read from file
	fread(buf, sizeof(buf), 1, fp);

	// close file	(read)
//	fclose(fp);

	// read buffer and 
	for (i = 0; i < BUFFER_LENGTH; i++)
	{
		if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
		{
			// package received
			printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n\n", msg.sysid, msg.compid, msg.len, msg.msgid);

			// swich ID to detect message type
			switch(msg.msgid) 
			{
				// decode attitude
				case MAVLINK_MSG_ID_ATTITUDE:
				{
					mavlink_attitude_t attitude;
					mavlink_msg_attitude_decode(&msg, &attitude);
					printf("attitude time: %d:\n", attitude.time_boot_ms);
					printf("roll: %f:\n", attitude.roll);
					printf("pitch: %f:\n", attitude.pitch);
					printf("yaw: %f:\n\n", attitude.yaw);
				}
				break;
				/*case MAVLINK_MSG_ID_SOMEOTHERMSG:
				{	
					mavlink_someothermsg_t someothermsg;
					mavlink_msg_someothermsg_decode(&msg, &someothermsg);
					do stuff ...
				}
				break;*/
			}

		}

	}

  // close file (read)
  fclose(fp);


}

/* QNX timer version */
#if (defined __QNX__) | (defined __QNXNTO__)
uint64_t microsSinceEpoch()
{

  struct timespec time;

  uint64_t micros = 0;

  clock_gettime(CLOCK_REALTIME, &time);
  micros = (uint64_t)time.tv_sec * 1000000 + time.tv_nsec/1000;

  return micros;
}
#else
uint64_t microsSinceEpoch()
{

  struct timeval tv;

  uint64_t micros = 0;

  gettimeofday(&tv, NULL);
  micros =  ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;

  return micros;
}
#endif
