/* Copyright (c) 2012-2015 ImFusion GmbH, Munich, Germany. All rights reserved. */
#ifndef IMFUSION_DEMO_CONTROLLER_H
#define IMFUSION_DEMO_CONTROLLER_H

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

#include <QtWidgets/QWidget>

#include <IIWA/IIWAMsg.h>

#include "DemoAlgorithm.h"
#include "ROSNode.h"
#include "Trajectory.h"
#include "TrajectoryExecutor.h"

//#include "C:\\Temp\\4Michael\\CAMPIGTLinkIOAlgorithmController.h"

class Ui_DemoController;

namespace ImFusion
{
	class DemoAlgorithm;

	/** \brief	Demonstrates usage of algorithm controllers
	*	\author	Alexander Ladikos
	*/
	class DemoController: public QWidget, public AlgorithmController
	{
		Q_OBJECT

	public:
		/// Constructor with the algorithm instance
		DemoController(DemoAlgorithm* algorithm);

		/// Destructor
		virtual ~DemoController();

		/// Initializes the widget
		void init();


		void updateOutput(mat4 outMat, double time);

		void onReceive(const IIWA::IIWAMsg::ConstPtr& msgPtr);

		mat4 poseFromMsg(std::vector<double>& position, std::vector<double>& orientation);
		
		double getCurrentTime();

		const IIWA::IIWAMsg::ConstPtr msgFromPose(mat4& pose);

		void moveTrajectory();

	public slots:
		/// Apply the chosen processing
		void onSend();
	    void executeTrajectory(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B);
		void moveTo(Eigen::Vector3d& point);
		void moveTo(std::vector<double>& point);
		Eigen::Vector3d getA();
		Eigen::Vector3d getB();
		Eigen::Vector3d getH();
		void onROSNodeNameChange();
		void onTopicNameChange();
		void onPositionXChange();
		void onPositionYChange();
		void onPositionZChange();
		void onOrientationXChange();
		void onOrientationYChange();
		void onOrientationZChange();
		void onOrientationWChange();
		void applyTransformation(Eigen::Vector3d& point, Eigen::Quaterniond& orientation);
	    void applyTransformation(Eigen::Vector3d& point, Eigen::Matrix3d& orientation);
	    void applyTransformation(std::vector<double>& point, std::vector<double>& orientation);

	protected:
		Ui_DemoController*	m_ui;	///< The actual GUI
		DemoAlgorithm* m_alg;		///< The algorithm instance

		ROSNode* node; // Communicates with robot

		SharedImageSet*		m_imgIn;		///< Input image to process
		UltrasoundSweep*	m_dataOut;		///< Output image after processing
		TrackingStream*		m_trackingStream;

		std::vector<double> currPosition;
		std::vector<double> currOrientation;

		bool acquireOutput;

		TrajectoryExecutor* executor; // Executes trajectory for visual servoing
		Trajectory* trajectory; // H -> A -> B
	};
}

#endif
