#include "Kinematics.h"


float deg2rad(float angle)
{
	return angle/180.0*M_PI;
}

float rad2deg(float angle)
{
	return angle*180.0/M_PI;
}

std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
{
	float x = L1 * cos(q1) + L2 * cos(q1+q2);
	float y = L1 * sin(q1) + L2 * sin(q1+q2);
	std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl;
	std::vector<float> X;
	X.push_back(x);
	X.push_back(y);
	
	return X;
}

int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
{
	int rank = -1;
	cv::Mat1f oJacobianMatrix(2, 2);
	
	if (vJacobianMatrix.size() == 4)
	{
		// Converts the Jacobian matrix from std::vector to cv::Mat
		oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
		oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
		oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
		oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
		std::cout << "=====Jacobian Matrix=====" << std::endl;
		std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " <<  oJacobianMatrix.at<float>(0,1)  << " ]" << std::endl;
		std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " <<  oJacobianMatrix.at<float>(1,1)  << " ]" << std::endl;
		// Computes the determinant of the Jacobian matrix
		float determinant = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
		std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl;
		// Computes SVD
		cv::Mat1f w, u, vt;
		cv::SVD::compute(oJacobianMatrix, w, u, vt);
		// Finds non zero singular values
		cv::Mat1f nonZeroSingularValues = w/w.at<float>(0,0) > threshold;
		// Counts the number of non zero singular values
		rank = cv::countNonZero(nonZeroSingularValues);
		std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl;
		// Determines the inverse of the Jacobian matrix
		cv::Mat oJacobianInverse =  oJacobianMatrix.inv(); 
		std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
		std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " <<  oJacobianInverse.at<float>(0,1)  << " ]" << std::endl;
		std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " <<  oJacobianInverse.at<float>(1,1)  << " ]" << std::endl;
	}
	else
		std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;

	return rank;
}

cv::Mat  computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
{
	cv::Mat1f oJacobianMatrix(2, 2);
	cv::Mat oJacobianInverse;
	
	if (vJacobianMatrix.size() == 4)
	{
		// Converts the Jacobian matrix from std::vector to cv::Mat
		oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
		oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
		oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
		oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
		std::cout << "=====Jacobian Matrix=====" << std::endl;
		std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " <<  oJacobianMatrix.at<float>(0,1)  << " ]" << std::endl;
		std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " <<  oJacobianMatrix.at<float>(1,1)  << " ]" << std::endl;
		// Determines the inverse of the Jacobian matrix
		cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD);
		//oJacobianInverse =  oJacobianMatrix.inv(); 
		std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
		std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " <<  oJacobianInverse.at<float>(0,1)  << " ]" << std::endl;
		std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " <<  oJacobianInverse.at<float>(1,1)  << " ]" << std::endl;
	}
	else
		std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;

	return oJacobianInverse;
}