From dbeb13d37c7dbaf38d60565c84783b5d149f978a Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Sun, 5 May 2024 22:13:51 +0200
Subject: [PATCH] do not take fallen flower

---
 TCPServer.cpp | 36 +++++++++++++++++++++++++++++++++++-
 TCPServer.h   |  2 ++
 utils.h       |  2 +-
 3 files changed, 38 insertions(+), 2 deletions(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 3e157f7..df7dbaa 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -842,6 +842,29 @@ std::optional<ArucoTag> TCPServer::getMostCenteredArucoTag(const float borneMinX
     return found ? std::optional(mostCenteredTag) : std::nullopt;
 }
 
+std::vector<int> TCPServer::getNotFallenFlowers(float BorneMin, float BorneMax) {
+    std::vector<int> res = {0, 1, 2};
+    for (auto & tag : arucoTags) {
+        if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) {
+            auto roll = tag.rot()[1];
+            auto yPos = tag.pos()[1];
+
+            if (roll < BorneMin && roll > BorneMax) {
+                if (yPos > 70) {
+                    res[2] = -1;
+                }
+                else if (yPos < -70) {
+                    res[0] = -1;
+                }
+                else {
+                    res[1] = -1;
+                }
+            }
+        }
+    }
+    return res;
+}
+
 void TCPServer::handleEmergency(int distance, double angle) {
     this->handleEmergencyFlag = true;
 
@@ -1469,8 +1492,19 @@ void TCPServer::go3Plants(const StratPattern sp) {
 
     // TODO Check for falling flowers
 
+    this->arucoTags.clear();
+    for (int i = 0; i < 5; i++) {
+        this->broadcastMessage("strat;aruco;get aruco;1\n");
+        usleep(110'000);
+    }
+
+    // TODO, Les bornes ici est l'angle que la plante doit avoir pour etre considére comme tombée (par rapport a la caméra)
+    std::vector<int> pinceCanTakeFLower = getNotFallenFlowers(0.3, 0.5);
+
     for (int i = 0; i < 3; i++) {
-        this->openPince(i);
+        if (pinceCanTakeFLower[i] != -1) {
+            this->openPince(i);
+        }
     }
     usleep(200'000);
 
diff --git a/TCPServer.h b/TCPServer.h
index 90a8471..76e6f24 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -215,6 +215,8 @@ public:
 
     std::optional<ArucoTag> getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
 
+    std::vector<int> getNotFallenFlowers(float BorneMin, float BorneMax);
+
     void handleEmergency(int distance, double angle);
 
     /*
diff --git a/utils.h b/utils.h
index ce106bc..60e5948 100644
--- a/utils.h
+++ b/utils.h
@@ -64,7 +64,7 @@ private:
     std::string _name;
     std::array<float, 2> _pos = {0, 0};
     std::array<float, 3> _rot = {0, 0, 0};
-    int nbFind = 0;
+    int nbFind = 1;
 };
 
 double distanceToTag(const ArucoTag& tag);
\ No newline at end of file