From 84268d6dbfcf1b86f90a921063970bf7d46ef447 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 11 Mar 2019 10:04:38 +0100
Subject: [PATCH] Update fipr

---
 decision_control/slotplanner.cc | 27 ++++++++++++++-------------
 1 file changed, 14 insertions(+), 13 deletions(-)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 56f5ba0..a81fd1e 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -178,14 +178,16 @@ void ParallelSlot::fipr(BicycleCar *B)
 {
         // TODO for right parallel parking also
         // it's only for lpar scenario now
-
         std::vector<RRTNode *> cusp;
         cusp.push_back(new RRTNode(B->x(), B->y(), B->h()));
         // just copied from fip()
-        this->DH(-0.01 / B->out_radi());
+        int di = 1;
+        if (this->slotSide() == LEFT)
+                di = -1;
+        this->DH(di * 0.01 / B->out_radi());
         BicycleCar *c;
         c = this->flncr(B);
-        while (c->lfx() < 0) {
+        while (this->slot().collide(new RRTNode(c->lfx(), c->lfy(), 0))) {
                 cusp.push_back(new RRTNode(c->x(), c->y(), c->h()));
                 BicycleCar *cc = this->flncr(c);
                 cc->s(c->s() + 1);
@@ -245,11 +247,10 @@ BicycleCar *ParallelSlot::flncr(BicycleCar *B)
         BicycleCar *p;
         int i = 1;
         p = B->move(cc, i * this->DH());
-        while (!this->slot().collide(p->frame())
-                        && (
-                                (this->DH() > 0 && p->x() >= 0)
-                                || (this->DH() < 0 && p->lfx() <= 0)
-                        )) {
+        while (
+                !this->slot().collide(p->frame())
+                && this->slot().collide(new RRTNode(p->lfx(), p->lfy(), 0))
+        ) {
                 delete p;
                 i += 10;
                 p = B->move(cc, i * this->DH());
@@ -257,14 +258,14 @@ BicycleCar *ParallelSlot::flncr(BicycleCar *B)
         i -= 10;
         p = B->move(cc, i * this->DH());
         while (!this->slot().collide(p->frame())) {
-                if (this->DH() > 0 && p->x() <= 0) {
-                        i += 1;
-                        break;
-                }
-                if (this->DH() < 0 && p->lfx() >= 0) {
+                if(!this->slot().collide(new RRTNode(p->lfx(), p->lfy(), 0))) {
                         i += 1;
                         break;
                 }
+                //if (this->DH() < 0 && p->lfx() >= 0) {
+                //        i += 1;
+                //        break;
+                //}
                 delete p;
                 i += 1;
                 p = B->move(cc, i * this->DH());
-- 
2.39.2