From 1ac485ec0eff95b11ba80a3c98b8ddfd9c5be56f Mon Sep 17 00:00:00 2001
From: Andy Spencer <andy753421@gmail.com>
Date: Fri, 7 Mar 2014 07:10:15 +0000
Subject: [PATCH] Merge changes from Yue

---
 AndroidManifest.xml                  |   3 +-
 res/layout/main.xml                  | 284 +++++++++++++-
 src/edu/ucla/iBeaconNav/CMD.java     |  28 +-
 src/edu/ucla/iBeaconNav/Main.java    | 127 ++++++-
 src/edu/ucla/iBeaconNav/Matrix.java  |  78 ++++
 src/edu/ucla/iBeaconNav/Sensors.java | 548 +++++++++++++++++++++++++++
 6 files changed, 1043 insertions(+), 25 deletions(-)
 create mode 100644 src/edu/ucla/iBeaconNav/Matrix.java
 create mode 100644 src/edu/ucla/iBeaconNav/Sensors.java

diff --git a/AndroidManifest.xml b/AndroidManifest.xml
index 8f0b15b..786e5c9 100644
--- a/AndroidManifest.xml
+++ b/AndroidManifest.xml
@@ -19,7 +19,7 @@
 		android:glEsVersion="0x00020000"
 		android:required="true"/>
 	<uses-sdk
-		android:minSdkVersion="18"
+		android:minSdkVersion="16"
 		android:targetSdkVersion="18" />
 
 	<!-- Application -->
@@ -33,6 +33,7 @@
 		</activity>
 		<activity android:name="Prefs" />
 		<service android:name="Task" />
+		<service android:name="Sensors" />
 		<service android:name="com.radiusnetworks.ibeacon.service.IBeaconService"
 			android:enabled="true"
 			android:exported="true"
diff --git a/res/layout/main.xml b/res/layout/main.xml
index 1219ee8..4a797b8 100644
--- a/res/layout/main.xml
+++ b/res/layout/main.xml
@@ -17,27 +17,287 @@
 			android:layout_width="fill_parent"
 			android:layout_height="fill_parent">
 			<LinearLayout
-				android:id="@+id/map"
+				android:id="@+id/state"
 				android:orientation="vertical"
 				android:layout_width="fill_parent"
 				android:layout_height="fill_parent">
-				<fragment
-					xmlns:android="http://schemas.android.com/apk/res/android"
-					android:id="@+id/map_fragment"
+				<GridLayout
 					android:layout_width="match_parent"
-					android:layout_height="match_parent"
-					android:name="com.google.android.gms.maps.MapFragment"/>
+					android:layout_height="wrap_content"
+					android:columnCount="1" >
+					<TextView
+						android:id="@+id/accText1"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="accValueX"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/accText2"
+						android:layout_column="0"
+						android:layout_gravity="center_horizontal|top"
+						android:layout_row="1"
+						android:text="accValueY"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/accText3"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="1"
+						android:text="accValueZ"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/textView1"
+						android:layout_width="wrap_content"
+						android:layout_height="wrap_content"
+						android:layout_column="0"
+						android:layout_gravity="left|center_vertical"
+						android:layout_row="0"
+						android:text="Accelerometer Info"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1"
+					android:paddingTop="10dp" >
+					<TextView
+						android:id="@+id/wrdAccText1"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/wrdAccText2"
+						android:layout_column="0"
+						android:layout_gravity="center_horizontal|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/wrdAccText3"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/textView4"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="0"
+						android:text="World Acceleration Info"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1"
+					android:paddingTop="10dp" >
+					<TextView
+						android:id="@+id/textView8"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="0"
+						android:text="Gyroscope Info"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/gyrText1"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/gyrText2"
+						android:layout_column="0"
+						android:layout_gravity="center_horizontal|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/gyrText3"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1"
+					android:paddingTop="10dp" >
+					<TextView
+						android:id="@+id/orientText1"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="2"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/orientText2"
+						android:layout_column="0"
+						android:layout_gravity="center_horizontal|top"
+						android:layout_row="2"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/orientText3"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="2"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/textView3"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="Orientation Info"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1"
+					android:paddingTop="10dp" >
+					<TextView
+						android:id="@+id/textView5"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="0"
+						android:text="Step Count"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/stepCntText"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/calGrvText"
+						android:layout_width="147dp"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/textView6"
+						android:layout_column="0"
+						android:layout_gravity="right|bottom"
+						android:layout_row="0"
+						android:text="Calculated Gravity"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1"
+					android:paddingTop="10dp" >
+					<TextView
+						android:id="@+id/textView7"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="0"
+						android:text="Current Position"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/curPosXText"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/curPosYText"
+						android:layout_width="147dp"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="1"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1"
+					android:paddingTop="10dp" >
+					<TextView
+						android:id="@+id/textView9"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="1"
+						android:text="Current Heading"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/headingText"
+						android:layout_width="wrap_content"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="2"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/textView2"
+						android:layout_width="146dp"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="1"
+						android:text="World GyroZ"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+					<TextView
+						android:id="@+id/wrdGyrText1"
+						android:layout_width="146dp"
+						android:layout_column="0"
+						android:layout_gravity="right|top"
+						android:layout_row="2"
+						android:text="Medium Text"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
+				<GridLayout
+					android:layout_width="match_parent"
+					android:layout_height="wrap_content"
+					android:columnCount="1" >
+					<Button
+						android:id="@+id/rstHdBttn"
+						android:layout_width="108dp"
+						android:layout_height="wrap_content"
+						android:layout_column="0"
+						android:layout_gravity="left|top"
+						android:layout_row="0"
+						android:text="Heading Reset" />
+					<Button
+						android:id="@+id/rstDstBttn"
+						android:layout_width="96dp"
+						android:layout_column="0"
+						android:layout_gravity="center_horizontal|top"
+						android:layout_row="0"
+						android:text="Distance Reset" />
+					<TextView
+						android:id="@+id/stableText"
+						android:layout_width="84dp"
+						android:layout_column="0"
+						android:layout_gravity="right|center_vertical"
+						android:layout_row="0"
+						android:text="ifStable"
+						android:textAppearance="?android:attr/textAppearanceMedium" />
+				</GridLayout>
 			</LinearLayout>
 			<LinearLayout
-				android:id="@+id/state"
+				android:id="@+id/map"
 				android:orientation="vertical"
 				android:layout_width="fill_parent"
 				android:layout_height="fill_parent">
-				<TextView
-					android:id="@+id/state_text"
-					android:layout_width="fill_parent"
-					android:layout_height="wrap_content"
-					android:text="State information" />
+				<fragment
+					xmlns:android="http://schemas.android.com/apk/res/android"
+					android:id="@+id/map_fragment"
+					android:layout_width="match_parent"
+					android:layout_height="match_parent"
+					android:name="com.google.android.gms.maps.MapFragment" />
 			</LinearLayout>
 			<ScrollView
 				android:id="@+id/debug_scroll"
diff --git a/src/edu/ucla/iBeaconNav/CMD.java b/src/edu/ucla/iBeaconNav/CMD.java
index 2ea4201..172baa9 100644
--- a/src/edu/ucla/iBeaconNav/CMD.java
+++ b/src/edu/ucla/iBeaconNav/CMD.java
@@ -1,16 +1,34 @@
 package edu.ucla.iBeaconNav;
 
 public class CMD {
-	/* Main -> Task Messsages */
+	/* Task Commands */
 	static enum Command {
 		REGISTER,
 		CONNECT,
 		DISCONNECT,
-	};
+		RSTHEAD,
+		RSTDST,
+	}
 
-	/* Task -> Main messages */
+	/* Responses Commands */
 	static enum Response {
-		POSITION,
+		REGISTER_TASK,
+		REGISTER_SENSORS,
 		NOTIFY,
-	};
+		SHOWDATA,
+	}
+
+	/* Show Data Commands */
+	static enum Data {
+		ACC,
+		MAG,
+		GYR,
+		ORIENT,
+		WRDACC,
+		WRDGYR,
+		STPCNT,
+		POSITION,
+		HEADING,
+		STABLE,
+	}
 }
diff --git a/src/edu/ucla/iBeaconNav/Main.java b/src/edu/ucla/iBeaconNav/Main.java
index b74f9c8..3e421c9 100644
--- a/src/edu/ucla/iBeaconNav/Main.java
+++ b/src/edu/ucla/iBeaconNav/Main.java
@@ -39,6 +39,7 @@ public class Main extends Activity
 	private Handler      handler;
 	private Messenger    messenger;
 	private Task         task;
+	private Sensors      sensors;
 	private Toast        toast;
 
 	/* Widgets */
@@ -48,6 +49,8 @@ public class Main extends Activity
 	private LinearLayout state;
 	private TextView     debug;
 	private ScrollView   scroll;
+	private Button       rstHdBttn;
+	private Button       rstDstBttn;
 
 	/* Private helper methods */
 	private void notice(String text)
@@ -61,9 +64,15 @@ public class Main extends Activity
 	/* Private handler methods */
 	private void onRegister(Task task)
 	{
-		Util.debug("Main: onRegister");
+		Util.debug("Main: onRegister_task");
 		this.task    = task;
 	}
+	
+	private void onRegister(Sensors sensors)
+	{
+		Util.debug("Main: onRegister_sensors");
+		this.sensors = sensors;
+	}
 
 	private void onPosition()
 	{
@@ -78,6 +87,75 @@ public class Main extends Activity
 		this.toast.show();
 	}
 
+	private void onShowData(float[] data){
+		TextView textView1 = null;
+		TextView textView2 = null;
+		TextView textView3 = null;
+		int displayNum = 3;
+		int dataId = (int)(data[0]+0.5);
+		CMD.Data dataType = CMD.Data.values()[dataId];
+		switch (dataType){
+		case ACC:
+			textView1 = (TextView)findViewById(R.id.accText1);
+			textView2 = (TextView)findViewById(R.id.accText2);
+			textView3 = (TextView)findViewById(R.id.accText3);
+			break;
+		case MAG:
+			return;
+			/*textView1 = (TextView)findViewById(R.id.magText1);
+			textView2 = (TextView)findViewById(R.id.magText2);
+			textView3 = (TextView)findViewById(R.id.magText3);
+			break;*/
+		case GYR:
+			textView1 = (TextView)findViewById(R.id.gyrText1);
+			textView2 = (TextView)findViewById(R.id.gyrText2);
+			textView3 = (TextView)findViewById(R.id.gyrText3);
+			break;
+		case ORIENT:
+			textView1 = (TextView)findViewById(R.id.orientText1);
+			textView2 = (TextView)findViewById(R.id.orientText2);
+			textView3 = (TextView)findViewById(R.id.orientText3);
+			break;
+		case WRDACC:
+			textView1 = (TextView)findViewById(R.id.wrdAccText1);
+			textView2 = (TextView)findViewById(R.id.wrdAccText2);
+			textView3 = (TextView)findViewById(R.id.wrdAccText3);
+			break;
+		case STPCNT:
+			textView1 = (TextView)findViewById(R.id.stepCntText);
+			textView2 = (TextView)findViewById(R.id.calGrvText);
+			displayNum = 2;
+			break;
+		case POSITION:
+			textView1 = (TextView)findViewById(R.id.curPosXText);
+			textView2 = (TextView)findViewById(R.id.curPosYText);
+			displayNum = 2;
+			break;
+		case HEADING:
+			textView1 = (TextView)findViewById(R.id.headingText);
+			displayNum = 1;
+			break;
+		case WRDGYR:
+			textView1 = (TextView)findViewById(R.id.wrdGyrText1);
+			displayNum = 1;
+			break;
+		case STABLE:
+			textView1 = (TextView)findViewById(R.id.stableText);
+			displayNum = 1;
+			break;
+		default:
+			Util.debug("Main: Nothing Matches");
+		}
+		
+		textView1.setText(Float.toString(data[1]));
+		if (displayNum >1){
+			textView2.setText(Float.toString(data[2]));
+		}
+		if (displayNum >2){
+			textView3.setText(Float.toString(data[3]));
+		}
+	}
+	
 	/* Private service methods */
 	private void register()
 	{
@@ -85,6 +163,9 @@ public class Main extends Activity
 		startService(new Intent(this, Task.class)
 				.putExtra("Command",   CMD.Command.REGISTER)
 				.putExtra("Messenger", this.messenger));
+		startService(new Intent(this, Sensors.class)
+				.putExtra("Command",   CMD.Command.REGISTER)
+				.putExtra("Messenger", this.messenger));
 	}
 
 	private void connect()
@@ -92,6 +173,8 @@ public class Main extends Activity
 		Util.debug("Main: connect");
 		startService(new Intent(this, Task.class)
 				.putExtra("Command", CMD.Command.CONNECT));
+		startService(new Intent(this, Sensors.class)
+				.putExtra("Command", CMD.Command.CONNECT));
 	}
 
 	private void disconnect()
@@ -99,6 +182,8 @@ public class Main extends Activity
 		Util.debug("Main: disconnect");
 		startService(new Intent(this, Task.class)
 				.putExtra("Command", CMD.Command.DISCONNECT));
+		startService(new Intent(this, Sensors.class)
+				.putExtra("Command", CMD.Command.DISCONNECT));
 	}
 
 	private void quit()
@@ -136,6 +221,27 @@ public class Main extends Activity
 			this.state     = (LinearLayout) findViewById(R.id.state);
 			this.debug     = (TextView)     findViewById(R.id.debug);
 			this.scroll    = (ScrollView)   findViewById(R.id.debug_scroll);
+			this.rstHdBttn = (Button)       findViewById(R.id.rstHdBttn);
+			this.rstDstBttn= (Button)       findViewById(R.id.rstDstBttn);
+			
+			// TODO - remove these
+			rstHdBttn.setOnClickListener(new View.OnClickListener() {
+				@Override
+				public void onClick(View v) {
+					// TODO Auto-generated method stub
+					startService(new Intent(Main.this, Sensors.class)
+					.putExtra("Command",   CMD.Command.RSTHEAD));
+				}
+			});
+			
+			rstDstBttn.setOnClickListener(new View.OnClickListener() {
+				@Override
+				public void onClick(View v) {
+					// TODO Auto-generated method stub
+					startService(new Intent(Main.this, Sensors.class)
+					.putExtra("Command",   CMD.Command.RSTDST));
+				}
+			});
 
 			// Get a handle to the Map Fragment
 			//GoogleMap map = ((MapFragment)getFragmentManager()
@@ -154,14 +260,14 @@ public class Main extends Activity
 			// Add window tabs
 			this.window.setup();
 
-			this.window.addTab(this.window
-					.newTabSpec("map")
-					.setIndicator("Map")
-					.setContent(R.id.map));
 			this.window.addTab(this.window
 					.newTabSpec("state")
 					.setIndicator("State")
 					.setContent(R.id.state));
+			this.window.addTab(this.window
+					.newTabSpec("map")
+					.setIndicator("Map")
+					.setContent(R.id.map));
 			this.window.addTab(this.window
 					.newTabSpec("debug")
 					.setIndicator("Debug")
@@ -257,16 +363,23 @@ public class Main extends Activity
 	/* Handler class */
 	class MainHandler extends Handler
 	{
+		@Override
 		public void handleMessage(android.os.Message msg)
 		{
 			CMD.Response resp = CMD.Response.values()[msg.what];
 			switch (resp) {
-				case POSITION:
-					Main.this.onPosition();
+				case REGISTER_TASK:
+					Main.this.onRegister((Task)msg.obj);
+					break;
+				case REGISTER_SENSORS:
+					Main.this.onRegister((Sensors)msg.obj);
 					break;
 				case NOTIFY:
 					Main.this.onNotify((String)msg.obj);
 					break;
+				case SHOWDATA:
+					Main.this.onShowData((float[])msg.obj);
+					break;
 				default:
 					Util.debug("Main: unknown message - " + resp);
 					break;
diff --git a/src/edu/ucla/iBeaconNav/Matrix.java b/src/edu/ucla/iBeaconNav/Matrix.java
new file mode 100644
index 0000000..bd0378e
--- /dev/null
+++ b/src/edu/ucla/iBeaconNav/Matrix.java
@@ -0,0 +1,78 @@
+package edu.ucla.iBeaconNav;
+
+class Matrix {
+	public float[] mValue = new float[9];
+	
+	public Matrix(){
+		for (int i=0; i<9; i++){
+			mValue[i] = 0;
+		}
+	}
+	
+	public Matrix(float[] m){
+		for (int i=0; i<9; i++){
+			mValue[i] = m[i];
+		}
+	}
+	
+	public Matrix rotateX(float thetaX){
+		float[] rtX = {1,                       0,                       0,
+					   0, (float)Math.cos(thetaX),(float)-Math.sin(thetaX),
+					   0, (float)Math.sin(thetaX),(float) Math.cos(thetaX)}; 
+		return new Matrix(rtX);
+	}
+	
+	public Matrix rotateY(float thetaY){
+		float[] rtY = {(float) Math.cos(thetaY), 0, (float)Math.sin(thetaY),
+				                              0, 1,                       0,
+				       (float)-Math.sin(thetaY), 0, (float)Math.cos(thetaY)};
+		return new Matrix(rtY);
+	}
+	
+	public Matrix rotateZ(float thetaZ){
+		float[] rtZ = {(float)Math.cos(thetaZ),(float)-Math.sin(thetaZ), 0,
+				       (float)Math.sin(thetaZ),(float) Math.cos(thetaZ), 0,
+				                             0,                       0, 1};
+		return new Matrix(rtZ);
+	}
+	
+	public Matrix multiple(Matrix m){
+		float[] result = new float[9];
+		for (int k1=0; k1<9; k1++){
+			int i = (int) (k1/3);
+			int j = k1%3;
+			result[k1] = 0;
+			for (int k2=0; k2<3; k2++){
+				result[k1]+=mValue[i*3+k2]*m.mValue[k2*3+j];
+			}
+			
+		}
+		return new Matrix(result);
+	}
+	
+	public float[] multipleV(float[] v){
+		float[] result = new float[3];
+		for(int i=0; i<3; i++){
+			result[i] = 0;
+			for(int j=0; j<3; j++){
+				result[i]+=mValue[3*i+j]*v[j];
+			}
+		}
+		return result;
+	}
+	
+	public Matrix transpose(){
+		float[] result = new float[9];
+		result[0] = mValue[0];
+		result[1] = mValue[3];
+		result[2] = mValue[6];
+		result[4] = mValue[4];
+		result[3] = mValue[1];
+		result[5] = mValue[7];
+		result[6] = mValue[2];
+		result[7] = mValue[5];
+		result[8] = mValue[8];
+		return new Matrix(result);
+	}
+	
+}
diff --git a/src/edu/ucla/iBeaconNav/Sensors.java b/src/edu/ucla/iBeaconNav/Sensors.java
new file mode 100644
index 0000000..e3d788a
--- /dev/null
+++ b/src/edu/ucla/iBeaconNav/Sensors.java
@@ -0,0 +1,548 @@
+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.Messenger;
+import android.os.SystemClock;
+import android.widget.EditText;
+import edu.ucla.iBeaconNav.R;
+
+public class Sensors extends Service implements SensorEventListener
+{
+	/* Private data */
+	private SensorManager sensorManager;
+	private Sensor        accSensor;
+	private Sensor        grvSensor;
+	private Sensor        magSensor;
+	private Sensor        gyrSensor;
+
+	private Messenger     messenger;
+	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;
+
+
+
+	/* Private methods */
+	private void tellMain(CMD.Response cmd, Object value)
+	{
+		try {
+			android.os.Message msg = android.os.Message.obtain();
+			msg.what = cmd.ordinal();
+			msg.obj  = value;
+			this.messenger.send(msg);
+		} catch (Exception e) {
+			Util.debug("Sensors: error sending message", e);
+		}
+	}
+
+	private void notify(String text, int icon)
+	{
+		// Notify Main
+		this.tellMain(CMD.Response.NOTIFY, text);
+
+		// Notification bar
+		//Notification  note   = new Notification(icon, null, 0);
+		//Intent        intent = new Intent(this, Main.class);
+		//PendingIntent pend   = PendingIntent.getActivity(this, 0, intent, 0);
+		//note.setLatestEventInfo(this, "iBeaconNav!", text, pend);
+		//PendingIntent pend = PendingIntent.getActivity(this, 0, intent, 0);
+
+		Notification  note = new Notification.Builder(this)
+			.setContentTitle("iBeaconNav!")
+			.setContentText("iBeaconNav!")
+			.setSmallIcon(icon)
+			.build();
+
+		this.startForeground(1, note);
+	}
+
+	private void handle(CMD.Command cmd, Messenger mgr)
+	{
+		// Validate messenger
+		if (cmd != CMD.Command.REGISTER && mgr != null && mgr != this.messenger)
+			Util.debug("Sensors: handle - invalid messenger");
+
+		// Handle the command
+		switch (cmd) {
+			// Setup communication with Main
+			case REGISTER:
+				Util.debug("Sensors: handle - register");
+				this.messenger = mgr;
+				break;
+
+			// Create client thread
+			case CONNECT:
+				Util.debug("Sensors: handle - connect");
+				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);
+				break;
+
+			// Stop client thread
+			case DISCONNECT:
+				Util.debug("Sensors: handle - register");
+				// TODO
+				break;
+
+			// Reset heading
+			case RSTHEAD:
+				Util.debug("Sensors: handle - reset heading");
+				currentHeading = 0;
+				displayData(CMD.Data.HEADING);
+				break;
+
+			// Reset distance
+			case RSTDST:
+				Util.debug("Sensors: handle - reset distance");
+				currentPosX = 0;
+				currentPosY = 0;
+				displayData(CMD.Data.POSITION);
+				break;
+		}
+	}
+
+	/* Public methods */
+	public boolean isRunning()
+	{
+		return true; // TODO
+	}
+
+	/* Service Methods */
+	@Override
+	public void onCreate()
+	{
+		Util.debug("Sensors: onCreate");
+		super.onCreate();
+		sensorManager =  (SensorManager) getSystemService(Context.SENSOR_SERVICE);
+		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);
+	}
+
+	@Override
+	public void onDestroy()
+	{
+		Util.debug("Sensors: onDestroy");
+		//this.handle(CMD.Response.DISCONNECT, null);
+	}
+
+	@Override
+	public int onStartCommand(Intent intent, int flags, int startId)
+	{
+		Util.debug("Sensors: onStartCommand");
+		int         rval = super.onStartCommand(intent, flags, startId);
+		CMD.Command cmd  = (CMD.Command)intent.getExtras().get("Command");
+		Messenger   mgr  = (Messenger)intent.getExtras().get("Messenger");
+		this.handle(cmd, mgr);
+		return rval;
+	}
+
+	@Override
+	public IBinder onBind(Intent intent)
+	{
+		Util.debug("Sensors: onBind");
+		return messenger.getBinder();
+	}
+	class IncomingHandler extends Handler{
+		@Override
+		public void handleMessage(Message msg) {
+			//Util.debug("Sensors: MSG HANDLERRRRRRRRR");
+			super.handleMessage(msg);
+		}
+	}
+
+	@Override
+	public void onAccuracyChanged(Sensor sensor, int accuracy) {
+		// TODO Auto-generated method stub
+		Util.debug("Sensors: onAccuracyChanged");
+
+	}
+
+	@Override
+	public void onSensorChanged(SensorEvent event) {
+		// TODO Auto-generated method stub
+		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);
+			}
+			break;
+
+
+		}
+		processSensorInfo();
+	}
+
+	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 GYR:
+			data[1] = gyrValues[0];
+			data[2] = gyrValues[1];
+			data[3] = gyrValues[2];
+			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.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);
+		}
+	}
+
+	public 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]);
+	}
+
+	public void printVector(String s, float[] v){
+		Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
+	}
+}
-- 
2.43.2