#ifndef _TRAJECTORY_EXECUTOR_H
#define _TRAJECTORY_EXECUTOR_H

#include <ImFusion/US/TrackingStream.h>
#include <ImFusion/GUI/AlgorithmController.h>

#include "Trajectory.h"
#include "ROSNode.h"

#include <thread>

class Ui_DemoController;

/* Sends messages to the robot corresponding to sequential locations
along a trajectory from H->A->B->H, where H is the initial position of the robot
and A and B are two additional key points that define the trajectory. */

namespace ImFusion
{
	class __declspec(dllimport) CAMPIGTLinkIOAlgorithmController
	{
	public:
		void startNewSweepWithExteralTracking(TrackingStream* ts); //Add to TrajectoryExecutor
	};
}

class TrajectoryExecutor
{
public:
	/* Create a new instance with the given trajectory and ROS node */
	TrajectoryExecutor(Trajectory* t, ROSNode* n, ImFusion::AlgorithmController* usC, ImFusion::TrackingStream* ts);
	/* Create a new thread that executes the trajectory */
	void run();
	/* Execute the trajectory on the current thread */
	void execute();
	/* Whether or not the trajectory is done being executed */
	bool isFinished();
protected:
	/* ROS node for sending messages */
	ROSNode* ros_node;
	/* Trajectory to be executed */
	Trajectory* trajectory;
	/* Acquires ultrasound */
	ImFusion::CAMPIGTLinkIOAlgorithmController* usC;
	/* Tracking stream */
	ImFusion::TrackingStream* ts;
	/* Thread for executing trajectory */
	std::thread trajectoryThread;
	/* Whether or not the trajectory is still being executed */
	volatile bool finished;
};

#endif