From 02e1d78671aa195178a6685fe58f39cd632e8900 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Fri, 23 Aug 2019 15:48:15 +0200
Subject: [PATCH] Add possible inits ut, skeleton

---
 api/psp.h   |  6 ++++++
 src/psp.cc  |  6 ++++++
 ut/psp.t.cc | 33 +++++++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+)

diff --git a/api/psp.h b/api/psp.h
index 223e329..1122d37 100644
--- a/api/psp.h
+++ b/api/psp.h
@@ -60,6 +60,12 @@ class PSPlanner {
                 parking slot `ps`.
                 */
                 bool parked();
+                /*! \brief Return possible starts of parking maneuver
+
+                When any `BicycleCar` of possible inits is reached, then
+                parking maneuver is a peace of cake.
+                */
+                std::vector<BicycleCar> possible_inits();
 
                 // find entry
                 /*! \brief Find entry to the parking slot.
diff --git a/src/psp.cc b/src/psp.cc
index 1bd9aed..86e6533 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -112,6 +112,12 @@ bool PSPlanner::parked()
                 && inside(this->gc().rfx(), this->gc().rfy(), slot);
 }
 
+std::vector<BicycleCar> PSPlanner::possible_inits()
+{
+        std::vector<BicycleCar> pi;
+        return pi;
+}
+
 // find entry
 void PSPlanner::fe()
 {
diff --git a/ut/psp.t.cc b/ut/psp.t.cc
index 72aefa4..7a343cd 100644
--- a/ut/psp.t.cc
+++ b/ut/psp.t.cc
@@ -126,3 +126,36 @@ WVTEST_MAIN("forward perpendicullar parking slot planner")
         WVPASS(psp.left());
         WVPASS(psp.parked());
 }
+
+WVTEST_MAIN("possible goals test")
+{
+        PSPlanner psp;
+        psp.gc().x(0);
+        psp.gc().y(0);
+        psp.gc().h(0);
+        psp.gc().mtr(10);
+        psp.gc().wb(2);
+        psp.gc().w(1);
+        psp.gc().l(3);
+        psp.gc().he(1.5);
+        psp.gc().df(2 + 0.5);
+        psp.gc().dr(0.5);
+        psp.cc() = BicycleCar(psp.gc());
+        psp.cc().sp(1);
+        psp.cc().st(0);
+        psp.cc().next();
+        WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
+        WVPASS(psp.possible_inits().size() > 0);
+        double i = 1;
+        for (auto &c: psp.possible_inits()) {
+                WVPASSEQ_DOUBLE(psp.gc().x() + 1 + i, c.x(), 0.00001);
+                WVPASSEQ_DOUBLE(psp.gc().y(), c.y(), 0.00001);
+                WVPASSEQ_DOUBLE(psp.gc().h(), c.h(), 0.00001);
+                i++;
+        }
+        WVPASSEQ_DOUBLE(psp.gc().x() + 1, psp.cc().x(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().y(), psp.cc().y(), 0.00001);
+        WVPASSEQ_DOUBLE(psp.gc().h(), psp.cc().h(), 0.00001);
+}
-- 
2.39.2