diff --git a/TCPServer.cpp b/TCPServer.cpp
index ef591ea..df1c13b 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -12,20 +12,23 @@ void ClientHandler::handle() {
 
         if (valread > 0) {
             buffer.append(tempBuffer, valread);
-            //std::cout << "Received: " << buffer << std::endl;
 
             if (buffer == "quit") {
                 std::cerr << "Client requested to quit. Closing connection." << std::endl;
                 break;
             }
-            processMessage(buffer);
+
+            std::vector<std::string> messages = TCPUtils::split(buffer, "\n");
+            for (const std::string& message : messages) {
+                processMessage(message);
+            }
 
             buffer.clear();
         } else if (valread == 0) {
-            std::cout << "Client disconnected." << std::endl;
+            std::cout << "Client disconnected. " << clientSocket << std::endl;
             break; // Client disconnected
         } else {
-            std::cerr << "Failed to receive data." << std::endl;
+            std::cerr << "Failed to receive data." << this->clientSocket << std::endl;
             break; // Error in receiving data
         }
     }
@@ -42,7 +45,7 @@ void ClientHandler::closeConnection() {
     server->clientDisconnected(clientSocket); // Inform the server that the client has disconnected
 }
 
-TCPServer::TCPServer(int port)
+TCPServer::TCPServer(int port) : team(TEST)
 {
     this->robotPose = {500, 500, -3.1415/2};
 
@@ -69,36 +72,16 @@ TCPServer::TCPServer(int port)
 
     std::cout << "Server started on port " << port << std::endl;
 
+    clients.reserve(5);
 
-    clients.reserve(7);
-    ClientTCP tirette;
-    tirette.name = "tirette";
+    clients.emplace_back("tirette");
+    // clients.emplace_back("aruco");
+    clients.emplace_back("ihm");
+    clients.emplace_back("lidar");
+    clients.emplace_back("arduino");
+    clients.emplace_back("servo_moteur");
+    // clients.emplace_back("point");
 
-    ClientTCP aruco;
-    aruco.name = "aruco";
-
-    ClientTCP ihm;
-    ihm.name = "ihm";
-
-    ClientTCP lidar;
-    lidar.name = "lidar";
-
-    ClientTCP arduino;
-    arduino.name = "arduino";
-
-    ClientTCP servo_moteur;
-    servo_moteur.name = "servo_moteur";
-
-    ClientTCP point;
-    point.name = "point";
-
-    clients.push_back(tirette);
-    clients.push_back(aruco);
-    clients.push_back(ihm);
-    clients.push_back(lidar);
-    clients.push_back(arduino);
-    clients.push_back(servo_moteur);
-    clients.push_back(point);
 }
 
 void TCPServer::acceptConnections()
@@ -130,121 +113,165 @@ void TCPServer::acceptConnections()
 
 void TCPServer::handleMessage(const std::string& message, int clientSocket)
 {
-    // std::cout << message << std::endl;
+    std::cout << message << std::endl;
 
     std::vector<std::string> tokens = TCPUtils::split(message, ";");
 
     if (tokens.size() != 4)
     {
-        std::cerr << "Invalid message format : " << message << std::endl;
+        std::cerr << "Invalid message format, token size : " << std::to_string(tokens.size()) << " from message : " << message << std::endl;
         return;
     }
-    if (tokens[1] != "strat")
-    {
-        this->broadcastMessage(message.c_str(), clientSocket);
-    }
+    if (TCPUtils::contains(tokens[2], "stop proximity")) {
+        if (!gameStarted) return;
 
+        std::vector<std::string> args = TCPUtils::split(tokens[3], ",");
+
+        if (stoi(args[0]) == -1) return;
+
+        this->broadcastMessage("strat;arduino;clear;1\n");
+
+        this->stopEmergency = true;
+
+        // if (!handleEmergencyFlag) {
+            // std::thread([this, args]() { this->handleEmergency(std::stoi(args[0]), std::stod(args[1]) / 100); }).detach();
+        // }
+    }
+    else if (tokens[1] != "strat") {
+        this->broadcastMessage(message, clientSocket);
+    }
     // EMERGENCY
-    if (tokens[2] == "stop proximity") {
-        // TODO handle emergency
-        // this->broadcastMessage("strat;arduino;stop;1");
+    else if (tokens[0] == "tirette" && tokens[2] == "set state") {
+        this->broadcastMessage(message, clientSocket);
     }
-
-    else if (tokens[0] == "tirette" && tokens[2] == "set state")
-    {
-        this->broadcastMessage(message.c_str(), clientSocket);
-    }
-    else if (tokens[2] == "ready")
-    {
+    else if (tokens[2] == "ready") {
         for (ClientTCP& client : clients)
         {
             if (client.name == tokens[0])
             {
                 client.isReady = true;
                 client.socket = clientSocket;
+                if (TCPUtils::contains(client.name, "lidar")) {
+                    this->lidarSocket = clientSocket;
+                }
+                std::cout << client.socket << " | " << client.name << " is ready" << std::endl;
                 break;
             }
         }
         checkIfAllClientsReady();
     }
-    else if (tokens[2] == "set pos") {
-        std::vector<std::string> pos = TCPUtils::split(tokens[3], ",");
-        this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100};
-    }
     else if (tokens[2] == "get pos") {
-        std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n";
-        this->sendToClient(toSend, clientSocket);
+        this->setPosition(this->robotPose, clientSocket);
     }
-    else if (tokens[2] == "spawn") {
-        int spawnPointNb = std::stoi(tokens[3]);
-        float spawnPoint[3];
-        float finishPoint[3];
-
-        switch (spawnPointNb) {
-            case 3:
-                this->team = BLUE;
-                spawnPoint[0] = 250;
-                spawnPoint[1] = 1790;
-                spawnPoint[2] = 0;
-
-                // For test
-                finishPoint[0] = 400;
-                finishPoint[1] = 1790;
-                finishPoint[2] = 0;
-
-                /*finishPoint[0] = 400;
-                finishPoint[1] = 400;
-                finishPoint[2] = 3.1415;*/
-                break;
-            case 6:
-                this->team = YELLOW;
-                spawnPoint[0] = 1750;
-                spawnPoint[1] = 1790;
-                spawnPoint[2] = 3.1415;
-                finishPoint[0] = 2600;
-                finishPoint[1] = 400;
-                finishPoint[2] = 0;
-                break;
-
-            default:
-                this->team = TEST;
-                spawnPoint[0] = 1200;
-                spawnPoint[1] = 1800;
-                spawnPoint[2] = 1.57;
-                finishPoint[0] = 1200;
-                finishPoint[1] = 1800;
-                finishPoint[2] = 1.57;
-                break;
+    else if (tokens[2] == "get speed") {
+        this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket);
+    }
+    else if (tokens[0] == "lidar" && tokens[2] == "set pos") {
+        std::vector<std::string> args = TCPUtils::split(tokens[3], ",");
+        // TODO replace angle with the real angle calculated by the lidar when working
+        this->lidarCalculatePos = {std::stof(args[0]), std::stof(args[1]), /*std::stof(args[2]) / 100*/ this->robotPose.theta};
+        if (lidarCalculatePos.pos.x == -1 || lidarCalculatePos.pos.y == -1) {
+            if (lidarGetPosTimeout > 10) {
+                awaitForLidar = false;
+            }
+            lidarGetPosTimeout++;
+            this->askLidarPosition();
         }
-
-        std::ofstream file("end_point.txt");
-        file << finishPoint[0] << " " << finishPoint[1];
-        file.close();
-
-        this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]};
-        this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]};
-        this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]};
-        std::string toSend = "strat;all;set pos;" + std::to_string(this->robotPose.pos.x) + "," + std::to_string(this->robotPose.pos.y) + "," + std::to_string(this->robotPose.theta * 100) + "\n";
-
-        for (int j = 0; j < 3; j++) {
-            this->broadcastMessage(toSend);
-            usleep(200'000);
+        else {
+            this->setPosition(this->lidarCalculatePos);
+            usleep(100'000);
+            this->setPosition(this->lidarCalculatePos);
+            awaitForLidar = false;
         }
     }
-    else if (tokens[1] == "strat" && tokens[2] == "start")
-    {
-        this->broadcastMessage(message.c_str(), clientSocket);
+    else if (tokens[0] == "ihm") {
+        if (tokens[2] == "spawn") {
+            int spawnPointNb = std::stoi(tokens[3]);
+            std::array<float, 3> spawnPoint{};
+            std::array<float, 3> finishPoint{};
 
-        switch (this->team) {
-            case BLUE:
-                std::thread([this]() { this->startGameBlueTeam(); }).detach();
-                break;
-            case YELLOW:
-                std::thread([this]() { this->startGameYellowTeam(); }).detach();
-                break;
-            case TEST:
-                std::thread([this]() { this->startGameTest(); }).detach();
-                break;
+            switch (spawnPointNb) {
+                case 3:
+                    this->setTeam(BLUE);
+                    spawnPoint[0] = 250;
+                    spawnPoint[1] = 1800;
+                    spawnPoint[2] = 0;
+
+                    finishPoint[0] = 400;
+                    finishPoint[1] = 500;
+                    finishPoint[2] = PI / 2;
+
+                    // For test
+
+                    /*spawnPoint[0] = 500;
+                    spawnPoint[1] = 1000;
+                    spawnPoint[2] = 0;*/
+
+                    /*finishPoint[0] = 400;
+                    finishPoint[1] = 1790;
+                    finishPoint[2] = 0;*/
+                    break;
+                case 6:
+                    this->setTeam(YELLOW);
+                    spawnPoint[0] = 2750;
+                    spawnPoint[1] = 1800;
+                    spawnPoint[2] = PI;
+
+                    finishPoint[0] = 2600;
+                    finishPoint[1] = 500;
+                    finishPoint[2] = PI / 2;
+                    break;
+
+                default:
+                    this->team = TEST;
+                    spawnPoint[0] = 1200;
+                    spawnPoint[1] = 1800;
+                    spawnPoint[2] = PI / 2;
+
+                    finishPoint[0] = 1200;
+                    finishPoint[1] = 1800;
+                    finishPoint[2] = PI / 2;
+                    break;
+            }
+
+            std::ofstream file("end_point.txt");
+            file << finishPoint[0] << " " << finishPoint[1];
+            file.close();
+
+            this->robotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]};
+            this->initRobotPose = {spawnPoint[0], spawnPoint[1], spawnPoint[2]};
+            this->endRobotPose = {finishPoint[0], finishPoint[1], finishPoint[2]};
+
+            for (int j = 0; j < 3; j++) {
+                this->setPosition(this->initRobotPose);
+                usleep(100'000);
+            }
+        }
+        else if (tokens[1] == "strat" && tokens[2] == "start")
+        {
+            if (this->gameStarted) {
+                return;
+            }
+
+            this->broadcastMessage(message, clientSocket);
+
+            this->gameStarted = true;
+
+            this->gameStart = std::chrono::system_clock::now();
+
+            this->setSpeed(200);
+
+            switch (this->team) {
+                case BLUE:
+                case YELLOW:
+                    this->gameThread = std::thread([this]() { this->startGame(); });
+                    break;
+                case TEST:
+                    this->gameThread = std::thread([this]() { this->startGameTest(); });
+                    break;
+            }
+
+            this->gameThread.detach();
         }
     }
     else if (tokens[0] == "aruco" && tokens[2] == "get aruco") {
@@ -259,10 +286,12 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
                 tag.setPos(std::stof(aruco[i + 2]), std::stof(aruco[i + 3]));
                 tag.setRot(std::stof(aruco[i + 4]), std::stof(aruco[i + 5]), std::stof(aruco[i + 6]));
 
+                // std::cout << tag << std::endl;
+
                 handleArucoTag(tag);
             }
             // Broadcast the aruco tag to all clients
-            this->broadcastMessage(message.c_str(), clientSocket);
+            this->broadcastMessage(message, clientSocket);
         }
     }
     else if (tokens[0] == "arduino") {
@@ -272,38 +301,71 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
             }
         } else if (tokens[2] == "set speed") {
             this->speed = std::stoi(tokens[3]);
+        } else if (tokens[2] == "set pos") {
+            std::vector<std::string> pos = TCPUtils::split(tokens[3], ",");
+            this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2]) / 100};
+            if (!awaitForLidar) {
+                this->setPosition(this->robotPose, lidarSocket);
+            }
         }
-    } else if (tokens[2] == "get speed") {
-        this->sendToClient("strat;" + tokens[0] + ";set speed;" + std::to_string(this->speed) + "\n", clientSocket);
     } else if (tokens[2] == "test aruco") {
         int pince = std::stoi(tokens[3]);
 
         std::thread([this, pince]() { this->startTestAruco(pince); }).detach();
     }
-    std::cout << "Received: " << message << std::endl;
+    // std::cout << "Received: " << message << std::endl;
 }
 
 void TCPServer::broadcastMessage(const char* message, int senderSocket)
 {
+    std::string temp = std::string(message);
+    if (temp[temp.size() - 1] != '\n') {
+        temp += '\n';
+    }
+
     for (int clientSocket : clientSockets) {
         if (clientSocket != senderSocket) { // Exclude the sender's socket
-            send(clientSocket, message, strlen(message), 0);
+            send(clientSocket, temp.c_str(), temp.length(), 0);
         }
     }
 }
 
 void TCPServer::broadcastMessage(const std::string &message, int senderSocket) {
-    this->broadcastMessage(message.c_str(), senderSocket);
+    std::string temp = const_cast<std::string&>(message);
+    if (temp[temp.size() - 1] != '\n') {
+        temp += '\n';
+    }
+
+    for (int clientSocket : clientSockets) {
+        if (clientSocket != senderSocket) { // Exclude the sender's socket
+            send(clientSocket, temp.c_str(), temp.length(), 0);
+        }
+    }
 }
 
 void TCPServer::sendToClient(const std::string &message, int clientSocket) {
-    this->sendToClient(message.c_str(), clientSocket);
+    std::string temp = const_cast<std::string&>(message);
+    if (temp[temp.size() - 1] != '\n') {
+        temp += '\n';
+    }
+
+    for (int socket : clientSockets) {
+        if (socket == clientSocket) {
+            send(socket, temp.c_str(), temp.size(), 0);
+            break;
+        }
+    }
 }
 
 void TCPServer::sendToClient(const char *message, int clientSocket) {
+    std::string temp = std::string(message);
+    if (temp[temp.size() - 1] != '\n') {
+        temp += '\n';
+    }
+
     for (int socket : clientSockets) {
         if (socket == clientSocket) {
-            send(socket, message, strlen(message), 0);
+            send(socket, temp.c_str(), temp.size(), 0);
             break;
         }
     }
@@ -343,6 +405,11 @@ void TCPServer::stop() {
     for (auto& thread : clientThreads) {
         thread.join();
     }
+
+    if (gameStarted) {
+        this->gameThread.~thread();
+    }
+
     // Close the server socket
     close(serverSocket);
 }
@@ -379,378 +446,102 @@ void TCPServer::checkIfAllClientsReady()
     }
 }
 
-void TCPServer::startGameBlueTeam() {
-    this->setSpeed(130);
-    this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
-    usleep(100'000);
-    std::string toSend;
-    this->go(380, this->robotPose.pos.y);
-    awaitRobotIdle();
+void TCPServer::startGame() {
+    gameStarted = true;
+    for (int i = whereAmI; i < stratPatterns.size(); i++) {
 
-    this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n");
-    this->go(474, this->robotPose.pos.y);
-    awaitRobotIdle();
-
-    this->rotate(0);
-    awaitRobotIdle();
-
-    this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
-    usleep(100'000);
-
-    this->go(620, this->robotPose.pos.y);
-    awaitRobotIdle();
-
-    this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n");
-    this->go(749, this->robotPose.pos.y);
-    awaitRobotIdle();
-
-    this->rotate(0);
-    awaitRobotIdle();
-
-    this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
-    usleep(100'000);
-
-    this->go(1000, 1700);
-    awaitRobotIdle();
-
-    this->broadcastMessage("strat;servo_moteur;uncheck panneau;7\n");
-
-    this->rotate(PI / 2);
-    awaitRobotIdle();
-    this->setSpeed(200);
-
-    this->go(1000, 1800);
-    awaitRobotIdle();
-
-    this->rotate(PI / 2);
-
-    usleep(1'000'000);
-
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
-    }
-
-    std::optional<ArucoTag> tag = getBiggestArucoTag(300, 700, -200, 200);
-
-    if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
-            goToAruco(tag.value(), 1);
-        } else if (pinceState[2] == NONE) {
-            goToAruco(tag.value(), 2);
-        } else if (pinceState[0] == NONE) {
-            goToAruco(tag.value(), 0);
-        }
-    }
-
-    this->go(1000, 1700);
-    awaitRobotIdle();
-
-    this->rotate(PI / 2);
-    awaitRobotIdle();
-
-    usleep(1'000'000);
-
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
-    }
-
-    tag = getBiggestArucoTag(300, 700, -200, 200);
-
-    if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
-            goToAruco(tag.value(), 1);
-        } else if (pinceState[2] == NONE) {
-            goToAruco(tag.value(), 2);
-        } else if (pinceState[0] == NONE) {
-            goToAruco(tag.value(), 0);
-        }
-    }
-
-    this->go(1000, 1700);
-    awaitRobotIdle();
-
-    this->rotate(PI / 2);
-    awaitRobotIdle();
-
-    usleep(1'000'000);
-
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
-    }
-
-    tag = getBiggestArucoTag(300, 700, -200, 200);
-
-    if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
-            goToAruco(tag.value(), 1);
-        } else if (pinceState[2] == NONE) {
-            goToAruco(tag.value(), 2);
-        } else if (pinceState[0] == NONE) {
-            goToAruco(tag.value(), 0);
-        }
-    }
-
-    this->go(1000, 1700);
-    awaitRobotIdle();
-
-    this->rotate(PI);
-    awaitRobotIdle();
-
-    usleep(1'000'000);
-
-    this->go(this->robotPose.pos.x - 350, this->robotPose.pos.y);
-    awaitRobotIdle();
-
-    this->go(500, 500);
-    awaitRobotIdle();
-
-    std::vector<int> pinceHavePurpleFlower;
-    for (int i = 0; i < 3; i++) {
-        if (pinceState[i] == PURPLE_FLOWER) {
-            pinceHavePurpleFlower.push_back(i);
-        }
-    }
-
-    if (!pinceHavePurpleFlower.empty()) {
-        this->go(300, 300);
-        awaitRobotIdle();
-        this->rotate(PI / 2);
-        awaitRobotIdle();
-
-        for (auto & toDrop : pinceHavePurpleFlower) {
-            toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n";
-            this->broadcastMessage(toSend);
-
-            this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150);
-            awaitRobotIdle();
-
-            pinceState[toDrop] = NONE;
-            toSend = "strat;servo_moteur;fermer pince;" + std::to_string(toDrop) + "\n";
-            this->broadcastMessage(toSend);
-            usleep(100'000);
+        auto time = std::chrono::system_clock::now();
+        if (time - gameStart > std::chrono::seconds(87)) {
+            this->goEnd();
+            return;
         }
 
-        this->go(this->robotPose.pos.x + 150, this->robotPose.pos.y);
-        awaitRobotIdle();
-    }
-
-
-    if (pinceHavePurpleFlower.size() < 3) {
-        this->go(762, 300);
-        awaitRobotIdle();
-
-        this->setSpeed(130);
-        this->rotate(PI / 2);
-        awaitRobotIdle();
-
-        this->broadcastMessage("strat;servo_moteur;lever bras;1\n");
-
-        this->go(762, 0);
-        usleep(3'000'000);
-
-        for (int i = 0; i < 3; i++) {
-            if (pinceState[i] == WHITE_FLOWER) {
-                toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(i) + "\n";
-                this->broadcastMessage(toSend);
-                usleep(500'000);
-                pinceState[i] = NONE;
-                toSend = "strat;servo_moteur;fermer pince;" + std::to_string(i) + "\n";
-                this->broadcastMessage(toSend);
-                usleep(100'000);
-            }
-        }
-
-        this->setSpeed(200);
-    }
-
-    /*
-     *  TODO
-     *  here add the whole strategy, for the moment the robot only took 3 plants and drop them
-     *  Handle the second plant spot
-     */
-
-    this->go(1000, 250);
-    awaitRobotIdle();
-
-    this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
-
-    this->rotate(-PI/4);
-    awaitRobotIdle();
-
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
-    }
-
-    tag = getBiggestArucoTag(300, 700, -200, 200);
-
-    if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
-            goToAruco(tag.value(), 1);
-        } else if (pinceState[2] == NONE) {
-            goToAruco(tag.value(), 2);
-        } else if (pinceState[0] == NONE) {
-            goToAruco(tag.value(), 0);
-        }
-    }
-
-    this->go(1000, 250);
-    awaitRobotIdle();
-
-    this->rotate(-PI / 2);
-    awaitRobotIdle();
-
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
-    }
-
-    tag = getBiggestArucoTag(300, 700, -200, 200);
-
-    if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
-            goToAruco(tag.value(), 1);
-        } else if (pinceState[2] == NONE) {
-            goToAruco(tag.value(), 2);
-        } else if (pinceState[0] == NONE) {
-            goToAruco(tag.value(), 0);
-        }
-    }
-
-
-    this->go(1000, 250);
-    awaitRobotIdle();
-
-    this->rotate(PI / 2);
-    awaitRobotIdle();
-
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
-    }
-
-    tag = getBiggestArucoTag(300, 700, -200, 200);
-
-    if (tag.has_value()) {
-        if (pinceState[1] == NONE) {
-            goToAruco(tag.value(), 1);
-        } else if (pinceState[2] == NONE) {
-            goToAruco(tag.value(), 2);
-        } else if (pinceState[0] == NONE) {
-            goToAruco(tag.value(), 0);
-        }
-    }
-
-    this->go(1000, 250);
-    awaitRobotIdle();
-
-    this->rotate(PI);
-    awaitRobotIdle();
-
-    pinceHavePurpleFlower.clear();
-    for (int i = 0; i < 3; i++) {
-        if (pinceState[i] == PURPLE_FLOWER) {
-            pinceHavePurpleFlower.push_back(i);
-        }
-    }
-
-    if (!pinceHavePurpleFlower.empty()) {
-        this->broadcastMessage("strat;servo_moteur;lever bras;1\n");
-
-        this->go(225, 225);
-        awaitRobotIdle();
-        this->rotate(PI);
-        awaitRobotIdle();
-
-        for (auto & toDrop : pinceHavePurpleFlower) {
-            toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(toDrop) + "\n";
-            this->broadcastMessage(toSend);
-            usleep(500'000);
-            pinceState[toDrop] = NONE;
-            toSend = "strat;servo_moteur;fermer pince;" + std::to_string(toDrop) + "\n";
-            this->broadcastMessage(toSend);
-            usleep(100'000);
-        }
-
-        this->go(this->robotPose.pos.x + 150, this->robotPose.pos.y);
-        awaitRobotIdle();
-    }
-
-
-    if (pinceHavePurpleFlower.size() < 3) {
-        this->go(762, 300);
-        awaitRobotIdle();
-
-        this->rotate(PI / 2);
-        awaitRobotIdle();
-
-        this->setSpeed(150);
-        this->go(762, 0);
-        usleep(1'000'000);
-
-
-        for (int i = 0; i < 3; i++) {
-            if (pinceState[i] == WHITE_FLOWER) {
-                toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(i) + "\n";
-                this->broadcastMessage(toSend);
+        switch (stratPatterns[i]) {
+            case TURN_SOLAR_PANNEL_1:
+                goAndTurnSolarPanel(TURN_SOLAR_PANNEL_1);
+                break;
+            case TURN_SOLAR_PANNEL_2:
+                goAndTurnSolarPanel(TURN_SOLAR_PANNEL_2);
+                break;
+            case TURN_SOLAR_PANNEL_3:
+                goAndTurnSolarPanel(TURN_SOLAR_PANNEL_3);
+                break;
+            case TAKE_FLOWER_TOP:
+                findAndGoFlower(TAKE_FLOWER_TOP);
+               break;
+            case TAKE_FLOWER_BOTTOM:
+                findAndGoFlower(TAKE_FLOWER_BOTTOM);
+                break;
+            case GO_END:
+                goEnd();
+                break;
+            case DROP_PURPLE_FLOWER:
+                dropPurpleFlowers();
+                break;
+            case DROP_WHITE_FLOWER_J1:
+                dropWhiteFlowers(DROP_WHITE_FLOWER_J1);
+                break;
+            case DROP_WHITE_FLOWER_J2:
+                dropWhiteFlowers(DROP_WHITE_FLOWER_J2);
+                break;
+            case GET_LIDAR_POS:
+                getLidarPos();
                 usleep(200'000);
-                pinceState[i] = NONE;
-                toSend = "strat;servo_moteur;fermer pince;" + std::to_string(i) + "\n";
-                this->broadcastMessage(toSend);
-                usleep(100'000);
-            }
-
-            this->setSpeed(200);
-
-            this->go(this->robotPose.pos.x, this->robotPose.pos.y + 350);
-            awaitRobotIdle();
+                break;
+            case CHECKPOINT_MIDDLE:
+                checkpoint(CHECKPOINT_MIDDLE);
+                break;
+            case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER:
+                checkpoint(CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER);
+                break;
+            case DROP_FLOWER_J1:
+                dropJardiniereFlowers(DROP_FLOWER_J1);
+                break;
+            case DROP_FLOWER_J2:
+                dropJardiniereFlowers(DROP_FLOWER_J2);
+                break;
+            case TAKE_3_PLANT_TOP_1:
+                go3Plants(TAKE_3_PLANT_TOP_1);
+                break;
+            case TAKE_3_PLANT_BOTTOM_1:
+                go3Plants(TAKE_3_PLANT_BOTTOM_1);
+                break;
+            case TAKE_3_PLANT_TOP_2:
+                go3Plants(TAKE_3_PLANT_TOP_2);
+                break;
+            case TAKE_3_PLANT_BOTTOM_2:
+                go3Plants(TAKE_3_PLANT_BOTTOM_2);
+                break;
+            case REMOVE_POT_J2:
+                removePot(REMOVE_POT_J2);
+                break;
+            case DROP_FLOWER_BASE_1:
+                dropBaseFlowers(DROP_FLOWER_BASE_1);
+                break;
+            case DROP_FLOWER_BASE_2:
+                dropBaseFlowers(DROP_FLOWER_BASE_2);
+                break;
+            case SLEEP_1S:
+                usleep(1'000'000);
+                break;
+            case SLEEP_5S:
+                usleep(5'000'000);
+                break;
+            case SLEEP_10S:
+                usleep(10'000'000);
+                break;
+            case ROTATE_0:
+                this->rotate(0);
+                if (awaitRobotIdle() < 0) return;
+                break;
+            case ROTATE_270:
+                this->rotate(-PI/2);
+                if (awaitRobotIdle() < 0) return;
+                break;
         }
+
+        whereAmI++;
     }
-
-
-    this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y);
-    awaitRobotIdle();
-
-    this->rotate(this->endRobotPose.theta);
-    awaitRobotIdle();
-
-    this->broadcastMessage("strat;servo_moteur;clear;1");
-}
-
-
-void TCPServer::startGameYellowTeam() {
-    this->broadcastMessage("strat;arduino;speed;200\n");
-    this->broadcastMessage("strat;servo_moteur;check panneau;7\n");
-    usleep(100'000);
-    std::string toSend = "strat;arduino;go;";
-    this->broadcastMessage(toSend);
-
-
-    toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->endRobotPose.pos.y)) + "\n";
-    this->broadcastMessage(toSend);
-    awaitRobotIdle();
-    toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>(this->endRobotPose.theta * 100));
-    this->broadcastMessage("strat;arduino;speed;150\n");
-    this->broadcastMessage(toSend);
-    awaitRobotIdle();
-    this->broadcastMessage("strat;arduino;speed;200\n");
-
-    this->broadcastMessage("strat;servo_moteur;clear;1");
 }
 
 void TCPServer::startGameTest() {
@@ -758,7 +549,6 @@ void TCPServer::startGameTest() {
     this->broadcastMessage("strat;servo_moteur;fermer pince;1\n");
     this->broadcastMessage("strat;servo_moteur;fermer pince;2\n");
     this->broadcastMessage("strat;servo_moteur;ouvrir pince;0\n");
-    // TODO set to 200 when the robot is ready
     this->broadcastMessage("strat;arduino;speed;200\n");
 
     arucoTags.clear();
@@ -800,7 +590,7 @@ void TCPServer::startGameTest() {
 
     // pi/4
     this->broadcastMessage("strat;arduino;angle;314\n");
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
     // ReSharper disable once CppDFAUnreachableCode
 
@@ -842,7 +632,7 @@ void TCPServer::startGameTest() {
     }
 
     this->broadcastMessage("strat;arduino;angle;157\n");
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
     // this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
 
@@ -888,10 +678,10 @@ void TCPServer::startGameTest() {
     std::string toSend = "strat;arduino;go;762,300\n";
     this->broadcastMessage(toSend);
     usleep(200'000);
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
     this->broadcastMessage("strat;arduino;angle;157\n");
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
     this->broadcastMessage("strat;arduino;speed;150\n");
     this->broadcastMessage("strat;arduino;go;762,0\n");
@@ -913,11 +703,11 @@ void TCPServer::startGameTest() {
 
     toSend = "strat;arduino;go;" + std::to_string(static_cast<int>(this->endRobotPose.pos.x)) + "," + std::to_string(static_cast<int>(this->endRobotPose.pos.y)) + "\n";
     this->broadcastMessage(toSend);
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
     toSend = "strat;arduino;angle;" + std::to_string(static_cast<int>(this->endRobotPose.theta * 100)) + "\n";
     this->broadcastMessage(toSend);
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
     // toSend = "start;arduino;angle;" + std::to_string(this->endRobotPose.theta * 100) + "\n";
     // this->broadcastMessage(toSend);
@@ -929,112 +719,125 @@ void TCPServer::startGameTest() {
 
 
 void TCPServer::goToAruco(const ArucoTag &arucoTag, const int pince) {
-    double robotPosX = this->robotPose.pos.x;
-    double robotPosY = this->robotPose.pos.y;
+
+    auto [robotPosX, robotPosY] = this->robotPose.pos;
+
     double theta = this->robotPose.theta;
     double decalage;
-    double rotate;
     if (pince < 0 || pince > 2) {
         return;
     }
 
     switch (pince) {
         case 0:
-            decalage = 60;
-            rotate = -0.07;
+            decalage = 75;
             break;
         case 1:
             decalage = 0;
-            rotate = 0;
             break;
         case 2:
-            decalage = -60;
-            rotate = 0.07;
+            decalage = -75;
             break;
         default:
             decalage = 0;
-            rotate = 0;
             break;
     }
 
-    this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
-    std::string toSend = "strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n";
+    this->baisserBras();
+    this->openPince(pince);
 
-    this->broadcastMessage(toSend);
-
-    double xPrime = arucoTag.pos()[0];
-    double yPrime = arucoTag.pos()[1];
+    auto [xPrime, yPrime] = arucoTag.pos();
     double roll = arucoTag.rot()[1];
 
-    auto centerPlantX = (20 * std::cos(roll)) + xPrime/* - 50*/;
+    auto centerPlantX = (20 * std::cos(roll)) + xPrime - 20 + (decalage / 10);
     auto centerPlantY = (-20 * std::sin(roll)) + yPrime + decalage;
 
     double thetaPrime = std::atan2(centerPlantY, centerPlantX);
 
-    this->rotate(this->robotPose.theta + rotate - thetaPrime);
-    awaitRobotIdle();
+    this->setSpeed(200);
+
+    this->rotate(this->robotPose.theta /*+ rotate*/ - thetaPrime);
+    if (awaitRobotIdle() < 0) return;
 
     double robotPosForPotX = (centerPlantX * std::cos(theta) + centerPlantY * std::sin(theta)) + robotPosX;
     double robotPosForPotY = (-centerPlantX * std::sin(theta) + centerPlantY * std::cos(theta)) + robotPosY;
 
-    int previousSpeed = this->speed;
 
     this->transit(robotPosForPotX, robotPosForPotY, 130);
-    awaitRobotIdle();
+    if (awaitRobotIdle() < 0) return;
 
-    toSend = "strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n";
-    this->broadcastMessage(toSend);
+    this->closePince(pince);
     usleep(500'000);
-    this->setSpeed(previousSpeed);
+    this->setSpeed(200);
     pinceState[pince] = TCPUtils::startWith(arucoTag.name(), "Purple_flower") ? PURPLE_FLOWER : WHITE_FLOWER;
+    this->transportBras();
 }
 
 void TCPServer::askArduinoPos() {
-    ClientTCP arduino;
     for (const auto & client : clients) {
         if (client.name == "arduino") {
-            arduino = client;
+            this->arduinoSocket = client.socket;
             break;
         }
     }
 
-    if (arduino.socket == -1) {
+    if (this->arduinoSocket == -1) {
         return;
     }
 
     while (!this->_shouldStop) {
-        this->sendToClient("strat;arduino;get pos;1\n", arduino.socket);
-        usleep(200'000);
+        this->sendToClient("strat;arduino;get pos;1\n", this->arduinoSocket);
+        usleep(20'000);
     }
 }
 
-void TCPServer::awaitRobotIdle() {
+int TCPServer::awaitRobotIdle() {
     isRobotIdle = 0;
     int timeout = 0;
     // ReSharper disable once CppDFAConstantConditions
     // ReSharper disable once CppDFAEndlessLoop
-    usleep(200'000);
-    while (isRobotIdle < 3) {
-        usleep(200'000);
-        this->broadcastMessage("strat;arduino;get state;1\n");
+    usleep(50'000);
+    while (isRobotIdle < 2) {
+        usleep(50'000);
+        this->sendToClient("strat;arduino;get state;1\n", this->arduinoSocket);
+        if (stopEmergency) {
+            while (stopEmergency) {
+                stopEmergency = false;
+                usleep(300'000);
+            }
+            this->broadcastMessage(lastArduinoCommand);
+            awaitRobotIdle();
+        }
+        if (gameStarted) {
+            auto time = std::chrono::system_clock::now();
+            if (time - gameStart > std::chrono::seconds(87)) {
+                return -1;
+            }
+        }
         timeout++;
-        if (timeout > 30) {
+        if (timeout > 80) {
             this->broadcastMessage("strat;arduino;clear;1");
+            return 1;
         }
     }
+    return 0;
 }
 
-void TCPServer::handleArucoTag(ArucoTag &tag) {
-    if (!TCPUtils::endWith(tag.name(), "flower")) {
+void TCPServer::handleArucoTag(const ArucoTag &tag) {
+    if (!TCPUtils::contains(tag.name(), "flower")) {
+        return;
+    }
+
+    auto rotArray = tag.rot();
+
+    if (rotArray[2] > 0.3 && rotArray[2] < -0.3 && rotArray[0] > 3 && rotArray[0] < 2.5) {
         return;
     }
 
     for (auto& t : arucoTags) {
         if (tag.id() == t.id()) {
-            float tPosX = t.pos()[0];
-            float tPosY = t.pos()[1];
-            float tagPosX = tag.pos()[0];
-            float tagPosY = tag.pos()[1];
+            auto [tPosX, tPosY] = t.pos();
+            auto [tagPosX, tagPosY] = tag.pos();
             if (tagPosX > tPosX - 10 && tagPosX < tPosX + 10 && tagPosY > tPosY - 10 && tagPosY < tPosY + 10) {
                 t.find();
                 return;
@@ -1045,9 +848,8 @@ void TCPServer::handleArucoTag(ArucoTag &tag) {
     this->arucoTags.push_back(tag);
 }
 
-std::optional<ArucoTag> TCPServer::getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY,
-                                                      float borneMaxY) {
-    // use optional
+std::optional<ArucoTag> TCPServer::getBiggestArucoTag(const float borneMinX, const float borneMaxX, const float borneMinY,
+                                                      const float borneMaxY) {
     bool found = false;
     ArucoTag biggestTag = ArucoTag();
     for (const auto & tag : arucoTags) {
@@ -1060,38 +862,1007 @@ std::optional<ArucoTag> TCPServer::getBiggestArucoTag(float borneMinX, float bor
     return found ? std::optional(biggestTag) : std::nullopt;
 }
 
-void TCPServer::startTestAruco(int pince) {
 
-    this->arucoTags.clear();
-    this->broadcastMessage("strat;aruco;get aruco;1\n");
-    for (int i = 0; i < 5; i++) {
-        usleep(500'000);
-        this->broadcastMessage("strat;aruco;get aruco;1\n");
+std::optional<ArucoTag> TCPServer::getMostCenteredArucoTag(const float borneMinX, const float borneMaxX, const float borneMinY, const float borneMaxY) {
+    bool found = false;
+    ArucoTag mostCenteredTag = ArucoTag();
+    for (const auto & tag : arucoTags) {
+        if (tag.getNbFind() < 2) continue;
+
+        auto [tagX, tagY] = tag.pos();
+
+        if (!found) {
+            if (tagX > borneMinX && tagX < borneMaxX && tagY > borneMinY && tagY < borneMaxY) {
+                mostCenteredTag = tag;
+                found = true;
+            }
+        } else if (distanceToTag(tag) < distanceToTag(mostCenteredTag)) {
+            if (tagX > borneMinX && tagX < borneMaxX && tagY > borneMinY && tagY < borneMaxY) {
+                mostCenteredTag = tag;
+            }
+        }
     }
 
-    std::optional<ArucoTag> tag = getBiggestArucoTag(300, 1500, -600, 600);
+    return found ? std::optional(mostCenteredTag) : std::nullopt;
+}
+
+std::vector<PinceState> TCPServer::getNotFallenFlowers() const {
+    std::vector<PinceState> res = {FLOWER, FLOWER, FLOWER};
+    for (auto & tag : arucoTags) {
+        if (TCPUtils::endWith(tag.name(), "flower") && tag.getNbFind() >= 1) {
+            auto angle = tag.rot()[0];
+            auto xPos = tag.pos()[0];
+            auto yPos = tag.pos()[1];
+
+            if (xPos > 700) continue;
+
+            if (yPos > 70 && yPos < 200) {
+                if (angle > 2.7f && angle < -2.f) {
+                    res[2] = NONE;
+                }
+                else {
+                    res[2] = (TCPUtils::contains(tag.name(), "White") ? WHITE_FLOWER : PURPLE_FLOWER);
+                }
+            }
+            else if (yPos < -70 && yPos > -200) {
+                if (angle > 2.7f && angle < -2.f) {
+                    res[0] = NONE;
+                }
+                else {
+                    res[0] = (TCPUtils::contains(tag.name(), "White") ? WHITE_FLOWER : PURPLE_FLOWER);
+                }
+            }
+            else {
+                if (angle > 2.7f && angle < -2.f) {
+                    res[1] = NONE;
+                }
+                else {
+                    res[1] = (TCPUtils::contains(tag.name(), "White") ? WHITE_FLOWER : PURPLE_FLOWER);
+                }
+            }
+        }
+    }
+    return res;
+}
+
+void TCPServer::handleEmergency(int distance, double angle) {
+    /*this->handleEmergencyFlag = true;
+
+    this->broadcastMessage("strat;arduino;clear;2\n");
+
+    // TODO handle here the emergency like wait for 2 second and then if emergency is again on, that means the robot of the other team do not move, if that go back otherwise continue
+    usleep(2'000'000);
+    // ReSharper disable once CppDFAConstantConditions
+    while (this->stopEmergency) {
+        // TODO here go back by twenty centimeter
+        // ReSharper disable once CppDFAUnreachableCode
+        this->broadcastMessage("strat;arduino;clear;3\n");
+
+        this->stopEmergency = false;
+
+        std::this_thread::sleep_for(std::chrono::seconds(1));*/
+
+        /*double newAngle = this->robotPose.theta + angle;
+        double newX = this->robotPose.pos.x + 200 * std::cos(newAngle);
+        double newY = this->robotPose.pos.y + 200 * std::sin(newAngle);
+        usleep(200'000);
+        this->go(newX, newY);
+        if (awaitRobotIdle() < 0) return;*/
+    /*}
+    this->broadcastMessage("strat;arduino;clear;4\n");
+
+    try {
+        this->gameThread.~thread();
+    } catch (const std::exception& ex) {
+        std::cout << ex.what() << std::endl;
+    }
+
+    this->gameStarted = false;
+
+    this->gameThread = std::thread([this]() { this->startGame(); });
+
+    this->gameThread.detach();
+
+    this->handleEmergencyFlag = false;*/
+}
+
+void TCPServer::startTestAruco(const int pince) {
+    this->arucoTags.clear();
+    std::optional<ArucoTag> tag = std::nullopt;
+
+    for (int i = 0; i < 5; i++) {
+        this->broadcastMessage("strat;aruco;get aruco;1\n");
+        usleep(220'000);
+    }
+    tag = getMostCenteredArucoTag(100, 800, -400, 400);
+
+    int timeout = 0;
+    while (!tag.has_value()) {
+        this->broadcastMessage("strat;aruco;get aruco;1\n");
+        usleep(220'000);
+        tag = getMostCenteredArucoTag(100, 800, -400, 400);
+
+        timeout++;
+        if (timeout > 5) {
+            break;
+        }
+    }
 
     if (tag.has_value()) {
         goToAruco(tag.value(), pince);
     }
 }
 
+void TCPServer::goEnd() {
+    this->setSpeed(200);
+    std::vector<std::array<int, 2>> checkponts;
+    if (this->robotPose.pos.y > 1000) {
+        if (team == BLUE) {
+            checkponts.emplace_back(std::array{500, 1700});
+        } else if (team == YELLOW) {
+            checkponts.emplace_back(std::array{2500, 1700});
+        }
+    }
+
+    for (const auto& checkpoint : checkponts) {
+        this->go(checkpoint);
+        if (awaitRobotIdle() < 0) return;
+    }
+
+    this->go(this->endRobotPose.pos.x, this->endRobotPose.pos.y);
+    if (awaitRobotIdle() < 0) return;
+    this->setSpeed(180);
+    this->rotate(this->endRobotPose.theta);
+    if (awaitRobotIdle() < 0) return;
+
+    this->baisserBras();
+
+    for (int i = 0 ; i < 3; i++) {
+        this->openPince(i);
+        usleep(50'000);
+    }
+    this->sendPoint(10);
+
+    this->broadcastMessage("strat;all;end;1");
+}
+
+void TCPServer::findAndGoFlower(const StratPattern sp) {
+    this->setSpeed(200);
+    if (team == BLUE) {
+        if (sp == TAKE_FLOWER_TOP) {
+            this->go(500, 700);
+            if (awaitRobotIdle() < 0) return;
+
+            this->rotate(0);
+            if (awaitRobotIdle() < 0) return;
+        }
+        else if (sp == TAKE_FLOWER_BOTTOM) {
+            this->go(500, 1300);
+            if (awaitRobotIdle() < 0) return;
+
+            this->rotate(0);
+            if (awaitRobotIdle() < 0) return;
+        } else {
+            return;
+        }
+    } else if (team == YELLOW) {
+        if (sp == TAKE_FLOWER_TOP) {
+            this->go(1500,  700);
+            if (awaitRobotIdle() < 0) return;
+
+            this->rotate(-PI);
+            if (awaitRobotIdle() < 0) return;
+        }
+        else if (sp == TAKE_FLOWER_BOTTOM) {
+            this->go(1500, 1300);
+            if (awaitRobotIdle() < 0) return;
+
+            this->rotate(-PI);
+            if (awaitRobotIdle() < 0) return;
+        } else {
+            return;
+        }
+    } else {
+        return;
+    }
+
+    this->arucoTags.clear();
+    std::optional<ArucoTag> tag = std::nullopt;
+
+    for (int i = 0; i < 5; i++) {
+        this->broadcastMessage("strat;aruco;get aruco;1\n");
+        usleep(110'000);
+    }
+    tag = getMostCenteredArucoTag(300, 700, -200, 200);
+
+    int timeout = 0;
+    while (!tag.has_value()) {
+        this->broadcastMessage("strat;aruco;get aruco;1\n");
+        usleep(110'000);
+        tag = getMostCenteredArucoTag(300, 700, -200, 200);
+
+        timeout++;
+        if (timeout > 3) {
+            break;
+        }
+    }
+
+    if (tag.has_value()) {
+        /*if (pinceState[1] == NONE) {
+            goToAruco(tag.value(), 1);
+        } else if (pinceState[2] == NONE) {
+            goToAruco(tag.value(), 2);
+        } else if (pinceState[0] == NONE) {
+            goToAruco(tag.value(), 0);
+        }*/
+        if (pinceState[0] == NONE) {
+            goToAruco(tag.value(), 0);
+        } else if (pinceState[2] == NONE) {
+            goToAruco(tag.value(), 2);
+        } else if (pinceState[1] == NONE) {
+            goToAruco(tag.value(), 1);
+        }
+    }
+}
+
+void TCPServer::dropPurpleFlowers() {
+    std::vector<int> pinceHavePurpleFlower;
+
+    pinceHavePurpleFlower.reserve(3);
+
+    for (int i = 0; i < 3; i++) {
+        switch (pinceState[i]) {
+            case PURPLE_FLOWER:
+                pinceHavePurpleFlower.push_back(i);
+                break;
+            default:
+                break;
+        }
+    }
+
+    std::array<int, 2> purpleDropPosition{};
+    if (team == BLUE) {
+        purpleDropPosition = {200, 400};
+    } else if (team == YELLOW) {
+        purpleDropPosition = {1800, 400};
+    }
+
+    this->setSpeed(200);
+
+    if (!pinceHavePurpleFlower.empty()) {
+        this->go(purpleDropPosition);
+        if (awaitRobotIdle() < 0) return;
+
+        this->setSpeed(150);
+
+        this->rotate(PI / 2);
+        if (awaitRobotIdle() < 0) return;
+
+        this->baisserBras();
+
+        for (const auto & toDrop : pinceHavePurpleFlower) {
+            this->openPince(toDrop);
+            usleep(200'000);
+
+            this->go(this->robotPose.pos.x, this->robotPose.pos.y - 150);
+            if (awaitRobotIdle() < 0) return;
+
+            this->go(this->robotPose.pos.x, this->robotPose.pos.y + 150);
+            if (awaitRobotIdle() < 0) return;
+
+            pinceState[toDrop] = NONE;
+            this->closePince(toDrop);
+            usleep(200'000);
+
+            this->sendPoint(3);
+        }
+    }
+
+    this->transportBras();
+
+    this->setSpeed(200);
+}
+
+void TCPServer::dropWhiteFlowers(const StratPattern sp) {
+    std::vector<int> pinceHaveWhiteFlower;
+
+    pinceHaveWhiteFlower.reserve(3);
+
+    for (int i = 0; i < 3; i++) {
+        switch (pinceState[i]) {
+            case WHITE_FLOWER:
+                pinceHaveWhiteFlower.push_back(i);
+            break;
+            default:
+                break;
+        }
+    }
+
+    if (pinceHaveWhiteFlower.empty()) {
+        return;
+    }
+
+    this->setSpeed(200);
+
+    std::array<int, 2> whiteDropSetup{};
+    std::array<int, 2> whiteDropPosition{};
+    double angle = PI / 2;
+    if (team == BLUE) {
+        if (sp == DROP_WHITE_FLOWER_J1) {
+            whiteDropSetup = std::array{762, 300};
+            whiteDropPosition = std::array{762, 0};
+            angle = PI / 2;
+        } else if (sp == DROP_WHITE_FLOWER_J2) {
+            whiteDropSetup = std::array{300, 612};
+            whiteDropPosition = std::array{0, 612};
+            angle = -PI;
+        }
+    } else if (team == YELLOW) {
+        if (sp == DROP_WHITE_FLOWER_J1) {
+            whiteDropSetup = std::array{2237, 300};
+            whiteDropPosition = std::array{2237, 0};
+            angle = PI / 2;
+        } else if (sp == DROP_WHITE_FLOWER_J2) {
+            whiteDropSetup = std::array{1700, 612};
+            whiteDropPosition = std::array{0, 612};
+            angle = 0;
+
+        }
+    }
+
+    this->setSpeed(200);
+
+    this->go(whiteDropSetup);
+    if (awaitRobotIdle() < 0) return;
+
+
+    this->rotate(angle);
+    if (awaitRobotIdle() < 0) return;
+
+    this->leverBras();
+    usleep(500'000);
+
+    this->setSpeed(130);
+
+    this->go(whiteDropPosition);
+    usleep(2'000'000);
+
+    for (int i = 0; i < 3; i++) {
+        if (pinceState[i] == WHITE_FLOWER) {
+            this->openPince(i);
+            usleep(1'000'000);
+
+            pinceState[i] = NONE;
+            this->closePince(i);
+            usleep(100'000);
+
+            this->sendPoint(4);
+        }
+    }
+
+    for (int i = 0; i < 3; i++) {
+        this->middlePince(i);
+    }
+    usleep(1'000'000);
+
+    this->setSpeed(200);
+
+    this->go(whiteDropSetup);
+    if (awaitRobotIdle() < 0) return;
+
+    for (int i = 0; i < 3; i++) {
+        this->closePince(i);
+    }
+
+    this->transportBras();
+}
+
+void TCPServer::goAndTurnSolarPanel(const StratPattern sp) {
+    this->setSpeed(170);
+    if (team == BLUE) {
+        switch (sp) {
+            case TURN_SOLAR_PANNEL_1:
+                this->go(250, 1800);
+                if (awaitRobotIdle() < 0) return;
+
+                this->rotate(0);
+                if (awaitRobotIdle() < 0) return;
+
+                this->checkPanneau(7);
+                usleep(300'000);
+                this->uncheckPanneau(7);
+                break;
+            case TURN_SOLAR_PANNEL_2:
+                this->go(460, 1800);
+                if (awaitRobotIdle() < 0) return;
+
+                this->rotate(0);
+                if (awaitRobotIdle() < 0) return;
+
+                this->checkPanneau(7);
+                usleep(300'000);
+                this->uncheckPanneau(7);
+                break;
+            case TURN_SOLAR_PANNEL_3:
+                this->go(690, 1800);
+                if (awaitRobotIdle() < 0) return;
+
+                this->rotate(0);
+                if (awaitRobotIdle() < 0) return;
+
+                this->checkPanneau(7);
+                usleep(300'000);
+                this->uncheckPanneau(7);
+                break;
+            default:
+                break;
+        }
+    } else if (team == YELLOW) {
+        switch (sp) {
+            case TURN_SOLAR_PANNEL_1:
+                this->go(2750, 1800);
+                if (awaitRobotIdle() < 0) return;
+
+                this->rotate(PI);
+                if (awaitRobotIdle() < 0) return;
+
+                this->checkPanneau(6);
+                usleep(300'000);
+                this->uncheckPanneau(6);
+                break;
+            case TURN_SOLAR_PANNEL_2:
+                this->go(2540, 1800);
+                if (awaitRobotIdle() < 0) return;
+
+                this->rotate(PI);
+                if (awaitRobotIdle() < 0) return;
+
+                this->checkPanneau(6);
+                usleep(300'000);
+                this->uncheckPanneau(6);
+                break;
+            case TURN_SOLAR_PANNEL_3:
+                this->go(2310, 1800);
+                if (awaitRobotIdle() < 0) return;
+
+                this->rotate(PI);
+                if (awaitRobotIdle() < 0) return;
+
+                this->checkPanneau(6);
+                usleep(300'000);
+                this->uncheckPanneau(6);
+                break;
+            default:
+                break;
+        }
+    }
+
+    this->sendPoint(5);
+}
+
+void TCPServer::dropJardiniereFlowers(const StratPattern sp) {
+
+    std::array<int, 2> whiteDropSetup{};
+    std::array<int, 2> whiteDropPosition{};
+    double angle = PI / 2;
+    if (team == BLUE) {
+        if (sp == DROP_FLOWER_J1) {
+            whiteDropSetup = std::array{755, 300};
+            whiteDropPosition = std::array{755, 0};
+            angle = PI / 2;
+        } else if (sp == DROP_FLOWER_J2) {
+            whiteDropSetup = std::array{300, 600};
+            whiteDropPosition = std::array{0, 600};
+            angle = -PI;
+        }
+    } else if (team == YELLOW) {
+        if (sp == DROP_FLOWER_J1) {
+            whiteDropSetup = std::array{2230, 300};
+            whiteDropPosition = std::array{2230, 0};
+            angle = PI / 2;
+        } else if (sp == DROP_FLOWER_J2) {
+            whiteDropSetup = std::array{2700, 600};
+            whiteDropPosition = std::array{3000, 600};
+            angle = 0;
+        }
+    }
+
+    this->setMaxSpeed();
+
+    this->transit(whiteDropSetup, 170);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setMaxSpeed();
+
+    this->rotate(angle);
+    if (awaitRobotIdle() < 0) return;
+
+    this->leverBras();
+
+    this->setSpeed(130);
+
+    this->go(whiteDropPosition);
+    usleep(2'000'000);
+
+    if (pinceState[0] != NONE) {
+        this->fullyOpenPince(0);
+        pinceState[0] = NONE;
+    }
+    if (pinceState[2] != NONE) {
+        this->fullyOpenPince(2);
+        pinceState[2] = NONE;
+    }
+
+    usleep(500'000);
+
+    this->closePince(0);
+    this->closePince(2);
+
+    usleep(100'000);
+
+    if (pinceState[1] != NONE) {
+        this->fullyOpenPince(1);
+        pinceState[1] = NONE;
+    }
+
+    this->sendPoint(3+1);
+    this->sendPoint(3+1);
+
+    usleep(500'000);
+
+    this->openPince(0);
+    this->openPince(1);
+    this->openPince(2);
+
+    this->setMaxSpeed();
+
+    this->go(whiteDropSetup);
+    if (awaitRobotIdle() < 0) return;
+
+    this->transportBras();
+}
+
+void TCPServer::dropBaseFlowers(StratPattern sp) {
+    std::array<int, 2> dropPosition{};
+    double angle;
+    double angleStart;
+    float distance;
+
+    if (team == BLUE) {
+        if (sp == DROP_FLOWER_BASE_1) {
+            dropPosition = {300, 400};
+            angle = PI / 2;
+            angleStart = 3 * PI / 4;
+            distance = 150;
+        }
+        else if (sp == DROP_FLOWER_BASE_2) {
+            dropPosition = {300, 1600};
+            angle = -PI / 2;
+            angleStart = -3 * PI / 4;
+            distance = -150;
+        }
+        else {
+            return;
+        }
+    }
+    else if (team == YELLOW) {
+        if (sp == DROP_FLOWER_BASE_1) {
+            dropPosition = {2700, 400};
+            angle = PI / 2;
+            angleStart = PI / 4;
+            distance = 150;
+        }
+        else if (sp == DROP_FLOWER_BASE_2) {
+            dropPosition = {2700, 1600};
+            angle = -PI / 2;
+            angleStart = -PI / 4;
+            distance = -150;
+        } else {
+            return;
+        }
+    }
+    else {
+        return;
+    }
+
+    this->setMaxSpeed();
+
+    this->rotate(angleStart);
+    if (awaitRobotIdle() < 0) return;
+
+    this->transit(dropPosition, 170);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setMaxSpeed();
+
+    this->go(dropPosition);
+    if (awaitRobotIdle() < 0) return;
+
+    this->rotate(angle);
+    if (awaitRobotIdle() < 0) return;
+
+    this->baisserBras();
+
+    for (int i = 0; i < 3; i++) {
+        this->openPince(i);
+    }
+
+    this->go(this->robotPose.pos.x, this->robotPose.pos.y - distance);
+    if (awaitRobotIdle() < 0) return;
+
+    this->go(this->robotPose.pos.x, this->robotPose.pos.y + distance);
+    if (awaitRobotIdle() < 0) return;
+
+    bool detectedPurple = false;
+
+    for (int i = 0; i < 3; i++) {
+        if (pinceState[i] == PURPLE_FLOWER) {
+            detectedPurple = true;
+            this->sendPoint(3);
+        }
+
+        pinceState[i] = NONE;
+        this->closePince(i);
+    }
+
+    if (!detectedPurple) {
+        this->sendPoint(3);
+    }
+
+    this->transportBras();
+
+    this->setMaxSpeed();
+}
+
+void TCPServer::go3Plants(const StratPattern sp) {
+    std::array<int, 2> plantPosition{};
+    double direction;
+
+    double angle;
+    if (team == BLUE) {
+        angle = 0;
+        direction = 1;
+        if (sp == TAKE_3_PLANT_TOP_1) {
+            plantPosition = {900, 700};
+        }
+        else if (sp == TAKE_3_PLANT_TOP_2) {
+            plantPosition = {1100, 700};
+        }
+        else if (sp == TAKE_3_PLANT_BOTTOM_1) {
+            plantPosition = {900, 1300};
+        }
+        else if (sp == TAKE_3_PLANT_BOTTOM_2) {
+            plantPosition = {1100, 1300};
+        }
+        else {
+            return;
+        }
+    }
+    else if (team == YELLOW) {
+        angle = -PI;
+        direction = -1;
+
+        if (sp == TAKE_3_PLANT_TOP_1) {
+            plantPosition = {2100, 700};
+        }
+        else if (sp == TAKE_3_PLANT_TOP_2) {
+            plantPosition = {1900, 700};
+        }
+        else if (sp == TAKE_3_PLANT_BOTTOM_1) {
+            plantPosition = {2100, 1300};
+        }
+        else if (sp == TAKE_3_PLANT_BOTTOM_2) {
+            plantPosition = {1900, 1300};
+        }
+        else {
+            return;
+        }
+    }
+    else {
+        return;
+    }
+
+    this->setMaxSpeed();
+
+    this->transit(plantPosition[0]-(600*direction), plantPosition[1], 150);
+    // this->go(plantPosition[0]-500, plantPosition[1]);
+    if (awaitRobotIdle() < 0) return;
+
+    this->baisserBras();
+
+    this->setSpeed(170);
+
+    this->rotate(angle);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setMaxSpeed();
+
+    this->transit(plantPosition[0]-(400*direction), plantPosition[1], 150);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setSpeed(170);
+
+    this->rotate(angle);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setMaxSpeed();
+
+    // this->arucoTags.clear();
+    // for (int i = 0; i < 5; i++) {
+        // this->broadcastMessage("strat;aruco;get aruco;1\n");
+        // usleep(110'000);
+    // }
+
+    // std::vector<PinceState> pinceCanTakeFLower = getNotFallenFlowers();
+
+    for (int i = 0; i < 3; i++) {
+        this->openPince(i);
+    }
+    usleep(200'000);
+
+    this->transit(plantPosition[0], this->robotPose.pos.y, 130);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setMaxSpeed();
+
+    usleep(500'000);
+
+    for (int i = 0; i < 3; i++) {
+        this->closePince(i);
+        pinceState[i] = FLOWER;
+    }
+    usleep(500'000);
+
+    this->rotate(angle);
+    if (awaitRobotIdle() < 0) return;
+
+    this->setSpeed(160);
+
+    this->go(this->robotPose.pos.x - (200 * direction), this->robotPose.pos.y);
+    if (awaitRobotIdle() < 0) return;
+
+    for (int i = 0; i < 3; i++) {
+        this->openPince(i);
+    }
+    usleep(200'000);
+
+    this->setSpeed(150);
+    this->go(this->robotPose.pos.x + (75 * direction), this->robotPose.pos.y);
+    if (awaitRobotIdle() < 0) return;
+
+    for (int i = 0; i < 3; i++) {
+        this->closePince(i);
+    }
+    usleep(500'000);
+
+    this->transportBras();
+}
+
+void TCPServer::removePot(StratPattern sp) {
+    this->setMaxSpeed();
+    if (team == BLUE) {
+        if (sp == REMOVE_POT_J2) {
+            this->transit(250, 1100, 150);
+            if (awaitRobotIdle() < 0) return;
+            this->setMaxSpeed();
+            this->go(210, 900);
+            if (awaitRobotIdle() < 0) return;
+            this->go(200, 400);
+            if (awaitRobotIdle() < 0) return;
+            this->go(250, 650);
+            if (awaitRobotIdle() < 0) return;
+        }
+    } else if (team == YELLOW) {
+        if (sp == REMOVE_POT_J2) {
+            this->transit(2750, 1100, 150);
+            if (awaitRobotIdle() < 0) return;
+            this->setMaxSpeed();
+            this->go(2780, 900);
+            if (awaitRobotIdle() < 0) return;
+            this->go(2780, 400);
+            if (awaitRobotIdle() < 0) return;
+            this->go(2750, 650);
+            if (awaitRobotIdle() < 0) return;
+        }
+    }
+}
+
+void TCPServer::getLidarPos() {
+
+    this->lidarGetPosTimeout = 0;
+
+    this->broadcastMessage("strat;arduino;clear;1\n");
+
+    usleep(1'000'000);
+
+    this->setPosition(this->robotPose, lidarSocket);
+
+    usleep(100'000);
+
+    awaitForLidar = true;
+
+    this->askLidarPosition();
+
+    int timeout = 0;
+
+    // ReSharper disable once CppDFAConstantConditions
+    // ReSharper disable once CppDFAEndlessLoop
+    while (awaitForLidar) {
+        usleep(50'000);
+        timeout++;
+        if (timeout > 100) {
+            break;
+        }
+    }
+
+    // ReSharper disable once CppDFAUnreachableCode
+    std::cout << lidarCalculatePos.pos.x << " " << lidarCalculatePos.pos.y << " " << lidarCalculatePos.theta << std::endl;
+
+}
+
+void TCPServer::checkpoint(const StratPattern sp) {
+    this->setMaxSpeed();
+    if (team == BLUE) {
+        switch (sp) {
+            case CHECKPOINT_MIDDLE:
+                this->go(500, 1500);
+                if (awaitRobotIdle() < 0) return;
+                break;
+            case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER:
+                this->go(800, 1800);
+                if (awaitRobotIdle() < 0) return;
+                this->go(500, 1700);
+                usleep(500'000);
+                break;
+            default:
+                break;
+        }
+    } else if (team == YELLOW) {
+        switch (sp) {
+            case CHECKPOINT_MIDDLE:
+                this->go(2500, 1500);
+                if (awaitRobotIdle() < 0) return;
+                break;
+            case CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER:
+                this->go(2200, 1800);
+                if (awaitRobotIdle() < 0) return;
+                this->go(2500, 1700);
+                usleep(500'000);
+                break;
+            default:
+                break;
+        }
+    }
+}
+
 template<class X, class Y>
 void TCPServer::go(X x, Y y) {
+    lastArduinoCommand = "strat;arduino;go;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "\n";
     this->broadcastMessage("strat;arduino;go;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "\n");
 }
 
 template<class X>
-void TCPServer::rotate(X angle) {
-    this->broadcastMessage("strat;arduino;angle" + std::to_string(static_cast<int>(angle * 100)) + "\n");
+void TCPServer::go(std::array<X, 2> data) {
+    lastArduinoCommand = "strat;arduino;go;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "\n";
+    this->broadcastMessage("strat;arduino;go;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "\n");
 }
 
-void TCPServer::setSpeed(int speed) {
+template<class X>
+void TCPServer::rotate(X angle) {
+    lastArduinoCommand = "strat;arduino;angle;" + std::to_string(static_cast<int>(angle * 100)) + "\n";
+    this->broadcastMessage("strat;arduino;angle;" + std::to_string(static_cast<int>(angle * 100)) + "\n");
+}
+
+void TCPServer::setSpeed(const int speed) {
     this->broadcastMessage("strat;arduino;speed;" + std::to_string(speed) + "\n");
     this->speed = speed;
 }
 
-template<class X, class Y>
-void TCPServer::transit(X x, Y y, int endSpeed) {
-    this->broadcastMessage("strat;arduino;transit" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "," + std::to_string(endSpeed) + "\n");
+void TCPServer::setMaxSpeed() {
+    this->setSpeed(MAX_SPEED);
+}
+
+void TCPServer::setMinSpeed() {
+    this->setSpeed(MIN_SPEED);
+}
+
+template<class X, class Y>
+void TCPServer::transit(X x, Y y, const int endSpeed) {
+    lastArduinoCommand = "strat;arduino;transit;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "," + std::to_string(endSpeed) + "\n";
+    this->broadcastMessage("strat;arduino;transit;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "," + std::to_string(endSpeed) + "\n");
+}
+
+template<class X>
+void TCPServer::transit(std::array<X, 2> data, const int endSpeed) {
+    lastArduinoCommand = "strat;arduino;transit;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "," + std::to_string(endSpeed) + "\n";
+    this->broadcastMessage("strat;arduino;transit;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "," + std::to_string(endSpeed) + "\n");
+}
+
+template<class X, class Y, class Z>
+void TCPServer::setPosition(X x, Y y, Z theta, const int clientSocket) {
+    if (clientSocket == -1) {
+        this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "," + std::to_string(static_cast<int>(theta * 100)) + "\n");
+    } else {
+        this->sendToClient("strat;all;set pos;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "," + std::to_string(static_cast<int>(theta * 100)) + "\n", clientSocket);
+    }
+}
+
+template<class X>
+void TCPServer::setPosition(std::array<X, 3> data, const int clientSocket) {
+    if (clientSocket == -1) {
+        this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "," + std::to_string(static_cast<int>(data[2] * 100)) + "\n");
+    } else {
+        this->sendToClient("strat;all;set pos;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "," + std::to_string(static_cast<int>(data[2] * 100)) + "\n", clientSocket);
+    }
+}
+
+void TCPServer::setPosition(const Position pos, const int clientSocket) {
+    if (clientSocket == -1) {
+        this->broadcastMessage("strat;all;set pos;" + std::to_string(static_cast<int>(pos.pos.x)) + "," + std::to_string(static_cast<int>(pos.pos.y)) + "," + std::to_string(static_cast<int>(pos.theta * 100)) + "\n");
+    } else {
+        this->sendToClient("strat;lidar;set pos;" + std::to_string(static_cast<int>(pos.pos.x)) + "," + std::to_string(static_cast<int>(pos.pos.y)) + "," + std::to_string(static_cast<int>(pos.theta * 100)) + "\n", clientSocket);
+    }
+}
+
+template<class X, class Y, class Z>
+void TCPServer::setPosition(X x, Y y, Z theta, const std::string &toSend) {
+    this->broadcastMessage("strat;" + toSend + ";set pos;" + std::to_string(static_cast<int>(x)) + "," + std::to_string(static_cast<int>(y)) + "," + std::to_string(static_cast<int>(theta * 100)) + "\n");
+}
+
+template<class X>
+void TCPServer::setPosition(std::array<X, 3> data, const std::string &toSend) {
+    this->broadcastMessage("strat;" + toSend + ";set pos;" + std::to_string(static_cast<int>(data[0])) + "," + std::to_string(static_cast<int>(data[1])) + "," + std::to_string(static_cast<int>(data[2] * 100)) + "\n");
+}
+
+void TCPServer::setPosition(const Position pos, const std::string &toSend) {
+    this->broadcastMessage("strat;" + toSend + ";set pos;" + std::to_string(static_cast<int>(pos.pos.x)) + "," + std::to_string(static_cast<int>(pos.pos.y)) + "," + std::to_string(static_cast<int>(pos.theta * 100)) + "\n");
+}
+
+void TCPServer::baisserBras() {
+    this->broadcastMessage("strat;servo_moteur;baisser bras;1\n");
+}
+
+void TCPServer::transportBras() {
+    this->broadcastMessage("strat;servo_moteur;transport bras;1\n");
+}
+
+void TCPServer::leverBras() {
+    this->broadcastMessage("strat;servo_moteur;lever bras;1\n");
+}
+
+void TCPServer::openPince(int pince) {
+    this->broadcastMessage("strat;servo_moteur;ouvrir pince;" + std::to_string(pince) + "\n");
+}
+
+void TCPServer::fullyOpenPince(int pince) {
+    this->broadcastMessage("strat;servo_moteur;ouvrir total pince;" + std::to_string(pince) + "\n");
+}
+
+void TCPServer::middlePince(int pince) {
+    this->broadcastMessage("strat;servo_moteur;middle pince;" + std::to_string(pince) + "\n");
+}
+
+void TCPServer::closePince(int pince) {
+    this->broadcastMessage("strat;servo_moteur;fermer pince;" + std::to_string(pince) + "\n");
+}
+
+void TCPServer::checkPanneau(int servo_moteur) {
+    this->broadcastMessage("strat;servo_moteur;check panneau;" + std::to_string(servo_moteur) + "\n");
+}
+
+void TCPServer::uncheckPanneau(int servo_moteur) {
+    this->broadcastMessage("strat;servo_moteur;uncheck panneau;" + std::to_string(servo_moteur) + "\n");
+}
+
+void TCPServer::askLidarPosition() {
+    this->broadcastMessage("start;lidar;get pos;1\n");
+}
+
+void TCPServer::sendPoint(int point) {
+    this->broadcastMessage("strat;ihm;add point;" + std::to_string(point) + "\n");
+}
+
+void TCPServer::setTeam(Team team) {
+    this->team = team;
+    this->broadcastMessage("strat;all;set team;" + std::to_string(team) + "\n");
 }
diff --git a/TCPServer.h b/TCPServer.h
index 4d7e491..2495415 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -6,27 +6,62 @@
 #include <unistd.h>
 #include <cstring>
 #include <thread>
+#include <utility>
 #include <vector>
 #include <algorithm>
 #include <atomic>
-#include <map>
-#include <cmath>
 #include <fstream>
 #include <optional>
 
 #include "utils.h"
 
+#define MAX_SPEED 200
+#define MIN_SPEED 150
+
 struct ClientTCP
 {
     std::string name;
     int socket = -1;
     bool isReady = false;
+
+    ClientTCP() = default;
+
+    explicit ClientTCP(std::string name, int socket = -1) : name(std::move(name)), socket(socket) {}
 };
 
 enum Team {
-    YELLOW,
     BLUE,
-    TEST
+    YELLOW,
+    TEST,
+};
+
+enum StratPattern {
+    TURN_SOLAR_PANNEL_1,
+    TURN_SOLAR_PANNEL_2,
+    TURN_SOLAR_PANNEL_3,
+    TAKE_FLOWER_BOTTOM,
+    TAKE_FLOWER_TOP,
+    DROP_PURPLE_FLOWER,
+    DROP_WHITE_FLOWER_J1,
+    DROP_WHITE_FLOWER_J2,
+    GO_END,
+    GET_LIDAR_POS,
+    CHECKPOINT_MIDDLE,
+    CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER,
+    TAKE_3_PLANT_BOTTOM_1,
+    TAKE_3_PLANT_BOTTOM_2,
+    TAKE_3_PLANT_TOP_1,
+    TAKE_3_PLANT_TOP_2,
+    DROP_FLOWER_J1,
+    DROP_FLOWER_J2,
+    REMOVE_POT_J2,
+    DROP_FLOWER_BASE_1,
+    DROP_FLOWER_BASE_2,
+    SLEEP_1S,
+    SLEEP_5S,
+    SLEEP_10S,
+    ROTATE_0,
+    ROTATE_270,
 };
 
 class TCPServer; // Forward declaration
@@ -55,9 +90,13 @@ private:
     std::atomic<bool> _shouldStop = false; // Flag to indicate if the server should stop
     std::vector<ClientTCP> clients; // Store connected clients
 
-    PinceState pinceState[3] = {NONE, NONE, NONE};
+    std::array<PinceState, 3> pinceState = {NONE, NONE, NONE};
     int isRobotIdle = 0;
 
+    bool gameStarted = false;
+
+    std::chrono::time_point<std::chrono::system_clock> gameStart;
+
     int speed = 0;
 
     struct Position {
@@ -66,15 +105,80 @@ private:
             float y;
         } pos;
         float theta;
-    } robotPose{};
+    };
 
+    Position robotPose{};
     Position initRobotPose{};
     Position endRobotPose{};
+    Position lidarCalculatePos{};
 
     std::vector<ArucoTag> arucoTags;
 
     Team team;
 
+    std::vector<StratPattern> stratPatterns = {
+        TURN_SOLAR_PANNEL_1,
+        TURN_SOLAR_PANNEL_2,
+        TURN_SOLAR_PANNEL_3,
+        CHECKPOINT_TRANSITION_SOLAR_PANEL_FLOWER,
+
+        /*TAKE_FLOWER_BOTTOM,
+        TAKE_FLOWER_BOTTOM,
+        TAKE_FLOWER_BOTTOM,*/
+
+        /*TAKE_3_PLANT_BOTTOM_1,
+        DROP_FLOWER_BASE_2,*/
+
+        TAKE_3_PLANT_BOTTOM_1,
+        GET_LIDAR_POS,
+        REMOVE_POT_J2,
+        DROP_FLOWER_J2,
+        ROTATE_270,
+
+        /*DROP_PURPLE_FLOWER,
+        DROP_WHITE_FLOWER_J1,*/
+        // GET_LIDAR_POS,
+        /*TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,*/
+
+        TAKE_3_PLANT_TOP_1,
+        GET_LIDAR_POS,
+        DROP_FLOWER_J1,
+
+        /*DROP_PURPLE_FLOWER,
+        DROP_WHITE_FLOWER_J2,*/
+        // GET_LIDAR_POS,
+        /*TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,
+        TAKE_FLOWER_TOP,*/
+        //TAKE_3_PLANT_TOP,
+        /*DROP_WHITE_FLOWER_J2,
+        DROP_PURPLE_FLOWER,*/
+
+        TAKE_3_PLANT_TOP_2,
+        DROP_FLOWER_BASE_1,
+
+        GO_END
+    };
+
+    // This is the index of the current pattern
+    int whereAmI = 0;
+
+    bool stopEmergency = false;
+    bool handleEmergencyFlag = false;
+
+    bool awaitForLidar = false;
+
+    std::thread gameThread;
+
+    int lidarSocket = -1;
+    int arduinoSocket = -1;
+
+    int lidarGetPosTimeout = 0;
+
+    std::string lastArduinoCommand{};
+
 public:
     explicit TCPServer(int port);
 
@@ -102,9 +206,7 @@ public:
 
     void checkIfAllClientsReady();
 
-    void startGameBlueTeam();
-
-    void startGameYellowTeam();
+    void startGame();
 
     void startGameTest();
 
@@ -114,25 +216,109 @@ public:
 
     [[nodiscard]] bool shouldStop() const;
 
-    void awaitRobotIdle();
+    int awaitRobotIdle();
 
-    void handleArucoTag(ArucoTag &tag);
+    void handleArucoTag(const ArucoTag &tag);
 
     std::optional<ArucoTag> getBiggestArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
 
+    std::optional<ArucoTag> getMostCenteredArucoTag(float borneMinX, float borneMaxX, float borneMinY, float borneMaxY);
+
+    std::vector<PinceState> getNotFallenFlowers() const;
+
+    void handleEmergency(int distance, double angle);
+
+    /*
+     * Start Strategy function
+    */
+    void goAndTurnSolarPanel(StratPattern sp);
+
+    void findAndGoFlower(StratPattern sp);
+
+    void goEnd();
+
+    void dropPurpleFlowers();
+
+    void dropWhiteFlowers(StratPattern sp);
+
+    void getLidarPos();
+
+    void checkpoint(StratPattern sp);
+
+    void dropJardiniereFlowers(StratPattern sp);
+
+    void dropBaseFlowers(StratPattern sp);
+
+    void go3Plants(StratPattern sp);
+
+    void removePot(StratPattern sp);
+    /*
+     *  End Strategy function
+     */
+
     void startTestAruco(int pince);
 
     // Call to broadcast
     void setSpeed(int speed);
 
+    void setMaxSpeed();
+
+    void setMinSpeed();
+
     template<class X, class Y>
     void go(X x, Y y);
 
+    template<class X>
+    void go(std::array<X, 2> data);
+
     template<class X>
     void rotate(X angle);
 
     template<class X, class Y>
     void transit(X x,Y y, int endSpeed);
 
+    template<class X>
+    void transit(std::array<X, 2> data, int endSpeed);
+
+    template<class X, class Y, class Z>
+    void setPosition(X x, Y y, Z theta, int clientSocket = -1);
+
+    template<class X>
+    void setPosition(std::array<X, 3> data, int clientSocket = -1);
+
+    void setPosition(Position pos, int clientSocket = -1);
+
+    template<class X, class Y, class Z>
+    void setPosition(X x, Y y, Z theta, const std::string &toSend);
+
+    template<class X>
+    void setPosition(std::array<X, 3> data, const std::string &toSend);
+
+    void setPosition(Position pos, const std::string &toSend);
+
+    void openPince(int pince);
+
+    void fullyOpenPince(int pince);
+
+    void middlePince(int pince);
+
+    void closePince(int pince);
+
+    void baisserBras();
+
+    void leverBras();
+
+    void transportBras();
+
+    void checkPanneau(int servo_moteur);
+
+    void uncheckPanneau(int servo_moteur);
+
+    void askLidarPosition();
+
+    void sendPoint(int point);
+
+    void setTeam(Team team);
+
     ~TCPServer();
 };
diff --git a/main.cpp b/main.cpp
index 7ef1894..b256592 100644
--- a/main.cpp
+++ b/main.cpp
@@ -5,14 +5,12 @@
 std::atomic<bool> shouldStop = false;
 
 void signalHandler( int signum ) {
-
     shouldStop = true;
-
-    exit(signum);
 }
 
 int main(int argc, char* argv[]) {
     signal(SIGINT, signalHandler);
+    signal(SIGTERM, signalHandler);
 
     CLParser clParser(argc, argv);
 
diff --git a/utils.cpp b/utils.cpp
index a7d1581..bd1c25c 100644
--- a/utils.cpp
+++ b/utils.cpp
@@ -88,18 +88,11 @@ int ArucoTag::getNbFind() const {
     return nbFind;
 }
 
-FlowerAruco::FlowerAruco() : tag(nullptr), _realPos({0, 0}) {
-
+std::ostream& operator<<(std::ostream& os, const ArucoTag& tag) {
+    os << "ArucoTag{id=" << tag.id() << ", name=" << tag.name() << ", pos=[" << tag.pos()[0] << ", " << tag.pos()[1] << "], rot=[" << tag.rot()[0] << ", " << tag.rot()[1] << ", " << tag.rot()[2] << "]}";
+    return os;
 }
 
-FlowerAruco::FlowerAruco(ArucoTag *tag) : tag(tag), _realPos({0, 0}) {
-
-}
-
-ArucoTag *FlowerAruco::getTag() const {
-    return tag;
-}
-
-std::array<float, 2> FlowerAruco::getPos() const {
-    return _realPos;
-}
+double distanceToTag(const ArucoTag& tag) {
+    return std::sqrt(pow(tag.pos()[0], 2) + pow(tag.pos()[1], 2));
+}
\ No newline at end of file
diff --git a/utils.h b/utils.h
index d51762d..60e5948 100644
--- a/utils.h
+++ b/utils.h
@@ -4,12 +4,15 @@
 #include <utility>
 #include <vector>
 #include <string>
+#include <ostream>
+#include <cmath>
 
 #define PI 3.14159265358979323846
 
 enum PinceState {
     WHITE_FLOWER,
     PURPLE_FLOWER,
+    FLOWER,
     NONE
 };
 
@@ -26,10 +29,12 @@ namespace TCPUtils {
 class ArucoTag {
 
 public:
-    ArucoTag(int id, std::string  name, std::array<float, 2> pos, std::array<float, 3> rot);
+    ArucoTag(int id, std::string name, std::array<float, 2> pos, std::array<float, 3> rot);
 
     ArucoTag() = default;
 
+    ArucoTag(const ArucoTag& other) = default;
+
     [[nodiscard]] int id() const;
 
     [[nodiscard]] std::string name() const;
@@ -52,27 +57,14 @@ public:
 
     [[nodiscard]] int getNbFind() const;
 
+    friend std::ostream& operator<<(std::ostream& os, const ArucoTag& tag);
+
 private:
     int _id = -1;
     std::string _name;
-    std::array<float, 2> _pos;
-    std::array<float, 3> _rot;
-    int nbFind = 0;
+    std::array<float, 2> _pos = {0, 0};
+    std::array<float, 3> _rot = {0, 0, 0};
+    int nbFind = 1;
 };
 
-class FlowerAruco {
-
-public:
-    FlowerAruco();
-
-    explicit FlowerAruco(ArucoTag* tag);
-
-    [[nodiscard]] ArucoTag* getTag() const;
-
-    [[nodiscard]] std::array<float, 2> getPos() const;
-
-private:
-    ArucoTag* tag;
-
-    std::array<float, 2> _realPos;
-};
\ No newline at end of file
+double distanceToTag(const ArucoTag& tag);
\ No newline at end of file