#pragma once

#include "SerialUnix.h"
#include "SerialCommon.h"

#include <list>
#include <vector>																																				
#include <string>
#include <stdio.h>
#include <cmath>
#include <chrono>

struct anchor_raw_data
{
	unsigned int from, to, distance;
	anchor_raw_data(unsigned int f, unsigned int t, unsigned int d) : from(f), to(t), distance(d) {}
};

struct anchor_position_data
{
	unsigned int id;
	double x, y, z;
	anchor_position_data(){}
	anchor_position_data(unsigned int _id, double _x, double _y, double _z) : id(_id), x(_x), y(_y), z(_z) {}
};

class DataListener
{
public:
	DataListener();
	~DataListener();

	void setOffset(double x, double y, double z);
	bool startAnchorLocalistaion();
	void invertAnchor(unsigned int id);

private:
	Serial serial;
	std::list<anchor_raw_data> anchors_raw;
	std::vector<anchor_position_data> anchor_position;
	anchor_position_data offset;

	std::chrono::high_resolution_clock::time_point locTimer;
	bool finishedLoc;

	bool _check_data_validity();
	double _get_distance_average(unsigned int anchor_from, unsigned int anchor_to);
	void _resetLocalisation();
	void _LOC_Debugger(std::string value);

	bool _startSerialCommunication();
	bool _calculateAnchorPosition();

	void _writeToYAML();
};