/* -----------------------------------------------------------------------------
 * 
 *
 *  ____                   _____     _      
 * / ___|  __ _ _ __  _ __|__  /  __| | ___ 
 * \___ \ / _` | '_ \| '_ \ / /  / _` |/ _ \
 *  ___) | (_| | |_) | |_) / /_ | (_| |  __/
 * |____/ \__,_| .__/| .__/____(_)__,_|\___|
 *             |_|   |_| 
 * 
 *
 * 
 * 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 

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

uint64_t microsSinceEpoch();

void main(void)
{
	int i = 0;

	mavlink_message_t msg;
	mavlink_message_t rmsg, rmsgbuf;
	mavlink_status_t status,statbuf;

	uint8_t buf[BUFFER_LENGTH];
	uint16_t len = 0;
	uint8_t statarr[BUFFER_LENGTH];

	for(i=0; i<BUFFER_LENGTH; i++) {
		buf[i]=0;
	}
	i = 0;

	FILE *fp;

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

	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);
	printf("size: %lu, %d\r\n", sizeof(msg), len);

  for(i=0; i<BUFFER_LENGTH; i++) {
    buf[i]=0;
  }
  i = 0;

//	printf("size: %d", sizeof(msg));
	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);
	printf("size: %lu\n", sizeof(msg));

  for(i=0; i<BUFFER_LENGTH; i++) {
    buf[i]=0;
  }
  i = 0;

//	printf("size: %d", sizeof(msg));
  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);
	printf("size: %lu\n", sizeof(msg));
*/

  for(i=0; i<BUFFER_LENGTH; i++) {
    buf[i]=0;
  }
  i = 0;

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

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

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

	// read buffer and 

	int dz, di, db;
  dz=di=db=0;
	int j=0;
	statbuf=status;
  rmsgbuf=rmsg;
	uint8_t *slayer; 

  uint8_t bitbuf;

  for(j=0; j<BUFFER_LENGTH-1; j++) {
		//printf("vorher: %x\r\n",buf[i]);
		buf[j] = buf[j]<<3;
		buf[j] |= buf[j+1]>>5;
		//printf("nachher: %x\r\n",buf[i]);
  }


  for(di=100; di<101; di++) {
		dz=0;
		status.parse_state=0;
		//for(j=0;j<sizeof(status);j++) { slayer=(&status+j); *slayer=0; }
		//for(j=0;j<sizeof(status);j++) { slayer=(&rmsg+j); *slayer=0; }
		for (i = 0; i <= di; i++)
		{
			if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &rmsg, &status))
			{
				// package received
				printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n\n", rmsg.sysid, msg.compid, msg.len, msg.msgid);
	
				// swich ID to detect message type
				dz++;
				switch(rmsg.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_someotherrmsg_t someothermsg;
						mavlink_rmsg_someothermsg_decode(&msg, &someothermsg);
						do stuff ...
					}
					break;*/
				}
	
			}
			statarr[i]=status.parse_state;

	  }
		if(dz>0) {
			for(j=0;j<=i;j++) printf("%d.",statarr[j]);
			printf("\r\n");
		}
		if(db != dz) {
			printf("Sprung bei Byte %d, von %d auf %d\r\n",di+1,db,dz);
			//for(j=0;j<=i;j++) printf("%d.",statarr[j]);
			//printf("\r\n");
		}
		//for(j=0;j<i;j++) printf("%d.",statarr[j]);
		//printf("\r\n");
		db=dz;
  }

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

int counter;

for(counter=0; counter<BUFFER_LENGTH; counter++) {
	printf("%d ", buf[counter]);
}

}

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