/*
    PIP - Platform Independent Primitives
    Class for quaternions
    Ivan Pelipenko peri4ko@yandex.ru, Andrey Bychkov work.a.b@yandex.ru

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "piquaternion.h"


PIQuaternion::PIQuaternion(const PIMathVectorT3d & u, double a) {
	q[0] = a;
	q[1] = u[0];
	q[2] = u[1];
	q[3] = u[2];
}


PIMathVectorT3d PIQuaternion::eyler() const {
	PIMathMatrixT33d rmat = rotationMatrix();
	double angle_y        = asin(rmat[0][2]), angle_x, angle_z;
	double c              = cos(angle_y);
	if (fabs(c) < 1.E-5) {
		angle_x = 0;
		angle_z = atan2(rmat[1][0], rmat[1][1]);
	} else {
		angle_x = -atan2(-rmat[1][2] / c, rmat[2][2] / c);
		angle_z = atan2(-rmat[0][1] / c, rmat[0][0] / c);
	}
	if (angle_z < 0) angle_z = 2 * M_PI + angle_z;
	return PIMathVectorT3d({angle_x, angle_y, angle_z});
}


PIMathMatrixT33d PIQuaternion::rotationMatrix() const {
	PIMathMatrixT33d ret;
	double xx = q[1] * q[1];
	double xy = q[1] * q[2];
	double xz = q[1] * q[3];
	double xw = q[1] * q[0];
	double yy = q[2] * q[2];
	double yz = q[2] * q[3];
	double yw = q[2] * q[0];
	double zz = q[3] * q[3];
	double zw = q[3] * q[0];
	ret[0][0] = 1. - 2. * (yy + zz);
	ret[0][1] = 2. * (xy - zw);
	ret[0][2] = 2. * (xz + yw);
	ret[1][0] = 2. * (xy + zw);
	ret[1][1] = 1. - 2. * (xx + zz);
	ret[1][2] = 2. * (yz - xw);
	ret[2][0] = 2. * (xz - yw);
	ret[2][1] = 2. * (yz + xw);
	ret[2][2] = 1. - 2. * (xx + yy);
	return ret;
}


void PIQuaternion::axis(PIMathVectorT3d * ret) const {
	if (!ret) return;
	ret[0] = vector();
}


PIQuaternion PIQuaternion::rotated(const PIMathVectorT3d & u, double a) const {
	PIQuaternion ret(*this);
	ret.rotate(u, a);
	return ret;
}


void PIQuaternion::rotate(const PIMathVectorT3d & u, double a) {
	PIQuaternion v(u * sin(a / 2.), cos(a / 2.));
	*this = (v * (*this) * v.conjugate());
}


void PIQuaternion::normalize() {
	double d = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
	if (d != 0.)
		for (int i = 0; i < 4; ++i)
			q[i] /= d;
}


PIQuaternion PIQuaternion::fromEyler(double ax, double ay, double az) {
	PIQuaternion q_heading;
	PIQuaternion q_pinch;
	PIQuaternion q_bank;
	PIQuaternion q_tmp;
	q_heading.q[0] = 0;
	q_heading.q[1] = 0;
	q_heading.q[2] = sin(ax / 2);
	q_heading.q[3] = -cos(ax / 2);
	q_pinch.q[0]   = 0;
	q_pinch.q[1]   = sin(ay / 2);
	q_pinch.q[2]   = 0;
	q_pinch.q[3]   = -cos(ay / 2);
	az             = M_PI - az;
	q_bank.q[0]    = sin(az / 2);
	q_bank.q[1]    = 0;
	q_bank.q[2]    = 0;
	q_bank.q[3]    = cos(az / 2);
	q_tmp          = q_heading * q_pinch;
	q_tmp          = q_tmp * q_bank;
	q_tmp.normalize();
	return q_tmp;
}


PIQuaternion PIQuaternion::fromAngles(double ax, double ay, double az) {
	double c1   = cos(ay / 2);
	double s1   = sin(ay / 2);
	double c2   = cos(az / 2);
	double s2   = sin(az / 2);
	double c3   = cos(ax / 2);
	double s3   = sin(ax / 2);
	double c1c2 = c1 * c2;
	double s1s2 = s1 * s2;
	double a    = c1c2 * c3 - s1s2 * s3;
	double x    = c1c2 * s3 + s1s2 * c3;
	double y    = s1 * c2 * c3 + c1 * s2 * s3;
	double z    = c1 * s2 * c3 - s1 * c2 * s3;
	PIQuaternion res;
	res.q[0] = a;
	res.q[1] = x;
	res.q[2] = y;
	res.q[3] = z;
	return res;
}

PIQuaternion PIQuaternion::fromAngles2(double ax, double ay, double az) {
	double om = PIMathVectorT3d({ax, ay, az}).length();
	if (om == 0.) return PIQuaternion(PIMathVectorT3d(), 1.);
	PIQuaternion res;
	res.q[0] = cos(om / 2);
	res.q[1] = ax / om * sin(om / 2);
	res.q[2] = ay / om * sin(om / 2);
	res.q[3] = az / om * sin(om / 2);
	return res;
}


PIQuaternion PIQuaternion::fromRotationMatrix(const PIMathMatrixT33d & m) {
	PIQuaternion ret;
	float tr = m[0][0] + m[1][1] + m[2][2];
	if (tr > 0.0f) {
		ret.q[0] = tr + 1.0f;
		ret.q[1] = m[2][1] - m[1][2];
		ret.q[2] = m[0][2] - m[2][0];
		ret.q[3] = m[1][0] - m[0][1];
	} else {
		if ((m[0][0] > m[1][1]) && (m[0][0] > m[2][2])) {
			ret.q[0] = m[2][1] - m[1][2];
			ret.q[1] = 1.0f + m[0][0] - m[1][1] - m[2][2];
			ret.q[2] = m[0][1] + m[1][0];
			ret.q[3] = m[0][2] + m[2][0];
		} else {
			if (m[1][1] > m[2][2]) {
				ret.q[0] = m[0][2] - m[2][0];
				ret.q[1] = m[0][1] + m[1][0];
				ret.q[2] = 1.0f + m[1][1] - m[0][0] - m[2][2];
				ret.q[3] = m[1][2] + m[2][1];
			} else {
				ret.q[0] = m[1][0] - m[0][1];
				ret.q[1] = m[0][2] + m[2][0];
				ret.q[2] = m[1][2] + m[2][1];
				ret.q[3] = 1.0f + m[2][2] - m[0][0] - m[1][1];
			}
		}
	}
	ret.normalize();
	return ret;
}


PIQuaternion operator*(const PIQuaternion & q0, const PIQuaternion & q1) {
	PIMathVectorT3d v0(q0.vector()), v1(q1.vector());
	double r0          = q0.q[0] * q1.q[0] - v0.dot(v1);
	PIMathVectorT3d qv = v1 * q0.q[0] + v0 * q1.q[0] + v0.cross(v1);
	PIQuaternion ret;
	ret.q[0] = r0;
	ret.q[1] = qv[0];
	ret.q[2] = qv[1];
	ret.q[3] = qv[2];
	return ret;
}


PIQuaternion operator*(const double & a, const PIQuaternion & q1) {
	PIQuaternion ret(q1);
	ret.q[0] *= a;
	ret.q[1] *= a;
	ret.q[2] *= a;
	ret.q[3] *= a;
	return ret;
}