/*
 *
 *	Localization Server
 *
 *	This node is subscribed to the ViconDataPublisher as well
 *	as to the UWBDataPublisher. It is used to relay the data
 *	of the desired localization source to the PPSClient and
 *	can compare the data provided by both.
 *
 */

#include <chrono>
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>

#include "LocalizationServer.h"

#include "d_fall_pps/CMQuery.h"
#include "d_fall_pps/Anchors.h"

#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Float32.h"

using namespace d_fall_pps;

// the clientID defines namespace and identifier in the CentralManagerService
int clientID;

// Self explanatory variables
ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher localizationPublisher;
CrazyflieContext context;
rosbag::Bag bag;

bool enableUWB = false;
bool useUWB = false;

// store resent localisation updates
CrazyflieData CFData_vicon;
CrazyflieData CFData_uwb;

int main(int argc, char* argv[])
{
	// Initialize the node and it's handles
	ros::init(argc, argv, "LocalizationServer");
	ros::NodeHandle nodeHandle("~");
	ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();

	// load context parameters
	if(!loadParameters(nodeHandle))
		return -1;

	// connect to the central manager service to receive CF Context and subscribe to CF Context updates of the DB
	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
	loadCrazyflieContext();
	ros::Subscriber dbChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, dbChangedCallback);

	uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);

	ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
	//ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
	loadUWBSettings();

	// subscribe to the global vicon publisher and local uwb publisher
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconDataCallback);
	ROS_INFO_STREAM("[LocalizationServer] subscribed to Vicon");
	ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
	ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");

	//TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here

	// initialize the position data publisher
	localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);

	std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	std::string record_file = package_path + "log/LocalizationServerLog.bag";
	bag.open(record_file, rosbag::bagmode::Write);

	ROS_INFO_STREAM("[LocalizationServer] started!");

	ros::spin();
	bag.close();

	return 0;
}

bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
	if(!nodeHandle.getParam("clientID", clientID))
	{
		ROS_ERROR("[LocalizationServer] Failed to get clientID!");
		return false;
	}

	return true;
}

void d_fall_pps::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = clientID;

	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall))
	{
		context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("[LocalizationServer] context updated");
	}
}

void d_fall_pps::loadUWBSettings()
{
	Anchors a;

	if(uwbManager.call(a))
	{
		enableUWB = a.response.enableUWB;
	}
	else
	{
		enableUWB = false;
		ROS_ERROR("[LocalizationServer] Could not update UWB Settings!");
	}

	if(enableUWB)
		ROS_WARN("[LocalizationServer] Use of UWB activated!");
	else
		ROS_WARN("[LocalizationServer] Use of UWB deactivated!");
}

void tiagosPrint(double x, double y, double z)
{
	// Anchor2
	//double anchorPos[3] = {1.89, 0.61, 1.6};

	// Anchor5
	double anchorPos[3] = {-0.76, -1.36, 0.6};

	/*ROS_WARN("Dist to A5: %lf", sqrt(
									(x - anchorPos[0])*(x - anchorPos[0])
									+ (y - anchorPos[1])*(y - anchorPos[1])
									+ (z - anchorPos[2])*(z - anchorPos[2])
								)
	);*/
}

void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
	{
		CrazyflieData data = *it;

		if(data.crazyflieName == context.crazyflieName)
		{
			if(! (enableUWB && useUWB))
				localizationPublisher.publish(data);

			CFData_vicon = data;
			bag.write("ViconData", ros::Time::now(), data);


			// leave the for-loop so not every vector element has to be compared, once the matching context is found
			break;
		}
	}

	tiagosPrint(CFData_vicon.x, CFData_vicon.y, CFData_vicon.z);
}

// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
	// override UWB Data
	CFData_uwb = data;

	//ROS_WARN("Roll: \tV: %f\tU: %f", CFData_vicon.roll, data.roll);
	//ROS_WARN("E(R): V-U = %f", (CFData_vicon.roll - data.roll) * 180/3.14159);

	// Uncomment the lines to be overriden by vicon when using UWB
	//CFData_uwb.x = CFData_vicon.x;
	//CFData_uwb.y = CFData_vicon.y;
	//CFData_uwb.z = CFData_vicon.z;
	//CFData_uwb.roll = CFData_vicon.roll;
	//CFData_uwb.pitch = CFData_vicon.pitch;
	//CFData_uwb.pitch *= -1;
	//CFData_uwb.yaw = CFData_vicon.yaw;

	if(enableUWB && useUWB)
	{
		localizationPublisher.publish(CFData_uwb);
	}
	
	bag.write("UWBData", ros::Time::now(), CFData_uwb);
}

void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
	d_fall_pps::loadCrazyflieContext();
}

void d_fall_pps::uwbChangedCallback(const std_msgs::Int32 &msg)
{
	useUWB = msg.data;

	if(useUWB)
		ROS_WARN("[LocalizationServer] UWB activated!");
	else
		ROS_WARN("[LocalizationServer] UWB deactivated!");
}