From 17f58f0bb769c57915cbdc95c8f2d21852c23a85 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 30 Jun 2020 15:13:13 +0200
Subject: [PATCH] Implement right parking slot forward movement

---
 src/psp.cc | 91 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 90 insertions(+), 1 deletion(-)

diff --git a/src/psp.cc b/src/psp.cc
index c440944..375c9d4 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -438,7 +438,96 @@ void PSPlanner::fe_parallel()
                         continue;
                     }
                 } else if (this->ps().right() && this->cc().sp() > 0) {
-                    // TODO right parking slot (forward)
+                    double ccrx = this->cc().ccr().x();
+                    double ccry = this->cc().ccr().y();
+
+                    double r1 = sqrt(
+                        pow(this->cc().rfy() - ccry, 2)
+                        + pow(this->cc().rfx() - ccrx, 2)
+                    );
+                    auto cli1 = ::intersect(
+                        ccrx, ccry, r1,
+                        this->ps().x3(), this->ps().y3(),
+                        this->ps().x4(), this->ps().y4()
+                    );
+                    double a11 = ::angle_between_three_points(
+                        this->cc().lrx(), this->cc().lry(),
+                        ccrx, ccry,
+                        std::get<1>(cli1), std::get<2>(cli1)
+                    );
+                    double a12 = ::angle_between_three_points(
+                        this->cc().lrx(), this->cc().lry(),
+                        ccrx, ccry,
+                        std::get<3>(cli1), std::get<4>(cli1)
+                    );
+                    double a1 = std::min(a11, a12);
+
+                    double r2 = sqrt(
+                        pow(this->cc().lfy() - ccry, 2)
+                        + pow(this->cc().lfx() - ccrx, 2)
+                    );
+                    auto cli2 = ::intersect(
+                        ccrx, ccry, r2,
+                        this->ps().x3(), this->ps().y3(),
+                        this->ps().x4(), this->ps().y4()
+                    );
+                    double a21 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        ccrx, ccry,
+                        std::get<1>(cli2), std::get<2>(cli2)
+                    );
+                    double a22 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        ccrx, ccry,
+                        std::get<3>(cli2), std::get<4>(cli2)
+                    );
+                    double a2 = std::min(a21, a22);
+
+                    double r3 = sqrt(
+                        pow(this->cc().rfy() - ccry, 2)
+                        + pow(this->cc().rfx() - ccrx, 2)
+                    );
+                    auto cli3 = ::intersect(
+                        ccrx, ccry, r3,
+                        this->ps().x3(), this->ps().y3(),
+                        this->ps().x2(), this->ps().y2()
+                    );
+                    double a31 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        ccrx, ccry,
+                        std::get<1>(cli3), std::get<2>(cli3)
+                    );
+                    double a32 = ::angle_between_three_points(
+                        this->cc().rrx(), this->cc().rry(),
+                        ccrx, ccry,
+                        std::get<3>(cli3), std::get<4>(cli3)
+                    );
+                    double a3 = std::min(a31, a32);
+
+                    if (std::get<0>(cli1) && (
+                        (!std::get<0>(cli2) && !std::get<0>(cli3))
+                        || (a1 < a2 && !std::get<0>(cli3))
+                        || (a1 < a3 && !std::get<0>(cli2))
+                        || (a1 < a2 && a1 < a3)
+                    )) {
+                        this->cc().rotate(ccrx, ccry, -a1);
+                    } else if (std::get<0>(cli2) && (
+                        (!std::get<0>(cli1) && !std::get<0>(cli3))
+                        || (a2 < a1 && !std::get<0>(cli3))
+                        || (a2 < a3 && !std::get<0>(cli1))
+                        || (a2 < a1 && a2 < a3)
+                    )) {
+                        this->cc().rotate(ccrx, ccry, -a2);
+                    } else if (std::get<0>(cli3) && (
+                        (!std::get<0>(cli1) && !std::get<0>(cli2))
+                        || (a3 < a1 && !std::get<0>(cli2))
+                        || (a3 < a2 && !std::get<0>(cli1))
+                        || (a3 < a1 && a3 < a2)
+                    )) {
+                        this->cc().rotate(ccrx, ccry, -a3);
+                    } else {
+                        continue;
+                    }
                 } else {
                     // TODO left parking slot (both forward, backward)
                 }
-- 
2.39.2