Updated math behind path error calculation, fixed WebSocket not starting.

code/src/Fitbot/app/src/main/java/com/example/fitbot/MainActivity.java

@@ -1,8 +1,6 @@
 package com.example.fitbot;
 
-import android.content.Intent;
 import android.os.Bundle;
-import android.widget.Button;
 
 import com.aldebaran.qi.sdk.QiContext;
 import com.aldebaran.qi.sdk.QiSDK;
@@ -14,7 +12,6 @@ import com.aldebaran.qi.sdk.object.conversation.Say;
 import com.aldebaran.qi.sdk.object.locale.Language;
 import com.aldebaran.qi.sdk.object.locale.Locale;
 import com.aldebaran.qi.sdk.object.locale.Region;
-import com.example.fitbot.ui.SportMenuActivity;
 
 public class MainActivity extends RobotActivity implements RobotLifecycleCallbacks {
 
@@ -22,12 +19,8 @@ public class MainActivity extends RobotActivity implements RobotLifecycleCallbac
     protected void onCreate(Bundle savedInstanceState) {
         super.onCreate(savedInstanceState);
         // Register the RobotLifecycleCallbacks to this Activity.
-        Button button = findViewById(R.id.menu_switch_btn);
-        button.setOnClickListener(v -> {
-            startActivity(new Intent(MainActivity.this, SportMenuActivity.class));
-        });
-        QiSDK.register(this, this);
 
+        QiSDK.register(this, this);
     }
 
     @Override

code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java

@@ -1,7 +1,6 @@
 package com.example.fitbot;
 
 import android.annotation.SuppressLint;
-import android.content.Intent;
 import android.os.Bundle;
 import android.support.design.widget.NavigationView;
 import android.support.v4.view.GravityCompat;
@@ -9,9 +8,10 @@ import android.support.v4.widget.DrawerLayout;
 import android.support.v7.app.ActionBarDrawerToggle;
 import android.support.v7.app.AppCompatActivity;
 import android.support.v7.widget.Toolbar;
-import android.widget.Button;
 
-import com.example.fitbot.ui.SportMenuActivity;
+import com.example.fitbot.util.processing.GesturePath;
+import com.example.fitbot.util.processing.MotionProcessor;
+import com.example.fitbot.util.processing.Vector3;
 
 public class MainScreen extends AppCompatActivity {
 
@@ -31,6 +31,21 @@ public class MainScreen extends AppCompatActivity {
         navigationView = findViewById(R.id.nav_view);
         toolbar = findViewById(R.id.toolbar);
 
+        MotionProcessor motionProcessor = new MotionProcessor();
+        motionProcessor.startListening();
+
+        motionProcessor.parsePacket("sampleRate 1");
+
+        Vector3[] path = new Vector3[100];
+
+        for ( int i = 0; i < 100; i++ ) {
+            path[i] = new Vector3(0, Math.sin(i * Math.PI / 50), 0);
+            motionProcessor.parsePacket("data 0;" + Math.cos(i * Math.PI / 50) + ";0;0;0;0");
+        }
+
+        motionProcessor.calculatePath(new GesturePath(path));
+        motionProcessor.printData();
+
         /*---Tool Bar---*/
 //      setSupportActionBar(toolbar);
 

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java

@@ -0,0 +1,28 @@
+package com.example.fitbot.util.processing;
+
+public class GesturePath {
+
+    public Vector3[] vectors;
+
+    public GesturePath(Vector3[] vectors)
+    {
+        this.vectors = vectors;
+    }
+
+    public Vector3 getError(Vector3 compare) {
+        Vector3 error = new Vector3(0, 0, 0);
+        double distance, previous = Double.MAX_VALUE;
+
+        // Calculate the minimum distance between the vectors.
+        // This is used to determine the error.
+        for (int i = 0; i < vectors.length; i++) {
+            distance = vectors[i].distance(compare);
+            if ( distance < previous ) {
+                error = vectors[i];
+                previous = distance;
+            }
+        }
+        return error;
+    }
+
+}

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java

@@ -61,4 +61,21 @@ public class MotionData {
         );
     }
 
+    /**
+     * Class for representing parsed motion data.
+     * This data is parsed in the `MotionProcessor` class.
+     */
+    public static class ParsedVector {
+        public Vector3 relativePosition;
+        public Vector3 relativeError;
+        public ParsedVector(Vector3 relativePosition, Vector3 relativeError) {
+            this.relativePosition = relativePosition;
+            this.relativeError = relativeError;
+        }
+
+        public String toString() {
+            return "position[" + relativePosition.toString() + "] error[" + relativeError.toString() + "]";
+        }
+    }
+
 }

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java

@@ -16,9 +16,10 @@ public class MotionProcessor {
     public static final String DELIMITER = ";";
 
     private final List<MotionData> motionData = new ArrayList<>();
+    private final List<MotionData.ParsedVector> parsedData = new ArrayList<>();
     private final Vector3[] relativePath = new Vector3[MAX_PATH_LENGTH];
     private Vector3 ZERO = new Vector3(0, 0, 0);
-    private int sampleRate = 1;
+    private double sampleRate = 1.0D; // samples/second
 
     public MotionProcessor() {}
 
@@ -31,7 +32,9 @@ public class MotionProcessor {
      */
     public void startListening() {
         // Create socket server
-        WebSocket socket = WebSocket.createServer(80);
+        WebSocket socket = WebSocket.createServer();
+
+        Log.i("MotionProcessor", "Listening for incoming connections.");
 
         // Check if the socket
         if (socket != null) {
@@ -50,9 +53,10 @@ public class MotionProcessor {
      * Function for parsing arbitrary packet data.
      * @param data The data to parse.
      */
-    private void parsePacket(@NotNull String data) {
+    public void parsePacket(@NotNull String data) {
         // If the message starts with 'data', it's a data packet.
         if ( data.startsWith("data")) {
+            Log.i("MotionProcessor", "Received data packet: " + data.split(" ")[1]);
             MotionData parsedData = MotionData.decode(data.split(" ")[1]);
             if (parsedData != null) {
                 this.motionData.add(parsedData);
@@ -67,7 +71,7 @@ public class MotionProcessor {
             );
             Log.i("MotionProcessor", "Device calibrated at " + ZERO.toString());
         } else if ( data.startsWith("sampleRate")) {
-            this.sampleRate = Integer.parseInt(data.split(" ")[1]);
+            this.sampleRate = Double.parseDouble(data.split(" ")[1]);
         }
     }
 
@@ -75,7 +79,7 @@ public class MotionProcessor {
      * Function for calculating the path of the device.
      * This only works when the ZERO point has been provided.
      */
-    private void calculatePath() {
+    public void calculatePath(GesturePath path) {
 
         int offset = Math.max(0, motionData.size() - MAX_PATH_LENGTH);
         Vector3 previous = ZERO.copy();
@@ -83,13 +87,22 @@ public class MotionProcessor {
         for (int i = offset; i < motionData.size(); i++) {
             MotionData data = motionData.get(i);
 
-            // Get relative vector from the ZERO point
             Vector3 relativePosition = data.acceleration
-                    .rotate(data.rotation.negate()) // Rotate acceleration vector with it's angle to get the acceleration relative to the vector of gravity
+                    .rotate(data.rotation.negate()) // Rotate acceleration vector with it's negated angle to get the acceleration relative to the vector of gravity
-                    .map((vector) ->  vector.) // Convert acceleration to relative spatial change
+                    .multiply(sampleRate * sampleRate / 6.0D) // Apply double integration to get the relative position
+                    .add(previous); // Add the previous position to get the new position
 
-            relativePath[i - offset]
+            Vector3 relativeError = path.getError(relativePosition);
+
+            previous = relativePosition;
+
+            // Get relative vector from the ZERO point
+            parsedData.add(new MotionData.ParsedVector(relativePosition, relativeError));
+        }
+    }
+    public void printData() {
+        for (MotionData.ParsedVector vector : parsedData) {
+            Log.i("MotionProcessor", vector.toString());
         }
     }
-
 }

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java

@@ -26,6 +26,26 @@ public class Vector3 {
         return new Vector3(this.x, this.y, this.z);
     }
 
+    /**
+     * Get the magnitude of the vector.
+     *
+     * @return The magnitude of the vector.
+     */
+    public double magnitude() {
+        return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
+    }
+
+    /**
+     * Normalize the vector.
+     *
+     * @return The normalized vector.
+     */
+    public Vector3 normalize() {
+        double mag = this.magnitude();
+        if (mag == 0) throw new IllegalArgumentException("Cannot normalize the zero vector.");
+        return new Vector3(this.x / mag, this.y / mag, this.z / mag);
+    }
+
     /**
      * Subtract the vector from another vector.
      *
@@ -155,6 +175,16 @@ public class Vector3 {
         );
     }
 
+    /**
+     * Get the distance between this vector and another vector.
+     *
+     * @param compare The other vector.
+     * @return The distance between the two vectors.
+     */
+    public double distance(Vector3 compare) {
+        return Math.sqrt(Math.pow(compare.x - x, 2) + Math.pow(compare.y - y, 2) + Math.pow(compare.z - z, 2));
+    }
+
     public Vector3 map(VectorMapFunction function) {
         return function.apply(this);
     }
@@ -162,4 +192,10 @@ public class Vector3 {
     public interface VectorMapFunction {
         Vector3 apply(Vector3 vector);
     }
+
+    @Override
+    public String toString()
+    {
+        return "Vector3(" + this.x + ", " + this.y + ", " + this.z + ")";
+    }
 }

code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java

@@ -24,14 +24,15 @@ public class WebSocket {
 
     /**
      * Function for creating a new WebSocket server given the provided port.
-     * @param port The port to configure the server to.
      * @return A WebSocket connection, or null if something went wrong.
      */
-    public static @Nullable WebSocket createServer(int port) {
+    public static @Nullable WebSocket createServer() {
         try {
-            WebSocket socket = new WebSocket();
+            WebSocket webSocket = new WebSocket();
-            socket.serverSocket = new ServerSocket(port);
+            webSocket.serverSocket = new ServerSocket();
-            return socket;
+            webSocket.serverSocket.bind(webSocket.serverSocket.getLocalSocketAddress());
+            Log.i("WebSocket -- Creating new WebSocket server", "Server created: " + webSocket.serverSocket.getLocalSocketAddress() + ", " + webSocket.serverSocket.getLocalPort());
+            return webSocket;
         } catch (IOException error)
         {
             String cause = error.getMessage() == null ? "Unknown reason" : error.getMessage();