From d21976724b31126ecc358fc754b6e5737c87d60d Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 4 Mar 2019 15:57:19 +0100
Subject: [PATCH] Add BFS proposal to fip()

---
 decision_control/slotplanner.cc | 58 ++++++++++++++++++++++++++++++++-
 1 file changed, 57 insertions(+), 1 deletion(-)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index 671186f..38d0989 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -49,7 +49,63 @@ void ParallelSlot::DH(float dh)
 // other
 void ParallelSlot::fip()
 {
-        std::queue<RRTNode *, std::list<RRTNode *>> q;
+        // see https://courses.cs.washington.edu/courses/cse326/03su/homework/hw3/bfs.html
+        // RRTNode.s() works as iteration level
+        std::queue<BicycleCar *, std::list<BicycleCar *>> q;
+
+        // TODO add init nodes
+        // for now just copy fpose()
+        bool left = false; // right parking slot
+        float di = -1;
+        BicycleCar *CC = new BicycleCar(
+                this->fposecenter()->x(),
+                this->fposecenter()->y() - 0.01,
+                M_PI / 2
+        );
+        BicycleCar *B = new BicycleCar(
+                CC->x() - CC->width() / 2,
+                CC->y() - (CC->length() + CC->wheelbase()) / 2,
+                M_PI / 2
+        );
+        if (this->slot().bnodes()[0]->x() > this->slot().bnodes()[1]->x()) {
+                left = true;
+                di = 1;
+                delete B;
+                B = new BicycleCar(
+                        CC->x() + CC->width() / 2,
+                        CC->y() - (CC->length() + CC->wheelbase()) / 2,
+                        M_PI / 2
+                );
+        }
+        this->DH(di * 0.01 / CC->out_radi());
+        BicycleCar *c;
+        int i = 0;
+        c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
+        while (!this->slot().collide(c->frame())) {
+                q.push(c);
+                c = B->move(CC, -i * di * 0.01 / CC->diag_radi());
+                i += 1;
+        }
+        delete c; // not in q and collide
+        // BFS
+        while (!q.empty()) {
+                c = q.front();
+                q.pop();
+                if (this->DH() > 0 && c->rfx() <= 0 && c->rrx() <= 0) {
+                        // TODO goal found
+                } else if (this->DH() < 0 && c->lfx() >= 0 && c->lrx() >= 0) {
+                        goto createcuspandfinish;
+                } else if (c->s() < 9) {
+                        BicycleCar *cc = this->flnc(c);
+                        cc->s(c->s() + 1);
+                        cc->bcparent(c);
+                        q.push(cc);
+                } else {
+                        delete c; // not in q and collide
+                }
+        }
+        // delete all from q
+        // return c
 }
 
 RRTNode *ParallelSlot::fposecenter()
-- 
2.39.2