From e018b5d0f41073b0ce603c211a3cbd2b3cac0d5c Mon Sep 17 00:00:00 2001
From: Yohan Boujon <yo.boujon@gmail.com>
Date: Sat, 11 May 2024 12:11:24 +0200
Subject: [PATCH] Storing into a vector the choosen Arena

---
 .../raspberry/superviseur-robot/tasks.cpp     | 34 +++++++++++++++++--
 software/raspberry/superviseur-robot/tasks.h  |  6 +++-
 2 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/software/raspberry/superviseur-robot/tasks.cpp b/software/raspberry/superviseur-robot/tasks.cpp
index 08bb180..9375fd7 100644
--- a/software/raspberry/superviseur-robot/tasks.cpp
+++ b/software/raspberry/superviseur-robot/tasks.cpp
@@ -90,6 +90,10 @@ void Tasks::Init() {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
     }
+    if (err = rt_mutex_create(&mutex_arenaList, NULL)) {
+        cerr << "Error mutex create: " << strerror(-err) << endl << flush;
+        exit(EXIT_FAILURE);
+    }
     if (err = rt_mutex_create(&mutex_arenaStatus, NULL)) {
         cerr << "Error mutex create: " << strerror(-err) << endl << flush;
         exit(EXIT_FAILURE);
@@ -365,11 +369,17 @@ void Tasks::ReceiveFromMonTask(void *arg) {
         }
         else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_CONFIRM)) {
             std::cout << "\n\n\nArena Confirm asked !!!\n\n\n" << std::endl;
-            WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
+            rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
+            if(cameraStatus == ArenaStatusEnum::SEARCHING)
+                cameraStatus = ArenaStatusEnum::CONFIRM;
+            rt_mutex_release(&mutex_arenaStatus);
         }
         else if (msgRcv->CompareID(MESSAGE_CAM_ARENA_INFIRM)) {
             std::cout << "\n\n\nArena Infirm asked !!!\n\n\n" << std::endl;
-            WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
+            rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
+            if(cameraStatus == ArenaStatusEnum::SEARCHING)
+                cameraStatus = ArenaStatusEnum::INFIRM;
+            rt_mutex_release(&mutex_arenaStatus);
         }
         else if (msgRcv->CompareID(MESSAGE_CAM_IMAGE)) {
             //?
@@ -674,6 +684,8 @@ void Tasks::ArenaChoice(void * arg)
         rt_mutex_acquire(&mutex_arenaStatus, TM_INFINITE);
         as = arenaStatus;
         rt_mutex_release(&mutex_arenaStatus);
+        
+        // ASK_ARENA
         if(as == ArenaStatusEnum::SEARCHING)
         {   
             // Gathering last image + closing camera when prompted
@@ -697,5 +709,23 @@ void Tasks::ArenaChoice(void * arg)
                 rt_mutex_release(&mutex_monitor);
             }
         }
+        
+        // ARENA_CONFIRM / INFIRM
+        else if(as != ArenaStatusEnum::NONE)
+        {
+            if(as == ArenaStatusEnum::CONFIRM)
+            {
+                rt_mutex_acquire(&mutex_arenaList, TM_INFINITE);
+                arenaList.emplace_back(a);
+                rt_mutex_release(&mutex_arenaList);
+            }
+            // Re-open the camera
+            rt_mutex_acquire(&mutex_camera, TM_INFINITE);
+            cam->Open();
+            rt_mutex_release(&mutex_camera);
+            // empty the arena and write ACK to the monitor
+            a = Arena();
+            WriteInQueue(&q_messageToMon, new Message(MESSAGE_ANSWER_ACK));
+        }
     }
 }
\ No newline at end of file
diff --git a/software/raspberry/superviseur-robot/tasks.h b/software/raspberry/superviseur-robot/tasks.h
index 58d7c5d..823cec5 100644
--- a/software/raspberry/superviseur-robot/tasks.h
+++ b/software/raspberry/superviseur-robot/tasks.h
@@ -20,6 +20,7 @@
 
 #include <unistd.h>
 #include <iostream>
+#include <vector>
 
 #include <sys/mman.h>
 #include <alchemy/task.h>
@@ -46,7 +47,8 @@ enum class CameraStatusEnum {
 enum class ArenaStatusEnum {
     NONE,
     SEARCHING,
-    CHOOSEN
+    CONFIRM,
+    INFIRM
 };
 
 class Tasks {
@@ -82,6 +84,7 @@ private:
     bool robotBatteryGet = false;
     CameraStatusEnum cameraStatus = CameraStatusEnum::CLOSED;
     ArenaStatusEnum arenaStatus = ArenaStatusEnum::NONE;
+    std::vector<Arena> arenaList;
     Camera* cam = nullptr;
 
     /**********************************************************************/
@@ -109,6 +112,7 @@ private:
     RT_MUTEX mutex_batteryGet;
     RT_MUTEX mutex_cameraStatus;
     RT_MUTEX mutex_camera;
+    RT_MUTEX mutex_arenaList;
     RT_MUTEX mutex_arenaStatus;
 
     /**********************************************************************/