#include "DemoController.h"
#include "DemoAlgorithm.h"

#include <ImFusion/Base/DataModel.h>
#include <ImFusion/GUI/MainWindowBase.h>
#include <ImFusion/Base/SharedImageSet.h>
#include <ImFusion/Base/Log.h>

#include <boost/bind.hpp>

#include "ui_DemoController.h"

#include <string>
#include <vector>


namespace ImFusion
{
	DemoController::DemoController(DemoAlgorithm* algorithm) : AlgorithmController(algorithm),
		m_alg(algorithm), m_trackingStream(new TrackingStream())
	{
		m_ui = new Ui_DemoController();
		m_ui->setupUi(this);

		m_ui->lineEdit_6->setText("550");
		m_ui->lineEdit_8->setText("100");
		m_ui->lineEdit_11->setText("390"); 
		m_ui->lineEdit_5->setText("550");
		m_ui->lineEdit_4->setText("-100");
		m_ui->lineEdit_7->setText("390");

		connect(m_ui->pushButton,SIGNAL(clicked()),this,SLOT(onSend())); // Send message

		// Default starting position
		currPosition = std::vector<double>(3);
		currPosition[0] = 500;
		currPosition[1] = 0;
		currPosition[2] = 500;

		// Create a ros node with the node name, publisher topic, and scubscriber topic
		// TODO Currently these are hard-coded but can be set dynamically in the future
	    node = new ROSNode("imfusion_node", "/iiwa/command", "/iiwa/state");

		// Add a callback function to subscriber topic
		boost::function<void (const IIWA::IIWAMsg::ConstPtr)> callback(boost::bind(&DemoController::onReceive, this, _1));
		node->addCallback(callback);

		// Whether or not output should be added to the tracking stream
		acquireOutput = true;
	}


	DemoController::~DemoController()
	{
		delete m_ui;
		delete node;
	}


	void DemoController::init()
	{
		addToAlgorithmDock();
	}

	void DemoController::updateOutput(mat4 outMat, double time)
	{
		if (m_trackingStream != nullptr) { // && m_dataOut != nullptr) {
			m_trackingStream->add(outMat, time);
			//LOG_INFO("Mat " << outMat);U
			//m_dataOut->notifyDataChanged();
		}
	}

	void DemoController::onSend()
	{
		Eigen::Vector3d A = getA();
		Eigen::Vector3d B = getB();
		Eigen::Vector3d H = getH();
		
		executeTrajectory(H, A, B);
	}

	Eigen::Vector3d DemoController::getA()
	{
		double Ax = std::stod(m_ui->lineEdit_6->text().toLocal8Bit().constData());
		double Ay = std::stod(m_ui->lineEdit_8->text().toLocal8Bit().constData());
		double Az = std::stod(m_ui->lineEdit_11->text().toLocal8Bit().constData());

		return Eigen::Vector3d(Ax, Ay, Az);
	}

	Eigen::Vector3d DemoController::getB()
	{
		double Bx = std::stod(m_ui->lineEdit_5->text().toLocal8Bit().constData());
		double By = std::stod(m_ui->lineEdit_4->text().toLocal8Bit().constData());
		double Bz = std::stod(m_ui->lineEdit_7->text().toLocal8Bit().constData());

		return Eigen::Vector3d(Bx, By, Bz);
	}

	Eigen::Vector3d DemoController::getH()
	{
		return Eigen::Vector3d(currPosition[0], currPosition[1], currPosition[2]);
	}

	void DemoController::executeTrajectory(Eigen::Vector3d& H, Eigen::Vector3d& A, Eigen::Vector3d& B)
	{
		LOG_INFO("Pos H " << H[0]  << " " << H[1] << " " << H[2]);

		AlgorithmController* usc = m_main->getAlgorithmController("CAMP IGTLink");

		trajectory = new Trajectory(H, A, B, 10, 10, 10);
		executor = new TrajectoryExecutor(trajectory, node, usc, m_trackingStream);
		executor->run();
	}

	void DemoController::moveTo(Eigen::Vector3d& point) {
		std::vector<double> currPoint(3);
		currPoint[0] = point[0];
		currPoint[0] = point[1];
		currPoint[0] = point[2];
		moveTo(currPoint, currOrientation);
	}
	
	void DemoController::moveTo(std::vector<double>& currPoint) {
		node->publishMessage(currPoint, currOrientation);
	}

	void DemoController::onReceive(const IIWA::IIWAMsg::ConstPtr& msgPtr)
	{
		std::vector<double> position = msgPtr->cartPosition;
		std::vector<double> orientation = msgPtr->cartOrientation;

		currPosition = position;
		currOrientation = orientation;

		if (acquireOutput) {
		    double time =  getCurrentTime();

		    Eigen::Matrix4d currentPose = poseFromMsg(position, orientation);

		    updateOutput(currentPose, time);
		}
	}

	mat4 DemoController::poseFromMsg(std::vector<double>& position, std::vector<double>& orientation)
	{

		mat4 currentPose = mat4::Identity();

		// The quaternions, q0, q1, q2, and q3 where q0 is the scaler component,
		// have been extracted from the MATRIX output using the algorithm described in "Quaternion
		// from Rotation Matrix" by Stanley W. Sheppard, Journal of Guidance and Control, Vol. 1,
		// May-June 1978, pp. 223-4.

		// Quaternion multiplication and orthogonal matrix multiplication can both be used to represent rotation.
		// If a quaternion is represented by qw + i qx + j qy + k qz , then the equivalent matrix, 
		// to represent the same rotation, is:
		// 1 - 2*qy2 - 2*qz2 	2*qx*qy - 2*qz*qw 	2*qx*qz + 2*qy*qw
		// 2*qx*qy + 2*qz*qw 	1 - 2*qx2 - 2*qz2 	2*qy*qz - 2*qx*qw
		// 2*qx*qz - 2*qy*qw 	2*qy*qz + 2*qx*qw 	1 - 2*qx2 - 2*qy2

		// calculate rotation matrix and store it
		//mat3 currentRotation = mat3::Identity();

		//currentRotation(0,0) = 1 - 2*qy*qy - 2*qz*qz; 
		//currentRotation(0,1) = 2*qx*qy - 2*qz*qw;
		//currentRotation(0,2) = 2*qx*qz + 2*qy*qw;
		//currentRotation(1,0) = 2*qx*qy + 2*qz*qw;
		//currentRotation(1,1) = 1 - 2*qx*qx - 2*qz*qz;
		//currentRotation(1,2) = 2*qy*qz - 2*qx*qw;
		//currentRotation(2,0) = 2*qx*qz - 2*qy*qw;
		//currentRotation(2,1) = 2*qy*qz + 2*qx*qw;
		//currentRotation(2,2) = 1 - 2*qx*qx - 2*qy*qy;

		mat3 currentRotation;

		for (int i = 0; i < 9; i++) {
			currentRotation(i / 3, i % 3) = orientation[i];
		}



		double tx = position[0];
		double ty = position[1];
		double tz = position[2];

		// fill in position information
		currentPose.block<3,3>(0,0) = currentRotation;
		currentPose(0,3) = tx;
		currentPose(1,3) = ty;
		currentPose(2,3) = tz;

		return currentPose;
	}

	double DemoController::getCurrentTime() {
		LONGLONG current_time;
		GetSystemTimeAsFileTime((LPFILETIME)&current_time);
		current_time -= 116444736000000000LL;

		// supposed 100 nanoseconds resolution
		return (double)current_time / 10000000.0f;
	}

	const IIWA::IIWAMsg::ConstPtr DemoController::msgFromPose(mat4& pose) {
		IIWA::IIWAMsg::Ptr msgPtr(new IIWA::IIWAMsg());

        std::vector<double> point;

		point.push_back(pose(0,3));
		point.push_back(pose(1,3));
		point.push_back(pose(2,3));

		msgPtr->cartPosition = point;

		mat3 rotation = pose.block<3,3>(0,0);
		Eigen::Quaternion<double> q(rotation);

		std::vector<double> orientation;

		orientation.push_back(q.x());
		orientation.push_back(q.y());
		orientation.push_back(q.z());
		orientation.push_back(q.w());

		msgPtr->cartOrientation = orientation;

		return msgPtr;
	}

	void DemoController::onROSNodeNameChange()
	{
		//delete node;
		//std::string sub_name = node->getPubName();
		//std::string pub_name = node->getSubName();

		//std::string node_name = m_ui->lineEdit->text().toLocal8Bit().constData();

		//node = new ROSNode(node_name, pub_name, sub_name);
	}

	void DemoController::onTopicNameChange()
	{
		node->advertise(m_ui->lineEdit_2->text().toLocal8Bit().constData());
	}

	void DemoController::onPositionXChange()
	{

	}

	void DemoController::onPositionYChange()
	{

	}

	void DemoController::onPositionZChange()
	{

	}

	void DemoController::onOrientationXChange()
	{

	}

	void DemoController::onOrientationYChange()
	{

	}

	void DemoController::onOrientationZChange()
	{

	}

	void DemoController::onOrientationWChange()
	{
	}
	
	void applyTransformation(Eigen::Vector3d& point, Eigen::Quaterniond& orientation) {
		node->publishMessage(point, orientation);
	}
	
	void applyTransformation(Eigen::Vector3d& point, Eigen::Matrix3d& orientation) {
		node->publishMessage(point, orientation);
	}
	
	void applyTransformation(std::vector<double>& point, std::vector<double>& orientation) {
		node->publishMessage(point, orientation);
	}

}
