package org.greenstone.android.tipple.base;

// Based on "Android Sensor Fusion" by Paul Lawitzki
// http://www.thousand-thoughts.com/2012/03/android-sensor-fusion-tutorial/

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import java.util.Timer;
import java.util.TimerTask;

/**
 * Singleton which uses the gyroscope, accelerometer and compass to calculate the orientation.
 */
public class SensorFusion implements SensorEventListener {

	private static final float EPSILON = 1e-9f;
	private static final float NS2S = 1e-9f; // conversion from nanoseconds to seconds
	private static final int TIME_CONSTANT = 30; // how often to correct gyro drift
	private static final float FILTER_COEFFICIENT = 0.98f;
	private static final float GYRO_TOLERANCE = 0.5f; // error level at which gyro gets reset

	private static SensorFusion instance;
	private int clients = 0; // the number of activities who currently need sensor data

	private SensorManager mSensorManager = null;

	private float[] gyro = new float[3]; // angular speeds from gyro
	private float[] gyroMatrix = new float[9]; // rotation matrix from gyro data
	private float[] gyroOrientation = new float[3]; // orientation angles from gyro matrix

	private float[] magnet = new float[3]; // magnetic field vector
	private float[] accel = new float[3]; // accelerometer vector
	private float[] accMagOrientation = new float[3]; // orientation angles from accel and magnet
	private float[] fusedOrientation = new float[3]; // final orientation angles from sensor fusion
	private float[] accMagMatrix = new float[9]; // accel and magnet based rotation matrix
	private float gyroTimestamp;

	private Timer fuseTimer = new Timer();
	private TimerTask fuseTask = null;

	private SensorFusion(SensorManager sm) {
		mSensorManager = sm;
	}

	public static SensorFusion getInstance() {
		if (instance == null) {
			instance = new SensorFusion(
					(SensorManager) Global.activity.getSystemService(Context.SENSOR_SERVICE));
		}
		return instance;
	}

	/**
	 * Tells the SensorFusion object that there is someone who would like to know the orientation.
	 * When there are no clients the SensorFusion object removes its event listeners.
	 */
	public void addClient(String name) {
		if (clients++ == 0) {
			registerListeners();
		}
	}

	/**
	 * Tells the SensorFusion object that someone no longer needs the orientation. When there are no
	 * clients the SensorFusion object removes its event listeners.
	 */
	public void removeClient(String name) {
		if (clients < 0) {
			throw new RuntimeException("SensorFusion: removed too many clients");
		}

		if (--clients == 0) {
			unregisterListeners();
		}
	}

	// Registers sensor listeners for the accelerometer, compass and gyroscope.
	// Creates a timer task to correct the gyro drift.
	private void registerListeners() {
		System.err.println("SensorFusion::registerListeners()");
		mSensorManager.registerListener(this, mSensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY),
				SensorManager.SENSOR_DELAY_FASTEST);

		mSensorManager.registerListener(this,
				mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
				SensorManager.SENSOR_DELAY_FASTEST);

		mSensorManager.registerListener(this,
				mSensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE),
				SensorManager.SENSOR_DELAY_NORMAL);

		fuseTask = new FuseTask();
		fuseTimer.scheduleAtFixedRate(fuseTask, 0, TIME_CONSTANT);
	}

	private void unregisterListeners() {
		System.err.println("SensorFusion::unregisterListeners()");
		mSensorManager.unregisterListener(this);
		fuseTask.cancel();
		fuseTask = null;
	}

	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
	}

	@Override
	public void onSensorChanged(SensorEvent event) {
		switch (event.sensor.getType()) {
		case Sensor.TYPE_GRAVITY:
			// copy new accelerometer data into accel array and calculate orientation
			System.arraycopy(event.values, 0, accel, 0, 3);
			calculateAccMagOrientation();
			break;

		case Sensor.TYPE_MAGNETIC_FIELD:
			// copy new compass data into magnet array
			System.arraycopy(event.values, 0, magnet, 0, 3);
			if(ARActivity.directionss){
				ArDirection.setChanges(getBearingDegrees());
			}
			break;

		case Sensor.TYPE_GYROSCOPE:
			// process gyro data
			gyroFunction(event);
			break;
		}
	}

	// calculates orientation angles from accelerometer and compass output
	private void calculateAccMagOrientation() {
		if (SensorManager.getRotationMatrix(accMagMatrix, null, accel, magnet)) {
			SensorManager.getOrientation(accMagMatrix, accMagOrientation);
		}
	}

	// This function is borrowed from the Android reference
	// at http://developer.android.com/reference/android/hardware/SensorEvent.html#values
	// It calculates a rotation vector from the gyroscope angular speed values.
	private void getRotationVectorFromGyro(float[] gyroValues, float[] deltaRotationVector,
			float timeFactor) {
		float[] normValues = new float[3];

		// Calculate the angular speed of the sample
		float omegaMagnitude = (float) Math.sqrt(gyroValues[0] * gyroValues[0] + gyroValues[1]
				* gyroValues[1] + gyroValues[2] * gyroValues[2]);

		// Normalize the rotation vector if it's big enough to get the axis
		if (omegaMagnitude > EPSILON) {
			normValues[0] = gyroValues[0] / omegaMagnitude;
			normValues[1] = gyroValues[1] / omegaMagnitude;
			normValues[2] = gyroValues[2] / omegaMagnitude;
		}

		// Integrate around this axis with the angular speed by the timestep
		// in order to get a delta rotation from this sample over the timestep
		// We will convert this axis-angle representation of the delta rotation
		// into a quaternion before turning it into the rotation matrix.
		float thetaOverTwo = omegaMagnitude * timeFactor;
		float sinThetaOverTwo = (float) Math.sin(thetaOverTwo);
		float cosThetaOverTwo = (float) Math.cos(thetaOverTwo);
		deltaRotationVector[0] = sinThetaOverTwo * normValues[0];
		deltaRotationVector[1] = sinThetaOverTwo * normValues[1];
		deltaRotationVector[2] = sinThetaOverTwo * normValues[2];
		deltaRotationVector[3] = cosThetaOverTwo;
	}

	// This function performs the integration of the gyroscope data.
	// It writes the gyroscope based orientation into gyroOrientation.
	private void gyroFunction(SensorEvent event) {
		// copy the new gyro values into the gyro array
		// convert the raw gyro data into a rotation vector
		float[] deltaVector = new float[4];
		if (gyroTimestamp != 0) {
			final float dT = (event.timestamp - gyroTimestamp) * NS2S;
			System.arraycopy(event.values, 0, gyro, 0, 3);
			getRotationVectorFromGyro(gyro, deltaVector, dT / 2.0f);
		}

		// measurement done, save current time for next interval
		gyroTimestamp = event.timestamp;

		// convert rotation vector into rotation matrix
		float[] deltaMatrix = new float[9];
		SensorManager.getRotationMatrixFromVector(deltaMatrix, deltaVector);

		// apply the new rotation interval on the gyroscope based rotation matrix
		gyroMatrix = matrixMultiplication(gyroMatrix, deltaMatrix);

		// get the gyroscope based orientation from the rotation matrix
		SensorManager.getOrientation(gyroMatrix, gyroOrientation);
	}

	private float getGyroError() {
		float error = 0;
		// distance between the pairs of axes
		for (int i = 0; i < 9; i++) {
			error += (accMagMatrix[i] - gyroMatrix[i]) * (accMagMatrix[i] - gyroMatrix[i]);
		}
		return error;
	}

	private float filterAngle(float gyroAngle, float accMagAngle) {
		// corrects the gyro angle using the angle from the accelerometer/compass

		float diff = accMagAngle - gyroAngle;
		if (diff > Math.PI) {
			accMagAngle -= 2 * Math.PI;
		} else if (diff < -Math.PI) {
			accMagAngle += 2 * Math.PI;
		}

		float result = FILTER_COEFFICIENT * gyroAngle + (1 - FILTER_COEFFICIENT) * accMagAngle;

		if (result > Math.PI) {
			result -= 2 * Math.PI;
		} else if (result < -Math.PI) {
			result += 2 * Math.PI;
		}

		return result;
	}

	private float[] getRotationMatrixFromOrientation(float[] o) {
		float sinX = (float) Math.sin(o[1]);
		float cosX = (float) Math.cos(o[1]);
		float sinY = (float) Math.sin(o[2]);
		float cosY = (float) Math.cos(o[2]);
		float sinZ = (float) Math.sin(o[0]);
		float cosZ = (float) Math.cos(o[0]);

		float[] result = new float[] { cosY * cosZ - sinX * sinY * sinZ, cosX * sinZ,
				sinY * cosZ + sinX * cosY * sinZ, -cosY * sinZ - sinX * sinY * cosZ, cosX * cosZ,
				-sinY * sinZ + sinX * cosY * cosZ, -cosX * sinY, -sinX, cosX * cosY };

		return result;
	}

	private float[] matrixMultiplication(float[] A, float[] B) {
		float[] result = new float[9];

		result[0] = A[0] * B[0] + A[1] * B[3] + A[2] * B[6];
		result[1] = A[0] * B[1] + A[1] * B[4] + A[2] * B[7];
		result[2] = A[0] * B[2] + A[1] * B[5] + A[2] * B[8];

		result[3] = A[3] * B[0] + A[4] * B[3] + A[5] * B[6];
		result[4] = A[3] * B[1] + A[4] * B[4] + A[5] * B[7];
		result[5] = A[3] * B[2] + A[4] * B[5] + A[5] * B[8];

		result[6] = A[6] * B[0] + A[7] * B[3] + A[8] * B[6];
		result[7] = A[6] * B[1] + A[7] * B[4] + A[8] * B[7];
		result[8] = A[6] * B[2] + A[7] * B[5] + A[8] * B[8];

		return result;
	}

	class FuseTask extends TimerTask {
		public void run() {
			
			for (int i = 0; i < 3; i++) {
				fusedOrientation[i] = filterAngle(gyroOrientation[i], accMagOrientation[i]);
			}

			// if the gyro is too far from the accel/magnet orientation, reset it
			if (getGyroError() > GYRO_TOLERANCE) {
				System.arraycopy(accMagOrientation, 0, fusedOrientation, 0, 3);
			}

			// overwrite gyro matrix and orientation with fused orientation
			// to compensate gyro drift
			gyroMatrix = getRotationMatrixFromOrientation(fusedOrientation);
			System.arraycopy(fusedOrientation, 0, gyroOrientation, 0, 3);
		}
	}

	public float[] getRotationMatrix() {
		return gyroMatrix;
	}

	/**
	 * Converts a 3D location in world coordinates to phone coordinates
	 */
	public float[] worldToPhone(float[] coord) {
		// gyroMatrix transforms phone coordinates to world coordinates. To convert the other way,
		// need to multiply by the inverse (which is the transpose for rotation matrices)
		return new float[] {
				coord[0] * gyroMatrix[0] + coord[1] * gyroMatrix[3] + coord[2] * gyroMatrix[6],
				coord[0] * gyroMatrix[1] + coord[1] * gyroMatrix[4] + coord[2] * gyroMatrix[7],
				coord[0] * gyroMatrix[2] + coord[1] * gyroMatrix[5] + coord[2] * gyroMatrix[8] };
	}

	public float getBearing() {
		return (float) fusedOrientation[0];
	}

	public float getBearingDegrees() {
		return (float) Math.toDegrees(getBearing());
	}

	/**
	 * Returns the angle (in radians) the screen has been turned anticlockwise from landscape
	 * position. e.g. 0 means the phone is landscape, -pi/2 means the phone is portrait.
	 */
	public float getScreenAngle() {
		return (float) -Math.atan2(gyroMatrix[7], gyroMatrix[6]);
	}

	/**
	 * Returns the angle (in degrees) the screen has been turned anticlockwise from landscape
	 * position. e.g. 0 means the phone is landscape, -90 means the phone is portrait.
	 */
	public float getScreenAngleDegrees() {
		return (float) Math.toDegrees(getScreenAngle());
	}
}