/* -----------------------------------------------------------------------------
 * 
 *
 *  ____                   _____     _      
 * / ___|  __ _ _ __  _ __|__  /  __| | ___ 
 * \___ \ / _` | '_ \| '_ \ / /  / _` |/ _ \
 *  ___) | (_| | |_) | |_) / /_ | (_| |  __/
 * |____/ \__,_| .__/| .__/____(_)__,_|\___|
 *             |_|   |_| 
 * 
 *
 * 
 * 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 BLIBBL 2000 // minimum buffer size that can be used with qnx (I don't know why)
const int fuckyou=10000;

uint64_t microsSinceEpoch();

void main(void)
{
	int i = 0;
  int di = 0;
  int dz = 0;
	int odz = 0;

	mavlink_message_t msg;
	mavlink_status_t status,statbuf;

	uint8_t buf[fuckyou];

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

  
printf("Msize: %lu\r\n", sizeof(msg));
printf("char: %lu\r\n", sizeof(char));
printf("uint8_T: %lu\r\n", sizeof(uint8_t));
//return;


	// pack attitude
	mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
	fwrite(&msg, sizeof(uint8_t),50, fp);
//printf("Msize: %lu\r\n", sizeof(msg));
	fclose(fp);	
	mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 0.2, 2.7, 1.14, 0.01, 0.02, 0.03);
	fp = fopen("test.bin", "ab"); 
	fwrite(&msg, sizeof(uint8_t), 50, fp);
//printf("Msize: %lu\r\n", sizeof(msg));
	fclose(fp);
	mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 42, 2.7, 1.14, 0.01, 0.02, 0.03);
	fp = fopen("test.bin", "ab"); 
	fwrite(&msg, sizeof(uint8_t), 50, fp);
//printf("Msize: %lu\r\n", sizeof(msg));
	fclose(fp);
	mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 2.2, 3.7, 4.14, 0.01, 0.02, 0.03);

	// write binary to file
	fp = fopen("test.bin", "ab");
	fwrite(&msg, sizeof(uint8_t), sizeof(msg), fp);
//printf("Msize: %lu\r\n", sizeof(msg));
	
	// close file (write)
  fclose(fp);

  statbuf=status;

	/////////////////
	// read from file
	//
	fp = fopen("test.bin", "rb");
	// read from file
	fread(&buf, sizeof(uint8_t), fuckyou, fp);
  fclose(fp);

  for(di=0; di<fuckyou; di++) {

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

	// read buffer and 
		dz=0;
		status = statbuf;
	for (i = 0; i < di; i++)
	{
		////printf("i: %d\r\n",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:
				{
					dz++;
					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);
					/*experimental*/
					//status.parse_state = 0;
				}
				break;
				/*case MAVLINK_MSG_ID_SOMEOTHERMSG:
				{	
					mavlink_someothermsg_t someothermsg;
					mavlink_msg_someothermsg_decode(&msg, &someothermsg);
					//do stuff ...
				}
				break;*/
			}

		}
		////printf("st:%d, ", status.parse_state);
	}
		if(odz != dz) printf("Jump at fuckyou %d! Got %d, former %d\r\n",di,dz,odz);
		odz = dz;

  // close file (read)
}


}

/* 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
