diff --git a/.idea/encodings.xml b/.idea/encodings.xml
new file mode 100644
index 0000000..15a15b2
--- /dev/null
+++ b/.idea/encodings.xml
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="Encoding" addBOMForNewFiles="with NO BOM" />
+</project>
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..84da703
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_8" project-jdk-name="1.8" project-jdk-type="JavaSDK">
+    <output url="file://$PROJECT_DIR$/classes" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..8626bf6
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/2020RobotCode.iml" filepath="$PROJECT_DIR$/2020RobotCode.iml" />
+    </modules>
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..35eb1dd
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="" vcs="Git" />
+  </component>
+</project>
\ No newline at end of file
diff --git a/.idea/workspace.xml b/.idea/workspace.xml
new file mode 100644
index 0000000..93b123f
--- /dev/null
+++ b/.idea/workspace.xml
@@ -0,0 +1,147 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ChangeListManager">
+    <list default="true" id="f1a381e1-64b1-4e42-9b30-623dabd68ae2" name="Default Changelist" comment="" />
+    <ignored path="$PROJECT_DIR$/bin/default/" />
+    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
+    <option name="SHOW_DIALOG" value="false" />
+    <option name="HIGHLIGHT_CONFLICTS" value="true" />
+    <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
+    <option name="LAST_RESOLUTION" value="IGNORE" />
+  </component>
+  <component name="FileEditorManager">
+    <leaf>
+      <file pinned="false" current-in-tab="true">
+        <entry file="file://$PROJECT_DIR$/src/main/java/frc/robot/Path.java">
+          <provider selected="true" editor-type-id="text-editor">
+            <state relative-caret-position="204">
+              <caret line="18" column="43" lean-forward="true" selection-start-line="18" selection-start-column="43" selection-end-line="18" selection-end-column="43" />
+            </state>
+          </provider>
+        </entry>
+      </file>
+    </leaf>
+  </component>
+  <component name="Git.Settings">
+    <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
+  </component>
+  <component name="ProjectFrameBounds" extendedState="6">
+    <option name="x" value="-12" />
+    <option name="y" value="-12" />
+    <option name="width" value="974" />
+    <option name="height" value="1048" />
+  </component>
+  <component name="ProjectView">
+    <navigator proportions="" version="1">
+      <foldersAlwaysOnTop value="true" />
+    </navigator>
+    <panes>
+      <pane id="ProjectPane">
+        <subPane>
+          <expand>
+            <path>
+              <item name="2020RobotCode" type="b2602c69:ProjectViewProjectNode" />
+              <item name="2020RobotCode" type="462c0819:PsiDirectoryNode" />
+            </path>
+            <path>
+              <item name="2020RobotCode" type="b2602c69:ProjectViewProjectNode" />
+              <item name="2020RobotCode" type="462c0819:PsiDirectoryNode" />
+              <item name="src" type="462c0819:PsiDirectoryNode" />
+            </path>
+            <path>
+              <item name="2020RobotCode" type="b2602c69:ProjectViewProjectNode" />
+              <item name="2020RobotCode" type="462c0819:PsiDirectoryNode" />
+              <item name="src" type="462c0819:PsiDirectoryNode" />
+              <item name="main" type="462c0819:PsiDirectoryNode" />
+            </path>
+            <path>
+              <item name="2020RobotCode" type="b2602c69:ProjectViewProjectNode" />
+              <item name="2020RobotCode" type="462c0819:PsiDirectoryNode" />
+              <item name="src" type="462c0819:PsiDirectoryNode" />
+              <item name="main" type="462c0819:PsiDirectoryNode" />
+              <item name="java" type="462c0819:PsiDirectoryNode" />
+            </path>
+            <path>
+              <item name="2020RobotCode" type="b2602c69:ProjectViewProjectNode" />
+              <item name="2020RobotCode" type="462c0819:PsiDirectoryNode" />
+              <item name="src" type="462c0819:PsiDirectoryNode" />
+              <item name="main" type="462c0819:PsiDirectoryNode" />
+              <item name="java" type="462c0819:PsiDirectoryNode" />
+              <item name="robot" type="462c0819:PsiDirectoryNode" />
+            </path>
+          </expand>
+          <select />
+        </subPane>
+      </pane>
+      <pane id="PackagesPane" />
+      <pane id="Scope" />
+    </panes>
+  </component>
+  <component name="PropertiesComponent">
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/../../AP Comp Sci" />
+  </component>
+  <component name="RunDashboard">
+    <option name="ruleStates">
+      <list>
+        <RuleState>
+          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
+        </RuleState>
+        <RuleState>
+          <option name="name" value="StatusDashboardGroupingRule" />
+        </RuleState>
+      </list>
+    </option>
+  </component>
+  <component name="SvnConfiguration">
+    <configuration />
+  </component>
+  <component name="TaskManager">
+    <task active="true" id="Default" summary="Default task">
+      <changelist id="f1a381e1-64b1-4e42-9b30-623dabd68ae2" name="Default Changelist" comment="" />
+      <created>1580876219386</created>
+      <option name="number" value="Default" />
+      <option name="presentableId" value="Default" />
+      <updated>1580876219386</updated>
+    </task>
+    <servers />
+  </component>
+  <component name="ToolWindowManager">
+    <frame x="-6" y="-6" width="1513" height="973" extended-state="6" />
+    <layout>
+      <window_info id="Image Layers" />
+      <window_info id="Designer" />
+      <window_info id="UI Designer" />
+      <window_info id="Capture Tool" />
+      <window_info id="Favorites" side_tool="true" />
+      <window_info active="true" content_ui="combo" id="Project" order="0" visible="true" weight="0.24948454" />
+      <window_info id="Structure" order="1" side_tool="true" weight="0.25" />
+      <window_info anchor="bottom" id="Version Control" />
+      <window_info anchor="bottom" id="Terminal" />
+      <window_info anchor="bottom" id="Event Log" side_tool="true" />
+      <window_info anchor="bottom" id="Message" order="0" />
+      <window_info anchor="bottom" id="Find" order="1" />
+      <window_info anchor="bottom" id="Run" order="2" />
+      <window_info anchor="bottom" id="Debug" order="3" weight="0.4" />
+      <window_info anchor="bottom" id="Cvs" order="4" weight="0.25" />
+      <window_info anchor="bottom" id="Inspection" order="5" weight="0.4" />
+      <window_info anchor="bottom" id="TODO" order="6" />
+      <window_info anchor="right" id="Palette" />
+      <window_info anchor="right" id="Theme Preview" />
+      <window_info anchor="right" id="Maven" />
+      <window_info anchor="right" id="Capture Analysis" />
+      <window_info anchor="right" id="Palette&#9;" />
+      <window_info anchor="right" id="Commander" internal_type="SLIDING" order="0" type="SLIDING" weight="0.4" />
+      <window_info anchor="right" id="Ant Build" order="1" weight="0.25" />
+      <window_info anchor="right" content_ui="combo" id="Hierarchy" order="2" weight="0.25" />
+    </layout>
+  </component>
+  <component name="editorHistoryManager">
+    <entry file="file://$PROJECT_DIR$/src/main/java/frc/robot/Path.java">
+      <provider selected="true" editor-type-id="text-editor">
+        <state relative-caret-position="204">
+          <caret line="18" column="43" lean-forward="true" selection-start-line="18" selection-start-column="43" selection-end-line="18" selection-end-column="43" />
+        </state>
+      </provider>
+    </entry>
+  </component>
+</project>
\ No newline at end of file
diff --git a/2020RobotCode.iml b/2020RobotCode.iml
new file mode 100644
index 0000000..aeac413
--- /dev/null
+++ b/2020RobotCode.iml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="JAVA_MODULE" version="4">
+  <component name="EclipseModuleManager">
+    <conelement value="org.eclipse.buildship.core.gradleclasspathcontainer" />
+    <src_description expected_position="0">
+      <src_folder value="file://$MODULE_DIR$/src/main/java" expected_position="0" />
+    </src_description>
+  </component>
+  <component name="NewModuleRootManager">
+    <output url="file://$MODULE_DIR$/bin/default" />
+    <exclude-output />
+    <content url="file://$MODULE_DIR$">
+      <sourceFolder url="file://$MODULE_DIR$/src/main/java" isTestSource="false" />
+    </content>
+    <orderEntry type="sourceFolder" forTests="false" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="library" name="org.eclipse.buildship.core.gradleclasspathcontainer" level="application" />
+  </component>
+</module>
\ No newline at end of file
diff --git a/src/main/deploy/anotherTestPath.left.pf1.csv b/src/main/deploy/anotherTestPath.left.pf1.csv
new file mode 100644
index 0000000..993953e
--- /dev/null
+++ b/src/main/deploy/anotherTestPath.left.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,9.250000,0.000012,0.001174,0.058707,2.935366,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,-0.058707,-5.870732,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,2.935366,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.001918,9.250000,0.001929,0.095879,4.793966,239.698284,0.000000
+0.020000,0.004559,9.250000,0.004571,0.132091,1.810608,-149.167885,0.000000
+0.020000,0.007565,9.250000,0.007577,0.150291,0.909963,-45.032225,0.000000
+0.020000,0.010959,9.250000,0.010970,0.169664,0.968671,2.935366,0.000000
+0.020000,0.014763,9.250000,0.014774,0.190212,1.027378,2.935366,0.000000
+0.020000,0.019001,9.250000,0.019013,0.211933,1.086085,2.935366,0.000000
+0.020000,0.023698,9.250000,0.023710,0.234829,1.144793,2.935366,0.000000
+0.020000,0.028876,9.250000,0.028888,0.258899,1.203500,2.935366,0.000000
+0.020000,0.034559,9.250000,0.034571,0.284143,1.262207,2.935366,0.000000
+0.020000,0.040770,9.250000,0.040782,0.310562,1.320915,2.935366,0.000000
+0.020000,0.047533,9.250000,0.047545,0.338154,1.379622,2.935366,0.000000
+0.020000,0.054872,9.250000,0.054883,0.366921,1.438329,2.935366,0.000000
+0.020000,0.062809,9.250000,0.062821,0.396861,1.497037,2.935366,0.000000
+0.020000,0.071368,9.250000,0.071380,0.427976,1.555744,2.935366,0.000000
+0.020000,0.080574,9.250000,0.080585,0.460265,1.614451,2.935366,0.000000
+0.020000,0.090448,9.250000,0.090460,0.493729,1.673159,2.935366,0.000000
+0.020000,0.101016,9.250000,0.101027,0.528366,1.731866,2.935366,0.000000
+0.020000,0.112299,9.250000,0.112311,0.564177,1.790573,2.935366,0.000000
+0.020000,0.124322,9.250000,0.124334,0.601163,1.849281,2.935366,0.000000
+0.020000,0.137109,9.250000,0.137121,0.639323,1.907988,2.935366,0.000000
+0.020000,0.150682,9.250000,0.150694,0.678657,1.966695,2.935366,0.000000
+0.020000,0.165065,9.250000,0.165077,0.719165,2.025403,2.935366,0.000000
+0.020000,0.180282,9.250000,0.180294,0.760847,2.084110,2.935366,0.000000
+0.020000,0.196356,9.250000,0.196368,0.803703,2.142817,2.935366,0.000000
+0.020000,0.213311,9.250000,0.213323,0.847734,2.201525,2.935366,0.000000
+0.020000,0.231170,9.250000,0.231181,0.892938,2.260232,2.935366,0.000000
+0.020000,0.249956,9.250000,0.249968,0.939317,2.318939,2.935366,0.000000
+0.020000,0.269693,9.250000,0.269705,0.986870,2.377647,2.935366,0.000000
+0.020000,0.290394,9.250000,0.290405,1.035010,2.407000,1.467683,0.000000
+0.020000,0.312057,9.250000,0.312068,1.083150,2.407000,0.000000,0.000000
+0.020000,0.334682,9.250000,0.334694,1.131290,2.407000,-0.000000,0.000000
+0.020000,0.358271,9.250000,0.358283,1.179430,2.407000,-0.000000,0.000000
+0.020000,0.382822,9.250000,0.382834,1.227570,2.407000,0.000000,0.000000
+0.020000,0.408337,9.250000,0.408348,1.275710,2.407000,-0.000000,0.000000
+0.020000,0.434814,9.250000,0.434825,1.323850,2.407000,-0.000000,0.000000
+0.020000,0.462253,9.250000,0.462265,1.371990,2.407000,0.000000,0.000000
+0.020000,0.490656,9.250000,0.490668,1.420130,2.407000,0.000000,0.000000
+0.020000,0.520021,9.250000,0.520033,1.468270,2.407000,-0.000000,0.000000
+0.020000,0.550350,9.250000,0.550361,1.516410,2.407000,0.000000,0.000000
+0.020000,0.581641,9.250000,0.581652,1.564550,2.407000,-0.000000,0.000000
+0.020000,0.613894,9.250000,0.613906,1.612690,2.407000,0.000000,0.000000
+0.020000,0.647111,9.250000,0.647123,1.660830,2.407000,-0.000000,0.000000
+0.020000,0.681290,9.250000,0.681302,1.708970,2.407000,0.000000,0.000000
+0.020000,0.716433,9.250000,0.716444,1.757110,2.407000,-0.000000,0.000000
+0.020000,0.752538,9.250000,0.752549,1.805250,2.407000,0.000000,0.000000
+0.020000,0.789605,9.250000,0.789617,1.853390,2.407000,-0.000000,0.000000
+0.020000,0.827636,9.250000,0.827648,1.901530,2.407000,0.000000,0.000000
+0.020000,0.866629,9.250000,0.866641,1.949670,2.407000,-0.000000,0.000000
+0.020000,0.906586,9.250000,0.906597,1.997810,2.407000,0.000000,0.000000
+0.020000,0.947505,9.250000,0.947516,2.045950,2.407000,-0.000000,0.000000
+0.020000,0.989386,9.250000,0.989398,2.094090,2.407000,0.000000,0.000000
+0.020000,1.032231,9.250000,1.032243,2.142230,2.407000,-0.000000,0.000000
+0.020000,1.076038,9.250000,1.076050,2.190370,2.407000,0.000000,0.000000
+0.020000,1.120809,9.250000,1.120820,2.238510,2.407000,-0.000000,0.000000
+0.020000,1.166542,9.250000,1.166553,2.286650,2.407000,0.000000,0.000000
+0.020000,1.213237,9.250000,1.213249,2.334790,2.407000,0.000000,0.000000
+0.020000,1.260896,9.250000,1.260908,2.382930,2.407000,0.000000,0.000000
+0.020000,1.309518,9.250000,1.309529,2.431070,2.407000,-0.000000,0.000000
+0.020000,1.359102,9.250000,1.359113,2.479210,2.407000,0.000000,0.000000
+0.020000,1.409649,9.250000,1.409660,2.527350,2.407000,-0.000000,0.000000
+0.020000,1.461159,9.250000,1.461170,2.575490,2.407000,0.000000,0.000000
+0.020000,1.513631,9.250000,1.513643,2.623630,2.407000,0.000000,0.000000
+0.020000,1.567067,9.250000,1.567078,2.671770,2.407000,-0.000000,0.000000
+0.020000,1.621465,9.250000,1.621476,2.719910,2.407000,0.000000,0.000000
+0.020000,1.676826,9.250000,1.676837,2.768050,2.407000,0.000000,0.000000
+0.020000,1.733150,9.250000,1.733161,2.816190,2.407000,0.000000,0.000000
+0.020000,1.790436,9.250000,1.790448,2.864330,2.407000,0.000000,0.000000
+0.020000,1.848686,9.250000,1.848697,2.912470,2.407000,-0.000000,0.000000
+0.020000,1.907898,9.250000,1.907909,2.960610,2.407000,0.000000,0.000000
+0.020000,1.968073,9.250000,1.968084,3.008750,2.407000,-0.000000,0.000000
+0.020000,2.029211,9.250000,2.029222,3.056890,2.407000,-0.000000,0.000000
+0.020000,2.091299,9.250000,2.091311,3.104443,2.377647,-1.467683,0.000000
+0.020000,2.154316,9.250000,2.154328,3.150822,2.318939,-2.935366,0.000000
+0.020000,2.218236,9.250000,2.218248,3.196027,2.260232,-2.935366,0.000000
+0.020000,2.283038,9.250000,2.283049,3.240057,2.201525,-2.935366,0.000000
+0.020000,2.348696,9.250000,2.348708,3.282913,2.142817,-2.935366,0.000000
+0.020000,2.415188,9.250000,2.415199,3.324596,2.084110,-2.935366,0.000000
+0.020000,2.482490,9.250000,2.482502,3.365104,2.025403,-2.935366,0.000000
+0.020000,2.550579,9.250000,2.550590,3.404438,1.966695,-2.935366,0.000000
+0.020000,2.619430,9.250000,2.619442,3.442597,1.907988,-2.935366,0.000000
+0.020000,2.689022,9.250000,2.689034,3.479583,1.849281,-2.935366,0.000000
+0.020000,2.759330,9.250000,2.759342,3.515394,1.790573,-2.935366,0.000000
+0.020000,2.830331,9.250000,2.830342,3.550032,1.731866,-2.935366,0.000000
+0.020000,2.902001,9.250000,2.902012,3.583495,1.673159,-2.935366,0.000000
+0.020000,2.974316,9.250000,2.974328,3.615784,1.614451,-2.935366,0.000000
+0.020000,3.047254,9.250000,3.047266,3.646899,1.555744,-2.935366,0.000000
+0.020000,3.120791,9.250000,3.120803,3.676840,1.497037,-2.935366,0.000000
+0.020000,3.194903,9.250000,3.194915,3.705606,1.438329,-2.935366,0.000000
+0.020000,3.269567,9.250000,3.269579,3.733199,1.379622,-2.935366,0.000000
+0.020000,3.344759,9.250000,3.344771,3.759617,1.320915,-2.935366,0.000000
+0.020000,3.420457,9.250000,3.420468,3.784861,1.262207,-2.935366,0.000000
+0.020000,3.496635,9.250000,3.496647,3.808931,1.203500,-2.935366,0.000000
+0.020000,3.573272,9.250000,3.573284,3.831827,1.144793,-2.935366,0.000000
+0.020000,3.650343,9.250000,3.650355,3.853549,1.086085,-2.935366,0.000000
+0.020000,3.727825,9.250000,3.727836,3.874096,1.027378,-2.935366,0.000000
+0.020000,3.805694,9.250000,3.805706,3.893470,0.968671,-2.935366,0.000000
+0.020000,3.883927,9.250000,3.883939,3.911669,0.909963,-2.935366,0.000000
+0.020000,3.962501,9.250000,3.962513,3.928694,0.851256,-2.935366,0.000000
+0.020000,4.041392,9.250000,4.041404,3.944545,0.792549,-2.935366,0.000000
+0.020000,4.120577,9.250000,4.120588,3.959222,0.733842,-2.935366,0.000000
+0.020000,4.200031,9.250000,4.200043,3.972724,0.675134,-2.935366,0.000000
+0.020000,4.279732,9.250000,4.279744,3.985053,0.616427,-2.935366,0.000000
+0.020000,4.359656,9.250000,4.359668,3.996207,0.557720,-2.935366,0.000000
+0.020000,4.439780,9.250000,4.439792,4.006188,0.499012,-2.935366,0.000000
+0.020000,4.520080,9.250000,4.520092,4.014994,0.440305,-2.935366,0.000000
+0.020000,4.600533,9.250000,4.600544,4.022626,0.381598,-2.935366,0.000000
+0.020000,4.681114,9.250000,4.681126,4.029083,0.322890,-2.935366,0.000000
+0.020000,4.761802,9.250000,4.761813,4.034367,0.264183,-2.935366,0.000000
+0.020000,4.842571,9.250000,4.842583,4.038477,0.205476,-2.935366,0.000000
+0.020000,4.923399,9.250000,4.923411,4.041412,0.146768,-2.935366,0.000000
+0.020000,5.004260,9.250000,5.004272,4.043039,0.081337,-3.271559,0.000000
+0.020000,5.085115,9.250000,5.085127,4.042770,-0.013448,-4.739242,0.000000
+0.020000,5.165930,9.250000,5.165942,4.040740,-0.101509,-4.403049,0.000000
+0.020000,5.246681,9.250000,5.246693,4.037535,-0.160216,-2.935366,0.000000
+0.020000,5.327344,9.250000,5.327356,4.033157,-0.218923,-2.935366,0.000000
+0.020000,5.407896,9.250000,5.407908,4.027604,-0.277631,-2.935366,0.000000
+0.020000,5.488314,9.250000,5.488325,4.020877,-0.336338,-2.935366,0.000000
+0.020000,5.568573,9.250000,5.568585,4.012977,-0.395045,-2.935366,0.000000
+0.020000,5.648651,9.250000,5.648663,4.003902,-0.453753,-2.935366,0.000000
+0.020000,5.728524,9.250000,5.728536,3.993652,-0.512460,-2.935366,0.000000
+0.020000,5.808169,9.250000,5.808181,3.982229,-0.571167,-2.935366,0.000000
+0.020000,5.887562,9.250000,5.887573,3.969631,-0.629875,-2.935366,0.000000
+0.020000,5.966679,9.250000,5.966691,3.955860,-0.688582,-2.935366,0.000000
+0.020000,6.045497,9.250000,6.045509,3.940914,-0.747289,-2.935366,0.000000
+0.020000,6.123993,9.250000,6.124005,3.924794,-0.805997,-2.935366,0.000000
+0.020000,6.202143,9.250000,6.202155,3.907500,-0.864704,-2.935366,0.000000
+0.020000,6.279924,9.250000,6.279935,3.889032,-0.923411,-2.935366,0.000000
+0.020000,6.357311,9.250000,6.357323,3.869389,-0.982119,-2.935366,0.000000
+0.020000,6.434283,9.250000,6.434295,3.848573,-1.040826,-2.935366,0.000000
+0.020000,6.510814,9.250000,6.510826,3.826582,-1.099533,-2.935366,0.000000
+0.020000,6.586883,9.250000,6.586895,3.803417,-1.158240,-2.935366,0.000000
+0.020000,6.662464,9.250000,6.662476,3.779079,-1.216948,-2.935366,0.000000
+0.020000,6.737536,9.250000,6.737547,3.753565,-1.275655,-2.935366,0.000000
+0.020000,6.812073,9.250000,6.812085,3.726878,-1.334362,-2.935366,0.000000
+0.020000,6.886054,9.250000,6.886065,3.699017,-1.393070,-2.935366,0.000000
+0.020000,6.959453,9.250000,6.959465,3.669981,-1.451777,-2.935366,0.000000
+0.020000,7.032249,9.250000,7.032260,3.639772,-1.510484,-2.935366,0.000000
+0.020000,7.104416,9.250000,7.104428,3.608388,-1.569192,-2.935366,0.000000
+0.020000,7.175933,9.250000,7.175945,3.575830,-1.627899,-2.935366,0.000000
+0.020000,7.246775,9.250000,7.246787,3.542098,-1.686606,-2.935366,0.000000
+0.020000,7.316919,9.250000,7.316931,3.507191,-1.745314,-2.935366,0.000000
+0.020000,7.386341,9.250000,7.386353,3.471111,-1.804021,-2.935366,0.000000
+0.020000,7.455018,9.250000,7.455030,3.433856,-1.862728,-2.935366,0.000000
+0.020000,7.522927,9.250000,7.522938,3.395428,-1.921436,-2.935366,0.000000
+0.020000,7.590043,9.250000,7.590055,3.355825,-1.980143,-2.935366,0.000000
+0.020000,7.656344,9.250000,7.656356,3.315048,-2.038850,-2.935366,0.000000
+0.020000,7.721806,9.250000,7.721818,3.273097,-2.097558,-2.935366,0.000000
+0.020000,7.786405,9.250000,7.786417,3.229971,-2.156265,-2.935366,0.000000
+0.020000,7.850119,9.250000,7.850131,3.185672,-2.214972,-2.935366,0.000000
+0.020000,7.912923,9.250000,7.912935,3.140198,-2.273680,-2.935366,0.000000
+0.020000,7.974794,9.250000,7.974806,3.093551,-2.332387,-2.935366,0.000000
+0.020000,8.035711,9.250000,8.035723,3.045863,-2.384370,-2.599173,0.000000
+0.020000,8.095666,9.250000,8.095677,2.997723,-2.407000,-1.131490,0.000000
+0.020000,8.154657,9.250000,8.154669,2.949583,-2.407000,-0.000000,0.000000
+0.020000,8.212686,9.250000,8.212698,2.901443,-2.407000,0.000000,0.000000
+0.020000,8.269752,9.250000,8.269764,2.853303,-2.407000,-0.000000,0.000000
+0.020000,8.325855,9.250000,8.325867,2.805163,-2.407000,0.000000,0.000000
+0.020000,8.380996,9.250000,8.381008,2.757023,-2.407000,0.000000,0.000000
+0.020000,8.435174,9.250000,8.435185,2.708883,-2.407000,0.000000,0.000000
+0.020000,8.488388,9.250000,8.488400,2.660743,-2.407000,0.000000,0.000000
+0.020000,8.540641,9.250000,8.540652,2.612603,-2.407000,0.000000,0.000000
+0.020000,8.591930,9.250000,8.591942,2.564463,-2.407000,0.000000,0.000000
+0.020000,8.642256,9.250000,8.642268,2.516323,-2.407000,0.000000,0.000000
+0.020000,8.691620,9.250000,8.691632,2.468183,-2.407000,0.000000,0.000000
+0.020000,8.740021,9.250000,8.740032,2.420043,-2.407000,0.000000,0.000000
+0.020000,8.787459,9.250000,8.787471,2.371903,-2.407000,0.000000,0.000000
+0.020000,8.833934,9.250000,8.833946,2.323763,-2.407000,0.000000,0.000000
+0.020000,8.879447,9.250000,8.879458,2.275623,-2.407000,0.000000,0.000000
+0.020000,8.923996,9.250000,8.924008,2.227483,-2.407000,0.000000,0.000000
+0.020000,8.967583,9.250000,8.967595,2.179343,-2.407000,0.000000,0.000000
+0.020000,9.010207,9.250000,9.010219,2.131203,-2.407000,0.000000,0.000000
+0.020000,9.051868,9.250000,9.051880,2.083063,-2.407000,0.000000,0.000000
+0.020000,9.092567,9.250000,9.092579,2.034923,-2.407000,0.000000,0.000000
+0.020000,9.132303,9.250000,9.132314,1.986783,-2.407000,0.000000,0.000000
+0.020000,9.171075,9.250000,9.171087,1.938643,-2.407000,0.000000,0.000000
+0.020000,9.208885,9.250000,9.208897,1.890503,-2.407000,0.000000,0.000000
+0.020000,9.245733,9.250000,9.245744,1.842363,-2.407000,0.000000,0.000000
+0.020000,9.281617,9.250000,9.281629,1.794223,-2.407000,0.000000,0.000000
+0.020000,9.316539,9.250000,9.316551,1.746083,-2.407000,0.000000,0.000000
+0.020000,9.350498,9.250000,9.350509,1.697943,-2.407000,-0.000000,0.000000
+0.020000,9.383494,9.250000,9.383505,1.649803,-2.407000,0.000000,0.000000
+0.020000,9.415527,9.250000,9.415539,1.601663,-2.407000,-0.000000,0.000000
+0.020000,9.446597,9.250000,9.446609,1.553523,-2.407000,0.000000,0.000000
+0.020000,9.476705,9.250000,9.476717,1.505383,-2.407000,-0.000000,0.000000
+0.020000,9.505850,9.250000,9.505862,1.457243,-2.407000,0.000000,0.000000
+0.020000,9.534032,9.250000,9.534044,1.409103,-2.407000,0.000000,0.000000
+0.020000,9.561251,9.250000,9.561263,1.360963,-2.407000,-0.000000,0.000000
+0.020000,9.587508,9.250000,9.587519,1.312823,-2.407000,0.000000,0.000000
+0.020000,9.612801,9.250000,9.612813,1.264683,-2.407000,-0.000000,0.000000
+0.020000,9.637132,9.250000,9.637144,1.216543,-2.407000,0.000000,0.000000
+0.020000,9.660500,9.250000,9.660512,1.168403,-2.407000,0.000000,0.000000
+0.020000,9.682906,9.250000,9.682917,1.120263,-2.407000,-0.000000,0.000000
+0.020000,9.704348,9.250000,9.704360,1.072123,-2.407000,0.000000,0.000000
+0.020000,9.724828,9.250000,9.724839,1.023983,-2.407000,-0.000000,0.000000
+0.020000,9.744347,9.250000,9.744359,0.975977,-2.400276,0.336193,0.000000
+0.020000,9.762921,9.250000,9.762933,0.928693,-2.364199,1.803876,0.000000
+0.020000,9.780573,9.250000,9.780585,0.882584,-2.305491,2.935366,0.000000
+0.020000,9.797326,9.250000,9.797337,0.837648,-2.246784,2.935366,0.000000
+0.020000,9.813203,9.250000,9.813215,0.793886,-2.188077,2.935366,0.000000
+0.020000,9.828229,9.250000,9.828241,0.751299,-2.129370,2.935366,0.000000
+0.020000,9.842427,9.250000,9.842439,0.709886,-2.070662,2.935366,0.000000
+0.020000,9.855820,9.250000,9.855832,0.669647,-2.011955,2.935366,0.000000
+0.020000,9.868432,9.250000,9.868443,0.630582,-1.953248,2.935366,0.000000
+0.020000,9.880286,9.250000,9.880297,0.592691,-1.894540,2.935366,0.000000
+0.020000,9.891405,9.250000,9.891417,0.555974,-1.835833,2.935366,0.000000
+0.020000,9.901814,9.250000,9.901825,0.520432,-1.777126,2.935366,0.000000
+0.020000,9.911535,9.250000,9.911547,0.486063,-1.718418,2.935366,0.000000
+0.020000,9.920592,9.250000,9.920604,0.452869,-1.659711,2.935366,0.000000
+0.020000,9.929009,9.250000,9.929021,0.420849,-1.601004,2.935366,0.000000
+0.020000,9.936809,9.250000,9.936821,0.390003,-1.542296,2.935366,0.000000
+0.020000,9.944016,9.250000,9.944028,0.360331,-1.483589,2.935366,0.000000
+0.020000,9.950653,9.250000,9.950664,0.331834,-1.424882,2.935366,0.000000
+0.020000,9.956743,9.250000,9.956755,0.304510,-1.366174,2.935366,0.000000
+0.020000,9.962310,9.250000,9.962322,0.278361,-1.307467,2.935366,0.000000
+0.020000,9.967378,9.250000,9.967390,0.253386,-1.248760,2.935366,0.000000
+0.020000,9.971970,9.250000,9.971981,0.229585,-1.190052,2.935366,0.000000
+0.020000,9.976109,9.250000,9.976120,0.206958,-1.131345,2.935366,0.000000
+0.020000,9.979819,9.250000,9.979831,0.185505,-1.072638,2.935366,0.000000
+0.020000,9.983123,9.250000,9.983135,0.165226,-1.013930,2.935366,0.000000
+0.020000,9.986046,9.250000,9.986057,0.146122,-0.955223,2.935366,0.000000
+0.020000,9.988610,9.250000,9.988621,0.128192,-0.896516,2.935366,0.000000
+0.020000,9.990838,9.250000,9.990850,0.111435,-0.837808,2.935366,0.000000
+0.020000,9.992755,9.250000,9.992767,0.095853,-0.779101,2.935366,0.000000
+0.020000,9.994384,9.250000,9.994396,0.081446,-0.720394,2.935366,0.000000
+0.020000,9.995748,9.250000,9.995760,0.068212,-0.661686,2.935366,0.000000
+0.020000,9.996872,9.250000,9.996883,0.056152,-0.602979,2.935366,0.000000
+0.020000,9.997777,9.250000,9.997789,0.045267,-0.544272,2.935366,0.000000
+0.020000,9.998488,9.250000,9.998500,0.035556,-0.485565,2.935366,0.000000
+0.020000,9.999028,9.250000,9.999040,0.027018,-0.426857,2.935366,0.000000
+0.020000,9.999421,9.250000,9.999433,0.019655,-0.368150,2.935366,0.000000
+0.020000,9.999691,9.250000,9.999703,0.013467,-0.309443,2.935366,0.000000
+0.020000,9.999860,9.250000,9.999872,0.008452,-0.250735,2.935366,0.000000
+0.020000,9.999952,9.250000,9.999964,0.004611,-0.192028,2.935366,0.000000
+0.020000,9.999991,9.250000,10.000003,0.001945,-0.133321,2.935366,0.000000
+0.020000,10.000000,9.250000,10.000012,0.000453,-0.074613,2.935366,0.000000
+0.020000,10.000000,9.250000,10.000012,0.000000,-0.022630,2.599173,0.000000
diff --git a/src/main/deploy/anotherTestPath.pf1.csv b/src/main/deploy/anotherTestPath.pf1.csv
new file mode 100644
index 0000000..076f14d
--- /dev/null
+++ b/src/main/deploy/anotherTestPath.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,8.000000,0.000012,0.001174,0.058707,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000059,0.003522,0.117415,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000164,0.007045,0.176122,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000352,0.011741,0.234829,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000646,0.017612,0.293537,2.935366,0.000000
+0.020000,0.000000,8.000000,0.001068,0.024657,0.352244,2.935366,0.000000
+0.020000,0.000000,8.000000,0.001644,0.032876,0.410951,2.935366,0.000000
+0.020000,0.000000,8.000000,0.002395,0.042269,0.469659,2.935366,0.000000
+0.020000,0.000000,8.000000,0.003346,0.052837,0.528366,2.935366,0.000000
+0.020000,0.000000,8.000000,0.004520,0.064578,0.587073,2.935366,0.000000
+0.020000,0.000000,8.000000,0.005941,0.077494,0.645781,2.935366,0.000000
+0.020000,0.000000,8.000000,0.007632,0.091583,0.704488,2.935366,0.000000
+0.020000,0.000000,8.000000,0.009616,0.106847,0.763195,2.935366,0.000000
+0.020000,0.001918,8.000000,0.011918,0.123285,0.821903,2.935366,0.000000
+0.020000,0.004559,8.000000,0.014559,0.140898,0.880610,2.935366,0.000000
+0.020000,0.007565,8.000000,0.017565,0.159684,0.939317,2.935366,0.000000
+0.020000,0.010959,8.000000,0.020959,0.179644,0.998024,2.935366,0.000000
+0.020000,0.014763,8.000000,0.024763,0.200779,1.056732,2.935366,0.000000
+0.020000,0.019001,8.000000,0.029001,0.223088,1.115439,2.935366,0.000000
+0.020000,0.023698,8.000000,0.033698,0.246571,1.174146,2.935366,0.000000
+0.020000,0.028876,8.000000,0.038876,0.271228,1.232854,2.935366,0.000000
+0.020000,0.034559,8.000000,0.044559,0.297059,1.291561,2.935366,0.000000
+0.020000,0.040770,8.000000,0.050770,0.324064,1.350268,2.935366,0.000000
+0.020000,0.047533,8.000000,0.057533,0.352244,1.408976,2.935366,0.000000
+0.020000,0.054872,8.000000,0.064872,0.381598,1.467683,2.935366,0.000000
+0.020000,0.062809,8.000000,0.072809,0.412125,1.526390,2.935366,0.000000
+0.020000,0.071368,8.000000,0.081368,0.443827,1.585098,2.935366,0.000000
+0.020000,0.080574,8.000000,0.090574,0.476703,1.643805,2.935366,0.000000
+0.020000,0.090448,8.000000,0.100448,0.510754,1.702512,2.935366,0.000000
+0.020000,0.101016,8.000000,0.111016,0.545978,1.761220,2.935366,0.000000
+0.020000,0.112299,8.000000,0.122299,0.582377,1.819927,2.935366,0.000000
+0.020000,0.124322,8.000000,0.134322,0.619949,1.878634,2.935366,0.000000
+0.020000,0.137109,8.000000,0.147109,0.658696,1.937342,2.935366,0.000000
+0.020000,0.150682,8.000000,0.160682,0.698617,1.996049,2.935366,0.000000
+0.020000,0.165065,8.000000,0.175065,0.739712,2.054756,2.935366,0.000000
+0.020000,0.180282,8.000000,0.190282,0.781982,2.113464,2.935366,0.000000
+0.020000,0.196356,8.000000,0.206356,0.825425,2.172171,2.935366,0.000000
+0.020000,0.213311,8.000000,0.223311,0.870043,2.230878,2.935366,0.000000
+0.020000,0.231170,8.000000,0.241170,0.915834,2.289586,2.935366,0.000000
+0.020000,0.249956,8.000000,0.259956,0.962800,2.348293,2.935366,0.000000
+0.020000,0.269693,8.000000,0.279693,1.010940,2.407000,2.935366,0.000000
+0.020000,0.290394,8.000000,0.300394,1.059080,2.407000,0.000000,0.000000
+0.020000,0.312057,8.000000,0.322057,1.107220,2.407000,0.000000,0.000000
+0.020000,0.334682,8.000000,0.344682,1.155360,2.407000,-0.000000,0.000000
+0.020000,0.358271,8.000000,0.368271,1.203500,2.407000,0.000000,0.000000
+0.020000,0.382822,8.000000,0.392822,1.251640,2.407000,0.000000,0.000000
+0.020000,0.408337,8.000000,0.418337,1.299780,2.407000,0.000000,0.000000
+0.020000,0.434814,8.000000,0.444814,1.347920,2.407000,-0.000000,0.000000
+0.020000,0.462253,8.000000,0.472253,1.396060,2.407000,0.000000,0.000000
+0.020000,0.490656,8.000000,0.500656,1.444200,2.407000,0.000000,0.000000
+0.020000,0.520021,8.000000,0.530021,1.492340,2.407000,0.000000,0.000000
+0.020000,0.550350,8.000000,0.560350,1.540480,2.407000,-0.000000,0.000000
+0.020000,0.581641,8.000000,0.591641,1.588620,2.407000,0.000000,0.000000
+0.020000,0.613894,8.000000,0.623894,1.636760,2.407000,0.000000,0.000000
+0.020000,0.647111,8.000000,0.657111,1.684900,2.407000,0.000000,0.000000
+0.020000,0.681290,8.000000,0.691290,1.733040,2.407000,-0.000000,0.000000
+0.020000,0.716433,8.000000,0.726433,1.781180,2.407000,0.000000,0.000000
+0.020000,0.752538,8.000000,0.762538,1.829320,2.407000,0.000000,0.000000
+0.020000,0.789605,8.000000,0.799605,1.877460,2.407000,-0.000000,0.000000
+0.020000,0.827636,8.000000,0.837636,1.925600,2.407000,0.000000,0.000000
+0.020000,0.866629,8.000000,0.876629,1.973740,2.407000,0.000000,0.000000
+0.020000,0.906586,8.000000,0.916586,2.021880,2.407000,0.000000,0.000000
+0.020000,0.947505,8.000000,0.957505,2.070020,2.407000,-0.000000,0.000000
+0.020000,0.989386,8.000000,0.999386,2.118160,2.407000,0.000000,0.000000
+0.020000,1.032231,8.000000,1.042231,2.166300,2.407000,-0.000000,0.000000
+0.020000,1.076038,8.000000,1.086038,2.214440,2.407000,0.000000,0.000000
+0.020000,1.120809,8.000000,1.130809,2.262580,2.407000,-0.000000,0.000000
+0.020000,1.166542,8.000000,1.176542,2.310720,2.407000,0.000000,0.000000
+0.020000,1.213237,8.000000,1.223237,2.358860,2.407000,0.000000,0.000000
+0.020000,1.260896,8.000000,1.270896,2.407000,2.407000,-0.000000,0.000000
+0.020000,1.309518,8.000000,1.319518,2.455140,2.407000,0.000000,0.000000
+0.020000,1.359102,8.000000,1.369102,2.503280,2.407000,0.000000,0.000000
+0.020000,1.409649,8.000000,1.419649,2.551420,2.407000,0.000000,0.000000
+0.020000,1.461159,8.000000,1.471159,2.599560,2.407000,0.000000,0.000000
+0.020000,1.513631,8.000000,1.523631,2.647700,2.407000,0.000000,0.000000
+0.020000,1.567067,8.000000,1.577067,2.695840,2.407000,-0.000000,0.000000
+0.020000,1.621465,8.000000,1.631465,2.743980,2.407000,0.000000,0.000000
+0.020000,1.676826,8.000000,1.686826,2.792120,2.407000,0.000000,0.000000
+0.020000,1.733150,8.000000,1.743150,2.840260,2.407000,-0.000000,0.000000
+0.020000,1.790436,8.000000,1.800436,2.888400,2.407000,0.000000,0.000000
+0.020000,1.848686,8.000000,1.858686,2.936540,2.407000,-0.000000,0.000000
+0.020000,1.907898,8.000000,1.917898,2.984680,2.407000,0.000000,0.000000
+0.020000,1.968073,8.000000,1.978073,3.032820,2.407000,-0.000000,0.000000
+0.020000,2.029211,8.000000,2.039211,3.080960,2.407000,0.000000,0.000000
+0.020000,2.091299,8.000000,2.101299,3.127926,2.348293,-2.935366,0.000000
+0.020000,2.154316,8.000000,2.164316,3.173718,2.289586,-2.935366,0.000000
+0.020000,2.218236,8.000000,2.228236,3.218335,2.230878,-2.935366,0.000000
+0.020000,2.283038,8.000000,2.293038,3.261779,2.172171,-2.935366,0.000000
+0.020000,2.348696,8.000000,2.358696,3.304048,2.113464,-2.935366,0.000000
+0.020000,2.415188,8.000000,2.425188,3.345143,2.054756,-2.935366,0.000000
+0.020000,2.482490,8.000000,2.492490,3.385064,1.996049,-2.935366,0.000000
+0.020000,2.550579,8.000000,2.560579,3.423811,1.937342,-2.935366,0.000000
+0.020000,2.619430,8.000000,2.629430,3.461384,1.878634,-2.935366,0.000000
+0.020000,2.689022,8.000000,2.699022,3.497782,1.819927,-2.935366,0.000000
+0.020000,2.759330,8.000000,2.769330,3.533007,1.761220,-2.935366,0.000000
+0.020000,2.830331,8.000000,2.840331,3.567057,1.702512,-2.935366,0.000000
+0.020000,2.902001,8.000000,2.912001,3.599933,1.643805,-2.935366,0.000000
+0.020000,2.974316,8.000000,2.984316,3.631635,1.585098,-2.935366,0.000000
+0.020000,3.047254,8.000000,3.057254,3.662163,1.526390,-2.935366,0.000000
+0.020000,3.120791,8.000000,3.130791,3.691516,1.467683,-2.935366,0.000000
+0.020000,3.194903,8.000000,3.204903,3.719696,1.408976,-2.935366,0.000000
+0.020000,3.269567,8.000000,3.279567,3.746701,1.350268,-2.935366,0.000000
+0.020000,3.344759,8.000000,3.354759,3.772532,1.291561,-2.935366,0.000000
+0.020000,3.420457,8.000000,3.430457,3.797190,1.232854,-2.935366,0.000000
+0.020000,3.496635,8.000000,3.506635,3.820672,1.174146,-2.935366,0.000000
+0.020000,3.573272,8.000000,3.583272,3.842981,1.115439,-2.935366,0.000000
+0.020000,3.650343,8.000000,3.660343,3.864116,1.056732,-2.935366,0.000000
+0.020000,3.727825,8.000000,3.737825,3.884076,0.998024,-2.935366,0.000000
+0.020000,3.805694,8.000000,3.815694,3.902863,0.939317,-2.935366,0.000000
+0.020000,3.883927,8.000000,3.893927,3.920475,0.880610,-2.935366,0.000000
+0.020000,3.962501,8.000000,3.972501,3.936913,0.821903,-2.935366,0.000000
+0.020000,4.041392,8.000000,4.051392,3.952177,0.763195,-2.935366,0.000000
+0.020000,4.120577,8.000000,4.130577,3.966267,0.704488,-2.935366,0.000000
+0.020000,4.200031,8.000000,4.210031,3.979182,0.645781,-2.935366,0.000000
+0.020000,4.279732,8.000000,4.289732,3.990924,0.587073,-2.935366,0.000000
+0.020000,4.359656,8.000000,4.369656,4.001491,0.528366,-2.935366,0.000000
+0.020000,4.439780,8.000000,4.449780,4.010884,0.469659,-2.935366,0.000000
+0.020000,4.520080,8.000000,4.530080,4.019103,0.410951,-2.935366,0.000000
+0.020000,4.600533,8.000000,4.610533,4.026148,0.352244,-2.935366,0.000000
+0.020000,4.681114,8.000000,4.691114,4.032019,0.293537,-2.935366,0.000000
+0.020000,4.761802,8.000000,4.771802,4.036715,0.234829,-2.935366,0.000000
+0.020000,4.842571,8.000000,4.852571,4.040238,0.176122,-2.935366,0.000000
+0.020000,4.923399,8.000000,4.933399,4.042586,0.117415,-2.935366,0.000000
+0.020000,5.004260,8.000000,5.014260,4.043491,0.045260,-3.607752,0.000000
+0.020000,5.085115,8.000000,5.095115,4.042048,-0.072155,-5.870732,0.000000
+0.020000,5.165930,8.000000,5.175930,4.039431,-0.130862,-2.935366,0.000000
+0.020000,5.246681,8.000000,5.256681,4.035640,-0.189570,-2.935366,0.000000
+0.020000,5.327344,8.000000,5.337344,4.030674,-0.248277,-2.935366,0.000000
+0.020000,5.407896,8.000000,5.417896,4.024534,-0.306984,-2.935366,0.000000
+0.020000,5.488314,8.000000,5.498314,4.017221,-0.365692,-2.935366,0.000000
+0.020000,5.568573,8.000000,5.578573,4.008733,-0.424399,-2.935366,0.000000
+0.020000,5.648651,8.000000,5.658651,3.999070,-0.483106,-2.935366,0.000000
+0.020000,5.728524,8.000000,5.738524,3.988234,-0.541814,-2.935366,0.000000
+0.020000,5.808169,8.000000,5.818169,3.976224,-0.600521,-2.935366,0.000000
+0.020000,5.887562,8.000000,5.897562,3.963039,-0.659228,-2.935366,0.000000
+0.020000,5.966679,8.000000,5.976679,3.948680,-0.717936,-2.935366,0.000000
+0.020000,6.045497,8.000000,6.055497,3.933148,-0.776643,-2.935366,0.000000
+0.020000,6.123993,8.000000,6.133993,3.916441,-0.835350,-2.935366,0.000000
+0.020000,6.202143,8.000000,6.212143,3.898559,-0.894058,-2.935366,0.000000
+0.020000,6.279924,8.000000,6.289924,3.879504,-0.952765,-2.935366,0.000000
+0.020000,6.357311,8.000000,6.367311,3.859275,-1.011472,-2.935366,0.000000
+0.020000,6.434283,8.000000,6.444283,3.837871,-1.070179,-2.935366,0.000000
+0.020000,6.510814,8.000000,6.520814,3.815293,-1.128887,-2.935366,0.000000
+0.020000,6.586883,8.000000,6.596883,3.791542,-1.187594,-2.935366,0.000000
+0.020000,6.662464,8.000000,6.672464,3.766615,-1.246301,-2.935366,0.000000
+0.020000,6.737536,8.000000,6.747536,3.740515,-1.305009,-2.935366,0.000000
+0.020000,6.812073,8.000000,6.822073,3.713241,-1.363716,-2.935366,0.000000
+0.020000,6.886054,8.000000,6.896054,3.684793,-1.422423,-2.935366,0.000000
+0.020000,6.959453,8.000000,6.969453,3.655170,-1.481131,-2.935366,0.000000
+0.020000,7.032249,8.000000,7.042249,3.624373,-1.539838,-2.935366,0.000000
+0.020000,7.104416,8.000000,7.114416,3.592402,-1.598545,-2.935366,0.000000
+0.020000,7.175933,8.000000,7.185933,3.559257,-1.657253,-2.935366,0.000000
+0.020000,7.246775,8.000000,7.256775,3.524938,-1.715960,-2.935366,0.000000
+0.020000,7.316919,8.000000,7.326919,3.489445,-1.774667,-2.935366,0.000000
+0.020000,7.386341,8.000000,7.396341,3.452777,-1.833375,-2.935366,0.000000
+0.020000,7.455018,8.000000,7.465018,3.414936,-1.892082,-2.935366,0.000000
+0.020000,7.522927,8.000000,7.532927,3.375920,-1.950789,-2.935366,0.000000
+0.020000,7.590043,8.000000,7.600043,3.335730,-2.009497,-2.935366,0.000000
+0.020000,7.656344,8.000000,7.666344,3.294366,-2.068204,-2.935366,0.000000
+0.020000,7.721806,8.000000,7.731806,3.251827,-2.126911,-2.935366,0.000000
+0.020000,7.786405,8.000000,7.796405,3.208115,-2.185619,-2.935366,0.000000
+0.020000,7.850119,8.000000,7.860119,3.163229,-2.244326,-2.935366,0.000000
+0.020000,7.912923,8.000000,7.922923,3.117168,-2.303033,-2.935366,0.000000
+0.020000,7.974794,8.000000,7.984794,3.069933,-2.361741,-2.935366,0.000000
+0.020000,8.035711,8.000000,8.045711,3.021793,-2.407000,-2.262981,0.000000
+0.020000,8.095666,8.000000,8.105666,2.973653,-2.407000,-0.000000,0.000000
+0.020000,8.154657,8.000000,8.164657,2.925513,-2.407000,0.000000,0.000000
+0.020000,8.212686,8.000000,8.222686,2.877373,-2.407000,0.000000,0.000000
+0.020000,8.269752,8.000000,8.279752,2.829233,-2.407000,-0.000000,0.000000
+0.020000,8.325855,8.000000,8.335855,2.781093,-2.407000,0.000000,0.000000
+0.020000,8.380996,8.000000,8.390996,2.732953,-2.407000,-0.000000,0.000000
+0.020000,8.435174,8.000000,8.445174,2.684813,-2.407000,0.000000,0.000000
+0.020000,8.488388,8.000000,8.498388,2.636673,-2.407000,-0.000000,0.000000
+0.020000,8.540641,8.000000,8.550641,2.588533,-2.407000,0.000000,0.000000
+0.020000,8.591930,8.000000,8.601930,2.540393,-2.407000,0.000000,0.000000
+0.020000,8.642256,8.000000,8.652256,2.492253,-2.407000,-0.000000,0.000000
+0.020000,8.691620,8.000000,8.701620,2.444113,-2.407000,0.000000,0.000000
+0.020000,8.740021,8.000000,8.750021,2.395973,-2.407000,-0.000000,0.000000
+0.020000,8.787459,8.000000,8.797459,2.347833,-2.407000,0.000000,0.000000
+0.020000,8.833934,8.000000,8.843934,2.299693,-2.407000,0.000000,0.000000
+0.020000,8.879447,8.000000,8.889447,2.251553,-2.407000,0.000000,0.000000
+0.020000,8.923996,8.000000,8.933996,2.203413,-2.407000,-0.000000,0.000000
+0.020000,8.967583,8.000000,8.977583,2.155273,-2.407000,0.000000,0.000000
+0.020000,9.010207,8.000000,9.020207,2.107133,-2.407000,0.000000,0.000000
+0.020000,9.051868,8.000000,9.061868,2.058993,-2.407000,0.000000,0.000000
+0.020000,9.092567,8.000000,9.102567,2.010853,-2.407000,0.000000,0.000000
+0.020000,9.132303,8.000000,9.142303,1.962713,-2.407000,0.000000,0.000000
+0.020000,9.171075,8.000000,9.181075,1.914573,-2.407000,-0.000000,0.000000
+0.020000,9.208885,8.000000,9.218885,1.866433,-2.407000,0.000000,0.000000
+0.020000,9.245733,8.000000,9.255733,1.818293,-2.407000,0.000000,0.000000
+0.020000,9.281617,8.000000,9.291617,1.770153,-2.407000,0.000000,0.000000
+0.020000,9.316539,8.000000,9.326539,1.722013,-2.407000,0.000000,0.000000
+0.020000,9.350498,8.000000,9.360498,1.673873,-2.407000,-0.000000,0.000000
+0.020000,9.383494,8.000000,9.393494,1.625733,-2.407000,0.000000,0.000000
+0.020000,9.415527,8.000000,9.425527,1.577593,-2.407000,0.000000,0.000000
+0.020000,9.446597,8.000000,9.456597,1.529453,-2.407000,0.000000,0.000000
+0.020000,9.476705,8.000000,9.486705,1.481313,-2.407000,-0.000000,0.000000
+0.020000,9.505850,8.000000,9.515850,1.433173,-2.407000,0.000000,0.000000
+0.020000,9.534032,8.000000,9.544032,1.385033,-2.407000,0.000000,0.000000
+0.020000,9.561251,8.000000,9.571251,1.336893,-2.407000,0.000000,0.000000
+0.020000,9.587508,8.000000,9.597508,1.288753,-2.407000,-0.000000,0.000000
+0.020000,9.612801,8.000000,9.622801,1.240613,-2.407000,0.000000,0.000000
+0.020000,9.637132,8.000000,9.647132,1.192473,-2.407000,0.000000,0.000000
+0.020000,9.660500,8.000000,9.670500,1.144333,-2.407000,0.000000,0.000000
+0.020000,9.682906,8.000000,9.692906,1.096193,-2.407000,0.000000,0.000000
+0.020000,9.704348,8.000000,9.714348,1.048053,-2.407000,0.000000,0.000000
+0.020000,9.724828,8.000000,9.734828,0.999913,-2.407000,-0.000000,0.000000
+0.020000,9.744347,8.000000,9.754347,0.952042,-2.393552,0.672385,0.000000
+0.020000,9.762921,8.000000,9.772921,0.905345,-2.334845,2.935366,0.000000
+0.020000,9.780573,8.000000,9.790573,0.859822,-2.276138,2.935366,0.000000
+0.020000,9.797326,8.000000,9.807326,0.815474,-2.217431,2.935366,0.000000
+0.020000,9.813203,8.000000,9.823203,0.772299,-2.158723,2.935366,0.000000
+0.020000,9.828229,8.000000,9.838229,0.730299,-2.100016,2.935366,0.000000
+0.020000,9.842427,8.000000,9.852427,0.689473,-2.041309,2.935366,0.000000
+0.020000,9.855820,8.000000,9.865820,0.649821,-1.982601,2.935366,0.000000
+0.020000,9.868432,8.000000,9.878432,0.611343,-1.923894,2.935366,0.000000
+0.020000,9.880286,8.000000,9.890286,0.574039,-1.865187,2.935366,0.000000
+0.020000,9.891405,8.000000,9.901405,0.537909,-1.806479,2.935366,0.000000
+0.020000,9.901814,8.000000,9.911814,0.502954,-1.747772,2.935366,0.000000
+0.020000,9.911535,8.000000,9.921535,0.469173,-1.689065,2.935366,0.000000
+0.020000,9.920592,8.000000,9.930592,0.436566,-1.630357,2.935366,0.000000
+0.020000,9.929009,8.000000,9.939009,0.405133,-1.571650,2.935366,0.000000
+0.020000,9.936809,8.000000,9.946809,0.374874,-1.512943,2.935366,0.000000
+0.020000,9.944016,8.000000,9.954016,0.345789,-1.454235,2.935366,0.000000
+0.020000,9.950653,8.000000,9.960653,0.317878,-1.395528,2.935366,0.000000
+0.020000,9.956743,8.000000,9.966743,0.291142,-1.336821,2.935366,0.000000
+0.020000,9.962310,8.000000,9.972310,0.265580,-1.278113,2.935366,0.000000
+0.020000,9.967378,8.000000,9.977378,0.241192,-1.219406,2.935366,0.000000
+0.020000,9.971970,8.000000,9.981970,0.217978,-1.160699,2.935366,0.000000
+0.020000,9.976109,8.000000,9.986109,0.195938,-1.101991,2.935366,0.000000
+0.020000,9.979819,8.000000,9.989819,0.175072,-1.043284,2.935366,0.000000
+0.020000,9.983123,8.000000,9.993123,0.155381,-0.984577,2.935366,0.000000
+0.020000,9.986046,8.000000,9.996046,0.136863,-0.925869,2.935366,0.000000
+0.020000,9.988610,8.000000,9.998610,0.119520,-0.867162,2.935366,0.000000
+0.020000,9.990838,8.000000,10.000838,0.103351,-0.808455,2.935366,0.000000
+0.020000,9.992755,8.000000,10.002755,0.088356,-0.749747,2.935366,0.000000
+0.020000,9.994384,8.000000,10.004384,0.074535,-0.691040,2.935366,0.000000
+0.020000,9.995748,8.000000,10.005748,0.061889,-0.632333,2.935366,0.000000
+0.020000,9.996872,8.000000,10.006872,0.050416,-0.573626,2.935366,0.000000
+0.020000,9.997777,8.000000,10.007777,0.040118,-0.514918,2.935366,0.000000
+0.020000,9.998488,8.000000,10.008488,0.030993,-0.456211,2.935366,0.000000
+0.020000,9.999028,8.000000,10.009028,0.023043,-0.397504,2.935366,0.000000
+0.020000,9.999421,8.000000,10.009421,0.016267,-0.338796,2.935366,0.000000
+0.020000,9.999691,8.000000,10.009691,0.010666,-0.280089,2.935366,0.000000
+0.020000,9.999860,8.000000,10.009860,0.006238,-0.221382,2.935366,0.000000
+0.020000,9.999952,8.000000,10.009952,0.002985,-0.162674,2.935366,0.000000
+0.020000,9.999991,8.000000,10.009991,0.000905,-0.103967,2.935366,0.000000
+0.020000,10.000000,8.000000,10.010000,0.000000,-0.045260,2.935366,0.000000
+0.020000,10.000000,8.000000,10.010000,0.000000,0.000000,2.262981,0.000000
diff --git a/src/main/deploy/anotherTestPath.right.pf1.csv b/src/main/deploy/anotherTestPath.right.pf1.csv
new file mode 100644
index 0000000..524f3ab
--- /dev/null
+++ b/src/main/deploy/anotherTestPath.right.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,6.750000,0.000012,0.001174,0.058707,2.935366,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,-0.058707,-5.870732,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,2.935366,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.001918,6.750000,0.001929,0.095879,4.793966,239.698284,0.000000
+0.020000,0.004559,6.750000,0.004571,0.132091,1.810608,-149.167885,0.000000
+0.020000,0.007565,6.750000,0.007577,0.150291,0.909963,-45.032225,0.000000
+0.020000,0.010959,6.750000,0.010970,0.169664,0.968671,2.935366,0.000000
+0.020000,0.014763,6.750000,0.014774,0.190212,1.027378,2.935366,0.000000
+0.020000,0.019001,6.750000,0.019013,0.211933,1.086085,2.935366,0.000000
+0.020000,0.023698,6.750000,0.023710,0.234829,1.144793,2.935366,0.000000
+0.020000,0.028876,6.750000,0.028888,0.258899,1.203500,2.935366,0.000000
+0.020000,0.034559,6.750000,0.034571,0.284143,1.262207,2.935366,0.000000
+0.020000,0.040770,6.750000,0.040782,0.310562,1.320915,2.935366,0.000000
+0.020000,0.047533,6.750000,0.047545,0.338154,1.379622,2.935366,0.000000
+0.020000,0.054872,6.750000,0.054883,0.366921,1.438329,2.935366,0.000000
+0.020000,0.062809,6.750000,0.062821,0.396861,1.497037,2.935366,0.000000
+0.020000,0.071368,6.750000,0.071380,0.427976,1.555744,2.935366,0.000000
+0.020000,0.080574,6.750000,0.080585,0.460265,1.614451,2.935366,0.000000
+0.020000,0.090448,6.750000,0.090460,0.493729,1.673159,2.935366,0.000000
+0.020000,0.101016,6.750000,0.101027,0.528366,1.731866,2.935366,0.000000
+0.020000,0.112299,6.750000,0.112311,0.564177,1.790573,2.935366,0.000000
+0.020000,0.124322,6.750000,0.124334,0.601163,1.849281,2.935366,0.000000
+0.020000,0.137109,6.750000,0.137121,0.639323,1.907988,2.935366,0.000000
+0.020000,0.150682,6.750000,0.150694,0.678657,1.966695,2.935366,0.000000
+0.020000,0.165065,6.750000,0.165077,0.719165,2.025403,2.935366,0.000000
+0.020000,0.180282,6.750000,0.180294,0.760847,2.084110,2.935366,0.000000
+0.020000,0.196356,6.750000,0.196368,0.803703,2.142817,2.935366,0.000000
+0.020000,0.213311,6.750000,0.213323,0.847734,2.201525,2.935366,0.000000
+0.020000,0.231170,6.750000,0.231181,0.892938,2.260232,2.935366,0.000000
+0.020000,0.249956,6.750000,0.249968,0.939317,2.318939,2.935366,0.000000
+0.020000,0.269693,6.750000,0.269705,0.986870,2.377647,2.935366,0.000000
+0.020000,0.290394,6.750000,0.290405,1.035010,2.407000,1.467683,0.000000
+0.020000,0.312057,6.750000,0.312068,1.083150,2.407000,0.000000,0.000000
+0.020000,0.334682,6.750000,0.334694,1.131290,2.407000,-0.000000,0.000000
+0.020000,0.358271,6.750000,0.358283,1.179430,2.407000,-0.000000,0.000000
+0.020000,0.382822,6.750000,0.382834,1.227570,2.407000,0.000000,0.000000
+0.020000,0.408337,6.750000,0.408348,1.275710,2.407000,-0.000000,0.000000
+0.020000,0.434814,6.750000,0.434825,1.323850,2.407000,-0.000000,0.000000
+0.020000,0.462253,6.750000,0.462265,1.371990,2.407000,0.000000,0.000000
+0.020000,0.490656,6.750000,0.490668,1.420130,2.407000,0.000000,0.000000
+0.020000,0.520021,6.750000,0.520033,1.468270,2.407000,-0.000000,0.000000
+0.020000,0.550350,6.750000,0.550361,1.516410,2.407000,0.000000,0.000000
+0.020000,0.581641,6.750000,0.581652,1.564550,2.407000,-0.000000,0.000000
+0.020000,0.613894,6.750000,0.613906,1.612690,2.407000,0.000000,0.000000
+0.020000,0.647111,6.750000,0.647123,1.660830,2.407000,-0.000000,0.000000
+0.020000,0.681290,6.750000,0.681302,1.708970,2.407000,0.000000,0.000000
+0.020000,0.716433,6.750000,0.716444,1.757110,2.407000,-0.000000,0.000000
+0.020000,0.752538,6.750000,0.752549,1.805250,2.407000,0.000000,0.000000
+0.020000,0.789605,6.750000,0.789617,1.853390,2.407000,-0.000000,0.000000
+0.020000,0.827636,6.750000,0.827648,1.901530,2.407000,0.000000,0.000000
+0.020000,0.866629,6.750000,0.866641,1.949670,2.407000,-0.000000,0.000000
+0.020000,0.906586,6.750000,0.906597,1.997810,2.407000,0.000000,0.000000
+0.020000,0.947505,6.750000,0.947516,2.045950,2.407000,-0.000000,0.000000
+0.020000,0.989386,6.750000,0.989398,2.094090,2.407000,0.000000,0.000000
+0.020000,1.032231,6.750000,1.032243,2.142230,2.407000,-0.000000,0.000000
+0.020000,1.076038,6.750000,1.076050,2.190370,2.407000,0.000000,0.000000
+0.020000,1.120809,6.750000,1.120820,2.238510,2.407000,-0.000000,0.000000
+0.020000,1.166542,6.750000,1.166553,2.286650,2.407000,0.000000,0.000000
+0.020000,1.213237,6.750000,1.213249,2.334790,2.407000,0.000000,0.000000
+0.020000,1.260896,6.750000,1.260908,2.382930,2.407000,0.000000,0.000000
+0.020000,1.309518,6.750000,1.309529,2.431070,2.407000,-0.000000,0.000000
+0.020000,1.359102,6.750000,1.359113,2.479210,2.407000,0.000000,0.000000
+0.020000,1.409649,6.750000,1.409660,2.527350,2.407000,-0.000000,0.000000
+0.020000,1.461159,6.750000,1.461170,2.575490,2.407000,0.000000,0.000000
+0.020000,1.513631,6.750000,1.513643,2.623630,2.407000,0.000000,0.000000
+0.020000,1.567067,6.750000,1.567078,2.671770,2.407000,-0.000000,0.000000
+0.020000,1.621465,6.750000,1.621476,2.719910,2.407000,0.000000,0.000000
+0.020000,1.676826,6.750000,1.676837,2.768050,2.407000,0.000000,0.000000
+0.020000,1.733150,6.750000,1.733161,2.816190,2.407000,0.000000,0.000000
+0.020000,1.790436,6.750000,1.790448,2.864330,2.407000,0.000000,0.000000
+0.020000,1.848686,6.750000,1.848697,2.912470,2.407000,-0.000000,0.000000
+0.020000,1.907898,6.750000,1.907909,2.960610,2.407000,0.000000,0.000000
+0.020000,1.968073,6.750000,1.968084,3.008750,2.407000,-0.000000,0.000000
+0.020000,2.029211,6.750000,2.029222,3.056890,2.407000,-0.000000,0.000000
+0.020000,2.091299,6.750000,2.091311,3.104443,2.377647,-1.467683,0.000000
+0.020000,2.154316,6.750000,2.154328,3.150822,2.318939,-2.935366,0.000000
+0.020000,2.218236,6.750000,2.218248,3.196027,2.260232,-2.935366,0.000000
+0.020000,2.283038,6.750000,2.283049,3.240057,2.201525,-2.935366,0.000000
+0.020000,2.348696,6.750000,2.348708,3.282913,2.142817,-2.935366,0.000000
+0.020000,2.415188,6.750000,2.415199,3.324596,2.084110,-2.935366,0.000000
+0.020000,2.482490,6.750000,2.482502,3.365104,2.025403,-2.935366,0.000000
+0.020000,2.550579,6.750000,2.550590,3.404438,1.966695,-2.935366,0.000000
+0.020000,2.619430,6.750000,2.619442,3.442597,1.907988,-2.935366,0.000000
+0.020000,2.689022,6.750000,2.689034,3.479583,1.849281,-2.935366,0.000000
+0.020000,2.759330,6.750000,2.759342,3.515394,1.790573,-2.935366,0.000000
+0.020000,2.830331,6.750000,2.830342,3.550032,1.731866,-2.935366,0.000000
+0.020000,2.902001,6.750000,2.902012,3.583495,1.673159,-2.935366,0.000000
+0.020000,2.974316,6.750000,2.974328,3.615784,1.614451,-2.935366,0.000000
+0.020000,3.047254,6.750000,3.047266,3.646899,1.555744,-2.935366,0.000000
+0.020000,3.120791,6.750000,3.120803,3.676840,1.497037,-2.935366,0.000000
+0.020000,3.194903,6.750000,3.194915,3.705606,1.438329,-2.935366,0.000000
+0.020000,3.269567,6.750000,3.269579,3.733199,1.379622,-2.935366,0.000000
+0.020000,3.344759,6.750000,3.344771,3.759617,1.320915,-2.935366,0.000000
+0.020000,3.420457,6.750000,3.420468,3.784861,1.262207,-2.935366,0.000000
+0.020000,3.496635,6.750000,3.496647,3.808931,1.203500,-2.935366,0.000000
+0.020000,3.573272,6.750000,3.573284,3.831827,1.144793,-2.935366,0.000000
+0.020000,3.650343,6.750000,3.650355,3.853549,1.086085,-2.935366,0.000000
+0.020000,3.727825,6.750000,3.727836,3.874096,1.027378,-2.935366,0.000000
+0.020000,3.805694,6.750000,3.805706,3.893470,0.968671,-2.935366,0.000000
+0.020000,3.883927,6.750000,3.883939,3.911669,0.909963,-2.935366,0.000000
+0.020000,3.962501,6.750000,3.962513,3.928694,0.851256,-2.935366,0.000000
+0.020000,4.041392,6.750000,4.041404,3.944545,0.792549,-2.935366,0.000000
+0.020000,4.120577,6.750000,4.120588,3.959222,0.733842,-2.935366,0.000000
+0.020000,4.200031,6.750000,4.200043,3.972724,0.675134,-2.935366,0.000000
+0.020000,4.279732,6.750000,4.279744,3.985053,0.616427,-2.935366,0.000000
+0.020000,4.359656,6.750000,4.359668,3.996207,0.557720,-2.935366,0.000000
+0.020000,4.439780,6.750000,4.439792,4.006188,0.499012,-2.935366,0.000000
+0.020000,4.520080,6.750000,4.520092,4.014994,0.440305,-2.935366,0.000000
+0.020000,4.600533,6.750000,4.600544,4.022626,0.381598,-2.935366,0.000000
+0.020000,4.681114,6.750000,4.681126,4.029083,0.322890,-2.935366,0.000000
+0.020000,4.761802,6.750000,4.761813,4.034367,0.264183,-2.935366,0.000000
+0.020000,4.842571,6.750000,4.842583,4.038477,0.205476,-2.935366,0.000000
+0.020000,4.923399,6.750000,4.923411,4.041412,0.146768,-2.935366,0.000000
+0.020000,5.004260,6.750000,5.004272,4.043039,0.081337,-3.271559,0.000000
+0.020000,5.085115,6.750000,5.085127,4.042770,-0.013448,-4.739242,0.000000
+0.020000,5.165930,6.750000,5.165942,4.040740,-0.101509,-4.403049,0.000000
+0.020000,5.246681,6.750000,5.246693,4.037535,-0.160216,-2.935366,0.000000
+0.020000,5.327344,6.750000,5.327356,4.033157,-0.218923,-2.935366,0.000000
+0.020000,5.407896,6.750000,5.407908,4.027604,-0.277631,-2.935366,0.000000
+0.020000,5.488314,6.750000,5.488325,4.020877,-0.336338,-2.935366,0.000000
+0.020000,5.568573,6.750000,5.568585,4.012977,-0.395045,-2.935366,0.000000
+0.020000,5.648651,6.750000,5.648663,4.003902,-0.453753,-2.935366,0.000000
+0.020000,5.728524,6.750000,5.728536,3.993652,-0.512460,-2.935366,0.000000
+0.020000,5.808169,6.750000,5.808181,3.982229,-0.571167,-2.935366,0.000000
+0.020000,5.887562,6.750000,5.887573,3.969631,-0.629875,-2.935366,0.000000
+0.020000,5.966679,6.750000,5.966691,3.955860,-0.688582,-2.935366,0.000000
+0.020000,6.045497,6.750000,6.045509,3.940914,-0.747289,-2.935366,0.000000
+0.020000,6.123993,6.750000,6.124005,3.924794,-0.805997,-2.935366,0.000000
+0.020000,6.202143,6.750000,6.202155,3.907500,-0.864704,-2.935366,0.000000
+0.020000,6.279924,6.750000,6.279935,3.889032,-0.923411,-2.935366,0.000000
+0.020000,6.357311,6.750000,6.357323,3.869389,-0.982119,-2.935366,0.000000
+0.020000,6.434283,6.750000,6.434295,3.848573,-1.040826,-2.935366,0.000000
+0.020000,6.510814,6.750000,6.510826,3.826582,-1.099533,-2.935366,0.000000
+0.020000,6.586883,6.750000,6.586895,3.803417,-1.158240,-2.935366,0.000000
+0.020000,6.662464,6.750000,6.662476,3.779079,-1.216948,-2.935366,0.000000
+0.020000,6.737536,6.750000,6.737547,3.753565,-1.275655,-2.935366,0.000000
+0.020000,6.812073,6.750000,6.812085,3.726878,-1.334362,-2.935366,0.000000
+0.020000,6.886054,6.750000,6.886065,3.699017,-1.393070,-2.935366,0.000000
+0.020000,6.959453,6.750000,6.959465,3.669981,-1.451777,-2.935366,0.000000
+0.020000,7.032249,6.750000,7.032260,3.639772,-1.510484,-2.935366,0.000000
+0.020000,7.104416,6.750000,7.104428,3.608388,-1.569192,-2.935366,0.000000
+0.020000,7.175933,6.750000,7.175945,3.575830,-1.627899,-2.935366,0.000000
+0.020000,7.246775,6.750000,7.246787,3.542098,-1.686606,-2.935366,0.000000
+0.020000,7.316919,6.750000,7.316931,3.507191,-1.745314,-2.935366,0.000000
+0.020000,7.386341,6.750000,7.386353,3.471111,-1.804021,-2.935366,0.000000
+0.020000,7.455018,6.750000,7.455030,3.433856,-1.862728,-2.935366,0.000000
+0.020000,7.522927,6.750000,7.522938,3.395428,-1.921436,-2.935366,0.000000
+0.020000,7.590043,6.750000,7.590055,3.355825,-1.980143,-2.935366,0.000000
+0.020000,7.656344,6.750000,7.656356,3.315048,-2.038850,-2.935366,0.000000
+0.020000,7.721806,6.750000,7.721818,3.273097,-2.097558,-2.935366,0.000000
+0.020000,7.786405,6.750000,7.786417,3.229971,-2.156265,-2.935366,0.000000
+0.020000,7.850119,6.750000,7.850131,3.185672,-2.214972,-2.935366,0.000000
+0.020000,7.912923,6.750000,7.912935,3.140198,-2.273680,-2.935366,0.000000
+0.020000,7.974794,6.750000,7.974806,3.093551,-2.332387,-2.935366,0.000000
+0.020000,8.035711,6.750000,8.035723,3.045863,-2.384370,-2.599173,0.000000
+0.020000,8.095666,6.750000,8.095677,2.997723,-2.407000,-1.131490,0.000000
+0.020000,8.154657,6.750000,8.154669,2.949583,-2.407000,-0.000000,0.000000
+0.020000,8.212686,6.750000,8.212698,2.901443,-2.407000,0.000000,0.000000
+0.020000,8.269752,6.750000,8.269764,2.853303,-2.407000,-0.000000,0.000000
+0.020000,8.325855,6.750000,8.325867,2.805163,-2.407000,0.000000,0.000000
+0.020000,8.380996,6.750000,8.381008,2.757023,-2.407000,0.000000,0.000000
+0.020000,8.435174,6.750000,8.435185,2.708883,-2.407000,0.000000,0.000000
+0.020000,8.488388,6.750000,8.488400,2.660743,-2.407000,0.000000,0.000000
+0.020000,8.540641,6.750000,8.540652,2.612603,-2.407000,0.000000,0.000000
+0.020000,8.591930,6.750000,8.591942,2.564463,-2.407000,0.000000,0.000000
+0.020000,8.642256,6.750000,8.642268,2.516323,-2.407000,0.000000,0.000000
+0.020000,8.691620,6.750000,8.691632,2.468183,-2.407000,0.000000,0.000000
+0.020000,8.740021,6.750000,8.740032,2.420043,-2.407000,0.000000,0.000000
+0.020000,8.787459,6.750000,8.787471,2.371903,-2.407000,0.000000,0.000000
+0.020000,8.833934,6.750000,8.833946,2.323763,-2.407000,0.000000,0.000000
+0.020000,8.879447,6.750000,8.879458,2.275623,-2.407000,0.000000,0.000000
+0.020000,8.923996,6.750000,8.924008,2.227483,-2.407000,0.000000,0.000000
+0.020000,8.967583,6.750000,8.967595,2.179343,-2.407000,0.000000,0.000000
+0.020000,9.010207,6.750000,9.010219,2.131203,-2.407000,0.000000,0.000000
+0.020000,9.051868,6.750000,9.051880,2.083063,-2.407000,0.000000,0.000000
+0.020000,9.092567,6.750000,9.092579,2.034923,-2.407000,0.000000,0.000000
+0.020000,9.132303,6.750000,9.132314,1.986783,-2.407000,0.000000,0.000000
+0.020000,9.171075,6.750000,9.171087,1.938643,-2.407000,0.000000,0.000000
+0.020000,9.208885,6.750000,9.208897,1.890503,-2.407000,0.000000,0.000000
+0.020000,9.245733,6.750000,9.245744,1.842363,-2.407000,0.000000,0.000000
+0.020000,9.281617,6.750000,9.281629,1.794223,-2.407000,0.000000,0.000000
+0.020000,9.316539,6.750000,9.316551,1.746083,-2.407000,0.000000,0.000000
+0.020000,9.350498,6.750000,9.350509,1.697943,-2.407000,-0.000000,0.000000
+0.020000,9.383494,6.750000,9.383505,1.649803,-2.407000,0.000000,0.000000
+0.020000,9.415527,6.750000,9.415539,1.601663,-2.407000,-0.000000,0.000000
+0.020000,9.446597,6.750000,9.446609,1.553523,-2.407000,0.000000,0.000000
+0.020000,9.476705,6.750000,9.476717,1.505383,-2.407000,-0.000000,0.000000
+0.020000,9.505850,6.750000,9.505862,1.457243,-2.407000,0.000000,0.000000
+0.020000,9.534032,6.750000,9.534044,1.409103,-2.407000,0.000000,0.000000
+0.020000,9.561251,6.750000,9.561263,1.360963,-2.407000,-0.000000,0.000000
+0.020000,9.587508,6.750000,9.587519,1.312823,-2.407000,0.000000,0.000000
+0.020000,9.612801,6.750000,9.612813,1.264683,-2.407000,-0.000000,0.000000
+0.020000,9.637132,6.750000,9.637144,1.216543,-2.407000,0.000000,0.000000
+0.020000,9.660500,6.750000,9.660512,1.168403,-2.407000,0.000000,0.000000
+0.020000,9.682906,6.750000,9.682917,1.120263,-2.407000,-0.000000,0.000000
+0.020000,9.704348,6.750000,9.704360,1.072123,-2.407000,0.000000,0.000000
+0.020000,9.724828,6.750000,9.724839,1.023983,-2.407000,-0.000000,0.000000
+0.020000,9.744347,6.750000,9.744359,0.975977,-2.400276,0.336193,0.000000
+0.020000,9.762921,6.750000,9.762933,0.928693,-2.364199,1.803876,0.000000
+0.020000,9.780573,6.750000,9.780585,0.882584,-2.305491,2.935366,0.000000
+0.020000,9.797326,6.750000,9.797337,0.837648,-2.246784,2.935366,0.000000
+0.020000,9.813203,6.750000,9.813215,0.793886,-2.188077,2.935366,0.000000
+0.020000,9.828229,6.750000,9.828241,0.751299,-2.129370,2.935366,0.000000
+0.020000,9.842427,6.750000,9.842439,0.709886,-2.070662,2.935366,0.000000
+0.020000,9.855820,6.750000,9.855832,0.669647,-2.011955,2.935366,0.000000
+0.020000,9.868432,6.750000,9.868443,0.630582,-1.953248,2.935366,0.000000
+0.020000,9.880286,6.750000,9.880297,0.592691,-1.894540,2.935366,0.000000
+0.020000,9.891405,6.750000,9.891417,0.555974,-1.835833,2.935366,0.000000
+0.020000,9.901814,6.750000,9.901825,0.520432,-1.777126,2.935366,0.000000
+0.020000,9.911535,6.750000,9.911547,0.486063,-1.718418,2.935366,0.000000
+0.020000,9.920592,6.750000,9.920604,0.452869,-1.659711,2.935366,0.000000
+0.020000,9.929009,6.750000,9.929021,0.420849,-1.601004,2.935366,0.000000
+0.020000,9.936809,6.750000,9.936821,0.390003,-1.542296,2.935366,0.000000
+0.020000,9.944016,6.750000,9.944028,0.360331,-1.483589,2.935366,0.000000
+0.020000,9.950653,6.750000,9.950664,0.331834,-1.424882,2.935366,0.000000
+0.020000,9.956743,6.750000,9.956755,0.304510,-1.366174,2.935366,0.000000
+0.020000,9.962310,6.750000,9.962322,0.278361,-1.307467,2.935366,0.000000
+0.020000,9.967378,6.750000,9.967390,0.253386,-1.248760,2.935366,0.000000
+0.020000,9.971970,6.750000,9.971981,0.229585,-1.190052,2.935366,0.000000
+0.020000,9.976109,6.750000,9.976120,0.206958,-1.131345,2.935366,0.000000
+0.020000,9.979819,6.750000,9.979831,0.185505,-1.072638,2.935366,0.000000
+0.020000,9.983123,6.750000,9.983135,0.165226,-1.013930,2.935366,0.000000
+0.020000,9.986046,6.750000,9.986057,0.146122,-0.955223,2.935366,0.000000
+0.020000,9.988610,6.750000,9.988621,0.128192,-0.896516,2.935366,0.000000
+0.020000,9.990838,6.750000,9.990850,0.111435,-0.837808,2.935366,0.000000
+0.020000,9.992755,6.750000,9.992767,0.095853,-0.779101,2.935366,0.000000
+0.020000,9.994384,6.750000,9.994396,0.081446,-0.720394,2.935366,0.000000
+0.020000,9.995748,6.750000,9.995760,0.068212,-0.661686,2.935366,0.000000
+0.020000,9.996872,6.750000,9.996883,0.056152,-0.602979,2.935366,0.000000
+0.020000,9.997777,6.750000,9.997789,0.045267,-0.544272,2.935366,0.000000
+0.020000,9.998488,6.750000,9.998500,0.035556,-0.485565,2.935366,0.000000
+0.020000,9.999028,6.750000,9.999040,0.027018,-0.426857,2.935366,0.000000
+0.020000,9.999421,6.750000,9.999433,0.019655,-0.368150,2.935366,0.000000
+0.020000,9.999691,6.750000,9.999703,0.013467,-0.309443,2.935366,0.000000
+0.020000,9.999860,6.750000,9.999872,0.008452,-0.250735,2.935366,0.000000
+0.020000,9.999952,6.750000,9.999964,0.004611,-0.192028,2.935366,0.000000
+0.020000,9.999991,6.750000,10.000003,0.001945,-0.133321,2.935366,0.000000
+0.020000,10.000000,6.750000,10.000012,0.000453,-0.074613,2.935366,0.000000
+0.020000,10.000000,6.750000,10.000012,0.000000,-0.022630,2.599173,0.000000
diff --git a/src/main/deploy/output/10S-5L.left.pf1.csv b/src/main/deploy/output/10S-5L.left.pf1.csv
new file mode 100644
index 0000000..0f0b510
--- /dev/null
+++ b/src/main/deploy/output/10S-5L.left.pf1.csv
@@ -0,0 +1,248 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,-0.001601,0.999999,0.000031,0.003125,0.156250,7.812500,0.001601
+0.020000,-0.001585,0.999999,0.000047,0.000805,-0.116007,-13.612871,0.001585
+0.020000,-0.001549,0.999999,0.000084,0.001811,0.050294,8.315085,0.001549
+0.020000,-0.001484,0.999999,0.000148,0.003218,0.070387,1.004634,0.001484
+0.020000,-0.001384,0.999999,0.000248,0.005027,0.090441,1.002714,0.001384
+0.020000,-0.001239,0.999999,0.000393,0.007236,0.110433,0.999573,0.001239
+0.020000,-0.001042,0.999999,0.000590,0.009843,0.130331,0.994905,0.001042
+0.020000,-0.000785,1.000000,0.000847,0.012845,0.150099,0.988409,0.000785
+0.020000,-0.000460,1.000000,0.001172,0.016238,0.169695,0.979786,0.000460
+0.020000,-0.000060,1.000000,0.001572,0.020020,0.189069,0.968740,0.000060
+0.020000,0.003735,0.999999,0.005367,0.189751,8.486564,414.874734,6.282762
+0.020000,0.008809,0.999996,0.010441,0.253701,3.197471,-264.454642,6.282188
+0.020000,0.014763,0.999988,0.016396,0.297721,2.201041,-49.821493,6.281515
+0.020000,0.021664,0.999973,0.023296,0.345044,2.366151,8.255479,6.280738
+0.020000,0.029585,0.999951,0.031217,0.396046,2.550065,9.195680,6.279850
+0.020000,0.038592,0.999916,0.040224,0.450321,2.713779,8.185742,6.278843
+0.020000,0.048754,0.999866,0.050386,0.508121,2.889992,8.810613,6.277711
+0.020000,0.060141,0.999797,0.061773,0.569354,3.061652,8.583023,6.276449
+0.020000,0.072819,0.999702,0.074452,0.633956,3.230082,8.421489,6.275052
+0.020000,0.086857,0.999577,0.088491,0.701917,3.398053,8.398562,6.273514
+0.020000,0.102321,0.999415,0.103955,0.773228,3.565581,8.376370,6.271830
+0.020000,0.119277,0.999207,0.120913,0.847882,3.732680,8.354974,6.269997
+0.020000,0.137793,0.998944,0.139430,0.925869,3.899369,8.334437,6.268011
+0.020000,0.157932,0.998617,0.159572,1.007073,4.060166,8.039869,6.265870
+0.020000,0.179758,0.998214,0.181402,1.091514,4.222082,8.095799,6.263571
+0.020000,0.203336,0.997722,0.204985,1.179162,4.382385,8.015160,6.261112
+0.020000,0.228730,0.997128,0.230386,1.270016,4.542694,8.015419,6.258493
+0.020000,0.256000,0.996417,0.257665,1.363954,4.696885,7.709546,6.255715
+0.020000,0.285207,0.995571,0.286884,1.460964,4.850499,7.680703,6.252776
+0.020000,0.316411,0.994573,0.318105,1.561035,5.003576,7.653870,6.249680
+0.020000,0.349674,0.993404,0.351388,1.664158,5.156155,7.628926,6.246429
+0.020000,0.385051,0.992043,0.386791,1.770163,5.300248,7.204651,6.243025
+0.020000,0.422567,0.990469,0.424340,1.877454,5.364538,3.214504,6.239477
+0.020000,0.462207,0.988662,0.464021,1.984038,5.329217,-1.766022,6.235796
+0.020000,0.503959,0.986602,0.505824,2.090156,5.305899,-1.165927,6.231992
+0.020000,0.547810,0.984268,0.549737,2.195668,5.275564,-1.516719,6.228079
+0.020000,0.593746,0.981642,0.595748,2.300538,5.243520,-1.602241,6.224069
+0.020000,0.641751,0.978701,0.643843,2.404750,5.210601,-1.645926,6.219973
+0.020000,0.691813,0.975427,0.694012,2.508458,5.185412,-1.259436,6.215805
+0.020000,0.743915,0.971800,0.746240,2.611370,5.145585,-1.991387,6.211577
+0.020000,0.798008,0.967803,0.800480,2.712031,5.033027,-5.627867,6.207304
+0.020000,0.854010,0.963424,0.856654,2.808672,4.832066,-10.048066,6.203006
+0.020000,0.911840,0.958651,0.914680,2.901321,4.632481,-9.979257,6.198698
+0.020000,0.971416,0.953476,0.974481,2.990035,4.435698,-9.839145,6.194398
+0.020000,1.032657,0.947892,1.035976,3.074752,4.235854,-9.992219,6.190121
+0.020000,1.095485,0.941893,1.099089,3.155672,4.045967,-9.494306,6.185883
+0.020000,1.159820,0.935476,1.163744,3.232721,3.852457,-9.675520,6.181697
+0.020000,1.225586,0.928641,1.229864,3.306011,3.664520,-9.396854,6.177578
+0.020000,1.292704,0.921388,1.297373,3.375426,3.470721,-9.689965,6.173537
+0.020000,1.361100,0.913721,1.366197,3.441209,3.289162,-9.077948,6.169588
+0.020000,1.430699,0.905644,1.436263,3.503290,3.104057,-9.255228,6.165741
+0.020000,1.501426,0.897164,1.507497,3.561717,2.921367,-9.134520,6.162007
+0.020000,1.573211,0.888289,1.579828,3.616535,2.740898,-9.023414,6.158395
+0.020000,1.645980,0.879031,1.653183,3.667785,2.562470,-8.921421,6.154915
+0.020000,1.719662,0.869402,1.727492,3.715445,2.383006,-8.973188,6.151574
+0.020000,1.794188,0.859414,1.802685,3.759635,2.209510,-8.674809,6.148381
+0.020000,1.869491,0.849083,1.878693,3.800384,2.037438,-8.603623,6.145343
+0.020000,1.945500,0.838426,1.955445,3.837611,1.861377,-8.803047,6.142465
+0.020000,2.022149,0.827461,2.032875,3.871496,1.694248,-8.356444,6.139754
+0.020000,2.099372,0.816207,2.110914,3.901941,1.522223,-8.601262,6.137215
+0.020000,2.177104,0.804684,2.189495,3.929083,1.357092,-8.256518,6.134853
+0.020000,2.255280,0.792914,2.268552,3.952815,1.186605,-8.524366,6.132671
+0.020000,2.333835,0.780918,2.348018,3.973305,1.024514,-8.104567,6.130674
+0.020000,2.412707,0.768721,2.427827,3.990488,0.859155,-8.267918,6.128864
+0.020000,2.491833,0.756345,2.507915,4.004378,0.694504,-8.232568,6.127245
+0.020000,2.571151,0.743816,2.588216,4.015059,0.534036,-8.023406,6.125819
+0.020000,2.650599,0.731158,2.668666,4.022510,0.372545,-8.074517,6.124588
+0.020000,2.730116,0.718396,2.749201,4.026747,0.211839,-8.035329,6.123553
+0.020000,2.809642,0.705557,2.829757,4.027792,0.052260,-7.978954,6.122716
+0.020000,2.889117,0.692666,2.910271,4.025681,-0.105527,-7.889356,6.122077
+0.020000,2.968481,0.679748,2.990679,4.020417,-0.263209,-7.884084,6.121638
+0.020000,3.047675,0.666831,3.070919,4.012017,-0.420018,-7.840468,6.121398
+0.020000,3.126670,0.653935,3.150961,4.002059,-0.497891,-3.893623,6.121356
+0.020000,3.205470,0.641075,3.230803,3.992107,-0.497592,0.014932,6.121514
+0.020000,3.284076,0.628268,3.310446,3.982143,-0.498189,-0.029816,6.121872
+0.020000,3.362491,0.615529,3.389889,3.972152,-0.499575,-0.069334,6.122429
+0.020000,3.440717,0.602873,3.469131,3.962118,-0.501676,-0.105043,6.123186
+0.020000,3.518754,0.590317,3.548172,3.952033,-0.504263,-0.129330,6.124146
+0.020000,3.596603,0.577875,3.627009,3.941888,-0.507238,-0.148762,6.125308
+0.020000,3.674267,0.565564,3.705642,3.931648,-0.512023,-0.239245,6.126675
+0.020000,3.751744,0.553399,3.784069,3.921307,-0.517052,-0.251471,6.128249
+0.020000,3.829034,0.541397,3.862286,3.910850,-0.522834,-0.289076,6.130032
+0.020000,3.906138,0.529572,3.940291,3.900262,-0.529381,-0.327371,6.132027
+0.020000,3.983054,0.517942,4.018082,3.889541,-0.536091,-0.335509,6.134236
+0.020000,4.059782,0.506521,4.095655,3.878661,-0.543998,-0.395303,6.136663
+0.020000,4.136320,0.495328,4.173007,3.867587,-0.553691,-0.484695,6.139311
+0.020000,4.212665,0.484378,4.250133,3.856316,-0.563530,-0.491922,6.142185
+0.020000,4.288815,0.473688,4.327030,3.844831,-0.574243,-0.535645,6.145288
+0.020000,4.364767,0.463275,4.403692,3.833114,-0.585859,-0.580806,6.148626
+0.020000,4.440517,0.453157,4.480115,3.821146,-0.598409,-0.627531,6.152203
+0.020000,4.516061,0.443350,4.556293,3.808907,-0.611928,-0.675945,6.156025
+0.020000,4.591395,0.433873,4.632220,3.796378,-0.626452,-0.726167,6.160097
+0.020000,4.666513,0.424743,4.707891,3.783538,-0.642018,-0.778311,6.164426
+0.020000,4.741409,0.415979,4.783298,3.770365,-0.658668,-0.832482,6.169019
+0.020000,4.816077,0.407599,4.858435,3.756836,-0.676443,-0.888772,6.173882
+0.020000,4.890510,0.399622,4.933294,3.742928,-0.695388,-0.947257,6.179024
+0.020000,4.964698,0.392068,5.007866,3.728617,-0.715548,-1.007994,6.184451
+0.020000,5.038635,0.384956,5.082144,3.713878,-0.736968,-1.071011,6.190174
+0.020000,5.112309,0.378305,5.156117,3.698684,-0.759694,-1.136309,6.196200
+0.020000,5.185710,0.372137,5.229778,3.683008,-0.783771,-1.203844,6.202540
+0.020000,5.258827,0.366470,5.303114,3.666824,-0.809242,-1.273528,6.209204
+0.020000,5.331648,0.361327,5.376116,3.650101,-0.836146,-1.345214,6.216201
+0.020000,5.404158,0.356728,5.448772,3.632810,-0.864520,-1.418686,6.223544
+0.020000,5.476345,0.352694,5.521072,3.614971,-0.891985,-1.373248,6.231245
+0.020000,5.548193,0.349247,5.593002,3.596508,-0.923131,-1.557297,6.239315
+0.020000,5.619683,0.346408,5.664549,3.577348,-0.958002,-1.743561,6.247768
+0.020000,5.690799,0.344201,5.735699,3.557499,-0.992456,-1.722705,6.256617
+0.020000,5.761523,0.342646,5.806439,3.537038,-1.023047,-1.529547,6.265877
+0.020000,5.831833,0.341767,5.876755,3.515769,-1.063458,-2.020552,6.275561
+0.020000,5.901706,0.341585,5.946629,3.493687,-1.104078,-2.030998,0.002501
+0.020000,5.971124,0.342123,6.016048,3.470985,-1.135104,-1.551311,0.013082
+0.020000,6.040058,0.343402,6.084994,3.447281,-1.185190,-2.504289,0.024135
+0.020000,6.108485,0.345446,6.153452,3.422887,-1.219716,-1.726290,0.035678
+0.020000,6.176376,0.348276,6.221402,3.397522,-1.268256,-2.427024,0.047726
+0.020000,6.243706,0.351912,6.288830,3.371418,-1.305171,-1.845723,0.060298
+0.020000,6.310443,0.356377,6.355717,3.344302,-1.355813,-2.532123,0.073411
+0.020000,6.376559,0.361690,6.422046,3.316456,-1.392299,-1.824297,0.087082
+0.020000,6.442021,0.367871,6.487799,3.287646,-1.440487,-2.409414,0.101329
+0.020000,6.506796,0.374939,6.552958,3.257975,-1.483572,-2.154225,0.116168
+0.020000,6.570853,0.382912,6.617509,3.227559,-1.520789,-1.860853,0.131616
+0.020000,6.634158,0.391807,6.681436,3.196319,-1.562016,-2.061349,0.147689
+0.020000,6.696676,0.401638,6.744722,3.164326,-1.599664,-1.882391,0.164402
+0.020000,6.758374,0.412420,6.807356,3.131680,-1.632267,-1.630144,0.181768
+0.020000,6.819220,0.424167,6.869325,3.098469,-1.660544,-1.413873,0.199797
+0.020000,6.879181,0.436889,6.930621,3.064800,-1.683480,-1.146775,0.218500
+0.020000,6.938227,0.450597,6.991237,3.030800,-1.699966,-0.824332,0.237883
+0.020000,6.996328,0.465299,7.051170,2.996624,-1.708821,-0.442744,0.257949
+0.020000,7.053458,0.481003,7.110419,2.962448,-1.708807,0.000711,0.278699
+0.020000,7.109593,0.497714,7.168988,2.928475,-1.698661,0.507277,0.300129
+0.020000,7.164713,0.515437,7.226887,2.894932,-1.677136,1.076291,0.322229
+0.020000,7.218800,0.534177,7.284128,2.862071,-1.643040,1.704768,0.344986
+0.020000,7.271842,0.553937,7.340731,2.830165,-1.595300,2.387009,0.368381
+0.020000,7.323831,0.574721,7.396722,2.799505,-1.533014,3.114307,0.392390
+0.020000,7.374765,0.596533,7.452129,2.770394,-1.455518,3.874775,0.416980
+0.020000,7.424645,0.619378,7.506992,2.743145,-1.362451,4.653359,0.442116
+0.020000,7.473479,0.643262,7.561354,2.718069,-1.253810,5.432087,0.467753
+0.020000,7.521278,0.668192,7.615263,2.695464,-1.130265,6.177217,0.493842
+0.020000,7.568057,0.694177,7.668775,2.675616,-0.992410,6.892768,0.520328
+0.020000,7.613838,0.721229,7.721951,2.658792,-0.841211,7.559927,0.547150
+0.020000,7.658643,0.749361,7.774855,2.645218,-0.678693,8.125923,0.574244
+0.020000,7.702497,0.778588,7.827557,2.635078,-0.506985,8.585394,0.601540
+0.020000,7.745428,0.808930,7.880127,2.628507,-0.328561,8.921190,0.628968
+0.020000,7.787462,0.840403,7.932639,2.625584,-0.146141,9.121006,0.656455
+0.020000,7.828627,0.873030,7.985166,2.626332,0.037419,9.177993,0.683926
+0.020000,7.868949,0.906830,8.037780,2.630717,0.219246,9.091345,0.711310
+0.020000,7.908451,0.941823,8.090553,2.638649,0.396575,8.866439,0.738535
+0.020000,7.947156,0.978030,8.143553,2.649986,0.566865,8.514517,0.765533
+0.020000,7.985081,1.015467,8.196843,2.664544,0.727903,8.051922,0.792241
+0.020000,8.022243,1.054152,8.250485,2.682102,0.877883,7.498981,0.818598
+0.020000,8.058651,1.094092,8.304530,2.702225,1.006150,6.413342,0.844551
+0.020000,8.094317,1.135299,8.359028,2.724918,1.134641,6.424572,0.870050
+0.020000,8.129246,1.177777,8.414023,2.749715,1.239848,5.260340,0.895055
+0.020000,8.163439,1.221523,8.469547,2.776216,1.325049,4.260037,0.919528
+0.020000,8.196900,1.266535,8.525633,2.804318,1.405134,4.004243,0.943441
+0.020000,8.229627,1.312804,8.582307,2.833676,1.467909,3.138785,0.966768
+0.020000,8.261615,1.360313,8.639581,2.863689,1.500633,1.636195,0.989491
+0.020000,8.292864,1.409046,8.697472,2.894573,1.544196,2.178162,1.011597
+0.020000,8.323370,1.458982,8.755989,2.925833,1.562984,0.939401,1.033079
+0.020000,8.353127,1.510096,8.815134,2.957261,1.571396,0.420568,1.053932
+0.020000,8.382134,1.562359,8.874907,2.988680,1.570955,-0.022043,1.074158
+0.020000,8.410387,1.615743,8.935306,3.019943,1.563183,-0.388586,1.093759
+0.020000,8.437883,1.670212,8.996322,3.050806,1.543157,-1.001333,1.112743
+0.020000,8.464620,1.725732,9.057944,3.081083,1.513805,-1.467597,1.131117
+0.020000,8.490600,1.782271,9.120167,3.111129,1.502324,-0.574013,1.148896
+0.020000,8.515821,1.839788,9.182970,3.140160,1.451533,-2.539587,1.166090
+0.020000,8.540287,1.898252,9.246347,3.168840,1.434002,-0.876526,1.182715
+0.020000,8.563997,1.957617,9.310272,3.196265,1.371264,-3.136923,1.198785
+0.020000,8.586959,2.017855,9.374738,3.223310,1.352269,-0.949739,1.214319
+0.020000,8.609178,2.078928,9.439727,3.249426,1.305781,-2.324379,1.229332
+0.020000,8.630658,2.140797,9.505219,3.274614,1.259404,-2.318880,1.243841
+0.020000,8.651405,2.203426,9.571195,3.298797,1.209124,-2.513967,1.257864
+0.020000,8.671429,2.266783,9.637641,3.322312,1.175788,-1.666833,1.271417
+0.020000,8.690736,2.330834,9.704539,3.344867,1.127746,-2.402086,1.284519
+0.020000,8.709337,2.395544,9.771869,3.366530,1.083115,-2.231557,1.297185
+0.020000,8.727240,2.460879,9.839613,3.387184,1.032725,-2.519471,1.309432
+0.020000,8.744447,2.526780,9.907722,3.405484,0.914983,-5.887119,1.321270
+0.020000,8.760957,2.593162,9.976127,3.420226,0.737101,-8.894093,1.332708
+0.020000,8.776771,2.659936,10.044748,3.431042,0.540807,-9.814707,1.343752
+0.020000,8.791893,2.727034,10.113529,3.439047,0.400236,-7.028571,1.354414
+0.020000,8.806327,2.794354,10.182379,3.442529,0.174138,-11.304906,1.364699
+0.020000,8.820083,2.861831,10.251244,3.443232,0.035132,-6.950294,1.374619
+0.020000,8.833170,2.929378,10.320047,3.440171,-0.153059,-9.409507,1.384183
+0.020000,8.845596,2.996904,10.388707,3.432991,-0.358985,-10.296342,1.393400
+0.020000,8.857374,3.064337,10.457161,3.422698,-0.514672,-7.784348,1.402280
+0.020000,8.868520,3.131595,10.525336,3.408747,-0.697513,-9.142017,1.410832
+0.020000,8.879046,3.198597,10.593160,3.391191,-0.877837,-9.016197,1.419066
+0.020000,8.888968,3.265265,10.660562,3.370086,-1.055240,-8.870174,1.426991
+0.020000,8.898303,3.331518,10.727469,3.345379,-1.235349,-9.005466,1.434616
+0.020000,8.907066,3.397271,10.793803,3.316709,-1.433500,-9.907536,1.441948
+0.020000,8.915277,3.462458,10.859506,3.285135,-1.578718,-7.260884,1.448997
+0.020000,8.922954,3.526998,10.924501,3.249756,-1.768946,-9.511419,1.455772
+0.020000,8.930116,3.590813,10.988717,3.210778,-1.948904,-8.997877,1.462279
+0.020000,8.936781,3.653828,11.052083,3.168314,-2.123152,-8.712428,1.468527
+0.020000,8.942970,3.715974,11.114536,3.122668,-2.282316,-7.958168,1.474522
+0.020000,8.948702,3.777171,11.176001,3.073245,-2.471139,-9.441165,1.480273
+0.020000,8.953996,3.837343,11.236405,3.020199,-2.652316,-9.058849,1.485784
+0.020000,8.958873,3.896422,11.295686,2.964037,-2.808111,-7.789730,1.491063
+0.020000,8.963353,3.954338,11.353775,2.904455,-2.979107,-8.549839,1.496115
+0.020000,8.967455,4.011012,11.410597,2.841103,-3.167580,-9.423625,1.500947
+0.020000,8.971199,4.066377,11.466089,2.774572,-3.326581,-7.950048,1.505563
+0.020000,8.974607,4.120370,11.520189,2.704993,-3.478948,-7.618349,1.509969
+0.020000,8.977695,4.172911,11.572820,2.631592,-3.670050,-9.555109,1.514170
+0.020000,8.980485,4.223937,11.623923,2.555121,-3.823519,-7.673437,1.518170
+0.020000,8.982995,4.273373,11.673423,2.474991,-4.006511,-9.149633,1.521972
+0.020000,8.985243,4.321161,11.721263,2.392031,-4.148000,-7.074452,1.525581
+0.020000,8.987248,4.367228,11.767374,2.305552,-4.323923,-8.796106,1.529001
+0.020000,8.989028,4.411512,11.811694,2.215978,-4.478727,-7.740222,1.532235
+0.020000,8.990602,4.453980,11.854191,2.124870,-4.555393,-3.833327,1.535288
+0.020000,8.991987,4.494626,11.894861,2.033473,-4.569870,-0.723816,1.538166
+0.020000,8.993201,4.533445,11.933699,1.941896,-4.578815,-0.447256,1.540877
+0.020000,8.994261,4.570435,11.970704,1.850247,-4.582448,-0.181669,1.543426
+0.020000,8.995181,4.605584,12.005864,1.758039,-4.610400,-1.397568,1.545818
+0.020000,8.995976,4.638884,12.039175,1.665514,-4.626291,-0.794555,1.548057
+0.020000,8.996658,4.670337,12.070635,1.573004,-4.625473,0.040868,1.550149
+0.020000,8.997241,4.699948,12.100251,1.480826,-4.608924,0.827455,1.552097
+0.020000,8.997735,4.727731,12.128039,1.389373,-4.572633,1.814590,1.553908
+0.020000,8.998153,4.753757,12.154068,1.301449,-4.396209,8.821172,1.555589
+0.020000,8.998504,4.778078,12.178392,1.216199,-4.262475,6.686728,1.557146
+0.020000,8.998797,4.800750,12.201066,1.133706,-4.124660,6.890748,1.558587
+0.020000,8.999040,4.821830,12.222147,1.054042,-3.983211,7.072450,1.559917
+0.020000,8.999241,4.841374,12.241692,0.977271,-3.838553,7.232857,1.561141
+0.020000,8.999405,4.859441,12.259760,0.903403,-3.693377,7.258841,1.562266
+0.020000,8.999538,4.876075,12.276394,0.831706,-3.584873,5.425186,1.563296
+0.020000,8.999646,4.891349,12.291669,0.763722,-3.399216,9.282840,1.564237
+0.020000,8.999731,4.905323,12.305643,0.698696,-3.251296,7.396011,1.565094
+0.020000,8.999799,4.918033,12.318353,0.635541,-3.157743,4.677658,1.565869
+0.020000,8.999852,4.929567,12.329888,0.576711,-2.941491,10.812604,1.566571
+0.020000,8.999892,4.939952,12.340272,0.519220,-2.874535,3.347758,1.567200
+0.020000,8.999923,4.949273,12.349593,0.466047,-2.658666,10.793489,1.567762
+0.020000,8.999946,4.957572,12.357892,0.414970,-2.553829,5.241817,1.568262
+0.020000,8.999963,4.964911,12.365231,0.366941,-2.401449,7.619025,1.568703
+0.020000,8.999976,4.971356,12.371676,0.322233,-2.235413,8.301784,1.569089
+0.020000,8.999984,4.976961,12.377282,0.280285,-2.097411,6.900119,1.569425
+0.020000,8.999990,4.981775,12.382095,0.240666,-1.980961,5.822481,1.569712
+0.020000,8.999994,4.985867,12.386187,0.204628,-1.801866,8.954778,1.569956
+0.020000,8.999997,4.989297,12.389617,0.171480,-1.657417,7.222421,1.570160
+0.020000,8.999998,4.992121,12.392442,0.141236,-1.512211,7.260310,1.570328
+0.020000,8.999999,4.994400,12.394720,0.113908,-1.366375,7.291813,1.570464
+0.020000,9.000000,4.996190,12.396510,0.089508,-1.220024,7.317516,1.570570
+0.020000,9.000000,4.997551,12.397871,0.068043,-1.073264,7.338006,1.570651
+0.020000,9.000000,4.998541,12.398861,0.049519,-0.926187,7.353874,1.570710
+0.020000,9.000000,4.999220,12.399540,0.033941,-0.778873,7.365710,1.570750
+0.020000,9.000000,4.999646,12.399966,0.021314,-0.631390,7.374109,1.570775
+0.020000,9.000000,4.999879,12.400199,0.011638,-0.483797,7.379664,1.570789
+0.020000,9.000000,4.999977,12.400297,0.004915,-0.336138,7.382970,1.570795
+0.020000,9.000000,5.000000,12.400320,0.001146,-0.188445,7.384623,1.570796
+0.020000,9.000000,5.000000,12.400320,0.000000,-0.057297,6.557411,1.570796
diff --git a/src/main/deploy/output/10S-5L.pf1.csv b/src/main/deploy/output/10S-5L.pf1.csv
new file mode 100644
index 0000000..985cbaf
--- /dev/null
+++ b/src/main/deploy/output/10S-5L.pf1.csv
@@ -0,0 +1,248 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,0.000000,0.000031,0.003125,0.156250,7.812500,0.001601
+0.020000,0.000000,0.000000,0.000156,0.009375,0.312500,7.812500,0.001585
+0.020000,0.000000,0.000000,0.000437,0.018750,0.468750,7.812500,0.001549
+0.020000,0.000000,0.000000,0.000937,0.031250,0.625000,7.812500,0.001484
+0.020000,0.000000,0.000000,0.001719,0.046875,0.781250,7.812500,0.001384
+0.020000,0.000000,0.000000,0.002844,0.065625,0.937500,7.812500,0.001239
+0.020000,0.000000,0.000000,0.004375,0.087500,1.093750,7.812500,0.001042
+0.020000,0.000000,0.000000,0.006375,0.112500,1.250000,7.812500,0.000785
+0.020000,0.000000,0.000000,0.008906,0.140625,1.406250,7.812500,0.000460
+0.020000,0.000000,0.000000,0.012031,0.171875,1.562500,7.812500,0.000060
+0.020000,0.003312,-0.000001,0.015813,0.206250,1.718750,7.812500,6.282762
+0.020000,0.007811,-0.000004,0.020313,0.243750,1.875000,7.812500,6.282188
+0.020000,0.013094,-0.000011,0.025594,0.284375,2.031250,7.812500,6.281515
+0.020000,0.019217,-0.000024,0.031719,0.328125,2.187500,7.812500,6.280738
+0.020000,0.026250,-0.000044,0.038750,0.375000,2.343750,7.812500,6.279850
+0.020000,0.034249,-0.000075,0.046750,0.425000,2.500000,7.812500,6.278843
+0.020000,0.043280,-0.000119,0.055781,0.478125,2.656250,7.812500,6.277711
+0.020000,0.053405,-0.000181,0.065906,0.534375,2.812500,7.812500,6.276449
+0.020000,0.064686,-0.000265,0.077188,0.593750,2.968750,7.812500,6.275052
+0.020000,0.077186,-0.000376,0.089688,0.656250,3.125000,7.812500,6.273514
+0.020000,0.090966,-0.000521,0.103469,0.721875,3.281250,7.812500,6.271830
+0.020000,0.106089,-0.000706,0.118594,0.790625,3.437500,7.812500,6.269997
+0.020000,0.122619,-0.000941,0.135125,0.862500,3.593750,7.812500,6.268011
+0.020000,0.140617,-0.001233,0.153125,0.937500,3.750000,7.812500,6.265870
+0.020000,0.160145,-0.001594,0.172656,1.015625,3.906250,7.812500,6.263571
+0.020000,0.181265,-0.002034,0.193781,1.096875,4.062500,7.812500,6.261112
+0.020000,0.204040,-0.002567,0.216563,1.181250,4.218750,7.812500,6.258493
+0.020000,0.228532,-0.003206,0.241063,1.268750,4.375000,7.812500,6.255715
+0.020000,0.254802,-0.003967,0.267344,1.359375,4.531250,7.812500,6.252776
+0.020000,0.282913,-0.004865,0.295469,1.453125,4.687500,7.812500,6.249680
+0.020000,0.312926,-0.005920,0.325500,1.550000,4.843750,7.812500,6.246429
+0.020000,0.344902,-0.007151,0.357500,1.650000,5.000000,7.812500,6.243025
+0.020000,0.378873,-0.008576,0.391500,1.750000,5.000000,0.000000,6.239477
+0.020000,0.414835,-0.010216,0.427500,1.850000,5.000000,0.000000,6.235796
+0.020000,0.452788,-0.012088,0.465500,1.950000,5.000000,-0.000000,6.231992
+0.020000,0.492732,-0.014214,0.505500,2.050000,5.000000,0.000000,6.228079
+0.020000,0.534664,-0.016611,0.547500,2.150000,5.000000,0.000000,6.224069
+0.020000,0.578581,-0.019301,0.591500,2.250000,5.000000,0.000000,6.219973
+0.020000,0.624484,-0.022303,0.637500,2.350000,5.000000,0.000000,6.215805
+0.020000,0.672367,-0.025637,0.685500,2.450000,5.000000,0.000000,6.211577
+0.020000,0.722200,-0.029319,0.735469,2.546875,4.843750,-7.812500,6.207304
+0.020000,0.773917,-0.033363,0.787344,2.640625,4.687500,-7.812500,6.203006
+0.020000,0.827454,-0.037782,0.841063,2.731250,4.531250,-7.812500,6.198698
+0.020000,0.882746,-0.042585,0.896563,2.818750,4.375000,-7.812500,6.194398
+0.020000,0.939728,-0.047781,0.953781,2.903125,4.218750,-7.812500,6.190121
+0.020000,0.998336,-0.053377,1.012656,2.984375,4.062500,-7.812500,6.185883
+0.020000,1.058506,-0.059378,1.073125,3.062500,3.906250,-7.812500,6.181697
+0.020000,1.120175,-0.065788,1.135125,3.137500,3.750000,-7.812500,6.177578
+0.020000,1.183276,-0.072607,1.198594,3.209375,3.593750,-7.812500,6.173537
+0.020000,1.247747,-0.079834,1.263469,3.278125,3.437500,-7.812500,6.169588
+0.020000,1.313524,-0.087468,1.329688,3.343750,3.281250,-7.812500,6.165741
+0.020000,1.380544,-0.095503,1.397188,3.406250,3.125000,-7.812500,6.162007
+0.020000,1.448744,-0.103934,1.465906,3.465625,2.968750,-7.812500,6.158395
+0.020000,1.518060,-0.112753,1.535781,3.521875,2.812500,-7.812500,6.154915
+0.020000,1.588430,-0.121950,1.606750,3.575000,2.656250,-7.812500,6.151574
+0.020000,1.659792,-0.131514,1.678750,3.625000,2.500000,-7.812500,6.148381
+0.020000,1.732084,-0.141432,1.751719,3.671875,2.343750,-7.812500,6.145343
+0.020000,1.805243,-0.151689,1.825594,3.715625,2.187500,-7.812500,6.142465
+0.020000,1.879209,-0.162270,1.900313,3.756250,2.031250,-7.812500,6.139754
+0.020000,1.953920,-0.173158,1.975812,3.793750,1.875000,-7.812500,6.137215
+0.020000,2.029315,-0.184335,2.052031,3.828125,1.718750,-7.812500,6.134853
+0.020000,2.105333,-0.195780,2.128906,3.859375,1.562500,-7.812500,6.132671
+0.020000,2.181914,-0.207474,2.206375,3.887500,1.406250,-7.812500,6.130674
+0.020000,2.258998,-0.219395,2.284375,3.912500,1.250000,-7.812500,6.128864
+0.020000,2.336524,-0.231521,2.362844,3.934375,1.093750,-7.812500,6.127245
+0.020000,2.414433,-0.243828,2.441719,3.953125,0.937500,-7.812500,6.125819
+0.020000,2.492665,-0.256292,2.520937,3.968750,0.781250,-7.812500,6.124588
+0.020000,2.571161,-0.268890,2.600437,3.981250,0.625000,-7.812500,6.123553
+0.020000,2.649861,-0.281596,2.680156,3.990625,0.468750,-7.812500,6.122716
+0.020000,2.728705,-0.294385,2.760031,3.996875,0.312500,-7.812500,6.122077
+0.020000,2.807635,-0.307231,2.840000,4.000000,0.156250,-7.812500,6.121638
+0.020000,2.886592,-0.320110,2.920000,4.000000,0.000000,-7.812500,6.121398
+0.020000,2.965547,-0.332999,3.000000,4.000000,0.000000,0.000000,6.121356
+0.020000,3.044503,-0.345884,3.080000,4.000000,0.000000,0.000000,6.121514
+0.020000,3.123461,-0.358749,3.160000,4.000000,0.000000,0.000000,6.121872
+0.020000,3.202426,-0.371577,3.240000,4.000000,0.000000,0.000000,6.122429
+0.020000,3.281399,-0.384354,3.320000,4.000000,0.000000,0.000000,6.123186
+0.020000,3.360383,-0.397063,3.400000,4.000000,0.000000,0.000000,6.124146
+0.020000,3.439381,-0.409688,3.480000,4.000000,0.000000,0.000000,6.125308
+0.020000,3.518395,-0.422213,3.560000,4.000000,0.000000,0.000000,6.126675
+0.020000,3.597426,-0.434622,3.640000,4.000000,0.000000,0.000000,6.128249
+0.020000,3.676479,-0.446898,3.720000,4.000000,0.000000,0.000000,6.130032
+0.020000,3.755554,-0.459025,3.800000,4.000000,0.000000,0.000000,6.132027
+0.020000,3.834655,-0.470986,3.880000,4.000000,0.000000,0.000000,6.134236
+0.020000,3.913784,-0.482763,3.960000,4.000000,0.000000,0.000000,6.136663
+0.020000,3.992942,-0.494340,4.040000,4.000000,0.000000,0.000000,6.139311
+0.020000,4.072132,-0.505698,4.120000,4.000000,0.000000,0.000000,6.142185
+0.020000,4.151355,-0.516819,4.200000,4.000000,0.000000,0.000000,6.145288
+0.020000,4.230613,-0.527685,4.280000,4.000000,0.000000,0.000000,6.148626
+0.020000,4.309909,-0.538277,4.360000,4.000000,0.000000,0.000000,6.152203
+0.020000,4.389243,-0.548576,4.440000,4.000000,0.000000,0.000000,6.156025
+0.020000,4.468617,-0.558562,4.520000,4.000000,0.000000,0.000000,6.160097
+0.020000,4.548033,-0.568214,4.600000,4.000000,0.000000,0.000000,6.164426
+0.020000,4.627491,-0.577511,4.680000,4.000000,0.000000,0.000000,6.169019
+0.020000,4.706992,-0.586433,4.760000,4.000000,0.000000,0.000000,6.173882
+0.020000,4.786536,-0.594958,4.840000,4.000000,0.000000,0.000000,6.179024
+0.020000,4.866125,-0.603062,4.920000,4.000000,0.000000,0.000000,6.184451
+0.020000,4.945757,-0.610722,5.000000,4.000000,0.000000,0.000000,6.190174
+0.020000,5.025433,-0.617914,5.080000,4.000000,0.000000,0.000000,6.196200
+0.020000,5.105152,-0.624613,5.160000,4.000000,0.000000,0.000000,6.202540
+0.020000,5.184913,-0.630794,5.240000,4.000000,0.000000,0.000000,6.209204
+0.020000,5.264714,-0.636431,5.320000,4.000000,0.000000,0.000000,6.216201
+0.020000,5.344553,-0.641494,5.400000,4.000000,0.000000,0.000000,6.223544
+0.020000,5.424428,-0.645958,5.480000,4.000000,0.000000,0.000000,6.231245
+0.020000,5.504337,-0.649791,5.560000,4.000000,0.000000,0.000000,6.239315
+0.020000,5.584274,-0.652965,5.640000,4.000000,0.000000,0.000000,6.247768
+0.020000,5.664234,-0.655446,5.720000,4.000000,0.000000,0.000000,6.256617
+0.020000,5.744215,-0.657204,5.800000,4.000000,0.000000,0.000000,6.265877
+0.020000,5.824209,-0.658204,5.880000,4.000000,0.000000,0.000000,6.275561
+0.020000,5.904207,-0.658412,5.960000,4.000000,0.000000,0.000000,0.002501
+0.020000,5.984205,-0.657792,6.040000,4.000000,0.000000,0.000000,0.013082
+0.020000,6.064191,-0.656306,6.120000,4.000000,0.000000,0.000000,0.024135
+0.020000,6.144155,-0.653917,6.200000,4.000000,0.000000,0.000000,0.035678
+0.020000,6.224084,-0.650586,6.280000,4.000000,0.000000,0.000000,0.047726
+0.020000,6.303968,-0.646270,6.360000,4.000000,0.000000,0.000000,0.060298
+0.020000,6.383788,-0.640930,6.440000,4.000000,0.000000,0.000000,0.073411
+0.020000,6.463531,-0.634521,6.520000,4.000000,0.000000,0.000000,0.087082
+0.020000,6.543176,-0.626999,6.600000,4.000000,0.000000,0.000000,0.101329
+0.020000,6.622703,-0.618321,6.680000,4.000000,0.000000,0.000000,0.116168
+0.020000,6.702089,-0.608439,6.760000,4.000000,0.000000,0.000000,0.131616
+0.020000,6.781311,-0.597307,6.840000,4.000000,0.000000,0.000000,0.147689
+0.020000,6.860338,-0.584879,6.920000,4.000000,0.000000,0.000000,0.164402
+0.020000,6.939143,-0.571105,7.000000,4.000000,0.000000,0.000000,0.181768
+0.020000,7.017691,-0.555940,7.080000,4.000000,0.000000,0.000000,0.199797
+0.020000,7.095947,-0.539334,7.160000,4.000000,0.000000,0.000000,0.218500
+0.020000,7.173873,-0.521242,7.240000,4.000000,0.000000,0.000000,0.237883
+0.020000,7.251426,-0.501616,7.320000,4.000000,0.000000,0.000000,0.257949
+0.020000,7.328564,-0.480411,7.400000,4.000000,0.000000,0.000000,0.278699
+0.020000,7.405236,-0.457585,7.480000,4.000000,0.000000,0.000000,0.300129
+0.020000,7.481394,-0.433095,7.560000,4.000000,0.000000,0.000000,0.322229
+0.020000,7.556983,-0.406903,7.640000,4.000000,0.000000,0.000000,0.344986
+0.020000,7.631948,-0.378975,7.720000,4.000000,0.000000,0.000000,0.368381
+0.020000,7.706229,-0.349277,7.800000,4.000000,0.000000,0.000000,0.392390
+0.020000,7.779766,-0.317783,7.880000,4.000000,0.000000,0.000000,0.416980
+0.020000,7.852498,-0.284470,7.960000,4.000000,0.000000,0.000000,0.442116
+0.020000,7.924361,-0.249322,8.040000,4.000000,0.000000,0.000000,0.467753
+0.020000,7.995290,-0.212326,8.120000,4.000000,0.000000,0.000000,0.493842
+0.020000,8.065222,-0.173479,8.200000,4.000000,0.000000,0.000000,0.520328
+0.020000,8.134094,-0.132782,8.280000,4.000000,0.000000,0.000000,0.547150
+0.020000,8.201843,-0.090243,8.360000,4.000000,0.000000,0.000000,0.574244
+0.020000,8.268410,-0.045877,8.440000,4.000000,0.000000,0.000000,0.601540
+0.020000,8.333738,0.000295,8.520000,4.000000,0.000000,0.000000,0.628968
+0.020000,8.397774,0.048242,8.600000,4.000000,0.000000,0.000000,0.656455
+0.020000,8.460468,0.097932,8.680000,4.000000,0.000000,0.000000,0.683926
+0.020000,8.521775,0.149322,8.760000,4.000000,0.000000,0.000000,0.711310
+0.020000,8.581656,0.202367,8.840000,4.000000,0.000000,0.000000,0.738535
+0.020000,8.640077,0.257017,8.920000,4.000000,0.000000,0.000000,0.765533
+0.020000,8.697010,0.313216,9.000000,4.000000,0.000000,0.000000,0.792241
+0.020000,8.752432,0.370906,9.080000,4.000000,0.000000,0.000000,0.818598
+0.020000,8.806324,0.430025,9.160000,4.000000,0.000000,0.000000,0.844551
+0.020000,8.858679,0.490511,9.240000,4.000000,0.000000,0.000000,0.870050
+0.020000,8.909490,0.552301,9.320000,4.000000,0.000000,0.000000,0.895055
+0.020000,8.958755,0.615328,9.400000,4.000000,0.000000,0.000000,0.919528
+0.020000,9.006482,0.679529,9.480000,4.000000,0.000000,0.000000,0.943441
+0.020000,9.052681,0.744842,9.560000,4.000000,0.000000,0.000000,0.966768
+0.020000,9.097362,0.811197,9.640000,4.000000,0.000000,0.000000,0.989491
+0.020000,9.140544,0.878538,9.720000,4.000000,0.000000,0.000000,1.011597
+0.020000,9.182249,0.946805,9.800000,4.000000,0.000000,0.000000,1.033079
+0.020000,9.222500,1.015939,9.880000,4.000000,0.000000,0.000000,1.053932
+0.020000,9.261323,1.085886,9.960000,4.000000,0.000000,0.000000,1.074158
+0.020000,9.298746,1.156594,10.040000,4.000000,0.000000,0.000000,1.093759
+0.020000,9.334798,1.228009,10.120000,4.000000,0.000000,0.000000,1.112743
+0.020000,9.369508,1.300082,10.200000,4.000000,0.000000,0.000000,1.131117
+0.020000,9.402912,1.372776,10.280000,4.000000,0.000000,0.000000,1.148896
+0.020000,9.435038,1.446038,10.360000,4.000000,0.000000,0.000000,1.166090
+0.020000,9.465923,1.519839,10.440000,4.000000,0.000000,0.000000,1.182715
+0.020000,9.495595,1.594127,10.520000,4.000000,0.000000,0.000000,1.198785
+0.020000,9.524091,1.668880,10.600000,4.000000,0.000000,0.000000,1.214319
+0.020000,9.551443,1.744060,10.680000,4.000000,0.000000,0.000000,1.229332
+0.020000,9.577682,1.819636,10.760000,4.000000,0.000000,0.000000,1.243841
+0.020000,9.602840,1.895576,10.840000,4.000000,0.000000,0.000000,1.257864
+0.020000,9.626948,1.971856,10.920000,4.000000,0.000000,0.000000,1.271417
+0.020000,9.650038,2.048450,11.000000,4.000000,0.000000,0.000000,1.284519
+0.020000,9.672139,2.125334,11.080000,4.000000,0.000000,0.000000,1.297185
+0.020000,9.693278,2.202480,11.159993,3.999299,-0.035027,-1.751374,1.309432
+0.020000,9.713477,2.279835,11.239941,3.995474,-0.191277,-7.812500,1.321270
+0.020000,9.732748,2.357317,11.319781,3.988523,-0.347527,-7.812500,1.332708
+0.020000,9.751107,2.434837,11.399450,3.978448,-0.503777,-7.812500,1.343752
+0.020000,9.768574,2.512336,11.478887,3.965247,-0.660027,-7.812500,1.354414
+0.020000,9.785164,2.589713,11.558029,3.948922,-0.816277,-7.812500,1.364699
+0.020000,9.800902,2.666909,11.636813,3.929471,-0.972527,-7.812500,1.374619
+0.020000,9.815808,2.743847,11.715177,3.906896,-1.128777,-7.812500,1.384183
+0.020000,9.829902,2.820437,11.793058,3.881195,-1.285027,-7.812500,1.393400
+0.020000,9.843209,2.896618,11.870393,3.852370,-1.441277,-7.812500,1.402280
+0.020000,9.855753,2.972313,11.947121,3.820419,-1.597527,-7.812500,1.410832
+0.020000,9.867557,3.047449,12.023179,3.785343,-1.753777,-7.812500,1.419066
+0.020000,9.878646,3.121955,12.098504,3.747143,-1.910027,-7.812500,1.426991
+0.020000,9.889044,3.195758,12.173033,3.705817,-2.066277,-7.812500,1.434616
+0.020000,9.898776,3.268778,12.246705,3.661367,-2.222527,-7.812500,1.441948
+0.020000,9.907869,3.340960,12.319457,3.613791,-2.378777,-7.812500,1.448997
+0.020000,9.916346,3.412227,12.391225,3.563091,-2.535027,-7.812500,1.455772
+0.020000,9.924234,3.482509,12.461949,3.509265,-2.691277,-7.812500,1.462279
+0.020000,9.931556,3.551736,12.531565,3.452315,-2.847527,-7.812500,1.468527
+0.020000,9.938339,3.619849,12.600010,3.392239,-3.003777,-7.812500,1.474522
+0.020000,9.944607,3.686771,12.667223,3.329038,-3.160027,-7.812500,1.480273
+0.020000,9.950384,3.752432,12.733141,3.262713,-3.316277,-7.812500,1.485784
+0.020000,9.955696,3.816773,12.797700,3.193262,-3.472527,-7.812500,1.491063
+0.020000,9.960565,3.879727,12.860840,3.120687,-3.628777,-7.812500,1.496115
+0.020000,9.965016,3.941220,12.922497,3.044986,-3.785027,-7.812500,1.500947
+0.020000,9.969072,4.001190,12.982608,2.966161,-3.941277,-7.812500,1.505563
+0.020000,9.972757,4.059580,13.041112,2.884210,-4.097527,-7.812500,1.509969
+0.020000,9.976093,4.116315,13.097945,2.799135,-4.253777,-7.812500,1.514170
+0.020000,9.979101,4.171335,13.153046,2.710934,-4.410027,-7.812500,1.518170
+0.020000,9.981803,4.224568,13.206351,2.619609,-4.566277,-7.812500,1.521972
+0.020000,9.984221,4.275961,13.257799,2.525158,-4.722527,-7.812500,1.525581
+0.020000,9.986375,4.325445,13.307326,2.427582,-4.878777,-7.812500,1.529001
+0.020000,9.988285,4.372960,13.354878,2.327582,-5.000000,-6.061126,1.532235
+0.020000,9.989971,4.418479,13.400430,2.227582,-5.000000,0.000000,1.535288
+0.020000,9.991455,4.462002,13.443981,2.127582,-5.000000,0.000000,1.538166
+0.020000,9.992754,4.503530,13.485533,2.027582,-5.000000,-0.000000,1.540877
+0.020000,9.993886,4.543068,13.525085,1.927582,-5.000000,0.000000,1.543426
+0.020000,9.994869,4.580608,13.562636,1.827582,-5.000000,0.000000,1.545818
+0.020000,9.995717,4.616147,13.598188,1.727582,-5.000000,0.000000,1.548057
+0.020000,9.996445,4.649691,13.631740,1.627582,-5.000000,-0.000000,1.550149
+0.020000,9.997066,4.681250,13.663298,1.528283,-4.964973,1.751374,1.552097
+0.020000,9.997592,4.710844,13.692902,1.432109,-4.808723,7.812500,1.553908
+0.020000,9.998037,4.738550,13.720614,1.339059,-4.652473,7.812500,1.555589
+0.020000,9.998410,4.764429,13.746496,1.249135,-4.496223,7.812500,1.557146
+0.020000,9.998722,4.788541,13.770610,1.162335,-4.339973,7.812500,1.558587
+0.020000,9.998981,4.810950,13.793020,1.078661,-4.183723,7.812500,1.559917
+0.020000,9.999194,4.831719,13.813788,0.998111,-4.027473,7.812500,1.561141
+0.020000,9.999369,4.850912,13.832976,0.920687,-3.871223,7.812500,1.562266
+0.020000,9.999510,4.868575,13.850647,0.846387,-3.714973,7.812500,1.563296
+0.020000,9.999624,4.884790,13.866863,0.775213,-3.558723,7.812500,1.564237
+0.020000,9.999715,4.899620,13.881687,0.707163,-3.402473,7.812500,1.565094
+0.020000,9.999787,4.913106,13.895181,0.642239,-3.246223,7.812500,1.565869
+0.020000,9.999843,4.925342,13.907407,0.580440,-3.089973,7.812500,1.566571
+0.020000,9.999886,4.936355,13.918429,0.521765,-2.933723,7.812500,1.567200
+0.020000,9.999919,4.946239,13.928309,0.466216,-2.777473,7.812500,1.567762
+0.020000,9.999943,4.955038,13.937109,0.413791,-2.621223,7.812500,1.568262
+0.020000,9.999961,4.962818,13.944892,0.364492,-2.464973,7.812500,1.568703
+0.020000,9.999974,4.969648,13.951720,0.318317,-2.308723,7.812500,1.569089
+0.020000,9.999983,4.975589,13.957656,0.275268,-2.152473,7.812500,1.569425
+0.020000,9.999990,4.980690,13.962762,0.235343,-1.996223,7.812500,1.569712
+0.020000,9.999994,4.985027,13.967101,0.198544,-1.839973,7.812500,1.569956
+0.020000,9.999996,4.988661,13.970735,0.164870,-1.683723,7.812500,1.570160
+0.020000,9.999998,4.991653,13.973727,0.134320,-1.527473,7.812500,1.570328
+0.020000,9.999999,4.994067,13.976139,0.106896,-1.371223,7.812500,1.570464
+0.020000,10.000000,4.995964,13.978034,0.082596,-1.214973,7.812500,1.570570
+0.020000,10.000000,4.997405,13.979474,0.061422,-1.058723,7.812500,1.570651
+0.020000,10.000000,4.998454,13.980522,0.043372,-0.902473,7.812500,1.570710
+0.020000,10.000000,4.999173,13.981241,0.028448,-0.746223,7.812500,1.570750
+0.020000,10.000000,4.999625,13.981691,0.016648,-0.589973,7.812500,1.570775
+0.020000,10.000000,4.999872,13.981938,0.007974,-0.433723,7.812500,1.570789
+0.020000,10.000000,4.999976,13.982042,0.002424,-0.277473,7.812500,1.570795
+0.020000,10.000000,5.000000,13.982066,0.000000,-0.121223,7.812500,1.570796
+0.020000,10.000000,5.000000,13.982066,0.000000,0.000000,6.061126,1.570796
diff --git a/src/main/deploy/output/10S-5L.right.pf1.csv b/src/main/deploy/output/10S-5L.right.pf1.csv
new file mode 100644
index 0000000..f266d3b
--- /dev/null
+++ b/src/main/deploy/output/10S-5L.right.pf1.csv
@@ -0,0 +1,248 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.001601,-0.999999,0.000031,0.003125,0.156250,7.812500,0.001601
+0.020000,0.001585,-0.999999,0.000047,0.000805,-0.116007,-13.612871,0.001585
+0.020000,0.001549,-0.999999,0.000084,0.001811,0.050294,8.315085,0.001549
+0.020000,0.001484,-0.999999,0.000148,0.003218,0.070387,1.004634,0.001484
+0.020000,0.001384,-0.999999,0.000248,0.005027,0.090441,1.002714,0.001384
+0.020000,0.001239,-0.999999,0.000393,0.007236,0.110433,0.999573,0.001239
+0.020000,0.001042,-0.999999,0.000590,0.009843,0.130331,0.994905,0.001042
+0.020000,0.000785,-1.000000,0.000847,0.012845,0.150099,0.988409,0.000785
+0.020000,0.000460,-1.000000,0.001172,0.016238,0.169695,0.979786,0.000460
+0.020000,0.000060,-1.000000,0.001572,0.020020,0.189069,0.968740,0.000060
+0.020000,0.002888,-1.000001,0.004400,0.141402,6.069089,294.000956,6.282762
+0.020000,0.006814,-1.000003,0.008326,0.196280,2.743903,-166.259274,6.282188
+0.020000,0.011424,-1.000010,0.012936,0.230498,1.710938,-51.648254,6.281515
+0.020000,0.016771,-1.000021,0.018283,0.267354,1.842776,6.591883,6.280738
+0.020000,0.022914,-1.000038,0.024426,0.307159,1.990268,7.374621,6.279850
+0.020000,0.029906,-1.000065,0.031418,0.349626,2.123351,6.654125,6.278843
+0.020000,0.037806,-1.000104,0.039318,0.394977,2.267535,7.209228,6.277711
+0.020000,0.046669,-1.000158,0.048181,0.443173,2.409777,7.112104,6.276449
+0.020000,0.056553,-1.000232,0.058065,0.494198,2.551268,7.074536,6.275052
+0.020000,0.067514,-1.000329,0.069027,0.548085,2.694342,7.153696,6.273514
+0.020000,0.079611,-1.000456,0.081125,0.604869,2.839199,7.242846,6.271830
+0.020000,0.092901,-1.000619,0.094416,0.664590,2.986043,7.342233,6.269997
+0.020000,0.107446,-1.000826,0.108962,0.727291,3.135085,7.452051,6.268011
+0.020000,0.123302,-1.001083,0.124821,0.792935,3.282198,7.355685,6.265870
+0.020000,0.140531,-1.001402,0.142053,0.861597,3.433066,7.543380,6.263571
+0.020000,0.159194,-1.001791,0.160719,0.933311,3.585727,7.633071,6.261112
+0.020000,0.179351,-1.002262,0.180882,1.008144,3.741642,7.795739,6.258493
+0.020000,0.201065,-1.002829,0.202603,1.086066,3.896114,7.723606,6.255715
+0.020000,0.224398,-1.003504,0.225946,1.167140,4.053677,7.878152,6.252776
+0.020000,0.249414,-1.004304,0.250975,1.251430,4.214522,8.042230,6.249680
+0.020000,0.276177,-1.005245,0.277755,1.339007,4.378829,8.215337,6.246429
+0.020000,0.304752,-1.006345,0.306351,1.429812,4.540274,8.072290,6.243025
+0.020000,0.335178,-1.007621,0.336804,1.522630,4.640896,5.031071,6.239477
+0.020000,0.367463,-1.009093,0.369121,1.615895,4.663233,1.116843,6.235796
+0.020000,0.401618,-1.010778,0.403318,1.709834,4.696936,1.685170,6.231992
+0.020000,0.437654,-1.012696,0.439405,1.804361,4.726385,1.472452,6.228079
+0.020000,0.475582,-1.014865,0.477395,1.899475,4.755666,1.464038,6.224069
+0.020000,0.515411,-1.017304,0.517298,1.995180,4.785266,1.480004,6.219973
+0.020000,0.557154,-1.020034,0.559131,2.091624,4.822188,1.846093,6.215805
+0.020000,0.600820,-1.023074,0.602902,2.188576,4.847637,1.272464,6.211577
+0.020000,0.646392,-1.026441,0.648599,2.284818,4.812071,-1.778306,6.207304
+0.020000,0.693823,-1.030151,0.696175,2.378825,4.700372,-5.584930,6.203006
+0.020000,0.743067,-1.034215,0.745587,2.470564,4.586943,-5.671445,6.198698
+0.020000,0.794076,-1.038646,0.796787,2.560020,4.472782,-5.708075,6.194398
+0.020000,0.846798,-1.043454,0.849728,2.647073,4.352675,-6.005337,6.190121
+0.020000,0.901187,-1.048647,0.904365,2.731827,4.237675,-5.749996,6.185883
+0.020000,0.957193,-1.054233,0.960648,2.814147,4.116026,-6.082453,6.181697
+0.020000,1.014764,-1.060217,1.018529,2.894060,3.995612,-6.020724,6.177578
+0.020000,1.073847,-1.066601,1.077957,2.971388,3.866403,-6.460427,6.173537
+0.020000,1.134394,-1.073389,1.138882,3.046273,3.744285,-6.105887,6.169588
+0.020000,1.196349,-1.080579,1.201254,3.118582,3.615456,-6.441460,6.165741
+0.020000,1.259662,-1.088170,1.265020,3.188286,3.485188,-6.513390,6.162007
+0.020000,1.324277,-1.096158,1.330127,3.255354,3.353400,-6.589414,6.158395
+0.020000,1.390141,-1.104538,1.396522,3.319755,3.220031,-6.668482,6.154915
+0.020000,1.457199,-1.113302,1.464150,3.381403,3.082388,-6.882128,6.151574
+0.020000,1.525396,-1.122442,1.532957,3.440342,2.946948,-6.771991,6.148381
+0.020000,1.594678,-1.131946,1.602888,3.496537,2.809753,-6.859771,6.145343
+0.020000,1.664987,-1.141804,1.673885,3.549855,2.665909,-7.192198,6.142465
+0.020000,1.736269,-1.152002,1.745893,3.600404,2.527461,-6.922400,6.139754
+0.020000,1.808468,-1.162524,1.818853,3.648039,2.381765,-7.284788,6.137215
+0.020000,1.881526,-1.173354,1.892710,3.692837,2.239909,-7.092790,6.134853
+0.020000,1.955386,-1.184475,1.967403,3.734649,2.090554,-7.467784,6.132671
+0.020000,2.029993,-1.195867,2.042875,3.773584,1.946754,-7.190002,6.130674
+0.020000,2.105289,-1.207511,2.119066,3.809536,1.797640,-7.455666,6.128864
+0.020000,2.181215,-1.219387,2.195915,3.842477,1.647037,-7.530154,6.127245
+0.020000,2.257716,-1.231471,2.273364,3.872445,1.498396,-7.432037,6.125819
+0.020000,2.334732,-1.243742,2.351352,3.899382,1.346827,-7.578482,6.124588
+0.020000,2.412206,-1.256175,2.429817,3.923264,1.194123,-7.635200,6.123553
+0.020000,2.490079,-1.268748,2.508699,3.944078,1.040687,-7.671803,6.122716
+0.020000,2.568293,-1.281435,2.587935,3.961823,0.887237,-7.672496,6.122077
+0.020000,2.646790,-1.294211,2.667464,3.976466,0.732169,-7.753407,6.121638
+0.020000,2.725509,-1.307051,2.747224,3.987991,0.576261,-7.795372,6.121398
+0.020000,2.804423,-1.319934,2.827183,3.997945,0.497715,-3.927323,6.121356
+0.020000,2.883535,-1.332844,2.907341,4.007900,0.497717,0.000128,6.121514
+0.020000,2.962846,-1.345766,2.987698,4.017865,0.498257,0.026989,6.121872
+0.020000,3.042361,-1.358684,3.068255,4.027854,0.499439,0.059097,6.122429
+0.020000,3.122082,-1.371581,3.149013,4.037881,0.501346,0.095352,6.123186
+0.020000,3.202013,-1.384443,3.229972,4.047965,0.504224,0.143890,6.124146
+0.020000,3.282159,-1.397251,3.311135,4.058129,0.508194,0.198499,6.125308
+0.020000,3.362523,-1.409990,3.392502,4.068365,0.511810,0.180808,6.126675
+0.020000,3.443109,-1.422643,3.474076,4.078699,0.516699,0.244457,6.128249
+0.020000,3.523924,-1.435193,3.555859,4.089147,0.522370,0.283535,6.130032
+0.020000,3.604971,-1.447623,3.637854,4.099723,0.528839,0.323467,6.132027
+0.020000,3.686256,-1.459914,3.720063,4.110459,0.536782,0.397150,6.134236
+0.020000,3.767785,-1.472048,3.802490,4.121362,0.545156,0.418707,6.136663
+0.020000,3.849564,-1.484008,3.885139,4.132429,0.553361,0.410227,6.139311
+0.020000,3.931598,-1.495774,3.968012,4.143693,0.563186,0.491272,6.142185
+0.020000,4.013894,-1.507327,4.051116,4.155171,0.573916,0.536462,6.145288
+0.020000,4.096460,-1.518646,4.134454,4.166883,0.585575,0.582948,6.148626
+0.020000,4.179301,-1.529712,4.218030,4.178847,0.598190,0.630789,6.152203
+0.020000,4.262425,-1.540502,4.301852,4.191082,0.611791,0.680041,6.156025
+0.020000,4.345840,-1.550996,4.385924,4.203611,0.626406,0.730752,6.160097
+0.020000,4.429553,-1.561170,4.470253,4.216452,0.642065,0.782965,6.164426
+0.020000,4.513572,-1.571002,4.554846,4.229628,0.658800,0.836710,6.169019
+0.020000,4.597906,-1.580466,4.639709,4.243161,0.676640,0.892009,6.173882
+0.020000,4.682563,-1.589538,4.724851,4.257073,0.695617,0.948871,6.179024
+0.020000,4.767551,-1.598191,4.810278,4.271388,0.715763,1.007288,6.184451
+0.020000,4.852880,-1.606399,4.896001,4.286130,0.737108,1.067235,6.190174
+0.020000,4.938558,-1.614133,4.982027,4.301324,0.759681,1.128664,6.196200
+0.020000,5.024595,-1.621363,5.068367,4.316994,0.783511,1.191501,6.202540
+0.020000,5.110999,-1.628059,5.155031,4.333167,0.808624,1.255642,6.209204
+0.020000,5.197780,-1.634188,5.242028,4.349868,0.835043,1.320944,6.216201
+0.020000,5.284947,-1.639716,5.329370,4.367123,0.862787,1.387221,6.223544
+0.020000,5.372511,-1.644609,5.417071,4.385020,0.894806,1.600958,6.231245
+0.020000,5.460481,-1.648829,5.505142,4.403532,0.925629,1.541154,6.239315
+0.020000,5.548864,-1.652337,5.593594,4.422634,0.955099,1.473477,6.247768
+0.020000,5.637669,-1.655093,5.682442,4.442399,0.988268,1.658452,6.256617
+0.020000,5.726907,-1.657054,5.771702,4.462991,1.029587,2.065964,6.265877
+0.020000,5.816585,-1.658175,5.861387,4.484227,1.061806,1.610959,6.275561
+0.020000,5.906708,-1.658409,5.951510,4.506161,1.096694,1.744355,0.002501
+0.020000,5.997287,-1.657706,6.042092,4.529101,1.147010,2.515813,0.013082
+0.020000,6.088324,-1.656015,6.133144,4.552607,1.175306,1.414786,0.024135
+0.020000,6.179825,-1.653281,6.224687,4.577136,1.226404,2.554941,0.035678
+0.020000,6.271793,-1.649447,6.316734,4.602355,1.260971,1.728350,0.047726
+0.020000,6.364230,-1.644453,6.409306,4.628610,1.312763,2.589603,0.060298
+0.020000,6.457133,-1.638236,6.502417,4.655545,1.346740,1.698809,0.073411
+0.020000,6.550503,-1.630731,6.596088,4.683554,1.400459,2.685960,0.087082
+0.020000,6.644332,-1.621870,6.690334,4.712296,1.437079,1.831026,0.101329
+0.020000,6.738609,-1.611581,6.785171,4.741872,1.478811,2.086596,0.116168
+0.020000,6.833326,-1.599790,6.880619,4.772390,1.525889,2.353889,0.131616
+0.020000,6.928464,-1.586421,6.976692,4.803636,1.562303,1.820706,0.147689
+0.020000,7.024001,-1.571395,7.073403,4.835579,1.597173,1.743495,0.164402
+0.020000,7.119911,-1.554631,7.170767,4.868195,1.630784,1.680546,0.181768
+0.020000,7.216161,-1.536047,7.268795,4.901391,1.659807,1.451160,0.199797
+0.020000,7.312712,-1.515558,7.367496,4.935056,1.683232,1.171231,0.218500
+0.020000,7.409518,-1.493081,7.466877,4.969055,1.699971,0.836974,0.237883
+0.020000,7.506524,-1.468531,7.566942,5.003233,1.708877,0.445288,0.257949
+0.020000,7.603669,-1.441826,7.667690,5.037408,1.708758,-0.005932,0.278699
+0.020000,7.700879,-1.412883,7.769118,5.071376,1.698409,-0.517462,0.300129
+0.020000,7.798075,-1.381627,7.871216,5.104909,1.676642,-1.088381,0.322229
+0.020000,7.895167,-1.347984,7.973971,5.137756,1.642327,-1.715703,0.344986
+0.020000,7.992054,-1.311886,8.077364,5.169644,1.594447,-2.394034,0.368381
+0.020000,8.088627,-1.273275,8.181370,5.200287,1.532141,-3.115295,0.392390
+0.020000,8.184768,-1.232099,8.285957,5.229383,1.454770,-3.868532,0.416980
+0.020000,8.280351,-1.188319,8.391090,5.256622,1.361973,-4.639885,0.442116
+0.020000,8.375242,-1.141906,8.496724,5.281696,1.253718,-5.412730,0.467753
+0.020000,8.469302,-1.092845,8.602810,5.304293,1.129829,-6.194453,0.493842
+0.020000,8.562386,-1.041136,8.709292,5.324124,0.991554,-6.913750,0.520328
+0.020000,8.654349,-0.986793,8.816111,5.340940,0.840773,-7.539046,0.547150
+0.020000,8.745043,-0.929846,8.923201,5.354511,0.678585,-8.109413,0.574244
+0.020000,8.834323,-0.870341,9.030494,5.364653,0.507069,-8.575765,0.601540
+0.020000,8.922049,-0.808340,9.137919,5.371226,0.328658,-8.920554,0.628968
+0.020000,9.008087,-0.743918,9.245402,5.374147,0.146078,-9.129037,0.656455
+0.020000,9.092309,-0.677166,9.352869,5.373393,-0.037740,-9.190899,0.683926
+0.020000,9.174602,-0.608185,9.460249,5.368997,-0.219759,-9.100951,0.711310
+0.020000,9.254862,-0.537088,9.567471,5.361058,-0.396949,-8.859483,0.738535
+0.020000,9.332999,-0.463996,9.674465,5.349731,-0.566394,-8.472256,0.765533
+0.020000,9.408938,-0.389036,9.781170,5.335223,-0.725397,-7.950119,0.792241
+0.020000,9.482621,-0.312339,9.887525,5.317791,-0.871563,-7.308317,0.818598
+0.020000,9.553997,-0.234043,9.993473,5.297371,-1.021006,-7.472133,0.844551
+0.020000,9.623040,-0.154277,10.098969,5.274829,-1.127087,-5.304091,0.870050
+0.020000,9.689733,-0.073174,10.203972,5.250146,-1.234166,-5.353907,0.895055
+0.020000,9.754071,0.009133,10.308442,5.223461,-1.334236,-5.003527,0.919528
+0.020000,9.816065,0.092523,10.412351,5.195471,-1.399516,-3.263981,0.943441
+0.020000,9.875736,0.176879,10.515679,5.166376,-1.454746,-2.761522,0.966768
+0.020000,9.933108,0.262081,10.618396,5.135892,-1.524210,-3.473188,0.989491
+0.020000,9.988224,0.348030,10.720500,5.105161,-1.536557,-0.617348,1.011597
+0.020000,10.041129,0.434628,10.821979,5.073971,-1.559515,-1.147899,1.033079
+0.020000,10.091874,0.521783,10.922830,5.042570,-1.570029,-0.525726,1.053932
+0.020000,10.140512,0.609413,11.023054,5.011192,-1.568898,0.056584,1.074158
+0.020000,10.187106,0.697444,11.122655,4.980052,-1.556981,0.595827,1.093759
+0.020000,10.231713,0.785806,11.221638,4.949143,-1.545473,0.575411,1.112743
+0.020000,10.274397,0.874433,11.320008,4.918502,-1.532035,0.671896,1.131117
+0.020000,10.315225,0.963281,11.417788,4.888970,-1.476634,2.770044,1.148896
+0.020000,10.354256,1.052289,11.514978,4.859523,-1.472341,0.214673,1.166090
+0.020000,10.391560,1.141426,11.611605,4.831374,-1.407438,3.245153,1.182715
+0.020000,10.427194,1.230638,11.707671,4.803272,-1.405081,0.117822,1.198785
+0.020000,10.461223,1.319904,11.803204,4.776641,-1.331544,3.676843,1.214319
+0.020000,10.493708,1.409193,11.898218,4.750713,-1.296444,1.755037,1.229332
+0.020000,10.524707,1.498475,11.992729,4.725536,-1.258844,1.879978,1.243841
+0.020000,10.554275,1.587725,12.086749,4.701023,-1.225625,1.660973,1.257864
+0.020000,10.582468,1.676929,12.180302,4.677658,-1.168243,2.869064,1.271417
+0.020000,10.609340,1.766067,12.273402,4.655006,-1.132637,1.780312,1.284519
+0.020000,10.634940,1.855123,12.366065,4.633138,-1.093400,1.961853,1.297185
+0.020000,10.659316,1.944081,12.458303,4.611864,-1.063677,1.486132,1.309432
+0.020000,10.682506,2.032890,12.550089,4.589344,-1.126014,-3.116850,1.321270
+0.020000,10.704539,2.121472,12.641370,4.564008,-1.266784,-7.038486,1.332708
+0.020000,10.725443,2.209739,12.732078,4.535447,-1.428063,-8.063936,1.343752
+0.020000,10.745254,2.297638,12.822182,4.505186,-1.513018,-4.247772,1.354414
+0.020000,10.764001,2.385071,12.911603,4.471021,-1.708289,-9.763551,1.364699
+0.020000,10.781721,2.471988,13.000308,4.435252,-1.788431,-4.007085,1.374619
+0.020000,10.798446,2.558315,13.088240,4.396611,-1.932062,-7.181542,1.384183
+0.020000,10.814209,2.643970,13.175333,4.354662,-2.097420,-8.267927,1.393400
+0.020000,10.829044,2.728898,13.261547,4.310685,-2.198866,-5.072299,1.402280
+0.020000,10.842986,2.813030,13.346826,4.263976,-2.335445,-6.828936,1.410832
+0.020000,10.856068,2.896300,13.431118,4.214586,-2.469497,-6.702617,1.419066
+0.020000,10.868323,2.978645,13.514369,4.162569,-2.600862,-6.568222,1.426991
+0.020000,10.879786,3.059998,13.596526,4.107835,-2.736717,-6.792762,1.434616
+0.020000,10.890487,3.140286,13.677525,4.049919,-2.895786,-7.953465,1.441948
+0.020000,10.900461,3.219462,13.757326,3.990086,-2.991636,-4.792496,1.448997
+0.020000,10.909738,3.297456,13.835870,3.927205,-3.144068,-7.621578,1.455772
+0.020000,10.918352,3.374205,13.913100,3.861496,-3.285435,-7.068366,1.462279
+0.020000,10.926331,3.449645,13.988962,3.793065,-3.421546,-6.805526,1.468527
+0.020000,10.933708,3.523723,14.063406,3.722237,-3.541426,-5.994031,1.474522
+0.020000,10.940513,3.596371,14.136372,3.648268,-3.698456,-7.851475,1.480273
+0.020000,10.946773,3.667522,14.207798,3.571310,-3.847897,-7.472052,1.485784
+0.020000,10.952519,3.737124,14.277636,3.491928,-3.969087,-6.059486,1.491063
+0.020000,10.957778,3.805115,14.345831,3.409726,-4.110104,-7.050874,1.496115
+0.020000,10.962578,3.871427,14.412316,3.324261,-4.273261,-8.157825,1.500947
+0.020000,10.966946,3.936003,14.477040,3.236190,-4.403547,-6.514321,1.505563
+0.020000,10.970908,3.998791,14.539953,3.145630,-4.527994,-6.222332,1.509969
+0.020000,10.974490,4.059719,14.600985,3.051646,-4.699199,-8.560246,1.514170
+0.020000,10.977716,4.118732,14.660087,2.955080,-4.828317,-6.455922,1.518170
+0.020000,10.980611,4.175763,14.717191,2.855214,-4.993278,-8.248033,1.521972
+0.020000,10.983199,4.230762,14.772251,2.752974,-5.111991,-5.935689,1.525581
+0.020000,10.985502,4.283662,14.825201,2.647532,-5.272087,-8.004787,1.529001
+0.020000,10.987542,4.334408,14.875988,2.539342,-5.409515,-6.871401,1.532235
+0.020000,10.989341,4.382978,14.924591,2.430155,-5.459358,-2.492118,1.535288
+0.020000,10.990922,4.429378,14.971018,2.321343,-5.440573,0.939208,1.538166
+0.020000,10.992306,4.473616,15.015278,2.212981,-5.418142,1.121573,1.540877
+0.020000,10.993512,4.515701,15.057381,2.105137,-5.392169,1.298642,1.543426
+0.020000,10.994557,4.555632,15.097325,1.997214,-5.396156,-0.199338,1.545818
+0.020000,10.995459,4.593410,15.135114,1.889444,-5.388507,0.382444,1.548057
+0.020000,10.996232,4.629045,15.170757,1.782166,-5.363923,1.229209,1.550149
+0.020000,10.996891,4.662552,15.204271,1.675696,-5.323496,2.021343,1.552097
+0.020000,10.997450,4.693956,15.235680,1.570442,-5.262665,3.041557,1.553908
+0.020000,10.997921,4.723343,15.265071,1.469531,-5.045542,10.856127,1.555589
+0.020000,10.998317,4.750779,15.292510,1.371954,-4.878861,8.334081,1.557146
+0.020000,10.998648,4.776332,15.318065,1.277765,-4.709487,8.468704,1.558587
+0.020000,10.998922,4.800071,15.341805,1.187009,-4.537758,8.586405,1.559917
+0.020000,10.999148,4.822064,15.363800,1.099729,-4.364002,8.687800,1.561141
+0.020000,10.999332,4.842382,15.384118,1.015907,-4.191098,8.645211,1.562266
+0.020000,10.999482,4.861075,15.402812,0.934694,-4.060691,6.520363,1.563296
+0.020000,10.999603,4.878230,15.419968,0.857799,-3.844726,10.798263,1.564237
+0.020000,10.999699,4.893917,15.435655,0.784354,-3.672263,8.623138,1.565094
+0.020000,10.999775,4.908179,15.449917,0.713119,-3.561733,5.526485,1.565869
+0.020000,10.999834,4.921116,15.462854,0.646832,-3.314359,12.368733,1.566571
+0.020000,10.999879,4.932758,15.474496,0.582128,-3.235218,3.957040,1.567200
+0.020000,10.999914,4.943205,15.484943,0.522333,-2.989752,12.273311,1.567762
+0.020000,10.999940,4.952504,15.494242,0.464945,-2.869369,6.019147,1.568262
+0.020000,10.999959,4.960724,15.502462,0.411021,-2.696209,8.657981,1.568703
+0.020000,10.999973,4.967941,15.509679,0.360856,-2.508230,9.398966,1.569089
+0.020000,10.999982,4.974218,15.515956,0.313815,-2.352055,7.808720,1.569425
+0.020000,10.999989,4.979606,15.521344,0.269408,-2.220346,6.585469,1.569712
+0.020000,10.999993,4.984186,15.525925,0.229032,-2.018805,10.077072,1.569956
+0.020000,10.999996,4.988025,15.529763,0.191906,-1.856311,8.124690,1.570160
+0.020000,10.999998,4.991185,15.532923,0.158042,-1.693181,8.156492,1.570328
+0.020000,10.999999,4.993734,15.535473,0.127452,-1.529522,8.182949,1.570464
+0.020000,11.000000,4.995737,15.537475,0.100143,-1.365431,8.204543,1.570570
+0.020000,11.000000,4.997260,15.538998,0.076123,-1.200996,8.221764,1.570651
+0.020000,11.000000,4.998368,15.540106,0.055398,-1.036294,8.235103,1.570710
+0.020000,11.000000,4.999127,15.540865,0.037970,-0.871393,8.245055,1.570750
+0.020000,11.000000,4.999604,15.541342,0.023843,-0.706350,8.252118,1.570775
+0.020000,11.000000,4.999864,15.541602,0.013018,-0.541214,8.256789,1.570789
+0.020000,11.000000,4.999974,15.541712,0.005498,-0.376023,8.259570,1.570795
+0.020000,11.000000,5.000000,15.541738,0.001282,-0.210804,8.260960,1.570796
+0.020000,11.000000,5.000000,15.541738,0.000000,-0.064095,7.335440,1.570796
diff --git a/src/main/deploy/output/BlueSideTrench.left.pf1.csv b/src/main/deploy/output/BlueSideTrench.left.pf1.csv
new file mode 100644
index 0000000..09f0d23
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench.left.pf1.csv
@@ -0,0 +1,186 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,42.000000,23.000000,0.000031,0.003125,0.156250,7.812500,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,-0.156250,-15.625000,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,7.812500,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,41.996969,23.000000,0.003062,0.151562,7.578125,378.906250,3.141593
+0.020000,41.993187,23.000000,0.006844,0.189063,1.875000,-285.156250,3.141593
+0.020000,41.988687,23.000000,0.011344,0.225000,1.796875,-3.906250,3.141593
+0.020000,41.983406,23.000000,0.016625,0.264062,1.953125,7.812500,3.141593
+0.020000,41.977281,23.000000,0.022750,0.306250,2.109375,7.812500,3.141593
+0.020000,41.970250,23.000000,0.029781,0.351562,2.265625,7.812500,3.141593
+0.020000,41.962250,23.000000,0.037781,0.400000,2.421875,7.812500,3.141593
+0.020000,41.953219,23.000000,0.046813,0.451562,2.578125,7.812500,3.141593
+0.020000,41.943094,23.000000,0.056937,0.506250,2.734375,7.812500,3.141593
+0.020000,41.931812,23.000000,0.068219,0.564063,2.890625,7.812500,3.141593
+0.020000,41.919312,23.000000,0.080719,0.625000,3.046875,7.812500,3.141593
+0.020000,41.905531,23.000000,0.094500,0.689062,3.203125,7.812500,3.141593
+0.020000,41.890406,23.000000,0.109625,0.756250,3.359375,7.812500,3.141593
+0.020000,41.873875,23.000000,0.126156,0.826562,3.515625,7.812500,3.141593
+0.020000,41.855875,23.000000,0.144156,0.900000,3.671875,7.812500,3.141593
+0.020000,41.836344,23.000000,0.163688,0.976563,3.828125,7.812500,3.141593
+0.020000,41.815219,23.000000,0.184813,1.056250,3.984375,7.812500,3.141593
+0.020000,41.792437,23.000000,0.207594,1.139063,4.140625,7.812500,3.141593
+0.020000,41.767938,23.000000,0.232094,1.225000,4.296875,7.812500,3.141593
+0.020000,41.741656,23.000000,0.258375,1.314063,4.453125,7.812500,3.141593
+0.020000,41.713531,23.000000,0.286500,1.406250,4.609375,7.812500,3.141593
+0.020000,41.683500,23.000000,0.316531,1.501563,4.765625,7.812500,3.141593
+0.020000,41.651500,23.000000,0.348531,1.600000,4.921875,7.812500,3.141593
+0.020000,41.617500,23.000000,0.382531,1.700000,5.000000,3.906250,3.141593
+0.020000,41.581500,23.000000,0.418531,1.800000,5.000000,0.000000,3.141593
+0.020000,41.543500,23.000000,0.456531,1.900000,5.000000,-0.000000,3.141593
+0.020000,41.503500,23.000000,0.496531,2.000000,5.000000,0.000000,3.141593
+0.020000,41.461500,23.000000,0.538531,2.100000,5.000000,0.000000,3.141593
+0.020000,41.417500,23.000000,0.582531,2.200000,5.000000,0.000000,3.141593
+0.020000,41.371500,23.000000,0.628531,2.300000,5.000000,-0.000000,3.141593
+0.020000,41.323500,23.000000,0.676531,2.400000,5.000000,0.000000,3.141593
+0.020000,41.273531,23.000000,0.726500,2.498438,4.921875,-3.906250,3.141593
+0.020000,41.221656,23.000000,0.778375,2.593750,4.765625,-7.812500,3.141593
+0.020000,41.167938,23.000000,0.832094,2.685938,4.609375,-7.812500,3.141593
+0.020000,41.112437,23.000000,0.887594,2.775000,4.453125,-7.812500,3.141593
+0.020000,41.055219,23.000000,0.944812,2.860937,4.296875,-7.812500,3.141593
+0.020000,40.996344,23.000000,1.003687,2.943750,4.140625,-7.812500,3.141593
+0.020000,40.935875,23.000000,1.064156,3.023437,3.984375,-7.812500,3.141593
+0.020000,40.873875,23.000000,1.126156,3.100000,3.828125,-7.812500,3.141593
+0.020000,40.810406,23.000000,1.189625,3.173437,3.671875,-7.812500,3.141593
+0.020000,40.745531,23.000000,1.254500,3.243750,3.515625,-7.812500,3.141593
+0.020000,40.679313,23.000000,1.320719,3.310937,3.359375,-7.812500,3.141593
+0.020000,40.611812,23.000000,1.388219,3.375000,3.203125,-7.812500,3.141593
+0.020000,40.543094,23.000000,1.456937,3.435937,3.046875,-7.812500,3.141593
+0.020000,40.473219,23.000000,1.526812,3.493750,2.890625,-7.812500,3.141593
+0.020000,40.402250,23.000000,1.597781,3.548437,2.734375,-7.812500,3.141593
+0.020000,40.330250,23.000000,1.669781,3.600000,2.578125,-7.812500,3.141593
+0.020000,40.257281,23.000000,1.742750,3.648438,2.421875,-7.812500,3.141593
+0.020000,40.183406,23.000000,1.816625,3.693750,2.265625,-7.812500,3.141593
+0.020000,40.108688,23.000000,1.891344,3.735938,2.109375,-7.812500,3.141593
+0.020000,40.033188,23.000000,1.966844,3.775000,1.953125,-7.812500,3.141593
+0.020000,39.956969,23.000000,2.043062,3.810938,1.796875,-7.812500,3.141593
+0.020000,39.880094,23.000000,2.119937,3.843750,1.640625,-7.812500,3.141593
+0.020000,39.802625,23.000000,2.197406,3.873438,1.484375,-7.812500,3.141593
+0.020000,39.724625,23.000000,2.275406,3.900000,1.328125,-7.812500,3.141593
+0.020000,39.646156,23.000000,2.353875,3.923437,1.171875,-7.812500,3.141593
+0.020000,39.567281,23.000000,2.432750,3.943750,1.015625,-7.812500,3.141593
+0.020000,39.488063,23.000000,2.511969,3.960937,0.859375,-7.812500,3.141593
+0.020000,39.408563,23.000000,2.591469,3.975000,0.703125,-7.812500,3.141593
+0.020000,39.328844,23.000000,2.671187,3.985937,0.546875,-7.812500,3.141593
+0.020000,39.248969,23.000000,2.751062,3.993750,0.390625,-7.812500,3.141593
+0.020000,39.169000,23.000000,2.831031,3.998437,0.234375,-7.812500,3.141593
+0.020000,39.089000,23.000000,2.911031,4.000000,0.078125,-7.812500,3.141593
+0.020000,39.009000,23.000000,2.991031,4.000000,0.000000,-3.906250,3.141593
+0.020000,38.929000,23.000000,3.071031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,38.849000,23.000000,3.151031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.769000,23.000000,3.231031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.689000,23.000000,3.311031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.609000,23.000000,3.391031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,38.529000,23.000000,3.471031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.449000,23.000000,3.551031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.369000,23.000000,3.631031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.289000,23.000000,3.711031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,38.209000,23.000000,3.791031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.129000,23.000000,3.871031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.049000,23.000000,3.951031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.969000,23.000000,4.031031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.889000,23.000000,4.111031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,37.809000,23.000000,4.191031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.729000,23.000000,4.271031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.649000,23.000000,4.351031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.569000,23.000000,4.431031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,37.489000,23.000000,4.511031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.409000,23.000000,4.591031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.329000,23.000000,4.671031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.249000,23.000000,4.751031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,37.169000,23.000000,4.831031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.089000,23.000000,4.911031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.009000,23.000000,4.991031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,36.929000,23.000000,5.071031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.849000,23.000000,5.151031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.769000,23.000000,5.231031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.689000,23.000000,5.311031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.609000,23.000000,5.391031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,36.529000,23.000000,5.471031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.449000,23.000000,5.551031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.369000,23.000000,5.631031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.289000,23.000000,5.711031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,36.209000,23.000000,5.791031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.129000,23.000000,5.871031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.049000,23.000000,5.951031,4.000000,0.000000,0.000000,3.141593
+0.020000,35.969000,23.000000,6.031031,4.000000,0.000000,0.000000,3.141593
+0.020000,35.889000,23.000000,6.111031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,35.809012,23.000000,6.191019,3.999395,-0.030273,-1.513672,3.141593
+0.020000,35.729080,23.000000,6.270952,3.996621,-0.138672,-5.419922,3.141593
+0.020000,35.649265,23.000000,6.350766,3.990723,-0.294922,-7.812500,3.141593
+0.020000,35.569631,23.000000,6.430400,3.981699,-0.451172,-7.812500,3.141593
+0.020000,35.490240,23.000000,6.509791,3.969551,-0.607422,-7.812500,3.141593
+0.020000,35.411155,23.000000,6.588877,3.954277,-0.763672,-7.812500,3.141593
+0.020000,35.332437,23.000000,6.667594,3.935879,-0.919922,-7.812500,3.141593
+0.020000,35.254150,23.000000,6.745881,3.914355,-1.076172,-7.812500,3.141593
+0.020000,35.176356,23.000000,6.823675,3.889707,-1.232422,-7.812500,3.141593
+0.020000,35.099117,23.000000,6.900914,3.861934,-1.388672,-7.812500,3.141593
+0.020000,35.022496,23.000000,6.977535,3.831035,-1.544922,-7.812500,3.141593
+0.020000,34.946556,23.000000,7.053475,3.797012,-1.701172,-7.812500,3.141593
+0.020000,34.871359,23.000000,7.128672,3.759863,-1.857422,-7.812500,3.141593
+0.020000,34.796967,23.000000,7.203064,3.719590,-2.013672,-7.812500,3.141593
+0.020000,34.723443,23.000000,7.276588,3.676191,-2.169922,-7.812500,3.141593
+0.020000,34.650850,23.000000,7.349181,3.629668,-2.326172,-7.812500,3.141593
+0.020000,34.579250,23.000000,7.420782,3.580020,-2.482422,-7.812500,3.141593
+0.020000,34.508705,23.000000,7.491327,3.527246,-2.638672,-7.812500,3.141593
+0.020000,34.439278,23.000000,7.560754,3.471348,-2.794922,-7.812500,3.141593
+0.020000,34.371031,23.000000,7.629000,3.412324,-2.951172,-7.812500,3.141593
+0.020000,34.304028,23.000000,7.696004,3.350176,-3.107422,-7.812500,3.141593
+0.020000,34.238330,23.000000,7.761702,3.284902,-3.263672,-7.812500,3.141593
+0.020000,34.174000,23.000000,7.826032,3.216504,-3.419922,-7.812500,3.141593
+0.020000,34.111100,23.000000,7.888931,3.144980,-3.576172,-7.812500,3.141593
+0.020000,34.049693,23.000000,7.950338,3.070332,-3.732422,-7.812500,3.141593
+0.020000,33.989842,23.000000,8.010189,2.992559,-3.888672,-7.812500,3.141593
+0.020000,33.931609,23.000000,8.068422,2.911660,-4.044922,-7.812500,3.141593
+0.020000,33.875056,23.000000,8.124975,2.827637,-4.201172,-7.812500,3.141593
+0.020000,33.820246,23.000000,8.179785,2.740488,-4.357422,-7.812500,3.141593
+0.020000,33.767242,23.000000,8.232789,2.650215,-4.513672,-7.812500,3.141593
+0.020000,33.716106,23.000000,8.283925,2.556816,-4.669922,-7.812500,3.141593
+0.020000,33.666900,23.000000,8.333131,2.460293,-4.826172,-7.812500,3.141593
+0.020000,33.619675,23.000000,8.380356,2.361250,-4.952148,-6.298828,3.141593
+0.020000,33.574450,23.000000,8.425581,2.261250,-5.000000,-2.392578,3.141593
+0.020000,33.531225,23.000000,8.468806,2.161250,-5.000000,-0.000000,3.141593
+0.020000,33.490000,23.000000,8.510031,2.061250,-5.000000,0.000000,3.141593
+0.020000,33.450775,23.000000,8.549256,1.961250,-5.000000,0.000000,3.141593
+0.020000,33.413550,23.000000,8.586481,1.861250,-5.000000,-0.000000,3.141593
+0.020000,33.378325,23.000000,8.621706,1.761250,-5.000000,0.000000,3.141593
+0.020000,33.345100,23.000000,8.654931,1.661250,-5.000000,0.000000,3.141593
+0.020000,33.313863,23.000000,8.686168,1.561855,-4.969727,1.513672,3.141593
+0.020000,33.284570,23.000000,8.715461,1.464629,-4.861328,5.419922,3.141593
+0.020000,33.257160,23.000000,8.742871,1.370527,-4.705078,7.812500,3.141593
+0.020000,33.231569,23.000000,8.768462,1.279551,-4.548828,7.812500,3.141593
+0.020000,33.207735,23.000000,8.792296,1.191699,-4.392578,7.812500,3.141593
+0.020000,33.185595,23.000000,8.814436,1.106973,-4.236328,7.812500,3.141593
+0.020000,33.165088,23.000000,8.834943,1.025371,-4.080078,7.812500,3.141593
+0.020000,33.146150,23.000000,8.853881,0.946895,-3.923828,7.812500,3.141593
+0.020000,33.128719,23.000000,8.871312,0.871543,-3.767578,7.812500,3.141593
+0.020000,33.112733,23.000000,8.887298,0.799316,-3.611328,7.812500,3.141593
+0.020000,33.098129,23.000000,8.901903,0.730215,-3.455078,7.812500,3.141593
+0.020000,33.084844,23.000000,8.915188,0.664238,-3.298828,7.812500,3.141593
+0.020000,33.072816,23.000000,8.927215,0.601387,-3.142578,7.812500,3.141593
+0.020000,33.061983,23.000000,8.938048,0.541660,-2.986328,7.812500,3.141593
+0.020000,33.052282,23.000000,8.947750,0.485059,-2.830078,7.812500,3.141593
+0.020000,33.043650,23.000000,8.956381,0.431582,-2.673828,7.812500,3.141593
+0.020000,33.036025,23.000000,8.964006,0.381230,-2.517578,7.812500,3.141593
+0.020000,33.029345,23.000000,8.970686,0.334004,-2.361328,7.812500,3.141593
+0.020000,33.023547,23.000000,8.976484,0.289902,-2.205078,7.812500,3.141593
+0.020000,33.018569,23.000000,8.981462,0.248926,-2.048828,7.812500,3.141593
+0.020000,33.014347,23.000000,8.985684,0.211074,-1.892578,7.812500,3.141593
+0.020000,33.010820,23.000000,8.989211,0.176348,-1.736328,7.812500,3.141593
+0.020000,33.007925,23.000000,8.992106,0.144746,-1.580078,7.812500,3.141593
+0.020000,33.005600,23.000000,8.994431,0.116270,-1.423828,7.812500,3.141593
+0.020000,33.003782,23.000000,8.996250,0.090918,-1.267578,7.812500,3.141593
+0.020000,33.002408,23.000000,8.997623,0.068691,-1.111328,7.812500,3.141593
+0.020000,33.001416,23.000000,8.998615,0.049590,-0.955078,7.812500,3.141593
+0.020000,33.000744,23.000000,8.999288,0.033613,-0.798828,7.812500,3.141593
+0.020000,33.000329,23.000000,8.999703,0.020762,-0.642578,7.812500,3.141593
+0.020000,33.000108,23.000000,8.999923,0.011035,-0.486328,7.812500,3.141593
+0.020000,33.000019,23.000000,9.000012,0.004434,-0.330078,7.812500,3.141593
+0.020000,33.000000,23.000000,9.000031,0.000957,-0.173828,7.812500,3.141593
+0.020000,33.000000,23.000000,9.000031,0.000000,-0.047852,6.298828,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench.pf1.csv b/src/main/deploy/output/BlueSideTrench.pf1.csv
new file mode 100644
index 0000000..884e9c6
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench.pf1.csv
@@ -0,0 +1,186 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,42.000000,24.000000,0.000031,0.003125,0.156250,7.812500,3.141593
+0.020000,42.000000,24.000000,0.000156,0.009375,0.312500,7.812500,3.141593
+0.020000,42.000000,24.000000,0.000437,0.018750,0.468750,7.812500,3.141593
+0.020000,42.000000,24.000000,0.000937,0.031250,0.625000,7.812500,3.141593
+0.020000,42.000000,24.000000,0.001719,0.046875,0.781250,7.812500,3.141593
+0.020000,42.000000,24.000000,0.002844,0.065625,0.937500,7.812500,3.141593
+0.020000,42.000000,24.000000,0.004375,0.087500,1.093750,7.812500,3.141593
+0.020000,42.000000,24.000000,0.006375,0.112500,1.250000,7.812500,3.141593
+0.020000,42.000000,24.000000,0.008906,0.140625,1.406250,7.812500,3.141593
+0.020000,41.996969,24.000000,0.012031,0.171875,1.562500,7.812500,3.141593
+0.020000,41.993187,24.000000,0.015813,0.206250,1.718750,7.812500,3.141593
+0.020000,41.988687,24.000000,0.020313,0.243750,1.875000,7.812500,3.141593
+0.020000,41.983406,24.000000,0.025594,0.284375,2.031250,7.812500,3.141593
+0.020000,41.977281,24.000000,0.031719,0.328125,2.187500,7.812500,3.141593
+0.020000,41.970250,24.000000,0.038750,0.375000,2.343750,7.812500,3.141593
+0.020000,41.962250,24.000000,0.046750,0.425000,2.500000,7.812500,3.141593
+0.020000,41.953219,24.000000,0.055781,0.478125,2.656250,7.812500,3.141593
+0.020000,41.943094,24.000000,0.065906,0.534375,2.812500,7.812500,3.141593
+0.020000,41.931812,24.000000,0.077188,0.593750,2.968750,7.812500,3.141593
+0.020000,41.919312,24.000000,0.089688,0.656250,3.125000,7.812500,3.141593
+0.020000,41.905531,24.000000,0.103469,0.721875,3.281250,7.812500,3.141593
+0.020000,41.890406,24.000000,0.118594,0.790625,3.437500,7.812500,3.141593
+0.020000,41.873875,24.000000,0.135125,0.862500,3.593750,7.812500,3.141593
+0.020000,41.855875,24.000000,0.153125,0.937500,3.750000,7.812500,3.141593
+0.020000,41.836344,24.000000,0.172656,1.015625,3.906250,7.812500,3.141593
+0.020000,41.815219,24.000000,0.193781,1.096875,4.062500,7.812500,3.141593
+0.020000,41.792437,24.000000,0.216563,1.181250,4.218750,7.812500,3.141593
+0.020000,41.767938,24.000000,0.241063,1.268750,4.375000,7.812500,3.141593
+0.020000,41.741656,24.000000,0.267344,1.359375,4.531250,7.812500,3.141593
+0.020000,41.713531,24.000000,0.295469,1.453125,4.687500,7.812500,3.141593
+0.020000,41.683500,24.000000,0.325500,1.550000,4.843750,7.812500,3.141593
+0.020000,41.651500,24.000000,0.357500,1.650000,5.000000,7.812500,3.141593
+0.020000,41.617500,24.000000,0.391500,1.750000,5.000000,0.000000,3.141593
+0.020000,41.581500,24.000000,0.427500,1.850000,5.000000,0.000000,3.141593
+0.020000,41.543500,24.000000,0.465500,1.950000,5.000000,-0.000000,3.141593
+0.020000,41.503500,24.000000,0.505500,2.050000,5.000000,0.000000,3.141593
+0.020000,41.461500,24.000000,0.547500,2.150000,5.000000,0.000000,3.141593
+0.020000,41.417500,24.000000,0.591500,2.250000,5.000000,0.000000,3.141593
+0.020000,41.371500,24.000000,0.637500,2.350000,5.000000,0.000000,3.141593
+0.020000,41.323500,24.000000,0.685500,2.450000,5.000000,0.000000,3.141593
+0.020000,41.273531,24.000000,0.735469,2.546875,4.843750,-7.812500,3.141593
+0.020000,41.221656,24.000000,0.787344,2.640625,4.687500,-7.812500,3.141593
+0.020000,41.167938,24.000000,0.841063,2.731250,4.531250,-7.812500,3.141593
+0.020000,41.112437,24.000000,0.896563,2.818750,4.375000,-7.812500,3.141593
+0.020000,41.055219,24.000000,0.953781,2.903125,4.218750,-7.812500,3.141593
+0.020000,40.996344,24.000000,1.012656,2.984375,4.062500,-7.812500,3.141593
+0.020000,40.935875,24.000000,1.073125,3.062500,3.906250,-7.812500,3.141593
+0.020000,40.873875,24.000000,1.135125,3.137500,3.750000,-7.812500,3.141593
+0.020000,40.810406,24.000000,1.198594,3.209375,3.593750,-7.812500,3.141593
+0.020000,40.745531,24.000000,1.263469,3.278125,3.437500,-7.812500,3.141593
+0.020000,40.679313,24.000000,1.329688,3.343750,3.281250,-7.812500,3.141593
+0.020000,40.611812,24.000000,1.397188,3.406250,3.125000,-7.812500,3.141593
+0.020000,40.543094,24.000000,1.465906,3.465625,2.968750,-7.812500,3.141593
+0.020000,40.473219,24.000000,1.535781,3.521875,2.812500,-7.812500,3.141593
+0.020000,40.402250,24.000000,1.606750,3.575000,2.656250,-7.812500,3.141593
+0.020000,40.330250,24.000000,1.678750,3.625000,2.500000,-7.812500,3.141593
+0.020000,40.257281,24.000000,1.751719,3.671875,2.343750,-7.812500,3.141593
+0.020000,40.183406,24.000000,1.825594,3.715625,2.187500,-7.812500,3.141593
+0.020000,40.108688,24.000000,1.900313,3.756250,2.031250,-7.812500,3.141593
+0.020000,40.033188,24.000000,1.975812,3.793750,1.875000,-7.812500,3.141593
+0.020000,39.956969,24.000000,2.052031,3.828125,1.718750,-7.812500,3.141593
+0.020000,39.880094,24.000000,2.128906,3.859375,1.562500,-7.812500,3.141593
+0.020000,39.802625,24.000000,2.206375,3.887500,1.406250,-7.812500,3.141593
+0.020000,39.724625,24.000000,2.284375,3.912500,1.250000,-7.812500,3.141593
+0.020000,39.646156,24.000000,2.362844,3.934375,1.093750,-7.812500,3.141593
+0.020000,39.567281,24.000000,2.441719,3.953125,0.937500,-7.812500,3.141593
+0.020000,39.488063,24.000000,2.520937,3.968750,0.781250,-7.812500,3.141593
+0.020000,39.408563,24.000000,2.600437,3.981250,0.625000,-7.812500,3.141593
+0.020000,39.328844,24.000000,2.680156,3.990625,0.468750,-7.812500,3.141593
+0.020000,39.248969,24.000000,2.760031,3.996875,0.312500,-7.812500,3.141593
+0.020000,39.169000,24.000000,2.840000,4.000000,0.156250,-7.812500,3.141593
+0.020000,39.089000,24.000000,2.920000,4.000000,0.000000,-7.812500,3.141593
+0.020000,39.009000,24.000000,3.000000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.929000,24.000000,3.080000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.849000,24.000000,3.160000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.769000,24.000000,3.240000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.689000,24.000000,3.320000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.609000,24.000000,3.400000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.529000,24.000000,3.480000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.449000,24.000000,3.560000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.369000,24.000000,3.640000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.289000,24.000000,3.720000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.209000,24.000000,3.800000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.129000,24.000000,3.880000,4.000000,0.000000,0.000000,3.141593
+0.020000,38.049000,24.000000,3.960000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.969000,24.000000,4.040000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.889000,24.000000,4.120000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.809000,24.000000,4.200000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.729000,24.000000,4.280000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.649000,24.000000,4.360000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.569000,24.000000,4.440000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.489000,24.000000,4.520000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.409000,24.000000,4.600000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.329000,24.000000,4.680000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.249000,24.000000,4.760000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.169000,24.000000,4.840000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.089000,24.000000,4.920000,4.000000,0.000000,0.000000,3.141593
+0.020000,37.009000,24.000000,5.000000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.929000,24.000000,5.080000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.849000,24.000000,5.160000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.769000,24.000000,5.240000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.689000,24.000000,5.320000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.609000,24.000000,5.400000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.529000,24.000000,5.480000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.449000,24.000000,5.560000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.369000,24.000000,5.640000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.289000,24.000000,5.720000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.209000,24.000000,5.800000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.129000,24.000000,5.880000,4.000000,0.000000,0.000000,3.141593
+0.020000,36.049000,24.000000,5.960000,4.000000,0.000000,0.000000,3.141593
+0.020000,35.969000,24.000000,6.040000,4.000000,0.000000,0.000000,3.141593
+0.020000,35.889000,24.000000,6.120000,4.000000,0.000000,0.000000,3.141593
+0.020000,35.809012,24.000000,6.199988,3.998789,-0.060547,-3.027344,3.141593
+0.020000,35.729080,24.000000,6.279920,3.994453,-0.216797,-7.812500,3.141593
+0.020000,35.649265,24.000000,6.359735,3.986992,-0.373047,-7.812500,3.141593
+0.020000,35.569631,24.000000,6.439369,3.976406,-0.529297,-7.812500,3.141593
+0.020000,35.490240,24.000000,6.518760,3.962695,-0.685547,-7.812500,3.141593
+0.020000,35.411155,24.000000,6.597845,3.945859,-0.841797,-7.812500,3.141593
+0.020000,35.332437,24.000000,6.676563,3.925898,-0.998047,-7.812500,3.141593
+0.020000,35.254150,24.000000,6.754850,3.902813,-1.154297,-7.812500,3.141593
+0.020000,35.176356,24.000000,6.832644,3.876602,-1.310547,-7.812500,3.141593
+0.020000,35.099117,24.000000,6.909883,3.847266,-1.466797,-7.812500,3.141593
+0.020000,35.022496,24.000000,6.986504,3.814805,-1.623047,-7.812500,3.141593
+0.020000,34.946556,24.000000,7.062444,3.779219,-1.779297,-7.812500,3.141593
+0.020000,34.871359,24.000000,7.137641,3.740508,-1.935547,-7.812500,3.141593
+0.020000,34.796967,24.000000,7.212033,3.698672,-2.091797,-7.812500,3.141593
+0.020000,34.723443,24.000000,7.285557,3.653711,-2.248047,-7.812500,3.141593
+0.020000,34.650850,24.000000,7.358150,3.605625,-2.404297,-7.812500,3.141593
+0.020000,34.579250,24.000000,7.429750,3.554414,-2.560547,-7.812500,3.141593
+0.020000,34.508705,24.000000,7.500295,3.500078,-2.716797,-7.812500,3.141593
+0.020000,34.439278,24.000000,7.569722,3.442617,-2.873047,-7.812500,3.141593
+0.020000,34.371031,24.000000,7.637969,3.382031,-3.029297,-7.812500,3.141593
+0.020000,34.304028,24.000000,7.704972,3.318320,-3.185547,-7.812500,3.141593
+0.020000,34.238330,24.000000,7.770670,3.251484,-3.341797,-7.812500,3.141593
+0.020000,34.174000,24.000000,7.835000,3.181523,-3.498047,-7.812500,3.141593
+0.020000,34.111100,24.000000,7.897900,3.108438,-3.654297,-7.812500,3.141593
+0.020000,34.049693,24.000000,7.959307,3.032227,-3.810547,-7.812500,3.141593
+0.020000,33.989842,24.000000,8.019158,2.952891,-3.966797,-7.812500,3.141593
+0.020000,33.931609,24.000000,8.077391,2.870430,-4.123047,-7.812500,3.141593
+0.020000,33.875056,24.000000,8.133944,2.784844,-4.279297,-7.812500,3.141593
+0.020000,33.820246,24.000000,8.188754,2.696133,-4.435547,-7.812500,3.141593
+0.020000,33.767242,24.000000,8.241758,2.604297,-4.591797,-7.812500,3.141593
+0.020000,33.716106,24.000000,8.292894,2.509336,-4.748047,-7.812500,3.141593
+0.020000,33.666900,24.000000,8.342100,2.411250,-4.904297,-7.812500,3.141593
+0.020000,33.619675,24.000000,8.389325,2.311250,-5.000000,-4.785156,3.141593
+0.020000,33.574450,24.000000,8.434550,2.211250,-5.000000,0.000000,3.141593
+0.020000,33.531225,24.000000,8.477775,2.111250,-5.000000,0.000000,3.141593
+0.020000,33.490000,24.000000,8.519000,2.011250,-5.000000,0.000000,3.141593
+0.020000,33.450775,24.000000,8.558225,1.911250,-5.000000,0.000000,3.141593
+0.020000,33.413550,24.000000,8.595450,1.811250,-5.000000,0.000000,3.141593
+0.020000,33.378325,24.000000,8.630675,1.711250,-5.000000,-0.000000,3.141593
+0.020000,33.345100,24.000000,8.663900,1.611250,-5.000000,0.000000,3.141593
+0.020000,33.313863,24.000000,8.695137,1.512461,-4.939453,3.027344,3.141593
+0.020000,33.284570,24.000000,8.724430,1.416797,-4.783203,7.812500,3.141593
+0.020000,33.257160,24.000000,8.751840,1.324258,-4.626953,7.812500,3.141593
+0.020000,33.231569,24.000000,8.777431,1.234844,-4.470703,7.812500,3.141593
+0.020000,33.207735,24.000000,8.801265,1.148555,-4.314453,7.812500,3.141593
+0.020000,33.185595,24.000000,8.823405,1.065391,-4.158203,7.812500,3.141593
+0.020000,33.165088,24.000000,8.843912,0.985352,-4.001953,7.812500,3.141593
+0.020000,33.146150,24.000000,8.862850,0.908438,-3.845703,7.812500,3.141593
+0.020000,33.128719,24.000000,8.880281,0.834648,-3.689453,7.812500,3.141593
+0.020000,33.112733,24.000000,8.896267,0.763984,-3.533203,7.812500,3.141593
+0.020000,33.098129,24.000000,8.910871,0.696445,-3.376953,7.812500,3.141593
+0.020000,33.084844,24.000000,8.924156,0.632031,-3.220703,7.812500,3.141593
+0.020000,33.072816,24.000000,8.936184,0.570742,-3.064453,7.812500,3.141593
+0.020000,33.061983,24.000000,8.947017,0.512578,-2.908203,7.812500,3.141593
+0.020000,33.052282,24.000000,8.956718,0.457539,-2.751953,7.812500,3.141593
+0.020000,33.043650,24.000000,8.965350,0.405625,-2.595703,7.812500,3.141593
+0.020000,33.036025,24.000000,8.972975,0.356836,-2.439453,7.812500,3.141593
+0.020000,33.029345,24.000000,8.979655,0.311172,-2.283203,7.812500,3.141593
+0.020000,33.023547,24.000000,8.985453,0.268633,-2.126953,7.812500,3.141593
+0.020000,33.018569,24.000000,8.990431,0.229219,-1.970703,7.812500,3.141593
+0.020000,33.014347,24.000000,8.994653,0.192930,-1.814453,7.812500,3.141593
+0.020000,33.010820,24.000000,8.998180,0.159766,-1.658203,7.812500,3.141593
+0.020000,33.007925,24.000000,9.001075,0.129727,-1.501953,7.812500,3.141593
+0.020000,33.005600,24.000000,9.003400,0.102813,-1.345703,7.812500,3.141593
+0.020000,33.003782,24.000000,9.005218,0.079023,-1.189453,7.812500,3.141593
+0.020000,33.002408,24.000000,9.006592,0.058359,-1.033203,7.812500,3.141593
+0.020000,33.001416,24.000000,9.007584,0.040820,-0.876953,7.812500,3.141593
+0.020000,33.000744,24.000000,9.008256,0.026406,-0.720703,7.812500,3.141593
+0.020000,33.000329,24.000000,9.008671,0.015117,-0.564453,7.812500,3.141593
+0.020000,33.000108,24.000000,9.008892,0.006953,-0.408203,7.812500,3.141593
+0.020000,33.000019,24.000000,9.008981,0.001914,-0.251953,7.812500,3.141593
+0.020000,33.000000,24.000000,9.009000,0.000000,-0.095703,7.812500,3.141593
+0.020000,33.000000,24.000000,9.009000,0.000000,0.000000,4.785156,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench.right.pf1.csv b/src/main/deploy/output/BlueSideTrench.right.pf1.csv
new file mode 100644
index 0000000..d9d8c37
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench.right.pf1.csv
@@ -0,0 +1,186 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,42.000000,25.000000,0.000031,0.003125,0.156250,7.812500,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,-0.156250,-15.625000,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,7.812500,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,41.996969,25.000000,0.003062,0.151562,7.578125,378.906250,3.141593
+0.020000,41.993187,25.000000,0.006844,0.189063,1.875000,-285.156250,3.141593
+0.020000,41.988687,25.000000,0.011344,0.225000,1.796875,-3.906250,3.141593
+0.020000,41.983406,25.000000,0.016625,0.264062,1.953125,7.812500,3.141593
+0.020000,41.977281,25.000000,0.022750,0.306250,2.109375,7.812500,3.141593
+0.020000,41.970250,25.000000,0.029781,0.351562,2.265625,7.812500,3.141593
+0.020000,41.962250,25.000000,0.037781,0.400000,2.421875,7.812500,3.141593
+0.020000,41.953219,25.000000,0.046813,0.451562,2.578125,7.812500,3.141593
+0.020000,41.943094,25.000000,0.056937,0.506250,2.734375,7.812500,3.141593
+0.020000,41.931812,25.000000,0.068219,0.564063,2.890625,7.812500,3.141593
+0.020000,41.919312,25.000000,0.080719,0.625000,3.046875,7.812500,3.141593
+0.020000,41.905531,25.000000,0.094500,0.689062,3.203125,7.812500,3.141593
+0.020000,41.890406,25.000000,0.109625,0.756250,3.359375,7.812500,3.141593
+0.020000,41.873875,25.000000,0.126156,0.826562,3.515625,7.812500,3.141593
+0.020000,41.855875,25.000000,0.144156,0.900000,3.671875,7.812500,3.141593
+0.020000,41.836344,25.000000,0.163688,0.976563,3.828125,7.812500,3.141593
+0.020000,41.815219,25.000000,0.184813,1.056250,3.984375,7.812500,3.141593
+0.020000,41.792437,25.000000,0.207594,1.139063,4.140625,7.812500,3.141593
+0.020000,41.767938,25.000000,0.232094,1.225000,4.296875,7.812500,3.141593
+0.020000,41.741656,25.000000,0.258375,1.314063,4.453125,7.812500,3.141593
+0.020000,41.713531,25.000000,0.286500,1.406250,4.609375,7.812500,3.141593
+0.020000,41.683500,25.000000,0.316531,1.501563,4.765625,7.812500,3.141593
+0.020000,41.651500,25.000000,0.348531,1.600000,4.921875,7.812500,3.141593
+0.020000,41.617500,25.000000,0.382531,1.700000,5.000000,3.906250,3.141593
+0.020000,41.581500,25.000000,0.418531,1.800000,5.000000,0.000000,3.141593
+0.020000,41.543500,25.000000,0.456531,1.900000,5.000000,-0.000000,3.141593
+0.020000,41.503500,25.000000,0.496531,2.000000,5.000000,0.000000,3.141593
+0.020000,41.461500,25.000000,0.538531,2.100000,5.000000,0.000000,3.141593
+0.020000,41.417500,25.000000,0.582531,2.200000,5.000000,0.000000,3.141593
+0.020000,41.371500,25.000000,0.628531,2.300000,5.000000,-0.000000,3.141593
+0.020000,41.323500,25.000000,0.676531,2.400000,5.000000,0.000000,3.141593
+0.020000,41.273531,25.000000,0.726500,2.498438,4.921875,-3.906250,3.141593
+0.020000,41.221656,25.000000,0.778375,2.593750,4.765625,-7.812500,3.141593
+0.020000,41.167938,25.000000,0.832094,2.685938,4.609375,-7.812500,3.141593
+0.020000,41.112437,25.000000,0.887594,2.775000,4.453125,-7.812500,3.141593
+0.020000,41.055219,25.000000,0.944812,2.860937,4.296875,-7.812500,3.141593
+0.020000,40.996344,25.000000,1.003687,2.943750,4.140625,-7.812500,3.141593
+0.020000,40.935875,25.000000,1.064156,3.023437,3.984375,-7.812500,3.141593
+0.020000,40.873875,25.000000,1.126156,3.100000,3.828125,-7.812500,3.141593
+0.020000,40.810406,25.000000,1.189625,3.173437,3.671875,-7.812500,3.141593
+0.020000,40.745531,25.000000,1.254500,3.243750,3.515625,-7.812500,3.141593
+0.020000,40.679313,25.000000,1.320719,3.310937,3.359375,-7.812500,3.141593
+0.020000,40.611812,25.000000,1.388219,3.375000,3.203125,-7.812500,3.141593
+0.020000,40.543094,25.000000,1.456937,3.435937,3.046875,-7.812500,3.141593
+0.020000,40.473219,25.000000,1.526812,3.493750,2.890625,-7.812500,3.141593
+0.020000,40.402250,25.000000,1.597781,3.548437,2.734375,-7.812500,3.141593
+0.020000,40.330250,25.000000,1.669781,3.600000,2.578125,-7.812500,3.141593
+0.020000,40.257281,25.000000,1.742750,3.648438,2.421875,-7.812500,3.141593
+0.020000,40.183406,25.000000,1.816625,3.693750,2.265625,-7.812500,3.141593
+0.020000,40.108688,25.000000,1.891344,3.735938,2.109375,-7.812500,3.141593
+0.020000,40.033188,25.000000,1.966844,3.775000,1.953125,-7.812500,3.141593
+0.020000,39.956969,25.000000,2.043062,3.810938,1.796875,-7.812500,3.141593
+0.020000,39.880094,25.000000,2.119937,3.843750,1.640625,-7.812500,3.141593
+0.020000,39.802625,25.000000,2.197406,3.873438,1.484375,-7.812500,3.141593
+0.020000,39.724625,25.000000,2.275406,3.900000,1.328125,-7.812500,3.141593
+0.020000,39.646156,25.000000,2.353875,3.923437,1.171875,-7.812500,3.141593
+0.020000,39.567281,25.000000,2.432750,3.943750,1.015625,-7.812500,3.141593
+0.020000,39.488063,25.000000,2.511969,3.960937,0.859375,-7.812500,3.141593
+0.020000,39.408563,25.000000,2.591469,3.975000,0.703125,-7.812500,3.141593
+0.020000,39.328844,25.000000,2.671187,3.985937,0.546875,-7.812500,3.141593
+0.020000,39.248969,25.000000,2.751062,3.993750,0.390625,-7.812500,3.141593
+0.020000,39.169000,25.000000,2.831031,3.998437,0.234375,-7.812500,3.141593
+0.020000,39.089000,25.000000,2.911031,4.000000,0.078125,-7.812500,3.141593
+0.020000,39.009000,25.000000,2.991031,4.000000,0.000000,-3.906250,3.141593
+0.020000,38.929000,25.000000,3.071031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,38.849000,25.000000,3.151031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.769000,25.000000,3.231031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.689000,25.000000,3.311031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.609000,25.000000,3.391031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,38.529000,25.000000,3.471031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.449000,25.000000,3.551031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.369000,25.000000,3.631031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.289000,25.000000,3.711031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,38.209000,25.000000,3.791031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.129000,25.000000,3.871031,4.000000,0.000000,0.000000,3.141593
+0.020000,38.049000,25.000000,3.951031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.969000,25.000000,4.031031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.889000,25.000000,4.111031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,37.809000,25.000000,4.191031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.729000,25.000000,4.271031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.649000,25.000000,4.351031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.569000,25.000000,4.431031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,37.489000,25.000000,4.511031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.409000,25.000000,4.591031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.329000,25.000000,4.671031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.249000,25.000000,4.751031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,37.169000,25.000000,4.831031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.089000,25.000000,4.911031,4.000000,0.000000,0.000000,3.141593
+0.020000,37.009000,25.000000,4.991031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,36.929000,25.000000,5.071031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.849000,25.000000,5.151031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.769000,25.000000,5.231031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.689000,25.000000,5.311031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.609000,25.000000,5.391031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,36.529000,25.000000,5.471031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.449000,25.000000,5.551031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.369000,25.000000,5.631031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.289000,25.000000,5.711031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,36.209000,25.000000,5.791031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.129000,25.000000,5.871031,4.000000,0.000000,0.000000,3.141593
+0.020000,36.049000,25.000000,5.951031,4.000000,0.000000,0.000000,3.141593
+0.020000,35.969000,25.000000,6.031031,4.000000,0.000000,0.000000,3.141593
+0.020000,35.889000,25.000000,6.111031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,35.809012,25.000000,6.191019,3.999395,-0.030273,-1.513672,3.141593
+0.020000,35.729080,25.000000,6.270952,3.996621,-0.138672,-5.419922,3.141593
+0.020000,35.649265,25.000000,6.350766,3.990723,-0.294922,-7.812500,3.141593
+0.020000,35.569631,25.000000,6.430400,3.981699,-0.451172,-7.812500,3.141593
+0.020000,35.490240,25.000000,6.509791,3.969551,-0.607422,-7.812500,3.141593
+0.020000,35.411155,25.000000,6.588877,3.954277,-0.763672,-7.812500,3.141593
+0.020000,35.332437,25.000000,6.667594,3.935879,-0.919922,-7.812500,3.141593
+0.020000,35.254150,25.000000,6.745881,3.914355,-1.076172,-7.812500,3.141593
+0.020000,35.176356,25.000000,6.823675,3.889707,-1.232422,-7.812500,3.141593
+0.020000,35.099117,25.000000,6.900914,3.861934,-1.388672,-7.812500,3.141593
+0.020000,35.022496,25.000000,6.977535,3.831035,-1.544922,-7.812500,3.141593
+0.020000,34.946556,25.000000,7.053475,3.797012,-1.701172,-7.812500,3.141593
+0.020000,34.871359,25.000000,7.128672,3.759863,-1.857422,-7.812500,3.141593
+0.020000,34.796967,25.000000,7.203064,3.719590,-2.013672,-7.812500,3.141593
+0.020000,34.723443,25.000000,7.276588,3.676191,-2.169922,-7.812500,3.141593
+0.020000,34.650850,25.000000,7.349181,3.629668,-2.326172,-7.812500,3.141593
+0.020000,34.579250,25.000000,7.420782,3.580020,-2.482422,-7.812500,3.141593
+0.020000,34.508705,25.000000,7.491327,3.527246,-2.638672,-7.812500,3.141593
+0.020000,34.439278,25.000000,7.560754,3.471348,-2.794922,-7.812500,3.141593
+0.020000,34.371031,25.000000,7.629000,3.412324,-2.951172,-7.812500,3.141593
+0.020000,34.304028,25.000000,7.696004,3.350176,-3.107422,-7.812500,3.141593
+0.020000,34.238330,25.000000,7.761702,3.284902,-3.263672,-7.812500,3.141593
+0.020000,34.174000,25.000000,7.826032,3.216504,-3.419922,-7.812500,3.141593
+0.020000,34.111100,25.000000,7.888931,3.144980,-3.576172,-7.812500,3.141593
+0.020000,34.049693,25.000000,7.950338,3.070332,-3.732422,-7.812500,3.141593
+0.020000,33.989842,25.000000,8.010189,2.992559,-3.888672,-7.812500,3.141593
+0.020000,33.931609,25.000000,8.068422,2.911660,-4.044922,-7.812500,3.141593
+0.020000,33.875056,25.000000,8.124975,2.827637,-4.201172,-7.812500,3.141593
+0.020000,33.820246,25.000000,8.179785,2.740488,-4.357422,-7.812500,3.141593
+0.020000,33.767242,25.000000,8.232789,2.650215,-4.513672,-7.812500,3.141593
+0.020000,33.716106,25.000000,8.283925,2.556816,-4.669922,-7.812500,3.141593
+0.020000,33.666900,25.000000,8.333131,2.460293,-4.826172,-7.812500,3.141593
+0.020000,33.619675,25.000000,8.380356,2.361250,-4.952148,-6.298828,3.141593
+0.020000,33.574450,25.000000,8.425581,2.261250,-5.000000,-2.392578,3.141593
+0.020000,33.531225,25.000000,8.468806,2.161250,-5.000000,-0.000000,3.141593
+0.020000,33.490000,25.000000,8.510031,2.061250,-5.000000,0.000000,3.141593
+0.020000,33.450775,25.000000,8.549256,1.961250,-5.000000,0.000000,3.141593
+0.020000,33.413550,25.000000,8.586481,1.861250,-5.000000,-0.000000,3.141593
+0.020000,33.378325,25.000000,8.621706,1.761250,-5.000000,0.000000,3.141593
+0.020000,33.345100,25.000000,8.654931,1.661250,-5.000000,0.000000,3.141593
+0.020000,33.313863,25.000000,8.686168,1.561855,-4.969727,1.513672,3.141593
+0.020000,33.284570,25.000000,8.715461,1.464629,-4.861328,5.419922,3.141593
+0.020000,33.257160,25.000000,8.742871,1.370527,-4.705078,7.812500,3.141593
+0.020000,33.231569,25.000000,8.768462,1.279551,-4.548828,7.812500,3.141593
+0.020000,33.207735,25.000000,8.792296,1.191699,-4.392578,7.812500,3.141593
+0.020000,33.185595,25.000000,8.814436,1.106973,-4.236328,7.812500,3.141593
+0.020000,33.165088,25.000000,8.834943,1.025371,-4.080078,7.812500,3.141593
+0.020000,33.146150,25.000000,8.853881,0.946895,-3.923828,7.812500,3.141593
+0.020000,33.128719,25.000000,8.871312,0.871543,-3.767578,7.812500,3.141593
+0.020000,33.112733,25.000000,8.887298,0.799316,-3.611328,7.812500,3.141593
+0.020000,33.098129,25.000000,8.901903,0.730215,-3.455078,7.812500,3.141593
+0.020000,33.084844,25.000000,8.915188,0.664238,-3.298828,7.812500,3.141593
+0.020000,33.072816,25.000000,8.927215,0.601387,-3.142578,7.812500,3.141593
+0.020000,33.061983,25.000000,8.938048,0.541660,-2.986328,7.812500,3.141593
+0.020000,33.052282,25.000000,8.947750,0.485059,-2.830078,7.812500,3.141593
+0.020000,33.043650,25.000000,8.956381,0.431582,-2.673828,7.812500,3.141593
+0.020000,33.036025,25.000000,8.964006,0.381230,-2.517578,7.812500,3.141593
+0.020000,33.029345,25.000000,8.970686,0.334004,-2.361328,7.812500,3.141593
+0.020000,33.023547,25.000000,8.976484,0.289902,-2.205078,7.812500,3.141593
+0.020000,33.018569,25.000000,8.981462,0.248926,-2.048828,7.812500,3.141593
+0.020000,33.014347,25.000000,8.985684,0.211074,-1.892578,7.812500,3.141593
+0.020000,33.010820,25.000000,8.989211,0.176348,-1.736328,7.812500,3.141593
+0.020000,33.007925,25.000000,8.992106,0.144746,-1.580078,7.812500,3.141593
+0.020000,33.005600,25.000000,8.994431,0.116270,-1.423828,7.812500,3.141593
+0.020000,33.003782,25.000000,8.996250,0.090918,-1.267578,7.812500,3.141593
+0.020000,33.002408,25.000000,8.997623,0.068691,-1.111328,7.812500,3.141593
+0.020000,33.001416,25.000000,8.998615,0.049590,-0.955078,7.812500,3.141593
+0.020000,33.000744,25.000000,8.999288,0.033613,-0.798828,7.812500,3.141593
+0.020000,33.000329,25.000000,8.999703,0.020762,-0.642578,7.812500,3.141593
+0.020000,33.000108,25.000000,8.999923,0.011035,-0.486328,7.812500,3.141593
+0.020000,33.000019,25.000000,9.000012,0.004434,-0.330078,7.812500,3.141593
+0.020000,33.000000,25.000000,9.000031,0.000957,-0.173828,7.812500,3.141593
+0.020000,33.000000,25.000000,9.000031,0.000000,-0.047852,6.298828,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench5Feet.left.pf1.csv b/src/main/deploy/output/BlueSideTrench5Feet.left.pf1.csv
new file mode 100644
index 0000000..b3b562f
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench5Feet.left.pf1.csv
@@ -0,0 +1,161 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,33.000000,23.000000,0.000031,0.003125,0.156250,7.812500,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,-0.156250,-15.625000,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,0.000000,7.812500,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,23.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,32.998094,23.000000,0.001937,0.095312,4.765625,238.281250,3.141593
+0.020000,32.994969,23.000000,0.005063,0.156250,3.046875,-85.937500,3.141593
+0.020000,32.991188,23.000000,0.008844,0.189062,1.640625,-70.312500,3.141593
+0.020000,32.986688,23.000000,0.013344,0.225000,1.796875,7.812500,3.141593
+0.020000,32.981406,23.000000,0.018625,0.264063,1.953125,7.812500,3.141593
+0.020000,32.975281,23.000000,0.024750,0.306250,2.109375,7.812500,3.141593
+0.020000,32.968250,23.000000,0.031781,0.351563,2.265625,7.812500,3.141593
+0.020000,32.960250,23.000000,0.039781,0.400000,2.421875,7.812500,3.141593
+0.020000,32.951219,23.000000,0.048812,0.451562,2.578125,7.812500,3.141593
+0.020000,32.941094,23.000000,0.058937,0.506250,2.734375,7.812500,3.141593
+0.020000,32.929812,23.000000,0.070219,0.564063,2.890625,7.812500,3.141593
+0.020000,32.917313,23.000000,0.082719,0.625000,3.046875,7.812500,3.141593
+0.020000,32.903531,23.000000,0.096500,0.689063,3.203125,7.812500,3.141593
+0.020000,32.888406,23.000000,0.111625,0.756250,3.359375,7.812500,3.141593
+0.020000,32.871875,23.000000,0.128156,0.826562,3.515625,7.812500,3.141593
+0.020000,32.853875,23.000000,0.146156,0.900000,3.671875,7.812500,3.141593
+0.020000,32.834344,23.000000,0.165687,0.976563,3.828125,7.812500,3.141593
+0.020000,32.813219,23.000000,0.186813,1.056250,3.984375,7.812500,3.141593
+0.020000,32.790438,23.000000,0.209594,1.139062,4.140625,7.812500,3.141593
+0.020000,32.765937,23.000000,0.234094,1.225000,4.296875,7.812500,3.141593
+0.020000,32.739656,23.000000,0.260375,1.314062,4.453125,7.812500,3.141593
+0.020000,32.711531,23.000000,0.288500,1.406250,4.609375,7.812500,3.141593
+0.020000,32.681500,23.000000,0.318531,1.501563,4.765625,7.812500,3.141593
+0.020000,32.649500,23.000000,0.350531,1.600000,4.921875,7.812500,3.141593
+0.020000,32.615500,23.000000,0.384531,1.700000,5.000000,3.906250,3.141593
+0.020000,32.579500,23.000000,0.420531,1.800000,5.000000,-0.000000,3.141593
+0.020000,32.541500,23.000000,0.458531,1.900000,5.000000,0.000000,3.141593
+0.020000,32.501500,23.000000,0.498531,2.000000,5.000000,-0.000000,3.141593
+0.020000,32.459500,23.000000,0.540531,2.100000,5.000000,0.000000,3.141593
+0.020000,32.415500,23.000000,0.584531,2.200000,5.000000,-0.000000,3.141593
+0.020000,32.369500,23.000000,0.630531,2.300000,5.000000,0.000000,3.141593
+0.020000,32.321500,23.000000,0.678531,2.400000,5.000000,0.000000,3.141593
+0.020000,32.271531,23.000000,0.728500,2.498437,4.921875,-3.906250,3.141593
+0.020000,32.219656,23.000000,0.780375,2.593750,4.765625,-7.812500,3.141593
+0.020000,32.165937,23.000000,0.834094,2.685938,4.609375,-7.812500,3.141593
+0.020000,32.110438,23.000000,0.889594,2.775000,4.453125,-7.812500,3.141593
+0.020000,32.053219,23.000000,0.946813,2.860938,4.296875,-7.812500,3.141593
+0.020000,31.994344,23.000000,1.005688,2.943750,4.140625,-7.812500,3.141593
+0.020000,31.933875,23.000000,1.066156,3.023437,3.984375,-7.812500,3.141593
+0.020000,31.871875,23.000000,1.128156,3.100000,3.828125,-7.812500,3.141593
+0.020000,31.808406,23.000000,1.191625,3.173437,3.671875,-7.812500,3.141593
+0.020000,31.743531,23.000000,1.256500,3.243750,3.515625,-7.812500,3.141593
+0.020000,31.677312,23.000000,1.322719,3.310938,3.359375,-7.812500,3.141593
+0.020000,31.609813,23.000000,1.390219,3.375000,3.203125,-7.812500,3.141593
+0.020000,31.541094,23.000000,1.458937,3.435937,3.046875,-7.812500,3.141593
+0.020000,31.471219,23.000000,1.528812,3.493750,2.890625,-7.812500,3.141593
+0.020000,31.400250,23.000000,1.599781,3.548438,2.734375,-7.812500,3.141593
+0.020000,31.328250,23.000000,1.671781,3.600000,2.578125,-7.812500,3.141593
+0.020000,31.255281,23.000000,1.744750,3.648438,2.421875,-7.812500,3.141593
+0.020000,31.181406,23.000000,1.818625,3.693750,2.265625,-7.812500,3.141593
+0.020000,31.106687,23.000000,1.893344,3.735938,2.109375,-7.812500,3.141593
+0.020000,31.031188,23.000000,1.968844,3.775000,1.953125,-7.812500,3.141593
+0.020000,30.954969,23.000000,2.045062,3.810937,1.796875,-7.812500,3.141593
+0.020000,30.878094,23.000000,2.121937,3.843750,1.640625,-7.812500,3.141593
+0.020000,30.800625,23.000000,2.199406,3.873437,1.484375,-7.812500,3.141593
+0.020000,30.722625,23.000000,2.277406,3.900000,1.328125,-7.812500,3.141593
+0.020000,30.644156,23.000000,2.355875,3.923437,1.171875,-7.812500,3.141593
+0.020000,30.565281,23.000000,2.434750,3.943750,1.015625,-7.812500,3.141593
+0.020000,30.486063,23.000000,2.513969,3.960937,0.859375,-7.812500,3.141593
+0.020000,30.406563,23.000000,2.593469,3.975000,0.703125,-7.812500,3.141593
+0.020000,30.326844,23.000000,2.673187,3.985938,0.546875,-7.812500,3.141593
+0.020000,30.246969,23.000000,2.753062,3.993750,0.390625,-7.812500,3.141593
+0.020000,30.167000,23.000000,2.833031,3.998438,0.234375,-7.812500,3.141593
+0.020000,30.087000,23.000000,2.913031,4.000000,0.078125,-7.812500,3.141593
+0.020000,30.007000,23.000000,2.993031,4.000000,0.000000,-3.906250,3.141593
+0.020000,29.927000,23.000000,3.073031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.847000,23.000000,3.153031,4.000000,0.000000,-0.000000,3.141593
+0.020000,29.767000,23.000000,3.233031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.687000,23.000000,3.313031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.607000,23.000000,3.393031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.527000,23.000000,3.473031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.447000,23.000000,3.553031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.367000,23.000000,3.633031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.287000,23.000000,3.713031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.207000,23.000000,3.793031,4.000000,0.000000,-0.000000,3.141593
+0.020000,29.127000,23.000000,3.873031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.047000,23.000000,3.953031,4.000000,0.000000,0.000000,3.141593
+0.020000,28.967000,23.000000,4.033031,4.000000,0.000000,0.000000,3.141593
+0.020000,28.887000,23.000000,4.113031,4.000000,0.000000,-0.000000,3.141593
+0.020000,28.807013,23.000000,4.193018,3.999355,-0.032227,-1.611328,3.141593
+0.020000,28.727083,23.000000,4.272948,3.996504,-0.142578,-5.517578,3.141593
+0.020000,28.647272,23.000000,4.352759,3.990527,-0.298828,-7.812500,3.141593
+0.020000,28.567644,23.000000,4.432387,3.981426,-0.455078,-7.812500,3.141593
+0.020000,28.488260,23.000000,4.511771,3.969199,-0.611328,-7.812500,3.141593
+0.020000,28.409183,23.000000,4.590848,3.953848,-0.767578,-7.812500,3.141593
+0.020000,28.330475,23.000000,4.669556,3.935371,-0.923828,-7.812500,3.141593
+0.020000,28.252200,23.000000,4.747831,3.913770,-1.080078,-7.812500,3.141593
+0.020000,28.174419,23.000000,4.825612,3.889043,-1.236328,-7.812500,3.141593
+0.020000,28.097195,23.000000,4.902836,3.861191,-1.392578,-7.812500,3.141593
+0.020000,28.020591,23.000000,4.979440,3.830215,-1.548828,-7.812500,3.141593
+0.020000,27.944669,23.000000,5.055362,3.796113,-1.705078,-7.812500,3.141593
+0.020000,27.869491,23.000000,5.130540,3.758887,-1.861328,-7.812500,3.141593
+0.020000,27.795120,23.000000,5.204911,3.718535,-2.017578,-7.812500,3.141593
+0.020000,27.721619,23.000000,5.278412,3.675059,-2.173828,-7.812500,3.141593
+0.020000,27.649050,23.000000,5.350981,3.628457,-2.330078,-7.812500,3.141593
+0.020000,27.577475,23.000000,5.422556,3.578730,-2.486328,-7.812500,3.141593
+0.020000,27.506958,23.000000,5.493073,3.525879,-2.642578,-7.812500,3.141593
+0.020000,27.437560,23.000000,5.562471,3.469902,-2.798828,-7.812500,3.141593
+0.020000,27.369344,23.000000,5.630687,3.410801,-2.955078,-7.812500,3.141593
+0.020000,27.302372,23.000000,5.697659,3.348574,-3.111328,-7.812500,3.141593
+0.020000,27.236708,23.000000,5.763323,3.283223,-3.267578,-7.812500,3.141593
+0.020000,27.172413,23.000000,5.827618,3.214746,-3.423828,-7.812500,3.141593
+0.020000,27.109550,23.000000,5.890481,3.143145,-3.580078,-7.812500,3.141593
+0.020000,27.048182,23.000000,5.951850,3.068418,-3.736328,-7.812500,3.141593
+0.020000,26.988370,23.000000,6.011661,2.990566,-3.892578,-7.812500,3.141593
+0.020000,26.930179,23.000000,6.069853,2.909590,-4.048828,-7.812500,3.141593
+0.020000,26.873669,23.000000,6.126362,2.825488,-4.205078,-7.812500,3.141593
+0.020000,26.818904,23.000000,6.181128,2.738262,-4.361328,-7.812500,3.141593
+0.020000,26.765945,23.000000,6.234086,2.647910,-4.517578,-7.812500,3.141593
+0.020000,26.714857,23.000000,6.285175,2.554434,-4.673828,-7.812500,3.141593
+0.020000,26.665700,23.000000,6.334331,2.457832,-4.830078,-7.812500,3.141593
+0.020000,26.618525,23.000000,6.381506,2.358750,-4.954102,-6.201172,3.141593
+0.020000,26.573350,23.000000,6.426681,2.258750,-5.000000,-2.294922,3.141593
+0.020000,26.530175,23.000000,6.469856,2.158750,-5.000000,0.000000,3.141593
+0.020000,26.489000,23.000000,6.511031,2.058750,-5.000000,-0.000000,3.141593
+0.020000,26.449825,23.000000,6.550206,1.958750,-5.000000,0.000000,3.141593
+0.020000,26.412650,23.000000,6.587381,1.858750,-5.000000,0.000000,3.141593
+0.020000,26.377475,23.000000,6.622556,1.758750,-5.000000,-0.000000,3.141593
+0.020000,26.344300,23.000000,6.655731,1.658750,-5.000000,0.000000,3.141593
+0.020000,26.313112,23.000000,6.686919,1.559395,-4.967773,1.611328,3.141593
+0.020000,26.283867,23.000000,6.716164,1.462246,-4.857422,5.517578,3.141593
+0.020000,26.256503,23.000000,6.743529,1.368223,-4.701172,7.812500,3.141593
+0.020000,26.230956,23.000000,6.769075,1.277324,-4.544922,7.812500,3.141593
+0.020000,26.207165,23.000000,6.792866,1.189551,-4.388672,7.812500,3.141593
+0.020000,26.185067,23.000000,6.814964,1.104902,-4.232422,7.812500,3.141593
+0.020000,26.164600,23.000000,6.835432,1.023379,-4.076172,7.812500,3.141593
+0.020000,26.145700,23.000000,6.854331,0.944980,-3.919922,7.812500,3.141593
+0.020000,26.128306,23.000000,6.871725,0.869707,-3.763672,7.812500,3.141593
+0.020000,26.112355,23.000000,6.887677,0.797559,-3.607422,7.812500,3.141593
+0.020000,26.097784,23.000000,6.902247,0.728535,-3.451172,7.812500,3.141593
+0.020000,26.084531,23.000000,6.915500,0.662637,-3.294922,7.812500,3.141593
+0.020000,26.072534,23.000000,6.927497,0.599863,-3.138672,7.812500,3.141593
+0.020000,26.061730,23.000000,6.938302,0.540215,-2.982422,7.812500,3.141593
+0.020000,26.052056,23.000000,6.947975,0.483691,-2.826172,7.812500,3.141593
+0.020000,26.043450,23.000000,6.956581,0.430293,-2.669922,7.812500,3.141593
+0.020000,26.035850,23.000000,6.964182,0.380020,-2.513672,7.812500,3.141593
+0.020000,26.029192,23.000000,6.970839,0.332871,-2.357422,7.812500,3.141593
+0.020000,26.023415,23.000000,6.976616,0.288848,-2.201172,7.812500,3.141593
+0.020000,26.018456,23.000000,6.981575,0.247949,-2.044922,7.812500,3.141593
+0.020000,26.014253,23.000000,6.985779,0.210176,-1.888672,7.812500,3.141593
+0.020000,26.010742,23.000000,6.989289,0.175527,-1.732422,7.812500,3.141593
+0.020000,26.007862,23.000000,6.992169,0.144004,-1.576172,7.812500,3.141593
+0.020000,26.005550,23.000000,6.994481,0.115605,-1.419922,7.812500,3.141593
+0.020000,26.003743,23.000000,6.996288,0.090332,-1.263672,7.812500,3.141593
+0.020000,26.002380,23.000000,6.997652,0.068184,-1.107422,7.812500,3.141593
+0.020000,26.001396,23.000000,6.998635,0.049160,-0.951172,7.812500,3.141593
+0.020000,26.000731,23.000000,6.999300,0.033262,-0.794922,7.812500,3.141593
+0.020000,26.000321,23.000000,6.999710,0.020488,-0.638672,7.812500,3.141593
+0.020000,26.000105,23.000000,6.999927,0.010840,-0.482422,7.812500,3.141593
+0.020000,26.000018,23.000000,7.000013,0.004316,-0.326172,7.812500,3.141593
+0.020000,26.000000,23.000000,7.000031,0.000918,-0.169922,7.812500,3.141593
+0.020000,26.000000,23.000000,7.000031,0.000000,-0.045898,6.201172,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench5Feet.pf1.csv b/src/main/deploy/output/BlueSideTrench5Feet.pf1.csv
new file mode 100644
index 0000000..3f51638
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench5Feet.pf1.csv
@@ -0,0 +1,161 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,33.000000,24.000000,0.000031,0.003125,0.156250,7.812500,3.141593
+0.020000,33.000000,24.000000,0.000156,0.009375,0.312500,7.812500,3.141593
+0.020000,33.000000,24.000000,0.000437,0.018750,0.468750,7.812500,3.141593
+0.020000,33.000000,24.000000,0.000937,0.031250,0.625000,7.812500,3.141593
+0.020000,33.000000,24.000000,0.001719,0.046875,0.781250,7.812500,3.141593
+0.020000,33.000000,24.000000,0.002844,0.065625,0.937500,7.812500,3.141593
+0.020000,33.000000,24.000000,0.004375,0.087500,1.093750,7.812500,3.141593
+0.020000,33.000000,24.000000,0.006375,0.112500,1.250000,7.812500,3.141593
+0.020000,32.998094,24.000000,0.008906,0.140625,1.406250,7.812500,3.141593
+0.020000,32.994969,24.000000,0.012031,0.171875,1.562500,7.812500,3.141593
+0.020000,32.991188,24.000000,0.015813,0.206250,1.718750,7.812500,3.141593
+0.020000,32.986688,24.000000,0.020313,0.243750,1.875000,7.812500,3.141593
+0.020000,32.981406,24.000000,0.025594,0.284375,2.031250,7.812500,3.141593
+0.020000,32.975281,24.000000,0.031719,0.328125,2.187500,7.812500,3.141593
+0.020000,32.968250,24.000000,0.038750,0.375000,2.343750,7.812500,3.141593
+0.020000,32.960250,24.000000,0.046750,0.425000,2.500000,7.812500,3.141593
+0.020000,32.951219,24.000000,0.055781,0.478125,2.656250,7.812500,3.141593
+0.020000,32.941094,24.000000,0.065906,0.534375,2.812500,7.812500,3.141593
+0.020000,32.929812,24.000000,0.077188,0.593750,2.968750,7.812500,3.141593
+0.020000,32.917313,24.000000,0.089688,0.656250,3.125000,7.812500,3.141593
+0.020000,32.903531,24.000000,0.103469,0.721875,3.281250,7.812500,3.141593
+0.020000,32.888406,24.000000,0.118594,0.790625,3.437500,7.812500,3.141593
+0.020000,32.871875,24.000000,0.135125,0.862500,3.593750,7.812500,3.141593
+0.020000,32.853875,24.000000,0.153125,0.937500,3.750000,7.812500,3.141593
+0.020000,32.834344,24.000000,0.172656,1.015625,3.906250,7.812500,3.141593
+0.020000,32.813219,24.000000,0.193781,1.096875,4.062500,7.812500,3.141593
+0.020000,32.790438,24.000000,0.216563,1.181250,4.218750,7.812500,3.141593
+0.020000,32.765937,24.000000,0.241063,1.268750,4.375000,7.812500,3.141593
+0.020000,32.739656,24.000000,0.267344,1.359375,4.531250,7.812500,3.141593
+0.020000,32.711531,24.000000,0.295469,1.453125,4.687500,7.812500,3.141593
+0.020000,32.681500,24.000000,0.325500,1.550000,4.843750,7.812500,3.141593
+0.020000,32.649500,24.000000,0.357500,1.650000,5.000000,7.812500,3.141593
+0.020000,32.615500,24.000000,0.391500,1.750000,5.000000,0.000000,3.141593
+0.020000,32.579500,24.000000,0.427500,1.850000,5.000000,0.000000,3.141593
+0.020000,32.541500,24.000000,0.465500,1.950000,5.000000,-0.000000,3.141593
+0.020000,32.501500,24.000000,0.505500,2.050000,5.000000,0.000000,3.141593
+0.020000,32.459500,24.000000,0.547500,2.150000,5.000000,0.000000,3.141593
+0.020000,32.415500,24.000000,0.591500,2.250000,5.000000,0.000000,3.141593
+0.020000,32.369500,24.000000,0.637500,2.350000,5.000000,0.000000,3.141593
+0.020000,32.321500,24.000000,0.685500,2.450000,5.000000,0.000000,3.141593
+0.020000,32.271531,24.000000,0.735469,2.546875,4.843750,-7.812500,3.141593
+0.020000,32.219656,24.000000,0.787344,2.640625,4.687500,-7.812500,3.141593
+0.020000,32.165937,24.000000,0.841063,2.731250,4.531250,-7.812500,3.141593
+0.020000,32.110438,24.000000,0.896563,2.818750,4.375000,-7.812500,3.141593
+0.020000,32.053219,24.000000,0.953781,2.903125,4.218750,-7.812500,3.141593
+0.020000,31.994344,24.000000,1.012656,2.984375,4.062500,-7.812500,3.141593
+0.020000,31.933875,24.000000,1.073125,3.062500,3.906250,-7.812500,3.141593
+0.020000,31.871875,24.000000,1.135125,3.137500,3.750000,-7.812500,3.141593
+0.020000,31.808406,24.000000,1.198594,3.209375,3.593750,-7.812500,3.141593
+0.020000,31.743531,24.000000,1.263469,3.278125,3.437500,-7.812500,3.141593
+0.020000,31.677312,24.000000,1.329688,3.343750,3.281250,-7.812500,3.141593
+0.020000,31.609813,24.000000,1.397188,3.406250,3.125000,-7.812500,3.141593
+0.020000,31.541094,24.000000,1.465906,3.465625,2.968750,-7.812500,3.141593
+0.020000,31.471219,24.000000,1.535781,3.521875,2.812500,-7.812500,3.141593
+0.020000,31.400250,24.000000,1.606750,3.575000,2.656250,-7.812500,3.141593
+0.020000,31.328250,24.000000,1.678750,3.625000,2.500000,-7.812500,3.141593
+0.020000,31.255281,24.000000,1.751719,3.671875,2.343750,-7.812500,3.141593
+0.020000,31.181406,24.000000,1.825594,3.715625,2.187500,-7.812500,3.141593
+0.020000,31.106687,24.000000,1.900313,3.756250,2.031250,-7.812500,3.141593
+0.020000,31.031188,24.000000,1.975812,3.793750,1.875000,-7.812500,3.141593
+0.020000,30.954969,24.000000,2.052031,3.828125,1.718750,-7.812500,3.141593
+0.020000,30.878094,24.000000,2.128906,3.859375,1.562500,-7.812500,3.141593
+0.020000,30.800625,24.000000,2.206375,3.887500,1.406250,-7.812500,3.141593
+0.020000,30.722625,24.000000,2.284375,3.912500,1.250000,-7.812500,3.141593
+0.020000,30.644156,24.000000,2.362844,3.934375,1.093750,-7.812500,3.141593
+0.020000,30.565281,24.000000,2.441719,3.953125,0.937500,-7.812500,3.141593
+0.020000,30.486063,24.000000,2.520937,3.968750,0.781250,-7.812500,3.141593
+0.020000,30.406563,24.000000,2.600437,3.981250,0.625000,-7.812500,3.141593
+0.020000,30.326844,24.000000,2.680156,3.990625,0.468750,-7.812500,3.141593
+0.020000,30.246969,24.000000,2.760031,3.996875,0.312500,-7.812500,3.141593
+0.020000,30.167000,24.000000,2.840000,4.000000,0.156250,-7.812500,3.141593
+0.020000,30.087000,24.000000,2.920000,4.000000,0.000000,-7.812500,3.141593
+0.020000,30.007000,24.000000,3.000000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.927000,24.000000,3.080000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.847000,24.000000,3.160000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.767000,24.000000,3.240000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.687000,24.000000,3.320000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.607000,24.000000,3.400000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.527000,24.000000,3.480000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.447000,24.000000,3.560000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.367000,24.000000,3.640000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.287000,24.000000,3.720000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.207000,24.000000,3.800000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.127000,24.000000,3.880000,4.000000,0.000000,0.000000,3.141593
+0.020000,29.047000,24.000000,3.960000,4.000000,0.000000,0.000000,3.141593
+0.020000,28.967000,24.000000,4.040000,4.000000,0.000000,0.000000,3.141593
+0.020000,28.887000,24.000000,4.120000,4.000000,0.000000,0.000000,3.141593
+0.020000,28.807013,24.000000,4.199987,3.998711,-0.064453,-3.222656,3.141593
+0.020000,28.727083,24.000000,4.279917,3.994297,-0.220703,-7.812500,3.141593
+0.020000,28.647272,24.000000,4.359728,3.986758,-0.376953,-7.812500,3.141593
+0.020000,28.567644,24.000000,4.439356,3.976094,-0.533203,-7.812500,3.141593
+0.020000,28.488260,24.000000,4.518740,3.962305,-0.689453,-7.812500,3.141593
+0.020000,28.409183,24.000000,4.597817,3.945391,-0.845703,-7.812500,3.141593
+0.020000,28.330475,24.000000,4.676525,3.925352,-1.001953,-7.812500,3.141593
+0.020000,28.252200,24.000000,4.754800,3.902188,-1.158203,-7.812500,3.141593
+0.020000,28.174419,24.000000,4.832581,3.875898,-1.314453,-7.812500,3.141593
+0.020000,28.097195,24.000000,4.909805,3.846484,-1.470703,-7.812500,3.141593
+0.020000,28.020591,24.000000,4.986409,3.813945,-1.626953,-7.812500,3.141593
+0.020000,27.944669,24.000000,5.062331,3.778281,-1.783203,-7.812500,3.141593
+0.020000,27.869491,24.000000,5.137509,3.739492,-1.939453,-7.812500,3.141593
+0.020000,27.795120,24.000000,5.211880,3.697578,-2.095703,-7.812500,3.141593
+0.020000,27.721619,24.000000,5.285381,3.652539,-2.251953,-7.812500,3.141593
+0.020000,27.649050,24.000000,5.357950,3.604375,-2.408203,-7.812500,3.141593
+0.020000,27.577475,24.000000,5.429525,3.553086,-2.564453,-7.812500,3.141593
+0.020000,27.506958,24.000000,5.500042,3.498672,-2.720703,-7.812500,3.141593
+0.020000,27.437560,24.000000,5.569440,3.441133,-2.876953,-7.812500,3.141593
+0.020000,27.369344,24.000000,5.637656,3.380469,-3.033203,-7.812500,3.141593
+0.020000,27.302372,24.000000,5.704628,3.316680,-3.189453,-7.812500,3.141593
+0.020000,27.236708,24.000000,5.770292,3.249766,-3.345703,-7.812500,3.141593
+0.020000,27.172413,24.000000,5.834587,3.179727,-3.501953,-7.812500,3.141593
+0.020000,27.109550,24.000000,5.897450,3.106563,-3.658203,-7.812500,3.141593
+0.020000,27.048182,24.000000,5.958818,3.030273,-3.814453,-7.812500,3.141593
+0.020000,26.988370,24.000000,6.018630,2.950859,-3.970703,-7.812500,3.141593
+0.020000,26.930179,24.000000,6.076821,2.868320,-4.126953,-7.812500,3.141593
+0.020000,26.873669,24.000000,6.133331,2.782656,-4.283203,-7.812500,3.141593
+0.020000,26.818904,24.000000,6.188096,2.693867,-4.439453,-7.812500,3.141593
+0.020000,26.765945,24.000000,6.241055,2.601953,-4.595703,-7.812500,3.141593
+0.020000,26.714857,24.000000,6.292143,2.506914,-4.751953,-7.812500,3.141593
+0.020000,26.665700,24.000000,6.341300,2.408750,-4.908203,-7.812500,3.141593
+0.020000,26.618525,24.000000,6.388475,2.308750,-5.000000,-4.589844,3.141593
+0.020000,26.573350,24.000000,6.433650,2.208750,-5.000000,-0.000000,3.141593
+0.020000,26.530175,24.000000,6.476825,2.108750,-5.000000,0.000000,3.141593
+0.020000,26.489000,24.000000,6.518000,2.008750,-5.000000,-0.000000,3.141593
+0.020000,26.449825,24.000000,6.557175,1.908750,-5.000000,0.000000,3.141593
+0.020000,26.412650,24.000000,6.594350,1.808750,-5.000000,0.000000,3.141593
+0.020000,26.377475,24.000000,6.629525,1.708750,-5.000000,-0.000000,3.141593
+0.020000,26.344300,24.000000,6.662700,1.608750,-5.000000,-0.000000,3.141593
+0.020000,26.313112,24.000000,6.693888,1.510039,-4.935547,3.222656,3.141593
+0.020000,26.283867,24.000000,6.723133,1.414453,-4.779297,7.812500,3.141593
+0.020000,26.256503,24.000000,6.750497,1.321992,-4.623047,7.812500,3.141593
+0.020000,26.230956,24.000000,6.776044,1.232656,-4.466797,7.812500,3.141593
+0.020000,26.207165,24.000000,6.799835,1.146445,-4.310547,7.812500,3.141593
+0.020000,26.185067,24.000000,6.821933,1.063359,-4.154297,7.812500,3.141593
+0.020000,26.164600,24.000000,6.842400,0.983398,-3.998047,7.812500,3.141593
+0.020000,26.145700,24.000000,6.861300,0.906563,-3.841797,7.812500,3.141593
+0.020000,26.128306,24.000000,6.878694,0.832852,-3.685547,7.812500,3.141593
+0.020000,26.112355,24.000000,6.894645,0.762266,-3.529297,7.812500,3.141593
+0.020000,26.097784,24.000000,6.909216,0.694805,-3.373047,7.812500,3.141593
+0.020000,26.084531,24.000000,6.922469,0.630469,-3.216797,7.812500,3.141593
+0.020000,26.072534,24.000000,6.934466,0.569258,-3.060547,7.812500,3.141593
+0.020000,26.061730,24.000000,6.945270,0.511172,-2.904297,7.812500,3.141593
+0.020000,26.052056,24.000000,6.954944,0.456211,-2.748047,7.812500,3.141593
+0.020000,26.043450,24.000000,6.963550,0.404375,-2.591797,7.812500,3.141593
+0.020000,26.035850,24.000000,6.971150,0.355664,-2.435547,7.812500,3.141593
+0.020000,26.029192,24.000000,6.977808,0.310078,-2.279297,7.812500,3.141593
+0.020000,26.023415,24.000000,6.983585,0.267617,-2.123047,7.812500,3.141593
+0.020000,26.018456,24.000000,6.988544,0.228281,-1.966797,7.812500,3.141593
+0.020000,26.014253,24.000000,6.992747,0.192070,-1.810547,7.812500,3.141593
+0.020000,26.010742,24.000000,6.996258,0.158984,-1.654297,7.812500,3.141593
+0.020000,26.007862,24.000000,6.999138,0.129023,-1.498047,7.812500,3.141593
+0.020000,26.005550,24.000000,7.001450,0.102188,-1.341797,7.812500,3.141593
+0.020000,26.003743,24.000000,7.003257,0.078477,-1.185547,7.812500,3.141593
+0.020000,26.002380,24.000000,7.004620,0.057891,-1.029297,7.812500,3.141593
+0.020000,26.001396,24.000000,7.005604,0.040430,-0.873047,7.812500,3.141593
+0.020000,26.000731,24.000000,7.006269,0.026094,-0.716797,7.812500,3.141593
+0.020000,26.000321,24.000000,7.006679,0.014883,-0.560547,7.812500,3.141593
+0.020000,26.000105,24.000000,7.006895,0.006797,-0.404297,7.812500,3.141593
+0.020000,26.000018,24.000000,7.006982,0.001836,-0.248047,7.812500,3.141593
+0.020000,26.000000,24.000000,7.007000,0.000000,-0.091797,7.812500,3.141593
+0.020000,26.000000,24.000000,7.007000,0.000000,0.000000,4.589844,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench5Feet.right.pf1.csv b/src/main/deploy/output/BlueSideTrench5Feet.right.pf1.csv
new file mode 100644
index 0000000..331fcbb
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench5Feet.right.pf1.csv
@@ -0,0 +1,161 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,33.000000,25.000000,0.000031,0.003125,0.156250,7.812500,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,-0.156250,-15.625000,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,0.000000,7.812500,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,33.000000,25.000000,0.000031,0.000000,0.000000,0.000000,3.141593
+0.020000,32.998094,25.000000,0.001937,0.095312,4.765625,238.281250,3.141593
+0.020000,32.994969,25.000000,0.005063,0.156250,3.046875,-85.937500,3.141593
+0.020000,32.991188,25.000000,0.008844,0.189062,1.640625,-70.312500,3.141593
+0.020000,32.986688,25.000000,0.013344,0.225000,1.796875,7.812500,3.141593
+0.020000,32.981406,25.000000,0.018625,0.264063,1.953125,7.812500,3.141593
+0.020000,32.975281,25.000000,0.024750,0.306250,2.109375,7.812500,3.141593
+0.020000,32.968250,25.000000,0.031781,0.351563,2.265625,7.812500,3.141593
+0.020000,32.960250,25.000000,0.039781,0.400000,2.421875,7.812500,3.141593
+0.020000,32.951219,25.000000,0.048812,0.451562,2.578125,7.812500,3.141593
+0.020000,32.941094,25.000000,0.058937,0.506250,2.734375,7.812500,3.141593
+0.020000,32.929812,25.000000,0.070219,0.564063,2.890625,7.812500,3.141593
+0.020000,32.917313,25.000000,0.082719,0.625000,3.046875,7.812500,3.141593
+0.020000,32.903531,25.000000,0.096500,0.689063,3.203125,7.812500,3.141593
+0.020000,32.888406,25.000000,0.111625,0.756250,3.359375,7.812500,3.141593
+0.020000,32.871875,25.000000,0.128156,0.826562,3.515625,7.812500,3.141593
+0.020000,32.853875,25.000000,0.146156,0.900000,3.671875,7.812500,3.141593
+0.020000,32.834344,25.000000,0.165687,0.976563,3.828125,7.812500,3.141593
+0.020000,32.813219,25.000000,0.186813,1.056250,3.984375,7.812500,3.141593
+0.020000,32.790438,25.000000,0.209594,1.139062,4.140625,7.812500,3.141593
+0.020000,32.765937,25.000000,0.234094,1.225000,4.296875,7.812500,3.141593
+0.020000,32.739656,25.000000,0.260375,1.314062,4.453125,7.812500,3.141593
+0.020000,32.711531,25.000000,0.288500,1.406250,4.609375,7.812500,3.141593
+0.020000,32.681500,25.000000,0.318531,1.501563,4.765625,7.812500,3.141593
+0.020000,32.649500,25.000000,0.350531,1.600000,4.921875,7.812500,3.141593
+0.020000,32.615500,25.000000,0.384531,1.700000,5.000000,3.906250,3.141593
+0.020000,32.579500,25.000000,0.420531,1.800000,5.000000,-0.000000,3.141593
+0.020000,32.541500,25.000000,0.458531,1.900000,5.000000,0.000000,3.141593
+0.020000,32.501500,25.000000,0.498531,2.000000,5.000000,-0.000000,3.141593
+0.020000,32.459500,25.000000,0.540531,2.100000,5.000000,0.000000,3.141593
+0.020000,32.415500,25.000000,0.584531,2.200000,5.000000,-0.000000,3.141593
+0.020000,32.369500,25.000000,0.630531,2.300000,5.000000,0.000000,3.141593
+0.020000,32.321500,25.000000,0.678531,2.400000,5.000000,0.000000,3.141593
+0.020000,32.271531,25.000000,0.728500,2.498437,4.921875,-3.906250,3.141593
+0.020000,32.219656,25.000000,0.780375,2.593750,4.765625,-7.812500,3.141593
+0.020000,32.165937,25.000000,0.834094,2.685938,4.609375,-7.812500,3.141593
+0.020000,32.110438,25.000000,0.889594,2.775000,4.453125,-7.812500,3.141593
+0.020000,32.053219,25.000000,0.946813,2.860938,4.296875,-7.812500,3.141593
+0.020000,31.994344,25.000000,1.005688,2.943750,4.140625,-7.812500,3.141593
+0.020000,31.933875,25.000000,1.066156,3.023437,3.984375,-7.812500,3.141593
+0.020000,31.871875,25.000000,1.128156,3.100000,3.828125,-7.812500,3.141593
+0.020000,31.808406,25.000000,1.191625,3.173437,3.671875,-7.812500,3.141593
+0.020000,31.743531,25.000000,1.256500,3.243750,3.515625,-7.812500,3.141593
+0.020000,31.677312,25.000000,1.322719,3.310938,3.359375,-7.812500,3.141593
+0.020000,31.609813,25.000000,1.390219,3.375000,3.203125,-7.812500,3.141593
+0.020000,31.541094,25.000000,1.458937,3.435937,3.046875,-7.812500,3.141593
+0.020000,31.471219,25.000000,1.528812,3.493750,2.890625,-7.812500,3.141593
+0.020000,31.400250,25.000000,1.599781,3.548438,2.734375,-7.812500,3.141593
+0.020000,31.328250,25.000000,1.671781,3.600000,2.578125,-7.812500,3.141593
+0.020000,31.255281,25.000000,1.744750,3.648438,2.421875,-7.812500,3.141593
+0.020000,31.181406,25.000000,1.818625,3.693750,2.265625,-7.812500,3.141593
+0.020000,31.106687,25.000000,1.893344,3.735938,2.109375,-7.812500,3.141593
+0.020000,31.031188,25.000000,1.968844,3.775000,1.953125,-7.812500,3.141593
+0.020000,30.954969,25.000000,2.045062,3.810937,1.796875,-7.812500,3.141593
+0.020000,30.878094,25.000000,2.121937,3.843750,1.640625,-7.812500,3.141593
+0.020000,30.800625,25.000000,2.199406,3.873437,1.484375,-7.812500,3.141593
+0.020000,30.722625,25.000000,2.277406,3.900000,1.328125,-7.812500,3.141593
+0.020000,30.644156,25.000000,2.355875,3.923437,1.171875,-7.812500,3.141593
+0.020000,30.565281,25.000000,2.434750,3.943750,1.015625,-7.812500,3.141593
+0.020000,30.486063,25.000000,2.513969,3.960937,0.859375,-7.812500,3.141593
+0.020000,30.406563,25.000000,2.593469,3.975000,0.703125,-7.812500,3.141593
+0.020000,30.326844,25.000000,2.673187,3.985938,0.546875,-7.812500,3.141593
+0.020000,30.246969,25.000000,2.753062,3.993750,0.390625,-7.812500,3.141593
+0.020000,30.167000,25.000000,2.833031,3.998438,0.234375,-7.812500,3.141593
+0.020000,30.087000,25.000000,2.913031,4.000000,0.078125,-7.812500,3.141593
+0.020000,30.007000,25.000000,2.993031,4.000000,0.000000,-3.906250,3.141593
+0.020000,29.927000,25.000000,3.073031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.847000,25.000000,3.153031,4.000000,0.000000,-0.000000,3.141593
+0.020000,29.767000,25.000000,3.233031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.687000,25.000000,3.313031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.607000,25.000000,3.393031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.527000,25.000000,3.473031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.447000,25.000000,3.553031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.367000,25.000000,3.633031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.287000,25.000000,3.713031,4.000000,0.000000,0.000000,3.141593
+0.020000,29.207000,25.000000,3.793031,4.000000,0.000000,-0.000000,3.141593
+0.020000,29.127000,25.000000,3.873031,4.000000,-0.000000,-0.000000,3.141593
+0.020000,29.047000,25.000000,3.953031,4.000000,0.000000,0.000000,3.141593
+0.020000,28.967000,25.000000,4.033031,4.000000,0.000000,0.000000,3.141593
+0.020000,28.887000,25.000000,4.113031,4.000000,0.000000,-0.000000,3.141593
+0.020000,28.807013,25.000000,4.193018,3.999355,-0.032227,-1.611328,3.141593
+0.020000,28.727083,25.000000,4.272948,3.996504,-0.142578,-5.517578,3.141593
+0.020000,28.647272,25.000000,4.352759,3.990527,-0.298828,-7.812500,3.141593
+0.020000,28.567644,25.000000,4.432387,3.981426,-0.455078,-7.812500,3.141593
+0.020000,28.488260,25.000000,4.511771,3.969199,-0.611328,-7.812500,3.141593
+0.020000,28.409183,25.000000,4.590848,3.953848,-0.767578,-7.812500,3.141593
+0.020000,28.330475,25.000000,4.669556,3.935371,-0.923828,-7.812500,3.141593
+0.020000,28.252200,25.000000,4.747831,3.913770,-1.080078,-7.812500,3.141593
+0.020000,28.174419,25.000000,4.825612,3.889043,-1.236328,-7.812500,3.141593
+0.020000,28.097195,25.000000,4.902836,3.861191,-1.392578,-7.812500,3.141593
+0.020000,28.020591,25.000000,4.979440,3.830215,-1.548828,-7.812500,3.141593
+0.020000,27.944669,25.000000,5.055362,3.796113,-1.705078,-7.812500,3.141593
+0.020000,27.869491,25.000000,5.130540,3.758887,-1.861328,-7.812500,3.141593
+0.020000,27.795120,25.000000,5.204911,3.718535,-2.017578,-7.812500,3.141593
+0.020000,27.721619,25.000000,5.278412,3.675059,-2.173828,-7.812500,3.141593
+0.020000,27.649050,25.000000,5.350981,3.628457,-2.330078,-7.812500,3.141593
+0.020000,27.577475,25.000000,5.422556,3.578730,-2.486328,-7.812500,3.141593
+0.020000,27.506958,25.000000,5.493073,3.525879,-2.642578,-7.812500,3.141593
+0.020000,27.437560,25.000000,5.562471,3.469902,-2.798828,-7.812500,3.141593
+0.020000,27.369344,25.000000,5.630687,3.410801,-2.955078,-7.812500,3.141593
+0.020000,27.302372,25.000000,5.697659,3.348574,-3.111328,-7.812500,3.141593
+0.020000,27.236708,25.000000,5.763323,3.283223,-3.267578,-7.812500,3.141593
+0.020000,27.172413,25.000000,5.827618,3.214746,-3.423828,-7.812500,3.141593
+0.020000,27.109550,25.000000,5.890481,3.143145,-3.580078,-7.812500,3.141593
+0.020000,27.048182,25.000000,5.951850,3.068418,-3.736328,-7.812500,3.141593
+0.020000,26.988370,25.000000,6.011661,2.990566,-3.892578,-7.812500,3.141593
+0.020000,26.930179,25.000000,6.069853,2.909590,-4.048828,-7.812500,3.141593
+0.020000,26.873669,25.000000,6.126362,2.825488,-4.205078,-7.812500,3.141593
+0.020000,26.818904,25.000000,6.181128,2.738262,-4.361328,-7.812500,3.141593
+0.020000,26.765945,25.000000,6.234086,2.647910,-4.517578,-7.812500,3.141593
+0.020000,26.714857,25.000000,6.285175,2.554434,-4.673828,-7.812500,3.141593
+0.020000,26.665700,25.000000,6.334331,2.457832,-4.830078,-7.812500,3.141593
+0.020000,26.618525,25.000000,6.381506,2.358750,-4.954102,-6.201172,3.141593
+0.020000,26.573350,25.000000,6.426681,2.258750,-5.000000,-2.294922,3.141593
+0.020000,26.530175,25.000000,6.469856,2.158750,-5.000000,0.000000,3.141593
+0.020000,26.489000,25.000000,6.511031,2.058750,-5.000000,-0.000000,3.141593
+0.020000,26.449825,25.000000,6.550206,1.958750,-5.000000,0.000000,3.141593
+0.020000,26.412650,25.000000,6.587381,1.858750,-5.000000,0.000000,3.141593
+0.020000,26.377475,25.000000,6.622556,1.758750,-5.000000,-0.000000,3.141593
+0.020000,26.344300,25.000000,6.655731,1.658750,-5.000000,0.000000,3.141593
+0.020000,26.313112,25.000000,6.686919,1.559395,-4.967773,1.611328,3.141593
+0.020000,26.283867,25.000000,6.716164,1.462246,-4.857422,5.517578,3.141593
+0.020000,26.256503,25.000000,6.743529,1.368223,-4.701172,7.812500,3.141593
+0.020000,26.230956,25.000000,6.769075,1.277324,-4.544922,7.812500,3.141593
+0.020000,26.207165,25.000000,6.792866,1.189551,-4.388672,7.812500,3.141593
+0.020000,26.185067,25.000000,6.814964,1.104902,-4.232422,7.812500,3.141593
+0.020000,26.164600,25.000000,6.835432,1.023379,-4.076172,7.812500,3.141593
+0.020000,26.145700,25.000000,6.854331,0.944980,-3.919922,7.812500,3.141593
+0.020000,26.128306,25.000000,6.871725,0.869707,-3.763672,7.812500,3.141593
+0.020000,26.112355,25.000000,6.887677,0.797559,-3.607422,7.812500,3.141593
+0.020000,26.097784,25.000000,6.902247,0.728535,-3.451172,7.812500,3.141593
+0.020000,26.084531,25.000000,6.915500,0.662637,-3.294922,7.812500,3.141593
+0.020000,26.072534,25.000000,6.927497,0.599863,-3.138672,7.812500,3.141593
+0.020000,26.061730,25.000000,6.938302,0.540215,-2.982422,7.812500,3.141593
+0.020000,26.052056,25.000000,6.947975,0.483691,-2.826172,7.812500,3.141593
+0.020000,26.043450,25.000000,6.956581,0.430293,-2.669922,7.812500,3.141593
+0.020000,26.035850,25.000000,6.964182,0.380020,-2.513672,7.812500,3.141593
+0.020000,26.029192,25.000000,6.970839,0.332871,-2.357422,7.812500,3.141593
+0.020000,26.023415,25.000000,6.976616,0.288848,-2.201172,7.812500,3.141593
+0.020000,26.018456,25.000000,6.981575,0.247949,-2.044922,7.812500,3.141593
+0.020000,26.014253,25.000000,6.985779,0.210176,-1.888672,7.812500,3.141593
+0.020000,26.010742,25.000000,6.989289,0.175527,-1.732422,7.812500,3.141593
+0.020000,26.007862,25.000000,6.992169,0.144004,-1.576172,7.812500,3.141593
+0.020000,26.005550,25.000000,6.994481,0.115605,-1.419922,7.812500,3.141593
+0.020000,26.003743,25.000000,6.996288,0.090332,-1.263672,7.812500,3.141593
+0.020000,26.002380,25.000000,6.997652,0.068184,-1.107422,7.812500,3.141593
+0.020000,26.001396,25.000000,6.998635,0.049160,-0.951172,7.812500,3.141593
+0.020000,26.000731,25.000000,6.999300,0.033262,-0.794922,7.812500,3.141593
+0.020000,26.000321,25.000000,6.999710,0.020488,-0.638672,7.812500,3.141593
+0.020000,26.000105,25.000000,6.999927,0.010840,-0.482422,7.812500,3.141593
+0.020000,26.000018,25.000000,7.000013,0.004316,-0.326172,7.812500,3.141593
+0.020000,26.000000,25.000000,7.000031,0.000918,-0.169922,7.812500,3.141593
+0.020000,26.000000,25.000000,7.000031,0.000000,-0.045898,6.201172,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench9Feet.left.pf1.csv b/src/main/deploy/output/BlueSideTrench9Feet.left.pf1.csv
new file mode 100644
index 0000000..d56e820
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench9Feet.left.pf1.csv
@@ -0,0 +1,126 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,42.000000,23.000000,0.000058,0.005815,0.290727,14.536326,3.141593
+0.020000,42.000000,23.000000,0.000058,0.000000,-0.290727,-29.072652,3.141593
+0.020000,42.000000,23.000000,0.000058,0.000000,0.000000,14.536326,3.141593
+0.020000,42.000000,23.000000,0.000058,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000058,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,23.000000,0.000058,0.000000,0.000000,0.000000,3.141593
+0.020000,41.998860,23.000000,0.001198,0.057017,2.850857,142.542825,3.141593
+0.020000,41.995138,23.000000,0.004920,0.186065,6.452392,180.076784,3.141593
+0.020000,41.990429,23.000000,0.009630,0.235488,2.471175,-199.060838,3.141593
+0.020000,41.984614,23.000000,0.015444,0.290727,2.761902,14.536326,3.141593
+0.020000,41.977578,23.000000,0.022480,0.351779,3.052628,14.536326,3.141593
+0.020000,41.969206,23.000000,0.030853,0.418646,3.343355,14.536326,3.141593
+0.020000,41.959379,23.000000,0.040679,0.491328,3.634082,14.536326,3.141593
+0.020000,41.947983,23.000000,0.052076,0.569824,3.924808,14.536326,3.141593
+0.020000,41.934900,23.000000,0.065158,0.654135,4.215535,14.536326,3.141593
+0.020000,41.920015,23.000000,0.080044,0.744260,4.506261,14.536326,3.141593
+0.020000,41.903211,23.000000,0.096848,0.840200,4.796988,14.536326,3.141593
+0.020000,41.884372,23.000000,0.115687,0.941954,5.087714,14.536326,3.141593
+0.020000,41.863381,23.000000,0.136677,1.049523,5.378441,14.536326,3.141593
+0.020000,41.840123,23.000000,0.159935,1.162906,5.669167,14.536326,3.141593
+0.020000,41.814481,23.000000,0.185577,1.282104,5.959894,14.536326,3.141593
+0.020000,41.786339,23.000000,0.213720,1.407116,6.250620,14.536326,3.141593
+0.020000,41.755580,23.000000,0.244478,1.537943,6.541347,14.536326,3.141593
+0.020000,41.722088,23.000000,0.277970,1.674585,6.832073,14.536326,3.141593
+0.020000,41.685747,23.000000,0.314311,1.817041,7.122800,14.536326,3.141593
+0.020000,41.646441,23.000000,0.353617,1.965311,7.413526,14.536326,3.141593
+0.020000,41.604053,23.000000,0.396005,2.119396,7.704253,14.536326,3.141593
+0.020000,41.558525,23.000000,0.441533,2.276389,7.849616,7.268163,3.141593
+0.020000,41.509858,23.000000,0.490200,2.433381,7.849616,0.000000,3.141593
+0.020000,41.458050,23.000000,0.542008,2.590373,7.849616,0.000000,3.141593
+0.020000,41.403103,23.000000,0.596955,2.747366,7.849616,0.000000,3.141593
+0.020000,41.345016,23.000000,0.655042,2.904358,7.849616,-0.000000,3.141593
+0.020000,41.283789,23.000000,0.716269,3.061350,7.849616,0.000000,3.141593
+0.020000,41.219422,23.000000,0.780636,3.218343,7.849616,0.000000,3.141593
+0.020000,41.151915,23.000000,0.848143,3.375335,7.849616,0.000000,3.141593
+0.020000,41.081269,23.000000,0.918790,3.532327,7.849616,0.000000,3.141593
+0.020000,41.007540,23.000000,0.992518,3.686412,7.704253,-7.268163,3.141593
+0.020000,40.930847,23.000000,1.069211,3.834683,7.413526,-14.536326,3.141593
+0.020000,40.851304,23.000000,1.148754,3.977139,7.122800,-14.536326,3.141593
+0.020000,40.769028,23.000000,1.231030,4.113780,6.832073,-14.536326,3.141593
+0.020000,40.684136,23.000000,1.315922,4.244607,6.541347,-14.536326,3.141593
+0.020000,40.596744,23.000000,1.403314,4.369620,6.250620,-14.536326,3.141593
+0.020000,40.506967,23.000000,1.493091,4.488817,5.959894,-14.536326,3.141593
+0.020000,40.414923,23.000000,1.585135,4.602201,5.669167,-14.536326,3.141593
+0.020000,40.320728,23.000000,1.679330,4.709770,5.378441,-14.536326,3.141593
+0.020000,40.224498,23.000000,1.775561,4.811524,5.087714,-14.536326,3.141593
+0.020000,40.126348,23.000000,1.873710,4.907464,4.796988,-14.536326,3.141593
+0.020000,40.026396,23.000000,1.973662,4.997589,4.506261,-14.536326,3.141593
+0.020000,39.924759,23.000000,2.075300,5.081900,4.215535,-14.536326,3.141593
+0.020000,39.821551,23.000000,2.178508,5.160396,3.924808,-14.536326,3.141593
+0.020000,39.716889,23.000000,2.283169,5.233077,3.634082,-14.536326,3.141593
+0.020000,39.610890,23.000000,2.389168,5.299944,3.343355,-14.536326,3.141593
+0.020000,39.503670,23.000000,2.496388,5.360997,3.052628,-14.536326,3.141593
+0.020000,39.395346,23.000000,2.604713,5.416235,2.761902,-14.536326,3.141593
+0.020000,39.286032,23.000000,2.714026,5.465659,2.471175,-14.536326,3.141593
+0.020000,39.175847,23.000000,2.824211,5.509268,2.180449,-14.536326,3.141593
+0.020000,39.064906,23.000000,2.935152,5.547062,1.889722,-14.536326,3.141593
+0.020000,38.953325,23.000000,3.046733,5.579042,1.598996,-14.536326,3.141593
+0.020000,38.841221,23.000000,3.158837,5.605207,1.308269,-14.536326,3.141593
+0.020000,38.728710,23.000000,3.271349,5.625558,1.017543,-14.536326,3.141593
+0.020000,38.615908,23.000000,3.384150,5.640095,0.726816,-14.536326,3.141593
+0.020000,38.502932,23.000000,3.497126,5.648787,0.434627,-14.609451,3.141593
+0.020000,38.389957,23.000000,3.610101,5.648729,-0.002925,-21.877614,3.141593
+0.020000,38.277158,23.000000,3.722900,5.639948,-0.439015,-21.804489,3.141593
+0.020000,38.164651,23.000000,3.835407,5.625353,-0.729741,-14.536326,3.141593
+0.020000,38.052552,23.000000,3.947506,5.604944,-1.020468,-14.536326,3.141593
+0.020000,37.940978,23.000000,4.059080,5.578720,-1.311194,-14.536326,3.141593
+0.020000,37.830044,23.000000,4.170014,5.546682,-1.601921,-14.536326,3.141593
+0.020000,37.719868,23.000000,4.280190,5.508829,-1.892647,-14.536326,3.141593
+0.020000,37.610565,23.000000,4.389494,5.465161,-2.183374,-14.536326,3.141593
+0.020000,37.502251,23.000000,4.497807,5.415679,-2.474100,-14.536326,3.141593
+0.020000,37.395043,23.000000,4.605015,5.360383,-2.764827,-14.536326,3.141593
+0.020000,37.289058,23.000000,4.711000,5.299272,-3.055553,-14.536326,3.141593
+0.020000,37.184411,23.000000,4.815647,5.232346,-3.346280,-14.536326,3.141593
+0.020000,37.081219,23.000000,4.918839,5.159606,-3.637007,-14.536326,3.141593
+0.020000,36.979598,23.000000,5.020460,5.081051,-3.927733,-14.536326,3.141593
+0.020000,36.879664,23.000000,5.120394,4.996682,-4.218460,-14.536326,3.141593
+0.020000,36.781534,23.000000,5.218524,4.906498,-4.509186,-14.536326,3.141593
+0.020000,36.685324,23.000000,5.314734,4.810500,-4.799913,-14.536326,3.141593
+0.020000,36.591151,23.000000,5.408908,4.708687,-5.090639,-14.536326,3.141593
+0.020000,36.499129,23.000000,5.500929,4.601060,-5.381366,-14.536326,3.141593
+0.020000,36.409377,23.000000,5.590681,4.487618,-5.672092,-14.536326,3.141593
+0.020000,36.322010,23.000000,5.678048,4.368362,-5.962819,-14.536326,3.141593
+0.020000,36.237144,23.000000,5.762914,4.243291,-6.253545,-14.536326,3.141593
+0.020000,36.154896,23.000000,5.845162,4.112406,-6.544272,-14.536326,3.141593
+0.020000,36.075382,23.000000,5.924676,3.975706,-6.834998,-14.536326,3.141593
+0.020000,35.998718,23.000000,6.001340,3.833191,-7.125725,-14.536326,3.141593
+0.020000,35.925021,23.000000,6.075038,3.684862,-7.416451,-14.536326,3.141593
+0.020000,35.854406,23.000000,6.145652,3.530748,-7.705715,-14.463201,3.141593
+0.020000,35.786931,23.000000,6.213128,3.373755,-7.849616,-7.195038,3.141593
+0.020000,35.722595,23.000000,6.277463,3.216763,-7.849616,0.000000,3.141593
+0.020000,35.661400,23.000000,6.338658,3.059771,-7.849616,-0.000000,3.141593
+0.020000,35.603344,23.000000,6.396714,2.902778,-7.849616,0.000000,3.141593
+0.020000,35.548429,23.000000,6.451630,2.745786,-7.849616,-0.000000,3.141593
+0.020000,35.496653,23.000000,6.503405,2.588794,-7.849616,0.000000,3.141593
+0.020000,35.448017,23.000000,6.552041,2.431801,-7.849616,0.000000,3.141593
+0.020000,35.402520,23.000000,6.597538,2.274809,-7.849616,-0.000000,3.141593
+0.020000,35.360164,23.000000,6.639895,2.117846,-7.848154,0.073125,3.141593
+0.020000,35.320887,23.000000,6.679171,1.963820,-7.701328,7.341288,3.141593
+0.020000,35.284575,23.000000,6.715483,1.815608,-7.410601,14.536326,3.141593
+0.020000,35.251111,23.000000,6.748947,1.673210,-7.119875,14.536326,3.141593
+0.020000,35.220378,23.000000,6.779680,1.536627,-6.829148,14.536326,3.141593
+0.020000,35.192261,23.000000,6.807797,1.405859,-6.538422,14.536326,3.141593
+0.020000,35.166643,23.000000,6.833415,1.280905,-6.247695,14.536326,3.141593
+0.020000,35.143408,23.000000,6.856650,1.161765,-5.956969,14.536326,3.141593
+0.020000,35.122439,23.000000,6.877619,1.048440,-5.666242,14.536326,3.141593
+0.020000,35.103620,23.000000,6.896438,0.940930,-5.375516,14.536326,3.141593
+0.020000,35.086836,23.000000,6.913223,0.839234,-5.084789,14.536326,3.141593
+0.020000,35.071969,23.000000,6.928090,0.743353,-4.794063,14.536326,3.141593
+0.020000,35.058903,23.000000,6.941155,0.653286,-4.503336,14.536326,3.141593
+0.020000,35.047522,23.000000,6.952536,0.569034,-4.212610,14.536326,3.141593
+0.020000,35.037710,23.000000,6.962348,0.490597,-3.921883,14.536326,3.141593
+0.020000,35.029351,23.000000,6.970707,0.417973,-3.631157,14.536326,3.141593
+0.020000,35.022327,23.000000,6.977731,0.351165,-3.340430,14.536326,3.141593
+0.020000,35.016524,23.000000,6.983534,0.290171,-3.049703,14.536326,3.141593
+0.020000,35.011824,23.000000,6.988234,0.234991,-2.758977,14.536326,3.141593
+0.020000,35.008112,23.000000,6.991946,0.185626,-2.468250,14.536326,3.141593
+0.020000,35.005270,23.000000,6.994788,0.142076,-2.177524,14.536326,3.141593
+0.020000,35.003183,23.000000,6.996875,0.104340,-1.886797,14.536326,3.141593
+0.020000,35.001735,23.000000,6.998323,0.072418,-1.596071,14.536326,3.141593
+0.020000,35.000809,23.000000,6.999249,0.046311,-1.305344,14.536326,3.141593
+0.020000,35.000288,23.000000,6.999770,0.026019,-1.014618,14.536326,3.141593
+0.020000,35.000058,23.000000,7.000001,0.011541,-0.723891,14.536326,3.141593
+0.020000,35.000000,23.000000,7.000058,0.002878,-0.433165,14.536326,3.141593
+0.020000,35.000000,23.000000,7.000058,0.000000,-0.143901,14.463201,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench9Feet.pf1.csv b/src/main/deploy/output/BlueSideTrench9Feet.pf1.csv
new file mode 100644
index 0000000..eb6e67e
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench9Feet.pf1.csv
@@ -0,0 +1,126 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,42.000000,24.000000,0.000058,0.005815,0.290727,14.536326,3.141593
+0.020000,42.000000,24.000000,0.000291,0.017444,0.581453,14.536326,3.141593
+0.020000,42.000000,24.000000,0.000814,0.034887,0.872180,14.536326,3.141593
+0.020000,42.000000,24.000000,0.001744,0.058145,1.162906,14.536326,3.141593
+0.020000,42.000000,24.000000,0.003198,0.087218,1.453633,14.536326,3.141593
+0.020000,42.000000,24.000000,0.005291,0.122105,1.744359,14.536326,3.141593
+0.020000,41.998860,24.000000,0.008140,0.162807,2.035086,14.536326,3.141593
+0.020000,41.995138,24.000000,0.011862,0.209323,2.325812,14.536326,3.141593
+0.020000,41.990429,24.000000,0.016571,0.261654,2.616539,14.536326,3.141593
+0.020000,41.984614,24.000000,0.022386,0.319799,2.907265,14.536326,3.141593
+0.020000,41.977578,24.000000,0.029422,0.383759,3.197992,14.536326,3.141593
+0.020000,41.969206,24.000000,0.037794,0.453533,3.488718,14.536326,3.141593
+0.020000,41.959379,24.000000,0.047621,0.529122,3.779445,14.536326,3.141593
+0.020000,41.947983,24.000000,0.059017,0.610526,4.070171,14.536326,3.141593
+0.020000,41.934900,24.000000,0.072100,0.697744,4.360898,14.536326,3.141593
+0.020000,41.920015,24.000000,0.086985,0.790776,4.651624,14.536326,3.141593
+0.020000,41.903211,24.000000,0.103789,0.889623,4.942351,14.536326,3.141593
+0.020000,41.884372,24.000000,0.122628,0.994285,5.233077,14.536326,3.141593
+0.020000,41.863381,24.000000,0.143619,1.104761,5.523804,14.536326,3.141593
+0.020000,41.840123,24.000000,0.166877,1.221051,5.814530,14.536326,3.141593
+0.020000,41.814481,24.000000,0.192519,1.343157,6.105257,14.536326,3.141593
+0.020000,41.786339,24.000000,0.220661,1.471076,6.395983,14.536326,3.141593
+0.020000,41.755580,24.000000,0.251420,1.604810,6.686710,14.536326,3.141593
+0.020000,41.722088,24.000000,0.284912,1.744359,6.977437,14.536326,3.141593
+0.020000,41.685747,24.000000,0.321253,1.889722,7.268163,14.536326,3.141593
+0.020000,41.646441,24.000000,0.360559,2.040900,7.558890,14.536326,3.141593
+0.020000,41.604053,24.000000,0.402947,2.197893,7.849616,14.536326,3.141593
+0.020000,41.558525,24.000000,0.448475,2.354885,7.849616,0.000000,3.141593
+0.020000,41.509858,24.000000,0.497142,2.511877,7.849616,0.000000,3.141593
+0.020000,41.458050,24.000000,0.548950,2.668869,7.849616,0.000000,3.141593
+0.020000,41.403103,24.000000,0.603897,2.825862,7.849616,0.000000,3.141593
+0.020000,41.345016,24.000000,0.661984,2.982854,7.849616,0.000000,3.141593
+0.020000,41.283789,24.000000,0.723211,3.139846,7.849616,0.000000,3.141593
+0.020000,41.219422,24.000000,0.787578,3.296839,7.849616,0.000000,3.141593
+0.020000,41.151915,24.000000,0.855085,3.453831,7.849616,0.000000,3.141593
+0.020000,41.081269,24.000000,0.925731,3.610823,7.849616,-0.000000,3.141593
+0.020000,41.007540,24.000000,0.999460,3.762001,7.558890,-14.536326,3.141593
+0.020000,40.930847,24.000000,1.076153,3.907364,7.268163,-14.536326,3.141593
+0.020000,40.851304,24.000000,1.155696,4.046913,6.977437,-14.536326,3.141593
+0.020000,40.769028,24.000000,1.237972,4.180647,6.686710,-14.536326,3.141593
+0.020000,40.684136,24.000000,1.322864,4.308567,6.395983,-14.536326,3.141593
+0.020000,40.596744,24.000000,1.410256,4.430672,6.105257,-14.536326,3.141593
+0.020000,40.506967,24.000000,1.500033,4.546963,5.814530,-14.536326,3.141593
+0.020000,40.414923,24.000000,1.592077,4.657439,5.523804,-14.536326,3.141593
+0.020000,40.320728,24.000000,1.686272,4.762100,5.233077,-14.536326,3.141593
+0.020000,40.224498,24.000000,1.782502,4.860947,4.942351,-14.536326,3.141593
+0.020000,40.126348,24.000000,1.880652,4.953980,4.651624,-14.536326,3.141593
+0.020000,40.026396,24.000000,1.980604,5.041198,4.360898,-14.536326,3.141593
+0.020000,39.924759,24.000000,2.082241,5.122601,4.070171,-14.536326,3.141593
+0.020000,39.821551,24.000000,2.185449,5.198190,3.779445,-14.536326,3.141593
+0.020000,39.716889,24.000000,2.290111,5.267965,3.488718,-14.536326,3.141593
+0.020000,39.610890,24.000000,2.396110,5.331924,3.197992,-14.536326,3.141593
+0.020000,39.503670,24.000000,2.503330,5.390070,2.907265,-14.536326,3.141593
+0.020000,39.395346,24.000000,2.611654,5.442400,2.616539,-14.536326,3.141593
+0.020000,39.286032,24.000000,2.720968,5.488917,2.325812,-14.536326,3.141593
+0.020000,39.175847,24.000000,2.831153,5.529618,2.035086,-14.536326,3.141593
+0.020000,39.064906,24.000000,2.942094,5.564506,1.744359,-14.536326,3.141593
+0.020000,38.953325,24.000000,3.053675,5.593578,1.453633,-14.536326,3.141593
+0.020000,38.841221,24.000000,3.165779,5.616836,1.162906,-14.536326,3.141593
+0.020000,38.728710,24.000000,3.278290,5.634280,0.872180,-14.536326,3.141593
+0.020000,38.615908,24.000000,3.391092,5.645909,0.581453,-14.536326,3.141593
+0.020000,38.502932,24.000000,3.504068,5.651665,0.287802,-14.682576,3.141593
+0.020000,38.389957,24.000000,3.617043,5.645792,-0.293652,-29.072652,3.141593
+0.020000,38.277158,24.000000,3.729842,5.634104,-0.584378,-14.536326,3.141593
+0.020000,38.164651,24.000000,3.842349,5.616602,-0.875105,-14.536326,3.141593
+0.020000,38.052552,24.000000,3.954448,5.593286,-1.165831,-14.536326,3.141593
+0.020000,37.940978,24.000000,4.066022,5.564155,-1.456558,-14.536326,3.141593
+0.020000,37.830044,24.000000,4.176956,5.529209,-1.747284,-14.536326,3.141593
+0.020000,37.719868,24.000000,4.287132,5.488449,-2.038011,-14.536326,3.141593
+0.020000,37.610565,24.000000,4.396435,5.441874,-2.328737,-14.536326,3.141593
+0.020000,37.502251,24.000000,4.504749,5.389485,-2.619464,-14.536326,3.141593
+0.020000,37.395043,24.000000,4.611957,5.331281,-2.910190,-14.536326,3.141593
+0.020000,37.289058,24.000000,4.717942,5.267263,-3.200917,-14.536326,3.141593
+0.020000,37.184411,24.000000,4.822589,5.197430,-3.491643,-14.536326,3.141593
+0.020000,37.081219,24.000000,4.925781,5.121782,-3.782370,-14.536326,3.141593
+0.020000,36.979598,24.000000,5.027402,5.040320,-4.073096,-14.536326,3.141593
+0.020000,36.879664,24.000000,5.127336,4.953044,-4.363823,-14.536326,3.141593
+0.020000,36.781534,24.000000,5.225466,4.859953,-4.654549,-14.536326,3.141593
+0.020000,36.685324,24.000000,5.321676,4.761047,-4.945276,-14.536326,3.141593
+0.020000,36.591151,24.000000,5.415849,4.656327,-5.236002,-14.536326,3.141593
+0.020000,36.499129,24.000000,5.507871,4.545793,-5.526729,-14.536326,3.141593
+0.020000,36.409377,24.000000,5.597623,4.429444,-5.817455,-14.536326,3.141593
+0.020000,36.322010,24.000000,5.684990,4.307280,-6.108182,-14.536326,3.141593
+0.020000,36.237144,24.000000,5.769856,4.179302,-6.398908,-14.536326,3.141593
+0.020000,36.154896,24.000000,5.852104,4.045509,-6.689635,-14.536326,3.141593
+0.020000,36.075382,24.000000,5.931618,3.905902,-6.980362,-14.536326,3.141593
+0.020000,35.998718,24.000000,6.008282,3.760480,-7.271088,-14.536326,3.141593
+0.020000,35.925021,24.000000,6.081979,3.609244,-7.561815,-14.536326,3.141593
+0.020000,35.854406,24.000000,6.152594,3.452252,-7.849616,-14.390077,3.141593
+0.020000,35.786931,24.000000,6.220069,3.295259,-7.849616,0.000000,3.141593
+0.020000,35.722595,24.000000,6.284405,3.138267,-7.849616,0.000000,3.141593
+0.020000,35.661400,24.000000,6.345600,2.981275,-7.849616,0.000000,3.141593
+0.020000,35.603344,24.000000,6.403656,2.824282,-7.849616,0.000000,3.141593
+0.020000,35.548429,24.000000,6.458571,2.667290,-7.849616,0.000000,3.141593
+0.020000,35.496653,24.000000,6.510347,2.510298,-7.849616,-0.000000,3.141593
+0.020000,35.448017,24.000000,6.558983,2.353305,-7.849616,0.000000,3.141593
+0.020000,35.402520,24.000000,6.604480,2.196313,-7.849616,0.000000,3.141593
+0.020000,35.360164,24.000000,6.646836,2.039379,-7.846691,0.146250,3.141593
+0.020000,35.320887,24.000000,6.686113,1.888260,-7.555965,14.536326,3.141593
+0.020000,35.284575,24.000000,6.722425,1.742955,-7.265238,14.536326,3.141593
+0.020000,35.251111,24.000000,6.755889,1.603465,-6.974512,14.536326,3.141593
+0.020000,35.220378,24.000000,6.786622,1.469789,-6.683785,14.536326,3.141593
+0.020000,35.192261,24.000000,6.814739,1.341928,-6.393058,14.536326,3.141593
+0.020000,35.166643,24.000000,6.840357,1.219881,-6.102332,14.536326,3.141593
+0.020000,35.143408,24.000000,6.863592,1.103649,-5.811605,14.536326,3.141593
+0.020000,35.122439,24.000000,6.884561,0.993232,-5.520879,14.536326,3.141593
+0.020000,35.103620,24.000000,6.903380,0.888629,-5.230152,14.536326,3.141593
+0.020000,35.086836,24.000000,6.920164,0.789840,-4.939426,14.536326,3.141593
+0.020000,35.071969,24.000000,6.935031,0.696866,-4.648699,14.536326,3.141593
+0.020000,35.058903,24.000000,6.948097,0.609707,-4.357973,14.536326,3.141593
+0.020000,35.047522,24.000000,6.959478,0.528362,-4.067246,14.536326,3.141593
+0.020000,35.037710,24.000000,6.969290,0.452831,-3.776520,14.536326,3.141593
+0.020000,35.029351,24.000000,6.977649,0.383116,-3.485793,14.536326,3.141593
+0.020000,35.022327,24.000000,6.984673,0.319214,-3.195067,14.536326,3.141593
+0.020000,35.016524,24.000000,6.990476,0.261127,-2.904340,14.536326,3.141593
+0.020000,35.011824,24.000000,6.995176,0.208855,-2.613614,14.536326,3.141593
+0.020000,35.008112,24.000000,6.998888,0.162397,-2.322887,14.536326,3.141593
+0.020000,35.005270,24.000000,7.001730,0.121754,-2.032161,14.536326,3.141593
+0.020000,35.003183,24.000000,7.003817,0.086925,-1.741434,14.536326,3.141593
+0.020000,35.001735,24.000000,7.005265,0.057911,-1.450708,14.536326,3.141593
+0.020000,35.000809,24.000000,7.006191,0.034712,-1.159981,14.536326,3.141593
+0.020000,35.000288,24.000000,7.006712,0.017327,-0.869255,14.536326,3.141593
+0.020000,35.000058,24.000000,7.006942,0.005756,-0.578528,14.536326,3.141593
+0.020000,35.000000,24.000000,7.007000,0.000000,-0.287802,14.536326,3.141593
+0.020000,35.000000,24.000000,7.007000,0.000000,0.000000,14.390077,3.141593
diff --git a/src/main/deploy/output/BlueSideTrench9Feet.right.pf1.csv b/src/main/deploy/output/BlueSideTrench9Feet.right.pf1.csv
new file mode 100644
index 0000000..3681b0e
--- /dev/null
+++ b/src/main/deploy/output/BlueSideTrench9Feet.right.pf1.csv
@@ -0,0 +1,126 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,42.000000,25.000000,0.000058,0.005815,0.290727,14.536326,3.141593
+0.020000,42.000000,25.000000,0.000058,0.000000,-0.290727,-29.072652,3.141593
+0.020000,42.000000,25.000000,0.000058,0.000000,0.000000,14.536326,3.141593
+0.020000,42.000000,25.000000,0.000058,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000058,0.000000,0.000000,0.000000,3.141593
+0.020000,42.000000,25.000000,0.000058,0.000000,0.000000,0.000000,3.141593
+0.020000,41.998860,25.000000,0.001198,0.057017,2.850857,142.542825,3.141593
+0.020000,41.995138,25.000000,0.004920,0.186065,6.452392,180.076784,3.141593
+0.020000,41.990429,25.000000,0.009630,0.235488,2.471175,-199.060838,3.141593
+0.020000,41.984614,25.000000,0.015444,0.290727,2.761902,14.536326,3.141593
+0.020000,41.977578,25.000000,0.022480,0.351779,3.052628,14.536326,3.141593
+0.020000,41.969206,25.000000,0.030853,0.418646,3.343355,14.536326,3.141593
+0.020000,41.959379,25.000000,0.040679,0.491328,3.634082,14.536326,3.141593
+0.020000,41.947983,25.000000,0.052076,0.569824,3.924808,14.536326,3.141593
+0.020000,41.934900,25.000000,0.065158,0.654135,4.215535,14.536326,3.141593
+0.020000,41.920015,25.000000,0.080044,0.744260,4.506261,14.536326,3.141593
+0.020000,41.903211,25.000000,0.096848,0.840200,4.796988,14.536326,3.141593
+0.020000,41.884372,25.000000,0.115687,0.941954,5.087714,14.536326,3.141593
+0.020000,41.863381,25.000000,0.136677,1.049523,5.378441,14.536326,3.141593
+0.020000,41.840123,25.000000,0.159935,1.162906,5.669167,14.536326,3.141593
+0.020000,41.814481,25.000000,0.185577,1.282104,5.959894,14.536326,3.141593
+0.020000,41.786339,25.000000,0.213720,1.407116,6.250620,14.536326,3.141593
+0.020000,41.755580,25.000000,0.244478,1.537943,6.541347,14.536326,3.141593
+0.020000,41.722088,25.000000,0.277970,1.674585,6.832073,14.536326,3.141593
+0.020000,41.685747,25.000000,0.314311,1.817041,7.122800,14.536326,3.141593
+0.020000,41.646441,25.000000,0.353617,1.965311,7.413526,14.536326,3.141593
+0.020000,41.604053,25.000000,0.396005,2.119396,7.704253,14.536326,3.141593
+0.020000,41.558525,25.000000,0.441533,2.276389,7.849616,7.268163,3.141593
+0.020000,41.509858,25.000000,0.490200,2.433381,7.849616,0.000000,3.141593
+0.020000,41.458050,25.000000,0.542008,2.590373,7.849616,0.000000,3.141593
+0.020000,41.403103,25.000000,0.596955,2.747366,7.849616,0.000000,3.141593
+0.020000,41.345016,25.000000,0.655042,2.904358,7.849616,-0.000000,3.141593
+0.020000,41.283789,25.000000,0.716269,3.061350,7.849616,0.000000,3.141593
+0.020000,41.219422,25.000000,0.780636,3.218343,7.849616,0.000000,3.141593
+0.020000,41.151915,25.000000,0.848143,3.375335,7.849616,0.000000,3.141593
+0.020000,41.081269,25.000000,0.918790,3.532327,7.849616,0.000000,3.141593
+0.020000,41.007540,25.000000,0.992518,3.686412,7.704253,-7.268163,3.141593
+0.020000,40.930847,25.000000,1.069211,3.834683,7.413526,-14.536326,3.141593
+0.020000,40.851304,25.000000,1.148754,3.977139,7.122800,-14.536326,3.141593
+0.020000,40.769028,25.000000,1.231030,4.113780,6.832073,-14.536326,3.141593
+0.020000,40.684136,25.000000,1.315922,4.244607,6.541347,-14.536326,3.141593
+0.020000,40.596744,25.000000,1.403314,4.369620,6.250620,-14.536326,3.141593
+0.020000,40.506967,25.000000,1.493091,4.488817,5.959894,-14.536326,3.141593
+0.020000,40.414923,25.000000,1.585135,4.602201,5.669167,-14.536326,3.141593
+0.020000,40.320728,25.000000,1.679330,4.709770,5.378441,-14.536326,3.141593
+0.020000,40.224498,25.000000,1.775561,4.811524,5.087714,-14.536326,3.141593
+0.020000,40.126348,25.000000,1.873710,4.907464,4.796988,-14.536326,3.141593
+0.020000,40.026396,25.000000,1.973662,4.997589,4.506261,-14.536326,3.141593
+0.020000,39.924759,25.000000,2.075300,5.081900,4.215535,-14.536326,3.141593
+0.020000,39.821551,25.000000,2.178508,5.160396,3.924808,-14.536326,3.141593
+0.020000,39.716889,25.000000,2.283169,5.233077,3.634082,-14.536326,3.141593
+0.020000,39.610890,25.000000,2.389168,5.299944,3.343355,-14.536326,3.141593
+0.020000,39.503670,25.000000,2.496388,5.360997,3.052628,-14.536326,3.141593
+0.020000,39.395346,25.000000,2.604713,5.416235,2.761902,-14.536326,3.141593
+0.020000,39.286032,25.000000,2.714026,5.465659,2.471175,-14.536326,3.141593
+0.020000,39.175847,25.000000,2.824211,5.509268,2.180449,-14.536326,3.141593
+0.020000,39.064906,25.000000,2.935152,5.547062,1.889722,-14.536326,3.141593
+0.020000,38.953325,25.000000,3.046733,5.579042,1.598996,-14.536326,3.141593
+0.020000,38.841221,25.000000,3.158837,5.605207,1.308269,-14.536326,3.141593
+0.020000,38.728710,25.000000,3.271349,5.625558,1.017543,-14.536326,3.141593
+0.020000,38.615908,25.000000,3.384150,5.640095,0.726816,-14.536326,3.141593
+0.020000,38.502932,25.000000,3.497126,5.648787,0.434627,-14.609451,3.141593
+0.020000,38.389957,25.000000,3.610101,5.648729,-0.002925,-21.877614,3.141593
+0.020000,38.277158,25.000000,3.722900,5.639948,-0.439015,-21.804489,3.141593
+0.020000,38.164651,25.000000,3.835407,5.625353,-0.729741,-14.536326,3.141593
+0.020000,38.052552,25.000000,3.947506,5.604944,-1.020468,-14.536326,3.141593
+0.020000,37.940978,25.000000,4.059080,5.578720,-1.311194,-14.536326,3.141593
+0.020000,37.830044,25.000000,4.170014,5.546682,-1.601921,-14.536326,3.141593
+0.020000,37.719868,25.000000,4.280190,5.508829,-1.892647,-14.536326,3.141593
+0.020000,37.610565,25.000000,4.389494,5.465161,-2.183374,-14.536326,3.141593
+0.020000,37.502251,25.000000,4.497807,5.415679,-2.474100,-14.536326,3.141593
+0.020000,37.395043,25.000000,4.605015,5.360383,-2.764827,-14.536326,3.141593
+0.020000,37.289058,25.000000,4.711000,5.299272,-3.055553,-14.536326,3.141593
+0.020000,37.184411,25.000000,4.815647,5.232346,-3.346280,-14.536326,3.141593
+0.020000,37.081219,25.000000,4.918839,5.159606,-3.637007,-14.536326,3.141593
+0.020000,36.979598,25.000000,5.020460,5.081051,-3.927733,-14.536326,3.141593
+0.020000,36.879664,25.000000,5.120394,4.996682,-4.218460,-14.536326,3.141593
+0.020000,36.781534,25.000000,5.218524,4.906498,-4.509186,-14.536326,3.141593
+0.020000,36.685324,25.000000,5.314734,4.810500,-4.799913,-14.536326,3.141593
+0.020000,36.591151,25.000000,5.408908,4.708687,-5.090639,-14.536326,3.141593
+0.020000,36.499129,25.000000,5.500929,4.601060,-5.381366,-14.536326,3.141593
+0.020000,36.409377,25.000000,5.590681,4.487618,-5.672092,-14.536326,3.141593
+0.020000,36.322010,25.000000,5.678048,4.368362,-5.962819,-14.536326,3.141593
+0.020000,36.237144,25.000000,5.762914,4.243291,-6.253545,-14.536326,3.141593
+0.020000,36.154896,25.000000,5.845162,4.112406,-6.544272,-14.536326,3.141593
+0.020000,36.075382,25.000000,5.924676,3.975706,-6.834998,-14.536326,3.141593
+0.020000,35.998718,25.000000,6.001340,3.833191,-7.125725,-14.536326,3.141593
+0.020000,35.925021,25.000000,6.075038,3.684862,-7.416451,-14.536326,3.141593
+0.020000,35.854406,25.000000,6.145652,3.530748,-7.705715,-14.463201,3.141593
+0.020000,35.786931,25.000000,6.213128,3.373755,-7.849616,-7.195038,3.141593
+0.020000,35.722595,25.000000,6.277463,3.216763,-7.849616,0.000000,3.141593
+0.020000,35.661400,25.000000,6.338658,3.059771,-7.849616,-0.000000,3.141593
+0.020000,35.603344,25.000000,6.396714,2.902778,-7.849616,0.000000,3.141593
+0.020000,35.548429,25.000000,6.451630,2.745786,-7.849616,-0.000000,3.141593
+0.020000,35.496653,25.000000,6.503405,2.588794,-7.849616,0.000000,3.141593
+0.020000,35.448017,25.000000,6.552041,2.431801,-7.849616,0.000000,3.141593
+0.020000,35.402520,25.000000,6.597538,2.274809,-7.849616,-0.000000,3.141593
+0.020000,35.360164,25.000000,6.639895,2.117846,-7.848154,0.073125,3.141593
+0.020000,35.320887,25.000000,6.679171,1.963820,-7.701328,7.341288,3.141593
+0.020000,35.284575,25.000000,6.715483,1.815608,-7.410601,14.536326,3.141593
+0.020000,35.251111,25.000000,6.748947,1.673210,-7.119875,14.536326,3.141593
+0.020000,35.220378,25.000000,6.779680,1.536627,-6.829148,14.536326,3.141593
+0.020000,35.192261,25.000000,6.807797,1.405859,-6.538422,14.536326,3.141593
+0.020000,35.166643,25.000000,6.833415,1.280905,-6.247695,14.536326,3.141593
+0.020000,35.143408,25.000000,6.856650,1.161765,-5.956969,14.536326,3.141593
+0.020000,35.122439,25.000000,6.877619,1.048440,-5.666242,14.536326,3.141593
+0.020000,35.103620,25.000000,6.896438,0.940930,-5.375516,14.536326,3.141593
+0.020000,35.086836,25.000000,6.913223,0.839234,-5.084789,14.536326,3.141593
+0.020000,35.071969,25.000000,6.928090,0.743353,-4.794063,14.536326,3.141593
+0.020000,35.058903,25.000000,6.941155,0.653286,-4.503336,14.536326,3.141593
+0.020000,35.047522,25.000000,6.952536,0.569034,-4.212610,14.536326,3.141593
+0.020000,35.037710,25.000000,6.962348,0.490597,-3.921883,14.536326,3.141593
+0.020000,35.029351,25.000000,6.970707,0.417973,-3.631157,14.536326,3.141593
+0.020000,35.022327,25.000000,6.977731,0.351165,-3.340430,14.536326,3.141593
+0.020000,35.016524,25.000000,6.983534,0.290171,-3.049703,14.536326,3.141593
+0.020000,35.011824,25.000000,6.988234,0.234991,-2.758977,14.536326,3.141593
+0.020000,35.008112,25.000000,6.991946,0.185626,-2.468250,14.536326,3.141593
+0.020000,35.005270,25.000000,6.994788,0.142076,-2.177524,14.536326,3.141593
+0.020000,35.003183,25.000000,6.996875,0.104340,-1.886797,14.536326,3.141593
+0.020000,35.001735,25.000000,6.998323,0.072418,-1.596071,14.536326,3.141593
+0.020000,35.000809,25.000000,6.999249,0.046311,-1.305344,14.536326,3.141593
+0.020000,35.000288,25.000000,6.999770,0.026019,-1.014618,14.536326,3.141593
+0.020000,35.000058,25.000000,7.000001,0.011541,-0.723891,14.536326,3.141593
+0.020000,35.000000,25.000000,7.000058,0.002878,-0.433165,14.536326,3.141593
+0.020000,35.000000,25.000000,7.000058,0.000000,-0.143901,14.463201,3.141593
diff --git a/src/main/deploy/output/DriveTrench.left.pf1.csv b/src/main/deploy/output/DriveTrench.left.pf1.csv
new file mode 100644
index 0000000..6335a0c
--- /dev/null
+++ b/src/main/deploy/output/DriveTrench.left.pf1.csv
@@ -0,0 +1,238 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,19.000000,4.250000,0.000012,0.001160,0.057988,2.899392,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,-0.057988,-5.798785,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,2.899392,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,4.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.001498,4.250000,0.001510,0.074920,3.746024,187.301177,0.000000
+0.020000,19.003772,4.250000,0.003783,0.113656,1.936786,-90.461901,0.000000
+0.020000,19.006381,4.250000,0.006393,0.130473,0.840824,-54.798086,0.000000
+0.020000,19.009350,4.250000,0.009362,0.148449,0.898812,2.899392,0.000000
+0.020000,19.012702,4.250000,0.012713,0.167585,0.956799,2.899392,0.000000
+0.020000,19.016459,4.250000,0.016471,0.187881,1.014787,2.899392,0.000000
+0.020000,19.020646,4.250000,0.020658,0.209336,1.072775,2.899392,0.000000
+0.020000,19.025285,4.250000,0.025297,0.231951,1.130763,2.899392,0.000000
+0.020000,19.030400,4.250000,0.030411,0.255726,1.188751,2.899392,0.000000
+0.020000,19.036013,4.250000,0.036024,0.280661,1.246739,2.899392,0.000000
+0.020000,19.042148,4.250000,0.042159,0.306756,1.304727,2.899392,0.000000
+0.020000,19.048828,4.250000,0.048840,0.334010,1.362714,2.899392,0.000000
+0.020000,19.056077,4.250000,0.056088,0.362424,1.420702,2.899392,0.000000
+0.020000,19.063917,4.250000,0.063928,0.391998,1.478690,2.899392,0.000000
+0.020000,19.072371,4.250000,0.072383,0.422731,1.536678,2.899392,0.000000
+0.020000,19.081464,4.250000,0.081475,0.454625,1.594666,2.899392,0.000000
+0.020000,19.091217,4.250000,0.091229,0.487678,1.652654,2.899392,0.000000
+0.020000,19.101655,4.250000,0.101667,0.521891,1.710642,2.899392,0.000000
+0.020000,19.112800,4.250000,0.112812,0.557263,1.768629,2.899392,0.000000
+0.020000,19.124676,4.250000,0.124688,0.593796,1.826617,2.899392,0.000000
+0.020000,19.137306,4.250000,0.137318,0.631488,1.884605,2.899392,0.000000
+0.020000,19.150713,4.250000,0.150724,0.670340,1.942593,2.899392,0.000000
+0.020000,19.164920,4.250000,0.164931,0.710351,2.000581,2.899392,0.000000
+0.020000,19.179939,4.250000,0.179950,0.750943,2.029575,1.449696,0.000000
+0.020000,19.195769,4.250000,0.195781,0.791534,2.029575,-0.000000,0.000000
+0.020000,19.212412,4.250000,0.212423,0.832126,2.029575,0.000000,0.000000
+0.020000,19.229866,4.250000,0.229878,0.872717,2.029575,0.000000,0.000000
+0.020000,19.248132,4.250000,0.248144,0.913309,2.029575,0.000000,0.000000
+0.020000,19.267210,4.250000,0.267222,0.953900,2.029575,-0.000000,0.000000
+0.020000,19.287100,4.250000,0.287112,0.994492,2.029575,0.000000,0.000000
+0.020000,19.307802,4.250000,0.307813,1.035083,2.029575,0.000000,0.000000
+0.020000,19.329315,4.250000,0.329327,1.075675,2.029575,-0.000000,0.000000
+0.020000,19.351641,4.250000,0.351652,1.116266,2.029575,0.000000,0.000000
+0.020000,19.374778,4.250000,0.374789,1.156858,2.029575,0.000000,0.000000
+0.020000,19.398727,4.250000,0.398738,1.197449,2.029575,0.000000,0.000000
+0.020000,19.423488,4.250000,0.423499,1.238041,2.029575,-0.000000,0.000000
+0.020000,19.449060,4.250000,0.449072,1.278632,2.029575,0.000000,0.000000
+0.020000,19.475445,4.250000,0.475456,1.319224,2.029575,0.000000,0.000000
+0.020000,19.502641,4.250000,0.502653,1.359815,2.029575,-0.000000,0.000000
+0.020000,19.530649,4.250000,0.530661,1.400407,2.029575,0.000000,0.000000
+0.020000,19.559469,4.250000,0.559481,1.440998,2.029575,-0.000000,0.000000
+0.020000,19.589101,4.250000,0.589112,1.481590,2.029575,0.000000,0.000000
+0.020000,19.619544,4.250000,0.619556,1.522181,2.029575,-0.000000,0.000000
+0.020000,19.650800,4.250000,0.650812,1.562772,2.029575,0.000000,0.000000
+0.020000,19.682867,4.250000,0.682879,1.603364,2.029575,0.000000,0.000000
+0.020000,19.715746,4.250000,0.715758,1.643955,2.029575,-0.000000,0.000000
+0.020000,19.749437,4.250000,0.749449,1.684547,2.029575,0.000000,0.000000
+0.020000,19.783940,4.250000,0.783952,1.725138,2.029575,0.000000,0.000000
+0.020000,19.819255,4.250000,0.819266,1.765730,2.029575,-0.000000,0.000000
+0.020000,19.855381,4.250000,0.855393,1.806321,2.029575,0.000000,0.000000
+0.020000,19.892319,4.250000,0.892331,1.846913,2.029575,-0.000000,0.000000
+0.020000,19.930069,4.250000,0.930081,1.887504,2.029575,0.000000,0.000000
+0.020000,19.968631,4.250000,0.968643,1.928096,2.029575,0.000000,0.000000
+0.020000,20.008005,4.250000,1.008017,1.968687,2.029575,0.000000,0.000000
+0.020000,20.048191,4.250000,1.048202,2.009279,2.029575,-0.000000,0.000000
+0.020000,20.089188,4.250000,1.089200,2.049870,2.029575,0.000000,0.000000
+0.020000,20.130997,4.250000,1.131009,2.090462,2.029575,-0.000000,0.000000
+0.020000,20.173618,4.250000,1.173630,2.131053,2.029575,0.000000,0.000000
+0.020000,20.217051,4.250000,1.217063,2.171645,2.029575,-0.000000,0.000000
+0.020000,20.261296,4.250000,1.261308,2.212236,2.029575,0.000000,0.000000
+0.020000,20.306353,4.250000,1.306364,2.252828,2.029575,0.000000,0.000000
+0.020000,20.352221,4.250000,1.352233,2.293419,2.029575,-0.000000,0.000000
+0.020000,20.398901,4.250000,1.398913,2.334011,2.029575,0.000000,0.000000
+0.020000,20.446393,4.250000,1.446405,2.374602,2.029575,0.000000,0.000000
+0.020000,20.494697,4.250000,1.494709,2.415194,2.029575,-0.000000,0.000000
+0.020000,20.543813,4.250000,1.543824,2.455785,2.029575,0.000000,0.000000
+0.020000,20.593740,4.250000,1.593752,2.496377,2.029575,0.000000,0.000000
+0.020000,20.644480,4.250000,1.644491,2.536968,2.029575,-0.000000,0.000000
+0.020000,20.696031,4.250000,1.696042,2.577560,2.029575,0.000000,0.000000
+0.020000,20.748394,4.250000,1.748406,2.618151,2.029575,-0.000000,0.000000
+0.020000,20.801569,4.250000,1.801580,2.658743,2.029575,0.000000,0.000000
+0.020000,20.855555,4.250000,1.855567,2.699334,2.029575,-0.000000,0.000000
+0.020000,20.910342,4.250000,1.910354,2.739346,2.000581,-1.449696,0.000000
+0.020000,20.965906,4.250000,1.965918,2.778198,1.942593,-2.899392,0.000000
+0.020000,21.022224,4.250000,2.022236,2.815890,1.884605,-2.899392,0.000000
+0.020000,21.079273,4.250000,2.079284,2.852422,1.826617,-2.899392,0.000000
+0.020000,21.137028,4.250000,2.137040,2.887795,1.768629,-2.899392,0.000000
+0.020000,21.195469,4.250000,2.195480,2.922008,1.710642,-2.899392,0.000000
+0.020000,21.254570,4.250000,2.254581,2.955061,1.652654,-2.899392,0.000000
+0.020000,21.314309,4.250000,2.314321,2.986954,1.594666,-2.899392,0.000000
+0.020000,21.374663,4.250000,2.374674,3.017688,1.536678,-2.899392,0.000000
+0.020000,21.435608,4.250000,2.435619,3.047261,1.478690,-2.899392,0.000000
+0.020000,21.497121,4.250000,2.497133,3.075675,1.420702,-2.899392,0.000000
+0.020000,21.559180,4.250000,2.559192,3.102930,1.362714,-2.899392,0.000000
+0.020000,21.621760,4.250000,2.621772,3.129024,1.304727,-2.899392,0.000000
+0.020000,21.684840,4.250000,2.684851,3.153959,1.246739,-2.899392,0.000000
+0.020000,21.748394,4.250000,2.748406,3.177734,1.188751,-2.899392,0.000000
+0.020000,21.812401,4.250000,2.812413,3.200349,1.130763,-2.899392,0.000000
+0.020000,21.876837,4.250000,2.876849,3.221805,1.072775,-2.899392,0.000000
+0.020000,21.941679,4.250000,2.941691,3.242101,1.014787,-2.899392,0.000000
+0.020000,22.006904,4.250000,3.006916,3.261237,0.956799,-2.899392,0.000000
+0.020000,22.072488,4.250000,3.072500,3.279213,0.898812,-2.899392,0.000000
+0.020000,22.138409,4.250000,3.138421,3.296029,0.840824,-2.899392,0.000000
+0.020000,22.204643,4.250000,3.204654,3.311686,0.782836,-2.899392,0.000000
+0.020000,22.271166,4.250000,3.271178,3.326183,0.724848,-2.899392,0.000000
+0.020000,22.337957,4.250000,3.337968,3.339520,0.666860,-2.899392,0.000000
+0.020000,22.404991,4.250000,3.405002,3.351698,0.608872,-2.899392,0.000000
+0.020000,22.472245,4.250000,3.472257,3.362715,0.550885,-2.899392,0.000000
+0.020000,22.539697,4.250000,3.539708,3.372573,0.492897,-2.899392,0.000000
+0.020000,22.607322,4.250000,3.607334,3.381271,0.434909,-2.899392,0.000000
+0.020000,22.675098,4.250000,3.675110,3.388810,0.376921,-2.899392,0.000000
+0.020000,22.743002,4.250000,3.743013,3.395188,0.318933,-2.899392,0.000000
+0.020000,22.811010,4.250000,3.811022,3.400407,0.260945,-2.899392,0.000000
+0.020000,22.879099,4.250000,3.879111,3.404467,0.202957,-2.899392,0.000000
+0.020000,22.947247,4.250000,3.947258,3.407366,0.144970,-2.899392,0.000000
+0.020000,23.015422,4.250000,4.015434,3.408775,0.070461,-3.725447,0.000000
+0.020000,23.083584,4.250000,4.083596,3.408114,-0.033042,-5.175143,0.000000
+0.020000,23.151699,4.250000,4.151710,3.405714,-0.120024,-4.349089,0.000000
+0.020000,23.219742,4.250000,4.219753,3.402154,-0.178012,-2.899392,0.000000
+0.020000,23.287691,4.250000,4.287702,3.397434,-0.236000,-2.899392,0.000000
+0.020000,23.355522,4.250000,4.355533,3.391554,-0.293988,-2.899392,0.000000
+0.020000,23.423212,4.250000,4.423223,3.384514,-0.351975,-2.899392,0.000000
+0.020000,23.490738,4.250000,4.490750,3.376315,-0.409963,-2.899392,0.000000
+0.020000,23.558077,4.250000,4.558089,3.366956,-0.467951,-2.899392,0.000000
+0.020000,23.625206,4.250000,4.625218,3.356437,-0.525939,-2.899392,0.000000
+0.020000,23.692101,4.250000,4.692113,3.344759,-0.583927,-2.899392,0.000000
+0.020000,23.758740,4.250000,4.758751,3.331920,-0.641915,-2.899392,0.000000
+0.020000,23.825098,4.250000,4.825110,3.317922,-0.699902,-2.899392,0.000000
+0.020000,23.891153,4.250000,4.891165,3.302765,-0.757890,-2.899392,0.000000
+0.020000,23.956882,4.250000,4.956894,3.286447,-0.815878,-2.899392,0.000000
+0.020000,24.022262,4.250000,5.022273,3.268970,-0.873866,-2.899392,0.000000
+0.020000,24.087268,4.250000,5.087280,3.250333,-0.931854,-2.899392,0.000000
+0.020000,24.151879,4.250000,5.151891,3.230536,-0.989842,-2.899392,0.000000
+0.020000,24.216071,4.250000,5.216082,3.209579,-1.047830,-2.899392,0.000000
+0.020000,24.279820,4.250000,5.279831,3.187463,-1.105817,-2.899392,0.000000
+0.020000,24.343104,4.250000,5.343115,3.164187,-1.163805,-2.899392,0.000000
+0.020000,24.405899,4.250000,5.405910,3.139751,-1.221793,-2.899392,0.000000
+0.020000,24.468182,4.250000,5.468193,3.114155,-1.279781,-2.899392,0.000000
+0.020000,24.529930,4.250000,5.529941,3.087400,-1.337769,-2.899392,0.000000
+0.020000,24.591119,4.250000,5.591131,3.059485,-1.395757,-2.899392,0.000000
+0.020000,24.651728,4.250000,5.651739,3.030410,-1.453744,-2.899392,0.000000
+0.020000,24.711731,4.250000,5.711743,3.000175,-1.511732,-2.899392,0.000000
+0.020000,24.771107,4.250000,5.771118,2.968781,-1.569720,-2.899392,0.000000
+0.020000,24.829831,4.250000,5.829843,2.936227,-1.627708,-2.899392,0.000000
+0.020000,24.887882,4.250000,5.887893,2.902513,-1.685696,-2.899392,0.000000
+0.020000,24.945234,4.250000,5.945246,2.867639,-1.743684,-2.899392,0.000000
+0.020000,25.001866,4.250000,6.001878,2.831606,-1.801672,-2.899392,0.000000
+0.020000,25.057755,4.250000,6.057766,2.794412,-1.859659,-2.899392,0.000000
+0.020000,25.112876,4.250000,6.112887,2.756060,-1.917647,-2.899392,0.000000
+0.020000,25.167207,4.250000,6.167218,2.716547,-1.975635,-2.899392,0.000000
+0.020000,25.220731,4.250000,6.220743,2.676205,-2.017102,-2.073338,0.000000
+0.020000,25.273443,4.250000,6.273455,2.635613,-2.029575,-0.623641,0.000000
+0.020000,25.325344,4.250000,6.325355,2.595022,-2.029575,0.000000,0.000000
+0.020000,25.376432,4.250000,6.376444,2.554430,-2.029575,-0.000000,0.000000
+0.020000,25.426709,4.250000,6.426721,2.513839,-2.029575,0.000000,0.000000
+0.020000,25.476174,4.250000,6.476186,2.473247,-2.029575,0.000000,0.000000
+0.020000,25.524827,4.250000,6.524839,2.432656,-2.029575,-0.000000,0.000000
+0.020000,25.572668,4.250000,6.572680,2.392064,-2.029575,0.000000,0.000000
+0.020000,25.619698,4.250000,6.619709,2.351473,-2.029575,-0.000000,0.000000
+0.020000,25.665915,4.250000,6.665927,2.310881,-2.029575,0.000000,0.000000
+0.020000,25.711321,4.250000,6.711333,2.270290,-2.029575,0.000000,0.000000
+0.020000,25.755915,4.250000,6.755927,2.229698,-2.029575,-0.000000,0.000000
+0.020000,25.799697,4.250000,6.799709,2.189107,-2.029575,0.000000,0.000000
+0.020000,25.842668,4.250000,6.842679,2.148515,-2.029575,-0.000000,0.000000
+0.020000,25.884826,4.250000,6.884838,2.107924,-2.029575,-0.000000,0.000000
+0.020000,25.926173,4.250000,6.926184,2.067332,-2.029575,0.000000,0.000000
+0.020000,25.966708,4.250000,6.966719,2.026741,-2.029575,0.000000,0.000000
+0.020000,26.006431,4.250000,7.006442,1.986149,-2.029575,-0.000000,0.000000
+0.020000,26.045342,4.250000,7.045353,1.945558,-2.029575,0.000000,0.000000
+0.020000,26.083441,4.250000,7.083453,1.904966,-2.029575,0.000000,0.000000
+0.020000,26.120729,4.250000,7.120740,1.864375,-2.029575,-0.000000,0.000000
+0.020000,26.157204,4.250000,7.157216,1.823783,-2.029575,0.000000,0.000000
+0.020000,26.192868,4.250000,7.192880,1.783192,-2.029575,-0.000000,0.000000
+0.020000,26.227720,4.250000,7.227732,1.742600,-2.029575,0.000000,0.000000
+0.020000,26.261760,4.250000,7.261772,1.702009,-2.029575,0.000000,0.000000
+0.020000,26.294989,4.250000,7.295000,1.661417,-2.029575,-0.000000,0.000000
+0.020000,26.327405,4.250000,7.327417,1.620826,-2.029575,0.000000,0.000000
+0.020000,26.359010,4.250000,7.359021,1.580234,-2.029575,-0.000000,0.000000
+0.020000,26.389803,4.250000,7.389814,1.539643,-2.029575,0.000000,0.000000
+0.020000,26.419784,4.250000,7.419795,1.499051,-2.029575,0.000000,0.000000
+0.020000,26.448953,4.250000,7.448964,1.458460,-2.029575,-0.000000,0.000000
+0.020000,26.477310,4.250000,7.477322,1.417868,-2.029575,0.000000,0.000000
+0.020000,26.504856,4.250000,7.504867,1.377277,-2.029575,-0.000000,0.000000
+0.020000,26.531590,4.250000,7.531601,1.336685,-2.029575,0.000000,0.000000
+0.020000,26.557511,4.250000,7.557523,1.296094,-2.029575,-0.000000,0.000000
+0.020000,26.582621,4.250000,7.582633,1.255503,-2.029575,0.000000,0.000000
+0.020000,26.606920,4.250000,7.606931,1.214911,-2.029575,-0.000000,0.000000
+0.020000,26.630406,4.250000,7.630418,1.174320,-2.029575,0.000000,0.000000
+0.020000,26.653081,4.250000,7.653092,1.133728,-2.029575,0.000000,0.000000
+0.020000,26.674943,4.250000,7.674955,1.093137,-2.029575,-0.000000,0.000000
+0.020000,26.695994,4.250000,7.696006,1.052545,-2.029575,0.000000,0.000000
+0.020000,26.716233,4.250000,7.716245,1.011954,-2.029575,0.000000,0.000000
+0.020000,26.735661,4.250000,7.735672,0.971362,-2.029575,-0.000000,0.000000
+0.020000,26.754276,4.250000,7.754288,0.930771,-2.029575,0.000000,0.000000
+0.020000,26.772080,4.250000,7.772091,0.890179,-2.029575,0.000000,0.000000
+0.020000,26.789071,4.250000,7.789083,0.849588,-2.029575,-0.000000,0.000000
+0.020000,26.805251,4.250000,7.805263,0.808996,-2.029575,0.000000,0.000000
+0.020000,26.820619,4.250000,7.820631,0.768405,-2.029575,-0.000000,0.000000
+0.020000,26.835176,4.250000,7.835187,0.727813,-2.029575,0.000000,0.000000
+0.020000,26.848927,4.250000,7.848938,0.687552,-2.013054,0.826055,0.000000
+0.020000,26.861891,4.250000,7.861902,0.648201,-1.967539,2.275751,0.000000
+0.020000,26.874091,4.250000,7.874102,0.610010,-1.909551,2.899392,0.000000
+0.020000,26.885550,4.250000,7.885562,0.572979,-1.851563,2.899392,0.000000
+0.020000,26.896293,4.250000,7.896304,0.537107,-1.793575,2.899392,0.000000
+0.020000,26.906340,4.250000,7.906352,0.502396,-1.735587,2.899392,0.000000
+0.020000,26.915717,4.250000,7.915729,0.468844,-1.677599,2.899392,0.000000
+0.020000,26.924446,4.250000,7.924458,0.436452,-1.619611,2.899392,0.000000
+0.020000,26.932551,4.250000,7.932562,0.405219,-1.561624,2.899392,0.000000
+0.020000,26.940054,4.250000,7.940065,0.375146,-1.503636,2.899392,0.000000
+0.020000,26.946978,4.250000,7.946990,0.346233,-1.445648,2.899392,0.000000
+0.020000,26.953348,4.250000,7.953360,0.318480,-1.387660,2.899392,0.000000
+0.020000,26.959186,4.250000,7.959197,0.291887,-1.329672,2.899392,0.000000
+0.020000,26.964515,4.250000,7.964526,0.266453,-1.271684,2.899392,0.000000
+0.020000,26.969358,4.250000,7.969370,0.242179,-1.213697,2.899392,0.000000
+0.020000,26.973740,4.250000,7.973751,0.219065,-1.155709,2.899392,0.000000
+0.020000,26.977682,4.250000,7.977693,0.197111,-1.097721,2.899392,0.000000
+0.020000,26.981208,4.250000,7.981220,0.176316,-1.039733,2.899392,0.000000
+0.020000,26.984342,4.250000,7.984353,0.156681,-0.981745,2.899392,0.000000
+0.020000,26.987106,4.250000,7.987118,0.138206,-0.923757,2.899392,0.000000
+0.020000,26.989524,4.250000,7.989535,0.120890,-0.865769,2.899392,0.000000
+0.020000,26.991618,4.250000,7.991630,0.104735,-0.807782,2.899392,0.000000
+0.020000,26.993413,4.250000,7.993425,0.089739,-0.749794,2.899392,0.000000
+0.020000,26.994931,4.250000,7.994943,0.075903,-0.691806,2.899392,0.000000
+0.020000,26.996196,4.250000,7.996207,0.063226,-0.633818,2.899392,0.000000
+0.020000,26.997230,4.250000,7.997242,0.051710,-0.575830,2.899392,0.000000
+0.020000,26.998057,4.250000,7.998069,0.041353,-0.517842,2.899392,0.000000
+0.020000,26.998700,4.250000,7.998712,0.032156,-0.459855,2.899392,0.000000
+0.020000,26.999183,4.250000,7.999194,0.024119,-0.401867,2.899392,0.000000
+0.020000,26.999527,4.250000,7.999539,0.017241,-0.343879,2.899392,0.000000
+0.020000,26.999758,4.250000,7.999769,0.011523,-0.285891,2.899392,0.000000
+0.020000,26.999897,4.250000,7.999909,0.006965,-0.227903,2.899392,0.000000
+0.020000,26.999968,4.250000,7.999980,0.003567,-0.169915,2.899392,0.000000
+0.020000,26.999995,4.250000,8.000007,0.001328,-0.111927,2.899392,0.000000
+0.020000,27.000000,4.250000,8.000012,0.000249,-0.053940,2.899392,0.000000
+0.020000,27.000000,4.250000,8.000012,0.000000,-0.012473,2.073338,0.000000
diff --git a/src/main/deploy/output/DriveTrench.pf1.csv b/src/main/deploy/output/DriveTrench.pf1.csv
new file mode 100644
index 0000000..5af8623
--- /dev/null
+++ b/src/main/deploy/output/DriveTrench.pf1.csv
@@ -0,0 +1,238 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,19.000000,3.000000,0.000012,0.001160,0.057988,2.899392,0.000000
+0.020000,19.000000,3.000000,0.000058,0.003479,0.115976,2.899392,0.000000
+0.020000,19.000000,3.000000,0.000162,0.006959,0.173964,2.899392,0.000000
+0.020000,19.000000,3.000000,0.000348,0.011598,0.231951,2.899392,0.000000
+0.020000,19.000000,3.000000,0.000638,0.017396,0.289939,2.899392,0.000000
+0.020000,19.000000,3.000000,0.001055,0.024355,0.347927,2.899392,0.000000
+0.020000,19.000000,3.000000,0.001624,0.032473,0.405915,2.899392,0.000000
+0.020000,19.000000,3.000000,0.002366,0.041751,0.463903,2.899392,0.000000
+0.020000,19.000000,3.000000,0.003305,0.052189,0.521891,2.899392,0.000000
+0.020000,19.000000,3.000000,0.004465,0.063787,0.579878,2.899392,0.000000
+0.020000,19.000000,3.000000,0.005868,0.076544,0.637866,2.899392,0.000000
+0.020000,19.000000,3.000000,0.007538,0.090461,0.695854,2.899392,0.000000
+0.020000,19.001498,3.000000,0.009498,0.105538,0.753842,2.899392,0.000000
+0.020000,19.003772,3.000000,0.011772,0.121774,0.811830,2.899392,0.000000
+0.020000,19.006381,3.000000,0.014381,0.139171,0.869818,2.899392,0.000000
+0.020000,19.009350,3.000000,0.017350,0.157727,0.927806,2.899392,0.000000
+0.020000,19.012702,3.000000,0.020702,0.177443,0.985793,2.899392,0.000000
+0.020000,19.016459,3.000000,0.024459,0.198318,1.043781,2.899392,0.000000
+0.020000,19.020646,3.000000,0.028646,0.220354,1.101769,2.899392,0.000000
+0.020000,19.025285,3.000000,0.033285,0.243549,1.159757,2.899392,0.000000
+0.020000,19.030400,3.000000,0.038400,0.267904,1.217745,2.899392,0.000000
+0.020000,19.036013,3.000000,0.044013,0.293419,1.275733,2.899392,0.000000
+0.020000,19.042148,3.000000,0.050148,0.320093,1.333720,2.899392,0.000000
+0.020000,19.048828,3.000000,0.056828,0.347927,1.391708,2.899392,0.000000
+0.020000,19.056077,3.000000,0.064077,0.376921,1.449696,2.899392,0.000000
+0.020000,19.063917,3.000000,0.071917,0.407075,1.507684,2.899392,0.000000
+0.020000,19.072371,3.000000,0.080371,0.438388,1.565672,2.899392,0.000000
+0.020000,19.081464,3.000000,0.089464,0.470861,1.623660,2.899392,0.000000
+0.020000,19.091217,3.000000,0.099217,0.504494,1.681648,2.899392,0.000000
+0.020000,19.101655,3.000000,0.109655,0.539287,1.739635,2.899392,0.000000
+0.020000,19.112800,3.000000,0.120800,0.575239,1.797623,2.899392,0.000000
+0.020000,19.124676,3.000000,0.132676,0.612352,1.855611,2.899392,0.000000
+0.020000,19.137306,3.000000,0.145306,0.650624,1.913599,2.899392,0.000000
+0.020000,19.150713,3.000000,0.158713,0.690055,1.971587,2.899392,0.000000
+0.020000,19.164920,3.000000,0.172920,0.730647,2.029575,2.899392,0.000000
+0.020000,19.179939,3.000000,0.187939,0.771238,2.029575,0.000000,0.000000
+0.020000,19.195769,3.000000,0.203769,0.811830,2.029575,-0.000000,0.000000
+0.020000,19.212412,3.000000,0.220412,0.852421,2.029575,0.000000,0.000000
+0.020000,19.229866,3.000000,0.237866,0.893013,2.029575,0.000000,0.000000
+0.020000,19.248132,3.000000,0.256132,0.933604,2.029575,-0.000000,0.000000
+0.020000,19.267210,3.000000,0.275210,0.974196,2.029575,-0.000000,0.000000
+0.020000,19.287100,3.000000,0.295100,1.014787,2.029575,0.000000,0.000000
+0.020000,19.307802,3.000000,0.315802,1.055379,2.029575,0.000000,0.000000
+0.020000,19.329315,3.000000,0.337315,1.095970,2.029575,0.000000,0.000000
+0.020000,19.351641,3.000000,0.359641,1.136562,2.029575,-0.000000,0.000000
+0.020000,19.374778,3.000000,0.382778,1.177153,2.029575,0.000000,0.000000
+0.020000,19.398727,3.000000,0.406727,1.217745,2.029575,0.000000,0.000000
+0.020000,19.423488,3.000000,0.431488,1.258336,2.029575,-0.000000,0.000000
+0.020000,19.449060,3.000000,0.457060,1.298928,2.029575,-0.000000,0.000000
+0.020000,19.475445,3.000000,0.483445,1.339519,2.029575,0.000000,0.000000
+0.020000,19.502641,3.000000,0.510641,1.380111,2.029575,0.000000,0.000000
+0.020000,19.530649,3.000000,0.538649,1.420702,2.029575,-0.000000,0.000000
+0.020000,19.559469,3.000000,0.567469,1.461294,2.029575,-0.000000,0.000000
+0.020000,19.589101,3.000000,0.597101,1.501885,2.029575,0.000000,0.000000
+0.020000,19.619544,3.000000,0.627544,1.542477,2.029575,-0.000000,0.000000
+0.020000,19.650800,3.000000,0.658800,1.583068,2.029575,0.000000,0.000000
+0.020000,19.682867,3.000000,0.690867,1.623660,2.029575,-0.000000,0.000000
+0.020000,19.715746,3.000000,0.723746,1.664251,2.029575,0.000000,0.000000
+0.020000,19.749437,3.000000,0.757437,1.704843,2.029575,-0.000000,0.000000
+0.020000,19.783940,3.000000,0.791940,1.745434,2.029575,-0.000000,0.000000
+0.020000,19.819255,3.000000,0.827255,1.786026,2.029575,0.000000,0.000000
+0.020000,19.855381,3.000000,0.863381,1.826617,2.029575,-0.000000,0.000000
+0.020000,19.892319,3.000000,0.900319,1.867209,2.029575,0.000000,0.000000
+0.020000,19.930069,3.000000,0.938069,1.907800,2.029575,-0.000000,0.000000
+0.020000,19.968631,3.000000,0.976631,1.948392,2.029575,0.000000,0.000000
+0.020000,20.008005,3.000000,1.016005,1.988983,2.029575,0.000000,0.000000
+0.020000,20.048191,3.000000,1.056191,2.029575,2.029575,-0.000000,0.000000
+0.020000,20.089188,3.000000,1.097188,2.070166,2.029575,0.000000,0.000000
+0.020000,20.130997,3.000000,1.138997,2.110758,2.029575,0.000000,0.000000
+0.020000,20.173618,3.000000,1.181618,2.151349,2.029575,0.000000,0.000000
+0.020000,20.217051,3.000000,1.225051,2.191941,2.029575,0.000000,0.000000
+0.020000,20.261296,3.000000,1.269296,2.232532,2.029575,-0.000000,0.000000
+0.020000,20.306353,3.000000,1.314353,2.273124,2.029575,0.000000,0.000000
+0.020000,20.352221,3.000000,1.360221,2.313715,2.029575,0.000000,0.000000
+0.020000,20.398901,3.000000,1.406901,2.354307,2.029575,-0.000000,0.000000
+0.020000,20.446393,3.000000,1.454393,2.394898,2.029575,0.000000,0.000000
+0.020000,20.494697,3.000000,1.502697,2.435490,2.029575,0.000000,0.000000
+0.020000,20.543813,3.000000,1.551813,2.476081,2.029575,-0.000000,0.000000
+0.020000,20.593740,3.000000,1.601740,2.516673,2.029575,0.000000,0.000000
+0.020000,20.644480,3.000000,1.652480,2.557264,2.029575,-0.000000,0.000000
+0.020000,20.696031,3.000000,1.704031,2.597856,2.029575,0.000000,0.000000
+0.020000,20.748394,3.000000,1.756394,2.638447,2.029575,0.000000,0.000000
+0.020000,20.801569,3.000000,1.809569,2.679039,2.029575,-0.000000,0.000000
+0.020000,20.855555,3.000000,1.863555,2.719630,2.029575,0.000000,0.000000
+0.020000,20.910342,3.000000,1.918342,2.759062,1.971587,-2.899392,0.000000
+0.020000,20.965906,3.000000,1.973906,2.797334,1.913599,-2.899392,0.000000
+0.020000,21.022224,3.000000,2.030224,2.834446,1.855611,-2.899392,0.000000
+0.020000,21.079273,3.000000,2.087273,2.870398,1.797623,-2.899392,0.000000
+0.020000,21.137028,3.000000,2.145028,2.905191,1.739635,-2.899392,0.000000
+0.020000,21.195469,3.000000,2.203469,2.938824,1.681648,-2.899392,0.000000
+0.020000,21.254570,3.000000,2.262570,2.971297,1.623660,-2.899392,0.000000
+0.020000,21.314309,3.000000,2.322309,3.002611,1.565672,-2.899392,0.000000
+0.020000,21.374663,3.000000,2.382663,3.032764,1.507684,-2.899392,0.000000
+0.020000,21.435608,3.000000,2.443608,3.061758,1.449696,-2.899392,0.000000
+0.020000,21.497121,3.000000,2.505121,3.089593,1.391708,-2.899392,0.000000
+0.020000,21.559180,3.000000,2.567180,3.116267,1.333720,-2.899392,0.000000
+0.020000,21.621760,3.000000,2.629760,3.141782,1.275733,-2.899392,0.000000
+0.020000,21.684840,3.000000,2.692840,3.166136,1.217745,-2.899392,0.000000
+0.020000,21.748394,3.000000,2.756394,3.189332,1.159757,-2.899392,0.000000
+0.020000,21.812401,3.000000,2.820401,3.211367,1.101769,-2.899392,0.000000
+0.020000,21.876837,3.000000,2.884837,3.232243,1.043781,-2.899392,0.000000
+0.020000,21.941679,3.000000,2.949679,3.251958,0.985793,-2.899392,0.000000
+0.020000,22.006904,3.000000,3.014904,3.270515,0.927806,-2.899392,0.000000
+0.020000,22.072488,3.000000,3.080488,3.287911,0.869818,-2.899392,0.000000
+0.020000,22.138409,3.000000,3.146409,3.304148,0.811830,-2.899392,0.000000
+0.020000,22.204643,3.000000,3.212643,3.319224,0.753842,-2.899392,0.000000
+0.020000,22.271166,3.000000,3.279166,3.333141,0.695854,-2.899392,0.000000
+0.020000,22.337957,3.000000,3.345957,3.345899,0.637866,-2.899392,0.000000
+0.020000,22.404991,3.000000,3.412991,3.357496,0.579878,-2.899392,0.000000
+0.020000,22.472245,3.000000,3.480245,3.367934,0.521891,-2.899392,0.000000
+0.020000,22.539697,3.000000,3.547697,3.377212,0.463903,-2.899392,0.000000
+0.020000,22.607322,3.000000,3.615322,3.385331,0.405915,-2.899392,0.000000
+0.020000,22.675098,3.000000,3.683098,3.392289,0.347927,-2.899392,0.000000
+0.020000,22.743002,3.000000,3.751002,3.398088,0.289939,-2.899392,0.000000
+0.020000,22.811010,3.000000,3.819010,3.402727,0.231951,-2.899392,0.000000
+0.020000,22.879099,3.000000,3.887099,3.406206,0.173964,-2.899392,0.000000
+0.020000,22.947247,3.000000,3.955247,3.408526,0.115976,-2.899392,0.000000
+0.020000,23.015422,3.000000,4.023422,3.409025,0.024946,-4.551502,0.000000
+0.020000,23.083584,3.000000,4.091584,3.407204,-0.091030,-5.798785,0.000000
+0.020000,23.151699,3.000000,4.159699,3.404224,-0.149018,-2.899392,0.000000
+0.020000,23.219742,3.000000,4.227742,3.400084,-0.207006,-2.899392,0.000000
+0.020000,23.287691,3.000000,4.295691,3.394784,-0.264994,-2.899392,0.000000
+0.020000,23.355522,3.000000,4.363522,3.388324,-0.322981,-2.899392,0.000000
+0.020000,23.423212,3.000000,4.431212,3.380705,-0.380969,-2.899392,0.000000
+0.020000,23.490738,3.000000,4.498738,3.371925,-0.438957,-2.899392,0.000000
+0.020000,23.558077,3.000000,4.566077,3.361987,-0.496945,-2.899392,0.000000
+0.020000,23.625206,3.000000,4.633206,3.350888,-0.554933,-2.899392,0.000000
+0.020000,23.692101,3.000000,4.700101,3.338630,-0.612921,-2.899392,0.000000
+0.020000,23.758740,3.000000,4.766740,3.325211,-0.670909,-2.899392,0.000000
+0.020000,23.825098,3.000000,4.833098,3.310633,-0.728896,-2.899392,0.000000
+0.020000,23.891153,3.000000,4.899153,3.294896,-0.786884,-2.899392,0.000000
+0.020000,23.956882,3.000000,4.964882,3.277998,-0.844872,-2.899392,0.000000
+0.020000,24.022262,3.000000,5.030262,3.259941,-0.902860,-2.899392,0.000000
+0.020000,24.087268,3.000000,5.095268,3.240724,-0.960848,-2.899392,0.000000
+0.020000,24.151879,3.000000,5.159879,3.220347,-1.018836,-2.899392,0.000000
+0.020000,24.216071,3.000000,5.224071,3.198811,-1.076823,-2.899392,0.000000
+0.020000,24.279820,3.000000,5.287820,3.176115,-1.134811,-2.899392,0.000000
+0.020000,24.343104,3.000000,5.351104,3.152259,-1.192799,-2.899392,0.000000
+0.020000,24.405899,3.000000,5.413899,3.127243,-1.250787,-2.899392,0.000000
+0.020000,24.468182,3.000000,5.476182,3.101068,-1.308775,-2.899392,0.000000
+0.020000,24.529930,3.000000,5.537930,3.073732,-1.366763,-2.899392,0.000000
+0.020000,24.591119,3.000000,5.599119,3.045237,-1.424751,-2.899392,0.000000
+0.020000,24.651728,3.000000,5.659728,3.015582,-1.482738,-2.899392,0.000000
+0.020000,24.711731,3.000000,5.719731,2.984768,-1.540726,-2.899392,0.000000
+0.020000,24.771107,3.000000,5.779107,2.952794,-1.598714,-2.899392,0.000000
+0.020000,24.829831,3.000000,5.837831,2.919660,-1.656702,-2.899392,0.000000
+0.020000,24.887882,3.000000,5.895882,2.885366,-1.714690,-2.899392,0.000000
+0.020000,24.945234,3.000000,5.953234,2.849912,-1.772678,-2.899392,0.000000
+0.020000,25.001866,3.000000,6.009866,2.813299,-1.830665,-2.899392,0.000000
+0.020000,25.057755,3.000000,6.065755,2.775526,-1.888653,-2.899392,0.000000
+0.020000,25.112876,3.000000,6.120876,2.736593,-1.946641,-2.899392,0.000000
+0.020000,25.167207,3.000000,6.175207,2.696501,-2.004629,-2.899392,0.000000
+0.020000,25.220731,3.000000,6.228731,2.655909,-2.029575,-1.247283,0.000000
+0.020000,25.273443,3.000000,6.281443,2.615318,-2.029575,0.000000,0.000000
+0.020000,25.325344,3.000000,6.333344,2.574726,-2.029575,0.000000,0.000000
+0.020000,25.376432,3.000000,6.384432,2.534135,-2.029575,0.000000,0.000000
+0.020000,25.426709,3.000000,6.434709,2.493543,-2.029575,-0.000000,0.000000
+0.020000,25.476174,3.000000,6.484174,2.452952,-2.029575,0.000000,0.000000
+0.020000,25.524827,3.000000,6.532827,2.412360,-2.029575,0.000000,0.000000
+0.020000,25.572668,3.000000,6.580668,2.371769,-2.029575,-0.000000,0.000000
+0.020000,25.619698,3.000000,6.627698,2.331177,-2.029575,0.000000,0.000000
+0.020000,25.665915,3.000000,6.673915,2.290586,-2.029575,-0.000000,0.000000
+0.020000,25.711321,3.000000,6.719321,2.249994,-2.029575,0.000000,0.000000
+0.020000,25.755915,3.000000,6.763915,2.209403,-2.029575,0.000000,0.000000
+0.020000,25.799697,3.000000,6.807697,2.168811,-2.029575,0.000000,0.000000
+0.020000,25.842668,3.000000,6.850668,2.128220,-2.029575,-0.000000,0.000000
+0.020000,25.884826,3.000000,6.892826,2.087628,-2.029575,0.000000,0.000000
+0.020000,25.926173,3.000000,6.934173,2.047037,-2.029575,-0.000000,0.000000
+0.020000,25.966708,3.000000,6.974708,2.006445,-2.029575,0.000000,0.000000
+0.020000,26.006431,3.000000,7.014431,1.965854,-2.029575,0.000000,0.000000
+0.020000,26.045342,3.000000,7.053342,1.925262,-2.029575,-0.000000,0.000000
+0.020000,26.083441,3.000000,7.091441,1.884671,-2.029575,0.000000,0.000000
+0.020000,26.120729,3.000000,7.128729,1.844079,-2.029575,-0.000000,0.000000
+0.020000,26.157204,3.000000,7.165204,1.803488,-2.029575,-0.000000,0.000000
+0.020000,26.192868,3.000000,7.200868,1.762896,-2.029575,0.000000,0.000000
+0.020000,26.227720,3.000000,7.235720,1.722305,-2.029575,-0.000000,0.000000
+0.020000,26.261760,3.000000,7.269760,1.681713,-2.029575,0.000000,0.000000
+0.020000,26.294989,3.000000,7.302989,1.641122,-2.029575,0.000000,0.000000
+0.020000,26.327405,3.000000,7.335405,1.600530,-2.029575,0.000000,0.000000
+0.020000,26.359010,3.000000,7.367010,1.559939,-2.029575,0.000000,0.000000
+0.020000,26.389803,3.000000,7.397803,1.519347,-2.029575,-0.000000,0.000000
+0.020000,26.419784,3.000000,7.427784,1.478756,-2.029575,0.000000,0.000000
+0.020000,26.448953,3.000000,7.456953,1.438164,-2.029575,0.000000,0.000000
+0.020000,26.477310,3.000000,7.485310,1.397573,-2.029575,0.000000,0.000000
+0.020000,26.504856,3.000000,7.512856,1.356981,-2.029575,0.000000,0.000000
+0.020000,26.531590,3.000000,7.539590,1.316390,-2.029575,0.000000,0.000000
+0.020000,26.557511,3.000000,7.565511,1.275798,-2.029575,0.000000,0.000000
+0.020000,26.582621,3.000000,7.590621,1.235207,-2.029575,0.000000,0.000000
+0.020000,26.606920,3.000000,7.614920,1.194615,-2.029575,0.000000,0.000000
+0.020000,26.630406,3.000000,7.638406,1.154024,-2.029575,-0.000000,0.000000
+0.020000,26.653081,3.000000,7.661081,1.113432,-2.029575,-0.000000,0.000000
+0.020000,26.674943,3.000000,7.682943,1.072841,-2.029575,0.000000,0.000000
+0.020000,26.695994,3.000000,7.703994,1.032249,-2.029575,0.000000,0.000000
+0.020000,26.716233,3.000000,7.724233,0.991658,-2.029575,-0.000000,0.000000
+0.020000,26.735661,3.000000,7.743661,0.951066,-2.029575,0.000000,0.000000
+0.020000,26.754276,3.000000,7.762276,0.910475,-2.029575,0.000000,0.000000
+0.020000,26.772080,3.000000,7.780080,0.869883,-2.029575,0.000000,0.000000
+0.020000,26.789071,3.000000,7.797071,0.829292,-2.029575,-0.000000,0.000000
+0.020000,26.805251,3.000000,7.813251,0.788700,-2.029575,-0.000000,0.000000
+0.020000,26.820619,3.000000,7.828619,0.748109,-2.029575,0.000000,0.000000
+0.020000,26.835176,3.000000,7.843176,0.707517,-2.029575,0.000000,0.000000
+0.020000,26.848927,3.000000,7.856927,0.667587,-1.996532,1.652110,0.000000
+0.020000,26.861891,3.000000,7.869891,0.628816,-1.938545,2.899392,0.000000
+0.020000,26.874091,3.000000,7.882091,0.591205,-1.880557,2.899392,0.000000
+0.020000,26.885550,3.000000,7.893550,0.554753,-1.822569,2.899392,0.000000
+0.020000,26.896293,3.000000,7.904293,0.519462,-1.764581,2.899392,0.000000
+0.020000,26.906340,3.000000,7.914340,0.485330,-1.706593,2.899392,0.000000
+0.020000,26.915717,3.000000,7.923717,0.452358,-1.648605,2.899392,0.000000
+0.020000,26.924446,3.000000,7.932446,0.420545,-1.590618,2.899392,0.000000
+0.020000,26.932551,3.000000,7.940551,0.389893,-1.532630,2.899392,0.000000
+0.020000,26.940054,3.000000,7.948054,0.360400,-1.474642,2.899392,0.000000
+0.020000,26.946978,3.000000,7.954978,0.332067,-1.416654,2.899392,0.000000
+0.020000,26.953348,3.000000,7.961348,0.304894,-1.358666,2.899392,0.000000
+0.020000,26.959186,3.000000,7.967186,0.278880,-1.300678,2.899392,0.000000
+0.020000,26.964515,3.000000,7.972515,0.254026,-1.242690,2.899392,0.000000
+0.020000,26.969358,3.000000,7.977358,0.230332,-1.184703,2.899392,0.000000
+0.020000,26.973740,3.000000,7.981740,0.207798,-1.126715,2.899392,0.000000
+0.020000,26.977682,3.000000,7.985682,0.186423,-1.068727,2.899392,0.000000
+0.020000,26.981208,3.000000,7.989208,0.166208,-1.010739,2.899392,0.000000
+0.020000,26.984342,3.000000,7.992342,0.147153,-0.952751,2.899392,0.000000
+0.020000,26.987106,3.000000,7.995106,0.129258,-0.894763,2.899392,0.000000
+0.020000,26.989524,3.000000,7.997524,0.112523,-0.836776,2.899392,0.000000
+0.020000,26.991618,3.000000,7.999618,0.096947,-0.778788,2.899392,0.000000
+0.020000,26.993413,3.000000,8.001413,0.082531,-0.720800,2.899392,0.000000
+0.020000,26.994931,3.000000,8.002931,0.069275,-0.662812,2.899392,0.000000
+0.020000,26.996196,3.000000,8.004196,0.057178,-0.604824,2.899392,0.000000
+0.020000,26.997230,3.000000,8.005230,0.046241,-0.546836,2.899392,0.000000
+0.020000,26.998057,3.000000,8.006057,0.036464,-0.488848,2.899392,0.000000
+0.020000,26.998700,3.000000,8.006700,0.027847,-0.430861,2.899392,0.000000
+0.020000,26.999183,3.000000,8.007183,0.020390,-0.372873,2.899392,0.000000
+0.020000,26.999527,3.000000,8.007527,0.014092,-0.314885,2.899392,0.000000
+0.020000,26.999758,3.000000,8.007758,0.008954,-0.256897,2.899392,0.000000
+0.020000,26.999897,3.000000,8.007897,0.004976,-0.198909,2.899392,0.000000
+0.020000,26.999968,3.000000,8.007968,0.002158,-0.140921,2.899392,0.000000
+0.020000,26.999995,3.000000,8.007995,0.000499,-0.082934,2.899392,0.000000
+0.020000,27.000000,3.000000,8.008000,0.000000,-0.024946,2.899392,0.000000
+0.020000,27.000000,3.000000,8.008000,0.000000,0.000000,1.247283,0.000000
diff --git a/src/main/deploy/output/DriveTrench.right.pf1.csv b/src/main/deploy/output/DriveTrench.right.pf1.csv
new file mode 100644
index 0000000..cd203bd
--- /dev/null
+++ b/src/main/deploy/output/DriveTrench.right.pf1.csv
@@ -0,0 +1,238 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,19.000000,1.750000,0.000012,0.001160,0.057988,2.899392,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,-0.057988,-5.798785,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,2.899392,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.000000,1.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,19.001498,1.750000,0.001510,0.074920,3.746024,187.301177,0.000000
+0.020000,19.003772,1.750000,0.003783,0.113656,1.936786,-90.461901,0.000000
+0.020000,19.006381,1.750000,0.006393,0.130473,0.840824,-54.798086,0.000000
+0.020000,19.009350,1.750000,0.009362,0.148449,0.898812,2.899392,0.000000
+0.020000,19.012702,1.750000,0.012713,0.167585,0.956799,2.899392,0.000000
+0.020000,19.016459,1.750000,0.016471,0.187881,1.014787,2.899392,0.000000
+0.020000,19.020646,1.750000,0.020658,0.209336,1.072775,2.899392,0.000000
+0.020000,19.025285,1.750000,0.025297,0.231951,1.130763,2.899392,0.000000
+0.020000,19.030400,1.750000,0.030411,0.255726,1.188751,2.899392,0.000000
+0.020000,19.036013,1.750000,0.036024,0.280661,1.246739,2.899392,0.000000
+0.020000,19.042148,1.750000,0.042159,0.306756,1.304727,2.899392,0.000000
+0.020000,19.048828,1.750000,0.048840,0.334010,1.362714,2.899392,0.000000
+0.020000,19.056077,1.750000,0.056088,0.362424,1.420702,2.899392,0.000000
+0.020000,19.063917,1.750000,0.063928,0.391998,1.478690,2.899392,0.000000
+0.020000,19.072371,1.750000,0.072383,0.422731,1.536678,2.899392,0.000000
+0.020000,19.081464,1.750000,0.081475,0.454625,1.594666,2.899392,0.000000
+0.020000,19.091217,1.750000,0.091229,0.487678,1.652654,2.899392,0.000000
+0.020000,19.101655,1.750000,0.101667,0.521891,1.710642,2.899392,0.000000
+0.020000,19.112800,1.750000,0.112812,0.557263,1.768629,2.899392,0.000000
+0.020000,19.124676,1.750000,0.124688,0.593796,1.826617,2.899392,0.000000
+0.020000,19.137306,1.750000,0.137318,0.631488,1.884605,2.899392,0.000000
+0.020000,19.150713,1.750000,0.150724,0.670340,1.942593,2.899392,0.000000
+0.020000,19.164920,1.750000,0.164931,0.710351,2.000581,2.899392,0.000000
+0.020000,19.179939,1.750000,0.179950,0.750943,2.029575,1.449696,0.000000
+0.020000,19.195769,1.750000,0.195781,0.791534,2.029575,-0.000000,0.000000
+0.020000,19.212412,1.750000,0.212423,0.832126,2.029575,0.000000,0.000000
+0.020000,19.229866,1.750000,0.229878,0.872717,2.029575,0.000000,0.000000
+0.020000,19.248132,1.750000,0.248144,0.913309,2.029575,0.000000,0.000000
+0.020000,19.267210,1.750000,0.267222,0.953900,2.029575,-0.000000,0.000000
+0.020000,19.287100,1.750000,0.287112,0.994492,2.029575,0.000000,0.000000
+0.020000,19.307802,1.750000,0.307813,1.035083,2.029575,0.000000,0.000000
+0.020000,19.329315,1.750000,0.329327,1.075675,2.029575,-0.000000,0.000000
+0.020000,19.351641,1.750000,0.351652,1.116266,2.029575,0.000000,0.000000
+0.020000,19.374778,1.750000,0.374789,1.156858,2.029575,0.000000,0.000000
+0.020000,19.398727,1.750000,0.398738,1.197449,2.029575,0.000000,0.000000
+0.020000,19.423488,1.750000,0.423499,1.238041,2.029575,-0.000000,0.000000
+0.020000,19.449060,1.750000,0.449072,1.278632,2.029575,0.000000,0.000000
+0.020000,19.475445,1.750000,0.475456,1.319224,2.029575,0.000000,0.000000
+0.020000,19.502641,1.750000,0.502653,1.359815,2.029575,-0.000000,0.000000
+0.020000,19.530649,1.750000,0.530661,1.400407,2.029575,0.000000,0.000000
+0.020000,19.559469,1.750000,0.559481,1.440998,2.029575,-0.000000,0.000000
+0.020000,19.589101,1.750000,0.589112,1.481590,2.029575,0.000000,0.000000
+0.020000,19.619544,1.750000,0.619556,1.522181,2.029575,-0.000000,0.000000
+0.020000,19.650800,1.750000,0.650812,1.562772,2.029575,0.000000,0.000000
+0.020000,19.682867,1.750000,0.682879,1.603364,2.029575,0.000000,0.000000
+0.020000,19.715746,1.750000,0.715758,1.643955,2.029575,-0.000000,0.000000
+0.020000,19.749437,1.750000,0.749449,1.684547,2.029575,0.000000,0.000000
+0.020000,19.783940,1.750000,0.783952,1.725138,2.029575,0.000000,0.000000
+0.020000,19.819255,1.750000,0.819266,1.765730,2.029575,-0.000000,0.000000
+0.020000,19.855381,1.750000,0.855393,1.806321,2.029575,0.000000,0.000000
+0.020000,19.892319,1.750000,0.892331,1.846913,2.029575,-0.000000,0.000000
+0.020000,19.930069,1.750000,0.930081,1.887504,2.029575,0.000000,0.000000
+0.020000,19.968631,1.750000,0.968643,1.928096,2.029575,0.000000,0.000000
+0.020000,20.008005,1.750000,1.008017,1.968687,2.029575,0.000000,0.000000
+0.020000,20.048191,1.750000,1.048202,2.009279,2.029575,-0.000000,0.000000
+0.020000,20.089188,1.750000,1.089200,2.049870,2.029575,0.000000,0.000000
+0.020000,20.130997,1.750000,1.131009,2.090462,2.029575,-0.000000,0.000000
+0.020000,20.173618,1.750000,1.173630,2.131053,2.029575,0.000000,0.000000
+0.020000,20.217051,1.750000,1.217063,2.171645,2.029575,-0.000000,0.000000
+0.020000,20.261296,1.750000,1.261308,2.212236,2.029575,0.000000,0.000000
+0.020000,20.306353,1.750000,1.306364,2.252828,2.029575,0.000000,0.000000
+0.020000,20.352221,1.750000,1.352233,2.293419,2.029575,-0.000000,0.000000
+0.020000,20.398901,1.750000,1.398913,2.334011,2.029575,0.000000,0.000000
+0.020000,20.446393,1.750000,1.446405,2.374602,2.029575,0.000000,0.000000
+0.020000,20.494697,1.750000,1.494709,2.415194,2.029575,-0.000000,0.000000
+0.020000,20.543813,1.750000,1.543824,2.455785,2.029575,0.000000,0.000000
+0.020000,20.593740,1.750000,1.593752,2.496377,2.029575,0.000000,0.000000
+0.020000,20.644480,1.750000,1.644491,2.536968,2.029575,-0.000000,0.000000
+0.020000,20.696031,1.750000,1.696042,2.577560,2.029575,0.000000,0.000000
+0.020000,20.748394,1.750000,1.748406,2.618151,2.029575,-0.000000,0.000000
+0.020000,20.801569,1.750000,1.801580,2.658743,2.029575,0.000000,0.000000
+0.020000,20.855555,1.750000,1.855567,2.699334,2.029575,-0.000000,0.000000
+0.020000,20.910342,1.750000,1.910354,2.739346,2.000581,-1.449696,0.000000
+0.020000,20.965906,1.750000,1.965918,2.778198,1.942593,-2.899392,0.000000
+0.020000,21.022224,1.750000,2.022236,2.815890,1.884605,-2.899392,0.000000
+0.020000,21.079273,1.750000,2.079284,2.852422,1.826617,-2.899392,0.000000
+0.020000,21.137028,1.750000,2.137040,2.887795,1.768629,-2.899392,0.000000
+0.020000,21.195469,1.750000,2.195480,2.922008,1.710642,-2.899392,0.000000
+0.020000,21.254570,1.750000,2.254581,2.955061,1.652654,-2.899392,0.000000
+0.020000,21.314309,1.750000,2.314321,2.986954,1.594666,-2.899392,0.000000
+0.020000,21.374663,1.750000,2.374674,3.017688,1.536678,-2.899392,0.000000
+0.020000,21.435608,1.750000,2.435619,3.047261,1.478690,-2.899392,0.000000
+0.020000,21.497121,1.750000,2.497133,3.075675,1.420702,-2.899392,0.000000
+0.020000,21.559180,1.750000,2.559192,3.102930,1.362714,-2.899392,0.000000
+0.020000,21.621760,1.750000,2.621772,3.129024,1.304727,-2.899392,0.000000
+0.020000,21.684840,1.750000,2.684851,3.153959,1.246739,-2.899392,0.000000
+0.020000,21.748394,1.750000,2.748406,3.177734,1.188751,-2.899392,0.000000
+0.020000,21.812401,1.750000,2.812413,3.200349,1.130763,-2.899392,0.000000
+0.020000,21.876837,1.750000,2.876849,3.221805,1.072775,-2.899392,0.000000
+0.020000,21.941679,1.750000,2.941691,3.242101,1.014787,-2.899392,0.000000
+0.020000,22.006904,1.750000,3.006916,3.261237,0.956799,-2.899392,0.000000
+0.020000,22.072488,1.750000,3.072500,3.279213,0.898812,-2.899392,0.000000
+0.020000,22.138409,1.750000,3.138421,3.296029,0.840824,-2.899392,0.000000
+0.020000,22.204643,1.750000,3.204654,3.311686,0.782836,-2.899392,0.000000
+0.020000,22.271166,1.750000,3.271178,3.326183,0.724848,-2.899392,0.000000
+0.020000,22.337957,1.750000,3.337968,3.339520,0.666860,-2.899392,0.000000
+0.020000,22.404991,1.750000,3.405002,3.351698,0.608872,-2.899392,0.000000
+0.020000,22.472245,1.750000,3.472257,3.362715,0.550885,-2.899392,0.000000
+0.020000,22.539697,1.750000,3.539708,3.372573,0.492897,-2.899392,0.000000
+0.020000,22.607322,1.750000,3.607334,3.381271,0.434909,-2.899392,0.000000
+0.020000,22.675098,1.750000,3.675110,3.388810,0.376921,-2.899392,0.000000
+0.020000,22.743002,1.750000,3.743013,3.395188,0.318933,-2.899392,0.000000
+0.020000,22.811010,1.750000,3.811022,3.400407,0.260945,-2.899392,0.000000
+0.020000,22.879099,1.750000,3.879111,3.404467,0.202957,-2.899392,0.000000
+0.020000,22.947247,1.750000,3.947258,3.407366,0.144970,-2.899392,0.000000
+0.020000,23.015422,1.750000,4.015434,3.408775,0.070461,-3.725447,0.000000
+0.020000,23.083584,1.750000,4.083596,3.408114,-0.033042,-5.175143,0.000000
+0.020000,23.151699,1.750000,4.151710,3.405714,-0.120024,-4.349089,0.000000
+0.020000,23.219742,1.750000,4.219753,3.402154,-0.178012,-2.899392,0.000000
+0.020000,23.287691,1.750000,4.287702,3.397434,-0.236000,-2.899392,0.000000
+0.020000,23.355522,1.750000,4.355533,3.391554,-0.293988,-2.899392,0.000000
+0.020000,23.423212,1.750000,4.423223,3.384514,-0.351975,-2.899392,0.000000
+0.020000,23.490738,1.750000,4.490750,3.376315,-0.409963,-2.899392,0.000000
+0.020000,23.558077,1.750000,4.558089,3.366956,-0.467951,-2.899392,0.000000
+0.020000,23.625206,1.750000,4.625218,3.356437,-0.525939,-2.899392,0.000000
+0.020000,23.692101,1.750000,4.692113,3.344759,-0.583927,-2.899392,0.000000
+0.020000,23.758740,1.750000,4.758751,3.331920,-0.641915,-2.899392,0.000000
+0.020000,23.825098,1.750000,4.825110,3.317922,-0.699902,-2.899392,0.000000
+0.020000,23.891153,1.750000,4.891165,3.302765,-0.757890,-2.899392,0.000000
+0.020000,23.956882,1.750000,4.956894,3.286447,-0.815878,-2.899392,0.000000
+0.020000,24.022262,1.750000,5.022273,3.268970,-0.873866,-2.899392,0.000000
+0.020000,24.087268,1.750000,5.087280,3.250333,-0.931854,-2.899392,0.000000
+0.020000,24.151879,1.750000,5.151891,3.230536,-0.989842,-2.899392,0.000000
+0.020000,24.216071,1.750000,5.216082,3.209579,-1.047830,-2.899392,0.000000
+0.020000,24.279820,1.750000,5.279831,3.187463,-1.105817,-2.899392,0.000000
+0.020000,24.343104,1.750000,5.343115,3.164187,-1.163805,-2.899392,0.000000
+0.020000,24.405899,1.750000,5.405910,3.139751,-1.221793,-2.899392,0.000000
+0.020000,24.468182,1.750000,5.468193,3.114155,-1.279781,-2.899392,0.000000
+0.020000,24.529930,1.750000,5.529941,3.087400,-1.337769,-2.899392,0.000000
+0.020000,24.591119,1.750000,5.591131,3.059485,-1.395757,-2.899392,0.000000
+0.020000,24.651728,1.750000,5.651739,3.030410,-1.453744,-2.899392,0.000000
+0.020000,24.711731,1.750000,5.711743,3.000175,-1.511732,-2.899392,0.000000
+0.020000,24.771107,1.750000,5.771118,2.968781,-1.569720,-2.899392,0.000000
+0.020000,24.829831,1.750000,5.829843,2.936227,-1.627708,-2.899392,0.000000
+0.020000,24.887882,1.750000,5.887893,2.902513,-1.685696,-2.899392,0.000000
+0.020000,24.945234,1.750000,5.945246,2.867639,-1.743684,-2.899392,0.000000
+0.020000,25.001866,1.750000,6.001878,2.831606,-1.801672,-2.899392,0.000000
+0.020000,25.057755,1.750000,6.057766,2.794412,-1.859659,-2.899392,0.000000
+0.020000,25.112876,1.750000,6.112887,2.756060,-1.917647,-2.899392,0.000000
+0.020000,25.167207,1.750000,6.167218,2.716547,-1.975635,-2.899392,0.000000
+0.020000,25.220731,1.750000,6.220743,2.676205,-2.017102,-2.073338,0.000000
+0.020000,25.273443,1.750000,6.273455,2.635613,-2.029575,-0.623641,0.000000
+0.020000,25.325344,1.750000,6.325355,2.595022,-2.029575,0.000000,0.000000
+0.020000,25.376432,1.750000,6.376444,2.554430,-2.029575,-0.000000,0.000000
+0.020000,25.426709,1.750000,6.426721,2.513839,-2.029575,0.000000,0.000000
+0.020000,25.476174,1.750000,6.476186,2.473247,-2.029575,0.000000,0.000000
+0.020000,25.524827,1.750000,6.524839,2.432656,-2.029575,-0.000000,0.000000
+0.020000,25.572668,1.750000,6.572680,2.392064,-2.029575,0.000000,0.000000
+0.020000,25.619698,1.750000,6.619709,2.351473,-2.029575,-0.000000,0.000000
+0.020000,25.665915,1.750000,6.665927,2.310881,-2.029575,0.000000,0.000000
+0.020000,25.711321,1.750000,6.711333,2.270290,-2.029575,0.000000,0.000000
+0.020000,25.755915,1.750000,6.755927,2.229698,-2.029575,-0.000000,0.000000
+0.020000,25.799697,1.750000,6.799709,2.189107,-2.029575,0.000000,0.000000
+0.020000,25.842668,1.750000,6.842679,2.148515,-2.029575,-0.000000,0.000000
+0.020000,25.884826,1.750000,6.884838,2.107924,-2.029575,-0.000000,0.000000
+0.020000,25.926173,1.750000,6.926184,2.067332,-2.029575,0.000000,0.000000
+0.020000,25.966708,1.750000,6.966719,2.026741,-2.029575,0.000000,0.000000
+0.020000,26.006431,1.750000,7.006442,1.986149,-2.029575,-0.000000,0.000000
+0.020000,26.045342,1.750000,7.045353,1.945558,-2.029575,0.000000,0.000000
+0.020000,26.083441,1.750000,7.083453,1.904966,-2.029575,0.000000,0.000000
+0.020000,26.120729,1.750000,7.120740,1.864375,-2.029575,-0.000000,0.000000
+0.020000,26.157204,1.750000,7.157216,1.823783,-2.029575,0.000000,0.000000
+0.020000,26.192868,1.750000,7.192880,1.783192,-2.029575,-0.000000,0.000000
+0.020000,26.227720,1.750000,7.227732,1.742600,-2.029575,0.000000,0.000000
+0.020000,26.261760,1.750000,7.261772,1.702009,-2.029575,0.000000,0.000000
+0.020000,26.294989,1.750000,7.295000,1.661417,-2.029575,-0.000000,0.000000
+0.020000,26.327405,1.750000,7.327417,1.620826,-2.029575,0.000000,0.000000
+0.020000,26.359010,1.750000,7.359021,1.580234,-2.029575,-0.000000,0.000000
+0.020000,26.389803,1.750000,7.389814,1.539643,-2.029575,0.000000,0.000000
+0.020000,26.419784,1.750000,7.419795,1.499051,-2.029575,0.000000,0.000000
+0.020000,26.448953,1.750000,7.448964,1.458460,-2.029575,-0.000000,0.000000
+0.020000,26.477310,1.750000,7.477322,1.417868,-2.029575,0.000000,0.000000
+0.020000,26.504856,1.750000,7.504867,1.377277,-2.029575,-0.000000,0.000000
+0.020000,26.531590,1.750000,7.531601,1.336685,-2.029575,0.000000,0.000000
+0.020000,26.557511,1.750000,7.557523,1.296094,-2.029575,-0.000000,0.000000
+0.020000,26.582621,1.750000,7.582633,1.255503,-2.029575,0.000000,0.000000
+0.020000,26.606920,1.750000,7.606931,1.214911,-2.029575,-0.000000,0.000000
+0.020000,26.630406,1.750000,7.630418,1.174320,-2.029575,0.000000,0.000000
+0.020000,26.653081,1.750000,7.653092,1.133728,-2.029575,0.000000,0.000000
+0.020000,26.674943,1.750000,7.674955,1.093137,-2.029575,-0.000000,0.000000
+0.020000,26.695994,1.750000,7.696006,1.052545,-2.029575,0.000000,0.000000
+0.020000,26.716233,1.750000,7.716245,1.011954,-2.029575,0.000000,0.000000
+0.020000,26.735661,1.750000,7.735672,0.971362,-2.029575,-0.000000,0.000000
+0.020000,26.754276,1.750000,7.754288,0.930771,-2.029575,0.000000,0.000000
+0.020000,26.772080,1.750000,7.772091,0.890179,-2.029575,0.000000,0.000000
+0.020000,26.789071,1.750000,7.789083,0.849588,-2.029575,-0.000000,0.000000
+0.020000,26.805251,1.750000,7.805263,0.808996,-2.029575,0.000000,0.000000
+0.020000,26.820619,1.750000,7.820631,0.768405,-2.029575,-0.000000,0.000000
+0.020000,26.835176,1.750000,7.835187,0.727813,-2.029575,0.000000,0.000000
+0.020000,26.848927,1.750000,7.848938,0.687552,-2.013054,0.826055,0.000000
+0.020000,26.861891,1.750000,7.861902,0.648201,-1.967539,2.275751,0.000000
+0.020000,26.874091,1.750000,7.874102,0.610010,-1.909551,2.899392,0.000000
+0.020000,26.885550,1.750000,7.885562,0.572979,-1.851563,2.899392,0.000000
+0.020000,26.896293,1.750000,7.896304,0.537107,-1.793575,2.899392,0.000000
+0.020000,26.906340,1.750000,7.906352,0.502396,-1.735587,2.899392,0.000000
+0.020000,26.915717,1.750000,7.915729,0.468844,-1.677599,2.899392,0.000000
+0.020000,26.924446,1.750000,7.924458,0.436452,-1.619611,2.899392,0.000000
+0.020000,26.932551,1.750000,7.932562,0.405219,-1.561624,2.899392,0.000000
+0.020000,26.940054,1.750000,7.940065,0.375146,-1.503636,2.899392,0.000000
+0.020000,26.946978,1.750000,7.946990,0.346233,-1.445648,2.899392,0.000000
+0.020000,26.953348,1.750000,7.953360,0.318480,-1.387660,2.899392,0.000000
+0.020000,26.959186,1.750000,7.959197,0.291887,-1.329672,2.899392,0.000000
+0.020000,26.964515,1.750000,7.964526,0.266453,-1.271684,2.899392,0.000000
+0.020000,26.969358,1.750000,7.969370,0.242179,-1.213697,2.899392,0.000000
+0.020000,26.973740,1.750000,7.973751,0.219065,-1.155709,2.899392,0.000000
+0.020000,26.977682,1.750000,7.977693,0.197111,-1.097721,2.899392,0.000000
+0.020000,26.981208,1.750000,7.981220,0.176316,-1.039733,2.899392,0.000000
+0.020000,26.984342,1.750000,7.984353,0.156681,-0.981745,2.899392,0.000000
+0.020000,26.987106,1.750000,7.987118,0.138206,-0.923757,2.899392,0.000000
+0.020000,26.989524,1.750000,7.989535,0.120890,-0.865769,2.899392,0.000000
+0.020000,26.991618,1.750000,7.991630,0.104735,-0.807782,2.899392,0.000000
+0.020000,26.993413,1.750000,7.993425,0.089739,-0.749794,2.899392,0.000000
+0.020000,26.994931,1.750000,7.994943,0.075903,-0.691806,2.899392,0.000000
+0.020000,26.996196,1.750000,7.996207,0.063226,-0.633818,2.899392,0.000000
+0.020000,26.997230,1.750000,7.997242,0.051710,-0.575830,2.899392,0.000000
+0.020000,26.998057,1.750000,7.998069,0.041353,-0.517842,2.899392,0.000000
+0.020000,26.998700,1.750000,7.998712,0.032156,-0.459855,2.899392,0.000000
+0.020000,26.999183,1.750000,7.999194,0.024119,-0.401867,2.899392,0.000000
+0.020000,26.999527,1.750000,7.999539,0.017241,-0.343879,2.899392,0.000000
+0.020000,26.999758,1.750000,7.999769,0.011523,-0.285891,2.899392,0.000000
+0.020000,26.999897,1.750000,7.999909,0.006965,-0.227903,2.899392,0.000000
+0.020000,26.999968,1.750000,7.999980,0.003567,-0.169915,2.899392,0.000000
+0.020000,26.999995,1.750000,8.000007,0.001328,-0.111927,2.899392,0.000000
+0.020000,27.000000,1.750000,8.000012,0.000249,-0.053940,2.899392,0.000000
+0.020000,27.000000,1.750000,8.000012,0.000000,-0.012473,2.073338,0.000000
diff --git a/src/main/deploy/output/Rook6.left.pf1.csv b/src/main/deploy/output/Rook6.left.pf1.csv
new file mode 100644
index 0000000..0b09d2f
--- /dev/null
+++ b/src/main/deploy/output/Rook6.left.pf1.csv
@@ -0,0 +1,277 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000997,1.000000,0.000031,0.003125,0.156250,7.812500,6.282188
+0.020000,0.000991,1.000000,0.000037,0.000312,-0.140656,-14.845304,6.282194
+0.020000,0.000977,1.000000,0.000052,0.000702,0.019493,8.007460,6.282208
+0.020000,0.000952,1.000000,0.000076,0.001248,0.027292,0.389962,6.282233
+0.020000,0.000913,1.000000,0.000115,0.001949,0.035095,0.390115,6.282272
+0.020000,0.000857,1.000000,0.000172,0.002808,0.042902,0.390365,6.282328
+0.020000,0.000781,1.000000,0.000248,0.003822,0.050717,0.390738,6.282405
+0.020000,0.000681,1.000000,0.000348,0.004993,0.058542,0.391257,6.282505
+0.020000,0.000554,1.000000,0.000474,0.006320,0.066381,0.391946,6.282631
+0.020000,0.000398,1.000000,0.000630,0.007805,0.074237,0.392832,6.282787
+0.020000,0.000209,1.000000,0.000819,0.009447,0.082116,0.393938,6.282976
+0.020000,0.000297,1.000000,0.000907,0.004384,-0.253145,-16.763064,0.000016
+0.020000,0.005316,1.000001,0.005926,0.250942,12.327870,629.050742,0.000280
+0.020000,0.011135,1.000003,0.011745,0.290937,1.999758,-516.405569,0.000587
+0.020000,0.017812,1.000008,0.018422,0.333857,2.145977,7.310934,0.000939
+0.020000,0.025412,1.000017,0.026022,0.380010,2.307677,8.085012,0.001340
+0.020000,0.033989,1.000030,0.034599,0.428850,2.442012,6.716755,0.001794
+0.020000,0.043605,1.000050,0.044215,0.480802,2.597601,7.779413,0.002303
+0.020000,0.054317,1.000078,0.054927,0.535597,2.739722,7.106063,0.002872
+0.020000,0.066187,1.000116,0.066797,0.593513,2.895813,7.804579,0.003503
+0.020000,0.079270,1.000166,0.079880,0.654163,3.032480,6.833342,0.004200
+0.020000,0.093627,1.000232,0.094238,0.717872,3.185454,7.648684,0.004966
+0.020000,0.109320,1.000316,0.109930,0.784622,3.337494,7.601984,0.005806
+0.020000,0.126403,1.000423,0.127014,0.854172,3.477494,7.000018,0.006723
+0.020000,0.144936,1.000557,0.145547,0.926675,3.625177,7.384162,0.007721
+0.020000,0.164977,1.000723,0.165589,1.002111,3.771795,7.330887,0.008804
+0.020000,0.186585,1.000926,0.187199,1.080455,3.917203,7.270393,0.009975
+0.020000,0.209820,1.001172,0.210434,1.161788,4.066625,7.471126,0.011239
+0.020000,0.234739,1.001469,0.235356,1.246064,4.213822,7.359818,0.012601
+0.020000,0.261398,1.001825,0.262017,1.333044,4.349021,6.759961,0.014063
+0.020000,0.289858,1.002247,0.290480,1.423174,4.506474,7.872671,0.015632
+0.020000,0.320172,1.002747,0.320798,1.515912,4.636920,6.522303,0.017312
+0.020000,0.352375,1.003333,0.353006,1.610394,4.724089,4.358409,0.019106
+0.020000,0.386459,1.004017,0.387097,1.704556,4.708113,-0.798803,0.021015
+0.020000,0.422424,1.004809,0.423070,1.798658,4.705077,-0.151798,0.023042
+0.020000,0.460266,1.005722,0.460924,1.892679,4.701059,-0.200886,0.025188
+0.020000,0.499984,1.006768,0.500656,1.986582,4.695154,-0.295269,0.027456
+0.020000,0.541574,1.007959,0.542262,2.080329,4.687339,-0.390703,0.029848
+0.020000,0.585037,1.009312,0.585746,2.174211,4.694115,0.338795,0.032366
+0.020000,0.630364,1.010839,0.631099,2.267635,4.671166,-1.147467,0.035012
+0.020000,0.677526,1.012557,0.678293,2.359691,4.602833,-3.416659,0.037788
+0.020000,0.726458,1.014478,0.727262,2.448453,4.438093,-8.236997,0.040693
+0.020000,0.777098,1.016616,0.777948,2.534287,4.291679,-7.320702,0.043725
+0.020000,0.829385,1.018987,0.830288,2.617031,4.137208,-7.723545,0.046885
+0.020000,0.883254,1.021603,0.884220,2.696602,3.978562,-7.932280,0.050172
+0.020000,0.938641,1.024479,0.939682,2.773083,3.824068,-7.724734,0.053585
+0.020000,0.995484,1.027628,0.996613,2.846532,3.672413,-7.582738,0.057123
+0.020000,1.053721,1.031065,1.054951,2.916926,3.519701,-7.635591,0.060786
+0.020000,1.113286,1.034803,1.114633,2.984078,3.357618,-8.104147,0.064572
+0.020000,1.174115,1.038856,1.175597,3.048221,3.207126,-7.524636,0.068483
+0.020000,1.236147,1.043236,1.237783,3.109315,3.054719,-7.620318,0.072515
+0.020000,1.299317,1.047957,1.301129,3.167271,2.897822,-7.844851,0.076670
+0.020000,1.363558,1.053030,1.365570,3.222061,2.739471,-7.917573,0.080946
+0.020000,1.428811,1.058467,1.431050,3.273993,2.596599,-7.143562,0.085342
+0.020000,1.495010,1.064281,1.497504,3.322690,2.434865,-8.086703,0.089858
+0.020000,1.562091,1.070481,1.564870,3.368310,2.281011,-7.692705,0.094491
+0.020000,1.629988,1.077079,1.633087,3.410874,2.128190,-7.641074,0.099243
+0.020000,1.698640,1.084083,1.702095,3.450374,1.974981,-7.660455,0.104110
+0.020000,1.767980,1.091502,1.771831,3.486804,1.821505,-7.673808,0.109092
+0.020000,1.837945,1.099345,1.842234,3.520161,1.667879,-7.681260,0.114188
+0.020000,1.908472,1.107619,1.913245,3.550540,1.518944,-7.446770,0.119396
+0.020000,1.979497,1.116331,1.984803,3.577886,1.367310,-7.581704,0.124714
+0.020000,2.050955,1.125486,2.056844,3.602069,1.209113,-7.909861,0.130141
+0.020000,2.122781,1.135089,2.129310,3.623277,1.060438,-7.433738,0.135675
+0.020000,2.194915,1.145143,2.202141,3.641580,0.915148,-7.264494,0.141313
+0.020000,2.267289,1.155650,2.275274,3.656628,0.752401,-8.137341,0.147053
+0.020000,2.339846,1.166614,2.348654,3.669010,0.619075,-6.666290,0.152894
+0.020000,2.412515,1.178032,2.422215,3.678052,0.452077,-8.349923,0.158831
+0.020000,2.485241,1.189906,2.495903,3.684416,0.318209,-6.693414,0.164863
+0.020000,2.557955,1.202232,2.569655,3.687592,0.158826,-7.969140,0.170985
+0.020000,2.630598,1.215007,2.643413,3.687882,0.014496,-7.216471,0.177196
+0.020000,2.703109,1.228227,2.717119,3.685321,-0.128062,-7.127928,0.183490
+0.020000,2.775452,1.241890,2.790741,3.681100,-0.211044,-4.149101,0.189867
+0.020000,2.847624,1.256002,2.864280,3.676920,-0.208991,0.102664,0.196328
+0.020000,2.919621,1.270566,2.937735,3.672757,-0.208146,0.042234,0.202874
+0.020000,2.991435,1.285586,3.011103,3.668411,-0.217310,-0.458166,0.209506
+0.020000,3.063062,1.301067,3.084384,3.664028,-0.219133,-0.091176,0.216224
+0.020000,3.134498,1.317013,3.157578,3.659696,-0.216610,0.126168,0.223030
+0.020000,3.205737,1.333428,3.230683,3.655290,-0.220331,-0.186048,0.229925
+0.020000,3.276772,1.350316,3.303699,3.650778,-0.225600,-0.263488,0.236909
+0.020000,3.347599,1.367681,3.376624,3.646228,-0.227460,-0.092956,0.243984
+0.020000,3.418212,1.385528,3.449456,3.641642,-0.229312,-0.092644,0.251151
+0.020000,3.488604,1.403861,3.522197,3.637019,-0.231150,-0.091886,0.258410
+0.020000,3.558771,1.422683,3.594844,3.632385,-0.231696,-0.027301,0.265761
+0.020000,3.628707,1.441999,3.667399,3.627715,-0.233531,-0.091769,0.273207
+0.020000,3.698405,1.461812,3.739858,3.622971,-0.237192,-0.183001,0.280748
+0.020000,3.767859,1.482127,3.812222,3.618194,-0.238840,-0.082403,0.288384
+0.020000,3.837062,1.502947,3.884490,3.613386,-0.240419,-0.078951,0.296116
+0.020000,3.906009,1.524277,3.956661,3.608547,-0.241916,-0.074893,0.303945
+0.020000,3.974693,1.546119,4.028734,3.603681,-0.243321,-0.070204,0.311871
+0.020000,4.043108,1.568478,4.100710,3.598789,-0.244618,-0.064859,0.319895
+0.020000,4.111247,1.591357,4.172588,3.593873,-0.245794,-0.058838,0.328017
+0.020000,4.179103,1.614761,4.244366,3.588936,-0.246837,-0.052120,0.336238
+0.020000,4.246671,1.638691,4.316047,3.584033,-0.245141,0.084818,0.344557
+0.020000,4.313944,1.663153,4.387628,3.579078,-0.247757,-0.130833,0.352976
+0.020000,4.380913,1.688149,4.459111,3.574106,-0.248622,-0.043236,0.361494
+0.020000,4.447573,1.713681,4.530493,3.569125,-0.249017,-0.019767,0.370111
+0.020000,4.513916,1.739755,4.601776,3.564141,-0.249204,-0.009360,0.378828
+0.020000,4.579936,1.766371,4.672959,3.559158,-0.249168,0.001819,0.387644
+0.020000,4.645626,1.793534,4.744044,3.554252,-0.245308,0.193014,0.396559
+0.020000,4.710980,1.821247,4.815031,3.549325,-0.246346,-0.051904,0.405573
+0.020000,4.775989,1.849511,4.885918,3.544377,-0.247396,-0.052515,0.414685
+0.020000,4.840647,1.878329,4.956707,3.539450,-0.246351,0.052273,0.423895
+0.020000,4.904947,1.907705,5.027400,3.534653,-0.239827,0.326163,0.433202
+0.020000,4.968882,1.937641,5.097996,3.529814,-0.241964,-0.106844,0.442606
+0.020000,5.032445,1.968137,5.168496,3.524987,-0.241350,0.030738,0.452105
+0.020000,5.095629,1.999197,5.238902,3.520305,-0.234113,0.361819,0.461699
+0.020000,5.158427,2.030823,5.309215,3.515617,-0.234360,-0.012346,0.471386
+0.020000,5.220832,2.063016,5.379434,3.510951,-0.233329,0.051577,0.481166
+0.020000,5.282838,2.095779,5.449563,3.506489,-0.223073,0.512782,0.491037
+0.020000,5.344437,2.129112,5.519603,3.501957,-0.226628,-0.177777,0.500997
+0.020000,5.405623,2.163017,5.589555,3.497598,-0.217937,0.434565,0.511045
+0.020000,5.466388,2.197496,5.659420,3.493288,-0.215489,0.122424,0.521179
+0.020000,5.526727,2.232549,5.729202,3.489080,-0.210419,0.253470,0.531397
+0.020000,5.586633,2.268177,5.798902,3.484997,-0.204129,0.314528,0.541697
+0.020000,5.646099,2.304381,5.868522,3.480990,-0.200369,0.187962,0.552076
+0.020000,5.705119,2.341162,5.938064,3.477142,-0.192418,0.397553,0.562533
+0.020000,5.763687,2.378519,6.007532,3.473381,-0.188034,0.219210,0.573065
+0.020000,5.821796,2.416454,6.076928,3.469779,-0.180069,0.398245,0.583669
+0.020000,5.879440,2.454967,6.146254,3.466306,-0.173674,0.319775,0.594343
+0.020000,5.936614,2.494057,6.215513,3.462969,-0.166831,0.342122,0.605083
+0.020000,5.993312,2.533725,6.284710,3.459818,-0.157562,0.463482,0.615886
+0.020000,6.049527,2.573969,6.353845,3.456769,-0.152463,0.254930,0.626750
+0.020000,6.105255,2.614789,6.422924,3.453968,-0.140053,0.620485,0.637671
+0.020000,6.160489,2.656185,6.491950,3.451263,-0.135260,0.239684,0.648645
+0.020000,6.215226,2.698156,6.560925,3.448773,-0.124466,0.539689,0.659669
+0.020000,6.269459,2.740700,6.629854,3.446465,-0.115413,0.452659,0.670740
+0.020000,6.323183,2.783817,6.698741,3.444321,-0.107220,0.409627,0.681852
+0.020000,6.376395,2.827504,6.767589,3.442413,-0.095364,0.592796,0.693004
+0.020000,6.429089,2.871761,6.836402,3.440668,-0.087284,0.404037,0.704190
+0.020000,6.481260,2.916585,6.905185,3.439130,-0.076906,0.518890,0.715407
+0.020000,6.532905,2.961975,6.973941,3.437818,-0.065570,0.566791,0.726650
+0.020000,6.584019,3.007928,7.042675,3.436691,-0.056367,0.460131,0.737916
+0.020000,6.634598,3.054443,7.111391,3.435785,-0.045289,0.553933,0.749200
+0.020000,6.684639,3.101516,7.180093,3.435097,-0.034388,0.545055,0.760498
+0.020000,6.734138,3.149145,7.248785,3.434616,-0.024066,0.516089,0.771805
+0.020000,6.783091,3.197327,7.317472,3.434357,-0.012973,0.554642,0.783117
+0.020000,6.831495,3.246059,7.386158,3.434312,-0.002213,0.538001,0.794431
+0.020000,6.879347,3.295338,7.454848,3.434487,0.008725,0.546887,0.805741
+0.020000,6.926645,3.345161,7.523546,3.434876,0.019481,0.537812,0.817043
+0.020000,6.973385,3.395523,7.592255,3.435479,0.030148,0.533361,0.828333
+0.020000,7.019565,3.446422,7.660981,3.436306,0.041337,0.559414,0.839606
+0.020000,7.065182,3.497853,7.729728,3.437333,0.051335,0.499938,0.850859
+0.020000,7.110234,3.549813,7.798500,3.438574,0.062047,0.535602,0.862087
+0.020000,7.154720,3.602297,7.867300,3.440036,0.073113,0.553290,0.873286
+0.020000,7.198637,3.655300,7.936134,3.441670,0.081720,0.430325,0.884452
+0.020000,7.241984,3.708818,8.005004,3.443531,0.093035,0.565768,0.895581
+0.020000,7.284759,3.762847,8.073916,3.445590,0.102968,0.496660,0.906669
+0.020000,7.326961,3.817381,8.142872,3.447803,0.110646,0.383910,0.917712
+0.020000,7.368589,3.872415,8.211877,3.450252,0.122411,0.588228,0.928707
+0.020000,7.409642,3.927945,8.280934,3.452840,0.129424,0.350672,0.939650
+0.020000,7.450118,3.983964,8.350046,3.455621,0.139050,0.481300,0.950537
+0.020000,7.490019,4.040468,8.419218,3.458585,0.148207,0.457824,0.961365
+0.020000,7.529341,4.097450,8.488451,3.461663,0.153870,0.283173,0.972131
+0.020000,7.568087,4.154906,8.557751,3.464972,0.165489,0.580912,0.982832
+0.020000,7.606255,4.212828,8.627117,3.468331,0.167954,0.123283,0.993464
+0.020000,7.643846,4.271212,8.696556,3.471937,0.180265,0.615519,1.004026
+0.020000,7.680859,4.330050,8.766068,3.475585,0.182425,0.108000,1.014513
+0.020000,7.717295,4.389337,8.835657,3.479447,0.193067,0.532100,1.024924
+0.020000,7.753155,4.449066,8.905324,3.483345,0.194926,0.092960,1.035256
+0.020000,7.788440,4.509232,8.975073,3.487449,0.205176,0.512491,1.045508
+0.020000,7.823150,4.569826,9.044904,3.491553,0.205195,0.000964,1.055676
+0.020000,7.857286,4.630843,9.114821,3.495880,0.216398,0.560157,1.065759
+0.020000,7.890850,4.692276,9.184825,3.500159,0.213931,-0.123331,1.075754
+0.020000,7.923842,4.754119,9.254918,3.504650,0.224552,0.531023,1.085661
+0.020000,7.956265,4.816364,9.325101,3.509149,0.224945,0.019646,1.095478
+0.020000,7.988119,4.879004,9.395375,3.513724,0.228771,0.191287,1.105203
+0.020000,8.019408,4.942034,9.465744,3.518431,0.235334,0.328156,1.114835
+0.020000,8.050131,5.005444,9.536205,3.523072,0.232048,-0.164275,1.124372
+0.020000,8.080292,5.069230,9.606762,3.527860,0.239415,0.368362,1.133814
+0.020000,8.109894,5.133384,9.677417,3.532711,0.242552,0.156841,1.143161
+0.020000,8.138936,5.197899,9.748167,3.537501,0.239484,-0.153421,1.152410
+0.020000,8.167424,5.262767,9.819014,3.542385,0.244213,0.236455,1.161562
+0.020000,8.195358,5.327982,9.889961,3.547326,0.247012,0.139963,1.170615
+0.020000,8.222743,5.393538,9.961006,3.552283,0.247887,0.043723,1.179570
+0.020000,8.249579,5.459426,10.032150,3.557163,0.243962,-0.196242,1.188426
+0.020000,8.275871,5.525639,10.103392,3.562130,0.248368,0.220305,1.197182
+0.020000,8.301621,5.592172,10.174735,3.567114,0.249222,0.042726,1.205840
+0.020000,8.326833,5.659018,10.246177,3.572097,0.249124,-0.004916,1.214398
+0.020000,8.351510,5.726169,10.317718,3.577073,0.248811,-0.015634,1.222857
+0.020000,8.375654,5.793618,10.389359,3.582039,0.248300,-0.025583,1.231216
+0.020000,8.399270,5.861359,10.461098,3.586965,0.246280,-0.100993,1.239477
+0.020000,8.422360,5.929385,10.532936,3.591884,0.245961,-0.015930,1.247638
+0.020000,8.444928,5.997689,10.604872,3.596809,0.246240,0.013935,1.255702
+0.020000,8.466979,6.066266,10.676906,3.601711,0.245116,-0.056188,1.263667
+0.020000,8.488514,6.135107,10.749038,3.606588,0.243866,-0.062488,1.271535
+0.020000,8.509540,6.204208,10.821267,3.611438,0.242504,-0.068103,1.279306
+0.020000,8.530058,6.273562,10.893592,3.616259,0.241043,-0.073055,1.286981
+0.020000,8.550073,6.343162,10.966013,3.621049,0.239496,-0.077365,1.294560
+0.020000,8.569589,6.413003,11.038529,3.625807,0.237875,-0.081059,1.302044
+0.020000,8.588610,6.483078,11.111139,3.630509,0.235114,-0.138060,1.309434
+0.020000,8.607139,6.553380,11.183842,3.635146,0.231850,-0.163200,1.316730
+0.020000,8.625181,6.623904,11.256638,3.639784,0.231897,0.002383,1.323934
+0.020000,8.642740,6.694645,11.329525,3.644385,0.230069,-0.091427,1.331045
+0.020000,8.659820,6.765597,11.402504,3.648950,0.228221,-0.092375,1.338066
+0.020000,8.676425,6.836755,11.475574,3.653477,0.226364,-0.092864,1.344997
+0.020000,8.692560,6.908113,11.548733,3.657953,0.223786,-0.128922,1.351839
+0.020000,8.708228,6.979662,11.621978,3.662254,0.215039,-0.437340,1.358592
+0.020000,8.723433,7.051402,11.695311,3.666651,0.219891,0.242597,1.365259
+0.020000,8.738181,7.123326,11.768731,3.671012,0.218056,-0.091709,1.371839
+0.020000,8.752474,7.195428,11.842237,3.675271,0.212914,-0.257121,1.378334
+0.020000,8.766318,7.267702,11.915825,3.679392,0.206070,-0.342179,1.384745
+0.020000,8.779716,7.340146,11.989497,3.683627,0.211751,0.284025,1.391073
+0.020000,8.792669,7.412730,12.063228,3.686525,0.144900,-3.342571,1.397316
+0.020000,8.805171,7.485394,12.136960,3.686599,0.003683,-7.060806,1.403472
+0.020000,8.817221,7.558080,12.210638,3.683930,-0.133432,-6.855776,1.409538
+0.020000,8.828815,7.630722,12.284199,3.678053,-0.293878,-8.022320,1.415509
+0.020000,8.839952,7.703260,12.357588,3.669413,-0.431970,-6.904561,1.421383
+0.020000,8.850632,7.775629,12.430740,3.657618,-0.589782,-7.890613,1.427157
+0.020000,8.860857,7.847768,12.503600,3.642996,-0.731062,-7.063985,1.432830
+0.020000,8.870627,7.919609,12.576103,3.625136,-0.893033,-8.098571,1.438397
+0.020000,8.879949,7.991094,12.648193,3.604500,-1.031798,-6.938238,1.443858
+0.020000,8.888824,8.062156,12.719807,3.580705,-1.189756,-7.897922,1.449210
+0.020000,8.897260,8.132730,12.790883,3.553827,-1.343882,-7.706302,1.454452
+0.020000,8.905261,8.202755,12.861364,3.524022,-1.490256,-7.318688,1.459581
+0.020000,8.912836,8.272166,12.931186,3.491142,-1.643972,-7.685804,1.464597
+0.020000,8.919992,8.340898,13.000290,3.455189,-1.797676,-7.685203,1.469497
+0.020000,8.926739,8.408887,13.068614,3.416164,-1.951253,-7.678848,1.474282
+0.020000,8.933084,8.476070,13.136095,3.374072,-2.104585,-7.666593,1.478948
+0.020000,8.939040,8.542381,13.202673,3.328921,-2.257551,-7.648307,1.483496
+0.020000,8.944616,8.607758,13.268288,3.280720,-2.410029,-7.623877,1.487925
+0.020000,8.949823,8.672138,13.332877,3.229483,-2.561893,-7.593208,1.492233
+0.020000,8.954674,8.735451,13.396376,3.174921,-2.728059,-8.308297,1.496419
+0.020000,8.959181,8.797639,13.458727,3.117565,-2.867813,-6.987707,1.500483
+0.020000,8.963356,8.858635,13.519866,3.056952,-3.030675,-8.143087,1.504424
+0.020000,8.967212,8.918379,13.579734,2.993400,-3.177590,-7.345759,1.508242
+0.020000,8.970763,8.976801,13.638264,2.926509,-3.344557,-8.348358,1.511935
+0.020000,8.974022,9.033843,13.695399,2.856744,-3.488220,-7.183153,1.515503
+0.020000,8.977003,9.089440,13.751076,2.783839,-3.645278,-7.852913,1.518946
+0.020000,8.979720,9.143528,13.805232,2.707813,-3.801288,-7.800478,1.522262
+0.020000,8.982187,9.196044,13.857806,2.628691,-3.956097,-7.740475,1.525452
+0.020000,8.984417,9.246925,13.908736,2.546500,-4.109554,-7.672848,1.528513
+0.020000,8.986426,9.296109,13.957962,2.461270,-4.261505,-7.597555,1.531446
+0.020000,8.988226,9.343531,14.005417,2.372786,-4.424176,-8.133515,1.534251
+0.020000,8.989832,9.389130,14.051045,2.281370,-4.570799,-7.331160,1.536925
+0.020000,8.991259,9.432867,14.094805,2.188029,-4.667060,-4.813041,1.539470
+0.020000,8.992520,9.474738,14.136695,2.094468,-4.678040,-0.549004,1.541889
+0.020000,8.993631,9.514735,14.176707,2.000641,-4.691374,-0.666726,1.544182
+0.020000,8.994604,9.552857,14.214842,1.906718,-4.696156,-0.239099,1.546354
+0.020000,8.995453,9.589102,14.251097,1.812764,-4.697668,-0.075562,1.548405
+0.020000,8.996189,9.623468,14.285471,1.718677,-4.704357,-0.334476,1.550339
+0.020000,8.996824,9.655950,14.317959,1.624400,-4.713855,-0.474913,1.552155
+0.020000,8.997369,9.686549,14.348563,1.530199,-4.710057,0.189924,1.553858
+0.020000,8.997832,9.715289,14.377306,1.437166,-4.651672,2.919262,1.555448
+0.020000,8.998226,9.742218,14.404238,1.346620,-4.527305,6.218335,1.556932
+0.020000,8.998558,9.767399,14.429421,1.259133,-4.374312,7.649636,1.558313
+0.020000,8.998836,9.790888,14.452912,1.174569,-4.228213,7.304974,1.559596
+0.020000,8.999068,9.812743,14.474768,1.092770,-4.089975,6.911908,1.560785
+0.020000,8.999259,9.833020,14.495047,1.013938,-3.941589,7.419284,1.561884
+0.020000,8.999417,9.851781,14.513808,0.938081,-3.792846,7.437133,1.562898
+0.020000,8.999546,9.869084,14.531111,0.865151,-3.646474,7.318592,1.563830
+0.020000,8.999650,9.884986,14.547013,0.795101,-3.502538,7.196845,1.564685
+0.020000,8.999733,9.899544,14.561572,0.727943,-3.357871,7.233348,1.565465
+0.020000,8.999799,9.912821,14.574849,0.663851,-3.204627,7.662176,1.566174
+0.020000,8.999851,9.924876,14.586904,0.602760,-3.054556,7.503566,1.566818
+0.020000,8.999891,9.935762,14.597791,0.544315,-2.922242,6.615685,1.567397
+0.020000,8.999922,9.945546,14.607574,0.489167,-2.757364,8.243896,1.567917
+0.020000,8.999945,9.954280,14.616308,0.436699,-2.623408,6.697793,1.568381
+0.020000,8.999962,9.962029,14.624058,0.387483,-2.460811,8.129851,1.568792
+0.020000,8.999974,9.968846,14.630874,0.340827,-2.332813,6.399913,1.569152
+0.020000,8.999983,9.974795,14.636823,0.297467,-2.167996,8.240858,1.569467
+0.020000,8.999989,9.979936,14.641964,0.257030,-2.021850,7.307276,1.569739
+0.020000,8.999994,9.984323,14.646351,0.219359,-1.883525,6.916283,1.569970
+0.020000,8.999996,9.988018,14.650047,0.184776,-1.729160,7.718224,1.570165
+0.020000,8.999998,9.991082,14.653111,0.153192,-1.579212,7.497413,1.570327
+0.020000,8.999999,9.993573,14.655602,0.124559,-1.431645,7.378351,1.570458
+0.020000,8.999999,9.995551,14.657579,0.098882,-1.283827,7.390911,1.570562
+0.020000,9.000000,9.997074,14.659103,0.076166,-1.135806,7.401016,1.570642
+0.020000,9.000000,9.998203,14.660231,0.056414,-0.987628,7.408932,1.570702
+0.020000,9.000000,9.998995,14.661024,0.039627,-0.839329,7.414927,1.570743
+0.020000,9.000000,9.999511,14.661540,0.025808,-0.690944,7.419269,1.570771
+0.020000,9.000000,9.999811,14.661839,0.014958,-0.542499,7.422226,1.570786
+0.020000,9.000000,9.999952,14.661980,0.007078,-0.394018,7.424067,1.570794
+0.020000,9.000000,9.999995,14.662024,0.002168,-0.245517,7.425061,1.570796
+0.020000,9.000000,10.000000,14.662028,0.000228,-0.097007,7.425475,1.570796
+0.020000,9.000000,10.000000,14.662028,0.000000,-0.011376,4.281572,1.570796
diff --git a/src/main/deploy/output/Rook6.pf1.csv b/src/main/deploy/output/Rook6.pf1.csv
new file mode 100644
index 0000000..380fd98
--- /dev/null
+++ b/src/main/deploy/output/Rook6.pf1.csv
@@ -0,0 +1,277 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,0.000000,0.000031,0.003125,0.156250,7.812500,6.282188
+0.020000,0.000000,0.000000,0.000156,0.009375,0.312500,7.812500,6.282194
+0.020000,0.000000,0.000000,0.000437,0.018750,0.468750,7.812500,6.282208
+0.020000,0.000000,0.000000,0.000937,0.031250,0.625000,7.812500,6.282233
+0.020000,0.000000,0.000000,0.001719,0.046875,0.781250,7.812500,6.282272
+0.020000,0.000000,0.000000,0.002844,0.065625,0.937500,7.812500,6.282328
+0.020000,0.000000,0.000000,0.004375,0.087500,1.093750,7.812500,6.282405
+0.020000,0.000000,0.000000,0.006375,0.112500,1.250000,7.812500,6.282505
+0.020000,0.000000,0.000000,0.008906,0.140625,1.406250,7.812500,6.282631
+0.020000,0.000000,0.000000,0.012031,0.171875,1.562500,7.812500,6.282787
+0.020000,0.000000,0.000000,0.015813,0.206250,1.718750,7.812500,6.282976
+0.020000,0.000313,0.000000,0.020313,0.243750,1.875000,7.812500,0.000016
+0.020000,0.005596,0.000001,0.025594,0.284375,2.031250,7.812500,0.000280
+0.020000,0.011721,0.000003,0.031719,0.328125,2.187500,7.812500,0.000587
+0.020000,0.018751,0.000009,0.038750,0.375000,2.343750,7.812500,0.000939
+0.020000,0.026752,0.000018,0.046750,0.425000,2.500000,7.812500,0.001340
+0.020000,0.035783,0.000032,0.055781,0.478125,2.656250,7.812500,0.001794
+0.020000,0.045908,0.000053,0.065906,0.534375,2.812500,7.812500,0.002303
+0.020000,0.057189,0.000082,0.077188,0.593750,2.968750,7.812500,0.002872
+0.020000,0.069690,0.000122,0.089688,0.656250,3.125000,7.812500,0.003503
+0.020000,0.083470,0.000175,0.103469,0.721875,3.281250,7.812500,0.004200
+0.020000,0.098594,0.000244,0.118594,0.790625,3.437500,7.812500,0.004966
+0.020000,0.115126,0.000333,0.135125,0.862500,3.593750,7.812500,0.005806
+0.020000,0.133126,0.000446,0.153125,0.937500,3.750000,7.812500,0.006723
+0.020000,0.152657,0.000587,0.172656,1.015625,3.906250,7.812500,0.007721
+0.020000,0.173781,0.000762,0.193781,1.096875,4.062500,7.812500,0.008804
+0.020000,0.196560,0.000976,0.216563,1.181250,4.218750,7.812500,0.009975
+0.020000,0.221059,0.001235,0.241063,1.268750,4.375000,7.812500,0.011239
+0.020000,0.247340,0.001549,0.267344,1.359375,4.531250,7.812500,0.012601
+0.020000,0.275461,0.001924,0.295469,1.453125,4.687500,7.812500,0.014063
+0.020000,0.305490,0.002369,0.325500,1.550000,4.843750,7.812500,0.015632
+0.020000,0.337484,0.002896,0.357500,1.650000,5.000000,7.812500,0.017312
+0.020000,0.371480,0.003516,0.391500,1.750000,5.000000,0.000000,0.019106
+0.020000,0.407473,0.004238,0.427500,1.850000,5.000000,0.000000,0.021015
+0.020000,0.445464,0.005075,0.465500,1.950000,5.000000,-0.000000,0.023042
+0.020000,0.485452,0.006039,0.505500,2.050000,5.000000,0.000000,0.025188
+0.020000,0.527437,0.007144,0.547500,2.150000,5.000000,0.000000,0.027456
+0.020000,0.571417,0.008405,0.591500,2.250000,5.000000,0.000000,0.029848
+0.020000,0.617397,0.009836,0.637500,2.350000,5.000000,0.000000,0.032366
+0.020000,0.665369,0.011452,0.685500,2.450000,5.000000,0.000000,0.035012
+0.020000,0.715305,0.013271,0.735469,2.546875,4.843750,-7.812500,0.037788
+0.020000,0.767139,0.015305,0.787344,2.640625,4.687500,-7.812500,0.040693
+0.020000,0.820810,0.017572,0.841063,2.731250,4.531250,-7.812500,0.043725
+0.020000,0.876253,0.020086,0.896563,2.818750,4.375000,-7.812500,0.046885
+0.020000,0.933405,0.022861,0.953781,2.903125,4.218750,-7.812500,0.050172
+0.020000,0.992200,0.025914,1.012656,2.984375,4.062500,-7.812500,0.053585
+0.020000,1.052576,0.029259,1.073125,3.062500,3.906250,-7.812500,0.057123
+0.020000,1.114470,0.032912,1.135125,3.137500,3.750000,-7.812500,0.060786
+0.020000,1.177813,0.036887,1.198594,3.209375,3.593750,-7.812500,0.064572
+0.020000,1.242544,0.041200,1.263469,3.278125,3.437500,-7.812500,0.068483
+0.020000,1.308599,0.045864,1.329688,3.343750,3.281250,-7.812500,0.072515
+0.020000,1.375912,0.050894,1.397188,3.406250,3.125000,-7.812500,0.076670
+0.020000,1.444415,0.056304,1.465906,3.465625,2.968750,-7.812500,0.080946
+0.020000,1.514050,0.062107,1.535781,3.521875,2.812500,-7.812500,0.085342
+0.020000,1.584747,0.068315,1.606750,3.575000,2.656250,-7.812500,0.089858
+0.020000,1.656442,0.074942,1.678750,3.625000,2.500000,-7.812500,0.094491
+0.020000,1.729068,0.081999,1.751719,3.671875,2.343750,-7.812500,0.099243
+0.020000,1.802562,0.089497,1.825594,3.715625,2.187500,-7.812500,0.104110
+0.020000,1.876856,0.097447,1.900313,3.756250,2.031250,-7.812500,0.109092
+0.020000,1.951885,0.105858,1.975812,3.793750,1.875000,-7.812500,0.114188
+0.020000,2.027584,0.114739,2.052031,3.828125,1.718750,-7.812500,0.119396
+0.020000,2.103889,0.124098,2.128906,3.859375,1.562500,-7.812500,0.124714
+0.020000,2.180729,0.133943,2.206375,3.887500,1.406250,-7.812500,0.130141
+0.020000,2.258040,0.144278,2.284375,3.912500,1.250000,-7.812500,0.135675
+0.020000,2.335759,0.155111,2.362844,3.934375,1.093750,-7.812500,0.141313
+0.020000,2.413813,0.166443,2.441719,3.953125,0.937500,-7.812500,0.147053
+0.020000,2.492145,0.178279,2.520937,3.968750,0.781250,-7.812500,0.152894
+0.020000,2.570679,0.190619,2.600437,3.981250,0.625000,-7.812500,0.158831
+0.020000,2.649358,0.203465,2.680156,3.990625,0.468750,-7.812500,0.164863
+0.020000,2.728109,0.216814,2.760031,3.996875,0.312500,-7.812500,0.170985
+0.020000,2.806868,0.230665,2.840000,4.000000,0.156250,-7.812500,0.177196
+0.020000,2.885572,0.245014,2.920000,4.000000,0.000000,-7.812500,0.183490
+0.020000,2.964181,0.259861,3.000000,4.000000,0.000000,0.000000,0.189867
+0.020000,3.042693,0.275212,3.080000,4.000000,0.000000,0.000000,0.196328
+0.020000,3.121106,0.291074,3.160000,4.000000,0.000000,0.000000,0.202874
+0.020000,3.199411,0.307452,3.240000,4.000000,0.000000,0.000000,0.209506
+0.020000,3.277605,0.324352,3.320000,4.000000,0.000000,0.000000,0.216224
+0.020000,3.355683,0.341781,3.400000,4.000000,0.000000,0.000000,0.223030
+0.020000,3.433641,0.359744,3.480000,4.000000,0.000000,0.000000,0.229925
+0.020000,3.511472,0.378248,3.560000,4.000000,0.000000,0.000000,0.236909
+0.020000,3.589170,0.397298,3.640000,4.000000,0.000000,0.000000,0.243984
+0.020000,3.666730,0.416901,3.720000,4.000000,0.000000,0.000000,0.251151
+0.020000,3.744147,0.437063,3.800000,4.000000,0.000000,0.000000,0.258410
+0.020000,3.821415,0.457790,3.880000,4.000000,0.000000,0.000000,0.265761
+0.020000,3.898528,0.479088,3.960000,4.000000,0.000000,0.000000,0.273207
+0.020000,3.975479,0.500964,4.040000,4.000000,0.000000,0.000000,0.280748
+0.020000,4.052262,0.523422,4.120000,4.000000,0.000000,0.000000,0.288384
+0.020000,4.128870,0.546470,4.200000,4.000000,0.000000,0.000000,0.296116
+0.020000,4.205296,0.570113,4.280000,4.000000,0.000000,0.000000,0.303945
+0.020000,4.281534,0.594358,4.360000,4.000000,0.000000,0.000000,0.311871
+0.020000,4.357575,0.619210,4.440000,4.000000,0.000000,0.000000,0.319895
+0.020000,4.433414,0.644674,4.520000,4.000000,0.000000,0.000000,0.328017
+0.020000,4.509041,0.670758,4.600000,4.000000,0.000000,0.000000,0.336238
+0.020000,4.584451,0.697466,4.680000,4.000000,0.000000,0.000000,0.344557
+0.020000,4.659635,0.724805,4.760000,4.000000,0.000000,0.000000,0.352976
+0.020000,4.734585,0.752779,4.840000,4.000000,0.000000,0.000000,0.361494
+0.020000,4.809292,0.781394,4.920000,4.000000,0.000000,0.000000,0.370111
+0.020000,4.883748,0.810656,5.000000,4.000000,0.000000,0.000000,0.378828
+0.020000,4.957944,0.840569,5.080000,4.000000,0.000000,0.000000,0.387644
+0.020000,5.031873,0.871139,5.160000,4.000000,0.000000,0.000000,0.396559
+0.020000,5.105525,0.902370,5.240000,4.000000,0.000000,0.000000,0.405573
+0.020000,5.178891,0.934268,5.320000,4.000000,0.000000,0.000000,0.414685
+0.020000,5.251960,0.966835,5.400000,4.000000,0.000000,0.000000,0.423895
+0.020000,5.324726,1.000079,5.480000,4.000000,0.000000,0.000000,0.433202
+0.020000,5.397178,1.034002,5.560000,4.000000,0.000000,0.000000,0.442606
+0.020000,5.469305,1.068607,5.640000,4.000000,0.000000,0.000000,0.452105
+0.020000,5.541099,1.103900,5.720000,4.000000,0.000000,0.000000,0.461699
+0.020000,5.612549,1.139884,5.800000,4.000000,0.000000,0.000000,0.471386
+0.020000,5.683645,1.176561,5.880000,4.000000,0.000000,0.000000,0.481166
+0.020000,5.754379,1.213935,5.960000,4.000000,0.000000,0.000000,0.491037
+0.020000,5.824737,1.252008,6.040000,4.000000,0.000000,0.000000,0.500997
+0.020000,5.894712,1.290784,6.120000,4.000000,0.000000,0.000000,0.511045
+0.020000,5.964291,1.330263,6.200000,4.000000,0.000000,0.000000,0.521179
+0.020000,6.033465,1.370448,6.280000,4.000000,0.000000,0.000000,0.531397
+0.020000,6.102224,1.411342,6.360000,4.000000,0.000000,0.000000,0.541697
+0.020000,6.170555,1.452943,6.440000,4.000000,0.000000,0.000000,0.552076
+0.020000,6.238450,1.495255,6.520000,4.000000,0.000000,0.000000,0.562533
+0.020000,6.305897,1.538276,6.600000,4.000000,0.000000,0.000000,0.573065
+0.020000,6.372885,1.582008,6.680000,4.000000,0.000000,0.000000,0.583669
+0.020000,6.439405,1.626450,6.760000,4.000000,0.000000,0.000000,0.594343
+0.020000,6.505444,1.671602,6.840000,4.000000,0.000000,0.000000,0.605083
+0.020000,6.570994,1.717463,6.920000,4.000000,0.000000,0.000000,0.615886
+0.020000,6.636042,1.764031,7.000000,4.000000,0.000000,0.000000,0.626750
+0.020000,6.700580,1.811305,7.080000,4.000000,0.000000,0.000000,0.637671
+0.020000,6.764597,1.859282,7.160000,4.000000,0.000000,0.000000,0.648645
+0.020000,6.828081,1.907961,7.240000,4.000000,0.000000,0.000000,0.659669
+0.020000,6.891024,1.957338,7.320000,4.000000,0.000000,0.000000,0.670740
+0.020000,6.953416,2.007410,7.400000,4.000000,0.000000,0.000000,0.681852
+0.020000,7.015246,2.058174,7.480000,4.000000,0.000000,0.000000,0.693004
+0.020000,7.076506,2.109625,7.560000,4.000000,0.000000,0.000000,0.704190
+0.020000,7.137185,2.161759,7.640000,4.000000,0.000000,0.000000,0.715407
+0.020000,7.197275,2.214571,7.720000,4.000000,0.000000,0.000000,0.726650
+0.020000,7.256766,2.268056,7.800000,4.000000,0.000000,0.000000,0.737916
+0.020000,7.315651,2.322209,7.880000,4.000000,0.000000,0.000000,0.749200
+0.020000,7.373921,2.377022,7.960000,4.000000,0.000000,0.000000,0.760498
+0.020000,7.431567,2.432491,8.040000,4.000000,0.000000,0.000000,0.771805
+0.020000,7.488583,2.488609,8.120000,4.000000,0.000000,0.000000,0.783117
+0.020000,7.544960,2.545368,8.200000,4.000000,0.000000,0.000000,0.794431
+0.020000,7.600691,2.602761,8.280000,4.000000,0.000000,0.000000,0.805741
+0.020000,7.655770,2.660780,8.360000,4.000000,0.000000,0.000000,0.817043
+0.020000,7.710190,2.719418,8.440000,4.000000,0.000000,0.000000,0.828333
+0.020000,7.763945,2.778666,8.520000,4.000000,0.000000,0.000000,0.839606
+0.020000,7.817029,2.838516,8.600000,4.000000,0.000000,0.000000,0.850859
+0.020000,7.869437,2.898958,8.680000,4.000000,0.000000,0.000000,0.862087
+0.020000,7.921164,2.959985,8.760000,4.000000,0.000000,0.000000,0.873286
+0.020000,7.972205,3.021586,8.840000,4.000000,0.000000,0.000000,0.884452
+0.020000,8.022557,3.083753,8.920000,4.000000,0.000000,0.000000,0.895581
+0.020000,8.072214,3.146475,9.000000,4.000000,0.000000,0.000000,0.906669
+0.020000,8.121175,3.209742,9.080000,4.000000,0.000000,0.000000,0.917712
+0.020000,8.169436,3.273545,9.160000,4.000000,0.000000,0.000000,0.928707
+0.020000,8.216993,3.337874,9.240000,4.000000,0.000000,0.000000,0.939650
+0.020000,8.263846,3.402718,9.320000,4.000000,0.000000,0.000000,0.950537
+0.020000,8.309992,3.468067,9.400000,4.000000,0.000000,0.000000,0.961365
+0.020000,8.355430,3.533910,9.480000,4.000000,0.000000,0.000000,0.972131
+0.020000,8.400158,3.600238,9.560000,4.000000,0.000000,0.000000,0.982832
+0.020000,8.444176,3.667038,9.640000,4.000000,0.000000,0.000000,0.993464
+0.020000,8.487485,3.734301,9.720000,4.000000,0.000000,0.000000,1.004026
+0.020000,8.530082,3.802016,9.800000,4.000000,0.000000,0.000000,1.014513
+0.020000,8.571970,3.870174,9.880000,4.000000,0.000000,0.000000,1.024924
+0.020000,8.613149,3.938761,9.960000,4.000000,0.000000,0.000000,1.035256
+0.020000,8.653620,4.007769,10.040000,4.000000,0.000000,0.000000,1.045508
+0.020000,8.693383,4.077186,10.120000,4.000000,0.000000,0.000000,1.055676
+0.020000,8.732443,4.147003,10.200000,4.000000,0.000000,0.000000,1.065759
+0.020000,8.770798,4.217207,10.280000,4.000000,0.000000,0.000000,1.075754
+0.020000,8.808454,4.287791,10.360000,4.000000,0.000000,0.000000,1.085661
+0.020000,8.845412,4.358742,10.440000,4.000000,0.000000,0.000000,1.095478
+0.020000,8.881674,4.430050,10.520000,4.000000,0.000000,0.000000,1.105203
+0.020000,8.917246,4.501708,10.600000,4.000000,0.000000,0.000000,1.114835
+0.020000,8.952128,4.573701,10.680000,4.000000,0.000000,0.000000,1.124372
+0.020000,8.986325,4.646023,10.760000,4.000000,0.000000,0.000000,1.133814
+0.020000,9.019842,4.718664,10.840000,4.000000,0.000000,0.000000,1.143161
+0.020000,9.052682,4.791612,10.920000,4.000000,0.000000,0.000000,1.152410
+0.020000,9.084849,4.864859,11.000000,4.000000,0.000000,0.000000,1.161562
+0.020000,9.116349,4.938397,11.080000,4.000000,0.000000,0.000000,1.170615
+0.020000,9.147185,5.012216,11.160000,4.000000,0.000000,0.000000,1.179570
+0.020000,9.177362,5.086305,11.240000,4.000000,0.000000,0.000000,1.188426
+0.020000,9.206886,5.160657,11.320000,4.000000,0.000000,0.000000,1.197182
+0.020000,9.235761,5.235264,11.400000,4.000000,0.000000,0.000000,1.205840
+0.020000,9.263993,5.310116,11.480000,4.000000,0.000000,0.000000,1.214398
+0.020000,9.291587,5.385207,11.560000,4.000000,0.000000,0.000000,1.222857
+0.020000,9.318549,5.460527,11.640000,4.000000,0.000000,0.000000,1.231216
+0.020000,9.344884,5.536068,11.720000,4.000000,0.000000,0.000000,1.239477
+0.020000,9.370597,5.611822,11.800000,4.000000,0.000000,0.000000,1.247638
+0.020000,9.395695,5.687783,11.880000,4.000000,0.000000,0.000000,1.255702
+0.020000,9.420184,5.763942,11.960000,4.000000,0.000000,0.000000,1.263667
+0.020000,9.444069,5.840293,12.040000,4.000000,0.000000,0.000000,1.271535
+0.020000,9.467356,5.916828,12.120000,4.000000,0.000000,0.000000,1.279306
+0.020000,9.490052,5.993542,12.200000,4.000000,0.000000,0.000000,1.286981
+0.020000,9.512162,6.070426,12.280000,4.000000,0.000000,0.000000,1.294560
+0.020000,9.533692,6.147474,12.360000,4.000000,0.000000,0.000000,1.302044
+0.020000,9.554648,6.224680,12.440000,4.000000,0.000000,0.000000,1.309434
+0.020000,9.575037,6.302038,12.520000,4.000000,0.000000,0.000000,1.316730
+0.020000,9.594865,6.379541,12.600000,4.000000,0.000000,0.000000,1.323934
+0.020000,9.614137,6.457184,12.680000,4.000000,0.000000,0.000000,1.331045
+0.020000,9.632860,6.534962,12.760000,4.000000,0.000000,0.000000,1.338066
+0.020000,9.651041,6.612870,12.840000,4.000000,0.000000,0.000000,1.344997
+0.020000,9.668684,6.690901,12.920000,4.000000,0.000000,0.000000,1.351839
+0.020000,9.685797,6.769047,13.000000,4.000000,0.000000,0.000000,1.358592
+0.020000,9.702384,6.847308,13.080000,4.000000,0.000000,0.000000,1.365259
+0.020000,9.718454,6.925678,13.160000,4.000000,0.000000,0.000000,1.371839
+0.020000,9.734010,7.004152,13.240000,4.000000,0.000000,0.000000,1.378334
+0.020000,9.749060,7.082722,13.320000,4.000000,0.000000,0.000000,1.384745
+0.020000,9.763609,7.161388,13.400000,4.000000,0.000000,0.000000,1.391073
+0.020000,9.777659,7.240118,13.479974,3.997354,-0.132313,-6.615651,1.397316
+0.020000,9.791205,7.318849,13.559863,3.991582,-0.288563,-7.812500,1.403472
+0.020000,9.804247,7.397520,13.639606,3.982686,-0.444813,-7.812500,1.409538
+0.020000,9.816782,7.476058,13.719139,3.970665,-0.601063,-7.812500,1.415509
+0.020000,9.828811,7.554402,13.798401,3.955519,-0.757313,-7.812500,1.421383
+0.020000,9.840334,7.632483,13.877329,3.937247,-0.913563,-7.812500,1.427157
+0.020000,9.851354,7.710238,13.955860,3.915851,-1.069813,-7.812500,1.432830
+0.020000,9.861875,7.787596,14.033931,3.891330,-1.226063,-7.812500,1.438397
+0.020000,9.871903,7.864496,14.111482,3.863684,-1.382313,-7.812500,1.443858
+0.020000,9.881442,7.940869,14.188447,3.832912,-1.538563,-7.812500,1.449210
+0.020000,9.890499,8.016648,14.264767,3.799016,-1.694813,-7.812500,1.454452
+0.020000,9.899083,8.091769,14.340377,3.761995,-1.851063,-7.812500,1.459581
+0.020000,9.907202,8.166166,14.415215,3.721849,-2.007313,-7.812500,1.464597
+0.020000,9.914866,8.239772,14.489220,3.678577,-2.163563,-7.812500,1.469497
+0.020000,9.922085,8.312522,14.562327,3.632181,-2.319813,-7.812500,1.474282
+0.020000,9.928869,8.384351,14.634476,3.582660,-2.476063,-7.812500,1.478948
+0.020000,9.935232,8.455192,14.705602,3.530014,-2.632313,-7.812500,1.483496
+0.020000,9.941184,8.524981,14.775645,3.474242,-2.788563,-7.812500,1.487925
+0.020000,9.946739,8.593655,14.844541,3.415346,-2.944813,-7.812500,1.492233
+0.020000,9.951909,8.661142,14.912227,3.353325,-3.101063,-7.812500,1.496419
+0.020000,9.956710,8.727384,14.978642,3.288179,-3.257313,-7.812500,1.500483
+0.020000,9.961154,8.792312,15.043723,3.219907,-3.413563,-7.812500,1.504424
+0.020000,9.965256,8.855865,15.107408,3.148511,-3.569813,-7.812500,1.508242
+0.020000,9.969031,8.917974,15.169633,3.073990,-3.726063,-7.812500,1.511935
+0.020000,9.972494,8.978578,15.230336,2.996343,-3.882313,-7.812500,1.515503
+0.020000,9.975659,9.037613,15.289455,2.915572,-4.038563,-7.812500,1.518946
+0.020000,9.978543,9.095013,15.346928,2.831676,-4.194813,-7.812500,1.522262
+0.020000,9.981159,9.150715,15.402691,2.744655,-4.351063,-7.812500,1.525452
+0.020000,9.983524,9.204654,15.456682,2.654508,-4.507313,-7.812500,1.528513
+0.020000,9.985652,9.256770,15.508840,2.561237,-4.663563,-7.812500,1.531446
+0.020000,9.987559,9.306993,15.559101,2.464841,-4.819813,-7.812500,1.534251
+0.020000,9.989259,9.355265,15.607402,2.365320,-4.976063,-7.812500,1.536925
+0.020000,9.990768,9.401546,15.653709,2.265320,-5.000000,-1.196849,1.539470
+0.020000,9.992102,9.445834,15.698015,2.165320,-5.000000,-0.000000,1.541889
+0.020000,9.993276,9.488124,15.740321,2.065320,-5.000000,0.000000,1.544182
+0.020000,9.994305,9.528417,15.780628,1.965320,-5.000000,0.000000,1.546354
+0.020000,9.995202,9.566713,15.818934,1.865320,-5.000000,0.000000,1.548405
+0.020000,9.995980,9.603012,15.855241,1.765320,-5.000000,-0.000000,1.550339
+0.020000,9.996650,9.637310,15.889547,1.665320,-5.000000,0.000000,1.552155
+0.020000,9.997225,9.669611,15.921853,1.565320,-5.000000,-0.000000,1.553858
+0.020000,9.997715,9.699941,15.952186,1.467966,-4.867687,6.615651,1.555448
+0.020000,9.998130,9.728354,15.980603,1.373737,-4.711437,7.812500,1.556932
+0.020000,9.998480,9.754915,16.007167,1.282633,-4.555187,7.812500,1.558313
+0.020000,9.998773,9.779688,16.031940,1.194655,-4.398937,7.812500,1.559596
+0.020000,9.999017,9.802731,16.054984,1.109801,-4.242687,7.812500,1.560785
+0.020000,9.999220,9.824108,16.076363,1.028072,-4.086437,7.812500,1.561884
+0.020000,9.999386,9.843883,16.096139,0.949468,-3.930187,7.812500,1.562898
+0.020000,9.999521,9.862118,16.114373,0.873990,-3.773937,7.812500,1.563830
+0.020000,9.999631,9.878874,16.131129,0.801636,-3.617687,7.812500,1.564685
+0.020000,9.999719,9.894213,16.146470,0.732407,-3.461437,7.812500,1.565465
+0.020000,9.999788,9.908199,16.160457,0.666304,-3.305187,7.812500,1.566174
+0.020000,9.999843,9.920897,16.173153,0.603325,-3.148937,7.812500,1.566818
+0.020000,9.999885,9.932363,16.184621,0.543471,-2.992687,7.812500,1.567397
+0.020000,9.999918,9.942667,16.194923,0.486742,-2.836437,7.812500,1.567917
+0.020000,9.999942,9.951864,16.204122,0.433139,-2.680187,7.812500,1.568381
+0.020000,9.999960,9.960024,16.212280,0.382660,-2.523937,7.812500,1.568792
+0.020000,9.999973,9.967202,16.219460,0.335306,-2.367687,7.812500,1.569152
+0.020000,9.999982,9.973466,16.225724,0.291077,-2.211437,7.812500,1.569467
+0.020000,9.999989,9.978878,16.231134,0.249974,-2.055187,7.812500,1.569739
+0.020000,9.999993,9.983497,16.235754,0.211995,-1.898937,7.812500,1.569970
+0.020000,9.999996,9.987387,16.239645,0.177141,-1.742687,7.812500,1.570165
+0.020000,9.999998,9.990613,16.242871,0.145412,-1.586437,7.812500,1.570327
+0.020000,9.999999,9.993235,16.245493,0.116809,-1.430187,7.812500,1.570458
+0.020000,9.999999,9.995317,16.247574,0.091330,-1.273937,7.812500,1.570562
+0.020000,10.000000,9.996920,16.249177,0.068976,-1.117687,7.812500,1.570642
+0.020000,10.000000,9.998108,16.250365,0.049747,-0.961437,7.812500,1.570702
+0.020000,10.000000,9.998942,16.251199,0.033644,-0.805187,7.812500,1.570743
+0.020000,10.000000,9.999486,16.251742,0.020665,-0.648937,7.812500,1.570771
+0.020000,10.000000,9.999801,16.252056,0.010811,-0.492687,7.812500,1.570786
+0.020000,10.000000,9.999950,16.252205,0.004082,-0.336437,7.812500,1.570794
+0.020000,10.000000,9.999995,16.252251,0.000479,-0.180187,7.812500,1.570796
+0.020000,10.000000,10.000000,16.252256,0.000000,-0.023937,7.812500,1.570796
+0.020000,10.000000,10.000000,16.252256,0.000000,0.000000,1.196849,1.570796
diff --git a/src/main/deploy/output/Rook6.right.pf1.csv b/src/main/deploy/output/Rook6.right.pf1.csv
new file mode 100644
index 0000000..b179b39
--- /dev/null
+++ b/src/main/deploy/output/Rook6.right.pf1.csv
@@ -0,0 +1,277 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,-0.000997,-1.000000,0.000031,0.003125,0.156250,7.812500,6.282188
+0.020000,-0.000991,-1.000000,0.000037,0.000312,-0.140656,-14.845304,6.282194
+0.020000,-0.000977,-1.000000,0.000052,0.000702,0.019493,8.007460,6.282208
+0.020000,-0.000952,-1.000000,0.000076,0.001248,0.027292,0.389962,6.282233
+0.020000,-0.000913,-1.000000,0.000115,0.001949,0.035095,0.390115,6.282272
+0.020000,-0.000857,-1.000000,0.000172,0.002808,0.042902,0.390365,6.282328
+0.020000,-0.000781,-1.000000,0.000248,0.003822,0.050717,0.390738,6.282405
+0.020000,-0.000681,-1.000000,0.000348,0.004993,0.058542,0.391257,6.282505
+0.020000,-0.000554,-1.000000,0.000474,0.006320,0.066381,0.391946,6.282631
+0.020000,-0.000398,-1.000000,0.000630,0.007805,0.074237,0.392832,6.282787
+0.020000,-0.000209,-1.000000,0.000819,0.009447,0.082116,0.393938,6.282976
+0.020000,0.000328,-1.000000,0.001357,0.026881,0.871676,39.477992,0.000016
+0.020000,0.005876,-0.999999,0.006904,0.277369,12.524412,582.636782,0.000280
+0.020000,0.012308,-0.999996,0.013336,0.321604,2.211738,-515.633664,0.000587
+0.020000,0.019689,-0.999991,0.020718,0.369084,2.374008,8.113485,0.000939
+0.020000,0.028093,-0.999981,0.029121,0.420155,2.553559,8.977556,0.001340
+0.020000,0.037577,-0.999966,0.038606,0.474216,2.703045,7.474283,0.001794
+0.020000,0.048212,-0.999945,0.049240,0.531741,2.876244,8.659946,0.002303
+0.020000,0.060060,-0.999914,0.061089,0.592437,3.034794,7.927487,0.002872
+0.020000,0.073193,-0.999872,0.074221,0.656618,3.209069,8.713780,0.003503
+0.020000,0.087670,-0.999816,0.088699,0.723861,3.362161,7.654590,0.004200
+0.020000,0.103560,-0.999743,0.104589,0.794534,3.533633,8.573582,0.004966
+0.020000,0.120932,-0.999650,0.121962,0.868623,3.704452,8.540989,0.005806
+0.020000,0.139850,-0.999531,0.140879,0.945870,3.862361,7.895411,0.006723
+0.020000,0.160378,-0.999383,0.161408,1.026455,4.029230,8.343461,0.007721
+0.020000,0.182585,-0.999200,0.183616,1.110363,4.195422,8.309617,0.008804
+0.020000,0.206535,-0.998975,0.207567,1.197580,4.360824,8.270089,0.009975
+0.020000,0.232298,-0.998701,0.233331,1.288205,4.531278,8.522713,0.011239
+0.020000,0.259940,-0.998372,0.260975,1.382204,4.699925,8.432329,0.012601
+0.020000,0.289524,-0.997978,0.290562,1.479323,4.855972,7.802384,0.014063
+0.020000,0.321122,-0.997508,0.322163,1.580073,5.037489,9.075841,0.015632
+0.020000,0.354795,-0.996954,0.355841,1.683871,5.189868,7.618953,0.017312
+0.020000,0.390584,-0.996302,0.391636,1.789771,5.295016,5.257391,0.019106
+0.020000,0.428486,-0.995542,0.429546,1.895496,5.286271,-0.437269,0.021015
+0.020000,0.468504,-0.994660,0.469573,2.001347,5.292538,0.313357,0.023042
+0.020000,0.510638,-0.993644,0.511719,2.107315,5.298375,0.291881,0.025188
+0.020000,0.554890,-0.992479,0.555987,2.213370,5.302793,0.220894,0.027456
+0.020000,0.601260,-0.991150,0.602376,2.319486,5.305785,0.149575,0.029848
+0.020000,0.649757,-0.989641,0.650897,2.426002,5.325784,0.999943,0.032366
+0.020000,0.700374,-0.987935,0.701542,2.532269,5.313376,-0.620380,0.035012
+0.020000,0.753084,-0.986016,0.754288,2.637288,5.250946,-3.121513,0.037788
+0.020000,0.807821,-0.983867,0.809066,2.738912,5.081184,-8.488075,0.040693
+0.020000,0.864521,-0.981472,0.865817,2.837554,4.932105,-7.453954,0.043725
+0.020000,0.923122,-0.978815,0.924478,2.933038,4.774182,-7.896154,0.046885
+0.020000,0.983556,-0.975881,0.984983,3.025271,4.611690,-8.124627,0.050172
+0.020000,1.045759,-0.972651,1.047270,3.114350,4.453917,-7.888641,0.053585
+0.020000,1.109668,-0.969110,1.111277,3.200337,4.299371,-7.727274,0.057123
+0.020000,1.175218,-0.965241,1.176941,3.283210,4.143640,-7.786549,0.060786
+0.020000,1.242341,-0.961028,1.244196,3.362756,3.977300,-8.317022,0.064572
+0.020000,1.310973,-0.956456,1.312981,3.439234,3.823906,-7.669717,0.068483
+0.020000,1.381051,-0.951508,1.383233,3.512599,3.668227,-7.783944,0.072515
+0.020000,1.452507,-0.946168,1.454888,3.582744,3.507287,-8.046996,0.076670
+0.020000,1.525273,-0.940422,1.527880,3.649633,3.344445,-8.142091,0.080946
+0.020000,1.599288,-0.934254,1.602153,3.713611,3.198861,-7.279222,0.085342
+0.020000,1.674484,-0.927650,1.677637,3.774242,3.031569,-8.364582,0.089858
+0.020000,1.750793,-0.920597,1.754271,3.831698,2.872807,-7.938095,0.094491
+0.020000,1.828148,-0.913080,1.831991,3.885994,2.714778,-7.901449,0.099243
+0.020000,1.906484,-0.905088,1.910734,3.937110,2.555813,-7.948251,0.104110
+0.020000,1.985732,-0.896609,1.990434,3.985030,2.395989,-7.991212,0.109092
+0.020000,2.065825,-0.887630,2.071029,4.029737,2.235379,-8.030506,0.114188
+0.020000,2.146697,-0.878142,2.152455,4.071327,2.079473,-7.795266,0.119396
+0.020000,2.228280,-0.868135,2.234650,4.109722,1.919763,-7.985536,0.124714
+0.020000,2.310503,-0.857601,2.317545,4.144756,1.751706,-8.402812,0.130141
+0.020000,2.393299,-0.846532,2.401078,4.176631,1.593731,-7.898790,0.135675
+0.020000,2.476602,-0.834921,2.485186,4.205406,1.438773,-7.747856,0.141313
+0.020000,2.560337,-0.822764,2.569799,4.230660,1.262670,-8.805170,0.147053
+0.020000,2.644443,-0.810055,2.654860,4.253051,1.119554,-7.155800,0.152894
+0.020000,2.728843,-0.796794,2.740295,4.271777,0.936335,-9.160929,0.158831
+0.020000,2.813474,-0.782976,2.826047,4.287585,0.790382,-7.297676,0.164863
+0.020000,2.898262,-0.768603,2.912044,4.299856,0.613557,-8.841249,0.170985
+0.020000,2.983138,-0.753677,2.998223,4.308915,0.452944,-8.030657,0.177196
+0.020000,3.068034,-0.738199,3.084518,4.314777,0.293088,-7.992796,0.183490
+0.020000,3.152909,-0.722168,3.170894,4.318814,0.201840,-4.562419,0.189867
+0.020000,3.237763,-0.705577,3.257355,4.323010,0.209845,0.400256,0.196328
+0.020000,3.322591,-0.688417,3.343901,4.327339,0.216415,0.328517,0.202874
+0.020000,3.407388,-0.670682,3.430533,4.331561,0.211124,-0.264540,0.209506
+0.020000,3.492148,-0.652362,3.517250,4.335849,0.214383,0.162926,0.216224
+0.020000,3.576869,-0.633451,3.604056,4.340303,0.222719,0.416807,0.223030
+0.020000,3.661545,-0.613939,3.690951,4.344775,0.223574,0.042776,0.229925
+0.020000,3.746171,-0.593820,3.777936,4.349224,0.222453,-0.056091,0.236909
+0.020000,3.830741,-0.573085,3.865010,4.353729,0.225245,0.139636,0.243984
+0.020000,3.915249,-0.551726,3.952176,4.358287,0.227904,0.132931,0.251151
+0.020000,3.999690,-0.529735,4.039434,4.362895,0.230423,0.125939,0.258410
+0.020000,4.084059,-0.507103,4.126785,4.367582,0.234321,0.194923,0.265761
+0.020000,4.168350,-0.483822,4.214232,4.372311,0.236485,0.108190,0.273207
+0.020000,4.252554,-0.459885,4.301772,4.377036,0.236229,-0.012793,0.280748
+0.020000,4.336666,-0.435282,4.389408,4.381799,0.238159,0.096478,0.288384
+0.020000,4.420678,-0.410007,4.477140,4.386598,0.239920,0.088079,0.296116
+0.020000,4.504583,-0.384050,4.564969,4.391428,0.241508,0.079376,0.303945
+0.020000,4.588374,-0.357403,4.652895,4.396286,0.242915,0.070368,0.311871
+0.020000,4.672042,-0.330059,4.740918,4.401169,0.244136,0.061055,0.319895
+0.020000,4.755580,-0.302009,4.829039,4.406072,0.245165,0.051435,0.328017
+0.020000,4.838979,-0.273245,4.917259,4.410992,0.245995,0.041511,0.336238
+0.020000,4.922231,-0.243759,5.005579,4.415988,0.249816,0.191046,0.344557
+0.020000,5.005327,-0.213543,5.093998,4.420947,0.247922,-0.094691,0.352976
+0.020000,5.088257,-0.182590,5.182516,4.425901,0.247740,-0.009127,0.361494
+0.020000,5.171011,-0.150893,5.271133,4.430855,0.247674,-0.003296,0.370111
+0.020000,5.253580,-0.118443,5.359849,4.435803,0.247378,-0.014773,0.378828
+0.020000,5.335952,-0.085234,5.448664,4.440739,0.246848,-0.026534,0.387644
+0.020000,5.418119,-0.051257,5.537579,4.445751,0.250566,0.185911,0.396559
+0.020000,5.500070,-0.016507,5.626593,4.450703,0.247613,-0.147624,0.405573
+0.020000,5.581792,0.019024,5.715705,4.455584,0.244035,-0.178917,0.414685
+0.020000,5.663273,0.055341,5.804913,4.460433,0.242462,-0.078636,0.423895
+0.020000,5.744505,0.092453,5.894221,4.465377,0.247178,0.235783,0.433202
+0.020000,5.825473,0.130363,5.983625,4.470183,0.240313,-0.343240,0.442606
+0.020000,5.906164,0.169078,6.073123,4.474907,0.236212,-0.205044,0.452105
+0.020000,5.986569,0.208603,6.162717,4.479704,0.239823,0.180533,0.461699
+0.020000,6.066671,0.248944,6.252404,4.484368,0.233208,-0.330746,0.471386
+0.020000,6.146458,0.290105,6.342182,4.488917,0.227447,-0.288055,0.481166
+0.020000,6.225919,0.332091,6.432054,4.493571,0.232732,0.264250,0.491037
+0.020000,6.305037,0.374904,6.522013,4.497962,0.219557,-0.658723,0.500997
+0.020000,6.383801,0.418550,6.612061,4.502388,0.221287,0.086491,0.511045
+0.020000,6.462194,0.463030,6.702194,4.506672,0.214201,-0.354305,0.521179
+0.020000,6.540203,0.508348,6.792412,4.510866,0.209670,-0.226561,0.531397
+0.020000,6.617814,0.554506,6.882711,4.514984,0.205904,-0.188295,0.541697
+0.020000,6.695011,0.601506,6.973090,4.518944,0.198035,-0.393431,0.552076
+0.020000,6.771781,0.649348,7.063547,4.522840,0.194780,-0.162749,0.562533
+0.020000,6.848107,0.698033,7.154078,4.526561,0.186070,-0.435529,0.573065
+0.020000,6.923975,0.747562,7.244682,4.530185,0.181203,-0.243365,0.583669
+0.020000,6.999369,0.797934,7.335355,4.533655,0.173485,-0.385883,0.594343
+0.020000,7.074274,0.849147,7.426094,4.536966,0.165552,-0.396641,0.605083
+0.020000,7.148676,0.901201,7.516898,4.540167,0.160023,-0.276436,0.615886
+0.020000,7.222558,0.954093,7.607760,4.543132,0.148262,-0.588065,0.626750
+0.020000,7.295906,1.007820,7.698681,4.546039,0.145372,-0.144496,0.637671
+0.020000,7.368704,1.062379,7.789655,4.548674,0.131759,-0.680648,0.648645
+0.020000,7.440937,1.117766,7.880678,4.551182,0.125365,-0.319694,0.659669
+0.020000,7.512590,1.173976,7.971748,4.553503,0.116040,-0.466258,0.670740
+0.020000,7.583648,1.231003,8.062860,4.555602,0.104971,-0.553475,0.681852
+0.020000,7.654097,1.288843,8.154012,4.557566,0.098184,-0.339319,0.693004
+0.020000,7.723922,1.347489,8.245197,4.559283,0.085886,-0.614929,0.704190
+0.020000,7.793109,1.406932,8.336413,4.560807,0.076156,-0.486469,0.715407
+0.020000,7.861645,1.467167,8.427656,4.562152,0.067281,-0.443786,0.726650
+0.020000,7.929514,1.528184,8.518922,4.563256,0.055209,-0.603602,0.737916
+0.020000,7.996704,1.589975,8.610205,4.564163,0.045315,-0.494680,0.749200
+0.020000,8.063203,1.652529,8.701502,4.564861,0.034933,-0.519097,0.760498
+0.020000,8.128997,1.715838,8.792809,4.565333,0.023586,-0.567328,0.771805
+0.020000,8.194075,1.779891,8.884121,4.565596,0.013130,-0.522827,0.783117
+0.020000,8.258425,1.844677,8.975433,4.565639,0.002157,-0.548629,0.794431
+0.020000,8.322035,1.910183,9.066743,4.565467,-0.008591,-0.537438,0.805741
+0.020000,8.384895,1.976400,9.158044,4.565076,-0.019532,-0.547050,0.817043
+0.020000,8.446995,2.043313,9.249334,4.564467,-0.030481,-0.547450,0.828333
+0.020000,8.508325,2.110910,9.340607,4.563655,-0.040568,-0.504317,0.839606
+0.020000,8.568876,2.179178,9.431859,4.562615,-0.052004,-0.571816,0.850859
+0.020000,8.628639,2.248104,9.523086,4.561371,-0.062205,-0.510042,0.862087
+0.020000,8.687608,2.317674,9.614285,4.559939,-0.071594,-0.469453,0.873286
+0.020000,8.745773,2.387873,9.705450,4.558262,-0.083844,-0.612523,0.884452
+0.020000,8.803129,2.458687,9.796579,4.556421,-0.092055,-0.410552,0.895581
+0.020000,8.859670,2.530103,9.887667,4.554389,-0.101602,-0.477301,0.906669
+0.020000,8.915389,2.602103,9.978709,4.552118,-0.113580,-0.598948,0.917712
+0.020000,8.970282,2.674675,10.069704,4.549726,-0.119576,-0.299762,0.928707
+0.020000,9.024345,2.747803,10.160646,4.547102,-0.131215,-0.581941,0.939650
+0.020000,9.077574,2.821472,10.251532,4.544327,-0.138744,-0.376465,0.950537
+0.020000,9.129966,2.895666,10.342360,4.541403,-0.146202,-0.372894,0.961365
+0.020000,9.181518,2.970370,10.433125,4.538253,-0.157519,-0.565846,0.972131
+0.020000,9.232230,3.045569,10.523826,4.535047,-0.160279,-0.138033,0.982832
+0.020000,9.282098,3.121247,10.614457,4.531562,-0.174253,-0.698692,0.993464
+0.020000,9.331124,3.197391,10.705019,4.528071,-0.174566,-0.015643,1.004026
+0.020000,9.379306,3.273983,10.795505,4.524324,-0.187329,-0.638156,1.014513
+0.020000,9.426645,3.351010,10.885916,4.520559,-0.188226,-0.044853,1.024924
+0.020000,9.473142,3.428455,10.976248,4.516565,-0.199725,-0.574962,1.035256
+0.020000,9.518799,3.506306,11.066499,4.512575,-0.199513,0.010614,1.045508
+0.020000,9.563616,3.584546,11.156666,4.508340,-0.211724,-0.610559,1.055676
+0.020000,9.607599,3.663162,11.246749,4.504167,-0.208664,0.153034,1.065759
+0.020000,9.650747,3.742138,11.336744,4.499719,-0.222414,-0.687512,1.075754
+0.020000,9.693066,3.821463,11.426651,4.495348,-0.218510,0.195209,1.085661
+0.020000,9.734559,3.901120,11.516467,4.490809,-0.226959,-0.422446,1.095478
+0.020000,9.775230,3.981097,11.606191,4.486205,-0.230207,-0.162398,1.105203
+0.020000,9.815084,4.061382,11.695823,4.481621,-0.229208,0.049914,1.114835
+0.020000,9.854124,4.141958,11.785360,4.476821,-0.240001,-0.539621,1.124372
+0.020000,9.892358,4.222816,11.874802,4.472091,-0.236499,0.175095,1.133814
+0.020000,9.929791,4.303943,11.964148,4.467337,-0.237688,-0.059457,1.143161
+0.020000,9.966428,4.385325,12.053397,4.462417,-0.246017,-0.416428,1.152410
+0.020000,10.002275,4.466952,12.142548,4.457541,-0.243816,0.110048,1.161562
+0.020000,10.037339,4.548812,12.231601,4.452672,-0.243428,0.019358,1.170615
+0.020000,10.071627,4.630893,12.320557,4.447775,-0.244846,-0.070899,1.179570
+0.020000,10.105145,4.713184,12.409411,4.442742,-0.251653,-0.340334,1.188426
+0.020000,10.137900,4.795675,12.498167,4.437793,-0.247464,0.209444,1.197182
+0.020000,10.169900,4.878355,12.586824,4.432849,-0.247186,0.013888,1.205840
+0.020000,10.201152,4.961215,12.675382,4.427897,-0.247575,-0.019447,1.214398
+0.020000,10.231664,5.044245,12.763841,4.422943,-0.247732,-0.007855,1.222857
+0.020000,10.261443,5.127436,12.852201,4.417990,-0.247663,0.003449,1.231216
+0.020000,10.290497,5.210777,12.940461,4.413010,-0.249002,-0.066914,1.239477
+0.020000,10.318834,5.294260,13.028622,4.408053,-0.247818,0.059207,1.247638
+0.020000,10.346462,5.377876,13.116685,4.403143,-0.245514,0.115202,1.255702
+0.020000,10.373389,5.461619,13.204650,4.398252,-0.244564,0.047499,1.263667
+0.020000,10.399623,5.545479,13.292518,4.393383,-0.243419,0.057241,1.271535
+0.020000,10.425173,5.629449,13.380288,4.388542,-0.242085,0.066676,1.279306
+0.020000,10.450046,5.713521,13.467963,4.383730,-0.240569,0.075806,1.286981
+0.020000,10.474250,5.797689,13.555542,4.378953,-0.238877,0.084631,1.294560
+0.020000,10.497795,5.881945,13.643026,4.374212,-0.237014,0.093151,1.302044
+0.020000,10.520687,5.966283,13.730416,4.369487,-0.236282,0.036577,1.309434
+0.020000,10.542936,6.050696,13.817711,4.364768,-0.235922,0.017996,1.316730
+0.020000,10.564549,6.135178,13.904914,4.360141,-0.231383,0.226973,1.323934
+0.020000,10.585534,6.219724,13.992025,4.355562,-0.228921,0.123058,1.331045
+0.020000,10.605901,6.304328,14.079046,4.351036,-0.226318,0.130164,1.338066
+0.020000,10.625656,6.388984,14.165977,4.346564,-0.223579,0.136983,1.344997
+0.020000,10.644809,6.473689,14.252820,4.342133,-0.221562,0.100833,1.351839
+0.020000,10.663366,6.558433,14.339572,4.337599,-0.226718,-0.257832,1.358592
+0.020000,10.681336,6.643215,14.426238,4.333286,-0.215648,0.553528,1.365259
+0.020000,10.698727,6.728031,14.512819,4.329037,-0.212434,0.160686,1.371839
+0.020000,10.715547,6.812875,14.599314,4.324776,-0.213033,-0.029949,1.378334
+0.020000,10.731802,6.897742,14.685723,4.320466,-0.215531,-0.124878,1.384745
+0.020000,10.747502,6.982630,14.772051,4.316400,-0.203267,0.613190,1.391073
+0.020000,10.762649,7.067507,14.858269,4.310885,-0.275764,-3.624844,1.397316
+0.020000,10.777239,7.152305,14.944313,4.302199,-0.434308,-7.927230,1.403472
+0.020000,10.791273,7.236959,15.030122,4.290463,-0.586778,-7.623455,1.409538
+0.020000,10.804749,7.321393,15.115626,4.275161,-0.765134,-8.917830,1.415509
+0.020000,10.817669,7.405544,15.200762,4.256836,-0.916230,-7.554804,1.421383
+0.020000,10.830035,7.489338,15.285463,4.235055,-1.089056,-8.641283,1.427157
+0.020000,10.841852,7.572709,15.369668,4.210223,-1.241602,-7.627325,1.432830
+0.020000,10.853123,7.655584,15.453306,4.181883,-1.417017,-8.770721,1.438397
+0.020000,10.863857,7.737898,15.536317,4.150590,-1.564646,-7.381445,1.443858
+0.020000,10.874059,7.819582,15.618636,4.115920,-1.733490,-8.442198,1.449210
+0.020000,10.883739,7.900565,15.700195,4.077979,-1.897021,-8.176574,1.454452
+0.020000,10.892905,7.980783,15.780935,4.036962,-2.050858,-7.691857,1.459581
+0.020000,10.901568,8.060166,15.860789,3.992715,-2.212387,-8.076460,1.464597
+0.020000,10.909740,8.138646,15.939694,3.945250,-2.373211,-8.041188,1.469497
+0.020000,10.917431,8.216157,16.017586,3.894585,-2.533260,-8.002460,1.474282
+0.020000,10.924654,8.292632,16.094400,3.840736,-2.692462,-7.960085,1.478948
+0.020000,10.931423,8.368003,16.170075,3.783721,-2.850740,-7.913885,1.483496
+0.020000,10.937752,8.442205,16.244546,3.723561,-3.008014,-7.863699,1.487925
+0.020000,10.943654,8.515172,16.317751,3.660277,-3.164201,-7.809375,1.492233
+0.020000,10.949145,8.586833,16.389622,3.593552,-3.336230,-8.601452,1.496419
+0.020000,10.954239,8.657128,16.460102,3.523991,-3.478056,-7.091263,1.500483
+0.020000,10.958952,8.725989,16.529124,3.451075,-3.645789,-8.386676,1.504424
+0.020000,10.963300,8.793352,16.596627,3.375169,-3.795307,-7.475869,1.508242
+0.020000,10.967299,8.859147,16.662544,3.295824,-3.967282,-8.598784,1.511935
+0.020000,10.970965,8.923314,16.726815,3.213566,-4.112888,-7.280272,1.515503
+0.020000,10.974315,8.985786,16.789377,3.128095,-4.273531,-8.032179,1.518946
+0.020000,10.977365,9.046498,16.850166,3.039436,-4.432969,-7.971877,1.522262
+0.020000,10.980131,9.105385,16.909118,2.947614,-4.591081,-7.905592,1.525452
+0.020000,10.982630,9.162384,16.966171,2.852659,-4.747745,-7.833228,1.528513
+0.020000,10.984878,9.217430,17.021263,2.754602,-4.902839,-7.754701,1.531446
+0.020000,10.986891,9.270456,17.074327,2.653201,-5.070074,-8.361741,1.534251
+0.020000,10.988685,9.321400,17.125303,2.548810,-5.219547,-7.473660,1.536925
+0.020000,10.990277,9.370225,17.174154,2.442548,-5.313081,-4.676705,1.539470
+0.020000,10.991684,9.416930,17.220880,2.336309,-5.311986,0.054744,1.541889
+0.020000,10.992922,9.461514,17.265481,2.230023,-5.314260,-0.113661,1.544182
+0.020000,10.994006,9.503977,17.307958,2.123869,-5.307722,0.326901,1.546354
+0.020000,10.994951,9.544324,17.348316,2.017904,-5.298257,0.473214,1.548405
+0.020000,10.995771,9.582555,17.386556,1.911999,-5.295250,0.150389,1.550339
+0.020000,10.996477,9.618670,17.422678,1.806078,-5.296024,-0.038733,1.552155
+0.020000,10.997082,9.652673,17.456686,1.700425,-5.282660,0.668198,1.553858
+0.020000,10.997597,9.684594,17.488611,1.596239,-5.209327,3.666676,1.555448
+0.020000,10.998034,9.714490,17.518511,1.494969,-5.063492,7.291738,1.556932
+0.020000,10.998402,9.742432,17.546455,1.397235,-4.886713,8.838928,1.558313
+0.020000,10.998710,9.768488,17.572513,1.302867,-4.718384,8.416491,1.559596
+0.020000,10.998967,9.792720,17.596746,1.211677,-4.559496,7.944393,1.560785
+0.020000,10.999180,9.815196,17.619224,1.123877,-4.390018,8.473909,1.561884
+0.020000,10.999355,9.835985,17.640013,1.039461,-4.220760,8.462882,1.562898
+0.020000,10.999497,9.855152,17.659180,0.958368,-4.054692,8.303402,1.563830
+0.020000,10.999612,9.872762,17.676791,0.880531,-3.891832,8.143014,1.564685
+0.020000,10.999705,9.888881,17.692910,0.805958,-3.728627,8.160254,1.565465
+0.020000,10.999778,9.903577,17.707607,0.734831,-3.556351,8.613784,1.566174
+0.020000,10.999835,9.916919,17.720948,0.667072,-3.387975,8.418777,1.566818
+0.020000,10.999879,9.928964,17.732994,0.602280,-3.239595,7.419005,1.567397
+0.020000,10.999913,9.939788,17.743817,0.541170,-3.055492,9.205140,1.567917
+0.020000,10.999939,9.949449,17.753478,0.483053,-2.905882,7.480504,1.568381
+0.020000,10.999958,9.958020,17.762049,0.428556,-2.724837,9.052253,1.568792
+0.020000,10.999972,9.965558,17.769588,0.376910,-2.582280,7.127883,1.569152
+0.020000,10.999981,9.972136,17.776166,0.328926,-2.399195,9.154208,1.569467
+0.020000,10.999988,9.977820,17.781850,0.284188,-2.236931,8.113224,1.569739
+0.020000,10.999993,9.982670,17.786700,0.242519,-2.083457,7.673711,1.569970
+0.020000,10.999996,9.986756,17.790786,0.204271,-1.912374,8.554116,1.570165
+0.020000,10.999998,9.990143,17.794173,0.169345,-1.746282,8.304641,1.570327
+0.020000,10.999999,9.992897,17.796926,0.137687,-1.582910,8.168598,1.570458
+0.020000,10.999999,9.995083,17.799112,0.109301,-1.419332,8.178866,1.570562
+0.020000,11.000000,9.996766,17.800796,0.084189,-1.255590,8.187129,1.570642
+0.020000,11.000000,9.998013,17.802043,0.062354,-1.091718,8.193603,1.570702
+0.020000,11.000000,9.998889,17.802919,0.043799,-0.927748,8.198506,1.570743
+0.020000,11.000000,9.999460,17.803490,0.028525,-0.763706,8.202058,1.570771
+0.020000,11.000000,9.999791,17.803820,0.016533,-0.599617,8.204478,1.570786
+0.020000,11.000000,9.999947,17.803977,0.007823,-0.435497,8.205984,1.570794
+0.020000,11.000000,9.999995,17.804025,0.002396,-0.271361,8.206797,1.570796
+0.020000,11.000000,10.000000,17.804030,0.000251,-0.107219,8.207136,1.570796
+0.020000,11.000000,10.000000,17.804030,0.000000,-0.012573,4.732266,1.570796
diff --git a/src/main/deploy/output/StraightTen.left.pf1.csv b/src/main/deploy/output/StraightTen.left.pf1.csv
new file mode 100644
index 0000000..47e465c
--- /dev/null
+++ b/src/main/deploy/output/StraightTen.left.pf1.csv
@@ -0,0 +1,199 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,1.000000,0.000031,0.003125,0.156250,7.812500,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,-0.156250,-15.625000,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,7.812500,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.002031,1.000000,0.002063,0.101563,5.078125,253.906250,0.000000
+0.020000,0.005812,1.000000,0.005844,0.189062,4.375000,-35.156250,0.000000
+0.020000,0.010313,1.000000,0.010344,0.225000,1.796875,-128.906250,0.000000
+0.020000,0.015594,1.000000,0.015625,0.264062,1.953125,7.812500,0.000000
+0.020000,0.021719,1.000000,0.021750,0.306250,2.109375,7.812500,0.000000
+0.020000,0.028750,1.000000,0.028781,0.351562,2.265625,7.812500,0.000000
+0.020000,0.036750,1.000000,0.036781,0.400000,2.421875,7.812500,0.000000
+0.020000,0.045781,1.000000,0.045812,0.451562,2.578125,7.812500,0.000000
+0.020000,0.055906,1.000000,0.055938,0.506250,2.734375,7.812500,0.000000
+0.020000,0.067188,1.000000,0.067219,0.564063,2.890625,7.812500,0.000000
+0.020000,0.079687,1.000000,0.079719,0.625000,3.046875,7.812500,0.000000
+0.020000,0.093469,1.000000,0.093500,0.689062,3.203125,7.812500,0.000000
+0.020000,0.108594,1.000000,0.108625,0.756250,3.359375,7.812500,0.000000
+0.020000,0.125125,1.000000,0.125156,0.826562,3.515625,7.812500,0.000000
+0.020000,0.143125,1.000000,0.143156,0.900000,3.671875,7.812500,0.000000
+0.020000,0.162656,1.000000,0.162687,0.976562,3.828125,7.812500,0.000000
+0.020000,0.183781,1.000000,0.183812,1.056250,3.984375,7.812500,0.000000
+0.020000,0.206562,1.000000,0.206594,1.139062,4.140625,7.812500,0.000000
+0.020000,0.231062,1.000000,0.231094,1.225000,4.296875,7.812500,0.000000
+0.020000,0.257344,1.000000,0.257375,1.314062,4.453125,7.812500,0.000000
+0.020000,0.285469,1.000000,0.285500,1.406250,4.609375,7.812500,0.000000
+0.020000,0.315500,1.000000,0.315531,1.501562,4.765625,7.812500,0.000000
+0.020000,0.347500,1.000000,0.347531,1.600000,4.921875,7.812500,0.000000
+0.020000,0.381500,1.000000,0.381531,1.700000,5.000000,3.906250,0.000000
+0.020000,0.417500,1.000000,0.417531,1.800000,5.000000,0.000000,0.000000
+0.020000,0.455500,1.000000,0.455531,1.900000,5.000000,-0.000000,0.000000
+0.020000,0.495500,1.000000,0.495531,2.000000,5.000000,0.000000,0.000000
+0.020000,0.537500,1.000000,0.537531,2.100000,5.000000,-0.000000,0.000000
+0.020000,0.581500,1.000000,0.581531,2.200000,5.000000,0.000000,0.000000
+0.020000,0.627500,1.000000,0.627531,2.300000,5.000000,-0.000000,0.000000
+0.020000,0.675500,1.000000,0.675531,2.400000,5.000000,0.000000,0.000000
+0.020000,0.725469,1.000000,0.725500,2.498437,4.921875,-3.906250,0.000000
+0.020000,0.777344,1.000000,0.777375,2.593750,4.765625,-7.812500,0.000000
+0.020000,0.831062,1.000000,0.831094,2.685937,4.609375,-7.812500,0.000000
+0.020000,0.886562,1.000000,0.886594,2.775000,4.453125,-7.812500,0.000000
+0.020000,0.943781,1.000000,0.943812,2.860938,4.296875,-7.812500,0.000000
+0.020000,1.002656,1.000000,1.002687,2.943750,4.140625,-7.812500,0.000000
+0.020000,1.063125,1.000000,1.063156,3.023437,3.984375,-7.812500,0.000000
+0.020000,1.125125,1.000000,1.125156,3.100000,3.828125,-7.812500,0.000000
+0.020000,1.188594,1.000000,1.188625,3.173437,3.671875,-7.812500,0.000000
+0.020000,1.253469,1.000000,1.253500,3.243750,3.515625,-7.812500,0.000000
+0.020000,1.319687,1.000000,1.319719,3.310937,3.359375,-7.812500,0.000000
+0.020000,1.387187,1.000000,1.387219,3.375000,3.203125,-7.812500,0.000000
+0.020000,1.455906,1.000000,1.455937,3.435937,3.046875,-7.812500,0.000000
+0.020000,1.525781,1.000000,1.525812,3.493750,2.890625,-7.812500,0.000000
+0.020000,1.596750,1.000000,1.596781,3.548438,2.734375,-7.812500,0.000000
+0.020000,1.668750,1.000000,1.668781,3.600000,2.578125,-7.812500,0.000000
+0.020000,1.741719,1.000000,1.741750,3.648437,2.421875,-7.812500,0.000000
+0.020000,1.815594,1.000000,1.815625,3.693750,2.265625,-7.812500,0.000000
+0.020000,1.890312,1.000000,1.890344,3.735937,2.109375,-7.812500,0.000000
+0.020000,1.965812,1.000000,1.965844,3.775000,1.953125,-7.812500,0.000000
+0.020000,2.042031,1.000000,2.042062,3.810937,1.796875,-7.812500,0.000000
+0.020000,2.118906,1.000000,2.118937,3.843750,1.640625,-7.812500,0.000000
+0.020000,2.196375,1.000000,2.196406,3.873438,1.484375,-7.812500,0.000000
+0.020000,2.274375,1.000000,2.274406,3.900000,1.328125,-7.812500,0.000000
+0.020000,2.352844,1.000000,2.352875,3.923437,1.171875,-7.812500,0.000000
+0.020000,2.431719,1.000000,2.431750,3.943750,1.015625,-7.812500,0.000000
+0.020000,2.510937,1.000000,2.510969,3.960937,0.859375,-7.812500,0.000000
+0.020000,2.590437,1.000000,2.590469,3.975000,0.703125,-7.812500,0.000000
+0.020000,2.670156,1.000000,2.670187,3.985938,0.546875,-7.812500,0.000000
+0.020000,2.750031,1.000000,2.750062,3.993750,0.390625,-7.812500,0.000000
+0.020000,2.830000,1.000000,2.830031,3.998437,0.234375,-7.812500,0.000000
+0.020000,2.910000,1.000000,2.910031,4.000000,0.078125,-7.812500,0.000000
+0.020000,2.990000,1.000000,2.990031,4.000000,0.000000,-3.906250,0.000000
+0.020000,3.070000,1.000000,3.070031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.150000,1.000000,3.150031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.230000,1.000000,3.230031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.310000,1.000000,3.310031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.390000,1.000000,3.390031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.470000,1.000000,3.470031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,3.550000,1.000000,3.550031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.630000,1.000000,3.630031,4.000000,0.000000,-0.000000,0.000000
+0.020000,3.710000,1.000000,3.710031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.790000,1.000000,3.790031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.870000,1.000000,3.870031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.950000,1.000000,3.950031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.030000,1.000000,4.030031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.110000,1.000000,4.110031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,4.190000,1.000000,4.190031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.270000,1.000000,4.270031,4.000000,0.000000,-0.000000,0.000000
+0.020000,4.350000,1.000000,4.350031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.430000,1.000000,4.430031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.510000,1.000000,4.510031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.590000,1.000000,4.590031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.670000,1.000000,4.670031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.750000,1.000000,4.750031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.830000,1.000000,4.830031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.910000,1.000000,4.910031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.990000,1.000000,4.990031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.070000,1.000000,5.070031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.150000,1.000000,5.150031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.230000,1.000000,5.230031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.310000,1.000000,5.310031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.390000,1.000000,5.390031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.470000,1.000000,5.470031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.550000,1.000000,5.550031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.630000,1.000000,5.630031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,5.710000,1.000000,5.710031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.790000,1.000000,5.790031,4.000000,0.000000,-0.000000,0.000000
+0.020000,5.870000,1.000000,5.870031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.950000,1.000000,5.950031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.030000,1.000000,6.030031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.110000,1.000000,6.110031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.190000,1.000000,6.190031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.270000,1.000000,6.270031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.350000,1.000000,6.350031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.430000,1.000000,6.430031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.510000,1.000000,6.510031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.590000,1.000000,6.590031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.670000,1.000000,6.670031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.750000,1.000000,6.750031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.830000,1.000000,6.830031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.910000,1.000000,6.910031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,6.990000,1.000000,6.990031,4.000000,0.000000,0.000000,0.000000
+0.020000,7.070000,1.000000,7.070031,4.000000,0.000000,-0.000000,0.000000
+0.020000,7.150000,1.000000,7.150031,4.000000,0.000000,0.000000,0.000000
+0.020000,7.229973,1.000000,7.230004,3.998633,-0.068359,-3.417969,0.000000
+0.020000,7.309859,1.000000,7.309891,3.994336,-0.214844,-7.324219,0.000000
+0.020000,7.389598,1.000000,7.389629,3.986914,-0.371094,-7.812500,0.000000
+0.020000,7.469125,1.000000,7.469156,3.976367,-0.527344,-7.812500,0.000000
+0.020000,7.548379,1.000000,7.548410,3.962695,-0.683594,-7.812500,0.000000
+0.020000,7.627297,1.000000,7.627328,3.945898,-0.839844,-7.812500,0.000000
+0.020000,7.705816,1.000000,7.705848,3.925977,-0.996094,-7.812500,0.000000
+0.020000,7.783875,1.000000,7.783906,3.902930,-1.152344,-7.812500,0.000000
+0.020000,7.861410,1.000000,7.861441,3.876758,-1.308594,-7.812500,0.000000
+0.020000,7.938359,1.000000,7.938391,3.847461,-1.464844,-7.812500,0.000000
+0.020000,8.014660,1.000000,8.014691,3.815039,-1.621094,-7.812500,0.000000
+0.020000,8.090250,1.000000,8.090281,3.779492,-1.777344,-7.812500,0.000000
+0.020000,8.165066,1.000000,8.165098,3.740820,-1.933594,-7.812500,0.000000
+0.020000,8.239047,1.000000,8.239078,3.699023,-2.089844,-7.812500,0.000000
+0.020000,8.312129,1.000000,8.312160,3.654102,-2.246094,-7.812500,0.000000
+0.020000,8.384250,1.000000,8.384281,3.606055,-2.402344,-7.812500,0.000000
+0.020000,8.455348,1.000000,8.455379,3.554883,-2.558594,-7.812500,0.000000
+0.020000,8.525359,1.000000,8.525391,3.500586,-2.714844,-7.812500,0.000000
+0.020000,8.594223,1.000000,8.594254,3.443164,-2.871094,-7.812500,0.000000
+0.020000,8.661875,1.000000,8.661906,3.382617,-3.027344,-7.812500,0.000000
+0.020000,8.728254,1.000000,8.728285,3.318945,-3.183594,-7.812500,0.000000
+0.020000,8.793297,1.000000,8.793328,3.252148,-3.339844,-7.812500,0.000000
+0.020000,8.856941,1.000000,8.856973,3.182227,-3.496094,-7.812500,0.000000
+0.020000,8.919125,1.000000,8.919156,3.109180,-3.652344,-7.812500,0.000000
+0.020000,8.979785,1.000000,8.979816,3.033008,-3.808594,-7.812500,0.000000
+0.020000,9.038859,1.000000,9.038891,2.953711,-3.964844,-7.812500,0.000000
+0.020000,9.096285,1.000000,9.096316,2.871289,-4.121094,-7.812500,0.000000
+0.020000,9.152000,1.000000,9.152031,2.785742,-4.277344,-7.812500,0.000000
+0.020000,9.205941,1.000000,9.205973,2.697070,-4.433594,-7.812500,0.000000
+0.020000,9.258047,1.000000,9.258078,2.605273,-4.589844,-7.812500,0.000000
+0.020000,9.308254,1.000000,9.308285,2.510352,-4.746094,-7.812500,0.000000
+0.020000,9.356500,1.000000,9.356531,2.412305,-4.902344,-7.812500,0.000000
+0.020000,9.402750,1.000000,9.402781,2.312500,-4.990234,-4.394531,0.000000
+0.020000,9.447000,1.000000,9.447031,2.212500,-5.000000,-0.488281,0.000000
+0.020000,9.489250,1.000000,9.489281,2.112500,-5.000000,0.000000,0.000000
+0.020000,9.529500,1.000000,9.529531,2.012500,-5.000000,0.000000,0.000000
+0.020000,9.567750,1.000000,9.567781,1.912500,-5.000000,-0.000000,0.000000
+0.020000,9.604000,1.000000,9.604031,1.812500,-5.000000,0.000000,0.000000
+0.020000,9.638250,1.000000,9.638281,1.712500,-5.000000,-0.000000,0.000000
+0.020000,9.670500,1.000000,9.670531,1.612500,-5.000000,0.000000,0.000000
+0.020000,9.700777,1.000000,9.700809,1.513867,-4.931641,3.417969,0.000000
+0.020000,9.729141,1.000000,9.729172,1.418164,-4.785156,7.324219,0.000000
+0.020000,9.755652,1.000000,9.755684,1.325586,-4.628906,7.812500,0.000000
+0.020000,9.780375,1.000000,9.780406,1.236133,-4.472656,7.812500,0.000000
+0.020000,9.803371,1.000000,9.803402,1.149805,-4.316406,7.812500,0.000000
+0.020000,9.824703,1.000000,9.824734,1.066602,-4.160156,7.812500,0.000000
+0.020000,9.844434,1.000000,9.844465,0.986523,-4.003906,7.812500,0.000000
+0.020000,9.862625,1.000000,9.862656,0.909570,-3.847656,7.812500,0.000000
+0.020000,9.879340,1.000000,9.879371,0.835742,-3.691406,7.812500,0.000000
+0.020000,9.894641,1.000000,9.894672,0.765039,-3.535156,7.812500,0.000000
+0.020000,9.908590,1.000000,9.908621,0.697461,-3.378906,7.812500,0.000000
+0.020000,9.921250,1.000000,9.921281,0.633008,-3.222656,7.812500,0.000000
+0.020000,9.932684,1.000000,9.932715,0.571680,-3.066406,7.812500,0.000000
+0.020000,9.942953,1.000000,9.942984,0.513477,-2.910156,7.812500,0.000000
+0.020000,9.952121,1.000000,9.952152,0.458398,-2.753906,7.812500,0.000000
+0.020000,9.960250,1.000000,9.960281,0.406445,-2.597656,7.812500,0.000000
+0.020000,9.967402,1.000000,9.967434,0.357617,-2.441406,7.812500,0.000000
+0.020000,9.973641,1.000000,9.973672,0.311914,-2.285156,7.812500,0.000000
+0.020000,9.979027,1.000000,9.979059,0.269336,-2.128906,7.812500,0.000000
+0.020000,9.983625,1.000000,9.983656,0.229883,-1.972656,7.812500,0.000000
+0.020000,9.987496,1.000000,9.987527,0.193555,-1.816406,7.812500,0.000000
+0.020000,9.990703,1.000000,9.990734,0.160352,-1.660156,7.812500,0.000000
+0.020000,9.993309,1.000000,9.993340,0.130273,-1.503906,7.812500,0.000000
+0.020000,9.995375,1.000000,9.995406,0.103320,-1.347656,7.812500,0.000000
+0.020000,9.996965,1.000000,9.996996,0.079492,-1.191406,7.812500,0.000000
+0.020000,9.998141,1.000000,9.998172,0.058789,-1.035156,7.812500,0.000000
+0.020000,9.998965,1.000000,9.998996,0.041211,-0.878906,7.812500,0.000000
+0.020000,9.999500,1.000000,9.999531,0.026758,-0.722656,7.812500,0.000000
+0.020000,9.999809,1.000000,9.999840,0.015430,-0.566406,7.812500,0.000000
+0.020000,9.999953,1.000000,9.999984,0.007227,-0.410156,7.812500,0.000000
+0.020000,9.999996,1.000000,10.000027,0.002148,-0.253906,7.812500,0.000000
+0.020000,10.000000,1.000000,10.000031,0.000195,-0.097656,7.812500,0.000000
+0.020000,10.000000,1.000000,10.000031,0.000000,-0.009766,4.394531,0.000000
diff --git a/src/main/deploy/output/StraightTen.pf1.csv b/src/main/deploy/output/StraightTen.pf1.csv
new file mode 100644
index 0000000..a40f9d0
--- /dev/null
+++ b/src/main/deploy/output/StraightTen.pf1.csv
@@ -0,0 +1,199 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,0.000000,0.000031,0.003125,0.156250,7.812500,0.000000
+0.020000,0.000000,0.000000,0.000156,0.009375,0.312500,7.812500,0.000000
+0.020000,0.000000,0.000000,0.000437,0.018750,0.468750,7.812500,0.000000
+0.020000,0.000000,0.000000,0.000937,0.031250,0.625000,7.812500,0.000000
+0.020000,0.000000,0.000000,0.001719,0.046875,0.781250,7.812500,0.000000
+0.020000,0.000000,0.000000,0.002844,0.065625,0.937500,7.812500,0.000000
+0.020000,0.000000,0.000000,0.004375,0.087500,1.093750,7.812500,0.000000
+0.020000,0.000000,0.000000,0.006375,0.112500,1.250000,7.812500,0.000000
+0.020000,0.000000,0.000000,0.008906,0.140625,1.406250,7.812500,0.000000
+0.020000,0.002031,0.000000,0.012031,0.171875,1.562500,7.812500,0.000000
+0.020000,0.005812,0.000000,0.015813,0.206250,1.718750,7.812500,0.000000
+0.020000,0.010313,0.000000,0.020313,0.243750,1.875000,7.812500,0.000000
+0.020000,0.015594,0.000000,0.025594,0.284375,2.031250,7.812500,0.000000
+0.020000,0.021719,0.000000,0.031719,0.328125,2.187500,7.812500,0.000000
+0.020000,0.028750,0.000000,0.038750,0.375000,2.343750,7.812500,0.000000
+0.020000,0.036750,0.000000,0.046750,0.425000,2.500000,7.812500,0.000000
+0.020000,0.045781,0.000000,0.055781,0.478125,2.656250,7.812500,0.000000
+0.020000,0.055906,0.000000,0.065906,0.534375,2.812500,7.812500,0.000000
+0.020000,0.067188,0.000000,0.077188,0.593750,2.968750,7.812500,0.000000
+0.020000,0.079687,0.000000,0.089688,0.656250,3.125000,7.812500,0.000000
+0.020000,0.093469,0.000000,0.103469,0.721875,3.281250,7.812500,0.000000
+0.020000,0.108594,0.000000,0.118594,0.790625,3.437500,7.812500,0.000000
+0.020000,0.125125,0.000000,0.135125,0.862500,3.593750,7.812500,0.000000
+0.020000,0.143125,0.000000,0.153125,0.937500,3.750000,7.812500,0.000000
+0.020000,0.162656,0.000000,0.172656,1.015625,3.906250,7.812500,0.000000
+0.020000,0.183781,0.000000,0.193781,1.096875,4.062500,7.812500,0.000000
+0.020000,0.206562,0.000000,0.216563,1.181250,4.218750,7.812500,0.000000
+0.020000,0.231062,0.000000,0.241063,1.268750,4.375000,7.812500,0.000000
+0.020000,0.257344,0.000000,0.267344,1.359375,4.531250,7.812500,0.000000
+0.020000,0.285469,0.000000,0.295469,1.453125,4.687500,7.812500,0.000000
+0.020000,0.315500,0.000000,0.325500,1.550000,4.843750,7.812500,0.000000
+0.020000,0.347500,0.000000,0.357500,1.650000,5.000000,7.812500,0.000000
+0.020000,0.381500,0.000000,0.391500,1.750000,5.000000,0.000000,0.000000
+0.020000,0.417500,0.000000,0.427500,1.850000,5.000000,0.000000,0.000000
+0.020000,0.455500,0.000000,0.465500,1.950000,5.000000,-0.000000,0.000000
+0.020000,0.495500,0.000000,0.505500,2.050000,5.000000,0.000000,0.000000
+0.020000,0.537500,0.000000,0.547500,2.150000,5.000000,0.000000,0.000000
+0.020000,0.581500,0.000000,0.591500,2.250000,5.000000,0.000000,0.000000
+0.020000,0.627500,0.000000,0.637500,2.350000,5.000000,0.000000,0.000000
+0.020000,0.675500,0.000000,0.685500,2.450000,5.000000,0.000000,0.000000
+0.020000,0.725469,0.000000,0.735469,2.546875,4.843750,-7.812500,0.000000
+0.020000,0.777344,0.000000,0.787344,2.640625,4.687500,-7.812500,0.000000
+0.020000,0.831062,0.000000,0.841063,2.731250,4.531250,-7.812500,0.000000
+0.020000,0.886562,0.000000,0.896563,2.818750,4.375000,-7.812500,0.000000
+0.020000,0.943781,0.000000,0.953781,2.903125,4.218750,-7.812500,0.000000
+0.020000,1.002656,0.000000,1.012656,2.984375,4.062500,-7.812500,0.000000
+0.020000,1.063125,0.000000,1.073125,3.062500,3.906250,-7.812500,0.000000
+0.020000,1.125125,0.000000,1.135125,3.137500,3.750000,-7.812500,0.000000
+0.020000,1.188594,0.000000,1.198594,3.209375,3.593750,-7.812500,0.000000
+0.020000,1.253469,0.000000,1.263469,3.278125,3.437500,-7.812500,0.000000
+0.020000,1.319687,0.000000,1.329688,3.343750,3.281250,-7.812500,0.000000
+0.020000,1.387187,0.000000,1.397188,3.406250,3.125000,-7.812500,0.000000
+0.020000,1.455906,0.000000,1.465906,3.465625,2.968750,-7.812500,0.000000
+0.020000,1.525781,0.000000,1.535781,3.521875,2.812500,-7.812500,0.000000
+0.020000,1.596750,0.000000,1.606750,3.575000,2.656250,-7.812500,0.000000
+0.020000,1.668750,0.000000,1.678750,3.625000,2.500000,-7.812500,0.000000
+0.020000,1.741719,0.000000,1.751719,3.671875,2.343750,-7.812500,0.000000
+0.020000,1.815594,0.000000,1.825594,3.715625,2.187500,-7.812500,0.000000
+0.020000,1.890312,0.000000,1.900313,3.756250,2.031250,-7.812500,0.000000
+0.020000,1.965812,0.000000,1.975812,3.793750,1.875000,-7.812500,0.000000
+0.020000,2.042031,0.000000,2.052031,3.828125,1.718750,-7.812500,0.000000
+0.020000,2.118906,0.000000,2.128906,3.859375,1.562500,-7.812500,0.000000
+0.020000,2.196375,0.000000,2.206375,3.887500,1.406250,-7.812500,0.000000
+0.020000,2.274375,0.000000,2.284375,3.912500,1.250000,-7.812500,0.000000
+0.020000,2.352844,0.000000,2.362844,3.934375,1.093750,-7.812500,0.000000
+0.020000,2.431719,0.000000,2.441719,3.953125,0.937500,-7.812500,0.000000
+0.020000,2.510937,0.000000,2.520937,3.968750,0.781250,-7.812500,0.000000
+0.020000,2.590437,0.000000,2.600437,3.981250,0.625000,-7.812500,0.000000
+0.020000,2.670156,0.000000,2.680156,3.990625,0.468750,-7.812500,0.000000
+0.020000,2.750031,0.000000,2.760031,3.996875,0.312500,-7.812500,0.000000
+0.020000,2.830000,0.000000,2.840000,4.000000,0.156250,-7.812500,0.000000
+0.020000,2.910000,0.000000,2.920000,4.000000,0.000000,-7.812500,0.000000
+0.020000,2.990000,0.000000,3.000000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.070000,0.000000,3.080000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.150000,0.000000,3.160000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.230000,0.000000,3.240000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.310000,0.000000,3.320000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.390000,0.000000,3.400000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.470000,0.000000,3.480000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.550000,0.000000,3.560000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.630000,0.000000,3.640000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.710000,0.000000,3.720000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.790000,0.000000,3.800000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.870000,0.000000,3.880000,4.000000,0.000000,0.000000,0.000000
+0.020000,3.950000,0.000000,3.960000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.030000,0.000000,4.040000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.110000,0.000000,4.120000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.190000,0.000000,4.200000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.270000,0.000000,4.280000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.350000,0.000000,4.360000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.430000,0.000000,4.440000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.510000,0.000000,4.520000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.590000,0.000000,4.600000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.670000,0.000000,4.680000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.750000,0.000000,4.760000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.830000,0.000000,4.840000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.910000,0.000000,4.920000,4.000000,0.000000,0.000000,0.000000
+0.020000,4.990000,0.000000,5.000000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.070000,0.000000,5.080000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.150000,0.000000,5.160000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.230000,0.000000,5.240000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.310000,0.000000,5.320000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.390000,0.000000,5.400000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.470000,0.000000,5.480000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.550000,0.000000,5.560000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.630000,0.000000,5.640000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.710000,0.000000,5.720000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.790000,0.000000,5.800000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.870000,0.000000,5.880000,4.000000,0.000000,0.000000,0.000000
+0.020000,5.950000,0.000000,5.960000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.030000,0.000000,6.040000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.110000,0.000000,6.120000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.190000,0.000000,6.200000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.270000,0.000000,6.280000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.350000,0.000000,6.360000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.430000,0.000000,6.440000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.510000,0.000000,6.520000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.590000,0.000000,6.600000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.670000,0.000000,6.680000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.750000,0.000000,6.760000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.830000,0.000000,6.840000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.910000,0.000000,6.920000,4.000000,0.000000,0.000000,0.000000
+0.020000,6.990000,0.000000,7.000000,4.000000,0.000000,0.000000,0.000000
+0.020000,7.070000,0.000000,7.080000,4.000000,0.000000,0.000000,0.000000
+0.020000,7.150000,0.000000,7.160000,4.000000,0.000000,0.000000,0.000000
+0.020000,7.229973,0.000000,7.239973,3.997266,-0.136719,-6.835938,0.000000
+0.020000,7.309859,0.000000,7.319859,3.991406,-0.292969,-7.812500,0.000000
+0.020000,7.389598,0.000000,7.399598,3.982422,-0.449219,-7.812500,0.000000
+0.020000,7.469125,0.000000,7.479125,3.970313,-0.605469,-7.812500,0.000000
+0.020000,7.548379,0.000000,7.558379,3.955078,-0.761719,-7.812500,0.000000
+0.020000,7.627297,0.000000,7.637297,3.936719,-0.917969,-7.812500,0.000000
+0.020000,7.705816,0.000000,7.715816,3.915234,-1.074219,-7.812500,0.000000
+0.020000,7.783875,0.000000,7.793875,3.890625,-1.230469,-7.812500,0.000000
+0.020000,7.861410,0.000000,7.871410,3.862891,-1.386719,-7.812500,0.000000
+0.020000,7.938359,0.000000,7.948359,3.832031,-1.542969,-7.812500,0.000000
+0.020000,8.014660,0.000000,8.024660,3.798047,-1.699219,-7.812500,0.000000
+0.020000,8.090250,0.000000,8.100250,3.760938,-1.855469,-7.812500,0.000000
+0.020000,8.165066,0.000000,8.175066,3.720703,-2.011719,-7.812500,0.000000
+0.020000,8.239047,0.000000,8.249047,3.677344,-2.167969,-7.812500,0.000000
+0.020000,8.312129,0.000000,8.322129,3.630859,-2.324219,-7.812500,0.000000
+0.020000,8.384250,0.000000,8.394250,3.581250,-2.480469,-7.812500,0.000000
+0.020000,8.455348,0.000000,8.465348,3.528516,-2.636719,-7.812500,0.000000
+0.020000,8.525359,0.000000,8.535359,3.472656,-2.792969,-7.812500,0.000000
+0.020000,8.594223,0.000000,8.604223,3.413672,-2.949219,-7.812500,0.000000
+0.020000,8.661875,0.000000,8.671875,3.351563,-3.105469,-7.812500,0.000000
+0.020000,8.728254,0.000000,8.738254,3.286328,-3.261719,-7.812500,0.000000
+0.020000,8.793297,0.000000,8.803297,3.217969,-3.417969,-7.812500,0.000000
+0.020000,8.856941,0.000000,8.866941,3.146484,-3.574219,-7.812500,0.000000
+0.020000,8.919125,0.000000,8.929125,3.071875,-3.730469,-7.812500,0.000000
+0.020000,8.979785,0.000000,8.989785,2.994141,-3.886719,-7.812500,0.000000
+0.020000,9.038859,0.000000,9.048859,2.913281,-4.042969,-7.812500,0.000000
+0.020000,9.096285,0.000000,9.106285,2.829297,-4.199219,-7.812500,0.000000
+0.020000,9.152000,0.000000,9.162000,2.742188,-4.355469,-7.812500,0.000000
+0.020000,9.205941,0.000000,9.215941,2.651953,-4.511719,-7.812500,0.000000
+0.020000,9.258047,0.000000,9.268047,2.558594,-4.667969,-7.812500,0.000000
+0.020000,9.308254,0.000000,9.318254,2.462109,-4.824219,-7.812500,0.000000
+0.020000,9.356500,0.000000,9.366500,2.362500,-4.980469,-7.812500,0.000000
+0.020000,9.402750,0.000000,9.412750,2.262500,-5.000000,-0.976563,0.000000
+0.020000,9.447000,0.000000,9.457000,2.162500,-5.000000,0.000000,0.000000
+0.020000,9.489250,0.000000,9.499250,2.062500,-5.000000,0.000000,0.000000
+0.020000,9.529500,0.000000,9.539500,1.962500,-5.000000,0.000000,0.000000
+0.020000,9.567750,0.000000,9.577750,1.862500,-5.000000,0.000000,0.000000
+0.020000,9.604000,0.000000,9.614000,1.762500,-5.000000,-0.000000,0.000000
+0.020000,9.638250,0.000000,9.648250,1.662500,-5.000000,-0.000000,0.000000
+0.020000,9.670500,0.000000,9.680500,1.562500,-5.000000,0.000000,0.000000
+0.020000,9.700777,0.000000,9.710777,1.465234,-4.863281,6.835937,0.000000
+0.020000,9.729141,0.000000,9.739141,1.371094,-4.707031,7.812500,0.000000
+0.020000,9.755652,0.000000,9.765652,1.280078,-4.550781,7.812500,0.000000
+0.020000,9.780375,0.000000,9.790375,1.192188,-4.394531,7.812500,0.000000
+0.020000,9.803371,0.000000,9.813371,1.107422,-4.238281,7.812500,0.000000
+0.020000,9.824703,0.000000,9.834703,1.025781,-4.082031,7.812500,0.000000
+0.020000,9.844434,0.000000,9.854434,0.947266,-3.925781,7.812500,0.000000
+0.020000,9.862625,0.000000,9.872625,0.871875,-3.769531,7.812500,0.000000
+0.020000,9.879340,0.000000,9.889340,0.799609,-3.613281,7.812500,0.000000
+0.020000,9.894641,0.000000,9.904641,0.730469,-3.457031,7.812500,0.000000
+0.020000,9.908590,0.000000,9.918590,0.664453,-3.300781,7.812500,0.000000
+0.020000,9.921250,0.000000,9.931250,0.601563,-3.144531,7.812500,0.000000
+0.020000,9.932684,0.000000,9.942684,0.541797,-2.988281,7.812500,0.000000
+0.020000,9.942953,0.000000,9.952953,0.485156,-2.832031,7.812500,0.000000
+0.020000,9.952121,0.000000,9.962121,0.431641,-2.675781,7.812500,0.000000
+0.020000,9.960250,0.000000,9.970250,0.381250,-2.519531,7.812500,0.000000
+0.020000,9.967402,0.000000,9.977402,0.333984,-2.363281,7.812500,0.000000
+0.020000,9.973641,0.000000,9.983641,0.289844,-2.207031,7.812500,0.000000
+0.020000,9.979027,0.000000,9.989027,0.248828,-2.050781,7.812500,0.000000
+0.020000,9.983625,0.000000,9.993625,0.210938,-1.894531,7.812500,0.000000
+0.020000,9.987496,0.000000,9.997496,0.176172,-1.738281,7.812500,0.000000
+0.020000,9.990703,0.000000,10.000703,0.144531,-1.582031,7.812500,0.000000
+0.020000,9.993309,0.000000,10.003309,0.116016,-1.425781,7.812500,0.000000
+0.020000,9.995375,0.000000,10.005375,0.090625,-1.269531,7.812500,0.000000
+0.020000,9.996965,0.000000,10.006965,0.068359,-1.113281,7.812500,0.000000
+0.020000,9.998141,0.000000,10.008141,0.049219,-0.957031,7.812500,0.000000
+0.020000,9.998965,0.000000,10.008965,0.033203,-0.800781,7.812500,0.000000
+0.020000,9.999500,0.000000,10.009500,0.020313,-0.644531,7.812500,0.000000
+0.020000,9.999809,0.000000,10.009809,0.010547,-0.488281,7.812500,0.000000
+0.020000,9.999953,0.000000,10.009953,0.003906,-0.332031,7.812500,0.000000
+0.020000,9.999996,0.000000,10.009996,0.000391,-0.175781,7.812500,0.000000
+0.020000,10.000000,0.000000,10.010000,0.000000,-0.019531,7.812500,0.000000
+0.020000,10.000000,0.000000,10.010000,0.000000,0.000000,0.976563,0.000000
diff --git a/src/main/deploy/output/StraightTen.right.pf1.csv b/src/main/deploy/output/StraightTen.right.pf1.csv
new file mode 100644
index 0000000..b72cf7a
--- /dev/null
+++ b/src/main/deploy/output/StraightTen.right.pf1.csv
@@ -0,0 +1,199 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,-1.000000,0.000031,0.003125,0.156250,7.812500,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,-0.156250,-15.625000,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,7.812500,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,-1.000000,0.000031,0.000000,0.000000,0.000000,0.000000
+0.020000,0.002031,-1.000000,0.002063,0.101563,5.078125,253.906250,0.000000
+0.020000,0.005812,-1.000000,0.005844,0.189062,4.375000,-35.156250,0.000000
+0.020000,0.010313,-1.000000,0.010344,0.225000,1.796875,-128.906250,0.000000
+0.020000,0.015594,-1.000000,0.015625,0.264062,1.953125,7.812500,0.000000
+0.020000,0.021719,-1.000000,0.021750,0.306250,2.109375,7.812500,0.000000
+0.020000,0.028750,-1.000000,0.028781,0.351562,2.265625,7.812500,0.000000
+0.020000,0.036750,-1.000000,0.036781,0.400000,2.421875,7.812500,0.000000
+0.020000,0.045781,-1.000000,0.045812,0.451562,2.578125,7.812500,0.000000
+0.020000,0.055906,-1.000000,0.055938,0.506250,2.734375,7.812500,0.000000
+0.020000,0.067188,-1.000000,0.067219,0.564063,2.890625,7.812500,0.000000
+0.020000,0.079687,-1.000000,0.079719,0.625000,3.046875,7.812500,0.000000
+0.020000,0.093469,-1.000000,0.093500,0.689062,3.203125,7.812500,0.000000
+0.020000,0.108594,-1.000000,0.108625,0.756250,3.359375,7.812500,0.000000
+0.020000,0.125125,-1.000000,0.125156,0.826562,3.515625,7.812500,0.000000
+0.020000,0.143125,-1.000000,0.143156,0.900000,3.671875,7.812500,0.000000
+0.020000,0.162656,-1.000000,0.162687,0.976562,3.828125,7.812500,0.000000
+0.020000,0.183781,-1.000000,0.183812,1.056250,3.984375,7.812500,0.000000
+0.020000,0.206562,-1.000000,0.206594,1.139062,4.140625,7.812500,0.000000
+0.020000,0.231062,-1.000000,0.231094,1.225000,4.296875,7.812500,0.000000
+0.020000,0.257344,-1.000000,0.257375,1.314062,4.453125,7.812500,0.000000
+0.020000,0.285469,-1.000000,0.285500,1.406250,4.609375,7.812500,0.000000
+0.020000,0.315500,-1.000000,0.315531,1.501562,4.765625,7.812500,0.000000
+0.020000,0.347500,-1.000000,0.347531,1.600000,4.921875,7.812500,0.000000
+0.020000,0.381500,-1.000000,0.381531,1.700000,5.000000,3.906250,0.000000
+0.020000,0.417500,-1.000000,0.417531,1.800000,5.000000,0.000000,0.000000
+0.020000,0.455500,-1.000000,0.455531,1.900000,5.000000,-0.000000,0.000000
+0.020000,0.495500,-1.000000,0.495531,2.000000,5.000000,0.000000,0.000000
+0.020000,0.537500,-1.000000,0.537531,2.100000,5.000000,-0.000000,0.000000
+0.020000,0.581500,-1.000000,0.581531,2.200000,5.000000,0.000000,0.000000
+0.020000,0.627500,-1.000000,0.627531,2.300000,5.000000,-0.000000,0.000000
+0.020000,0.675500,-1.000000,0.675531,2.400000,5.000000,0.000000,0.000000
+0.020000,0.725469,-1.000000,0.725500,2.498437,4.921875,-3.906250,0.000000
+0.020000,0.777344,-1.000000,0.777375,2.593750,4.765625,-7.812500,0.000000
+0.020000,0.831062,-1.000000,0.831094,2.685937,4.609375,-7.812500,0.000000
+0.020000,0.886562,-1.000000,0.886594,2.775000,4.453125,-7.812500,0.000000
+0.020000,0.943781,-1.000000,0.943812,2.860938,4.296875,-7.812500,0.000000
+0.020000,1.002656,-1.000000,1.002687,2.943750,4.140625,-7.812500,0.000000
+0.020000,1.063125,-1.000000,1.063156,3.023437,3.984375,-7.812500,0.000000
+0.020000,1.125125,-1.000000,1.125156,3.100000,3.828125,-7.812500,0.000000
+0.020000,1.188594,-1.000000,1.188625,3.173437,3.671875,-7.812500,0.000000
+0.020000,1.253469,-1.000000,1.253500,3.243750,3.515625,-7.812500,0.000000
+0.020000,1.319687,-1.000000,1.319719,3.310937,3.359375,-7.812500,0.000000
+0.020000,1.387187,-1.000000,1.387219,3.375000,3.203125,-7.812500,0.000000
+0.020000,1.455906,-1.000000,1.455937,3.435937,3.046875,-7.812500,0.000000
+0.020000,1.525781,-1.000000,1.525812,3.493750,2.890625,-7.812500,0.000000
+0.020000,1.596750,-1.000000,1.596781,3.548438,2.734375,-7.812500,0.000000
+0.020000,1.668750,-1.000000,1.668781,3.600000,2.578125,-7.812500,0.000000
+0.020000,1.741719,-1.000000,1.741750,3.648437,2.421875,-7.812500,0.000000
+0.020000,1.815594,-1.000000,1.815625,3.693750,2.265625,-7.812500,0.000000
+0.020000,1.890312,-1.000000,1.890344,3.735937,2.109375,-7.812500,0.000000
+0.020000,1.965812,-1.000000,1.965844,3.775000,1.953125,-7.812500,0.000000
+0.020000,2.042031,-1.000000,2.042062,3.810937,1.796875,-7.812500,0.000000
+0.020000,2.118906,-1.000000,2.118937,3.843750,1.640625,-7.812500,0.000000
+0.020000,2.196375,-1.000000,2.196406,3.873438,1.484375,-7.812500,0.000000
+0.020000,2.274375,-1.000000,2.274406,3.900000,1.328125,-7.812500,0.000000
+0.020000,2.352844,-1.000000,2.352875,3.923437,1.171875,-7.812500,0.000000
+0.020000,2.431719,-1.000000,2.431750,3.943750,1.015625,-7.812500,0.000000
+0.020000,2.510937,-1.000000,2.510969,3.960937,0.859375,-7.812500,0.000000
+0.020000,2.590437,-1.000000,2.590469,3.975000,0.703125,-7.812500,0.000000
+0.020000,2.670156,-1.000000,2.670187,3.985938,0.546875,-7.812500,0.000000
+0.020000,2.750031,-1.000000,2.750062,3.993750,0.390625,-7.812500,0.000000
+0.020000,2.830000,-1.000000,2.830031,3.998437,0.234375,-7.812500,0.000000
+0.020000,2.910000,-1.000000,2.910031,4.000000,0.078125,-7.812500,0.000000
+0.020000,2.990000,-1.000000,2.990031,4.000000,0.000000,-3.906250,0.000000
+0.020000,3.070000,-1.000000,3.070031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.150000,-1.000000,3.150031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.230000,-1.000000,3.230031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.310000,-1.000000,3.310031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.390000,-1.000000,3.390031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.470000,-1.000000,3.470031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,3.550000,-1.000000,3.550031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.630000,-1.000000,3.630031,4.000000,0.000000,-0.000000,0.000000
+0.020000,3.710000,-1.000000,3.710031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.790000,-1.000000,3.790031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.870000,-1.000000,3.870031,4.000000,0.000000,0.000000,0.000000
+0.020000,3.950000,-1.000000,3.950031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.030000,-1.000000,4.030031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.110000,-1.000000,4.110031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,4.190000,-1.000000,4.190031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.270000,-1.000000,4.270031,4.000000,0.000000,-0.000000,0.000000
+0.020000,4.350000,-1.000000,4.350031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.430000,-1.000000,4.430031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.510000,-1.000000,4.510031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.590000,-1.000000,4.590031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.670000,-1.000000,4.670031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.750000,-1.000000,4.750031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.830000,-1.000000,4.830031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.910000,-1.000000,4.910031,4.000000,0.000000,0.000000,0.000000
+0.020000,4.990000,-1.000000,4.990031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.070000,-1.000000,5.070031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.150000,-1.000000,5.150031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.230000,-1.000000,5.230031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.310000,-1.000000,5.310031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.390000,-1.000000,5.390031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.470000,-1.000000,5.470031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.550000,-1.000000,5.550031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.630000,-1.000000,5.630031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,5.710000,-1.000000,5.710031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.790000,-1.000000,5.790031,4.000000,0.000000,-0.000000,0.000000
+0.020000,5.870000,-1.000000,5.870031,4.000000,0.000000,0.000000,0.000000
+0.020000,5.950000,-1.000000,5.950031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.030000,-1.000000,6.030031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.110000,-1.000000,6.110031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.190000,-1.000000,6.190031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.270000,-1.000000,6.270031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.350000,-1.000000,6.350031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.430000,-1.000000,6.430031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.510000,-1.000000,6.510031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.590000,-1.000000,6.590031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.670000,-1.000000,6.670031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.750000,-1.000000,6.750031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.830000,-1.000000,6.830031,4.000000,0.000000,0.000000,0.000000
+0.020000,6.910000,-1.000000,6.910031,4.000000,-0.000000,-0.000000,0.000000
+0.020000,6.990000,-1.000000,6.990031,4.000000,0.000000,0.000000,0.000000
+0.020000,7.070000,-1.000000,7.070031,4.000000,0.000000,-0.000000,0.000000
+0.020000,7.150000,-1.000000,7.150031,4.000000,0.000000,0.000000,0.000000
+0.020000,7.229973,-1.000000,7.230004,3.998633,-0.068359,-3.417969,0.000000
+0.020000,7.309859,-1.000000,7.309891,3.994336,-0.214844,-7.324219,0.000000
+0.020000,7.389598,-1.000000,7.389629,3.986914,-0.371094,-7.812500,0.000000
+0.020000,7.469125,-1.000000,7.469156,3.976367,-0.527344,-7.812500,0.000000
+0.020000,7.548379,-1.000000,7.548410,3.962695,-0.683594,-7.812500,0.000000
+0.020000,7.627297,-1.000000,7.627328,3.945898,-0.839844,-7.812500,0.000000
+0.020000,7.705816,-1.000000,7.705848,3.925977,-0.996094,-7.812500,0.000000
+0.020000,7.783875,-1.000000,7.783906,3.902930,-1.152344,-7.812500,0.000000
+0.020000,7.861410,-1.000000,7.861441,3.876758,-1.308594,-7.812500,0.000000
+0.020000,7.938359,-1.000000,7.938391,3.847461,-1.464844,-7.812500,0.000000
+0.020000,8.014660,-1.000000,8.014691,3.815039,-1.621094,-7.812500,0.000000
+0.020000,8.090250,-1.000000,8.090281,3.779492,-1.777344,-7.812500,0.000000
+0.020000,8.165066,-1.000000,8.165098,3.740820,-1.933594,-7.812500,0.000000
+0.020000,8.239047,-1.000000,8.239078,3.699023,-2.089844,-7.812500,0.000000
+0.020000,8.312129,-1.000000,8.312160,3.654102,-2.246094,-7.812500,0.000000
+0.020000,8.384250,-1.000000,8.384281,3.606055,-2.402344,-7.812500,0.000000
+0.020000,8.455348,-1.000000,8.455379,3.554883,-2.558594,-7.812500,0.000000
+0.020000,8.525359,-1.000000,8.525391,3.500586,-2.714844,-7.812500,0.000000
+0.020000,8.594223,-1.000000,8.594254,3.443164,-2.871094,-7.812500,0.000000
+0.020000,8.661875,-1.000000,8.661906,3.382617,-3.027344,-7.812500,0.000000
+0.020000,8.728254,-1.000000,8.728285,3.318945,-3.183594,-7.812500,0.000000
+0.020000,8.793297,-1.000000,8.793328,3.252148,-3.339844,-7.812500,0.000000
+0.020000,8.856941,-1.000000,8.856973,3.182227,-3.496094,-7.812500,0.000000
+0.020000,8.919125,-1.000000,8.919156,3.109180,-3.652344,-7.812500,0.000000
+0.020000,8.979785,-1.000000,8.979816,3.033008,-3.808594,-7.812500,0.000000
+0.020000,9.038859,-1.000000,9.038891,2.953711,-3.964844,-7.812500,0.000000
+0.020000,9.096285,-1.000000,9.096316,2.871289,-4.121094,-7.812500,0.000000
+0.020000,9.152000,-1.000000,9.152031,2.785742,-4.277344,-7.812500,0.000000
+0.020000,9.205941,-1.000000,9.205973,2.697070,-4.433594,-7.812500,0.000000
+0.020000,9.258047,-1.000000,9.258078,2.605273,-4.589844,-7.812500,0.000000
+0.020000,9.308254,-1.000000,9.308285,2.510352,-4.746094,-7.812500,0.000000
+0.020000,9.356500,-1.000000,9.356531,2.412305,-4.902344,-7.812500,0.000000
+0.020000,9.402750,-1.000000,9.402781,2.312500,-4.990234,-4.394531,0.000000
+0.020000,9.447000,-1.000000,9.447031,2.212500,-5.000000,-0.488281,0.000000
+0.020000,9.489250,-1.000000,9.489281,2.112500,-5.000000,0.000000,0.000000
+0.020000,9.529500,-1.000000,9.529531,2.012500,-5.000000,0.000000,0.000000
+0.020000,9.567750,-1.000000,9.567781,1.912500,-5.000000,-0.000000,0.000000
+0.020000,9.604000,-1.000000,9.604031,1.812500,-5.000000,0.000000,0.000000
+0.020000,9.638250,-1.000000,9.638281,1.712500,-5.000000,-0.000000,0.000000
+0.020000,9.670500,-1.000000,9.670531,1.612500,-5.000000,0.000000,0.000000
+0.020000,9.700777,-1.000000,9.700809,1.513867,-4.931641,3.417969,0.000000
+0.020000,9.729141,-1.000000,9.729172,1.418164,-4.785156,7.324219,0.000000
+0.020000,9.755652,-1.000000,9.755684,1.325586,-4.628906,7.812500,0.000000
+0.020000,9.780375,-1.000000,9.780406,1.236133,-4.472656,7.812500,0.000000
+0.020000,9.803371,-1.000000,9.803402,1.149805,-4.316406,7.812500,0.000000
+0.020000,9.824703,-1.000000,9.824734,1.066602,-4.160156,7.812500,0.000000
+0.020000,9.844434,-1.000000,9.844465,0.986523,-4.003906,7.812500,0.000000
+0.020000,9.862625,-1.000000,9.862656,0.909570,-3.847656,7.812500,0.000000
+0.020000,9.879340,-1.000000,9.879371,0.835742,-3.691406,7.812500,0.000000
+0.020000,9.894641,-1.000000,9.894672,0.765039,-3.535156,7.812500,0.000000
+0.020000,9.908590,-1.000000,9.908621,0.697461,-3.378906,7.812500,0.000000
+0.020000,9.921250,-1.000000,9.921281,0.633008,-3.222656,7.812500,0.000000
+0.020000,9.932684,-1.000000,9.932715,0.571680,-3.066406,7.812500,0.000000
+0.020000,9.942953,-1.000000,9.942984,0.513477,-2.910156,7.812500,0.000000
+0.020000,9.952121,-1.000000,9.952152,0.458398,-2.753906,7.812500,0.000000
+0.020000,9.960250,-1.000000,9.960281,0.406445,-2.597656,7.812500,0.000000
+0.020000,9.967402,-1.000000,9.967434,0.357617,-2.441406,7.812500,0.000000
+0.020000,9.973641,-1.000000,9.973672,0.311914,-2.285156,7.812500,0.000000
+0.020000,9.979027,-1.000000,9.979059,0.269336,-2.128906,7.812500,0.000000
+0.020000,9.983625,-1.000000,9.983656,0.229883,-1.972656,7.812500,0.000000
+0.020000,9.987496,-1.000000,9.987527,0.193555,-1.816406,7.812500,0.000000
+0.020000,9.990703,-1.000000,9.990734,0.160352,-1.660156,7.812500,0.000000
+0.020000,9.993309,-1.000000,9.993340,0.130273,-1.503906,7.812500,0.000000
+0.020000,9.995375,-1.000000,9.995406,0.103320,-1.347656,7.812500,0.000000
+0.020000,9.996965,-1.000000,9.996996,0.079492,-1.191406,7.812500,0.000000
+0.020000,9.998141,-1.000000,9.998172,0.058789,-1.035156,7.812500,0.000000
+0.020000,9.998965,-1.000000,9.998996,0.041211,-0.878906,7.812500,0.000000
+0.020000,9.999500,-1.000000,9.999531,0.026758,-0.722656,7.812500,0.000000
+0.020000,9.999809,-1.000000,9.999840,0.015430,-0.566406,7.812500,0.000000
+0.020000,9.999953,-1.000000,9.999984,0.007227,-0.410156,7.812500,0.000000
+0.020000,9.999996,-1.000000,10.000027,0.002148,-0.253906,7.812500,0.000000
+0.020000,10.000000,-1.000000,10.000031,0.000195,-0.097656,7.812500,0.000000
+0.020000,10.000000,-1.000000,10.000031,0.000000,-0.009766,4.394531,0.000000
diff --git a/src/main/deploy/output/Unnamed.left.pf1.csv b/src/main/deploy/output/Unnamed.left.pf1.csv
new file mode 100644
index 0000000..cfaaecb
--- /dev/null
+++ b/src/main/deploy/output/Unnamed.left.pf1.csv
@@ -0,0 +1,258 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.813291,2.032195,0.000031,0.003125,0.156250,7.812500,6.282238
+0.020000,0.813284,2.032195,0.000038,0.000330,-0.139734,-14.799206,6.282244
+0.020000,0.813269,2.032195,0.000053,0.000743,0.020646,8.018991,6.282259
+0.020000,0.813243,2.032195,0.000079,0.001321,0.028906,0.413032,6.282286
+0.020000,0.813202,2.032195,0.000120,0.002065,0.037171,0.413214,6.282327
+0.020000,0.813142,2.032196,0.000180,0.002974,0.045441,0.413511,6.282386
+0.020000,0.813061,2.032196,0.000261,0.004048,0.053720,0.413953,6.282467
+0.020000,0.812955,2.032196,0.000367,0.005288,0.062011,0.414569,6.282573
+0.020000,0.812821,2.032196,0.000501,0.006695,0.070319,0.415389,6.282707
+0.020000,0.812656,2.032196,0.000666,0.008268,0.078648,0.416440,6.282872
+0.020000,0.812456,2.032196,0.000866,0.010008,0.087003,0.417754,6.283073
+0.020000,0.814590,2.032196,0.003000,0.106711,4.835166,237.408140,0.000126
+0.020000,0.819593,2.032197,0.008003,0.250118,7.170366,116.760029,0.000406
+0.020000,0.825392,2.032201,0.013802,0.289977,1.992950,-258.870815,0.000731
+0.020000,0.832050,2.032207,0.020460,0.332875,2.144887,7.596866,0.001104
+0.020000,0.839625,2.032217,0.028035,0.378772,2.294839,7.497580,0.001529
+0.020000,0.848174,2.032232,0.036584,0.427463,2.434574,6.986736,0.002010
+0.020000,0.857760,2.032254,0.046170,0.479298,2.591757,7.859187,0.002550
+0.020000,0.868439,2.032284,0.056849,0.533925,2.731350,6.979635,0.003153
+0.020000,0.880268,2.032325,0.068678,0.591463,2.876894,7.277206,0.003822
+0.020000,0.893311,2.032380,0.081721,0.652163,3.034989,7.904724,0.004561
+0.020000,0.907623,2.032451,0.096034,0.715609,3.172272,6.864164,0.005375
+0.020000,0.923263,2.032542,0.111674,0.782005,3.319824,7.377599,0.006266
+0.020000,0.940289,2.032657,0.128700,0.851339,3.466720,7.344789,0.007239
+0.020000,0.958760,2.032801,0.147172,0.923591,3.612566,7.292303,0.008298
+0.020000,0.978734,2.032978,0.167147,0.998735,3.757217,7.232557,0.009448
+0.020000,1.000270,2.033195,0.188684,1.076845,3.905513,7.414793,0.010692
+0.020000,1.023426,2.033458,0.211842,1.157888,4.052124,7.330572,0.012035
+0.020000,1.048257,2.033775,0.236674,1.241615,4.186371,6.712329,0.013482
+0.020000,1.074825,2.034154,0.263245,1.328561,4.347308,8.046865,0.015038
+0.020000,1.103184,2.034604,0.291607,1.418092,4.476526,6.460908,0.016707
+0.020000,1.133390,2.035135,0.321818,1.510569,4.623843,7.365842,0.018495
+0.020000,1.165474,2.035759,0.353908,1.604479,4.695508,3.583219,0.020405
+0.020000,1.199432,2.036487,0.387874,1.698308,4.691453,-0.202725,0.022439
+0.020000,1.235263,2.037330,0.423714,1.792017,4.685443,-0.300493,0.024599
+0.020000,1.272961,2.038300,0.461426,1.885566,4.677454,-0.399448,0.026889
+0.020000,1.312524,2.039412,0.501004,1.978915,4.667463,-0.499542,0.029309
+0.020000,1.353952,2.040680,0.542451,2.072369,4.672701,0.261869,0.031863
+0.020000,1.397236,2.042118,0.585760,2.165416,4.652328,-1.018622,0.034553
+0.020000,1.442378,2.043742,0.630930,2.258515,4.654963,0.131753,0.037383
+0.020000,1.489341,2.045568,0.677929,2.349935,4.570975,-4.199398,0.040354
+0.020000,1.538062,2.047611,0.726693,2.438211,4.413800,-7.858771,0.043465
+0.020000,1.588479,2.049886,0.777161,2.523383,4.258638,-7.758101,0.046715
+0.020000,1.640528,2.052407,0.829271,2.605512,4.106423,-7.610767,0.050105
+0.020000,1.694147,2.055191,0.882962,2.684553,3.952068,-7.717736,0.053634
+0.020000,1.749268,2.058251,0.938168,2.760318,3.788224,-8.192185,0.057301
+0.020000,1.805833,2.061604,0.994833,2.833223,3.645286,-7.146920,0.061106
+0.020000,1.863773,2.065263,1.052888,2.902777,3.477687,-8.379956,0.065050
+0.020000,1.923027,2.069244,1.112276,2.969369,3.329578,-7.405447,0.069131
+0.020000,1.983531,2.073562,1.172933,3.032865,3.174838,-7.736982,0.073349
+0.020000,2.045218,2.078229,1.234797,3.093182,3.015827,-7.950559,0.077704
+0.020000,2.108025,2.083261,1.297805,3.150404,2.861095,-7.736576,0.082196
+0.020000,2.171886,2.088671,1.361895,3.204515,2.705567,-7.776441,0.086824
+0.020000,2.236739,2.094471,1.427007,3.255579,2.553202,-7.618240,0.091587
+0.020000,2.302519,2.100674,1.493078,3.303568,2.399435,-7.688347,0.096486
+0.020000,2.369157,2.107293,1.560045,3.348333,2.238279,-8.057779,0.101518
+0.020000,2.436593,2.114338,1.627847,3.390121,2.089392,-7.444387,0.106685
+0.020000,2.504759,2.121820,1.696423,3.428780,1.932960,-7.821580,0.111984
+0.020000,2.573592,2.129750,1.765711,3.464399,1.780946,-7.600688,0.117415
+0.020000,2.643024,2.138135,1.835647,3.496835,1.621766,-7.958996,0.122977
+0.020000,2.712994,2.146985,1.906175,3.526395,1.478012,-7.187710,0.128668
+0.020000,2.783436,2.156307,1.977231,3.552779,1.319188,-7.941195,0.134488
+0.020000,2.854283,2.166107,2.048753,3.576105,1.166313,-7.643753,0.140435
+0.020000,2.925472,2.176391,2.120681,3.596409,1.015198,-7.555766,0.146506
+0.020000,2.996939,2.187163,2.192955,3.613693,0.864207,-7.549558,0.152701
+0.020000,3.068619,2.198425,2.265514,3.627962,0.713449,-7.537875,0.159017
+0.020000,3.140448,2.210181,2.338299,3.639223,0.563031,-7.520911,0.165451
+0.020000,3.212362,2.222431,2.411248,3.647484,0.413053,-7.498886,0.172001
+0.020000,3.284297,2.235174,2.484303,3.652756,0.263612,-7.472048,0.178665
+0.020000,3.356190,2.248408,2.557404,3.655052,0.114799,-7.440670,0.185438
+0.020000,3.427978,2.262130,2.630492,3.654386,-0.033302,-7.405052,0.192318
+0.020000,3.499600,2.276335,2.703510,3.650882,-0.175197,-7.094715,0.199301
+0.020000,3.571022,2.291024,2.776426,3.645800,-0.254117,-3.946000,0.206385
+0.020000,3.642235,2.306200,2.849238,3.640613,-0.259335,-0.260935,0.213573
+0.020000,3.713233,2.321869,2.921945,3.635348,-0.263253,-0.195891,0.220864
+0.020000,3.784014,2.338036,2.994549,3.630178,-0.258481,0.238588,0.228262
+0.020000,3.854567,2.354704,3.067044,3.624777,-0.270060,-0.578952,0.235766
+0.020000,3.924886,2.371880,3.139430,3.619293,-0.274219,-0.207955,0.243379
+0.020000,3.994967,2.389568,3.211709,3.613943,-0.267473,0.337345,0.251101
+0.020000,4.064800,2.407772,3.283875,3.608315,-0.281407,-0.696736,0.258934
+0.020000,4.134379,2.426498,3.355930,3.602746,-0.278456,0.147544,0.266879
+0.020000,4.203697,2.445749,3.427872,3.597088,-0.282889,-0.221625,0.274937
+0.020000,4.272746,2.465531,3.499699,3.591356,-0.286604,-0.185762,0.283110
+0.020000,4.341521,2.485847,3.571411,3.585622,-0.286709,-0.005258,0.291397
+0.020000,4.410011,2.506703,3.643007,3.579787,-0.291746,-0.251811,0.299801
+0.020000,4.478211,2.528103,3.714486,3.573934,-0.292627,-0.044066,0.308322
+0.020000,4.546113,2.550051,3.785847,3.568055,-0.293989,-0.068085,0.316961
+0.020000,4.613708,2.572551,3.857088,3.562050,-0.300230,-0.312089,0.325719
+0.020000,4.680991,2.595608,3.928211,3.556181,-0.293447,0.339189,0.334596
+0.020000,4.747950,2.619225,3.999214,3.550119,-0.303124,-0.483852,0.343593
+0.020000,4.814579,2.643406,4.070095,3.544076,-0.302146,0.048866,0.352711
+0.020000,4.880872,2.668156,4.140857,3.538101,-0.298754,0.169611,0.361949
+0.020000,4.946818,2.693478,4.211498,3.532005,-0.304760,-0.300283,0.371309
+0.020000,5.012409,2.719375,4.282016,3.525914,-0.304588,0.008577,0.380789
+0.020000,5.077638,2.745851,4.352413,3.519882,-0.301569,0.150952,0.390392
+0.020000,5.142497,2.772910,4.422690,3.513842,-0.301991,-0.021088,0.400115
+0.020000,5.206976,2.800554,4.492846,3.507779,-0.303172,-0.059071,0.409959
+0.020000,5.271068,2.828787,4.562881,3.501742,-0.301852,0.066022,0.419924
+0.020000,5.334764,2.857612,4.632795,3.495738,-0.300197,0.082739,0.430008
+0.020000,5.398057,2.887030,4.702591,3.489774,-0.298186,0.100559,0.440212
+0.020000,5.460938,2.917046,4.772268,3.483877,-0.294873,0.165636,0.450534
+0.020000,5.523398,2.947662,4.841829,3.478025,-0.292615,0.112918,0.460974
+0.020000,5.585431,2.978879,4.911273,3.472220,-0.290216,0.119945,0.471529
+0.020000,5.647026,3.010701,4.980603,3.466490,-0.286528,0.184395,0.482199
+0.020000,5.708177,3.043128,5.049820,3.460842,-0.282382,0.207286,0.492982
+0.020000,5.768876,3.076163,5.118926,3.455287,-0.277762,0.231024,0.503876
+0.020000,5.829113,3.109808,5.187922,3.449834,-0.272651,0.255532,0.514879
+0.020000,5.888883,3.144063,5.256812,3.444493,-0.267037,0.280721,0.525988
+0.020000,5.948176,3.178930,5.325598,3.439275,-0.260907,0.306492,0.537202
+0.020000,6.006986,3.214411,5.394282,3.434190,-0.254252,0.332734,0.548518
+0.020000,6.065305,3.250506,5.462867,3.429249,-0.247066,0.359328,0.559932
+0.020000,6.123125,3.287215,5.531356,3.424480,-0.238421,0.432239,0.571443
+0.020000,6.180440,3.324540,5.599753,3.419862,-0.230910,0.375555,0.583045
+0.020000,6.237243,3.362481,5.668062,3.415416,-0.222289,0.431029,0.594737
+0.020000,6.293526,3.401037,5.736285,3.411156,-0.212992,0.464854,0.606513
+0.020000,6.349283,3.440209,5.804427,3.407093,-0.203169,0.491179,0.618371
+0.020000,6.404508,3.479997,5.872491,3.403236,-0.192829,0.516976,0.630305
+0.020000,6.459194,3.520399,5.940483,3.399597,-0.181988,0.542079,0.642313
+0.020000,6.513335,3.561417,6.008407,3.396200,-0.169836,0.607557,0.654388
+0.020000,6.566926,3.603047,6.076268,3.393027,-0.158667,0.558479,0.666528
+0.020000,6.619960,3.645290,6.144070,3.390090,-0.146841,0.591311,0.678725
+0.020000,6.672432,3.688145,6.211818,3.387405,-0.134216,0.631211,0.690977
+0.020000,6.724336,3.731609,6.279517,3.384981,-0.121206,0.650500,0.703276
+0.020000,6.775669,3.775681,6.347174,3.382834,-0.107350,0.692828,0.715619
+0.020000,6.826425,3.820360,6.414793,3.380950,-0.094203,0.657351,0.728000
+0.020000,6.876599,3.865643,6.482380,3.379341,-0.080485,0.685903,0.740413
+0.020000,6.926187,3.911528,6.549940,3.378015,-0.066295,0.709505,0.752852
+0.020000,6.975185,3.958012,6.617480,3.376977,-0.051876,0.720938,0.765312
+0.020000,7.023589,4.005093,6.685004,3.376228,-0.037480,0.719806,0.777787
+0.020000,7.071395,4.052768,6.752520,3.375767,-0.023003,0.723816,0.790271
+0.020000,7.118599,4.101034,6.820032,3.375602,-0.008280,0.736183,0.802758
+0.020000,7.165199,4.149888,6.887546,3.375731,0.006466,0.737270,0.815243
+0.020000,7.211191,4.199325,6.955069,3.376143,0.020575,0.705461,0.827720
+0.020000,7.256573,4.249342,7.022606,3.376853,0.035521,0.747296,0.840182
+0.020000,7.301342,4.299936,7.090163,3.377855,0.050102,0.729079,0.852625
+0.020000,7.345495,4.351102,7.157746,3.379145,0.064489,0.719353,0.865041
+0.020000,7.389031,4.402836,7.225360,3.380713,0.078382,0.694639,0.877427
+0.020000,7.431946,4.455132,7.293011,3.382546,0.091651,0.663414,0.889775
+0.020000,7.474240,4.507986,7.360705,3.384667,0.106085,0.721723,0.902081
+0.020000,7.515912,4.561394,7.428446,3.387056,0.119450,0.668254,0.914340
+0.020000,7.556958,4.615349,7.496240,3.389706,0.132463,0.650665,0.926545
+0.020000,7.597380,4.669847,7.564092,3.392607,0.145093,0.631484,0.938693
+0.020000,7.637175,4.724882,7.632007,3.395742,0.156735,0.582074,0.950778
+0.020000,7.676342,4.780447,7.699989,3.399108,0.168309,0.578717,0.962795
+0.020000,7.714882,4.836537,7.768043,3.402716,0.180397,0.604394,0.974740
+0.020000,7.752794,4.893145,7.836174,3.406542,0.191273,0.543806,0.986608
+0.020000,7.790078,4.950265,7.904386,3.410575,0.201655,0.519129,0.998396
+0.020000,7.826733,5.007891,7.972682,3.414805,0.211532,0.493833,1.010100
+0.020000,7.862761,5.066015,8.041066,3.419223,0.220894,0.468086,1.021715
+0.020000,7.898161,5.124631,8.109543,3.423818,0.229735,0.442053,1.033238
+0.020000,7.932935,5.183732,8.178114,3.428579,0.238053,0.415894,1.044666
+0.020000,7.967083,5.243309,8.246784,3.433496,0.245848,0.389764,1.055996
+0.020000,8.000606,5.303356,8.315555,3.438558,0.253124,0.363810,1.067224
+0.020000,8.033506,5.363866,8.384431,3.443756,0.259888,0.338171,1.078349
+0.020000,8.065783,5.424830,8.453412,3.449079,0.266147,0.312976,1.089367
+0.020000,8.097440,5.486241,8.522502,3.454517,0.271914,0.288346,1.100276
+0.020000,8.128478,5.548091,8.591704,3.460062,0.277202,0.264388,1.111075
+0.020000,8.158900,5.610373,8.661018,3.465702,0.282026,0.241201,1.121760
+0.020000,8.188707,5.673077,8.730446,3.471430,0.286403,0.218871,1.132332
+0.020000,8.217901,5.736196,8.799990,3.477165,0.286737,0.016705,1.142788
+0.020000,8.246486,5.799722,8.869650,3.483027,0.293098,0.318054,1.153126
+0.020000,8.274463,5.863646,8.939429,3.488952,0.296279,0.159015,1.163347
+0.020000,8.301837,5.927962,9.009328,3.494933,0.299020,0.137060,1.173449
+0.020000,8.328608,5.992660,9.079346,3.500904,0.298580,-0.021998,1.183430
+0.020000,8.354782,6.057731,9.149484,3.506888,0.299184,0.030204,1.193291
+0.020000,8.380360,6.123169,9.219743,3.512974,0.304322,0.256873,1.203032
+0.020000,8.405348,6.188965,9.290124,3.519045,0.303508,-0.040668,1.212651
+0.020000,8.429746,6.255109,9.360625,3.525039,0.299724,-0.189219,1.222149
+0.020000,8.453561,6.321597,9.431248,3.531177,0.306904,0.358992,1.231526
+0.020000,8.476794,6.388416,9.501992,3.537177,0.299987,-0.345810,1.240782
+0.020000,8.499451,6.455561,9.572857,3.543247,0.303504,0.175849,1.249917
+0.020000,8.521536,6.523024,9.643842,3.549276,0.301464,-0.102022,1.258931
+0.020000,8.543051,6.590796,9.714948,3.555263,0.299321,-0.107160,1.267825
+0.020000,8.564003,6.658870,9.786173,3.561251,0.299415,0.004722,1.276600
+0.020000,8.584394,6.727238,9.857517,3.567195,0.297190,-0.111279,1.285256
+0.020000,8.604229,6.795891,9.928978,3.573068,0.293671,-0.175936,1.293794
+0.020000,8.623514,6.864825,10.000558,3.579021,0.297640,0.198432,1.302215
+0.020000,8.642251,6.934027,10.072252,3.584702,0.284068,-0.678599,1.310519
+0.020000,8.660446,7.003495,10.144063,3.590549,0.292340,0.413618,1.318708
+0.020000,8.678104,7.073220,10.215990,3.596311,0.288102,-0.211905,1.326783
+0.020000,8.695229,7.143192,10.288027,3.601860,0.277455,-0.532348,1.334744
+0.020000,8.711827,7.213407,10.360177,3.607517,0.282846,0.269537,1.342593
+0.020000,8.727901,7.283859,10.432439,3.613104,0.279319,-0.176310,1.350332
+0.020000,8.743458,7.354539,10.504811,3.618601,0.274884,-0.221784,1.357960
+0.020000,8.758497,7.425424,10.577274,3.623150,0.227436,-2.372410,1.365479
+0.020000,8.773017,7.496466,10.649785,3.625540,0.119486,-5.397495,1.372884
+0.020000,8.787012,7.567603,10.722285,3.625000,-0.026971,-7.322846,1.380172
+0.020000,8.800480,7.638770,10.794715,3.621515,-0.174254,-7.364155,1.387339
+0.020000,8.813419,7.709904,10.867017,3.615070,-0.322270,-7.400763,1.394382
+0.020000,8.825829,7.780941,10.939130,3.605652,-0.470915,-7.432277,1.401297
+0.020000,8.837710,7.851817,11.010995,3.593250,-0.620082,-7.458341,1.408082
+0.020000,8.849067,7.922468,11.082552,3.577857,-0.769655,-7.478631,1.414734
+0.020000,8.859902,7.992827,11.153741,3.559467,-0.919512,-7.492855,1.421251
+0.020000,8.870221,8.062833,11.224503,3.538076,-1.069527,-7.500757,1.427630
+0.020000,8.880029,8.132415,11.294773,3.513501,-1.228766,-7.961942,1.433870
+0.020000,8.889335,8.201511,11.364493,3.486024,-1.373825,-7.252945,1.439970
+0.020000,8.898146,8.270059,11.433605,3.455579,-1.522260,-7.421794,1.445927
+0.020000,8.906471,8.337986,11.502040,3.421768,-1.690556,-8.414761,1.451741
+0.020000,8.914322,8.405234,11.569745,3.385251,-1.825865,-6.765457,1.457411
+0.020000,8.921708,8.471732,11.636652,3.345336,-1.995715,-8.492507,1.462935
+0.020000,8.928641,8.537418,11.702703,3.302538,-2.139932,-7.210855,1.468313
+0.020000,8.935135,8.602226,11.767835,3.256638,-2.294978,-7.752317,1.473543
+0.020000,8.941201,8.666091,11.831988,3.207604,-2.451696,-7.835885,1.478627
+0.020000,8.946855,8.728946,11.895097,3.155447,-2.607859,-7.808138,1.483561
+0.020000,8.952108,8.790727,11.957100,3.100181,-2.763301,-7.772124,1.488348
+0.020000,8.956978,8.851368,12.017937,3.041824,-2.917857,-7.727781,1.492985
+0.020000,8.961478,8.910806,12.077545,2.980397,-3.071358,-7.675075,1.497472
+0.020000,8.965623,8.968977,12.135863,2.915924,-3.223638,-7.613988,1.501811
+0.020000,8.969431,9.025812,12.192826,2.848140,-3.389188,-8.277465,1.505998
+0.020000,8.972916,9.081252,12.248375,2.777478,-3.533111,-7.196194,1.510036
+0.020000,8.976094,9.135228,12.302445,2.703467,-3.700533,-8.371071,1.513923
+0.020000,8.978982,9.187680,12.354976,2.626550,-3.845887,-7.267703,1.517659
+0.020000,8.981595,9.238542,12.405905,2.546459,-4.004535,-7.932427,1.521244
+0.020000,8.983951,9.287750,12.455169,2.463226,-4.161630,-7.854718,1.524677
+0.020000,8.986064,9.335241,12.502707,2.376887,-4.316980,-7.767493,1.527958
+0.020000,8.987952,9.380951,12.548457,2.287479,-4.470394,-7.670740,1.531086
+0.020000,8.989630,9.424836,12.592374,2.195855,-4.581166,-5.538596,1.534063
+0.020000,8.991116,9.466874,12.634438,2.103192,-4.633182,-2.600792,1.536890
+0.020000,8.992425,9.507066,12.674651,2.010664,-4.626378,0.340207,1.539571
+0.020000,8.993574,9.545402,12.713004,1.917659,-4.650242,-1.193181,1.542108
+0.020000,8.994577,9.581883,12.749499,1.824759,-4.645033,0.260414,1.544505
+0.020000,8.995448,9.616506,12.784133,1.731673,-4.654281,-0.462396,1.546764
+0.020000,8.996201,9.649266,12.816902,1.638447,-4.661291,-0.350470,1.548887
+0.020000,8.996847,9.680162,12.847804,1.545126,-4.666086,-0.239760,1.550877
+0.020000,8.997398,9.709208,12.876856,1.452565,-4.628039,1.902343,1.552737
+0.020000,8.997867,9.736443,12.904095,1.361954,-4.530525,4.875720,1.554471
+0.020000,8.998262,9.761930,12.929584,1.274494,-4.373015,7.875506,1.556086
+0.020000,8.998594,9.785721,12.953378,1.189657,-4.241848,6.558348,1.557587
+0.020000,8.998872,9.807877,12.975536,1.107903,-4.087712,7.706796,1.558978
+0.020000,8.999102,9.828454,12.996114,1.028911,-3.949575,6.906837,1.560264
+0.020000,8.999291,9.847507,13.015168,0.952727,-3.809218,7.017853,1.561451
+0.020000,8.999446,9.865098,13.032760,0.879555,-3.658598,7.530983,1.562543
+0.020000,8.999571,9.881283,13.048945,0.809275,-3.513998,7.230012,1.563544
+0.020000,8.999672,9.896121,13.063783,0.741897,-3.368885,7.255650,1.564460
+0.020000,8.999752,9.909666,13.077329,0.677290,-3.230351,6.926693,1.565293
+0.020000,8.999815,9.921981,13.089644,0.615755,-3.076763,7.679400,1.566049
+0.020000,8.999864,9.933125,13.100788,0.557195,-2.927981,7.439090,1.566731
+0.020000,8.999902,9.943152,13.110815,0.501359,-2.791802,6.808979,1.567344
+0.020000,8.999931,9.952124,13.119787,0.448607,-2.637610,7.709571,1.567891
+0.020000,8.999952,9.960098,13.127761,0.398705,-2.495121,7.124486,1.568377
+0.020000,8.999967,9.967135,13.134798,0.351834,-2.343548,7.578654,1.568805
+0.020000,8.999978,9.973289,13.140952,0.307698,-2.206780,6.838362,1.569179
+0.020000,8.999986,9.978623,13.146287,0.266733,-2.048239,7.927065,1.569503
+0.020000,8.999991,9.983196,13.150859,0.228641,-1.904617,7.181106,1.569780
+0.020000,8.999995,9.987062,13.154726,0.193308,-1.766662,6.897759,1.570014
+0.020000,8.999997,9.990285,13.157948,0.161109,-1.609958,7.835176,1.570209
+0.020000,8.999998,9.992921,13.160584,0.131834,-1.463738,7.310996,1.570368
+0.020000,8.999999,9.995031,13.162694,0.105490,-1.317205,7.326653,1.570496
+0.020000,9.000000,9.996673,13.164336,0.082081,-1.170418,7.339336,1.570595
+0.020000,9.000000,9.997905,13.165568,0.061613,-1.023431,7.349358,1.570670
+0.020000,9.000000,9.998787,13.166450,0.044087,-0.876291,7.357034,1.570723
+0.020000,9.000000,9.999377,13.167040,0.029506,-0.729037,7.362676,1.570759
+0.020000,9.000000,9.999734,13.167397,0.017872,-0.581705,7.366600,1.570780
+0.020000,9.000000,9.999918,13.167581,0.009186,-0.434323,7.369120,1.570791
+0.020000,9.000000,9.999987,13.167650,0.003447,-0.286912,7.370553,1.570796
+0.020000,9.000000,10.000000,13.167663,0.000658,-0.139487,7.371214,1.570796
+0.020000,9.000000,10.000000,13.167663,0.000000,-0.032887,5.330032,1.570796
diff --git a/src/main/deploy/output/Unnamed.pf1.csv b/src/main/deploy/output/Unnamed.pf1.csv
new file mode 100644
index 0000000..b6b5e79
--- /dev/null
+++ b/src/main/deploy/output/Unnamed.pf1.csv
@@ -0,0 +1,258 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.812343,1.032196,0.000031,0.003125,0.156250,7.812500,6.282238
+0.020000,0.812343,1.032196,0.000156,0.009375,0.312500,7.812500,6.282244
+0.020000,0.812343,1.032196,0.000437,0.018750,0.468750,7.812500,6.282259
+0.020000,0.812343,1.032196,0.000937,0.031250,0.625000,7.812500,6.282286
+0.020000,0.812343,1.032196,0.001719,0.046875,0.781250,7.812500,6.282327
+0.020000,0.812343,1.032196,0.002844,0.065625,0.937500,7.812500,6.282386
+0.020000,0.812343,1.032196,0.004375,0.087500,1.093750,7.812500,6.282467
+0.020000,0.812343,1.032196,0.006375,0.112500,1.250000,7.812500,6.282573
+0.020000,0.812343,1.032196,0.008906,0.140625,1.406250,7.812500,6.282707
+0.020000,0.812343,1.032196,0.012031,0.171875,1.562500,7.812500,6.282872
+0.020000,0.812343,1.032196,0.015813,0.206250,1.718750,7.812500,6.283073
+0.020000,0.814716,1.032196,0.020313,0.243750,1.875000,7.812500,0.000126
+0.020000,0.819998,1.032197,0.025594,0.284375,2.031250,7.812500,0.000406
+0.020000,0.826123,1.032201,0.031719,0.328125,2.187500,7.812500,0.000731
+0.020000,0.833154,1.032207,0.038750,0.375000,2.343750,7.812500,0.001104
+0.020000,0.841154,1.032218,0.046750,0.425000,2.500000,7.812500,0.001529
+0.020000,0.850184,1.032234,0.055781,0.478125,2.656250,7.812500,0.002010
+0.020000,0.860310,1.032257,0.065906,0.534375,2.812500,7.812500,0.002550
+0.020000,0.871592,1.032289,0.077188,0.593750,2.968750,7.812500,0.003153
+0.020000,0.884090,1.032333,0.089688,0.656250,3.125000,7.812500,0.003822
+0.020000,0.897872,1.032390,0.103469,0.721875,3.281250,7.812500,0.004561
+0.020000,0.912998,1.032466,0.118594,0.790625,3.437500,7.812500,0.005375
+0.020000,0.929529,1.032562,0.135125,0.862500,3.593750,7.812500,0.006266
+0.020000,0.947528,1.032683,0.153125,0.937500,3.750000,7.812500,0.007239
+0.020000,0.967059,1.032835,0.172656,1.015625,3.906250,7.812500,0.008298
+0.020000,0.988182,1.033023,0.193781,1.096875,4.062500,7.812500,0.009448
+0.020000,1.010962,1.033252,0.216563,1.181250,4.218750,7.812500,0.010692
+0.020000,1.035462,1.033530,0.241063,1.268750,4.375000,7.812500,0.012035
+0.020000,1.061739,1.033866,0.267344,1.359375,4.531250,7.812500,0.013482
+0.020000,1.089863,1.034267,0.295469,1.453125,4.687500,7.812500,0.015038
+0.020000,1.119890,1.034743,0.325500,1.550000,4.843750,7.812500,0.016707
+0.020000,1.151884,1.035306,0.357500,1.650000,5.000000,7.812500,0.018495
+0.020000,1.185877,1.035968,0.391500,1.750000,5.000000,0.000000,0.020405
+0.020000,1.221869,1.036739,0.427500,1.850000,5.000000,0.000000,0.022439
+0.020000,1.259860,1.037632,0.465500,1.950000,5.000000,-0.000000,0.024599
+0.020000,1.299847,1.038662,0.505500,2.050000,5.000000,0.000000,0.026889
+0.020000,1.341829,1.039842,0.547500,2.150000,5.000000,0.000000,0.029309
+0.020000,1.385810,1.041187,0.591500,2.250000,5.000000,0.000000,0.031863
+0.020000,1.431783,1.042715,0.637500,2.350000,5.000000,0.000000,0.034553
+0.020000,1.479752,1.044441,0.685500,2.450000,5.000000,0.000000,0.037383
+0.020000,1.529684,1.046382,0.735469,2.546875,4.843750,-7.812500,0.040354
+0.020000,1.581513,1.048556,0.787344,2.640625,4.687500,-7.812500,0.043465
+0.020000,1.635177,1.050977,0.841063,2.731250,4.531250,-7.812500,0.046715
+0.020000,1.690612,1.053662,0.896563,2.818750,4.375000,-7.812500,0.050105
+0.020000,1.747754,1.056629,0.953781,2.903125,4.218750,-7.812500,0.053634
+0.020000,1.806537,1.059892,1.012656,2.984375,4.062500,-7.812500,0.057301
+0.020000,1.866902,1.063470,1.073125,3.062500,3.906250,-7.812500,0.061106
+0.020000,1.928777,1.067378,1.135125,3.137500,3.750000,-7.812500,0.065050
+0.020000,1.992103,1.071633,1.198594,3.209375,3.593750,-7.812500,0.069131
+0.020000,2.056814,1.076250,1.263469,3.278125,3.437500,-7.812500,0.073349
+0.020000,2.122844,1.081247,1.329688,3.343750,3.281250,-7.812500,0.077704
+0.020000,2.190128,1.086637,1.397188,3.406250,3.125000,-7.812500,0.082196
+0.020000,2.258601,1.092437,1.465906,3.465625,2.968750,-7.812500,0.086824
+0.020000,2.328198,1.098662,1.535781,3.521875,2.812500,-7.812500,0.091587
+0.020000,2.398855,1.105326,1.606750,3.575000,2.656250,-7.812500,0.096486
+0.020000,2.470501,1.112442,1.678750,3.625000,2.500000,-7.812500,0.101518
+0.020000,2.543075,1.120024,1.751719,3.671875,2.343750,-7.812500,0.106685
+0.020000,2.616509,1.128084,1.825594,3.715625,2.187500,-7.812500,0.111984
+0.020000,2.690737,1.136635,1.900313,3.756250,2.031250,-7.812500,0.117415
+0.020000,2.765691,1.145687,1.975812,3.793750,1.875000,-7.812500,0.122977
+0.020000,2.841308,1.155252,2.052031,3.828125,1.718750,-7.812500,0.128668
+0.020000,2.917519,1.165337,2.128906,3.859375,1.562500,-7.812500,0.134488
+0.020000,2.994257,1.175952,2.206375,3.887500,1.406250,-7.812500,0.140435
+0.020000,3.071455,1.187104,2.284375,3.912500,1.250000,-7.812500,0.146506
+0.020000,3.149047,1.198799,2.362844,3.934375,1.093750,-7.812500,0.152701
+0.020000,3.226966,1.211042,2.441719,3.953125,0.937500,-7.812500,0.159017
+0.020000,3.305145,1.223837,2.520937,3.968750,0.781250,-7.812500,0.165451
+0.020000,3.383516,1.237186,2.600437,3.981250,0.625000,-7.812500,0.172001
+0.020000,3.462012,1.251092,2.680156,3.990625,0.468750,-7.812500,0.178665
+0.020000,3.540567,1.265552,2.760031,3.996875,0.312500,-7.812500,0.185438
+0.020000,3.619113,1.280566,2.840000,4.000000,0.156250,-7.812500,0.192318
+0.020000,3.697584,1.296130,2.920000,4.000000,0.000000,-7.812500,0.199301
+0.020000,3.775945,1.312246,3.000000,4.000000,0.000000,0.000000,0.206385
+0.020000,3.854187,1.328920,3.080000,4.000000,0.000000,0.000000,0.213573
+0.020000,3.932306,1.346161,3.160000,4.000000,0.000000,0.000000,0.220864
+0.020000,4.010299,1.363974,3.240000,4.000000,0.000000,0.000000,0.228262
+0.020000,4.088155,1.382369,3.320000,4.000000,0.000000,0.000000,0.235766
+0.020000,4.165869,1.401351,3.400000,4.000000,0.000000,0.000000,0.243379
+0.020000,4.243438,1.420929,3.480000,4.000000,0.000000,0.000000,0.251101
+0.020000,4.320850,1.441109,3.560000,4.000000,0.000000,0.000000,0.258934
+0.020000,4.398101,1.461899,3.640000,4.000000,0.000000,0.000000,0.266879
+0.020000,4.475183,1.483307,3.720000,4.000000,0.000000,0.000000,0.274937
+0.020000,4.552089,1.505339,3.800000,4.000000,0.000000,0.000000,0.283110
+0.020000,4.628811,1.528004,3.880000,4.000000,0.000000,0.000000,0.291397
+0.020000,4.705341,1.551308,3.960000,4.000000,0.000000,0.000000,0.299801
+0.020000,4.781672,1.575259,4.040000,4.000000,0.000000,0.000000,0.308322
+0.020000,4.857794,1.599864,4.120000,4.000000,0.000000,0.000000,0.316961
+0.020000,4.933698,1.625130,4.200000,4.000000,0.000000,0.000000,0.325719
+0.020000,5.009378,1.651065,4.280000,4.000000,0.000000,0.000000,0.334596
+0.020000,5.084822,1.677675,4.360000,4.000000,0.000000,0.000000,0.343593
+0.020000,5.160022,1.704967,4.440000,4.000000,0.000000,0.000000,0.352711
+0.020000,5.234970,1.732948,4.520000,4.000000,0.000000,0.000000,0.361949
+0.020000,5.309653,1.761625,4.600000,4.000000,0.000000,0.000000,0.371309
+0.020000,5.384062,1.791003,4.680000,4.000000,0.000000,0.000000,0.380789
+0.020000,5.458188,1.821091,4.760000,4.000000,0.000000,0.000000,0.390392
+0.020000,5.532021,1.851894,4.840000,4.000000,0.000000,0.000000,0.400115
+0.020000,5.605548,1.883417,4.920000,4.000000,0.000000,0.000000,0.409959
+0.020000,5.678759,1.915667,5.000000,4.000000,0.000000,0.000000,0.419924
+0.020000,5.751643,1.948649,5.080000,4.000000,0.000000,0.000000,0.430008
+0.020000,5.824188,1.982369,5.160000,4.000000,0.000000,0.000000,0.440212
+0.020000,5.896384,2.016832,5.240000,4.000000,0.000000,0.000000,0.450534
+0.020000,5.968219,2.052042,5.320000,4.000000,0.000000,0.000000,0.460974
+0.020000,6.039680,2.088005,5.400000,4.000000,0.000000,0.000000,0.471529
+0.020000,6.110755,2.124724,5.480000,4.000000,0.000000,0.000000,0.482199
+0.020000,6.181432,2.162203,5.560000,4.000000,0.000000,0.000000,0.492982
+0.020000,6.251699,2.200445,5.640000,4.000000,0.000000,0.000000,0.503876
+0.020000,6.321543,2.239455,5.720000,4.000000,0.000000,0.000000,0.514879
+0.020000,6.390951,2.279235,5.800000,4.000000,0.000000,0.000000,0.525988
+0.020000,6.459911,2.319787,5.880000,4.000000,0.000000,0.000000,0.537202
+0.020000,6.528409,2.361113,5.960000,4.000000,0.000000,0.000000,0.548518
+0.020000,6.596434,2.403215,6.040000,4.000000,0.000000,0.000000,0.559932
+0.020000,6.663971,2.446094,6.120000,4.000000,0.000000,0.000000,0.571443
+0.020000,6.731009,2.489750,6.200000,4.000000,0.000000,0.000000,0.583045
+0.020000,6.797534,2.534185,6.280000,4.000000,0.000000,0.000000,0.594737
+0.020000,6.863532,2.579397,6.360000,4.000000,0.000000,0.000000,0.606513
+0.020000,6.928992,2.625385,6.440000,4.000000,0.000000,0.000000,0.618371
+0.020000,6.993900,2.672149,6.520000,4.000000,0.000000,0.000000,0.630305
+0.020000,7.058243,2.719687,6.600000,4.000000,0.000000,0.000000,0.642313
+0.020000,7.122009,2.767996,6.680000,4.000000,0.000000,0.000000,0.654388
+0.020000,7.185186,2.817074,6.760000,4.000000,0.000000,0.000000,0.666528
+0.020000,7.247761,2.866917,6.840000,4.000000,0.000000,0.000000,0.678725
+0.020000,7.309722,2.917521,6.920000,4.000000,0.000000,0.000000,0.690977
+0.020000,7.371057,2.968881,7.000000,4.000000,0.000000,0.000000,0.703276
+0.020000,7.431754,3.020994,7.080000,4.000000,0.000000,0.000000,0.715619
+0.020000,7.491803,3.073853,7.160000,4.000000,0.000000,0.000000,0.728000
+0.020000,7.551192,3.127452,7.240000,4.000000,0.000000,0.000000,0.740413
+0.020000,7.609910,3.181786,7.320000,4.000000,0.000000,0.000000,0.752852
+0.020000,7.667947,3.236846,7.400000,4.000000,0.000000,0.000000,0.765312
+0.020000,7.725293,3.292625,7.480000,4.000000,0.000000,0.000000,0.777787
+0.020000,7.781938,3.349115,7.560000,4.000000,0.000000,0.000000,0.790271
+0.020000,7.837874,3.406309,7.640000,4.000000,0.000000,0.000000,0.802758
+0.020000,7.893092,3.464196,7.720000,4.000000,0.000000,0.000000,0.815243
+0.020000,7.947582,3.522768,7.800000,4.000000,0.000000,0.000000,0.827720
+0.020000,8.001338,3.582015,7.880000,4.000000,0.000000,0.000000,0.840182
+0.020000,8.054352,3.641927,7.960000,4.000000,0.000000,0.000000,0.852625
+0.020000,8.106617,3.702494,8.040000,4.000000,0.000000,0.000000,0.865041
+0.020000,8.158127,3.763703,8.120000,4.000000,0.000000,0.000000,0.877427
+0.020000,8.208876,3.825545,8.200000,4.000000,0.000000,0.000000,0.889775
+0.020000,8.258859,3.888008,8.280000,4.000000,0.000000,0.000000,0.902081
+0.020000,8.308071,3.951080,8.360000,4.000000,0.000000,0.000000,0.914340
+0.020000,8.356508,4.014749,8.440000,4.000000,0.000000,0.000000,0.926545
+0.020000,8.404166,4.079004,8.520000,4.000000,0.000000,0.000000,0.938693
+0.020000,8.451042,4.143831,8.600000,4.000000,0.000000,0.000000,0.950778
+0.020000,8.497133,4.209218,8.680000,4.000000,0.000000,0.000000,0.962795
+0.020000,8.542438,4.275153,8.760000,4.000000,0.000000,0.000000,0.974740
+0.020000,8.586954,4.341623,8.840000,4.000000,0.000000,0.000000,0.986608
+0.020000,8.630681,4.408614,8.920000,4.000000,0.000000,0.000000,0.998396
+0.020000,8.673618,4.476115,9.000000,4.000000,0.000000,0.000000,1.010100
+0.020000,8.715765,4.544112,9.080000,4.000000,0.000000,0.000000,1.021715
+0.020000,8.757123,4.612591,9.160000,4.000000,0.000000,0.000000,1.033238
+0.020000,8.797692,4.681541,9.240000,4.000000,0.000000,0.000000,1.044666
+0.020000,8.837474,4.750948,9.320000,4.000000,0.000000,0.000000,1.055996
+0.020000,8.876470,4.820799,9.400000,4.000000,0.000000,0.000000,1.067224
+0.020000,8.914684,4.891082,9.480000,4.000000,0.000000,0.000000,1.078349
+0.020000,8.952117,4.961783,9.560000,4.000000,0.000000,0.000000,1.089367
+0.020000,8.988772,5.032891,9.640000,4.000000,0.000000,0.000000,1.100276
+0.020000,9.024654,5.104393,9.720000,4.000000,0.000000,0.000000,1.111075
+0.020000,9.059766,5.176275,9.800000,4.000000,0.000000,0.000000,1.121760
+0.020000,9.094111,5.248528,9.880000,4.000000,0.000000,0.000000,1.132332
+0.020000,9.127695,5.321136,9.960000,4.000000,0.000000,0.000000,1.142788
+0.020000,9.160522,5.394090,10.040000,4.000000,0.000000,0.000000,1.153126
+0.020000,9.192598,5.467378,10.120000,4.000000,0.000000,0.000000,1.163347
+0.020000,9.223927,5.540988,10.200000,4.000000,0.000000,0.000000,1.173449
+0.020000,9.254516,5.614909,10.280000,4.000000,0.000000,0.000000,1.183430
+0.020000,9.284369,5.689129,10.360000,4.000000,0.000000,0.000000,1.193291
+0.020000,9.313494,5.763639,10.440000,4.000000,0.000000,0.000000,1.203032
+0.020000,9.341896,5.838427,10.520000,4.000000,0.000000,0.000000,1.212651
+0.020000,9.369582,5.913483,10.600000,4.000000,0.000000,0.000000,1.222149
+0.020000,9.396559,5.988798,10.680000,4.000000,0.000000,0.000000,1.231526
+0.020000,9.422832,6.064359,10.760000,4.000000,0.000000,0.000000,1.240782
+0.020000,9.448410,6.140160,10.840000,4.000000,0.000000,0.000000,1.249917
+0.020000,9.473299,6.216190,10.920000,4.000000,0.000000,0.000000,1.258931
+0.020000,9.497506,6.292439,11.000000,4.000000,0.000000,0.000000,1.267825
+0.020000,9.521038,6.368899,11.080000,4.000000,0.000000,0.000000,1.276600
+0.020000,9.543903,6.445562,11.160000,4.000000,0.000000,0.000000,1.285256
+0.020000,9.566109,6.522418,11.240000,4.000000,0.000000,0.000000,1.293794
+0.020000,9.587662,6.599461,11.320000,4.000000,0.000000,0.000000,1.302215
+0.020000,9.608569,6.676679,11.400000,4.000000,0.000000,0.000000,1.310519
+0.020000,9.628840,6.754068,11.480000,4.000000,0.000000,0.000000,1.318708
+0.020000,9.648481,6.831621,11.560000,4.000000,0.000000,0.000000,1.326783
+0.020000,9.667498,6.909326,11.640000,4.000000,0.000000,0.000000,1.334744
+0.020000,9.685901,6.987180,11.720000,4.000000,0.000000,0.000000,1.342593
+0.020000,9.703697,7.065176,11.800000,4.000000,0.000000,0.000000,1.350332
+0.020000,9.720893,7.143306,11.880000,4.000000,0.000000,0.000000,1.357960
+0.020000,9.737493,7.221546,11.959983,3.998269,-0.086541,-4.327055,1.365479
+0.020000,9.753496,7.299844,12.039900,3.993413,-0.242791,-7.812500,1.372884
+0.020000,9.768898,7.378131,12.119688,3.985433,-0.399041,-7.812500,1.380172
+0.020000,9.783699,7.456340,12.199286,3.974327,-0.555291,-7.812500,1.387339
+0.020000,9.797898,7.534404,12.278630,3.960096,-0.711541,-7.812500,1.394382
+0.020000,9.811498,7.612253,12.357658,3.942740,-0.867791,-7.812500,1.401297
+0.020000,9.824502,7.689820,12.436308,3.922259,-1.024041,-7.812500,1.408082
+0.020000,9.836914,7.767038,12.514517,3.898653,-1.180291,-7.812500,1.414734
+0.020000,9.848741,7.843838,12.592223,3.871923,-1.336541,-7.812500,1.421251
+0.020000,9.859990,7.920155,12.669363,3.842067,-1.492791,-7.812500,1.427630
+0.020000,9.870670,7.995916,12.745874,3.809086,-1.649041,-7.812500,1.433870
+0.020000,9.880789,8.071058,12.821695,3.772980,-1.805291,-7.812500,1.439970
+0.020000,9.890360,8.145514,12.896762,3.733749,-1.961541,-7.812500,1.445927
+0.020000,9.899393,8.219212,12.971014,3.691393,-2.117791,-7.812500,1.451741
+0.020000,9.907901,8.292092,13.044387,3.645913,-2.274041,-7.812500,1.457411
+0.020000,9.915896,8.364080,13.116819,3.597307,-2.430291,-7.812500,1.462935
+0.020000,9.923395,8.435114,13.188248,3.545576,-2.586541,-7.812500,1.468313
+0.020000,9.930410,8.505127,13.258611,3.490720,-2.742791,-7.812500,1.473543
+0.020000,9.936957,8.574051,13.327845,3.432739,-2.899041,-7.812500,1.478627
+0.020000,9.943052,8.641822,13.395889,3.371634,-3.055291,-7.812500,1.483561
+0.020000,9.948711,8.708371,13.462680,3.307403,-3.211541,-7.812500,1.488348
+0.020000,9.953952,8.773635,13.528154,3.240047,-3.367791,-7.812500,1.492985
+0.020000,9.958791,8.837548,13.592250,3.169566,-3.524041,-7.812500,1.497472
+0.020000,9.963245,8.900046,13.654905,3.095960,-3.680291,-7.812500,1.501811
+0.020000,9.967332,8.961060,13.716057,3.019229,-3.836541,-7.812500,1.505998
+0.020000,9.971070,9.020530,13.775643,2.939374,-3.992791,-7.812500,1.510036
+0.020000,9.974477,9.078385,13.833601,2.856393,-4.149041,-7.812500,1.513923
+0.020000,9.977570,9.134567,13.889868,2.770287,-4.305291,-7.812500,1.517659
+0.020000,9.980368,9.189009,13.944381,2.681056,-4.461541,-7.812500,1.521244
+0.020000,9.982888,9.241647,13.997079,2.588700,-4.617791,-7.812500,1.524677
+0.020000,9.985147,9.292415,14.047898,2.493220,-4.774041,-7.812500,1.527958
+0.020000,9.987164,9.341251,14.096776,2.394614,-4.930291,-7.812500,1.531086
+0.020000,9.988956,9.388111,14.143669,2.294614,-5.000000,-3.485445,1.534063
+0.020000,9.990541,9.432974,14.188561,2.194614,-5.000000,-0.000000,1.536890
+0.020000,9.991937,9.475845,14.231453,2.094614,-5.000000,0.000000,1.539571
+0.020000,9.993162,9.516717,14.272345,1.994614,-5.000000,0.000000,1.542108
+0.020000,9.994231,9.555595,14.311238,1.894614,-5.000000,0.000000,1.544505
+0.020000,9.995159,9.592475,14.348130,1.794614,-5.000000,-0.000000,1.546764
+0.020000,9.995961,9.627358,14.383022,1.694614,-5.000000,0.000000,1.548887
+0.020000,9.996648,9.660244,14.415915,1.594614,-5.000000,-0.000000,1.550877
+0.020000,9.997235,9.691149,14.446824,1.496345,-4.913459,4.327055,1.552737
+0.020000,9.997733,9.720119,14.475800,1.401200,-4.757209,7.812500,1.554471
+0.020000,9.998154,9.747220,14.502903,1.309181,-4.600959,7.812500,1.556086
+0.020000,9.998507,9.772511,14.528198,1.220287,-4.444709,7.812500,1.557587
+0.020000,9.998802,9.796059,14.551746,1.134518,-4.288459,7.812500,1.558978
+0.020000,9.999046,9.817922,14.573610,1.051874,-4.132209,7.812500,1.560264
+0.020000,9.999247,9.838162,14.593852,0.972354,-3.975959,7.812500,1.561451
+0.020000,9.999412,9.856845,14.612535,0.895960,-3.819709,7.812500,1.562543
+0.020000,9.999545,9.874031,14.629722,0.822691,-3.663459,7.812500,1.563544
+0.020000,9.999652,9.889784,14.645474,0.752547,-3.507209,7.812500,1.564460
+0.020000,9.999737,9.904163,14.659855,0.685528,-3.350959,7.812500,1.565293
+0.020000,9.999804,9.917234,14.672927,0.621634,-3.194709,7.812500,1.566049
+0.020000,9.999856,9.929060,14.684752,0.560864,-3.038459,7.812500,1.566731
+0.020000,9.999896,9.939700,14.695393,0.503220,-2.882209,7.812500,1.567344
+0.020000,9.999926,9.949219,14.704912,0.448701,-2.725959,7.812500,1.567891
+0.020000,9.999949,9.957679,14.713372,0.397307,-2.569709,7.812500,1.568377
+0.020000,9.999965,9.965143,14.720835,0.349038,-2.413459,7.812500,1.568805
+0.020000,9.999977,9.971671,14.727365,0.303893,-2.257209,7.812500,1.569179
+0.020000,9.999985,9.977330,14.733022,0.261874,-2.100959,7.812500,1.569503
+0.020000,9.999991,9.982180,14.737871,0.222980,-1.944709,7.812500,1.569780
+0.020000,9.999995,9.986280,14.741973,0.187211,-1.788459,7.812500,1.570014
+0.020000,9.999997,9.989697,14.745391,0.154567,-1.632209,7.812500,1.570209
+0.020000,9.999998,9.992493,14.748187,0.125048,-1.475959,7.812500,1.570368
+0.020000,9.999999,9.994731,14.750424,0.098653,-1.319709,7.812500,1.570496
+0.020000,10.000000,9.996472,14.752164,0.075384,-1.163459,7.812500,1.570595
+0.020000,10.000000,9.997778,14.753470,0.055240,-1.007209,7.812500,1.570670
+0.020000,10.000000,9.998713,14.754405,0.038221,-0.850959,7.812500,1.570723
+0.020000,10.000000,9.999339,14.755030,0.024327,-0.694709,7.812500,1.570759
+0.020000,10.000000,9.999718,14.755409,0.013558,-0.538459,7.812500,1.570780
+0.020000,10.000000,9.999913,14.755604,0.005913,-0.382209,7.812500,1.570791
+0.020000,10.000000,9.999986,14.755677,0.001394,-0.225959,7.812500,1.570796
+0.020000,10.000000,10.000000,14.755691,0.000000,-0.069709,7.812500,1.570796
+0.020000,10.000000,10.000000,14.755691,0.000000,0.000000,3.485445,1.570796
diff --git a/src/main/deploy/output/Unnamed.right.pf1.csv b/src/main/deploy/output/Unnamed.right.pf1.csv
new file mode 100644
index 0000000..a55d27b
--- /dev/null
+++ b/src/main/deploy/output/Unnamed.right.pf1.csv
@@ -0,0 +1,258 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.811396,0.032196,0.000031,0.003125,0.156250,7.812500,6.282238
+0.020000,0.811402,0.032196,0.000038,0.000330,-0.139734,-14.799206,6.282244
+0.020000,0.811417,0.032196,0.000053,0.000743,0.020646,8.018991,6.282259
+0.020000,0.811444,0.032196,0.000079,0.001321,0.028906,0.413032,6.282286
+0.020000,0.811485,0.032196,0.000120,0.002065,0.037171,0.413214,6.282327
+0.020000,0.811544,0.032196,0.000180,0.002974,0.045441,0.413511,6.282386
+0.020000,0.811625,0.032196,0.000261,0.004048,0.053720,0.413953,6.282467
+0.020000,0.811731,0.032196,0.000367,0.005288,0.062011,0.414569,6.282573
+0.020000,0.811865,0.032196,0.000501,0.006695,0.070319,0.415389,6.282707
+0.020000,0.812030,0.032196,0.000666,0.008268,0.078648,0.416440,6.282872
+0.020000,0.812231,0.032196,0.000866,0.010008,0.087003,0.417754,6.283073
+0.020000,0.814842,0.032196,0.003477,0.130548,6.026998,296.999774,0.000126
+0.020000,0.820404,0.032197,0.009039,0.278115,7.378356,67.567879,0.000406
+0.020000,0.826853,0.032201,0.015489,0.322467,2.217629,-258.036343,0.000731
+0.020000,0.834257,0.032208,0.022893,0.370214,2.387333,8.485168,0.001104
+0.020000,0.842684,0.032219,0.031319,0.421314,2.555021,8.384406,0.001529
+0.020000,0.852195,0.032236,0.040830,0.475546,2.711563,7.827126,0.002010
+0.020000,0.862861,0.032260,0.051496,0.533301,2.887767,8.810183,0.002550
+0.020000,0.874744,0.032294,0.063380,0.594195,3.044683,7.845800,0.003153
+0.020000,0.887912,0.032340,0.076547,0.658365,3.208536,8.192639,0.003822
+0.020000,0.902434,0.032401,0.091069,0.726100,3.386717,8.909083,0.004561
+0.020000,0.918372,0.032480,0.107008,0.796942,3.542129,7.770562,0.005375
+0.020000,0.935794,0.032581,0.124431,0.871130,3.709409,8.364033,0.006266
+0.020000,0.954767,0.032710,0.143404,0.948659,3.876446,8.351833,0.007239
+0.020000,0.975357,0.032870,0.163994,1.029516,4.042849,8.320147,0.008298
+0.020000,0.997630,0.033067,0.186268,1.113686,4.208508,8.282935,0.009448
+0.020000,1.021654,0.033309,0.210293,1.201264,4.378876,8.518440,0.010692
+0.020000,1.047497,0.033603,0.236138,1.292226,4.548076,8.460005,0.012035
+0.020000,1.075221,0.033956,0.263864,1.386310,4.704242,7.808281,0.013482
+0.020000,1.104900,0.034380,0.293547,1.484131,4.891049,9.340324,0.015038
+0.020000,1.136596,0.034883,0.325247,1.584998,5.043347,7.614930,0.016707
+0.020000,1.170378,0.035477,0.359033,1.689335,5.216850,8.675156,0.018495
+0.020000,1.206280,0.036176,0.394943,1.795470,5.306758,4.495395,0.020405
+0.020000,1.244306,0.036990,0.432977,1.901726,5.312802,0.302177,0.022439
+0.020000,1.284457,0.037935,0.473139,2.008075,5.317422,0.231032,0.024599
+0.020000,1.326732,0.039023,0.515428,2.114487,5.320613,0.159544,0.026889
+0.020000,1.371133,0.040271,0.559847,2.220935,5.322369,0.087800,0.029309
+0.020000,1.417667,0.041695,0.606403,2.327776,5.342073,0.985211,0.031863
+0.020000,1.466329,0.043311,0.655092,2.434454,5.333903,-0.408505,0.034553
+0.020000,1.517127,0.045139,0.705922,2.541504,5.352499,0.929776,0.037383
+0.020000,1.570027,0.047196,0.758862,2.646993,5.274471,-3.901392,0.040354
+0.020000,1.624964,0.049500,0.813847,2.749284,5.114505,-7.998310,0.043465
+0.020000,1.681875,0.052068,0.870816,2.848425,4.957072,-7.871626,0.046715
+0.020000,1.740695,0.054917,0.929706,2.944489,4.803197,-7.693785,0.050105
+0.020000,1.801362,0.058067,0.990454,3.037431,4.647095,-7.805082,0.053634
+0.020000,1.863807,0.061534,1.052995,3.127038,4.480377,-8.335914,0.057301
+0.020000,1.927970,0.065337,1.117271,3.213787,4.337424,-7.147634,0.061106
+0.020000,1.993781,0.069493,1.183213,3.297118,4.166555,-8.543467,0.065050
+0.020000,2.061179,0.074021,1.250763,3.377474,4.017781,-7.438656,0.069131
+0.020000,2.130098,0.078939,1.319857,3.454702,3.861433,-7.817400,0.073349
+0.020000,2.200470,0.084264,1.390431,3.528704,3.700097,-8.066807,0.077704
+0.020000,2.272232,0.090013,1.462422,3.599573,3.543458,-7.831982,0.082196
+0.020000,2.345316,0.096204,1.535768,3.667287,3.385680,-7.888892,0.086824
+0.020000,2.419657,0.102853,1.610406,3.731912,3.231241,-7.721921,0.091587
+0.020000,2.495190,0.109977,1.686274,3.793409,3.074864,-7.818882,0.096486
+0.020000,2.571845,0.117590,1.763306,3.851601,2.909577,-8.264338,0.101518
+0.020000,2.649558,0.125709,1.841442,3.906759,2.757909,-7.583396,0.106685
+0.020000,2.728259,0.134348,1.920616,3.958699,2.597025,-8.044215,0.111984
+0.020000,2.807882,0.143520,2.000766,4.007512,2.440621,-7.820207,0.117415
+0.020000,2.888358,0.153240,2.081826,4.053016,2.275231,-8.269472,0.122977
+0.020000,2.969622,0.163518,2.163737,4.095556,2.126960,-7.413572,0.128668
+0.020000,3.051602,0.174367,2.246432,4.134763,1.960382,-8.328864,0.134488
+0.020000,3.134231,0.185797,2.329848,4.170759,1.799774,-8.030439,0.140435
+0.020000,3.217438,0.197817,2.413919,4.203563,1.640219,-7.977729,0.146506
+0.020000,3.301156,0.210435,2.498582,4.233158,1.479736,-8.024147,0.152701
+0.020000,3.385314,0.223658,2.583773,4.259525,1.318367,-8.068480,0.159017
+0.020000,3.469842,0.237493,2.669426,4.282648,1.156147,-8.110955,0.165451
+0.020000,3.554670,0.251942,2.755476,4.302510,0.993111,-8.151813,0.172001
+0.020000,3.639728,0.267010,2.841858,4.319096,0.829285,-8.191302,0.178665
+0.020000,3.724944,0.282696,2.928505,4.332390,0.664692,-8.229678,0.185438
+0.020000,3.810247,0.299002,3.015353,4.342377,0.499348,-8.267203,0.192318
+0.020000,3.895568,0.315925,3.102336,4.349171,0.339725,-7.981134,0.199301
+0.020000,3.980868,0.333468,3.189421,4.354246,0.253714,-4.300540,0.206385
+0.020000,4.066140,0.351640,3.276608,4.359347,0.255074,0.068020,0.213573
+0.020000,4.151379,0.370452,3.363898,4.364505,0.257884,0.140467,0.220864
+0.020000,4.236583,0.389913,3.451297,4.369926,0.271049,0.658268,0.228262
+0.020000,4.321743,0.410033,3.538801,4.375214,0.264426,-0.331142,0.235766
+0.020000,4.406852,0.430821,3.626412,4.380545,0.266535,0.105450,0.243379
+0.020000,4.491908,0.452289,3.714136,4.386179,0.281711,0.758760,0.251101
+0.020000,4.576900,0.474445,3.801968,4.391611,0.271614,-0.504823,0.258934
+0.020000,4.661823,0.497300,3.889913,4.397247,0.281782,0.508408,0.266879
+0.020000,4.746670,0.520865,3.977971,4.402902,0.282730,0.047385,0.274937
+0.020000,4.831432,0.545148,4.066143,4.408587,0.284257,0.076349,0.283110
+0.020000,4.916102,0.570161,4.154430,4.414385,0.289910,0.282678,0.291397
+0.020000,5.000672,0.595913,4.242834,4.420168,0.289161,-0.037482,0.299801
+0.020000,5.085132,0.622415,4.331354,4.426031,0.293152,0.199573,0.308322
+0.020000,5.169474,0.649677,4.419993,4.431954,0.296158,0.150303,0.316961
+0.020000,5.253688,0.677709,4.508750,4.437807,0.292643,-0.175782,0.325719
+0.020000,5.337766,0.706522,4.597628,4.443905,0.304883,0.612006,0.334596
+0.020000,5.421695,0.736124,4.686624,4.449826,0.296069,-0.440676,0.343593
+0.020000,5.505465,0.766527,4.775741,4.455826,0.299989,0.195967,0.352711
+0.020000,5.589067,0.797739,4.864980,4.461954,0.306394,0.320264,0.361949
+0.020000,5.672488,0.829771,4.954339,4.467960,0.300322,-0.303620,0.371309
+0.020000,5.755716,0.862632,5.043819,4.473987,0.301336,0.050728,0.380789
+0.020000,5.838739,0.896331,5.133421,4.480092,0.305266,0.196508,0.390392
+0.020000,5.921545,0.930878,5.223144,4.486174,0.304080,-0.059333,0.400115
+0.020000,6.004120,0.966280,5.312988,4.492196,0.301100,-0.149002,0.409959
+0.020000,6.086450,1.002547,5.402952,4.498205,0.300459,-0.032034,0.419924
+0.020000,6.168521,1.039687,5.493036,4.504192,0.299361,-0.054872,0.430008
+0.020000,6.250320,1.077708,5.583239,4.510148,0.297795,-0.078344,0.440212
+0.020000,6.331831,1.116617,5.673561,4.516087,0.296945,-0.042473,0.450534
+0.020000,6.413039,1.156422,5.764000,4.521962,0.293725,-0.161024,0.460974
+0.020000,6.493929,1.197130,5.854555,4.527754,0.289629,-0.204801,0.471529
+0.020000,6.574484,1.238746,5.945224,4.533477,0.286121,-0.175363,0.482199
+0.020000,6.654687,1.281277,6.036007,4.539119,0.282095,-0.201300,0.492982
+0.020000,6.734522,1.324728,6.126900,4.544669,0.277544,-0.227575,0.503876
+0.020000,6.813972,1.369103,6.217903,4.550119,0.272461,-0.254117,0.514879
+0.020000,6.893019,1.414407,6.309012,4.555456,0.266844,-0.280850,0.525988
+0.020000,6.971645,1.460643,6.400225,4.560669,0.260691,-0.307695,0.537202
+0.020000,7.049832,1.507815,6.491540,4.565749,0.253999,-0.334565,0.548518
+0.020000,7.127563,1.555924,6.582954,4.570685,0.246772,-0.361369,0.559932
+0.020000,7.204817,1.604972,6.674464,4.575490,0.240245,-0.326351,0.571443
+0.020000,7.281578,1.654960,6.766066,4.580109,0.230960,-0.464232,0.583045
+0.020000,7.357824,1.705889,6.857757,4.584547,0.221915,-0.452250,0.594737
+0.020000,7.433538,1.757756,6.949533,4.588798,0.212551,-0.468208,0.606513
+0.020000,7.508700,1.810561,7.041390,4.592852,0.202689,-0.493095,0.618371
+0.020000,7.583291,1.864302,7.133324,4.596699,0.192344,-0.517279,0.630305
+0.020000,7.657292,1.918975,7.225330,4.600330,0.181531,-0.540649,0.642313
+0.020000,7.730683,1.974576,7.317405,4.603757,0.171388,-0.507151,0.654388
+0.020000,7.803447,2.031101,7.409544,4.606934,0.158859,-0.626445,0.666528
+0.020000,7.875562,2.088543,7.501741,4.609859,0.146213,-0.632292,0.678725
+0.020000,7.947012,2.146897,7.593992,4.612533,0.133707,-0.625275,0.690977
+0.020000,8.017777,2.206154,7.686291,4.614950,0.120852,-0.642785,0.703276
+0.020000,8.087839,2.266307,7.778633,4.617117,0.108349,-0.625137,0.715619
+0.020000,8.157181,2.327346,7.871013,4.619000,0.094155,-0.709705,0.728000
+0.020000,8.225784,2.389262,7.963425,4.620602,0.080099,-0.702777,0.740413
+0.020000,8.293632,2.452044,8.055864,4.621925,0.066138,-0.698055,0.752852
+0.020000,8.360709,2.515679,8.148323,4.622965,0.052033,-0.705279,0.765312
+0.020000,8.426997,2.580157,8.240797,4.623716,0.037535,-0.724885,0.777787
+0.020000,8.492482,2.645463,8.333281,4.624174,0.022885,-0.732505,0.790271
+0.020000,8.557149,2.711584,8.425768,4.624342,0.008413,-0.723607,0.802758
+0.020000,8.620984,2.778505,8.518252,4.624220,-0.006088,-0.725036,0.815243
+0.020000,8.683973,2.846212,8.610728,4.623792,-0.021416,-0.766377,0.827720
+0.020000,8.746103,2.914688,8.703190,4.623083,-0.035450,-0.701710,0.840182
+0.020000,8.807362,2.983918,8.795631,4.622088,-0.049735,-0.714247,0.852625
+0.020000,8.867739,3.053885,8.888048,4.620809,-0.063938,-0.710175,0.865041
+0.020000,8.927224,3.124571,8.980432,4.619242,-0.078373,-0.721726,0.877427
+0.020000,8.985807,3.195958,9.072780,4.617380,-0.093120,-0.737384,0.889775
+0.020000,9.043478,3.268030,9.165085,4.615267,-0.105648,-0.626366,0.902081
+0.020000,9.100231,3.340766,9.257343,4.612888,-0.118924,-0.663800,0.914340
+0.020000,9.156058,3.414150,9.349548,4.610250,-0.131887,-0.648173,0.926545
+0.020000,9.210953,3.488161,9.441695,4.607360,-0.144507,-0.630978,0.938693
+0.020000,9.264910,3.562781,9.533780,4.604210,-0.157533,-0.651303,0.950778
+0.020000,9.317925,3.637990,9.625796,4.600817,-0.169650,-0.605879,0.962795
+0.020000,9.369994,3.713770,9.717740,4.597216,-0.180024,-0.518661,0.974740
+0.020000,9.421114,3.790101,9.809608,4.593397,-0.190940,-0.545828,0.986608
+0.020000,9.471285,3.866963,9.901396,4.589370,-0.201383,-0.522127,0.998396
+0.020000,9.520503,3.944339,9.993098,4.585143,-0.211332,-0.497470,1.010100
+0.020000,9.568770,4.022208,10.084713,4.580728,-0.220772,-0.471968,1.021715
+0.020000,9.616085,4.100551,10.176236,4.576134,-0.229686,-0.445733,1.033238
+0.020000,9.662449,4.179350,10.267663,4.571373,-0.238064,-0.418872,1.044666
+0.020000,9.707865,4.258587,10.358992,4.566455,-0.245894,-0.391497,1.055996
+0.020000,9.752335,4.338242,10.450220,4.561391,-0.253168,-0.363713,1.067224
+0.020000,9.795862,4.418298,10.541344,4.556194,-0.259880,-0.335625,1.078349
+0.020000,9.838451,4.498737,10.632361,4.550873,-0.266027,-0.307333,1.089367
+0.020000,9.880105,4.579541,10.723270,4.545441,-0.271606,-0.278935,1.100276
+0.020000,9.920830,4.660694,10.814068,4.539909,-0.276616,-0.250523,1.111075
+0.020000,9.960632,4.742178,10.904754,4.534288,-0.281060,-0.222186,1.121760
+0.020000,9.999516,4.823978,10.995326,4.528589,-0.284940,-0.194007,1.132332
+0.020000,10.037489,4.906076,11.085780,4.522730,-0.292957,-0.400840,1.142788
+0.020000,10.074559,4.988458,11.176118,4.516889,-0.292032,0.046230,1.153126
+0.020000,10.110732,5.071109,11.266338,4.511005,-0.294213,-0.109023,1.163347
+0.020000,10.146018,5.154014,11.356440,4.505086,-0.295952,-0.086979,1.173449
+0.020000,10.180423,5.237158,11.446421,4.499070,-0.300807,-0.242725,1.183430
+0.020000,10.213956,5.320527,11.536281,4.493002,-0.303356,-0.127448,1.193291
+0.020000,10.246627,5.404109,11.626022,4.487018,-0.299217,0.206919,1.203032
+0.020000,10.278445,5.487890,11.715641,4.480981,-0.301858,-0.132041,1.212651
+0.020000,10.309418,5.571856,11.805138,4.474832,-0.307433,-0.278742,1.222149
+0.020000,10.339556,5.655999,11.894515,4.468866,-0.298324,0.455465,1.231526
+0.020000,10.368870,5.740303,11.983770,4.462739,-0.306360,-0.401816,1.240782
+0.020000,10.397368,5.824759,12.072905,4.456728,-0.300510,0.292484,1.249917
+0.020000,10.425062,5.909355,12.161919,4.450708,-0.301043,-0.026632,1.258931
+0.020000,10.451960,5.994082,12.250813,4.444686,-0.301088,-0.002234,1.267825
+0.020000,10.478074,6.078929,12.339587,4.438730,-0.297774,0.165683,1.276600
+0.020000,10.503413,6.163886,12.428243,4.432793,-0.296842,0.046612,1.285256
+0.020000,10.527988,6.248944,12.516780,4.426853,-0.297019,-0.008836,1.293794
+0.020000,10.551810,6.334097,12.605202,4.421103,-0.287490,0.476417,1.302215
+0.020000,10.574888,6.419330,12.693505,4.415119,-0.299211,-0.586065,1.310519
+0.020000,10.597234,6.504641,12.781694,4.409447,-0.283606,0.780251,1.318708
+0.020000,10.618857,6.590021,12.869769,4.403785,-0.283108,0.024944,1.326783
+0.020000,10.639767,6.675459,12.957729,4.397983,-0.290086,-0.348943,1.334744
+0.020000,10.659976,6.760952,13.045578,4.392440,-0.277174,0.645644,1.342593
+0.020000,10.679493,6.846493,13.133317,4.386941,-0.274931,0.112124,1.350332
+0.020000,10.698329,6.932074,13.220946,4.381470,-0.273553,0.068920,1.357960
+0.020000,10.716490,7.017668,13.308446,4.374994,-0.323822,-2.513443,1.365479
+0.020000,10.733975,7.103221,13.395767,4.366071,-0.446102,-6.114006,1.372884
+0.020000,10.750785,7.188659,13.482843,4.353810,-0.613096,-8.349726,1.380172
+0.020000,10.766918,7.273911,13.569608,4.338225,-0.779203,-8.305359,1.387339
+0.020000,10.782378,7.358903,13.655995,4.319337,-0.944399,-8.259804,1.394382
+0.020000,10.797167,7.443564,13.741938,4.297164,-1.108655,-8.212771,1.401297
+0.020000,10.811293,7.527823,13.827372,4.271726,-1.271934,-8.163975,1.408082
+0.020000,10.824761,7.611608,13.912233,4.243042,-1.434197,-8.113139,1.414734
+0.020000,10.837580,7.694849,13.996456,4.211134,-1.595397,-8.059997,1.421251
+0.020000,10.849759,7.777477,14.079976,4.176024,-1.755483,-8.004291,1.427630
+0.020000,10.861310,7.859417,14.162727,4.137520,-1.925216,-8.486665,1.433870
+0.020000,10.872244,7.940604,14.244646,4.095978,-2.077107,-7.594519,1.439970
+0.020000,10.882574,8.020970,14.325673,4.051338,-2.231997,-7.744513,1.445927
+0.020000,10.892314,8.100438,14.405736,4.003154,-2.409197,-8.860008,1.451741
+0.020000,10.901479,8.178949,14.484781,3.952215,-2.546925,-6.886394,1.457411
+0.020000,10.910085,8.256427,14.562735,3.897730,-2.724262,-8.866846,1.462935
+0.020000,10.918148,8.332809,14.639541,3.840310,-2.870987,-7.336241,1.468313
+0.020000,10.925684,8.408027,14.715136,3.779715,-3.029752,-7.938266,1.473543
+0.020000,10.932712,8.482012,14.789454,3.715917,-3.189916,-8.008228,1.478627
+0.020000,10.939249,8.554697,14.862433,3.648938,-3.348968,-7.952577,1.483561
+0.020000,10.945314,8.626016,14.934009,3.578802,-3.506796,-7.891416,1.488348
+0.020000,10.950926,8.695902,15.004120,3.505536,-3.663288,-7.824592,1.492985
+0.020000,10.956104,8.764290,15.072703,3.429169,-3.818328,-7.751975,1.497472
+0.020000,10.960866,8.831115,15.139698,3.349733,-3.971797,-7.673453,1.501811
+0.020000,10.965233,8.896307,15.205036,3.266926,-4.140377,-8.429022,1.505998
+0.020000,10.969225,8.959807,15.268661,3.181244,-4.284080,-7.185138,1.510036
+0.020000,10.972860,9.021543,15.330504,3.092150,-4.454715,-8.531740,1.513923
+0.020000,10.976159,9.081455,15.390507,3.000147,-4.600142,-7.271379,1.517659
+0.020000,10.979140,9.139477,15.448606,2.904929,-4.760898,-8.037791,1.521244
+0.020000,10.981824,9.195543,15.504736,2.806529,-4.920030,-7.956589,1.524677
+0.020000,10.984230,9.249589,15.558836,2.704981,-5.077385,-7.867775,1.527958
+0.020000,10.986375,9.301552,15.610842,2.600325,-5.232811,-7.771282,1.531086
+0.020000,10.988281,9.351386,15.660713,2.493521,-5.340166,-5.367755,1.534063
+0.020000,10.989966,9.399074,15.708430,2.385892,-5.381466,-2.064990,1.536890
+0.020000,10.991450,9.444625,15.754006,2.278753,-5.356968,1.224902,1.539571
+0.020000,10.992751,9.488033,15.797433,2.171388,-5.368233,-0.563269,1.542108
+0.020000,10.993885,9.529306,15.838722,2.064438,-5.347514,1.035983,1.544505
+0.020000,10.994870,9.568445,15.877873,1.957556,-5.344097,0.170822,1.546764
+0.020000,10.995721,9.605451,15.914889,1.850775,-5.339033,0.253212,1.548887
+0.020000,10.996450,9.640325,15.949771,1.744129,-5.332327,0.335279,1.550877
+0.020000,10.997072,9.673091,15.982543,1.638566,-5.278144,2.709156,1.552737
+0.020000,10.997600,9.703795,16.013251,1.535408,-5.157862,6.014116,1.554471
+0.020000,10.998046,9.732511,16.041970,1.435988,-4.971023,9.341925,1.556086
+0.020000,10.998420,9.759302,16.068764,1.339689,-4.814933,7.804534,1.557587
+0.020000,10.998732,9.784240,16.093704,1.247011,-4.633937,9.049810,1.558978
+0.020000,10.998991,9.807390,16.116856,1.157574,-4.471848,8.104450,1.560264
+0.020000,10.999204,9.828817,16.138284,1.071412,-4.308065,8.189110,1.561451
+0.020000,10.999378,9.848592,16.158059,0.988743,-4.133488,8.728870,1.562543
+0.020000,10.999519,9.866779,16.176247,0.909415,-3.966365,8.356165,1.563544
+0.020000,10.999632,9.883448,16.192916,0.833430,-3.799285,8.353986,1.564460
+0.020000,10.999722,9.898660,16.208128,0.760627,-3.640156,7.956441,1.565293
+0.020000,10.999793,9.912486,16.221955,0.691334,-3.464630,8.776304,1.566049
+0.020000,10.999848,9.924995,16.234464,0.625435,-3.294968,8.483080,1.566731
+0.020000,10.999890,9.936248,16.245717,0.562637,-3.139865,7.755172,1.567344
+0.020000,10.999922,9.946314,16.255783,0.503339,-2.964917,8.747415,1.567891
+0.020000,10.999946,9.955260,16.264729,0.447270,-2.803432,8.074226,1.568377
+0.020000,10.999963,9.963152,16.272621,0.394629,-2.632051,8.569064,1.568805
+0.020000,10.999976,9.970054,16.279523,0.345079,-2.477535,7.725806,1.569179
+0.020000,10.999985,9.976036,16.285505,0.299102,-2.298825,8.935510,1.569503
+0.020000,10.999990,9.981163,16.290632,0.256361,-2.137043,8.089058,1.569780
+0.020000,10.999994,9.985498,16.294967,0.216726,-1.981782,7.763055,1.570014
+0.020000,10.999997,9.989110,16.298579,0.180613,-1.805644,8.806904,1.570209
+0.020000,10.999998,9.992065,16.301535,0.147785,-1.641381,8.213174,1.570368
+0.020000,10.999999,9.994430,16.303900,0.118248,-1.476865,8.225797,1.570496
+0.020000,11.000000,9.996271,16.305740,0.092005,-1.312144,8.236026,1.570595
+0.020000,11.000000,9.997652,16.307121,0.069060,-1.147262,8.244111,1.570670
+0.020000,11.000000,9.998640,16.308109,0.049414,-0.982256,8.250304,1.570723
+0.020000,11.000000,9.999301,16.308771,0.033071,-0.817159,8.254857,1.570759
+0.020000,11.000000,9.999702,16.309171,0.020031,-0.651998,8.258023,1.570780
+0.020000,11.000000,9.999908,16.309377,0.010295,-0.486797,8.260058,1.570791
+0.020000,11.000000,9.999985,16.309454,0.003864,-0.321573,8.261214,1.570796
+0.020000,11.000000,10.000000,16.309469,0.000737,-0.156338,8.261748,1.570796
+0.020000,11.000000,10.000000,16.309469,0.000000,-0.036860,5.973921,1.570796
diff --git a/src/main/deploy/output/anotherTestPath.left.pf1.csv b/src/main/deploy/output/anotherTestPath.left.pf1.csv
new file mode 100644
index 0000000..993953e
--- /dev/null
+++ b/src/main/deploy/output/anotherTestPath.left.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,9.250000,0.000012,0.001174,0.058707,2.935366,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,-0.058707,-5.870732,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,2.935366,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.001918,9.250000,0.001929,0.095879,4.793966,239.698284,0.000000
+0.020000,0.004559,9.250000,0.004571,0.132091,1.810608,-149.167885,0.000000
+0.020000,0.007565,9.250000,0.007577,0.150291,0.909963,-45.032225,0.000000
+0.020000,0.010959,9.250000,0.010970,0.169664,0.968671,2.935366,0.000000
+0.020000,0.014763,9.250000,0.014774,0.190212,1.027378,2.935366,0.000000
+0.020000,0.019001,9.250000,0.019013,0.211933,1.086085,2.935366,0.000000
+0.020000,0.023698,9.250000,0.023710,0.234829,1.144793,2.935366,0.000000
+0.020000,0.028876,9.250000,0.028888,0.258899,1.203500,2.935366,0.000000
+0.020000,0.034559,9.250000,0.034571,0.284143,1.262207,2.935366,0.000000
+0.020000,0.040770,9.250000,0.040782,0.310562,1.320915,2.935366,0.000000
+0.020000,0.047533,9.250000,0.047545,0.338154,1.379622,2.935366,0.000000
+0.020000,0.054872,9.250000,0.054883,0.366921,1.438329,2.935366,0.000000
+0.020000,0.062809,9.250000,0.062821,0.396861,1.497037,2.935366,0.000000
+0.020000,0.071368,9.250000,0.071380,0.427976,1.555744,2.935366,0.000000
+0.020000,0.080574,9.250000,0.080585,0.460265,1.614451,2.935366,0.000000
+0.020000,0.090448,9.250000,0.090460,0.493729,1.673159,2.935366,0.000000
+0.020000,0.101016,9.250000,0.101027,0.528366,1.731866,2.935366,0.000000
+0.020000,0.112299,9.250000,0.112311,0.564177,1.790573,2.935366,0.000000
+0.020000,0.124322,9.250000,0.124334,0.601163,1.849281,2.935366,0.000000
+0.020000,0.137109,9.250000,0.137121,0.639323,1.907988,2.935366,0.000000
+0.020000,0.150682,9.250000,0.150694,0.678657,1.966695,2.935366,0.000000
+0.020000,0.165065,9.250000,0.165077,0.719165,2.025403,2.935366,0.000000
+0.020000,0.180282,9.250000,0.180294,0.760847,2.084110,2.935366,0.000000
+0.020000,0.196356,9.250000,0.196368,0.803703,2.142817,2.935366,0.000000
+0.020000,0.213311,9.250000,0.213323,0.847734,2.201525,2.935366,0.000000
+0.020000,0.231170,9.250000,0.231181,0.892938,2.260232,2.935366,0.000000
+0.020000,0.249956,9.250000,0.249968,0.939317,2.318939,2.935366,0.000000
+0.020000,0.269693,9.250000,0.269705,0.986870,2.377647,2.935366,0.000000
+0.020000,0.290394,9.250000,0.290405,1.035010,2.407000,1.467683,0.000000
+0.020000,0.312057,9.250000,0.312068,1.083150,2.407000,0.000000,0.000000
+0.020000,0.334682,9.250000,0.334694,1.131290,2.407000,-0.000000,0.000000
+0.020000,0.358271,9.250000,0.358283,1.179430,2.407000,-0.000000,0.000000
+0.020000,0.382822,9.250000,0.382834,1.227570,2.407000,0.000000,0.000000
+0.020000,0.408337,9.250000,0.408348,1.275710,2.407000,-0.000000,0.000000
+0.020000,0.434814,9.250000,0.434825,1.323850,2.407000,-0.000000,0.000000
+0.020000,0.462253,9.250000,0.462265,1.371990,2.407000,0.000000,0.000000
+0.020000,0.490656,9.250000,0.490668,1.420130,2.407000,0.000000,0.000000
+0.020000,0.520021,9.250000,0.520033,1.468270,2.407000,-0.000000,0.000000
+0.020000,0.550350,9.250000,0.550361,1.516410,2.407000,0.000000,0.000000
+0.020000,0.581641,9.250000,0.581652,1.564550,2.407000,-0.000000,0.000000
+0.020000,0.613894,9.250000,0.613906,1.612690,2.407000,0.000000,0.000000
+0.020000,0.647111,9.250000,0.647123,1.660830,2.407000,-0.000000,0.000000
+0.020000,0.681290,9.250000,0.681302,1.708970,2.407000,0.000000,0.000000
+0.020000,0.716433,9.250000,0.716444,1.757110,2.407000,-0.000000,0.000000
+0.020000,0.752538,9.250000,0.752549,1.805250,2.407000,0.000000,0.000000
+0.020000,0.789605,9.250000,0.789617,1.853390,2.407000,-0.000000,0.000000
+0.020000,0.827636,9.250000,0.827648,1.901530,2.407000,0.000000,0.000000
+0.020000,0.866629,9.250000,0.866641,1.949670,2.407000,-0.000000,0.000000
+0.020000,0.906586,9.250000,0.906597,1.997810,2.407000,0.000000,0.000000
+0.020000,0.947505,9.250000,0.947516,2.045950,2.407000,-0.000000,0.000000
+0.020000,0.989386,9.250000,0.989398,2.094090,2.407000,0.000000,0.000000
+0.020000,1.032231,9.250000,1.032243,2.142230,2.407000,-0.000000,0.000000
+0.020000,1.076038,9.250000,1.076050,2.190370,2.407000,0.000000,0.000000
+0.020000,1.120809,9.250000,1.120820,2.238510,2.407000,-0.000000,0.000000
+0.020000,1.166542,9.250000,1.166553,2.286650,2.407000,0.000000,0.000000
+0.020000,1.213237,9.250000,1.213249,2.334790,2.407000,0.000000,0.000000
+0.020000,1.260896,9.250000,1.260908,2.382930,2.407000,0.000000,0.000000
+0.020000,1.309518,9.250000,1.309529,2.431070,2.407000,-0.000000,0.000000
+0.020000,1.359102,9.250000,1.359113,2.479210,2.407000,0.000000,0.000000
+0.020000,1.409649,9.250000,1.409660,2.527350,2.407000,-0.000000,0.000000
+0.020000,1.461159,9.250000,1.461170,2.575490,2.407000,0.000000,0.000000
+0.020000,1.513631,9.250000,1.513643,2.623630,2.407000,0.000000,0.000000
+0.020000,1.567067,9.250000,1.567078,2.671770,2.407000,-0.000000,0.000000
+0.020000,1.621465,9.250000,1.621476,2.719910,2.407000,0.000000,0.000000
+0.020000,1.676826,9.250000,1.676837,2.768050,2.407000,0.000000,0.000000
+0.020000,1.733150,9.250000,1.733161,2.816190,2.407000,0.000000,0.000000
+0.020000,1.790436,9.250000,1.790448,2.864330,2.407000,0.000000,0.000000
+0.020000,1.848686,9.250000,1.848697,2.912470,2.407000,-0.000000,0.000000
+0.020000,1.907898,9.250000,1.907909,2.960610,2.407000,0.000000,0.000000
+0.020000,1.968073,9.250000,1.968084,3.008750,2.407000,-0.000000,0.000000
+0.020000,2.029211,9.250000,2.029222,3.056890,2.407000,-0.000000,0.000000
+0.020000,2.091299,9.250000,2.091311,3.104443,2.377647,-1.467683,0.000000
+0.020000,2.154316,9.250000,2.154328,3.150822,2.318939,-2.935366,0.000000
+0.020000,2.218236,9.250000,2.218248,3.196027,2.260232,-2.935366,0.000000
+0.020000,2.283038,9.250000,2.283049,3.240057,2.201525,-2.935366,0.000000
+0.020000,2.348696,9.250000,2.348708,3.282913,2.142817,-2.935366,0.000000
+0.020000,2.415188,9.250000,2.415199,3.324596,2.084110,-2.935366,0.000000
+0.020000,2.482490,9.250000,2.482502,3.365104,2.025403,-2.935366,0.000000
+0.020000,2.550579,9.250000,2.550590,3.404438,1.966695,-2.935366,0.000000
+0.020000,2.619430,9.250000,2.619442,3.442597,1.907988,-2.935366,0.000000
+0.020000,2.689022,9.250000,2.689034,3.479583,1.849281,-2.935366,0.000000
+0.020000,2.759330,9.250000,2.759342,3.515394,1.790573,-2.935366,0.000000
+0.020000,2.830331,9.250000,2.830342,3.550032,1.731866,-2.935366,0.000000
+0.020000,2.902001,9.250000,2.902012,3.583495,1.673159,-2.935366,0.000000
+0.020000,2.974316,9.250000,2.974328,3.615784,1.614451,-2.935366,0.000000
+0.020000,3.047254,9.250000,3.047266,3.646899,1.555744,-2.935366,0.000000
+0.020000,3.120791,9.250000,3.120803,3.676840,1.497037,-2.935366,0.000000
+0.020000,3.194903,9.250000,3.194915,3.705606,1.438329,-2.935366,0.000000
+0.020000,3.269567,9.250000,3.269579,3.733199,1.379622,-2.935366,0.000000
+0.020000,3.344759,9.250000,3.344771,3.759617,1.320915,-2.935366,0.000000
+0.020000,3.420457,9.250000,3.420468,3.784861,1.262207,-2.935366,0.000000
+0.020000,3.496635,9.250000,3.496647,3.808931,1.203500,-2.935366,0.000000
+0.020000,3.573272,9.250000,3.573284,3.831827,1.144793,-2.935366,0.000000
+0.020000,3.650343,9.250000,3.650355,3.853549,1.086085,-2.935366,0.000000
+0.020000,3.727825,9.250000,3.727836,3.874096,1.027378,-2.935366,0.000000
+0.020000,3.805694,9.250000,3.805706,3.893470,0.968671,-2.935366,0.000000
+0.020000,3.883927,9.250000,3.883939,3.911669,0.909963,-2.935366,0.000000
+0.020000,3.962501,9.250000,3.962513,3.928694,0.851256,-2.935366,0.000000
+0.020000,4.041392,9.250000,4.041404,3.944545,0.792549,-2.935366,0.000000
+0.020000,4.120577,9.250000,4.120588,3.959222,0.733842,-2.935366,0.000000
+0.020000,4.200031,9.250000,4.200043,3.972724,0.675134,-2.935366,0.000000
+0.020000,4.279732,9.250000,4.279744,3.985053,0.616427,-2.935366,0.000000
+0.020000,4.359656,9.250000,4.359668,3.996207,0.557720,-2.935366,0.000000
+0.020000,4.439780,9.250000,4.439792,4.006188,0.499012,-2.935366,0.000000
+0.020000,4.520080,9.250000,4.520092,4.014994,0.440305,-2.935366,0.000000
+0.020000,4.600533,9.250000,4.600544,4.022626,0.381598,-2.935366,0.000000
+0.020000,4.681114,9.250000,4.681126,4.029083,0.322890,-2.935366,0.000000
+0.020000,4.761802,9.250000,4.761813,4.034367,0.264183,-2.935366,0.000000
+0.020000,4.842571,9.250000,4.842583,4.038477,0.205476,-2.935366,0.000000
+0.020000,4.923399,9.250000,4.923411,4.041412,0.146768,-2.935366,0.000000
+0.020000,5.004260,9.250000,5.004272,4.043039,0.081337,-3.271559,0.000000
+0.020000,5.085115,9.250000,5.085127,4.042770,-0.013448,-4.739242,0.000000
+0.020000,5.165930,9.250000,5.165942,4.040740,-0.101509,-4.403049,0.000000
+0.020000,5.246681,9.250000,5.246693,4.037535,-0.160216,-2.935366,0.000000
+0.020000,5.327344,9.250000,5.327356,4.033157,-0.218923,-2.935366,0.000000
+0.020000,5.407896,9.250000,5.407908,4.027604,-0.277631,-2.935366,0.000000
+0.020000,5.488314,9.250000,5.488325,4.020877,-0.336338,-2.935366,0.000000
+0.020000,5.568573,9.250000,5.568585,4.012977,-0.395045,-2.935366,0.000000
+0.020000,5.648651,9.250000,5.648663,4.003902,-0.453753,-2.935366,0.000000
+0.020000,5.728524,9.250000,5.728536,3.993652,-0.512460,-2.935366,0.000000
+0.020000,5.808169,9.250000,5.808181,3.982229,-0.571167,-2.935366,0.000000
+0.020000,5.887562,9.250000,5.887573,3.969631,-0.629875,-2.935366,0.000000
+0.020000,5.966679,9.250000,5.966691,3.955860,-0.688582,-2.935366,0.000000
+0.020000,6.045497,9.250000,6.045509,3.940914,-0.747289,-2.935366,0.000000
+0.020000,6.123993,9.250000,6.124005,3.924794,-0.805997,-2.935366,0.000000
+0.020000,6.202143,9.250000,6.202155,3.907500,-0.864704,-2.935366,0.000000
+0.020000,6.279924,9.250000,6.279935,3.889032,-0.923411,-2.935366,0.000000
+0.020000,6.357311,9.250000,6.357323,3.869389,-0.982119,-2.935366,0.000000
+0.020000,6.434283,9.250000,6.434295,3.848573,-1.040826,-2.935366,0.000000
+0.020000,6.510814,9.250000,6.510826,3.826582,-1.099533,-2.935366,0.000000
+0.020000,6.586883,9.250000,6.586895,3.803417,-1.158240,-2.935366,0.000000
+0.020000,6.662464,9.250000,6.662476,3.779079,-1.216948,-2.935366,0.000000
+0.020000,6.737536,9.250000,6.737547,3.753565,-1.275655,-2.935366,0.000000
+0.020000,6.812073,9.250000,6.812085,3.726878,-1.334362,-2.935366,0.000000
+0.020000,6.886054,9.250000,6.886065,3.699017,-1.393070,-2.935366,0.000000
+0.020000,6.959453,9.250000,6.959465,3.669981,-1.451777,-2.935366,0.000000
+0.020000,7.032249,9.250000,7.032260,3.639772,-1.510484,-2.935366,0.000000
+0.020000,7.104416,9.250000,7.104428,3.608388,-1.569192,-2.935366,0.000000
+0.020000,7.175933,9.250000,7.175945,3.575830,-1.627899,-2.935366,0.000000
+0.020000,7.246775,9.250000,7.246787,3.542098,-1.686606,-2.935366,0.000000
+0.020000,7.316919,9.250000,7.316931,3.507191,-1.745314,-2.935366,0.000000
+0.020000,7.386341,9.250000,7.386353,3.471111,-1.804021,-2.935366,0.000000
+0.020000,7.455018,9.250000,7.455030,3.433856,-1.862728,-2.935366,0.000000
+0.020000,7.522927,9.250000,7.522938,3.395428,-1.921436,-2.935366,0.000000
+0.020000,7.590043,9.250000,7.590055,3.355825,-1.980143,-2.935366,0.000000
+0.020000,7.656344,9.250000,7.656356,3.315048,-2.038850,-2.935366,0.000000
+0.020000,7.721806,9.250000,7.721818,3.273097,-2.097558,-2.935366,0.000000
+0.020000,7.786405,9.250000,7.786417,3.229971,-2.156265,-2.935366,0.000000
+0.020000,7.850119,9.250000,7.850131,3.185672,-2.214972,-2.935366,0.000000
+0.020000,7.912923,9.250000,7.912935,3.140198,-2.273680,-2.935366,0.000000
+0.020000,7.974794,9.250000,7.974806,3.093551,-2.332387,-2.935366,0.000000
+0.020000,8.035711,9.250000,8.035723,3.045863,-2.384370,-2.599173,0.000000
+0.020000,8.095666,9.250000,8.095677,2.997723,-2.407000,-1.131490,0.000000
+0.020000,8.154657,9.250000,8.154669,2.949583,-2.407000,-0.000000,0.000000
+0.020000,8.212686,9.250000,8.212698,2.901443,-2.407000,0.000000,0.000000
+0.020000,8.269752,9.250000,8.269764,2.853303,-2.407000,-0.000000,0.000000
+0.020000,8.325855,9.250000,8.325867,2.805163,-2.407000,0.000000,0.000000
+0.020000,8.380996,9.250000,8.381008,2.757023,-2.407000,0.000000,0.000000
+0.020000,8.435174,9.250000,8.435185,2.708883,-2.407000,0.000000,0.000000
+0.020000,8.488388,9.250000,8.488400,2.660743,-2.407000,0.000000,0.000000
+0.020000,8.540641,9.250000,8.540652,2.612603,-2.407000,0.000000,0.000000
+0.020000,8.591930,9.250000,8.591942,2.564463,-2.407000,0.000000,0.000000
+0.020000,8.642256,9.250000,8.642268,2.516323,-2.407000,0.000000,0.000000
+0.020000,8.691620,9.250000,8.691632,2.468183,-2.407000,0.000000,0.000000
+0.020000,8.740021,9.250000,8.740032,2.420043,-2.407000,0.000000,0.000000
+0.020000,8.787459,9.250000,8.787471,2.371903,-2.407000,0.000000,0.000000
+0.020000,8.833934,9.250000,8.833946,2.323763,-2.407000,0.000000,0.000000
+0.020000,8.879447,9.250000,8.879458,2.275623,-2.407000,0.000000,0.000000
+0.020000,8.923996,9.250000,8.924008,2.227483,-2.407000,0.000000,0.000000
+0.020000,8.967583,9.250000,8.967595,2.179343,-2.407000,0.000000,0.000000
+0.020000,9.010207,9.250000,9.010219,2.131203,-2.407000,0.000000,0.000000
+0.020000,9.051868,9.250000,9.051880,2.083063,-2.407000,0.000000,0.000000
+0.020000,9.092567,9.250000,9.092579,2.034923,-2.407000,0.000000,0.000000
+0.020000,9.132303,9.250000,9.132314,1.986783,-2.407000,0.000000,0.000000
+0.020000,9.171075,9.250000,9.171087,1.938643,-2.407000,0.000000,0.000000
+0.020000,9.208885,9.250000,9.208897,1.890503,-2.407000,0.000000,0.000000
+0.020000,9.245733,9.250000,9.245744,1.842363,-2.407000,0.000000,0.000000
+0.020000,9.281617,9.250000,9.281629,1.794223,-2.407000,0.000000,0.000000
+0.020000,9.316539,9.250000,9.316551,1.746083,-2.407000,0.000000,0.000000
+0.020000,9.350498,9.250000,9.350509,1.697943,-2.407000,-0.000000,0.000000
+0.020000,9.383494,9.250000,9.383505,1.649803,-2.407000,0.000000,0.000000
+0.020000,9.415527,9.250000,9.415539,1.601663,-2.407000,-0.000000,0.000000
+0.020000,9.446597,9.250000,9.446609,1.553523,-2.407000,0.000000,0.000000
+0.020000,9.476705,9.250000,9.476717,1.505383,-2.407000,-0.000000,0.000000
+0.020000,9.505850,9.250000,9.505862,1.457243,-2.407000,0.000000,0.000000
+0.020000,9.534032,9.250000,9.534044,1.409103,-2.407000,0.000000,0.000000
+0.020000,9.561251,9.250000,9.561263,1.360963,-2.407000,-0.000000,0.000000
+0.020000,9.587508,9.250000,9.587519,1.312823,-2.407000,0.000000,0.000000
+0.020000,9.612801,9.250000,9.612813,1.264683,-2.407000,-0.000000,0.000000
+0.020000,9.637132,9.250000,9.637144,1.216543,-2.407000,0.000000,0.000000
+0.020000,9.660500,9.250000,9.660512,1.168403,-2.407000,0.000000,0.000000
+0.020000,9.682906,9.250000,9.682917,1.120263,-2.407000,-0.000000,0.000000
+0.020000,9.704348,9.250000,9.704360,1.072123,-2.407000,0.000000,0.000000
+0.020000,9.724828,9.250000,9.724839,1.023983,-2.407000,-0.000000,0.000000
+0.020000,9.744347,9.250000,9.744359,0.975977,-2.400276,0.336193,0.000000
+0.020000,9.762921,9.250000,9.762933,0.928693,-2.364199,1.803876,0.000000
+0.020000,9.780573,9.250000,9.780585,0.882584,-2.305491,2.935366,0.000000
+0.020000,9.797326,9.250000,9.797337,0.837648,-2.246784,2.935366,0.000000
+0.020000,9.813203,9.250000,9.813215,0.793886,-2.188077,2.935366,0.000000
+0.020000,9.828229,9.250000,9.828241,0.751299,-2.129370,2.935366,0.000000
+0.020000,9.842427,9.250000,9.842439,0.709886,-2.070662,2.935366,0.000000
+0.020000,9.855820,9.250000,9.855832,0.669647,-2.011955,2.935366,0.000000
+0.020000,9.868432,9.250000,9.868443,0.630582,-1.953248,2.935366,0.000000
+0.020000,9.880286,9.250000,9.880297,0.592691,-1.894540,2.935366,0.000000
+0.020000,9.891405,9.250000,9.891417,0.555974,-1.835833,2.935366,0.000000
+0.020000,9.901814,9.250000,9.901825,0.520432,-1.777126,2.935366,0.000000
+0.020000,9.911535,9.250000,9.911547,0.486063,-1.718418,2.935366,0.000000
+0.020000,9.920592,9.250000,9.920604,0.452869,-1.659711,2.935366,0.000000
+0.020000,9.929009,9.250000,9.929021,0.420849,-1.601004,2.935366,0.000000
+0.020000,9.936809,9.250000,9.936821,0.390003,-1.542296,2.935366,0.000000
+0.020000,9.944016,9.250000,9.944028,0.360331,-1.483589,2.935366,0.000000
+0.020000,9.950653,9.250000,9.950664,0.331834,-1.424882,2.935366,0.000000
+0.020000,9.956743,9.250000,9.956755,0.304510,-1.366174,2.935366,0.000000
+0.020000,9.962310,9.250000,9.962322,0.278361,-1.307467,2.935366,0.000000
+0.020000,9.967378,9.250000,9.967390,0.253386,-1.248760,2.935366,0.000000
+0.020000,9.971970,9.250000,9.971981,0.229585,-1.190052,2.935366,0.000000
+0.020000,9.976109,9.250000,9.976120,0.206958,-1.131345,2.935366,0.000000
+0.020000,9.979819,9.250000,9.979831,0.185505,-1.072638,2.935366,0.000000
+0.020000,9.983123,9.250000,9.983135,0.165226,-1.013930,2.935366,0.000000
+0.020000,9.986046,9.250000,9.986057,0.146122,-0.955223,2.935366,0.000000
+0.020000,9.988610,9.250000,9.988621,0.128192,-0.896516,2.935366,0.000000
+0.020000,9.990838,9.250000,9.990850,0.111435,-0.837808,2.935366,0.000000
+0.020000,9.992755,9.250000,9.992767,0.095853,-0.779101,2.935366,0.000000
+0.020000,9.994384,9.250000,9.994396,0.081446,-0.720394,2.935366,0.000000
+0.020000,9.995748,9.250000,9.995760,0.068212,-0.661686,2.935366,0.000000
+0.020000,9.996872,9.250000,9.996883,0.056152,-0.602979,2.935366,0.000000
+0.020000,9.997777,9.250000,9.997789,0.045267,-0.544272,2.935366,0.000000
+0.020000,9.998488,9.250000,9.998500,0.035556,-0.485565,2.935366,0.000000
+0.020000,9.999028,9.250000,9.999040,0.027018,-0.426857,2.935366,0.000000
+0.020000,9.999421,9.250000,9.999433,0.019655,-0.368150,2.935366,0.000000
+0.020000,9.999691,9.250000,9.999703,0.013467,-0.309443,2.935366,0.000000
+0.020000,9.999860,9.250000,9.999872,0.008452,-0.250735,2.935366,0.000000
+0.020000,9.999952,9.250000,9.999964,0.004611,-0.192028,2.935366,0.000000
+0.020000,9.999991,9.250000,10.000003,0.001945,-0.133321,2.935366,0.000000
+0.020000,10.000000,9.250000,10.000012,0.000453,-0.074613,2.935366,0.000000
+0.020000,10.000000,9.250000,10.000012,0.000000,-0.022630,2.599173,0.000000
diff --git a/src/main/deploy/output/anotherTestPath.pf1.csv b/src/main/deploy/output/anotherTestPath.pf1.csv
new file mode 100644
index 0000000..076f14d
--- /dev/null
+++ b/src/main/deploy/output/anotherTestPath.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,8.000000,0.000012,0.001174,0.058707,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000059,0.003522,0.117415,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000164,0.007045,0.176122,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000352,0.011741,0.234829,2.935366,0.000000
+0.020000,0.000000,8.000000,0.000646,0.017612,0.293537,2.935366,0.000000
+0.020000,0.000000,8.000000,0.001068,0.024657,0.352244,2.935366,0.000000
+0.020000,0.000000,8.000000,0.001644,0.032876,0.410951,2.935366,0.000000
+0.020000,0.000000,8.000000,0.002395,0.042269,0.469659,2.935366,0.000000
+0.020000,0.000000,8.000000,0.003346,0.052837,0.528366,2.935366,0.000000
+0.020000,0.000000,8.000000,0.004520,0.064578,0.587073,2.935366,0.000000
+0.020000,0.000000,8.000000,0.005941,0.077494,0.645781,2.935366,0.000000
+0.020000,0.000000,8.000000,0.007632,0.091583,0.704488,2.935366,0.000000
+0.020000,0.000000,8.000000,0.009616,0.106847,0.763195,2.935366,0.000000
+0.020000,0.001918,8.000000,0.011918,0.123285,0.821903,2.935366,0.000000
+0.020000,0.004559,8.000000,0.014559,0.140898,0.880610,2.935366,0.000000
+0.020000,0.007565,8.000000,0.017565,0.159684,0.939317,2.935366,0.000000
+0.020000,0.010959,8.000000,0.020959,0.179644,0.998024,2.935366,0.000000
+0.020000,0.014763,8.000000,0.024763,0.200779,1.056732,2.935366,0.000000
+0.020000,0.019001,8.000000,0.029001,0.223088,1.115439,2.935366,0.000000
+0.020000,0.023698,8.000000,0.033698,0.246571,1.174146,2.935366,0.000000
+0.020000,0.028876,8.000000,0.038876,0.271228,1.232854,2.935366,0.000000
+0.020000,0.034559,8.000000,0.044559,0.297059,1.291561,2.935366,0.000000
+0.020000,0.040770,8.000000,0.050770,0.324064,1.350268,2.935366,0.000000
+0.020000,0.047533,8.000000,0.057533,0.352244,1.408976,2.935366,0.000000
+0.020000,0.054872,8.000000,0.064872,0.381598,1.467683,2.935366,0.000000
+0.020000,0.062809,8.000000,0.072809,0.412125,1.526390,2.935366,0.000000
+0.020000,0.071368,8.000000,0.081368,0.443827,1.585098,2.935366,0.000000
+0.020000,0.080574,8.000000,0.090574,0.476703,1.643805,2.935366,0.000000
+0.020000,0.090448,8.000000,0.100448,0.510754,1.702512,2.935366,0.000000
+0.020000,0.101016,8.000000,0.111016,0.545978,1.761220,2.935366,0.000000
+0.020000,0.112299,8.000000,0.122299,0.582377,1.819927,2.935366,0.000000
+0.020000,0.124322,8.000000,0.134322,0.619949,1.878634,2.935366,0.000000
+0.020000,0.137109,8.000000,0.147109,0.658696,1.937342,2.935366,0.000000
+0.020000,0.150682,8.000000,0.160682,0.698617,1.996049,2.935366,0.000000
+0.020000,0.165065,8.000000,0.175065,0.739712,2.054756,2.935366,0.000000
+0.020000,0.180282,8.000000,0.190282,0.781982,2.113464,2.935366,0.000000
+0.020000,0.196356,8.000000,0.206356,0.825425,2.172171,2.935366,0.000000
+0.020000,0.213311,8.000000,0.223311,0.870043,2.230878,2.935366,0.000000
+0.020000,0.231170,8.000000,0.241170,0.915834,2.289586,2.935366,0.000000
+0.020000,0.249956,8.000000,0.259956,0.962800,2.348293,2.935366,0.000000
+0.020000,0.269693,8.000000,0.279693,1.010940,2.407000,2.935366,0.000000
+0.020000,0.290394,8.000000,0.300394,1.059080,2.407000,0.000000,0.000000
+0.020000,0.312057,8.000000,0.322057,1.107220,2.407000,0.000000,0.000000
+0.020000,0.334682,8.000000,0.344682,1.155360,2.407000,-0.000000,0.000000
+0.020000,0.358271,8.000000,0.368271,1.203500,2.407000,0.000000,0.000000
+0.020000,0.382822,8.000000,0.392822,1.251640,2.407000,0.000000,0.000000
+0.020000,0.408337,8.000000,0.418337,1.299780,2.407000,0.000000,0.000000
+0.020000,0.434814,8.000000,0.444814,1.347920,2.407000,-0.000000,0.000000
+0.020000,0.462253,8.000000,0.472253,1.396060,2.407000,0.000000,0.000000
+0.020000,0.490656,8.000000,0.500656,1.444200,2.407000,0.000000,0.000000
+0.020000,0.520021,8.000000,0.530021,1.492340,2.407000,0.000000,0.000000
+0.020000,0.550350,8.000000,0.560350,1.540480,2.407000,-0.000000,0.000000
+0.020000,0.581641,8.000000,0.591641,1.588620,2.407000,0.000000,0.000000
+0.020000,0.613894,8.000000,0.623894,1.636760,2.407000,0.000000,0.000000
+0.020000,0.647111,8.000000,0.657111,1.684900,2.407000,0.000000,0.000000
+0.020000,0.681290,8.000000,0.691290,1.733040,2.407000,-0.000000,0.000000
+0.020000,0.716433,8.000000,0.726433,1.781180,2.407000,0.000000,0.000000
+0.020000,0.752538,8.000000,0.762538,1.829320,2.407000,0.000000,0.000000
+0.020000,0.789605,8.000000,0.799605,1.877460,2.407000,-0.000000,0.000000
+0.020000,0.827636,8.000000,0.837636,1.925600,2.407000,0.000000,0.000000
+0.020000,0.866629,8.000000,0.876629,1.973740,2.407000,0.000000,0.000000
+0.020000,0.906586,8.000000,0.916586,2.021880,2.407000,0.000000,0.000000
+0.020000,0.947505,8.000000,0.957505,2.070020,2.407000,-0.000000,0.000000
+0.020000,0.989386,8.000000,0.999386,2.118160,2.407000,0.000000,0.000000
+0.020000,1.032231,8.000000,1.042231,2.166300,2.407000,-0.000000,0.000000
+0.020000,1.076038,8.000000,1.086038,2.214440,2.407000,0.000000,0.000000
+0.020000,1.120809,8.000000,1.130809,2.262580,2.407000,-0.000000,0.000000
+0.020000,1.166542,8.000000,1.176542,2.310720,2.407000,0.000000,0.000000
+0.020000,1.213237,8.000000,1.223237,2.358860,2.407000,0.000000,0.000000
+0.020000,1.260896,8.000000,1.270896,2.407000,2.407000,-0.000000,0.000000
+0.020000,1.309518,8.000000,1.319518,2.455140,2.407000,0.000000,0.000000
+0.020000,1.359102,8.000000,1.369102,2.503280,2.407000,0.000000,0.000000
+0.020000,1.409649,8.000000,1.419649,2.551420,2.407000,0.000000,0.000000
+0.020000,1.461159,8.000000,1.471159,2.599560,2.407000,0.000000,0.000000
+0.020000,1.513631,8.000000,1.523631,2.647700,2.407000,0.000000,0.000000
+0.020000,1.567067,8.000000,1.577067,2.695840,2.407000,-0.000000,0.000000
+0.020000,1.621465,8.000000,1.631465,2.743980,2.407000,0.000000,0.000000
+0.020000,1.676826,8.000000,1.686826,2.792120,2.407000,0.000000,0.000000
+0.020000,1.733150,8.000000,1.743150,2.840260,2.407000,-0.000000,0.000000
+0.020000,1.790436,8.000000,1.800436,2.888400,2.407000,0.000000,0.000000
+0.020000,1.848686,8.000000,1.858686,2.936540,2.407000,-0.000000,0.000000
+0.020000,1.907898,8.000000,1.917898,2.984680,2.407000,0.000000,0.000000
+0.020000,1.968073,8.000000,1.978073,3.032820,2.407000,-0.000000,0.000000
+0.020000,2.029211,8.000000,2.039211,3.080960,2.407000,0.000000,0.000000
+0.020000,2.091299,8.000000,2.101299,3.127926,2.348293,-2.935366,0.000000
+0.020000,2.154316,8.000000,2.164316,3.173718,2.289586,-2.935366,0.000000
+0.020000,2.218236,8.000000,2.228236,3.218335,2.230878,-2.935366,0.000000
+0.020000,2.283038,8.000000,2.293038,3.261779,2.172171,-2.935366,0.000000
+0.020000,2.348696,8.000000,2.358696,3.304048,2.113464,-2.935366,0.000000
+0.020000,2.415188,8.000000,2.425188,3.345143,2.054756,-2.935366,0.000000
+0.020000,2.482490,8.000000,2.492490,3.385064,1.996049,-2.935366,0.000000
+0.020000,2.550579,8.000000,2.560579,3.423811,1.937342,-2.935366,0.000000
+0.020000,2.619430,8.000000,2.629430,3.461384,1.878634,-2.935366,0.000000
+0.020000,2.689022,8.000000,2.699022,3.497782,1.819927,-2.935366,0.000000
+0.020000,2.759330,8.000000,2.769330,3.533007,1.761220,-2.935366,0.000000
+0.020000,2.830331,8.000000,2.840331,3.567057,1.702512,-2.935366,0.000000
+0.020000,2.902001,8.000000,2.912001,3.599933,1.643805,-2.935366,0.000000
+0.020000,2.974316,8.000000,2.984316,3.631635,1.585098,-2.935366,0.000000
+0.020000,3.047254,8.000000,3.057254,3.662163,1.526390,-2.935366,0.000000
+0.020000,3.120791,8.000000,3.130791,3.691516,1.467683,-2.935366,0.000000
+0.020000,3.194903,8.000000,3.204903,3.719696,1.408976,-2.935366,0.000000
+0.020000,3.269567,8.000000,3.279567,3.746701,1.350268,-2.935366,0.000000
+0.020000,3.344759,8.000000,3.354759,3.772532,1.291561,-2.935366,0.000000
+0.020000,3.420457,8.000000,3.430457,3.797190,1.232854,-2.935366,0.000000
+0.020000,3.496635,8.000000,3.506635,3.820672,1.174146,-2.935366,0.000000
+0.020000,3.573272,8.000000,3.583272,3.842981,1.115439,-2.935366,0.000000
+0.020000,3.650343,8.000000,3.660343,3.864116,1.056732,-2.935366,0.000000
+0.020000,3.727825,8.000000,3.737825,3.884076,0.998024,-2.935366,0.000000
+0.020000,3.805694,8.000000,3.815694,3.902863,0.939317,-2.935366,0.000000
+0.020000,3.883927,8.000000,3.893927,3.920475,0.880610,-2.935366,0.000000
+0.020000,3.962501,8.000000,3.972501,3.936913,0.821903,-2.935366,0.000000
+0.020000,4.041392,8.000000,4.051392,3.952177,0.763195,-2.935366,0.000000
+0.020000,4.120577,8.000000,4.130577,3.966267,0.704488,-2.935366,0.000000
+0.020000,4.200031,8.000000,4.210031,3.979182,0.645781,-2.935366,0.000000
+0.020000,4.279732,8.000000,4.289732,3.990924,0.587073,-2.935366,0.000000
+0.020000,4.359656,8.000000,4.369656,4.001491,0.528366,-2.935366,0.000000
+0.020000,4.439780,8.000000,4.449780,4.010884,0.469659,-2.935366,0.000000
+0.020000,4.520080,8.000000,4.530080,4.019103,0.410951,-2.935366,0.000000
+0.020000,4.600533,8.000000,4.610533,4.026148,0.352244,-2.935366,0.000000
+0.020000,4.681114,8.000000,4.691114,4.032019,0.293537,-2.935366,0.000000
+0.020000,4.761802,8.000000,4.771802,4.036715,0.234829,-2.935366,0.000000
+0.020000,4.842571,8.000000,4.852571,4.040238,0.176122,-2.935366,0.000000
+0.020000,4.923399,8.000000,4.933399,4.042586,0.117415,-2.935366,0.000000
+0.020000,5.004260,8.000000,5.014260,4.043491,0.045260,-3.607752,0.000000
+0.020000,5.085115,8.000000,5.095115,4.042048,-0.072155,-5.870732,0.000000
+0.020000,5.165930,8.000000,5.175930,4.039431,-0.130862,-2.935366,0.000000
+0.020000,5.246681,8.000000,5.256681,4.035640,-0.189570,-2.935366,0.000000
+0.020000,5.327344,8.000000,5.337344,4.030674,-0.248277,-2.935366,0.000000
+0.020000,5.407896,8.000000,5.417896,4.024534,-0.306984,-2.935366,0.000000
+0.020000,5.488314,8.000000,5.498314,4.017221,-0.365692,-2.935366,0.000000
+0.020000,5.568573,8.000000,5.578573,4.008733,-0.424399,-2.935366,0.000000
+0.020000,5.648651,8.000000,5.658651,3.999070,-0.483106,-2.935366,0.000000
+0.020000,5.728524,8.000000,5.738524,3.988234,-0.541814,-2.935366,0.000000
+0.020000,5.808169,8.000000,5.818169,3.976224,-0.600521,-2.935366,0.000000
+0.020000,5.887562,8.000000,5.897562,3.963039,-0.659228,-2.935366,0.000000
+0.020000,5.966679,8.000000,5.976679,3.948680,-0.717936,-2.935366,0.000000
+0.020000,6.045497,8.000000,6.055497,3.933148,-0.776643,-2.935366,0.000000
+0.020000,6.123993,8.000000,6.133993,3.916441,-0.835350,-2.935366,0.000000
+0.020000,6.202143,8.000000,6.212143,3.898559,-0.894058,-2.935366,0.000000
+0.020000,6.279924,8.000000,6.289924,3.879504,-0.952765,-2.935366,0.000000
+0.020000,6.357311,8.000000,6.367311,3.859275,-1.011472,-2.935366,0.000000
+0.020000,6.434283,8.000000,6.444283,3.837871,-1.070179,-2.935366,0.000000
+0.020000,6.510814,8.000000,6.520814,3.815293,-1.128887,-2.935366,0.000000
+0.020000,6.586883,8.000000,6.596883,3.791542,-1.187594,-2.935366,0.000000
+0.020000,6.662464,8.000000,6.672464,3.766615,-1.246301,-2.935366,0.000000
+0.020000,6.737536,8.000000,6.747536,3.740515,-1.305009,-2.935366,0.000000
+0.020000,6.812073,8.000000,6.822073,3.713241,-1.363716,-2.935366,0.000000
+0.020000,6.886054,8.000000,6.896054,3.684793,-1.422423,-2.935366,0.000000
+0.020000,6.959453,8.000000,6.969453,3.655170,-1.481131,-2.935366,0.000000
+0.020000,7.032249,8.000000,7.042249,3.624373,-1.539838,-2.935366,0.000000
+0.020000,7.104416,8.000000,7.114416,3.592402,-1.598545,-2.935366,0.000000
+0.020000,7.175933,8.000000,7.185933,3.559257,-1.657253,-2.935366,0.000000
+0.020000,7.246775,8.000000,7.256775,3.524938,-1.715960,-2.935366,0.000000
+0.020000,7.316919,8.000000,7.326919,3.489445,-1.774667,-2.935366,0.000000
+0.020000,7.386341,8.000000,7.396341,3.452777,-1.833375,-2.935366,0.000000
+0.020000,7.455018,8.000000,7.465018,3.414936,-1.892082,-2.935366,0.000000
+0.020000,7.522927,8.000000,7.532927,3.375920,-1.950789,-2.935366,0.000000
+0.020000,7.590043,8.000000,7.600043,3.335730,-2.009497,-2.935366,0.000000
+0.020000,7.656344,8.000000,7.666344,3.294366,-2.068204,-2.935366,0.000000
+0.020000,7.721806,8.000000,7.731806,3.251827,-2.126911,-2.935366,0.000000
+0.020000,7.786405,8.000000,7.796405,3.208115,-2.185619,-2.935366,0.000000
+0.020000,7.850119,8.000000,7.860119,3.163229,-2.244326,-2.935366,0.000000
+0.020000,7.912923,8.000000,7.922923,3.117168,-2.303033,-2.935366,0.000000
+0.020000,7.974794,8.000000,7.984794,3.069933,-2.361741,-2.935366,0.000000
+0.020000,8.035711,8.000000,8.045711,3.021793,-2.407000,-2.262981,0.000000
+0.020000,8.095666,8.000000,8.105666,2.973653,-2.407000,-0.000000,0.000000
+0.020000,8.154657,8.000000,8.164657,2.925513,-2.407000,0.000000,0.000000
+0.020000,8.212686,8.000000,8.222686,2.877373,-2.407000,0.000000,0.000000
+0.020000,8.269752,8.000000,8.279752,2.829233,-2.407000,-0.000000,0.000000
+0.020000,8.325855,8.000000,8.335855,2.781093,-2.407000,0.000000,0.000000
+0.020000,8.380996,8.000000,8.390996,2.732953,-2.407000,-0.000000,0.000000
+0.020000,8.435174,8.000000,8.445174,2.684813,-2.407000,0.000000,0.000000
+0.020000,8.488388,8.000000,8.498388,2.636673,-2.407000,-0.000000,0.000000
+0.020000,8.540641,8.000000,8.550641,2.588533,-2.407000,0.000000,0.000000
+0.020000,8.591930,8.000000,8.601930,2.540393,-2.407000,0.000000,0.000000
+0.020000,8.642256,8.000000,8.652256,2.492253,-2.407000,-0.000000,0.000000
+0.020000,8.691620,8.000000,8.701620,2.444113,-2.407000,0.000000,0.000000
+0.020000,8.740021,8.000000,8.750021,2.395973,-2.407000,-0.000000,0.000000
+0.020000,8.787459,8.000000,8.797459,2.347833,-2.407000,0.000000,0.000000
+0.020000,8.833934,8.000000,8.843934,2.299693,-2.407000,0.000000,0.000000
+0.020000,8.879447,8.000000,8.889447,2.251553,-2.407000,0.000000,0.000000
+0.020000,8.923996,8.000000,8.933996,2.203413,-2.407000,-0.000000,0.000000
+0.020000,8.967583,8.000000,8.977583,2.155273,-2.407000,0.000000,0.000000
+0.020000,9.010207,8.000000,9.020207,2.107133,-2.407000,0.000000,0.000000
+0.020000,9.051868,8.000000,9.061868,2.058993,-2.407000,0.000000,0.000000
+0.020000,9.092567,8.000000,9.102567,2.010853,-2.407000,0.000000,0.000000
+0.020000,9.132303,8.000000,9.142303,1.962713,-2.407000,0.000000,0.000000
+0.020000,9.171075,8.000000,9.181075,1.914573,-2.407000,-0.000000,0.000000
+0.020000,9.208885,8.000000,9.218885,1.866433,-2.407000,0.000000,0.000000
+0.020000,9.245733,8.000000,9.255733,1.818293,-2.407000,0.000000,0.000000
+0.020000,9.281617,8.000000,9.291617,1.770153,-2.407000,0.000000,0.000000
+0.020000,9.316539,8.000000,9.326539,1.722013,-2.407000,0.000000,0.000000
+0.020000,9.350498,8.000000,9.360498,1.673873,-2.407000,-0.000000,0.000000
+0.020000,9.383494,8.000000,9.393494,1.625733,-2.407000,0.000000,0.000000
+0.020000,9.415527,8.000000,9.425527,1.577593,-2.407000,0.000000,0.000000
+0.020000,9.446597,8.000000,9.456597,1.529453,-2.407000,0.000000,0.000000
+0.020000,9.476705,8.000000,9.486705,1.481313,-2.407000,-0.000000,0.000000
+0.020000,9.505850,8.000000,9.515850,1.433173,-2.407000,0.000000,0.000000
+0.020000,9.534032,8.000000,9.544032,1.385033,-2.407000,0.000000,0.000000
+0.020000,9.561251,8.000000,9.571251,1.336893,-2.407000,0.000000,0.000000
+0.020000,9.587508,8.000000,9.597508,1.288753,-2.407000,-0.000000,0.000000
+0.020000,9.612801,8.000000,9.622801,1.240613,-2.407000,0.000000,0.000000
+0.020000,9.637132,8.000000,9.647132,1.192473,-2.407000,0.000000,0.000000
+0.020000,9.660500,8.000000,9.670500,1.144333,-2.407000,0.000000,0.000000
+0.020000,9.682906,8.000000,9.692906,1.096193,-2.407000,0.000000,0.000000
+0.020000,9.704348,8.000000,9.714348,1.048053,-2.407000,0.000000,0.000000
+0.020000,9.724828,8.000000,9.734828,0.999913,-2.407000,-0.000000,0.000000
+0.020000,9.744347,8.000000,9.754347,0.952042,-2.393552,0.672385,0.000000
+0.020000,9.762921,8.000000,9.772921,0.905345,-2.334845,2.935366,0.000000
+0.020000,9.780573,8.000000,9.790573,0.859822,-2.276138,2.935366,0.000000
+0.020000,9.797326,8.000000,9.807326,0.815474,-2.217431,2.935366,0.000000
+0.020000,9.813203,8.000000,9.823203,0.772299,-2.158723,2.935366,0.000000
+0.020000,9.828229,8.000000,9.838229,0.730299,-2.100016,2.935366,0.000000
+0.020000,9.842427,8.000000,9.852427,0.689473,-2.041309,2.935366,0.000000
+0.020000,9.855820,8.000000,9.865820,0.649821,-1.982601,2.935366,0.000000
+0.020000,9.868432,8.000000,9.878432,0.611343,-1.923894,2.935366,0.000000
+0.020000,9.880286,8.000000,9.890286,0.574039,-1.865187,2.935366,0.000000
+0.020000,9.891405,8.000000,9.901405,0.537909,-1.806479,2.935366,0.000000
+0.020000,9.901814,8.000000,9.911814,0.502954,-1.747772,2.935366,0.000000
+0.020000,9.911535,8.000000,9.921535,0.469173,-1.689065,2.935366,0.000000
+0.020000,9.920592,8.000000,9.930592,0.436566,-1.630357,2.935366,0.000000
+0.020000,9.929009,8.000000,9.939009,0.405133,-1.571650,2.935366,0.000000
+0.020000,9.936809,8.000000,9.946809,0.374874,-1.512943,2.935366,0.000000
+0.020000,9.944016,8.000000,9.954016,0.345789,-1.454235,2.935366,0.000000
+0.020000,9.950653,8.000000,9.960653,0.317878,-1.395528,2.935366,0.000000
+0.020000,9.956743,8.000000,9.966743,0.291142,-1.336821,2.935366,0.000000
+0.020000,9.962310,8.000000,9.972310,0.265580,-1.278113,2.935366,0.000000
+0.020000,9.967378,8.000000,9.977378,0.241192,-1.219406,2.935366,0.000000
+0.020000,9.971970,8.000000,9.981970,0.217978,-1.160699,2.935366,0.000000
+0.020000,9.976109,8.000000,9.986109,0.195938,-1.101991,2.935366,0.000000
+0.020000,9.979819,8.000000,9.989819,0.175072,-1.043284,2.935366,0.000000
+0.020000,9.983123,8.000000,9.993123,0.155381,-0.984577,2.935366,0.000000
+0.020000,9.986046,8.000000,9.996046,0.136863,-0.925869,2.935366,0.000000
+0.020000,9.988610,8.000000,9.998610,0.119520,-0.867162,2.935366,0.000000
+0.020000,9.990838,8.000000,10.000838,0.103351,-0.808455,2.935366,0.000000
+0.020000,9.992755,8.000000,10.002755,0.088356,-0.749747,2.935366,0.000000
+0.020000,9.994384,8.000000,10.004384,0.074535,-0.691040,2.935366,0.000000
+0.020000,9.995748,8.000000,10.005748,0.061889,-0.632333,2.935366,0.000000
+0.020000,9.996872,8.000000,10.006872,0.050416,-0.573626,2.935366,0.000000
+0.020000,9.997777,8.000000,10.007777,0.040118,-0.514918,2.935366,0.000000
+0.020000,9.998488,8.000000,10.008488,0.030993,-0.456211,2.935366,0.000000
+0.020000,9.999028,8.000000,10.009028,0.023043,-0.397504,2.935366,0.000000
+0.020000,9.999421,8.000000,10.009421,0.016267,-0.338796,2.935366,0.000000
+0.020000,9.999691,8.000000,10.009691,0.010666,-0.280089,2.935366,0.000000
+0.020000,9.999860,8.000000,10.009860,0.006238,-0.221382,2.935366,0.000000
+0.020000,9.999952,8.000000,10.009952,0.002985,-0.162674,2.935366,0.000000
+0.020000,9.999991,8.000000,10.009991,0.000905,-0.103967,2.935366,0.000000
+0.020000,10.000000,8.000000,10.010000,0.000000,-0.045260,2.935366,0.000000
+0.020000,10.000000,8.000000,10.010000,0.000000,0.000000,2.262981,0.000000
diff --git a/src/main/deploy/output/anotherTestPath.right.pf1.csv b/src/main/deploy/output/anotherTestPath.right.pf1.csv
new file mode 100644
index 0000000..524f3ab
--- /dev/null
+++ b/src/main/deploy/output/anotherTestPath.right.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,6.750000,0.000012,0.001174,0.058707,2.935366,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,-0.058707,-5.870732,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,2.935366,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.001918,6.750000,0.001929,0.095879,4.793966,239.698284,0.000000
+0.020000,0.004559,6.750000,0.004571,0.132091,1.810608,-149.167885,0.000000
+0.020000,0.007565,6.750000,0.007577,0.150291,0.909963,-45.032225,0.000000
+0.020000,0.010959,6.750000,0.010970,0.169664,0.968671,2.935366,0.000000
+0.020000,0.014763,6.750000,0.014774,0.190212,1.027378,2.935366,0.000000
+0.020000,0.019001,6.750000,0.019013,0.211933,1.086085,2.935366,0.000000
+0.020000,0.023698,6.750000,0.023710,0.234829,1.144793,2.935366,0.000000
+0.020000,0.028876,6.750000,0.028888,0.258899,1.203500,2.935366,0.000000
+0.020000,0.034559,6.750000,0.034571,0.284143,1.262207,2.935366,0.000000
+0.020000,0.040770,6.750000,0.040782,0.310562,1.320915,2.935366,0.000000
+0.020000,0.047533,6.750000,0.047545,0.338154,1.379622,2.935366,0.000000
+0.020000,0.054872,6.750000,0.054883,0.366921,1.438329,2.935366,0.000000
+0.020000,0.062809,6.750000,0.062821,0.396861,1.497037,2.935366,0.000000
+0.020000,0.071368,6.750000,0.071380,0.427976,1.555744,2.935366,0.000000
+0.020000,0.080574,6.750000,0.080585,0.460265,1.614451,2.935366,0.000000
+0.020000,0.090448,6.750000,0.090460,0.493729,1.673159,2.935366,0.000000
+0.020000,0.101016,6.750000,0.101027,0.528366,1.731866,2.935366,0.000000
+0.020000,0.112299,6.750000,0.112311,0.564177,1.790573,2.935366,0.000000
+0.020000,0.124322,6.750000,0.124334,0.601163,1.849281,2.935366,0.000000
+0.020000,0.137109,6.750000,0.137121,0.639323,1.907988,2.935366,0.000000
+0.020000,0.150682,6.750000,0.150694,0.678657,1.966695,2.935366,0.000000
+0.020000,0.165065,6.750000,0.165077,0.719165,2.025403,2.935366,0.000000
+0.020000,0.180282,6.750000,0.180294,0.760847,2.084110,2.935366,0.000000
+0.020000,0.196356,6.750000,0.196368,0.803703,2.142817,2.935366,0.000000
+0.020000,0.213311,6.750000,0.213323,0.847734,2.201525,2.935366,0.000000
+0.020000,0.231170,6.750000,0.231181,0.892938,2.260232,2.935366,0.000000
+0.020000,0.249956,6.750000,0.249968,0.939317,2.318939,2.935366,0.000000
+0.020000,0.269693,6.750000,0.269705,0.986870,2.377647,2.935366,0.000000
+0.020000,0.290394,6.750000,0.290405,1.035010,2.407000,1.467683,0.000000
+0.020000,0.312057,6.750000,0.312068,1.083150,2.407000,0.000000,0.000000
+0.020000,0.334682,6.750000,0.334694,1.131290,2.407000,-0.000000,0.000000
+0.020000,0.358271,6.750000,0.358283,1.179430,2.407000,-0.000000,0.000000
+0.020000,0.382822,6.750000,0.382834,1.227570,2.407000,0.000000,0.000000
+0.020000,0.408337,6.750000,0.408348,1.275710,2.407000,-0.000000,0.000000
+0.020000,0.434814,6.750000,0.434825,1.323850,2.407000,-0.000000,0.000000
+0.020000,0.462253,6.750000,0.462265,1.371990,2.407000,0.000000,0.000000
+0.020000,0.490656,6.750000,0.490668,1.420130,2.407000,0.000000,0.000000
+0.020000,0.520021,6.750000,0.520033,1.468270,2.407000,-0.000000,0.000000
+0.020000,0.550350,6.750000,0.550361,1.516410,2.407000,0.000000,0.000000
+0.020000,0.581641,6.750000,0.581652,1.564550,2.407000,-0.000000,0.000000
+0.020000,0.613894,6.750000,0.613906,1.612690,2.407000,0.000000,0.000000
+0.020000,0.647111,6.750000,0.647123,1.660830,2.407000,-0.000000,0.000000
+0.020000,0.681290,6.750000,0.681302,1.708970,2.407000,0.000000,0.000000
+0.020000,0.716433,6.750000,0.716444,1.757110,2.407000,-0.000000,0.000000
+0.020000,0.752538,6.750000,0.752549,1.805250,2.407000,0.000000,0.000000
+0.020000,0.789605,6.750000,0.789617,1.853390,2.407000,-0.000000,0.000000
+0.020000,0.827636,6.750000,0.827648,1.901530,2.407000,0.000000,0.000000
+0.020000,0.866629,6.750000,0.866641,1.949670,2.407000,-0.000000,0.000000
+0.020000,0.906586,6.750000,0.906597,1.997810,2.407000,0.000000,0.000000
+0.020000,0.947505,6.750000,0.947516,2.045950,2.407000,-0.000000,0.000000
+0.020000,0.989386,6.750000,0.989398,2.094090,2.407000,0.000000,0.000000
+0.020000,1.032231,6.750000,1.032243,2.142230,2.407000,-0.000000,0.000000
+0.020000,1.076038,6.750000,1.076050,2.190370,2.407000,0.000000,0.000000
+0.020000,1.120809,6.750000,1.120820,2.238510,2.407000,-0.000000,0.000000
+0.020000,1.166542,6.750000,1.166553,2.286650,2.407000,0.000000,0.000000
+0.020000,1.213237,6.750000,1.213249,2.334790,2.407000,0.000000,0.000000
+0.020000,1.260896,6.750000,1.260908,2.382930,2.407000,0.000000,0.000000
+0.020000,1.309518,6.750000,1.309529,2.431070,2.407000,-0.000000,0.000000
+0.020000,1.359102,6.750000,1.359113,2.479210,2.407000,0.000000,0.000000
+0.020000,1.409649,6.750000,1.409660,2.527350,2.407000,-0.000000,0.000000
+0.020000,1.461159,6.750000,1.461170,2.575490,2.407000,0.000000,0.000000
+0.020000,1.513631,6.750000,1.513643,2.623630,2.407000,0.000000,0.000000
+0.020000,1.567067,6.750000,1.567078,2.671770,2.407000,-0.000000,0.000000
+0.020000,1.621465,6.750000,1.621476,2.719910,2.407000,0.000000,0.000000
+0.020000,1.676826,6.750000,1.676837,2.768050,2.407000,0.000000,0.000000
+0.020000,1.733150,6.750000,1.733161,2.816190,2.407000,0.000000,0.000000
+0.020000,1.790436,6.750000,1.790448,2.864330,2.407000,0.000000,0.000000
+0.020000,1.848686,6.750000,1.848697,2.912470,2.407000,-0.000000,0.000000
+0.020000,1.907898,6.750000,1.907909,2.960610,2.407000,0.000000,0.000000
+0.020000,1.968073,6.750000,1.968084,3.008750,2.407000,-0.000000,0.000000
+0.020000,2.029211,6.750000,2.029222,3.056890,2.407000,-0.000000,0.000000
+0.020000,2.091299,6.750000,2.091311,3.104443,2.377647,-1.467683,0.000000
+0.020000,2.154316,6.750000,2.154328,3.150822,2.318939,-2.935366,0.000000
+0.020000,2.218236,6.750000,2.218248,3.196027,2.260232,-2.935366,0.000000
+0.020000,2.283038,6.750000,2.283049,3.240057,2.201525,-2.935366,0.000000
+0.020000,2.348696,6.750000,2.348708,3.282913,2.142817,-2.935366,0.000000
+0.020000,2.415188,6.750000,2.415199,3.324596,2.084110,-2.935366,0.000000
+0.020000,2.482490,6.750000,2.482502,3.365104,2.025403,-2.935366,0.000000
+0.020000,2.550579,6.750000,2.550590,3.404438,1.966695,-2.935366,0.000000
+0.020000,2.619430,6.750000,2.619442,3.442597,1.907988,-2.935366,0.000000
+0.020000,2.689022,6.750000,2.689034,3.479583,1.849281,-2.935366,0.000000
+0.020000,2.759330,6.750000,2.759342,3.515394,1.790573,-2.935366,0.000000
+0.020000,2.830331,6.750000,2.830342,3.550032,1.731866,-2.935366,0.000000
+0.020000,2.902001,6.750000,2.902012,3.583495,1.673159,-2.935366,0.000000
+0.020000,2.974316,6.750000,2.974328,3.615784,1.614451,-2.935366,0.000000
+0.020000,3.047254,6.750000,3.047266,3.646899,1.555744,-2.935366,0.000000
+0.020000,3.120791,6.750000,3.120803,3.676840,1.497037,-2.935366,0.000000
+0.020000,3.194903,6.750000,3.194915,3.705606,1.438329,-2.935366,0.000000
+0.020000,3.269567,6.750000,3.269579,3.733199,1.379622,-2.935366,0.000000
+0.020000,3.344759,6.750000,3.344771,3.759617,1.320915,-2.935366,0.000000
+0.020000,3.420457,6.750000,3.420468,3.784861,1.262207,-2.935366,0.000000
+0.020000,3.496635,6.750000,3.496647,3.808931,1.203500,-2.935366,0.000000
+0.020000,3.573272,6.750000,3.573284,3.831827,1.144793,-2.935366,0.000000
+0.020000,3.650343,6.750000,3.650355,3.853549,1.086085,-2.935366,0.000000
+0.020000,3.727825,6.750000,3.727836,3.874096,1.027378,-2.935366,0.000000
+0.020000,3.805694,6.750000,3.805706,3.893470,0.968671,-2.935366,0.000000
+0.020000,3.883927,6.750000,3.883939,3.911669,0.909963,-2.935366,0.000000
+0.020000,3.962501,6.750000,3.962513,3.928694,0.851256,-2.935366,0.000000
+0.020000,4.041392,6.750000,4.041404,3.944545,0.792549,-2.935366,0.000000
+0.020000,4.120577,6.750000,4.120588,3.959222,0.733842,-2.935366,0.000000
+0.020000,4.200031,6.750000,4.200043,3.972724,0.675134,-2.935366,0.000000
+0.020000,4.279732,6.750000,4.279744,3.985053,0.616427,-2.935366,0.000000
+0.020000,4.359656,6.750000,4.359668,3.996207,0.557720,-2.935366,0.000000
+0.020000,4.439780,6.750000,4.439792,4.006188,0.499012,-2.935366,0.000000
+0.020000,4.520080,6.750000,4.520092,4.014994,0.440305,-2.935366,0.000000
+0.020000,4.600533,6.750000,4.600544,4.022626,0.381598,-2.935366,0.000000
+0.020000,4.681114,6.750000,4.681126,4.029083,0.322890,-2.935366,0.000000
+0.020000,4.761802,6.750000,4.761813,4.034367,0.264183,-2.935366,0.000000
+0.020000,4.842571,6.750000,4.842583,4.038477,0.205476,-2.935366,0.000000
+0.020000,4.923399,6.750000,4.923411,4.041412,0.146768,-2.935366,0.000000
+0.020000,5.004260,6.750000,5.004272,4.043039,0.081337,-3.271559,0.000000
+0.020000,5.085115,6.750000,5.085127,4.042770,-0.013448,-4.739242,0.000000
+0.020000,5.165930,6.750000,5.165942,4.040740,-0.101509,-4.403049,0.000000
+0.020000,5.246681,6.750000,5.246693,4.037535,-0.160216,-2.935366,0.000000
+0.020000,5.327344,6.750000,5.327356,4.033157,-0.218923,-2.935366,0.000000
+0.020000,5.407896,6.750000,5.407908,4.027604,-0.277631,-2.935366,0.000000
+0.020000,5.488314,6.750000,5.488325,4.020877,-0.336338,-2.935366,0.000000
+0.020000,5.568573,6.750000,5.568585,4.012977,-0.395045,-2.935366,0.000000
+0.020000,5.648651,6.750000,5.648663,4.003902,-0.453753,-2.935366,0.000000
+0.020000,5.728524,6.750000,5.728536,3.993652,-0.512460,-2.935366,0.000000
+0.020000,5.808169,6.750000,5.808181,3.982229,-0.571167,-2.935366,0.000000
+0.020000,5.887562,6.750000,5.887573,3.969631,-0.629875,-2.935366,0.000000
+0.020000,5.966679,6.750000,5.966691,3.955860,-0.688582,-2.935366,0.000000
+0.020000,6.045497,6.750000,6.045509,3.940914,-0.747289,-2.935366,0.000000
+0.020000,6.123993,6.750000,6.124005,3.924794,-0.805997,-2.935366,0.000000
+0.020000,6.202143,6.750000,6.202155,3.907500,-0.864704,-2.935366,0.000000
+0.020000,6.279924,6.750000,6.279935,3.889032,-0.923411,-2.935366,0.000000
+0.020000,6.357311,6.750000,6.357323,3.869389,-0.982119,-2.935366,0.000000
+0.020000,6.434283,6.750000,6.434295,3.848573,-1.040826,-2.935366,0.000000
+0.020000,6.510814,6.750000,6.510826,3.826582,-1.099533,-2.935366,0.000000
+0.020000,6.586883,6.750000,6.586895,3.803417,-1.158240,-2.935366,0.000000
+0.020000,6.662464,6.750000,6.662476,3.779079,-1.216948,-2.935366,0.000000
+0.020000,6.737536,6.750000,6.737547,3.753565,-1.275655,-2.935366,0.000000
+0.020000,6.812073,6.750000,6.812085,3.726878,-1.334362,-2.935366,0.000000
+0.020000,6.886054,6.750000,6.886065,3.699017,-1.393070,-2.935366,0.000000
+0.020000,6.959453,6.750000,6.959465,3.669981,-1.451777,-2.935366,0.000000
+0.020000,7.032249,6.750000,7.032260,3.639772,-1.510484,-2.935366,0.000000
+0.020000,7.104416,6.750000,7.104428,3.608388,-1.569192,-2.935366,0.000000
+0.020000,7.175933,6.750000,7.175945,3.575830,-1.627899,-2.935366,0.000000
+0.020000,7.246775,6.750000,7.246787,3.542098,-1.686606,-2.935366,0.000000
+0.020000,7.316919,6.750000,7.316931,3.507191,-1.745314,-2.935366,0.000000
+0.020000,7.386341,6.750000,7.386353,3.471111,-1.804021,-2.935366,0.000000
+0.020000,7.455018,6.750000,7.455030,3.433856,-1.862728,-2.935366,0.000000
+0.020000,7.522927,6.750000,7.522938,3.395428,-1.921436,-2.935366,0.000000
+0.020000,7.590043,6.750000,7.590055,3.355825,-1.980143,-2.935366,0.000000
+0.020000,7.656344,6.750000,7.656356,3.315048,-2.038850,-2.935366,0.000000
+0.020000,7.721806,6.750000,7.721818,3.273097,-2.097558,-2.935366,0.000000
+0.020000,7.786405,6.750000,7.786417,3.229971,-2.156265,-2.935366,0.000000
+0.020000,7.850119,6.750000,7.850131,3.185672,-2.214972,-2.935366,0.000000
+0.020000,7.912923,6.750000,7.912935,3.140198,-2.273680,-2.935366,0.000000
+0.020000,7.974794,6.750000,7.974806,3.093551,-2.332387,-2.935366,0.000000
+0.020000,8.035711,6.750000,8.035723,3.045863,-2.384370,-2.599173,0.000000
+0.020000,8.095666,6.750000,8.095677,2.997723,-2.407000,-1.131490,0.000000
+0.020000,8.154657,6.750000,8.154669,2.949583,-2.407000,-0.000000,0.000000
+0.020000,8.212686,6.750000,8.212698,2.901443,-2.407000,0.000000,0.000000
+0.020000,8.269752,6.750000,8.269764,2.853303,-2.407000,-0.000000,0.000000
+0.020000,8.325855,6.750000,8.325867,2.805163,-2.407000,0.000000,0.000000
+0.020000,8.380996,6.750000,8.381008,2.757023,-2.407000,0.000000,0.000000
+0.020000,8.435174,6.750000,8.435185,2.708883,-2.407000,0.000000,0.000000
+0.020000,8.488388,6.750000,8.488400,2.660743,-2.407000,0.000000,0.000000
+0.020000,8.540641,6.750000,8.540652,2.612603,-2.407000,0.000000,0.000000
+0.020000,8.591930,6.750000,8.591942,2.564463,-2.407000,0.000000,0.000000
+0.020000,8.642256,6.750000,8.642268,2.516323,-2.407000,0.000000,0.000000
+0.020000,8.691620,6.750000,8.691632,2.468183,-2.407000,0.000000,0.000000
+0.020000,8.740021,6.750000,8.740032,2.420043,-2.407000,0.000000,0.000000
+0.020000,8.787459,6.750000,8.787471,2.371903,-2.407000,0.000000,0.000000
+0.020000,8.833934,6.750000,8.833946,2.323763,-2.407000,0.000000,0.000000
+0.020000,8.879447,6.750000,8.879458,2.275623,-2.407000,0.000000,0.000000
+0.020000,8.923996,6.750000,8.924008,2.227483,-2.407000,0.000000,0.000000
+0.020000,8.967583,6.750000,8.967595,2.179343,-2.407000,0.000000,0.000000
+0.020000,9.010207,6.750000,9.010219,2.131203,-2.407000,0.000000,0.000000
+0.020000,9.051868,6.750000,9.051880,2.083063,-2.407000,0.000000,0.000000
+0.020000,9.092567,6.750000,9.092579,2.034923,-2.407000,0.000000,0.000000
+0.020000,9.132303,6.750000,9.132314,1.986783,-2.407000,0.000000,0.000000
+0.020000,9.171075,6.750000,9.171087,1.938643,-2.407000,0.000000,0.000000
+0.020000,9.208885,6.750000,9.208897,1.890503,-2.407000,0.000000,0.000000
+0.020000,9.245733,6.750000,9.245744,1.842363,-2.407000,0.000000,0.000000
+0.020000,9.281617,6.750000,9.281629,1.794223,-2.407000,0.000000,0.000000
+0.020000,9.316539,6.750000,9.316551,1.746083,-2.407000,0.000000,0.000000
+0.020000,9.350498,6.750000,9.350509,1.697943,-2.407000,-0.000000,0.000000
+0.020000,9.383494,6.750000,9.383505,1.649803,-2.407000,0.000000,0.000000
+0.020000,9.415527,6.750000,9.415539,1.601663,-2.407000,-0.000000,0.000000
+0.020000,9.446597,6.750000,9.446609,1.553523,-2.407000,0.000000,0.000000
+0.020000,9.476705,6.750000,9.476717,1.505383,-2.407000,-0.000000,0.000000
+0.020000,9.505850,6.750000,9.505862,1.457243,-2.407000,0.000000,0.000000
+0.020000,9.534032,6.750000,9.534044,1.409103,-2.407000,0.000000,0.000000
+0.020000,9.561251,6.750000,9.561263,1.360963,-2.407000,-0.000000,0.000000
+0.020000,9.587508,6.750000,9.587519,1.312823,-2.407000,0.000000,0.000000
+0.020000,9.612801,6.750000,9.612813,1.264683,-2.407000,-0.000000,0.000000
+0.020000,9.637132,6.750000,9.637144,1.216543,-2.407000,0.000000,0.000000
+0.020000,9.660500,6.750000,9.660512,1.168403,-2.407000,0.000000,0.000000
+0.020000,9.682906,6.750000,9.682917,1.120263,-2.407000,-0.000000,0.000000
+0.020000,9.704348,6.750000,9.704360,1.072123,-2.407000,0.000000,0.000000
+0.020000,9.724828,6.750000,9.724839,1.023983,-2.407000,-0.000000,0.000000
+0.020000,9.744347,6.750000,9.744359,0.975977,-2.400276,0.336193,0.000000
+0.020000,9.762921,6.750000,9.762933,0.928693,-2.364199,1.803876,0.000000
+0.020000,9.780573,6.750000,9.780585,0.882584,-2.305491,2.935366,0.000000
+0.020000,9.797326,6.750000,9.797337,0.837648,-2.246784,2.935366,0.000000
+0.020000,9.813203,6.750000,9.813215,0.793886,-2.188077,2.935366,0.000000
+0.020000,9.828229,6.750000,9.828241,0.751299,-2.129370,2.935366,0.000000
+0.020000,9.842427,6.750000,9.842439,0.709886,-2.070662,2.935366,0.000000
+0.020000,9.855820,6.750000,9.855832,0.669647,-2.011955,2.935366,0.000000
+0.020000,9.868432,6.750000,9.868443,0.630582,-1.953248,2.935366,0.000000
+0.020000,9.880286,6.750000,9.880297,0.592691,-1.894540,2.935366,0.000000
+0.020000,9.891405,6.750000,9.891417,0.555974,-1.835833,2.935366,0.000000
+0.020000,9.901814,6.750000,9.901825,0.520432,-1.777126,2.935366,0.000000
+0.020000,9.911535,6.750000,9.911547,0.486063,-1.718418,2.935366,0.000000
+0.020000,9.920592,6.750000,9.920604,0.452869,-1.659711,2.935366,0.000000
+0.020000,9.929009,6.750000,9.929021,0.420849,-1.601004,2.935366,0.000000
+0.020000,9.936809,6.750000,9.936821,0.390003,-1.542296,2.935366,0.000000
+0.020000,9.944016,6.750000,9.944028,0.360331,-1.483589,2.935366,0.000000
+0.020000,9.950653,6.750000,9.950664,0.331834,-1.424882,2.935366,0.000000
+0.020000,9.956743,6.750000,9.956755,0.304510,-1.366174,2.935366,0.000000
+0.020000,9.962310,6.750000,9.962322,0.278361,-1.307467,2.935366,0.000000
+0.020000,9.967378,6.750000,9.967390,0.253386,-1.248760,2.935366,0.000000
+0.020000,9.971970,6.750000,9.971981,0.229585,-1.190052,2.935366,0.000000
+0.020000,9.976109,6.750000,9.976120,0.206958,-1.131345,2.935366,0.000000
+0.020000,9.979819,6.750000,9.979831,0.185505,-1.072638,2.935366,0.000000
+0.020000,9.983123,6.750000,9.983135,0.165226,-1.013930,2.935366,0.000000
+0.020000,9.986046,6.750000,9.986057,0.146122,-0.955223,2.935366,0.000000
+0.020000,9.988610,6.750000,9.988621,0.128192,-0.896516,2.935366,0.000000
+0.020000,9.990838,6.750000,9.990850,0.111435,-0.837808,2.935366,0.000000
+0.020000,9.992755,6.750000,9.992767,0.095853,-0.779101,2.935366,0.000000
+0.020000,9.994384,6.750000,9.994396,0.081446,-0.720394,2.935366,0.000000
+0.020000,9.995748,6.750000,9.995760,0.068212,-0.661686,2.935366,0.000000
+0.020000,9.996872,6.750000,9.996883,0.056152,-0.602979,2.935366,0.000000
+0.020000,9.997777,6.750000,9.997789,0.045267,-0.544272,2.935366,0.000000
+0.020000,9.998488,6.750000,9.998500,0.035556,-0.485565,2.935366,0.000000
+0.020000,9.999028,6.750000,9.999040,0.027018,-0.426857,2.935366,0.000000
+0.020000,9.999421,6.750000,9.999433,0.019655,-0.368150,2.935366,0.000000
+0.020000,9.999691,6.750000,9.999703,0.013467,-0.309443,2.935366,0.000000
+0.020000,9.999860,6.750000,9.999872,0.008452,-0.250735,2.935366,0.000000
+0.020000,9.999952,6.750000,9.999964,0.004611,-0.192028,2.935366,0.000000
+0.020000,9.999991,6.750000,10.000003,0.001945,-0.133321,2.935366,0.000000
+0.020000,10.000000,6.750000,10.000012,0.000453,-0.074613,2.935366,0.000000
+0.020000,10.000000,6.750000,10.000012,0.000000,-0.022630,2.599173,0.000000
diff --git a/src/main/deploy/output/testPath.left.pf1.csv b/src/main/deploy/output/testPath.left.pf1.csv
new file mode 100644
index 0000000..4541b8a
--- /dev/null
+++ b/src/main/deploy/output/testPath.left.pf1.csv
@@ -0,0 +1,254 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,9.996821,9.249996,0.000012,0.001170,0.058501,2.925055,0.002544
+0.020000,9.996833,9.249996,0.000024,0.000632,-0.026898,-4.269935,0.002533
+0.020000,9.996862,9.249996,0.000053,0.001422,0.039505,3.320118,0.002511
+0.020000,9.996912,9.249996,0.000103,0.002528,0.055308,0.790150,0.002470
+0.020000,9.996991,9.249996,0.000182,0.003951,0.071112,0.790233,0.002407
+0.020000,9.997105,9.249997,0.000296,0.005689,0.086920,0.790370,0.002316
+0.020000,9.997260,9.249997,0.000451,0.007744,0.102731,0.790573,0.002192
+0.020000,9.997462,9.249997,0.000653,0.010115,0.118548,0.790854,0.002030
+0.020000,9.997718,9.249998,0.000909,0.012802,0.134373,0.791228,0.001825
+0.020000,9.998034,9.249998,0.001225,0.015806,0.150207,0.791705,0.001573
+0.020000,9.998417,9.249999,0.001608,0.019127,0.166053,0.792298,0.001267
+0.020000,9.998872,9.249999,0.002063,0.022765,0.181913,0.793017,0.000902
+0.020000,9.999407,9.250000,0.002598,0.026721,0.197791,0.793874,0.000475
+0.020000,10.000125,9.250000,0.003316,0.035897,0.458768,13.048883,6.283164
+0.020000,10.003471,9.249999,0.006662,0.167310,6.570683,305.595739,6.282594
+0.020000,10.007277,9.249995,0.010468,0.190319,1.150451,-271.011610,6.281946
+0.020000,10.011573,9.249989,0.014764,0.214798,1.223944,3.674658,6.281214
+0.020000,10.016390,9.249977,0.019581,0.240844,1.302298,3.917694,6.280392
+0.020000,10.021760,9.249960,0.024952,0.268528,1.384184,4.094284,6.279476
+0.020000,10.027709,9.249935,0.030900,0.297433,1.445237,3.052687,6.278460
+0.020000,10.034271,9.249900,0.037462,0.328098,1.533250,4.400631,6.277339
+0.020000,10.041473,9.249853,0.044664,0.360102,1.600243,3.349665,6.276108
+0.020000,10.049347,9.249792,0.052539,0.393713,1.680537,4.014670,6.274760
+0.020000,10.057921,9.249714,0.061113,0.428728,1.750737,3.510009,6.273291
+0.020000,10.067230,9.249614,0.070422,0.465460,1.836600,4.293143,6.271695
+0.020000,10.077298,9.249490,0.080492,0.503458,1.899903,3.165162,6.269966
+0.020000,10.088160,9.249336,0.091354,0.543128,1.983499,4.179784,6.268099
+0.020000,10.099846,9.249148,0.103042,0.584404,2.063819,4.016023,6.266088
+0.020000,10.112385,9.248920,0.115584,0.627068,2.133180,3.468056,6.263927
+0.020000,10.125809,9.248646,0.129009,0.671298,2.211488,3.915367,6.261610
+0.020000,10.140147,9.248319,0.143351,0.717088,2.289518,3.901520,6.259132
+0.020000,10.155430,9.247931,0.158640,0.764433,2.367226,3.885389,6.256486
+0.020000,10.171690,9.247474,0.174906,0.813324,2.444564,3.866932,6.253666
+0.020000,10.188957,9.246938,0.192181,0.863754,2.521487,3.846113,6.250666
+0.020000,10.207261,9.246313,0.210496,0.915713,2.597945,3.822898,6.247480
+0.020000,10.226636,9.245588,0.229884,0.969439,2.686312,4.418371,6.244100
+0.020000,10.247110,9.244751,0.250375,1.024546,2.755350,3.451883,6.240522
+0.020000,10.268712,9.243788,0.271999,1.081206,2.832998,3.882431,6.236738
+0.020000,10.291479,9.242684,0.294792,1.139641,2.921786,4.439382,6.232741
+0.020000,10.315435,9.241424,0.318782,1.199489,2.992381,3.529760,6.228526
+0.020000,10.340613,9.239991,0.344000,1.260911,3.071098,3.935814,6.224084
+0.020000,10.367046,9.238365,0.370483,1.324153,3.162096,4.549940,6.219410
+0.020000,10.394761,9.236526,0.398260,1.388822,3.233440,3.567184,6.214496
+0.020000,10.423774,9.234455,0.427347,1.454348,3.276326,2.144293,6.209338
+0.020000,10.454085,9.232130,0.457746,1.519975,3.281330,0.250191,6.203935
+0.020000,10.485692,9.229530,0.489460,1.585675,3.285023,0.184680,6.198286
+0.020000,10.518597,9.226632,0.522493,1.651646,3.298543,0.675991,6.192387
+0.020000,10.552796,9.223412,0.556843,1.717494,3.292393,-0.307501,6.186239
+0.020000,10.588285,9.219846,0.592511,1.783413,3.295943,0.177498,6.179840
+0.020000,10.625066,9.215908,0.629502,1.849568,3.307774,0.591552,6.173190
+0.020000,10.663133,9.211571,0.667815,1.915625,3.302825,-0.247440,6.166286
+0.020000,10.702480,9.206808,0.707449,1.981719,3.304685,0.092980,6.159130
+0.020000,10.743104,9.201589,0.748408,2.047925,3.310330,0.282277,6.151719
+0.020000,10.785001,9.195886,0.790690,2.114133,3.310414,0.004155,6.144055
+0.020000,10.828161,9.189668,0.834296,2.180270,3.306824,-0.179454,6.136137
+0.020000,10.872577,9.182903,0.879225,2.246455,3.309263,0.121924,6.127966
+0.020000,10.918242,9.175561,0.925476,2.312543,3.304376,-0.244361,6.119542
+0.020000,10.965142,9.167607,0.973045,2.378481,3.296889,-0.374329,6.110867
+0.020000,11.013268,9.159010,1.021933,2.444384,3.295172,-0.085870,6.101944
+0.020000,11.062606,9.149735,1.072135,2.510127,3.287168,-0.400168,6.092773
+0.020000,11.113143,9.139747,1.123650,2.575728,3.280024,-0.357218,6.083358
+0.020000,11.164862,9.129012,1.176471,2.641046,3.265928,-0.704789,6.073702
+0.020000,11.217745,9.117495,1.230594,2.706159,3.255618,-0.515536,6.063810
+0.020000,11.271775,9.105161,1.286014,2.771002,3.242166,-0.672586,6.053685
+0.020000,11.326931,9.091974,1.342724,2.835515,3.225644,-0.826099,6.043333
+0.020000,11.383190,9.077900,1.400717,2.899638,3.206134,-0.975485,6.032760
+0.020000,11.440527,9.062903,1.459983,2.963312,3.183731,-1.120166,6.021973
+0.020000,11.498918,9.046949,1.520514,3.026540,3.161374,-1.117827,6.010979
+0.020000,11.558335,9.030005,1.582300,3.089295,3.137749,-1.181283,5.999787
+0.020000,11.618748,9.012037,1.645329,3.151421,3.106321,-1.571377,5.988405
+0.020000,11.680126,8.993012,1.709587,3.212929,3.075416,-1.545229,5.976844
+0.020000,11.742437,8.972900,1.775064,3.273845,3.045787,-1.481459,5.965113
+0.020000,11.805647,8.951671,1.841744,3.333985,3.006990,-1.939866,5.953225
+0.020000,11.869720,8.929295,1.909611,3.393358,2.968667,-1.916157,5.941192
+0.020000,11.934618,8.905746,1.978649,3.451922,2.928185,-2.024066,5.929027
+0.020000,12.000304,8.880999,2.048843,3.509658,2.886793,-2.069602,5.916744
+0.020000,12.066737,8.855030,2.120171,3.566435,2.838856,-2.396855,5.904357
+0.020000,12.133877,8.827817,2.192617,3.622274,2.791954,-2.345113,5.891881
+0.020000,12.201684,8.799341,2.266160,3.677145,2.743553,-2.420058,5.879332
+0.020000,12.270114,8.769586,2.340779,3.730976,2.691536,-2.600872,5.866727
+0.020000,12.339125,8.738536,2.416454,3.783742,2.638324,-2.660563,5.854083
+0.020000,12.408676,8.706179,2.493163,3.835432,2.584502,-2.691134,5.841416
+0.020000,12.478722,8.672505,2.570883,3.886015,2.529124,-2.768889,5.828746
+0.020000,12.549222,8.637507,2.649592,3.935467,2.472605,-2.825921,5.816089
+0.020000,12.620122,8.601187,2.729253,3.983046,2.378937,-4.683396,5.803466
+0.020000,12.691353,8.563556,2.809814,4.028030,2.249213,-6.486200,5.790901
+0.020000,12.762853,8.524631,2.891223,4.070444,2.120711,-6.425129,5.778415
+0.020000,12.834559,8.484429,2.973429,4.110313,1.993427,-6.364206,5.766030
+0.020000,12.906411,8.442973,3.056382,4.147671,1.867919,-6.275390,5.753769
+0.020000,12.978352,8.400289,3.140033,4.182557,1.744292,-6.181337,5.741651
+0.020000,13.050328,8.356404,3.224334,4.215016,1.622958,-6.066706,5.729698
+0.020000,13.122289,8.311348,3.309236,4.245106,1.504491,-5.923336,5.717927
+0.020000,13.194187,8.265156,3.394693,4.272875,1.388453,-5.801908,5.706359
+0.020000,13.265976,8.217862,3.480661,4.298381,1.275297,-5.657791,5.695009
+0.020000,13.337617,8.169504,3.567095,4.321685,1.165203,-5.504719,5.683895
+0.020000,13.409069,8.120121,3.653952,4.342851,1.058292,-5.345573,5.673031
+0.020000,13.480299,8.069754,3.741191,4.361943,0.954632,-5.182998,5.662434
+0.020000,13.551276,8.018444,3.828771,4.379028,0.854244,-5.019381,5.652115
+0.020000,13.621970,7.966235,3.916655,4.394170,0.757107,-4.856844,5.642087
+0.020000,13.692357,7.913171,4.004803,4.407410,0.661981,-4.756292,5.632363
+0.020000,13.762414,7.859296,4.093180,4.418838,0.571399,-4.529101,5.622952
+0.020000,13.832122,7.804657,4.181750,4.428521,0.484147,-4.362600,5.613863
+0.020000,13.901464,7.749298,4.270479,4.436455,0.396689,-4.372887,5.605106
+0.020000,13.970426,7.693266,4.359335,4.442786,0.316537,-4.007626,5.596687
+0.020000,14.038996,7.636608,4.448284,4.447474,0.234417,-4.106018,5.588615
+0.020000,14.107165,7.579369,4.537297,4.450654,0.159012,-3.770239,5.580895
+0.020000,14.174926,7.521597,4.626343,4.452278,0.081212,-3.889985,5.573532
+0.020000,14.242272,7.463336,4.715393,4.452480,0.010077,-3.556777,5.566531
+0.020000,14.309201,7.404634,4.804417,4.451243,-0.061850,-3.596306,5.559896
+0.020000,14.375711,7.345537,4.893389,4.448597,-0.132282,-3.521613,5.553631
+0.020000,14.441801,7.286090,4.982282,4.444616,-0.199064,-3.339088,5.547739
+0.020000,14.507472,7.226339,5.071068,4.439300,-0.265785,-3.336070,5.542222
+0.020000,14.572727,7.166328,5.159721,4.432661,-0.331960,-3.308746,5.537083
+0.020000,14.637567,7.106103,5.248216,4.424749,-0.395619,-3.182935,5.532323
+0.020000,14.701998,7.045707,5.336527,4.415576,-0.458627,-3.150410,5.527943
+0.020000,14.766023,6.985186,5.424630,4.405154,-0.521102,-3.123764,5.523946
+0.020000,14.829649,6.924582,5.512500,4.393497,-0.582854,-3.087575,5.520332
+0.020000,14.892882,6.863938,5.600113,4.380613,-0.644203,-3.067494,5.517102
+0.020000,14.955727,6.803299,5.687443,4.366518,-0.704755,-3.027579,5.514257
+0.020000,15.018191,6.742707,5.774467,4.351223,-0.764759,-3.000212,5.511796
+0.020000,15.080281,6.682203,5.861162,4.334729,-0.824692,-2.996635,5.509722
+0.020000,15.142005,6.621829,5.947503,4.317041,-0.884378,-2.984290,5.508033
+0.020000,15.203369,6.561628,6.033466,4.298164,-0.943879,-2.975063,5.506730
+0.020000,15.264379,6.501640,6.119028,4.278099,-1.003247,-2.968395,5.505815
+0.020000,15.325044,6.441907,6.204165,4.256848,-1.062521,-2.963728,5.505285
+0.020000,15.385365,6.382471,6.288849,4.234177,-1.133583,-3.553069,5.505143
+0.020000,15.445338,6.323384,6.373039,4.209509,-1.233371,-4.989387,5.505388
+0.020000,15.504961,6.264695,6.456701,4.183101,-1.320423,-4.352611,5.506020
+0.020000,15.564239,6.206440,6.539812,4.155539,-1.378086,-2.883138,5.507039
+0.020000,15.623177,6.148660,6.622348,4.126826,-1.435677,-2.879590,5.508444
+0.020000,15.681780,6.091392,6.704287,4.096962,-1.493170,-2.874653,5.510234
+0.020000,15.740053,6.034672,6.785606,4.065952,-1.550524,-2.867663,5.512411
+0.020000,15.797997,5.978538,6.866282,4.033798,-1.607685,-2.858073,5.514972
+0.020000,15.855617,5.923026,6.946292,4.000506,-1.664592,-2.845341,5.517919
+0.020000,15.912913,5.868171,7.025614,3.966083,-1.721145,-2.827669,5.521249
+0.020000,15.969887,5.814007,7.104225,3.930547,-1.776809,-2.783199,5.524962
+0.020000,16.026538,5.760570,7.182103,3.893890,-1.832855,-2.802299,5.529058
+0.020000,16.082867,5.707892,7.259226,3.856132,-1.887893,-2.751861,5.533535
+0.020000,16.138871,5.656005,7.335571,3.817288,-1.942201,-2.715430,5.538392
+0.020000,16.194547,5.604941,7.411119,3.777378,-1.995482,-2.664058,5.543627
+0.020000,16.249894,5.554729,7.485848,3.736435,-2.047162,-2.583962,5.549238
+0.020000,16.304905,5.505401,7.559737,3.694450,-2.099272,-2.605545,5.555223
+0.020000,16.359576,5.456982,7.632766,3.651463,-2.149319,-2.502336,5.561580
+0.020000,16.413901,5.409500,7.704916,3.607530,-2.196685,-2.368269,5.568304
+0.020000,16.467873,5.362981,7.776169,3.562642,-2.244410,-2.386292,5.575393
+0.020000,16.521483,5.317448,7.846506,3.516849,-2.289641,-2.261533,5.582841
+0.020000,16.574723,5.272924,7.915911,3.470218,-2.331511,-2.093493,5.590644
+0.020000,16.627584,5.229429,7.984365,3.422734,-2.374212,-2.135062,5.598797
+0.020000,16.680055,5.186982,8.051856,3.374518,-2.410789,-1.828868,5.607293
+0.020000,16.732125,5.145601,8.118366,3.325539,-2.448972,-1.909119,5.616124
+0.020000,16.783782,5.105300,8.183885,3.275918,-2.481064,-1.604582,5.625284
+0.020000,16.835015,5.066094,8.248398,3.225665,-2.512656,-1.579631,5.634762
+0.020000,16.885810,5.027992,8.311895,3.174849,-2.540786,-1.406493,5.644550
+0.020000,16.936154,4.991004,8.374366,3.123549,-2.564980,-1.209694,5.654636
+0.020000,16.986034,4.955138,8.435802,3.071806,-2.587153,-1.108657,5.665009
+0.020000,17.035436,4.920398,8.496196,3.019684,-2.606105,-0.947590,5.675656
+0.020000,17.084344,4.886785,8.555541,2.967253,-2.621574,-0.773450,5.686564
+0.020000,17.132746,4.854302,8.613832,2.914574,-2.633911,-0.616873,5.697717
+0.020000,17.180626,4.822945,8.671067,2.861712,-2.643139,-0.461397,5.709099
+0.020000,17.227970,4.792711,8.727241,2.808726,-2.649307,-0.308387,5.720695
+0.020000,17.274764,4.763593,8.782355,2.755676,-2.652491,-0.159203,5.732486
+0.020000,17.320993,4.735583,8.836407,2.702626,-2.652495,-0.000177,5.744454
+0.020000,17.366642,4.708670,8.889400,2.649616,-2.650505,0.099481,5.756579
+0.020000,17.411699,4.682842,8.941334,2.596703,-2.645625,0.243973,5.768841
+0.020000,17.456148,4.658085,8.992212,2.543932,-2.638562,0.353165,5.781219
+0.020000,17.499976,4.634382,9.042039,2.491338,-2.629690,0.443605,5.793692
+0.020000,17.543169,4.611717,9.090818,2.438957,-2.619066,0.531191,5.806236
+0.020000,17.585715,4.590069,9.138554,2.386808,-2.607459,0.580348,5.818831
+0.020000,17.627602,4.569417,9.185256,2.335091,-2.585853,1.080292,5.831454
+0.020000,17.668829,4.549733,9.230941,2.284247,-2.542184,2.183463,5.844085
+0.020000,17.709398,4.530989,9.275631,2.234522,-2.486223,2.798083,5.856707
+0.020000,17.749313,4.513155,9.319349,2.185884,-2.431903,2.715984,5.869302
+0.020000,17.788576,4.496200,9.362116,2.138333,-2.377555,2.717395,5.881853
+0.020000,17.827189,4.480097,9.403952,2.091833,-2.325002,2.627646,5.894344
+0.020000,17.865155,4.464816,9.444879,2.046317,-2.275816,2.459283,5.906757
+0.020000,17.902478,4.450328,9.484915,2.001803,-2.225708,2.505403,5.919078
+0.020000,17.939159,4.436604,9.524079,1.958222,-2.179058,2.332514,5.931291
+0.020000,17.975201,4.423617,9.562390,1.915516,-2.135304,2.187686,5.943382
+0.020000,18.010605,4.411338,9.599863,1.873662,-2.092661,2.132144,5.955338
+0.020000,18.045375,4.399741,9.636515,1.832615,-2.052365,2.014833,5.967146
+0.020000,18.079509,4.388798,9.672361,1.792296,-2.015930,1.821719,5.978793
+0.020000,18.113012,4.378483,9.707415,1.752702,-1.979730,1.810040,5.990270
+0.020000,18.145883,4.368772,9.741691,1.713781,-1.946058,1.683560,6.001564
+0.020000,18.178122,4.359638,9.775199,1.675392,-1.919425,1.331665,6.012667
+0.020000,18.209730,4.351058,9.807951,1.637636,-1.887810,1.580758,6.023570
+0.020000,18.240708,4.343007,9.839958,1.600314,-1.866117,1.084663,6.034265
+0.020000,18.271053,4.335462,9.871227,1.563471,-1.842116,1.200029,6.044744
+0.020000,18.300767,4.328401,9.901768,1.527045,-1.821291,1.041245,6.055001
+0.020000,18.329847,4.321801,9.931587,1.490978,-1.803384,0.895334,6.065030
+0.020000,18.358292,4.315642,9.960692,1.455234,-1.787190,0.809746,6.074827
+0.020000,18.386101,4.309901,9.989087,1.419762,-1.773605,0.679220,6.084386
+0.020000,18.413270,4.304559,10.016777,1.384482,-1.763989,0.480816,6.093704
+0.020000,18.439800,4.299596,10.043767,1.349500,-1.749106,0.744148,6.102777
+0.020000,18.465685,4.294992,10.070059,1.314576,-1.746187,0.145934,6.111603
+0.020000,18.490926,4.290730,10.095656,1.279889,-1.734366,0.591035,6.120181
+0.020000,18.515517,4.286791,10.120561,1.245246,-1.732134,0.111644,6.128507
+0.020000,18.539456,4.283157,10.144775,1.210663,-1.729150,0.149176,6.136581
+0.020000,18.562741,4.279812,10.168298,1.176173,-1.724528,0.231108,6.144403
+0.020000,18.585367,4.276739,10.191132,1.141709,-1.723190,0.066899,6.151971
+0.020000,18.607332,4.273922,10.213277,1.107248,-1.723030,0.008005,6.159285
+0.020000,18.628631,4.271345,10.234731,1.072713,-1.726740,-0.185511,6.166346
+0.020000,18.649263,4.268995,10.255496,1.038243,-1.723519,0.161054,6.173155
+0.020000,18.669223,4.266856,10.275570,1.003713,-1.726529,-0.150479,6.179711
+0.020000,18.688507,4.264915,10.294952,0.969076,-1.731824,-0.264750,6.186016
+0.020000,18.707112,4.263158,10.313640,0.934413,-1.733157,-0.066655,6.192071
+0.020000,18.725036,4.261573,10.331634,0.899704,-1.735447,-0.114515,6.197876
+0.020000,18.742275,4.260147,10.348932,0.864882,-1.741088,-0.282055,6.203434
+0.020000,18.758824,4.258869,10.365531,0.829929,-1.747650,-0.328096,6.208746
+0.020000,18.774682,4.257727,10.381429,0.794942,-1.749349,-0.084957,6.213812
+0.020000,18.789848,4.256710,10.396629,0.759973,-1.748467,0.044125,6.218637
+0.020000,18.804330,4.255807,10.411139,0.725528,-1.722269,1.309900,6.223224
+0.020000,18.818144,4.255008,10.424977,0.691859,-1.683424,1.942236,6.227581
+0.020000,18.831304,4.254303,10.438156,0.658947,-1.645626,1.889898,6.231715
+0.020000,18.843822,4.253682,10.450689,0.626652,-1.614748,1.543882,6.235632
+0.020000,18.855715,4.253138,10.462594,0.595291,-1.568013,2.336774,6.239340
+0.020000,18.866996,4.252663,10.473885,0.564535,-1.537838,1.508737,6.242844
+0.020000,18.877680,4.252250,10.484577,0.534616,-1.495948,2.094522,6.246151
+0.020000,18.887784,4.251891,10.494687,0.505489,-1.456328,1.980977,6.249268
+0.020000,18.897321,4.251582,10.504229,0.477112,-1.418863,1.873234,6.252201
+0.020000,18.906307,4.251316,10.513219,0.449497,-1.380716,1.907391,6.254956
+0.020000,18.914757,4.251088,10.521673,0.422659,-1.341910,1.940275,6.257539
+0.020000,18.922687,4.250894,10.529605,0.396610,-1.302476,1.971690,6.259956
+0.020000,18.930112,4.250730,10.537032,0.371361,-1.262447,2.001469,6.262214
+0.020000,18.937049,4.250592,10.543970,0.346924,-1.221857,2.029477,6.264319
+0.020000,18.943514,4.250476,10.550437,0.323309,-1.180745,2.055604,6.266275
+0.020000,18.949524,4.250380,10.556447,0.300526,-1.139150,2.079767,6.268090
+0.020000,18.955092,4.250300,10.562016,0.278434,-1.104609,1.727060,6.269768
+0.020000,18.960237,4.250235,10.567161,0.257257,-1.058848,2.288057,6.271316
+0.020000,18.964975,4.250183,10.571900,0.236932,-1.016237,2.130513,6.272739
+0.020000,18.969321,4.250140,10.576245,0.217290,-0.982077,1.707997,6.274042
+0.020000,18.973294,4.250106,10.580218,0.198653,-0.931880,2.509863,6.275232
+0.020000,18.976907,4.250079,10.583832,0.180685,-0.898382,1.674915,6.276313
+0.020000,18.980180,4.250058,10.587105,0.163656,-0.851467,2.345763,6.277290
+0.020000,18.983130,4.250042,10.590055,0.147487,-0.808429,2.151903,6.278170
+0.020000,18.985770,4.250030,10.592695,0.131989,-0.774895,1.676688,6.278957
+0.020000,18.988120,4.250021,10.595045,0.117500,-0.724483,2.520600,6.279657
+0.020000,18.990197,4.250014,10.597121,0.103837,-0.683142,2.067060,6.280275
+0.020000,18.992016,4.250009,10.598941,0.090963,-0.643674,1.973372,6.280815
+0.020000,18.993594,4.250006,10.600519,0.078899,-0.603213,2.023047,6.281284
+0.020000,18.994949,4.250004,10.601874,0.067759,-0.557005,2.310423,6.281687
+0.020000,18.996098,4.250002,10.603023,0.057460,-0.514922,2.104115,6.282028
+0.020000,18.997058,4.250001,10.603983,0.048006,-0.472702,2.111011,6.282313
+0.020000,18.997846,4.250001,10.604771,0.039399,-0.430365,2.116836,6.282547
+0.020000,18.998479,4.250000,10.605404,0.031641,-0.387932,2.121675,6.282734
+0.020000,18.998974,4.250000,10.605899,0.024732,-0.345420,2.125615,6.282881
+0.020000,18.999347,4.250000,10.606272,0.018675,-0.302845,2.128746,6.282992
+0.020000,18.999617,4.250000,10.606542,0.013471,-0.260222,2.131159,6.283072
+0.020000,18.999799,4.250000,10.606724,0.009120,-0.217563,2.132946,6.283126
+0.020000,18.999912,4.250000,10.606836,0.005622,-0.174879,2.134202,6.283159
+0.020000,18.999971,4.250000,10.606896,0.002978,-0.132178,2.135020,6.283177
+0.020000,18.999995,4.250000,10.606920,0.001189,-0.089468,2.135496,6.283184
+0.020000,19.000000,4.250000,10.606925,0.000254,-0.046754,2.135724,6.283185
+0.020000,19.000000,4.250000,10.606925,0.000000,-0.012698,1.702791,6.283185
diff --git a/src/main/deploy/output/testPath.pf1.csv b/src/main/deploy/output/testPath.pf1.csv
new file mode 100644
index 0000000..851d122
--- /dev/null
+++ b/src/main/deploy/output/testPath.pf1.csv
@@ -0,0 +1,254 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,10.000000,8.000000,0.000012,0.001170,0.058501,2.925055,0.002544
+0.020000,10.000000,8.000000,0.000059,0.003510,0.117002,2.925055,0.002533
+0.020000,10.000000,8.000000,0.000164,0.007020,0.175503,2.925055,0.002511
+0.020000,10.000000,8.000000,0.000351,0.011700,0.234004,2.925055,0.002470
+0.020000,10.000000,8.000000,0.000644,0.017550,0.292505,2.925055,0.002407
+0.020000,10.000000,8.000000,0.001065,0.024570,0.351007,2.925055,0.002316
+0.020000,10.000000,8.000000,0.001638,0.032761,0.409508,2.925055,0.002192
+0.020000,10.000000,8.000000,0.002387,0.042121,0.468009,2.925055,0.002030
+0.020000,10.000000,8.000000,0.003335,0.052651,0.526510,2.925055,0.001825
+0.020000,10.000000,8.000000,0.004505,0.064351,0.585011,2.925055,0.001573
+0.020000,10.000000,8.000000,0.005920,0.077221,0.643512,2.925055,0.001267
+0.020000,10.000000,8.000000,0.007605,0.091262,0.702013,2.925055,0.000902
+0.020000,10.000000,8.000000,0.009582,0.106472,0.760514,2.925055,0.000475
+0.020000,10.000098,8.000000,0.011876,0.122852,0.819015,2.925055,6.283164
+0.020000,10.002732,7.999999,0.014508,0.140403,0.877516,2.925055,6.282594
+0.020000,10.005728,7.999996,0.017504,0.159123,0.936018,2.925055,6.281946
+0.020000,10.009109,7.999991,0.020885,0.179013,0.994519,2.925055,6.281214
+0.020000,10.012899,7.999982,0.024676,0.200074,1.053020,2.925055,6.280392
+0.020000,10.017124,7.999968,0.028900,0.222304,1.111521,2.925055,6.279476
+0.020000,10.021803,7.999949,0.033580,0.245705,1.170022,2.925055,6.278460
+0.020000,10.026963,7.999921,0.038739,0.270275,1.228523,2.925055,6.277339
+0.020000,10.032626,7.999885,0.044402,0.296016,1.287024,2.925055,6.276108
+0.020000,10.038815,7.999837,0.050592,0.322926,1.345525,2.925055,6.274760
+0.020000,10.045553,7.999775,0.057331,0.351007,1.404026,2.925055,6.273291
+0.020000,10.052867,7.999697,0.064644,0.380257,1.462527,2.925055,6.271695
+0.020000,10.060775,7.999599,0.072553,0.410678,1.521029,2.925055,6.269966
+0.020000,10.069303,7.999478,0.081083,0.442268,1.579530,2.925055,6.268099
+0.020000,10.078476,7.999331,0.090255,0.475029,1.638031,2.925055,6.266088
+0.020000,10.088314,7.999152,0.100095,0.508960,1.696532,2.925055,6.263927
+0.020000,10.098842,7.998937,0.110626,0.544060,1.755033,2.925055,6.261610
+0.020000,10.110083,7.998680,0.121869,0.580331,1.813534,2.925055,6.259132
+0.020000,10.122060,7.998376,0.133851,0.617772,1.872035,2.925055,6.256486
+0.020000,10.134797,7.998018,0.146592,0.656382,1.930536,2.925055,6.253666
+0.020000,10.148315,7.997599,0.160118,0.696163,1.989037,2.925055,6.250666
+0.020000,10.162638,7.997110,0.174450,0.737114,2.047538,2.925055,6.247480
+0.020000,10.177792,7.996543,0.189614,0.779235,2.106039,2.925055,6.244100
+0.020000,10.193797,7.995889,0.205631,0.822525,2.164541,2.925055,6.240522
+0.020000,10.210674,7.995136,0.222526,0.866986,2.223042,2.925055,6.236738
+0.020000,10.228450,7.994274,0.240323,0.912617,2.281543,2.925055,6.232741
+0.020000,10.247145,7.993291,0.259043,0.959418,2.340044,2.925055,6.228526
+0.020000,10.266779,7.992173,0.278711,1.007389,2.398545,2.925055,6.224084
+0.020000,10.287380,7.990906,0.299350,1.056530,2.457046,2.925055,6.219410
+0.020000,10.308967,7.989474,0.320984,1.106841,2.515547,2.925055,6.214496
+0.020000,10.331550,7.987862,0.343624,1.157152,2.515547,0.000000,6.209338
+0.020000,10.355126,7.986054,0.367270,1.207463,2.515547,-0.000000,6.203935
+0.020000,10.379694,7.984033,0.391922,1.257774,2.515547,0.000000,6.198286
+0.020000,10.405255,7.981781,0.417581,1.308085,2.515547,0.000000,6.192387
+0.020000,10.431803,7.979282,0.444246,1.358395,2.515547,0.000000,6.186239
+0.020000,10.459334,7.976516,0.471917,1.408706,2.515547,0.000000,6.179840
+0.020000,10.487849,7.973462,0.500594,1.459017,2.515547,0.000000,6.173190
+0.020000,10.517342,7.970102,0.530277,1.509328,2.515547,0.000000,6.166286
+0.020000,10.547808,7.966414,0.560967,1.559639,2.515547,0.000000,6.159130
+0.020000,10.579245,7.962375,0.592663,1.609950,2.515547,0.000000,6.151719
+0.020000,10.611648,7.957964,0.625365,1.660261,2.515547,0.000000,6.144055
+0.020000,10.645012,7.953158,0.659073,1.710572,2.515547,0.000000,6.136137
+0.020000,10.679331,7.947931,0.693788,1.760883,2.515547,0.000000,6.127966
+0.020000,10.714599,7.942260,0.729509,1.811194,2.515547,0.000000,6.119542
+0.020000,10.750809,7.936120,0.766236,1.861505,2.515547,0.000000,6.110867
+0.020000,10.787954,7.929484,0.803969,1.911816,2.515547,0.000000,6.101944
+0.020000,10.826026,7.922327,0.842708,1.962127,2.515547,0.000000,6.092773
+0.020000,10.865018,7.914621,0.882454,2.012438,2.515547,0.000000,6.083358
+0.020000,10.904919,7.906339,0.923206,2.062749,2.515547,-0.000000,6.073702
+0.020000,10.945720,7.897453,0.964964,2.113060,2.515547,0.000000,6.063810
+0.020000,10.987411,7.887936,1.007728,2.163371,2.515547,-0.000000,6.053685
+0.020000,11.029982,7.877758,1.051499,2.213681,2.515547,0.000000,6.043333
+0.020000,11.073420,7.866891,1.096275,2.263992,2.515547,-0.000000,6.032760
+0.020000,11.117713,7.855306,1.142058,2.314303,2.515547,0.000000,6.021973
+0.020000,11.162847,7.842974,1.188848,2.364614,2.515547,-0.000000,6.010979
+0.020000,11.208810,7.829867,1.236643,2.414925,2.515547,0.000000,5.999787
+0.020000,11.255586,7.815954,1.285445,2.465236,2.515547,0.000000,5.988405
+0.020000,11.303160,7.801208,1.335252,2.515547,2.515547,-0.000000,5.976844
+0.020000,11.351518,7.785600,1.386066,2.565858,2.515547,0.000000,5.965113
+0.020000,11.400641,7.769101,1.437887,2.616169,2.515547,0.000000,5.953225
+0.020000,11.450513,7.751685,1.490713,2.666480,2.515547,-0.000000,5.941192
+0.020000,11.501117,7.733323,1.544546,2.716791,2.515547,0.000000,5.929027
+0.020000,11.552435,7.713989,1.599385,2.767102,2.515547,0.000000,5.916744
+0.020000,11.604447,7.693657,1.655230,2.817413,2.515547,-0.000000,5.904357
+0.020000,11.657134,7.672302,1.712081,2.867724,2.515547,0.000000,5.891881
+0.020000,11.710478,7.649899,1.769939,2.918035,2.515547,-0.000000,5.879332
+0.020000,11.764459,7.626427,1.828803,2.968346,2.515547,0.000000,5.866727
+0.020000,11.819057,7.601862,1.888673,3.018657,2.515547,0.000000,5.854083
+0.020000,11.874252,7.576183,1.949549,3.068968,2.515547,-0.000000,5.841416
+0.020000,11.930024,7.549371,2.011432,3.119278,2.515547,0.000000,5.828746
+0.020000,11.986353,7.521408,2.074320,3.169589,2.515547,-0.000000,5.816089
+0.020000,12.043209,7.492281,2.138203,3.218730,2.457046,-2.925055,5.803466
+0.020000,12.100553,7.461987,2.203058,3.266701,2.398545,-2.925055,5.790901
+0.020000,12.158345,7.430524,2.268860,3.313502,2.340044,-2.925055,5.778415
+0.020000,12.216547,7.397892,2.335586,3.359133,2.281543,-2.925055,5.766030
+0.020000,12.275123,7.364096,2.403213,3.403594,2.223042,-2.925055,5.753769
+0.020000,12.334038,7.329140,2.471718,3.446885,2.164541,-2.925055,5.741651
+0.020000,12.393257,7.293033,2.541077,3.489005,2.106039,-2.925055,5.729698
+0.020000,12.452747,7.255785,2.611267,3.529956,2.047538,-2.925055,5.717927
+0.020000,12.512478,7.217409,2.682264,3.569737,1.989037,-2.925055,5.706359
+0.020000,12.572420,7.177920,2.754044,3.608348,1.930536,-2.925055,5.695009
+0.020000,12.632545,7.137334,2.826586,3.645788,1.872035,-2.925055,5.683895
+0.020000,12.692827,7.095671,2.899864,3.682059,1.813534,-2.925055,5.673031
+0.020000,12.753241,7.052952,2.973856,3.717160,1.755033,-2.925055,5.662434
+0.020000,12.813764,7.009199,3.048539,3.751090,1.696532,-2.925055,5.652115
+0.020000,12.874376,6.964435,3.123888,3.783851,1.638031,-2.925055,5.642087
+0.020000,12.935056,6.918689,3.199881,3.815442,1.579530,-2.925055,5.632363
+0.020000,12.995787,6.871985,3.276494,3.845862,1.521029,-2.925055,5.622952
+0.020000,13.056554,6.824354,3.353704,3.875113,1.462527,-2.925055,5.613863
+0.020000,13.117341,6.775825,3.431487,3.903193,1.404026,-2.925055,5.605106
+0.020000,13.178136,6.726428,3.509820,3.930104,1.345525,-2.925055,5.596687
+0.020000,13.238927,6.676197,3.588680,3.955844,1.287024,-2.925055,5.588615
+0.020000,13.299705,6.625163,3.668042,3.980415,1.228523,-2.925055,5.580895
+0.020000,13.360462,6.573362,3.747884,4.003815,1.170022,-2.925055,5.573532
+0.020000,13.421190,6.520827,3.828183,4.026045,1.111521,-2.925055,5.566531
+0.020000,13.481884,6.467593,3.908915,4.047106,1.053020,-2.925055,5.559896
+0.020000,13.542539,6.413698,3.990056,4.066996,0.994519,-2.925055,5.553631
+0.020000,13.603153,6.359176,4.071583,4.085717,0.936018,-2.925055,5.547739
+0.020000,13.663724,6.304065,4.153473,4.103267,0.877516,-2.925055,5.542222
+0.020000,13.724249,6.248403,4.235702,4.119647,0.819015,-2.925055,5.537083
+0.020000,13.784730,6.192227,4.318247,4.134858,0.760514,-2.925055,5.532323
+0.020000,13.845167,6.135575,4.401084,4.148898,0.702013,-2.925055,5.527943
+0.020000,13.905561,6.078486,4.484191,4.161768,0.643512,-2.925055,5.523946
+0.020000,13.965916,6.020997,4.567543,4.173468,0.585011,-2.925055,5.520332
+0.020000,14.026234,5.963149,4.651118,4.183998,0.526510,-2.925055,5.517102
+0.020000,14.086519,5.904979,4.734892,4.193359,0.468009,-2.925055,5.514257
+0.020000,14.146776,5.846528,4.818841,4.201549,0.409508,-2.925055,5.511796
+0.020000,14.207009,5.787834,4.902942,4.208569,0.351007,-2.925055,5.509722
+0.020000,14.267224,5.728936,4.987172,4.214419,0.292505,-2.925055,5.508033
+0.020000,14.327425,5.669875,5.071507,4.219099,0.234004,-2.925055,5.506730
+0.020000,14.387619,5.610690,5.155924,4.222609,0.175503,-2.925055,5.505815
+0.020000,14.447813,5.551420,5.240400,4.224949,0.117002,-2.925055,5.505285
+0.020000,14.508008,5.492109,5.324905,4.225645,0.034780,-4.111090,5.505143
+0.020000,14.568199,5.432808,5.409402,4.224000,-0.082222,-5.850110,5.505388
+0.020000,14.628384,5.373564,5.493854,4.221186,-0.140723,-2.925055,5.506020
+0.020000,14.688570,5.314418,5.578238,4.217201,-0.199224,-2.925055,5.507039
+0.020000,14.748763,5.255408,5.662530,4.212047,-0.257725,-2.925055,5.508444
+0.020000,14.808967,5.196575,5.746708,4.205722,-0.316226,-2.925055,5.510234
+0.020000,14.869189,5.137958,5.830747,4.198228,-0.374727,-2.925055,5.512411
+0.020000,14.929433,5.079596,5.914625,4.189563,-0.433228,-2.925055,5.514972
+0.020000,14.989705,5.021529,5.998318,4.179729,-0.491729,-2.925055,5.517919
+0.020000,15.050008,4.963795,6.081803,4.168724,-0.550231,-2.925055,5.521249
+0.020000,15.110346,4.906434,6.165055,4.156549,-0.608732,-2.925055,5.524962
+0.020000,15.170722,4.849483,6.248053,4.143205,-0.667233,-2.925055,5.529058
+0.020000,15.231138,4.792983,6.330772,4.128690,-0.725734,-2.925055,5.533535
+0.020000,15.291596,4.736970,6.413189,4.113005,-0.784235,-2.925055,5.538392
+0.020000,15.352095,4.681483,6.495280,4.096151,-0.842736,-2.925055,5.543627
+0.020000,15.412637,4.626559,6.577023,4.078126,-0.901237,-2.925055,5.549238
+0.020000,15.473218,4.572236,6.658394,4.058931,-0.959738,-2.925055,5.555223
+0.020000,15.533837,4.518549,6.739369,4.038566,-1.018239,-2.925055,5.561580
+0.020000,15.594491,4.465537,6.819925,4.017032,-1.076740,-2.925055,5.568304
+0.020000,15.655175,4.413233,6.900038,3.994327,-1.135242,-2.925055,5.575393
+0.020000,15.715881,4.361673,6.979686,3.970452,-1.193743,-2.925055,5.582841
+0.020000,15.776605,4.310891,7.058845,3.945407,-1.252244,-2.925055,5.590644
+0.020000,15.837335,4.260922,7.137491,3.919192,-1.310745,-2.925055,5.598797
+0.020000,15.898062,4.211796,7.215601,3.891807,-1.369246,-2.925055,5.607293
+0.020000,15.958775,4.163547,7.293151,3.863252,-1.427747,-2.925055,5.616124
+0.020000,16.019460,4.116204,7.370119,3.833527,-1.486248,-2.925055,5.625284
+0.020000,16.080102,4.069797,7.446481,3.802632,-1.544749,-2.925055,5.634762
+0.020000,16.140685,4.024354,7.522213,3.770567,-1.603250,-2.925055,5.644550
+0.020000,16.201190,3.979903,7.597292,3.737332,-1.661751,-2.925055,5.654636
+0.020000,16.261597,3.936467,7.671694,3.702927,-1.720253,-2.925055,5.665009
+0.020000,16.321885,3.894071,7.745397,3.667352,-1.778754,-2.925055,5.675656
+0.020000,16.382031,3.852737,7.818377,3.630607,-1.837255,-2.925055,5.686564
+0.020000,16.442008,3.812486,7.890610,3.592692,-1.895756,-2.925055,5.697717
+0.020000,16.501792,3.773334,7.962073,3.553607,-1.954257,-2.925055,5.709099
+0.020000,16.561352,3.735299,8.032742,3.513352,-2.012758,-2.925055,5.720695
+0.020000,16.620660,3.698394,8.102595,3.471926,-2.071259,-2.925055,5.732486
+0.020000,16.679683,3.662632,8.171608,3.429331,-2.129760,-2.925055,5.744454
+0.020000,16.738389,3.628023,8.239757,3.385566,-2.188261,-2.925055,5.756579
+0.020000,16.796744,3.594573,8.307019,3.340631,-2.246762,-2.925055,5.768841
+0.020000,16.854710,3.562287,8.373370,3.294526,-2.305263,-2.925055,5.781219
+0.020000,16.912252,3.531169,8.438788,3.247250,-2.363765,-2.925055,5.793692
+0.020000,16.969331,3.501217,8.503248,3.198805,-2.422266,-2.925055,5.806236
+0.020000,17.025908,3.472430,8.566728,3.149190,-2.480767,-2.925055,5.818831
+0.020000,17.081947,3.444801,8.629209,3.098879,-2.515547,-1.739020,5.831454
+0.020000,17.137423,3.418315,8.690683,3.048568,-2.515547,0.000000,5.844085
+0.020000,17.192315,3.392953,8.751152,2.998257,-2.515547,-0.000000,5.856707
+0.020000,17.246604,3.368697,8.810614,2.947946,-2.515547,0.000000,5.869302
+0.020000,17.300270,3.345524,8.869070,2.897635,-2.515547,0.000000,5.881853
+0.020000,17.353293,3.323411,8.926519,2.847324,-2.515547,-0.000000,5.894344
+0.020000,17.405654,3.302337,8.982963,2.797013,-2.515547,0.000000,5.906757
+0.020000,17.457333,3.282276,9.038400,2.746702,-2.515547,-0.000000,5.919078
+0.020000,17.508313,3.263203,9.092831,2.696391,-2.515547,0.000000,5.931291
+0.020000,17.558574,3.245092,9.146255,2.646080,-2.515547,0.000000,5.943382
+0.020000,17.608098,3.227916,9.198674,2.595769,-2.515547,-0.000000,5.955338
+0.020000,17.656869,3.211648,9.250086,2.545458,-2.515547,0.000000,5.967146
+0.020000,17.704868,3.196261,9.300492,2.495147,-2.515547,-0.000000,5.978793
+0.020000,17.752081,3.181726,9.349892,2.444836,-2.515547,0.000000,5.990270
+0.020000,17.798491,3.168014,9.398286,2.394525,-2.515547,0.000000,6.001564
+0.020000,17.844083,3.155098,9.445673,2.344215,-2.515547,-0.000000,6.012667
+0.020000,17.888845,3.142947,9.492054,2.293904,-2.515547,0.000000,6.023570
+0.020000,17.932760,3.131533,9.537429,2.243593,-2.515547,0.000000,6.034265
+0.020000,17.975818,3.120828,9.581798,2.193282,-2.515547,-0.000000,6.044744
+0.020000,18.018005,3.110803,9.625160,2.142971,-2.515547,0.000000,6.055001
+0.020000,18.059311,3.101428,9.667517,2.092660,-2.515547,0.000000,6.065030
+0.020000,18.099724,3.092677,9.708867,2.042349,-2.515547,-0.000000,6.074827
+0.020000,18.139235,3.084520,9.749211,1.992038,-2.515547,0.000000,6.084386
+0.020000,18.177833,3.076931,9.788548,1.941727,-2.515547,0.000000,6.093704
+0.020000,18.215511,3.069882,9.826880,1.891416,-2.515547,-0.000000,6.102777
+0.020000,18.252259,3.063347,9.864205,1.841105,-2.515547,-0.000000,6.111603
+0.020000,18.288071,3.057300,9.900524,1.790794,-2.515547,0.000000,6.120181
+0.020000,18.322940,3.051714,9.935837,1.740483,-2.515547,0.000000,6.128507
+0.020000,18.356857,3.046566,9.970143,1.690172,-2.515547,-0.000000,6.136581
+0.020000,18.389819,3.041830,10.003444,1.639861,-2.515547,-0.000000,6.144403
+0.020000,18.421819,3.037484,10.035738,1.589550,-2.515547,0.000000,6.151971
+0.020000,18.452853,3.033504,10.067026,1.539239,-2.515547,0.000000,6.159285
+0.020000,18.482915,3.029868,10.097307,1.488928,-2.515547,0.000000,6.166346
+0.020000,18.512002,3.026554,10.126583,1.438618,-2.515547,-0.000000,6.173155
+0.020000,18.540111,3.023542,10.154852,1.388307,-2.515547,-0.000000,6.179711
+0.020000,18.567236,3.020812,10.182115,1.337996,-2.515547,0.000000,6.186016
+0.020000,18.593376,3.018343,10.208372,1.287685,-2.515547,0.000000,6.192071
+0.020000,18.618529,3.016119,10.233622,1.237374,-2.515547,-0.000000,6.197876
+0.020000,18.642692,3.014120,10.257867,1.187063,-2.515547,-0.000000,6.203434
+0.020000,18.665861,3.012331,10.281105,1.136752,-2.515547,0.000000,6.208746
+0.020000,18.688036,3.010733,10.303337,1.086441,-2.515547,0.000000,6.213812
+0.020000,18.709218,3.009313,10.324567,1.036604,-2.491826,1.186035,6.218637
+0.020000,18.729423,3.008053,10.344813,0.987938,-2.433325,2.925055,6.223224
+0.020000,18.748675,3.006940,10.364097,0.940441,-2.374824,2.925055,6.227581
+0.020000,18.766995,3.005958,10.382442,0.894115,-2.316323,2.925055,6.231715
+0.020000,18.784403,3.005095,10.399873,0.848959,-2.257822,2.925055,6.235632
+0.020000,18.800926,3.004340,10.416412,0.804972,-2.199321,2.925055,6.239340
+0.020000,18.816583,3.003680,10.432083,0.762156,-2.140820,2.925055,6.242844
+0.020000,18.831398,3.003107,10.446910,0.720509,-2.082319,2.925055,6.246151
+0.020000,18.845395,3.002610,10.460915,0.680033,-2.023818,2.925055,6.249268
+0.020000,18.858596,3.002182,10.474123,0.640727,-1.965317,2.925055,6.252201
+0.020000,18.871024,3.001814,10.486556,0.602590,-1.906815,2.925055,6.254956
+0.020000,18.882702,3.001499,10.498238,0.565624,-1.848314,2.925055,6.257539
+0.020000,18.893653,3.001231,10.509193,0.529828,-1.789813,2.925055,6.259956
+0.020000,18.903900,3.001005,10.519443,0.495202,-1.731312,2.925055,6.262214
+0.020000,18.913467,3.000814,10.529013,0.461745,-1.672811,2.925055,6.264319
+0.020000,18.922378,3.000655,10.537925,0.429459,-1.614310,2.925055,6.266275
+0.020000,18.930656,3.000522,10.546203,0.398343,-1.555809,2.925055,6.268090
+0.020000,18.938322,3.000413,10.553870,0.368397,-1.497308,2.925055,6.269768
+0.020000,18.945401,3.000323,10.560950,0.339621,-1.438807,2.925055,6.271316
+0.020000,18.951918,3.000251,10.567467,0.312015,-1.380306,2.925055,6.272739
+0.020000,18.957892,3.000192,10.573443,0.285578,-1.321805,2.925055,6.274042
+0.020000,18.963352,3.000146,10.578901,0.260312,-1.263303,2.925055,6.275232
+0.020000,18.968317,3.000109,10.583867,0.236216,-1.204802,2.925055,6.276313
+0.020000,18.972812,3.000080,10.588362,0.213290,-1.146301,2.925055,6.277290
+0.020000,18.976861,3.000058,10.592410,0.191534,-1.087800,2.925055,6.278170
+0.020000,18.980484,3.000041,10.596035,0.170948,-1.029299,2.925055,6.278957
+0.020000,18.983709,3.000029,10.599260,0.151532,-0.970798,2.925055,6.279657
+0.020000,18.986558,3.000020,10.602108,0.133286,-0.912297,2.925055,6.280275
+0.020000,18.989054,3.000013,10.604603,0.116211,-0.853796,2.925055,6.280815
+0.020000,18.991218,3.000008,10.606768,0.100305,-0.795295,2.925055,6.281284
+0.020000,18.993076,3.000005,10.608627,0.085569,-0.736794,2.925055,6.281687
+0.020000,18.994652,3.000003,10.610202,0.072003,-0.678292,2.925055,6.282028
+0.020000,18.995968,3.000002,10.611519,0.059607,-0.619791,2.925055,6.282313
+0.020000,18.997048,3.000001,10.612598,0.048381,-0.561290,2.925055,6.282547
+0.020000,18.997916,3.000000,10.613466,0.038325,-0.502789,2.925055,6.282734
+0.020000,18.998594,3.000000,10.614143,0.029440,-0.444288,2.925055,6.282881
+0.020000,18.999105,3.000000,10.614655,0.021724,-0.385787,2.925055,6.282992
+0.020000,18.999475,3.000000,10.615024,0.015178,-0.327286,2.925055,6.283072
+0.020000,18.999725,3.000000,10.615274,0.009803,-0.268785,2.925055,6.283126
+0.020000,18.999879,3.000000,10.615428,0.005597,-0.210284,2.925055,6.283159
+0.020000,18.999960,3.000000,10.615509,0.002561,-0.151783,2.925055,6.283177
+0.020000,18.999993,3.000000,10.615542,0.000696,-0.093281,2.925055,6.283184
+0.020000,19.000000,3.000000,10.615549,0.000000,-0.034780,2.925055,6.283185
+0.020000,19.000000,3.000000,10.615549,0.000000,0.000000,1.739020,6.283185
diff --git a/src/main/deploy/output/testPath.right.pf1.csv b/src/main/deploy/output/testPath.right.pf1.csv
new file mode 100644
index 0000000..c568db0
--- /dev/null
+++ b/src/main/deploy/output/testPath.right.pf1.csv
@@ -0,0 +1,254 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,10.003179,6.750004,0.000012,0.001170,0.058501,2.925055,0.002544
+0.020000,10.003167,6.750004,0.000024,0.000632,-0.026898,-4.269935,0.002533
+0.020000,10.003138,6.750004,0.000053,0.001422,0.039505,3.320118,0.002511
+0.020000,10.003088,6.750004,0.000103,0.002528,0.055308,0.790150,0.002470
+0.020000,10.003009,6.750004,0.000182,0.003951,0.071112,0.790233,0.002407
+0.020000,10.002895,6.750003,0.000296,0.005689,0.086920,0.790370,0.002316
+0.020000,10.002740,6.750003,0.000451,0.007744,0.102731,0.790573,0.002192
+0.020000,10.002538,6.750003,0.000653,0.010115,0.118548,0.790854,0.002030
+0.020000,10.002282,6.750002,0.000909,0.012802,0.134373,0.791228,0.001825
+0.020000,10.001966,6.750002,0.001225,0.015806,0.150207,0.791705,0.001573
+0.020000,10.001583,6.750001,0.001608,0.019127,0.166053,0.792298,0.001267
+0.020000,10.001128,6.750001,0.002063,0.022765,0.181913,0.793017,0.000902
+0.020000,10.000593,6.750000,0.002598,0.026721,0.197791,0.793874,0.000475
+0.020000,10.000072,6.750000,0.003120,0.026095,-0.031298,-11.454459,6.283164
+0.020000,10.001993,6.749999,0.005041,0.096086,3.499514,176.540644,6.282594
+0.020000,10.004179,6.749997,0.007227,0.109265,0.658992,-142.026126,6.281946
+0.020000,10.006644,6.749993,0.009692,0.123276,0.700509,2.075851,6.281214
+0.020000,10.009407,6.749987,0.012456,0.138169,0.744668,2.207958,6.280392
+0.020000,10.012487,6.749977,0.015535,0.153983,0.790684,2.300810,6.279476
+0.020000,10.015897,6.749962,0.018945,0.170474,0.824573,1.694440,6.278460
+0.020000,10.019655,6.749943,0.022704,0.187948,0.873717,2.457189,6.277339
+0.020000,10.023779,6.749916,0.026827,0.206160,0.910594,1.843865,6.276108
+0.020000,10.028284,6.749881,0.031332,0.225257,0.954851,2.212859,6.274760
+0.020000,10.033186,6.749836,0.036234,0.245119,0.993074,1.911150,6.273291
+0.020000,10.038504,6.749779,0.041553,0.265918,1.039974,2.344977,6.271695
+0.020000,10.044251,6.749708,0.047301,0.287392,1.073686,1.685592,6.269966
+0.020000,10.050446,6.749621,0.053496,0.309766,1.118675,2.249486,6.268099
+0.020000,10.057105,6.749513,0.060156,0.332995,1.161451,2.138796,6.266088
+0.020000,10.064243,6.749384,0.067295,0.356947,1.197632,1.809031,6.263927
+0.020000,10.071875,6.749228,0.074929,0.381718,1.238547,2.045762,6.261610
+0.020000,10.080019,6.749042,0.083075,0.407297,1.278921,2.018674,6.259132
+0.020000,10.088690,6.748822,0.091748,0.433671,1.318715,1.989718,6.256486
+0.020000,10.097903,6.748563,0.100965,0.460829,1.357895,1.959009,6.253666
+0.020000,10.107673,6.748260,0.110740,0.488757,1.396429,1.926694,6.250666
+0.020000,10.118016,6.747907,0.121089,0.517443,1.434288,1.892947,6.247480
+0.020000,10.128949,6.747498,0.132029,0.547012,1.478452,2.208186,6.244100
+0.020000,10.140484,6.747026,0.143574,0.577238,1.511288,1.641790,6.240522
+0.020000,10.152636,6.746484,0.155738,0.608210,1.548594,1.865338,6.236738
+0.020000,10.165422,6.745864,0.168539,0.640045,1.591770,2.158805,6.232741
+0.020000,10.178854,6.745158,0.181990,0.672532,1.624355,1.629227,6.228526
+0.020000,10.192946,6.744355,0.196105,0.705756,1.661181,1.841302,6.224084
+0.020000,10.207715,6.743447,0.210902,0.739847,1.704573,2.169624,6.219410
+0.020000,10.223173,6.742422,0.226394,0.774582,1.736724,1.607528,6.214496
+0.020000,10.239325,6.741269,0.242586,0.809640,1.752898,0.808680,6.209338
+0.020000,10.256167,6.739977,0.259478,0.844602,1.748086,-0.240606,6.203935
+0.020000,10.273697,6.738535,0.277068,0.879458,1.742840,-0.262290,6.198286
+0.020000,10.291913,6.736931,0.295354,0.914327,1.743427,0.029343,6.192387
+0.020000,10.310810,6.735152,0.314334,0.948999,1.733620,-0.490324,6.186239
+0.020000,10.330383,6.733185,0.334006,0.983596,1.729820,-0.189995,6.179840
+0.020000,10.350632,6.731017,0.354370,1.018222,1.731319,0.074960,6.173190
+0.020000,10.371551,6.728633,0.375425,1.052711,1.724448,-0.343561,6.166286
+0.020000,10.393136,6.726020,0.397168,1.087158,1.722355,-0.104652,6.159130
+0.020000,10.415386,6.723162,0.419600,1.121628,1.723499,0.057195,6.151719
+0.020000,10.438296,6.720043,0.442722,1.156086,1.722909,-0.029512,6.144055
+0.020000,10.461863,6.716648,0.466533,1.190522,1.721800,-0.055459,6.136137
+0.020000,10.486085,6.712959,0.491033,1.225035,1.725637,0.191846,6.127966
+0.020000,10.510957,6.708960,0.516225,1.259581,1.727302,0.083256,6.119542
+0.020000,10.536476,6.704632,0.542108,1.294172,1.729565,0.113142,6.110867
+0.020000,10.562640,6.699958,0.568687,1.328916,1.737157,0.379615,6.101944
+0.020000,10.589446,6.694919,0.595962,1.363790,1.743701,0.327215,6.092773
+0.020000,10.616893,6.689495,0.623939,1.398855,1.753271,0.478473,6.083358
+0.020000,10.644976,6.683666,0.652621,1.434091,1.761806,0.426763,6.073702
+0.020000,10.673695,6.677411,0.682013,1.469597,1.775316,0.675529,6.063810
+0.020000,10.703048,6.670710,0.712121,1.505402,1.790214,0.744876,6.053685
+0.020000,10.733033,6.663541,0.742952,1.541536,1.806701,0.824372,6.043333
+0.020000,10.763650,6.655882,0.774513,1.578035,1.824974,0.913640,6.032760
+0.020000,10.794898,6.647709,0.806812,1.614940,1.845218,1.012167,6.021973
+0.020000,10.826776,6.638999,0.839858,1.652323,1.869153,1.196763,6.010979
+0.020000,10.859285,6.629728,0.873663,1.690246,1.896170,1.350862,5.999787
+0.020000,10.892424,6.619872,0.908237,1.728701,1.922766,1.329802,5.988405
+0.020000,10.926194,6.609404,0.943592,1.767772,1.953534,1.538400,5.976844
+0.020000,10.960598,6.598300,0.979743,1.807551,1.988931,1.769835,5.965113
+0.020000,10.995635,6.586532,1.016704,1.848014,2.023184,1.712647,5.953225
+0.020000,11.031307,6.574075,1.054489,1.889245,2.061548,1.918239,5.941192
+0.020000,11.067616,6.560899,1.093114,1.931295,2.102497,2.047434,5.929027
+0.020000,11.104565,6.546978,1.132599,1.974227,2.146615,2.205881,5.916744
+0.020000,11.142156,6.532284,1.172960,2.018039,2.190568,2.197677,5.904357
+0.020000,11.180391,6.516786,1.214216,2.062807,2.238414,2.392298,5.891881
+0.020000,11.219273,6.500457,1.256387,2.108577,2.288516,2.505090,5.879332
+0.020000,11.258804,6.483267,1.299495,2.155365,2.339388,2.543594,5.866727
+0.020000,11.298988,6.465187,1.343559,2.203207,2.392104,2.635803,5.854083
+0.020000,11.339828,6.446187,1.388602,2.252141,2.446684,2.729010,5.841416
+0.020000,11.381325,6.426237,1.434645,2.302186,2.502230,2.777312,5.828746
+0.020000,11.423483,6.405308,1.481713,2.353358,2.558606,2.818752,5.816089
+0.020000,11.466297,6.383375,1.529817,2.405229,2.593558,1.747603,5.803466
+0.020000,11.509752,6.360418,1.578964,2.457353,2.606197,0.631989,5.790901
+0.020000,11.553836,6.336417,1.629158,2.509711,2.617930,0.586650,5.778415
+0.020000,11.598536,6.311356,1.680404,2.562275,2.628188,0.512882,5.766030
+0.020000,11.643836,6.285218,1.732704,2.615011,2.636777,0.429452,5.753769
+0.020000,11.689724,6.257991,1.786062,2.667876,2.643266,0.324434,5.741651
+0.020000,11.736185,6.229662,1.840478,2.720825,2.647462,0.209828,5.729698
+0.020000,11.783205,6.200222,1.895954,2.773812,2.649344,0.094086,5.717927
+0.020000,11.830770,6.169662,1.952490,2.826778,2.648290,-0.052698,5.706359
+0.020000,11.878864,6.137978,2.010083,2.879664,2.644313,-0.198866,5.695009
+0.020000,11.927474,6.105164,2.068732,2.932411,2.637315,-0.349882,5.683895
+0.020000,11.976585,6.071222,2.128431,2.984955,2.627227,-0.504389,5.673031
+0.020000,12.026182,6.036150,2.189175,3.037235,2.614007,-0.661004,5.662434
+0.020000,12.076252,5.999953,2.250959,3.089188,2.597640,-0.818352,5.652115
+0.020000,12.126781,5.962636,2.313774,3.140751,2.578138,-0.975102,5.642087
+0.020000,12.177755,5.924206,2.377611,3.191844,2.554677,-1.173045,5.632363
+0.020000,12.229160,5.884674,2.442460,3.242428,2.529198,-1.273945,5.622952
+0.020000,12.280985,5.844051,2.508309,3.292449,2.501031,-1.408378,5.613863
+0.020000,12.333217,5.802351,2.575145,3.341803,2.467716,-1.665749,5.605106
+0.020000,12.385845,5.759590,2.642955,3.390505,2.435107,-1.630464,5.596687
+0.020000,12.438858,5.715786,2.711723,3.438435,2.396476,-1.931550,5.588615
+0.020000,12.492245,5.670958,2.781436,3.485609,2.358721,-1.887743,5.580895
+0.020000,12.545998,5.625127,2.852074,3.531911,2.315080,-2.182012,5.573532
+0.020000,12.600108,5.578317,2.923621,3.577370,2.272949,-2.106574,5.566531
+0.020000,12.654567,5.530552,2.996059,3.621905,2.226766,-2.309153,5.559896
+0.020000,12.709368,5.481858,3.069369,3.665477,2.178598,-2.408380,5.553631
+0.020000,12.764506,5.432262,3.143531,3.708088,2.130552,-2.402324,5.547739
+0.020000,12.819975,5.381792,3.218524,3.749688,2.079985,-2.528335,5.542222
+0.020000,12.875772,5.330478,3.294329,3.790239,2.027540,-2.622267,5.537083
+0.020000,12.931893,5.278351,3.370924,3.829742,1.975176,-2.618199,5.532323
+0.020000,12.988336,5.225443,3.448287,3.868174,1.921591,-2.679223,5.527943
+0.020000,13.045099,5.171786,3.526398,3.905512,1.866919,-2.733636,5.523946
+0.020000,13.102183,5.117413,3.605233,3.941743,1.811551,-2.768382,5.520332
+0.020000,13.159587,5.062359,3.684770,3.976852,1.755414,-2.806840,5.517102
+0.020000,13.217312,5.006659,3.764986,4.010833,1.699065,-2.817470,5.514257
+0.020000,13.275361,4.950349,3.845860,4.043682,1.642466,-2.829952,5.511796
+0.020000,13.333737,4.893465,3.927368,4.075389,1.585336,-2.856465,5.509722
+0.020000,13.392442,4.836043,4.009487,4.105948,1.527972,-2.868214,5.508033
+0.020000,13.451482,4.778123,4.092194,4.135357,1.470433,-2.876975,5.506730
+0.020000,13.510860,4.719740,4.175466,4.163612,1.412767,-2.883294,5.505815
+0.020000,13.570582,4.660934,4.259280,4.190713,1.355012,-2.887718,5.505285
+0.020000,13.630650,4.601748,4.343609,4.216419,1.285327,-3.484264,5.505143
+0.020000,13.691060,4.542231,4.428411,4.240137,1.185900,-4.971344,5.505388
+0.020000,13.751808,4.482433,4.513653,4.262088,1.097533,-4.418339,5.506020
+0.020000,13.812902,4.422395,4.599310,4.282851,1.038166,-2.968384,5.507039
+0.020000,13.874348,4.362156,4.685359,4.302426,0.978728,-2.971865,5.508444
+0.020000,13.936154,4.301758,4.771775,4.320810,0.919187,-2.977076,5.510234
+0.020000,13.998325,4.241244,4.858535,4.338000,0.859498,-2.984454,5.512411
+0.020000,14.060869,4.180654,4.945615,4.353992,0.799607,-2.994557,5.514972
+0.020000,14.123793,4.120032,5.032990,4.368781,0.739448,-3.007942,5.517919
+0.020000,14.187103,4.059419,5.120638,4.382360,0.678973,-3.023767,5.521249
+0.020000,14.250806,3.998860,5.208532,4.394732,0.618602,-3.018504,5.524962
+0.020000,14.314906,3.938397,5.296650,4.405866,0.556716,-3.094332,5.529058
+0.020000,14.379410,3.878074,5.384965,4.415759,0.494624,-3.104590,5.533535
+0.020000,14.444321,3.817935,5.473453,4.424394,0.431773,-3.142573,5.538392
+0.020000,14.509643,3.758025,5.562088,4.431759,0.368236,-3.176807,5.543627
+0.020000,14.575379,3.698388,5.650845,4.437847,0.304415,-3.191081,5.549238
+0.020000,14.641531,3.639071,5.739697,4.442599,0.237572,-3.342130,5.555223
+0.020000,14.708099,3.580117,5.828617,4.446005,0.170309,-3.363142,5.561580
+0.020000,14.775082,3.521573,5.917578,4.448067,0.103093,-3.360835,5.568304
+0.020000,14.842477,3.463484,6.006552,4.448703,0.031826,-3.563349,5.575393
+0.020000,14.910280,3.405898,6.095510,4.447895,-0.040395,-3.611045,5.582841
+0.020000,14.978486,3.348859,6.184423,4.445640,-0.112787,-3.619585,5.590644
+0.020000,15.047086,3.292414,6.273259,4.441818,-0.191099,-3.915586,5.598797
+0.020000,15.116070,3.236610,6.361989,4.436482,-0.266763,-3.783235,5.607293
+0.020000,15.185426,3.181493,6.450578,4.429479,-0.350193,-4.171486,5.616124
+0.020000,15.255138,3.127107,6.538995,4.420846,-0.431648,-4.072767,5.625284
+0.020000,15.325189,3.073501,6.627205,4.410469,-0.518856,-4.360361,5.634762
+0.020000,15.395559,3.020717,6.715171,4.398305,-0.608187,-4.466594,5.644550
+0.020000,15.466225,2.968801,6.802857,4.384321,-0.699187,-4.549986,5.654636
+0.020000,15.537160,2.917796,6.890226,4.368427,-0.794722,-4.776748,5.665009
+0.020000,15.608335,2.867745,6.977237,4.350561,-0.893303,-4.929058,5.675656
+0.020000,15.679717,2.818689,7.063850,4.330668,-0.994626,-5.066124,5.686564
+0.020000,15.751271,2.770669,7.150024,4.308684,-1.099193,-5.228377,5.697717
+0.020000,15.822957,2.723723,7.235715,4.284545,-1.206969,-5.388805,5.709099
+0.020000,15.894734,2.677887,7.320879,4.258187,-1.317868,-5.544946,5.720695
+0.020000,15.966556,2.633196,7.405470,4.229552,-1.431750,-5.694091,5.732486
+0.020000,16.038374,2.589682,7.489442,4.198593,-1.547948,-5.809874,5.744454
+0.020000,16.110136,2.547376,7.572746,4.165237,-1.667847,-5.994984,5.756579
+0.020000,16.181788,2.506303,7.655335,4.129446,-1.789518,-6.083518,5.768841
+0.020000,16.253272,2.466489,7.737159,4.091178,-1.913405,-6.194387,5.781219
+0.020000,16.324528,2.427955,7.818167,4.050390,-2.039381,-6.298761,5.793692
+0.020000,16.395492,2.390717,7.898308,4.007056,-2.166721,-6.367039,5.806236
+0.020000,16.466101,2.354791,7.977531,3.961140,-2.295802,-6.454015,5.818831
+0.020000,16.536292,2.320185,8.055789,3.912930,-2.410513,-5.735584,5.831454
+0.020000,16.606016,2.286896,8.133052,3.863156,-2.488676,-3.908140,5.844085
+0.020000,16.675231,2.254918,8.209298,3.812266,-2.544510,-2.791692,5.856707
+0.020000,16.743895,2.224239,8.284503,3.760257,-2.600439,-2.796463,5.869302
+0.020000,16.811964,2.194847,8.358647,3.707198,-2.652969,-2.626467,5.881853
+0.020000,16.879397,2.166725,8.431709,3.653106,-2.704612,-2.582166,5.894344
+0.020000,16.946152,2.139858,8.503668,3.597953,-2.757609,-2.649858,5.906757
+0.020000,17.012189,2.114223,8.574505,3.541875,-2.803931,-2.316117,5.919078
+0.020000,17.077467,2.089801,8.644203,3.484859,-2.850792,-2.343037,5.931291
+0.020000,17.141947,2.066566,8.712741,3.426922,-2.896866,-2.303669,5.943382
+0.020000,17.205591,2.044494,8.780104,3.368149,-2.938631,-2.088287,5.955338
+0.020000,17.268363,2.023556,8.846276,3.308589,-2.978007,-1.968795,5.967146
+0.020000,17.330227,2.003724,8.911241,3.248238,-3.017546,-1.976950,5.978793
+0.020000,17.391149,1.984968,8.974985,3.187227,-3.050547,-1.650037,5.990270
+0.020000,17.451100,1.967256,9.037497,3.125606,-3.081066,-1.525941,6.001564
+0.020000,17.510045,1.950557,9.098762,3.063259,-3.117353,-1.814346,6.012667
+0.020000,17.567959,1.934836,9.158773,3.000509,-3.137497,-1.007220,6.023570
+0.020000,17.624813,1.920060,9.217515,2.937132,-3.168843,-1.567296,6.034265
+0.020000,17.680582,1.906194,9.274982,2.873349,-3.189156,-1.015664,6.044744
+0.020000,17.735244,1.893204,9.331166,2.809176,-3.208659,-0.975132,6.055001
+0.020000,17.788775,1.881055,9.386058,2.744633,-3.227115,-0.922786,6.065030
+0.020000,17.841157,1.869712,9.439654,2.679782,-3.242562,-0.772392,6.074827
+0.020000,17.892370,1.859140,9.491947,2.614643,-3.256939,-0.718845,6.084386
+0.020000,17.942395,1.849304,9.542930,2.549182,-3.273047,-0.805391,6.093704
+0.020000,17.991222,1.840169,9.592604,2.483698,-3.274202,-0.057722,6.102777
+0.020000,18.038832,1.831703,9.640961,2.417849,-3.292452,-0.912501,6.111603
+0.020000,18.085217,1.823870,9.688002,2.352055,-3.289734,0.135897,6.120181
+0.020000,18.130362,1.816638,9.733723,2.286047,-3.300399,-0.533261,6.128507
+0.020000,18.174258,1.809975,9.778122,2.219931,-3.305760,-0.268039,6.136581
+0.020000,18.216897,1.803849,9.821199,2.153841,-3.304542,0.060906,6.144403
+0.020000,18.258271,1.798229,9.862953,2.087718,-3.306102,-0.078026,6.151971
+0.020000,18.298375,1.793086,9.903385,2.021581,-3.306887,-0.039241,6.159285
+0.020000,18.337198,1.788390,9.942492,1.955339,-3.312102,-0.260732,6.166346
+0.020000,18.374741,1.784113,9.980277,1.889292,-3.302353,0.487432,6.173155
+0.020000,18.410999,1.780228,10.016743,1.823253,-3.301930,0.021156,6.179711
+0.020000,18.445966,1.776708,10.051886,1.757176,-3.303863,-0.096644,6.186016
+0.020000,18.479641,1.773529,10.085711,1.691234,-3.297077,0.339295,6.192071
+0.020000,18.512023,1.770665,10.118219,1.625419,-3.290761,0.315797,6.197876
+0.020000,18.543109,1.768093,10.149412,1.559630,-3.289440,0.066050,6.203434
+0.020000,18.572897,1.765792,10.179289,1.493852,-3.288901,0.026962,6.208746
+0.020000,18.601389,1.763740,10.207854,1.428273,-3.278943,0.497864,6.213812
+0.020000,18.628588,1.761916,10.235114,1.362995,-3.263916,0.751373,6.218637
+0.020000,18.654516,1.760300,10.261093,1.298928,-3.203358,3.027885,6.223224
+0.020000,18.679205,1.758872,10.285823,1.236519,-3.120425,4.146647,6.227581
+0.020000,18.702686,1.757613,10.309338,1.175723,-3.039825,4.030002,6.231715
+0.020000,18.724984,1.756508,10.331663,1.116281,-2.972076,3.387442,6.235632
+0.020000,18.746137,1.755541,10.352838,1.058748,-2.876665,4.770557,6.239340
+0.020000,18.766170,1.754697,10.372889,1.002520,-2.811383,3.264120,6.242844
+0.020000,18.785115,1.753964,10.391849,0.948002,-2.725938,4.272258,6.246151
+0.020000,18.803006,1.753329,10.409750,0.895097,-2.645252,4.034286,6.249268
+0.020000,18.819872,1.752782,10.426625,0.843715,-2.569101,3.807527,6.252201
+0.020000,18.835742,1.752312,10.442502,0.793865,-2.492454,3.832357,6.254956
+0.020000,18.850648,1.751910,10.457413,0.745558,-2.415359,3.854774,6.257539
+0.020000,18.864619,1.751568,10.471389,0.698801,-2.337863,3.874807,6.259956
+0.020000,18.877688,1.751280,10.484461,0.653601,-2.260013,3.892491,6.262214
+0.020000,18.889885,1.751037,10.496661,0.609964,-2.181855,3.907864,6.264319
+0.020000,18.901241,1.750833,10.508018,0.567895,-2.103436,3.920971,6.266275
+0.020000,18.911788,1.750665,10.518566,0.527399,-2.024799,3.931858,6.268090
+0.020000,18.921551,1.750525,10.528331,0.488216,-1.959127,3.283576,6.269768
+0.020000,18.930565,1.750412,10.537345,0.450730,-1.874337,4.239539,6.271316
+0.020000,18.938861,1.750319,10.545642,0.414817,-1.795620,3.935836,6.272739
+0.020000,18.946464,1.750244,10.553245,0.380174,-1.732190,3.171505,6.274042
+0.020000,18.953411,1.750185,10.560192,0.347350,-1.641162,4.551373,6.275232
+0.020000,18.959726,1.750138,10.566507,0.315755,-1.579778,3.069218,6.276313
+0.020000,18.965443,1.750102,10.572224,0.285848,-1.495335,4.222166,6.277290
+0.020000,18.970592,1.750074,10.577374,0.257487,-1.418044,3.864555,6.278170
+0.020000,18.975199,1.750052,10.581981,0.230334,-1.357672,3.018575,6.278957
+0.020000,18.979298,1.750037,10.586080,0.204971,-1.268144,4.476426,6.279657
+0.020000,18.982920,1.750025,10.589702,0.181076,-1.194724,3.670987,6.280275
+0.020000,18.986091,1.750016,10.592873,0.158580,-1.124811,3.495661,6.280815
+0.020000,18.988842,1.750011,10.595623,0.137513,-1.053376,3.571744,6.281284
+0.020000,18.991203,1.750007,10.597985,0.118070,-0.972113,4.063140,6.281687
+0.020000,18.993205,1.750004,10.599987,0.100106,-0.898207,3.695299,6.282028
+0.020000,18.994878,1.750002,10.601659,0.083622,-0.824200,3.700349,6.282313
+0.020000,18.996250,1.750001,10.603032,0.068620,-0.750108,3.704607,6.282547
+0.020000,18.997352,1.750001,10.604134,0.055101,-0.675945,3.708138,6.282734
+0.020000,18.998213,1.750000,10.604995,0.043067,-0.601725,3.711009,6.282881
+0.020000,18.998864,1.750000,10.605646,0.032518,-0.527459,3.713289,6.282992
+0.020000,18.999333,1.750000,10.606115,0.023454,-0.453158,3.715044,6.283072
+0.020000,18.999650,1.750000,10.606432,0.015878,-0.378831,3.716344,6.283126
+0.020000,18.999846,1.750000,10.606628,0.009788,-0.304486,3.717256,6.283159
+0.020000,18.999950,1.750000,10.606732,0.005185,-0.230129,3.717851,6.283177
+0.020000,18.999991,1.750000,10.606773,0.002070,-0.155765,3.718196,6.283184
+0.020000,19.000000,1.750000,10.606782,0.000442,-0.081398,3.718362,6.283185
+0.020000,19.000000,1.750000,10.606782,0.000000,-0.022107,2.964554,6.283185
diff --git a/src/main/deploy/output/trenchAndAim.left.pf1.csv b/src/main/deploy/output/trenchAndAim.left.pf1.csv
new file mode 100644
index 0000000..bee5a14
--- /dev/null
+++ b/src/main/deploy/output/trenchAndAim.left.pf1.csv
@@ -0,0 +1,306 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,9.250000,0.000012,0.001175,0.058748,2.937403,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,-0.058748,-5.874806,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,2.937403,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,9.250000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.001926,9.250000,0.001938,0.096293,4.814639,240.731970,0.000000
+0.020000,0.004570,9.250000,0.004581,0.132183,1.794517,-151.006114,0.000000
+0.020000,0.007577,9.250000,0.007589,0.150395,0.910595,-44.196111,0.000000
+0.020000,0.010973,9.250000,0.010985,0.169782,0.969343,2.937403,0.000000
+0.020000,0.014780,9.250000,0.014792,0.190344,1.028091,2.937403,0.000000
+0.020000,0.019022,9.250000,0.019033,0.212080,1.086839,2.937403,0.000000
+0.020000,0.023721,9.250000,0.023733,0.234992,1.145587,2.937403,0.000000
+0.020000,0.028903,9.250000,0.028915,0.259079,1.204335,2.937403,0.000000
+0.020000,0.034590,9.250000,0.034602,0.284341,1.263083,2.937403,0.000000
+0.020000,0.040805,9.250000,0.040817,0.310777,1.321831,2.937403,0.000000
+0.020000,0.047573,9.250000,0.047585,0.338389,1.380579,2.937403,0.000000
+0.020000,0.054917,9.250000,0.054928,0.367175,1.439327,2.937403,0.000000
+0.020000,0.062859,9.250000,0.062871,0.397137,1.498075,2.937403,0.000000
+0.020000,0.071425,9.250000,0.071437,0.428273,1.556824,2.937403,0.000000
+0.020000,0.080637,9.250000,0.080648,0.460585,1.615572,2.937403,0.000000
+0.020000,0.090518,9.250000,0.090530,0.494071,1.674320,2.937403,0.000000
+0.020000,0.101093,9.250000,0.101104,0.528733,1.733068,2.937403,0.000000
+0.020000,0.112384,9.250000,0.112396,0.564569,1.791816,2.937403,0.000000
+0.020000,0.124416,9.250000,0.124427,0.601580,1.850564,2.937403,0.000000
+0.020000,0.137211,9.250000,0.137223,0.639766,1.909312,2.937403,0.000000
+0.020000,0.150793,9.250000,0.150805,0.679128,1.968060,2.937403,0.000000
+0.020000,0.165187,9.250000,0.165198,0.719664,2.026808,2.937403,0.000000
+0.020000,0.180414,9.250000,0.180426,0.761375,2.085556,2.937403,0.000000
+0.020000,0.196499,9.250000,0.196511,0.804261,2.144304,2.937403,0.000000
+0.020000,0.213466,9.250000,0.213478,0.848322,2.203052,2.937403,0.000000
+0.020000,0.231337,9.250000,0.231349,0.893558,2.261800,2.937403,0.000000
+0.020000,0.250136,9.250000,0.250148,0.939969,2.320548,2.937403,0.000000
+0.020000,0.269887,9.250000,0.269899,0.987555,2.379296,2.937403,0.000000
+0.020000,0.290614,9.250000,0.290626,1.036316,2.438044,2.937403,0.000000
+0.020000,0.312339,9.250000,0.312351,1.086252,2.496792,2.937403,0.000000
+0.020000,0.335086,9.250000,0.335098,1.137362,2.555541,2.937403,0.000000
+0.020000,0.358879,9.250000,0.358891,1.189648,2.614289,2.937403,0.000000
+0.020000,0.383741,9.250000,0.383753,1.243109,2.673037,2.937403,0.000000
+0.020000,0.409696,9.250000,0.409708,1.297745,2.731785,2.937403,0.000000
+0.020000,0.436767,9.250000,0.436779,1.353555,2.790533,2.937403,0.000000
+0.020000,0.464978,9.250000,0.464990,1.410541,2.849281,2.937403,0.000000
+0.020000,0.494352,9.250000,0.494364,1.468701,2.908029,2.937403,0.000000
+0.020000,0.524913,9.250000,0.524925,1.528037,2.966777,2.937403,0.000000
+0.020000,0.556684,9.250000,0.556696,1.588547,3.025525,2.937403,0.000000
+0.020000,0.589688,9.250000,0.589700,1.650233,3.084273,2.937403,0.000000
+0.020000,0.623950,9.250000,0.623962,1.713093,3.143021,2.937403,0.000000
+0.020000,0.659493,9.250000,0.659505,1.777129,3.201769,2.937403,0.000000
+0.020000,0.696340,9.250000,0.696351,1.842339,3.260517,2.937403,0.000000
+0.020000,0.734514,9.250000,0.734526,1.908724,3.319265,2.937403,0.000000
+0.020000,0.774040,9.250000,0.774052,1.976285,3.378013,2.937403,0.000000
+0.020000,0.814940,9.250000,0.814952,2.045020,3.436761,2.937403,0.000000
+0.020000,0.857239,9.250000,0.857251,2.114930,3.495509,2.937403,0.000000
+0.020000,0.900959,9.250000,0.900971,2.186015,3.554258,2.937403,0.000000
+0.020000,0.946125,9.250000,0.946136,2.258275,3.613006,2.937403,0.000000
+0.020000,0.992759,9.250000,0.992771,2.331710,3.671754,2.937403,0.000000
+0.020000,1.040885,9.250000,1.040897,2.406320,3.730502,2.937403,0.000000
+0.020000,1.090527,9.250000,1.090539,2.482105,3.789250,2.937403,0.000000
+0.020000,1.141709,9.250000,1.141720,2.559065,3.847998,2.937403,0.000000
+0.020000,1.194453,9.250000,1.194464,2.637200,3.906746,2.937403,0.000000
+0.020000,1.248783,9.250000,1.248795,2.716510,3.965494,2.937403,0.000000
+0.020000,1.304723,9.250000,1.304735,2.796995,4.024242,2.937403,0.000000
+0.020000,1.362284,9.250000,1.362296,2.878067,4.053616,1.468701,0.000000
+0.020000,1.421467,9.250000,1.421479,2.959140,4.053616,0.000000,0.000000
+0.020000,1.482271,9.250000,1.482283,3.040212,4.053616,0.000000,0.000000
+0.020000,1.544697,9.250000,1.544709,3.121284,4.053616,0.000000,0.000000
+0.020000,1.608744,9.250000,1.608756,3.202357,4.053616,-0.000000,0.000000
+0.020000,1.674413,9.250000,1.674424,3.283429,4.053616,0.000000,0.000000
+0.020000,1.741703,9.250000,1.741714,3.364501,4.053616,-0.000000,0.000000
+0.020000,1.810614,9.250000,1.810626,3.445574,4.053616,0.000000,0.000000
+0.020000,1.881147,9.250000,1.881159,3.526646,4.053616,-0.000000,0.000000
+0.020000,1.953301,9.250000,1.953313,3.607718,4.053616,0.000000,0.000000
+0.020000,2.027077,9.250000,2.027089,3.688791,4.053616,0.000000,0.000000
+0.020000,2.102474,9.250000,2.102486,3.769863,4.053616,-0.000000,0.000000
+0.020000,2.179493,9.250000,2.179505,3.850935,4.053616,-0.000000,0.000000
+0.020000,2.258133,9.250000,2.258145,3.932008,4.053616,0.000000,0.000000
+0.020000,2.338395,9.250000,2.338407,4.013080,4.053616,-0.000000,0.000000
+0.020000,2.420266,9.250000,2.420278,4.093565,4.024242,-1.468701,0.000000
+0.020000,2.503724,9.250000,2.503735,4.172875,3.965494,-2.937403,0.000000
+0.020000,2.588744,9.250000,2.588756,4.251009,3.906746,-2.937403,0.000000
+0.020000,2.675303,9.250000,2.675315,4.327969,3.847998,-2.937403,0.000000
+0.020000,2.763378,9.250000,2.763390,4.403754,3.789250,-2.937403,0.000000
+0.020000,2.852946,9.250000,2.852957,4.478364,3.730502,-2.937403,0.000000
+0.020000,2.943982,9.250000,2.943993,4.551800,3.671754,-2.937403,0.000000
+0.020000,3.036463,9.250000,3.036475,4.624060,3.613006,-2.937403,0.000000
+0.020000,3.130366,9.250000,3.130377,4.695145,3.554258,-2.937403,0.000000
+0.020000,3.225667,9.250000,3.225679,4.765055,3.495509,-2.937403,0.000000
+0.020000,3.322343,9.250000,3.322354,4.833790,3.436761,-2.937403,0.000000
+0.020000,3.420370,9.250000,3.420381,4.901350,3.378013,-2.937403,0.000000
+0.020000,3.519724,9.250000,3.519736,4.967736,3.319265,-2.937403,0.000000
+0.020000,3.620383,9.250000,3.620395,5.032946,3.260517,-2.937403,0.000000
+0.020000,3.722323,9.250000,3.722335,5.096982,3.201769,-2.937403,0.000000
+0.020000,3.825520,9.250000,3.825531,5.159842,3.143021,-2.937403,0.000000
+0.020000,3.929950,9.250000,3.929962,5.221527,3.084273,-2.937403,0.000000
+0.020000,4.035591,9.250000,4.035603,5.282038,3.025525,-2.937403,0.000000
+0.020000,4.142418,9.250000,4.142430,5.341373,2.966777,-2.937403,0.000000
+0.020000,4.250409,9.250000,4.250421,5.399534,2.908029,-2.937403,0.000000
+0.020000,4.359540,9.250000,4.359551,5.456520,2.849281,-2.937403,0.000000
+0.020000,4.469786,9.250000,4.469798,5.512330,2.790533,-2.937403,0.000000
+0.020000,4.581125,9.250000,4.581137,5.566966,2.731785,-2.937403,0.000000
+0.020000,4.693534,9.250000,4.693546,5.620427,2.673037,-2.937403,0.000000
+0.020000,4.806988,9.250000,4.807000,5.672712,2.614289,-2.937403,0.000000
+0.020000,4.921465,9.250000,4.921476,5.723823,2.555541,-2.937403,0.000000
+0.020000,5.036940,9.250000,5.036952,5.773759,2.496792,-2.937403,0.000000
+0.020000,5.153390,9.250000,5.153402,5.822520,2.438044,-2.937403,0.000000
+0.020000,5.270792,9.250000,5.270804,5.870106,2.379296,-2.937403,0.000000
+0.020000,5.389123,9.250000,5.389135,5.916517,2.320548,-2.937403,0.000000
+0.020000,5.508358,9.250000,5.508370,5.961753,2.261800,-2.937403,0.000000
+0.020000,5.628474,9.250000,5.628486,6.005814,2.203052,-2.937403,0.000000
+0.020000,5.749448,9.250000,5.749460,6.048700,2.144304,-2.937403,0.000000
+0.020000,5.871256,9.250000,5.871268,6.090411,2.085556,-2.937403,0.000000
+0.020000,5.993875,9.250000,5.993887,6.130947,2.026808,-2.937403,0.000000
+0.020000,6.117281,9.250000,6.117293,6.170309,1.968060,-2.937403,0.000000
+0.020000,6.241451,9.250000,6.241463,6.208495,1.909312,-2.937403,0.000000
+0.020000,6.366361,9.250000,6.366373,6.245506,1.850564,-2.937403,0.000000
+0.020000,6.491988,9.250000,6.492000,6.281342,1.791816,-2.937403,0.000000
+0.020000,6.618308,9.250000,6.618320,6.316004,1.733068,-2.937403,0.000000
+0.020000,6.745298,9.250000,6.745310,6.349490,1.674320,-2.937403,0.000000
+0.020000,6.872934,9.250000,6.872946,6.381802,1.615572,-2.937403,0.000000
+0.020000,7.001193,9.250000,7.001205,6.412938,1.556824,-2.937403,0.000000
+0.020000,7.130051,9.250000,7.130063,6.442900,1.498075,-2.937403,0.000000
+0.020000,7.259485,9.250000,7.259496,6.471686,1.439327,-2.937403,0.000000
+0.020000,7.389471,9.250000,7.389482,6.499298,1.380579,-2.937403,0.000000
+0.020000,7.519985,9.250000,7.519997,6.525734,1.321831,-2.937403,0.000000
+0.020000,7.651005,9.250000,7.651017,6.550996,1.263083,-2.937403,0.000000
+0.020000,7.782507,9.250000,7.782519,6.575083,1.204335,-2.937403,0.000000
+0.020000,7.914467,9.250000,7.914479,6.597994,1.145587,-2.937403,0.000000
+0.020000,8.046861,9.250000,8.046873,6.619731,1.086839,-2.937403,0.000000
+0.020000,8.179667,9.250000,8.179679,6.640293,1.028091,-2.937403,0.000000
+0.020000,8.312861,9.250000,8.312873,6.659680,0.969343,-2.937403,0.000000
+0.020000,8.446419,9.250000,8.446430,6.677892,0.910595,-2.937403,0.000000
+0.020000,8.580317,9.250000,8.580329,6.694929,0.851847,-2.937403,0.000000
+0.020000,8.714533,9.250000,8.714545,6.710791,0.793099,-2.937403,0.000000
+0.020000,8.849043,9.250000,8.849054,6.725478,0.734351,-2.937403,0.000000
+0.020000,8.983822,9.250000,8.983834,6.738990,0.675603,-2.937403,0.000000
+0.020000,9.118849,9.250000,9.118861,6.751327,0.616855,-2.937403,0.000000
+0.020000,9.254099,9.250000,9.254111,6.762489,0.558107,-2.937403,0.000000
+0.020000,9.389548,9.250000,9.389560,6.772476,0.499358,-2.937403,0.000000
+0.020000,9.525174,9.250000,9.525186,6.781288,0.440610,-2.937403,0.000000
+0.020000,9.660953,9.250000,9.660964,6.788926,0.381862,-2.937403,0.000000
+0.020000,9.796860,9.250000,9.796872,6.795388,0.323114,-2.937403,0.000000
+0.020000,9.932874,9.250000,9.932886,6.800675,0.264366,-2.937403,0.000000
+0.020000,10.072735,9.249548,10.072748,6.993106,9.621550,467.859174,6.270750
+0.020000,10.246461,9.244779,10.246539,8.689580,84.823710,3760.108006,6.240635
+0.020000,10.420806,9.234674,10.421176,8.731849,2.113430,-4135.514003,6.209867
+0.020000,10.595418,9.219102,10.596482,8.765266,1.670858,-22.128572,6.178552
+0.020000,10.769929,9.197975,10.772266,8.789238,1.198594,-23.613207,6.146815
+0.020000,10.943965,9.171250,10.948343,8.803829,0.729525,-23.453476,6.114787
+0.020000,11.117140,9.138936,11.124507,8.808207,0.218921,-25.530191,6.082612
+0.020000,11.289065,9.101092,11.300548,8.802035,-0.308596,-26.375834,6.050441
+0.020000,11.459357,9.057834,11.476248,8.784992,-0.852131,-27.176743,6.018429
+0.020000,11.627649,9.009326,11.651392,8.757202,-1.389545,-26.870721,5.986728
+0.020000,11.793602,8.955781,11.825769,8.718885,-1.915833,-26.314409,5.955490
+0.020000,11.956907,8.897452,11.999178,8.670455,-2.421497,-25.283186,5.924856
+0.020000,12.117295,8.834629,12.171431,8.612638,-2.890836,-23.466952,5.894958
+0.020000,12.274540,8.767626,12.342356,8.546261,-3.318864,-21.401377,5.865914
+0.020000,12.428463,8.696777,12.511802,8.472262,-3.699957,-19.054690,5.837828
+0.020000,12.578928,8.622427,12.679635,8.391643,-4.030953,-16.549803,5.810787
+0.020000,12.725847,8.544922,12.845743,8.305423,-4.310976,-14.001151,5.784861
+0.020000,12.869169,8.464607,13.010035,8.214600,-4.541135,-11.507946,5.760105
+0.020000,13.008885,8.381817,13.172437,8.120118,-4.724137,-9.150096,5.736559
+0.020000,13.145014,8.296875,13.332894,8.022840,-4.863870,-6.986658,5.714250
+0.020000,13.277608,8.210089,13.491365,7.923540,-4.964999,-5.056420,5.693190
+0.020000,13.406740,8.121749,13.647823,7.822888,-5.032599,-3.379991,5.673382
+0.020000,13.532501,8.032125,13.802251,7.721423,-5.073292,-2.034682,5.654820
+0.020000,13.654997,7.941470,13.954644,7.619647,-5.088779,-0.774353,5.637492
+0.020000,13.774345,7.850017,14.105002,7.517910,-5.086841,0.096942,5.621376
+0.020000,13.890669,7.757982,14.253332,7.416483,-5.071363,0.773867,5.606450
+0.020000,14.004096,7.665562,14.399644,7.315593,-5.044512,1.342571,5.592686
+0.020000,14.114757,7.572940,14.543951,7.215384,-5.010439,1.703653,5.580054
+0.020000,14.222781,7.480283,14.686270,7.115922,-4.973104,1.866752,5.568524
+0.020000,14.328296,7.387745,14.826615,7.017253,-4.933427,1.983857,5.558064
+0.020000,14.431428,7.295467,14.965003,6.919402,-4.892564,2.043151,5.548641
+0.020000,14.532297,7.203582,15.101450,6.822329,-4.853627,1.946823,5.540226
+0.020000,14.631021,7.112208,15.235969,6.725975,-4.817702,1.796260,5.532787
+0.020000,14.727711,7.021459,15.368575,6.630300,-4.783759,1.697159,5.526295
+0.020000,14.822473,6.931438,15.499280,6.535221,-4.753952,1.490355,5.520722
+0.020000,14.915408,6.842243,15.628092,6.440637,-4.729221,1.236550,5.516040
+0.020000,15.006610,6.753963,15.755022,6.346477,-4.708004,1.060820,5.512224
+0.020000,15.096168,6.666684,15.880075,6.252640,-4.691856,0.807404,5.509251
+0.020000,15.184164,6.580485,16.003256,6.159046,-4.679692,0.608192,5.507097
+0.020000,15.270676,6.495443,16.124567,6.065597,-4.672425,0.363344,5.505742
+0.020000,15.355776,6.411628,16.244012,5.972216,-4.669044,0.169057,5.505165
+0.020000,15.439529,6.329107,16.361588,5.878826,-4.669519,-0.023724,5.505348
+0.020000,15.521995,6.247943,16.477295,5.785352,-4.673703,-0.209212,5.506273
+0.020000,15.603229,6.168198,16.591130,5.691730,-4.681105,-0.370089,5.507924
+0.020000,15.683281,6.089928,16.703088,5.597909,-4.691045,-0.497005,5.510285
+0.020000,15.762196,6.013186,16.813165,5.503831,-4.703890,-0.642258,5.513343
+0.020000,15.840014,5.938025,16.921354,5.409456,-4.718738,-0.742390,5.517081
+0.020000,15.916771,5.864491,17.027649,5.314780,-4.733823,-0.754237,5.521487
+0.020000,15.992495,5.792629,17.132044,5.219739,-4.752051,-0.911413,5.526548
+0.020000,16.067215,5.722482,17.234532,5.124371,-4.768410,-0.817945,5.532250
+0.020000,16.140951,5.654087,17.335104,5.028641,-4.786480,-0.903515,5.538580
+0.020000,16.213723,5.587481,17.433756,4.932573,-4.803384,-0.845219,5.545525
+0.020000,16.285543,5.522694,17.530480,4.836193,-4.818996,-0.780577,5.553070
+0.020000,16.356424,5.459757,17.625270,4.739533,-4.833040,-0.702225,5.561202
+0.020000,16.426372,5.398692,17.718123,4.642632,-4.845023,-0.599126,5.569906
+0.020000,16.495392,5.339522,17.809034,4.545543,-4.854447,-0.471203,5.579165
+0.020000,16.563484,5.282264,17.898001,4.448337,-4.860298,-0.292569,5.588963
+0.020000,16.630648,5.226930,17.985023,4.351095,-4.862124,-0.091301,5.599282
+0.020000,16.696879,5.173530,18.070100,4.253865,-4.861497,0.031375,5.610103
+0.020000,16.762172,5.122067,18.153236,4.156780,-4.854261,0.361815,5.621404
+0.020000,16.826518,5.072542,18.234434,4.059910,-4.843498,0.538117,5.633163
+0.020000,16.889909,5.024951,18.313701,3.963357,-4.827619,0.793986,5.645357
+0.020000,16.952333,4.979285,18.391046,3.867236,-4.806084,1.076751,5.657959
+0.020000,17.013779,4.935530,18.466479,3.771648,-4.779375,1.335428,5.670943
+0.020000,17.074235,4.893669,18.540013,3.676699,-4.747450,1.596271,5.684279
+0.020000,17.133687,4.853678,18.611663,3.582512,-4.709342,1.905386,5.697937
+0.020000,17.192121,4.815533,18.681446,3.489151,-4.668053,2.064449,5.711884
+0.020000,17.249526,4.779203,18.749381,3.396748,-4.620145,2.395384,5.726086
+0.020000,17.305886,4.744653,18.815488,3.305361,-4.569378,2.538337,5.740508
+0.020000,17.361189,4.711846,18.879790,3.215078,-4.514153,2.761253,5.755115
+0.020000,17.415421,4.680741,18.942309,3.125958,-4.455967,2.909325,5.769867
+0.020000,17.468575,4.651293,19.003075,3.038305,-4.382678,3.664440,5.784729
+0.020000,17.520649,4.623448,19.062126,2.952585,-4.285984,4.834728,5.799665
+0.020000,17.571649,4.597151,19.119506,2.868992,-4.179679,5.315238,5.814642
+0.020000,17.621576,4.572347,19.175256,2.787491,-4.075037,5.232109,5.829625
+0.020000,17.670437,4.548978,19.229417,2.708060,-3.971531,5.175270,5.844582
+0.020000,17.718234,4.526989,19.282030,2.630645,-3.870753,5.038899,5.859481
+0.020000,17.764973,4.506322,19.333134,2.555189,-3.772787,4.898310,5.874290
+0.020000,17.810656,4.486923,19.382766,2.481593,-3.679844,4.647165,5.888979
+0.020000,17.855289,4.468735,19.430963,2.409836,-3.587837,4.600335,5.903519
+0.020000,17.898875,4.451704,19.477758,2.339752,-3.504193,4.182201,5.917883
+0.020000,17.941417,4.435776,19.523184,2.271302,-3.422475,4.085890,5.932046
+0.020000,17.982919,4.420900,19.567271,2.204367,-3.346796,3.783953,5.945982
+0.020000,18.023382,4.407023,19.610047,2.138824,-3.277130,3.483282,5.959670
+0.020000,18.062808,4.394097,19.651539,2.074564,-3.213001,3.206450,5.973088
+0.020000,18.101200,4.382072,19.691769,2.011521,-3.152150,3.042577,5.986219
+0.020000,18.138561,4.370900,19.730765,1.949767,-3.087708,3.222103,5.999046
+0.020000,18.174903,4.360533,19.768557,1.889606,-3.008041,3.983362,6.011558
+0.020000,18.210242,4.350922,19.805180,1.831156,-2.922472,4.278416,6.023747
+0.020000,18.244593,4.342022,19.840665,1.774240,-2.845816,3.832808,6.035607
+0.020000,18.277970,4.333790,19.875042,1.718857,-2.769141,3.833754,6.047132
+0.020000,18.310386,4.326184,19.908339,1.664841,-2.700798,3.417166,6.058320
+0.020000,18.341856,4.319165,19.940582,1.612157,-2.634196,3.330095,6.069168
+0.020000,18.372391,4.312696,19.971795,1.560643,-2.575728,2.923376,6.079676
+0.020000,18.402005,4.306742,20.002001,1.510332,-2.515540,3.009411,6.089843
+0.020000,18.430710,4.301268,20.031223,1.461095,-2.461833,2.685354,6.099671
+0.020000,18.458518,4.296243,20.059481,1.412892,-2.410173,2.582977,6.109162
+0.020000,18.485441,4.291637,20.086796,1.365744,-2.357396,2.638873,6.118320
+0.020000,18.511491,4.287421,20.113185,1.319448,-2.314782,2.130684,6.127146
+0.020000,18.536681,4.283568,20.138668,1.274153,-2.264780,2.500099,6.135647
+0.020000,18.561023,4.280052,20.163262,1.229693,-2.222993,2.089350,6.143827
+0.020000,18.584526,4.276850,20.186983,1.186043,-2.182505,2.024400,6.151690
+0.020000,18.607205,4.273937,20.209848,1.143248,-2.139760,2.137257,6.159243
+0.020000,18.629070,4.271294,20.231872,1.101226,-2.101055,1.935248,6.166492
+0.020000,18.650135,4.268899,20.253073,1.060034,-2.059621,2.071696,6.173442
+0.020000,18.670413,4.266733,20.273466,1.019660,-2.018701,2.046040,6.180101
+0.020000,18.689913,4.264778,20.293063,0.979878,-1.989082,1.480950,6.186475
+0.020000,18.708651,4.263018,20.311884,0.941029,-1.942461,2.331028,6.192570
+0.020000,18.726639,4.261437,20.329941,0.902860,-1.908482,1.698965,6.198394
+0.020000,18.743888,4.260019,20.347249,0.865377,-1.874145,1.716815,6.203953
+0.020000,18.760414,4.258751,20.363823,0.828706,-1.833510,2.031768,6.209255
+0.020000,18.776229,4.257620,20.379678,0.792771,-1.796771,1.836972,6.214305
+0.020000,18.791347,4.256613,20.394830,0.757584,-1.759334,1.871838,6.219112
+0.020000,18.805781,4.255720,20.409291,0.723072,-1.725627,1.685333,6.223682
+0.020000,18.819545,4.254930,20.423078,0.689351,-1.686058,1.978467,6.228022
+0.020000,18.832656,4.254233,20.436208,0.656451,-1.644965,2.054662,6.232139
+0.020000,18.845124,4.253621,20.448691,0.624157,-1.614733,1.511597,6.236039
+0.020000,18.856968,4.253084,20.460546,0.592790,-1.568322,2.320542,6.239730
+0.020000,18.868198,4.252615,20.471786,0.561991,-1.539979,1.417156,6.243216
+0.020000,18.878832,4.252207,20.482429,0.532122,-1.493453,2.326302,6.246507
+0.020000,18.888886,4.251854,20.492489,0.502992,-1.456465,1.849403,6.249607
+0.020000,18.898373,4.251549,20.501981,0.474608,-1.419238,1.861338,6.252524
+0.020000,18.907309,4.251287,20.510920,0.446972,-1.381783,1.872733,6.255262
+0.020000,18.915709,4.251064,20.519324,0.420182,-1.339518,2.113234,6.257829
+0.020000,18.923591,4.250873,20.527207,0.394184,-1.299874,1.982234,6.260231
+0.020000,18.930968,4.250712,20.534586,0.368931,-1.262653,1.861058,6.262474
+0.020000,18.937856,4.250577,20.541476,0.344508,-1.221167,2.074297,6.264563
+0.020000,18.944274,4.250463,20.547895,0.320922,-1.179262,2.095227,6.266505
+0.020000,18.950237,4.250369,20.553858,0.298174,-1.137438,2.091200,6.268305
+0.020000,18.955759,4.250292,20.559381,0.276166,-1.100374,1.853192,6.269969
+0.020000,18.960859,4.250228,20.564481,0.254984,-1.059128,2.062322,6.271503
+0.020000,18.965553,4.250177,20.569176,0.234747,-1.011851,2.363837,6.272913
+0.020000,18.969855,4.250135,20.573478,0.215114,-0.981616,1.511748,6.274203
+0.020000,18.973787,4.250102,20.577410,0.196567,-0.927343,2.713659,6.275379
+0.020000,18.977359,4.250076,20.580982,0.178623,-0.897198,1.507233,6.276447
+0.020000,18.980593,4.250056,20.584216,0.161688,-0.846778,2.521013,6.277413
+0.020000,18.983504,4.250040,20.587127,0.145557,-0.806547,2.011575,6.278281
+0.020000,18.986107,4.250029,20.589730,0.130156,-0.770053,1.824692,6.279057
+0.020000,18.988422,4.250020,20.592045,0.115739,-0.720870,2.459134,6.279746
+0.020000,18.990465,4.250013,20.594088,0.102152,-0.679320,2.077523,6.280354
+0.020000,18.992251,4.250009,20.595875,0.089343,-0.640475,1.942217,6.280885
+0.020000,18.993799,4.250006,20.597423,0.077382,-0.598044,2.121564,6.281345
+0.020000,18.995126,4.250004,20.598749,0.066330,-0.552576,2.273377,6.281739
+0.020000,18.996248,4.250002,20.599872,0.056124,-0.510295,2.114055,6.282073
+0.020000,18.997184,4.250001,20.600807,0.046767,-0.467879,2.120830,6.282350
+0.020000,18.997949,4.250001,20.601572,0.038260,-0.425348,2.126539,6.282577
+0.020000,18.998561,4.250000,20.602184,0.030605,-0.382723,2.131267,6.282759
+0.020000,18.999037,4.250000,20.602660,0.023805,-0.340021,2.135104,6.282900
+0.020000,18.999394,4.250000,20.603018,0.017860,-0.297258,2.138139,6.283006
+0.020000,18.999650,4.250000,20.603273,0.012771,-0.254448,2.140464,6.283081
+0.020000,18.999820,4.250000,20.603444,0.008539,-0.211605,2.142175,6.283132
+0.020000,18.999924,4.250000,20.603547,0.005164,-0.168738,2.143364,6.283163
+0.020000,18.999977,4.250000,20.603600,0.002647,-0.125855,2.144127,6.283178
+0.020000,18.999996,4.250000,20.603620,0.000988,-0.082964,2.144560,6.283184
+0.020000,19.000000,4.250000,20.603623,0.000186,-0.040069,2.144759,6.283185
+0.020000,19.000000,4.250000,20.603623,0.000000,-0.009310,1.537922,6.283185
diff --git a/src/main/deploy/output/trenchAndAim.pf1.csv b/src/main/deploy/output/trenchAndAim.pf1.csv
new file mode 100644
index 0000000..20a014f
--- /dev/null
+++ b/src/main/deploy/output/trenchAndAim.pf1.csv
@@ -0,0 +1,306 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,8.000000,0.000012,0.001175,0.058748,2.937403,0.000000
+0.020000,0.000000,8.000000,0.000059,0.003525,0.117496,2.937403,0.000000
+0.020000,0.000000,8.000000,0.000164,0.007050,0.176244,2.937403,0.000000
+0.020000,0.000000,8.000000,0.000352,0.011750,0.234992,2.937403,0.000000
+0.020000,0.000000,8.000000,0.000646,0.017624,0.293740,2.937403,0.000000
+0.020000,0.000000,8.000000,0.001069,0.024674,0.352488,2.937403,0.000000
+0.020000,0.000000,8.000000,0.001645,0.032899,0.411236,2.937403,0.000000
+0.020000,0.000000,8.000000,0.002397,0.042299,0.469984,2.937403,0.000000
+0.020000,0.000000,8.000000,0.003349,0.052873,0.528733,2.937403,0.000000
+0.020000,0.000000,8.000000,0.004524,0.064623,0.587481,2.937403,0.000000
+0.020000,0.000000,8.000000,0.005945,0.077547,0.646229,2.937403,0.000000
+0.020000,0.000000,8.000000,0.007637,0.091647,0.704977,2.937403,0.000000
+0.020000,0.000000,8.000000,0.009623,0.106921,0.763725,2.937403,0.000000
+0.020000,0.001926,8.000000,0.011926,0.123371,0.822473,2.937403,0.000000
+0.020000,0.004570,8.000000,0.014570,0.140995,0.881221,2.937403,0.000000
+0.020000,0.007577,8.000000,0.017577,0.159795,0.939969,2.937403,0.000000
+0.020000,0.010973,8.000000,0.020973,0.179769,0.998717,2.937403,0.000000
+0.020000,0.014780,8.000000,0.024780,0.200918,1.057465,2.937403,0.000000
+0.020000,0.019022,8.000000,0.029022,0.223243,1.116213,2.937403,0.000000
+0.020000,0.023721,8.000000,0.033721,0.246742,1.174961,2.937403,0.000000
+0.020000,0.028903,8.000000,0.038903,0.271416,1.233709,2.937403,0.000000
+0.020000,0.034590,8.000000,0.044590,0.297265,1.292457,2.937403,0.000000
+0.020000,0.040805,8.000000,0.050805,0.324289,1.351205,2.937403,0.000000
+0.020000,0.047573,8.000000,0.057573,0.352488,1.409953,2.937403,0.000000
+0.020000,0.054917,8.000000,0.064917,0.381862,1.468701,2.937403,0.000000
+0.020000,0.062859,8.000000,0.072859,0.412411,1.527450,2.937403,0.000000
+0.020000,0.071425,8.000000,0.081425,0.444135,1.586198,2.937403,0.000000
+0.020000,0.080637,8.000000,0.090637,0.477034,1.644946,2.937403,0.000000
+0.020000,0.090518,8.000000,0.100518,0.511108,1.703694,2.937403,0.000000
+0.020000,0.101093,8.000000,0.111093,0.546357,1.762442,2.937403,0.000000
+0.020000,0.112384,8.000000,0.122384,0.582781,1.821190,2.937403,0.000000
+0.020000,0.124416,8.000000,0.134416,0.620379,1.879938,2.937403,0.000000
+0.020000,0.137211,8.000000,0.147211,0.659153,1.938686,2.937403,0.000000
+0.020000,0.150793,8.000000,0.160793,0.699102,1.997434,2.937403,0.000000
+0.020000,0.165187,8.000000,0.175187,0.740226,2.056182,2.937403,0.000000
+0.020000,0.180414,8.000000,0.190414,0.782524,2.114930,2.937403,0.000000
+0.020000,0.196499,8.000000,0.206499,0.825998,2.173678,2.937403,0.000000
+0.020000,0.213466,8.000000,0.223466,0.870646,2.232426,2.937403,0.000000
+0.020000,0.231337,8.000000,0.241337,0.916470,2.291174,2.937403,0.000000
+0.020000,0.250136,8.000000,0.260136,0.963468,2.349922,2.937403,0.000000
+0.020000,0.269887,8.000000,0.279887,1.011642,2.408670,2.937403,0.000000
+0.020000,0.290614,8.000000,0.300614,1.060990,2.467418,2.937403,0.000000
+0.020000,0.312339,8.000000,0.322339,1.111513,2.526166,2.937403,0.000000
+0.020000,0.335086,8.000000,0.345086,1.163212,2.584915,2.937403,0.000000
+0.020000,0.358879,8.000000,0.368879,1.216085,2.643663,2.937403,0.000000
+0.020000,0.383741,8.000000,0.393741,1.270133,2.702411,2.937403,0.000000
+0.020000,0.409696,8.000000,0.419696,1.325356,2.761159,2.937403,0.000000
+0.020000,0.436767,8.000000,0.446767,1.381754,2.819907,2.937403,0.000000
+0.020000,0.464978,8.000000,0.474978,1.439327,2.878655,2.937403,0.000000
+0.020000,0.494352,8.000000,0.504352,1.498075,2.937403,2.937403,0.000000
+0.020000,0.524913,8.000000,0.534913,1.557998,2.996151,2.937403,0.000000
+0.020000,0.556684,8.000000,0.566684,1.619096,3.054899,2.937403,0.000000
+0.020000,0.589688,8.000000,0.599688,1.681369,3.113647,2.937403,0.000000
+0.020000,0.623950,8.000000,0.633950,1.744817,3.172395,2.937403,0.000000
+0.020000,0.659493,8.000000,0.669493,1.809440,3.231143,2.937403,0.000000
+0.020000,0.696340,8.000000,0.706340,1.875238,3.289891,2.937403,0.000000
+0.020000,0.734514,8.000000,0.744514,1.942211,3.348639,2.937403,0.000000
+0.020000,0.774040,8.000000,0.784040,2.010359,3.407387,2.937403,0.000000
+0.020000,0.814940,8.000000,0.824940,2.079681,3.466135,2.937403,0.000000
+0.020000,0.857239,8.000000,0.867239,2.150179,3.524883,2.937403,0.000000
+0.020000,0.900959,8.000000,0.910959,2.221852,3.583632,2.937403,0.000000
+0.020000,0.946125,8.000000,0.956125,2.294699,3.642380,2.937403,0.000000
+0.020000,0.992759,8.000000,1.002759,2.368722,3.701128,2.937403,0.000000
+0.020000,1.040885,8.000000,1.050885,2.443919,3.759876,2.937403,0.000000
+0.020000,1.090527,8.000000,1.100527,2.520292,3.818624,2.937403,0.000000
+0.020000,1.141709,8.000000,1.151709,2.597839,3.877372,2.937403,0.000000
+0.020000,1.194453,8.000000,1.204453,2.676562,3.936120,2.937403,0.000000
+0.020000,1.248783,8.000000,1.258783,2.756459,3.994868,2.937403,0.000000
+0.020000,1.304723,8.000000,1.314723,2.837531,4.053616,2.937403,0.000000
+0.020000,1.362284,8.000000,1.372284,2.918604,4.053616,-0.000000,0.000000
+0.020000,1.421467,8.000000,1.431467,2.999676,4.053616,0.000000,0.000000
+0.020000,1.482271,8.000000,1.492271,3.080748,4.053616,0.000000,0.000000
+0.020000,1.544697,8.000000,1.554697,3.161820,4.053616,0.000000,0.000000
+0.020000,1.608744,8.000000,1.618744,3.242893,4.053616,0.000000,0.000000
+0.020000,1.674413,8.000000,1.684413,3.323965,4.053616,0.000000,0.000000
+0.020000,1.741703,8.000000,1.751703,3.405037,4.053616,0.000000,0.000000
+0.020000,1.810614,8.000000,1.820614,3.486110,4.053616,-0.000000,0.000000
+0.020000,1.881147,8.000000,1.891147,3.567182,4.053616,0.000000,0.000000
+0.020000,1.953301,8.000000,1.963301,3.648254,4.053616,-0.000000,0.000000
+0.020000,2.027077,8.000000,2.037077,3.729327,4.053616,0.000000,0.000000
+0.020000,2.102474,8.000000,2.112474,3.810399,4.053616,-0.000000,0.000000
+0.020000,2.179493,8.000000,2.189493,3.891471,4.053616,0.000000,0.000000
+0.020000,2.258133,8.000000,2.268133,3.972544,4.053616,0.000000,0.000000
+0.020000,2.338395,8.000000,2.348395,4.053616,4.053616,-0.000000,0.000000
+0.020000,2.420266,8.000000,2.430266,4.133513,3.994868,-2.937403,0.000000
+0.020000,2.503724,8.000000,2.513724,4.212236,3.936120,-2.937403,0.000000
+0.020000,2.588744,8.000000,2.598744,4.289783,3.877372,-2.937403,0.000000
+0.020000,2.675303,8.000000,2.685303,4.366156,3.818624,-2.937403,0.000000
+0.020000,2.763378,8.000000,2.773378,4.441353,3.759876,-2.937403,0.000000
+0.020000,2.852946,8.000000,2.862946,4.515376,3.701128,-2.937403,0.000000
+0.020000,2.943982,8.000000,2.953982,4.588223,3.642380,-2.937403,0.000000
+0.020000,3.036463,8.000000,3.046463,4.659896,3.583632,-2.937403,0.000000
+0.020000,3.130366,8.000000,3.140366,4.730394,3.524883,-2.937403,0.000000
+0.020000,3.225667,8.000000,3.235667,4.799716,3.466135,-2.937403,0.000000
+0.020000,3.322343,8.000000,3.332343,4.867864,3.407387,-2.937403,0.000000
+0.020000,3.420370,8.000000,3.430370,4.934837,3.348639,-2.937403,0.000000
+0.020000,3.519724,8.000000,3.529724,5.000635,3.289891,-2.937403,0.000000
+0.020000,3.620383,8.000000,3.630383,5.065258,3.231143,-2.937403,0.000000
+0.020000,3.722323,8.000000,3.732323,5.128705,3.172395,-2.937403,0.000000
+0.020000,3.825520,8.000000,3.835520,5.190978,3.113647,-2.937403,0.000000
+0.020000,3.929950,8.000000,3.939950,5.252076,3.054899,-2.937403,0.000000
+0.020000,4.035591,8.000000,4.045591,5.311999,2.996151,-2.937403,0.000000
+0.020000,4.142418,8.000000,4.152418,5.370747,2.937403,-2.937403,0.000000
+0.020000,4.250409,8.000000,4.260409,5.428321,2.878655,-2.937403,0.000000
+0.020000,4.359540,8.000000,4.369540,5.484719,2.819907,-2.937403,0.000000
+0.020000,4.469786,8.000000,4.479786,5.539942,2.761159,-2.937403,0.000000
+0.020000,4.581125,8.000000,4.591125,5.593990,2.702411,-2.937403,0.000000
+0.020000,4.693534,8.000000,4.703534,5.646863,2.643663,-2.937403,0.000000
+0.020000,4.806988,8.000000,4.816988,5.698562,2.584915,-2.937403,0.000000
+0.020000,4.921465,8.000000,4.931465,5.749085,2.526166,-2.937403,0.000000
+0.020000,5.036940,8.000000,5.046940,5.798433,2.467418,-2.937403,0.000000
+0.020000,5.153390,8.000000,5.163390,5.846607,2.408670,-2.937403,0.000000
+0.020000,5.270792,8.000000,5.280792,5.893605,2.349922,-2.937403,0.000000
+0.020000,5.389123,8.000000,5.399123,5.939429,2.291174,-2.937403,0.000000
+0.020000,5.508358,8.000000,5.518358,5.984077,2.232426,-2.937403,0.000000
+0.020000,5.628474,8.000000,5.638474,6.027551,2.173678,-2.937403,0.000000
+0.020000,5.749448,8.000000,5.759448,6.069849,2.114930,-2.937403,0.000000
+0.020000,5.871256,8.000000,5.881256,6.110973,2.056182,-2.937403,0.000000
+0.020000,5.993875,8.000000,6.003875,6.150922,1.997434,-2.937403,0.000000
+0.020000,6.117281,8.000000,6.127281,6.189695,1.938686,-2.937403,0.000000
+0.020000,6.241451,8.000000,6.251451,6.227294,1.879938,-2.937403,0.000000
+0.020000,6.366361,8.000000,6.376361,6.263718,1.821190,-2.937403,0.000000
+0.020000,6.491988,8.000000,6.501988,6.298967,1.762442,-2.937403,0.000000
+0.020000,6.618308,8.000000,6.628308,6.333041,1.703694,-2.937403,0.000000
+0.020000,6.745298,8.000000,6.755298,6.365940,1.644946,-2.937403,0.000000
+0.020000,6.872934,8.000000,6.882934,6.397664,1.586198,-2.937403,0.000000
+0.020000,7.001193,8.000000,7.011193,6.428212,1.527450,-2.937403,0.000000
+0.020000,7.130051,8.000000,7.140051,6.457587,1.468701,-2.937403,0.000000
+0.020000,7.259485,8.000000,7.269485,6.485786,1.409953,-2.937403,0.000000
+0.020000,7.389471,8.000000,7.399471,6.512810,1.351205,-2.937403,0.000000
+0.020000,7.519985,8.000000,7.529985,6.538659,1.292457,-2.937403,0.000000
+0.020000,7.651005,8.000000,7.661005,6.563333,1.233709,-2.937403,0.000000
+0.020000,7.782507,8.000000,7.792507,6.586832,1.174961,-2.937403,0.000000
+0.020000,7.914467,8.000000,7.924467,6.609157,1.116213,-2.937403,0.000000
+0.020000,8.046861,8.000000,8.056861,6.630306,1.057465,-2.937403,0.000000
+0.020000,8.179667,8.000000,8.189667,6.650280,0.998717,-2.937403,0.000000
+0.020000,8.312861,8.000000,8.322861,6.669080,0.939969,-2.937403,0.000000
+0.020000,8.446419,8.000000,8.456419,6.686704,0.881221,-2.937403,0.000000
+0.020000,8.580317,8.000000,8.590317,6.703153,0.822473,-2.937403,0.000000
+0.020000,8.714533,8.000000,8.724533,6.718428,0.763725,-2.937403,0.000000
+0.020000,8.849043,8.000000,8.859043,6.732527,0.704977,-2.937403,0.000000
+0.020000,8.983822,8.000000,8.993822,6.745452,0.646229,-2.937403,0.000000
+0.020000,9.118849,8.000000,9.128849,6.757202,0.587481,-2.937403,0.000000
+0.020000,9.254099,8.000000,9.264099,6.767776,0.528733,-2.937403,0.000000
+0.020000,9.389548,8.000000,9.399548,6.777176,0.469984,-2.937403,0.000000
+0.020000,9.525174,8.000000,9.535174,6.785401,0.411236,-2.937403,0.000000
+0.020000,9.660953,8.000000,9.670953,6.792450,0.352488,-2.937403,0.000000
+0.020000,9.796860,8.000000,9.806860,6.798325,0.293740,-2.937403,0.000000
+0.020000,9.932874,8.000000,9.942874,6.803025,0.234992,-2.937403,0.000000
+0.020000,10.057191,7.999645,10.078970,6.806550,0.176244,-2.937403,6.270750
+0.020000,10.193290,7.995910,10.215124,6.808900,0.117496,-2.937403,6.240635
+0.020000,10.329240,7.988032,10.351307,6.809410,0.025501,-4.599732,6.209867
+0.020000,10.464865,7.975938,10.487477,6.807570,-0.091995,-5.874806,6.178552
+0.020000,10.599993,7.959580,10.623598,6.804555,-0.150743,-2.937403,6.146815
+0.020000,10.734461,7.938932,10.759647,6.800365,-0.209491,-2.937403,6.114787
+0.020000,10.868102,7.913995,10.895601,6.795001,-0.268239,-2.937403,6.082612
+0.020000,11.000755,7.884796,11.031436,6.788461,-0.326987,-2.937403,6.050441
+0.020000,11.132264,7.851389,11.167128,6.780746,-0.385735,-2.937403,6.018429
+0.020000,11.262482,7.813854,11.302654,6.771857,-0.444483,-2.937403,5.986728
+0.020000,11.391275,7.772297,11.437990,6.761792,-0.503231,-2.937403,5.955490
+0.020000,11.518519,7.726847,11.573114,6.750552,-0.561979,-2.937403,5.924856
+0.020000,11.644110,7.677652,11.708001,6.738138,-0.620727,-2.937403,5.894958
+0.020000,11.767956,7.624878,11.842627,6.724548,-0.679475,-2.937403,5.865914
+0.020000,11.889988,7.568706,11.976971,6.709784,-0.738223,-2.937403,5.837828
+0.020000,12.010149,7.509328,12.111007,6.693844,-0.796971,-2.937403,5.810787
+0.020000,12.128404,7.446941,12.244713,6.676730,-0.855719,-2.937403,5.784861
+0.020000,12.244731,7.381751,12.378065,6.658441,-0.914467,-2.937403,5.760105
+0.020000,12.359125,7.313963,12.511039,6.638976,-0.973216,-2.937403,5.736559
+0.020000,12.471595,7.243782,12.643612,6.618337,-1.031964,-2.937403,5.714250
+0.020000,12.582162,7.171410,12.775760,6.596523,-1.090712,-2.937403,5.693190
+0.020000,12.690857,7.097048,12.907461,6.573534,-1.149460,-2.937403,5.673382
+0.020000,12.797722,7.020888,13.038690,6.549369,-1.208208,-2.937403,5.654820
+0.020000,12.902806,6.943117,13.169424,6.524030,-1.266956,-2.937403,5.637492
+0.020000,13.006164,6.863915,13.299639,6.497516,-1.325704,-2.937403,5.621376
+0.020000,13.107855,6.783455,13.429313,6.469827,-1.384452,-2.937403,5.606450
+0.020000,13.207943,6.701902,13.558421,6.440963,-1.443200,-2.937403,5.592686
+0.020000,13.306495,6.619413,13.686940,6.410924,-1.501948,-2.937403,5.580054
+0.020000,13.403579,6.536138,13.814846,6.379710,-1.560696,-2.937403,5.568524
+0.020000,13.499263,6.452221,13.942116,6.347321,-1.619444,-2.937403,5.558064
+0.020000,13.593617,6.367797,14.068727,6.313758,-1.678192,-2.937403,5.548641
+0.020000,13.686710,6.282994,14.194655,6.279019,-1.736940,-2.937403,5.540226
+0.020000,13.778609,6.197937,14.319876,6.243105,-1.795688,-2.937403,5.532787
+0.020000,13.869381,6.112741,14.444367,6.206016,-1.854436,-2.937403,5.526295
+0.020000,13.959092,6.027518,14.568105,6.167753,-1.913184,-2.937403,5.520722
+0.020000,14.047804,5.942374,14.691066,6.128314,-1.971932,-2.937403,5.516040
+0.020000,14.135579,5.857411,14.813226,6.087700,-2.030681,-2.937403,5.512224
+0.020000,14.222475,5.772726,14.934562,6.045912,-2.089429,-2.937403,5.509251
+0.020000,14.308548,5.688411,15.055051,6.002948,-2.148177,-2.937403,5.507097
+0.020000,14.393852,5.604557,15.174668,5.958810,-2.206925,-2.937403,5.505742
+0.020000,14.478437,5.521247,15.293391,5.913496,-2.265673,-2.937403,5.505165
+0.020000,14.562353,5.438566,15.411196,5.867008,-2.324421,-2.937403,5.505348
+0.020000,14.645643,5.356591,15.528060,5.819345,-2.383169,-2.937403,5.506273
+0.020000,14.728350,5.275400,15.643958,5.770506,-2.441917,-2.937403,5.507924
+0.020000,14.810514,5.195066,15.758868,5.720493,-2.500665,-2.937403,5.510285
+0.020000,14.892169,5.115661,15.872766,5.669305,-2.559413,-2.937403,5.513343
+0.020000,14.973348,5.037253,15.985629,5.616941,-2.618161,-2.937403,5.517081
+0.020000,15.054082,4.959909,16.097432,5.563403,-2.676909,-2.937403,5.521487
+0.020000,15.134395,4.883693,16.208153,5.508690,-2.735657,-2.937403,5.526548
+0.020000,15.214311,4.808668,16.317768,5.452802,-2.794405,-2.937403,5.532250
+0.020000,15.293849,4.734893,16.426253,5.395739,-2.853153,-2.937403,5.538580
+0.020000,15.373024,4.662426,16.533586,5.337501,-2.911901,-2.937403,5.545525
+0.020000,15.451849,4.591322,16.639742,5.278088,-2.970649,-2.937403,5.553070
+0.020000,15.530331,4.521636,16.744698,5.217500,-3.029398,-2.937403,5.561202
+0.020000,15.608476,4.453417,16.848430,5.155737,-3.088146,-2.937403,5.569906
+0.020000,15.686283,4.386715,16.950915,5.092799,-3.146894,-2.937403,5.579165
+0.020000,15.763750,4.321574,17.052130,5.028686,-3.205642,-2.937403,5.588963
+0.020000,15.840869,4.258039,17.152051,4.963399,-3.264390,-2.937403,5.599282
+0.020000,15.917630,4.196150,17.250654,4.896936,-3.323138,-2.937403,5.610103
+0.020000,15.994018,4.135944,17.347917,4.829298,-3.381886,-2.937403,5.621404
+0.020000,16.070013,4.077455,17.443815,4.760485,-3.440634,-2.937403,5.633163
+0.020000,16.145594,4.020713,17.538324,4.690498,-3.499382,-2.937403,5.645357
+0.020000,16.220732,3.965747,17.631423,4.619335,-3.558130,-2.937403,5.657959
+0.020000,16.295400,3.912578,17.723086,4.546998,-3.616878,-2.937403,5.670943
+0.020000,16.369561,3.861228,17.813291,4.473485,-3.675626,-2.937403,5.684279
+0.020000,16.443179,3.811710,17.902014,4.398798,-3.734374,-2.937403,5.697937
+0.020000,16.516212,3.764036,17.989231,4.322935,-3.793122,-2.937403,5.711884
+0.020000,16.588618,3.718212,18.074919,4.245898,-3.851870,-2.937403,5.726086
+0.020000,16.660348,3.674241,18.159055,4.167685,-3.910618,-2.937403,5.740508
+0.020000,16.731354,3.632120,18.241615,4.088298,-3.969366,-2.937403,5.755115
+0.020000,16.801583,3.591841,18.322575,4.007736,-4.028115,-2.937403,5.769867
+0.020000,16.870987,3.553391,18.401919,3.926663,-4.053616,-1.275074,5.784729
+0.020000,16.939526,3.516743,18.479642,3.845591,-4.053616,0.000000,5.799665
+0.020000,17.007164,3.481868,18.555743,3.764519,-4.053616,-0.000000,5.814642
+0.020000,17.073866,3.448731,18.630223,3.683446,-4.053616,0.000000,5.829625
+0.020000,17.139593,3.417296,18.703081,3.602374,-4.053616,-0.000000,5.844582
+0.020000,17.204309,3.387524,18.774318,3.521302,-4.053616,-0.000000,5.859481
+0.020000,17.267977,3.359372,18.843933,3.440229,-4.053616,0.000000,5.874290
+0.020000,17.330561,3.332796,18.911927,3.359157,-4.053616,-0.000000,5.888979
+0.020000,17.392026,3.307749,18.978299,3.278085,-4.053616,0.000000,5.903519
+0.020000,17.452336,3.284184,19.043050,3.197013,-4.053616,0.000000,5.917883
+0.020000,17.511457,3.262049,19.106180,3.115940,-4.053616,0.000000,5.932046
+0.020000,17.569357,3.241295,19.167688,3.034868,-4.053616,0.000000,5.945982
+0.020000,17.626005,3.221869,19.227574,2.953796,-4.053616,0.000000,5.959670
+0.020000,17.681370,3.203717,19.285840,2.872723,-4.053616,0.000000,5.973088
+0.020000,17.735424,3.186786,19.342483,2.791651,-4.053616,0.000000,5.986219
+0.020000,17.788146,3.171021,19.397512,2.711244,-4.020369,1.662329,5.999046
+0.020000,17.839528,3.156363,19.450945,2.632011,-3.961621,2.937403,6.011558
+0.020000,17.889570,3.142754,19.502804,2.553954,-3.902873,2.937403,6.023747
+0.020000,17.938272,3.130136,19.553115,2.477071,-3.844125,2.937403,6.035607
+0.020000,17.985636,3.118454,19.601899,2.401364,-3.785377,2.937403,6.047132
+0.020000,18.031668,3.107654,19.649181,2.326831,-3.726629,2.937403,6.058320
+0.020000,18.076373,3.097683,19.694984,2.253473,-3.667881,2.937403,6.069168
+0.020000,18.119757,3.088492,19.739332,2.181291,-3.609133,2.937403,6.079676
+0.020000,18.161831,3.080032,19.782247,2.110283,-3.550385,2.937403,6.089843
+0.020000,18.202603,3.072257,19.823755,2.040450,-3.491637,2.937403,6.099671
+0.020000,18.242085,3.065123,19.863877,1.971793,-3.432889,2.937403,6.109162
+0.020000,18.280291,3.058587,19.902638,1.904310,-3.374141,2.937403,6.118320
+0.020000,18.317233,3.052608,19.940061,1.838002,-3.315393,2.937403,6.127146
+0.020000,18.352927,3.047148,19.976170,1.772869,-3.256645,2.937403,6.135647
+0.020000,18.387388,3.042171,20.010988,1.708911,-3.197897,2.937403,6.143827
+0.020000,18.420631,3.037641,20.044538,1.646128,-3.139149,2.937403,6.151690
+0.020000,18.452674,3.033526,20.076845,1.584520,-3.080400,2.937403,6.159243
+0.020000,18.483534,3.029795,20.107931,1.524087,-3.021652,2.937403,6.166492
+0.020000,18.513232,3.026418,20.137820,1.464829,-2.962904,2.937403,6.173442
+0.020000,18.541786,3.023368,20.166536,1.406746,-2.904156,2.937403,6.180101
+0.020000,18.569213,3.020619,20.194102,1.349838,-2.845408,2.937403,6.186475
+0.020000,18.595537,3.018147,20.220541,1.294104,-2.786660,2.937403,6.192570
+0.020000,18.620777,3.015927,20.245877,1.239546,-2.727912,2.937403,6.198394
+0.020000,18.644952,3.013940,20.270135,1.186163,-2.669164,2.937403,6.203953
+0.020000,18.668085,3.012165,20.293336,1.133955,-2.610416,2.937403,6.209255
+0.020000,18.690197,3.010584,20.315504,1.082921,-2.551668,2.937403,6.214305
+0.020000,18.711311,3.009178,20.336664,1.033063,-2.492920,2.937403,6.219112
+0.020000,18.731446,3.007932,20.356839,0.984379,-2.434172,2.937403,6.223682
+0.020000,18.750626,3.006832,20.376051,0.936871,-2.375424,2.937403,6.228022
+0.020000,18.768876,3.005861,20.394325,0.890537,-2.316676,2.937403,6.232139
+0.020000,18.786213,3.005010,20.411685,0.845379,-2.257928,2.937403,6.236039
+0.020000,18.802665,3.004264,20.428152,0.801395,-2.199180,2.937403,6.239730
+0.020000,18.818250,3.003613,20.443752,0.758587,-2.140432,2.937403,6.243216
+0.020000,18.832994,3.003048,20.458507,0.716953,-2.081684,2.937403,6.246507
+0.020000,18.846921,3.002559,20.472442,0.676494,-2.022935,2.937403,6.249607
+0.020000,18.860052,3.002137,20.485579,0.637210,-1.964187,2.937403,6.252524
+0.020000,18.872409,3.001775,20.497942,0.599102,-1.905439,2.937403,6.255262
+0.020000,18.884018,3.001465,20.509555,0.562168,-1.846691,2.937403,6.257829
+0.020000,18.894901,3.001202,20.520441,0.526409,-1.787943,2.937403,6.260231
+0.020000,18.905080,3.000980,20.530623,0.491825,-1.729195,2.937403,6.262474
+0.020000,18.914580,3.000793,20.540125,0.458416,-1.670447,2.937403,6.264563
+0.020000,18.923424,3.000637,20.548971,0.426182,-1.611699,2.937403,6.266505
+0.020000,18.931637,3.000508,20.557184,0.395123,-1.552951,2.937403,6.268305
+0.020000,18.939240,3.000401,20.564788,0.365239,-1.494203,2.937403,6.269969
+0.020000,18.946256,3.000313,20.571806,0.336530,-1.435455,2.937403,6.271503
+0.020000,18.952713,3.000243,20.578261,0.308996,-1.376707,2.937403,6.272913
+0.020000,18.958627,3.000186,20.584177,0.282637,-1.317959,2.937403,6.274203
+0.020000,18.964029,3.000140,20.589578,0.257452,-1.259211,2.937403,6.275379
+0.020000,18.968937,3.000105,20.594487,0.233443,-1.200463,2.937403,6.276447
+0.020000,18.973377,3.000077,20.598928,0.210609,-1.141715,2.937403,6.277413
+0.020000,18.977374,3.000055,20.602923,0.188950,-1.082967,2.937403,6.278281
+0.020000,18.980947,3.000039,20.606497,0.168465,-1.024218,2.937403,6.279057
+0.020000,18.984123,3.000027,20.609674,0.149156,-0.965470,2.937403,6.279746
+0.020000,18.986926,3.000018,20.612475,0.131021,-0.906722,2.937403,6.280354
+0.020000,18.989377,3.000012,20.614926,0.114062,-0.847974,2.937403,6.280885
+0.020000,18.991499,3.000008,20.617050,0.098277,-0.789226,2.937403,6.281345
+0.020000,18.993318,3.000005,20.618869,0.083668,-0.730478,2.937403,6.281739
+0.020000,18.994857,3.000003,20.620408,0.070233,-0.671730,2.937403,6.282073
+0.020000,18.996140,3.000002,20.621690,0.057974,-0.612982,2.937403,6.282350
+0.020000,18.997188,3.000001,20.622739,0.046889,-0.554234,2.937403,6.282577
+0.020000,18.998027,3.000000,20.623577,0.036979,-0.495486,2.937403,6.282759
+0.020000,18.998680,3.000000,20.624230,0.028244,-0.436738,2.937403,6.282900
+0.020000,18.999170,3.000000,20.624719,0.020685,-0.377990,2.937403,6.283006
+0.020000,18.999520,3.000000,20.625069,0.014300,-0.319242,2.937403,6.283081
+0.020000,18.999754,3.000000,20.625303,0.009090,-0.260494,2.937403,6.283132
+0.020000,18.999895,3.000000,20.625444,0.005055,-0.201746,2.937403,6.283163
+0.020000,18.999968,3.000000,20.625517,0.002195,-0.142998,2.937403,6.283178
+0.020000,18.999995,3.000000,20.625544,0.000510,-0.084250,2.937403,6.283184
+0.020000,19.000000,3.000000,20.625549,0.000000,-0.025501,2.937403,6.283185
+0.020000,19.000000,3.000000,20.625549,0.000000,0.000000,1.275074,6.283185
diff --git a/src/main/deploy/output/trenchAndAim.right.pf1.csv b/src/main/deploy/output/trenchAndAim.right.pf1.csv
new file mode 100644
index 0000000..111330c
--- /dev/null
+++ b/src/main/deploy/output/trenchAndAim.right.pf1.csv
@@ -0,0 +1,306 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,6.750000,0.000012,0.001175,0.058748,2.937403,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,-0.058748,-5.874806,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,2.937403,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.000000,6.750000,0.000012,0.000000,0.000000,0.000000,0.000000
+0.020000,0.001926,6.750000,0.001938,0.096293,4.814639,240.731970,0.000000
+0.020000,0.004570,6.750000,0.004581,0.132183,1.794517,-151.006114,0.000000
+0.020000,0.007577,6.750000,0.007589,0.150395,0.910595,-44.196111,0.000000
+0.020000,0.010973,6.750000,0.010985,0.169782,0.969343,2.937403,0.000000
+0.020000,0.014780,6.750000,0.014792,0.190344,1.028091,2.937403,0.000000
+0.020000,0.019022,6.750000,0.019033,0.212080,1.086839,2.937403,0.000000
+0.020000,0.023721,6.750000,0.023733,0.234992,1.145587,2.937403,0.000000
+0.020000,0.028903,6.750000,0.028915,0.259079,1.204335,2.937403,0.000000
+0.020000,0.034590,6.750000,0.034602,0.284341,1.263083,2.937403,0.000000
+0.020000,0.040805,6.750000,0.040817,0.310777,1.321831,2.937403,0.000000
+0.020000,0.047573,6.750000,0.047585,0.338389,1.380579,2.937403,0.000000
+0.020000,0.054917,6.750000,0.054928,0.367175,1.439327,2.937403,0.000000
+0.020000,0.062859,6.750000,0.062871,0.397137,1.498075,2.937403,0.000000
+0.020000,0.071425,6.750000,0.071437,0.428273,1.556824,2.937403,0.000000
+0.020000,0.080637,6.750000,0.080648,0.460585,1.615572,2.937403,0.000000
+0.020000,0.090518,6.750000,0.090530,0.494071,1.674320,2.937403,0.000000
+0.020000,0.101093,6.750000,0.101104,0.528733,1.733068,2.937403,0.000000
+0.020000,0.112384,6.750000,0.112396,0.564569,1.791816,2.937403,0.000000
+0.020000,0.124416,6.750000,0.124427,0.601580,1.850564,2.937403,0.000000
+0.020000,0.137211,6.750000,0.137223,0.639766,1.909312,2.937403,0.000000
+0.020000,0.150793,6.750000,0.150805,0.679128,1.968060,2.937403,0.000000
+0.020000,0.165187,6.750000,0.165198,0.719664,2.026808,2.937403,0.000000
+0.020000,0.180414,6.750000,0.180426,0.761375,2.085556,2.937403,0.000000
+0.020000,0.196499,6.750000,0.196511,0.804261,2.144304,2.937403,0.000000
+0.020000,0.213466,6.750000,0.213478,0.848322,2.203052,2.937403,0.000000
+0.020000,0.231337,6.750000,0.231349,0.893558,2.261800,2.937403,0.000000
+0.020000,0.250136,6.750000,0.250148,0.939969,2.320548,2.937403,0.000000
+0.020000,0.269887,6.750000,0.269899,0.987555,2.379296,2.937403,0.000000
+0.020000,0.290614,6.750000,0.290626,1.036316,2.438044,2.937403,0.000000
+0.020000,0.312339,6.750000,0.312351,1.086252,2.496792,2.937403,0.000000
+0.020000,0.335086,6.750000,0.335098,1.137362,2.555541,2.937403,0.000000
+0.020000,0.358879,6.750000,0.358891,1.189648,2.614289,2.937403,0.000000
+0.020000,0.383741,6.750000,0.383753,1.243109,2.673037,2.937403,0.000000
+0.020000,0.409696,6.750000,0.409708,1.297745,2.731785,2.937403,0.000000
+0.020000,0.436767,6.750000,0.436779,1.353555,2.790533,2.937403,0.000000
+0.020000,0.464978,6.750000,0.464990,1.410541,2.849281,2.937403,0.000000
+0.020000,0.494352,6.750000,0.494364,1.468701,2.908029,2.937403,0.000000
+0.020000,0.524913,6.750000,0.524925,1.528037,2.966777,2.937403,0.000000
+0.020000,0.556684,6.750000,0.556696,1.588547,3.025525,2.937403,0.000000
+0.020000,0.589688,6.750000,0.589700,1.650233,3.084273,2.937403,0.000000
+0.020000,0.623950,6.750000,0.623962,1.713093,3.143021,2.937403,0.000000
+0.020000,0.659493,6.750000,0.659505,1.777129,3.201769,2.937403,0.000000
+0.020000,0.696340,6.750000,0.696351,1.842339,3.260517,2.937403,0.000000
+0.020000,0.734514,6.750000,0.734526,1.908724,3.319265,2.937403,0.000000
+0.020000,0.774040,6.750000,0.774052,1.976285,3.378013,2.937403,0.000000
+0.020000,0.814940,6.750000,0.814952,2.045020,3.436761,2.937403,0.000000
+0.020000,0.857239,6.750000,0.857251,2.114930,3.495509,2.937403,0.000000
+0.020000,0.900959,6.750000,0.900971,2.186015,3.554258,2.937403,0.000000
+0.020000,0.946125,6.750000,0.946136,2.258275,3.613006,2.937403,0.000000
+0.020000,0.992759,6.750000,0.992771,2.331710,3.671754,2.937403,0.000000
+0.020000,1.040885,6.750000,1.040897,2.406320,3.730502,2.937403,0.000000
+0.020000,1.090527,6.750000,1.090539,2.482105,3.789250,2.937403,0.000000
+0.020000,1.141709,6.750000,1.141720,2.559065,3.847998,2.937403,0.000000
+0.020000,1.194453,6.750000,1.194464,2.637200,3.906746,2.937403,0.000000
+0.020000,1.248783,6.750000,1.248795,2.716510,3.965494,2.937403,0.000000
+0.020000,1.304723,6.750000,1.304735,2.796995,4.024242,2.937403,0.000000
+0.020000,1.362284,6.750000,1.362296,2.878067,4.053616,1.468701,0.000000
+0.020000,1.421467,6.750000,1.421479,2.959140,4.053616,0.000000,0.000000
+0.020000,1.482271,6.750000,1.482283,3.040212,4.053616,0.000000,0.000000
+0.020000,1.544697,6.750000,1.544709,3.121284,4.053616,0.000000,0.000000
+0.020000,1.608744,6.750000,1.608756,3.202357,4.053616,-0.000000,0.000000
+0.020000,1.674413,6.750000,1.674424,3.283429,4.053616,0.000000,0.000000
+0.020000,1.741703,6.750000,1.741714,3.364501,4.053616,-0.000000,0.000000
+0.020000,1.810614,6.750000,1.810626,3.445574,4.053616,0.000000,0.000000
+0.020000,1.881147,6.750000,1.881159,3.526646,4.053616,-0.000000,0.000000
+0.020000,1.953301,6.750000,1.953313,3.607718,4.053616,0.000000,0.000000
+0.020000,2.027077,6.750000,2.027089,3.688791,4.053616,0.000000,0.000000
+0.020000,2.102474,6.750000,2.102486,3.769863,4.053616,-0.000000,0.000000
+0.020000,2.179493,6.750000,2.179505,3.850935,4.053616,-0.000000,0.000000
+0.020000,2.258133,6.750000,2.258145,3.932008,4.053616,0.000000,0.000000
+0.020000,2.338395,6.750000,2.338407,4.013080,4.053616,-0.000000,0.000000
+0.020000,2.420266,6.750000,2.420278,4.093565,4.024242,-1.468701,0.000000
+0.020000,2.503724,6.750000,2.503735,4.172875,3.965494,-2.937403,0.000000
+0.020000,2.588744,6.750000,2.588756,4.251009,3.906746,-2.937403,0.000000
+0.020000,2.675303,6.750000,2.675315,4.327969,3.847998,-2.937403,0.000000
+0.020000,2.763378,6.750000,2.763390,4.403754,3.789250,-2.937403,0.000000
+0.020000,2.852946,6.750000,2.852957,4.478364,3.730502,-2.937403,0.000000
+0.020000,2.943982,6.750000,2.943993,4.551800,3.671754,-2.937403,0.000000
+0.020000,3.036463,6.750000,3.036475,4.624060,3.613006,-2.937403,0.000000
+0.020000,3.130366,6.750000,3.130377,4.695145,3.554258,-2.937403,0.000000
+0.020000,3.225667,6.750000,3.225679,4.765055,3.495509,-2.937403,0.000000
+0.020000,3.322343,6.750000,3.322354,4.833790,3.436761,-2.937403,0.000000
+0.020000,3.420370,6.750000,3.420381,4.901350,3.378013,-2.937403,0.000000
+0.020000,3.519724,6.750000,3.519736,4.967736,3.319265,-2.937403,0.000000
+0.020000,3.620383,6.750000,3.620395,5.032946,3.260517,-2.937403,0.000000
+0.020000,3.722323,6.750000,3.722335,5.096982,3.201769,-2.937403,0.000000
+0.020000,3.825520,6.750000,3.825531,5.159842,3.143021,-2.937403,0.000000
+0.020000,3.929950,6.750000,3.929962,5.221527,3.084273,-2.937403,0.000000
+0.020000,4.035591,6.750000,4.035603,5.282038,3.025525,-2.937403,0.000000
+0.020000,4.142418,6.750000,4.142430,5.341373,2.966777,-2.937403,0.000000
+0.020000,4.250409,6.750000,4.250421,5.399534,2.908029,-2.937403,0.000000
+0.020000,4.359540,6.750000,4.359551,5.456520,2.849281,-2.937403,0.000000
+0.020000,4.469786,6.750000,4.469798,5.512330,2.790533,-2.937403,0.000000
+0.020000,4.581125,6.750000,4.581137,5.566966,2.731785,-2.937403,0.000000
+0.020000,4.693534,6.750000,4.693546,5.620427,2.673037,-2.937403,0.000000
+0.020000,4.806988,6.750000,4.807000,5.672712,2.614289,-2.937403,0.000000
+0.020000,4.921465,6.750000,4.921476,5.723823,2.555541,-2.937403,0.000000
+0.020000,5.036940,6.750000,5.036952,5.773759,2.496792,-2.937403,0.000000
+0.020000,5.153390,6.750000,5.153402,5.822520,2.438044,-2.937403,0.000000
+0.020000,5.270792,6.750000,5.270804,5.870106,2.379296,-2.937403,0.000000
+0.020000,5.389123,6.750000,5.389135,5.916517,2.320548,-2.937403,0.000000
+0.020000,5.508358,6.750000,5.508370,5.961753,2.261800,-2.937403,0.000000
+0.020000,5.628474,6.750000,5.628486,6.005814,2.203052,-2.937403,0.000000
+0.020000,5.749448,6.750000,5.749460,6.048700,2.144304,-2.937403,0.000000
+0.020000,5.871256,6.750000,5.871268,6.090411,2.085556,-2.937403,0.000000
+0.020000,5.993875,6.750000,5.993887,6.130947,2.026808,-2.937403,0.000000
+0.020000,6.117281,6.750000,6.117293,6.170309,1.968060,-2.937403,0.000000
+0.020000,6.241451,6.750000,6.241463,6.208495,1.909312,-2.937403,0.000000
+0.020000,6.366361,6.750000,6.366373,6.245506,1.850564,-2.937403,0.000000
+0.020000,6.491988,6.750000,6.492000,6.281342,1.791816,-2.937403,0.000000
+0.020000,6.618308,6.750000,6.618320,6.316004,1.733068,-2.937403,0.000000
+0.020000,6.745298,6.750000,6.745310,6.349490,1.674320,-2.937403,0.000000
+0.020000,6.872934,6.750000,6.872946,6.381802,1.615572,-2.937403,0.000000
+0.020000,7.001193,6.750000,7.001205,6.412938,1.556824,-2.937403,0.000000
+0.020000,7.130051,6.750000,7.130063,6.442900,1.498075,-2.937403,0.000000
+0.020000,7.259485,6.750000,7.259496,6.471686,1.439327,-2.937403,0.000000
+0.020000,7.389471,6.750000,7.389482,6.499298,1.380579,-2.937403,0.000000
+0.020000,7.519985,6.750000,7.519997,6.525734,1.321831,-2.937403,0.000000
+0.020000,7.651005,6.750000,7.651017,6.550996,1.263083,-2.937403,0.000000
+0.020000,7.782507,6.750000,7.782519,6.575083,1.204335,-2.937403,0.000000
+0.020000,7.914467,6.750000,7.914479,6.597994,1.145587,-2.937403,0.000000
+0.020000,8.046861,6.750000,8.046873,6.619731,1.086839,-2.937403,0.000000
+0.020000,8.179667,6.750000,8.179679,6.640293,1.028091,-2.937403,0.000000
+0.020000,8.312861,6.750000,8.312873,6.659680,0.969343,-2.937403,0.000000
+0.020000,8.446419,6.750000,8.446430,6.677892,0.910595,-2.937403,0.000000
+0.020000,8.580317,6.750000,8.580329,6.694929,0.851847,-2.937403,0.000000
+0.020000,8.714533,6.750000,8.714545,6.710791,0.793099,-2.937403,0.000000
+0.020000,8.849043,6.750000,8.849054,6.725478,0.734351,-2.937403,0.000000
+0.020000,8.983822,6.750000,8.983834,6.738990,0.675603,-2.937403,0.000000
+0.020000,9.118849,6.750000,9.118861,6.751327,0.616855,-2.937403,0.000000
+0.020000,9.254099,6.750000,9.254111,6.762489,0.558107,-2.937403,0.000000
+0.020000,9.389548,6.750000,9.389560,6.772476,0.499358,-2.937403,0.000000
+0.020000,9.525174,6.750000,9.525186,6.781288,0.440610,-2.937403,0.000000
+0.020000,9.660953,6.750000,9.660964,6.788926,0.381862,-2.937403,0.000000
+0.020000,9.796860,6.750000,9.796872,6.795388,0.323114,-2.937403,0.000000
+0.020000,9.932874,6.750000,9.932886,6.800675,0.264366,-2.937403,0.000000
+0.020000,10.041647,6.749742,10.041659,5.438681,-68.099694,-3418.203006,6.270750
+0.020000,10.140119,6.747042,10.140168,4.925432,-25.662445,2121.862444,6.240635
+0.020000,10.237673,6.741390,10.237886,4.885905,-1.976366,1184.303960,6.209867
+0.020000,10.334313,6.732775,10.334908,4.851125,-1.739009,11.867832,6.178552
+0.020000,10.430058,6.721185,10.431353,4.822229,-1.444788,14.711066,6.146815
+0.020000,10.524956,6.706614,10.527363,4.800520,-1.085478,17.965492,6.114787
+0.020000,10.619063,6.689054,10.623094,4.786551,-0.698405,19.353639,6.082612
+0.020000,10.712445,6.668499,10.718711,4.780844,-0.285387,20.650902,6.050441
+0.020000,10.805171,6.644943,10.814383,4.783575,0.136565,21.097591,6.018429
+0.020000,10.897315,6.618382,10.910279,4.794792,0.560847,21.214126,5.986728
+0.020000,10.988947,6.588814,11.006563,4.814224,0.971595,20.537391,5.955490
+0.020000,11.080131,6.556242,11.103390,4.841355,1.356541,19.247293,5.924856
+0.020000,11.170924,6.520675,11.200901,4.875538,1.709145,17.630194,5.894958
+0.020000,11.261372,6.482130,11.299220,4.915939,2.020075,15.546491,5.865914
+0.020000,11.351512,6.440635,11.398452,4.961617,2.283895,13.191001,5.837828
+0.020000,11.441369,6.396228,11.498683,5.011567,2.497504,10.680493,5.810787
+0.020000,11.530960,6.348960,11.599979,5.064768,2.660049,8.127238,5.784861
+0.020000,11.620292,6.298895,11.702383,5.120221,2.772663,5.630668,5.760105
+0.020000,11.709365,6.246109,11.805923,5.176983,2.838100,3.271892,5.736559
+0.020000,11.798175,6.190688,11.910607,5.234190,2.860323,1.111109,5.714250
+0.020000,11.886715,6.132732,12.016428,5.291071,2.844074,-0.812447,5.693190
+0.020000,11.974974,6.072347,12.123367,5.346961,2.794500,-2.478665,5.673382
+0.020000,12.062944,6.009651,12.231393,5.401277,2.715812,-3.934409,5.654820
+0.020000,12.150615,5.944764,12.340465,5.453585,2.615378,-5.021701,5.637492
+0.020000,12.237982,5.877813,12.450535,5.503501,2.495822,-5.977817,5.621376
+0.020000,12.325041,5.808928,12.561549,5.550734,2.361642,-6.708993,5.606450
+0.020000,12.411790,5.738242,12.673451,5.595091,2.217823,-7.190934,5.592686
+0.020000,12.498233,5.665886,12.786180,5.636431,2.067015,-7.540417,5.580054
+0.020000,12.584377,5.591994,12.899673,5.674651,1.911020,-7.799751,5.568524
+0.020000,12.670230,5.516698,13.013867,5.709715,1.753156,-7.893212,5.558064
+0.020000,12.755806,5.440126,13.128700,5.741632,1.595878,-7.863880,5.548641
+0.020000,12.841122,5.362407,13.244108,5.770419,1.439324,-7.827679,5.540226
+0.020000,12.926196,5.283665,13.360030,5.796111,1.284609,-7.735763,5.532787
+0.020000,13.011051,5.204022,13.476406,5.818794,1.134180,-7.521452,5.526295
+0.020000,13.095711,5.123597,13.593177,5.838541,0.987330,-7.342526,5.520722
+0.020000,13.180200,5.042505,13.710285,5.855420,0.843924,-7.170299,5.516040
+0.020000,13.264548,4.960859,13.827676,5.869535,0.705783,-6.907055,5.512224
+0.020000,13.348782,4.878768,13.945295,5.880969,0.571668,-6.705719,5.509251
+0.020000,13.432932,4.796337,14.063092,5.889819,0.442522,-6.457318,5.507097
+0.020000,13.517027,4.713670,14.181015,5.896164,0.317242,-6.263991,5.505742
+0.020000,13.601099,4.630867,14.299017,5.900092,0.196392,-6.042508,5.505165
+0.020000,13.685177,4.548025,14.417051,5.901681,0.079492,-5.844974,5.505348
+0.020000,13.769292,4.465239,14.535071,5.901003,-0.033944,-5.671800,5.506273
+0.020000,13.853472,4.382603,14.653033,5.898120,-0.144146,-5.510124,5.507924
+0.020000,13.937746,4.300205,14.770895,5.893096,-0.251208,-5.353098,5.510285
+0.020000,14.022141,4.218136,14.888614,5.885968,-0.356385,-5.258838,5.513343
+0.020000,14.106682,4.136482,15.006150,5.876774,-0.459677,-5.164604,5.517081
+0.020000,14.191393,4.055328,15.123461,5.865572,-0.560127,-5.022507,5.521487
+0.020000,14.276295,3.974758,15.240508,5.852333,-0.661962,-5.091748,5.526548
+0.020000,14.361408,3.894854,15.357250,5.837118,-0.760712,-4.937514,5.532250
+0.020000,14.446747,3.815698,15.473648,5.819889,-0.861475,-5.038122,5.538580
+0.020000,14.532326,3.737371,15.589661,5.800646,-0.962161,-5.034302,5.545525
+0.020000,14.618155,3.659950,15.705248,5.779371,-1.063746,-5.079251,5.553070
+0.020000,14.704238,3.583515,15.820369,5.756029,-1.167070,-5.166205,5.561202
+0.020000,14.790579,3.508142,15.934980,5.730575,-1.272717,-5.282338,5.569906
+0.020000,14.877174,3.433907,16.049039,5.702950,-1.381245,-5.426421,5.579165
+0.020000,14.964015,3.360885,16.162501,5.673100,-1.492507,-5.563101,5.588963
+0.020000,15.051090,3.289149,16.275321,5.640961,-1.606959,-5.722576,5.599282
+0.020000,15.138381,3.218771,16.387449,5.606408,-1.727659,-6.035003,5.610103
+0.020000,15.225863,3.149821,16.498837,5.569406,-1.850081,-6.121124,5.621404
+0.020000,15.313508,3.082367,16.609433,5.529826,-1.979005,-6.446173,5.633163
+0.020000,15.401278,3.016475,16.719185,5.487566,-2.112972,-6.698386,5.645357
+0.020000,15.489132,2.952208,16.828035,5.442533,-2.251654,-6.934085,5.657959
+0.020000,15.577020,2.889627,16.935928,5.394615,-2.395924,-7.213508,5.670943
+0.020000,15.664886,2.828787,17.042802,5.343700,-2.545729,-7.490229,5.684279
+0.020000,15.752671,2.769741,17.148596,5.289713,-2.699379,-7.682501,5.697937
+0.020000,15.840303,2.712538,17.253246,5.232497,-2.860762,-8.069159,5.711884
+0.020000,15.927710,2.657222,17.356686,5.172017,-3.023999,-8.161836,5.726086
+0.020000,16.014811,2.603829,17.458849,5.108145,-3.193625,-8.481294,5.740508
+0.020000,16.101519,2.552394,17.559666,5.040827,-3.365905,-8.614037,5.755115
+0.020000,16.187745,2.502942,17.659066,4.969996,-3.541535,-8.781495,5.769867
+0.020000,16.273399,2.455489,17.756986,4.896016,-3.698993,-7.872869,5.784729
+0.020000,16.358402,2.410038,17.853378,4.819590,-3.821301,-6.115416,5.799665
+0.020000,16.442680,2.366584,17.948199,4.741047,-3.927162,-5.293047,5.814642
+0.020000,16.526155,2.325115,18.041407,4.660392,-4.032735,-5.278657,5.829625
+0.020000,16.608749,2.285614,18.132960,4.577684,-4.135412,-5.133876,5.844582
+0.020000,16.690384,2.248059,18.222819,4.492961,-4.236144,-5.036553,5.859481
+0.020000,16.770982,2.212421,18.310945,4.406292,-4.333461,-4.865875,5.874290
+0.020000,16.850467,2.178669,18.397299,4.317698,-4.429690,-4.811457,5.888979
+0.020000,16.928763,2.146764,18.481847,4.227371,-4.516364,-4.333707,5.903519
+0.020000,17.005797,2.116664,18.564552,4.135266,-4.605247,-4.444120,5.917883
+0.020000,17.081497,2.088323,18.645384,4.041584,-4.684091,-3.942195,5.932046
+0.020000,17.155796,2.061691,18.724312,3.946398,-4.759315,-3.761216,5.945982
+0.020000,17.228628,2.036714,18.801307,3.849791,-4.830350,-3.551765,5.959670
+0.020000,17.299931,2.013337,18.876345,3.751871,-4.895998,-3.282411,5.973088
+0.020000,17.369648,1.991500,18.949401,3.652831,-4.951992,-2.799655,5.986219
+0.020000,17.437732,1.971142,19.020464,3.553116,-4.985765,-1.688652,5.999046
+0.020000,17.504154,1.952194,19.089536,3.453595,-4.976045,0.485999,6.011558
+0.020000,17.568898,1.934586,19.156632,3.354795,-4.940012,1.801605,6.023747
+0.020000,17.631950,1.918251,19.221765,3.256685,-4.905504,1.725405,6.035607
+0.020000,17.693302,1.903118,19.284956,3.159543,-4.857100,2.420231,6.047132
+0.020000,17.752949,1.889124,19.346223,3.063320,-4.811108,2.299605,6.058320
+0.020000,17.810889,1.876201,19.405586,2.968192,-4.756437,2.733548,6.069168
+0.020000,17.867123,1.864288,19.463068,2.874074,-4.705862,2.528721,6.079676
+0.020000,17.921656,1.853323,19.518692,2.781230,-4.642235,3.181343,6.089843
+0.020000,17.974496,1.843247,19.572484,2.689597,-4.581629,3.030314,6.099671
+0.020000,18.025652,1.834003,19.624470,2.599261,-4.516813,3.240809,6.109162
+0.020000,18.075141,1.825536,19.674678,2.510404,-4.442863,3.697489,6.118320
+0.020000,18.122975,1.817795,19.723133,2.422776,-4.381360,3.075181,6.127146
+0.020000,18.169172,1.810728,19.769868,2.336760,-4.300834,4.026267,6.135647
+0.020000,18.213753,1.804289,19.814911,2.252146,-4.230682,3.507634,6.143827
+0.020000,18.256735,1.798432,19.858291,2.168969,-4.158853,3.591414,6.151690
+0.020000,18.298143,1.793115,19.900038,2.087380,-4.079474,3.968963,6.159243
+0.020000,18.337998,1.788296,19.940184,2.007287,-4.004622,3.742588,6.166492
+0.020000,18.376328,1.783938,19.978761,1.928838,-3.922449,4.108651,6.173442
+0.020000,18.413159,1.780004,20.015802,1.852051,-3.839348,4.155056,6.180101
+0.020000,18.448513,1.776460,20.051333,1.776546,-3.775268,3.204009,6.186475
+0.020000,18.482423,1.773275,20.085392,1.702962,-3.679213,4.802741,6.192570
+0.020000,18.514915,1.770418,20.118009,1.630856,-3.605284,3.696472,6.198394
+0.020000,18.546015,1.767862,20.149214,1.560253,-3.530134,3.757497,6.203953
+0.020000,18.575755,1.765580,20.179042,1.491387,-3.443327,4.340318,6.209255
+0.020000,18.604165,1.763548,20.207524,1.424117,-3.363472,3.992745,6.214305
+0.020000,18.631274,1.761743,20.234694,1.358468,-3.282451,4.051056,6.219112
+0.020000,18.657111,1.760145,20.260580,1.294301,-3.208340,3.705557,6.223682
+0.020000,18.681707,1.758733,20.285216,1.231820,-3.124077,4.213169,6.228022
+0.020000,18.705095,1.757490,20.308637,1.171068,-3.037583,4.324670,6.232139
+0.020000,18.727301,1.756399,20.330870,1.111648,-2.971000,3.329188,6.236039
+0.020000,18.748362,1.755444,20.351953,1.054124,-2.876187,4.740616,6.239730
+0.020000,18.768302,1.754612,20.371910,0.997841,-2.814175,3.100625,6.243216
+0.020000,18.787157,1.753889,20.390778,0.943430,-2.720532,4.682158,6.246507
+0.020000,18.804956,1.753263,20.408589,0.890539,-2.644555,3.798838,6.249607
+0.020000,18.821731,1.752724,20.425372,0.839162,-2.568861,3.784713,6.252524
+0.020000,18.837510,1.752262,20.441158,0.789293,-2.493474,3.769336,6.255262
+0.020000,18.852326,1.751867,20.455980,0.741086,-2.410316,4.157886,6.257829
+0.020000,18.866211,1.751532,20.469869,0.694436,-2.332507,3.890479,6.260231
+0.020000,18.879193,1.751248,20.482853,0.649243,-2.259655,3.642603,6.262474
+0.020000,18.891303,1.751010,20.494966,0.605644,-2.179965,3.984489,6.264563
+0.020000,18.902574,1.750811,20.506239,0.563640,-2.100202,3.988161,6.266505
+0.020000,18.913037,1.750646,20.516703,0.523215,-2.021214,3.949390,6.268305
+0.020000,18.922720,1.750510,20.526387,0.484192,-1.951170,3.502193,6.269969
+0.020000,18.931654,1.750399,20.535321,0.446705,-1.874344,3.841274,6.271503
+0.020000,18.939872,1.750308,20.543540,0.410955,-1.787500,4.342238,6.272913
+0.020000,18.947399,1.750236,20.551067,0.376335,-1.730986,2.825685,6.274203
+0.020000,18.954272,1.750178,20.557941,0.343678,-1.632873,4.905655,6.275379
+0.020000,18.960514,1.750133,20.564183,0.312130,-1.577393,2.773975,6.276447
+0.020000,18.966162,1.750098,20.569831,0.282393,-1.486866,4.526386,6.277413
+0.020000,18.971244,1.750070,20.574913,0.254102,-1.414514,3.617577,6.278281
+0.020000,18.975787,1.750050,20.579456,0.227122,-1.349009,3.275250,6.279057
+0.020000,18.979824,1.750035,20.583493,0.201889,-1.261660,4.367447,6.279746
+0.020000,18.983387,1.750024,20.587056,0.178131,-1.187905,3.687772,6.280354
+0.020000,18.986502,1.750016,20.590171,0.155749,-1.119104,3.440035,6.280885
+0.020000,18.989199,1.750010,20.592868,0.134864,-1.044259,3.742276,6.281345
+0.020000,18.991511,1.750006,20.595180,0.115578,-0.964309,3.997455,6.281739
+0.020000,18.993466,1.750004,20.597135,0.097776,-0.890076,3.711674,6.282073
+0.020000,18.995095,1.750002,20.598765,0.081461,-0.815743,3.716634,6.282350
+0.020000,18.996428,1.750001,20.600097,0.066635,-0.741327,3.720805,6.282577
+0.020000,18.997494,1.750001,20.601163,0.053298,-0.666842,3.724255,6.282759
+0.020000,18.998323,1.750000,20.601992,0.041452,-0.592301,3.727051,6.282900
+0.020000,18.998945,1.750000,20.602614,0.031097,-0.517716,3.729260,6.283006
+0.020000,18.999390,1.750000,20.603059,0.022235,-0.443097,3.730952,6.283081
+0.020000,18.999687,1.750000,20.603356,0.014866,-0.368453,3.732195,6.283132
+0.020000,18.999867,1.750000,20.603536,0.008991,-0.293792,3.733059,6.283163
+0.020000,18.999959,1.750000,20.603628,0.004608,-0.219119,3.733614,6.283178
+0.020000,18.999994,1.750000,20.603663,0.001719,-0.144441,3.733928,6.283184
+0.020000,19.000000,1.750000,20.603669,0.000324,-0.069759,3.734073,6.283185
+0.020000,19.000000,1.750000,20.603669,0.000000,-0.016209,2.677514,6.283185
diff --git a/src/main/deploy/output/turn180.left.pf1.csv b/src/main/deploy/output/turn180.left.pf1.csv
new file mode 100644
index 0000000..1cf9504
--- /dev/null
+++ b/src/main/deploy/output/turn180.left.pf1.csv
@@ -0,0 +1,239 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,9.057128,9.650359,0.000012,0.001190,0.059499,2.974938,4.804810
+0.020000,9.057130,9.650339,0.000032,0.001014,-0.008817,-3.415786,4.804789
+0.020000,9.057134,9.650294,0.000078,0.002280,0.063337,3.607720,4.804744
+0.020000,9.057142,9.650213,0.000159,0.004053,0.088631,1.264679,4.804663
+0.020000,9.057153,9.650087,0.000285,0.006330,0.113861,1.261503,4.804536
+0.020000,9.057170,9.649906,0.000468,0.009110,0.138987,1.256302,4.804354
+0.020000,9.057221,9.649357,0.001019,0.027568,0.922884,39.194847,4.804106
+0.020000,9.057320,9.648276,0.002104,0.054243,1.333763,20.543936,4.803783
+0.020000,9.057445,9.646909,0.003476,0.068622,0.718956,-30.740332,4.803374
+0.020000,9.057598,9.645223,0.005170,0.084673,0.802558,4.180110,4.802871
+0.020000,9.057783,9.643184,0.007218,0.102388,0.885733,4.158746,4.802263
+0.020000,9.058000,9.640758,0.009653,0.121755,0.968337,4.130174,4.801541
+0.020000,9.058253,9.637914,0.012508,0.142760,1.050277,4.097028,4.800697
+0.020000,9.058543,9.634619,0.015816,0.165389,1.131442,4.058218,4.799722
+0.020000,9.058872,9.630841,0.019608,0.189622,1.211668,4.011333,4.798607
+0.020000,9.059240,9.626548,0.023917,0.215439,1.290819,3.957549,4.797344
+0.020000,9.059649,9.621709,0.028773,0.242814,1.368782,3.898110,4.795927
+0.020000,9.060098,9.616293,0.034208,0.271720,1.445293,3.825550,4.794348
+0.020000,9.060588,9.610270,0.040250,0.302126,1.520273,3.749034,4.792602
+0.020000,9.061116,9.603612,0.046930,0.333995,1.593463,3.659512,4.790682
+0.020000,9.061683,9.596288,0.054276,0.367289,1.664713,3.562493,4.788584
+0.020000,9.062286,9.588271,0.062315,0.401964,1.733744,3.451554,4.786304
+0.020000,9.062922,9.579535,0.071074,0.437971,1.800354,3.330467,4.783839
+0.020000,9.063588,9.570053,0.080580,0.475257,1.864306,3.197616,4.781188
+0.020000,9.064280,9.559801,0.090855,0.513764,1.925346,3.051975,4.778350
+0.020000,9.064993,9.548755,0.101923,0.553428,1.983183,2.891879,4.775326
+0.020000,9.065721,9.536894,0.113807,0.594179,2.037572,2.719464,4.772117
+0.020000,9.066459,9.524197,0.126526,0.635943,2.088188,2.530795,4.768728
+0.020000,9.067199,9.510644,0.140099,0.678638,2.134757,2.328428,4.765163
+0.020000,9.067934,9.496219,0.154542,0.722178,2.176964,2.110369,4.761429
+0.020000,9.068655,9.480907,0.169871,0.766467,2.214475,1.875549,4.757535
+0.020000,9.069355,9.464694,0.186100,0.811407,2.246992,1.625849,4.753493
+0.020000,9.070023,9.447569,0.203237,0.856890,2.274164,1.358599,4.749314
+0.020000,9.070651,9.429524,0.221294,0.902803,2.295661,1.074830,4.745014
+0.020000,9.071228,9.410552,0.240274,0.949026,2.311155,0.774708,4.740610
+0.020000,9.071744,9.390666,0.260167,0.994668,2.282065,-1.454478,4.736127
+0.020000,9.072190,9.369893,0.280945,1.038865,2.209856,-3.610461,4.731593
+0.020000,9.072556,9.348265,0.302575,1.081540,2.133776,-3.804007,4.727038
+0.020000,9.072834,9.325815,0.325028,1.122618,2.053874,-3.995112,4.722495
+0.020000,9.073016,9.302575,0.348268,1.162022,1.970214,-4.182982,4.717997
+0.020000,9.073097,9.278582,0.372262,1.199679,1.882841,-4.368636,4.713578
+0.020000,9.073073,9.253871,0.396972,1.235516,1.791830,-4.550560,4.709276
+0.020000,9.072940,9.228482,0.422361,1.269460,1.697217,-4.730673,4.705128
+0.020000,9.072699,9.202455,0.448390,1.301441,1.599041,-4.908814,4.701173
+0.020000,9.072350,9.175829,0.475018,1.331388,1.497346,-5.084734,4.697452
+0.020000,9.071897,9.148648,0.502203,1.359230,1.392119,-5.261344,4.694008
+0.020000,9.071343,9.120956,0.529901,1.384897,1.283367,-5.437616,4.690883
+0.020000,9.070698,9.092797,0.558067,1.408318,1.171039,-5.616363,4.688122
+0.020000,9.069969,9.064218,0.586655,1.429420,1.055081,-5.797911,4.685773
+0.020000,9.069170,9.035266,0.615618,1.448128,0.935394,-5.984366,4.683882
+0.020000,9.068314,9.005992,0.644905,1.464365,0.811862,-6.176608,4.682500
+0.020000,9.067417,8.976444,0.674466,1.478051,0.684329,-6.376625,4.681677
+0.020000,9.066497,8.946676,0.704248,1.489104,0.552641,-6.584414,4.681465
+0.020000,9.065576,8.916742,0.734197,1.497436,0.416605,-6.801803,4.681921
+0.020000,9.064677,8.886696,0.764256,1.502957,0.276041,-7.028195,4.683098
+0.020000,9.063822,8.856597,0.794368,1.505573,0.130783,-7.262904,4.685057
+0.020000,9.063039,8.826503,0.824471,1.505187,-0.019310,-7.504651,4.687856
+0.020000,9.062356,8.796477,0.854505,1.501701,-0.174290,-7.749003,4.691557
+0.020000,9.061800,8.766582,0.884406,1.495018,-0.334133,-7.992138,4.696226
+0.020000,9.061402,8.736883,0.914107,1.485046,-0.498630,-8.224843,4.701927
+0.020000,9.061191,8.707450,0.943541,1.471698,-0.667393,-8.438138,4.708727
+0.020000,9.061197,8.678352,0.972639,1.454903,-0.839735,-8.617139,4.716697
+0.020000,9.061450,8.649661,1.001331,1.434611,-1.014584,-8.742411,4.725905
+0.020000,9.061976,8.621450,1.029547,1.410803,-1.190428,-8.792206,4.736423
+0.020000,9.062802,8.593792,1.057217,1.383500,-1.365144,-8.735805,4.748319
+0.020000,9.063950,8.566761,1.084273,1.352782,-1.535919,-8.538770,4.761663
+0.020000,9.065440,8.540427,1.110649,1.318799,-1.699110,-8.159542,4.776520
+0.020000,9.067289,8.514858,1.136284,1.281797,-1.850110,-7.549998,4.792949
+0.020000,9.069507,8.490115,1.161127,1.242131,-1.983322,-6.660585,4.811005
+0.020000,9.072101,8.466249,1.185133,1.200290,-2.092035,-5.435677,4.830730
+0.020000,9.075074,8.443303,1.208271,1.156918,-2.168586,-3.827545,4.852156
+0.020000,9.078425,8.421300,1.230528,1.112830,-2.204425,-1.791926,4.875296
+0.020000,9.082149,8.400246,1.251908,1.069020,-2.190512,0.695642,4.900146
+0.020000,9.088047,8.370844,1.281896,1.499381,21.518061,1185.428660,4.915678
+0.020000,9.095616,8.335152,1.318382,1.824293,16.245615,-263.622320,4.927088
+0.020000,9.103711,8.299048,1.355382,1.850021,1.286395,-747.961003,4.938817
+0.020000,9.112352,8.262545,1.392894,1.875599,1.278908,-0.374326,4.950867
+0.020000,9.121561,8.225656,1.430915,1.901056,1.272839,-0.303487,4.963241
+0.020000,9.131361,8.188395,1.469444,1.926420,1.268191,-0.232403,4.975941
+0.020000,9.141775,8.150775,1.508478,1.951724,1.265210,-0.149008,4.988968
+0.020000,9.152825,8.112811,1.548018,1.977006,1.264079,-0.056581,5.002322
+0.020000,9.164536,8.074515,1.588064,2.002305,1.264977,0.044925,5.016003
+0.020000,9.176930,8.035902,1.628618,2.027666,1.268050,0.153618,5.030010
+0.020000,9.190033,7.996986,1.669680,2.053136,1.273512,0.273096,5.044340
+0.020000,9.203866,7.957789,1.711247,2.078328,1.259574,-0.696863,5.058987
+0.020000,9.218448,7.918340,1.753304,2.102860,1.226618,-1.647798,5.073941
+0.020000,9.233796,7.878670,1.795840,2.126798,1.196867,-1.487589,5.089190
+0.020000,9.249929,7.838807,1.838844,2.150205,1.170378,-1.324418,5.104721
+0.020000,9.266864,7.798779,1.882307,2.173148,1.147129,-1.162458,5.120518
+0.020000,9.284615,7.758613,1.926221,2.195689,1.127092,-1.001849,5.136567
+0.020000,9.303200,7.718336,1.970579,2.217893,1.110175,-0.845866,5.152849
+0.020000,9.322633,7.677974,2.015375,2.239818,1.096259,-0.695813,5.169348
+0.020000,9.342927,7.637552,2.060606,2.261522,1.085184,-0.553707,5.186043
+0.020000,9.364097,7.597095,2.106267,2.283056,1.076710,-0.423707,5.202915
+0.020000,9.386155,7.556627,2.152356,2.304468,1.070614,-0.304797,5.219941
+0.020000,9.409111,7.516170,2.198872,2.325799,1.066536,-0.203910,5.237100
+0.020000,9.432978,7.475748,2.245814,2.347084,1.064241,-0.114751,5.254369
+0.020000,9.457763,7.435383,2.293181,2.368349,1.063277,-0.048202,5.271724
+0.020000,9.483474,7.395097,2.340973,2.389615,1.063282,0.000230,5.289142
+0.020000,9.513038,7.350540,2.394446,2.673643,14.201377,656.904744,5.300432
+0.020000,9.592974,7.217989,2.549234,7.739423,253.289020,11954.382183,5.211708
+0.020000,9.650666,7.103680,2.677276,6.402097,-66.866308,-16007.766431,5.150177
+0.020000,9.692643,7.009242,2.780624,5.167388,-61.735447,256.543090,5.113748
+0.020000,9.723434,6.935018,2.860981,4.017840,-57.477420,212.901351,5.100683
+0.020000,9.745475,6.881542,2.918821,2.892019,-56.291031,59.319412,5.110481
+0.020000,9.759166,6.850113,2.953103,1.714096,-58.896151,-130.255991,5.144157
+0.020000,9.762703,6.842653,2.961359,0.412771,-65.066223,-308.503580,5.204158
+0.020000,9.751855,6.860554,2.982289,1.046540,31.688441,4837.733175,5.293626
+0.020000,9.720712,6.901957,3.034098,2.590446,77.195275,2275.341736,5.414246
+0.020000,9.665335,6.958216,3.113039,3.947017,67.828588,-468.334397,5.562265
+0.020000,9.590194,7.014057,3.206657,4.680939,36.696055,-1556.626613,5.725196
+0.020000,9.510062,7.055662,3.296946,4.514444,-8.324716,-2251.038580,5.884968
+0.020000,9.441522,7.079063,3.369371,3.621247,-44.659880,-1816.758190,6.026995
+0.020000,9.394633,7.088617,3.417224,2.392642,-61.430240,-838.517970,6.143945
+0.020000,9.401213,7.087866,3.423848,0.331180,-103.073075,-2082.141772,6.206356
+0.020000,9.397175,7.087954,3.427887,0.201948,-6.461604,4830.573543,6.280259
+0.020000,9.379457,7.087142,3.445623,0.886840,34.244574,2035.308911,0.084738
+0.020000,9.346323,7.082496,3.479082,1.672916,39.303816,252.962123,0.188170
+0.020000,9.297884,7.070124,3.529075,2.499680,41.338208,101.719597,0.308156
+0.020000,9.237670,7.046306,3.593830,3.237716,36.901811,-221.819852,0.442893
+0.020000,9.173204,7.009792,3.667918,3.704429,23.335620,-678.309570,0.586928
+0.020000,9.113943,6.963889,3.742878,3.747999,2.178524,-1057.854822,0.731760
+0.020000,9.067176,6.915815,3.809947,3.353461,-19.726940,-1095.273191,0.868589
+0.020000,9.035360,6.873312,3.863039,2.654567,-34.944672,-760.886593,0.991297
+0.020000,9.016820,6.841596,3.899776,1.836853,-40.885712,-297.052000,1.097488
+0.020000,9.008086,6.822671,3.920619,1.042165,-39.734387,57.566267,1.187604
+0.020000,9.005701,6.816259,3.927461,0.342099,-35.003285,236.555102,1.263518
+0.020000,9.006966,6.820960,3.932330,0.243439,-4.933031,1503.512713,1.327501
+0.020000,9.010009,6.835048,3.946743,0.720633,23.859698,1439.636438,1.381697
+0.020000,9.018231,6.882545,3.994946,2.410167,84.476690,3030.849583,1.400217
+0.020000,9.030195,6.951564,4.064994,3.502401,54.611709,-1493.249034,1.398223
+0.020000,9.042077,7.019465,4.133927,3.446659,-2.787083,-2869.939590,1.397031
+0.020000,9.053818,7.086247,4.201733,3.390297,-2.818110,-1.551370,1.396628
+0.020000,9.065362,7.151907,4.268400,3.333353,-2.847218,-1.455417,1.397002
+0.020000,9.076659,7.216443,4.333917,3.275864,-2.874438,-1.360976,1.398142
+0.020000,9.087660,7.279853,4.398275,3.217869,-2.899742,-1.265231,1.400032
+0.020000,9.098320,7.342135,4.461463,3.159408,-2.923056,-1.165675,1.402659
+0.020000,9.108599,7.403288,4.523473,3.100523,-2.944252,-1.059811,1.406007
+0.020000,9.118459,7.463309,4.584299,3.041259,-2.963187,-0.946746,1.410060
+0.020000,9.127867,7.522195,4.643932,2.981666,-2.979667,-0.823973,1.414801
+0.020000,9.136793,7.579945,4.702368,2.921796,-2.993488,-0.691064,1.420211
+0.020000,9.145212,7.636557,4.759602,2.861707,-3.004448,-0.548019,1.426271
+0.020000,9.153099,7.692028,4.815631,2.801461,-3.012322,-0.393693,1.432961
+0.020000,9.160437,7.746357,4.870454,2.741122,-3.016909,-0.229346,1.440258
+0.020000,9.167210,7.799543,4.924069,2.680762,-3.018021,-0.055603,1.448139
+0.020000,9.173406,7.851584,4.976478,2.620452,-3.015496,0.126235,1.456579
+0.020000,9.179016,7.902481,5.027683,2.560268,-3.009214,0.314117,1.465553
+0.020000,9.184037,7.952235,5.077689,2.500286,-2.999090,0.506204,1.475031
+0.020000,9.188465,8.000845,5.126501,2.440584,-2.985116,0.698694,1.484986
+0.020000,9.192302,8.048315,5.174125,2.381238,-2.967301,0.890748,1.495385
+0.020000,9.195554,8.094647,5.220572,2.322322,-2.945788,1.075645,1.506196
+0.020000,9.198226,8.139847,5.265850,2.263908,-2.920703,1.254246,1.517386
+0.020000,9.200330,8.183918,5.309971,2.206061,-2.892345,1.417892,1.528920
+0.020000,9.201877,8.226873,5.352954,2.149157,-2.845169,2.358808,1.540762
+0.020000,9.202883,8.268735,5.394828,2.093694,-2.773155,3.600727,1.552881
+0.020000,9.203299,8.304301,5.430397,1.778458,-15.761800,-649.432253,1.568726
+0.020000,9.203029,8.335445,5.461542,1.557234,-11.061201,235.029961,1.589907
+0.020000,9.202103,8.367256,5.493367,1.591228,1.699694,638.044708,1.609576
+0.020000,9.200543,8.399710,5.525858,1.624574,1.667271,-1.621153,1.627745
+0.020000,9.198376,8.432773,5.558992,1.656673,1.604972,-3.114946,1.644439
+0.020000,9.195632,8.466402,5.592732,1.687047,1.518711,-4.313045,1.659693
+0.020000,9.192345,8.500551,5.627039,1.715328,1.414038,-5.233660,1.673548
+0.020000,9.188553,8.535168,5.661864,1.741241,1.295643,-5.919710,1.686051
+0.020000,9.184294,8.570202,5.697156,1.764593,1.167629,-6.400736,1.697255
+0.020000,9.179610,8.605599,5.732861,1.785260,1.033330,-6.714945,1.707213
+0.020000,9.174545,8.641305,5.768924,1.803170,0.895479,-6.892540,1.715979
+0.020000,9.169144,8.677267,5.805290,1.818294,0.756200,-6.963962,1.723610
+0.020000,9.163449,8.713434,5.841903,1.830637,0.617144,-6.952798,1.730162
+0.020000,9.157504,8.749756,5.878707,1.840227,0.479530,-6.880680,1.735688
+0.020000,9.151355,8.786182,5.915650,1.847111,0.344213,-6.765835,1.740244
+0.020000,9.145041,8.822667,5.952677,1.851347,0.211804,-6.620479,1.743882
+0.020000,9.138606,8.859164,5.989737,1.853001,0.082668,-6.456806,1.746654
+0.020000,9.132087,8.895629,6.026779,1.852140,-0.043019,-6.284325,1.748610
+0.020000,9.125523,8.932018,6.063756,1.848837,-0.165181,-6.108090,1.749799
+0.020000,9.118949,8.968291,6.100619,1.843160,-0.283840,-5.932970,1.750269
+0.020000,9.112398,9.004405,6.137323,1.835178,-0.399096,-5.762801,1.750066
+0.020000,9.105903,9.040321,6.173822,1.824956,-0.511086,-5.599492,1.749234
+0.020000,9.099492,9.076001,6.210073,1.812557,-0.619958,-5.443615,1.747817
+0.020000,9.093193,9.111406,6.246034,1.798039,-0.725887,-5.296462,1.745857
+0.020000,9.087029,9.146498,6.281663,1.781459,-0.829032,-5.157238,1.743396
+0.020000,9.081023,9.181240,6.316921,1.762868,-0.929533,-5.025067,1.740474
+0.020000,9.075195,9.215595,6.351767,1.742317,-1.027550,-4.900849,1.737130
+0.020000,9.069562,9.249528,6.386164,1.719854,-1.123163,-4.780618,1.733402
+0.020000,9.064139,9.283002,6.420074,1.695524,-1.216488,-4.666244,1.729328
+0.020000,9.058940,9.315982,6.453462,1.669373,-1.307570,-4.554122,1.724944
+0.020000,9.053974,9.348434,6.486291,1.641443,-1.396465,-4.444720,1.720285
+0.020000,9.049250,9.380321,6.518526,1.611780,-1.483178,-4.335662,1.715387
+0.020000,9.044775,9.411611,6.550135,1.580426,-1.567718,-4.227012,1.710283
+0.020000,9.040552,9.442270,6.581083,1.547425,-1.650048,-4.116490,1.705006
+0.020000,9.036585,9.472265,6.611340,1.512822,-1.730150,-4.005105,1.699587
+0.020000,9.032872,9.501564,6.640873,1.476663,-1.807948,-3.889893,1.694060
+0.020000,9.029413,9.530136,6.669653,1.438994,-1.883408,-3.773010,1.688452
+0.020000,9.026205,9.557949,6.697650,1.399866,-1.956444,-3.651820,1.682794
+0.020000,9.023242,9.584973,6.724837,1.359326,-2.026997,-3.527643,1.677114
+0.020000,9.020520,9.611181,6.751185,1.317426,-2.094988,-3.399532,1.671439
+0.020000,9.018031,9.636543,6.776670,1.274219,-2.160350,-3.268111,1.665795
+0.020000,9.015766,9.661034,6.801265,1.229758,-2.223042,-3.134607,1.660207
+0.020000,9.013716,9.684627,6.824947,1.184098,-2.282981,-2.996947,1.654700
+0.020000,9.011871,9.707298,6.847693,1.137296,-2.340108,-2.856345,1.649295
+0.020000,9.010220,9.729023,6.869481,1.089408,-2.394413,-2.715238,1.644016
+0.020000,9.008751,9.749781,6.890291,1.040490,-2.445899,-2.574297,1.638881
+0.020000,9.007452,9.769551,6.910103,0.990600,-2.494486,-2.429368,1.633912
+0.020000,9.006310,9.788323,6.928910,0.940343,-2.512884,-0.919875,1.629123
+0.020000,9.005313,9.806107,6.946721,0.890587,-2.487769,1.255722,1.624524
+0.020000,9.004446,9.822918,6.963555,0.841671,-2.445812,2.097874,1.620122
+0.020000,9.003696,9.838773,6.979428,0.793673,-2.399881,2.296523,1.615923
+0.020000,9.003052,9.853693,6.994362,0.746667,-2.350332,2.477477,1.611931
+0.020000,9.002503,9.867697,7.008376,0.700721,-2.297268,2.653190,1.608148
+0.020000,9.002036,9.880806,7.021494,0.655900,-2.241053,2.810769,1.604575
+0.020000,9.001643,9.893045,7.033739,0.612262,-2.181920,2.956633,1.601214
+0.020000,9.001314,9.904438,7.045136,0.569860,-2.120090,3.091484,1.598062
+0.020000,9.001042,9.915009,7.055711,0.528748,-2.055614,3.223798,1.595117
+0.020000,9.000817,9.924786,7.065491,0.488968,-1.989009,3.330261,1.592378
+0.020000,9.000634,9.933795,7.074502,0.450564,-1.920202,3.440367,1.589840
+0.020000,9.000486,9.942065,7.082773,0.413572,-1.849587,3.530728,1.587499
+0.020000,9.000368,9.949625,7.090334,0.378030,-1.777115,3.623602,1.585349
+0.020000,9.000275,9.956504,7.097213,0.343967,-1.703141,3.698701,1.583385
+0.020000,9.000202,9.962732,7.103442,0.311411,-1.627776,3.768264,1.581600
+0.020000,9.000146,9.968339,7.109049,0.280389,-1.551107,3.833461,1.579988
+0.020000,9.000103,9.973357,7.114068,0.250922,-1.473352,3.887724,1.578542
+0.020000,9.000072,9.977818,7.118528,0.223032,-1.394501,3.942566,1.577252
+0.020000,9.000049,9.981752,7.122463,0.196733,-1.314931,3.978476,1.576113
+0.020000,9.000032,9.985193,7.125904,0.172048,-1.234283,4.032413,1.575114
+0.020000,9.000020,9.988173,7.128884,0.148981,-1.153340,4.047165,1.574248
+0.020000,9.000013,9.990724,7.131435,0.127551,-1.071492,4.092363,1.573505
+0.020000,9.000007,9.992879,7.133590,0.107766,-0.989262,4.111519,1.572877
+0.020000,9.000004,9.994672,7.135383,0.089633,-0.906662,4.130024,1.572354
+0.020000,9.000002,9.996135,7.136846,0.073160,-0.823635,4.151352,1.571927
+0.020000,9.000001,9.997302,7.138013,0.058355,-0.740242,4.169641,1.571585
+0.020000,9.000000,9.998207,7.138917,0.045218,-0.656841,4.170029,1.571321
+0.020000,9.000000,9.998882,7.139592,0.033758,-0.573029,4.190604,1.571124
+0.020000,9.000000,9.999361,7.140072,0.023975,-0.489152,4.193850,1.570983
+0.020000,9.000000,9.999679,7.140389,0.015871,-0.405206,4.197320,1.570890
+0.020000,9.000000,9.999868,7.140578,0.009447,-0.321177,4.201420,1.570835
+0.020000,9.000000,9.999962,7.140672,0.004705,-0.237097,4.204020,1.570808
+0.020000,9.000000,9.999995,7.140705,0.001645,-0.152988,4.205467,1.570798
+0.020000,9.000000,10.000000,7.140711,0.000268,-0.068865,4.206109,1.570796
+0.020000,9.000000,10.000000,7.140711,0.000000,-0.013401,2.773201,1.570796
diff --git a/src/main/deploy/output/turn180.pf1.csv b/src/main/deploy/output/turn180.pf1.csv
new file mode 100644
index 0000000..41fa1a5
--- /dev/null
+++ b/src/main/deploy/output/turn180.pf1.csv
@@ -0,0 +1,239 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,8.061396,9.558070,0.000012,0.001190,0.059499,2.974938,4.804810
+0.020000,8.061396,9.558070,0.000059,0.003570,0.118998,2.974938,4.804789
+0.020000,8.061396,9.558070,0.000167,0.007140,0.178496,2.974938,4.804744
+0.020000,8.061396,9.558070,0.000357,0.011900,0.237995,2.974938,4.804663
+0.020000,8.061396,9.558070,0.000654,0.017850,0.297494,2.974938,4.804536
+0.020000,8.061396,9.558070,0.001083,0.024989,0.356993,2.974938,4.804354
+0.020000,8.061424,9.557768,0.001666,0.033319,0.416491,2.974938,4.804106
+0.020000,8.061493,9.557010,0.002428,0.042839,0.475990,2.974938,4.803783
+0.020000,8.061581,9.556050,0.003391,0.053549,0.535489,2.974938,4.803374
+0.020000,8.061689,9.554865,0.004581,0.065449,0.594988,2.974938,4.802871
+0.020000,8.061819,9.553431,0.006021,0.078538,0.654486,2.974938,4.802263
+0.020000,8.061972,9.551724,0.007735,0.092818,0.713985,2.974938,4.801541
+0.020000,8.062150,9.549721,0.009746,0.108288,0.773484,2.974938,4.800697
+0.020000,8.062355,9.547397,0.012078,0.124947,0.832983,2.974938,4.799722
+0.020000,8.062587,9.544730,0.014756,0.142797,0.892481,2.974938,4.798607
+0.020000,8.062847,9.541695,0.017802,0.161837,0.951980,2.974938,4.797344
+0.020000,8.063136,9.538268,0.021241,0.182066,1.011479,2.974938,4.795927
+0.020000,8.063455,9.534426,0.025097,0.203486,1.070978,2.974938,4.794348
+0.020000,8.063803,9.530144,0.029392,0.226095,1.130476,2.974938,4.792602
+0.020000,8.064180,9.525399,0.034152,0.249895,1.189975,2.974938,4.790682
+0.020000,8.064585,9.520167,0.039400,0.274884,1.249474,2.974938,4.788584
+0.020000,8.065017,9.514424,0.045160,0.301064,1.308973,2.974938,4.786304
+0.020000,8.065474,9.508145,0.051455,0.328433,1.368471,2.974938,4.783839
+0.020000,8.065954,9.501308,0.058309,0.356993,1.427970,2.974938,4.781188
+0.020000,8.066455,9.493887,0.065746,0.386742,1.487469,2.974938,4.778350
+0.020000,8.066973,9.485860,0.073790,0.417681,1.546968,2.974938,4.775326
+0.020000,8.067504,9.477201,0.082465,0.449811,1.606466,2.974938,4.772117
+0.020000,8.068045,9.467888,0.091795,0.483130,1.665965,2.974938,4.768728
+0.020000,8.068591,9.457895,0.101802,0.517639,1.725464,2.974938,4.765163
+0.020000,8.069136,9.447199,0.112512,0.553338,1.784963,2.974938,4.761429
+0.020000,8.069674,9.435776,0.123948,0.590228,1.844461,2.974938,4.757535
+0.020000,8.070199,9.423602,0.136133,0.628307,1.903960,2.974938,4.753493
+0.020000,8.070705,9.410653,0.149092,0.667576,1.963459,2.974938,4.749314
+0.020000,8.071183,9.396905,0.162848,0.708035,2.022958,2.974938,4.745014
+0.020000,8.071626,9.382335,0.177425,0.749684,2.082456,2.974938,4.740610
+0.020000,8.072026,9.366930,0.192835,0.791333,2.082456,0.000000,4.736127
+0.020000,8.072375,9.350690,0.209079,0.832983,2.082456,0.000000,4.731593
+0.020000,8.072663,9.333617,0.226155,0.874632,2.082456,0.000000,4.727038
+0.020000,8.072885,9.315709,0.244064,0.916281,2.082456,0.000000,4.722495
+0.020000,8.073032,9.296967,0.262806,0.957930,2.082456,0.000000,4.717997
+0.020000,8.073098,9.277392,0.282381,0.999579,2.082456,-0.000000,4.713578
+0.020000,8.073077,9.256984,0.302789,1.041228,2.082456,0.000000,4.709276
+0.020000,8.072967,9.235744,0.324030,1.082877,2.082456,0.000000,4.705128
+0.020000,8.072762,9.213670,0.346104,1.124526,2.082456,-0.000000,4.701173
+0.020000,8.072462,9.190765,0.369011,1.166176,2.082456,0.000000,4.697452
+0.020000,8.072066,9.167029,0.392751,1.207825,2.082456,0.000000,4.694008
+0.020000,8.071575,9.142461,0.417324,1.249474,2.082456,-0.000000,4.690883
+0.020000,8.070992,9.117061,0.442730,1.291123,2.082456,0.000000,4.688122
+0.020000,8.070324,9.090831,0.468969,1.332772,2.082456,-0.000000,4.685773
+0.020000,8.069576,9.063769,0.496041,1.374421,2.082456,0.000000,4.683882
+0.020000,8.068760,9.035876,0.523946,1.416070,2.082456,-0.000000,4.682500
+0.020000,8.067888,9.007152,0.552684,1.457719,2.082456,0.000000,4.681677
+0.020000,8.066975,8.977595,0.582255,1.499369,2.082456,-0.000000,4.681465
+0.020000,8.066041,8.947205,0.612659,1.541018,2.082456,0.000000,4.681921
+0.020000,8.065105,8.915983,0.643896,1.582667,2.082456,0.000000,4.683098
+0.020000,8.064196,8.883926,0.675965,1.624316,2.082456,0.000000,4.685057
+0.020000,8.063340,8.851034,0.708868,1.665965,2.082456,-0.000000,4.687856
+0.020000,8.062573,8.817307,0.742604,1.707614,2.082456,0.000000,4.691557
+0.020000,8.061931,8.782744,0.777173,1.749263,2.082456,0.000000,4.696226
+0.020000,8.061457,8.747346,0.812574,1.790912,2.082456,-0.000000,4.701927
+0.020000,8.061198,8.711112,0.848809,1.832562,2.082456,0.000000,4.708727
+0.020000,8.061207,8.674044,0.885877,1.874211,2.082456,-0.000000,4.716697
+0.020000,8.061541,8.636145,0.923778,1.915860,2.082456,0.000000,4.725905
+0.020000,8.062265,8.597418,0.962511,1.957509,2.082456,-0.000000,4.736423
+0.020000,8.063447,8.557870,1.002078,1.999158,2.082456,-0.000000,4.748319
+0.020000,8.065163,8.517507,1.042478,2.040807,2.082456,0.000000,4.761663
+0.020000,8.067496,8.476341,1.083710,2.082456,2.082456,-0.000000,4.776520
+0.020000,8.070532,8.434385,1.125776,2.124106,2.082456,0.000000,4.792949
+0.020000,8.074365,8.391659,1.168675,2.165755,2.082456,0.000000,4.811005
+0.020000,8.079095,8.348184,1.212406,2.207404,2.082456,-0.000000,4.830730
+0.020000,8.084826,8.303991,1.256971,2.249053,2.082456,0.000000,4.852156
+0.020000,8.091665,8.259112,1.302368,2.290702,2.082456,-0.000000,4.875296
+0.020000,8.099724,8.213591,1.348599,2.332351,2.082456,0.000000,4.900146
+0.020000,8.108639,8.168953,1.395662,2.374000,2.082456,0.000000,4.915678
+0.020000,8.118576,8.122099,1.443559,2.415649,2.082456,-0.000000,4.927088
+0.020000,8.129236,8.074550,1.492288,2.457299,2.082456,0.000000,4.938817
+0.020000,8.140653,8.026321,1.541851,2.498948,2.082456,0.000000,4.950867
+0.020000,8.152860,7.977426,1.592246,2.540597,2.082456,0.000000,4.963241
+0.020000,8.165891,7.927883,1.643475,2.582246,2.082456,0.000000,4.975941
+0.020000,8.179780,7.877709,1.695536,2.623895,2.082456,-0.000000,4.988968
+0.020000,8.194562,7.826923,1.748430,2.665544,2.082456,0.000000,5.002322
+0.020000,8.210273,7.775544,1.802158,2.707193,2.082456,0.000000,5.016003
+0.020000,8.226949,7.723595,1.856718,2.748842,2.082456,-0.000000,5.030010
+0.020000,8.244625,7.671098,1.912111,2.790492,2.082456,0.000000,5.044340
+0.020000,8.263332,7.618088,1.968326,2.830951,2.022958,-2.974938,5.058987
+0.020000,8.283099,7.564614,2.025338,2.870220,1.963459,-2.974938,5.073941
+0.020000,8.303950,7.510722,2.083123,2.908299,1.903960,-2.974938,5.089190
+0.020000,8.325909,7.456463,2.141658,2.945188,1.844461,-2.974938,5.104721
+0.020000,8.348999,7.401886,2.200918,2.980888,1.784963,-2.974938,5.120518
+0.020000,8.373238,7.347041,2.260881,3.015397,1.725464,-2.974938,5.136567
+0.020000,8.398644,7.291980,2.321522,3.048716,1.665965,-2.974938,5.152849
+0.020000,8.425234,7.236753,2.382818,3.080845,1.606466,-2.974938,5.169348
+0.020000,8.453020,7.181411,2.444744,3.111785,1.546968,-2.974938,5.186043
+0.020000,8.482012,7.126005,2.507277,3.141534,1.487469,-2.974938,5.202915
+0.020000,8.512218,7.070587,2.570394,3.170094,1.427970,-2.974938,5.219941
+0.020000,8.543643,7.015207,2.634069,3.197463,1.368471,-2.974938,5.237100
+0.020000,8.576289,6.959915,2.698280,3.223642,1.308973,-2.974938,5.254369
+0.020000,8.610155,6.904760,2.763003,3.248632,1.249474,-2.974938,5.271724
+0.020000,8.645237,6.849792,2.828214,3.272431,1.189975,-2.974938,5.289142
+0.020000,8.681011,6.795806,2.893888,3.295041,1.130476,-2.974938,5.300432
+0.020000,8.715065,6.739161,2.960003,3.316461,1.070978,-2.974938,5.211708
+0.020000,8.744974,6.679744,3.026535,3.336690,1.011479,-2.974938,5.150177
+0.020000,8.772112,6.618573,3.093459,3.355730,0.951980,-2.974938,5.113748
+0.020000,8.797877,6.556408,3.160752,3.373579,0.892481,-2.974938,5.100683
+0.020000,8.823672,6.493882,3.228390,3.390239,0.832983,-2.974938,5.110481
+0.020000,8.850939,6.431636,3.296350,3.405709,0.773484,-2.974938,5.144157
+0.020000,8.881204,6.370467,3.364607,3.419988,0.713985,-2.974938,5.204158
+0.020000,8.916071,6.311495,3.433138,3.433078,0.654486,-2.974938,5.293626
+0.020000,8.957067,6.256320,3.501918,3.444978,0.594988,-2.974938,5.414246
+0.020000,9.005259,6.207017,3.570925,3.455688,0.535489,-2.974938,5.562265
+0.020000,9.060713,6.165736,3.640134,3.465207,0.475990,-2.974938,5.725196
+0.020000,9.122286,6.133908,3.709521,3.473537,0.416491,-2.974938,5.884968
+0.020000,9.188125,6.111700,3.779063,3.480677,0.356993,-2.974938,6.026995
+0.020000,9.255842,6.098295,3.848736,3.486627,0.297494,-2.974938,6.143945
+0.020000,9.324460,6.090816,3.918517,3.491387,0.237995,-2.974938,6.206356
+0.020000,9.394249,6.087958,3.988380,3.494957,0.178496,-2.974938,6.280259
+0.020000,9.464094,6.090731,4.058303,3.497337,0.118998,-2.974938,0.084738
+0.020000,9.533385,6.100148,4.128262,3.498527,0.059499,-2.974938,0.188170
+0.020000,9.601186,6.117230,4.198224,3.497716,-0.040542,-5.002059,0.308156
+0.020000,9.666225,6.142790,4.268158,3.495715,-0.100041,-2.974938,0.442893
+0.020000,9.727010,6.177146,4.338041,3.492524,-0.159540,-2.974938,0.586928
+0.020000,9.782123,6.219889,4.407847,3.488143,-0.219039,-2.974938,0.731760
+0.020000,9.830594,6.269910,4.477554,3.482573,-0.278537,-2.974938,0.868589
+0.020000,9.872097,6.325707,4.547138,3.475812,-0.338036,-2.974938,0.991297
+0.020000,9.906886,6.385763,4.616575,3.467861,-0.397535,-2.974938,1.097488
+0.020000,9.935562,6.448788,4.685841,3.458721,-0.457034,-2.974938,1.187604
+0.020000,9.958861,6.513793,4.754912,3.448390,-0.516532,-2.974938,1.263518
+0.020000,9.977515,6.580058,4.823765,3.436869,-0.576031,-2.974938,1.327501
+0.020000,9.992183,6.647074,4.892375,3.424159,-0.635530,-2.974938,1.381697
+0.020000,10.003717,6.712792,4.960719,3.410258,-0.695029,-2.974938,1.400217
+0.020000,10.015341,6.779846,5.028773,3.395168,-0.754527,-2.974938,1.398223
+0.020000,10.027017,6.846572,5.096514,3.378887,-0.814026,-2.974938,1.397031
+0.020000,10.038689,6.912957,5.163917,3.361417,-0.873525,-2.974938,1.396628
+0.020000,10.050298,6.978986,5.230959,3.342756,-0.933024,-2.974938,1.397002
+0.020000,10.061791,7.044645,5.297615,3.322906,-0.992522,-2.974938,1.398142
+0.020000,10.073115,7.109917,5.363863,3.301865,-1.052021,-2.974938,1.400032
+0.020000,10.084218,7.174789,5.429678,3.279635,-1.111520,-2.974938,1.402659
+0.020000,10.095051,7.239243,5.495036,3.256214,-1.171019,-2.974938,1.406007
+0.020000,10.105569,7.303263,5.559915,3.231604,-1.230517,-2.974938,1.410060
+0.020000,10.115724,7.366831,5.624289,3.205804,-1.290016,-2.974938,1.414801
+0.020000,10.125477,7.429928,5.688135,3.178814,-1.349515,-2.974938,1.420211
+0.020000,10.134786,7.492534,5.751429,3.150633,-1.409014,-2.974938,1.426271
+0.020000,10.143615,7.554628,5.814148,3.121263,-1.468513,-2.974938,1.432961
+0.020000,10.151929,7.616189,5.876268,3.090703,-1.528011,-2.974938,1.440258
+0.020000,10.159697,7.677193,5.937764,3.058953,-1.587510,-2.974938,1.448139
+0.020000,10.166890,7.737616,5.998614,3.026012,-1.647009,-2.974938,1.456579
+0.020000,10.173483,7.797432,6.058793,2.991882,-1.706508,-2.974938,1.465553
+0.020000,10.179455,7.856616,6.118278,2.956562,-1.766006,-2.974938,1.475031
+0.020000,10.184785,7.915140,6.177044,2.920052,-1.825505,-2.974938,1.484986
+0.020000,10.189460,7.972975,6.235068,2.882352,-1.885004,-2.974938,1.495385
+0.020000,10.193468,8.030092,6.292326,2.843462,-1.944503,-2.974938,1.506196
+0.020000,10.196801,8.086462,6.348794,2.803382,-2.004001,-2.974938,1.517386
+0.020000,10.199453,8.142053,6.404449,2.762112,-2.063500,-2.974938,1.528920
+0.020000,10.201426,8.196843,6.459275,2.720463,-2.082456,-0.947816,1.540762
+0.020000,10.202723,8.250820,6.513268,2.678814,-2.082456,0.000000,1.552881
+0.020000,10.203297,8.302231,6.566428,2.637164,-2.082456,-0.000000,1.568726
+0.020000,10.202846,8.354555,6.618754,2.595515,-2.082456,0.000000,1.589907
+0.020000,10.201351,8.406026,6.670248,2.553866,-2.082456,0.000000,1.609576
+0.020000,10.198922,8.456628,6.720909,2.512217,-2.082456,-0.000000,1.627745
+0.020000,10.195666,8.506349,6.770737,2.470568,-2.082456,0.000000,1.644439
+0.020000,10.191684,8.555181,6.819732,2.428919,-2.082456,0.000000,1.659693
+0.020000,10.187071,8.603121,6.867894,2.387270,-2.082456,0.000000,1.673548
+0.020000,10.181918,8.650168,6.915222,2.345621,-2.082456,0.000000,1.686051
+0.020000,10.176308,8.696324,6.961718,2.303971,-2.082456,0.000000,1.697255
+0.020000,10.170320,8.741593,7.007381,2.262322,-2.082456,-0.000000,1.707213
+0.020000,10.164025,8.785978,7.052211,2.220673,-2.082456,0.000000,1.715979
+0.020000,10.157490,8.829487,7.096208,2.179024,-2.082456,-0.000000,1.723610
+0.020000,10.150777,8.872126,7.139372,2.137375,-2.082456,0.000000,1.730162
+0.020000,10.143941,8.913901,7.181703,2.095726,-2.082456,-0.000000,1.735688
+0.020000,10.137033,8.954820,7.223201,2.054077,-2.082456,-0.000000,1.740244
+0.020000,10.130099,8.994890,7.263866,2.012428,-2.082456,0.000000,1.743882
+0.020000,10.123183,9.034117,7.303698,1.970778,-2.082456,-0.000000,1.746654
+0.020000,10.116320,9.072507,7.342697,1.929129,-2.082456,0.000000,1.748610
+0.020000,10.109544,9.110067,7.380864,1.887480,-2.082456,0.000000,1.749799
+0.020000,10.102886,9.146802,7.418197,1.845831,-2.082456,-0.000000,1.750269
+0.020000,10.096372,9.182716,7.454697,1.804182,-2.082456,0.000000,1.750066
+0.020000,10.090025,9.217814,7.490364,1.762533,-2.082456,0.000000,1.749234
+0.020000,10.083865,9.252099,7.525198,1.720884,-2.082456,-0.000000,1.747817
+0.020000,10.077909,9.285574,7.559199,1.679235,-2.082456,0.000000,1.745857
+0.020000,10.072171,9.318242,7.592367,1.637585,-2.082456,-0.000000,1.743396
+0.020000,10.066663,9.350105,7.624703,1.595936,-2.082456,0.000000,1.740474
+0.020000,10.061394,9.381163,7.656205,1.554287,-2.082456,-0.000000,1.737130
+0.020000,10.056371,9.411419,7.686874,1.512638,-2.082456,0.000000,1.733402
+0.020000,10.051600,9.440871,7.716710,1.470989,-2.082456,0.000000,1.729328
+0.020000,10.047083,9.469520,7.745714,1.429340,-2.082456,-0.000000,1.724944
+0.020000,10.042821,9.497366,7.773884,1.387691,-2.082456,0.000000,1.720285
+0.020000,10.038815,9.524408,7.801221,1.346042,-2.082456,0.000000,1.715387
+0.020000,10.035063,9.550646,7.827726,1.304392,-2.082456,0.000000,1.710283
+0.020000,10.031560,9.576077,7.853397,1.262743,-2.082456,-0.000000,1.705006
+0.020000,10.028302,9.600701,7.878235,1.221094,-2.082456,0.000000,1.699587
+0.020000,10.025284,9.624516,7.902241,1.179445,-2.082456,0.000000,1.694060
+0.020000,10.022499,9.647520,7.925413,1.137796,-2.082456,-0.000000,1.688452
+0.020000,10.019939,9.669712,7.947753,1.096147,-2.082456,0.000000,1.682794
+0.020000,10.017596,9.691091,7.969259,1.054498,-2.082456,0.000000,1.677114
+0.020000,10.015460,9.711654,7.989933,1.012848,-2.082456,0.000000,1.671439
+0.020000,10.013522,9.731399,8.009773,0.971199,-2.082456,-0.000000,1.665795
+0.020000,10.011772,9.750326,8.028781,0.929550,-2.082456,0.000000,1.660207
+0.020000,10.010199,9.768432,8.046955,0.887901,-2.082456,-0.000000,1.654700
+0.020000,10.008792,9.785716,8.064297,0.846252,-2.082456,0.000000,1.649295
+0.020000,10.007540,9.802177,8.080805,0.804603,-2.082456,-0.000000,1.644016
+0.020000,10.006434,9.817814,8.096481,0.762954,-2.082456,0.000000,1.638881
+0.020000,10.005461,9.832625,8.111323,0.721305,-2.082456,0.000000,1.633912
+0.020000,10.004610,9.846616,8.125341,0.680466,-2.041914,2.027121,1.629123
+0.020000,10.003870,9.859808,8.138554,0.640818,-1.982415,2.974938,1.624524
+0.020000,10.003230,9.872224,8.150986,0.602360,-1.922916,2.974938,1.620122
+0.020000,10.002678,9.883885,8.162660,0.565091,-1.863418,2.974938,1.615923
+0.020000,10.002207,9.894816,8.173601,0.529013,-1.803919,2.974938,1.611931
+0.020000,10.001805,9.905040,8.183833,0.494125,-1.744420,2.974938,1.608148
+0.020000,10.001466,9.914579,8.193378,0.460426,-1.684921,2.974938,1.604575
+0.020000,10.001181,9.923458,8.202261,0.427918,-1.625423,2.974938,1.601214
+0.020000,10.000943,9.931700,8.210507,0.396599,-1.565924,2.974938,1.598062
+0.020000,10.000746,9.939328,8.218137,0.366471,-1.506425,2.974938,1.595117
+0.020000,10.000584,9.946366,8.225177,0.337532,-1.446926,2.974938,1.592378
+0.020000,10.000453,9.952838,8.231651,0.309784,-1.387428,2.974938,1.589840
+0.020000,10.000347,9.958767,8.237581,0.283225,-1.327929,2.974938,1.587499
+0.020000,10.000262,9.964177,8.242991,0.257856,-1.268430,2.974938,1.585349
+0.020000,10.000196,9.969092,8.247907,0.233678,-1.208931,2.974938,1.583385
+0.020000,10.000144,9.973535,8.252350,0.210689,-1.149433,2.974938,1.581600
+0.020000,10.000104,9.977531,8.256346,0.188890,-1.089934,2.974938,1.579988
+0.020000,10.000073,9.981102,8.259918,0.168282,-1.030435,2.974938,1.578542
+0.020000,10.000051,9.984274,8.263089,0.148863,-0.970936,2.974938,1.577252
+0.020000,10.000034,9.987069,8.265884,0.130634,-0.911438,2.974938,1.576113
+0.020000,10.000023,9.989511,8.268327,0.113596,-0.851939,2.974938,1.575114
+0.020000,10.000014,9.991624,8.270440,0.097747,-0.792440,2.974938,1.574248
+0.020000,10.000009,9.993433,8.272248,0.083088,-0.732941,2.974938,1.573505
+0.020000,10.000005,9.994960,8.273776,0.069619,-0.673443,2.974938,1.572877
+0.020000,10.000003,9.996229,8.275045,0.057340,-0.613944,2.974938,1.572354
+0.020000,10.000002,9.997265,8.276081,0.046251,-0.554445,2.974938,1.571927
+0.020000,10.000001,9.998091,8.276907,0.036352,-0.494946,2.974938,1.571585
+0.020000,10.000000,9.998731,8.277547,0.027643,-0.435448,2.974938,1.571321
+0.020000,10.000000,9.999209,8.278025,0.020124,-0.375949,2.974938,1.571124
+0.020000,10.000000,9.999548,8.278364,0.013795,-0.316450,2.974938,1.570983
+0.020000,10.000000,9.999773,8.278588,0.008656,-0.256951,2.974938,1.570890
+0.020000,10.000000,9.999906,8.278722,0.004707,-0.197453,2.974938,1.570835
+0.020000,10.000000,9.999973,8.278789,0.001948,-0.137954,2.974938,1.570808
+0.020000,10.000000,9.999996,8.278812,0.000379,-0.078455,2.974938,1.570798
+0.020000,10.000000,10.000000,8.278816,0.000000,-0.018956,2.974938,1.570796
+0.020000,10.000000,10.000000,8.278816,0.000000,0.000000,0.947816,1.570796
diff --git a/src/main/deploy/output/turn180.right.pf1.csv b/src/main/deploy/output/turn180.right.pf1.csv
new file mode 100644
index 0000000..5547779
--- /dev/null
+++ b/src/main/deploy/output/turn180.right.pf1.csv
@@ -0,0 +1,239 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,7.065664,9.465781,0.000012,0.001190,0.059499,2.974938,4.804810
+0.020000,7.065662,9.465801,0.000032,0.001014,-0.008817,-3.415786,4.804789
+0.020000,7.065658,9.465847,0.000078,0.002280,0.063337,3.607720,4.804744
+0.020000,7.065650,9.465927,0.000159,0.004053,0.088631,1.264679,4.804663
+0.020000,7.065638,9.466053,0.000285,0.006330,0.113861,1.261503,4.804536
+0.020000,7.065622,9.466235,0.000468,0.009110,0.138987,1.256302,4.804354
+0.020000,7.065627,9.466179,0.000523,0.002789,-0.316041,-22.751416,4.804106
+0.020000,7.065667,9.465743,0.000962,0.021915,0.956317,63.617916,4.803783
+0.020000,7.065717,9.465190,0.001517,0.027766,0.292532,-33.189246,4.803374
+0.020000,7.065780,9.464506,0.002204,0.034324,0.327906,1.768694,4.802871
+0.020000,7.065855,9.463678,0.003036,0.041599,0.363744,1.791901,4.802263
+0.020000,7.065943,9.462689,0.004028,0.049602,0.400130,1.819298,4.801541
+0.020000,7.066047,9.461527,0.005194,0.058345,0.437184,1.852705,4.800697
+0.020000,7.066166,9.460175,0.006551,0.067846,0.475033,1.892445,4.799722
+0.020000,7.066301,9.458619,0.008114,0.078122,0.513799,1.938314,4.798607
+0.020000,7.066454,9.456842,0.009898,0.089195,0.553631,1.991584,4.797344
+0.020000,7.066624,9.454827,0.011920,0.101089,0.594703,2.053599,4.795927
+0.020000,7.066812,9.452558,0.014196,0.113832,0.637145,2.122093,4.794348
+0.020000,7.067018,9.450017,0.016745,0.127455,0.681179,2.201728,4.792602
+0.020000,7.067243,9.447186,0.019585,0.141995,0.726971,2.289604,4.790682
+0.020000,7.067486,9.444046,0.022735,0.157490,0.774750,2.388921,4.788584
+0.020000,7.067747,9.440576,0.026215,0.173984,0.824707,2.497850,4.786304
+0.020000,7.068025,9.436756,0.030045,0.191525,0.877085,2.618880,4.783839
+0.020000,7.068320,9.432563,0.034248,0.210168,0.932129,2.752233,4.781188
+0.020000,7.068629,9.427974,0.038848,0.229970,0.990096,2.898356,4.778350
+0.020000,7.068953,9.422964,0.043868,0.250995,1.051244,3.057369,4.775326
+0.020000,7.069288,9.417508,0.049334,0.273312,1.115865,3.231071,4.772117
+0.020000,7.069632,9.411578,0.055274,0.296997,1.184235,3.418481,4.768728
+0.020000,7.069983,9.405145,0.061717,0.322130,1.256665,3.621501,4.765163
+0.020000,7.070338,9.398178,0.068693,0.348799,1.333463,3.839905,4.761429
+0.020000,7.070693,9.390645,0.076235,0.377098,1.414938,4.073761,4.757535
+0.020000,7.071044,9.382510,0.084377,0.407127,1.501428,4.324489,4.753493
+0.020000,7.071386,9.373737,0.093157,0.438992,1.593252,4.591208,4.749314
+0.020000,7.071715,9.364286,0.102613,0.472807,1.690746,4.874720,4.745014
+0.020000,7.072024,9.354117,0.112787,0.508692,1.794253,5.175351,4.740610
+0.020000,7.072308,9.343194,0.123714,0.546349,1.882842,4.429440,4.736127
+0.020000,7.072559,9.331487,0.135423,0.585450,1.955051,3.610449,4.731593
+0.020000,7.072771,9.318968,0.147944,0.626072,2.031134,3.804117,4.727038
+0.020000,7.072936,9.305603,0.161310,0.668293,2.111034,3.995025,4.722495
+0.020000,7.073047,9.291360,0.175554,0.712187,2.194700,4.183318,4.717997
+0.020000,7.073098,9.276203,0.190710,0.757828,2.282066,4.368283,4.713578
+0.020000,7.073082,9.260097,0.206816,0.805290,2.373086,4.551010,4.709276
+0.020000,7.072993,9.243005,0.223909,0.854644,2.467702,4.730782,4.705128
+0.020000,7.072825,9.224886,0.242028,0.905962,2.565871,4.908446,4.701173
+0.020000,7.072573,9.205702,0.261215,0.959313,2.667578,5.085362,4.697452
+0.020000,7.072234,9.185409,0.281510,1.014769,2.772798,5.261009,4.694008
+0.020000,7.071806,9.163965,0.302958,1.072400,2.881557,5.437936,4.690883
+0.020000,7.071287,9.141326,0.325604,1.132278,2.993882,5.616246,4.688122
+0.020000,7.070678,9.117444,0.349493,1.194475,3.109843,5.798063,4.685773
+0.020000,7.069983,9.092272,0.374674,1.259065,3.229529,5.984278,4.683882
+0.020000,7.069207,9.065761,0.401197,1.326127,3.353063,6.176738,4.682500
+0.020000,7.068360,9.037859,0.429112,1.395738,3.480586,6.376139,4.681677
+0.020000,7.067453,9.008514,0.458471,1.467984,3.612277,6.584522,4.681465
+0.020000,7.066505,8.977669,0.489330,1.542950,3.748307,6.801511,4.681921
+0.020000,7.065534,8.945269,0.521745,1.620727,3.888863,7.027820,4.683098
+0.020000,7.064569,8.911255,0.555773,1.701410,4.034117,7.262704,4.685057
+0.020000,7.063641,8.875565,0.591475,1.785093,4.184191,7.503659,4.687856
+0.020000,7.062790,8.838137,0.628912,1.871877,4.339163,7.748617,4.691557
+0.020000,7.062061,8.798907,0.668150,1.961856,4.498977,7.990724,4.696226
+0.020000,7.061511,8.757808,0.709252,2.055125,4.663464,8.224340,4.701927
+0.020000,7.061205,8.714774,0.752287,2.151769,4.832201,8.436854,4.708727
+0.020000,7.061216,8.669736,0.797325,2.251859,5.004497,8.614800,4.716697
+0.020000,7.061632,8.622629,0.844434,2.355446,5.179320,8.741147,4.725905
+0.020000,7.062553,8.573387,0.893685,2.462548,5.355113,8.789650,4.736423
+0.020000,7.064092,8.521947,0.945147,2.573144,5.529783,8.733468,4.748319
+0.020000,7.066377,8.468253,0.998890,2.687154,5.700502,8.535961,4.761663
+0.020000,7.069552,8.412254,1.054979,2.804426,5.863614,8.155612,4.776520
+0.020000,7.073775,8.353912,1.113473,2.924717,6.014560,7.547301,4.792949
+0.020000,7.079224,8.293203,1.174427,3.047671,6.147670,6.655479,4.811005
+0.020000,7.086089,8.230119,1.237883,3.172797,6.256325,5.432779,4.830730
+0.020000,7.094577,8.164679,1.303872,3.299452,6.332768,3.822161,4.852156
+0.020000,7.104905,8.096925,1.372408,3.426823,6.368527,1.787921,4.875296
+0.020000,7.117299,8.026935,1.443487,3.553915,6.354603,-0.696170,4.900146
+0.020000,7.129231,7.967061,1.504538,3.052575,-25.067023,-1571.081333,4.915678
+0.020000,7.141535,7.909045,1.563845,2.965330,-4.362227,1035.239815,4.927088
+0.020000,7.154762,7.850052,1.624303,3.022900,2.878474,362.035045,4.938817
+0.020000,7.168955,7.790096,1.685915,3.080616,2.885827,0.367671,4.950867
+0.020000,7.184159,7.729196,1.748684,3.138456,2.892018,0.309526,4.963241
+0.020000,7.200420,7.667371,1.812612,3.196389,2.896614,0.229826,4.975941
+0.020000,7.217784,7.604643,1.877699,3.254380,2.899564,0.147488,4.988968
+0.020000,7.236299,7.541035,1.943947,3.312394,2.900695,0.056531,5.002322
+0.020000,7.256011,7.476573,2.011355,3.370391,2.899827,-0.043369,5.016003
+0.020000,7.276968,7.411288,2.079922,3.428325,2.896717,-0.155531,5.030010
+0.020000,7.299216,7.345210,2.149645,3.486150,2.891243,-0.273681,5.044340
+0.020000,7.322799,7.278388,2.220506,3.543064,2.845717,-2.276302,5.058987
+0.020000,7.347750,7.210887,2.292471,3.598257,2.759666,-4.302570,5.073941
+0.020000,7.374104,7.142774,2.365504,3.651665,2.670394,-4.463600,5.089190
+0.020000,7.401889,7.074119,2.439569,3.703223,2.577904,-4.624512,5.104721
+0.020000,7.431134,7.004993,2.514626,3.752866,2.482153,-4.787514,5.120518
+0.020000,7.461860,6.935470,2.590637,3.800530,2.383204,-4.947469,5.136567
+0.020000,7.494089,6.865624,2.667560,3.846153,2.281118,-5.104276,5.152849
+0.020000,7.527836,6.795531,2.745353,3.889674,2.176034,-5.254203,5.169348
+0.020000,7.563113,6.725269,2.823974,3.931036,2.068142,-5.394633,5.186043
+0.020000,7.599927,6.654915,2.903378,3.970188,1.957607,-5.526723,5.202915
+0.020000,7.638281,6.584548,2.983519,4.007083,1.844750,-5.642838,5.219941
+0.020000,7.678174,6.514244,3.064353,4.041679,1.729760,-5.749518,5.237100
+0.020000,7.719600,6.444082,3.145832,4.073942,1.613143,-5.830848,5.254369
+0.020000,7.762547,6.374137,3.227909,4.103844,1.495114,-5.901444,5.271724
+0.020000,7.806999,6.304487,3.310536,4.131366,1.376114,-5.950020,5.289142
+0.020000,7.848983,6.241072,3.386589,3.802631,-16.436752,-890.643291,5.300432
+0.020000,7.837156,6.260334,3.409191,1.130136,-133.624775,-5859.401159,5.211708
+0.020000,7.839283,6.255807,3.414193,0.250100,-44.001753,4481.151103,5.150177
+0.020000,7.851581,6.227903,3.444687,1.524666,63.728293,5386.502310,5.113748
+0.020000,7.872321,6.177798,3.498915,2.711421,59.337748,-219.527238,5.100683
+0.020000,7.901870,6.106222,3.576351,3.871772,58.017538,-66.010529,5.110481
+0.020000,7.942712,6.013159,3.677981,5.081535,60.488128,123.529537,5.144157
+0.020000,7.999705,5.898280,3.806220,6.411961,66.521322,301.659686,5.204158
+0.020000,8.080287,5.762437,3.964166,7.897265,74.265175,387.192625,5.293626
+0.020000,8.193423,5.610683,4.153452,9.464289,78.351206,204.301590,5.414246
+0.020000,8.345183,5.455819,4.370278,10.841345,68.852800,-474.920323,5.562265
+0.020000,8.531231,5.317415,4.602161,11.594143,37.639939,-1560.643064,5.725196
+0.020000,8.734511,5.212155,4.831077,11.445778,-7.418271,-2252.910480,5.884968
+0.020000,8.934729,5.144338,5.042469,10.569583,-43.809763,-1819.574627,6.026995
+0.020000,9.117051,5.107973,5.228382,9.295690,-63.694620,-994.242850,6.143945
+0.020000,9.247706,5.093766,5.359807,6.571244,-136.222318,-3626.384856,6.206356
+0.020000,9.391322,5.087963,5.503541,7.186669,30.771272,8349.679461,6.280259
+0.020000,9.548731,5.094319,5.661078,7.876867,34.509891,186.930945,0.084738
+0.020000,9.720447,5.117800,5.834391,8.665655,39.439372,246.474094,0.188170
+0.020000,9.904488,5.164335,6.024225,9.491709,41.302695,93.166146,0.308156
+0.020000,10.094781,5.239275,6.228742,10.225835,36.706337,-229.817919,0.442893
+0.020000,10.280816,5.344501,6.442474,10.686615,23.038963,-683.368721,0.586928
+0.020000,10.450303,5.475890,6.656925,10.722549,1.796724,-1062.111906,0.731760
+0.020000,10.594012,5.624006,6.863300,10.318723,-20.191305,-1099.401460,0.868589
+0.020000,10.708834,5.778102,7.055471,9.608569,-35.507706,-765.820085,0.991297
+0.020000,10.796951,5.929929,7.231016,8.777255,-41.565675,-302.898410,1.097488
+0.020000,10.863037,6.074905,7.390344,7.966382,-40.543646,51.101406,1.187604
+0.020000,10.912022,6.211328,7.535294,7.247527,-35.942769,230.043890,1.263518
+0.020000,10.948065,6.339157,7.668108,6.640663,-30.343223,279.977272,1.327501
+0.020000,10.974357,6.459100,7.790899,6.139558,-25.055221,264.400094,1.381697
+0.020000,10.989204,6.543038,7.876140,4.262070,-93.874407,-3440.959262,1.400217
+0.020000,11.000487,6.608128,7.942201,3.303024,-47.952308,2296.104909,1.398223
+0.020000,11.011958,6.673680,8.008749,3.327395,1.218565,2458.543686,1.397031
+0.020000,11.023559,6.739668,8.075749,3.350007,1.130577,-4.399419,1.396628
+0.020000,11.035234,6.806066,8.143165,3.370820,1.040670,-4.495351,1.397002
+0.020000,11.046923,6.872847,8.210961,3.389798,0.948876,-4.589715,1.398142
+0.020000,11.058570,6.939982,8.279099,3.406901,0.855167,-4.685422,1.400032
+0.020000,11.070116,7.007443,8.347541,3.422090,0.759467,-4.785025,1.402659
+0.020000,11.081504,7.075199,8.416247,3.435324,0.661660,-4.890320,1.406007
+0.020000,11.092678,7.143218,8.485178,3.446555,0.561579,-5.004066,1.410060
+0.020000,11.103582,7.211467,8.554293,3.455736,0.459051,-5.126415,1.414801
+0.020000,11.114160,7.279911,8.623549,3.462814,0.353873,-5.258874,1.420211
+0.020000,11.124360,7.348511,8.692904,3.467730,0.245821,-5.402590,1.426271
+0.020000,11.134131,7.417229,8.762313,3.470424,0.134695,-5.556338,1.432961
+0.020000,11.143421,7.486021,8.831729,3.470830,0.020281,-5.720686,1.440258
+0.020000,11.152184,7.554843,8.901107,3.468877,-0.097608,-5.894428,1.448139
+0.020000,11.160374,7.623647,8.970397,3.464495,-0.219129,-6.076087,1.456579
+0.020000,11.167950,7.692383,9.039549,3.457607,-0.344411,-6.264085,1.465553
+0.020000,11.174873,7.760997,9.108511,3.448136,-0.473522,-6.455532,1.475031
+0.020000,11.181106,7.829434,9.177232,3.436006,-0.606507,-6.649253,1.484986
+0.020000,11.186618,7.897634,9.245654,3.421140,-0.743290,-6.839138,1.495385
+0.020000,11.191382,7.965537,9.313724,3.403464,-0.883823,-7.026651,1.506196
+0.020000,11.195375,8.033077,9.381382,3.382907,-1.027863,-7.202018,1.517386
+0.020000,11.198577,8.100189,9.448570,3.359402,-1.175243,-7.369001,1.528920
+0.020000,11.200975,8.166813,9.515237,3.333385,-1.300823,-6.279007,1.540762
+0.020000,11.202562,8.232905,9.581348,3.305548,-1.391835,-4.550602,1.552881
+0.020000,11.203295,8.300160,9.648608,3.362957,2.870425,213.113007,1.568726
+0.020000,11.202663,8.373665,9.722115,3.675348,15.619529,637.455185,1.589907
+0.020000,11.200599,8.444796,9.793276,3.558070,-5.863853,-1074.169087,1.609576
+0.020000,11.197301,8.513546,9.862105,3.441439,-5.831570,1.614161,1.627745
+0.020000,11.192955,8.579925,9.928626,3.326054,-5.769247,3.116158,1.644439
+0.020000,11.187735,8.643960,9.992874,3.212391,-5.683137,4.305488,1.659693
+0.020000,11.181797,8.705692,10.054890,3.100822,-5.578476,5.233049,1.673548
+0.020000,11.175283,8.765168,10.114722,2.991618,-5.460178,5.914898,1.686051
+0.020000,11.168323,8.822447,10.172422,2.884974,-5.332210,6.398380,1.697255
+0.020000,11.161029,8.877587,10.228042,2.781015,-5.197975,6.711773,1.707213
+0.020000,11.153504,8.930652,10.281638,2.679811,-5.060159,6.890780,1.715979
+0.020000,11.145837,8.981707,10.333266,2.581393,-4.920929,6.961520,1.723610
+0.020000,11.138105,9.030817,10.382981,2.485755,-4.781907,6.951075,1.730162
+0.020000,11.130377,9.078047,10.430839,2.392868,-4.644315,6.879607,1.735688
+0.020000,11.122711,9.123458,10.476893,2.302688,-4.509044,6.763540,1.740244
+0.020000,11.115158,9.167112,10.521196,2.215154,-4.376657,6.619371,1.743882
+0.020000,11.107759,9.209069,10.563800,2.130204,-4.247523,6.456721,1.746654
+0.020000,11.100552,9.249385,10.604755,2.047767,-4.121858,6.283232,1.748610
+0.020000,11.093566,9.288116,10.644110,1.967772,-3.999716,6.107079,1.749799
+0.020000,11.086824,9.325313,10.681913,1.890151,-3.881064,5.932639,1.750269
+0.020000,11.080347,9.361027,10.718210,1.814835,-3.765814,5.762503,1.750066
+0.020000,11.074148,9.395306,10.753045,1.741758,-3.653835,5.598953,1.749234
+0.020000,11.068238,9.428196,10.786463,1.670859,-3.544961,5.443654,1.747817
+0.020000,11.062625,9.459743,10.818504,1.602078,-3.439038,5.296158,1.745857
+0.020000,11.057312,9.489987,10.849211,1.535360,-3.335899,5.156942,1.743396
+0.020000,11.052302,9.518970,10.878624,1.470653,-3.235386,5.025662,1.740474
+0.020000,11.047592,9.546731,10.906782,1.407905,-3.137387,4.899980,1.737130
+0.020000,11.043180,9.573309,10.933724,1.347070,-3.041761,4.781279,1.733402
+0.020000,11.039060,9.598739,10.959486,1.288101,-2.948444,4.665873,1.729328
+0.020000,11.035226,9.623058,10.984105,1.230954,-2.857354,4.554483,1.724944
+0.020000,11.031669,9.646299,11.007617,1.175584,-2.768462,4.444593,1.720285
+0.020000,11.028380,9.668496,11.030056,1.121950,-2.681742,4.335987,1.715387
+0.020000,11.025350,9.689680,11.051456,1.070005,-2.597206,4.226798,1.710283
+0.020000,11.022567,9.709884,11.071850,1.019708,-2.514866,4.117033,1.705006
+0.020000,11.020020,9.729136,11.091270,0.971013,-2.434772,4.004675,1.699587
+0.020000,11.017697,9.747467,11.109748,0.923873,-2.356960,3.890623,1.694060
+0.020000,11.015586,9.764905,11.127312,0.878243,-2.281506,3.772685,1.688452
+0.020000,11.013674,9.781476,11.143994,0.834074,-2.208465,3.652029,1.682794
+0.020000,11.011950,9.797208,11.159820,0.791316,-2.137914,3.527556,1.677114
+0.020000,11.010400,9.812126,11.174819,0.749917,-2.069918,3.399818,1.671439
+0.020000,11.009013,9.826255,11.189015,0.709826,-2.004545,3.268630,1.665795
+0.020000,11.007777,9.839618,11.202435,0.670989,-1.941871,3.133706,1.660207
+0.020000,11.006681,9.852237,11.215102,0.633350,-1.881937,2.996736,1.654700
+0.020000,11.005712,9.864135,11.227039,0.596855,-1.824784,2.857645,1.649295
+0.020000,11.004861,9.875331,11.238268,0.561445,-1.770466,2.715894,1.644016
+0.020000,11.004117,9.885846,11.248809,0.527065,-1.719026,2.572014,1.638881
+0.020000,11.003470,9.895698,11.258682,0.493656,-1.670447,2.428944,1.633912
+0.020000,11.002909,9.904910,11.267911,0.461427,-1.611447,2.949988,1.629123
+0.020000,11.002427,9.913510,11.276525,0.430696,-1.536553,3.744718,1.624524
+0.020000,11.002013,9.921530,11.284555,0.401505,-1.459521,3.851591,1.620122
+0.020000,11.001660,9.928997,11.292030,0.373777,-1.386420,3.655031,1.615923
+0.020000,11.001361,9.935939,11.298979,0.347436,-1.317035,3.469231,1.611931
+0.020000,11.001108,9.942383,11.305427,0.322415,-1.251044,3.299591,1.608148
+0.020000,11.000895,9.948352,11.311400,0.298650,-1.188254,3.139505,1.604575
+0.020000,11.000718,9.953871,11.316922,0.276082,-1.128425,2.991415,1.601214
+0.020000,11.000571,9.958962,11.322015,0.254655,-1.071326,2.854948,1.598062
+0.020000,11.000450,9.963646,11.326702,0.234322,-1.016664,2.733116,1.595117
+0.020000,11.000351,9.967946,11.331002,0.215035,-0.964368,2.614795,1.592378
+0.020000,11.000272,9.971880,11.334937,0.196752,-0.914121,2.512365,1.589840
+0.020000,11.000207,9.975468,11.338526,0.179436,-0.865827,2.414693,1.587499
+0.020000,11.000156,9.978729,11.341787,0.163052,-0.819212,2.330759,1.585349
+0.020000,11.000116,9.981680,11.344739,0.147568,-0.774192,2.250964,1.583385
+0.020000,11.000085,9.984339,11.347398,0.132956,-0.730594,2.179915,1.581600
+0.020000,11.000061,9.986723,11.349781,0.119191,-0.688247,2.117360,1.579988
+0.020000,11.000043,9.988848,11.351906,0.106250,-0.647051,2.059798,1.578542
+0.020000,11.000030,9.990730,11.353789,0.094113,-0.606822,2.011455,1.577252
+0.020000,11.000020,9.992385,11.355444,0.082762,-0.567553,1.963439,1.576113
+0.020000,11.000013,9.993829,11.356888,0.072184,-0.528939,1.930717,1.575114
+0.020000,11.000009,9.995076,11.358135,0.062360,-0.491166,1.888635,1.574248
+0.020000,11.000005,9.996142,11.359200,0.053283,-0.453848,1.865883,1.573505
+0.020000,11.000003,9.997040,11.360099,0.044942,-0.417084,1.838210,1.572877
+0.020000,11.000002,9.997787,11.360846,0.037326,-0.380769,1.815765,1.572354
+0.020000,11.000001,9.998396,11.361454,0.030431,-0.344779,1.799475,1.571927
+0.020000,11.000000,9.998881,11.361939,0.024250,-0.309051,1.786408,1.571585
+0.020000,11.000000,9.999256,11.362315,0.018777,-0.273653,1.769932,1.571321
+0.020000,11.000000,9.999536,11.362595,0.014010,-0.238344,1.765418,1.571124
+0.020000,11.000000,9.999735,11.362794,0.009946,-0.203208,1.756828,1.570983
+0.020000,11.000000,9.999867,11.362926,0.006582,-0.168187,1.751023,1.570890
+0.020000,11.000000,9.999945,11.363004,0.003917,-0.133233,1.747735,1.570835
+0.020000,11.000000,9.999984,11.363043,0.001951,-0.098320,1.745651,1.570808
+0.020000,11.000000,9.999998,11.363057,0.000682,-0.063430,1.744493,1.570798
+0.020000,11.000000,10.000000,11.363059,0.000111,-0.028550,1.743979,1.570796
+0.020000,11.000000,10.000000,11.363059,0.000000,-0.005556,1.149716,1.570796
diff --git a/src/main/deploy/output/turnTest.left.pf1.csv b/src/main/deploy/output/turnTest.left.pf1.csv
new file mode 100644
index 0000000..b2c5945
--- /dev/null
+++ b/src/main/deploy/output/turnTest.left.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.001758,9.249999,0.000012,0.001180,0.059025,2.951235,6.281779
+0.020000,0.001752,9.249999,0.000017,0.000280,-0.045015,-5.201995,6.281784
+0.020000,0.001739,9.249999,0.000030,0.000630,0.017512,3.126378,6.281794
+0.020000,0.001717,9.249999,0.000052,0.001121,0.024519,0.350315,6.281812
+0.020000,0.001682,9.249999,0.000087,0.001751,0.031527,0.350419,6.281840
+0.020000,0.001631,9.249999,0.000138,0.002522,0.038539,0.350589,6.281880
+0.020000,0.001563,9.249999,0.000207,0.003433,0.045556,0.350841,6.281935
+0.020000,0.001473,9.249999,0.000296,0.004485,0.052580,0.351194,6.282007
+0.020000,0.001360,9.249999,0.000410,0.005677,0.059613,0.351662,6.282098
+0.020000,0.001219,9.249999,0.000550,0.007010,0.066658,0.352262,6.282210
+0.020000,0.001050,9.250000,0.000720,0.008485,0.073718,0.353013,6.282346
+0.020000,0.000848,9.250000,0.000922,0.010101,0.080797,0.353929,6.282507
+0.020000,0.000610,9.250000,0.001159,0.011858,0.087897,0.355029,6.282697
+0.020000,0.000335,9.250000,0.001434,0.013759,0.095024,0.356329,6.282917
+0.020000,0.000019,9.250000,0.001750,0.015803,0.102181,0.357846,6.283170
+0.020000,0.002522,9.250000,0.004252,0.125116,5.465689,268.175388,0.000273
+0.020000,0.005528,9.250002,0.007259,0.150303,1.259353,-210.316760,0.000598
+0.020000,0.008896,9.250004,0.010626,0.168397,0.904699,-17.732744,0.000963
+0.020000,0.012645,9.250009,0.014376,0.187492,0.954753,2.502740,0.001371
+0.020000,0.016805,9.250015,0.018535,0.207954,1.023069,3.415793,0.001823
+0.020000,0.021387,9.250025,0.023118,0.229106,1.057616,1.727318,0.002322
+0.020000,0.026411,9.250038,0.028142,0.251239,1.106646,2.451521,0.002871
+0.020000,0.031907,9.250055,0.033638,0.274802,1.178127,3.574068,0.003473
+0.020000,0.037885,9.250078,0.039616,0.298866,1.203228,1.255025,0.004128
+0.020000,0.044373,9.250107,0.046104,0.324410,1.277204,3.698809,0.004842
+0.020000,0.051383,9.250144,0.053114,0.350511,1.305044,1.391978,0.005616
+0.020000,0.058945,9.250189,0.060677,0.378122,1.380563,3.775976,0.006453
+0.020000,0.067071,9.250245,0.068802,0.406273,1.407540,1.348821,0.007355
+0.020000,0.075784,9.250314,0.077515,0.435668,1.469745,3.110244,0.008327
+0.020000,0.085105,9.250396,0.086837,0.466100,1.521606,2.593093,0.009370
+0.020000,0.095052,9.250495,0.096784,0.497344,1.562187,2.029050,0.010488
+0.020000,0.105643,9.250612,0.107376,0.529613,1.613464,2.563826,0.011684
+0.020000,0.116900,9.250751,0.118634,0.562893,1.663985,2.526043,0.012961
+0.020000,0.128842,9.250914,0.130578,0.597166,1.713665,2.484008,0.014323
+0.020000,0.141489,9.251104,0.143226,0.632415,1.762416,2.437557,0.015773
+0.020000,0.154860,9.251326,0.156598,0.668617,1.810147,2.386529,0.017315
+0.020000,0.168973,9.251582,0.170713,0.705753,1.856762,2.330764,0.018953
+0.020000,0.183846,9.251876,0.185589,0.743796,1.902164,2.270105,0.020690
+0.020000,0.199496,9.252215,0.201244,0.782721,1.946252,2.204397,0.022531
+0.020000,0.215942,9.252601,0.217694,0.822499,1.988922,2.133491,0.024479
+0.020000,0.233206,9.253042,0.234964,0.863501,2.050094,3.058634,0.026540
+0.020000,0.251290,9.253541,0.253055,0.904539,2.051875,0.089022,0.028717
+0.020000,0.270194,9.254106,0.271967,0.945627,2.054407,0.126599,0.031012
+0.020000,0.289912,9.254742,0.291695,0.986377,2.037504,-0.845136,0.033426
+0.020000,0.310445,9.255454,0.312240,1.027277,2.044974,0.373490,0.035965
+0.020000,0.331784,9.256250,0.333594,1.067702,2.021261,-1.185651,0.038629
+0.020000,0.353926,9.257137,0.355754,1.108010,2.015424,-0.291868,0.041421
+0.020000,0.376869,9.258122,0.378718,1.148171,2.008019,-0.370226,0.044346
+0.020000,0.400607,9.259211,0.402481,1.188151,1.999011,-0.450424,0.047406
+0.020000,0.425136,9.260414,0.427039,1.227918,1.988362,-0.532416,0.050604
+0.020000,0.450450,9.261739,0.452388,1.267439,1.976039,-0.616145,0.053946
+0.020000,0.476543,9.263193,0.478522,1.306679,1.962008,-0.701550,0.057433
+0.020000,0.503408,9.264787,0.505434,1.345604,1.946237,-0.788558,0.061071
+0.020000,0.531040,9.266529,0.533121,1.384368,1.938201,-0.401791,0.064863
+0.020000,0.559434,9.268429,0.561579,1.422884,1.925792,-0.620484,0.068815
+0.020000,0.588579,9.270498,0.590797,1.460929,1.902261,-1.176530,0.072930
+0.020000,0.618471,9.272746,0.620774,1.498815,1.894322,-0.396940,0.077215
+0.020000,0.649102,9.275185,0.651501,1.536360,1.877217,-0.855249,0.081674
+0.020000,0.680460,9.277825,0.682970,1.573440,1.854036,-1.159070,0.086313
+0.020000,0.712537,9.280678,0.715174,1.610210,1.838465,-0.778525,0.091139
+0.020000,0.745325,9.283757,0.748106,1.646622,1.820609,-0.892838,0.096156
+0.020000,0.778814,9.287075,0.781759,1.682630,1.800412,-1.009853,0.101372
+0.020000,0.812992,9.290645,0.816122,1.718187,1.777825,-1.129353,0.106793
+0.020000,0.847846,9.294481,0.851187,1.753243,1.752803,-1.251081,0.112428
+0.020000,0.883365,9.298596,0.886944,1.787827,1.729208,-1.179768,0.118282
+0.020000,0.919540,9.303006,0.923386,1.822115,1.714393,-0.740715,0.124365
+0.020000,0.956349,9.307725,0.960497,1.855549,1.671739,-2.132728,0.130684
+0.020000,0.993786,9.312770,0.998272,1.888740,1.659514,-0.611247,0.137250
+0.020000,1.031830,9.318157,1.036696,1.921202,1.623105,-1.820454,0.144070
+0.020000,1.070466,9.323901,1.075757,1.953042,1.592027,-1.553870,0.151154
+0.020000,1.109678,9.330021,1.115443,1.984291,1.562422,-1.480248,0.158512
+0.020000,1.149445,9.336533,1.155740,2.014884,1.529640,-1.639137,0.166156
+0.020000,1.189750,9.343456,1.196635,2.044757,1.493661,-1.798929,0.174096
+0.020000,1.230570,9.350809,1.238112,2.073847,1.454488,-1.958635,0.182343
+0.020000,1.271889,9.358610,1.280161,2.102427,1.429004,-1.274194,0.190911
+0.020000,1.313677,9.366878,1.322759,2.129885,1.372894,-2.805499,0.199810
+0.020000,1.355915,9.375635,1.365895,2.156836,1.347551,-1.267155,0.209056
+0.020000,1.398575,9.384899,1.409549,2.182707,1.293548,-2.700178,0.218661
+0.020000,1.441631,9.394691,1.453705,2.207760,1.252669,-2.043960,0.228639
+0.020000,1.485055,9.405032,1.498343,2.231923,1.208153,-2.225788,0.239005
+0.020000,1.528817,9.415944,1.543446,2.255126,1.160149,-2.400200,0.249775
+0.020000,1.572887,9.427448,1.588992,2.277303,1.108862,-2.564365,0.260962
+0.020000,1.617233,9.439566,1.634964,2.298617,1.065701,-2.158010,0.272585
+0.020000,1.661820,9.452320,1.681339,2.318756,1.006930,-2.938585,0.284658
+0.020000,1.706608,9.465728,1.728091,2.337589,0.941669,-3.263015,0.297197
+0.020000,1.751541,9.479809,1.775178,2.354376,0.839333,-5.116840,0.310213
+0.020000,1.796567,9.494577,1.822565,2.369315,0.746939,-4.619661,0.323716
+0.020000,1.841633,9.510048,1.870213,2.382397,0.654127,-4.640639,0.337718
+0.020000,1.886686,9.526235,1.918085,2.393636,0.561949,-4.608864,0.352229
+0.020000,1.931676,9.543154,1.966152,2.403307,0.483560,-3.919487,0.367258
+0.020000,1.976546,9.560816,2.014372,2.411044,0.386856,-4.835154,0.382813
+0.020000,2.021249,9.579234,2.062720,2.417397,0.317643,-3.460674,0.398900
+0.020000,2.065732,9.598418,2.111164,2.422165,0.238388,-3.962770,0.415525
+0.020000,2.109946,9.618379,2.159675,2.425561,0.169793,-3.429717,0.432689
+0.020000,2.153845,9.639126,2.208229,2.427722,0.108067,-3.086326,0.450395
+0.020000,2.197384,9.660669,2.256807,2.428874,0.057566,-2.525060,0.468640
+0.020000,2.240521,9.683016,2.305388,2.429077,0.010154,-2.370558,0.487421
+0.020000,2.283215,9.706175,2.353960,2.428568,-0.025459,-1.780645,0.506728
+0.020000,2.325430,9.730156,2.402511,2.427551,-0.050809,-1.267511,0.526551
+0.020000,2.367135,9.754967,2.451038,2.426356,-0.059777,-0.448395,0.546877
+0.020000,2.408299,9.780617,2.499539,2.425066,-0.064511,-0.236731,0.567685
+0.020000,2.448897,9.807115,2.548019,2.424011,-0.052752,0.587974,0.588954
+0.020000,2.488907,9.834474,2.596489,2.423505,-0.025267,1.374236,0.610657
+0.020000,2.528311,9.862705,2.644963,2.423691,0.009302,1.728429,0.632762
+0.020000,2.567096,9.891822,2.693461,2.424895,0.060167,2.543283,0.655235
+0.020000,2.605251,9.921841,2.742009,2.427413,0.125916,3.287457,0.678036
+0.020000,2.642769,9.952778,2.790638,2.431402,0.199459,3.677159,0.701123
+0.020000,2.679645,9.984652,2.839380,2.437110,0.285409,4.297489,0.724449
+0.020000,2.715877,10.017484,2.888275,2.444741,0.381532,4.806138,0.747964
+0.020000,2.751466,10.051295,2.937363,2.454442,0.485071,5.176949,0.771617
+0.020000,2.786411,10.086108,2.986690,2.466329,0.594329,5.462888,0.795354
+0.020000,2.820716,10.121945,3.036299,2.480476,0.707370,5.652074,0.819119
+0.020000,2.854381,10.158830,3.086238,2.496918,0.822090,5.735985,0.842858
+0.020000,2.887408,10.196785,3.136551,2.515644,0.936288,5.709898,0.866513
+0.020000,2.919798,10.235831,3.187283,2.536599,1.047750,5.573097,0.890032
+0.020000,2.951551,10.275988,3.238477,2.559694,1.154746,5.349831,0.913361
+0.020000,2.982666,10.317271,3.290172,2.584764,1.253535,4.939420,0.936450
+0.020000,3.013138,10.359694,3.342405,2.611653,1.344432,4.544878,0.959249
+0.020000,3.042964,10.403267,3.395208,2.640148,1.424721,4.014425,0.981716
+0.020000,3.072139,10.447993,3.448608,2.670023,1.493790,3.453438,1.003809
+0.020000,3.100654,10.493874,3.502629,2.701021,1.549865,2.803779,1.025490
+0.020000,3.128504,10.540905,3.557287,2.732892,1.593570,2.185230,1.046728
+0.020000,3.155678,10.589076,3.612594,2.765369,1.623850,1.513993,1.067493
+0.020000,3.182169,10.638372,3.668557,2.798161,1.639601,0.787560,1.087762
+0.020000,3.207965,10.688766,3.725170,2.830632,1.623565,-0.801795,1.107512
+0.020000,3.233052,10.740226,3.782419,2.862479,1.592341,-1.561199,1.126723
+0.020000,3.257426,10.792721,3.840297,2.893870,1.569547,-1.139693,1.145384
+0.020000,3.281079,10.846218,3.898789,2.924606,1.536814,-1.636663,1.163488
+0.020000,3.304006,10.900678,3.957878,2.954460,1.492698,-2.205795,1.181029
+0.020000,3.326205,10.956059,4.017543,2.983243,1.439115,-2.679143,1.198005
+0.020000,3.347672,11.012318,4.077759,3.010777,1.376721,-3.119708,1.214418
+0.020000,3.368407,11.069408,4.138497,3.036931,1.307701,-3.451017,1.230270
+0.020000,3.388412,11.127279,4.199729,3.061573,1.232080,-3.781014,1.245568
+0.020000,3.407689,11.185881,4.261420,3.084569,1.149827,-4.112689,1.260318
+0.020000,3.426243,11.245162,4.323537,3.105841,1.063606,-4.311027,1.274529
+0.020000,3.444080,11.305070,4.386043,3.125316,0.973748,-4.492922,1.288210
+0.020000,3.461206,11.365550,4.448902,3.142929,0.880625,-4.656130,1.301375
+0.020000,3.477633,11.426549,4.512073,3.158584,0.782764,-4.893047,1.314033
+0.020000,3.493368,11.488012,4.575519,3.172287,0.685141,-4.881177,1.326198
+0.020000,3.508426,11.549886,4.639199,3.183997,0.585479,-4.983082,1.337883
+0.020000,3.522817,11.612118,4.703073,3.193690,0.484672,-5.040322,1.349101
+0.020000,3.536557,11.674653,4.767099,3.201317,0.381348,-5.166237,1.359866
+0.020000,3.549659,11.737439,4.831238,3.206944,0.281378,-4.998507,1.370191
+0.020000,3.562140,11.800425,4.895449,3.210521,0.178837,-5.127033,1.380091
+0.020000,3.574016,11.863559,4.959690,3.212056,0.076719,-5.105896,1.389579
+0.020000,3.585302,11.926790,5.023921,3.211561,-0.024744,-5.073162,1.398668
+0.020000,3.596018,11.990071,5.088102,3.209053,-0.125365,-5.031060,1.407373
+0.020000,3.606180,12.053351,5.152193,3.204553,-0.224997,-4.981598,1.415705
+0.020000,3.615806,12.116584,5.216154,3.198068,-0.324276,-4.963931,1.423679
+0.020000,3.624915,12.179722,5.279946,3.189572,-0.424798,-5.026099,1.431306
+0.020000,3.633525,12.242720,5.343530,3.179209,-0.518132,-4.666687,1.438598
+0.020000,3.641653,12.305533,5.406867,3.166825,-0.619230,-5.054896,1.445569
+0.020000,3.649318,12.368117,5.469919,3.152607,-0.710890,-4.583022,1.452229
+0.020000,3.656539,12.430430,5.532648,3.136489,-0.805875,-4.749262,1.458590
+0.020000,3.663333,12.492429,5.595018,3.118496,-0.899663,-4.689393,1.464662
+0.020000,3.669719,12.554072,5.656991,3.098655,-0.992071,-4.620415,1.470456
+0.020000,3.675713,12.615319,5.718531,3.076992,-1.083113,-4.552092,1.475983
+0.020000,3.681333,12.676131,5.779602,3.053536,-1.172814,-4.485026,1.481252
+0.020000,3.686597,12.736468,5.840168,3.028312,-1.261209,-4.419734,1.486273
+0.020000,3.691520,12.796292,5.900195,3.001326,-1.349294,-4.404289,1.491055
+0.020000,3.696120,12.855565,5.959646,2.972561,-1.438248,-4.447698,1.495608
+0.020000,3.700412,12.914251,6.018488,2.942120,-1.522042,-4.189710,1.499939
+0.020000,3.704412,12.972313,6.076688,2.909962,-1.607910,-4.293373,1.504058
+0.020000,3.708135,13.029714,6.134209,2.876084,-1.693929,-4.300948,1.507972
+0.020000,3.711596,13.086420,6.191021,2.840573,-1.775507,-4.078909,1.511689
+0.020000,3.714809,13.142407,6.247100,2.803958,-1.830758,-2.762558,1.515218
+0.020000,3.717790,13.197662,6.302436,2.766809,-1.857439,-1.334035,1.518567
+0.020000,3.720553,13.252175,6.357019,2.729121,-1.884412,-1.348647,1.521745
+0.020000,3.723110,13.305934,6.410838,2.690975,-1.907326,-1.145711,1.524758
+0.020000,3.725475,13.358928,6.463886,2.652379,-1.929797,-1.123565,1.527614
+0.020000,3.727660,13.411149,6.516152,2.613297,-1.954091,-1.214668,1.530322
+0.020000,3.729677,13.462586,6.567628,2.573832,-1.973236,-0.957253,1.532886
+0.020000,3.731536,13.513231,6.618307,2.533962,-1.993506,-1.013508,1.535314
+0.020000,3.733247,13.563076,6.668181,2.493698,-2.013181,-0.983764,1.537612
+0.020000,3.734821,13.612112,6.717243,2.453064,-2.031737,-0.927806,1.539786
+0.020000,3.736267,13.660332,6.765484,2.412079,-2.049257,-0.876003,1.541840
+0.020000,3.737594,13.707728,6.812900,2.370762,-2.065821,-0.828192,1.543782
+0.020000,3.738809,13.754295,6.859482,2.329132,-2.081505,-0.784211,1.545615
+0.020000,3.739921,13.800026,6.905226,2.287204,-2.096384,-0.743902,1.547345
+0.020000,3.740937,13.844913,6.950125,2.244937,-2.113394,-0.850502,1.548976
+0.020000,3.741864,13.888952,6.994174,2.202440,-2.124805,-0.570586,1.550513
+0.020000,3.742709,13.932136,7.037366,2.159628,-2.140610,-0.790259,1.551961
+0.020000,3.743477,13.974461,7.079698,2.116578,-2.152517,-0.595329,1.553323
+0.020000,3.744175,14.015920,7.121163,2.073277,-2.165057,-0.626998,1.554603
+0.020000,3.744807,14.056510,7.161758,2.029731,-2.177295,-0.611921,1.555806
+0.020000,3.745380,14.096225,7.201477,1.985953,-2.188906,-0.580539,1.556935
+0.020000,3.745898,14.135061,7.240316,1.941954,-2.199937,-0.551560,1.557993
+0.020000,3.746365,14.173013,7.278271,1.897745,-2.210435,-0.524884,1.558985
+0.020000,3.746785,14.210077,7.315337,1.853316,-2.221466,-0.551561,1.559912
+0.020000,3.747163,14.246249,7.351511,1.808699,-2.230840,-0.468702,1.560780
+0.020000,3.747502,14.281525,7.386789,1.763893,-2.240292,-0.472561,1.561590
+0.020000,3.747806,14.315902,7.421167,1.718901,-2.249629,-0.466876,1.562345
+0.020000,3.748077,14.349375,7.454642,1.673724,-2.258843,-0.460675,1.563048
+0.020000,3.748318,14.381942,7.487210,1.628388,-2.266769,-0.396327,1.563703
+0.020000,3.748533,14.413599,7.518867,1.582885,-2.275160,-0.419530,1.564311
+0.020000,3.748724,14.444343,7.549612,1.537223,-2.283125,-0.398248,1.564876
+0.020000,3.748893,14.474170,7.579440,1.491409,-2.290693,-0.378426,1.565399
+0.020000,3.749042,14.503079,7.608349,1.445451,-2.297893,-0.359999,1.565883
+0.020000,3.749173,14.531066,7.636336,1.399356,-2.304751,-0.342907,1.566330
+0.020000,3.749288,14.558128,7.663398,1.353112,-2.312210,-0.372940,1.566743
+0.020000,3.749389,14.584263,7.689533,1.306755,-2.317843,-0.281655,1.567123
+0.020000,3.749477,14.609468,7.714739,1.260264,-2.324566,-0.336135,1.567472
+0.020000,3.749554,14.633741,7.739012,1.213664,-2.329975,-0.270442,1.567793
+0.020000,3.749620,14.657080,7.762351,1.166935,-2.336446,-0.323562,1.568086
+0.020000,3.749678,14.679482,7.784753,1.120108,-2.341375,-0.246444,1.568355
+0.020000,3.749728,14.700945,7.806216,1.073175,-2.346614,-0.261976,1.568600
+0.020000,3.749771,14.721468,7.826739,1.026143,-2.351623,-0.250433,1.568824
+0.020000,3.749807,14.741048,7.846320,0.979022,-2.356034,-0.220550,1.569027
+0.020000,3.749839,14.759696,7.864968,0.932402,-2.331007,1.251371,1.569211
+0.020000,3.749865,14.777433,7.882705,0.886858,-2.277190,2.690834,1.569378
+0.020000,3.749888,14.794282,7.899553,0.842410,-2.222411,2.738974,1.569529
+0.020000,3.749907,14.810263,7.915534,0.799061,-2.167476,2.746709,1.569665
+0.020000,3.749923,14.825399,7.930670,0.756805,-2.112775,2.735062,1.569789
+0.020000,3.749937,14.839712,7.944984,0.715666,-2.056969,2.790286,1.569900
+0.020000,3.749948,14.853225,7.958496,0.675639,-2.001315,2.782703,1.570001
+0.020000,3.749958,14.865960,7.971231,0.636725,-1.945713,2.780092,1.570091
+0.020000,3.749966,14.877938,7.983210,0.598941,-1.889199,2.825742,1.570172
+0.020000,3.749972,14.889184,7.994455,0.562285,-1.832823,2.818761,1.570245
+0.020000,3.749978,14.899719,8.004991,0.526761,-1.776208,2.830754,1.570311
+0.020000,3.749982,14.909567,8.014838,0.492373,-1.719373,2.841755,1.570370
+0.020000,3.749986,14.918749,8.024021,0.459126,-1.662337,2.851828,1.570422
+0.020000,3.749989,14.927290,8.032561,0.427024,-1.605116,2.861034,1.570469
+0.020000,3.749991,14.935211,8.040482,0.396066,-1.547893,2.861153,1.570511
+0.020000,3.749993,14.942536,8.047808,0.366260,-1.490304,2.879443,1.570549
+0.020000,3.749995,14.949288,8.054560,0.337610,-1.432525,2.888948,1.570582
+0.020000,3.749996,14.955491,8.060762,0.310111,-1.374922,2.880171,1.570612
+0.020000,3.749997,14.961166,8.066437,0.283772,-1.316953,2.898424,1.570638
+0.020000,3.749998,14.966338,8.071609,0.258594,-1.258915,2.901886,1.570661
+0.020000,3.749998,14.971030,8.076301,0.234576,-1.200879,2.901808,1.570682
+0.020000,3.749999,14.975264,8.080535,0.211723,-1.142665,2.910733,1.570700
+0.020000,3.749999,14.979065,8.084336,0.190037,-1.084272,2.919613,1.570716
+0.020000,3.749999,14.982455,8.087726,0.169513,-1.026209,2.903173,1.570729
+0.020000,3.750000,14.985458,8.090730,0.150162,-0.967543,2.933290,1.570741
+0.020000,3.750000,14.988098,8.093369,0.131980,-0.909099,2.922230,1.570752
+0.020000,3.750000,14.990397,8.095669,0.114967,-0.850664,2.921729,1.570761
+0.020000,3.750000,14.992380,8.097651,0.099125,-0.792124,2.927004,1.570768
+0.020000,3.750000,14.994069,8.099340,0.084456,-0.733433,2.934567,1.570775
+0.020000,3.750000,14.995488,8.100759,0.070960,-0.674805,2.931391,1.570780
+0.020000,3.750000,14.996661,8.101932,0.058637,-0.616146,2.932929,1.570784
+0.020000,3.750000,14.997610,8.102882,0.047488,-0.557462,2.934208,1.570788
+0.020000,3.750000,14.998361,8.103632,0.037513,-0.498757,2.935254,1.570790
+0.020000,3.750000,14.998935,8.104206,0.028712,-0.440035,2.936091,1.570792
+0.020000,3.750000,14.999357,8.104628,0.021086,-0.381300,2.936742,1.570794
+0.020000,3.750000,14.999649,8.104921,0.014635,-0.322556,2.937231,1.570795
+0.020000,3.750000,14.999837,8.105108,0.009359,-0.263804,2.937582,1.570796
+0.020000,3.750000,14.999942,8.105213,0.005258,-0.205048,2.937818,1.570796
+0.020000,3.750000,14.999988,8.105260,0.002332,-0.146288,2.937962,1.570796
+0.020000,3.750000,15.000000,8.105271,0.000581,-0.087528,2.938037,1.570796
+0.020000,3.750000,15.000000,8.105271,0.000000,-0.029074,2.922705,1.570796
diff --git a/src/main/deploy/output/turnTest.pf1.csv b/src/main/deploy/output/turnTest.pf1.csv
new file mode 100644
index 0000000..50b450c
--- /dev/null
+++ b/src/main/deploy/output/turnTest.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,8.000000,0.000012,0.001180,0.059025,2.951235,6.281779
+0.020000,0.000000,8.000000,0.000059,0.003541,0.118049,2.951235,6.281784
+0.020000,0.000000,8.000000,0.000165,0.007083,0.177074,2.951235,6.281794
+0.020000,0.000000,8.000000,0.000354,0.011805,0.236099,2.951235,6.281812
+0.020000,0.000000,8.000000,0.000649,0.017707,0.295123,2.951235,6.281840
+0.020000,0.000000,8.000000,0.001074,0.024790,0.354148,2.951235,6.281880
+0.020000,0.000000,8.000000,0.001653,0.033054,0.413173,2.951235,6.281935
+0.020000,0.000000,8.000000,0.002408,0.042498,0.472198,2.951235,6.282007
+0.020000,0.000000,8.000000,0.003364,0.053122,0.531222,2.951235,6.282098
+0.020000,0.000000,8.000000,0.004545,0.064927,0.590247,2.951235,6.282210
+0.020000,0.000000,8.000000,0.005973,0.077913,0.649272,2.951235,6.282346
+0.020000,0.000000,8.000000,0.007673,0.092079,0.708296,2.951235,6.282507
+0.020000,0.000000,8.000000,0.009668,0.107425,0.767321,2.951235,6.282697
+0.020000,0.000000,8.000000,0.011982,0.123952,0.826346,2.951235,6.282917
+0.020000,0.000000,8.000000,0.014638,0.141659,0.885370,2.951235,6.283170
+0.020000,0.002862,8.000000,0.017660,0.160547,0.944395,2.951235,0.000273
+0.020000,0.006275,8.000002,0.021072,0.180616,1.003420,2.951235,0.000598
+0.020000,0.010100,8.000005,0.024897,0.201864,1.062445,2.951235,0.000963
+0.020000,0.014359,8.000010,0.029158,0.224294,1.121469,2.951235,0.001371
+0.020000,0.019083,8.000017,0.033880,0.247904,1.180494,2.951235,0.001823
+0.020000,0.024289,8.000028,0.039086,0.272694,1.239519,2.951235,0.002322
+0.020000,0.030000,8.000043,0.044800,0.298665,1.298543,2.951235,0.002871
+0.020000,0.036248,8.000063,0.051045,0.325816,1.357568,2.951235,0.003473
+0.020000,0.043045,8.000089,0.057844,0.354148,1.416593,2.951235,0.004128
+0.020000,0.050426,8.000122,0.065222,0.383661,1.475617,2.951235,0.004842
+0.020000,0.058403,8.000163,0.073202,0.414353,1.534642,2.951235,0.005616
+0.020000,0.067011,8.000215,0.081808,0.446227,1.593667,2.951235,0.006453
+0.020000,0.076265,8.000279,0.091063,0.479281,1.652691,2.951235,0.007355
+0.020000,0.086192,8.000357,0.100991,0.513515,1.711716,2.951235,0.008327
+0.020000,0.096818,8.000451,0.111616,0.548930,1.770741,2.951235,0.009370
+0.020000,0.108162,8.000564,0.122960,0.585525,1.829766,2.951235,0.010488
+0.020000,0.120248,8.000698,0.135049,0.623301,1.888790,2.951235,0.011684
+0.020000,0.133101,8.000856,0.147904,0.662257,1.947815,2.951235,0.012961
+0.020000,0.146746,8.001042,0.161551,0.702394,2.006840,2.951235,0.014323
+0.020000,0.161205,8.001260,0.176012,0.743711,2.065864,2.951235,0.015773
+0.020000,0.176503,8.001513,0.191311,0.786209,2.124889,2.951235,0.017315
+0.020000,0.192663,8.001806,0.207472,0.829887,2.183914,2.951235,0.018953
+0.020000,0.209707,8.002144,0.224518,0.874746,2.242938,2.951235,0.020690
+0.020000,0.227658,8.002532,0.242473,0.920785,2.301963,2.951235,0.022531
+0.020000,0.246538,8.002976,0.261361,0.968005,2.360988,2.951235,0.024479
+0.020000,0.266378,8.003482,0.281205,1.016405,2.420013,2.951235,0.026540
+0.020000,0.287181,8.004057,0.302018,1.064806,2.420013,0.000000,0.028717
+0.020000,0.308952,8.004707,0.323798,1.113206,2.420013,-0.000000,0.031012
+0.020000,0.331687,8.005440,0.346546,1.161606,2.420013,0.000000,0.033426
+0.020000,0.355391,8.006263,0.370262,1.210006,2.420013,0.000000,0.035965
+0.020000,0.380058,8.007183,0.394946,1.258407,2.420013,0.000000,0.038629
+0.020000,0.405688,8.008209,0.420598,1.306807,2.420013,-0.000000,0.041421
+0.020000,0.432283,8.009350,0.447218,1.355207,2.420013,0.000000,0.044346
+0.020000,0.459842,8.010616,0.474806,1.403607,2.420013,0.000000,0.047406
+0.020000,0.488364,8.012014,0.503363,1.452008,2.420013,-0.000000,0.050604
+0.020000,0.517849,8.013557,0.532887,1.500408,2.420013,0.000000,0.053946
+0.020000,0.548295,8.015254,0.563379,1.548808,2.420013,-0.000000,0.057433
+0.020000,0.579698,8.017117,0.594839,1.597208,2.420013,0.000000,0.061071
+0.020000,0.612062,8.019157,0.627267,1.645609,2.420013,-0.000000,0.064863
+0.020000,0.645385,8.021388,0.660663,1.694009,2.420013,0.000000,0.068815
+0.020000,0.679661,8.023821,0.695028,1.742409,2.420013,-0.000000,0.072930
+0.020000,0.714894,8.026471,0.730360,1.790809,2.420013,0.000000,0.077215
+0.020000,0.751081,8.029352,0.766660,1.839210,2.420013,0.000000,0.081674
+0.020000,0.788217,8.032478,0.803928,1.887610,2.420013,-0.000000,0.086313
+0.020000,0.826303,8.035866,0.842164,1.936010,2.420013,0.000000,0.091139
+0.020000,0.865335,8.039532,0.881369,1.984410,2.420013,0.000000,0.096156
+0.020000,0.905312,8.043492,0.921541,2.032811,2.420013,0.000000,0.101372
+0.020000,0.946230,8.047766,0.962681,2.081211,2.420013,-0.000000,0.106793
+0.020000,0.988085,8.052372,1.004789,2.129611,2.420013,0.000000,0.112428
+0.020000,1.030873,8.057330,1.047865,2.178011,2.420013,-0.000000,0.118282
+0.020000,1.074596,8.062660,1.091910,2.226412,2.420013,0.000000,0.124365
+0.020000,1.119240,8.068384,1.136922,2.274812,2.420013,-0.000000,0.130684
+0.020000,1.164810,8.074525,1.182902,2.323212,2.420013,0.000000,0.137250
+0.020000,1.211295,8.081107,1.229850,2.371612,2.420013,0.000000,0.144070
+0.020000,1.258690,8.088154,1.277767,2.420013,2.420013,-0.000000,0.151154
+0.020000,1.306989,8.095692,1.326651,2.468413,2.420013,0.000000,0.158512
+0.020000,1.356186,8.103749,1.376503,2.516813,2.420013,0.000000,0.166156
+0.020000,1.406272,8.112352,1.427323,2.565213,2.420013,-0.000000,0.174096
+0.020000,1.457238,8.121532,1.479112,2.613614,2.420013,0.000000,0.182343
+0.020000,1.509080,8.131320,1.531868,2.662014,2.420013,0.000000,0.190911
+0.020000,1.561781,8.141748,1.585592,2.710414,2.420013,0.000000,0.199810
+0.020000,1.615336,8.152851,1.640284,2.758814,2.420013,0.000000,0.209056
+0.020000,1.669728,8.164663,1.695945,2.807215,2.420013,-0.000000,0.218661
+0.020000,1.724946,8.177221,1.752573,2.855615,2.420013,-0.000000,0.228639
+0.020000,1.780975,8.190565,1.810169,2.904015,2.420013,0.000000,0.239005
+0.020000,1.837799,8.204734,1.868734,2.952415,2.420013,-0.000000,0.249775
+0.020000,1.895400,8.219771,1.928266,3.000816,2.420013,0.000000,0.260962
+0.020000,1.953761,8.235719,1.988766,3.049216,2.420013,-0.000000,0.272585
+0.020000,2.012857,8.252623,2.050235,3.097616,2.420013,-0.000000,0.284658
+0.020000,2.072660,8.270527,2.112659,3.144836,2.360988,-2.951235,0.297197
+0.020000,2.133117,8.289473,2.176016,3.190875,2.301963,-2.951235,0.310213
+0.020000,2.194182,8.309502,2.240282,3.235734,2.242938,-2.951235,0.323716
+0.020000,2.255803,8.330656,2.305434,3.279412,2.183914,-2.951235,0.337718
+0.020000,2.317925,8.352978,2.371447,3.321910,2.124889,-2.951235,0.352229
+0.020000,2.380499,8.376510,2.438298,3.363227,2.065864,-2.951235,0.367258
+0.020000,2.443460,8.401294,2.505964,3.403364,2.006840,-2.951235,0.382813
+0.020000,2.506755,8.427373,2.574421,3.442320,1.947815,-2.951235,0.398900
+0.020000,2.570319,8.454787,2.643645,3.480096,1.888790,-2.951235,0.415525
+0.020000,2.634088,8.483577,2.713613,3.516691,1.829766,-2.951235,0.432689
+0.020000,2.697996,8.513782,2.784301,3.552106,1.770741,-2.951235,0.450395
+0.020000,2.761976,8.545440,2.855686,3.586341,1.711716,-2.951235,0.468640
+0.020000,2.825956,8.578586,2.927743,3.619394,1.652691,-2.951235,0.487421
+0.020000,2.889864,8.613254,3.000450,3.651268,1.593667,-2.951235,0.506728
+0.020000,2.953624,8.649474,3.073782,3.681961,1.534642,-2.951235,0.526551
+0.020000,3.017163,8.687276,3.147716,3.711473,1.475617,-2.951235,0.546877
+0.020000,3.080401,8.726682,3.222229,3.739805,1.416593,-2.951235,0.567685
+0.020000,3.143261,8.767713,3.297297,3.766956,1.357568,-2.951235,0.588954
+0.020000,3.205664,8.810385,3.372895,3.792927,1.298543,-2.951235,0.610657
+0.020000,3.267530,8.854709,3.449002,3.817717,1.239519,-2.951235,0.632762
+0.020000,3.328778,8.900691,3.525592,3.841327,1.180494,-2.951235,0.655235
+0.020000,3.389332,8.948334,3.602643,3.863757,1.121469,-2.951235,0.678036
+0.020000,3.449114,8.997631,3.680131,3.885005,1.062445,-2.951235,0.701123
+0.020000,3.508048,9.048572,3.758032,3.905074,1.003420,-2.951235,0.724449
+0.020000,3.566062,9.101140,3.836322,3.923962,0.944395,-2.951235,0.747964
+0.020000,3.623085,9.155315,3.914978,3.941669,0.885370,-2.951235,0.771617
+0.020000,3.679051,9.211068,3.993977,3.958196,0.826346,-2.951235,0.795354
+0.020000,3.733897,9.268364,4.073294,3.973543,0.767321,-2.951235,0.819119
+0.020000,3.787565,9.327165,4.152907,3.987708,0.708296,-2.951235,0.842858
+0.020000,3.840003,9.387426,4.232791,4.000694,0.649272,-2.951235,0.866513
+0.020000,3.891163,9.449098,4.312923,4.012499,0.590247,-2.951235,0.890032
+0.020000,3.941004,9.512127,4.393279,4.023123,0.531222,-2.951235,0.913361
+0.020000,3.989489,9.576457,4.473836,4.032567,0.472198,-2.951235,0.936450
+0.020000,4.036589,9.642026,4.554570,4.040831,0.413173,-2.951235,0.959249
+0.020000,4.082279,9.708771,4.635457,4.047914,0.354148,-2.951235,0.981716
+0.020000,4.126542,9.776626,4.716475,4.053816,0.295123,-2.951235,1.003809
+0.020000,4.169365,9.845524,4.797598,4.058538,0.236099,-2.951235,1.025490
+0.020000,4.210742,9.915396,4.878804,4.062080,0.177074,-2.951235,1.046728
+0.020000,4.250671,9.986174,4.960069,4.064441,0.118049,-2.951235,1.067493
+0.020000,4.289157,10.057787,5.041370,4.065609,0.058408,-2.982093,1.087762
+0.020000,4.326201,10.130155,5.122670,4.064416,-0.059642,-5.902470,1.107512
+0.020000,4.361814,10.203200,5.203935,4.062043,-0.118667,-2.951235,1.126723
+0.020000,4.396012,10.276851,5.285140,4.058489,-0.177691,-2.951235,1.145384
+0.020000,4.428817,10.351044,5.366263,4.053754,-0.236716,-2.951235,1.163488
+0.020000,4.460253,10.425711,5.447278,4.047840,-0.295741,-2.951235,1.181029
+0.020000,4.490348,10.500789,5.528164,4.040744,-0.354765,-2.951235,1.198005
+0.020000,4.519130,10.576215,5.608896,4.032468,-0.413790,-2.951235,1.214418
+0.020000,4.546631,10.651929,5.689451,4.023012,-0.472815,-2.951235,1.230270
+0.020000,4.572884,10.727872,5.769805,4.012375,-0.531839,-2.951235,1.245568
+0.020000,4.597924,10.803988,5.849934,4.000558,-0.590864,-2.951235,1.260318
+0.020000,4.621784,10.880222,5.929816,3.987560,-0.649889,-2.951235,1.274529
+0.020000,4.644502,10.956520,6.009425,3.973382,-0.708914,-2.951235,1.288210
+0.020000,4.666113,11.032833,6.088739,3.958023,-0.767938,-2.951235,1.301375
+0.020000,4.686654,11.109110,6.167734,3.941484,-0.826963,-2.951235,1.314033
+0.020000,4.706162,11.185304,6.246387,3.923764,-0.885988,-2.951235,1.326198
+0.020000,4.724673,11.261370,6.324673,3.904864,-0.945012,-2.951235,1.337883
+0.020000,4.742225,11.337263,6.402569,3.884783,-1.004037,-2.951235,1.349101
+0.020000,4.758852,11.412940,6.480053,3.863522,-1.063062,-2.951235,1.359866
+0.020000,4.774592,11.488361,6.557099,3.841080,-1.122086,-2.951235,1.370191
+0.020000,4.789479,11.563485,6.633684,3.817458,-1.181111,-2.951235,1.380091
+0.020000,4.803547,11.638275,6.709785,3.792655,-1.240136,-2.951235,1.389579
+0.020000,4.816831,11.712691,6.785378,3.766672,-1.299160,-2.951235,1.398668
+0.020000,4.829363,11.786699,6.860440,3.739508,-1.358185,-2.951235,1.407373
+0.020000,4.841177,11.860263,6.934947,3.711164,-1.417210,-2.951235,1.415705
+0.020000,4.852304,11.933349,7.008875,3.681640,-1.476235,-2.951235,1.423679
+0.020000,4.862774,12.005923,7.082201,3.650934,-1.535259,-2.951235,1.431306
+0.020000,4.872618,12.077954,7.154900,3.619049,-1.594284,-2.951235,1.438598
+0.020000,4.881864,12.149408,7.226951,3.585983,-1.653309,-2.951235,1.445569
+0.020000,4.890542,12.220255,7.298328,3.551736,-1.712333,-2.951235,1.452229
+0.020000,4.898678,12.290466,7.369008,3.516309,-1.771358,-2.951235,1.458590
+0.020000,4.906299,12.360010,7.438969,3.479701,-1.830383,-2.951235,1.464662
+0.020000,4.913431,12.428857,7.508185,3.441913,-1.889407,-2.951235,1.470456
+0.020000,4.920098,12.496980,7.576633,3.402944,-1.948432,-2.951235,1.475983
+0.020000,4.926325,12.564350,7.644291,3.362795,-2.007457,-2.951235,1.481252
+0.020000,4.932134,12.630940,7.711133,3.321466,-2.066482,-2.951235,1.486273
+0.020000,4.937548,12.696722,7.777137,3.278955,-2.125506,-2.951235,1.491055
+0.020000,4.942589,12.761668,7.842280,3.235265,-2.184531,-2.951235,1.495608
+0.020000,4.947276,12.825754,7.906536,3.190394,-2.243556,-2.951235,1.499939
+0.020000,4.951630,12.888952,7.969884,3.144342,-2.302580,-2.951235,1.504058
+0.020000,4.955669,12.951235,8.032298,3.097110,-2.361605,-2.951235,1.507972
+0.020000,4.959413,13.012579,8.093756,3.048710,-2.420013,-2.920376,1.511689
+0.020000,4.962879,13.072970,8.154246,3.000309,-2.420013,-0.000000,1.515218
+0.020000,4.966086,13.132406,8.213769,2.951909,-2.420013,0.000000,1.518567
+0.020000,4.969049,13.190885,8.272323,2.903509,-2.420013,0.000000,1.521745
+0.020000,4.971785,13.248406,8.329909,2.855109,-2.420013,0.000000,1.524758
+0.020000,4.974310,13.304968,8.386527,2.806708,-2.420013,0.000000,1.527614
+0.020000,4.976637,13.360569,8.442177,2.758308,-2.420013,-0.000000,1.530322
+0.020000,4.978779,13.415209,8.496860,2.709908,-2.420013,0.000000,1.532886
+0.020000,4.980749,13.468888,8.550574,2.661508,-2.420013,-0.000000,1.535314
+0.020000,4.982559,13.521603,8.603320,2.613107,-2.420013,0.000000,1.537612
+0.020000,4.984220,13.573354,8.655098,2.564707,-2.420013,0.000000,1.539786
+0.020000,4.985743,13.624141,8.705908,2.516307,-2.420013,-0.000000,1.541840
+0.020000,4.987138,13.673964,8.755750,2.467907,-2.420013,0.000000,1.543782
+0.020000,4.988413,13.722821,8.804624,2.419506,-2.420013,-0.000000,1.545615
+0.020000,4.989577,13.770714,8.852531,2.371106,-2.420013,-0.000000,1.547345
+0.020000,4.990640,13.817640,8.899469,2.322706,-2.420013,0.000000,1.548976
+0.020000,4.991607,13.863600,8.945439,2.274306,-2.420013,0.000000,1.550513
+0.020000,4.992487,13.908593,8.990441,2.225905,-2.420013,0.000000,1.551961
+0.020000,4.993286,13.952620,9.034475,2.177505,-2.420013,0.000000,1.553323
+0.020000,4.994011,13.995680,9.077541,2.129105,-2.420013,0.000000,1.554603
+0.020000,4.994667,14.037773,9.119639,2.080705,-2.420013,-0.000000,1.555806
+0.020000,4.995260,14.078899,9.160769,2.032304,-2.420013,0.000000,1.556935
+0.020000,4.995795,14.119057,9.200931,1.983904,-2.420013,0.000000,1.557993
+0.020000,4.996278,14.158248,9.240125,1.935504,-2.420013,0.000000,1.558985
+0.020000,4.996711,14.196472,9.278352,1.887104,-2.420013,-0.000000,1.559912
+0.020000,4.997101,14.233728,9.315610,1.838703,-2.420013,0.000000,1.560780
+0.020000,4.997449,14.270017,9.351900,1.790303,-2.420013,0.000000,1.561590
+0.020000,4.997761,14.305337,9.387222,1.741903,-2.420013,0.000000,1.562345
+0.020000,4.998039,14.339690,9.421576,1.693503,-2.420013,-0.000000,1.563048
+0.020000,4.998287,14.373075,9.454962,1.645102,-2.420013,0.000000,1.563703
+0.020000,4.998507,14.405493,9.487380,1.596702,-2.420013,0.000000,1.564311
+0.020000,4.998702,14.436942,9.518830,1.548302,-2.420013,-0.000000,1.564876
+0.020000,4.998874,14.467424,9.549312,1.499902,-2.420013,0.000000,1.565399
+0.020000,4.999026,14.496937,9.578826,1.451501,-2.420013,-0.000000,1.565883
+0.020000,4.999160,14.525483,9.607372,1.403101,-2.420013,0.000000,1.566330
+0.020000,4.999278,14.553061,9.634950,1.354701,-2.420013,-0.000000,1.566743
+0.020000,4.999380,14.579671,9.661560,1.306301,-2.420013,0.000000,1.567123
+0.020000,4.999470,14.605312,9.687202,1.257900,-2.420013,0.000000,1.567472
+0.020000,4.999548,14.629986,9.711876,1.209500,-2.420013,0.000000,1.567793
+0.020000,4.999616,14.653692,9.735582,1.161100,-2.420013,0.000000,1.568086
+0.020000,4.999674,14.676430,9.758320,1.112700,-2.420013,-0.000000,1.568355
+0.020000,4.999725,14.698200,9.780090,1.064299,-2.420013,0.000000,1.568600
+0.020000,4.999768,14.719002,9.800892,1.015899,-2.420013,-0.000000,1.568824
+0.020000,4.999805,14.738836,9.820726,0.967511,-2.419395,0.030858,1.569027
+0.020000,4.999837,14.757714,9.839604,0.920304,-2.360371,2.951235,1.569211
+0.020000,4.999864,14.775660,9.857550,0.874277,-2.301346,2.951235,1.569378
+0.020000,4.999887,14.792697,9.874587,0.829431,-2.242321,2.951235,1.569529
+0.020000,4.999906,14.808849,9.890739,0.785765,-2.183297,2.951235,1.569665
+0.020000,4.999922,14.824139,9.906030,0.743279,-2.124272,2.951235,1.569789
+0.020000,4.999936,14.838592,9.920482,0.701974,-2.065247,2.951235,1.569900
+0.020000,4.999948,14.852230,9.934120,0.661850,-2.006222,2.951235,1.570001
+0.020000,4.999957,14.865078,9.946968,0.622906,-1.947198,2.951235,1.570091
+0.020000,4.999965,14.877158,9.959048,0.585142,-1.888173,2.951235,1.570172
+0.020000,4.999972,14.888495,9.970385,0.548559,-1.829148,2.951235,1.570245
+0.020000,4.999978,14.899112,9.981003,0.513157,-1.770124,2.951235,1.570311
+0.020000,4.999982,14.909033,9.990923,0.478935,-1.711099,2.951235,1.570370
+0.020000,4.999986,14.918282,10.000172,0.445893,-1.652074,2.951235,1.570422
+0.020000,4.999989,14.926881,10.008771,0.414032,-1.593050,2.951235,1.570469
+0.020000,4.999991,14.934855,10.016745,0.383352,-1.534025,2.951235,1.570511
+0.020000,4.999993,14.942227,10.024117,0.353852,-1.475000,2.951235,1.570549
+0.020000,4.999995,14.949021,10.030911,0.325532,-1.415976,2.951235,1.570582
+0.020000,4.999996,14.955260,10.037150,0.298393,-1.356951,2.951235,1.570612
+0.020000,4.999997,14.960968,10.042858,0.272435,-1.297926,2.951235,1.570638
+0.020000,4.999998,14.966169,10.048059,0.247657,-1.238901,2.951235,1.570661
+0.020000,4.999998,14.970886,10.052776,0.224059,-1.179877,2.951235,1.570682
+0.020000,4.999999,14.975143,10.057033,0.201642,-1.120852,2.951235,1.570700
+0.020000,4.999999,14.978964,10.060854,0.180406,-1.061827,2.951235,1.570716
+0.020000,4.999999,14.982371,10.064261,0.160350,-1.002803,2.951235,1.570729
+0.020000,5.000000,14.985390,10.067280,0.141474,-0.943778,2.951235,1.570741
+0.020000,5.000000,14.988042,10.069932,0.123779,-0.884753,2.951235,1.570752
+0.020000,5.000000,14.990353,10.072243,0.107264,-0.825729,2.951235,1.570761
+0.020000,5.000000,14.992345,10.074235,0.091930,-0.766704,2.951235,1.570768
+0.020000,5.000000,14.994042,10.075932,0.077777,-0.707679,2.951235,1.570775
+0.020000,5.000000,14.995467,10.077357,0.064804,-0.648654,2.951235,1.570780
+0.020000,5.000000,14.996646,10.078536,0.053011,-0.589630,2.951235,1.570784
+0.020000,5.000000,14.997600,10.079490,0.042399,-0.530605,2.951235,1.570788
+0.020000,5.000000,14.998353,10.080243,0.032967,-0.471580,2.951235,1.570790
+0.020000,5.000000,14.998930,10.080820,0.024716,-0.412556,2.951235,1.570792
+0.020000,5.000000,14.999354,10.081244,0.017646,-0.353531,2.951235,1.570794
+0.020000,5.000000,14.999648,10.081538,0.011756,-0.294506,2.951235,1.570795
+0.020000,5.000000,14.999836,10.081726,0.007046,-0.235482,2.951235,1.570796
+0.020000,5.000000,14.999941,10.081831,0.003517,-0.176457,2.951235,1.570796
+0.020000,5.000000,14.999988,10.081878,0.001168,-0.117432,2.951235,1.570796
+0.020000,5.000000,15.000000,10.081890,0.000000,-0.058408,2.951235,1.570796
+0.020000,5.000000,15.000000,10.081890,0.000000,0.000000,2.920376,1.570796
diff --git a/src/main/deploy/output/turnTest.right.pf1.csv b/src/main/deploy/output/turnTest.right.pf1.csv
new file mode 100644
index 0000000..f2c36c3
--- /dev/null
+++ b/src/main/deploy/output/turnTest.right.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,-0.001758,6.750001,0.000012,0.001180,0.059025,2.951235,6.281779
+0.020000,-0.001752,6.750001,0.000017,0.000280,-0.045015,-5.201995,6.281784
+0.020000,-0.001739,6.750001,0.000030,0.000630,0.017512,3.126378,6.281794
+0.020000,-0.001717,6.750001,0.000052,0.001121,0.024519,0.350315,6.281812
+0.020000,-0.001682,6.750001,0.000087,0.001751,0.031527,0.350419,6.281840
+0.020000,-0.001631,6.750001,0.000138,0.002522,0.038539,0.350589,6.281880
+0.020000,-0.001563,6.750001,0.000207,0.003433,0.045556,0.350841,6.281935
+0.020000,-0.001473,6.750001,0.000296,0.004485,0.052580,0.351194,6.282007
+0.020000,-0.001360,6.750001,0.000410,0.005677,0.059613,0.351662,6.282098
+0.020000,-0.001219,6.750001,0.000550,0.007010,0.066658,0.352262,6.282210
+0.020000,-0.001050,6.750000,0.000720,0.008485,0.073718,0.353013,6.282346
+0.020000,-0.000848,6.750000,0.000922,0.010101,0.080797,0.353929,6.282507
+0.020000,-0.000610,6.750000,0.001159,0.011858,0.087897,0.355029,6.282697
+0.020000,-0.000335,6.750000,0.001434,0.013759,0.095024,0.356329,6.282917
+0.020000,-0.000019,6.750000,0.001750,0.015803,0.102181,0.357846,6.283170
+0.020000,0.003203,6.750000,0.004973,0.161130,7.266373,358.209629,0.000273
+0.020000,0.007023,6.750002,0.008792,0.190988,1.492890,-288.674191,0.000598
+0.020000,0.011304,6.750005,0.013073,0.214042,1.152710,-17.009001,0.000963
+0.020000,0.016072,6.750011,0.017841,0.238391,1.217437,3.236369,0.001371
+0.020000,0.021362,6.750019,0.023131,0.264503,1.305612,4.408768,0.001823
+0.020000,0.027192,6.750032,0.028962,0.291525,1.351090,2.273887,0.002322
+0.020000,0.033589,6.750048,0.035358,0.319830,1.415249,3.207923,0.002871
+0.020000,0.040589,6.750070,0.042358,0.349996,1.508305,4.652811,0.003473
+0.020000,0.048206,6.750099,0.049975,0.380849,1.542631,1.716321,0.004128
+0.020000,0.056478,6.750136,0.058248,0.413640,1.639589,4.847890,0.004842
+0.020000,0.065422,6.750183,0.067192,0.447203,1.678141,1.927575,0.005616
+0.020000,0.075077,6.750241,0.076847,0.482762,1.777952,4.990561,0.006453
+0.020000,0.085459,6.750313,0.087229,0.519088,1.816292,1.917014,0.007355
+0.020000,0.096600,6.750400,0.098371,0.557091,1.900130,4.191905,0.008327
+0.020000,0.108530,6.750506,0.110301,0.596518,1.971347,3.560836,0.009370
+0.020000,0.121271,6.750633,0.123043,0.637092,2.028737,2.869489,0.010488
+0.020000,0.134853,6.750783,0.136625,0.679102,2.100474,3.586886,0.011684
+0.020000,0.149302,6.750961,0.151076,0.722541,2.171977,3.575133,0.012961
+0.020000,0.164649,6.751171,0.166424,0.767405,2.243192,3.560756,0.014323
+0.020000,0.180921,6.751415,0.182698,0.813686,2.314066,3.543683,0.015773
+0.020000,0.198146,6.751700,0.199925,0.861377,2.384543,3.523848,0.017315
+0.020000,0.216353,6.752031,0.218135,0.910469,2.454567,3.501191,0.018953
+0.020000,0.235568,6.752411,0.237354,0.960950,2.524080,3.475657,0.020690
+0.020000,0.255819,6.752849,0.257610,1.012811,2.593024,3.447200,0.022531
+0.020000,0.277134,6.753350,0.278931,1.066037,2.661339,3.415785,0.024479
+0.020000,0.299550,6.753922,0.301353,1.121137,2.754994,4.682725,0.026540
+0.020000,0.323072,6.754572,0.324885,1.176595,2.772898,0.895199,0.028717
+0.020000,0.347711,6.755308,0.349534,1.232453,2.792875,0.998853,0.031012
+0.020000,0.373462,6.756138,0.375299,1.288225,2.788601,-0.213676,0.033426
+0.020000,0.400337,6.757071,0.402190,1.344570,2.817258,1.432847,0.035965
+0.020000,0.428331,6.758115,0.430204,1.400696,2.806292,-0.548324,0.038629
+0.020000,0.457450,6.759281,0.459346,1.457093,2.819870,0.678916,0.041421
+0.020000,0.487697,6.760579,0.489621,1.513747,2.832667,0.639858,0.044346
+0.020000,0.519077,6.762020,0.521034,1.570640,2.844676,0.600420,0.047406
+0.020000,0.551593,6.763615,0.553589,1.627758,2.855890,0.560692,0.050604
+0.020000,0.585248,6.765375,0.587291,1.685084,2.866305,0.520767,0.053946
+0.020000,0.620046,6.767315,0.622143,1.742602,2.875920,0.480748,0.057433
+0.020000,0.655989,6.769447,0.658149,1.800297,2.884735,0.440743,0.061071
+0.020000,0.693084,6.771786,0.695317,1.858408,2.905535,1.040013,0.064863
+0.020000,0.731335,6.774346,0.733654,1.916853,2.922252,0.835863,0.068815
+0.020000,0.770743,6.777144,0.773161,1.975368,2.925747,0.174726,0.072930
+0.020000,0.811317,6.780195,0.813849,2.034411,2.952162,1.320738,0.077215
+0.020000,0.853060,6.783518,0.855725,2.093775,2.968213,0.802554,0.081674
+0.020000,0.895975,6.787131,0.898792,2.153335,2.978007,0.489734,0.086313
+0.020000,0.940068,6.791054,0.943058,2.213342,3.000339,1.116572,0.091139
+0.020000,0.985344,6.795306,0.988534,2.273780,3.021869,1.076508,0.096156
+0.020000,1.031810,6.799910,1.035227,2.334631,3.042592,1.036175,0.101372
+0.020000,1.079468,6.804888,1.083144,2.395882,3.062508,0.995756,0.106793
+0.020000,1.128323,6.810264,1.132295,2.457514,3.081616,0.955443,0.112428
+0.020000,1.178381,6.816064,1.182687,2.519623,3.105436,1.190987,0.118282
+0.020000,1.229652,6.822314,1.234337,2.582515,3.144637,1.960024,0.124365
+0.020000,1.282131,6.829043,1.287247,2.645463,3.147398,0.138086,0.130684
+0.020000,1.335834,6.836280,1.341434,2.709394,3.196539,2.457055,0.137250
+0.020000,1.390759,6.844057,1.396908,2.773684,3.214503,0.898161,0.144070
+0.020000,1.446913,6.852406,1.453679,2.838552,3.243390,1.444361,0.151154
+0.020000,1.504301,6.861363,1.511761,2.904111,3.277962,1.728632,0.158512
+0.020000,1.562927,6.870964,1.571168,2.970343,3.311603,1.682027,0.166156
+0.020000,1.622794,6.881248,1.631913,3.037229,3.344298,1.634750,0.174096
+0.020000,1.683906,6.892255,1.694008,3.104750,3.376039,1.587034,0.182343
+0.020000,1.746272,6.904030,1.757476,3.173397,3.432374,2.816775,0.190911
+0.020000,1.809885,6.916618,1.822322,3.242329,3.446588,0.710672,0.199810
+0.020000,1.874757,6.930067,1.888574,3.312559,3.511483,3.244763,0.209056
+0.020000,1.940882,6.944427,1.956239,3.383294,3.536737,1.262726,0.218661
+0.020000,2.008261,6.959751,2.025340,3.455015,3.586047,2.465477,0.228639
+0.020000,2.076895,6.976097,2.095894,3.527690,3.633764,2.385847,0.239005
+0.020000,2.146781,6.993524,2.167919,3.601286,3.679794,2.301495,0.249775
+0.020000,2.217913,7.012093,2.241435,3.675767,3.724041,2.212365,0.260962
+0.020000,2.290288,7.031871,2.316464,3.751460,3.784696,3.032755,0.272585
+0.020000,2.363894,7.052925,2.393022,3.827915,3.822750,1.902694,0.284658
+0.020000,2.438712,7.075326,2.471121,3.904946,3.851531,1.439028,0.297197
+0.020000,2.514694,7.099137,2.550747,3.981298,3.817576,-1.697732,0.310213
+0.020000,2.591797,7.124427,2.631892,4.057235,3.796882,-1.034690,0.323716
+0.020000,2.669972,7.151265,2.714545,4.132654,3.770936,-1.297319,0.337718
+0.020000,2.749164,7.179720,2.798694,4.207451,3.739834,-1.555089,0.352229
+0.020000,2.829321,7.209866,2.884333,4.281955,3.725239,-0.729746,0.367258
+0.020000,2.910374,7.241772,2.971439,4.355319,3.668181,-2.852920,0.382813
+0.020000,2.992261,7.275512,3.060005,4.428284,3.648265,-0.995774,0.398900
+0.020000,3.074906,7.311156,3.150009,4.500203,3.595911,-2.617714,0.415525
+0.020000,3.158230,7.348775,3.241431,4.571121,3.545927,-2.499221,0.432689
+0.020000,3.242147,7.388438,3.334249,4.640900,3.488967,-2.847985,0.450395
+0.020000,3.326568,7.430211,3.428440,4.709516,3.430772,-2.909736,0.468640
+0.020000,3.411392,7.474156,3.523971,4.776574,3.352881,-3.894557,0.487421
+0.020000,3.496512,7.520333,3.620810,4.841957,3.269193,-4.184408,0.506728
+0.020000,3.581817,7.568793,3.718919,4.905441,3.174173,-4.751008,0.526551
+0.020000,3.667191,7.619585,3.818259,4.966999,3.077899,-4.813702,0.546877
+0.020000,3.752504,7.672747,3.918781,5.026069,2.953507,-6.219566,0.567685
+0.020000,3.837626,7.728310,4.020432,5.082552,2.824161,-6.467337,0.588954
+0.020000,3.922421,7.786295,4.123158,5.136304,2.687568,-6.829635,0.610657
+0.020000,4.006748,7.846713,4.226894,5.186804,2.525008,-8.127980,0.632762
+0.020000,4.090460,7.909560,4.331572,5.233929,2.356279,-8.436464,0.655235
+0.020000,4.173414,7.974826,4.437123,5.277529,2.179962,-8.815834,0.678036
+0.020000,4.255460,8.042483,4.543467,5.317188,1.982984,-9.848933,0.701123
+0.020000,4.336452,8.112491,4.650522,5.352760,1.778605,-10.218950,0.724449
+0.020000,4.416247,8.184797,4.758204,5.384087,1.566329,-10.613776,0.747964
+0.020000,4.494704,8.259335,4.866423,5.410993,1.345330,-11.049969,0.771617
+0.020000,4.571690,8.336027,4.975090,5.433351,1.117877,-11.372625,0.795354
+0.020000,4.647077,8.414783,5.084112,5.451078,0.886353,-11.576220,0.819119
+0.020000,4.720749,8.495500,5.193395,5.464142,0.653183,-11.658510,0.842858
+0.020000,4.792598,8.578066,5.302846,5.472557,0.420770,-11.620645,0.866513
+0.020000,4.862528,8.662364,5.412374,5.476386,0.191428,-11.467111,0.890032
+0.020000,4.930457,8.748267,5.521889,5.475750,-0.031788,-11.160804,0.913361
+0.020000,4.996313,8.835643,5.631304,5.470738,-0.250619,-10.941509,0.936450
+0.020000,5.060040,8.924357,5.740535,5.461562,-0.458778,-10.407952,0.959249
+0.020000,5.121594,9.014275,5.849503,5.448416,-0.657323,-9.927269,0.981716
+0.020000,5.180945,9.105259,5.958134,5.431540,-0.843759,-9.321795,1.003809
+0.020000,5.238076,9.197174,6.066357,5.411162,-1.018941,-8.759114,1.025490
+0.020000,5.292980,9.289888,6.174108,5.387565,-1.179807,-8.043305,1.046728
+0.020000,5.345664,9.383272,6.281329,5.361013,-1.327636,-7.391438,1.067493
+0.020000,5.396144,9.477201,6.387963,5.331723,-1.464464,-6.841416,1.087762
+0.020000,5.444438,9.571544,6.493948,5.299267,-1.622797,-7.916642,1.107512
+0.020000,5.490575,9.666173,6.599226,5.263856,-1.770571,-7.388689,1.126723
+0.020000,5.534598,9.760981,6.703756,5.226517,-1.866934,-4.818169,1.145384
+0.020000,5.576555,9.855870,6.807506,5.187523,-1.949719,-4.139216,1.163488
+0.020000,5.616500,9.950744,6.910447,5.147042,-2.024027,-3.715399,1.181029
+0.020000,5.654491,10.045519,7.012553,5.105263,-2.088977,-3.247503,1.198005
+0.020000,5.690588,10.140112,7.113799,5.062342,-2.146041,-2.853200,1.214418
+0.020000,5.724855,10.234450,7.214169,5.018459,-2.194130,-2.404482,1.230270
+0.020000,5.757357,10.328466,7.313643,4.973744,-2.235783,-2.082623,1.245568
+0.020000,5.788158,10.422095,7.412209,4.928276,-2.273370,-1.879356,1.260318
+0.020000,5.817325,10.515281,7.509853,4.882181,-2.304779,-1.570453,1.274529
+0.020000,5.844924,10.607970,7.606564,4.835549,-2.331613,-1.341689,1.288210
+0.020000,5.871019,10.700115,7.702332,4.788445,-2.355165,-1.177583,1.301375
+0.020000,5.895675,10.791671,7.797150,4.740860,-2.379260,-1.204748,1.314033
+0.020000,5.918955,10.882596,7.891008,4.692904,-2.397811,-0.927598,1.326198
+0.020000,5.940921,10.972853,7.983900,4.644591,-2.415638,-0.891329,1.337883
+0.020000,5.961632,11.062408,8.075818,4.595941,-2.432518,-0.843978,1.349101
+0.020000,5.981148,11.151228,8.166757,4.546914,-2.451339,-0.941073,1.359866
+0.020000,5.999525,11.239283,8.256709,4.497622,-2.464575,-0.661783,1.370191
+0.020000,6.016817,11.326546,8.345669,4.447994,-2.481399,-0.841219,1.380091
+0.020000,6.033078,11.412991,8.433630,4.398032,-2.498102,-0.835150,1.389579
+0.020000,6.048359,11.498592,8.520584,4.347735,-2.514873,-0.838557,1.398668
+0.020000,6.062708,11.583328,8.606526,4.297097,-2.531876,-0.850131,1.407373
+0.020000,6.076174,11.667175,8.691449,4.246112,-2.549250,-0.868724,1.415705
+0.020000,6.088801,11.750115,8.775344,4.194751,-2.568092,-0.942101,1.423679
+0.020000,6.100632,11.832124,8.858202,4.142938,-2.590626,-1.126660,1.431306
+0.020000,6.111711,11.913187,8.940019,4.090817,-2.606065,-0.771980,1.438598
+0.020000,6.122076,11.993282,9.020781,4.038140,-2.633858,-1.389654,1.445569
+0.020000,6.131766,12.072393,9.100483,3.985101,-2.651922,-0.903197,1.452229
+0.020000,6.140818,12.150502,9.179115,3.931568,-2.676682,-1.237989,1.458590
+0.020000,6.149266,12.227591,9.256665,3.877521,-2.702314,-1.281613,1.464662
+0.020000,6.157144,12.303643,9.333124,3.822949,-2.728596,-1.314086,1.470456
+0.020000,6.164484,12.378641,9.408481,3.767839,-2.755535,-1.346951,1.475983
+0.020000,6.171317,12.452569,9.482724,3.712176,-2.783138,-1.380167,1.481252
+0.020000,6.177672,12.525412,9.555843,3.655948,-2.811413,-1.413728,1.486273
+0.020000,6.183576,12.597151,9.627826,3.599117,-2.841504,-1.504579,1.491055
+0.020000,6.189057,12.667772,9.698658,3.541623,-2.874721,-1.660846,1.495608
+0.020000,6.194139,12.737257,9.768329,3.483547,-2.903816,-1.454743,1.499939
+0.020000,6.198847,12.805591,9.836825,3.424800,-2.937361,-1.677244,1.504058
+0.020000,6.203203,12.872757,9.904132,3.365341,-2.972938,-1.778843,1.507972
+0.020000,6.207230,12.938739,9.970237,3.305238,-3.005117,-1.608958,1.511689
+0.020000,6.210949,13.003533,10.035138,3.245065,-3.008682,-0.178227,1.515218
+0.020000,6.214381,13.067149,10.098847,3.185430,-2.981734,1.347381,1.518567
+0.020000,6.217546,13.129595,10.161372,3.126272,-2.957912,1.191095,1.521745
+0.020000,6.220461,13.190878,10.222725,3.067631,-2.932056,1.292793,1.524758
+0.020000,6.223145,13.251007,10.282914,3.009467,-2.908180,1.193804,1.527614
+0.020000,6.225613,13.309990,10.341948,2.951694,-2.888670,0.975532,1.530322
+0.020000,6.227881,13.367833,10.399836,2.894384,-2.865495,1.158746,1.532886
+0.020000,6.229962,13.424544,10.456585,2.837471,-2.845639,0.992772,1.535314
+0.020000,6.231871,13.480130,10.512204,2.780931,-2.827020,0.930939,1.537612
+0.020000,6.233619,13.534597,10.566699,2.724752,-2.808941,0.903992,1.539786
+0.020000,6.235219,13.587951,10.620077,2.668924,-2.791396,0.877255,1.541840
+0.020000,6.236682,13.640200,10.672346,2.613436,-2.774383,0.850636,1.543782
+0.020000,6.238017,13.691348,10.723511,2.558278,-2.757902,0.824050,1.545615
+0.020000,6.239234,13.741402,10.773580,2.503439,-2.741953,0.797424,1.547345
+0.020000,6.240342,13.790366,10.822557,2.448846,-2.729663,0.614536,1.548976
+0.020000,6.241350,13.838248,10.870449,2.394596,-2.712518,0.857222,1.550513
+0.020000,6.242265,13.885050,10.917260,2.340568,-2.701354,0.558213,1.551961
+0.020000,6.243095,13.930779,10.962997,2.286827,-2.687079,0.713770,1.553323
+0.020000,6.243847,13.975439,11.007664,2.233336,-2.674566,0.625628,1.554603
+0.020000,6.244527,14.019036,11.051265,2.180081,-2.662742,0.591186,1.555806
+0.020000,6.245140,14.061572,11.093806,2.127057,-2.651207,0.576747,1.556935
+0.020000,6.245693,14.103054,11.135292,2.074257,-2.639970,0.561862,1.557993
+0.020000,6.246190,14.143484,11.175725,2.021676,-2.629040,0.546506,1.558985
+0.020000,6.246637,14.182868,11.215111,1.969286,-2.619512,0.476379,1.559912
+0.020000,6.247038,14.221208,11.253453,1.917106,-2.609020,0.524598,1.560780
+0.020000,6.247396,14.258508,11.290755,1.865118,-2.599380,0.482036,1.561590
+0.020000,6.247716,14.294773,11.327021,1.813311,-2.590353,0.451331,1.562345
+0.020000,6.248002,14.330005,11.362255,1.761673,-2.581899,0.422700,1.563048
+0.020000,6.248255,14.364209,11.396459,1.710218,-2.572770,0.456463,1.563703
+0.020000,6.248481,14.397386,11.429638,1.658922,-2.564786,0.399199,1.564311
+0.020000,6.248680,14.429541,11.461793,1.607782,-2.556987,0.389923,1.564876
+0.020000,6.248856,14.460677,11.492929,1.556795,-2.549386,0.380042,1.565399
+0.020000,6.249011,14.490795,11.523048,1.505955,-2.541995,0.369557,1.565883
+0.020000,6.249148,14.519900,11.552154,1.455258,-2.534826,0.358470,1.566330
+0.020000,6.249267,14.547994,11.580247,1.404681,-2.528841,0.299230,1.566743
+0.020000,6.249372,14.575079,11.607332,1.354251,-2.521508,0.366679,1.567123
+0.020000,6.249463,14.601157,11.633411,1.303934,-2.515840,0.283380,1.567472
+0.020000,6.249543,14.626232,11.658486,1.253748,-2.509335,0.325254,1.567793
+0.020000,6.249611,14.650305,11.682559,1.203659,-2.504440,0.244754,1.568086
+0.020000,6.249671,14.673379,11.705633,1.153690,-2.498455,0.299257,1.568355
+0.020000,6.249722,14.695455,11.727709,1.103824,-2.493286,0.258463,1.568600
+0.020000,6.249766,14.716536,11.748790,1.054057,-2.488365,0.246035,1.568824
+0.020000,6.249803,14.736624,11.768878,1.004390,-2.483310,0.252742,1.569027
+0.020000,6.249835,14.755732,11.787987,0.955419,-2.448582,1.736410,1.569211
+0.020000,6.249863,14.773887,11.806141,0.907719,-2.385004,3.178889,1.569378
+0.020000,6.249886,14.791113,11.823367,0.861296,-2.321142,3.193095,1.569529
+0.020000,6.249905,14.807435,11.839690,0.816140,-2.257786,3.167811,1.569665
+0.020000,6.249922,14.822880,11.855134,0.772234,-2.195293,3.124652,1.569789
+0.020000,6.249936,14.837472,11.869726,0.729589,-2.132262,3.151527,1.569900
+0.020000,6.249947,14.851236,11.883490,0.688190,-2.069943,3.115967,1.570001
+0.020000,6.249957,14.864196,11.896450,0.648026,-2.008199,3.087190,1.570091
+0.020000,6.249965,14.876378,11.908633,0.609106,-1.946017,3.109097,1.570172
+0.020000,6.249972,14.887807,11.920061,0.571417,-1.884439,3.078915,1.570245
+0.020000,6.249977,14.898506,11.930760,0.534956,-1.823049,3.069511,1.570311
+0.020000,6.249982,14.908500,11.940754,0.499719,-1.761839,3.060484,1.570370
+0.020000,6.249986,14.917814,11.950069,0.465703,-1.700802,3.051851,1.570422
+0.020000,6.249989,14.926472,11.958727,0.432905,-1.639930,3.043621,1.570469
+0.020000,6.249991,14.934499,11.966753,0.401317,-1.579381,3.027418,1.570511
+0.020000,6.249993,14.941917,11.974172,0.370942,-1.518765,3.030817,1.570549
+0.020000,6.249995,14.948753,11.981007,0.341777,-1.458236,3.026436,1.570582
+0.020000,6.249996,14.955029,11.987284,0.313814,-1.398143,3.004642,1.570612
+0.020000,6.249997,14.960770,11.993025,0.287056,-1.337918,3.011256,1.570638
+0.020000,6.249998,14.966000,11.998255,0.261499,-1.277841,3.003889,1.570661
+0.020000,6.249998,14.970743,12.002998,0.237140,-1.217963,2.993897,1.570682
+0.020000,6.249999,14.975023,12.007277,0.213978,-1.158084,2.993916,1.570700
+0.020000,6.249999,14.978863,12.011117,0.192014,-1.098189,2.994744,1.570716
+0.020000,6.249999,14.982288,12.014542,0.171239,-1.038773,2.970807,1.570729
+0.020000,6.250000,14.985321,12.017575,0.151661,-0.978880,2.994682,1.570741
+0.020000,6.250000,14.987986,12.020241,0.133275,-0.919326,2.977682,1.570752
+0.020000,6.250000,14.990308,12.022562,0.116077,-0.859885,2.972040,1.570761
+0.020000,6.250000,14.992309,12.024564,0.100068,-0.800428,2.972873,1.570768
+0.020000,6.250000,14.994014,12.026269,0.085251,-0.740895,2.976612,1.570775
+0.020000,6.250000,14.995447,12.027701,0.071621,-0.681494,2.970065,1.570780
+0.020000,6.250000,14.996630,12.028885,0.059178,-0.622118,2.968791,1.570784
+0.020000,6.250000,14.997589,12.029843,0.047923,-0.562764,2.967725,1.570788
+0.020000,6.250000,14.998346,12.030600,0.037854,-0.503427,2.966851,1.570790
+0.020000,6.250000,14.998925,12.031180,0.028972,-0.444104,2.966149,1.570792
+0.020000,6.250000,14.999351,12.031605,0.021277,-0.384792,2.965602,1.570794
+0.020000,6.250000,14.999646,12.031901,0.014767,-0.325488,2.965190,1.570795
+0.020000,6.250000,14.999835,12.032089,0.009443,-0.266190,2.964894,1.570796
+0.020000,6.250000,14.999941,12.032196,0.005305,-0.206896,2.964695,1.570796
+0.020000,6.250000,14.999988,12.032243,0.002353,-0.147605,2.964573,1.570796
+0.020000,6.250000,15.000000,12.032254,0.000587,-0.088315,2.964510,1.570796
+0.020000,6.250000,15.000000,12.032254,0.000000,-0.029335,2.948987,1.570796
diff --git a/src/main/deploy/testPath.left.pf1.csv b/src/main/deploy/testPath.left.pf1.csv
new file mode 100644
index 0000000..4541b8a
--- /dev/null
+++ b/src/main/deploy/testPath.left.pf1.csv
@@ -0,0 +1,254 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,9.996821,9.249996,0.000012,0.001170,0.058501,2.925055,0.002544
+0.020000,9.996833,9.249996,0.000024,0.000632,-0.026898,-4.269935,0.002533
+0.020000,9.996862,9.249996,0.000053,0.001422,0.039505,3.320118,0.002511
+0.020000,9.996912,9.249996,0.000103,0.002528,0.055308,0.790150,0.002470
+0.020000,9.996991,9.249996,0.000182,0.003951,0.071112,0.790233,0.002407
+0.020000,9.997105,9.249997,0.000296,0.005689,0.086920,0.790370,0.002316
+0.020000,9.997260,9.249997,0.000451,0.007744,0.102731,0.790573,0.002192
+0.020000,9.997462,9.249997,0.000653,0.010115,0.118548,0.790854,0.002030
+0.020000,9.997718,9.249998,0.000909,0.012802,0.134373,0.791228,0.001825
+0.020000,9.998034,9.249998,0.001225,0.015806,0.150207,0.791705,0.001573
+0.020000,9.998417,9.249999,0.001608,0.019127,0.166053,0.792298,0.001267
+0.020000,9.998872,9.249999,0.002063,0.022765,0.181913,0.793017,0.000902
+0.020000,9.999407,9.250000,0.002598,0.026721,0.197791,0.793874,0.000475
+0.020000,10.000125,9.250000,0.003316,0.035897,0.458768,13.048883,6.283164
+0.020000,10.003471,9.249999,0.006662,0.167310,6.570683,305.595739,6.282594
+0.020000,10.007277,9.249995,0.010468,0.190319,1.150451,-271.011610,6.281946
+0.020000,10.011573,9.249989,0.014764,0.214798,1.223944,3.674658,6.281214
+0.020000,10.016390,9.249977,0.019581,0.240844,1.302298,3.917694,6.280392
+0.020000,10.021760,9.249960,0.024952,0.268528,1.384184,4.094284,6.279476
+0.020000,10.027709,9.249935,0.030900,0.297433,1.445237,3.052687,6.278460
+0.020000,10.034271,9.249900,0.037462,0.328098,1.533250,4.400631,6.277339
+0.020000,10.041473,9.249853,0.044664,0.360102,1.600243,3.349665,6.276108
+0.020000,10.049347,9.249792,0.052539,0.393713,1.680537,4.014670,6.274760
+0.020000,10.057921,9.249714,0.061113,0.428728,1.750737,3.510009,6.273291
+0.020000,10.067230,9.249614,0.070422,0.465460,1.836600,4.293143,6.271695
+0.020000,10.077298,9.249490,0.080492,0.503458,1.899903,3.165162,6.269966
+0.020000,10.088160,9.249336,0.091354,0.543128,1.983499,4.179784,6.268099
+0.020000,10.099846,9.249148,0.103042,0.584404,2.063819,4.016023,6.266088
+0.020000,10.112385,9.248920,0.115584,0.627068,2.133180,3.468056,6.263927
+0.020000,10.125809,9.248646,0.129009,0.671298,2.211488,3.915367,6.261610
+0.020000,10.140147,9.248319,0.143351,0.717088,2.289518,3.901520,6.259132
+0.020000,10.155430,9.247931,0.158640,0.764433,2.367226,3.885389,6.256486
+0.020000,10.171690,9.247474,0.174906,0.813324,2.444564,3.866932,6.253666
+0.020000,10.188957,9.246938,0.192181,0.863754,2.521487,3.846113,6.250666
+0.020000,10.207261,9.246313,0.210496,0.915713,2.597945,3.822898,6.247480
+0.020000,10.226636,9.245588,0.229884,0.969439,2.686312,4.418371,6.244100
+0.020000,10.247110,9.244751,0.250375,1.024546,2.755350,3.451883,6.240522
+0.020000,10.268712,9.243788,0.271999,1.081206,2.832998,3.882431,6.236738
+0.020000,10.291479,9.242684,0.294792,1.139641,2.921786,4.439382,6.232741
+0.020000,10.315435,9.241424,0.318782,1.199489,2.992381,3.529760,6.228526
+0.020000,10.340613,9.239991,0.344000,1.260911,3.071098,3.935814,6.224084
+0.020000,10.367046,9.238365,0.370483,1.324153,3.162096,4.549940,6.219410
+0.020000,10.394761,9.236526,0.398260,1.388822,3.233440,3.567184,6.214496
+0.020000,10.423774,9.234455,0.427347,1.454348,3.276326,2.144293,6.209338
+0.020000,10.454085,9.232130,0.457746,1.519975,3.281330,0.250191,6.203935
+0.020000,10.485692,9.229530,0.489460,1.585675,3.285023,0.184680,6.198286
+0.020000,10.518597,9.226632,0.522493,1.651646,3.298543,0.675991,6.192387
+0.020000,10.552796,9.223412,0.556843,1.717494,3.292393,-0.307501,6.186239
+0.020000,10.588285,9.219846,0.592511,1.783413,3.295943,0.177498,6.179840
+0.020000,10.625066,9.215908,0.629502,1.849568,3.307774,0.591552,6.173190
+0.020000,10.663133,9.211571,0.667815,1.915625,3.302825,-0.247440,6.166286
+0.020000,10.702480,9.206808,0.707449,1.981719,3.304685,0.092980,6.159130
+0.020000,10.743104,9.201589,0.748408,2.047925,3.310330,0.282277,6.151719
+0.020000,10.785001,9.195886,0.790690,2.114133,3.310414,0.004155,6.144055
+0.020000,10.828161,9.189668,0.834296,2.180270,3.306824,-0.179454,6.136137
+0.020000,10.872577,9.182903,0.879225,2.246455,3.309263,0.121924,6.127966
+0.020000,10.918242,9.175561,0.925476,2.312543,3.304376,-0.244361,6.119542
+0.020000,10.965142,9.167607,0.973045,2.378481,3.296889,-0.374329,6.110867
+0.020000,11.013268,9.159010,1.021933,2.444384,3.295172,-0.085870,6.101944
+0.020000,11.062606,9.149735,1.072135,2.510127,3.287168,-0.400168,6.092773
+0.020000,11.113143,9.139747,1.123650,2.575728,3.280024,-0.357218,6.083358
+0.020000,11.164862,9.129012,1.176471,2.641046,3.265928,-0.704789,6.073702
+0.020000,11.217745,9.117495,1.230594,2.706159,3.255618,-0.515536,6.063810
+0.020000,11.271775,9.105161,1.286014,2.771002,3.242166,-0.672586,6.053685
+0.020000,11.326931,9.091974,1.342724,2.835515,3.225644,-0.826099,6.043333
+0.020000,11.383190,9.077900,1.400717,2.899638,3.206134,-0.975485,6.032760
+0.020000,11.440527,9.062903,1.459983,2.963312,3.183731,-1.120166,6.021973
+0.020000,11.498918,9.046949,1.520514,3.026540,3.161374,-1.117827,6.010979
+0.020000,11.558335,9.030005,1.582300,3.089295,3.137749,-1.181283,5.999787
+0.020000,11.618748,9.012037,1.645329,3.151421,3.106321,-1.571377,5.988405
+0.020000,11.680126,8.993012,1.709587,3.212929,3.075416,-1.545229,5.976844
+0.020000,11.742437,8.972900,1.775064,3.273845,3.045787,-1.481459,5.965113
+0.020000,11.805647,8.951671,1.841744,3.333985,3.006990,-1.939866,5.953225
+0.020000,11.869720,8.929295,1.909611,3.393358,2.968667,-1.916157,5.941192
+0.020000,11.934618,8.905746,1.978649,3.451922,2.928185,-2.024066,5.929027
+0.020000,12.000304,8.880999,2.048843,3.509658,2.886793,-2.069602,5.916744
+0.020000,12.066737,8.855030,2.120171,3.566435,2.838856,-2.396855,5.904357
+0.020000,12.133877,8.827817,2.192617,3.622274,2.791954,-2.345113,5.891881
+0.020000,12.201684,8.799341,2.266160,3.677145,2.743553,-2.420058,5.879332
+0.020000,12.270114,8.769586,2.340779,3.730976,2.691536,-2.600872,5.866727
+0.020000,12.339125,8.738536,2.416454,3.783742,2.638324,-2.660563,5.854083
+0.020000,12.408676,8.706179,2.493163,3.835432,2.584502,-2.691134,5.841416
+0.020000,12.478722,8.672505,2.570883,3.886015,2.529124,-2.768889,5.828746
+0.020000,12.549222,8.637507,2.649592,3.935467,2.472605,-2.825921,5.816089
+0.020000,12.620122,8.601187,2.729253,3.983046,2.378937,-4.683396,5.803466
+0.020000,12.691353,8.563556,2.809814,4.028030,2.249213,-6.486200,5.790901
+0.020000,12.762853,8.524631,2.891223,4.070444,2.120711,-6.425129,5.778415
+0.020000,12.834559,8.484429,2.973429,4.110313,1.993427,-6.364206,5.766030
+0.020000,12.906411,8.442973,3.056382,4.147671,1.867919,-6.275390,5.753769
+0.020000,12.978352,8.400289,3.140033,4.182557,1.744292,-6.181337,5.741651
+0.020000,13.050328,8.356404,3.224334,4.215016,1.622958,-6.066706,5.729698
+0.020000,13.122289,8.311348,3.309236,4.245106,1.504491,-5.923336,5.717927
+0.020000,13.194187,8.265156,3.394693,4.272875,1.388453,-5.801908,5.706359
+0.020000,13.265976,8.217862,3.480661,4.298381,1.275297,-5.657791,5.695009
+0.020000,13.337617,8.169504,3.567095,4.321685,1.165203,-5.504719,5.683895
+0.020000,13.409069,8.120121,3.653952,4.342851,1.058292,-5.345573,5.673031
+0.020000,13.480299,8.069754,3.741191,4.361943,0.954632,-5.182998,5.662434
+0.020000,13.551276,8.018444,3.828771,4.379028,0.854244,-5.019381,5.652115
+0.020000,13.621970,7.966235,3.916655,4.394170,0.757107,-4.856844,5.642087
+0.020000,13.692357,7.913171,4.004803,4.407410,0.661981,-4.756292,5.632363
+0.020000,13.762414,7.859296,4.093180,4.418838,0.571399,-4.529101,5.622952
+0.020000,13.832122,7.804657,4.181750,4.428521,0.484147,-4.362600,5.613863
+0.020000,13.901464,7.749298,4.270479,4.436455,0.396689,-4.372887,5.605106
+0.020000,13.970426,7.693266,4.359335,4.442786,0.316537,-4.007626,5.596687
+0.020000,14.038996,7.636608,4.448284,4.447474,0.234417,-4.106018,5.588615
+0.020000,14.107165,7.579369,4.537297,4.450654,0.159012,-3.770239,5.580895
+0.020000,14.174926,7.521597,4.626343,4.452278,0.081212,-3.889985,5.573532
+0.020000,14.242272,7.463336,4.715393,4.452480,0.010077,-3.556777,5.566531
+0.020000,14.309201,7.404634,4.804417,4.451243,-0.061850,-3.596306,5.559896
+0.020000,14.375711,7.345537,4.893389,4.448597,-0.132282,-3.521613,5.553631
+0.020000,14.441801,7.286090,4.982282,4.444616,-0.199064,-3.339088,5.547739
+0.020000,14.507472,7.226339,5.071068,4.439300,-0.265785,-3.336070,5.542222
+0.020000,14.572727,7.166328,5.159721,4.432661,-0.331960,-3.308746,5.537083
+0.020000,14.637567,7.106103,5.248216,4.424749,-0.395619,-3.182935,5.532323
+0.020000,14.701998,7.045707,5.336527,4.415576,-0.458627,-3.150410,5.527943
+0.020000,14.766023,6.985186,5.424630,4.405154,-0.521102,-3.123764,5.523946
+0.020000,14.829649,6.924582,5.512500,4.393497,-0.582854,-3.087575,5.520332
+0.020000,14.892882,6.863938,5.600113,4.380613,-0.644203,-3.067494,5.517102
+0.020000,14.955727,6.803299,5.687443,4.366518,-0.704755,-3.027579,5.514257
+0.020000,15.018191,6.742707,5.774467,4.351223,-0.764759,-3.000212,5.511796
+0.020000,15.080281,6.682203,5.861162,4.334729,-0.824692,-2.996635,5.509722
+0.020000,15.142005,6.621829,5.947503,4.317041,-0.884378,-2.984290,5.508033
+0.020000,15.203369,6.561628,6.033466,4.298164,-0.943879,-2.975063,5.506730
+0.020000,15.264379,6.501640,6.119028,4.278099,-1.003247,-2.968395,5.505815
+0.020000,15.325044,6.441907,6.204165,4.256848,-1.062521,-2.963728,5.505285
+0.020000,15.385365,6.382471,6.288849,4.234177,-1.133583,-3.553069,5.505143
+0.020000,15.445338,6.323384,6.373039,4.209509,-1.233371,-4.989387,5.505388
+0.020000,15.504961,6.264695,6.456701,4.183101,-1.320423,-4.352611,5.506020
+0.020000,15.564239,6.206440,6.539812,4.155539,-1.378086,-2.883138,5.507039
+0.020000,15.623177,6.148660,6.622348,4.126826,-1.435677,-2.879590,5.508444
+0.020000,15.681780,6.091392,6.704287,4.096962,-1.493170,-2.874653,5.510234
+0.020000,15.740053,6.034672,6.785606,4.065952,-1.550524,-2.867663,5.512411
+0.020000,15.797997,5.978538,6.866282,4.033798,-1.607685,-2.858073,5.514972
+0.020000,15.855617,5.923026,6.946292,4.000506,-1.664592,-2.845341,5.517919
+0.020000,15.912913,5.868171,7.025614,3.966083,-1.721145,-2.827669,5.521249
+0.020000,15.969887,5.814007,7.104225,3.930547,-1.776809,-2.783199,5.524962
+0.020000,16.026538,5.760570,7.182103,3.893890,-1.832855,-2.802299,5.529058
+0.020000,16.082867,5.707892,7.259226,3.856132,-1.887893,-2.751861,5.533535
+0.020000,16.138871,5.656005,7.335571,3.817288,-1.942201,-2.715430,5.538392
+0.020000,16.194547,5.604941,7.411119,3.777378,-1.995482,-2.664058,5.543627
+0.020000,16.249894,5.554729,7.485848,3.736435,-2.047162,-2.583962,5.549238
+0.020000,16.304905,5.505401,7.559737,3.694450,-2.099272,-2.605545,5.555223
+0.020000,16.359576,5.456982,7.632766,3.651463,-2.149319,-2.502336,5.561580
+0.020000,16.413901,5.409500,7.704916,3.607530,-2.196685,-2.368269,5.568304
+0.020000,16.467873,5.362981,7.776169,3.562642,-2.244410,-2.386292,5.575393
+0.020000,16.521483,5.317448,7.846506,3.516849,-2.289641,-2.261533,5.582841
+0.020000,16.574723,5.272924,7.915911,3.470218,-2.331511,-2.093493,5.590644
+0.020000,16.627584,5.229429,7.984365,3.422734,-2.374212,-2.135062,5.598797
+0.020000,16.680055,5.186982,8.051856,3.374518,-2.410789,-1.828868,5.607293
+0.020000,16.732125,5.145601,8.118366,3.325539,-2.448972,-1.909119,5.616124
+0.020000,16.783782,5.105300,8.183885,3.275918,-2.481064,-1.604582,5.625284
+0.020000,16.835015,5.066094,8.248398,3.225665,-2.512656,-1.579631,5.634762
+0.020000,16.885810,5.027992,8.311895,3.174849,-2.540786,-1.406493,5.644550
+0.020000,16.936154,4.991004,8.374366,3.123549,-2.564980,-1.209694,5.654636
+0.020000,16.986034,4.955138,8.435802,3.071806,-2.587153,-1.108657,5.665009
+0.020000,17.035436,4.920398,8.496196,3.019684,-2.606105,-0.947590,5.675656
+0.020000,17.084344,4.886785,8.555541,2.967253,-2.621574,-0.773450,5.686564
+0.020000,17.132746,4.854302,8.613832,2.914574,-2.633911,-0.616873,5.697717
+0.020000,17.180626,4.822945,8.671067,2.861712,-2.643139,-0.461397,5.709099
+0.020000,17.227970,4.792711,8.727241,2.808726,-2.649307,-0.308387,5.720695
+0.020000,17.274764,4.763593,8.782355,2.755676,-2.652491,-0.159203,5.732486
+0.020000,17.320993,4.735583,8.836407,2.702626,-2.652495,-0.000177,5.744454
+0.020000,17.366642,4.708670,8.889400,2.649616,-2.650505,0.099481,5.756579
+0.020000,17.411699,4.682842,8.941334,2.596703,-2.645625,0.243973,5.768841
+0.020000,17.456148,4.658085,8.992212,2.543932,-2.638562,0.353165,5.781219
+0.020000,17.499976,4.634382,9.042039,2.491338,-2.629690,0.443605,5.793692
+0.020000,17.543169,4.611717,9.090818,2.438957,-2.619066,0.531191,5.806236
+0.020000,17.585715,4.590069,9.138554,2.386808,-2.607459,0.580348,5.818831
+0.020000,17.627602,4.569417,9.185256,2.335091,-2.585853,1.080292,5.831454
+0.020000,17.668829,4.549733,9.230941,2.284247,-2.542184,2.183463,5.844085
+0.020000,17.709398,4.530989,9.275631,2.234522,-2.486223,2.798083,5.856707
+0.020000,17.749313,4.513155,9.319349,2.185884,-2.431903,2.715984,5.869302
+0.020000,17.788576,4.496200,9.362116,2.138333,-2.377555,2.717395,5.881853
+0.020000,17.827189,4.480097,9.403952,2.091833,-2.325002,2.627646,5.894344
+0.020000,17.865155,4.464816,9.444879,2.046317,-2.275816,2.459283,5.906757
+0.020000,17.902478,4.450328,9.484915,2.001803,-2.225708,2.505403,5.919078
+0.020000,17.939159,4.436604,9.524079,1.958222,-2.179058,2.332514,5.931291
+0.020000,17.975201,4.423617,9.562390,1.915516,-2.135304,2.187686,5.943382
+0.020000,18.010605,4.411338,9.599863,1.873662,-2.092661,2.132144,5.955338
+0.020000,18.045375,4.399741,9.636515,1.832615,-2.052365,2.014833,5.967146
+0.020000,18.079509,4.388798,9.672361,1.792296,-2.015930,1.821719,5.978793
+0.020000,18.113012,4.378483,9.707415,1.752702,-1.979730,1.810040,5.990270
+0.020000,18.145883,4.368772,9.741691,1.713781,-1.946058,1.683560,6.001564
+0.020000,18.178122,4.359638,9.775199,1.675392,-1.919425,1.331665,6.012667
+0.020000,18.209730,4.351058,9.807951,1.637636,-1.887810,1.580758,6.023570
+0.020000,18.240708,4.343007,9.839958,1.600314,-1.866117,1.084663,6.034265
+0.020000,18.271053,4.335462,9.871227,1.563471,-1.842116,1.200029,6.044744
+0.020000,18.300767,4.328401,9.901768,1.527045,-1.821291,1.041245,6.055001
+0.020000,18.329847,4.321801,9.931587,1.490978,-1.803384,0.895334,6.065030
+0.020000,18.358292,4.315642,9.960692,1.455234,-1.787190,0.809746,6.074827
+0.020000,18.386101,4.309901,9.989087,1.419762,-1.773605,0.679220,6.084386
+0.020000,18.413270,4.304559,10.016777,1.384482,-1.763989,0.480816,6.093704
+0.020000,18.439800,4.299596,10.043767,1.349500,-1.749106,0.744148,6.102777
+0.020000,18.465685,4.294992,10.070059,1.314576,-1.746187,0.145934,6.111603
+0.020000,18.490926,4.290730,10.095656,1.279889,-1.734366,0.591035,6.120181
+0.020000,18.515517,4.286791,10.120561,1.245246,-1.732134,0.111644,6.128507
+0.020000,18.539456,4.283157,10.144775,1.210663,-1.729150,0.149176,6.136581
+0.020000,18.562741,4.279812,10.168298,1.176173,-1.724528,0.231108,6.144403
+0.020000,18.585367,4.276739,10.191132,1.141709,-1.723190,0.066899,6.151971
+0.020000,18.607332,4.273922,10.213277,1.107248,-1.723030,0.008005,6.159285
+0.020000,18.628631,4.271345,10.234731,1.072713,-1.726740,-0.185511,6.166346
+0.020000,18.649263,4.268995,10.255496,1.038243,-1.723519,0.161054,6.173155
+0.020000,18.669223,4.266856,10.275570,1.003713,-1.726529,-0.150479,6.179711
+0.020000,18.688507,4.264915,10.294952,0.969076,-1.731824,-0.264750,6.186016
+0.020000,18.707112,4.263158,10.313640,0.934413,-1.733157,-0.066655,6.192071
+0.020000,18.725036,4.261573,10.331634,0.899704,-1.735447,-0.114515,6.197876
+0.020000,18.742275,4.260147,10.348932,0.864882,-1.741088,-0.282055,6.203434
+0.020000,18.758824,4.258869,10.365531,0.829929,-1.747650,-0.328096,6.208746
+0.020000,18.774682,4.257727,10.381429,0.794942,-1.749349,-0.084957,6.213812
+0.020000,18.789848,4.256710,10.396629,0.759973,-1.748467,0.044125,6.218637
+0.020000,18.804330,4.255807,10.411139,0.725528,-1.722269,1.309900,6.223224
+0.020000,18.818144,4.255008,10.424977,0.691859,-1.683424,1.942236,6.227581
+0.020000,18.831304,4.254303,10.438156,0.658947,-1.645626,1.889898,6.231715
+0.020000,18.843822,4.253682,10.450689,0.626652,-1.614748,1.543882,6.235632
+0.020000,18.855715,4.253138,10.462594,0.595291,-1.568013,2.336774,6.239340
+0.020000,18.866996,4.252663,10.473885,0.564535,-1.537838,1.508737,6.242844
+0.020000,18.877680,4.252250,10.484577,0.534616,-1.495948,2.094522,6.246151
+0.020000,18.887784,4.251891,10.494687,0.505489,-1.456328,1.980977,6.249268
+0.020000,18.897321,4.251582,10.504229,0.477112,-1.418863,1.873234,6.252201
+0.020000,18.906307,4.251316,10.513219,0.449497,-1.380716,1.907391,6.254956
+0.020000,18.914757,4.251088,10.521673,0.422659,-1.341910,1.940275,6.257539
+0.020000,18.922687,4.250894,10.529605,0.396610,-1.302476,1.971690,6.259956
+0.020000,18.930112,4.250730,10.537032,0.371361,-1.262447,2.001469,6.262214
+0.020000,18.937049,4.250592,10.543970,0.346924,-1.221857,2.029477,6.264319
+0.020000,18.943514,4.250476,10.550437,0.323309,-1.180745,2.055604,6.266275
+0.020000,18.949524,4.250380,10.556447,0.300526,-1.139150,2.079767,6.268090
+0.020000,18.955092,4.250300,10.562016,0.278434,-1.104609,1.727060,6.269768
+0.020000,18.960237,4.250235,10.567161,0.257257,-1.058848,2.288057,6.271316
+0.020000,18.964975,4.250183,10.571900,0.236932,-1.016237,2.130513,6.272739
+0.020000,18.969321,4.250140,10.576245,0.217290,-0.982077,1.707997,6.274042
+0.020000,18.973294,4.250106,10.580218,0.198653,-0.931880,2.509863,6.275232
+0.020000,18.976907,4.250079,10.583832,0.180685,-0.898382,1.674915,6.276313
+0.020000,18.980180,4.250058,10.587105,0.163656,-0.851467,2.345763,6.277290
+0.020000,18.983130,4.250042,10.590055,0.147487,-0.808429,2.151903,6.278170
+0.020000,18.985770,4.250030,10.592695,0.131989,-0.774895,1.676688,6.278957
+0.020000,18.988120,4.250021,10.595045,0.117500,-0.724483,2.520600,6.279657
+0.020000,18.990197,4.250014,10.597121,0.103837,-0.683142,2.067060,6.280275
+0.020000,18.992016,4.250009,10.598941,0.090963,-0.643674,1.973372,6.280815
+0.020000,18.993594,4.250006,10.600519,0.078899,-0.603213,2.023047,6.281284
+0.020000,18.994949,4.250004,10.601874,0.067759,-0.557005,2.310423,6.281687
+0.020000,18.996098,4.250002,10.603023,0.057460,-0.514922,2.104115,6.282028
+0.020000,18.997058,4.250001,10.603983,0.048006,-0.472702,2.111011,6.282313
+0.020000,18.997846,4.250001,10.604771,0.039399,-0.430365,2.116836,6.282547
+0.020000,18.998479,4.250000,10.605404,0.031641,-0.387932,2.121675,6.282734
+0.020000,18.998974,4.250000,10.605899,0.024732,-0.345420,2.125615,6.282881
+0.020000,18.999347,4.250000,10.606272,0.018675,-0.302845,2.128746,6.282992
+0.020000,18.999617,4.250000,10.606542,0.013471,-0.260222,2.131159,6.283072
+0.020000,18.999799,4.250000,10.606724,0.009120,-0.217563,2.132946,6.283126
+0.020000,18.999912,4.250000,10.606836,0.005622,-0.174879,2.134202,6.283159
+0.020000,18.999971,4.250000,10.606896,0.002978,-0.132178,2.135020,6.283177
+0.020000,18.999995,4.250000,10.606920,0.001189,-0.089468,2.135496,6.283184
+0.020000,19.000000,4.250000,10.606925,0.000254,-0.046754,2.135724,6.283185
+0.020000,19.000000,4.250000,10.606925,0.000000,-0.012698,1.702791,6.283185
diff --git a/src/main/deploy/testPath.pf1.csv b/src/main/deploy/testPath.pf1.csv
new file mode 100644
index 0000000..851d122
--- /dev/null
+++ b/src/main/deploy/testPath.pf1.csv
@@ -0,0 +1,254 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,10.000000,8.000000,0.000012,0.001170,0.058501,2.925055,0.002544
+0.020000,10.000000,8.000000,0.000059,0.003510,0.117002,2.925055,0.002533
+0.020000,10.000000,8.000000,0.000164,0.007020,0.175503,2.925055,0.002511
+0.020000,10.000000,8.000000,0.000351,0.011700,0.234004,2.925055,0.002470
+0.020000,10.000000,8.000000,0.000644,0.017550,0.292505,2.925055,0.002407
+0.020000,10.000000,8.000000,0.001065,0.024570,0.351007,2.925055,0.002316
+0.020000,10.000000,8.000000,0.001638,0.032761,0.409508,2.925055,0.002192
+0.020000,10.000000,8.000000,0.002387,0.042121,0.468009,2.925055,0.002030
+0.020000,10.000000,8.000000,0.003335,0.052651,0.526510,2.925055,0.001825
+0.020000,10.000000,8.000000,0.004505,0.064351,0.585011,2.925055,0.001573
+0.020000,10.000000,8.000000,0.005920,0.077221,0.643512,2.925055,0.001267
+0.020000,10.000000,8.000000,0.007605,0.091262,0.702013,2.925055,0.000902
+0.020000,10.000000,8.000000,0.009582,0.106472,0.760514,2.925055,0.000475
+0.020000,10.000098,8.000000,0.011876,0.122852,0.819015,2.925055,6.283164
+0.020000,10.002732,7.999999,0.014508,0.140403,0.877516,2.925055,6.282594
+0.020000,10.005728,7.999996,0.017504,0.159123,0.936018,2.925055,6.281946
+0.020000,10.009109,7.999991,0.020885,0.179013,0.994519,2.925055,6.281214
+0.020000,10.012899,7.999982,0.024676,0.200074,1.053020,2.925055,6.280392
+0.020000,10.017124,7.999968,0.028900,0.222304,1.111521,2.925055,6.279476
+0.020000,10.021803,7.999949,0.033580,0.245705,1.170022,2.925055,6.278460
+0.020000,10.026963,7.999921,0.038739,0.270275,1.228523,2.925055,6.277339
+0.020000,10.032626,7.999885,0.044402,0.296016,1.287024,2.925055,6.276108
+0.020000,10.038815,7.999837,0.050592,0.322926,1.345525,2.925055,6.274760
+0.020000,10.045553,7.999775,0.057331,0.351007,1.404026,2.925055,6.273291
+0.020000,10.052867,7.999697,0.064644,0.380257,1.462527,2.925055,6.271695
+0.020000,10.060775,7.999599,0.072553,0.410678,1.521029,2.925055,6.269966
+0.020000,10.069303,7.999478,0.081083,0.442268,1.579530,2.925055,6.268099
+0.020000,10.078476,7.999331,0.090255,0.475029,1.638031,2.925055,6.266088
+0.020000,10.088314,7.999152,0.100095,0.508960,1.696532,2.925055,6.263927
+0.020000,10.098842,7.998937,0.110626,0.544060,1.755033,2.925055,6.261610
+0.020000,10.110083,7.998680,0.121869,0.580331,1.813534,2.925055,6.259132
+0.020000,10.122060,7.998376,0.133851,0.617772,1.872035,2.925055,6.256486
+0.020000,10.134797,7.998018,0.146592,0.656382,1.930536,2.925055,6.253666
+0.020000,10.148315,7.997599,0.160118,0.696163,1.989037,2.925055,6.250666
+0.020000,10.162638,7.997110,0.174450,0.737114,2.047538,2.925055,6.247480
+0.020000,10.177792,7.996543,0.189614,0.779235,2.106039,2.925055,6.244100
+0.020000,10.193797,7.995889,0.205631,0.822525,2.164541,2.925055,6.240522
+0.020000,10.210674,7.995136,0.222526,0.866986,2.223042,2.925055,6.236738
+0.020000,10.228450,7.994274,0.240323,0.912617,2.281543,2.925055,6.232741
+0.020000,10.247145,7.993291,0.259043,0.959418,2.340044,2.925055,6.228526
+0.020000,10.266779,7.992173,0.278711,1.007389,2.398545,2.925055,6.224084
+0.020000,10.287380,7.990906,0.299350,1.056530,2.457046,2.925055,6.219410
+0.020000,10.308967,7.989474,0.320984,1.106841,2.515547,2.925055,6.214496
+0.020000,10.331550,7.987862,0.343624,1.157152,2.515547,0.000000,6.209338
+0.020000,10.355126,7.986054,0.367270,1.207463,2.515547,-0.000000,6.203935
+0.020000,10.379694,7.984033,0.391922,1.257774,2.515547,0.000000,6.198286
+0.020000,10.405255,7.981781,0.417581,1.308085,2.515547,0.000000,6.192387
+0.020000,10.431803,7.979282,0.444246,1.358395,2.515547,0.000000,6.186239
+0.020000,10.459334,7.976516,0.471917,1.408706,2.515547,0.000000,6.179840
+0.020000,10.487849,7.973462,0.500594,1.459017,2.515547,0.000000,6.173190
+0.020000,10.517342,7.970102,0.530277,1.509328,2.515547,0.000000,6.166286
+0.020000,10.547808,7.966414,0.560967,1.559639,2.515547,0.000000,6.159130
+0.020000,10.579245,7.962375,0.592663,1.609950,2.515547,0.000000,6.151719
+0.020000,10.611648,7.957964,0.625365,1.660261,2.515547,0.000000,6.144055
+0.020000,10.645012,7.953158,0.659073,1.710572,2.515547,0.000000,6.136137
+0.020000,10.679331,7.947931,0.693788,1.760883,2.515547,0.000000,6.127966
+0.020000,10.714599,7.942260,0.729509,1.811194,2.515547,0.000000,6.119542
+0.020000,10.750809,7.936120,0.766236,1.861505,2.515547,0.000000,6.110867
+0.020000,10.787954,7.929484,0.803969,1.911816,2.515547,0.000000,6.101944
+0.020000,10.826026,7.922327,0.842708,1.962127,2.515547,0.000000,6.092773
+0.020000,10.865018,7.914621,0.882454,2.012438,2.515547,0.000000,6.083358
+0.020000,10.904919,7.906339,0.923206,2.062749,2.515547,-0.000000,6.073702
+0.020000,10.945720,7.897453,0.964964,2.113060,2.515547,0.000000,6.063810
+0.020000,10.987411,7.887936,1.007728,2.163371,2.515547,-0.000000,6.053685
+0.020000,11.029982,7.877758,1.051499,2.213681,2.515547,0.000000,6.043333
+0.020000,11.073420,7.866891,1.096275,2.263992,2.515547,-0.000000,6.032760
+0.020000,11.117713,7.855306,1.142058,2.314303,2.515547,0.000000,6.021973
+0.020000,11.162847,7.842974,1.188848,2.364614,2.515547,-0.000000,6.010979
+0.020000,11.208810,7.829867,1.236643,2.414925,2.515547,0.000000,5.999787
+0.020000,11.255586,7.815954,1.285445,2.465236,2.515547,0.000000,5.988405
+0.020000,11.303160,7.801208,1.335252,2.515547,2.515547,-0.000000,5.976844
+0.020000,11.351518,7.785600,1.386066,2.565858,2.515547,0.000000,5.965113
+0.020000,11.400641,7.769101,1.437887,2.616169,2.515547,0.000000,5.953225
+0.020000,11.450513,7.751685,1.490713,2.666480,2.515547,-0.000000,5.941192
+0.020000,11.501117,7.733323,1.544546,2.716791,2.515547,0.000000,5.929027
+0.020000,11.552435,7.713989,1.599385,2.767102,2.515547,0.000000,5.916744
+0.020000,11.604447,7.693657,1.655230,2.817413,2.515547,-0.000000,5.904357
+0.020000,11.657134,7.672302,1.712081,2.867724,2.515547,0.000000,5.891881
+0.020000,11.710478,7.649899,1.769939,2.918035,2.515547,-0.000000,5.879332
+0.020000,11.764459,7.626427,1.828803,2.968346,2.515547,0.000000,5.866727
+0.020000,11.819057,7.601862,1.888673,3.018657,2.515547,0.000000,5.854083
+0.020000,11.874252,7.576183,1.949549,3.068968,2.515547,-0.000000,5.841416
+0.020000,11.930024,7.549371,2.011432,3.119278,2.515547,0.000000,5.828746
+0.020000,11.986353,7.521408,2.074320,3.169589,2.515547,-0.000000,5.816089
+0.020000,12.043209,7.492281,2.138203,3.218730,2.457046,-2.925055,5.803466
+0.020000,12.100553,7.461987,2.203058,3.266701,2.398545,-2.925055,5.790901
+0.020000,12.158345,7.430524,2.268860,3.313502,2.340044,-2.925055,5.778415
+0.020000,12.216547,7.397892,2.335586,3.359133,2.281543,-2.925055,5.766030
+0.020000,12.275123,7.364096,2.403213,3.403594,2.223042,-2.925055,5.753769
+0.020000,12.334038,7.329140,2.471718,3.446885,2.164541,-2.925055,5.741651
+0.020000,12.393257,7.293033,2.541077,3.489005,2.106039,-2.925055,5.729698
+0.020000,12.452747,7.255785,2.611267,3.529956,2.047538,-2.925055,5.717927
+0.020000,12.512478,7.217409,2.682264,3.569737,1.989037,-2.925055,5.706359
+0.020000,12.572420,7.177920,2.754044,3.608348,1.930536,-2.925055,5.695009
+0.020000,12.632545,7.137334,2.826586,3.645788,1.872035,-2.925055,5.683895
+0.020000,12.692827,7.095671,2.899864,3.682059,1.813534,-2.925055,5.673031
+0.020000,12.753241,7.052952,2.973856,3.717160,1.755033,-2.925055,5.662434
+0.020000,12.813764,7.009199,3.048539,3.751090,1.696532,-2.925055,5.652115
+0.020000,12.874376,6.964435,3.123888,3.783851,1.638031,-2.925055,5.642087
+0.020000,12.935056,6.918689,3.199881,3.815442,1.579530,-2.925055,5.632363
+0.020000,12.995787,6.871985,3.276494,3.845862,1.521029,-2.925055,5.622952
+0.020000,13.056554,6.824354,3.353704,3.875113,1.462527,-2.925055,5.613863
+0.020000,13.117341,6.775825,3.431487,3.903193,1.404026,-2.925055,5.605106
+0.020000,13.178136,6.726428,3.509820,3.930104,1.345525,-2.925055,5.596687
+0.020000,13.238927,6.676197,3.588680,3.955844,1.287024,-2.925055,5.588615
+0.020000,13.299705,6.625163,3.668042,3.980415,1.228523,-2.925055,5.580895
+0.020000,13.360462,6.573362,3.747884,4.003815,1.170022,-2.925055,5.573532
+0.020000,13.421190,6.520827,3.828183,4.026045,1.111521,-2.925055,5.566531
+0.020000,13.481884,6.467593,3.908915,4.047106,1.053020,-2.925055,5.559896
+0.020000,13.542539,6.413698,3.990056,4.066996,0.994519,-2.925055,5.553631
+0.020000,13.603153,6.359176,4.071583,4.085717,0.936018,-2.925055,5.547739
+0.020000,13.663724,6.304065,4.153473,4.103267,0.877516,-2.925055,5.542222
+0.020000,13.724249,6.248403,4.235702,4.119647,0.819015,-2.925055,5.537083
+0.020000,13.784730,6.192227,4.318247,4.134858,0.760514,-2.925055,5.532323
+0.020000,13.845167,6.135575,4.401084,4.148898,0.702013,-2.925055,5.527943
+0.020000,13.905561,6.078486,4.484191,4.161768,0.643512,-2.925055,5.523946
+0.020000,13.965916,6.020997,4.567543,4.173468,0.585011,-2.925055,5.520332
+0.020000,14.026234,5.963149,4.651118,4.183998,0.526510,-2.925055,5.517102
+0.020000,14.086519,5.904979,4.734892,4.193359,0.468009,-2.925055,5.514257
+0.020000,14.146776,5.846528,4.818841,4.201549,0.409508,-2.925055,5.511796
+0.020000,14.207009,5.787834,4.902942,4.208569,0.351007,-2.925055,5.509722
+0.020000,14.267224,5.728936,4.987172,4.214419,0.292505,-2.925055,5.508033
+0.020000,14.327425,5.669875,5.071507,4.219099,0.234004,-2.925055,5.506730
+0.020000,14.387619,5.610690,5.155924,4.222609,0.175503,-2.925055,5.505815
+0.020000,14.447813,5.551420,5.240400,4.224949,0.117002,-2.925055,5.505285
+0.020000,14.508008,5.492109,5.324905,4.225645,0.034780,-4.111090,5.505143
+0.020000,14.568199,5.432808,5.409402,4.224000,-0.082222,-5.850110,5.505388
+0.020000,14.628384,5.373564,5.493854,4.221186,-0.140723,-2.925055,5.506020
+0.020000,14.688570,5.314418,5.578238,4.217201,-0.199224,-2.925055,5.507039
+0.020000,14.748763,5.255408,5.662530,4.212047,-0.257725,-2.925055,5.508444
+0.020000,14.808967,5.196575,5.746708,4.205722,-0.316226,-2.925055,5.510234
+0.020000,14.869189,5.137958,5.830747,4.198228,-0.374727,-2.925055,5.512411
+0.020000,14.929433,5.079596,5.914625,4.189563,-0.433228,-2.925055,5.514972
+0.020000,14.989705,5.021529,5.998318,4.179729,-0.491729,-2.925055,5.517919
+0.020000,15.050008,4.963795,6.081803,4.168724,-0.550231,-2.925055,5.521249
+0.020000,15.110346,4.906434,6.165055,4.156549,-0.608732,-2.925055,5.524962
+0.020000,15.170722,4.849483,6.248053,4.143205,-0.667233,-2.925055,5.529058
+0.020000,15.231138,4.792983,6.330772,4.128690,-0.725734,-2.925055,5.533535
+0.020000,15.291596,4.736970,6.413189,4.113005,-0.784235,-2.925055,5.538392
+0.020000,15.352095,4.681483,6.495280,4.096151,-0.842736,-2.925055,5.543627
+0.020000,15.412637,4.626559,6.577023,4.078126,-0.901237,-2.925055,5.549238
+0.020000,15.473218,4.572236,6.658394,4.058931,-0.959738,-2.925055,5.555223
+0.020000,15.533837,4.518549,6.739369,4.038566,-1.018239,-2.925055,5.561580
+0.020000,15.594491,4.465537,6.819925,4.017032,-1.076740,-2.925055,5.568304
+0.020000,15.655175,4.413233,6.900038,3.994327,-1.135242,-2.925055,5.575393
+0.020000,15.715881,4.361673,6.979686,3.970452,-1.193743,-2.925055,5.582841
+0.020000,15.776605,4.310891,7.058845,3.945407,-1.252244,-2.925055,5.590644
+0.020000,15.837335,4.260922,7.137491,3.919192,-1.310745,-2.925055,5.598797
+0.020000,15.898062,4.211796,7.215601,3.891807,-1.369246,-2.925055,5.607293
+0.020000,15.958775,4.163547,7.293151,3.863252,-1.427747,-2.925055,5.616124
+0.020000,16.019460,4.116204,7.370119,3.833527,-1.486248,-2.925055,5.625284
+0.020000,16.080102,4.069797,7.446481,3.802632,-1.544749,-2.925055,5.634762
+0.020000,16.140685,4.024354,7.522213,3.770567,-1.603250,-2.925055,5.644550
+0.020000,16.201190,3.979903,7.597292,3.737332,-1.661751,-2.925055,5.654636
+0.020000,16.261597,3.936467,7.671694,3.702927,-1.720253,-2.925055,5.665009
+0.020000,16.321885,3.894071,7.745397,3.667352,-1.778754,-2.925055,5.675656
+0.020000,16.382031,3.852737,7.818377,3.630607,-1.837255,-2.925055,5.686564
+0.020000,16.442008,3.812486,7.890610,3.592692,-1.895756,-2.925055,5.697717
+0.020000,16.501792,3.773334,7.962073,3.553607,-1.954257,-2.925055,5.709099
+0.020000,16.561352,3.735299,8.032742,3.513352,-2.012758,-2.925055,5.720695
+0.020000,16.620660,3.698394,8.102595,3.471926,-2.071259,-2.925055,5.732486
+0.020000,16.679683,3.662632,8.171608,3.429331,-2.129760,-2.925055,5.744454
+0.020000,16.738389,3.628023,8.239757,3.385566,-2.188261,-2.925055,5.756579
+0.020000,16.796744,3.594573,8.307019,3.340631,-2.246762,-2.925055,5.768841
+0.020000,16.854710,3.562287,8.373370,3.294526,-2.305263,-2.925055,5.781219
+0.020000,16.912252,3.531169,8.438788,3.247250,-2.363765,-2.925055,5.793692
+0.020000,16.969331,3.501217,8.503248,3.198805,-2.422266,-2.925055,5.806236
+0.020000,17.025908,3.472430,8.566728,3.149190,-2.480767,-2.925055,5.818831
+0.020000,17.081947,3.444801,8.629209,3.098879,-2.515547,-1.739020,5.831454
+0.020000,17.137423,3.418315,8.690683,3.048568,-2.515547,0.000000,5.844085
+0.020000,17.192315,3.392953,8.751152,2.998257,-2.515547,-0.000000,5.856707
+0.020000,17.246604,3.368697,8.810614,2.947946,-2.515547,0.000000,5.869302
+0.020000,17.300270,3.345524,8.869070,2.897635,-2.515547,0.000000,5.881853
+0.020000,17.353293,3.323411,8.926519,2.847324,-2.515547,-0.000000,5.894344
+0.020000,17.405654,3.302337,8.982963,2.797013,-2.515547,0.000000,5.906757
+0.020000,17.457333,3.282276,9.038400,2.746702,-2.515547,-0.000000,5.919078
+0.020000,17.508313,3.263203,9.092831,2.696391,-2.515547,0.000000,5.931291
+0.020000,17.558574,3.245092,9.146255,2.646080,-2.515547,0.000000,5.943382
+0.020000,17.608098,3.227916,9.198674,2.595769,-2.515547,-0.000000,5.955338
+0.020000,17.656869,3.211648,9.250086,2.545458,-2.515547,0.000000,5.967146
+0.020000,17.704868,3.196261,9.300492,2.495147,-2.515547,-0.000000,5.978793
+0.020000,17.752081,3.181726,9.349892,2.444836,-2.515547,0.000000,5.990270
+0.020000,17.798491,3.168014,9.398286,2.394525,-2.515547,0.000000,6.001564
+0.020000,17.844083,3.155098,9.445673,2.344215,-2.515547,-0.000000,6.012667
+0.020000,17.888845,3.142947,9.492054,2.293904,-2.515547,0.000000,6.023570
+0.020000,17.932760,3.131533,9.537429,2.243593,-2.515547,0.000000,6.034265
+0.020000,17.975818,3.120828,9.581798,2.193282,-2.515547,-0.000000,6.044744
+0.020000,18.018005,3.110803,9.625160,2.142971,-2.515547,0.000000,6.055001
+0.020000,18.059311,3.101428,9.667517,2.092660,-2.515547,0.000000,6.065030
+0.020000,18.099724,3.092677,9.708867,2.042349,-2.515547,-0.000000,6.074827
+0.020000,18.139235,3.084520,9.749211,1.992038,-2.515547,0.000000,6.084386
+0.020000,18.177833,3.076931,9.788548,1.941727,-2.515547,0.000000,6.093704
+0.020000,18.215511,3.069882,9.826880,1.891416,-2.515547,-0.000000,6.102777
+0.020000,18.252259,3.063347,9.864205,1.841105,-2.515547,-0.000000,6.111603
+0.020000,18.288071,3.057300,9.900524,1.790794,-2.515547,0.000000,6.120181
+0.020000,18.322940,3.051714,9.935837,1.740483,-2.515547,0.000000,6.128507
+0.020000,18.356857,3.046566,9.970143,1.690172,-2.515547,-0.000000,6.136581
+0.020000,18.389819,3.041830,10.003444,1.639861,-2.515547,-0.000000,6.144403
+0.020000,18.421819,3.037484,10.035738,1.589550,-2.515547,0.000000,6.151971
+0.020000,18.452853,3.033504,10.067026,1.539239,-2.515547,0.000000,6.159285
+0.020000,18.482915,3.029868,10.097307,1.488928,-2.515547,0.000000,6.166346
+0.020000,18.512002,3.026554,10.126583,1.438618,-2.515547,-0.000000,6.173155
+0.020000,18.540111,3.023542,10.154852,1.388307,-2.515547,-0.000000,6.179711
+0.020000,18.567236,3.020812,10.182115,1.337996,-2.515547,0.000000,6.186016
+0.020000,18.593376,3.018343,10.208372,1.287685,-2.515547,0.000000,6.192071
+0.020000,18.618529,3.016119,10.233622,1.237374,-2.515547,-0.000000,6.197876
+0.020000,18.642692,3.014120,10.257867,1.187063,-2.515547,-0.000000,6.203434
+0.020000,18.665861,3.012331,10.281105,1.136752,-2.515547,0.000000,6.208746
+0.020000,18.688036,3.010733,10.303337,1.086441,-2.515547,0.000000,6.213812
+0.020000,18.709218,3.009313,10.324567,1.036604,-2.491826,1.186035,6.218637
+0.020000,18.729423,3.008053,10.344813,0.987938,-2.433325,2.925055,6.223224
+0.020000,18.748675,3.006940,10.364097,0.940441,-2.374824,2.925055,6.227581
+0.020000,18.766995,3.005958,10.382442,0.894115,-2.316323,2.925055,6.231715
+0.020000,18.784403,3.005095,10.399873,0.848959,-2.257822,2.925055,6.235632
+0.020000,18.800926,3.004340,10.416412,0.804972,-2.199321,2.925055,6.239340
+0.020000,18.816583,3.003680,10.432083,0.762156,-2.140820,2.925055,6.242844
+0.020000,18.831398,3.003107,10.446910,0.720509,-2.082319,2.925055,6.246151
+0.020000,18.845395,3.002610,10.460915,0.680033,-2.023818,2.925055,6.249268
+0.020000,18.858596,3.002182,10.474123,0.640727,-1.965317,2.925055,6.252201
+0.020000,18.871024,3.001814,10.486556,0.602590,-1.906815,2.925055,6.254956
+0.020000,18.882702,3.001499,10.498238,0.565624,-1.848314,2.925055,6.257539
+0.020000,18.893653,3.001231,10.509193,0.529828,-1.789813,2.925055,6.259956
+0.020000,18.903900,3.001005,10.519443,0.495202,-1.731312,2.925055,6.262214
+0.020000,18.913467,3.000814,10.529013,0.461745,-1.672811,2.925055,6.264319
+0.020000,18.922378,3.000655,10.537925,0.429459,-1.614310,2.925055,6.266275
+0.020000,18.930656,3.000522,10.546203,0.398343,-1.555809,2.925055,6.268090
+0.020000,18.938322,3.000413,10.553870,0.368397,-1.497308,2.925055,6.269768
+0.020000,18.945401,3.000323,10.560950,0.339621,-1.438807,2.925055,6.271316
+0.020000,18.951918,3.000251,10.567467,0.312015,-1.380306,2.925055,6.272739
+0.020000,18.957892,3.000192,10.573443,0.285578,-1.321805,2.925055,6.274042
+0.020000,18.963352,3.000146,10.578901,0.260312,-1.263303,2.925055,6.275232
+0.020000,18.968317,3.000109,10.583867,0.236216,-1.204802,2.925055,6.276313
+0.020000,18.972812,3.000080,10.588362,0.213290,-1.146301,2.925055,6.277290
+0.020000,18.976861,3.000058,10.592410,0.191534,-1.087800,2.925055,6.278170
+0.020000,18.980484,3.000041,10.596035,0.170948,-1.029299,2.925055,6.278957
+0.020000,18.983709,3.000029,10.599260,0.151532,-0.970798,2.925055,6.279657
+0.020000,18.986558,3.000020,10.602108,0.133286,-0.912297,2.925055,6.280275
+0.020000,18.989054,3.000013,10.604603,0.116211,-0.853796,2.925055,6.280815
+0.020000,18.991218,3.000008,10.606768,0.100305,-0.795295,2.925055,6.281284
+0.020000,18.993076,3.000005,10.608627,0.085569,-0.736794,2.925055,6.281687
+0.020000,18.994652,3.000003,10.610202,0.072003,-0.678292,2.925055,6.282028
+0.020000,18.995968,3.000002,10.611519,0.059607,-0.619791,2.925055,6.282313
+0.020000,18.997048,3.000001,10.612598,0.048381,-0.561290,2.925055,6.282547
+0.020000,18.997916,3.000000,10.613466,0.038325,-0.502789,2.925055,6.282734
+0.020000,18.998594,3.000000,10.614143,0.029440,-0.444288,2.925055,6.282881
+0.020000,18.999105,3.000000,10.614655,0.021724,-0.385787,2.925055,6.282992
+0.020000,18.999475,3.000000,10.615024,0.015178,-0.327286,2.925055,6.283072
+0.020000,18.999725,3.000000,10.615274,0.009803,-0.268785,2.925055,6.283126
+0.020000,18.999879,3.000000,10.615428,0.005597,-0.210284,2.925055,6.283159
+0.020000,18.999960,3.000000,10.615509,0.002561,-0.151783,2.925055,6.283177
+0.020000,18.999993,3.000000,10.615542,0.000696,-0.093281,2.925055,6.283184
+0.020000,19.000000,3.000000,10.615549,0.000000,-0.034780,2.925055,6.283185
+0.020000,19.000000,3.000000,10.615549,0.000000,0.000000,1.739020,6.283185
diff --git a/src/main/deploy/testPath.right.pf1.csv b/src/main/deploy/testPath.right.pf1.csv
new file mode 100644
index 0000000..c568db0
--- /dev/null
+++ b/src/main/deploy/testPath.right.pf1.csv
@@ -0,0 +1,254 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,10.003179,6.750004,0.000012,0.001170,0.058501,2.925055,0.002544
+0.020000,10.003167,6.750004,0.000024,0.000632,-0.026898,-4.269935,0.002533
+0.020000,10.003138,6.750004,0.000053,0.001422,0.039505,3.320118,0.002511
+0.020000,10.003088,6.750004,0.000103,0.002528,0.055308,0.790150,0.002470
+0.020000,10.003009,6.750004,0.000182,0.003951,0.071112,0.790233,0.002407
+0.020000,10.002895,6.750003,0.000296,0.005689,0.086920,0.790370,0.002316
+0.020000,10.002740,6.750003,0.000451,0.007744,0.102731,0.790573,0.002192
+0.020000,10.002538,6.750003,0.000653,0.010115,0.118548,0.790854,0.002030
+0.020000,10.002282,6.750002,0.000909,0.012802,0.134373,0.791228,0.001825
+0.020000,10.001966,6.750002,0.001225,0.015806,0.150207,0.791705,0.001573
+0.020000,10.001583,6.750001,0.001608,0.019127,0.166053,0.792298,0.001267
+0.020000,10.001128,6.750001,0.002063,0.022765,0.181913,0.793017,0.000902
+0.020000,10.000593,6.750000,0.002598,0.026721,0.197791,0.793874,0.000475
+0.020000,10.000072,6.750000,0.003120,0.026095,-0.031298,-11.454459,6.283164
+0.020000,10.001993,6.749999,0.005041,0.096086,3.499514,176.540644,6.282594
+0.020000,10.004179,6.749997,0.007227,0.109265,0.658992,-142.026126,6.281946
+0.020000,10.006644,6.749993,0.009692,0.123276,0.700509,2.075851,6.281214
+0.020000,10.009407,6.749987,0.012456,0.138169,0.744668,2.207958,6.280392
+0.020000,10.012487,6.749977,0.015535,0.153983,0.790684,2.300810,6.279476
+0.020000,10.015897,6.749962,0.018945,0.170474,0.824573,1.694440,6.278460
+0.020000,10.019655,6.749943,0.022704,0.187948,0.873717,2.457189,6.277339
+0.020000,10.023779,6.749916,0.026827,0.206160,0.910594,1.843865,6.276108
+0.020000,10.028284,6.749881,0.031332,0.225257,0.954851,2.212859,6.274760
+0.020000,10.033186,6.749836,0.036234,0.245119,0.993074,1.911150,6.273291
+0.020000,10.038504,6.749779,0.041553,0.265918,1.039974,2.344977,6.271695
+0.020000,10.044251,6.749708,0.047301,0.287392,1.073686,1.685592,6.269966
+0.020000,10.050446,6.749621,0.053496,0.309766,1.118675,2.249486,6.268099
+0.020000,10.057105,6.749513,0.060156,0.332995,1.161451,2.138796,6.266088
+0.020000,10.064243,6.749384,0.067295,0.356947,1.197632,1.809031,6.263927
+0.020000,10.071875,6.749228,0.074929,0.381718,1.238547,2.045762,6.261610
+0.020000,10.080019,6.749042,0.083075,0.407297,1.278921,2.018674,6.259132
+0.020000,10.088690,6.748822,0.091748,0.433671,1.318715,1.989718,6.256486
+0.020000,10.097903,6.748563,0.100965,0.460829,1.357895,1.959009,6.253666
+0.020000,10.107673,6.748260,0.110740,0.488757,1.396429,1.926694,6.250666
+0.020000,10.118016,6.747907,0.121089,0.517443,1.434288,1.892947,6.247480
+0.020000,10.128949,6.747498,0.132029,0.547012,1.478452,2.208186,6.244100
+0.020000,10.140484,6.747026,0.143574,0.577238,1.511288,1.641790,6.240522
+0.020000,10.152636,6.746484,0.155738,0.608210,1.548594,1.865338,6.236738
+0.020000,10.165422,6.745864,0.168539,0.640045,1.591770,2.158805,6.232741
+0.020000,10.178854,6.745158,0.181990,0.672532,1.624355,1.629227,6.228526
+0.020000,10.192946,6.744355,0.196105,0.705756,1.661181,1.841302,6.224084
+0.020000,10.207715,6.743447,0.210902,0.739847,1.704573,2.169624,6.219410
+0.020000,10.223173,6.742422,0.226394,0.774582,1.736724,1.607528,6.214496
+0.020000,10.239325,6.741269,0.242586,0.809640,1.752898,0.808680,6.209338
+0.020000,10.256167,6.739977,0.259478,0.844602,1.748086,-0.240606,6.203935
+0.020000,10.273697,6.738535,0.277068,0.879458,1.742840,-0.262290,6.198286
+0.020000,10.291913,6.736931,0.295354,0.914327,1.743427,0.029343,6.192387
+0.020000,10.310810,6.735152,0.314334,0.948999,1.733620,-0.490324,6.186239
+0.020000,10.330383,6.733185,0.334006,0.983596,1.729820,-0.189995,6.179840
+0.020000,10.350632,6.731017,0.354370,1.018222,1.731319,0.074960,6.173190
+0.020000,10.371551,6.728633,0.375425,1.052711,1.724448,-0.343561,6.166286
+0.020000,10.393136,6.726020,0.397168,1.087158,1.722355,-0.104652,6.159130
+0.020000,10.415386,6.723162,0.419600,1.121628,1.723499,0.057195,6.151719
+0.020000,10.438296,6.720043,0.442722,1.156086,1.722909,-0.029512,6.144055
+0.020000,10.461863,6.716648,0.466533,1.190522,1.721800,-0.055459,6.136137
+0.020000,10.486085,6.712959,0.491033,1.225035,1.725637,0.191846,6.127966
+0.020000,10.510957,6.708960,0.516225,1.259581,1.727302,0.083256,6.119542
+0.020000,10.536476,6.704632,0.542108,1.294172,1.729565,0.113142,6.110867
+0.020000,10.562640,6.699958,0.568687,1.328916,1.737157,0.379615,6.101944
+0.020000,10.589446,6.694919,0.595962,1.363790,1.743701,0.327215,6.092773
+0.020000,10.616893,6.689495,0.623939,1.398855,1.753271,0.478473,6.083358
+0.020000,10.644976,6.683666,0.652621,1.434091,1.761806,0.426763,6.073702
+0.020000,10.673695,6.677411,0.682013,1.469597,1.775316,0.675529,6.063810
+0.020000,10.703048,6.670710,0.712121,1.505402,1.790214,0.744876,6.053685
+0.020000,10.733033,6.663541,0.742952,1.541536,1.806701,0.824372,6.043333
+0.020000,10.763650,6.655882,0.774513,1.578035,1.824974,0.913640,6.032760
+0.020000,10.794898,6.647709,0.806812,1.614940,1.845218,1.012167,6.021973
+0.020000,10.826776,6.638999,0.839858,1.652323,1.869153,1.196763,6.010979
+0.020000,10.859285,6.629728,0.873663,1.690246,1.896170,1.350862,5.999787
+0.020000,10.892424,6.619872,0.908237,1.728701,1.922766,1.329802,5.988405
+0.020000,10.926194,6.609404,0.943592,1.767772,1.953534,1.538400,5.976844
+0.020000,10.960598,6.598300,0.979743,1.807551,1.988931,1.769835,5.965113
+0.020000,10.995635,6.586532,1.016704,1.848014,2.023184,1.712647,5.953225
+0.020000,11.031307,6.574075,1.054489,1.889245,2.061548,1.918239,5.941192
+0.020000,11.067616,6.560899,1.093114,1.931295,2.102497,2.047434,5.929027
+0.020000,11.104565,6.546978,1.132599,1.974227,2.146615,2.205881,5.916744
+0.020000,11.142156,6.532284,1.172960,2.018039,2.190568,2.197677,5.904357
+0.020000,11.180391,6.516786,1.214216,2.062807,2.238414,2.392298,5.891881
+0.020000,11.219273,6.500457,1.256387,2.108577,2.288516,2.505090,5.879332
+0.020000,11.258804,6.483267,1.299495,2.155365,2.339388,2.543594,5.866727
+0.020000,11.298988,6.465187,1.343559,2.203207,2.392104,2.635803,5.854083
+0.020000,11.339828,6.446187,1.388602,2.252141,2.446684,2.729010,5.841416
+0.020000,11.381325,6.426237,1.434645,2.302186,2.502230,2.777312,5.828746
+0.020000,11.423483,6.405308,1.481713,2.353358,2.558606,2.818752,5.816089
+0.020000,11.466297,6.383375,1.529817,2.405229,2.593558,1.747603,5.803466
+0.020000,11.509752,6.360418,1.578964,2.457353,2.606197,0.631989,5.790901
+0.020000,11.553836,6.336417,1.629158,2.509711,2.617930,0.586650,5.778415
+0.020000,11.598536,6.311356,1.680404,2.562275,2.628188,0.512882,5.766030
+0.020000,11.643836,6.285218,1.732704,2.615011,2.636777,0.429452,5.753769
+0.020000,11.689724,6.257991,1.786062,2.667876,2.643266,0.324434,5.741651
+0.020000,11.736185,6.229662,1.840478,2.720825,2.647462,0.209828,5.729698
+0.020000,11.783205,6.200222,1.895954,2.773812,2.649344,0.094086,5.717927
+0.020000,11.830770,6.169662,1.952490,2.826778,2.648290,-0.052698,5.706359
+0.020000,11.878864,6.137978,2.010083,2.879664,2.644313,-0.198866,5.695009
+0.020000,11.927474,6.105164,2.068732,2.932411,2.637315,-0.349882,5.683895
+0.020000,11.976585,6.071222,2.128431,2.984955,2.627227,-0.504389,5.673031
+0.020000,12.026182,6.036150,2.189175,3.037235,2.614007,-0.661004,5.662434
+0.020000,12.076252,5.999953,2.250959,3.089188,2.597640,-0.818352,5.652115
+0.020000,12.126781,5.962636,2.313774,3.140751,2.578138,-0.975102,5.642087
+0.020000,12.177755,5.924206,2.377611,3.191844,2.554677,-1.173045,5.632363
+0.020000,12.229160,5.884674,2.442460,3.242428,2.529198,-1.273945,5.622952
+0.020000,12.280985,5.844051,2.508309,3.292449,2.501031,-1.408378,5.613863
+0.020000,12.333217,5.802351,2.575145,3.341803,2.467716,-1.665749,5.605106
+0.020000,12.385845,5.759590,2.642955,3.390505,2.435107,-1.630464,5.596687
+0.020000,12.438858,5.715786,2.711723,3.438435,2.396476,-1.931550,5.588615
+0.020000,12.492245,5.670958,2.781436,3.485609,2.358721,-1.887743,5.580895
+0.020000,12.545998,5.625127,2.852074,3.531911,2.315080,-2.182012,5.573532
+0.020000,12.600108,5.578317,2.923621,3.577370,2.272949,-2.106574,5.566531
+0.020000,12.654567,5.530552,2.996059,3.621905,2.226766,-2.309153,5.559896
+0.020000,12.709368,5.481858,3.069369,3.665477,2.178598,-2.408380,5.553631
+0.020000,12.764506,5.432262,3.143531,3.708088,2.130552,-2.402324,5.547739
+0.020000,12.819975,5.381792,3.218524,3.749688,2.079985,-2.528335,5.542222
+0.020000,12.875772,5.330478,3.294329,3.790239,2.027540,-2.622267,5.537083
+0.020000,12.931893,5.278351,3.370924,3.829742,1.975176,-2.618199,5.532323
+0.020000,12.988336,5.225443,3.448287,3.868174,1.921591,-2.679223,5.527943
+0.020000,13.045099,5.171786,3.526398,3.905512,1.866919,-2.733636,5.523946
+0.020000,13.102183,5.117413,3.605233,3.941743,1.811551,-2.768382,5.520332
+0.020000,13.159587,5.062359,3.684770,3.976852,1.755414,-2.806840,5.517102
+0.020000,13.217312,5.006659,3.764986,4.010833,1.699065,-2.817470,5.514257
+0.020000,13.275361,4.950349,3.845860,4.043682,1.642466,-2.829952,5.511796
+0.020000,13.333737,4.893465,3.927368,4.075389,1.585336,-2.856465,5.509722
+0.020000,13.392442,4.836043,4.009487,4.105948,1.527972,-2.868214,5.508033
+0.020000,13.451482,4.778123,4.092194,4.135357,1.470433,-2.876975,5.506730
+0.020000,13.510860,4.719740,4.175466,4.163612,1.412767,-2.883294,5.505815
+0.020000,13.570582,4.660934,4.259280,4.190713,1.355012,-2.887718,5.505285
+0.020000,13.630650,4.601748,4.343609,4.216419,1.285327,-3.484264,5.505143
+0.020000,13.691060,4.542231,4.428411,4.240137,1.185900,-4.971344,5.505388
+0.020000,13.751808,4.482433,4.513653,4.262088,1.097533,-4.418339,5.506020
+0.020000,13.812902,4.422395,4.599310,4.282851,1.038166,-2.968384,5.507039
+0.020000,13.874348,4.362156,4.685359,4.302426,0.978728,-2.971865,5.508444
+0.020000,13.936154,4.301758,4.771775,4.320810,0.919187,-2.977076,5.510234
+0.020000,13.998325,4.241244,4.858535,4.338000,0.859498,-2.984454,5.512411
+0.020000,14.060869,4.180654,4.945615,4.353992,0.799607,-2.994557,5.514972
+0.020000,14.123793,4.120032,5.032990,4.368781,0.739448,-3.007942,5.517919
+0.020000,14.187103,4.059419,5.120638,4.382360,0.678973,-3.023767,5.521249
+0.020000,14.250806,3.998860,5.208532,4.394732,0.618602,-3.018504,5.524962
+0.020000,14.314906,3.938397,5.296650,4.405866,0.556716,-3.094332,5.529058
+0.020000,14.379410,3.878074,5.384965,4.415759,0.494624,-3.104590,5.533535
+0.020000,14.444321,3.817935,5.473453,4.424394,0.431773,-3.142573,5.538392
+0.020000,14.509643,3.758025,5.562088,4.431759,0.368236,-3.176807,5.543627
+0.020000,14.575379,3.698388,5.650845,4.437847,0.304415,-3.191081,5.549238
+0.020000,14.641531,3.639071,5.739697,4.442599,0.237572,-3.342130,5.555223
+0.020000,14.708099,3.580117,5.828617,4.446005,0.170309,-3.363142,5.561580
+0.020000,14.775082,3.521573,5.917578,4.448067,0.103093,-3.360835,5.568304
+0.020000,14.842477,3.463484,6.006552,4.448703,0.031826,-3.563349,5.575393
+0.020000,14.910280,3.405898,6.095510,4.447895,-0.040395,-3.611045,5.582841
+0.020000,14.978486,3.348859,6.184423,4.445640,-0.112787,-3.619585,5.590644
+0.020000,15.047086,3.292414,6.273259,4.441818,-0.191099,-3.915586,5.598797
+0.020000,15.116070,3.236610,6.361989,4.436482,-0.266763,-3.783235,5.607293
+0.020000,15.185426,3.181493,6.450578,4.429479,-0.350193,-4.171486,5.616124
+0.020000,15.255138,3.127107,6.538995,4.420846,-0.431648,-4.072767,5.625284
+0.020000,15.325189,3.073501,6.627205,4.410469,-0.518856,-4.360361,5.634762
+0.020000,15.395559,3.020717,6.715171,4.398305,-0.608187,-4.466594,5.644550
+0.020000,15.466225,2.968801,6.802857,4.384321,-0.699187,-4.549986,5.654636
+0.020000,15.537160,2.917796,6.890226,4.368427,-0.794722,-4.776748,5.665009
+0.020000,15.608335,2.867745,6.977237,4.350561,-0.893303,-4.929058,5.675656
+0.020000,15.679717,2.818689,7.063850,4.330668,-0.994626,-5.066124,5.686564
+0.020000,15.751271,2.770669,7.150024,4.308684,-1.099193,-5.228377,5.697717
+0.020000,15.822957,2.723723,7.235715,4.284545,-1.206969,-5.388805,5.709099
+0.020000,15.894734,2.677887,7.320879,4.258187,-1.317868,-5.544946,5.720695
+0.020000,15.966556,2.633196,7.405470,4.229552,-1.431750,-5.694091,5.732486
+0.020000,16.038374,2.589682,7.489442,4.198593,-1.547948,-5.809874,5.744454
+0.020000,16.110136,2.547376,7.572746,4.165237,-1.667847,-5.994984,5.756579
+0.020000,16.181788,2.506303,7.655335,4.129446,-1.789518,-6.083518,5.768841
+0.020000,16.253272,2.466489,7.737159,4.091178,-1.913405,-6.194387,5.781219
+0.020000,16.324528,2.427955,7.818167,4.050390,-2.039381,-6.298761,5.793692
+0.020000,16.395492,2.390717,7.898308,4.007056,-2.166721,-6.367039,5.806236
+0.020000,16.466101,2.354791,7.977531,3.961140,-2.295802,-6.454015,5.818831
+0.020000,16.536292,2.320185,8.055789,3.912930,-2.410513,-5.735584,5.831454
+0.020000,16.606016,2.286896,8.133052,3.863156,-2.488676,-3.908140,5.844085
+0.020000,16.675231,2.254918,8.209298,3.812266,-2.544510,-2.791692,5.856707
+0.020000,16.743895,2.224239,8.284503,3.760257,-2.600439,-2.796463,5.869302
+0.020000,16.811964,2.194847,8.358647,3.707198,-2.652969,-2.626467,5.881853
+0.020000,16.879397,2.166725,8.431709,3.653106,-2.704612,-2.582166,5.894344
+0.020000,16.946152,2.139858,8.503668,3.597953,-2.757609,-2.649858,5.906757
+0.020000,17.012189,2.114223,8.574505,3.541875,-2.803931,-2.316117,5.919078
+0.020000,17.077467,2.089801,8.644203,3.484859,-2.850792,-2.343037,5.931291
+0.020000,17.141947,2.066566,8.712741,3.426922,-2.896866,-2.303669,5.943382
+0.020000,17.205591,2.044494,8.780104,3.368149,-2.938631,-2.088287,5.955338
+0.020000,17.268363,2.023556,8.846276,3.308589,-2.978007,-1.968795,5.967146
+0.020000,17.330227,2.003724,8.911241,3.248238,-3.017546,-1.976950,5.978793
+0.020000,17.391149,1.984968,8.974985,3.187227,-3.050547,-1.650037,5.990270
+0.020000,17.451100,1.967256,9.037497,3.125606,-3.081066,-1.525941,6.001564
+0.020000,17.510045,1.950557,9.098762,3.063259,-3.117353,-1.814346,6.012667
+0.020000,17.567959,1.934836,9.158773,3.000509,-3.137497,-1.007220,6.023570
+0.020000,17.624813,1.920060,9.217515,2.937132,-3.168843,-1.567296,6.034265
+0.020000,17.680582,1.906194,9.274982,2.873349,-3.189156,-1.015664,6.044744
+0.020000,17.735244,1.893204,9.331166,2.809176,-3.208659,-0.975132,6.055001
+0.020000,17.788775,1.881055,9.386058,2.744633,-3.227115,-0.922786,6.065030
+0.020000,17.841157,1.869712,9.439654,2.679782,-3.242562,-0.772392,6.074827
+0.020000,17.892370,1.859140,9.491947,2.614643,-3.256939,-0.718845,6.084386
+0.020000,17.942395,1.849304,9.542930,2.549182,-3.273047,-0.805391,6.093704
+0.020000,17.991222,1.840169,9.592604,2.483698,-3.274202,-0.057722,6.102777
+0.020000,18.038832,1.831703,9.640961,2.417849,-3.292452,-0.912501,6.111603
+0.020000,18.085217,1.823870,9.688002,2.352055,-3.289734,0.135897,6.120181
+0.020000,18.130362,1.816638,9.733723,2.286047,-3.300399,-0.533261,6.128507
+0.020000,18.174258,1.809975,9.778122,2.219931,-3.305760,-0.268039,6.136581
+0.020000,18.216897,1.803849,9.821199,2.153841,-3.304542,0.060906,6.144403
+0.020000,18.258271,1.798229,9.862953,2.087718,-3.306102,-0.078026,6.151971
+0.020000,18.298375,1.793086,9.903385,2.021581,-3.306887,-0.039241,6.159285
+0.020000,18.337198,1.788390,9.942492,1.955339,-3.312102,-0.260732,6.166346
+0.020000,18.374741,1.784113,9.980277,1.889292,-3.302353,0.487432,6.173155
+0.020000,18.410999,1.780228,10.016743,1.823253,-3.301930,0.021156,6.179711
+0.020000,18.445966,1.776708,10.051886,1.757176,-3.303863,-0.096644,6.186016
+0.020000,18.479641,1.773529,10.085711,1.691234,-3.297077,0.339295,6.192071
+0.020000,18.512023,1.770665,10.118219,1.625419,-3.290761,0.315797,6.197876
+0.020000,18.543109,1.768093,10.149412,1.559630,-3.289440,0.066050,6.203434
+0.020000,18.572897,1.765792,10.179289,1.493852,-3.288901,0.026962,6.208746
+0.020000,18.601389,1.763740,10.207854,1.428273,-3.278943,0.497864,6.213812
+0.020000,18.628588,1.761916,10.235114,1.362995,-3.263916,0.751373,6.218637
+0.020000,18.654516,1.760300,10.261093,1.298928,-3.203358,3.027885,6.223224
+0.020000,18.679205,1.758872,10.285823,1.236519,-3.120425,4.146647,6.227581
+0.020000,18.702686,1.757613,10.309338,1.175723,-3.039825,4.030002,6.231715
+0.020000,18.724984,1.756508,10.331663,1.116281,-2.972076,3.387442,6.235632
+0.020000,18.746137,1.755541,10.352838,1.058748,-2.876665,4.770557,6.239340
+0.020000,18.766170,1.754697,10.372889,1.002520,-2.811383,3.264120,6.242844
+0.020000,18.785115,1.753964,10.391849,0.948002,-2.725938,4.272258,6.246151
+0.020000,18.803006,1.753329,10.409750,0.895097,-2.645252,4.034286,6.249268
+0.020000,18.819872,1.752782,10.426625,0.843715,-2.569101,3.807527,6.252201
+0.020000,18.835742,1.752312,10.442502,0.793865,-2.492454,3.832357,6.254956
+0.020000,18.850648,1.751910,10.457413,0.745558,-2.415359,3.854774,6.257539
+0.020000,18.864619,1.751568,10.471389,0.698801,-2.337863,3.874807,6.259956
+0.020000,18.877688,1.751280,10.484461,0.653601,-2.260013,3.892491,6.262214
+0.020000,18.889885,1.751037,10.496661,0.609964,-2.181855,3.907864,6.264319
+0.020000,18.901241,1.750833,10.508018,0.567895,-2.103436,3.920971,6.266275
+0.020000,18.911788,1.750665,10.518566,0.527399,-2.024799,3.931858,6.268090
+0.020000,18.921551,1.750525,10.528331,0.488216,-1.959127,3.283576,6.269768
+0.020000,18.930565,1.750412,10.537345,0.450730,-1.874337,4.239539,6.271316
+0.020000,18.938861,1.750319,10.545642,0.414817,-1.795620,3.935836,6.272739
+0.020000,18.946464,1.750244,10.553245,0.380174,-1.732190,3.171505,6.274042
+0.020000,18.953411,1.750185,10.560192,0.347350,-1.641162,4.551373,6.275232
+0.020000,18.959726,1.750138,10.566507,0.315755,-1.579778,3.069218,6.276313
+0.020000,18.965443,1.750102,10.572224,0.285848,-1.495335,4.222166,6.277290
+0.020000,18.970592,1.750074,10.577374,0.257487,-1.418044,3.864555,6.278170
+0.020000,18.975199,1.750052,10.581981,0.230334,-1.357672,3.018575,6.278957
+0.020000,18.979298,1.750037,10.586080,0.204971,-1.268144,4.476426,6.279657
+0.020000,18.982920,1.750025,10.589702,0.181076,-1.194724,3.670987,6.280275
+0.020000,18.986091,1.750016,10.592873,0.158580,-1.124811,3.495661,6.280815
+0.020000,18.988842,1.750011,10.595623,0.137513,-1.053376,3.571744,6.281284
+0.020000,18.991203,1.750007,10.597985,0.118070,-0.972113,4.063140,6.281687
+0.020000,18.993205,1.750004,10.599987,0.100106,-0.898207,3.695299,6.282028
+0.020000,18.994878,1.750002,10.601659,0.083622,-0.824200,3.700349,6.282313
+0.020000,18.996250,1.750001,10.603032,0.068620,-0.750108,3.704607,6.282547
+0.020000,18.997352,1.750001,10.604134,0.055101,-0.675945,3.708138,6.282734
+0.020000,18.998213,1.750000,10.604995,0.043067,-0.601725,3.711009,6.282881
+0.020000,18.998864,1.750000,10.605646,0.032518,-0.527459,3.713289,6.282992
+0.020000,18.999333,1.750000,10.606115,0.023454,-0.453158,3.715044,6.283072
+0.020000,18.999650,1.750000,10.606432,0.015878,-0.378831,3.716344,6.283126
+0.020000,18.999846,1.750000,10.606628,0.009788,-0.304486,3.717256,6.283159
+0.020000,18.999950,1.750000,10.606732,0.005185,-0.230129,3.717851,6.283177
+0.020000,18.999991,1.750000,10.606773,0.002070,-0.155765,3.718196,6.283184
+0.020000,19.000000,1.750000,10.606782,0.000442,-0.081398,3.718362,6.283185
+0.020000,19.000000,1.750000,10.606782,0.000000,-0.022107,2.964554,6.283185
diff --git a/src/main/deploy/turnTest.left.pf1.csv b/src/main/deploy/turnTest.left.pf1.csv
new file mode 100644
index 0000000..b2c5945
--- /dev/null
+++ b/src/main/deploy/turnTest.left.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.001758,9.249999,0.000012,0.001180,0.059025,2.951235,6.281779
+0.020000,0.001752,9.249999,0.000017,0.000280,-0.045015,-5.201995,6.281784
+0.020000,0.001739,9.249999,0.000030,0.000630,0.017512,3.126378,6.281794
+0.020000,0.001717,9.249999,0.000052,0.001121,0.024519,0.350315,6.281812
+0.020000,0.001682,9.249999,0.000087,0.001751,0.031527,0.350419,6.281840
+0.020000,0.001631,9.249999,0.000138,0.002522,0.038539,0.350589,6.281880
+0.020000,0.001563,9.249999,0.000207,0.003433,0.045556,0.350841,6.281935
+0.020000,0.001473,9.249999,0.000296,0.004485,0.052580,0.351194,6.282007
+0.020000,0.001360,9.249999,0.000410,0.005677,0.059613,0.351662,6.282098
+0.020000,0.001219,9.249999,0.000550,0.007010,0.066658,0.352262,6.282210
+0.020000,0.001050,9.250000,0.000720,0.008485,0.073718,0.353013,6.282346
+0.020000,0.000848,9.250000,0.000922,0.010101,0.080797,0.353929,6.282507
+0.020000,0.000610,9.250000,0.001159,0.011858,0.087897,0.355029,6.282697
+0.020000,0.000335,9.250000,0.001434,0.013759,0.095024,0.356329,6.282917
+0.020000,0.000019,9.250000,0.001750,0.015803,0.102181,0.357846,6.283170
+0.020000,0.002522,9.250000,0.004252,0.125116,5.465689,268.175388,0.000273
+0.020000,0.005528,9.250002,0.007259,0.150303,1.259353,-210.316760,0.000598
+0.020000,0.008896,9.250004,0.010626,0.168397,0.904699,-17.732744,0.000963
+0.020000,0.012645,9.250009,0.014376,0.187492,0.954753,2.502740,0.001371
+0.020000,0.016805,9.250015,0.018535,0.207954,1.023069,3.415793,0.001823
+0.020000,0.021387,9.250025,0.023118,0.229106,1.057616,1.727318,0.002322
+0.020000,0.026411,9.250038,0.028142,0.251239,1.106646,2.451521,0.002871
+0.020000,0.031907,9.250055,0.033638,0.274802,1.178127,3.574068,0.003473
+0.020000,0.037885,9.250078,0.039616,0.298866,1.203228,1.255025,0.004128
+0.020000,0.044373,9.250107,0.046104,0.324410,1.277204,3.698809,0.004842
+0.020000,0.051383,9.250144,0.053114,0.350511,1.305044,1.391978,0.005616
+0.020000,0.058945,9.250189,0.060677,0.378122,1.380563,3.775976,0.006453
+0.020000,0.067071,9.250245,0.068802,0.406273,1.407540,1.348821,0.007355
+0.020000,0.075784,9.250314,0.077515,0.435668,1.469745,3.110244,0.008327
+0.020000,0.085105,9.250396,0.086837,0.466100,1.521606,2.593093,0.009370
+0.020000,0.095052,9.250495,0.096784,0.497344,1.562187,2.029050,0.010488
+0.020000,0.105643,9.250612,0.107376,0.529613,1.613464,2.563826,0.011684
+0.020000,0.116900,9.250751,0.118634,0.562893,1.663985,2.526043,0.012961
+0.020000,0.128842,9.250914,0.130578,0.597166,1.713665,2.484008,0.014323
+0.020000,0.141489,9.251104,0.143226,0.632415,1.762416,2.437557,0.015773
+0.020000,0.154860,9.251326,0.156598,0.668617,1.810147,2.386529,0.017315
+0.020000,0.168973,9.251582,0.170713,0.705753,1.856762,2.330764,0.018953
+0.020000,0.183846,9.251876,0.185589,0.743796,1.902164,2.270105,0.020690
+0.020000,0.199496,9.252215,0.201244,0.782721,1.946252,2.204397,0.022531
+0.020000,0.215942,9.252601,0.217694,0.822499,1.988922,2.133491,0.024479
+0.020000,0.233206,9.253042,0.234964,0.863501,2.050094,3.058634,0.026540
+0.020000,0.251290,9.253541,0.253055,0.904539,2.051875,0.089022,0.028717
+0.020000,0.270194,9.254106,0.271967,0.945627,2.054407,0.126599,0.031012
+0.020000,0.289912,9.254742,0.291695,0.986377,2.037504,-0.845136,0.033426
+0.020000,0.310445,9.255454,0.312240,1.027277,2.044974,0.373490,0.035965
+0.020000,0.331784,9.256250,0.333594,1.067702,2.021261,-1.185651,0.038629
+0.020000,0.353926,9.257137,0.355754,1.108010,2.015424,-0.291868,0.041421
+0.020000,0.376869,9.258122,0.378718,1.148171,2.008019,-0.370226,0.044346
+0.020000,0.400607,9.259211,0.402481,1.188151,1.999011,-0.450424,0.047406
+0.020000,0.425136,9.260414,0.427039,1.227918,1.988362,-0.532416,0.050604
+0.020000,0.450450,9.261739,0.452388,1.267439,1.976039,-0.616145,0.053946
+0.020000,0.476543,9.263193,0.478522,1.306679,1.962008,-0.701550,0.057433
+0.020000,0.503408,9.264787,0.505434,1.345604,1.946237,-0.788558,0.061071
+0.020000,0.531040,9.266529,0.533121,1.384368,1.938201,-0.401791,0.064863
+0.020000,0.559434,9.268429,0.561579,1.422884,1.925792,-0.620484,0.068815
+0.020000,0.588579,9.270498,0.590797,1.460929,1.902261,-1.176530,0.072930
+0.020000,0.618471,9.272746,0.620774,1.498815,1.894322,-0.396940,0.077215
+0.020000,0.649102,9.275185,0.651501,1.536360,1.877217,-0.855249,0.081674
+0.020000,0.680460,9.277825,0.682970,1.573440,1.854036,-1.159070,0.086313
+0.020000,0.712537,9.280678,0.715174,1.610210,1.838465,-0.778525,0.091139
+0.020000,0.745325,9.283757,0.748106,1.646622,1.820609,-0.892838,0.096156
+0.020000,0.778814,9.287075,0.781759,1.682630,1.800412,-1.009853,0.101372
+0.020000,0.812992,9.290645,0.816122,1.718187,1.777825,-1.129353,0.106793
+0.020000,0.847846,9.294481,0.851187,1.753243,1.752803,-1.251081,0.112428
+0.020000,0.883365,9.298596,0.886944,1.787827,1.729208,-1.179768,0.118282
+0.020000,0.919540,9.303006,0.923386,1.822115,1.714393,-0.740715,0.124365
+0.020000,0.956349,9.307725,0.960497,1.855549,1.671739,-2.132728,0.130684
+0.020000,0.993786,9.312770,0.998272,1.888740,1.659514,-0.611247,0.137250
+0.020000,1.031830,9.318157,1.036696,1.921202,1.623105,-1.820454,0.144070
+0.020000,1.070466,9.323901,1.075757,1.953042,1.592027,-1.553870,0.151154
+0.020000,1.109678,9.330021,1.115443,1.984291,1.562422,-1.480248,0.158512
+0.020000,1.149445,9.336533,1.155740,2.014884,1.529640,-1.639137,0.166156
+0.020000,1.189750,9.343456,1.196635,2.044757,1.493661,-1.798929,0.174096
+0.020000,1.230570,9.350809,1.238112,2.073847,1.454488,-1.958635,0.182343
+0.020000,1.271889,9.358610,1.280161,2.102427,1.429004,-1.274194,0.190911
+0.020000,1.313677,9.366878,1.322759,2.129885,1.372894,-2.805499,0.199810
+0.020000,1.355915,9.375635,1.365895,2.156836,1.347551,-1.267155,0.209056
+0.020000,1.398575,9.384899,1.409549,2.182707,1.293548,-2.700178,0.218661
+0.020000,1.441631,9.394691,1.453705,2.207760,1.252669,-2.043960,0.228639
+0.020000,1.485055,9.405032,1.498343,2.231923,1.208153,-2.225788,0.239005
+0.020000,1.528817,9.415944,1.543446,2.255126,1.160149,-2.400200,0.249775
+0.020000,1.572887,9.427448,1.588992,2.277303,1.108862,-2.564365,0.260962
+0.020000,1.617233,9.439566,1.634964,2.298617,1.065701,-2.158010,0.272585
+0.020000,1.661820,9.452320,1.681339,2.318756,1.006930,-2.938585,0.284658
+0.020000,1.706608,9.465728,1.728091,2.337589,0.941669,-3.263015,0.297197
+0.020000,1.751541,9.479809,1.775178,2.354376,0.839333,-5.116840,0.310213
+0.020000,1.796567,9.494577,1.822565,2.369315,0.746939,-4.619661,0.323716
+0.020000,1.841633,9.510048,1.870213,2.382397,0.654127,-4.640639,0.337718
+0.020000,1.886686,9.526235,1.918085,2.393636,0.561949,-4.608864,0.352229
+0.020000,1.931676,9.543154,1.966152,2.403307,0.483560,-3.919487,0.367258
+0.020000,1.976546,9.560816,2.014372,2.411044,0.386856,-4.835154,0.382813
+0.020000,2.021249,9.579234,2.062720,2.417397,0.317643,-3.460674,0.398900
+0.020000,2.065732,9.598418,2.111164,2.422165,0.238388,-3.962770,0.415525
+0.020000,2.109946,9.618379,2.159675,2.425561,0.169793,-3.429717,0.432689
+0.020000,2.153845,9.639126,2.208229,2.427722,0.108067,-3.086326,0.450395
+0.020000,2.197384,9.660669,2.256807,2.428874,0.057566,-2.525060,0.468640
+0.020000,2.240521,9.683016,2.305388,2.429077,0.010154,-2.370558,0.487421
+0.020000,2.283215,9.706175,2.353960,2.428568,-0.025459,-1.780645,0.506728
+0.020000,2.325430,9.730156,2.402511,2.427551,-0.050809,-1.267511,0.526551
+0.020000,2.367135,9.754967,2.451038,2.426356,-0.059777,-0.448395,0.546877
+0.020000,2.408299,9.780617,2.499539,2.425066,-0.064511,-0.236731,0.567685
+0.020000,2.448897,9.807115,2.548019,2.424011,-0.052752,0.587974,0.588954
+0.020000,2.488907,9.834474,2.596489,2.423505,-0.025267,1.374236,0.610657
+0.020000,2.528311,9.862705,2.644963,2.423691,0.009302,1.728429,0.632762
+0.020000,2.567096,9.891822,2.693461,2.424895,0.060167,2.543283,0.655235
+0.020000,2.605251,9.921841,2.742009,2.427413,0.125916,3.287457,0.678036
+0.020000,2.642769,9.952778,2.790638,2.431402,0.199459,3.677159,0.701123
+0.020000,2.679645,9.984652,2.839380,2.437110,0.285409,4.297489,0.724449
+0.020000,2.715877,10.017484,2.888275,2.444741,0.381532,4.806138,0.747964
+0.020000,2.751466,10.051295,2.937363,2.454442,0.485071,5.176949,0.771617
+0.020000,2.786411,10.086108,2.986690,2.466329,0.594329,5.462888,0.795354
+0.020000,2.820716,10.121945,3.036299,2.480476,0.707370,5.652074,0.819119
+0.020000,2.854381,10.158830,3.086238,2.496918,0.822090,5.735985,0.842858
+0.020000,2.887408,10.196785,3.136551,2.515644,0.936288,5.709898,0.866513
+0.020000,2.919798,10.235831,3.187283,2.536599,1.047750,5.573097,0.890032
+0.020000,2.951551,10.275988,3.238477,2.559694,1.154746,5.349831,0.913361
+0.020000,2.982666,10.317271,3.290172,2.584764,1.253535,4.939420,0.936450
+0.020000,3.013138,10.359694,3.342405,2.611653,1.344432,4.544878,0.959249
+0.020000,3.042964,10.403267,3.395208,2.640148,1.424721,4.014425,0.981716
+0.020000,3.072139,10.447993,3.448608,2.670023,1.493790,3.453438,1.003809
+0.020000,3.100654,10.493874,3.502629,2.701021,1.549865,2.803779,1.025490
+0.020000,3.128504,10.540905,3.557287,2.732892,1.593570,2.185230,1.046728
+0.020000,3.155678,10.589076,3.612594,2.765369,1.623850,1.513993,1.067493
+0.020000,3.182169,10.638372,3.668557,2.798161,1.639601,0.787560,1.087762
+0.020000,3.207965,10.688766,3.725170,2.830632,1.623565,-0.801795,1.107512
+0.020000,3.233052,10.740226,3.782419,2.862479,1.592341,-1.561199,1.126723
+0.020000,3.257426,10.792721,3.840297,2.893870,1.569547,-1.139693,1.145384
+0.020000,3.281079,10.846218,3.898789,2.924606,1.536814,-1.636663,1.163488
+0.020000,3.304006,10.900678,3.957878,2.954460,1.492698,-2.205795,1.181029
+0.020000,3.326205,10.956059,4.017543,2.983243,1.439115,-2.679143,1.198005
+0.020000,3.347672,11.012318,4.077759,3.010777,1.376721,-3.119708,1.214418
+0.020000,3.368407,11.069408,4.138497,3.036931,1.307701,-3.451017,1.230270
+0.020000,3.388412,11.127279,4.199729,3.061573,1.232080,-3.781014,1.245568
+0.020000,3.407689,11.185881,4.261420,3.084569,1.149827,-4.112689,1.260318
+0.020000,3.426243,11.245162,4.323537,3.105841,1.063606,-4.311027,1.274529
+0.020000,3.444080,11.305070,4.386043,3.125316,0.973748,-4.492922,1.288210
+0.020000,3.461206,11.365550,4.448902,3.142929,0.880625,-4.656130,1.301375
+0.020000,3.477633,11.426549,4.512073,3.158584,0.782764,-4.893047,1.314033
+0.020000,3.493368,11.488012,4.575519,3.172287,0.685141,-4.881177,1.326198
+0.020000,3.508426,11.549886,4.639199,3.183997,0.585479,-4.983082,1.337883
+0.020000,3.522817,11.612118,4.703073,3.193690,0.484672,-5.040322,1.349101
+0.020000,3.536557,11.674653,4.767099,3.201317,0.381348,-5.166237,1.359866
+0.020000,3.549659,11.737439,4.831238,3.206944,0.281378,-4.998507,1.370191
+0.020000,3.562140,11.800425,4.895449,3.210521,0.178837,-5.127033,1.380091
+0.020000,3.574016,11.863559,4.959690,3.212056,0.076719,-5.105896,1.389579
+0.020000,3.585302,11.926790,5.023921,3.211561,-0.024744,-5.073162,1.398668
+0.020000,3.596018,11.990071,5.088102,3.209053,-0.125365,-5.031060,1.407373
+0.020000,3.606180,12.053351,5.152193,3.204553,-0.224997,-4.981598,1.415705
+0.020000,3.615806,12.116584,5.216154,3.198068,-0.324276,-4.963931,1.423679
+0.020000,3.624915,12.179722,5.279946,3.189572,-0.424798,-5.026099,1.431306
+0.020000,3.633525,12.242720,5.343530,3.179209,-0.518132,-4.666687,1.438598
+0.020000,3.641653,12.305533,5.406867,3.166825,-0.619230,-5.054896,1.445569
+0.020000,3.649318,12.368117,5.469919,3.152607,-0.710890,-4.583022,1.452229
+0.020000,3.656539,12.430430,5.532648,3.136489,-0.805875,-4.749262,1.458590
+0.020000,3.663333,12.492429,5.595018,3.118496,-0.899663,-4.689393,1.464662
+0.020000,3.669719,12.554072,5.656991,3.098655,-0.992071,-4.620415,1.470456
+0.020000,3.675713,12.615319,5.718531,3.076992,-1.083113,-4.552092,1.475983
+0.020000,3.681333,12.676131,5.779602,3.053536,-1.172814,-4.485026,1.481252
+0.020000,3.686597,12.736468,5.840168,3.028312,-1.261209,-4.419734,1.486273
+0.020000,3.691520,12.796292,5.900195,3.001326,-1.349294,-4.404289,1.491055
+0.020000,3.696120,12.855565,5.959646,2.972561,-1.438248,-4.447698,1.495608
+0.020000,3.700412,12.914251,6.018488,2.942120,-1.522042,-4.189710,1.499939
+0.020000,3.704412,12.972313,6.076688,2.909962,-1.607910,-4.293373,1.504058
+0.020000,3.708135,13.029714,6.134209,2.876084,-1.693929,-4.300948,1.507972
+0.020000,3.711596,13.086420,6.191021,2.840573,-1.775507,-4.078909,1.511689
+0.020000,3.714809,13.142407,6.247100,2.803958,-1.830758,-2.762558,1.515218
+0.020000,3.717790,13.197662,6.302436,2.766809,-1.857439,-1.334035,1.518567
+0.020000,3.720553,13.252175,6.357019,2.729121,-1.884412,-1.348647,1.521745
+0.020000,3.723110,13.305934,6.410838,2.690975,-1.907326,-1.145711,1.524758
+0.020000,3.725475,13.358928,6.463886,2.652379,-1.929797,-1.123565,1.527614
+0.020000,3.727660,13.411149,6.516152,2.613297,-1.954091,-1.214668,1.530322
+0.020000,3.729677,13.462586,6.567628,2.573832,-1.973236,-0.957253,1.532886
+0.020000,3.731536,13.513231,6.618307,2.533962,-1.993506,-1.013508,1.535314
+0.020000,3.733247,13.563076,6.668181,2.493698,-2.013181,-0.983764,1.537612
+0.020000,3.734821,13.612112,6.717243,2.453064,-2.031737,-0.927806,1.539786
+0.020000,3.736267,13.660332,6.765484,2.412079,-2.049257,-0.876003,1.541840
+0.020000,3.737594,13.707728,6.812900,2.370762,-2.065821,-0.828192,1.543782
+0.020000,3.738809,13.754295,6.859482,2.329132,-2.081505,-0.784211,1.545615
+0.020000,3.739921,13.800026,6.905226,2.287204,-2.096384,-0.743902,1.547345
+0.020000,3.740937,13.844913,6.950125,2.244937,-2.113394,-0.850502,1.548976
+0.020000,3.741864,13.888952,6.994174,2.202440,-2.124805,-0.570586,1.550513
+0.020000,3.742709,13.932136,7.037366,2.159628,-2.140610,-0.790259,1.551961
+0.020000,3.743477,13.974461,7.079698,2.116578,-2.152517,-0.595329,1.553323
+0.020000,3.744175,14.015920,7.121163,2.073277,-2.165057,-0.626998,1.554603
+0.020000,3.744807,14.056510,7.161758,2.029731,-2.177295,-0.611921,1.555806
+0.020000,3.745380,14.096225,7.201477,1.985953,-2.188906,-0.580539,1.556935
+0.020000,3.745898,14.135061,7.240316,1.941954,-2.199937,-0.551560,1.557993
+0.020000,3.746365,14.173013,7.278271,1.897745,-2.210435,-0.524884,1.558985
+0.020000,3.746785,14.210077,7.315337,1.853316,-2.221466,-0.551561,1.559912
+0.020000,3.747163,14.246249,7.351511,1.808699,-2.230840,-0.468702,1.560780
+0.020000,3.747502,14.281525,7.386789,1.763893,-2.240292,-0.472561,1.561590
+0.020000,3.747806,14.315902,7.421167,1.718901,-2.249629,-0.466876,1.562345
+0.020000,3.748077,14.349375,7.454642,1.673724,-2.258843,-0.460675,1.563048
+0.020000,3.748318,14.381942,7.487210,1.628388,-2.266769,-0.396327,1.563703
+0.020000,3.748533,14.413599,7.518867,1.582885,-2.275160,-0.419530,1.564311
+0.020000,3.748724,14.444343,7.549612,1.537223,-2.283125,-0.398248,1.564876
+0.020000,3.748893,14.474170,7.579440,1.491409,-2.290693,-0.378426,1.565399
+0.020000,3.749042,14.503079,7.608349,1.445451,-2.297893,-0.359999,1.565883
+0.020000,3.749173,14.531066,7.636336,1.399356,-2.304751,-0.342907,1.566330
+0.020000,3.749288,14.558128,7.663398,1.353112,-2.312210,-0.372940,1.566743
+0.020000,3.749389,14.584263,7.689533,1.306755,-2.317843,-0.281655,1.567123
+0.020000,3.749477,14.609468,7.714739,1.260264,-2.324566,-0.336135,1.567472
+0.020000,3.749554,14.633741,7.739012,1.213664,-2.329975,-0.270442,1.567793
+0.020000,3.749620,14.657080,7.762351,1.166935,-2.336446,-0.323562,1.568086
+0.020000,3.749678,14.679482,7.784753,1.120108,-2.341375,-0.246444,1.568355
+0.020000,3.749728,14.700945,7.806216,1.073175,-2.346614,-0.261976,1.568600
+0.020000,3.749771,14.721468,7.826739,1.026143,-2.351623,-0.250433,1.568824
+0.020000,3.749807,14.741048,7.846320,0.979022,-2.356034,-0.220550,1.569027
+0.020000,3.749839,14.759696,7.864968,0.932402,-2.331007,1.251371,1.569211
+0.020000,3.749865,14.777433,7.882705,0.886858,-2.277190,2.690834,1.569378
+0.020000,3.749888,14.794282,7.899553,0.842410,-2.222411,2.738974,1.569529
+0.020000,3.749907,14.810263,7.915534,0.799061,-2.167476,2.746709,1.569665
+0.020000,3.749923,14.825399,7.930670,0.756805,-2.112775,2.735062,1.569789
+0.020000,3.749937,14.839712,7.944984,0.715666,-2.056969,2.790286,1.569900
+0.020000,3.749948,14.853225,7.958496,0.675639,-2.001315,2.782703,1.570001
+0.020000,3.749958,14.865960,7.971231,0.636725,-1.945713,2.780092,1.570091
+0.020000,3.749966,14.877938,7.983210,0.598941,-1.889199,2.825742,1.570172
+0.020000,3.749972,14.889184,7.994455,0.562285,-1.832823,2.818761,1.570245
+0.020000,3.749978,14.899719,8.004991,0.526761,-1.776208,2.830754,1.570311
+0.020000,3.749982,14.909567,8.014838,0.492373,-1.719373,2.841755,1.570370
+0.020000,3.749986,14.918749,8.024021,0.459126,-1.662337,2.851828,1.570422
+0.020000,3.749989,14.927290,8.032561,0.427024,-1.605116,2.861034,1.570469
+0.020000,3.749991,14.935211,8.040482,0.396066,-1.547893,2.861153,1.570511
+0.020000,3.749993,14.942536,8.047808,0.366260,-1.490304,2.879443,1.570549
+0.020000,3.749995,14.949288,8.054560,0.337610,-1.432525,2.888948,1.570582
+0.020000,3.749996,14.955491,8.060762,0.310111,-1.374922,2.880171,1.570612
+0.020000,3.749997,14.961166,8.066437,0.283772,-1.316953,2.898424,1.570638
+0.020000,3.749998,14.966338,8.071609,0.258594,-1.258915,2.901886,1.570661
+0.020000,3.749998,14.971030,8.076301,0.234576,-1.200879,2.901808,1.570682
+0.020000,3.749999,14.975264,8.080535,0.211723,-1.142665,2.910733,1.570700
+0.020000,3.749999,14.979065,8.084336,0.190037,-1.084272,2.919613,1.570716
+0.020000,3.749999,14.982455,8.087726,0.169513,-1.026209,2.903173,1.570729
+0.020000,3.750000,14.985458,8.090730,0.150162,-0.967543,2.933290,1.570741
+0.020000,3.750000,14.988098,8.093369,0.131980,-0.909099,2.922230,1.570752
+0.020000,3.750000,14.990397,8.095669,0.114967,-0.850664,2.921729,1.570761
+0.020000,3.750000,14.992380,8.097651,0.099125,-0.792124,2.927004,1.570768
+0.020000,3.750000,14.994069,8.099340,0.084456,-0.733433,2.934567,1.570775
+0.020000,3.750000,14.995488,8.100759,0.070960,-0.674805,2.931391,1.570780
+0.020000,3.750000,14.996661,8.101932,0.058637,-0.616146,2.932929,1.570784
+0.020000,3.750000,14.997610,8.102882,0.047488,-0.557462,2.934208,1.570788
+0.020000,3.750000,14.998361,8.103632,0.037513,-0.498757,2.935254,1.570790
+0.020000,3.750000,14.998935,8.104206,0.028712,-0.440035,2.936091,1.570792
+0.020000,3.750000,14.999357,8.104628,0.021086,-0.381300,2.936742,1.570794
+0.020000,3.750000,14.999649,8.104921,0.014635,-0.322556,2.937231,1.570795
+0.020000,3.750000,14.999837,8.105108,0.009359,-0.263804,2.937582,1.570796
+0.020000,3.750000,14.999942,8.105213,0.005258,-0.205048,2.937818,1.570796
+0.020000,3.750000,14.999988,8.105260,0.002332,-0.146288,2.937962,1.570796
+0.020000,3.750000,15.000000,8.105271,0.000581,-0.087528,2.938037,1.570796
+0.020000,3.750000,15.000000,8.105271,0.000000,-0.029074,2.922705,1.570796
diff --git a/src/main/deploy/turnTest.pf1.csv b/src/main/deploy/turnTest.pf1.csv
new file mode 100644
index 0000000..50b450c
--- /dev/null
+++ b/src/main/deploy/turnTest.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,0.000000,8.000000,0.000012,0.001180,0.059025,2.951235,6.281779
+0.020000,0.000000,8.000000,0.000059,0.003541,0.118049,2.951235,6.281784
+0.020000,0.000000,8.000000,0.000165,0.007083,0.177074,2.951235,6.281794
+0.020000,0.000000,8.000000,0.000354,0.011805,0.236099,2.951235,6.281812
+0.020000,0.000000,8.000000,0.000649,0.017707,0.295123,2.951235,6.281840
+0.020000,0.000000,8.000000,0.001074,0.024790,0.354148,2.951235,6.281880
+0.020000,0.000000,8.000000,0.001653,0.033054,0.413173,2.951235,6.281935
+0.020000,0.000000,8.000000,0.002408,0.042498,0.472198,2.951235,6.282007
+0.020000,0.000000,8.000000,0.003364,0.053122,0.531222,2.951235,6.282098
+0.020000,0.000000,8.000000,0.004545,0.064927,0.590247,2.951235,6.282210
+0.020000,0.000000,8.000000,0.005973,0.077913,0.649272,2.951235,6.282346
+0.020000,0.000000,8.000000,0.007673,0.092079,0.708296,2.951235,6.282507
+0.020000,0.000000,8.000000,0.009668,0.107425,0.767321,2.951235,6.282697
+0.020000,0.000000,8.000000,0.011982,0.123952,0.826346,2.951235,6.282917
+0.020000,0.000000,8.000000,0.014638,0.141659,0.885370,2.951235,6.283170
+0.020000,0.002862,8.000000,0.017660,0.160547,0.944395,2.951235,0.000273
+0.020000,0.006275,8.000002,0.021072,0.180616,1.003420,2.951235,0.000598
+0.020000,0.010100,8.000005,0.024897,0.201864,1.062445,2.951235,0.000963
+0.020000,0.014359,8.000010,0.029158,0.224294,1.121469,2.951235,0.001371
+0.020000,0.019083,8.000017,0.033880,0.247904,1.180494,2.951235,0.001823
+0.020000,0.024289,8.000028,0.039086,0.272694,1.239519,2.951235,0.002322
+0.020000,0.030000,8.000043,0.044800,0.298665,1.298543,2.951235,0.002871
+0.020000,0.036248,8.000063,0.051045,0.325816,1.357568,2.951235,0.003473
+0.020000,0.043045,8.000089,0.057844,0.354148,1.416593,2.951235,0.004128
+0.020000,0.050426,8.000122,0.065222,0.383661,1.475617,2.951235,0.004842
+0.020000,0.058403,8.000163,0.073202,0.414353,1.534642,2.951235,0.005616
+0.020000,0.067011,8.000215,0.081808,0.446227,1.593667,2.951235,0.006453
+0.020000,0.076265,8.000279,0.091063,0.479281,1.652691,2.951235,0.007355
+0.020000,0.086192,8.000357,0.100991,0.513515,1.711716,2.951235,0.008327
+0.020000,0.096818,8.000451,0.111616,0.548930,1.770741,2.951235,0.009370
+0.020000,0.108162,8.000564,0.122960,0.585525,1.829766,2.951235,0.010488
+0.020000,0.120248,8.000698,0.135049,0.623301,1.888790,2.951235,0.011684
+0.020000,0.133101,8.000856,0.147904,0.662257,1.947815,2.951235,0.012961
+0.020000,0.146746,8.001042,0.161551,0.702394,2.006840,2.951235,0.014323
+0.020000,0.161205,8.001260,0.176012,0.743711,2.065864,2.951235,0.015773
+0.020000,0.176503,8.001513,0.191311,0.786209,2.124889,2.951235,0.017315
+0.020000,0.192663,8.001806,0.207472,0.829887,2.183914,2.951235,0.018953
+0.020000,0.209707,8.002144,0.224518,0.874746,2.242938,2.951235,0.020690
+0.020000,0.227658,8.002532,0.242473,0.920785,2.301963,2.951235,0.022531
+0.020000,0.246538,8.002976,0.261361,0.968005,2.360988,2.951235,0.024479
+0.020000,0.266378,8.003482,0.281205,1.016405,2.420013,2.951235,0.026540
+0.020000,0.287181,8.004057,0.302018,1.064806,2.420013,0.000000,0.028717
+0.020000,0.308952,8.004707,0.323798,1.113206,2.420013,-0.000000,0.031012
+0.020000,0.331687,8.005440,0.346546,1.161606,2.420013,0.000000,0.033426
+0.020000,0.355391,8.006263,0.370262,1.210006,2.420013,0.000000,0.035965
+0.020000,0.380058,8.007183,0.394946,1.258407,2.420013,0.000000,0.038629
+0.020000,0.405688,8.008209,0.420598,1.306807,2.420013,-0.000000,0.041421
+0.020000,0.432283,8.009350,0.447218,1.355207,2.420013,0.000000,0.044346
+0.020000,0.459842,8.010616,0.474806,1.403607,2.420013,0.000000,0.047406
+0.020000,0.488364,8.012014,0.503363,1.452008,2.420013,-0.000000,0.050604
+0.020000,0.517849,8.013557,0.532887,1.500408,2.420013,0.000000,0.053946
+0.020000,0.548295,8.015254,0.563379,1.548808,2.420013,-0.000000,0.057433
+0.020000,0.579698,8.017117,0.594839,1.597208,2.420013,0.000000,0.061071
+0.020000,0.612062,8.019157,0.627267,1.645609,2.420013,-0.000000,0.064863
+0.020000,0.645385,8.021388,0.660663,1.694009,2.420013,0.000000,0.068815
+0.020000,0.679661,8.023821,0.695028,1.742409,2.420013,-0.000000,0.072930
+0.020000,0.714894,8.026471,0.730360,1.790809,2.420013,0.000000,0.077215
+0.020000,0.751081,8.029352,0.766660,1.839210,2.420013,0.000000,0.081674
+0.020000,0.788217,8.032478,0.803928,1.887610,2.420013,-0.000000,0.086313
+0.020000,0.826303,8.035866,0.842164,1.936010,2.420013,0.000000,0.091139
+0.020000,0.865335,8.039532,0.881369,1.984410,2.420013,0.000000,0.096156
+0.020000,0.905312,8.043492,0.921541,2.032811,2.420013,0.000000,0.101372
+0.020000,0.946230,8.047766,0.962681,2.081211,2.420013,-0.000000,0.106793
+0.020000,0.988085,8.052372,1.004789,2.129611,2.420013,0.000000,0.112428
+0.020000,1.030873,8.057330,1.047865,2.178011,2.420013,-0.000000,0.118282
+0.020000,1.074596,8.062660,1.091910,2.226412,2.420013,0.000000,0.124365
+0.020000,1.119240,8.068384,1.136922,2.274812,2.420013,-0.000000,0.130684
+0.020000,1.164810,8.074525,1.182902,2.323212,2.420013,0.000000,0.137250
+0.020000,1.211295,8.081107,1.229850,2.371612,2.420013,0.000000,0.144070
+0.020000,1.258690,8.088154,1.277767,2.420013,2.420013,-0.000000,0.151154
+0.020000,1.306989,8.095692,1.326651,2.468413,2.420013,0.000000,0.158512
+0.020000,1.356186,8.103749,1.376503,2.516813,2.420013,0.000000,0.166156
+0.020000,1.406272,8.112352,1.427323,2.565213,2.420013,-0.000000,0.174096
+0.020000,1.457238,8.121532,1.479112,2.613614,2.420013,0.000000,0.182343
+0.020000,1.509080,8.131320,1.531868,2.662014,2.420013,0.000000,0.190911
+0.020000,1.561781,8.141748,1.585592,2.710414,2.420013,0.000000,0.199810
+0.020000,1.615336,8.152851,1.640284,2.758814,2.420013,0.000000,0.209056
+0.020000,1.669728,8.164663,1.695945,2.807215,2.420013,-0.000000,0.218661
+0.020000,1.724946,8.177221,1.752573,2.855615,2.420013,-0.000000,0.228639
+0.020000,1.780975,8.190565,1.810169,2.904015,2.420013,0.000000,0.239005
+0.020000,1.837799,8.204734,1.868734,2.952415,2.420013,-0.000000,0.249775
+0.020000,1.895400,8.219771,1.928266,3.000816,2.420013,0.000000,0.260962
+0.020000,1.953761,8.235719,1.988766,3.049216,2.420013,-0.000000,0.272585
+0.020000,2.012857,8.252623,2.050235,3.097616,2.420013,-0.000000,0.284658
+0.020000,2.072660,8.270527,2.112659,3.144836,2.360988,-2.951235,0.297197
+0.020000,2.133117,8.289473,2.176016,3.190875,2.301963,-2.951235,0.310213
+0.020000,2.194182,8.309502,2.240282,3.235734,2.242938,-2.951235,0.323716
+0.020000,2.255803,8.330656,2.305434,3.279412,2.183914,-2.951235,0.337718
+0.020000,2.317925,8.352978,2.371447,3.321910,2.124889,-2.951235,0.352229
+0.020000,2.380499,8.376510,2.438298,3.363227,2.065864,-2.951235,0.367258
+0.020000,2.443460,8.401294,2.505964,3.403364,2.006840,-2.951235,0.382813
+0.020000,2.506755,8.427373,2.574421,3.442320,1.947815,-2.951235,0.398900
+0.020000,2.570319,8.454787,2.643645,3.480096,1.888790,-2.951235,0.415525
+0.020000,2.634088,8.483577,2.713613,3.516691,1.829766,-2.951235,0.432689
+0.020000,2.697996,8.513782,2.784301,3.552106,1.770741,-2.951235,0.450395
+0.020000,2.761976,8.545440,2.855686,3.586341,1.711716,-2.951235,0.468640
+0.020000,2.825956,8.578586,2.927743,3.619394,1.652691,-2.951235,0.487421
+0.020000,2.889864,8.613254,3.000450,3.651268,1.593667,-2.951235,0.506728
+0.020000,2.953624,8.649474,3.073782,3.681961,1.534642,-2.951235,0.526551
+0.020000,3.017163,8.687276,3.147716,3.711473,1.475617,-2.951235,0.546877
+0.020000,3.080401,8.726682,3.222229,3.739805,1.416593,-2.951235,0.567685
+0.020000,3.143261,8.767713,3.297297,3.766956,1.357568,-2.951235,0.588954
+0.020000,3.205664,8.810385,3.372895,3.792927,1.298543,-2.951235,0.610657
+0.020000,3.267530,8.854709,3.449002,3.817717,1.239519,-2.951235,0.632762
+0.020000,3.328778,8.900691,3.525592,3.841327,1.180494,-2.951235,0.655235
+0.020000,3.389332,8.948334,3.602643,3.863757,1.121469,-2.951235,0.678036
+0.020000,3.449114,8.997631,3.680131,3.885005,1.062445,-2.951235,0.701123
+0.020000,3.508048,9.048572,3.758032,3.905074,1.003420,-2.951235,0.724449
+0.020000,3.566062,9.101140,3.836322,3.923962,0.944395,-2.951235,0.747964
+0.020000,3.623085,9.155315,3.914978,3.941669,0.885370,-2.951235,0.771617
+0.020000,3.679051,9.211068,3.993977,3.958196,0.826346,-2.951235,0.795354
+0.020000,3.733897,9.268364,4.073294,3.973543,0.767321,-2.951235,0.819119
+0.020000,3.787565,9.327165,4.152907,3.987708,0.708296,-2.951235,0.842858
+0.020000,3.840003,9.387426,4.232791,4.000694,0.649272,-2.951235,0.866513
+0.020000,3.891163,9.449098,4.312923,4.012499,0.590247,-2.951235,0.890032
+0.020000,3.941004,9.512127,4.393279,4.023123,0.531222,-2.951235,0.913361
+0.020000,3.989489,9.576457,4.473836,4.032567,0.472198,-2.951235,0.936450
+0.020000,4.036589,9.642026,4.554570,4.040831,0.413173,-2.951235,0.959249
+0.020000,4.082279,9.708771,4.635457,4.047914,0.354148,-2.951235,0.981716
+0.020000,4.126542,9.776626,4.716475,4.053816,0.295123,-2.951235,1.003809
+0.020000,4.169365,9.845524,4.797598,4.058538,0.236099,-2.951235,1.025490
+0.020000,4.210742,9.915396,4.878804,4.062080,0.177074,-2.951235,1.046728
+0.020000,4.250671,9.986174,4.960069,4.064441,0.118049,-2.951235,1.067493
+0.020000,4.289157,10.057787,5.041370,4.065609,0.058408,-2.982093,1.087762
+0.020000,4.326201,10.130155,5.122670,4.064416,-0.059642,-5.902470,1.107512
+0.020000,4.361814,10.203200,5.203935,4.062043,-0.118667,-2.951235,1.126723
+0.020000,4.396012,10.276851,5.285140,4.058489,-0.177691,-2.951235,1.145384
+0.020000,4.428817,10.351044,5.366263,4.053754,-0.236716,-2.951235,1.163488
+0.020000,4.460253,10.425711,5.447278,4.047840,-0.295741,-2.951235,1.181029
+0.020000,4.490348,10.500789,5.528164,4.040744,-0.354765,-2.951235,1.198005
+0.020000,4.519130,10.576215,5.608896,4.032468,-0.413790,-2.951235,1.214418
+0.020000,4.546631,10.651929,5.689451,4.023012,-0.472815,-2.951235,1.230270
+0.020000,4.572884,10.727872,5.769805,4.012375,-0.531839,-2.951235,1.245568
+0.020000,4.597924,10.803988,5.849934,4.000558,-0.590864,-2.951235,1.260318
+0.020000,4.621784,10.880222,5.929816,3.987560,-0.649889,-2.951235,1.274529
+0.020000,4.644502,10.956520,6.009425,3.973382,-0.708914,-2.951235,1.288210
+0.020000,4.666113,11.032833,6.088739,3.958023,-0.767938,-2.951235,1.301375
+0.020000,4.686654,11.109110,6.167734,3.941484,-0.826963,-2.951235,1.314033
+0.020000,4.706162,11.185304,6.246387,3.923764,-0.885988,-2.951235,1.326198
+0.020000,4.724673,11.261370,6.324673,3.904864,-0.945012,-2.951235,1.337883
+0.020000,4.742225,11.337263,6.402569,3.884783,-1.004037,-2.951235,1.349101
+0.020000,4.758852,11.412940,6.480053,3.863522,-1.063062,-2.951235,1.359866
+0.020000,4.774592,11.488361,6.557099,3.841080,-1.122086,-2.951235,1.370191
+0.020000,4.789479,11.563485,6.633684,3.817458,-1.181111,-2.951235,1.380091
+0.020000,4.803547,11.638275,6.709785,3.792655,-1.240136,-2.951235,1.389579
+0.020000,4.816831,11.712691,6.785378,3.766672,-1.299160,-2.951235,1.398668
+0.020000,4.829363,11.786699,6.860440,3.739508,-1.358185,-2.951235,1.407373
+0.020000,4.841177,11.860263,6.934947,3.711164,-1.417210,-2.951235,1.415705
+0.020000,4.852304,11.933349,7.008875,3.681640,-1.476235,-2.951235,1.423679
+0.020000,4.862774,12.005923,7.082201,3.650934,-1.535259,-2.951235,1.431306
+0.020000,4.872618,12.077954,7.154900,3.619049,-1.594284,-2.951235,1.438598
+0.020000,4.881864,12.149408,7.226951,3.585983,-1.653309,-2.951235,1.445569
+0.020000,4.890542,12.220255,7.298328,3.551736,-1.712333,-2.951235,1.452229
+0.020000,4.898678,12.290466,7.369008,3.516309,-1.771358,-2.951235,1.458590
+0.020000,4.906299,12.360010,7.438969,3.479701,-1.830383,-2.951235,1.464662
+0.020000,4.913431,12.428857,7.508185,3.441913,-1.889407,-2.951235,1.470456
+0.020000,4.920098,12.496980,7.576633,3.402944,-1.948432,-2.951235,1.475983
+0.020000,4.926325,12.564350,7.644291,3.362795,-2.007457,-2.951235,1.481252
+0.020000,4.932134,12.630940,7.711133,3.321466,-2.066482,-2.951235,1.486273
+0.020000,4.937548,12.696722,7.777137,3.278955,-2.125506,-2.951235,1.491055
+0.020000,4.942589,12.761668,7.842280,3.235265,-2.184531,-2.951235,1.495608
+0.020000,4.947276,12.825754,7.906536,3.190394,-2.243556,-2.951235,1.499939
+0.020000,4.951630,12.888952,7.969884,3.144342,-2.302580,-2.951235,1.504058
+0.020000,4.955669,12.951235,8.032298,3.097110,-2.361605,-2.951235,1.507972
+0.020000,4.959413,13.012579,8.093756,3.048710,-2.420013,-2.920376,1.511689
+0.020000,4.962879,13.072970,8.154246,3.000309,-2.420013,-0.000000,1.515218
+0.020000,4.966086,13.132406,8.213769,2.951909,-2.420013,0.000000,1.518567
+0.020000,4.969049,13.190885,8.272323,2.903509,-2.420013,0.000000,1.521745
+0.020000,4.971785,13.248406,8.329909,2.855109,-2.420013,0.000000,1.524758
+0.020000,4.974310,13.304968,8.386527,2.806708,-2.420013,0.000000,1.527614
+0.020000,4.976637,13.360569,8.442177,2.758308,-2.420013,-0.000000,1.530322
+0.020000,4.978779,13.415209,8.496860,2.709908,-2.420013,0.000000,1.532886
+0.020000,4.980749,13.468888,8.550574,2.661508,-2.420013,-0.000000,1.535314
+0.020000,4.982559,13.521603,8.603320,2.613107,-2.420013,0.000000,1.537612
+0.020000,4.984220,13.573354,8.655098,2.564707,-2.420013,0.000000,1.539786
+0.020000,4.985743,13.624141,8.705908,2.516307,-2.420013,-0.000000,1.541840
+0.020000,4.987138,13.673964,8.755750,2.467907,-2.420013,0.000000,1.543782
+0.020000,4.988413,13.722821,8.804624,2.419506,-2.420013,-0.000000,1.545615
+0.020000,4.989577,13.770714,8.852531,2.371106,-2.420013,-0.000000,1.547345
+0.020000,4.990640,13.817640,8.899469,2.322706,-2.420013,0.000000,1.548976
+0.020000,4.991607,13.863600,8.945439,2.274306,-2.420013,0.000000,1.550513
+0.020000,4.992487,13.908593,8.990441,2.225905,-2.420013,0.000000,1.551961
+0.020000,4.993286,13.952620,9.034475,2.177505,-2.420013,0.000000,1.553323
+0.020000,4.994011,13.995680,9.077541,2.129105,-2.420013,0.000000,1.554603
+0.020000,4.994667,14.037773,9.119639,2.080705,-2.420013,-0.000000,1.555806
+0.020000,4.995260,14.078899,9.160769,2.032304,-2.420013,0.000000,1.556935
+0.020000,4.995795,14.119057,9.200931,1.983904,-2.420013,0.000000,1.557993
+0.020000,4.996278,14.158248,9.240125,1.935504,-2.420013,0.000000,1.558985
+0.020000,4.996711,14.196472,9.278352,1.887104,-2.420013,-0.000000,1.559912
+0.020000,4.997101,14.233728,9.315610,1.838703,-2.420013,0.000000,1.560780
+0.020000,4.997449,14.270017,9.351900,1.790303,-2.420013,0.000000,1.561590
+0.020000,4.997761,14.305337,9.387222,1.741903,-2.420013,0.000000,1.562345
+0.020000,4.998039,14.339690,9.421576,1.693503,-2.420013,-0.000000,1.563048
+0.020000,4.998287,14.373075,9.454962,1.645102,-2.420013,0.000000,1.563703
+0.020000,4.998507,14.405493,9.487380,1.596702,-2.420013,0.000000,1.564311
+0.020000,4.998702,14.436942,9.518830,1.548302,-2.420013,-0.000000,1.564876
+0.020000,4.998874,14.467424,9.549312,1.499902,-2.420013,0.000000,1.565399
+0.020000,4.999026,14.496937,9.578826,1.451501,-2.420013,-0.000000,1.565883
+0.020000,4.999160,14.525483,9.607372,1.403101,-2.420013,0.000000,1.566330
+0.020000,4.999278,14.553061,9.634950,1.354701,-2.420013,-0.000000,1.566743
+0.020000,4.999380,14.579671,9.661560,1.306301,-2.420013,0.000000,1.567123
+0.020000,4.999470,14.605312,9.687202,1.257900,-2.420013,0.000000,1.567472
+0.020000,4.999548,14.629986,9.711876,1.209500,-2.420013,0.000000,1.567793
+0.020000,4.999616,14.653692,9.735582,1.161100,-2.420013,0.000000,1.568086
+0.020000,4.999674,14.676430,9.758320,1.112700,-2.420013,-0.000000,1.568355
+0.020000,4.999725,14.698200,9.780090,1.064299,-2.420013,0.000000,1.568600
+0.020000,4.999768,14.719002,9.800892,1.015899,-2.420013,-0.000000,1.568824
+0.020000,4.999805,14.738836,9.820726,0.967511,-2.419395,0.030858,1.569027
+0.020000,4.999837,14.757714,9.839604,0.920304,-2.360371,2.951235,1.569211
+0.020000,4.999864,14.775660,9.857550,0.874277,-2.301346,2.951235,1.569378
+0.020000,4.999887,14.792697,9.874587,0.829431,-2.242321,2.951235,1.569529
+0.020000,4.999906,14.808849,9.890739,0.785765,-2.183297,2.951235,1.569665
+0.020000,4.999922,14.824139,9.906030,0.743279,-2.124272,2.951235,1.569789
+0.020000,4.999936,14.838592,9.920482,0.701974,-2.065247,2.951235,1.569900
+0.020000,4.999948,14.852230,9.934120,0.661850,-2.006222,2.951235,1.570001
+0.020000,4.999957,14.865078,9.946968,0.622906,-1.947198,2.951235,1.570091
+0.020000,4.999965,14.877158,9.959048,0.585142,-1.888173,2.951235,1.570172
+0.020000,4.999972,14.888495,9.970385,0.548559,-1.829148,2.951235,1.570245
+0.020000,4.999978,14.899112,9.981003,0.513157,-1.770124,2.951235,1.570311
+0.020000,4.999982,14.909033,9.990923,0.478935,-1.711099,2.951235,1.570370
+0.020000,4.999986,14.918282,10.000172,0.445893,-1.652074,2.951235,1.570422
+0.020000,4.999989,14.926881,10.008771,0.414032,-1.593050,2.951235,1.570469
+0.020000,4.999991,14.934855,10.016745,0.383352,-1.534025,2.951235,1.570511
+0.020000,4.999993,14.942227,10.024117,0.353852,-1.475000,2.951235,1.570549
+0.020000,4.999995,14.949021,10.030911,0.325532,-1.415976,2.951235,1.570582
+0.020000,4.999996,14.955260,10.037150,0.298393,-1.356951,2.951235,1.570612
+0.020000,4.999997,14.960968,10.042858,0.272435,-1.297926,2.951235,1.570638
+0.020000,4.999998,14.966169,10.048059,0.247657,-1.238901,2.951235,1.570661
+0.020000,4.999998,14.970886,10.052776,0.224059,-1.179877,2.951235,1.570682
+0.020000,4.999999,14.975143,10.057033,0.201642,-1.120852,2.951235,1.570700
+0.020000,4.999999,14.978964,10.060854,0.180406,-1.061827,2.951235,1.570716
+0.020000,4.999999,14.982371,10.064261,0.160350,-1.002803,2.951235,1.570729
+0.020000,5.000000,14.985390,10.067280,0.141474,-0.943778,2.951235,1.570741
+0.020000,5.000000,14.988042,10.069932,0.123779,-0.884753,2.951235,1.570752
+0.020000,5.000000,14.990353,10.072243,0.107264,-0.825729,2.951235,1.570761
+0.020000,5.000000,14.992345,10.074235,0.091930,-0.766704,2.951235,1.570768
+0.020000,5.000000,14.994042,10.075932,0.077777,-0.707679,2.951235,1.570775
+0.020000,5.000000,14.995467,10.077357,0.064804,-0.648654,2.951235,1.570780
+0.020000,5.000000,14.996646,10.078536,0.053011,-0.589630,2.951235,1.570784
+0.020000,5.000000,14.997600,10.079490,0.042399,-0.530605,2.951235,1.570788
+0.020000,5.000000,14.998353,10.080243,0.032967,-0.471580,2.951235,1.570790
+0.020000,5.000000,14.998930,10.080820,0.024716,-0.412556,2.951235,1.570792
+0.020000,5.000000,14.999354,10.081244,0.017646,-0.353531,2.951235,1.570794
+0.020000,5.000000,14.999648,10.081538,0.011756,-0.294506,2.951235,1.570795
+0.020000,5.000000,14.999836,10.081726,0.007046,-0.235482,2.951235,1.570796
+0.020000,5.000000,14.999941,10.081831,0.003517,-0.176457,2.951235,1.570796
+0.020000,5.000000,14.999988,10.081878,0.001168,-0.117432,2.951235,1.570796
+0.020000,5.000000,15.000000,10.081890,0.000000,-0.058408,2.951235,1.570796
+0.020000,5.000000,15.000000,10.081890,0.000000,0.000000,2.920376,1.570796
diff --git a/src/main/deploy/turnTest.right.pf1.csv b/src/main/deploy/turnTest.right.pf1.csv
new file mode 100644
index 0000000..f2c36c3
--- /dev/null
+++ b/src/main/deploy/turnTest.right.pf1.csv
@@ -0,0 +1,250 @@
+dt,x,y,position,velocity,acceleration,jerk,heading
+0.020000,-0.001758,6.750001,0.000012,0.001180,0.059025,2.951235,6.281779
+0.020000,-0.001752,6.750001,0.000017,0.000280,-0.045015,-5.201995,6.281784
+0.020000,-0.001739,6.750001,0.000030,0.000630,0.017512,3.126378,6.281794
+0.020000,-0.001717,6.750001,0.000052,0.001121,0.024519,0.350315,6.281812
+0.020000,-0.001682,6.750001,0.000087,0.001751,0.031527,0.350419,6.281840
+0.020000,-0.001631,6.750001,0.000138,0.002522,0.038539,0.350589,6.281880
+0.020000,-0.001563,6.750001,0.000207,0.003433,0.045556,0.350841,6.281935
+0.020000,-0.001473,6.750001,0.000296,0.004485,0.052580,0.351194,6.282007
+0.020000,-0.001360,6.750001,0.000410,0.005677,0.059613,0.351662,6.282098
+0.020000,-0.001219,6.750001,0.000550,0.007010,0.066658,0.352262,6.282210
+0.020000,-0.001050,6.750000,0.000720,0.008485,0.073718,0.353013,6.282346
+0.020000,-0.000848,6.750000,0.000922,0.010101,0.080797,0.353929,6.282507
+0.020000,-0.000610,6.750000,0.001159,0.011858,0.087897,0.355029,6.282697
+0.020000,-0.000335,6.750000,0.001434,0.013759,0.095024,0.356329,6.282917
+0.020000,-0.000019,6.750000,0.001750,0.015803,0.102181,0.357846,6.283170
+0.020000,0.003203,6.750000,0.004973,0.161130,7.266373,358.209629,0.000273
+0.020000,0.007023,6.750002,0.008792,0.190988,1.492890,-288.674191,0.000598
+0.020000,0.011304,6.750005,0.013073,0.214042,1.152710,-17.009001,0.000963
+0.020000,0.016072,6.750011,0.017841,0.238391,1.217437,3.236369,0.001371
+0.020000,0.021362,6.750019,0.023131,0.264503,1.305612,4.408768,0.001823
+0.020000,0.027192,6.750032,0.028962,0.291525,1.351090,2.273887,0.002322
+0.020000,0.033589,6.750048,0.035358,0.319830,1.415249,3.207923,0.002871
+0.020000,0.040589,6.750070,0.042358,0.349996,1.508305,4.652811,0.003473
+0.020000,0.048206,6.750099,0.049975,0.380849,1.542631,1.716321,0.004128
+0.020000,0.056478,6.750136,0.058248,0.413640,1.639589,4.847890,0.004842
+0.020000,0.065422,6.750183,0.067192,0.447203,1.678141,1.927575,0.005616
+0.020000,0.075077,6.750241,0.076847,0.482762,1.777952,4.990561,0.006453
+0.020000,0.085459,6.750313,0.087229,0.519088,1.816292,1.917014,0.007355
+0.020000,0.096600,6.750400,0.098371,0.557091,1.900130,4.191905,0.008327
+0.020000,0.108530,6.750506,0.110301,0.596518,1.971347,3.560836,0.009370
+0.020000,0.121271,6.750633,0.123043,0.637092,2.028737,2.869489,0.010488
+0.020000,0.134853,6.750783,0.136625,0.679102,2.100474,3.586886,0.011684
+0.020000,0.149302,6.750961,0.151076,0.722541,2.171977,3.575133,0.012961
+0.020000,0.164649,6.751171,0.166424,0.767405,2.243192,3.560756,0.014323
+0.020000,0.180921,6.751415,0.182698,0.813686,2.314066,3.543683,0.015773
+0.020000,0.198146,6.751700,0.199925,0.861377,2.384543,3.523848,0.017315
+0.020000,0.216353,6.752031,0.218135,0.910469,2.454567,3.501191,0.018953
+0.020000,0.235568,6.752411,0.237354,0.960950,2.524080,3.475657,0.020690
+0.020000,0.255819,6.752849,0.257610,1.012811,2.593024,3.447200,0.022531
+0.020000,0.277134,6.753350,0.278931,1.066037,2.661339,3.415785,0.024479
+0.020000,0.299550,6.753922,0.301353,1.121137,2.754994,4.682725,0.026540
+0.020000,0.323072,6.754572,0.324885,1.176595,2.772898,0.895199,0.028717
+0.020000,0.347711,6.755308,0.349534,1.232453,2.792875,0.998853,0.031012
+0.020000,0.373462,6.756138,0.375299,1.288225,2.788601,-0.213676,0.033426
+0.020000,0.400337,6.757071,0.402190,1.344570,2.817258,1.432847,0.035965
+0.020000,0.428331,6.758115,0.430204,1.400696,2.806292,-0.548324,0.038629
+0.020000,0.457450,6.759281,0.459346,1.457093,2.819870,0.678916,0.041421
+0.020000,0.487697,6.760579,0.489621,1.513747,2.832667,0.639858,0.044346
+0.020000,0.519077,6.762020,0.521034,1.570640,2.844676,0.600420,0.047406
+0.020000,0.551593,6.763615,0.553589,1.627758,2.855890,0.560692,0.050604
+0.020000,0.585248,6.765375,0.587291,1.685084,2.866305,0.520767,0.053946
+0.020000,0.620046,6.767315,0.622143,1.742602,2.875920,0.480748,0.057433
+0.020000,0.655989,6.769447,0.658149,1.800297,2.884735,0.440743,0.061071
+0.020000,0.693084,6.771786,0.695317,1.858408,2.905535,1.040013,0.064863
+0.020000,0.731335,6.774346,0.733654,1.916853,2.922252,0.835863,0.068815
+0.020000,0.770743,6.777144,0.773161,1.975368,2.925747,0.174726,0.072930
+0.020000,0.811317,6.780195,0.813849,2.034411,2.952162,1.320738,0.077215
+0.020000,0.853060,6.783518,0.855725,2.093775,2.968213,0.802554,0.081674
+0.020000,0.895975,6.787131,0.898792,2.153335,2.978007,0.489734,0.086313
+0.020000,0.940068,6.791054,0.943058,2.213342,3.000339,1.116572,0.091139
+0.020000,0.985344,6.795306,0.988534,2.273780,3.021869,1.076508,0.096156
+0.020000,1.031810,6.799910,1.035227,2.334631,3.042592,1.036175,0.101372
+0.020000,1.079468,6.804888,1.083144,2.395882,3.062508,0.995756,0.106793
+0.020000,1.128323,6.810264,1.132295,2.457514,3.081616,0.955443,0.112428
+0.020000,1.178381,6.816064,1.182687,2.519623,3.105436,1.190987,0.118282
+0.020000,1.229652,6.822314,1.234337,2.582515,3.144637,1.960024,0.124365
+0.020000,1.282131,6.829043,1.287247,2.645463,3.147398,0.138086,0.130684
+0.020000,1.335834,6.836280,1.341434,2.709394,3.196539,2.457055,0.137250
+0.020000,1.390759,6.844057,1.396908,2.773684,3.214503,0.898161,0.144070
+0.020000,1.446913,6.852406,1.453679,2.838552,3.243390,1.444361,0.151154
+0.020000,1.504301,6.861363,1.511761,2.904111,3.277962,1.728632,0.158512
+0.020000,1.562927,6.870964,1.571168,2.970343,3.311603,1.682027,0.166156
+0.020000,1.622794,6.881248,1.631913,3.037229,3.344298,1.634750,0.174096
+0.020000,1.683906,6.892255,1.694008,3.104750,3.376039,1.587034,0.182343
+0.020000,1.746272,6.904030,1.757476,3.173397,3.432374,2.816775,0.190911
+0.020000,1.809885,6.916618,1.822322,3.242329,3.446588,0.710672,0.199810
+0.020000,1.874757,6.930067,1.888574,3.312559,3.511483,3.244763,0.209056
+0.020000,1.940882,6.944427,1.956239,3.383294,3.536737,1.262726,0.218661
+0.020000,2.008261,6.959751,2.025340,3.455015,3.586047,2.465477,0.228639
+0.020000,2.076895,6.976097,2.095894,3.527690,3.633764,2.385847,0.239005
+0.020000,2.146781,6.993524,2.167919,3.601286,3.679794,2.301495,0.249775
+0.020000,2.217913,7.012093,2.241435,3.675767,3.724041,2.212365,0.260962
+0.020000,2.290288,7.031871,2.316464,3.751460,3.784696,3.032755,0.272585
+0.020000,2.363894,7.052925,2.393022,3.827915,3.822750,1.902694,0.284658
+0.020000,2.438712,7.075326,2.471121,3.904946,3.851531,1.439028,0.297197
+0.020000,2.514694,7.099137,2.550747,3.981298,3.817576,-1.697732,0.310213
+0.020000,2.591797,7.124427,2.631892,4.057235,3.796882,-1.034690,0.323716
+0.020000,2.669972,7.151265,2.714545,4.132654,3.770936,-1.297319,0.337718
+0.020000,2.749164,7.179720,2.798694,4.207451,3.739834,-1.555089,0.352229
+0.020000,2.829321,7.209866,2.884333,4.281955,3.725239,-0.729746,0.367258
+0.020000,2.910374,7.241772,2.971439,4.355319,3.668181,-2.852920,0.382813
+0.020000,2.992261,7.275512,3.060005,4.428284,3.648265,-0.995774,0.398900
+0.020000,3.074906,7.311156,3.150009,4.500203,3.595911,-2.617714,0.415525
+0.020000,3.158230,7.348775,3.241431,4.571121,3.545927,-2.499221,0.432689
+0.020000,3.242147,7.388438,3.334249,4.640900,3.488967,-2.847985,0.450395
+0.020000,3.326568,7.430211,3.428440,4.709516,3.430772,-2.909736,0.468640
+0.020000,3.411392,7.474156,3.523971,4.776574,3.352881,-3.894557,0.487421
+0.020000,3.496512,7.520333,3.620810,4.841957,3.269193,-4.184408,0.506728
+0.020000,3.581817,7.568793,3.718919,4.905441,3.174173,-4.751008,0.526551
+0.020000,3.667191,7.619585,3.818259,4.966999,3.077899,-4.813702,0.546877
+0.020000,3.752504,7.672747,3.918781,5.026069,2.953507,-6.219566,0.567685
+0.020000,3.837626,7.728310,4.020432,5.082552,2.824161,-6.467337,0.588954
+0.020000,3.922421,7.786295,4.123158,5.136304,2.687568,-6.829635,0.610657
+0.020000,4.006748,7.846713,4.226894,5.186804,2.525008,-8.127980,0.632762
+0.020000,4.090460,7.909560,4.331572,5.233929,2.356279,-8.436464,0.655235
+0.020000,4.173414,7.974826,4.437123,5.277529,2.179962,-8.815834,0.678036
+0.020000,4.255460,8.042483,4.543467,5.317188,1.982984,-9.848933,0.701123
+0.020000,4.336452,8.112491,4.650522,5.352760,1.778605,-10.218950,0.724449
+0.020000,4.416247,8.184797,4.758204,5.384087,1.566329,-10.613776,0.747964
+0.020000,4.494704,8.259335,4.866423,5.410993,1.345330,-11.049969,0.771617
+0.020000,4.571690,8.336027,4.975090,5.433351,1.117877,-11.372625,0.795354
+0.020000,4.647077,8.414783,5.084112,5.451078,0.886353,-11.576220,0.819119
+0.020000,4.720749,8.495500,5.193395,5.464142,0.653183,-11.658510,0.842858
+0.020000,4.792598,8.578066,5.302846,5.472557,0.420770,-11.620645,0.866513
+0.020000,4.862528,8.662364,5.412374,5.476386,0.191428,-11.467111,0.890032
+0.020000,4.930457,8.748267,5.521889,5.475750,-0.031788,-11.160804,0.913361
+0.020000,4.996313,8.835643,5.631304,5.470738,-0.250619,-10.941509,0.936450
+0.020000,5.060040,8.924357,5.740535,5.461562,-0.458778,-10.407952,0.959249
+0.020000,5.121594,9.014275,5.849503,5.448416,-0.657323,-9.927269,0.981716
+0.020000,5.180945,9.105259,5.958134,5.431540,-0.843759,-9.321795,1.003809
+0.020000,5.238076,9.197174,6.066357,5.411162,-1.018941,-8.759114,1.025490
+0.020000,5.292980,9.289888,6.174108,5.387565,-1.179807,-8.043305,1.046728
+0.020000,5.345664,9.383272,6.281329,5.361013,-1.327636,-7.391438,1.067493
+0.020000,5.396144,9.477201,6.387963,5.331723,-1.464464,-6.841416,1.087762
+0.020000,5.444438,9.571544,6.493948,5.299267,-1.622797,-7.916642,1.107512
+0.020000,5.490575,9.666173,6.599226,5.263856,-1.770571,-7.388689,1.126723
+0.020000,5.534598,9.760981,6.703756,5.226517,-1.866934,-4.818169,1.145384
+0.020000,5.576555,9.855870,6.807506,5.187523,-1.949719,-4.139216,1.163488
+0.020000,5.616500,9.950744,6.910447,5.147042,-2.024027,-3.715399,1.181029
+0.020000,5.654491,10.045519,7.012553,5.105263,-2.088977,-3.247503,1.198005
+0.020000,5.690588,10.140112,7.113799,5.062342,-2.146041,-2.853200,1.214418
+0.020000,5.724855,10.234450,7.214169,5.018459,-2.194130,-2.404482,1.230270
+0.020000,5.757357,10.328466,7.313643,4.973744,-2.235783,-2.082623,1.245568
+0.020000,5.788158,10.422095,7.412209,4.928276,-2.273370,-1.879356,1.260318
+0.020000,5.817325,10.515281,7.509853,4.882181,-2.304779,-1.570453,1.274529
+0.020000,5.844924,10.607970,7.606564,4.835549,-2.331613,-1.341689,1.288210
+0.020000,5.871019,10.700115,7.702332,4.788445,-2.355165,-1.177583,1.301375
+0.020000,5.895675,10.791671,7.797150,4.740860,-2.379260,-1.204748,1.314033
+0.020000,5.918955,10.882596,7.891008,4.692904,-2.397811,-0.927598,1.326198
+0.020000,5.940921,10.972853,7.983900,4.644591,-2.415638,-0.891329,1.337883
+0.020000,5.961632,11.062408,8.075818,4.595941,-2.432518,-0.843978,1.349101
+0.020000,5.981148,11.151228,8.166757,4.546914,-2.451339,-0.941073,1.359866
+0.020000,5.999525,11.239283,8.256709,4.497622,-2.464575,-0.661783,1.370191
+0.020000,6.016817,11.326546,8.345669,4.447994,-2.481399,-0.841219,1.380091
+0.020000,6.033078,11.412991,8.433630,4.398032,-2.498102,-0.835150,1.389579
+0.020000,6.048359,11.498592,8.520584,4.347735,-2.514873,-0.838557,1.398668
+0.020000,6.062708,11.583328,8.606526,4.297097,-2.531876,-0.850131,1.407373
+0.020000,6.076174,11.667175,8.691449,4.246112,-2.549250,-0.868724,1.415705
+0.020000,6.088801,11.750115,8.775344,4.194751,-2.568092,-0.942101,1.423679
+0.020000,6.100632,11.832124,8.858202,4.142938,-2.590626,-1.126660,1.431306
+0.020000,6.111711,11.913187,8.940019,4.090817,-2.606065,-0.771980,1.438598
+0.020000,6.122076,11.993282,9.020781,4.038140,-2.633858,-1.389654,1.445569
+0.020000,6.131766,12.072393,9.100483,3.985101,-2.651922,-0.903197,1.452229
+0.020000,6.140818,12.150502,9.179115,3.931568,-2.676682,-1.237989,1.458590
+0.020000,6.149266,12.227591,9.256665,3.877521,-2.702314,-1.281613,1.464662
+0.020000,6.157144,12.303643,9.333124,3.822949,-2.728596,-1.314086,1.470456
+0.020000,6.164484,12.378641,9.408481,3.767839,-2.755535,-1.346951,1.475983
+0.020000,6.171317,12.452569,9.482724,3.712176,-2.783138,-1.380167,1.481252
+0.020000,6.177672,12.525412,9.555843,3.655948,-2.811413,-1.413728,1.486273
+0.020000,6.183576,12.597151,9.627826,3.599117,-2.841504,-1.504579,1.491055
+0.020000,6.189057,12.667772,9.698658,3.541623,-2.874721,-1.660846,1.495608
+0.020000,6.194139,12.737257,9.768329,3.483547,-2.903816,-1.454743,1.499939
+0.020000,6.198847,12.805591,9.836825,3.424800,-2.937361,-1.677244,1.504058
+0.020000,6.203203,12.872757,9.904132,3.365341,-2.972938,-1.778843,1.507972
+0.020000,6.207230,12.938739,9.970237,3.305238,-3.005117,-1.608958,1.511689
+0.020000,6.210949,13.003533,10.035138,3.245065,-3.008682,-0.178227,1.515218
+0.020000,6.214381,13.067149,10.098847,3.185430,-2.981734,1.347381,1.518567
+0.020000,6.217546,13.129595,10.161372,3.126272,-2.957912,1.191095,1.521745
+0.020000,6.220461,13.190878,10.222725,3.067631,-2.932056,1.292793,1.524758
+0.020000,6.223145,13.251007,10.282914,3.009467,-2.908180,1.193804,1.527614
+0.020000,6.225613,13.309990,10.341948,2.951694,-2.888670,0.975532,1.530322
+0.020000,6.227881,13.367833,10.399836,2.894384,-2.865495,1.158746,1.532886
+0.020000,6.229962,13.424544,10.456585,2.837471,-2.845639,0.992772,1.535314
+0.020000,6.231871,13.480130,10.512204,2.780931,-2.827020,0.930939,1.537612
+0.020000,6.233619,13.534597,10.566699,2.724752,-2.808941,0.903992,1.539786
+0.020000,6.235219,13.587951,10.620077,2.668924,-2.791396,0.877255,1.541840
+0.020000,6.236682,13.640200,10.672346,2.613436,-2.774383,0.850636,1.543782
+0.020000,6.238017,13.691348,10.723511,2.558278,-2.757902,0.824050,1.545615
+0.020000,6.239234,13.741402,10.773580,2.503439,-2.741953,0.797424,1.547345
+0.020000,6.240342,13.790366,10.822557,2.448846,-2.729663,0.614536,1.548976
+0.020000,6.241350,13.838248,10.870449,2.394596,-2.712518,0.857222,1.550513
+0.020000,6.242265,13.885050,10.917260,2.340568,-2.701354,0.558213,1.551961
+0.020000,6.243095,13.930779,10.962997,2.286827,-2.687079,0.713770,1.553323
+0.020000,6.243847,13.975439,11.007664,2.233336,-2.674566,0.625628,1.554603
+0.020000,6.244527,14.019036,11.051265,2.180081,-2.662742,0.591186,1.555806
+0.020000,6.245140,14.061572,11.093806,2.127057,-2.651207,0.576747,1.556935
+0.020000,6.245693,14.103054,11.135292,2.074257,-2.639970,0.561862,1.557993
+0.020000,6.246190,14.143484,11.175725,2.021676,-2.629040,0.546506,1.558985
+0.020000,6.246637,14.182868,11.215111,1.969286,-2.619512,0.476379,1.559912
+0.020000,6.247038,14.221208,11.253453,1.917106,-2.609020,0.524598,1.560780
+0.020000,6.247396,14.258508,11.290755,1.865118,-2.599380,0.482036,1.561590
+0.020000,6.247716,14.294773,11.327021,1.813311,-2.590353,0.451331,1.562345
+0.020000,6.248002,14.330005,11.362255,1.761673,-2.581899,0.422700,1.563048
+0.020000,6.248255,14.364209,11.396459,1.710218,-2.572770,0.456463,1.563703
+0.020000,6.248481,14.397386,11.429638,1.658922,-2.564786,0.399199,1.564311
+0.020000,6.248680,14.429541,11.461793,1.607782,-2.556987,0.389923,1.564876
+0.020000,6.248856,14.460677,11.492929,1.556795,-2.549386,0.380042,1.565399
+0.020000,6.249011,14.490795,11.523048,1.505955,-2.541995,0.369557,1.565883
+0.020000,6.249148,14.519900,11.552154,1.455258,-2.534826,0.358470,1.566330
+0.020000,6.249267,14.547994,11.580247,1.404681,-2.528841,0.299230,1.566743
+0.020000,6.249372,14.575079,11.607332,1.354251,-2.521508,0.366679,1.567123
+0.020000,6.249463,14.601157,11.633411,1.303934,-2.515840,0.283380,1.567472
+0.020000,6.249543,14.626232,11.658486,1.253748,-2.509335,0.325254,1.567793
+0.020000,6.249611,14.650305,11.682559,1.203659,-2.504440,0.244754,1.568086
+0.020000,6.249671,14.673379,11.705633,1.153690,-2.498455,0.299257,1.568355
+0.020000,6.249722,14.695455,11.727709,1.103824,-2.493286,0.258463,1.568600
+0.020000,6.249766,14.716536,11.748790,1.054057,-2.488365,0.246035,1.568824
+0.020000,6.249803,14.736624,11.768878,1.004390,-2.483310,0.252742,1.569027
+0.020000,6.249835,14.755732,11.787987,0.955419,-2.448582,1.736410,1.569211
+0.020000,6.249863,14.773887,11.806141,0.907719,-2.385004,3.178889,1.569378
+0.020000,6.249886,14.791113,11.823367,0.861296,-2.321142,3.193095,1.569529
+0.020000,6.249905,14.807435,11.839690,0.816140,-2.257786,3.167811,1.569665
+0.020000,6.249922,14.822880,11.855134,0.772234,-2.195293,3.124652,1.569789
+0.020000,6.249936,14.837472,11.869726,0.729589,-2.132262,3.151527,1.569900
+0.020000,6.249947,14.851236,11.883490,0.688190,-2.069943,3.115967,1.570001
+0.020000,6.249957,14.864196,11.896450,0.648026,-2.008199,3.087190,1.570091
+0.020000,6.249965,14.876378,11.908633,0.609106,-1.946017,3.109097,1.570172
+0.020000,6.249972,14.887807,11.920061,0.571417,-1.884439,3.078915,1.570245
+0.020000,6.249977,14.898506,11.930760,0.534956,-1.823049,3.069511,1.570311
+0.020000,6.249982,14.908500,11.940754,0.499719,-1.761839,3.060484,1.570370
+0.020000,6.249986,14.917814,11.950069,0.465703,-1.700802,3.051851,1.570422
+0.020000,6.249989,14.926472,11.958727,0.432905,-1.639930,3.043621,1.570469
+0.020000,6.249991,14.934499,11.966753,0.401317,-1.579381,3.027418,1.570511
+0.020000,6.249993,14.941917,11.974172,0.370942,-1.518765,3.030817,1.570549
+0.020000,6.249995,14.948753,11.981007,0.341777,-1.458236,3.026436,1.570582
+0.020000,6.249996,14.955029,11.987284,0.313814,-1.398143,3.004642,1.570612
+0.020000,6.249997,14.960770,11.993025,0.287056,-1.337918,3.011256,1.570638
+0.020000,6.249998,14.966000,11.998255,0.261499,-1.277841,3.003889,1.570661
+0.020000,6.249998,14.970743,12.002998,0.237140,-1.217963,2.993897,1.570682
+0.020000,6.249999,14.975023,12.007277,0.213978,-1.158084,2.993916,1.570700
+0.020000,6.249999,14.978863,12.011117,0.192014,-1.098189,2.994744,1.570716
+0.020000,6.249999,14.982288,12.014542,0.171239,-1.038773,2.970807,1.570729
+0.020000,6.250000,14.985321,12.017575,0.151661,-0.978880,2.994682,1.570741
+0.020000,6.250000,14.987986,12.020241,0.133275,-0.919326,2.977682,1.570752
+0.020000,6.250000,14.990308,12.022562,0.116077,-0.859885,2.972040,1.570761
+0.020000,6.250000,14.992309,12.024564,0.100068,-0.800428,2.972873,1.570768
+0.020000,6.250000,14.994014,12.026269,0.085251,-0.740895,2.976612,1.570775
+0.020000,6.250000,14.995447,12.027701,0.071621,-0.681494,2.970065,1.570780
+0.020000,6.250000,14.996630,12.028885,0.059178,-0.622118,2.968791,1.570784
+0.020000,6.250000,14.997589,12.029843,0.047923,-0.562764,2.967725,1.570788
+0.020000,6.250000,14.998346,12.030600,0.037854,-0.503427,2.966851,1.570790
+0.020000,6.250000,14.998925,12.031180,0.028972,-0.444104,2.966149,1.570792
+0.020000,6.250000,14.999351,12.031605,0.021277,-0.384792,2.965602,1.570794
+0.020000,6.250000,14.999646,12.031901,0.014767,-0.325488,2.965190,1.570795
+0.020000,6.250000,14.999835,12.032089,0.009443,-0.266190,2.964894,1.570796
+0.020000,6.250000,14.999941,12.032196,0.005305,-0.206896,2.964695,1.570796
+0.020000,6.250000,14.999988,12.032243,0.002353,-0.147605,2.964573,1.570796
+0.020000,6.250000,15.000000,12.032254,0.000587,-0.088315,2.964510,1.570796
+0.020000,6.250000,15.000000,12.032254,0.000000,-0.029335,2.948987,1.570796
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 18e88a1..9e23707 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -23,25 +23,40 @@ public final class Constants {
     public static final double kNeutralDeadband = 0.001;
     public static final double kMaxRpms = 5700;
     public static final double kGearRatio = 10.71;
-    public static final double kWheelCircumference = Math.PI*6;
-  
+    public static final double kWheelCircumference = Math.PI*5.9;
+
     //public final static Gains kGains_Velocity 
     //= new Gains(5e-5, 0, 0.0, 0, 1, -1);
-  
-    public final static int REMOTE_0 = 0;
-    public final static int REMOTE_1 = 1;
-    public final static int PID_PRIMARY = 0;
-    public final static int PID_TURN = 1;
-    public final static int SLOT_0 = 0;
-    public final static int SLOT_1 = 1;
-    public final static int SLOT_2 = 2;
-    public final static int SLOT_3 = 3;
-  
-  
-    public final static int kSlot_Distance = SLOT_0;
-    public final static int kSlot_Turning = SLOT_1;
-    public final static int kSlot_Velocity = SLOT_2;
-    public final static int kSlot_MotProf = SLOT_3;
-  
-    public static boolean onOrOff = false;
+    
+    public final static double kPDrive = 0.000600;
+    public final static double kIDrive = 0.0000005;
+    public final static double kDDrive = 0;
+    public final static double kIzDrive = 0;
+    public final static double kFFDrive = 0.000000;
+    public final static double kMaxOutputDrive = 1;
+    public final static double kMinOutputDrive = -1;
+
+    public final static int LL_LIGHT_DEFAULT = 0;
+    public final static int LL_LIGHT_OFF = 1;
+    public final static int LL_LIGHT_BLINK = 2;
+    public final static int LL_LIGHT_ON = 3;
+//     6     13 14  16
+    public final static int DriveLeft1 = 1;
+    public final static int DriveLeft2 = 2;
+    public final static int DriveLeft3 = 16;
+    public final static int DriveRight1 = 15;
+    public final static int DriveRight2 = 14;
+    public final static int DriveRight3 = 13;
+    public final static int Shooter = 10;
+    public final static int Intake = 5;
+    public final static int Hopper1 = 4;
+    public final static int Hopper2 = 11;
+    public final static int Elevator1 = 12;
+    public final static int Elevator2 = 3;
+    public final static int Belt = 9;    
+
+
+    public final static double maxRPMShooter = 5676;
+
 }
+
diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java
deleted file mode 100644
index a07fdf3..0000000
--- a/src/main/java/frc/robot/OI.java
+++ /dev/null
@@ -1,28 +0,0 @@
-    /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package frc.robot;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
-
-/**
- * This class is the glue that binds the controls on the physical operator
- * interface to the commands and command groups that allow control of the robot.
- */
-public class OI {
-  public Joystick controller = new Joystick(0);
-  public JoystickButton buttonOne = new JoystickButton(controller, 1);  
-  public JoystickButton buttonTwo = new JoystickButton(controller, 2);  
-  public JoystickButton buttonThree = new JoystickButton(controller, 3);  
-  public JoystickButton buttonFour = new JoystickButton(controller, 4);
-  public JoystickButton buttonFive = new JoystickButton(controller, 5);
-  public JoystickButton buttonSix = new JoystickButton(controller, 6);
-
-  public final static int joystick = 0;
-}
diff --git a/src/main/java/frc/robot/Path.java b/src/main/java/frc/robot/Path.java
index a5a87fa..544925d 100644
--- a/src/main/java/frc/robot/Path.java
+++ b/src/main/java/frc/robot/Path.java
@@ -2,39 +2,42 @@
 import java.io.File;
 import java.io.IOException;
 import java.util.ArrayList;
-import java.util.List;
 import java.util.Scanner; 
 import java.util.logging.*;
 
 public class Path{
-    private ArrayList rightString = new ArrayList<>();
-    private ArrayList leftString = new ArrayList<>();
+    private ArrayList<Double> m_rightString = new ArrayList<Double>();
+    private ArrayList<Double> m_leftString = new ArrayList<Double>();
     private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
-    public Path(){}    
+    private String pathName;
+    public Path(String PathName){
+        pathName = PathName;
+    }    
 
-    public ArrayList returnLeftList() throws IOException{
-            File file = new File("/paths/StraightTen.left.pf1.csv");
+    public ArrayList<Double> returnLeftList() throws IOException{
+            File file = new File("/home/lvuser/deploy/output/" + pathName + ".left.pf1.csv");
             Scanner sc = new Scanner(file);
             String[] testArray;
         
         sc.next();
         while(sc.hasNext()){
             testArray = sc.next().split(",");
-            leftString.add(testArray[4]);
+            m_leftString.add(Double.valueOf(testArray[4]));
         }
-        
-        return leftString;
+        sc.close();
+        return m_leftString;
     }
 
-    public ArrayList returnRightList() throws IOException{
-        File file = new File("/paths/StraightTen.right.pf1.csv");
+    public ArrayList<Double> returnRightList() throws IOException{
+        File file = new File("/home/lvuser/deploy/output/" + pathName + ".right.pf1.csv");
         Scanner sc = new Scanner(file);
         String[] testArray;
         sc.next();
         while(sc.hasNext()){
             testArray = sc.next().split(",");
-            rightString.add(testArray[4]);
+            m_rightString.add(Double.valueOf(testArray[4]));
         }
-        return rightString;
+        sc.close();
+        return m_rightString;
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1af4c02..3a1c1c3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -7,12 +7,16 @@
 
 package frc.robot;
 
+import java.util.logging.Logger;
+
+
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.ScheduleCommand;
-import frc.robot.commands.DriveCommand;
 
 /**
  * The VM is configured to automatically run this class, and to call the functions corresponding to
@@ -22,8 +26,10 @@
  */
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
-
-  private RobotContainer m_robotContainer;
+  private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
+  public static RobotContainer m_robotContainer;
+  private ShuffleboardTab driverTab = Shuffleboard.getTab("Driver Tab");
+  
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -31,10 +37,17 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
+    
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+    driverTab.add("PDP", 0).withWidget(BuiltInWidgets.kPowerDistributionPanel);
+    driverTab.add("Differential Drive", 0).withWidget(BuiltInWidgets.kDifferentialDrive);
+    
+
   }
+  
 
   /**
    * This function is called every robot packet, no matter the mode. Use this for items like
@@ -45,9 +58,17 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
-    SmartDashboard.putNumber("Left Velocity", -1 * m_robotContainer.mDriveSubsystem.leftVelocity());    
-    SmartDashboard.putNumber("Right Velocity", -1 * m_robotContainer.mDriveSubsystem.rightVelocity());
-    m_robotContainer.mDriveSubsystem.updatePID();
+    RobotContainer.AutoChooser = SmartDashboard.getNumber("Auto Selection", 0);
+    SmartDashboard.putNumber("Auto Selected", RobotContainer.AutoChooser);
+    
+    RobotContainer.mDriveSubsystem.updatePID();
+    double x = RobotContainer.tx.getDouble(0.0);
+    double y = RobotContainer.ty.getDouble(0.0);
+    double area = RobotContainer.ta.getDouble(0.0);
+    SmartDashboard.putNumber("Elevator Pos", RobotContainer.mElevatorSubystem.getPosition());
+    SmartDashboard.putNumber("LimelightX", x);
+    SmartDashboard.putNumber("LimelightY", y);
+    SmartDashboard.putNumber("LimelightArea", area);
     // Runs the Smcheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
@@ -71,7 +92,7 @@ public void disabledPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    //m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
@@ -84,6 +105,7 @@ public void autonomousInit() {
    */
   @Override
   public void autonomousPeriodic() {
+    CommandScheduler.getInstance().run();
   }
 
   @Override
@@ -95,7 +117,6 @@ public void teleopInit() {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    m_robotContainer.mDriveCommand.schedule();
   }
 
   /**
@@ -118,4 +139,4 @@ public void testInit() {
   @Override
   public void testPeriodic() {
   }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9cdaee0..afa8ccf 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -11,14 +11,57 @@
 import java.util.ArrayList;
 import java.util.logging.Logger;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.XboxController;
-import frc.robot.commands.DriveCommand;
-import frc.robot.commands.RunPath;
-import frc.robot.commands.RunVelocity;
-import frc.robot.commands.toggleOff;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ElevatorSubystem;
+import frc.robot.subsystems.HopperSubsystem;
+import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.ConveyorSubsystem;
 import frc.robot.Path;
+import frc.robot.commands.auto.RunPath;
+import frc.robot.commands.auto.RunPathBack;
+import frc.robot.commands.auto.Turn;
+import frc.robot.commands.auto.TurnAimShoot;
+import frc.robot.commands.auto.autoCloseShoot;
+import frc.robot.commands.auto.autoConveyor;
+import frc.robot.commands.auto.autoDeCorrect;
+import frc.robot.commands.auto.autoHopperVortex;
+import frc.robot.commands.auto.autoResetEncoders;
+import frc.robot.commands.auto.autoShoot;
+import frc.robot.commands.auto.autoStoreValue;
+import frc.robot.commands.auto.setConveyorSpeed;
+import frc.robot.commands.auto.setIntakeSpeed;
+import frc.robot.commands.auto.stopConveyor;
+import frc.robot.commands.auto.stopIntake;
+import frc.robot.commands.drive.Aiming;
+import frc.robot.commands.drive.DriveCommand;
+import frc.robot.commands.elevator.ArmElevator;
+import frc.robot.commands.elevator.brakeElevator;
+import frc.robot.commands.elevator.moveLift;
+import frc.robot.commands.hopper.hopperIn;
+import frc.robot.commands.hopper.hopperOut;
+import frc.robot.commands.hopper.hopperVortex;
+import frc.robot.commands.intake.runIntakeIn;
+import frc.robot.commands.intake.runIntakeOut;
+import frc.robot.commands.intake.toggleIntake;
+import frc.robot.commands.shooter.ShootCommand;
+import frc.robot.commands.shooter.conveyorIn;
+import frc.robot.commands.shooter.conveyorOut;
+import frc.robot.commands.shooter.conveyorShooter;
+import frc.robot.commands.shooter.toggleShooterHood;
 
 /**
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -28,13 +71,54 @@
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  public static OI mOI = new OI();
+  public static Joystick DriverController = new Joystick(0);
+  public static Joystick OperatorController = new Joystick(1);
+  public JoystickButton buttonOneDrive = new JoystickButton(DriverController, 1);  
+  public JoystickButton buttonTwoDrive = new JoystickButton(DriverController, 2);  
+  public JoystickButton buttonThreeDrive = new JoystickButton(DriverController, 3);  
+  public JoystickButton buttonFourDrive = new JoystickButton(DriverController, 4);
+  public JoystickButton buttonFiveDrive = new JoystickButton(DriverController, 5);
+  public JoystickButton buttonSixDrive = new JoystickButton(DriverController, 6);
+  public JoystickButton buttonSevenDrive = new JoystickButton(DriverController, 7);
+  public JoystickButton buttonEightDrive = new JoystickButton(DriverController, 8);
+  public JoystickButton buttonNineDrive = new JoystickButton(DriverController, 9);
+
+  public JoystickButton buttonOneOp = new JoystickButton(OperatorController, 1);  
+  public JoystickButton buttonTwoOp = new JoystickButton(OperatorController, 2);  
+  public JoystickButton buttonThreeOp = new JoystickButton(OperatorController, 3);  
+  public JoystickButton buttonFourOp = new JoystickButton(OperatorController, 4);
+  public JoystickButton buttonFiveOp = new JoystickButton(OperatorController, 5);
+  public JoystickButton buttonSixOp = new JoystickButton(OperatorController, 6);
+  public JoystickButton buttonSevenOp = new JoystickButton(OperatorController, 7);
+  public JoystickButton buttonEightOp = new JoystickButton(OperatorController, 8);
+  public JoystickButton buttonNineOp = new JoystickButton(OperatorController, 9);
+  public JoystickButton buttonTenOp = new JoystickButton(OperatorController, 10);
+
+
   public static DriveSubsystem mDriveSubsystem = new DriveSubsystem();
   public static DriveCommand mDriveCommand = new DriveCommand();
+  public static ShooterSubsystem mShooterSubsystem = new ShooterSubsystem();
+  public static IntakeSubsystem mIntakeSubsystem = new IntakeSubsystem();
+  public static ElevatorSubystem mElevatorSubystem = new ElevatorSubystem();
+  public static HopperSubsystem mHopperSubsystem = new HopperSubsystem();
+  public static ConveyorSubsystem mConveyorSubsystem = new ConveyorSubsystem();
+ // public static Compressor mCompressor = new Compressor(0);
+
   private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
-  public static Path path = new Path();
-  public static ArrayList leftArray;
-  public static ArrayList rightArray;
+  private static Path path1 = new Path("BlueSideTrench9Feet");
+  private static Path path2 = new Path("BlueSideTrench5Feet");
+  private static ArrayList<Double> leftArray1;
+  private static ArrayList<Double> rightArray1;
+  private static ArrayList<Double> leftArray2;
+  private static ArrayList<Double> rightArray2;
+  public static NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+  public static NetworkTableEntry tx = table.getEntry("tx");
+  public static NetworkTableEntry ty = table.getEntry("ty");
+  public static NetworkTableEntry ta = table.getEntry("ta");
+  public static NetworkTableEntry light = table.getEntry("ledMode");
+  public static double AutoChooser;
+
+
 
   /**
    * The container for the robot.  Contains subsystems, OI devices, and commands.
@@ -42,14 +126,26 @@ public class RobotContainer {
   public RobotContainer() {
     // Configure the button bindings
     try{
-      rightArray = path.returnRightList();
-      leftArray = path.returnLeftList();
+      rightArray1 = path1.returnRightList();
+      leftArray1 = path1.returnLeftList();
+      rightArray2 = path2.returnRightList();
+      leftArray2 = path2.returnLeftList();
     }
       catch(IOException e){
         LOGGER.warning("real really test");
       }
      configureButtonBindings();
-   }
+     mDriveSubsystem.setDefaultCommand(mDriveCommand);
+     mElevatorSubystem.setDefaultCommand(new moveLift());
+     SmartDashboard.putNumber("Auto Selection", 0);
+     SmartDashboard.putString("Auto Op 0", "default foward drive");
+     SmartDashboard.putString("Auto Op 1", "5 ball trench");
+     SmartDashboard.putString("Auto Op 2", "3 ball midfield");
+     SmartDashboard.putString("Auto Op 3", "3 ball upclose");
+
+     //mCompressor.start();
+
+     }
 
   /**
    * Use this method to define your button->command mappings.  Buttons can be created by
@@ -58,8 +154,25 @@ public RobotContainer() {
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    mOI.buttonOne.whenPressed(new RunPath(leftArray, rightArray));
-    mOI.buttonTwo.whenPressed(new toggleOff());
+    //driver configuration
+    buttonFiveDrive.whileHeld(new ParallelCommandGroup(new ShootCommand(), new hopperVortex(), new conveyorShooter()));
+    buttonSixDrive.whileHeld(new Aiming());
+    //buttonThreeDrive.whenPressed(new toggleShooterHood());
+    
+
+    //operator configuration
+    buttonThreeOp.whenPressed(new toggleIntake());
+    buttonEightOp.whileHeld(new ParallelCommandGroup(new runIntakeIn(), new hopperOut()));
+    buttonSixOp.whileHeld(
+      //new ParallelCommandGroup(
+        new runIntakeOut()
+        //, new hopperIn())
+        );
+    buttonSevenOp.whileHeld(new ParallelCommandGroup(new conveyorOut(), new hopperOut()));
+    buttonFiveOp.whileHeld(new ParallelCommandGroup(new hopperIn(), new conveyorIn()));
+    buttonTenOp.whenPressed(new ArmElevator());
+    buttonNineOp.whenPressed(new brakeElevator());
+
   }
 
 
@@ -68,8 +181,27 @@ private void configureButtonBindings() {
    *
    * @return the command to run in autonomous
    */
-  //public Command getAutonomousCommand() {
+  public Command getAutonomousCommand() {
     // An ExampleCommand will run in autonomous
-  //  return m_autoCommand;
-  //}
-}
+    if(AutoChooser == 1){
+      return new SequentialCommandGroup(new toggleIntake(),new RunPathBack(leftArray1, rightArray1),new autoStoreValue(), new TurnAimShoot(),
+    new ParallelCommandGroup(new autoShoot(.64,3600,3), new autoHopperVortex(3), new autoConveyor(3)),new autoDeCorrect(),
+    new autoResetEncoders(),new Turn(-1), new setIntakeSpeed(),new RunPath(leftArray2, rightArray2),new autoResetEncoders(), new Turn(1), new TurnAimShoot()
+    ,new ParallelCommandGroup(new autoShoot(.71,4050,4), new autoHopperVortex(4), new autoConveyor(4)));
+    }
+    else if(AutoChooser == 2){
+      return new SequentialCommandGroup(new toggleIntake(),new RunPathBack(leftArray1, rightArray1),new autoStoreValue(), new TurnAimShoot(),
+      new ParallelCommandGroup(new autoShoot(.66,3800,3), new autoHopperVortex(3), new autoConveyor(3)),new autoDeCorrect(),
+      new autoResetEncoders(),new Turn(-1));
+
+    }
+    else if(AutoChooser == 3){
+      return new SequentialCommandGroup(new RunPath(leftArray2, rightArray2), 
+      new ParallelCommandGroup(new autoCloseShoot(.45, 2500, 3), new autoConveyor(3)));
+    }
+    else{
+      return new RunPath(leftArray2, rightArray2);
+    }
+   
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/RunPath.java b/src/main/java/frc/robot/commands/auto/RunPath.java
similarity index 60%
rename from src/main/java/frc/robot/commands/RunPath.java
rename to src/main/java/frc/robot/commands/auto/RunPath.java
index 44520a2..99926e7 100644
--- a/src/main/java/frc/robot/commands/RunPath.java
+++ b/src/main/java/frc/robot/commands/auto/RunPath.java
@@ -5,19 +5,16 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc.robot.commands;
+package frc.robot.commands.auto;
 
 import frc.robot.Robot;
 import frc.robot.RobotContainer;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.ExampleSubsystem;
-import jaci.pathfinder.PathfinderFRC;
 
+import java.util.Iterator;
 import java.util.List;
 import java.util.logging.Logger;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 
@@ -28,17 +25,15 @@ public class RunPath extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
   private List rightPath;
   private List leftPath;
-  private double timeToRun;
-  private Timer timer;
   int i = 0;
+  private Iterator<Double> m_leftIterator;
+  private Iterator<Double> m_rightIterator;
   private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
 
 
   public RunPath(List leftPath, List rightPath) {
     this.rightPath = rightPath;
     this.leftPath = leftPath;
-    timeToRun = .020 * (leftPath.size()-1);
-    timer = new Timer();
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(RobotContainer.mDriveSubsystem);
   }
@@ -46,36 +41,37 @@ public RunPath(List leftPath, List rightPath) {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    m_leftIterator = leftPath.iterator();
+    m_rightIterator = rightPath.iterator();
     i = 0;
-    timer.reset();
-    timer.start();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-    
-    RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(-1*RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(leftPath.get(i).toString())));
-    RobotContainer.mDriveSubsystem.setRightPidVelocitySetpoint(RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(rightPath.get(i).toString())));
-    SmartDashboard.putNumber("Left Commanded Velocity", RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(leftPath.get(i).toString())));    
-    SmartDashboard.putNumber("Right Commanded Velocity", RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(rightPath.get(i).toString())));
-
-    i++;
+  public void execute(){
+    if(i< leftPath.size()){
+      RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(-1*RobotContainer.mDriveSubsystem.fpsToRPM(m_leftIterator.next()));
+      RobotContainer.mDriveSubsystem.setRightPidVelocitySetpoint(RobotContainer.mDriveSubsystem.fpsToRPM(m_rightIterator.next()));
+      SmartDashboard.putNumber("Left Commanded Velocity", -1*RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(leftPath.get(i).toString())));    
+      SmartDashboard.putNumber("Right Commanded Velocity", RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(rightPath.get(i).toString())));
+      i++;
+    }
+  
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_rightIterator.remove();
+    m_leftIterator.remove();
+    RobotContainer.mDriveSubsystem.setRightSetpoint(0);
     RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(0);
-    RobotContainer.mDriveSubsystem.setRightPidVelocitySetpoint(0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if(timer.get() >= timeToRun){
-        LOGGER.warning("" + i);
-      }
-      return timer.get() >= timeToRun;
+    return i >= leftPath.size();  
+    //return timer.get() >= timeToRun;
   }
 }
diff --git a/src/main/java/frc/robot/commands/auto/RunPathBack.java b/src/main/java/frc/robot/commands/auto/RunPathBack.java
new file mode 100644
index 0000000..d91173a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/RunPathBack.java
@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.Robot;
+import frc.robot.RobotContainer;
+
+import java.util.Iterator;
+import java.util.List;
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class RunPathBack extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private List rightPath;
+  private List leftPath;
+  int i = 0;
+  private Iterator<Double> m_leftIterator;
+  private Iterator<Double> m_rightIterator;
+  private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
+
+
+  public RunPathBack(List leftPath, List rightPath) {
+    this.rightPath = rightPath;
+    this.leftPath = leftPath;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mDriveSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_leftIterator = leftPath.iterator();
+    m_rightIterator = rightPath.iterator();
+    i = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute(){
+    if(i< leftPath.size()){
+      RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(RobotContainer.mDriveSubsystem.fpsToRPM(m_leftIterator.next()));
+      RobotContainer.mDriveSubsystem.setRightPidVelocitySetpoint(-1*RobotContainer.mDriveSubsystem.fpsToRPM(m_rightIterator.next()));
+      SmartDashboard.putNumber("Left Commanded Velocity", -1*RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(leftPath.get(i).toString())));    
+      SmartDashboard.putNumber("Right Commanded Velocity", RobotContainer.mDriveSubsystem.fpsToRPM(Double.valueOf(rightPath.get(i).toString())));
+      i++;
+    }
+  
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_rightIterator.remove();
+    m_leftIterator.remove();
+    RobotContainer.mDriveSubsystem.setRightSetpoint(0);
+    RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return i >= leftPath.size();  
+    //return timer.get() >= timeToRun;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/Turn.java b/src/main/java/frc/robot/commands/auto/Turn.java
new file mode 100644
index 0000000..5bda965
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/Turn.java
@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ExampleSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class Turn extends CommandBase {
+    @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param subsystem The subsystem used by this command.
+     */
+    int i =0;
+    private static final Logger LOGGER = Logger.getLogger(Turn.class.getName());
+
+    public Turn(int i) {
+        this.i = i;
+        addRequirements(RobotContainer.mDriveSubsystem);
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        RobotContainer.mDriveSubsystem.setTurnPID();
+        DriveSubsystem.l_encoder.setPosition(0);
+        DriveSubsystem.r_encoder.setPosition(0);
+        //LOGGER.warning("TURN HAPPENING");
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mDriveSubsystem.setLeftPidPositionSetpoint(i*25.5);
+      RobotContainer.mDriveSubsystem.setRightPidPositionSetpoint(i*25.5);
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.mDriveSubsystem.setNormalPID();
+    //LOGGER.warning("TURN DONE");
+
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if(i>0)
+    return RobotContainer.mDriveSubsystem.l_encoder.getPosition() >= 25.4;
+    else
+    return RobotContainer.mDriveSubsystem.l_encoder.getPosition() <= -25.4;
+
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/TurnAimShoot.java b/src/main/java/frc/robot/commands/auto/TurnAimShoot.java
new file mode 100644
index 0000000..376cf8a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/TurnAimShoot.java
@@ -0,0 +1,93 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.commands.drive.DriveCommand;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.*;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class TurnAimShoot extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private static final Logger LOGGER = Logger.getLogger(DriveCommand.class.getName());
+  double tx = SmartDashboard.getNumber("LimelightX", 0);
+  double ty = SmartDashboard.getNumber("LimelightY", 0);
+  double ta = SmartDashboard.getNumber("LimelightArea", 0);
+  double d = 0;
+  
+  double minSteerAdjust = .25;
+  double steeringAdjust = 0.0;
+  double headingCommand = 0;
+  double p = .013;
+  double targetValue = -3;
+  boolean seen = false;
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public TurnAimShoot() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mDriveSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_ON);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  // tx is the degrees the limelight detects we are off by the target 
+  // adjust p until it works
+  @Override
+  public void execute() {
+    tx = SmartDashboard.getNumber("LimelightX", 0);
+    ty = SmartDashboard.getNumber("LimelightY", 0);
+    ta = SmartDashboard.getNumber("LimelightArea", 0);
+    if(tx>(targetValue +1)){
+      steeringAdjust = p * (tx-targetValue) +minSteerAdjust;
+   }
+   else if(tx<(targetValue -1)){
+      steeringAdjust = p * (tx-targetValue) -minSteerAdjust;
+   }
+
+   if (tx <= (targetValue +1) && tx >= (targetValue -1) && ShooterSubsystem.hoodedShooter.get() == Value.kForward && ta != 0 ) {
+    RobotContainer.mShooterSubsystem.toggleHood();
+    seen = true;
+  }
+  if (ta != 0)
+  RobotContainer.mDriveSubsystem.arcadeDrive(0, steeringAdjust);
+      steeringAdjust = 0;
+      
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+    seen = false;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (tx < targetValue +1 && tx > targetValue -1 && seen == true);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoCloseShoot.java b/src/main/java/frc/robot/commands/auto/autoCloseShoot.java
new file mode 100644
index 0000000..a880505
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoCloseShoot.java
@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoCloseShoot extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  int i;
+  int timeRun;
+  double speed;
+  double targetSpeed;
+  private static final Logger LOGGER = Logger.getLogger(autoShoot.class.getName());
+
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoCloseShoot(double speed, double targetSpeed,int timeRun) {
+    this.speed=speed;
+    this.timeRun = timeRun;
+    this.targetSpeed = targetSpeed;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mShooterSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    i=0;
+   
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    i++;
+    RobotContainer.mShooterSubsystem.setVelocitySetpoint(speed*Constants.maxRPMShooter);
+    RobotContainer.mShooterSubsystem.armShooter(targetSpeed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+
+      RobotContainer.mShooterSubsystem.setSpeed(0);
+        ShooterSubsystem.shooterReady = false;
+       // LOGGER.warning("AUTO SHOOT DONE");
+    
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return ((i*.02) >=timeRun);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoConveyor.java b/src/main/java/frc/robot/commands/auto/autoConveyor.java
new file mode 100644
index 0000000..6135501
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoConveyor.java
@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoConveyor extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  int i;
+  double timeRun;
+  private static final Logger LOGGER = Logger.getLogger(autoConveyor.class.getName());
+
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoConveyor(double timeRun) {
+      this.timeRun = timeRun;
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      i=0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      i++;
+    if(ShooterSubsystem.shooterReady == true)
+      RobotContainer.mConveyorSubsystem.runIntake(-.4);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mConveyorSubsystem.stopIntake();
+      //LOGGER.warning("CONVEYOR DONE");
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return ((i*.02) >=timeRun);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoDeCorrect.java b/src/main/java/frc/robot/commands/auto/autoDeCorrect.java
new file mode 100644
index 0000000..4e1521a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoDeCorrect.java
@@ -0,0 +1,89 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoDeCorrect extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  double tx = SmartDashboard.getNumber("LimelightX", 0);
+  double ty = SmartDashboard.getNumber("LimelightY", 0);
+  double ta = SmartDashboard.getNumber("LimelightArea", 0);
+  double d = 0;
+  
+  double minSteerAdjust = .25;
+  double steeringAdjust = 0.0;
+  double headingCommand = 0;
+  double p = .008;
+  double targetValue = 0;
+  private static final Logger LOGGER = Logger.getLogger(autoDeCorrect.class.getName());
+
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoDeCorrect() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_ON);
+    targetValue = SmartDashboard.getNumber("Data Offset", 0);
+    tx = SmartDashboard.getNumber("LimelightX", 0);
+    ty = SmartDashboard.getNumber("LimelightY", 0);
+    ta = SmartDashboard.getNumber("LimelightArea", 0);
+    d = 73.5/Math.tan(Math.toRadians(ty+63));
+    SmartDashboard.putNumber("Distance", d);
+    if(ta != 0){
+        if(tx>(targetValue +1)){
+           steeringAdjust = p * (tx-targetValue) +minSteerAdjust;
+        }
+        else if(tx<(targetValue -1)){
+           steeringAdjust = p * (tx-targetValue) -minSteerAdjust;
+        }
+    RobotContainer.mDriveSubsystem.arcadeDrive(0,steeringAdjust);
+}
+
+    steeringAdjust = 0;
+    
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if(((tx - targetValue) < 1 && (tx - targetValue) > -1) || ta == 0 ){
+        return true;
+    }
+    return false;
+}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/auto/autoDelay.java b/src/main/java/frc/robot/commands/auto/autoDelay.java
new file mode 100644
index 0000000..c78aad2
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoDelay.java
@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoDelay extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  int i;
+  double timeRun;
+  private static final Logger LOGGER = Logger.getLogger(autoHopperVortex.class.getName());
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoDelay( double timeRun) {
+    this.timeRun = timeRun;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      i=0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      i++;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      //LOGGER.warning("AUTO HOPPER DONE");
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return ((i*.02) >=timeRun);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoHopperVortex.java b/src/main/java/frc/robot/commands/auto/autoHopperVortex.java
new file mode 100644
index 0000000..a59846d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoHopperVortex.java
@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoHopperVortex extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  int i;
+  double timeRun;
+  private static final Logger LOGGER = Logger.getLogger(autoHopperVortex.class.getName());
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoHopperVortex( double timeRun) {
+    this.timeRun = timeRun;
+
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      i=0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      i++;
+      RobotContainer.mHopperSubsystem.runIntake(0,.25);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mHopperSubsystem.stopIntake();
+      //LOGGER.warning("AUTO HOPPER DONE");
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return ((i*.02) >=timeRun);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoResetEncoders.java b/src/main/java/frc/robot/commands/auto/autoResetEncoders.java
new file mode 100644
index 0000000..43c2b2b
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoResetEncoders.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.DriveSubsystem;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoResetEncoders extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoResetEncoders() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    DriveSubsystem.l_encoder.setPosition(0);
+    DriveSubsystem.r_encoder.setPosition(0);  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoShoot.java b/src/main/java/frc/robot/commands/auto/autoShoot.java
new file mode 100644
index 0000000..e22a951
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoShoot.java
@@ -0,0 +1,101 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoShoot extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  double tx = SmartDashboard.getNumber("LimelightX", 0);
+  double ty = SmartDashboard.getNumber("LimelightY", 0);
+  double ta = SmartDashboard.getNumber("LimelightArea", 0);
+  double targetValue = -3;
+  int i;
+  int timeRun;
+  double minSteerAdjust = .25;
+  double steeringAdjust = 0.0;
+  double headingCommand = 0;
+  double p = .013;
+  double speed;
+  double targetSpeed;
+  private static final Logger LOGGER = Logger.getLogger(autoShoot.class.getName());
+
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoShoot(double speed, double targetSpeed,int timeRun) {
+    this.speed=speed;
+    this.timeRun = timeRun;
+    this.targetSpeed = targetSpeed;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mShooterSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    i=0;
+    RobotContainer.light.setValue(Constants.LL_LIGHT_ON);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    tx = SmartDashboard.getNumber("LimelightX", 0);
+    ty = SmartDashboard.getNumber("LimelightY", 0);
+    ta = SmartDashboard.getNumber("LimelightArea", 0);
+    if(tx>(targetValue +1)){
+        steeringAdjust = p * (tx-targetValue) +minSteerAdjust;
+     }
+     else if(tx<(targetValue -1)){
+        steeringAdjust = p * (tx-targetValue) -minSteerAdjust;
+     }
+  
+  
+      if (ta != 0)
+        RobotContainer.mDriveSubsystem.arcadeDrive(0, steeringAdjust);
+      steeringAdjust = 0;
+      i++;
+    RobotContainer.mShooterSubsystem.setVelocitySetpoint(speed*Constants.maxRPMShooter);
+    RobotContainer.mShooterSubsystem.armShooter(targetSpeed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+
+      RobotContainer.mShooterSubsystem.setSpeed(0);
+        ShooterSubsystem.shooterReady = false;
+        RobotContainer.mShooterSubsystem.toggleHood();  
+       // LOGGER.warning("AUTO SHOOT DONE");
+    
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return ((i*.02) >=timeRun);
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/autoStoreValue.java b/src/main/java/frc/robot/commands/auto/autoStoreValue.java
new file mode 100644
index 0000000..604eddc
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/autoStoreValue.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class autoStoreValue extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private static final Logger LOGGER = Logger.getLogger(autoStoreValue.class.getName());
+
+  private double targetValue = 0;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public autoStoreValue() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_ON);
+    SmartDashboard.putNumber("Data Offset",  SmartDashboard.getNumber("LimelightX", 0));
+    targetValue = SmartDashboard.getNumber("Data Offset", 0);
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+    //LOGGER.warning("STORE VALUE DONE");
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (targetValue != 0) ;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/RunVelocity.java b/src/main/java/frc/robot/commands/auto/setConveyorSpeed.java
similarity index 67%
rename from src/main/java/frc/robot/commands/RunVelocity.java
rename to src/main/java/frc/robot/commands/auto/setConveyorSpeed.java
index 1f74940..3149d99 100644
--- a/src/main/java/frc/robot/commands/RunVelocity.java
+++ b/src/main/java/frc/robot/commands/auto/setConveyorSpeed.java
@@ -5,51 +5,47 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc.robot.commands;
+package frc.robot.commands.auto;
 
 import frc.robot.RobotContainer;
-import edu.wpi.first.wpilibj.Timer;
+import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
  */
-public class RunVelocity extends CommandBase {
+public class setConveyorSpeed extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private double timeToRun;
-  private double velocity;
-  private Timer timer;
 
-  public RunVelocity(double velRPM, int timeToRunMs) {
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public setConveyorSpeed() {
     // Use addRequirements() here to declare subsystem dependencies.
-    timeToRun = timeToRunMs;
-    velocity = velRPM;
-    timer = new Timer();
-    addRequirements(RobotContainer.mDriveSubsystem);
+    addRequirements(RobotContainer.mIntakeSubsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    timer.reset();
-    timer.start();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(velocity);
+      RobotContainer.mConveyorSubsystem.runIntake(-.4);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    RobotContainer.mDriveSubsystem.setLeftPidVelocitySetpoint(0);
   }
 
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return timer.get() >= timeToRun;
+    return true;
   }
 }
diff --git a/src/main/java/frc/robot/commands/auto/setIntakeSpeed.java b/src/main/java/frc/robot/commands/auto/setIntakeSpeed.java
new file mode 100644
index 0000000..6fe3997
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/setIntakeSpeed.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class setIntakeSpeed extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public setIntakeSpeed() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mIntakeSubsystem.runIntake(-.4);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/stopConveyor.java b/src/main/java/frc/robot/commands/auto/stopConveyor.java
new file mode 100644
index 0000000..1e900c2
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/stopConveyor.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class stopConveyor extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public stopConveyor() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mConveyorSubsystem.runIntake(0);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/auto/stopIntake.java b/src/main/java/frc/robot/commands/auto/stopIntake.java
new file mode 100644
index 0000000..eafeb14
--- /dev/null
+++ b/src/main/java/frc/robot/commands/auto/stopIntake.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.auto;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class stopIntake extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public stopIntake() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mIntakeSubsystem.runIntake(0);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/drive/Aiming.java b/src/main/java/frc/robot/commands/drive/Aiming.java
new file mode 100644
index 0000000..65640df
--- /dev/null
+++ b/src/main/java/frc/robot/commands/drive/Aiming.java
@@ -0,0 +1,100 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.drive;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.commands.shooter.toggleShooterHood;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.*;
+
+import java.util.logging.Logger;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class Aiming extends CommandBase {
+  @SuppressWarnings({ "PMD.UnusedPrivateField", "PMD.SingularField" })
+  private static final Logger LOGGER = Logger.getLogger(DriveCommand.class.getName());
+  double tx = SmartDashboard.getNumber("LimelightX", 0);
+  double ty = SmartDashboard.getNumber("LimelightY", 0);
+  double ta = SmartDashboard.getNumber("LimelightArea", 0);
+  double d = 0;
+
+  double minSteerAdjust = .25;
+  double steeringAdjust = 0.0;
+  double headingCommand = 0;
+  double p = .013;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public Aiming() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mDriveSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_ON);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  // tx is the degrees the limelight detects we are off by the target
+  // adjust p until it works
+  @Override
+  public void execute() {
+
+    tx = SmartDashboard.getNumber("LimelightX", 0);
+    ty = SmartDashboard.getNumber("LimelightY", 0);
+    ta = SmartDashboard.getNumber("LimelightArea", 0);
+    d = 64/Math.tan(Math.toRadians(ty+16));
+    SmartDashboard.putNumber("Distance", d);
+    double targetValue = -3;
+    if(tx>(targetValue +1)){
+      steeringAdjust = p * (tx-targetValue) +minSteerAdjust;
+   }
+   else if(tx<(targetValue -1)){
+      steeringAdjust = p * (tx-targetValue) -minSteerAdjust;
+   }
+
+    if (tx <= (targetValue +1) && tx >= (targetValue -1) && ShooterSubsystem.hoodedShooter.get() == Value.kForward && ta != 0 ) {
+      RobotContainer.mShooterSubsystem.toggleHood();
+    }
+
+    if (tx != 0)
+      RobotContainer.mDriveSubsystem.arcadeDrive(RobotContainer.DriverController.getRawAxis(1), steeringAdjust);
+    else
+      RobotContainer.mDriveSubsystem.arcadeDrive(RobotContainer.DriverController.getRawAxis(1),
+          RobotContainer.DriverController.getRawAxis(2));
+    steeringAdjust = 0;
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    RobotContainer.light.setValue(Constants.LL_LIGHT_OFF);
+    if (ShooterSubsystem.hoodedShooter.get() == Value.kReverse){
+      RobotContainer.mShooterSubsystem.toggleHood();
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/drive/DriveCommand.java
similarity index 83%
rename from src/main/java/frc/robot/commands/DriveCommand.java
rename to src/main/java/frc/robot/commands/drive/DriveCommand.java
index ba51f9e..7fa30c0 100644
--- a/src/main/java/frc/robot/commands/DriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive/DriveCommand.java
@@ -5,13 +5,10 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc.robot.commands;
+package frc.robot.commands.drive;
 
 import frc.robot.Constants;
-import frc.robot.OI;
 import frc.robot.RobotContainer;
-import frc.robot.subsystems.DriveSubsystem;
-import frc.robot.subsystems.ExampleSubsystem;
 
 import java.util.logging.Logger;
 
@@ -24,7 +21,6 @@
 public class DriveCommand extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
   private static final Logger LOGGER = Logger.getLogger(DriveCommand.class.getName());
-  private Joystick joystick = new Joystick(OI.joystick);
 
   /**
    * Creates a new ExampleCommand.
@@ -45,8 +41,7 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(Constants.onOrOff == false)
-      RobotContainer.mDriveSubsystem.arcadeDrive(joystick.getRawAxis(1), joystick.getRawAxis(2));
+      RobotContainer.mDriveSubsystem.arcadeDrive(RobotContainer.DriverController.getRawAxis(1), RobotContainer.DriverController.getRawAxis(2)*.8);
   }
 
   // Called once the command ends or is interrupted.
diff --git a/src/main/java/frc/robot/commands/elevator/ArmElevator.java b/src/main/java/frc/robot/commands/elevator/ArmElevator.java
new file mode 100644
index 0000000..d115755
--- /dev/null
+++ b/src/main/java/frc/robot/commands/elevator/ArmElevator.java
@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.elevator;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class ArmElevator extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private double targetValue = 0;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ArmElevator() {
+      addRequirements(RobotContainer.mElevatorSubystem);
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+      RobotContainer.mElevatorSubystem.armElevator();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+   
+
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+     return true;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/elevator/brakeElevator.java b/src/main/java/frc/robot/commands/elevator/brakeElevator.java
new file mode 100644
index 0000000..dff6c11
--- /dev/null
+++ b/src/main/java/frc/robot/commands/elevator/brakeElevator.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.elevator;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class brakeElevator extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public brakeElevator() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mElevatorSubystem.toggleBrake();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/elevator/moveLift.java b/src/main/java/frc/robot/commands/elevator/moveLift.java
new file mode 100644
index 0000000..e8c28b6
--- /dev/null
+++ b/src/main/java/frc/robot/commands/elevator/moveLift.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.elevator;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class moveLift extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  double axis;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public moveLift() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mElevatorSubystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mElevatorSubystem.moveLift(RobotContainer.OperatorController.getRawAxis(1));
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+      return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/hopper/hopperIn.java
similarity index 84%
rename from src/main/java/frc/robot/commands/ExampleCommand.java
rename to src/main/java/frc/robot/commands/hopper/hopperIn.java
index 42c175f..09ba562 100644
--- a/src/main/java/frc/robot/commands/ExampleCommand.java
+++ b/src/main/java/frc/robot/commands/hopper/hopperIn.java
@@ -5,27 +5,25 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc.robot.commands;
+package frc.robot.commands.hopper;
 
+import frc.robot.RobotContainer;
 import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
  */
-public class ExampleCommand extends CommandBase {
+public class hopperIn extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private final ExampleSubsystem m_subsystem;
 
   /**
    * Creates a new ExampleCommand.
    *
    * @param subsystem The subsystem used by this command.
    */
-  public ExampleCommand(ExampleSubsystem subsystem) {
-    m_subsystem = subsystem;
+  public hopperIn() {
     // Use addRequirements() here to declare subsystem dependencies.
-    addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
@@ -36,11 +34,13 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
+      RobotContainer.mHopperSubsystem.runIntake(.05,-.05);
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+      RobotContainer.mHopperSubsystem.stopIntake();
   }
 
   // Returns true when the command should end.
diff --git a/src/main/java/frc/robot/commands/hopper/hopperOut.java b/src/main/java/frc/robot/commands/hopper/hopperOut.java
new file mode 100644
index 0000000..8054b3d
--- /dev/null
+++ b/src/main/java/frc/robot/commands/hopper/hopperOut.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.hopper;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class hopperOut extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public hopperOut() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mHopperSubsystem.runIntake(.25,.25);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mHopperSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/hopper/hopperVortex.java b/src/main/java/frc/robot/commands/hopper/hopperVortex.java
new file mode 100644
index 0000000..0838b8a
--- /dev/null
+++ b/src/main/java/frc/robot/commands/hopper/hopperVortex.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.hopper;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class hopperVortex extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public hopperVortex() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mHopperSubsystem.runIntake(.25,.25);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mHopperSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/intake/runIntakeIn.java b/src/main/java/frc/robot/commands/intake/runIntakeIn.java
new file mode 100644
index 0000000..6568fa5
--- /dev/null
+++ b/src/main/java/frc/robot/commands/intake/runIntakeIn.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.intake;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class runIntakeIn extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public runIntakeIn() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mIntakeSubsystem.runIntake(.35);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mIntakeSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/intake/runIntakeOut.java b/src/main/java/frc/robot/commands/intake/runIntakeOut.java
new file mode 100644
index 0000000..a82040c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/intake/runIntakeOut.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.intake;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class runIntakeOut extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public runIntakeOut() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mIntakeSubsystem.runIntake(-.35);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mIntakeSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/toggleOff.java b/src/main/java/frc/robot/commands/intake/toggleIntake.java
similarity index 73%
rename from src/main/java/frc/robot/commands/toggleOff.java
rename to src/main/java/frc/robot/commands/intake/toggleIntake.java
index d36e66d..ee705cd 100644
--- a/src/main/java/frc/robot/commands/toggleOff.java
+++ b/src/main/java/frc/robot/commands/intake/toggleIntake.java
@@ -5,23 +5,25 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc.robot.commands;
+package frc.robot.commands.intake;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc.robot.Constants;
 import frc.robot.RobotContainer;
-
-import java.util.logging.*;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * An example command that uses an example subsystem.
  */
-public class toggleOff extends CommandBase {
+public class toggleIntake extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
-  private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getName());
 
-  public toggleOff(){
-    
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public toggleIntake() {
+    // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
@@ -32,15 +34,8 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if(Constants.onOrOff == false){
-        Constants.onOrOff = true;
-    }
-    else{
-      Constants.onOrOff = false;
-      LOGGER.warning("" + Constants.onOrOff);
-    }
-}
-  
+      RobotContainer.mIntakeSubsystem.toggleIntake();
+  }
 
   // Called once the command ends or is interrupted.
   @Override
diff --git a/src/main/java/frc/robot/commands/shooter/ShootCommand.java b/src/main/java/frc/robot/commands/shooter/ShootCommand.java
new file mode 100644
index 0000000..3e81581
--- /dev/null
+++ b/src/main/java/frc/robot/commands/shooter/ShootCommand.java
@@ -0,0 +1,69 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.shooter;
+
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class ShootCommand extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ShootCommand() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mShooterSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      if(ShooterSubsystem.hoodedShooter.get() == Value.kForward){
+        RobotContainer.mShooterSubsystem.setVelocitySetpoint(.45*Constants.maxRPMShooter);
+        RobotContainer.mShooterSubsystem.armShooter(2450);
+
+      }
+      else{
+        RobotContainer.mShooterSubsystem.setVelocitySetpoint(.65*Constants.maxRPMShooter);
+         RobotContainer.mShooterSubsystem.armShooter(3740);
+        
+
+
+      }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mShooterSubsystem.setSpeed(0);
+      RobotContainer.mShooterSubsystem.shooterReady = false;
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/shooter/conveyorIn.java b/src/main/java/frc/robot/commands/shooter/conveyorIn.java
new file mode 100644
index 0000000..3355cdd
--- /dev/null
+++ b/src/main/java/frc/robot/commands/shooter/conveyorIn.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.shooter;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class conveyorIn extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public conveyorIn() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mConveyorSubsystem.runIntake(-.4);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mConveyorSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/shooter/conveyorOut.java b/src/main/java/frc/robot/commands/shooter/conveyorOut.java
new file mode 100644
index 0000000..9fabe49
--- /dev/null
+++ b/src/main/java/frc/robot/commands/shooter/conveyorOut.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.shooter;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class conveyorOut extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public conveyorOut() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mConveyorSubsystem.runIntake(.6);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mConveyorSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/shooter/conveyorShooter.java b/src/main/java/frc/robot/commands/shooter/conveyorShooter.java
new file mode 100644
index 0000000..dae979f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/shooter/conveyorShooter.java
@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.shooter;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class conveyorShooter extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public conveyorShooter() {
+    // Use addRequirements() here to declare subsystem dependencies.
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if(RobotContainer.mShooterSubsystem.shooterReady == true)
+      RobotContainer.mConveyorSubsystem.runIntake(-.4);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+      RobotContainer.mConveyorSubsystem.stopIntake();
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/src/main/java/frc/robot/commands/shooter/toggleShooterHood.java b/src/main/java/frc/robot/commands/shooter/toggleShooterHood.java
new file mode 100644
index 0000000..7198021
--- /dev/null
+++ b/src/main/java/frc/robot/commands/shooter/toggleShooterHood.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands.shooter;
+
+import frc.robot.RobotContainer;
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/**
+ * An example command that uses an example subsystem.
+ */
+public class toggleShooterHood extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public toggleShooterHood() {
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(RobotContainer.mIntakeSubsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      RobotContainer.mShooterSubsystem.toggleHood();
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
new file mode 100644
index 0000000..316ac61
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ConveyorSubsystem extends SubsystemBase {
+    
+  /**
+   * Creates a new ExampleSubsystem.
+   */
+  private static CANSparkMax conveyor= new CANSparkMax(Constants.Belt, MotorType.kBrushless);
+
+    public ConveyorSubsystem(){
+
+        conveyor.restoreFactoryDefaults();
+        conveyor.enableVoltageCompensation(12);
+        conveyor.setIdleMode(IdleMode.kBrake);
+        conveyor.setSmartCurrentLimit(30);
+
+  }
+  public void runIntake(double speed){
+        conveyor.set(speed);
+    }
+
+    public void stopIntake(){
+        conveyor.set(0);
+    }
+  
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
index e840d4c..4ccf30c 100644
--- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java
@@ -11,49 +11,40 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.commands.drive.DriveCommand;
 
 import com.revrobotics.CANEncoder;
 import com.revrobotics.CANPIDController;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.ControlType;
 import com.revrobotics.CANSparkMax.IdleMode;
-import com.revrobotics.CANError;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import java.util.logging.*;
 
 
 public class DriveSubsystem extends SubsystemBase {
-    private CANSparkMax mLeft1;
-    private CANSparkMax mLeft2;
-    private CANSparkMax mRight1;
-    private CANSparkMax mRight2;
+    private static CANSparkMax mLeft1 = new CANSparkMax(Constants.DriveLeft1, MotorType.kBrushless);
+    private static CANSparkMax mLeft2 = new CANSparkMax(Constants.DriveLeft2, MotorType.kBrushless);
+    private static CANSparkMax mLeft3 = new CANSparkMax(Constants.DriveLeft3, MotorType.kBrushless);
+    private static CANSparkMax mRight1 = new CANSparkMax(Constants.DriveRight1, MotorType.kBrushless);
+    private static CANSparkMax mRight2 = new CANSparkMax(Constants.DriveRight2, MotorType.kBrushless);
+    private static CANSparkMax mRight3 = new CANSparkMax(Constants.DriveRight3, MotorType.kBrushless);
     private CANPIDController l_pidController;
     private CANPIDController r_pidController;
-    private CANEncoder l_encoder;
-    private CANEncoder r_encoder;
+    public static CANEncoder l_encoder;
+    public static CANEncoder r_encoder;
     private DifferentialDrive differentialDrive;
-    public double kP, kI, kD, kIz, kFF, kMaxOutput, kMinOutput, maxRPM;
+    public double kP = Constants.kPDrive, kI = Constants.kIDrive, kD = Constants.kDDrive, kIz = Constants.kIzDrive, kFF = Constants.kFFDrive,
+     kMaxOutput = Constants.kMaxOutputDrive, kMinOutput = Constants.kMinOutputDrive;
+
+     public double kPTurn = 0, kITurn = Constants.kIDrive, kDTurn = Constants.kDDrive, kIzTurn = Constants.kIzDrive, kFFTurn = Constants.kFFDrive,
+     kMaxOutputTurn = Constants.kMaxOutputDrive, kMinOutputTurn = Constants.kMinOutputDrive;
     private static final Logger LOGGER = Logger.getLogger(DriveSubsystem.class.getName());
 
   public DriveSubsystem() {
-    mLeft1 = new CANSparkMax(1, MotorType.kBrushless);
-    mLeft2 = new CANSparkMax(2, MotorType.kBrushless);
-    mRight1 = new CANSparkMax(3, MotorType.kBrushless);
-    mRight2 = new CANSparkMax(4, MotorType.kBrushless);
-    mLeft1.restoreFactoryDefaults();
-    mLeft2.restoreFactoryDefaults();
-    mRight1.restoreFactoryDefaults();
-    mRight2.restoreFactoryDefaults();
-    mLeft1.enableVoltageCompensation(12);
-    mLeft2.enableVoltageCompensation(12);
-    mRight1.enableVoltageCompensation(12);
-    mRight2.enableVoltageCompensation(12);
-    mLeft1.setIdleMode(IdleMode.kBrake);
-    mLeft2.setIdleMode(IdleMode.kBrake);
-    mRight1.setIdleMode(IdleMode.kBrake);
-    mRight2.setIdleMode(IdleMode.kBrake);
-    mLeft2.follow(mLeft1);
-    mRight2.follow(mRight1);
+    intializeDriveSubystem(mLeft1, mLeft2, mLeft3);
+    intializeDriveSubystem(mRight1, mRight2, mRight3);
     differentialDrive = new DifferentialDrive(mLeft1, mRight1);
 
 
@@ -61,15 +52,9 @@ public DriveSubsystem() {
     r_pidController = mRight1.getPIDController();
 
     l_encoder = mLeft1.getEncoder();
-    r_encoder = mLeft1.getEncoder();
-
-    kP = 0.00010; 
-    kI = 0;
-    kD = .0000; 
-    kIz = 0; 
-    kFF = 0.000175; 
-    kMaxOutput = 1; 
-    kMinOutput = -1;
+    r_encoder = mRight1.getEncoder();
+    l_encoder.setPosition(0);
+    r_encoder.setPosition(0);
 
     l_pidController.setP(kP);
     l_pidController.setI(kI);
@@ -91,7 +76,11 @@ public DriveSubsystem() {
   }
 
   public void arcadeDrive(double velocity, double heading){
-    this.differentialDrive.arcadeDrive(velocity, heading * .70, true);
+    this.differentialDrive.arcadeDrive(velocity, heading, true);
+  }
+
+  public void tankDrive(double lVelocity, double rVelocity){
+    this.differentialDrive.tankDrive(lVelocity, rVelocity);
   }
 
   public void updatePID(){
@@ -104,12 +93,12 @@ public void updatePID(){
   double min = SmartDashboard.getNumber("Min Output", 0);
 
   //if PID coefficients on SmartDashboard have changed, write new values to controller
-  if((p != kP)) { l_pidController.setP(p); r_pidController.setP(p); kP = p; 
-  LOGGER.warning("PID CHANGED");}
-  if((i != kI)) { l_pidController.setI(i); r_pidController.setI(i); kI = i; 
-  LOGGER.warning("PID CHANGED");}
-  if((d != kD)) { l_pidController.setD(d); r_pidController.setD(d); kD = d; 
-  LOGGER.warning(l_pidController.getD() +" D CHANGED");}
+  // if((p != kP)) { l_pidController.setP(p); r_pidController.setP(p); kP = p; 
+  // LOGGER.warning("PID CHANGED");}
+  // if((i != kI)) { l_pidController.setI(i); r_pidController.setI(i); kI = i; 
+  // LOGGER.warning("PID CHANGED");}
+  // if((d != kD)) { l_pidController.setD(d); r_pidController.setD(d); kD = d; 
+  // LOGGER.warning(l_pidController.getD() +" D CHANGED");}
   // if((iz != kIz)) { m_pidController.setIZone(iz); kIz = iz; }
   // if((ff != kFF)) { m_pidController.setFF(ff); kFF = ff; }
   if((max != kMaxOutput) || (min != kMinOutput)) 
@@ -124,11 +113,35 @@ public void setLeftPidVelocitySetpoint(double setpoint)
     l_pidController.setReference(setpoint, ControlType.kVelocity);
 }
 
+public void checkPathEnd(){
+  
+}
+
 public void setRightPidVelocitySetpoint(double setpoint)
 {
     r_pidController.setReference(setpoint, ControlType.kVelocity);
 }
 
+public void setLeftPidPositionSetpoint(double setpoint)
+{
+    l_pidController.setReference(setpoint, ControlType.kPosition);
+}
+
+public void setRightPidPositionSetpoint(double setpoint)
+{
+    r_pidController.setReference(setpoint, ControlType.kPosition);
+}
+
+public void setLeftSetpoint(double setpoint)
+{
+    mLeft1.set(setpoint);
+}
+
+public void setRightSetpoint(double setpoint)
+{
+    mRight1.set(setpoint);
+}
+
 public double leftVelocity()
 {
     return l_encoder.getVelocity();
@@ -139,6 +152,17 @@ public double rightVelocity()
     return r_encoder.getVelocity();
 }
 
+
+public double getLeftEncPosition()
+{
+  return l_encoder.getPosition();
+}
+
+public double getRightEncPosition()
+{
+  return r_encoder.getPosition();
+}
+
 public double fpsToRPM(double fps){
     fps = fps * 12;
     fps = fps/Constants.kWheelCircumference;
@@ -146,9 +170,58 @@ public double fpsToRPM(double fps){
     fps = fps*Constants.kGearRatio;
     return fps;
 }
+
+public void intializeDriveSubystem(CANSparkMax master, CANSparkMax... slaves){
+  master.restoreFactoryDefaults();
+  master.enableVoltageCompensation(12);
+  master.setIdleMode(IdleMode.kBrake);
+  master.setOpenLoopRampRate(.2);
+  master.setSmartCurrentLimit(40);
+ 
+  for(CANSparkMax slave : slaves) {
+    slave.restoreFactoryDefaults();
+    slave.enableVoltageCompensation(12);
+    slave.setIdleMode(IdleMode.kBrake);
+    slave.setOpenLoopRampRate(.2);
+    slave.setSmartCurrentLimit(40);
+    slave.follow(master);
+}
+  }
+
+  public void setTurnPID(){
+    l_pidController.setP(.07);
+    l_pidController.setI(0);
+    l_pidController.setD(0);
+    l_pidController.setIZone(0);
+    l_pidController.setFF(0);
+    r_pidController.setP(.07);
+    r_pidController.setI(0);
+    r_pidController.setD(0);
+    r_pidController.setIZone(0);
+    r_pidController.setFF(0);
+  }
+
+  public void setNormalPID(){
+    l_pidController.setP(kP);
+    l_pidController.setI(kI);
+    l_pidController.setD(kD);
+    l_pidController.setIZone(kIz);
+    l_pidController.setFF(kFF);
+    r_pidController.setP(kP);
+    r_pidController.setI(kI);
+    r_pidController.setD(kD);
+    r_pidController.setIZone(kIz);
+    r_pidController.setFF(kFF);
+  }
+
   @Override
   public void periodic() {
+    SmartDashboard.putNumber("Left Velocity", -1 * RobotContainer.mDriveSubsystem.leftVelocity());    
+    SmartDashboard.putNumber("Right Velocity", -1 * RobotContainer.mDriveSubsystem.rightVelocity());
+    SmartDashboard.putNumber("lift position", RobotContainer.mElevatorSubystem.getPosition());
+    SmartDashboard.putNumber("left encoder", RobotContainer.mDriveSubsystem.getLeftEncPosition());
+    SmartDashboard.putNumber("right encoder", RobotContainer.mDriveSubsystem.getRightEncPosition());
     // This method will be called once per scheduler run
-
   }
 }
+
diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubystem.java
new file mode 100644
index 0000000..d2cdcb1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/ElevatorSubystem.java
@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.RobotContainer;
+import frc.robot.commands.elevator.brakeElevator;
+import frc.robot.commands.elevator.moveLift;
+
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import java.util.logging.*;
+
+
+public class ElevatorSubystem extends SubsystemBase {
+    private static CANSparkMax motor1 = new CANSparkMax(Constants.Elevator1,MotorType.kBrushless);
+    private static CANSparkMax motor2 = new CANSparkMax(Constants.Elevator2, MotorType.kBrushless);
+    private CANPIDController pidController;
+    private CANEncoder encoder;
+    private boolean readyUse = false;
+    private double position = 0;
+    public double kP= .01, kI=0, kD=0, kIz, kFF,
+     kMaxOutput = 1, kMinOutput = -1;
+    private static final Logger LOGGER = Logger.getLogger(DriveSubsystem.class.getName());
+    private DoubleSolenoid brakeElevator = new DoubleSolenoid(4,7);
+
+
+  public ElevatorSubystem() {
+    brakeElevator.set(DoubleSolenoid.Value.kForward);
+
+    motor1.restoreFactoryDefaults();
+    motor2.restoreFactoryDefaults();
+    motor2.follow(motor1, true);
+    
+    pidController = motor1.getPIDController();
+    motor1.setSmartCurrentLimit(40);
+    motor2.setSmartCurrentLimit(40);
+
+    encoder = motor1.getEncoder();
+    encoder.setPosition(0);
+
+    pidController.setP(kP);
+    pidController.setI(kI);
+    pidController.setD(kD);
+    pidController.setIZone(kIz);
+    pidController.setFF(kFF);
+    pidController.setOutputRange(kMinOutput, kMaxOutput);
+  }
+public void moveLift(double joystickInput){
+  if(readyUse == true){
+    if((joystickInput < 0 && position > -570) || (joystickInput > 0 && position < 0))
+      position = position+(10*joystickInput);
+    if(position >=-570 && position <= 0)
+      pidController.setReference(position, ControlType.kPosition);
+    SmartDashboard.putNumber("commanded position", position);
+  }
+    
+}
+
+public double getPosition(){
+    return encoder.getPosition();
+}
+public void armElevator(){
+  readyUse = true;
+}
+
+public boolean getReadyUse(){
+  return readyUse;
+}
+
+public void setBrake(DoubleSolenoid.Value state){  
+  brakeElevator.set(state);
+}
+
+public void toggleBrake(){
+  if(brakeElevator.get() == Value.kForward){
+    brakeElevator.set(Value.kReverse);
+  }
+
+  else{
+    brakeElevator.set(Value.kForward);
+  }
+}
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
+
diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java
new file mode 100644
index 0000000..363cd67
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java
@@ -0,0 +1,51 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class HopperSubsystem extends SubsystemBase {
+    
+  /**
+   * Creates a new ExampleSubsystem.
+   */
+  private static CANSparkMax hopper1= new CANSparkMax(Constants.Hopper2, MotorType.kBrushless);
+  private static CANSparkMax hopper2= new CANSparkMax(Constants.Hopper1, MotorType.kBrushless);
+
+    public HopperSubsystem(){
+        hopper1.restoreFactoryDefaults();
+        hopper1.enableVoltageCompensation(12);
+        hopper1.setIdleMode(IdleMode.kBrake);
+        hopper2.restoreFactoryDefaults();
+        hopper2.enableVoltageCompensation(12);
+        hopper2.setIdleMode(IdleMode.kBrake);
+        hopper1.setInverted(false);
+        hopper2.setInverted(true);
+        hopper1.setSmartCurrentLimit(30);
+        hopper2.setSmartCurrentLimit(30);
+
+  }
+  public void runIntake(double speed1, double speed2){
+        hopper1.set(speed1);
+        hopper2.set(speed2);
+    }
+
+    public void stopIntake(){
+        hopper1.set(0);
+        hopper2.set(0);
+    }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
new file mode 100644
index 0000000..9a2d150
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java
@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class IntakeSubsystem extends SubsystemBase {
+    
+  /**
+   * Creates a new ExampleSubsystem.
+   */
+  private static CANSparkMax intakeMotor= new CANSparkMax(Constants.Intake,MotorType.kBrushless);;
+  private DoubleSolenoid intakeSolenoid2 = new DoubleSolenoid(0, 2);
+  public enum IntakeDirection {IN, OUT}
+
+    public IntakeSubsystem() {
+        intakeSolenoid2.set(DoubleSolenoid.Value.kForward);
+
+        intakeMotor.restoreFactoryDefaults();
+        intakeMotor.enableVoltageCompensation(12);
+        intakeMotor.setIdleMode(IdleMode.kBrake);
+        intakeMotor.setSmartCurrentLimit(50);
+        
+
+  }
+  public void runIntake(double speed){
+        intakeMotor.set(speed);
+    }
+
+  public void setIntake(DoubleSolenoid.Value state){  
+     intakeSolenoid2.set(state);
+
+  }
+  public void stopIntake(){
+      intakeMotor.set(0);
+  }
+
+  public void toggleIntake(){
+    if(intakeSolenoid2.get() == Value.kForward){
+      intakeSolenoid2.set(Value.kReverse);
+    }
+
+    else{
+      intakeSolenoid2.set(Value.kForward);
+    }
+  }
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
new file mode 100644
index 0000000..3aa0cf7
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java
@@ -0,0 +1,121 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.subsystems;
+
+import java.util.logging.Logger;
+
+import com.revrobotics.CANEncoder;
+import com.revrobotics.CANPIDController;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.ControlType;
+import com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class ShooterSubsystem extends SubsystemBase {
+    private CANPIDController shooterPidController;
+    private static CANSparkMax shooterMotor;   
+    private CANEncoder shooterEncoder;
+    public static boolean shooterReady = false;
+    public static DoubleSolenoid hoodedShooter = new DoubleSolenoid(1,3);
+    private double kP=.0003,kI=0.0,kD=0,kIz=0,kFF=0.000195,kMinOutput=0,kMaxOutput=1;
+    private static final Logger LOGGER = Logger.getLogger(ShooterSubsystem.class.getName());
+
+  /**
+   * Creates a new ExampleSubsystem.
+   */
+  public ShooterSubsystem() {
+    shooterMotor = new CANSparkMax(Constants.Shooter, MotorType.kBrushless);
+    shooterMotor.restoreFactoryDefaults();
+    shooterMotor.setIdleMode(IdleMode.kCoast);
+    shooterPidController = shooterMotor.getPIDController();
+    shooterEncoder = shooterMotor.getEncoder();
+    shooterPidController.setP(kP);
+    shooterPidController.setI(kI);
+    shooterPidController.setD(kD);
+    shooterPidController.setIZone(kIz);
+    shooterPidController.setFF(kFF);
+    shooterPidController.setOutputRange(kMinOutput, kMaxOutput);
+    shooterMotor.setSmartCurrentLimit(30);
+    shooterMotor.setInverted(true);
+    hoodedShooter.set(DoubleSolenoid.Value.kForward);
+    SmartDashboard.putNumber("Shooter P", kP);
+    SmartDashboard.putNumber("Shooter I", kI);
+    SmartDashboard.putNumber("Shooter D", kD);
+    SmartDashboard.putNumber("Shooter FF", kFF);
+  
+  }
+
+  public void setVelocitySetpoint(double setpoint){
+    shooterPidController.setReference(setpoint, ControlType.kVelocity);
+    SmartDashboard.putNumber("ShooterSetpoint", setpoint);
+    // if(hoodedShooter.get() == Value.kForward && (getVelocity() >= 2350 && getVelocity() <= 2550))
+    //   shooterReady = true;
+    // else if(hoodedShooter.get() == Value.kReverse && (getVelocity() >= 3877 && getVelocity() <= 4077))
+    // shooterReady = true;
+    // else if(getVelocity() >= setpoint -100 && getVelocity() <= setpoint+100)
+    //   shooterReady = true;
+    // else
+    //   shooterReady = false;
+  }
+
+  public void armShooter(double targetSpeed){
+    if( (getVelocity() >= targetSpeed-100 && getVelocity() <= targetSpeed+100))
+      shooterReady = true;
+    else
+      shooterReady = false;
+  }
+
+  public void setSpeed(double setpoint){
+    shooterMotor.set(setpoint);
+    SmartDashboard.putNumber("ShooterSetpoint", setpoint);
+  }
+
+
+  public double getVelocity(){
+      return shooterEncoder.getVelocity();
+  }
+  
+  public void toggleHood(){
+    if(hoodedShooter.get() == Value.kForward){
+      hoodedShooter.set(Value.kReverse);
+    }
+
+    else{
+      hoodedShooter.set(Value.kForward);
+    }
+   
+  }
+
+  @Override
+  public void periodic() {
+    SmartDashboard.putNumber("Shooter Speed", shooterEncoder.getVelocity());
+
+    double p = SmartDashboard.getNumber("Shooter P", 0);
+    double i = SmartDashboard.getNumber("Shooter I", 0);
+    double d = SmartDashboard.getNumber("Shooter D", 0);
+    double ff = SmartDashboard.getNumber("Shooter FF", 0);
+
+
+    if((p != kP)) { shooterPidController.setP(p); kP = p;
+    LOGGER.warning("pee pee poo poo");} 
+      if((i != kI)) { shooterPidController.setI(i); kI = i;
+        LOGGER.warning("iee iee ioo ioo");} 
+      if((d != kD)) { shooterPidController.setD(d); kD = d;
+        LOGGER.warning("dee dee doo doo");} 
+        if((ff != kFF)) { shooterPidController.setFF(ff); kFF = ff;
+          LOGGER.warning("fee fee foo foo");} 
+    
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/vendordeps/ADIS16448.json b/vendordeps/ADIS16448.json
new file mode 100644
index 0000000..754dc89
--- /dev/null
+++ b/vendordeps/ADIS16448.json
@@ -0,0 +1,32 @@
+{
+    "fileName": "ADIS16448.json",
+    "name": "ADIS16448-IMU",
+    "version": "2020.r1",
+    "uuid": "38c21ab6-aa8b-44bc-b844-8086c77f09ec",
+    "mavenUrls": [
+        "http://maven.highcurrent.io/maven"
+    ],
+    "jsonUrl": "http://maven.highcurrent.io/vendordeps/ADIS16448.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.github.juchong",
+            "artifactId": "adis16448-java",
+            "version": "2020.r1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.github.juchong",
+            "artifactId": "adis16448-cpp",
+            "version": "2020.r1",
+            "libName": "libadis16448imu",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "linuxathena"
+            ]
+        }
+    ]
+}
\ No newline at end of file