#include "TrajectoryExecutor.h"

#include <ImFusion/Base/Mat.h>
#include <ImFusion/Base/Log.h>
#include <IIWA/IIWAMsg.h>

#include <chrono>

TrajectoryExecutor::TrajectoryExecutor(Trajectory* t, ROSNode* n, ImFusion::AlgorithmController* _usC, ImFusion::TrackingStream* _ts):
	  trajectory(t), ros_node(n), usC(dynamic_cast<ImFusion::CAMPIGTLinkIOAlgorithmController*>(_usC)), ts(_ts) {
	finished = true;
}

void TrajectoryExecutor::run() {
	if (finished) {
	    finished = false;
	    trajectoryThread = std::thread(&TrajectoryExecutor::execute, this);
	}
}

void TrajectoryExecutor::execute() {
	LOG_INFO("TE execute");

	// Orientation remains constant during movement
	Eigen::Quaterniond orientation = trajectory->getOrientation();

	char* orientationMsg = new char[200];
	sprintf(orientationMsg, "Adjusting orientation: [%.2f, %.2f, %.2f, %.2f]",  orientation.x(),  orientation.y(),  orientation.z(), orientation.w());
	LOG_INFO(orientationMsg);

	// Send message with initial position and orientation, then wait
	// for robot to adjust

	Eigen::Vector3d start = trajectory->H;

	ros_node->publishMessage(start, orientation);

	while (trajectory->hasNext()) {
		Eigen::Vector3d position = trajectory->next();

		ros_node->publishMessage(position, orientation);

		int sleepTime = 1000*trajectory->getDeltaTime();

		char* pointMsg = new char[200];
		sprintf(pointMsg, "Moving to position: [%.2f, %.2f, %.2f]", position.x(), position.y(), position.z());
		LOG_INFO(pointMsg);

		std::this_thread::sleep_for(std::chrono::milliseconds(sleepTime));

		// Check if ultrasound should be acquired
		if (trajectory->pointA()) {
			usC->startNewSweepWithExteralTracking(ts);
		} else if (trajectory->pointB()) {
			usC->startNewSweepWithExteralTracking(ts);
		}
	}

	finished = true;
}

bool TrajectoryExecutor::isFinished() {
	return finished;
}