Updated motion processing and WebSocket code.

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

@@ -5,38 +5,13 @@ import java.util.List;
 
 public class GesturePath {
 
-    public Vector3[] vectors;
+    // The vectors that make up the path.
+    private final Vector3[] vectors;
 
     public GesturePath(Vector3[] vectors) {
         this.vectors = vectors;
     }
 
-    /**
-     * Get the error between a line segment and the reference point.
-     *
-     * @param linePointA     The first point of the line.
-     * @param linePointB     The second point of the line.
-     * @param referencePoint The reference point to calculate the error of.
-     * @return The error offset between the line and the reference point.
-     */
-    public static double getError(Vector3 linePointA, Vector3 linePointB, Vector3 referencePoint) {
-        double lineDistance = linePointA.distanceSq(linePointB);
-        if (lineDistance == 0)
-            return referencePoint.distanceSq(linePointA);
-
-        double t = ((referencePoint.x - linePointA.x) * (linePointB.x - linePointA.x) +
-                    (referencePoint.y - linePointA.y) * (linePointB.y - linePointA.y) +
-                    (referencePoint.z - linePointA.z) * (linePointB.z - linePointA.z)) / lineDistance;
-
-        t = Math.max(0, Math.min(1, t));
-
-        return referencePoint.distanceSq(new Vector3(
-                linePointA.x + t * (linePointB.x - linePointA.x),
-                linePointA.y + t * (linePointB.y - linePointA.y),
-                linePointA.z + t * (linePointB.z - linePointA.z))
-        );
-    }
-
     /**
      * Get the error between an arbitrary path segment and the reference point.
      *
@@ -79,7 +54,7 @@ public class GesturePath {
                         vectors[closestVectorIdx - 1] :
                         vectors[closestVectorIdx + 1];
 
-        return getError(vectors[closestVectorIdx], pointB, referencePoint);
+        return referencePoint.distanceToLine(vectors[closestVectorIdx], pointB);
     }
 
     // Builder class for the GesturePath object.

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

@@ -9,16 +9,20 @@ import org.jetbrains.annotations.NotNull;
 
 import java.util.ArrayList;
 import java.util.List;
+import java.util.function.Consumer;
 import java.util.stream.Collectors;
 
 public class MotionProcessor {
 
     public static final String DELIMITER = ";";
 
-    private final List<MotionData> motionData = new ArrayList<>();
+    private final List<MotionData> preprocessedData = new ArrayList<>(); // Preprocessed motion data
     private final List<Vector3> relativePath = new ArrayList<>(); // Relative path of the motion data
     private Vector3 ZERO = new Vector3(0, 0, 0);
     private double sampleRate = 1.0D; // samples/second
+    private Consumer<Vector3> motionDataConsumer = (data) -> {};
+    private GesturePath path;
+    private WebSocket socket;
 
     public MotionProcessor() {}
 
@@ -31,7 +35,7 @@ public class MotionProcessor {
      */
     public void startListening() {
         // Create socket server
-        WebSocket socket = WebSocket.createServer();
+        this.socket = WebSocket.createServer();
 
         Log.i("MotionProcessor", "Listening for incoming connections.");
 
@@ -48,6 +52,17 @@ public class MotionProcessor {
         }
     }
 
+    /**
+     * Function for stopping the listening process
+     * of the motion sensor. This function will stop
+     * the WebSocket server.
+     */
+    public void stopListening() {
+        if (socket != null) {
+            socket.stop();
+        }
+    }
+
     /**
      * Function for parsing arbitrary packet data.
      * @param data The data to parse.
@@ -58,10 +73,10 @@ public class MotionProcessor {
             Log.i("MotionProcessor", "Received data packet: " + data.split(" ")[1]);
             MotionData parsedData = MotionData.decode(data.split(" ")[1]);
             if (parsedData != null) {
-                this.motionData.add(parsedData);
+                addMotionData(parsedData);
             }
             // Otherwise check if it starts with 'calibrate', this is the ZERO point.
-        } else if ( data.startsWith("calibrate")) { // message to calibrate device
+        } else if ( data.startsWith("zero")) { // message to calibrate device
             String[] vectorData = data.split(" ")[1].split(DELIMITER);
             ZERO = new Vector3(
                     Float.parseFloat(vectorData[0]),
@@ -74,13 +89,26 @@ public class MotionProcessor {
         }
     }
 
+    /**
+     * Function for setting the gesture path of the processor.
+     *
+     * @param path The path to set.
+     */
+    public void setGesturePath(GesturePath path) {
+        this.path = path;
+    }
+
     /**
      * Function for adding motion data to the processor.
      *
      * @param data The motion data to add.
      */
     public void addMotionData(MotionData data) {
-        this.motionData.add(data);
+        preprocessedData.add(data);
+        Vector3 previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1);
+        Vector3 relativeVector = getRelativeVector(data).add(previous);
+        this.relativePath.add(relativeVector);
+        motionDataConsumer.accept(relativeVector);
     }
 
     /**
@@ -94,25 +122,12 @@ public class MotionProcessor {
     }
 
     /**
-     * Function for calculating the 'absolute' path of the motion data.
+     * Function for setting the motion data receiver.
-     * This converts the relative acceleration and rotation vectors and converts
+     * @param consumer The consumer to set.
-     * them to a position that is relative to the ZERO point.
      */
-    public void calculateRelativePath() {
+    public void setMotionDataEventHandler(Consumer<Vector3> consumer) {
-
+        if ( consumer != null)
-        relativePath.clear(); // Clear previous path
+            this.motionDataConsumer = consumer;
-
-        Vector3 previous = ZERO.copy();
-
-        for (int i = 0; i < motionData.size(); i++) {
-            MotionData data = motionData.get(i);
-
-            Vector3 relativeVector = getRelativeVector(data)
-                    .add(previous);
-
-            relativePath.add(relativeVector);
-            previous = relativeVector;
-        }
     }
 
     /**
@@ -144,10 +159,6 @@ public class MotionProcessor {
      */
     public List<Double> getErrors(GesturePath referencePath) {
 
-        // If the relative path is empty, calculate it.
-        if ( relativePath.isEmpty())
-            calculateRelativePath();
-
         // Return the errors of the relative path compared to the reference path.
         return relativePath
                 .stream()
@@ -155,6 +166,44 @@ public class MotionProcessor {
                 .collect(Collectors.toList());
     }
 
+    /**
+     * Function for getting the error offsets of the motion data compared to the
+     * reference path.
+     *
+     * @return A list of error offsets of the motion data compared to the reference path.
+     *        If no path is set, an empty list will be returned.
+     */
+    public List<Double> getErrors() {
+        if ( path == null)
+            return new ArrayList<>();
+        return getErrors(path);
+    }
+
+    /**
+     * Function for getting the error of the motion data compared to the reference path.
+     *
+     * @param path The path to compare the motion data to.
+     * @param referencePoint The reference point to compare the motion data to.
+     * @return The error of the motion data compared to the reference path.
+     */
+    public double getError(GesturePath path, Vector3 referencePoint)
+    {
+        return path.getError(referencePoint);
+    }
+
+    /**
+     * Function for getting the error of the provided vector and the set path.
+     * If no path is set, the error will be 0.
+     *
+     * @param referencePoint The reference point to compare the path data to.
+     * @return The error of the motion data compared to the reference path.
+     */
+    public double getError(Vector3 referencePoint) {
+        if ( path == null)
+            return 0;
+        return path.getError(referencePoint);
+    }
+
     /**
      * Function for calculating the average error of the motion data
      * compared to the reference path.
@@ -162,7 +211,7 @@ public class MotionProcessor {
      * @param referencePath The reference path to compare the motion data to.
      * @return The average error of the motion data compared to the reference path.
      */
-    public double averageError(GesturePath referencePath) {
+    public double getAverageError(GesturePath referencePath) {
         return getErrors(referencePath)
                 .stream()
                 .mapToDouble(Double::doubleValue)
@@ -170,14 +219,28 @@ public class MotionProcessor {
                 .orElse(0.0D);
     }
 
+    /**
+     * Function for calculating the average error of the motion data
+     * compared to the reference path.
+     *
+     * @return The average error of the motion data compared to the reference path.
+     */
+    public double getAverageError() {
+        if ( path == null)
+            return 0;
+        return getAverageError(path);
+    }
+
+    /**
+     * Function for logging the statistics of the motion data.
+     *
+     * @param referencePath The reference path to compare the motion data to.
+     */
     public void logStatistics(GesturePath referencePath) {
-        Log.i("MotionProcessor", "Average path error: " + averageError(referencePath));
+        Log.i("MotionProcessor", "Average path error: " + getAverageError(referencePath));
         Log.i("MotionProcessor", "Path length: " + relativePath.size());
         Log.i("MotionProcessor", "Sample rate: " + sampleRate);
         Log.i("MotionProcessor", "Calibration point: " + ZERO.toString());
-        Log.i("MotionProcessor", "Path comparison:\n" +
-                relativePath.stream().map(Vector3::toString).collect(Collectors.joining(", \t")) +
-                "\n" + getErrors(referencePath).stream().map(Object::toString).collect(Collectors.joining(", \t")));
 
     }
 }

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

@@ -1,5 +1,8 @@
 package com.example.fitbot.util.processing;
 
+import java.util.Arrays;
+import java.util.Comparator;
+
 public class Vector3 {
 
     public double x, y, z;
@@ -25,6 +28,15 @@ public class Vector3 {
     public Vector3 copy() {
         return new Vector3(this.x, this.y, this.z);
     }
+    
+    /**
+     * Get the zero vector.
+     * 
+     * @return The zero vector.
+     */
+    public static Vector3 zero() {
+        return new Vector3(0, 0, 0);
+    }
 
     /**
      * Get the magnitude of the vector.
@@ -195,6 +207,41 @@ public class Vector3 {
         return Math.sqrt(distanceSq(compare));
     }
 
+    /**
+     * Calculate the distance to a line defined by two points.
+     *
+     * @param lineStart The starting point of the line.
+     * @param lineEnd The ending point of the line.
+     * @return The distance to the line.
+     */
+    public double distanceToLine(Vector3 lineStart, Vector3 lineEnd) {
+        double lineDistance = lineStart.distanceSq(lineEnd);
+        if (lineDistance == 0)
+            return this.distanceSq(lineStart);
+
+        double t = ((this.x - lineStart.x) * (lineEnd.x - lineStart.x) +
+                (this.y - lineStart.y) * (lineEnd.y - lineStart.y) +
+                (this.z - lineStart.z) * (lineEnd.z - lineStart.z)) / lineDistance;
+
+        t = Math.max(0, Math.min(1, t));
+
+        return this.distanceSq(new Vector3(
+                lineStart.x + t * (lineEnd.x - lineStart.x),
+                lineStart.y + t * (lineEnd.y - lineStart.y),
+                lineStart.z + t * (lineEnd.z - lineStart.z))
+        );
+    }
+
+    /**
+     * Retrieve the closest vector to this one given a list of vectors.
+     *
+     * @param vectors The list of vectors to compare.
+     * @return The closest vector.
+     */
+    public Vector3 closest(Vector3 ... vectors) {
+        return Arrays.stream(vectors).min(Comparator.comparingDouble(this::distanceSq)).orElse(null);
+    }
+
     public Vector3 map(VectorMapFunction function) {
         return function.apply(this);
     }

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

@@ -49,6 +49,19 @@ public class WebSocket {
         this.connectionHandler.listen();
     }
 
+    /**
+     * Method for stopping the WebSocket server.
+     */
+    public void stop() {
+        try {
+            this.serverSocket.close();
+            this.connectionHandler.stop();
+        } catch (IOException error) {
+            String cause = error.getMessage() == null ? "Unknown reason" : error.getMessage();
+            Log.e("WebSocket -- Closing server connection", cause);
+        }
+    }
+
     /**
      * Method for setting the event handler for this WebSocket server.
      * @param handler The handler to use. This handler will parse all events

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

@@ -9,7 +9,10 @@ import java.net.Socket;
 import java.nio.charset.StandardCharsets;
 import java.security.MessageDigest;
 import java.security.NoSuchAlgorithmException;
+import java.util.ArrayList;
 import java.util.Base64;
+import java.util.Collections;
+import java.util.List;
 import java.util.Scanner;
 import java.util.regex.Matcher;
 import java.util.regex.Pattern;
@@ -17,7 +20,9 @@ import java.util.regex.Pattern;
 public class WebSocketConnectionHandler implements Runnable {
 
     private final WebSocket theSocket;
+    private List<Socket> clients = Collections.synchronizedList(new ArrayList<>());
     private Thread thread;
+    private boolean forceClose = false;
 
     /**
      * Constructor for WebSocketConnectionHandler.
@@ -31,13 +36,13 @@ public class WebSocketConnectionHandler implements Runnable {
 
     @Override
     public void run() {
-        // Listen for new connections until the socket
+        // Listen for new connections until the socket closes.
-        // closes.
         while (theSocket.isConnected()) {
             try {
                 // Find a new connection
                 Socket newSocket = this.theSocket.getSocket().accept();
                 this.theSocket.eventHandler.onConnected(newSocket);
+                clients.add(newSocket);
 
                 InputStream streamIn = newSocket.getInputStream();
                 OutputStream streamOut = newSocket.getOutputStream();
@@ -179,4 +184,34 @@ public class WebSocketConnectionHandler implements Runnable {
         this.thread.start();
         Log.i("WebSocketConnectionHandler", "Listening started.");
     }
+
+    /**
+     * Method for stopping the connection handler.
+     */
+    public void stop() {
+        // Close the socket connection if not already closed
+        if (!this.theSocket.getSocket().isClosed()) {
+            try {
+                this.theSocket.getSocket().close();
+            } catch (IOException error) {
+                Log.e("WebSocketConnectionHandler", "Failed to close the socket connection: " + error.getMessage());
+            }
+        }
+
+        // Interrupt the thread
+        this.thread.interrupt();
+
+        // Close all connections
+        this.clients.forEach(client -> {
+            try {
+                client.close();
+            } catch (IOException error) {
+                Log.e("WebSocketConnectionHandler", "Failed to close client: " + error.getMessage());
+            }
+        });
+        this.clients.clear();
+
+
+        Log.i("WebSocketConnectionHandler", "Listening stopped.");
+    }
 }