From 251d87a91a73c70a6d00955aab1528aed9771f2c Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Sat, 14 Aug 2021 20:23:04 +0200
Subject: [PATCH] Add drive of slot method

---
 incl/pslot.hh |  6 ++++++
 src/pslot.cc  | 34 ++++++++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+)

diff --git a/incl/pslot.hh b/incl/pslot.hh
index fc460b1..c1c5311 100644
--- a/incl/pslot.hh
+++ b/incl/pslot.hh
@@ -130,6 +130,12 @@ public:
 	 */
 	std::vector<BicycleCar> drive_in_slot(BicycleCar c);
 
+	/*! \brief Drive car `c` from slot.
+	 *
+	 * \param c Starting bicycle car.
+	 */
+	std::vector<BicycleCar> drive_of_slot(BicycleCar c);
+
 	/*! \brief Steer car `c` into the parking slot `this`.
 	 *
 	 * `steer_in_slot` returns the complete path as the list of `Pose`s, not
diff --git a/src/pslot.cc b/src/pslot.cc
index dd23213..20509e1 100644
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -255,6 +255,40 @@ ParkingSlot::drive_in_slot(BicycleCar c)
 	return std::vector<BicycleCar>();
 }
 
+std::vector<BicycleCar>
+ParkingSlot::drive_of_slot(BicycleCar c)
+{
+	assert(this->parallel());
+	assert(this->right());
+	assert(c.len() < this->len());
+	assert(c.w() < this->w());
+	assert(this->parked(c));
+	std::vector<BicycleCar> path;
+	path.reserve(this->max_cusp_ + 2);
+	path.push_back(c);
+	unsigned int cusp = 0;
+	auto b_len = sizeof(this->border_) / sizeof(this->border_[0]);
+	std::vector<Point> b(this->border_, this->border_ + b_len);
+	while (cusp < this->max_cusp_ + 1) {
+		if (!c.lf().inside_of(b) && !c.rf().inside_of(b)) {
+			if (cusp < this->max_cusp_) {
+				this->max_cusp_ = cusp;
+			}
+			path.push_back(c);
+			return path;
+		}
+		c.next();
+		if (this->collide(c)) {
+			c.sp(c.sp() * -1.0);
+			c.next();
+			path.push_back(c);
+			c.st(c.st() * -1.0);
+			cusp += 1;
+		}
+	}
+	return std::vector<BicycleCar>();
+}
+
 std::vector<Pose>
 ParkingSlot::steer_in_slot(BicycleCar c)
 {
-- 
2.39.2