#include "../Header/quaternion_operation.h"

using namespace std;
using namespace cv;

vector<double> last_w_unit{ 0.,0.,0. };
double last_angle_vel;
bool INIT_FLAG = true;
double last_d_angle;
const double d_angle_thres = 0.015; // threshold for maxium change of d_angle (rad)
const double w_damping_const = 0.3;

 void Rotation2Quaternion(cv::Mat* rot_vec, std::vector<double>* q) {
	double angle_rad = cv::norm(*rot_vec, cv::NORM_L2);
	Mat rot_axis = *rot_vec / angle_rad;
	(*q)[0] = cos(angle_rad/2);
	(*q)[1] = sin(angle_rad / 2)*rot_axis.at<double>(0, 0);
	(*q)[2] = sin(angle_rad / 2)*rot_axis.at<double>(1, 0);
	(*q)[3] = sin(angle_rad / 2)*rot_axis.at<double>(2, 0);
}

void Quaternion2Rotation(std::vector<double>* q, cv::Mat* rot_mat) {
	rot_mat->at<double>(0, 0) = 1 - 2 * (((*q)[2])*((*q)[2]) + ((*q)[3])*((*q)[3]));
	rot_mat->at<double>(0, 1) = 2*(((*q)[1])*((*q)[2]) - ((*q)[3])*((*q)[0]));
	rot_mat->at<double>(0, 2) = 2 * (((*q)[1])*((*q)[3]) + ((*q)[2])*((*q)[0]));
	rot_mat->at<double>(1, 0) = 2 * (((*q)[1])*((*q)[2]) + ((*q)[3])*((*q)[0]));
	rot_mat->at<double>(1, 1) = 1 - 2 * (((*q)[1])*((*q)[1]) + ((*q)[3])*((*q)[3]));
	rot_mat->at<double>(1, 2) = 2 * (((*q)[2])*((*q)[3]) - ((*q)[1])*((*q)[0]));
	rot_mat->at<double>(2, 0) = 2 * (((*q)[1])*((*q)[3]) - ((*q)[2])*((*q)[0]));
	rot_mat->at<double>(2, 1) = 2 * (((*q)[2])*((*q)[3]) + ((*q)[1])*((*q)[0]));
	rot_mat->at<double>(2, 2) = 1 - 2 * (((*q)[1])*((*q)[1]) + ((*q)[2])*((*q)[2]));
}

 void MultiplyQuaternions(std::vector<double>* q1, std::vector<double>* q2, std::vector<double>* q) {
	(*q)[0] = ((*q1)[0])*((*q2)[0]) - ((*q1)[1])*((*q2)[1]) - ((*q1)[2])*((*q2)[2]) - ((*q1)[3])*((*q2)[3]);
	(*q)[1] = ((*q1)[0])*((*q2)[1]) + ((*q1)[1])*((*q2)[0]) + ((*q1)[2])*((*q2)[3]) - ((*q1)[3])*((*q2)[2]);
	(*q)[2] = ((*q1)[0])*((*q2)[2]) - ((*q1)[1])*((*q2)[3]) + ((*q1)[2])*((*q2)[0]) + ((*q1)[3])*((*q2)[1]);
	(*q)[3] = ((*q1)[0])*((*q2)[3]) + ((*q1)[1])*((*q2)[2]) - ((*q1)[2])*((*q2)[1]) + ((*q1)[3])*((*q2)[0]);
}

void ConjugateQuaternion(std::vector<double>* q, std::vector<double>* q_conj){
	(*q_conj)[0] = (*q)[0];
	(*q_conj)[1] = -(*q)[1];
	(*q_conj)[2] = -(*q)[2];
	(*q_conj)[3] = -(*q)[3];
}

 void Quaternion2Omega(std::vector<double>* q, double dt, std::vector<double>* w, double* d_angle) {
	double q_norm_xyz = sqrt(((*q)[1])*((*q)[1]) + ((*q)[2])*((*q)[2]) + ((*q)[3])*((*q)[3]));
	*d_angle = atan2(q_norm_xyz, (*q)[0]);

	double angle_vel = 0.;
	vector<double> current_w_unit{ 0.,0.,0. };

	if (q_norm_xyz > 0) {
		current_w_unit[0] = (*q)[1] / q_norm_xyz;
		current_w_unit[1] = (*q)[2] / q_norm_xyz;
		current_w_unit[2] = (*q)[3] / q_norm_xyz;

		if (!INIT_FLAG) {
			double dot_product = current_w_unit[0] * last_w_unit[0] + current_w_unit[1] * last_w_unit[1] + current_w_unit[2] * last_w_unit[2];
			if (abs(*d_angle - last_d_angle) < d_angle_thres) { // case 1: white noise (static) and actual motion, do not consider windup
				angle_vel = 2 * 1000. * (*d_angle) / dt; // dt: ms
			}
			else {
				if (dot_product < 0 && 3.1415926 - *d_angle < d_angle_thres) { // case 2: windup case, the rotation axis flips
					current_w_unit[0] = -current_w_unit[0];
					current_w_unit[1] = -current_w_unit[1];
					current_w_unit[2] = -current_w_unit[2];
					*d_angle = 3.1415926 - *d_angle;
					angle_vel = 2 * 1000. * *d_angle / dt; // dt: ms
				}
				else { // case 3: large noise, last_d_angle + d_angle_thres < *d_angle < 1.5707963
					current_w_unit[0] = last_w_unit[0];
					current_w_unit[1] = last_w_unit[1];
					current_w_unit[2] = last_w_unit[2];
					angle_vel = last_angle_vel; // dt: ms
					*d_angle = last_d_angle;
				}
			}
		}
		(*w)[0] = angle_vel * current_w_unit[0];
		(*w)[1] = angle_vel * current_w_unit[1];
		(*w)[2] = angle_vel * current_w_unit[2];
	}
	else {
		current_w_unit[0] = 1.;
		current_w_unit[1] = 0.;
		current_w_unit[2] = 0.;

		(*w)[0] = angle_vel;
		(*w)[1] = 0.;
		(*w)[2] = 0.;
	}

	if (INIT_FLAG) {
		if (*d_angle > 1.5707963) {
			*d_angle = 3.1415926 - *d_angle;
			current_w_unit[0] = -current_w_unit[0];
			current_w_unit[1] = -current_w_unit[1];
			current_w_unit[2] = -current_w_unit[2];
		}
		if (*d_angle < 0.01) {
			INIT_FLAG = false;
		}
	}

	last_w_unit = current_w_unit;
	last_angle_vel = angle_vel;
	last_d_angle = *d_angle;

}

 void QuaternionPropagate(std::vector<double>* q, std::vector<double>* w, double dt, std::vector<double>* q_out) {
	double w_norm = sqrt(((*w)[0])*((*w)[0]) + ((*w)[1])*((*w)[1]) + ((*w)[2])*((*w)[2]));
	vector<double> dq{ cos(w_damping_const*w_norm*dt / 2000), sin(w_damping_const*w_norm*dt / 2000) * ((*w)[0]) / w_norm ,sin(w_damping_const*w_norm*dt / 2000) * ((*w)[1]) / w_norm, sin(w_damping_const*w_norm*dt / 2000) * ((*w)[2]) / w_norm };
	MultiplyQuaternions(q, &dq, q_out);
}