/* Copyright 2010, ARM Limited
 * Original C++ Code written by Aaron Berk
 * Ported to Vala by Sebastian Reichel <sre@ring0.de>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to
 * deal in the Software without restriction, including without limitation the
 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
 * sell copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
 * IN THE SOFTWARE.
 */

public class Kinematics {
	double[] result = new double[3];
    bool firstUpdate;

    /* Quaternion orientation of earth frame relative to auxiliary frame. */
    double AEq_1;
    double AEq_2;
    double AEq_3;
    double AEq_4;

    /* Estimated orientation quaternion elements with initial conditions. */
    double SEq_1;
    double SEq_2;
    double SEq_3;
    double SEq_4;

	/* filter tuning constant */
	double beta;

	uint i = 0;

	public Kinematics() {
		reset();

		var gyroMeasError = 10.0;
		beta = Math.sqrt(3.0 / 4.0) * (Math.PI * (gyroMeasError / 180.0));
	}

	public void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double time) {
		//var deltat = time/1000000.0;
		//stdout.printf(@"$deltat\n");
		var deltat = 0.01;

		i++;

		//stdout.printf(@"$i $gx $gy $gz\n");

		/* auxiliary variables to avoid reapeated calcualtions */
		var halfSEq_1 = 0.5 * SEq_1;
		var halfSEq_2 = 0.5 * SEq_2;
		var halfSEq_3 = 0.5 * SEq_3;
		var halfSEq_4 = 0.5 * SEq_4;
		var twoSEq_1  = 2.0 * SEq_1;
		var twoSEq_2  = 2.0 * SEq_2;
		var twoSEq_3  = 2.0 * SEq_3;

		/* compute the quaternion rate measured by gyroscopes */
		var SEqDot_omega_1 = -halfSEq_2 * gx - halfSEq_3 * gy - halfSEq_4 * gz;
		var SEqDot_omega_2 =  halfSEq_1 * gx + halfSEq_3 * gz - halfSEq_4 * gy;
		var SEqDot_omega_3 =  halfSEq_1 * gy - halfSEq_2 * gz + halfSEq_4 * gx;
		var SEqDot_omega_4 =  halfSEq_1 * gz + halfSEq_2 * gy - halfSEq_3 * gx;

		/* normalise the accelerometer measurement */
		var norm = Math.sqrt(ax * ax + ay * ay + az * az);
		ax /= norm;
		ay /= norm;
		az /= norm;

		/* compute the objective function and Jacobian */
		var f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - ax;
		var f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - ay;
		var f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - az;

		/* J_11 negated in matrix multiplication */
		var J_11or24 = twoSEq_3;
		var J_12or23 = 2 * SEq_4;

		/* J_12 negated in matrix multiplication */
		var J_13or22 = twoSEq_1;
		var J_14or21 = twoSEq_2;

		/* negated in matrix multiplication */
		var J_32 = 2 * J_14or21;

		/* negated in matrix multiplication */
		var J_33 = 2 * J_11or24;

		/* compute the gradient (matrix multiplication) */
		var nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1;
		var nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
		var nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
		var nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2;

		/* normalise the gradient */
		norm = Math.sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4);
		nablaf_1 /= norm;
		nablaf_2 /= norm;
		nablaf_3 /= norm;
		nablaf_4 /= norm;

		/* compute then integrate the estimated quaternion rate */
		SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat;
		SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat;
		SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat;
		SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat;

		/* normalise quaternion */
		norm = Math.sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
		SEq_1 /= norm;
		SEq_2 /= norm;
		SEq_3 /= norm;
		SEq_4 /= norm;

		if(firstUpdate) {
			/* store orientation of auxiliary frame. */
			AEq_1 = SEq_1;
			AEq_2 = SEq_2;
			AEq_3 = SEq_3;
			AEq_4 = SEq_4;
			firstUpdate = false;
		}

		compute_euler();
	}

	private void compute_euler() {
		/* compute the quaternion conjugate */
		var ESq_1 =  SEq_1;
		var ESq_2 = -SEq_2;
		var ESq_3 = -SEq_3;
		var ESq_4 = -SEq_4;

		/* compute the quaternion product */
		var ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4;
		var ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3;
		var ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2;
		var ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1;

		/* compute the Euler angles from the quaternion */
		var phi   = Math.atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1);
		var theta = Math.asin(2 * ASq_2 * ASq_4 - 2 * ASq_1 * ASq_3);
		var psi   = Math.atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1);

		result[AXIS.X] = phi;   /* roll */
		result[AXIS.Y] = theta; /* pitch */
		result[AXIS.Z] = psi;   /* yaw */

		stdout.printf(@"$i $(degree(result[AXIS.X]))\t$(degree(result[AXIS.Y]))\t$(degree(result[AXIS.Z]))\n");
	}

	public double[] get_data() {
		return result;
	}

	public void reset() {
		firstUpdate = true;

		/* Quaternion orientation of earth frame relative to auxiliary frame. */
		AEq_1 = 1;
		AEq_2 = 0;
		AEq_3 = 0;
		AEq_4 = 0;

		/* Estimated orientation quaternion elements with initial conditions. */
		SEq_1 = 1;
		SEq_2 = 0;
		SEq_3 = 0;
		SEq_4 = 0;
	}
}