/****************************************************************************
 *
 *   Copyright (C) 2018 - 2019 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file ControlMath.cpp
 */

#include "ControlMath.hpp"
#include <px4_platform_common/defines.h>
#include <float.h>
#include <mathlib/mathlib.h>

using namespace matrix;

namespace ControlMath
{
void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode,
		      const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
		      float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
{

	// Print an error if the omni_att_mode parameter is out of range
	if (omni_att_mode > 6 || omni_att_mode < 0) {
		PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!");
	}

	switch (omni_att_mode) {
	case 1: // Attitude is set to the minimum roll and pitch (used for omnidirectional vehicles)
		thrustToMinTiltAttitude(thr_sp, yaw_sp, omni_dfc_max_thrust, att, omni_proj_axes, att_sp);
		break;

	case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles)
		thrustToZeroTiltAttitude(thr_sp, yaw_sp, att, omni_proj_axes, att_sp);
		break;

	case 3: { // Attitude is set to a fixed tilt at a fixed global direction (used for omnidirectional vehicles)
			thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, omni_att_tilt_angle, omni_att_tilt_dir, omni_proj_axes, att_sp);
			break;
		}

	case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles)
			thrustToFixedRollPitch(thr_sp, yaw_sp, att, omni_att_roll, omni_att_pitch, omni_proj_axes, att_sp);
			break;
		}

	default: // Attitude is calculated from the desired thrust direction
		bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
		att_sp.thrust_body[2] = -thr_sp.length();
	}

	// Estimate the optimal tilt angle and direction to counteract the wind
	if (omni_att_mode == 5) {
		// Calculate the tilt angle
		omni_att_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm());

		// Calculate the tilt direction
		omni_att_tilt_dir = atan2f(thr_sp(1), thr_sp(0));

		// Set the roll angle
		omni_att_roll = att_sp.roll_body;

		// Set the pitch angle
		omni_att_pitch = att_sp.pitch_body;
	}
}

void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
	// zero vector, no direction, set safe level value
	if (body_z.norm_squared() < FLT_EPSILON) {
		body_z(2) = 1.f;
	}

	body_z.normalize();

	// vector of desired yaw direction in XY plane, rotated by PI/2
	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);

	// desired body_x axis, orthogonal to body_z
	Vector3f body_x = y_C % body_z;

	// keep nose to front while inverted upside down
	if (body_z(2) < 0.0f) {
		body_x = -body_x;
	}

	if (fabsf(body_z(2)) < 0.000001f) {
		// desired thrust is in XY plane, set X downside to construct correct matrix,
		// but yaw component will not be used actually
		body_x.zero();
		body_x(2) = 1.0f;
	}

	body_x.normalize();

	// desired body_y axis
	Vector3f body_y = body_z % body_x;

	Dcmf R_sp;

	// fill rotation matrix
	for (int i = 0; i < 3; i++) {
		R_sp(i, 0) = body_x(i);
		R_sp(i, 1) = body_y(i);
		R_sp(i, 2) = body_z(i);
	}

	// copy quaternion setpoint to attitude setpoint topic
	Quatf q_sp = R_sp;
	q_sp.copyTo(att_sp.q_d);
	att_sp.q_d_valid = true;

	// calculate euler angles, for logging only, must not be used for control
	Eulerf euler = R_sp;
	att_sp.roll_body = euler(0);
	att_sp.pitch_body = euler(1);
	att_sp.yaw_body = euler(2);
}

void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, int omni_proj_axes,
			      vehicle_attitude_setpoint_s &att_sp)
{
	// set Z axis to upward direction
	Vector3f body_z = Vector3f(0.f, 0.f, 1.f);

	// desired body_x and body_y axis
	Vector3f body_x = Vector3f(cos(yaw_sp), sin(yaw_sp), 0.0f);
	Vector3f body_y = Vector3f(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);

	Dcmf R_sp;

	// fill rotation matrix
	for (int i = 0; i < 3; i++) {
		R_sp(i, 0) = body_x(i);
		R_sp(i, 1) = body_y(i);
		R_sp(i, 2) = body_z(i);
	}

	// copy quaternion setpoint to attitude setpoint topic
	Quatf q_sp = R_sp;
	q_sp.copyTo(att_sp.q_d);
	att_sp.q_d_valid = true;

	// set the euler angles, for logging only, must not be used for control
	att_sp.roll_body = 0;
	att_sp.pitch_body = 0;
	att_sp.yaw_body = yaw_sp;


	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
		matrix::Dcmf R_body = att;

		for (int i = 0; i < 3; i++) {
			body_x(i) = R_body(i, 0);
			body_y(i) = R_body(i, 1);
			body_z(i) = R_body(i, 2);
		}
	}

	att_sp.thrust_body[0] = thr_sp.dot(body_x);
	att_sp.thrust_body[1] = thr_sp.dot(body_y);
	att_sp.thrust_body[2] = thr_sp.dot(body_z);
}

void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
			     const matrix::Quatf &att, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
{
	Vector3f body_z;
	float lambda = 0.f; // the minimum tilt angle

	// zero vector, no direction, set safe level value
	if (thr_sp.norm_squared() < FLT_EPSILON) {
		body_z(2) = 1.f;

	} else {
		// Check if the horizontal force is less than the maximum possible
		Vector2f thr_sp_h(thr_sp(0), thr_sp(1));

		if (thr_sp_h.norm() <= omni_dfc_max_thrust) {
			thrustToZeroTiltAttitude(thr_sp, yaw_sp, att, omni_proj_axes, att_sp);
			return;
		}

		// Calculate the tilt angle
		float thr_sp_norm = thr_sp.norm();
		float xi = asinf(Vector2f(thr_sp(0),
					  thr_sp(1)).norm() / thr_sp_norm); // angle between upward direction and the desired thrust
		float mu = asinf(omni_dfc_max_thrust / thr_sp_norm); // angle between the Z thrust and the desired thrust
		lambda = xi - mu; // the desired tilt angle

		// Calculate the direction of the body Z axis
		Vector3f v_hat(0.f, 0.f, -1.f); // upward direction
		Vector3f p_hat = v_hat % thr_sp; // the axis of rotation for lambda
		p_hat.normalize();
		body_z = -(1 - cosf(lambda)) * p_hat * (p_hat.dot(v_hat)) + cosf(lambda) * v_hat - sinf(lambda) *
			 (v_hat % p_hat); // Rodrigues' rotation formula
		body_z = -body_z;
	}

	// vector of desired yaw direction in XY plane, rotated by PI/2
	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);

	// desired body_x axis, orthogonal to body_z
	Vector3f body_x = y_C % body_z;

	// keep nose to front while inverted upside down
	if (body_z(2) < 0.0f) {
		body_x = -body_x;
	}

	if (fabsf(body_z(2)) < 0.000001f) {
		// desired thrust is in XY plane, set X downside to construct correct matrix,
		// but yaw component will not be used actually
		body_x.zero();
		body_x(2) = 1.0f;
	}

	body_x.normalize();

	// desired body_y axis
	Vector3f body_y = body_z % body_x;

	Dcmf R_sp;

	// fill rotation matrix
	for (int i = 0; i < 3; i++) {
		R_sp(i, 0) = body_x(i);
		R_sp(i, 1) = body_y(i);
		R_sp(i, 2) = body_z(i);
	}

	// copy quaternion setpoint to attitude setpoint topic
	Quatf q_sp = R_sp;
	q_sp.copyTo(att_sp.q_d);
	att_sp.q_d_valid = true;

	// calculate euler angles, for logging only, must not be used for control
	Eulerf euler = R_sp;
	att_sp.roll_body = euler(0);
	att_sp.pitch_body = euler(1);
	att_sp.yaw_body = euler(2);


	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
		matrix::Dcmf R_body = att;

		for (int i = 0; i < 3; i++) {
			body_x(i) = R_body(i, 0);
			body_y(i) = R_body(i, 1);
			body_z(i) = R_body(i, 2);
		}
	}

	// Calculate the direct force vector
	float f_eff_z = -(omni_dfc_max_thrust * tanf(lambda) + thr_sp(2) / cosf(lambda));
	Vector2f f_eff_h(thr_sp.dot(body_x), thr_sp.dot(body_y));

	// Prevent the division by zero
	float f_norm = f_eff_h.norm();

	if (f_norm > 0.0001f) {
		f_eff_h = f_eff_h / f_eff_h.norm() * omni_dfc_max_thrust;

	} else {
		f_eff_h.zero();
	}

	att_sp.thrust_body[0] = f_eff_h(0);
	att_sp.thrust_body[1] = f_eff_h(1);
	att_sp.thrust_body[2] = -f_eff_z;
}

void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
			       const float tilt_angle, const float tilt_dir,
			       int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
{
	Vector3f body_z;

	// zero vector, no direction, set safe level value
	if (thr_sp.norm_squared() < FLT_EPSILON) {
		body_z(2) = 1.f;

	} else {
		// Calculate the direction of the body Z axis
		Vector3f v_hat(0.f, 0.f, -1.f); // upward direction
		// vector of desired yaw direction in XY plane, rotated by PI/2
		Vector3f kappa_C(cosf(tilt_dir), sinf(tilt_dir), 0.0f);
		Vector3f p_hat = v_hat % kappa_C; // the axis of rotation for tilt_angle
		p_hat.normalize();
		body_z = -(1 - cosf(tilt_angle)) * p_hat * (p_hat.dot(v_hat)) + cosf(tilt_angle) * v_hat - sinf(tilt_angle) *
			 (v_hat % p_hat); // Rodrigues' rotation formula
		body_z = -body_z;
	}

	// vector of desired yaw direction in XY plane, rotated by PI/2
	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);

	// desired body_x axis, orthogonal to body_z
	Vector3f body_x = y_C % body_z;

	// keep nose to front while inverted upside down
	if (body_z(2) < 0.0f) {
		body_x = -body_x;
	}

	if (fabsf(body_z(2)) < 0.000001f) {
		// desired thrust is in XY plane, set X downside to construct correct matrix,
		// but yaw component will not be used actually
		body_x.zero();
		body_x(2) = 1.0f;
	}

	body_x.normalize();

	// desired body_y axis
	Vector3f body_y = body_z % body_x;

	Dcmf R_sp;

	// fill rotation matrix
	for (int i = 0; i < 3; i++) {
		R_sp(i, 0) = body_x(i);
		R_sp(i, 1) = body_y(i);
		R_sp(i, 2) = body_z(i);
	}

	// copy quaternion setpoint to attitude setpoint topic
	Quatf q_sp = R_sp;
	q_sp.copyTo(att_sp.q_d);
	att_sp.q_d_valid = true;

	// calculate euler angles, for logging only, must not be used for control
	Eulerf euler = R_sp;
	att_sp.roll_body = euler(0);
	att_sp.pitch_body = euler(1);
	att_sp.yaw_body = euler(2);

	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
		matrix::Dcmf R_body = att;

		for (int i = 0; i < 3; i++) {
			body_x(i) = R_body(i, 0);
			body_y(i) = R_body(i, 1);
			body_z(i) = R_body(i, 2);
		}
	}

	att_sp.thrust_body[0] = thr_sp.dot(body_x);
	att_sp.thrust_body[1] = thr_sp.dot(body_y);
	att_sp.thrust_body[2] = thr_sp.dot(body_z);
}

void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
			    const float roll_angle, const float pitch_angle, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
{
	Eulerf euler_cmd(roll_angle, pitch_angle, yaw_sp);

	Quatf q_sp = euler_cmd;
	q_sp.copyTo(att_sp.q_d);

	// calculate euler angles, for logging only, must not be used for control
	att_sp.roll_body = roll_angle;
	att_sp.pitch_body = pitch_angle;
	att_sp.yaw_body = yaw_sp;

	matrix::Dcmf R_body;

	if (omni_proj_axes == 0) { // if thrust is projected onm the commanded attitude
		R_body = q_sp;

	} else if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
		R_body = att;
	}

	Vector3f body_x, body_y, body_z;

	for (int i = 0; i < 3; i++) {
		body_x(i) = R_body(i, 0);
		body_y(i) = R_body(i, 1);
		body_z(i) = R_body(i, 2);
	}

	att_sp.thrust_body[0] = thr_sp.dot(body_x);
	att_sp.thrust_body[1] = thr_sp.dot(body_y);
	att_sp.thrust_body[2] = thr_sp.dot(body_z);
}

Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
{
	if (Vector2f(v0 + v1).norm() <= max) {
		// vector does not exceed maximum magnitude
		return v0 + v1;

	} else if (v0.length() >= max) {
		// the magnitude along v0, which has priority, already exceeds maximum.
		return v0.normalized() * max;

	} else if (fabsf(Vector2f(v1 - v0).norm()) < 0.001f) {
		// the two vectors are equal
		return v0.normalized() * max;

	} else if (v0.length() < 0.001f) {
		// the first vector is 0.
		return v1.normalized() * max;

	} else {
		// vf = final vector with ||vf|| <= max
		// s = scaling factor
		// u1 = unit of v1
		// vf = v0 + v1 = v0 + s * u1
		// constraint: ||vf|| <= max
		//
		// solve for s: ||vf|| = ||v0 + s * u1|| <= max
		//
		// Derivation:
		// For simplicity, replace v0 -> v, u1 -> u
		// 				   		   v0(0/1/2) -> v0/1/2
		// 				   		   u1(0/1/2) -> u0/1/2
		//
		// ||v + s * u||^2 = (v0+s*u0)^2+(v1+s*u1)^2+(v2+s*u2)^2 = max^2
		// v0^2+2*s*u0*v0+s^2*u0^2 + v1^2+2*s*u1*v1+s^2*u1^2 + v2^2+2*s*u2*v2+s^2*u2^2 = max^2
		// s^2*(u0^2+u1^2+u2^2) + s*2*(u0*v0+u1*v1+u2*v2) + (v0^2+v1^2+v2^2-max^2) = 0
		//
		// quadratic equation:
		// -> s^2*a + s*b + c = 0 with solution: s1/2 = (-b +- sqrt(b^2 - 4*a*c))/(2*a)
		//
		// b = 2 * u.dot(v)
		// a = 1 (because u is normalized)
		// c = (v0^2+v1^2+v2^2-max^2) = -max^2 + ||v||^2
		//
		// sqrt(b^2 - 4*a*c) =
		// 		sqrt(4*u.dot(v)^2 - 4*(||v||^2 - max^2)) = 2*sqrt(u.dot(v)^2 +- (||v||^2 -max^2))
		//
		// s1/2 = ( -2*u.dot(v) +- 2*sqrt(u.dot(v)^2 - (||v||^2 -max^2)) / 2
		//      =  -u.dot(v) +- sqrt(u.dot(v)^2 - (||v||^2 -max^2))
		// m = u.dot(v)
		// s = -m + sqrt(m^2 - c)
		//
		//
		//
		// notes:
		// 	- s (=scaling factor) needs to be positive
		// 	- (max - ||v||) always larger than zero, otherwise it never entered this if-statement

		Vector2f u1 = v1.normalized();
		float m = u1.dot(v0);
		float c = v0.dot(v0) - max * max;
		float s = -m + sqrtf(m * m - c);
		return v0 + u1 * s;
	}
}

bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r,
		       const Vector3f &line_a, const Vector3f &line_b, Vector3f &res)
{
	// project center of sphere on line  normalized AB
	Vector3f ab_norm = line_b - line_a;

	if (ab_norm.length() < 0.01f) {
		return true;
	}

	ab_norm.normalize();
	Vector3f d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
	float cd_len = (sphere_c - d).length();

	if (sphere_r > cd_len) {
		// we have triangle CDX with known CD and CX = R, find DX
		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);

		if ((sphere_c - line_b) * ab_norm > 0.0f) {
			// target waypoint is already behind us
			res = line_b;

		} else {
			// target is in front of us
			res = d + ab_norm * dx_len; // vector A->B on line
		}

		return true;

	} else {

		// have no roots, return D
		res = d; // go directly to line

		// previous waypoint is still in front of us
		if ((sphere_c - line_a) * ab_norm < 0.0f) {
			res = line_a;
		}

		// target waypoint is already behind us
		if ((sphere_c - line_b) * ab_norm > 0.0f) {
			res = line_b;
		}

		return false;
	}
}
}