/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
 * Copyright 2012, Ted Carancho
 *
 * Ported from AeroQuad
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

public class Kinematics {
	/* quaternion elements representing the estimated orientation */
	double q0;
	double q1;
	double q2;
	double q3;

	/* scaled integral error */
	double exInt = 0.0;
	double eyInt = 0.0;
	double ezInt = 0.0;

	/* proportional gain governs rate of convergence to accelerometer */
	double kpAcc = 0.2;

	/* integral gain governs rate of convergence of gyroscope biases */
	double kiAcc = 0.0005;

	/* proportional gain governs rate of convergence to magnetometer */
	double kpMag = 2.0;
	/* integral gain governs rate of convergence of gyroscope biases */
	double kiMag = 0.005;

	/* half the sample period*/
	double halfT = 0.0;

	public double[] kinematicsAngle = new double[3];
	public double[] correctedRateVector = new double[3];
	public double[] gyroAngle = new double[2];

	public Kinematics(double hdgX, double hdgY) {
		for (var axis = AXIS.X; axis <= AXIS.Z; axis+=1)
			kinematicsAngle[axis] = 0;
		for (var axis = AXIS.X; axis <= AXIS.Y; axis+=1)
			gyroAngle[axis] = 0;

		double hdg = Math.atan2(hdgY, hdgX);
		q0 = Math.cos(hdg/2);
		q1 = 0;
		q2 = 0;
		q3 = Math.sin(hdg/2);
	}

	public void update(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz, double time) {
		double norm;
		double hx, hy, hz, bx, bz;
		double vx, vy, vz, wx, wy, wz;
		double exAcc, eyAcc, ezAcc;
		double exMag, eyMag, ezMag;
		double q0i, q1i, q2i, q3i;

		log("Kinematics", LogLevelFlags.LEVEL_DEBUG, @"sensors: GYRO: $gx $gy $gz, ACCEL: $ax $ay $az, COMPASS: $mx $my $mz");

		halfT = time/2;

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

		/* normalise the magnometer measurements */
		norm = Math.sqrt(mx*mx + my*my + mz*mz);
		mx = mx / norm;
		my = my / norm;
		mz = mz / norm;

		/* compute reference direction of flux */
		hx = mx * 2 * (0.5 - q2 * q2 - q3 * q3) + my * 2 * (q1 * q2 - q0 * q3)       + mz * 2 * (q1 * q3 + q0 * q2);
		hy = mx * 2 * (q1 * q2 + q0 * q3)       + my * 2 * (0.5 - q1 * q1 - q3 * q3) + mz * 2 * (q2 * q3 - q0 * q1);
		hz = mx * 2 * (q1 * q3 - q0 * q2)       + my * 2 * (q2 * q3 + q0 * q1)       + mz * 2 * (0.5 - q1 * q1 - q2 * q2);

		bx = Math.sqrt((hx*hx) + (hy*hy));
		bz = hz;

		/* estimate direction of gravity and flux (v and w) */
		vx = 2 *(q1 * q3 - q0 * q2);
		vy = 2 *(q0 * q1 + q2 * q3);
		vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

		wx = bx * 2*(0.5 - q2 * q2 - q3 * q3) + bz * 2 * (q1 * q3 - q0 * q2);
		wy = bx * 2*(q1 * q2 - q0 * q3)       + bz * 2 * (q0 * q1 + q2 * q3);
		wz = bx * 2*(q0 * q2 + q1 * q3)       + bz * 2 * (0.5 - q1 * q1 - q2 * q2);

		/* error is sum of cross product between reference direction of fields and direction measured by sensors */
		exAcc = (vy*az - vz*ay);
		eyAcc = (vz*ax - vx*az);
		ezAcc = (vx*ay - vy*ax);

		exMag = (my*wz - mz*wy);
		eyMag = (mz*wx - mx*wz);
		ezMag = (mx*wy - my*wx);

		/* integral error scaled integral gain */
		exInt = exInt + exAcc*kiAcc + exMag*kiMag;
		eyInt = eyInt + eyAcc*kiAcc + eyMag*kiMag;
		ezInt = ezInt + ezAcc*kiAcc + ezMag*kiMag;

		/* adjusted gyroscope measurements */
		correctedRateVector[AXIS.X] = gx = gx + exAcc*kpAcc + exMag*kpMag + exInt;
		correctedRateVector[AXIS.Y] = gy = gy + eyAcc*kpAcc + eyMag*kpMag + eyInt;
		correctedRateVector[AXIS.Z] = gz = gz + ezAcc*kpAcc + ezMag*kpMag + ezInt;

		/* integrate quaternion rate and normalise */
		q0i = (-q1*gx - q2*gy - q3*gz) * halfT;
		q1i = ( q0*gx + q2*gz - q3*gy) * halfT;
		q2i = ( q0*gy - q1*gz + q3*gx) * halfT;
		q3i = ( q0*gz + q1*gy - q2*gx) * halfT;
		q0 += q0i;
		q1 += q1i;
		q2 += q2i;
		q3 += q3i;

		/* normalise quaternion */
		norm = Math.sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
		q0 = q0 / norm;
		q1 = q1 / norm;
		q2 = q2 / norm;
		q3 = q3 / norm;

		/* save the adjusted gyroscope measurements */
		correctedRateVector[AXIS.X] = gx;
		correctedRateVector[AXIS.Y] = gy;
		correctedRateVector[AXIS.Z] = gz;

		/* calculate euler angles */
		kinematicsAngle[AXIS.X] = Math.atan2(2 * (q0*q1 + q2*q3), 1 - 2 *(q1*q1 + q2*q2));
		kinematicsAngle[AXIS.Y] = Math.asin(2 * (q0*q2 - q1*q3));
		kinematicsAngle[AXIS.Z] = Math.atan2(2 * (q0*q3 + q1*q2), 1 - 2 *(q2*q2 + q3*q3));

		log("Kinematics", LogLevelFlags.LEVEL_INFO, @"angles: $(kinematicsAngle[AXIS.X]), $(kinematicsAngle[AXIS.Y]), $(kinematicsAngle[AXIS.Z])");
	}
}