#include <err.h>
#include <string.h>
#include <stdio.h>

#include "frontend_tracker.h"

#define MAGIC "TRACKERDATA"

int frontend_track(
		struct backend_conn * conn,
		struct frontend_tracker_data * data)
{
	ucart_backendWrite(conn, MAGIC "\n");

	char * line;
	for (;;) {
		line = ucart_backendGetline(conn);
		if (line == NULL) {
			warnx("Line not returned from backend");
			return 1;
		}

		if (strncmp(line, MAGIC, strlen(MAGIC)) == 0) {
			break;
		}
	}

	if (strncmp(line, MAGIC " ERROR", strlen(MAGIC " ERROR")) == 0) {
		warnx("Backend returned an error: %s", strstr(line, "ERROR"));
		return 1;
	}

	/* Line format: Height Lat Long Pitch Roll Yaw */
	sscanf(line, MAGIC " %lf %lf %lf %lf %lf %lf ",
			&data->height,
			&data->lateral,
			&data->longitudinal,
			&data->pitch,
			&data->roll,
			&data->yaw);
	return 0;
}