Attempt to make good

code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java

@@ -152,7 +152,7 @@ public class PersonalMotionPreviewElement extends View {
         );
 
         timePassed = (System.nanoTime() - startingTime) / 1E9D;
-        this.exerciseProgress = timePassed*.4 % 1.0d;
+        this.exerciseProgress = Math.min(1, this.motionProcessor.getAverageError() / 10);
 
         this.invalidate(); // Causes a redraw.
     }

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

@@ -2,11 +2,11 @@ package com.example.fitbot.util.processing;
 
 import android.util.Log;
 
-import com.aldebaran.qi.sdk.QiContext;
-import com.example.fitbot.util.FitnessCycle;
 import com.example.fitbot.util.path.GesturePath;
-import com.example.fitbot.util.server.IWebServerHandler;
 import com.example.fitbot.util.server.WebServer;
+import com.google.gson.JsonElement;
+import com.google.gson.JsonObject;
+import com.google.gson.JsonParser;
 
 import org.jetbrains.annotations.NotNull;
 import org.joml.Vector3f;
@@ -20,11 +20,16 @@ public class MotionProcessor {
     public static final String DELIMITER = ";";
 
     private final List<MotionData> preprocessedData = new ArrayList<>(); // Preprocessed motion data
-    private final List<Vector3f> relativePath = new ArrayList<>(); // Relative path of the motion data
+
+    private final List<Vector3f> relativeLeftPath = new ArrayList<>(); // Relative path of the left motion data
+    private final List<Vector3f> relativeRightPath = new ArrayList<>(); // Relative path of the motion data
+
     private Vector3f ZERO = new Vector3f(0, 0, 0);
 
-    private float sampleRate = 1.0F; // samples/second
+    private final float sampleRate = 10.0F; // samples/second
-    private IMotionDataConsumer motionDataConsumer = (p1, p2, p3, p4, p5) -> {};
+
+    private IMotionDataConsumer motionDataConsumer = (p1, p2, p3, p4, p5) -> {
+    };
     private GesturePath path;
     private WebServer server;
 
@@ -65,30 +70,51 @@ public class MotionProcessor {
 
     /**
      * Function for parsing arbitrary packet data.
+     *
      * @param data The data to parse.
      */
     public void parsePacket(@NotNull String data) {
 
-        // If the message starts with 'data', it's a data packet.
+        try {
-        if ( data.startsWith("data")) {
+            JsonElement json = JsonParser.parseString(data);
-            Log.i("MotionProcessor", "Received data packet: " + data.split(" ")[1]);
+
-            MotionData parsedData = MotionData.decode(data.split(" ")[1]);
+            if (!json.isJsonObject())
-            if (parsedData != null) {
+                return;
-                addMotionData(parsedData);
+
+            JsonObject object = json.getAsJsonObject();
+
+            // Object must contain device identifier
+            if (!object.has("deviceId"))
+                return;
+
+            String[] required = {
+                    "rotationX", "rotationY", "rotationZ",
+                    "accelerationX", "accelerationY", "accelerationZ",
+                    "type",
+                    "deviceId"
+            };
+
+            // Ensure all properties are present in the received JSON object
+            for (String s : required) {
+                if (!object.has(s))
+                    return;
             }
-            // Otherwise check if it starts with 'calibrate', this is the ZERO point.
+
-        } else if ( data.startsWith("zero")) { // message to calibrate device
+            // Parse the data
-            String[] vectorData = data.split(" ")[1].split(DELIMITER);
+            Vector3f rotation = new Vector3f(object.get("rotationX").getAsFloat(), object.get("rotationY").getAsFloat(), object.get("rotationZ").getAsFloat());
-            ZERO = new Vector3f(
+            Vector3f acceleration = new Vector3f(object.get("accelerationX").getAsFloat(), object.get("accelerationY").getAsFloat(), object.get("accelerationZ").getAsFloat());
-                    Float.parseFloat(vectorData[0]),
+            int deviceId = object.get("deviceId").getAsInt();
-                    Float.parseFloat(vectorData[1]),
+            String type = object.get("type").getAsString();
-                    Float.parseFloat(vectorData[2])
+            MotionData motionData = new MotionData(rotation, acceleration, deviceId);
-            );
+
-            Log.i("MotionProcessor", "Device calibrated at " + ZERO);
+            if (type.equals("calibrate")) {
-        } else if ( data.startsWith("sampleRate")) {
+                ZERO = getRelativeVector(motionData);
-            this.sampleRate = Float.parseFloat(data.split(" ")[1]);
+                return;
-        } else {
+            }
-            Log.i("MotionProcessor", "Received unknown packet: " + data);
+
+            addMotionData(motionData);
+        } catch (Exception e) {
+            // Don't do anything ... just ignore the exception
         }
     }
 
@@ -108,28 +134,35 @@ public class MotionProcessor {
      */
     public void addMotionData(MotionData data) {
         preprocessedData.add(data);
-        Vector3f previous = this.relativePath.isEmpty() ? ZERO : this.relativePath.get(this.relativePath.size() - 1);
+        List<Vector3f> target;
+        if (data.sensorId == 0)
+            target = relativeLeftPath;
+        else target = relativeRightPath;
+        Vector3f previous = target.isEmpty() ? ZERO : target.get(target.size() - 1);
         Vector3f relativeVector = getRelativeVector(data).add(previous);
-        this.relativePath.add(relativeVector);
+        target.add(relativeVector);
-        motionDataConsumer.accept(relativeVector, data, this.relativePath.size(), this.sampleRate, data.sensorId);
+        motionDataConsumer.accept(relativeVector, data, target.size(), this.sampleRate, data.sensorId);
     }
 
     /**
      * Function for updating the relative path.
      *
-     * @param relativePath The new relative path.
+     * @param relativeRightPath The new relative path.
      */
-    public void setRelativePath(List<Vector3f> relativePath) {
+    public void setRelativeRightPath(List<Vector3f> relativeLeftPath, List<Vector3f> relativeRightPath) {
-        this.relativePath.clear();
+        this.relativeRightPath.clear();
-        this.relativePath.addAll(relativePath);
+        this.relativeLeftPath.clear();
+        this.relativeLeftPath.addAll(relativeLeftPath);
+        this.relativeRightPath.addAll(relativeRightPath);
     }
 
     /**
      * Function for setting the motion data receiver.
+     *
      * @param consumer The consumer to set.
      */
     public void setMotionDataEventHandler(IMotionDataConsumer consumer) {
-        if ( consumer != null)
+        if (consumer != null)
             this.motionDataConsumer = consumer;
     }
 
@@ -164,11 +197,11 @@ public class MotionProcessor {
      */
     public List<Double> getErrors(GesturePath referencePath) {
 
-        // Return the errors of the relative path compared to the reference path.
+        List<Double> errors = new ArrayList<>();
-        return relativePath
+        for (Vector3f vector : relativeRightPath) {
-                .stream()
+            errors.add(referencePath.getError(vector));
-                .map(referencePath::getError)
+        }
-                .collect(Collectors.toList());
+        return errors;
     }
 
     /**
@@ -176,10 +209,10 @@ public class MotionProcessor {
      * 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.
+     * If no path is set, an empty list will be returned.
      */
     public List<Double> getErrors() {
-        if ( path == null)
+        if (path == null)
             return new ArrayList<>();
         return getErrors(path);
     }
@@ -187,12 +220,11 @@ public class MotionProcessor {
     /**
      * 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 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, Vector3f referencePoint)
+    public double getError(GesturePath path, Vector3f referencePoint) {
-    {
         return path.getError(referencePoint);
     }
 
@@ -204,7 +236,7 @@ public class MotionProcessor {
      * @return The error of the motion data compared to the reference path.
      */
     public double getError(Vector3f referencePoint) {
-        if ( path == null)
+        if (path == null)
             return 0;
         return path.getError(referencePoint);
     }
@@ -217,11 +249,11 @@ public class MotionProcessor {
      * @return The average error of the motion data compared to the reference path.
      */
     public double getAverageError(GesturePath referencePath) {
-        return getErrors(referencePath)
+        double error = 0;
-                .stream()
+        for (Double e : getErrors(referencePath)) {
-                .mapToDouble(Double::doubleValue)
+            error += e;
-                .average()
+        }
-                .orElse(0.0D);
+        return error / Math.max(1, relativeRightPath.size());
     }
 
     /**
@@ -231,7 +263,7 @@ public class MotionProcessor {
      * @return The average error of the motion data compared to the reference path.
      */
     public double getAverageError() {
-        if ( path == null)
+        if (path == null)
             return 0;
         return getAverageError(path);
     }
@@ -243,7 +275,7 @@ public class MotionProcessor {
      */
     public void logStatistics(GesturePath referencePath) {
         Log.i("MotionProcessor", "Average path error: " + getAverageError(referencePath));
-        Log.i("MotionProcessor", "Path length: " + relativePath.size());
+        Log.i("MotionProcessor", "Path length: " + relativeRightPath.size());
         Log.i("MotionProcessor", "Sample rate: " + sampleRate);
         Log.i("MotionProcessor", "Calibration point: " + ZERO.toString());
     }