#include "DataListener.h"

#include "IndoorNewton.h"

#include <string>
#include <vector>

#include "ros/ros.h"
#include <ros/package.h>

bool compare_anchor_data(const anchor_raw_data& a, const anchor_raw_data& b)
{
	if ((a.from == b.from && a.to < b.to) || a.from < b.from)
		return true;

	return false;
}

DataListener::DataListener()
{
}

DataListener::~DataListener()
{
}

void DataListener::setOffset(double x, double y, double z)
{
	//printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
	offset.x += x;
	offset.y += y;
	offset.z += z;

	_writeToYAML();
}

void DataListener::_LOC_Debugger(std::string value)
{
	unsigned int source, dest, distance;
	sscanf(value.c_str(), "$%u/%u/%u", &source, &dest, &distance);
	distance -= 500;

	anchors_raw.push_back(anchor_raw_data(source, dest, distance));

	//printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);

	//Reset Timer
	locTimer = std::chrono::high_resolution_clock::now();
}

bool DataListener::_check_data_validity()
{
	if (anchors_raw.size() == 0)
		return false;

	for (unsigned int i = 0; i < 5; i++)
	{
		for (unsigned int j = i + 1; j < 6; j++)
		{
			if (_get_distance_average(i, j) == -1)
				return false;
		}
	}

	return true;
}

double DataListener::_get_distance_average(unsigned int anchor_from, unsigned int anchor_to)
{
	unsigned int sum = 0;
	unsigned int count = 0;
	std::list<anchor_raw_data>::iterator it;
	for (it = anchors_raw.begin(); it != anchors_raw.end(); it++)
	{
		//Not a measurement from and to
		if (it->from != anchor_from || it->to != anchor_to)
			continue;

		if (it->distance < 10000) //Simple filter -> if the distance is less than 10m = 10'000mm
		{
			sum += it->distance;
			count++;
		}
	}

	//FAILED
	if (count == 0 || sum == 0)
		return -1;

	//Divide by 100 to get it in dm
	return ((double)sum / (double)count) / 100.0;
}


////////////////////////////////////////////////////////////
/*

I KNOW, THIS NEWTON ALGORITHM IS MADE FOR 6 ANCHORS. NOT THE BEST SOLUTION WHEN USING ONLY 3 TO DETERMINE
THE POSITION OF THE OTHER ANCHORS, BUT HEY, IT WORKS xD

*/

bool DataListener::_calculateAnchorPosition()
{
	if (!_check_data_validity())
		return false;

	anchors_raw.sort(compare_anchor_data);

	double d01, d02, d12;
	d01 = _get_distance_average(0, 1);
	d02 = _get_distance_average(0, 2);
	d12 = _get_distance_average(1, 2);

	double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
	double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));

	double anchor_positions[18] = {0, 0, 0, \
	                               x, -y, 0, \
	                               d02, 0, 0, \
	                               0, 0, 0, \
	                               x, -y, 0, \
	                               d02, 0, 0
	                              };

	anchor_position.push_back(anchor_position_data(0, anchor_positions[0], anchor_positions[1], anchor_positions[2]));
	anchor_position.push_back(anchor_position_data(1, anchor_positions[3], anchor_positions[4], anchor_positions[5]));
	anchor_position.push_back(anchor_position_data(2, anchor_positions[6], anchor_positions[7], anchor_positions[8]));

	//For anchor n we only need d0n, d1n, d2n
	for (unsigned int n = 3; n < 6; n++)
	{
		double anchor_distances[6] = { _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n), \
		                               _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n)
		                             };


		//Calculate position of anchor n using IndoorNewton
		double result[3];
		IndoorNewton(anchor_positions, anchor_distances, result, 200);

		anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2]));
	}

	//Save anchor positions to yaml file
	_writeToYAML();

	finishedLoc = true;
	return true;
}

void DataListener::_resetLocalisation()
{
	serial.closeConnection();

	anchors_raw.clear();
	anchor_position.clear();

	//offset.x = 0;
	//offset.y = 0;
	//offset.z = 0;

	finishedLoc = false;
}

bool DataListener::_startSerialCommunication()
{
	if (!serial.startConnection("/dev/ttyACM0")) return false;
	if (!serial.writeSerial("loc100\r")) return false;

	locTimer = std::chrono::high_resolution_clock::now();
	while (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - locTimer).count() < 2000) {
		if (!serial.manageSerial())
			return false;

		std::string value = serial.readSerial();
		while (value != "") {
			if (value.at(0) == '$')
				_LOC_Debugger(value);

			value = serial.readSerial();
		}
	}

	return true;
}

bool DataListener::startAnchorLocalistaion()
{
	_resetLocalisation();

	for (unsigned int i = 0; i < 5 && !finishedLoc; i++)
	{
		if (i != 0)
			ROS_WARN("[Selflocalisation] Looks like something went wrong... %u tries left\n", 5 - i);

		_resetLocalisation();
		if (!_startSerialCommunication())
			continue;
		if (!_calculateAnchorPosition()) {
			ROS_ERROR("[Selflocalisation] One or more distances are missing");
			continue;
		}
	}
	serial.closeConnection();

	if (finishedLoc)
		return true;

	return false;
}

void DataListener::invertAnchor(unsigned int id)
{
	anchor_position[id].z *= -1;

	_writeToYAML();
}

void DataListener::_writeToYAML()
{
	ros::NodeHandle nodeHandle("~");

	//std::vector<float> anchorArr;
	std::vector<double> anchorArr;

	for (unsigned int i = 0; i < anchor_position.size(); i++)
	{
		//printf("anchor%i (%f, %f, %f)\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
		anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
		nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
	}
}