package edu.ucla.iBeaconNav;

import java.util.LinkedList;
import java.util.Queue;

import android.app.Notification;
import android.app.Activity;
import android.app.Service;
import android.content.Context;
import android.content.Intent;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;
import android.os.Handler;
import android.os.IBinder;
import android.os.Message;
import android.os.SystemClock;
import android.widget.EditText;
import edu.ucla.iBeaconNav.R;

public class Sensors implements SensorEventListener
{
	/* Private data */
	private Task          task;

	private SensorManager sensorManager;
	private Sensor        accSensor;
	private Sensor        grvSensor;
	private Sensor        magSensor;
	private Sensor        gyrSensor;

	private boolean       active;
	private long          lastTime_ms    = 0;
	private float         snsInterval_ms = 0;
	private long          lastTime_ns    = 0;
	private long          snsInterval_ns = 0;


	/* Sensor data */
	private float[] accValues        = new float[3];
	private float[] magValues        = new float[3];
	private float[] gyrValues        = new float[3];
	private float[] rotationMatrix_R = new float[9];
	private float[] rtMatrixStable_R = new float[9];
	private float[] rotationMatrix_I = new float[9];
	private float[] orientValues_rd  = new float[3];
	private float[] orientValues     = new float[3];
	private float[] accWorldCoord    = {0,0,0};
	private float[] gyrWorldCoord    = {0,0,0};


	/* Auxiliary Variables for Sensor Processing */

	private final int cycle = 32;

	private LinkedList<Float> accBuffers    = new LinkedList<Float>();
	private LinkedList<Float> magBuffers    = new LinkedList<Float>();
	private LinkedList<Float> gyrBuffers    = new LinkedList<Float>();
	private LinkedList<Float> orientBuffers = new LinkedList<Float>();
	private LinkedList<Float> rtMtrxBuffers = new LinkedList<Float>();
	private float[] data        = new float[4];
	private float[] accSum      = {0,0,0};
	private float[] accAvg      = {0,0,0};
	private float[] magSum      = {0,0,0};
	private float[] gyrSum      = {0,0,0};
	private float[] gyrOffset   = {0,0,0};
	private float[] orientSum   = {0,0,0};
	private float[] rtMtrxSum   = {0,0,0,
			                       0,0,0,
			                       0,0,0};
	private float[] accBuffer   = {0,0,0};
	private float[] magBuffer   = {0,0,0};
	private float[] orientBuffer= {0,0,0};
	private int     cnt         = 0;
	private int     accCnt      = 0;
	private int     magCnt      = 0;
	private int     gyrCnt      = 0;
	private int     orientCnt   = 0;
	private int     rtMtrxCnt   = 0;

	private boolean ifGyrOffset = false;
	private boolean ifGrvOffset = false;
	private boolean ifStable    = false;

	private final float EPSILON       = (float)0.01;

	private float   accValueTotal     = 0;
	private float   calculatedGravity = SensorManager.GRAVITY_EARTH;
	private float   gravityRef        = 0;
	private float   gyroscopeRef      = 0;
	private boolean ifSetGrvRef       = false;

	/* Position Related Stuff */
	//private float   startPosX      = 0;
	//private float   startPos       = 0;
	private float   currentPosX    = 0;
	private float   currentPosY    = 0;
	// rotate around gravity direction, positive when counterclock-wise, initially align with user's first step
	private float   currentHeading = 0;
	private float   headingOffset  = 0;
	private float   stepLength     = (float)0.5;    // in m
	private int     stepCount      = 0;
	private boolean stepStart      = false;


	/* Integrated positions */
	private Vect    position       = new Vect();
	private Quat    rotation       = new Quat();

	/* Test data */
	private long    time1hz        = 0;
	private Vect    gyr1hz         = new Vect();

	/* Public methods */
	public Sensors(Task task)
	{
		Util.debug("Sensors: constructor");
		this.task          = task;
		this.sensorManager = (SensorManager)task.getSystemService(Context.SENSOR_SERVICE);

		// Get sensors
		accSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
		grvSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
		magSensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
		gyrSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);

		// Connect sensor listeners
		sensorManager.registerListener(this, accSensor, SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(this, grvSensor, SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_FASTEST);
		sensorManager.registerListener(this, gyrSensor, SensorManager.SENSOR_DELAY_FASTEST);
	}

	public void reset_heading() {
		Util.debug("Sensors: handle - reset heading");
		this.rotation = new Quat();
		currentHeading = 0;
		displayData(CMD.Data.HEADING);
	}

	public void reset_distance() {
		Util.debug("Sensors: handle - reset distance");
		currentPosX = 0;
		currentPosY = 0;
		displayData(CMD.Data.POSITION);
	}

	/* Sensor Event Listener functions */
	@Override
	public void onAccuracyChanged(Sensor sensor, int accuracy) {
		Util.debug("Sensors: onAccuracyChanged");
	}

	@Override
	public void onSensorChanged(SensorEvent event) {
		switch(event.sensor.getType()){
		case Sensor.TYPE_ACCELEROMETER:
			accCnt++;
			System.arraycopy(event.values, 0, accValues, 0, 3);
			//accCnt = everyAveragedBuffer(accBuffer, accValues, 3, accCnt, cycle, CMD.Data.ACC);
			forwardAveragedBuffer(accBuffers, accSum, accAvg, accValues, 3, accCnt, cycle, CMD.Data.ACC);
			toWorldCoordinates(accWorldCoord, accValues, rotationMatrix_R, accCnt, CMD.Data.WRDACC);
			accValueTotal = accWorldCoord[2];
			if (ifSetGrvRef && !ifGrvOffset && Math.abs((accValueTotal-gravityRef)/gravityRef)<0.05){
				rtMatrixStable_R  = rotationMatrix_R;
				calculatedGravity = accValueTotal;
				ifGrvOffset = true;
				ifGyrOffset = false;
			}
			else if(!ifSetGrvRef && accCnt>400){
				ifSetGrvRef = true;
				gravityRef  = accValueTotal;
			}
			updateOrientation();
			break;
		case Sensor.TYPE_MAGNETIC_FIELD:
			magCnt++;
			System.arraycopy(event.values, 0, magValues, 0, 3);
			//magCnt = everyAveragedBuffer(magBuffer, magValues, 3, magCnt, cycle, CMD.Data.MAG);
			forwardAveragedBuffer(magBuffers, magSum, null, magValues, 3, magCnt, cycle, CMD.Data.MAG);
			updateOrientation();       // This maybe useless
			break;
		case Sensor.TYPE_GYROSCOPE:
			gyrCnt++;
			float gyrTemp[] = new float[3];
			long currentTime_ns = event.timestamp;
			snsInterval_ns = currentTime_ns-lastTime_ns;
			lastTime_ns    = currentTime_ns;

			int chkIntCnt = 8;				//check Interval Count
			if (gyrCnt%chkIntCnt==0){
				//long currentTime_ms = SystemClock.elapsedRealtime();
				//snsInterval_ms = (float)(currentTime_ms-lastTime_ms)/chkIntCnt;
				if(ifSetGrvRef){
					ifStable = isStable(accBuffers, 3, cycle/2) && isStable(gyrBuffers, 3, cycle/2);
				}
				if (ifStable){
					ifGrvOffset = false;
				}
				//lastTime_ms = SystemClock.elapsedRealtime();
				//Util.debug("Interal in ms: "+Float.toString(snsInterval_ms));
			}

			System.arraycopy(event.values, 0, gyrValues, 0, 3);
			System.arraycopy(gyrValues, 0, gyrTemp, 0, 3);
			forwardAveragedBuffer(gyrBuffers, gyrSum, null, gyrValues, 3, gyrCnt, 2*cycle, CMD.Data.GYR);
			System.arraycopy(gyrTemp, 0, gyrValues, 0, 3);
			if (!ifGyrOffset && ifGrvOffset){
				//Util.debug("[GYR1] Reset  "+Float.toString(gyrValues[2]));
				gyrOffset[0] = gyrValues[0];			//x
				gyrOffset[1] = gyrValues[1];			//y
				gyrOffset[2] = gyrValues[2];			//z
				ifGyrOffset = true;
			}
			depleteOffset(gyrValues, gyrOffset, 3);
			//cycleFloatArray(gyrValues, 3);
			toWorldCoordinates(gyrWorldCoord, gyrValues, rtMatrixStable_R, gyrCnt, CMD.Data.WRDGYR);

			float gyrZ = gyrWorldCoord[2];
			//if (!ifStable && ifGyrOffset && Math.abs(gyrZ)>0.05){
			if (ifGyrOffset){
				//currentHeading+=gyrZ*snsInterval_ms/1000*180/Math.PI;
				currentHeading += gyrZ*snsInterval_ns/1000000000*180/Math.PI;
				currentHeading%=360;
				displayData(CMD.Data.HEADING);
			}

			// Integrate rotations
			Vect rpy = new Vect(event.values[1], event.values[0], -event.values[2]);
			rpy.mul((double)snsInterval_ns/1E9);
			Quat rot = new Quat(rpy.x, rpy.y, rpy.z);
			this.rotation.mul(rot);
			displayData(CMD.Data.ROTATION);

			// Calculate 1 second gryo data
			this.gyr1hz.add(rpy);
			if (currentTime_ns > time1hz+1E9) {
				displayData(CMD.Data.GYR1HZ);
				this.gyr1hz.set(0, 0, 0);
				time1hz = currentTime_ns;
			}

			break;


		}
		processSensorInfo();
	}

	/* Private helper functions */
	private void cycleFloatArray(float[] array, int length){
		float temp = array[length-1];
		for(int i=1; i<length; i++){
			array[i] = array[i-1];
		}
		array[0]=temp;
	}

	private void depleteOffset(float[] values, float[] offset, int len){
		for(int i=0; i<len; i++){
			values[i]-=offset[i];
		}
	}

	private boolean isStable(LinkedList<Float> buffer,  int length, int cycle){
		int len = buffer.size();
		float[] avrg   = new float[length];
		float[] devSum = new float[length];
		float   ttDev  = 0;
		for(int i=0; i<length; i++){
			avrg[length-1-i]=0;
			for (int j=0; j<cycle; j++){
				avrg[length-1-i]+=buffer.get(len-1-j*length-i);
			}
			avrg[length-1-i]/=cycle;
		}
		for (int i=0; i<length; i++){
			devSum[i] = 0;
			for(int j=0; j<cycle; j++){
				devSum[length-1-i]+=(float) Math.pow(buffer.get(len-1-j*length-i)-avrg[length-1-i],2);
			}
			devSum[length-1-i] = (float) Math.sqrt(devSum[length-1-i]/cycle);
		}

		for (int i=0; i<length; i++){
			ttDev+=devSum[i];
		}

		if (ttDev>0.1){
			//Util.debug("[DEV] unStable");
			displayData(CMD.Data.STABLE);
			return false;
		}else{
			//Util.debug("[DEV] Stable");
			displayData(CMD.Data.STABLE);
			return true;
		}
	}

	private void processSensorInfo(){
		displayData(CMD.Data.STPCNT);
		float epsl = (float)0.6;
		// on step count????
		if (stepStart){
			//judge if stop
			if(accValueTotal-calculatedGravity>epsl){
				stepStart = false;
				stepCount++;
				currentPosX += stepLength*Math.sin(currentHeading);
				currentPosY += stepLength*Math.cos(currentHeading);
				displayData(CMD.Data.POSITION);
			}

		}else{
			//judge if start
			if (calculatedGravity-accValueTotal>epsl ){
				stepStart = true;
			}
		}
	}

	private void updateOrientation(){
		if (accValues == null || magValues == null){
			return;
		}
		float R[] = new float[9];
		float I[] = new float[9];
		boolean success;
		success = SensorManager.getRotationMatrix(R, I, accValues, magValues);
		if(success){
			orientCnt++;
			rtMtrxCnt++;
			rotationMatrix_R = R;
			rotationMatrix_I = I;
			SensorManager.getOrientation(rotationMatrix_R, orientValues_rd);
		}
		// to degree
		orientValues[0] = (float) (orientValues_rd[0]*180/Math.PI);
		orientValues[1] = (float) (orientValues_rd[1]*180/Math.PI);
		orientValues[2] = (float) (orientValues_rd[2]*180/Math.PI);

		//orientCnt=everyAveragedBuffer(orientBuffer, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
		forwardAveragedBuffer(rtMtrxBuffers,rtMtrxSum, null, rotationMatrix_R, 9, rtMtrxCnt, 2*cycle, null);
		forwardAveragedBuffer(orientBuffers,orientSum, null, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
		//displayData(CMD.Data.ORIENT);
	}

	private void toWorldCoordinates(float[] worldCoord, float[] values, float[] rotationMatrix, int cnt, CMD.Data cmd){
		float result[] = new float[3];
		result = new Matrix(rotationMatrix).multipleV(values);
		worldCoord[0] = result[0];
		worldCoord[1] = result[1];
		worldCoord[2] = result[2];
		if(cnt%32==0){
			displayData(cmd);
		}
	}

	private void displayData(CMD.Data dataType){
		//Util.debug("Sensors: displayData");
		float data[] = new float[4];
		data[0] = dataType.ordinal();
		switch(dataType) {
		case ACC:
			data[1] = accValues[0];
			data[2] = accValues[1];
			data[3] = accValues[2];
			break;
		case MAG:
			data[1] = magValues[0];
			data[2] = magValues[1];
			data[3] = magValues[2];
			break;
		case ORIENT:
			data[1] = orientValues[0];
			data[2] = orientValues[1];
			data[3] = orientValues[2];
			break;
		case WRDACC:
			data[1] = accWorldCoord[0];
			data[2] = accWorldCoord[1];
			data[3] = accWorldCoord[2];
			break;
		case STPCNT:
			data[1] = stepCount;
			data[2] = calculatedGravity;
			break;
		case POSITION:
			data[1] = currentPosX;
			data[2] = currentPosY;
			break;
		case ROTATION:
			Vect rpy = new Vect();
			this.rotation.get(rpy);
			data[1] = (float)(rpy.x * (180/Math.PI));
			data[2] = (float)(rpy.y * (180/Math.PI));
			data[3] = (float)(rpy.z * (180/Math.PI));
			break;
		case GYR:
			data[1] = gyrValues[0];
			data[2] = gyrValues[1];
			data[3] = gyrValues[2];
			break;
		case GYR1HZ:
			data[1] = (float)(gyr1hz.x * (180/Math.PI) * 60 * 60);
			data[2] = (float)(gyr1hz.y * (180/Math.PI) * 60 * 60);
			data[3] = (float)(gyr1hz.z * (180/Math.PI) * 60 * 60);
			break;
		case HEADING:
			data[1] = currentHeading;
			break;
		case WRDGYR:
			data[1] = gyrWorldCoord[0];
			break;
		case STABLE:
			data[1] = ifStable?1:0;
			break;
		default:
			Util.debug("Bad Data Sending Command!");
			return;
		}

		this.task.tellMain(CMD.Response.SHOWDATA, data);
	}

	private int everyAveragedBuffer(float[] buffer, float[] values, int length, int cnt, int cycle, CMD.Data cmd){
		for(int i=0; i<length; i++){
			buffer[i]+=values[i];
		}
		if (cnt == cycle){
			cnt = 0;
			for(int i=0; i<length; i++){
				values[i] = buffer[i]/cycle;
				buffer[i] = 0;
			}
			displayData(cmd);
		}
		return cnt;
	}

	private void forwardAveragedBuffer(LinkedList<Float> buffer, float[] sum, float[] avg,
			float[] values, int length, int cnt, int cycle, CMD.Data cmd){
		if (buffer==null||buffer.size()<cycle*length){
			for(int i=0; i<length; i++){
				buffer.addLast(values[i]);
				sum[i]+=values[i];
				values[i] = sum[i]/buffer.size();
			}
		}
		else{
			float[] discarded = new float[length];
			for(int i=0; i<length; i++){
				discarded[i]= buffer.removeFirst();
				buffer.addLast(values[i]);
				sum[i]-=discarded[i];
				sum[i]+=values[i];
				values[i]=sum[i]/cycle;
			}
		}
		if(avg!=null){
			for(int i=0; i<length; i++){
				avg[i]=sum[i]/cycle;
			}
		}
		if (cnt%32==0 && cmd != null){
			displayData(cmd);
		}
	}

	private void printMatrix(String s, Matrix m){
		Util.debug("Sensor: ["+s+"] "+m.mValue[0]+" "+m.mValue[1]+" "+m.mValue[2]);
		Util.debug("Sensor:       "  +m.mValue[3]+" "+m.mValue[4]+" "+m.mValue[5]);
		Util.debug("Sensor:       "  +m.mValue[6]+" "+m.mValue[7]+" "+m.mValue[8]);
	}

	private void printVector(String s, float[] v){
		Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
	}
}