From 118b8dd215cba706dfe8b0889ac8f0c0230b1c33 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 19 Jul 2021 09:57:52 +0200
Subject: [PATCH] Add steer in slot method

---
 incl/pslot.hh |  9 +++++++++
 src/pslot.cc  | 17 +++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/incl/pslot.hh b/incl/pslot.hh
index b7d3956..1562631 100644
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -89,6 +89,15 @@ public:
 	 */
 	std::vector<BicycleCar> drive_in_slot(BicycleCar c, unsigned int& max);
 
+	/*! \brief Steer car `c` into the parking slot `this`.
+	 *
+	 * `steer_in_slot` returns the complete path as the list of `Pose`s, not
+	 * just cusp `BicycleCar`s as `drive_to_slot`.
+	 *
+	 * \param c Starting bicycle car.
+	 */
+	std::vector<Pose> steer_in_slot(BicycleCar c);
+
 	/*! \brief Find entry.
 	 *
 	 * \param c For which `BicycleCar` should entry be found?
diff --git a/src/pslot.cc b/src/pslot.cc
index a10b17c..fb42ab0 100644
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -174,6 +174,23 @@ ParkingSlot::drive_in_slot(BicycleCar c, unsigned int& max)
 	return std::vector<BicycleCar>();
 }
 
+std::vector<Pose>
+ParkingSlot::steer_in_slot(BicycleCar c)
+{
+	std::vector<Pose> path;
+	while (!this->parked(c)) {
+		path.push_back(c);
+		c.next();
+		if (this->collide(c)) {
+			c.sp(c.sp() * -1.0);
+			c.next();
+			c.st(c.st() * -1.0);
+		}
+	}
+	path.push_back(c);
+	return path;
+}
+
 PoseRange
 ParkingSlot::fe(BicycleCar c, unsigned int& max)
 {
-- 
2.39.2