From c74027a1101d4fb611aa5551a9d7c4a691d97e7a Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 20 Jul 2020 13:49:04 +0200
Subject: [PATCH] Use forward parking macro

---
 api/psp.h  |  2 ++
 src/psp.cc | 23 +++++++----------------
 2 files changed, 9 insertions(+), 16 deletions(-)

diff --git a/api/psp.h b/api/psp.h
index b604984..2294a5b 100644
--- a/api/psp.h
+++ b/api/psp.h
@@ -7,6 +7,8 @@
 #include "bcar.h"
 #include "pslot.h"
 
+#define FORWARD_PARKING 1
+
 /*! \brief Parking Slot Planner basic class.
 
 \param cc Current bicycle car.
diff --git a/src/psp.cc b/src/psp.cc
index f69e1f2..cc11af7 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -21,20 +21,11 @@ bool PSPlanner::collide()
 
 bool PSPlanner::forward()
 {
-        if (this->ps().parallel())
-                return false;
-        else
-                return true;
-        double heading = atan2(
-                this->ps().y2() - this->ps().y1(),
-                this->ps().x2() - this->ps().x1()
-        );
-        while (heading < 0) heading += 2 * M_PI;
-        double h = this->gc().h();
-        while (h < 0) h += 2 * M_PI;
-        if (std::abs(heading - h) < M_PI / 4)
-                return true;
+#if FORWARD_PARKING > 0
+        return true;
+#else
         return false;
+#endif
 }
 
 void PSPlanner::gc_to_4()
@@ -102,8 +93,8 @@ void PSPlanner::guess_gc()
                 x += (this->gc().dr() + 0.01) * cos(h);
                 y += (this->gc().w() / 2 + 0.01) * sin(h + dts);
                 y += (this->gc().dr() + 0.01) * sin(h);
-#if 0
         } else {
+#if FORWARD_PARKING > 0
                 // Forward parking
                 double entry_width = edist(
                         this->ps().x1(), this->ps().y1(),
@@ -118,8 +109,7 @@ void PSPlanner::guess_gc()
                 while (h < 0) h += 2 * M_PI;
                 x += this->gc().dr() * cos(h + M_PI);
                 y += this->gc().dr() * sin(h + M_PI);
-#endif
-        } else {
+#else
                 // Backward parking
                 double entry_width = edist(
                         this->ps().x1(), this->ps().y1(),
@@ -134,6 +124,7 @@ void PSPlanner::guess_gc()
                 while (h < 0) h += 2 * M_PI;
                 x += this->gc().df() * cos(h + M_PI);
                 y += this->gc().df() * sin(h + M_PI);
+#endif
         }
         while (h > M_PI)
                 h -= 2 * M_PI;
-- 
2.39.2