From c2efaae3c0e59ee27c0169d9e7158459f77a4a05 Mon Sep 17 00:00:00 2001
From: ackimixs <ackimixs@protonmail.com>
Date: Tue, 9 Apr 2024 21:10:11 +0200
Subject: [PATCH] handle the position of the robot

---
 TCPServer.cpp | 14 +++++++++++++-
 TCPServer.h   |  8 ++++++++
 2 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/TCPServer.cpp b/TCPServer.cpp
index 5db0cba..fb94a01 100644
--- a/TCPServer.cpp
+++ b/TCPServer.cpp
@@ -44,6 +44,8 @@ void ClientHandler::closeConnection() {
 
 TCPServer::TCPServer(int port)
 {
+    this->robotPose = {500, 500, -3.1415/2};
+
     serverSocket = socket(AF_INET, SOCK_STREAM, 0);
     if (serverSocket == -1) {
         std::cerr << "Socket creation failed" << std::endl;
@@ -153,9 +155,19 @@ void TCPServer::handleMessage(const std::string& message, int clientSocket)
         }
         checkIfAllClientsReady();
     }
+    if (tokens[2] == "set pos") {
+        std::vector<std::string> pos = split(tokens[3], ",");
+        this->robotPose = {std::stof(pos[0]), std::stof(pos[1]), std::stof(pos[2])};
+        this->broadcastMessage(message.c_str(), clientSocket);
+    }
+    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);
+        this->broadcastMessage(toSend.c_str(), clientSocket);
+    }
     if (tokens[2] == "spawn") {
         // TODO change that to handle spawn point
-        const std::string toSend = "ihm;arduino;set;200,150,-1.57079";
+        this->robotPose = {200, 150, -1.57079};
+        const std::string toSend = "strat;all;set pos;200,150,-1.57079";
         this->broadcastMessage(toSend.c_str(), clientSocket);
     }
     if (tokens[2] == "start")
diff --git a/TCPServer.h b/TCPServer.h
index 332240e..94785c7 100644
--- a/TCPServer.h
+++ b/TCPServer.h
@@ -46,6 +46,14 @@ private:
 
     bool canMove = false;
 
+    struct Position {
+        struct {
+            float x;
+            float y;
+        } pos;
+        float theta;
+    } robotPose;
+
 public:
     explicit TCPServer(int port);