From 446e671ac617200e217d9f70db42e3dafccd6c9e Mon Sep 17 00:00:00 2001
From: 66945 <54561572+66945@users.noreply.github.com>
Date: Sun, 20 Mar 2022 19:49:28 -0600
Subject: [PATCH] Better filtering

---
 .../commands/ShooterCommands/TrackTarget.java | 35 +++++++++++++------
 1 file changed, 25 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
index f489de4..79f2a11 100644
--- a/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+++ b/src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
@@ -5,6 +5,9 @@
 package frc4388.robot.commands.ShooterCommands;
 
 import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.stream.Collector;
+import java.util.stream.Collectors;
 
 import org.opencv.core.Point;
 
@@ -80,17 +83,9 @@ public class TrackTarget extends CommandBase {
       m_visionOdometry.setLEDs(true);
       points = m_visionOdometry.getTargetPoints();
       
+      points = filterPoints(points);
+      
       Point average = VisionOdometry.averagePoint(points);
-      
-      for(Point point : points) {
-        Vector2D difference = new Vector2D(point);
-        difference.subtract(new Vector2D(average));
-
-        if(difference.magnitude() < VisionConstants.POINTS_THRESHOLD)
-        points.remove(point);
-      }
-      
-      average = VisionOdometry.averagePoint(points);
       DesmosServer.putPoint("average", average);
       
       for(int i = 0; i < points.size(); i++) {
@@ -151,6 +146,26 @@ public class TrackTarget extends CommandBase {
     // m_turret.runshooterRotatePID(m_targetAngle);
   }
 
+  public ArrayList<Point> filterPoints(ArrayList<Point> points) {
+    Point average = VisionOdometry.averagePoint(points);
+      
+    HashMap<Point, Double> pointDistances = new HashMap<>();
+    double distanceSum = 0;
+
+    for(Point point : points) {
+      Vector2D difference = new Vector2D(point);
+      difference.subtract(new Vector2D(average));
+
+      double mag = difference.magnitude();
+      distanceSum += mag;
+
+      pointDistances.put(point, mag);
+    }
+
+    final double averageDist = distanceSum / points.size();
+    return (ArrayList<Point>) pointDistances.keySet().stream().filter(p -> pointDistances.get(p) < 2 * averageDist).collect(Collectors.toList());
+  }
+
   public final double distanceRegression(double distance) {
     return (79.6078 * Math.pow(1.01343, distance) - 56.6671);
   }