/**
	@author: Dmitrij Sojma <sojmadmi@fel.cvut.cz>
	@institution: CTU in Prague
	@date: 17.5.2020
*/

/** TESTING_ANGLES: Allows to test all angles for given pose in virtual environment. 
	Checking of limits in virtual_client.cpp in iiwa_controller_virtual needs to be disabled.
*/
//#define TESTING_ANGLES 
//#define DEBUG_ANGLE_OPTIMIZATION // breaks realtime execution


#ifndef IIWA_FRI_CONTROLLER_VIVE_VIVECONTROLLER_H
#define IIWA_FRI_CONTROLLER_VIVE_VIVECONTROLLER_H

#include <array>
#include <cmath>

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <realtime_tools/realtime_publisher.h>

#include <iiwa_kinematic/Jacobian.h>
#include <iiwa_kinematic/KukaIKT.h>
#include <iiwa_kinematic/RobotLimits.h>
#include <iiwa_kinematic/RobotParameters.h>
#include <iiwa_kinematic/Utils.h>
#include <hardware_interface/joint_command_interface.h>
#include <controller_interface/controller.h>

#include <pluginlib/class_list_macros.h>

#include <capek_time_tools/TimeUtils.h>
#include <capek_msgs/ViveDebugStatus.h>

#include <vive_events/EventStamped.h>
#include <vive_data/ViveData.h>

#include <chrono>

class ViveController : public controller_interface::Controller<hardware_interface::PositionJointInterface> {

public:
	ViveController();
	//~ViveController();

	constexpr static unsigned int CART_DOF = RobotParameters::CART_DOF;
	constexpr static unsigned int NUM_OF_JOINTS = RobotParameters::NUM_OF_JOINTS;

	bool init(hardware_interface::PositionJointInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
	void starting(const ros::Time& time1) override;
	void stopping(const ros::Time& time1) override;
	void update(const ros::Time& time, const ros::Duration& period) override;

	/** Update internal states of this class from FRI */
	void updateRobotState();

private:
	capek::Clocks clocks;
	capek::TimePoint whole{ "whole" };

	double T = 0.01;

	/** Motion scaling */
	static const uint8_t SCALE_FACTORS_SIZE = 7;
	const double scale_factors[SCALE_FACTORS_SIZE] = { 0.1, 0.25, 0.50, 0.75, 1.00, 1.5, 2.0 };
	uint8_t distance_scale_factor_idx = 4;
	uint8_t angular_distance_scale_factor_idx = 4;
	double distance_scale_factor = scale_factors[distance_scale_factor_idx];
	double angular_distance_scale_factor = scale_factors[angular_distance_scale_factor_idx];

	/** vive data obejct */
	std::shared_ptr<vive_data::ViveData> vive_data;

	/** computational stuff - position */
	geometry_msgs::PoseStamped new_pose;
	Eigen::Vector3d translation_vector;

	/** computational stuff - orientation */
	Eigen::Quaternion<double> rotation_quaternion;
	Eigen::AngleAxisd rotation_angleaxis;

	/** Vive transformation matrices */
	Eigen::Isometry3d T_vive_tran, T_vive_rot, T_vive_tran_last, T_vive_rot_last, T_rob_tracker;

	/** Debug status publisher */
    std::shared_ptr<realtime_tools::RealtimePublisher<capek_msgs::ViveDebugStatus>> pub_debug_status;
    void publishRobotState();
	
	/** Controller errors */
	std::vector<std::string> errors;

	/** Robot parameters */
	std::shared_ptr<RobotParameters> RP;

	/** Robot jacobian */
	std::shared_ptr<Jacobian> Jac;

	/** Robot dkt */
	std::shared_ptr<DKT> dkt;

	/** Robot limits */
	std::shared_ptr<RobotLimits> lim;

	/** Robot inverse kinematics */
	std::shared_ptr<KukaIKT> ikt;

	/** Array of joint handlers */
	std::array<hardware_interface::JointHandle, NUM_OF_JOINTS> joints;

	/** IKT, current and commanded position */
	Eigen::Matrix<double, NUM_OF_JOINTS, 1> cmd, eff, pos;
	Eigen::Matrix<double, NUM_OF_JOINTS, 1> cmd_new, dq;
	std::array<std::array<double, NUM_OF_JOINTS>, 8> ikt_solutions;

	/** Robot arm angle */
	double angle = 0;
	double angle_last = 0;

	/** Configuration of robot */
	std::uint8_t configuration = 0;

	/** Robot transformation matrices */
	Eigen::Isometry3d T_rob_tran, T_rob_rot, T_ee_c;

	/** Tool translation in ee coords */
	Eigen::Vector3d T_tool;

	/** Vive events */
	void handle_event(ViveEvents::EventType event_type);
	void handle_events(std::list<ViveEvents::EventType> event_list);

	/** Event flags */
	bool abort = false;
	bool useForAngle = true;
	bool autoOptimizeAngle = false;
	bool changeAngle = false;
	bool trackpad_touched = false;
	bool vive_timeout = false;

	bool pos_reachable = true;

	/** Change of angle via Tracker */
	short angleSign;
	const double angleChangeAmount = 0.1 * M_PI/180;
	
	/** Angle change limits */
	const double ANGLE_SAFETY = M_PI/18;
	const double angle_limits[2] = {-M_PI/2 + ANGLE_SAFETY, M_PI/2 - ANGLE_SAFETY};

	/** Jacobi, etc */
	Eigen::Matrix<double, CART_DOF, NUM_OF_JOINTS> J;
	Eigen::Matrix<double, CART_DOF, NUM_OF_JOINTS> J_pub;
	std::array<double, CART_DOF> singular_values;
	double product_pub;
	

/** ------ Angle testing ------ */
#ifdef TESTING_ANGLES
	bool cycle_angle = false;
	int count = 0;
#endif

	
	/** ------------------- Angle optimization ----------------------- */
	bool via_determinant = false;
	void optimizeAngle(bool via_determinant=false);
	double computeSingularValuesProduct(Eigen::Matrix<double, NUM_OF_JOINTS, 1> command, bool via_determinant=false);
	double computeSingularValuesProductSVD(Eigen::Matrix<double, NUM_OF_JOINTS, 1> command);
	double computeSingularValuesProductDet(Eigen::Matrix<double, NUM_OF_JOINTS, 1> command);

	const double epsilon = 5 * M_PI/180;
	Eigen::Matrix<double, NUM_OF_JOINTS, NUM_OF_JOINTS> T_vel;
	Eigen::Matrix<double, NUM_OF_JOINTS, 1> cmd_opt, cmd_plus, cmd_minus;
	Eigen::JacobiSVD<Eigen::Matrix<double, CART_DOF, NUM_OF_JOINTS>> svd;
	std::array<double, 3> singularValuesProducts;
	std::array<short, 3> singularValuesIndexes = {0,1,2};
	const double angleChangeConstMultiplier = 0.1;
	double angleChangeConstMax = 0;
	Eigen::Matrix<double, NUM_OF_JOINTS, 1> angleChangeConst;

	/** Signum function */
	Eigen::Matrix<double, NUM_OF_JOINTS, 1> sgn(Eigen::Matrix<double, NUM_OF_JOINTS, 1> vec);

	
};

#endif //IIWA_FRI_CONTROLLER_VIVE_VIVECONTROLLER_H