Create README.md

2023-11-24 17:17:26 -05:00
51 changed files with 400 additions and 2438 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -27,6 +27,3 @@ bin/
 ### Mac OS ###
 .DS_Store
 map.txt
 processing-4.3/
--- a/.idea/inspectionProfiles/Project_Default.xml
+++ b/.idea/inspectionProfiles/Project_Default.xml
@@ -1,8 +0,0 @@
 <component name="InspectionProjectProfileManager">
  <profile version="1.0">
    <option name="myName" value="Project Default" />
    <inspection_tool class="JavadocDeclaration" enabled="true" level="WARNING" enabled_by_default="true">
      <option name="ADDITIONAL_TAGS" value="brief" />
    </inspection_tool>
  </profile>
 </component>
--- a/.idea/libraries/core.xml
+++ b/.idea/libraries/core.xml
@@ -1,9 +0,0 @@
 <component name="libraryTable">
  <library name="core">
    <CLASSES>
      <root url="jar://$PROJECT_DIR$/../../processing-4.3/core/library/core.jar!/" />
    </CLASSES>
    <JAVADOC />
    <SOURCES />
  </library>
 </component>
--- a/.idea/libraries/ejml.xml
+++ b/.idea/libraries/ejml.xml
@@ -1,13 +0,0 @@
 <component name="libraryTable">
  <library name="ejml">
    <CLASSES>
      <root url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" />
    </CLASSES>
    <JAVADOC />
    <SOURCES>
      <root url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" />
    </SOURCES>
    <jarDirectory url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" recursive="false" />
    <jarDirectory url="file://$PROJECT_DIR$/lib/ejml-v0.42-libs" recursive="false" type="SOURCES" />
  </library>
 </component>
--- a/.idea/misc.xml
+++ b/.idea/misc.xml
@@ -1,3 +1,4 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
  <component name="ProjectRootManager" version="2" languageLevel="JDK_20" default="true" project-jdk-name="openjdk-20" project-jdk-type="JavaSDK">
    <output url="file://$PROJECT_DIR$/out" />
--- a/.idea/uiDesigner.xml
+++ b/.idea/uiDesigner.xml
@@ -1,124 +0,0 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <project version="4">
  <component name="Palette2">
    <group name="Swing">
      <item class="com.intellij.uiDesigner.HSpacer" tooltip-text="Horizontal Spacer" icon="/com/intellij/uiDesigner/icons/hspacer.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="1" hsize-policy="6" anchor="0" fill="1" />
      </item>
      <item class="com.intellij.uiDesigner.VSpacer" tooltip-text="Vertical Spacer" icon="/com/intellij/uiDesigner/icons/vspacer.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="6" hsize-policy="1" anchor="0" fill="2" />
      </item>
      <item class="javax.swing.JPanel" icon="/com/intellij/uiDesigner/icons/panel.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3" />
      </item>
      <item class="javax.swing.JScrollPane" icon="/com/intellij/uiDesigner/icons/scrollPane.svg" removable="false" auto-create-binding="false" can-attach-label="true">
        <default-constraints vsize-policy="7" hsize-policy="7" anchor="0" fill="3" />
      </item>
      <item class="javax.swing.JButton" icon="/com/intellij/uiDesigner/icons/button.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="3" anchor="0" fill="1" />
        <initial-values>
          <property name="text" value="Button" />
        </initial-values>
      </item>
      <item class="javax.swing.JRadioButton" icon="/com/intellij/uiDesigner/icons/radioButton.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="3" anchor="8" fill="0" />
        <initial-values>
          <property name="text" value="RadioButton" />
        </initial-values>
      </item>
      <item class="javax.swing.JCheckBox" icon="/com/intellij/uiDesigner/icons/checkBox.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="3" anchor="8" fill="0" />
        <initial-values>
          <property name="text" value="CheckBox" />
        </initial-values>
      </item>
      <item class="javax.swing.JLabel" icon="/com/intellij/uiDesigner/icons/label.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="0" anchor="8" fill="0" />
        <initial-values>
          <property name="text" value="Label" />
        </initial-values>
      </item>
      <item class="javax.swing.JTextField" icon="/com/intellij/uiDesigner/icons/textField.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
          <preferred-size width="150" height="-1" />
        </default-constraints>
      </item>
      <item class="javax.swing.JPasswordField" icon="/com/intellij/uiDesigner/icons/passwordField.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
          <preferred-size width="150" height="-1" />
        </default-constraints>
      </item>
      <item class="javax.swing.JFormattedTextField" icon="/com/intellij/uiDesigner/icons/formattedTextField.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1">
          <preferred-size width="150" height="-1" />
        </default-constraints>
      </item>
      <item class="javax.swing.JTextArea" icon="/com/intellij/uiDesigner/icons/textArea.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
          <preferred-size width="150" height="50" />
        </default-constraints>
      </item>
      <item class="javax.swing.JTextPane" icon="/com/intellij/uiDesigner/icons/textPane.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
          <preferred-size width="150" height="50" />
        </default-constraints>
      </item>
      <item class="javax.swing.JEditorPane" icon="/com/intellij/uiDesigner/icons/editorPane.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
          <preferred-size width="150" height="50" />
        </default-constraints>
      </item>
      <item class="javax.swing.JComboBox" icon="/com/intellij/uiDesigner/icons/comboBox.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="0" hsize-policy="2" anchor="8" fill="1" />
      </item>
      <item class="javax.swing.JTable" icon="/com/intellij/uiDesigner/icons/table.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
          <preferred-size width="150" height="50" />
        </default-constraints>
      </item>
      <item class="javax.swing.JList" icon="/com/intellij/uiDesigner/icons/list.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="6" hsize-policy="2" anchor="0" fill="3">
          <preferred-size width="150" height="50" />
        </default-constraints>
      </item>
      <item class="javax.swing.JTree" icon="/com/intellij/uiDesigner/icons/tree.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3">
          <preferred-size width="150" height="50" />
        </default-constraints>
      </item>
      <item class="javax.swing.JTabbedPane" icon="/com/intellij/uiDesigner/icons/tabbedPane.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3">
          <preferred-size width="200" height="200" />
        </default-constraints>
      </item>
      <item class="javax.swing.JSplitPane" icon="/com/intellij/uiDesigner/icons/splitPane.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="3" hsize-policy="3" anchor="0" fill="3">
          <preferred-size width="200" height="200" />
        </default-constraints>
      </item>
      <item class="javax.swing.JSpinner" icon="/com/intellij/uiDesigner/icons/spinner.svg" removable="false" auto-create-binding="true" can-attach-label="true">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1" />
      </item>
      <item class="javax.swing.JSlider" icon="/com/intellij/uiDesigner/icons/slider.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="8" fill="1" />
      </item>
      <item class="javax.swing.JSeparator" icon="/com/intellij/uiDesigner/icons/separator.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="6" hsize-policy="6" anchor="0" fill="3" />
      </item>
      <item class="javax.swing.JProgressBar" icon="/com/intellij/uiDesigner/icons/progressbar.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="0" fill="1" />
      </item>
      <item class="javax.swing.JToolBar" icon="/com/intellij/uiDesigner/icons/toolbar.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="6" anchor="0" fill="1">
          <preferred-size width="-1" height="20" />
        </default-constraints>
      </item>
      <item class="javax.swing.JToolBar$Separator" icon="/com/intellij/uiDesigner/icons/toolbarSeparator.svg" removable="false" auto-create-binding="false" can-attach-label="false">
        <default-constraints vsize-policy="0" hsize-policy="0" anchor="0" fill="1" />
      </item>
      <item class="javax.swing.JScrollBar" icon="/com/intellij/uiDesigner/icons/scrollbar.svg" removable="false" auto-create-binding="true" can-attach-label="false">
        <default-constraints vsize-policy="6" hsize-policy="0" anchor="0" fill="2" />
      </item>
    </group>
  </component>
 </project>
--- a/README.md
+++ b/README.md
@@ -0,0 +1,2 @@
 # SLAM-Sim
 SImulate SLAM algorithm using java and processing for visualizaiton.
--- a/SLAM-Sim.iml
+++ b/SLAM-Sim.iml
@@ -43,9 +43,5 @@
        <SOURCES />
      </library>
    </orderEntry>
    <orderEntry type="library" exported="" name="ejml" level="project" />
  </component>
 </module>
--- a/lib/ejml-v0.42-libs/ejml-cdense-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-cdense-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-cdense-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-cdense-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-core-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-core-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-core-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-core-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-ddense-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-ddense-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-ddense-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-ddense-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-dsparse-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-dsparse-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-dsparse-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-dsparse-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-experimental-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-experimental-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-experimental-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-experimental-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-fdense-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-fdense-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-fdense-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-fdense-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-simple-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-simple-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-simple-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-simple-0.42.jar
--- a/lib/ejml-v0.42-libs/ejml-zdense-0.42-sources.jar
+++ b/lib/ejml-v0.42-libs/ejml-zdense-0.42-sources.jar
--- a/lib/ejml-v0.42-libs/ejml-zdense-0.42.jar
+++ b/lib/ejml-v0.42-libs/ejml-zdense-0.42.jar
--- a/src/Car.java
+++ b/src/Car.java
@@ -3,9 +3,6 @@ import java.util.ArrayList;
 import static java.lang.Math.PI;
 import static processing.core.PApplet.degrees;
 import static processing.core.PApplet.radians;
 import Graph.*;
 import Vector.Vector;
 import processing.core.PApplet;
 public class Car{
@@ -38,21 +35,22 @@ public class Car{
    }
    //draw the car and its views
-    public void drawCar(PointGraph map, boolean SLAMIsHidden){
+    public void drawCar(ArrayList<Wall> walls){
        proc.stroke(255);
        proc.ellipse(pose.x, pose.y, carWidth, carLength);
-        this.updateScan(map);
+        this.updateScan(walls);
-        if(!SLAMIsHidden){
+        this.slam.drawLines();
            this.slam.drawFeatures(proc);
        }
    }
    //With all the views that the car has, get their point list
-    void updateScan(PointGraph map){
+    void updateScan(ArrayList<Wall> walls){
        for(View view : views){
-            view.calculatePointScan(map);
+            view.look(walls);
-            slam.RANSAC(view);
+        }
        for(View view : views){
            ArrayList<Vector> pointList = view.getPoints();
            slam.RANSAC(pointList, view.getFOV() / view.getRayNum());
        }
    }
--- a/src/Graph/Edge.java
+++ b/src/Graph/Edge.java
@@ -1,59 +0,0 @@
 package Graph;
 public class Edge {
    Vertex vStart = null;
    Vertex vEnd = null;
    float weight = 0;
    /**
     * @param vStart the vertex the edge starts at
     * @param vEnd the vertex the edge ends at
     * @param weight the weight of the edge
     */
    public Edge(Vertex vStart, Vertex vEnd, float weight){
        this.vStart = vStart;
        this.vEnd = vEnd;
        this.weight = weight;
    }
    /**
     * @param vStart the vertex the edge starts at
     * @param vEnd the vertex the edge ends at
     */
    public Edge(Vertex vStart, Vertex vEnd){
        this.vStart = vStart;
        this.vEnd = vEnd;
    }
    Edge(){}
    /**
     * @return the weight of the edge
     */
    public float getWeight(){
        return weight;
    }
    /**
     * @param newWeight set the edge's weight to something new
     */
    public void setWeight(float newWeight){
        this.weight = newWeight;
    }
    /**
     * @return the vertex at the end of the edge
     */
    public Vertex getEndVertex(){
        return vEnd;
    }
    /**
     * @return the vertex at the start of the edge
     */
    public Vertex getStartVertex(){
        return vStart;
    }
 }
--- a/src/Graph/Graph.java
+++ b/src/Graph/Graph.java
@@ -1,225 +0,0 @@
 package Graph;
 import java.util.ArrayList;
 import java.util.HashMap;
 import java.util.LinkedList;
 public class Graph {
    /**
     * Create a new empty graph
     */
    public Graph(){
        // hash maps require an initial size to get started
        adjList = new HashMap<Vertex, LinkedList<Edge>>(1024);
    }
    /**
     * @return the total number of vertices in the graph
     */
    public int numVertices(){
        return adjList.size();
    }
    /**
     * @return the total number of edges in the graph.
     */
    public int numEdges(){
        int sum = 0;
        for(Vertex v : adjList.keySet()){
            sum += adjList.get(v).size();
        }
        return sum;
    }
    /**
     * @param v add the vertex v to the graph
     */
    public void addVertex(Vertex v){
        // check if it's already in the adjacency listVertex
        if(adjList.containsKey(v)){
            return;
        }
        adjList.put(v, new LinkedList<Edge>());
    }
    /**
     * @param vStart the starting vertex
     * @param vEnd the ending vertex
     */
    public void addEdge(Vertex vStart, Vertex vEnd){
        // don't add the edge if it is already added
        for(Edge e : adjList.get(vStart)){
            if(e.getEndVertex() == vEnd){
                return;
            }
        }
        adjList.get(vStart).add(new Edge(vStart, vEnd));
    }
    /**
     * @brief Gets how many other vertices a given vertex points to
     * @param v the vertex you want to know about
     * @return the number of vertices this vertex points to
     */
    public int outDegree(Vertex v){
        return adjList.get(v).size();
    }
    /**
     * @param v the vertex of interest
     * @return the number of edges going into the vertex
     */
    public int inDegree(Vertex v){
        int count = 0;
        for(Vertex v1 : adjList.keySet()){
            for(Edge edge : adjList.get(v1)){
                if(edge.getEndVertex() == v){
                    count++;
                }
            }
        }
        return count;
    }
    /**
     * Print out all vertexes in the graph
     */
    public void print(){
        for(Vertex key : adjList.keySet()){
            System.out.print("Key: " + key.getLabel() + ": ");
            for(Edge edge : adjList.get(key)){
                System.out.print(edge.getEndVertex().getLabel());
                System.out.print(",");
            }
            System.out.println("NULL");
        }
    }
    /**
     * @param v the vertext to start the traverse at
     * @return a list of vertexes in order of when they were visited from first to last.
     */
    public ArrayList<Vertex> depthFirstTraverse(Vertex v){
        ArrayList<Vertex> vertexList = new ArrayList<Vertex>();
        DFSTraverse(v, vertexList);
        resetVisits();
        return vertexList;
    }
    /**
     * @param v the vertex to begin the traverse at
     * @param vertexList the vertex list to append visited vertexes to
     */
    private void DFSTraverse(Vertex v, ArrayList<Vertex> vertexList){
        vertexList.add(v);
        v.setVisitedStatus(true);
        LinkedList<Edge> edgeList = adjList.get(v);
        for(Edge edge : edgeList){
            if(!(edge.getEndVertex().visitedStatus())){
                DFSTraverse(edge.getEndVertex(), vertexList);
            }
        }
    }
    /**
     * @post all vertexes in the graph will be set to unvisited
     */
    private void resetVisits(){
        for(Vertex v : adjList.keySet()){
            v.setVisitedStatus(false);
        }
    }
    /**
     * @param v the vertex that you wish to see the neighbors of
     * @return a list of vertexes that are connected to v with ingoing or outgoing edges
     */
    public ArrayList<Vertex> getNeightborVerts(Vertex v){
        // iterate through all of the vertexes and make sure they're marked as unvisited
        for(Vertex v1 : adjList.keySet()){
            v1.setVisitedStatus(false);
        }
        ArrayList<Vertex> neighbors = new ArrayList<>();
        for(Vertex v1 : adjList.keySet()){
            for(Edge edge : adjList.get(v1)){
                if(edge.getStartVertex() == v && !edge.getEndVertex().visitedStatus()){
                    edge.getEndVertex().setVisitedStatus(true);
                    neighbors.add(edge.getEndVertex());
                }
                if(edge.getEndVertex() == v && !edge.getStartVertex().visitedStatus()){
                    edge.getStartVertex().setVisitedStatus(true);
                    neighbors.add(edge.getStartVertex());
                }
            }
        }
        resetVisits();
        return neighbors;
    }
    /**
     * @param v the vertex that you wish to start the search at
     * @return A list of vertexes in order of visitation where the search goes wide and then deep
     */
    public ArrayList<Vertex> breadthFirstSearch(Vertex v){
        ArrayList<Vertex> queue = new ArrayList<>();
        ArrayList<Vertex> outputList = new ArrayList<>();
        outputList.add(v);
        queue.add(v);
        v.setVisitedStatus(true);
        while(queue.size() > 0){
            Vertex next = queue.get(0);
            queue.remove(0);
            LinkedList<Edge> edgeList = adjList.get(next);
            for(Edge edge : edgeList){
                if(!edge.getEndVertex().visitedStatus()){
                    queue.add(edge.getEndVertex());
                    outputList.add(edge.getEndVertex());
                    edge.getEndVertex().setVisitedStatus(true);
                }
            }
        }
        resetVisits();
        return outputList;
    }
    /**
     * @brief remove the given vertex and all edges that reference it from the graph
     * @param v the vertex to remove from the graph.
     */
    public void removeVertex(Vertex v){
        // find all edges that point to the removed vertex
        ArrayList<Edge> edgesToRemove = new ArrayList<>();
        ArrayList<Vertex> startVertex = new ArrayList<>();
        for(Vertex v1 : adjList.keySet()){
            for(Edge e : adjList.get(v1)){
                if(e.getEndVertex() == v){
                    edgesToRemove.add(e);
                    startVertex.add(e.getStartVertex());
                }
            }
        }
        // remove all of those edges from the adjList
        int i = 0;
        for(Edge e : edgesToRemove){
            adjList.get(startVertex.get(i)).remove(e);
            i++;
        }
        // remove the vertex from the adjacency list
        adjList.remove(v);
    }
    protected HashMap<Vertex, LinkedList<Edge>> adjList;
 }
--- a/src/Graph/LineEdge.java
+++ b/src/Graph/LineEdge.java
@@ -1,53 +0,0 @@
 package Graph;
 import Vector.*;
 import processing.core.PApplet;
 import java.awt.*;
 import static java.lang.Math.PI;
 public class LineEdge extends Edge implements LineInterface{
    protected PointVertex vStart;
    protected PointVertex vEnd;
    protected Line line;
    public LineEdge(PointVertex vStart, PointVertex vEnd) {
        super(vStart, vEnd);
        this.line = new Line(vStart.getPos(), vEnd.getPos());
    }
    public Vector getDirection(){
        return line.getDirection();
    }
    public Vector getPosition(){
        return line.getPosition();
    }
    public float getLength(){
        return line.getLength();
    }
    public float getAngle(){
        return line.getAngle();
    }
    public Vector endPoint(){
        return line.endPoint();
    }
    public float getDistance(Vector point){
        return line.getDistance(point);
    }
    public void draw(PApplet proc){
        line.draw(proc);
        Vector leftFlange = line.getDirection().rotate2D((float)(-3*PI/4)).normalize().mul(20);
        Vector rightFlange = line.getDirection().rotate2D((float) (3*PI/4)).normalize().mul(20);
        Line l1 = new Line(line.endPoint(), line.endPoint().add(leftFlange));
        Line l2 = new Line(line.endPoint(), line.endPoint().add(rightFlange));
        l1.draw(proc);
        l2.draw(proc);
    }
 }
--- a/src/Graph/PointGraph.java
+++ b/src/Graph/PointGraph.java
@@ -1,152 +0,0 @@
 package Graph;
 import Vector.Vector;
 import processing.core.PApplet;
 import java.io.File;  // Import the File class
 import java.io.FileWriter;
 import java.io.IOException;  // Import the IOException class to handle errors
 import static java.lang.Math.pow;
 import static java.lang.Math.sqrt;
 import java.util.ArrayList;
 public class PointGraph extends Graph {
    PointVertex selectedVertex;
    public PointGraph(){
        super();
    }
    public void draw(PApplet proc){
        for(Vertex v : adjList.keySet()){
            PointVertex v1 = (PointVertex) v;
            v1.draw(proc);
            for(Edge e : adjList.get(v)){
                LineEdge e1 = (LineEdge) e;
                e1.draw(proc);
            }
        }
    }
    public void addEdge(PointVertex vStart, PointVertex vEnd){
        addVertex(vStart);
        // don't add the edge if it is already added
        for(Edge e : adjList.get(vStart)){
            if(e.getEndVertex() == (Vertex) vEnd){
                return;
            }
        }
        addVertex(vEnd);
        LineEdge edge = new LineEdge(vStart, vEnd);
        adjList.get((Vertex) vStart).add(new LineEdge(vStart, vEnd));
    }
    /**
     * @param v set this vertex as the selected vertex in the graph
     */
    public void setSelectedVertex(PointVertex v){
        if(selectedVertex != null){
            deselectVertex();
        }
        selectedVertex = v;
        selectedVertex.setColor(new int[]{255, 0, 255, 0});
    }
    /**
     * @pre a vertex needs to be selected
     * @return the current pointvertex which is selected in the graph
     */
    public PointVertex getSelectedVertex(){
        if(selectedVertex == null){
            if(super.adjList.size() > 0){
                selectedVertex = (PointVertex)super.adjList.keySet().iterator().next();
            }
            else{
                throw new NullPointerException();
            }
        }
        return selectedVertex;
    }
    /**
     * @return true if a vertex is currently selected in the graph
     */
    public boolean vertexIsSelected(){
        return selectedVertex != null;
    }
    /**
     * @brief deselect the currently selected vertex
     */
    public void deselectVertex(){
        if(selectedVertex == null){
            return;
        }
        selectedVertex.setColor(new int[]{127, 255, 0, 0});
        selectedVertex = null;
    }
    /**
     * @param x the x coordinate to search by
     * @param y the y coordinate to search by
     * @return the pointvertex closest to the given x, y coordinates
     */
    public PointVertex getClosestVertex(Vector point){
        if(super.adjList.size() == 0){
            // TODO: choose a better exception name
            throw new NullPointerException();
        }
        PointVertex closestVertex = (PointVertex) super.adjList.keySet().iterator().next();
        float closestDist = -1;
        for(Vertex v : super.adjList.keySet()){
            PointVertex v1 = (PointVertex) v;
            Vector p2 = v1.getPos();
            float dist = p2.sub(point).mag();
            if(dist < closestDist || closestDist == -1){
                closestDist = dist;
                closestVertex = v1;
            }
        }
        return closestVertex;
    }
    public void removeVertex(PointVertex v){
        if(selectedVertex == v){
            selectedVertex = null;
        }
        super.removeVertex(v);
    }
    /**
     * @return all edges in the graph
     */
    public ArrayList<LineEdge> getAllEdges(){
        ArrayList<LineEdge> edges = new ArrayList<>();
        for(Vertex v : adjList.keySet()){
            for(Edge e : adjList.get(v)){
                LineEdge e1 = (LineEdge) e;
                edges.add(e1);
            }
        }
        return edges;
    }
    /**
     * @return all vertexes in the graph
     */
    public ArrayList<PointVertex> getAllVertexes(){
        ArrayList<PointVertex> points = new ArrayList<>();
        for(Vertex v : adjList.keySet()){
            PointVertex v1 = (PointVertex) v;
            points.add(v1);
        }
        return points;
    }
 }
--- a/src/Graph/PointGraphWriter.java
+++ b/src/Graph/PointGraphWriter.java
@@ -1,122 +0,0 @@
 package Graph;
 import java.io.File;
 import java.io.FileNotFoundException;
 import java.io.FileWriter;
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.Scanner;
 public class PointGraphWriter {
    public void save(String filename, PointGraph g) throws IOException {
        FileWriter file = new FileWriter(filename);
        file.write("numVerts," + g.numVertices());
        file.write("\nnumEdges," + g.numEdges());
        // turn the hash map into something linear
        ArrayList<PointVertex> verts = g.getAllVertexes();
        ArrayList<LineEdge> edges = g.getAllEdges();
        // save the vertexes
        int countVerts = 0;
        for(PointVertex v : verts){
            // save the vertex position
            file.write("\nvert,"+v.getPos().x+","+v.getPos().y);
            countVerts++;
        }
        // save the edges
        for(Edge e : edges){
            int idx = 0;
            file.write("\nedge,");
            boolean otherIsWritten = false;
            for(PointVertex v : verts){
                if(e.getStartVertex() == (Vertex)v){
                    file.write("start,"+ idx);
                    if(!otherIsWritten){
                        file.write(",");
                        otherIsWritten = true;
                    }
                }
                else if(e.getEndVertex() == (Vertex)v){
                    file.write("end," + idx);
                    if(!otherIsWritten){
                        file.write(",");
                        otherIsWritten = true;
                    }
                }
                idx++;
            }
        }
        file.close();
    }
    public PointGraph loadFile(String filename) throws NumberFormatException {
        PointGraph g = new PointGraph();
        File file = new File(filename);
        Scanner reader;
        try {
            reader = new Scanner(file);
        }
        catch (FileNotFoundException e){
            System.out.println("File not found");
            return g;
        }
        ArrayList<PointVertex> vertices = new ArrayList<>();
        while(reader.hasNextLine()){
            String line = reader.nextLine();
            ArrayList<String> args = parseLine(line);
            String key = args.get(0);
            switch (key) {
                case "numVerts" -> System.out.println("Number of Vertexes: " + Integer.parseInt(args.get(1)));
                case "numEdges" -> System.out.println("Number of Edges: " + Integer.parseInt(args.get(1)));
                case "vert" -> {
                    float x = Float.parseFloat(args.get(1));
                    float y = Float.parseFloat(args.get(2));
                    PointVertex v = new PointVertex(x, y);
                    g.addVertex(v);
                    vertices.add(v);
                }
                case "edge" -> {
                    int startIdx;
                    int endIdx;
                    if (args.get(1).contains("start")) {
                        startIdx = Integer.parseInt(args.get(2));
                        endIdx = Integer.parseInt(args.get(4));
                    }
                    else{
                        startIdx = Integer.parseInt(args.get(4));
                        endIdx = Integer.parseInt(args.get(2));
                    }
                    g.addEdge(vertices.get(startIdx), vertices.get(endIdx));
                }
                default -> System.out.println("Unrecognized Line: " + line);
            }
        }
        return g;
    }
    private ArrayList<String> parseLine(String line){
        ArrayList<String> args = new ArrayList<>();
        StringBuilder arg = new StringBuilder();
        for(char letter : line.toCharArray()){
            if(letter == ','){
                args.add(arg.toString());
                arg = new StringBuilder();
                continue;
            }
            if(letter == '\n'){
                args.add(arg.toString());
                break;
            }
            arg.append(letter);
        }
        if(!args.get(args.size()-1).contains(arg.toString())){
            args.add(arg.toString());
        }
        return args;
    }
 }
--- a/src/Graph/PointVertex.java
+++ b/src/Graph/PointVertex.java
@@ -1,66 +0,0 @@
 package Graph;
 import Vector.Vector;
 import processing.core.PApplet;
 public class PointVertex extends Vertex {
    private Vector position;
    private int[] color = new int[]{127, 255, 0, 0};
    /**
     * @param xPos the x position of the vertex
     * @param yPos the y posiiton of the vertex
     */
    public PointVertex(float xPos, float yPos){
        this.position = new Vector(xPos, yPos);
    }
    public PointVertex(Vector position){
        super();
        this.position = position;
    }
    /**
     * @param xPos the x position of the vertex
     * @param yPos the y posiiton of the vertex
     * @param label the label of the vertex
     */
    PointVertex(float xPos, float yPos, String label){
        super(label);
        this.position = new Vector(xPos, yPos);
    }
    /**
     * @param x the new x position of the vertex
     * @param y the new y posiiton of the vertex
     */
    public void setPos(float x, float y){
        this.position = new Vector(x, y);
    }
    /**
     * @return a two eleement float array containing the x and y coordinates of the vertex respectively.
     */
    public Vector getPos(){
        return position;
    }
    /**
     * @param newColor a 4 element int array containing th alpha, r, g ,and b components respectively
     */
    public void setColor(int[] newColor){
        this.color = newColor;
    }
    /**
     * @return a 4 element int array containing th alpha, r, g ,and b components respectively
     */
    public int[] getColor(){
        return color;
    }
    public void draw(PApplet proc){
        proc.stroke(color[1], color[2], color[3], color[0]);
        proc.circle(position.x, position.y, 20);
    }
 }
--- a/src/Graph/Vertex.java
+++ b/src/Graph/Vertex.java
@@ -1,45 +0,0 @@
 package Graph;
 public class Vertex {
    private String vertexLabel;
    private boolean isVisitedStatus = false;
    /**
     * Create a new vertex with a default label
     */
    public Vertex(){
        this.vertexLabel = "Unassigned";
    }
    /**
     * @param label create a new vertex with this label
     */
    public Vertex(String label){
        this.vertexLabel = label;
    }
    /**
     * @return the label of the vertex
     */
    public String getLabel(){
        return vertexLabel;
    }
    public void setLabel(String newLabel){
        vertexLabel = newLabel;
    }
    /**
     * @return if the node has been visited or not
     */
    public boolean visitedStatus(){
        return this.isVisitedStatus;
    }
    /**
     * @param status set the visited status to true or false
     */
    public void setVisitedStatus(boolean status){
        this.isVisitedStatus = status;
    }
 }
--- a/src/Line.java
+++ b/src/Line.java
@@ -0,0 +1,88 @@
 import processing.core.PApplet;
 import java.util.List;
 import static processing.core.PApplet.*;
 public class Line{
    private Vector direction = new Vector(0,0);
    private Vector position = new Vector(0,0);
    Line(Vector startPosition, Vector endPosition){
        direction = endPosition.sub(startPosition);
        position = startPosition;
    }
    /**
     * attempt to find the line of best fit for the given points
     * @param points the points to get the line of best for
     */
    Line(List<Vector> points){
        bestFit(points);
    }
    // least squares line of best fit algorithm
    private void bestFit(List<Vector> points){
        // get the mean of all the points
        Vector mean = new Vector();
        for(Vector point : points){
            mean = mean.add(point);
        }
        mean = mean.div(points.size());
        // this section calculates the direction vector of the line of best fit
        Vector direction = new Vector();
        float length = 1;
        // get the rise and run of the line of best fit
        for(Vector point : points){
            direction.y += (point.x - mean.x)*(point.y - mean.y); // rise
            direction.x += pow((point.x - mean.x),2);
            // find the point that's furthest from the mean and use it to set the line length.
            float dist = abs(point.sub(mean).mag());
            if(dist > length){
                length = 2*dist;
            }
        }
        if(direction.y == 0){
            this.direction = new Vector(0, 1);
        }
        else{
            this.direction = new Vector(1, direction.y/direction.x);
        }
        this.direction = this.direction.normalize().mul(length);
        this.position = mean.sub(this.direction.div(2));
    }
    public Vector getSlopeIntForm(){
        float slope = direction.y / direction.x;
        float intercept = position.y - slope * position.x;
        return new Vector(slope, intercept);
    }
    public Vector getDirection(){
        return direction;
    }
    public Vector getPosition(){
        return position;
    }
    public float getLength(){
        return direction.mag();
    }
    public void draw(PApplet screen){
        Vector endPoint = this.position.add(this.direction);
        screen.line(position.x, position.y, endPoint.x, endPoint.y);
    }
    /**
     * @param point
     * @return the smallest distance from the point to this line
     */
    public float getDistance(Vector point){
        return (point.sub(position).cross(direction)).mag() / direction.mag();
    }
 }
--- a/src/Processing.java
+++ b/src/Processing.java
@@ -1,19 +1,13 @@
 import Graph.*;
 import Vector.Vector;
 import processing.core.PApplet;
-import java.io.FileNotFoundException;
+import java.util.ArrayList;
 import java.io.IOException;
 public class Processing extends PApplet {
    Car car;
-    public static PApplet processing;
+    ArrayList<Wall> objects = new ArrayList<>();
-    PointGraph map = new PointGraph();
+    public static PApplet processing;
    boolean mapIsHidden = false;
    boolean SLAMIsHidden = false;
    public static void main(String[] args) {
        PApplet.main("Processing");
@@ -23,37 +17,33 @@ public class Processing extends PApplet {
        processing = this;
        car = new Car(processing, 100,100,50,40);
        size(1000, 1000);
-        car.addView(90,180);
+        car.addView(180,180);
-
+        for(int i = 0; i < 20; i++){
-//        for(int i = 0; i < 10; i++){
+            Wall wall = new Wall(processing, new Vector((int)random(40, 1840), (int)random(40, 960)), (int)random(360), (int)random(100, 1000));
-//            PointVertex vStart = new PointVertex(random(50, 950), random(50, 950));
+            objects.add(wall);
-//            PointVertex vEnd = new PointVertex(random(50, 950), random(50, 950));
+        }
 //            map.addEdge(vStart, vEnd);
 //        }
    }
    public void draw(){
        background(0);
-        if(!mapIsHidden){
+//        for(Wall object : objects){
-            map.draw(processing);
+//            object.drawWall();
-        }
+//        }
-        car.drawCar(map, SLAMIsHidden);
+        car.drawCar(objects);
-        strokeWeight(2);
+        //car.drive(new int[] {0, 0});
        stroke(255);
    }
    public void keyPressed(){
        if(key == 'd'){
-            car.setPose(car.getPose().add(10, 0));
+            car.setPose(car.getPose().add(1, 0));
        }
        if(key == 'w'){
-            car.setPose(car.getPose().add(0, -10));
+            car.setPose(car.getPose().add(0, -1));
        }
        if(key == 'a'){
-            car.setPose(car.getPose().add(-10, 0));
+            car.setPose(car.getPose().add(-1, 0));
        }
        if(key == 's'){
-            car.setPose(car.getPose().add(0, 10));
+            car.setPose(car.getPose().add(0, 1));
        }
        if(key == 'q'){
            car.setAngle(car.getAngle()+1);
@@ -61,64 +51,9 @@ public class Processing extends PApplet {
        if(key == 'e'){
            car.setAngle(car.getAngle()-1);
        }
-        if(key == DELETE && map.vertexIsSelected()){
+
            map.removeVertex(map.getSelectedVertex());
        }
        if(key == ' ' && map.vertexIsSelected()){
            map.deselectVertex();
        }
        if(key == ESC){
            System.out.println("Attempting to save map to file.");
            try{
                PointGraphWriter writer = new PointGraphWriter();
                writer.save("map.txt", map);
            }
            catch(IOException e){
                e.printStackTrace();
            }
        }
        if(key == 'l'){
            System.out.println("Attempting to load a map from file");
            PointGraphWriter writer = new PointGraphWriter();
            try {
                map = writer.loadFile("map.txt");
            } catch (NumberFormatException e) {
                System.out.println("Number format incorrect");
                e.printStackTrace();
            }
        }
        if(key == 'h'){
            mapIsHidden = !mapIsHidden;
        }
        if(key == 'j'){
            SLAMIsHidden = !SLAMIsHidden;
        }
    }
    public void mousePressed(){
        Vector clickPosition = new Vector(mouseX, mouseY);
-        if(map.numVertices() == 0){
+
            PointVertex v = new PointVertex(clickPosition);
            map.addVertex(v);
            return;
        }
        PointVertex closestVertex = map.getClosestVertex(clickPosition);
        float distance = closestVertex.getPos().sub(clickPosition).mag();
        if(distance < 15){
            if(map.vertexIsSelected()){
                if(map.getSelectedVertex() == closestVertex){
                    map.deselectVertex();
                }
                else{
                    map.addEdge(map.getSelectedVertex(), closestVertex);
                }
                return;
            }
            map.setSelectedVertex(closestVertex);
            return;
        }
        PointVertex v = new PointVertex(clickPosition);
        map.addVertex(v);
    }
 }
--- a/src/Ray.java
+++ b/src/Ray.java
@@ -1,97 +0,0 @@
 import Graph.*;
 import Vector.*;
 import processing.core.PApplet;
 import java.util.ArrayList;
 import static processing.core.PApplet.*;
 public class Ray extends Line {
    float maxRayDistance = 1000;
    int[] color = new int[]{255, 255, 255};
    //takes the starting position of the ray, the length of the ray, and it's casting angle (radians)
    Ray(Vector startPosition, float angle){
        super(startPosition, startPosition.add(new Vector(cos(angle), sin(angle))));
        direction = direction.mul(maxRayDistance);
    }
    public void drawRay(PApplet proc){
        proc.stroke(color[0], color[1], color[2]);
        proc.line(position.x, position.y, position.x + direction.x, position.y + direction.y);
 //        proc.noFill();
 //        proc.circle(position.x, position.y, 2*direction.mag());
 //        proc.fill(255);
    }
    //checks to see at what coordinate the ray will collide with an object and sets the ray length to meet that point.
    public void castRay(PointGraph map){
        float shortestWallDistance = maxRayDistance;
        ArrayList<LineEdge> walls = map.getAllEdges();
        for(LineEdge wall : walls){
            // get the necessary vectors for two parameterized lines
            // parameterized lines are of the form L = d*t + p
            Vector d1 = this.direction.normalize().mul(maxRayDistance);
            Vector d2 = wall.getDirection();
            Vector p1 = this.position;
            Vector p2 = wall.getPosition();
            // calculate the parameters for the intersection t and u
            float t = -(d2.x*(p2.y-p1.y) + d2.y*(p1.x-p2.x))/(d1.x*d2.y - d2.x*d1.y);
            float u = -(d1.x*(p2.y-p1.y) + d1.y*(p1.x-p2.x))/(d1.x*d2.y-d2.x*d1.y);
            // the lines will only be intersecting when both t and u are between 0 and 1.
            if(!(0 <= t && t <= 1 && 0 <= u && u <= 1)){
                continue;
            }
            // if the distance from the ray to the intersection is shorter than the shortestWallDistance, this is our new closest wall
            float distance = d1.mul(t).add(p1).sub(this.position).mag();
            if(distance < shortestWallDistance){
                shortestWallDistance = distance;
            }
        }
        // if we collided with a wall, set the ray's length to the distance from it to the collision
        if(shortestWallDistance != maxRayDistance){
            this.direction = this.direction.normalize().mul(shortestWallDistance);
        }
        else{
            this.direction = this.direction.normalize().mul(maxRayDistance);
        }
    }
    public Vector getPos(){ return this.position;}
    public float getRayLength(){return this.direction.mag();}
    public boolean hasCollided(){
        return abs(this.direction.mag() - maxRayDistance) > 0.001;
    }
    //returns the absolute position of the point
    public Vector getPoint(){
        if(this.direction.mag() == 0){
            return this.position;
        }
        return this.position.add(this.direction);
    }
    public void setPos(Vector newPosition){
        this.position = newPosition;
    }
    public void setRayLength(int rayLength){this.direction = this.direction.normalize().mul(rayLength);}
    public void setAngle(float angle){
        float currentAngle = direction.angle();
        this.direction = direction.rotate2D(angle - currentAngle);
    }
 }
--- a/src/SLAM.java
+++ b/src/SLAM.java
@@ -1,16 +1,13 @@
 import Vector.*;
 import processing.core.*;
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
 import static java.lang.Math.random;
 import static processing.core.PApplet.*;
 public class SLAM{
    ArrayList<Line> lines = new ArrayList<>();
    ShortTermMem unassociatedPoints = new ShortTermMem();
    private static PApplet proc;
    SLAM(PApplet processing){
@@ -19,41 +16,38 @@ public class SLAM{
    /**
     * @param set the set to take a sub sample of
     * @param indexRange the range within to take the sub sample
     * @param subSampleSize the size of the sub sample
-     * @param minAngle the minimum angle allowed in the subset
+     * @return A random subset of the set within an indexRange and of size: subSampleSize
     * @param maxAngle the maximum angle allowed in the subset
     * @return A random subset of the set within the angle range
     */
-    private List<Vector> randomSampleInAngleRange(ArrayList<Vector> set, int subSampleSize, float minAngle, float maxAngle){
+    private List<Vector> randomSample(ArrayList<Vector> set, int indexRange, int subSampleSize){
        // select a random laser data reading
        int randomIdx = (int) proc.random(set.size() - 1); // index of starter reading
        Vector point = set.get(randomIdx); // point of starter reading
-        // create an arraylist with all points within the angle range from the given set
+        // get a random sample of size numSampleReadings within degreeRange degrees of this laser reading.
-        ArrayList<Vector> pointsInAngleRange = new ArrayList<>();
+        List<Vector> subSample;
-        for(Vector point : set){
+        int rangeStart = randomIdx - indexRange >= 0 ? randomIdx - indexRange : 0;
-            if(minAngle <= point.z && point.z <= maxAngle){
+        int rangeEnd = randomIdx + indexRange < set.size() ? randomIdx + indexRange : set.size()-1;
-                pointsInAngleRange.add(point);
+        subSample = set.subList(rangeStart, rangeEnd); // get the sub-sample
-            }
+        Collections.shuffle(subSample); // shuffle the list
        List<Vector> randomSample = subSample.subList(0, rangeEnd-rangeStart); // get our random sample
        if (!randomSample.contains(point)) {
            randomSample.add(point);
        }
-        // shuffle the list to randomize it
+        return randomSample;
        Collections.shuffle(pointsInAngleRange);
        // if the list is too small, just return the whole list
        if(pointsInAngleRange.size() < subSampleSize){
            return pointsInAngleRange;
        }
        // return a subSample of the list
        return pointsInAngleRange.subList(0, subSampleSize);
    }
    /**
     * @param originalList the list which the randomSample of points originated from
     * @param randomSample a random subsampling of points from the originalList
     * @param maxRange the maximum distance away from the line of best fit of the subSample of points for a given point's consensus to count.
     * @param consensus the number of points that have to give their consensus for the line of best fit to count as a valid feature.
     */
-    private void extractFeature(List<Vector> randomSample, float maxRange, int consensus){
+    private void extractFeature(ArrayList<Vector> originalList, List<Vector> randomSample, float maxRange, int consensus){
        // get a line of best fit for this list.
        Line bestFit = new Line(randomSample);
        // check that there are enough points in the sample that are less than the maxRange away to form a consensus
        int count = 0;
        ArrayList<Vector> newRandomSample = new ArrayList<>();
        for (Vector v : randomSample) {
@@ -68,66 +62,44 @@ public class SLAM{
            lines.add(bestFit);
            // remove the associated readings from the total available readings.
            for (Vector v : newRandomSample) {
-                this.unassociatedPoints.remove(v);
+                originalList.remove(v);
            }
        }
    }
    private void fitToPreviousReadings(List<Vector> sample, float maxRange){
        // keep track of points that were succesffully associated so they can be removed from the sample at the end
        ArrayList<Vector> pointsToRemove = new ArrayList<>();
        // try to associate points from the smaple with pre-existing lines
        for(Vector v: sample){
            for(Line l : lines){
                if(l.getDistance(v) < maxRange){
                    l.refitLine(v);
                    pointsToRemove.add(v);
                }
            }
        }
        for(Vector v : pointsToRemove){
            sample.remove(v);
        }
    }
    /**
-     * @param view a laser scan view
+     * @param newPoints a new scan of points to perform feature detection on
     * @param raysPerDegree How many degrees apart are each ray that was cast
     */
-    public void RANSAC(View view){
+    public void RANSAC(ArrayList<Vector> newPoints, float raysPerDegree){
-        unassociatedPoints.addScan(view.getPos(), view.getScan().getPoints());
+        float degreeRange = radians(10/2); // range to randomly sample readings within
-
+        int indexRange = (int) (degreeRange / raysPerDegree);
-        float degreeRange = radians(5); // range to randomly sample readings within
+        int numSampleReadings = 10; // number of readings to randomly sample
-        int numSampleReadings = 15; // number of readings to randomly sample
+        // constrain numSampleReadings so that it cant be higher than possible
-
+        if(numSampleReadings >= 2 * indexRange){
-        int consensus = 10; // the number of points that need to lie near a line for it to be considered valid.
+            numSampleReadings = 2 * indexRange;
        }
        int consensus = 6; // the number of points that need to lie near a line for it to be considered valid.
        float maxRange = 10; // the maximum distance a point can be away from the line for it to count as a consensus
        // this for loop determines the maximum number of trials we're willing to do.
        for(int j = 0; j < 20; j++) {
            // if there aren't enough points left in the set to form a consensus, we're done.
-            if(this.unassociatedPoints.size() < maxRange){
+            if(newPoints.size() < consensus){
                break;
            }
            // get a random angle between -PI and PI
            float randomAngle = (float) (2*PI*(random()) - 0.5);
            // get a random sub sample of newPoints within the index range of a given size
-            List<Vector> randomSample = this.randomSampleInAngleRange(this.unassociatedPoints.getPoints(), numSampleReadings, randomAngle-degreeRange, randomAngle+degreeRange);
+            List<Vector> randomSample = this.randomSample(newPoints, indexRange, numSampleReadings);
-            if(randomSample.size() >= numSampleReadings){
+            // check if the sub sample forms a valid line and remove the randomSample points if it does.
-                // try to associate points from the sample with previously made lines
+            extractFeature(newPoints, randomSample, maxRange, consensus);
                fitToPreviousReadings(randomSample, maxRange);
                // check if the sub sample forms a valid line and remove the randomSample points if it does.
                extractFeature(randomSample, maxRange, consensus);
            }
        }
    }
-    public void drawFeatures(PApplet proc){
+    public void drawLines(){
        for(Line line : lines){
            line.draw(proc);
        }
--- a/src/ScanGraph/ScanEdge.java
+++ b/src/ScanGraph/ScanEdge.java
@@ -1,47 +0,0 @@
 package ScanGraph;
 import Graph.Edge;
 import Vector.Line;
 import Vector.LineInterface;
 import Vector.Vector;
 import processing.core.PApplet;
 import static java.lang.Math.PI;
 public class ScanEdge extends Edge {
    protected Line line;
    // Additional properties specific to scan edges
    private boolean isLoopClosure = false;
    /**
     * @brief Constructor for a scan edge
     * @param vStart the starting vertex
     * @param vEnd   the ending vertex
     */
    public ScanEdge(ScanPoint vStart, ScanPoint vEnd){
        super(vStart, vEnd);
        this.line = new Line(vStart.getPos(), vEnd.getPos());
    }
    /**
     * @brief Constructor for a scan edge
     * @param vStart the starting vertex
     * @param vEnd   the ending vertex
     * @param weight the weight of the edge
     */
    public ScanEdge(ScanPoint vStart, ScanPoint vEnd, float weight) {
        super(vStart, vEnd, weight);
        this.line = new Line(vStart.getPos(), vEnd.getPos());
    }
    // Getter and setter for loop closure flag
    public boolean isLoopClosure() {
        return isLoopClosure;
    }
    public void setLoopClosure(boolean loopClosure) {
        isLoopClosure = loopClosure;
    }
 }
--- a/src/ScanGraph/ScanGraph.java
+++ b/src/ScanGraph/ScanGraph.java
@@ -1,170 +0,0 @@
 package ScanGraph;
 import Graph.Graph;
 import Graph.Edge;
 import Graph.Vertex;
 import Vector.Vector;
 import org.ejml.simple.SimpleMatrix;
 import org.ejml.simple.SimpleSVD;
 import java.util.ArrayList;
 public class ScanGraph extends Graph {
    ScanPoint lastPoint;
    public ScanGraph(ScanPoint startingPoint) {
        super();
        this.lastPoint = startingPoint;
    }
    /**
     * @brief Add a new scan to the graph
     * @param newScan the new scan to add
     */
    public void addScan(ScanPoint newScan) {
        addVertex(newScan);
        // check if the new scan matches any of the existing scans
        MatchedScanTransform matchedScan = getAssociatedScan(newScan);
        if (matchedScan.scan != null) {
            // if it does, add a loop closure constraint
            addLoopClosureConstraint(this.lastPoint, matchedScan);
        }
        else{
            // if it doesn't match anything else, add an edge between the last scan and the new scan
            ScanEdge edge = new ScanEdge(this.lastPoint, newScan);
            adjList.get((Vertex) this.lastPoint).add(edge);
        }
        this.lastPoint = newScan;
    }
    /**
     * @param newScan the scan to match
     * @return null if no match can be found, or an existing scan the matches the new scan.
     * @brief Get a new scan in and try to match it with all other scans in the graph
     */
    private MatchedScanTransform getAssociatedScan(ScanPoint newScan) {
        ScanMatcher matcher = new ScanMatcher();
        ScanPoint matchedScan = null;
        // go through all of our available scans and try to match the new scan with the old scans. If no match can be found return null
        for (Vertex v : adjList.keySet()) {
            ScanPoint referenceScan = (ScanPoint) v;
            matchedScan = matcher.iterativeScanMatch(referenceScan, newScan, 0.00001F, 5);
            if(matchedScan != null){
                // apply the transformation to the new scan
                break;
            }
        }
        return new MatchedScanTransform(matcher.getRotationMatrix(), matcher.getTranslationVector(), matchedScan);
    }
    /**
     * @param referenceScan the existing scan in the graph
     * @param matchedScan the new scan that matches the reference scan
     */
    private void addLoopClosureConstraint(ScanPoint referenceScan, MatchedScanTransform matchedScan) {
        // Compute relative transformation between referenceScan and newScan
        // You need to implement the logic for computing the relative transformation
        // Create a loop closure edge and add it to the graph
        ScanEdge loopClosureEdge = new ScanEdge(referenceScan, matchedScan.scan);
        loopClosureEdge.setLoopClosure(true);  // Mark the edge as a loop closure
        adjList.get(referenceScan).add(loopClosureEdge);
        // Optimize the graph after adding the loop closure constraint
        optimizeGraph();
    }
    /**
     * Perform graph optimization using the Levenberg-Marquardt algorithm
     */
    private void optimizeGraph() {
        // Create a matrix for pose parameters (you may need to adjust the size based on your requirements)
        int numPoses = adjList.size();
        int poseDim = 3; // Assuming 3D poses
        SimpleMatrix poses = new SimpleMatrix(numPoses * poseDim, 1);
        // Populate the poses matrix with current pose estimates from the graph
        int i = 0;
        for (Vertex v : adjList.keySet()) {
            ScanPoint scan = (ScanPoint) v;
            Vector poseVector = scan.getPos(); // You need to implement the method to get the pose vector
            poses.set(i++, 0, poseVector.x);
            poses.set(i++, 0, poseVector.y);
            poses.set(i++, 0, scan.getAngle());
        }
        // TODO: Create a matrix for loop closure constraints (you need to implement this)
        SimpleMatrix loopClosureConstraints = computeLoopClosureConstraints();
        // Use Levenberg-Marquardt optimization to adjust poses
        SimpleSVD<SimpleMatrix> svd = poses.svd();
        SimpleMatrix U = svd.getU();
        SimpleMatrix S = svd.getW();
        SimpleMatrix Vt = svd.getV().transpose();
        // Adjust poses using loop closure constraints
        SimpleMatrix adjustment = Vt.mult(loopClosureConstraints).mult(U.transpose())
                .scale(0.01); // Adjust this factor based on your problem
        // Update the poses in the graph and handle loop closure edges
        i = 0;
        for (Vertex v : adjList.keySet()) {
            ScanPoint scan = (ScanPoint) v;
            scan.setPos(new Vector(adjustment.get(i++, 0), adjustment.get(i++, 0), adjustment.get(i++, 0)));
            // TODO: Handle loop closure edges
            for (Edge edge : adjList.get(v)) {
                ScanEdge scanEdge = (ScanEdge) edge;
                if (scanEdge.isLoopClosure()) {
                    // Update any additional information specific to loop closure edges
                    // For example, you might update the weight or perform other adjustments
                    // based on the loop closure information
                }
            }
        }
    }
    /**
     * @return Matrix representing loop closure constraints (you need to implement this)
     */
    private SimpleMatrix computeLoopClosureConstraints() {
        // Implement the logic to compute loop closure constraints
        // This matrix should represent the relative transformation between loop closure nodes
        // It may involve iterating through loop closure edges and extracting relevant information
        // You may use a similar structure to the poses matrix
        // For simplicity, this example assumes a 3D pose with translation only
        int numPoses = adjList.size();
        int poseDim = 3;
        SimpleMatrix loopClosureConstraints = new SimpleMatrix(numPoses * poseDim, 1);
        // Populate loopClosureConstraints matrix with relevant information
        return loopClosureConstraints;
    }
 }
 /**
 * @brief struct to hold the rotation and translation between two scans
 */
 class MatchedScanTransform{
    public SimpleMatrix rotation;
    public SimpleMatrix translation;
    public ScanPoint scan;
    /**
     * @brief constructor
     * @param rotation the rotation between the two scans
     * @param translation the translation between the two scans
     * @param scan the scan that was matched
     */
    MatchedScanTransform(SimpleMatrix rotation, SimpleMatrix translation, ScanPoint scan){
        this.rotation = rotation;
        this.translation = translation;
        this.scan = scan;
    }
 }
--- a/src/ScanGraph/ScanMatcher.java
+++ b/src/ScanGraph/ScanMatcher.java
@@ -1,302 +0,0 @@
 package ScanGraph;
 import Vector.Vector;
 import org.ejml.simple.SimpleMatrix;
 import org.ejml.simple.SimpleSVD;
 import java.util.ArrayList;
 import static java.lang.Math.abs;
 /**
 * @brief A class that can match two point scans together
 */
 public class ScanMatcher{
    // A 2x2 matrix describing a rotation to apply to the new scan
    public SimpleMatrix rotationMatrix = null;
    // A 2x1 matrix describing a translation to apply to the new scan
    public SimpleMatrix translationVector = null;
    public ScanMatcher(){
    }
    /**
     * @brief iteratively calculate new rotation and transpose matrices to determien if the two scans match
     * @param referenceScan the scan to be referenced
     * @param newScan the scan that will be rotated and moved until it matches the reference scan
     * @param iterations The number of iterations that the scan matcher will attempt
     * @param errorThreshold The error threshold that the match will have to meet before considering it a valid match
     */
    public ScanPoint iterativeScanMatch(ScanPoint referenceScan, ScanPoint newScan, float errorThreshold, int iterations){
        // make a copy of the new scan so we don't modify the original
        ScanPoint scanBeingMatched = new ScanPoint(newScan);
        // calculate the rotation and translation matrices between the two scans
        this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched);
        SimpleMatrix cumulativeRotationMatrix = new SimpleMatrix(this.rotationMatrix);
        SimpleMatrix cumulativeTranslationVector = new SimpleMatrix(this.translationVector);
        // iterate through the scan matching algorithm
        for (int i = 0; i < iterations; i++) {
            // calculate the rotation and translation matrices between the two scans
            this.calculateRotationAndTranslationMatrices(referenceScan, scanBeingMatched);
            // apply the rotation and translation matrices to the new scan
            scanBeingMatched = this.applyRotationAndTranslationMatrices(scanBeingMatched);
            // calculate the error between the new scan and the reference scan
            float error = this.getError(referenceScan, scanBeingMatched);
            // if the error is less than the error threshold, then we have a valid match
            if(error < errorThreshold){
                this.rotationMatrix = cumulativeRotationMatrix;
                this.translationVector = cumulativeTranslationVector;
                return scanBeingMatched;
            }
            // otherwise, we need to keep iterating
            // add the rotation and translation matrices to the cumulative rotation and translation matrices
            cumulativeRotationMatrix = cumulativeRotationMatrix.mult(this.rotationMatrix);
            cumulativeTranslationVector = cumulativeTranslationVector.plus(this.translationVector);
        }
        // if we get to this point, then we have not found a valid match
        return null;
    }
    /**
     * @brief Compute the cross covariance matrix between the new scan and the reference scan
     * @return a 2x2 matrix containing the cross covariance matrix
     */
    private SimpleMatrix crossCovarianceMatrix(ScanPoint referenceScan, ScanPoint newScan, CorrespondenceMatrix correspondenceMatrix){
        Vector referenceScanAveragePosition = correspondenceMatrix.getAverageOldPosition();
        Vector newScanAveragePosition = correspondenceMatrix.getAverageNewPosition();
        // compute the cross covariance matrix which is given by the formula:
        // covariance = the sum from 1 to N of (p_i) * (q_i)^T
        // where p_i is the ith point in the new scan and q_i is the ith point in the reference scan and N is the number of points in the scan
        // the cross covariance matrix is a 2x2 matrix
        float[][] crossCovarianceMatrix = new float[2][2];
        for (int i = 0; i < correspondenceMatrix.getOldPointIndices().size(); i++) {
            int oldIndex = correspondenceMatrix.getOldPointIndices().get(i);
            int newIndex = correspondenceMatrix.getNewPointIndices().get(i);
            Vector oldPoint = referenceScan.getPoints().get(oldIndex);
            Vector newPoint = newScan.getPoints().get(newIndex);
            if (oldPoint != null && newPoint != null) {
                Vector oldPointOffset = oldPoint.sub(referenceScanAveragePosition);
                Vector newPointOffset = newPoint.sub(newScanAveragePosition);
                crossCovarianceMatrix[0][0] += oldPointOffset.x * newPointOffset.x;
                crossCovarianceMatrix[0][1] += oldPointOffset.x * newPointOffset.y;
                crossCovarianceMatrix[1][0] += oldPointOffset.y * newPointOffset.x;
                crossCovarianceMatrix[1][1] += oldPointOffset.y * newPointOffset.y;
            }
        }
        return new SimpleMatrix(crossCovarianceMatrix);
    }
    /**
     * @brief Compute the rotation and translation matrices between the new scan and the reference scan. Then cache them as private variables.
     * The rotation matrix is a 2x2 matrix and the translation vector is a 2x1 matrix
     */
    public void calculateRotationAndTranslationMatrices(ScanPoint referenceScan, ScanPoint newScan){
        CorrespondenceMatrix correspondenceMatrix = new CorrespondenceMatrix(newScan, referenceScan);
        // compute the rotation matrix which is given by the formula:
        // R = V * U^T
        // where V and U are the singular value decomposition of the cross covariance matrix
        // the rotation matrix is a 2x2 matrix
        SimpleMatrix crossCovarianceMatrixSimple = crossCovarianceMatrix(referenceScan, newScan, correspondenceMatrix);
        SimpleSVD<SimpleMatrix> svd = crossCovarianceMatrixSimple.svd();
        this.rotationMatrix = svd.getU().mult(svd.getV().transpose());
        // calculate what the angle of the rotation matrix is
        float angle = (float) Math.atan2(this.rotationMatrix.get(1, 0), this.rotationMatrix.get(0, 0));
        // scale the angle by a small amount to make the rotation matrix more accurate
        angle*= 1.75F;
        this.rotationMatrix = new SimpleMatrix(new double[][]{{Math.cos(angle), -Math.sin(angle)}, {Math.sin(angle), Math.cos(angle)}});
        // percentage of the local position to use in the translation calculation
        double weightedAverage = 0.9;
        SimpleMatrix localNewScanAveragePosition = new SimpleMatrix(correspondenceMatrix.getAverageNewPosition().toArray());//this.averageScanPosition(newScan);
        SimpleMatrix globalNewScanAveragePosition = new SimpleMatrix(this.averageScanPosition(newScan));
        SimpleMatrix weightedAverageNewScanPostion = localNewScanAveragePosition.scale(weightedAverage).plus(globalNewScanAveragePosition.scale(1-weightedAverage));
        SimpleMatrix localReferenceScanAveragePosition = new SimpleMatrix(correspondenceMatrix.getAverageOldPosition().toArray()); //this.averageScanPosition(referenceScan);
        SimpleMatrix globalReferenceScanAveragePosition = new SimpleMatrix(this.averageScanPosition(referenceScan));
        SimpleMatrix weightedAverageReferenceScanPostion = localReferenceScanAveragePosition.scale(weightedAverage).plus(globalReferenceScanAveragePosition.scale(1-weightedAverage));
        this.translationVector = weightedAverageReferenceScanPostion.minus(rotationMatrix.mult(weightedAverageNewScanPostion));
    }
    public SimpleMatrix getRotationMatrix(){
        return this.rotationMatrix;
    }
    public SimpleMatrix getTranslationVector(){
        return this.translationVector;
    }
    public ScanPoint applyRotationAndTranslationMatrices(ScanPoint newScan){
        // copy the new scan so we don't modify the original
        ScanPoint tempScan = new ScanPoint(newScan);
        // apply the rotation matrix and translation vector to the new scan
        for (int i = 0; i < tempScan.getPoints().size(); i++) {
            Vector point = tempScan.getPoints().get(i);
            if (point != null) {
                SimpleMatrix pointMatrix = new SimpleMatrix(point.toArray());
                SimpleMatrix newPointMatrix = rotationMatrix.mult(pointMatrix).plus(translationVector);
                tempScan.getPoints().set(i, new Vector((float) newPointMatrix.get(0), (float) newPointMatrix.get(1)));
            }
        }
        newScan.UpdatePose(rotationMatrix, translationVector);
        return tempScan;
    }
    /**
     * @brief Compute the average position of the scan
     * @param scan the scan to compute the average position of
     * @return a 2x1 matrix containing the x,y coordinates of the average position of the scan
     */
    private SimpleMatrix averageScanPosition(ScanPoint scan){
        Vector averagePosition = new Vector(0, 0);
        int invalidPoints = 0;
        for (Vector point : scan.getPoints()) {
            if (point != null) {
                averagePosition = averagePosition.add(point);
            }
            else{
                invalidPoints++;
            }
        }
        return new SimpleMatrix(averagePosition.div(scan.getPoints().size() - invalidPoints).toArray());
    }
    public float getError(ScanPoint referenceScan, ScanPoint newScan){
        // calculate the error between the new scan and the reference scan
        // q is reference scan and p is new scan
        // error is given as abs(Q_mean - R * P_mean)
        // where Q_mean is the average position of the reference scan
        // P_mean is the average position of the new scan
        // R is the rotation matrix
        SimpleMatrix newScanAveragePosition = averageScanPosition(newScan);
        SimpleMatrix referenceScanAveragePosition = averageScanPosition(referenceScan);
        SimpleMatrix error = referenceScanAveragePosition.minus(rotationMatrix.mult(newScanAveragePosition));
        return (float) abs(error.elementSum());
    }
 }
 /**
 * @brief A class to hold the correspondence matrix between two scans
 * The correspondence matrix is a 3xN matrix where N is the number of valid points in the scan.
 * This calculates the closest point in the old scan for each point in the new scan and gets rid of redundant closest points.
 */
 class CorrespondenceMatrix{
    private ArrayList<Integer> oldPointIndices = new ArrayList<>();
    private ArrayList<Integer> newPointIndices = new ArrayList<>();
    private ArrayList<Float> distances = new ArrayList<>();
    private Vector averageOldPosition = new Vector(0, 0);
    private Vector averageNewPosition = new Vector(0, 0);
    CorrespondenceMatrix(ScanPoint newScan, ScanPoint oldScan){
        this.calculateCorrespondenceMatrix(newScan, oldScan);
        this.calculateAveragePositions(newScan, oldScan);
    }
    public ArrayList<Integer> getOldPointIndices(){
        return this.oldPointIndices;
    }
    public ArrayList<Integer> getNewPointIndices(){
        return this.newPointIndices;
    }
    public ArrayList<Float> getDistances(){
        return this.distances;
    }
    public Vector getAverageOldPosition(){
        return this.averageOldPosition;
    }
    public Vector getAverageNewPosition(){
        return this.averageNewPosition;
    }
    private void calculateAveragePositions(ScanPoint newScan, ScanPoint oldScan){
        int invalidPoints = 0;
        for (int i = 0; i < this.oldPointIndices.size(); i++){
            int oldIndex = this.oldPointIndices.get(i);
            int newIndex = this.newPointIndices.get(i);
            Vector oldPoint = oldScan.getPoints().get(oldIndex);
            Vector newPoint = newScan.getPoints().get(newIndex);
            if (oldPoint != null && newPoint != null) {
                this.averageOldPosition = this.averageOldPosition.add(oldPoint);
                this.averageNewPosition = this.averageNewPosition.add(newPoint);
            }
            else{
                invalidPoints++;
            }
        }
        this.averageOldPosition = this.averageOldPosition.div(this.oldPointIndices.size() - invalidPoints);
        this.averageNewPosition = this.averageNewPosition.div(this.newPointIndices.size() - invalidPoints);
    }
    /**
     * @brief Calculate the correspondence matrix between two scans
     * @param newScan the new scan
     * @param referenceScan the reference scan
     */
    private void calculateCorrespondenceMatrix(ScanPoint newScan, ScanPoint referenceScan) {
        for (int newPointIndex = 0; newPointIndex < newScan.getPoints().size(); newPointIndex++) {
            Vector newPoint = newScan.getPoints().get(newPointIndex);
            // Skip null points in the new scan
            if (newPoint == null) {
                continue;
            }
            float closestDistance = Float.MAX_VALUE;
            int closestIndex = -1;
            for (int oldPointIndex = 0; oldPointIndex < referenceScan.getPoints().size(); oldPointIndex++) {
                Vector oldPoint = referenceScan.getPoints().get(oldPointIndex);
                // Skip null points in the old scan
                if (oldPoint == null) {
                    continue;
                }
                float distance = newPoint.sub(oldPoint).mag();
                if (distance < closestDistance) {
                    closestDistance = distance;
                    closestIndex = oldPointIndex;
                }
            }
            // if we find a closest point...
            if (closestIndex != -1) {
                // check if the oldPointIndex is already in the list of oldPointIndices
                if(this.oldPointIndices.contains(closestIndex)){
                    int index = this.oldPointIndices.indexOf(closestIndex);
                    // if the index is already in our list, then we need to check if the new point is closer than the old point
                    if(this.distances.get(index) > closestDistance){
                        // if the new point is closer than the old point, then we need to replace the old point with the new point
                        this.oldPointIndices.set(index, closestIndex);
                        this.newPointIndices.set(index, newPointIndex);
                        this.distances.set(index, closestDistance);
                    }
                }
                // if the index is not in our list, then we need to add it
                else{
                    this.oldPointIndices.add(closestIndex);
                    this.newPointIndices.add(newPointIndex);
                    this.distances.add(closestDistance);
                }
            }
        }
    }
 }
--- a/src/ScanGraph/ScanPoint.java
+++ b/src/ScanGraph/ScanPoint.java
@@ -1,68 +0,0 @@
 package ScanGraph;
 import Graph.Vertex;
 import Vector.Vector;
 import org.ejml.simple.SimpleMatrix;
 import java.util.ArrayList;
 public class ScanPoint extends Vertex{
    private Vector position;
    private float orientation;
    private ArrayList<Vector> scan;
    public ScanPoint(Vector scanPosition, float orientation, ArrayList<Vector> scan) {
        super();
        this.position = scanPosition;
        this.orientation = orientation;
        this.scan = scan;
    }
    /**
     * @brief Copy constructor
     * @param other The scan point to copy
     */
    public ScanPoint(ScanPoint other){
        super();
        this.position = new Vector(other.getPos().x, other.getPos().y);
        this.orientation = other.getAngle();
        this.scan = new ArrayList<>(other.getPoints());
    }
    /**
     * @return a two eleement float array containing the x and y coordinates of the vertex respectively.
     */
    public Vector getPos(){
        return position;
    }
    public void setPos(Vector pos){
        this.position = pos;
    }
    public float getAngle(){
        return this.orientation;
    }
    public ArrayList<Vector> getPoints(){
        return this.scan;
    }
    /**
     * @brief Update the pose of the scan point
     * @param rotation The rotation matrix to apply to the scan point
     * @param translation The translation matrix to apply to the scan point
     */
    public void UpdatePose(SimpleMatrix rotation, SimpleMatrix translation){
        SimpleMatrix pose = new SimpleMatrix(3,1);
        pose.set(0,0, this.position.x);
        pose.set(1,0, this.position.y);
        pose.set(2,0, this.orientation);
        SimpleMatrix newPose = translation.plus(rotation.mult(pose));
        this.position.x = (float)newPose.get(0,0);
        this.position.y = (float)newPose.get(1,0);
        this.orientation = (float)newPose.get(2,0);
    }
 }
--- a/src/ShortTermMem.java
+++ b/src/ShortTermMem.java
@@ -1,59 +0,0 @@
 import java.util.ArrayList;
 import Vector.Vector;
 public class ShortTermMem {
    ArrayList<ArrayList<Vector>> scans = new ArrayList<>();
    ArrayList<Vector> scanPositions = new ArrayList<>();
    ArrayList<Long> scanTimes = new ArrayList<>();
    private int size = 0;
    public void addScan(Vector scanPosition, ArrayList<Vector> scan){
        size += scan.size();
        scans.add(scan);
        scanPositions.add(scanPosition);
        scanTimes.add(System.currentTimeMillis());
        purgeScans();
    }
    public ArrayList<Vector> getPoints(){
        ArrayList<Vector> points = new ArrayList<>();
        for(ArrayList<Vector> pointList : this.scans){
            points.addAll(pointList);
        }
        return points;
    }
    public void remove(Vector point){
        for(ArrayList<Vector> pointList : this.scans){
            int listSize = pointList.size();
            pointList.remove(point);
            if(listSize - pointList.size() != 0){
                size--;
                break;
            }
        }
    }
    private void purgeScans(){
        long currentTime = System.currentTimeMillis();
        int i = scanTimes.size();
        // loop through the list backwards and remove all scans that are over second old
        // we loop backwards to avoid removal conflicts
        while(i > 0){
            i--;
            long dt = currentTime - scanTimes.get(i);
            if(dt < 1000){
                continue;
            }
            size -= scans.get(i).size();
            scanTimes.remove(i);
            scanPositions.remove(i);
            scans.remove(i);
        }
    }
    public int size(){
        return this.size;
    }
 }
--- a/src/Vector.java
+++ b/src/Vector.java
@@ -0,0 +1,74 @@
 import static java.lang.Math.acos;
 import static java.lang.Math.sqrt;
 public class Vector {
    public float x = 0;
    public float y = 0;
    public float z = 0;
    Vector(){}
    Vector(float x, float y){
        this.x = x;
        this.y = y;
    }
    Vector(float x, float y, float z){
        this.x = x;
        this.y = y;
        this.z = z;
    }
    Vector add(Vector other){
        return new Vector(this.x + other.x, this.y + other.y, this.z + other.z);
    }
    Vector add(float x, float y){
        return new Vector(this.x + x, this.y + y);
    }
    Vector add(float x, float y, float z){
        return new Vector(this.x + x, this.y + y, this.z + z);
    }
    Vector sub(Vector other){
        return new Vector(this.x - other.x, this.y - other.y, this.z - other.z);
    }
    Vector sub(float x, float y){
        return new Vector(this.x - x, this.y - y);
    }
    Vector sub(float x, float y, float z){
        return new Vector(this.x - x, this.y - y, this.z - z);
    }
    Vector mul(float scalar){
        return new Vector(this.x * scalar, this.y * scalar, this.z * scalar);
    }
    Vector div(float scalar){
        return mul(1/scalar);
    }
    float mag(){
        return (float)sqrt(x*x + y*y + z*z);
    }
    float dot(Vector other){
        return x * other.x + y * other.y + z * other.z;
    }
    Vector cross(Vector other){
        return new Vector(this.y*other.z - this.z*other.y, this.z*other.x - this.x*other.z, this.x*other.y - this.y*other.x);
    }
    Vector normalize(){
        float mag = this.mag();
        return new Vector(x / mag, y / mag, z / mag);
    }
    float angleDiff(Vector other){
        float dot = this.dot(other);
        return (float)acos(dot / (this.mag() * other.mag()));
    }
 }
--- a/src/Vector/Line.java
+++ b/src/Vector/Line.java
@@ -1,114 +0,0 @@
 package Vector;
 import Vector.Vector;
 import processing.core.PApplet;
 import java.util.ArrayList;
 import java.util.List;
 import static processing.core.PApplet.*;
 public class Line implements LineInterface{
    // vector which represents  the direction and length of the line from its starting position
    protected Vector direction = new Vector(0,0);
    // store the starting position of the line
    protected Vector position = new Vector(0,0);
    protected ArrayList<Vector> points = new ArrayList<>();
    public Line(Vector startPosition, Vector endPosition){
        direction = endPosition.sub(startPosition);
        position = startPosition;
    }
    /**
     * attempt to find the line of best fit for the given points
     * @param points the points to get the line of best for
     */
    public Line(List<Vector> points){
        bestFit(points);
    }
    public void refitLine(Vector newPoint){
        // add the new point to our list
        this.points.add(newPoint);
        // rerun the bestFit algorithm with the new point
        bestFit(new ArrayList<>());
    }
    // least squares line of best fit algorithm
    public void bestFit(List<Vector> fitPoints){
        this.points.addAll(fitPoints);
        // get the mean of all the points
        Vector mean = new Vector();
        for(Vector point : points){
            mean = mean.add(point);
        }
        mean = mean.div(points.size());
        // this section calculates the direction vector of the line of best fit
        Vector direction = new Vector();
        float length = 0;
        // get the rise and run of the line of best fit
        for(Vector point : points){
            direction.y += (point.x - mean.x)*(point.y - mean.y); // rise
            direction.x += pow((point.x - mean.x),2);
            // get the average distance avery point is away from the mean.
            float dist = abs(point.sub(mean).mag());
            length += dist;
        }
        length = 2f*length/points.size();
        // if the direction is perfectly vertical create a line to represent that.
        if(direction.y == 0){
            this.direction = new Vector(0, 1);
        }
        else{
            this.direction = new Vector(1, direction.y/direction.x);
        }
        // scale the direction vector to be the correct length of the line.
        this.direction = this.direction.normalize().mul(length);
        this.position = mean.sub(this.direction.div(2));
    }
    public Vector getSlopeIntForm(){
        float slope = direction.y / direction.x;
        float intercept = position.y - slope * position.x;
        return new Vector(slope, intercept);
    }
    public Vector getDirection(){
        return direction;
    }
    public Vector getPosition(){
        return position;
    }
    public float getLength(){
        return direction.mag();
    }
    public float getAngle(){return atan2(this.direction.y, this.direction.x);}
    public Vector endPoint(){
        return this.position.add(this.direction);
    }
    public float getDistance(Vector point){
        float line_dist = direction.mag();
        if(line_dist == 0) return this.position.sub(point).mag();
        Vector l2 = this.endPoint();
        float t = ((point.x - position.x) * (l2.x - position.x) + (point.y - position.y) * (l2.y - position.y) + (point.z - position.z) * (l2.z - position.z)) / line_dist;
        t = constrain(t, 0, 1);
        Vector closestPoint = new Vector(position.x + t * (l2.x - position.x), position.y + t * (l2.y - position.y), position.z + t * (l2.z - position.z));
        return closestPoint.sub(point).mag();
    }
    public void draw(PApplet proc){
        proc.line(position.x, position.y, endPoint().x, endPoint().y);
    }
 }
--- a/src/Vector/LineInterface.java
+++ b/src/Vector/LineInterface.java
@@ -1,20 +0,0 @@
 package Vector;
 import processing.core.PApplet;
 public interface LineInterface {
    Vector getDirection();
    Vector getPosition();
    float getLength();
    float getAngle();
    Vector endPoint();
    float getDistance(Vector point);
    void draw(PApplet proc);
 }
--- a/src/Vector/Vector.java
+++ b/src/Vector/Vector.java
@@ -1,135 +0,0 @@
 package Vector;
 import org.ejml.simple.SimpleMatrix;
 import processing.core.PApplet;
 import static java.lang.Math.*;
 import static processing.core.PApplet.cos;
 import static processing.core.PApplet.sin;
 public class Vector {
    public float x = 0;
    public float y = 0;
    public float z = 0;
    Vector(){}
    public Vector(float x, float y){
        this.x = x;
        this.y = y;
    }
    public Vector(float x, float y, float z){
        this.x = x;
        this.y = y;
        this.z = z;
    }
    public Vector(double x, double y){
        this.x = (float)x;
        this.y = (float)y;
    }
    public Vector(double x, double y, double z){
        this.x = (float)x;
        this.y = (float)y;
        this.z = (float)z;
    }
    public Vector(SimpleMatrix matrix){
        // initialize x,y if matrix is 2x1 and x,y,z if matrix is 3x1
        if(matrix.getNumRows() == 2){
            this.x = (float)matrix.get(0,0);
            this.y = (float)matrix.get(1,0);
        }else if(matrix.getNumRows() == 3){
            this.x = (float)matrix.get(0,0);
            this.y = (float)matrix.get(1,0);
            this.z = (float)matrix.get(2,0);
        }
    }
    public Vector add(Vector other){
        return new Vector(this.x + other.x, this.y + other.y, this.z + other.z);
    }
    public Vector add(float x, float y){
        return new Vector(this.x + x, this.y + y);
    }
    public Vector add(float x, float y, float z){
        return new Vector(this.x + x, this.y + y, this.z + z);
    }
    public Vector sub(Vector other){
        return new Vector(this.x - other.x, this.y - other.y, this.z - other.z);
    }
    public Vector sub(float x, float y){
        return new Vector(this.x - x, this.y - y);
    }
    public Vector sub(float x, float y, float z){
        return new Vector(this.x - x, this.y - y, this.z - z);
    }
    public Vector mul(float scalar){
        return new Vector(this.x * scalar, this.y * scalar, this.z * scalar);
    }
    public Vector div(float scalar){
        return mul(1/scalar);
    }
    public float mag(){
        return (float)sqrt(x*x + y*y + z*z);
    }
    public float dot(Vector other){
        return x * other.x + y * other.y + z * other.z;
    }
    public Vector cross(Vector other){
        return new Vector(this.y*other.z - this.z*other.y, this.z*other.x - this.x*other.z, this.x*other.y - this.y*other.x);
    }
    public Vector normalize(){
        float mag = this.mag();
        return new Vector(x / mag, y / mag, z / mag);
    }
    /**
     * @param other
     * @return
     */
    public float angleDiff(Vector other){
        float dot = this.dot(other);     // dot product
        float det = this.x*other.y - this.y*other.x;    // determinant
        float angle = (float) atan2(det, dot);  // atan2(y, x) or atan2(sin, cos)
        return angle;
    }
    /**
     * @return The angle of the vector in radians
     */
    public float angle(){
        return (float) atan2(y, x);
    }
    /**
     * @brief Rotate a 2D vector by a given angle
     * @param angle The angle to rotate the vector by in radians
     * @return The rotated vector
     */
    public Vector rotate2D(float angle){
        float distance = mag();
        float currentAngle = this.angle();
        return new Vector(cos(currentAngle + angle), sin(currentAngle + angle)).mul(distance);
    }
    public void draw(PApplet proc){
        proc.circle(this.x, this.y, 8);
    }
    public float[] toArray() {
        return new float[]{x, y};
    }
 }
--- a/src/View.java
+++ b/src/View.java
@@ -1,141 +1,168 @@
 import Graph.PointGraph;
 import Vector.Vector;
 import processing.core.*;
 import java.util.ArrayList;
-import ScanGraph.ScanPoint;
+import java.util.Objects;
-public class View {
+import static processing.core.PApplet.*;
-    Vector position;
+
 public class View{
    Vector pose;
    float angle = 0;
    float FOV;
    ArrayList<Ray> rays = new ArrayList<>();
    private static PApplet proc;
-    /**
+    //the x,y position of the view, what angle it's looking at and its FOV
-     * @brief Constructor for the View class
+    View(PApplet processing, Vector newPose, int numberOfRays, float FOV){
     * @param processing The PApplet that the view will be drawn on
     * @param newPose The position of the view
     * @param numberOfRays The number of rays that the view will have
     * @param FOV The field of view of the view
     */
    View(PApplet processing, Vector newPose, int numberOfRays, float FOV) {
        proc = processing;
-        this.position = newPose;
+        this.pose = newPose;
        this.FOV = FOV;
        this.setRayNum(numberOfRays, FOV, this.angle);
    }
    //sets the number of rays and their starting values in the ray list
-    public void setRayNum(int numberOfRays, float FOV, float angleOffset) {
+    public void setRayNum(int numberOfRays, float FOV, float angleOffset){
-        float rayStep = FOV / numberOfRays;
+        float rayStep = FOV/numberOfRays;
        rays.clear();
-        float angle = (float) (angleOffset); //the 0.01 fixes some bugs
+        float angle =  (float)(0.01-angleOffset); //the 0.01 fixes some bugs
-        for (int i = 0; i < numberOfRays; i++) {
+        for(int i = 0; i < numberOfRays; i++){
-            Ray ray = new Ray(position, angle);
+            Ray ray = new Ray(proc, pose, 100000, angle);
            angle = angle + rayStep;
            rays.add(ray);
        }
    }
-    /**
+    //sees if the ray will collide with the walls in the wall list
-     * @brief Calculates the points of intersection of the rays with the map
+    public void look(ArrayList<Wall> walls){
-     * @param map The map that the view is looking at
+        for (Ray ray : rays){
-     */
+            ray.castRay(walls);
-    public void calculatePointScan(PointGraph map) {
+            ray.drawRay();
        for (Ray ray : rays) {
            ray.castRay(map);
            if(ray.hasCollided()){
                ray.getPoint().draw(proc);
            }
        }
    }
-    /**
+    //changes the position of the view
-     * @brief Sets the position of the view
+    public void setPos(Vector newPose){
-     * @param newPosition The new position of the view
+        pose = newPose;
-     */
+        for(Ray ray : rays){ray.setPos(pose);}
    public void setPos(Vector newPosition) {
        position = newPosition;
        for (Ray ray : rays) {
            ray.setPos(position);
        }
    }
-    /**
+    //changes the angle of the view
-     * @brief Sets the angle of the view
+    public void setAngle(float angle){
-     * @param newAngle The new angle of the view
+        this.angle = angle;
-     */
+        this.setRayNum(rays.size(), this.FOV, angle);
    public void setAngle(float newAngle) {
        this.angle = newAngle;
        for(Ray ray : rays){
            float angleOffset = ray.getAngle() - this.angle;
            ray.setAngle(this.angle+angleOffset);
        }
    }
    //changes the field of view of the view
-    public void setFOV(float FOV) {
+    public void setFOV(float FOV){
        this.FOV = FOV;
        this.setRayNum(this.rays.size(), this.FOV, this.angle);
    }
-    /**
+    public Vector getPos(){return pose;}
     * @return The position of the view
     */
    public Vector getPos() {
        return position;
    }
-    /**
+    public float getAngle(){return this.angle;}
     * @return The angle of the view
     */
    public float getAngle() {
        return this.angle;
    }
-    /**
+    public float getFOV(){return this.FOV;}
     * @return The field of view of the view
     */
    public float getFOV() {
        return this.FOV;
    }
-    /**
+    public int getRayNum(){return this.rays.size();}
     * @return The number of rays that the view has
     */
    public int getRayNum() {
        return this.rays.size();
    }
-    /**
+    //gets the point that each ray has collided with
-     * @brief Get the most recent scan from the view
+    public ArrayList<Vector> getPoints(){
     * @return A ScanPoint object containing the position, angle and points of the view
     */
    public ScanPoint getScan() {
        ArrayList<Vector> points = new ArrayList<>();
-        for (Ray ray : rays) {
+        for(Ray ray : rays){
-            if(ray.hasCollided()){
+            if(!Objects.equals(ray.getPoint(), new Vector(0, 0) {
-                Vector point = ray.getPoint();
+            })){
-                // store the angle information for that point in the z coordinate
+                points.add(ray.getPoint());
                point.z = ray.getAngle();
                points.add(point);
            }
        }
-        return new ScanPoint(this.position,this.angle, points);
+        return points;
    }
    /**
     * @return A list of the angles where a collision was detected
     */
    public ArrayList<Float> getAngles(){
        ArrayList<Float> angles = new ArrayList<>();
        for (Ray ray : rays) {
            if (ray.hasCollided()){
                angles.add(ray.getAngle());
            }
        }
        return angles;
    }
 }
 class Ray{
    Vector pose;
    int rayLength;
    int defaultRayLength;
    float angle; // IN RADIANS
    private static PApplet proc;
    //takes the starting position of the ray, the length of the ray, and it's casting angle (radians)
    Ray(PApplet processing, Vector position, int defaultRayLength, float angle){
        proc = processing;
        this.pose = position;
        this.defaultRayLength = defaultRayLength;
        this.rayLength = defaultRayLength;
        this.angle = angle;
    }
    public void drawRay(){
        proc.line(pose.x, pose.y, (pose.x + cos(angle)*rayLength), (pose.y + sin(angle)*rayLength));
    }
    //checks to see at what coordinate the ray will collide with an object and sets the ray length to meet that point.
    public void castRay(ArrayList<Wall> objects){
        this.rayLength = defaultRayLength;
        ArrayList<Integer> distances = new ArrayList<>();
        //sees what objects it collides with
        for(Wall object : objects){
            float theta1 = angle;
            float theta2 = radians(object.getAngle());
            Vector wallPos = object.getPos();
            //finds where along the wall the ray collides
            float b = (pose.x*sin(theta1) + wallPos.y*cos(theta1) - pose.y*cos(theta1) - wallPos.x*sin(theta1)) / (cos(theta2)*sin(theta1) - sin(theta2)*cos(theta1));
            //if the place along the wall is further away than the wall extends, then it didn't collide
            if(b < object.getLength() && b > 0){
                //finds the length of the ray needed to collide with the wall
                float a = (b*sin(theta2) + wallPos.y-pose.y) / sin(theta1);
                //add that length to a list
                if(a > 0){
                    distances.add((int)abs(a));
                }
            }
        }
        //finds the shortest distance and sets the length of the ray to that distance
        if(distances.size() > 0){
            for(Integer distance : distances){
                if(distance < rayLength){
                    rayLength = distance;
                }
            }
        }
        else this.rayLength = defaultRayLength;
    }
    public Vector getPos(){ return pose;}
    public int getRayLength(){return this.rayLength;}
    public float getAngle(){return this.angle;}
    public boolean hasCollided(){
        return this.defaultRayLength != this.rayLength;
    }
    //returns the absolute position of the point
    public Vector getPoint(){
        if(this.rayLength != this.defaultRayLength){
            return new Vector(rayLength * (int)cos(this.angle) + pose.x, rayLength * (int)sin(this.angle) + pose.y);
        }
        else{
            return new Vector(0,0);
        }
    }
    public void setPos(Vector newPose){
        pose = newPose;
    }
    public void setRayLength(int rayLength){this.rayLength = rayLength;}
    public void setDefaultRayLength(int defaultRayLength){this.defaultRayLength = defaultRayLength;}
    public void setAngle(float angle){this.angle = angle;}
 }
--- a/src/Wall.java
+++ b/src/Wall.java
@@ -0,0 +1,41 @@
 import processing.core.*;
 import static processing.core.PApplet.*;
 public class Wall{
    Vector pos;
    float angle;
    int wallLength;
    private static PApplet proc;
    int r;
    int g;
    int b;
    Wall(PApplet processing, Vector pos, float angle, int wallLength){
        proc = processing;
        this.pos = pos;
        this.angle = angle;
        this.wallLength = wallLength;
        r = (int)proc.random(50, 255);
        g = (int)proc.random(50, 255);
        b = (int)proc.random(50, 255);
    }
    void drawWall(){
        proc.stroke(r,g,b);
        proc.line(pos.x, pos.y, (pos.x + cos(radians(angle))*wallLength), (pos.y + sin(radians(angle))*wallLength));
        //ellipse((xPos + cos(radians(angle))*wallLength), (yPos + sin(radians(angle))*wallLength), 20, 20);
    }
    Vector getPos(){
        return pos;
    }
    float getAngle(){
        return angle;
    }
    int getLength(){
        return wallLength;
    }
 }
--- a/tests/LineTest.java
+++ b/tests/LineTest.java
@@ -1,9 +1,8 @@
 import Vector.Vector;
 import Vector.Line;
 import org.junit.jupiter.api.Test;
 import processing.core.PApplet;
 import java.util.ArrayList;
-import java.util.Random;
+import java.util.List;
 import static java.lang.Math.abs;
 import static org.junit.jupiter.api.Assertions.*;
@@ -32,34 +31,6 @@ class LineTest{
        // make sure the line is starting in the correct place
        assertFloatEquals(v1.x, line.getPosition().x);
        assertFloatEquals(v1.y, line.getPosition().y);
        // make sure the line ends in the correct place
        assertFloatEquals(v2.x, line.endPoint().x);
        assertFloatEquals(v2.y, line.endPoint().y);
        Random rand = new Random();
        // repeat this test with 100 random lines
        for(int i = 0; i < 100; i++){
            v1 = new Vector(rand.nextInt(1001), rand.nextInt(1001));
            v2 = new Vector(rand.nextInt(1001), rand.nextInt(1001));
            line = new Line(v1, v2);
            // make sure the line is pointing in the right direction
            lineDirection = v2.sub(v1);
            float angleDiff = lineDirection.angleDiff(line.getDirection());
            assertFloatEquals(0, angleDiff, 0.001f);
            // make sure the line is the correct length
            assertFloatEquals(lineDirection.mag(), line.getLength());
            // make sure the line is starting in the correct place
            assertFloatEquals(v1.x, line.getPosition().x);
            assertFloatEquals(v1.y, line.getPosition().y);
            // make sure the line ends in the correct place
            assertFloatEquals(v2.x, line.endPoint().x);
            assertFloatEquals(v2.y, line.endPoint().y);
        }
    }
    @Test
--- a/tests/MatcherVisualizer.java
+++ b/tests/MatcherVisualizer.java
@@ -1,123 +0,0 @@
 import ScanGraph.ScanMatcher;
 import ScanGraph.ScanPoint;
 import Vector.Vector;
 import processing.core.PApplet;
 import java.util.ArrayList;
 public class MatcherVisualizer extends PApplet{
    public static PApplet processing;
    ScanPoint referenceScan;
    ScanPoint scanToMatch;
    ScanPoint scanBeingMatched;
    public static void main(String[] args) {
        PApplet.main("MatcherVisualizer");
    }
    public void settings(){
        processing = this;
        size(1000, 1000);
        float connectingAngle = (float) (3 * Math.PI / 9);
        // generate two scans rotated by 45 degrees and append them together
        Vector descriptor = new Vector(200, 200);
        Vector position = new Vector(500, 500);
        ScanPoint scan1 = generateScanPoint(position, descriptor, 12);
        ScanPoint scan2 = generateScanPoint(position, descriptor.rotate2D(connectingAngle), 12);
        Vector p1 = scan1.getPoints().get(11);
        Vector p2 = scan2.getPoints().get(11);
        ScanPoint scan3 = generateScanPoint(p1, p2.sub(p1), 12);
        this.referenceScan = appendScanPoints(scan1, scan2);
        this.referenceScan = appendScanPoints(this.referenceScan, scan3);
        // generate two scans offset by some amount and rotated by 55 degrees and append them together
        Vector rotated = descriptor.rotate2D((float) Math.PI);
        Vector offset = new Vector(100, 150);
        ScanPoint scan4 = generateScanPoint(position.add(offset), rotated, 12);
        ScanPoint scan5 = generateScanPoint(position.add(offset), rotated.rotate2D(connectingAngle), 12);
        p1 = scan4.getPoints().get(11);
        p2 = scan5.getPoints().get(11);
        ScanPoint scan6 = generateScanPoint(p1, p2.sub(p1), 12);
        this.scanToMatch = appendScanPoints(scan4, scan5);
        this.scanToMatch = appendScanPoints(this.scanToMatch, scan6);
        this.scanBeingMatched = new ScanPoint(this.scanToMatch);
    }
    public void draw(){
        iterativeScanMatch();
 //        background(0);
    }
    /**
     * @brief Generate a scan point from a scan description
     * @param offset The offset of the scan point from the origin
     * @param scanDescription A vector which describes the length of the line and direction of the line
     * @return A scan point with the given offset and scan description
     */
    public static ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
        // generate a scan point with the given offset and scan description
        ArrayList<Vector> scan = new ArrayList<>();
        // divide the scan description by the number of points to allow us to scale it back up in the loop
        Vector directionVector = scanDescription.div(numPoints-1);
        for (int i = 0; i < numPoints; i++) {
            scan.add(offset.add(directionVector.mul(i)));
        }
        return new ScanPoint(new Vector(0, 0), 0, scan);
    }
    /**
     * @brief Append two scan points together
     * @param scan1 The first scan point to append
     * @param scan2 The second scan point to append
     * @return A scan point that is the combination of the two scan points
     */
    public static ScanPoint appendScanPoints(ScanPoint scan1, ScanPoint scan2){
        ArrayList<Vector> points = new ArrayList<>();
        points.addAll(scan1.getPoints());
        points.addAll(scan2.getPoints());
        return new ScanPoint(new Vector(0, 0), 0, points);
    }
    /**
     * @brief Draw a scan point to the screen
     * @param scan The scan point to draw
     * @param color The color to draw the scan point
     */
    public void drawScan(ScanPoint scan, int[] color) {
        processing.stroke(color[0], color[1], color[2]);
        processing.fill(color[0], color[1], color[2]);
        ArrayList<Vector> points = scan.getPoints();
        for (int i = 0; i < points.size() - 1; i++) {
            Vector point = points.get(i);
            processing.ellipse(point.x, point.y, 5, 5);
        }
    }
    public void iterativeScanMatch() {
        background(0);
        int[] red = {255, 0, 0};
        int[] green = {0, 255, 0};
        int[] blue = {0, 0, 255};
        drawScan(this.referenceScan, red);
        drawScan(this.scanToMatch, green);
        // do a single scan match and calculate the error
        ScanMatcher matcher = new ScanMatcher();
        matcher.calculateRotationAndTranslationMatrices(this.referenceScan, this.scanBeingMatched);
        this.scanBeingMatched = matcher.applyRotationAndTranslationMatrices(this.scanBeingMatched);
        float error = matcher.getError(this.referenceScan, this.scanBeingMatched);
        drawScan(this.scanBeingMatched, blue);
        // do an iterative scan match and calculate the error
 //        ScanPoint matchedScan = matcher.iterativeScanMatch(scan1, scan2, 0.01f, 10);
 //        float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
    }
 }
--- a/tests/ScanMatcherTest.java
+++ b/tests/ScanMatcherTest.java
@@ -1,129 +0,0 @@
 import ScanGraph.ScanMatcher;
 import ScanGraph.ScanPoint;
 import org.junit.Before;
 import org.junit.jupiter.api.BeforeEach;
 import processing.core.PApplet;
 import org.junit.jupiter.api.Test;
 import Vector.Vector;
 import java.util.ArrayList;
 import static org.junit.jupiter.api.Assertions.*;
 import static processing.core.PApplet.main;
 class ScanMatcherTest{
    /**
     * @brief Generate a scan point from a scan description
     * @param offset The offset of the scan point from the origin
     * @param scanDescription A vector which describes the length of the line and direction of the line
     * @return A scan point with the given offset and scan description
     */
    public ScanPoint generateScanPoint(Vector offset, Vector scanDescription, int numPoints){
        // generate a scan point with the given offset and scan description
        ArrayList<Vector> scan = new ArrayList<>();
        // divide the scan description by the number of points to allow us to scale it back up in the loop
        Vector directionVector = scanDescription.div(numPoints-1);
        for (int i = 0; i < numPoints; i++) {
            scan.add(offset.add(directionVector.mul(i)));
        }
        return new ScanPoint(new Vector(0, 0), 0, scan);
    }
    /**
     * @brief Append two scan points together
     * @param scan1 The first scan point to append
     * @param scan2 The second scan point to append
     * @return A scan point that is the combination of the two scan points
     */
    public ScanPoint appendScanPoints(ScanPoint scan1, ScanPoint scan2){
        ArrayList<Vector> points = new ArrayList<>();
        points.addAll(scan1.getPoints());
        points.addAll(scan2.getPoints());
        return new ScanPoint(new Vector(0, 0), 0, points);
    }
    @Test
    public void applyRotationAndTranslationMatrices() {
        // generate one scan that is level and another that is rotated 45 degrees.
        Vector scanDescription = new Vector(10, 0);
        ScanPoint referenceScan = generateScanPoint(new Vector(0, 0), scanDescription, 10);
        ScanPoint newScan = generateScanPoint(new Vector(0, 0), scanDescription.rotate2D((float) Math.PI / 4), 10);
        // calculate the rotation and translation matrices between the two scans
        ScanMatcher matcher = new ScanMatcher();
        matcher.calculateRotationAndTranslationMatrices(referenceScan, newScan);
        // apply the rotation and translation matrices to the new scan
        ScanPoint newScanWithRotationAndTranslation = matcher.applyRotationAndTranslationMatrices(newScan);
        // Get the first and last points of the new scan with rotation and translation and calculate the angle between them
        ArrayList<Vector> points = newScanWithRotationAndTranslation.getPoints();
        Vector firstPoint = points.get(0);
        Vector lastPoint = points.get(points.size() - 1);
        Vector rotatedDirection = lastPoint.sub(firstPoint);
        float angle = scanDescription.angleDiff(rotatedDirection);
        // The angle between the first and last points should be zero
        assertEquals(0, angle);
    }
    @Test
    public void getError() {
        // generate two scans that are the same. The error should be zero.
        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
        ScanMatcher matcher = new ScanMatcher();
        matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
        assertEquals(0, matcher.getError(scan1, scan2));
        // generate two scans that are the same but one is offset by 10 in the y direction. The error should be 10.
        scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
        scan2 = generateScanPoint(new Vector(0, 10), new Vector(10, 10), 12);
        matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
        assertEquals(10, matcher.getError(scan1, scan2));
        // generate two scans that are the same but one is rotated by 45 degrees. The error should be near zero.
        scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
        scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10).rotate2D((float) Math.PI / 4), 12);
        matcher.calculateRotationAndTranslationMatrices(scan1, scan2);
        assertEquals(0, matcher.getError(scan1, scan2), 0.1);
    }
    @Test
    public void iterativeScanMatch() {
        float bendAngle = (float) (5 * Math.PI / 9);
        // generate two scans rotated by 45 degrees and append them together
        ScanPoint scan1 = generateScanPoint(new Vector(0, 0), new Vector(10, 10), 12);
        ScanPoint scan2 = generateScanPoint(new Vector(0, 0), new Vector(10, 10).rotate2D(bendAngle), 12);
        ScanPoint scan3 = appendScanPoints(scan1, scan2);
        // generate two scans offset by some amount and rotated by 55 degrees and append them together
        Vector rotated = (new Vector(10, 10)).rotate2D((float) Math.PI);
        ScanPoint scan4 = generateScanPoint(new Vector(10, 10), rotated, 12);
        ScanPoint scan5 = generateScanPoint(new Vector(10, 10), rotated.rotate2D(bendAngle), 12);
        ScanPoint scan6 = appendScanPoints(scan4, scan5);
        // do a single scan match and calculate the error
        ScanMatcher matcher = new ScanMatcher();
        matcher.calculateRotationAndTranslationMatrices(scan3, scan6);
        ScanPoint oneCalcMatch = matcher.applyRotationAndTranslationMatrices(scan6);
        float singleScanMatchError = matcher.getError(scan3, oneCalcMatch);
        // do an iterative scan match and calculate the error
        ScanPoint matchedScan = matcher.iterativeScanMatch(scan1, scan2, 0.0001f, 10);
        // if it's null something has gone wrong with the algorithm because these scans can easily be matched.
        assertNotNull(matchedScan);
        float iterativeScanMatchError = matcher.getError(scan1, matchedScan);
        // the iterative scan match should have a lower error than the single scan match
        assertTrue(iterativeScanMatchError < singleScanMatchError);
    }
 }
--- a/tests/VectorTest.java
+++ b/tests/VectorTest.java
@@ -1,5 +1,5 @@
 import Vector.Vector;
 import org.junit.jupiter.api.Test;
 import processing.core.PApplet;
 import static java.lang.Math.*;
 import static org.junit.jupiter.api.Assertions.*;
@@ -28,8 +28,8 @@ class VectorTest{
                assertFloatEquals((float)sqrt(x2*x2 + y2*y2), v2.mag());
                // test dot product
-                assertFloatEquals((x1*x2+y1*y2), v1.dot(v2));
+                assertFloatEquals((float)(x1*x2+y1*y2), v1.dot(v2));
-                assertFloatEquals((x1*x2+y1*y2), v2.dot(v1));
+                assertFloatEquals((float)(x1*x2+y1*y2), v2.dot(v1));
                // test addition
                Vector vSum = v1.add(v2);
		`@@ -0,0 +1,2 @@`
							`# SLAM-Sim`
							`SImulate SLAM algorithm using java and processing for visualizaiton.`