From 2229a1bb045ab355833665c3f261e3019f460a0e Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Wed, 19 Apr 2023 19:25:51 -0400
Subject: [PATCH 1/9] changes

---
 .../SerialController/SerialController.hpp     |  29 ++++-
 .../MechanicalComponent.hpp                   |   2 +-
 src/Controllers/RoboController.cpp            |  13 ++-
 src/Controllers/SerialController.cpp          | 104 ++++++++++--------
 src/main.cpp                                  |  57 ++++++++--
 5 files changed, 137 insertions(+), 68 deletions(-)

diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp
index c3acc00..26e0f4b 100644
--- a/include/Controllers/SerialController/SerialController.hpp
+++ b/include/Controllers/SerialController/SerialController.hpp
@@ -17,6 +17,9 @@
 #include "pros/apix.h" //added this in
 #include <vector>
 #include <iostream>
+#include <queue>
+#include <string>
+#include <atomic>
 
 class TerriBull::SerialController {
     public:
@@ -37,7 +40,7 @@ class TerriBull::SerialController {
     private:
     vector<char> __next_packet;
     char __packet_header[4] { (char)115, (char)111, (char)117, (char)116 };
-    char __end_of_transmission[4] { (char)11, (char)11, (char)10, (char)0 };
+    char __end_of_transmission[4] { (char)0, (char)0, (char)10, (char)10 };
     int __header_length = sizeof(__packet_header) + 1;
     int __footer_length = sizeof(__end_of_transmission);
     int __packet_index_offset = 15;
@@ -46,8 +49,24 @@ class TerriBull::SerialController {
     vector<ScheduledCallback*> ScheduledCallbacks;
     TerriBull::RoboController* motherSys;
 
-    bool compareBuffer(vector<char> buffer1, int start, int end, char* buffer2);
-    
+    static void* TerriBull::SerialController::read_input_task(void* ignore)
+    {
+        std::string buffer;
+        while (true) 
+        {
+            if (!buffer_has_data)
+            {
+                std::cin >> input_buffer;
+                buffer_has_data = true;
+                pros::delay(10);
+            }
+        }
+    }
+
+    std::string input_buffer;
+    volatile bool buffer_has_data = false;
+
+    bool CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2);    
 
     public:
     SerialController(TerriBull::RoboController* _motherSys);
@@ -60,8 +79,8 @@ class TerriBull::SerialController {
     void ExchangeTags();
     int RegisterCallback(std::string tag_name, PacketCallback callback);
     void DeserializePacket();
-    void update(float delta);
-    void readBuffer();
+    void Update(float delta);
+    void ReadBuffer();
     void processDataFromBuffer();
     void SendData(::std::string data);
     void updateExchangeTags();
diff --git a/include/MechanicalComponents/MechanicalComponent.hpp b/include/MechanicalComponents/MechanicalComponent.hpp
index d0e1589..ff3178a 100644
--- a/include/MechanicalComponents/MechanicalComponent.hpp
+++ b/include/MechanicalComponents/MechanicalComponent.hpp
@@ -33,7 +33,7 @@ class TerriBull::MechanicalComponent  {
 
     float kP, kD, kI;
 
-    private:
+    public:
     MotorRefs *motorRefs;
 
     public:
diff --git a/src/Controllers/RoboController.cpp b/src/Controllers/RoboController.cpp
index be1d5fd..3a1c64b 100644
--- a/src/Controllers/RoboController.cpp
+++ b/src/Controllers/RoboController.cpp
@@ -43,7 +43,7 @@ SerialController* RoboController::getSerialController() {
 }
 
 void RoboController::runSerialTask(void* args) {
-    this->serialController->update(this->delta());
+    this->serialController->Update(this->delta());
 }
 void RoboController::runMainTask(void* args) {
     this->Run();
@@ -73,8 +73,8 @@ void TerriBull::RoboController::Init() {
         this->taskManager = new TaskManager();
         /* Init Serial Controller */
         this->serialController = new SerialController(this); /* TODO: Needs SerialController::Init() */
-        this->serialController->RegisterCallback("serial_test_v5_to_jetson", (SerialController::PacketCallback)SerialTestV5ToJetsonCallback);
-        this->serialController->RegisterCallback("serial_test_jetson_to_v5", (SerialController::PacketCallback)SerialTestJetsonToV5Callback);
+        //this->serialController->RegisterCallback("serial_test_v5_to_jetson", (SerialController::PacketCallback)SerialTestV5ToJetsonCallback);
+        //this->serialController->RegisterCallback("serial_test_jetson_to_v5", (SerialController::PacketCallback)SerialTestJetsonToV5Callback);
         this->serialController->RegisterCallback("set_disk_obj", (SerialController::PacketCallback)SetDiskObjectCallback);
         this->serialController->RegisterCallback("get_disk_obj", (SerialController::PacketCallback)GetDiskObjectCallback);
         this->serialController->RegisterCallback("set_goal_obj", (SerialController::PacketCallback)SetGoalObjectCallback);
@@ -202,7 +202,7 @@ void TerriBull::RoboController::Init() {
 void TerriBull::RoboController::Run() {
     this->updateTime();
     this->system->update(this->delta());
-    this->serialController->update(this->delta());
+    this->serialController->Update(this->delta());
     if (pros::competition::is_autonomous()) { /*TODO: Or engaged Autonomous Control */
         this->taskManager->run(this->delta());
     } else {
@@ -554,6 +554,7 @@ void TagExchangeCallback(TerriBull::RoboController* robot, char * array, int sta
  * @param start 
  * @param length 
  */
+/*
 void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array, int start, int length) {
     std::stringstream s3;
     float theta = robot->getSystem()->getAngle();
@@ -563,6 +564,7 @@ void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array
     s3 << SerialController::SerializeNumber(3.14159);
     robot->getSerialController()->SendData(s3.str());
 }
+*/
 /**
  * @brief Recieves Test Data from the Jetson
  * 
@@ -571,6 +573,7 @@ void SerialTestV5ToJetsonCallback(TerriBull::RoboController* robot, char * array
  * @param start 
  * @param length 
  */
+/*
 void SerialTestJetsonToV5Callback(TerriBull::RoboController* robot, char * array, int start, int length) {
     
-}
\ No newline at end of file
+}*/
\ No newline at end of file
diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 0bf1da8..8dd1455 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -9,14 +9,19 @@
  * @copyright Copyright (c) 2022
  * 
  */
+#include <stdio.h>
+#include <stdlib.h>
+#include <stddef.h>
+
 #include "../../include/Controllers/SerialController/SerialController.hpp"
 TerriBull::SerialController::SerialController(TerriBull::RoboController* _motherSys) : motherSys(_motherSys), tagExchange(false), isCollectingTags(false) {
         ::pros::c::serctl(SERCTL_DISABLE_COBS, nullptr);
+        pros::Task input_task(read_input_task, nullptr);
 }
 
-void TerriBull::SerialController::update(float delta)
+void TerriBull::SerialController::Update(float delta)
 { 
-	this->readBuffer();
+	this->ReadBuffer();
     for (auto it = this->ScheduledCallbacks.begin(); it!= this->ScheduledCallbacks.end(); ++it) {
         ScheduledCallback* callback = *it;
         callback->sumTime+=delta;
@@ -27,14 +32,11 @@ void TerriBull::SerialController::update(float delta)
     }
 }
 
-void TerriBull::SerialController::updateExchangeTags() {
-    this->tagExchange = true;
-}
 bool TerriBull::SerialController::isInitialized() {
     return this->tagExchange;
 }
 
-bool TerriBull::SerialController::compareBuffer(vector<char> buffer1, int start, int end, char* buffer2) {
+bool TerriBull::SerialController::CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2) {
     for(int i = start; i < end; i++)
     {
         if (buffer1[i] != buffer2[i-start])
@@ -45,42 +47,41 @@ bool TerriBull::SerialController::compareBuffer(vector<char> buffer1, int start,
     return true;
 }
 
-void TerriBull::SerialController::readBuffer()
+void TerriBull::SerialController::ReadBuffer()
 {
-    std::string input;
-    int _packet_cnt = 0;
-    while (_packet_cnt < 3) { /* Process 3 */
-        input = "";
-        getline(std::cin, input);
-        if (input.empty()) break;
-        double last_length = 0;
-        for (char _int : input)
+    if (!buffer_has_data) return;
+    std::string input = input_buffer;
+    input_buffer = std::string();
+    buffer_has_data = false;
+
+    if (input.empty()) return;
+
+    double last_length = 0;
+    for (char _int : input)
+    {
+        int packet_length = sizeof(__next_packet);
+        int lchar = __next_packet[packet_length-1];
+        if (packet_length - 1 < lchar)
         {
-            int packet_length = sizeof(__next_packet);
-            int lchar = __next_packet[packet_length-1];
-            if (packet_length - 1 < lchar)
-            {
-                if (packet_length >= __header_length){
-                    __next_packet.push_back(_int);
-                    /* Checks to see if Transmition is terminated */
-                    if (this->compareBuffer(__next_packet, packet_length-__footer_length, packet_length, __end_of_transmission))
-                    {
-                        DeserializePacket();
-                        __next_packet.clear();
-                        __next_packet.push_back(1);
-                        continue;
-                    }
-                }
-                else if (_int == __packet_header[packet_length-1])
+            if (packet_length >= __header_length){
+                __next_packet.push_back(_int);
+                /* Checks to see if Transmition is terminated */
+                if (this->CompareBuffer(__next_packet, packet_length-__footer_length, packet_length, __end_of_transmission))
                 {
-                    __next_packet.push_back(_int);
+                    DeserializePacket();
+                    __next_packet.clear();
+                    __next_packet.push_back(1);
                     continue;
                 }
             }
-            __next_packet.clear();
-            __next_packet.push_back(_int);
+            else if (_int == __packet_header[packet_length-1])
+            {
+                __next_packet.push_back(_int);
+                continue;
+            }
         }
-        _packet_cnt++;
+        __next_packet.clear();
+        __next_packet.push_back(_int);
     }
 }
 
@@ -126,6 +127,7 @@ std::string TerriBull::SerialController::SerializeNumber( double f )
     std::string ret = stream.str();
     ret = (char)(ret.length() + 10) + ret;
     ret = (char)1 + ret;
+
     return ret;
 }
 
@@ -206,22 +208,26 @@ std::string TerriBull::SerialController::DeserializeString( char *array, int *si
     return stream.str();
 }
 
-void TerriBull::SerialController::ExchangeTags() {
-
-    while (!this->tagExchange) {
-        if (!(this->isCollectingTags)) {
-            for (auto it = this->Callbacks.begin(); it!= this->Callbacks.end(); ++it) {
+void TerriBull::SerialController::ExchangeTags()
+{
+    while (!this->tagExchange)
+    {
+        if (!(this->isCollectingTags))
+        {
+            for (auto it = this->Callbacks.begin(); it!= this->Callbacks.end(); ++it)
+            {
                 CallbackItem* item = it->second;
                 if (item->callback!= nullptr) {
                     std::stringstream s3;
-                    s3 << (unsigned char) 1;
+                    s3 << (char) 1;
+                    s3 << SerialController::SerializeNumber(this->Callbacks.size() - 1 == it->first ? 0 : 1);
                     s3 << SerialController::SerializeNumber(it->first);
-                    s3 << SerialController::SerializeString(item->friendly_name); 
+                    s3 << SerialController::SerializeString(item->friendly_name);
                     this->SendData(s3.str());
                 }
             }
         }
-        this->readBuffer();
+        this->ReadBuffer();
         pros::delay(5);
     } 
 }
@@ -279,7 +285,8 @@ void TerriBull::SerialController::DeserializePacket()
  * @param start_index 
  * @param length 
  */
-void TerriBull::SerialController::ProcessTagExchange(char * array, int start_index, int length) {
+void TerriBull::SerialController::ProcessTagExchange(char * array, int start_index, int length)
+{
     int step = (int) SerialController::DeserializeNumber(array, &start_index);
     int tag_id = (int) SerialController::DeserializeNumber(array, &start_index);
     std::string friendly_name = SerialController::DeserializeString(array, &start_index);
@@ -312,5 +319,10 @@ int TerriBull::SerialController::GetCallbackIndex(std::string tag_name) {
 
 */
 void TerriBull::SerialController::SendData(std::string data) {
-    ::std::cout<<data<<SerialController::__end_of_transmission;
-}
+    for(auto c : SerialController::__packet_header) std::cout << c;
+    std::cout << data;
+    for(auto c : SerialController::__end_of_transmission) std::cout << c;
+    
+    //std::cout << (char)0 << (char)0 << (char)10 << (char)10;
+    //std::cout << SerialController::__end_of_transmission;
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index a6dd51b..9599c05 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -38,30 +38,65 @@ TerriBull::RoboController controlSys;
 
 
 void on_center_button() {
-	controlSys.ClearTasks();
+	//controlSys.ClearTasks();
 	// pros::delay(100);
-	// controlSys.getSystem()->ResetDrive();
+	// controlSys.getSystem()->resetDrive();
+}
+
+void TestFunction( RoboController* c, char * array, int start_index, int length )
+{
+	int* i = new int(start_index);
+	double number0 = SerialController::DeserializeNumber(array, i);
+	double number1 = SerialController::DeserializeNumber(array, i);
+	std::string string0 = SerialController::DeserializeString(array, i);
+
+	char* cNumber0 = new char(number0 + 48);
+	char* cNumber1 = new char(number1 + 48);
+	pros::lcd::clear_line(0);
+	pros::lcd::print(0, cNumber0);
+	pros::lcd::clear_line(1);
+	pros::lcd::print(1, cNumber1);
+	pros::lcd::clear_line(2);
+	pros::lcd::print(2, string0.c_str());
 }
 
 void initialize() {
 	pros::lcd::initialize();
-	controlSys.Init();
-	pros::lcd::register_btn1_cb(on_center_button);
+	// controlSys.Init();
+	SerialController* serialController = new SerialController(nullptr);
+	serialController->RegisterCallback("TestFunction", (SerialController::PacketCallback)TestFunction);
+	pros::lcd::print(0, "Exchanging Tags...");
+	serialController->ExchangeTags();
+	pros::lcd::print(0, "Finished            ");
+	while(true)
+	{
+		serialController->ReadBuffer();
+		
+		stringstream stream;
+		stream << serialController->GetCallbackIndex("test_function");
+		stream << SerialController::SerializeNumber(654);
+		stream << SerialController::SerializeString("this is a super long string that we are sending over serial that i hope will just be over 255 characters to ensure strings are working properly");
+		stream << SerialController::SerializeNumber(5);
+		stream << SerialController::SerializeNumber(6568964);
+		stream << SerialController::SerializeNumber(7896786);
+		serialController->SendData(stream.str());
+	}
+	//pros::lcd::register_btn1_cb(on_center_button);
 }
 
 
 void autonomous() {
-	while (true) {
-		controlSys.Run();
-	}
+	//while (true) {
+	//	controlSys.Run();
+	//}
 }
 
 
 
 void opcontrol() {
 	// controlSys.ClearTasks();
-	controlSys.getSystem()->setStartingPosition(0, 0);
-	while (true) {
-		controlSys.Run();
-    }
+	//controlSys.getSystem()->setStartingPosition(0, 0);
+	//while (true) {
+	//	controlSys.Run();
+    //}
 }

From d75a0491266cdaadf3e5e20decccbe0413fcc3c4 Mon Sep 17 00:00:00 2001
From: "DESKTOP-N9VDHAM\\John" <johnnykoch02@gmail.com>
Date: Wed, 19 Apr 2023 20:01:19 -0400
Subject: [PATCH 2/9] update args breh

---
 .../SerialController/SerialController.hpp        | 16 +++++++++++-----
 src/Controllers/SerialController.cpp             |  9 ++++++++-
 2 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp
index 26e0f4b..18ee9d9 100644
--- a/include/Controllers/SerialController/SerialController.hpp
+++ b/include/Controllers/SerialController/SerialController.hpp
@@ -49,15 +49,21 @@ class TerriBull::SerialController {
     vector<ScheduledCallback*> ScheduledCallbacks;
     TerriBull::RoboController* motherSys;
 
-    static void* TerriBull::SerialController::read_input_task(void* ignore)
+    struct UpdateArgs {
+        volatile bool* buffer_update;
+        std::string* buffer;
+    };
+
+
+    static void TerriBull::SerialController::read_input_task(void* ignore)
     {
-        std::string buffer;
+        SerialController::UpdateArgs* args = static_cast<SerialController::UpdateArgs*>(ignore);
         while (true) 
         {
-            if (!buffer_has_data)
+            if (!*(args->buffer_update))
             {
-                std::cin >> input_buffer;
-                buffer_has_data = true;
+                std::cin >> *(args->buffer);
+                *(args->buffer_update) = true;
                 pros::delay(10);
             }
         }
diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 8dd1455..b5a4c70 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -16,7 +16,14 @@
 #include "../../include/Controllers/SerialController/SerialController.hpp"
 TerriBull::SerialController::SerialController(TerriBull::RoboController* _motherSys) : motherSys(_motherSys), tagExchange(false), isCollectingTags(false) {
         ::pros::c::serctl(SERCTL_DISABLE_COBS, nullptr);
-        pros::Task input_task(read_input_task, nullptr);
+        void* tsk_updater = malloc(sizeof(volatile bool*) + sizeof(std::string*));
+        int offset = 0;
+        volatile bool* mem_loc_a = &(this->buffer_has_data);
+        memcpy((char*)tsk_updater + offset, &mem_loc_a, sizeof(float));
+        offset += sizeof(volatile bool*);
+        std::string* mem_loc_b = &(this->input_buffer);
+        memcpy((char*)tsk_updater + offset, &mem_loc_b, sizeof(std::string*));
+        pros::Task input_task(SerialController::read_input_task, tsk_updater);
 }
 
 void TerriBull::SerialController::Update(float delta)

From cf94ac5b9204d020127d0ae8129959873c74c458 Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Wed, 19 Apr 2023 20:34:49 -0400
Subject: [PATCH 3/9] e

---
 .../SerialController/SerialController.hpp       | 17 +++++++++--------
 src/Controllers/SerialController.cpp            |  8 ++++++--
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp
index 18ee9d9..135f177 100644
--- a/include/Controllers/SerialController/SerialController.hpp
+++ b/include/Controllers/SerialController/SerialController.hpp
@@ -49,32 +49,33 @@ class TerriBull::SerialController {
     vector<ScheduledCallback*> ScheduledCallbacks;
     TerriBull::RoboController* motherSys;
 
+    std::string input_buffer;
+    volatile bool buffer_has_data = false;
+
+    bool CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2);    
+
+    public:
     struct UpdateArgs {
         volatile bool* buffer_update;
         std::string* buffer;
     };
 
 
-    static void TerriBull::SerialController::read_input_task(void* ignore)
+    static void read_input_task(void* ignore)
     {
+        pros::lcd::print(4, "I STARTED EATING POOP");
         SerialController::UpdateArgs* args = static_cast<SerialController::UpdateArgs*>(ignore);
         while (true) 
         {
             if (!*(args->buffer_update))
             {
                 std::cin >> *(args->buffer);
+                pros::lcd::print(5, args->buffer->c_str());
                 *(args->buffer_update) = true;
                 pros::delay(10);
             }
         }
     }
-
-    std::string input_buffer;
-    volatile bool buffer_has_data = false;
-
-    bool CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2);    
-
-    public:
     SerialController(TerriBull::RoboController* _motherSys);
 
     static std::string SerializeNumber( double f );
diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index b5a4c70..69234d1 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -23,7 +23,7 @@ TerriBull::SerialController::SerialController(TerriBull::RoboController* _mother
         offset += sizeof(volatile bool*);
         std::string* mem_loc_b = &(this->input_buffer);
         memcpy((char*)tsk_updater + offset, &mem_loc_b, sizeof(std::string*));
-        pros::Task input_task(SerialController::read_input_task, tsk_updater);
+        pros::Task input_task(SerialController::read_input_task, tsk_updater, "ogga_booga");
 }
 
 void TerriBull::SerialController::Update(float delta)
@@ -58,7 +58,8 @@ void TerriBull::SerialController::ReadBuffer()
 {
     if (!buffer_has_data) return;
     std::string input = input_buffer;
-    input_buffer = std::string();
+    pros::lcd::print(3, input.c_str());
+    input_buffer.clear();
     buffer_has_data = false;
 
     if (input.empty()) return;
@@ -217,6 +218,7 @@ std::string TerriBull::SerialController::DeserializeString( char *array, int *si
 
 void TerriBull::SerialController::ExchangeTags()
 {
+    int dfoklgjldkf = 0;
     while (!this->tagExchange)
     {
         if (!(this->isCollectingTags))
@@ -234,8 +236,10 @@ void TerriBull::SerialController::ExchangeTags()
                 }
             }
         }
+        dfoklgjldkf++;
         this->ReadBuffer();
         pros::delay(5);
+        pros::lcd::print(2, to_string(dfoklgjldkf).c_str());
     } 
 }
 

From d6238c4bf8867fd945da9a19f8b3898aaf927d50 Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Fri, 21 Apr 2023 00:07:17 -0400
Subject: [PATCH 4/9] e

---
 .../SerialController/SerialController.hpp     |  47 ++-
 src/Controllers/SerialController.cpp          | 290 ++++++++++--------
 src/main.cpp                                  |  80 +++--
 3 files changed, 237 insertions(+), 180 deletions(-)

diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp
index 135f177..fa39d48 100644
--- a/include/Controllers/SerialController/SerialController.hpp
+++ b/include/Controllers/SerialController/SerialController.hpp
@@ -17,9 +17,9 @@
 #include "pros/apix.h" //added this in
 #include <vector>
 #include <iostream>
-#include <queue>
 #include <string>
-#include <atomic>
+#include <mutex>
+#include <cstring>
 
 class TerriBull::SerialController {
     public:
@@ -39,41 +39,36 @@ class TerriBull::SerialController {
 
     private:
     vector<char> __next_packet;
-    char __packet_header[4] { (char)115, (char)111, (char)117, (char)116 };
-    char __end_of_transmission[4] { (char)0, (char)0, (char)10, (char)10 };
-    int __header_length = sizeof(__packet_header) + 1;
-    int __footer_length = sizeof(__end_of_transmission);
-    int __packet_index_offset = 15;
+    static constexpr char __packet_header[4] = { (char)115, (char)111, (char)117, (char)116 };
+    static constexpr char __end_of_transmission[4] = { (char)0, (char)0, (char)10, (char)10 };
+    static const int __header_length = (sizeof(__packet_header) / sizeof(char));
+    static const int __footer_length = sizeof(__end_of_transmission) / sizeof(char);
+    static const int __packet_index_offset = 15;
     bool isCollectingTags, tagExchange;
     map<int, CallbackItem*> Callbacks;
     vector<ScheduledCallback*> ScheduledCallbacks;
     TerriBull::RoboController* motherSys;
 
-    std::string input_buffer;
-    volatile bool buffer_has_data = false;
-
     bool CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2);    
 
     public:
-    struct UpdateArgs {
-        volatile bool* buffer_update;
-        std::string* buffer;
-    };
+    // struct UpdateArgs {
+    //     volatile bool* buffer_update;
+        
+    // };
 
+    static std::string input_buffer;
+    static pros::Mutex input_mutex;
 
     static void read_input_task(void* ignore)
     {
-        pros::lcd::print(4, "I STARTED EATING POOP");
-        SerialController::UpdateArgs* args = static_cast<SerialController::UpdateArgs*>(ignore);
-        while (true) 
+        char c;
+        while(true) 
         {
-            if (!*(args->buffer_update))
-            {
-                std::cin >> *(args->buffer);
-                pros::lcd::print(5, args->buffer->c_str());
-                *(args->buffer_update) = true;
-                pros::delay(10);
-            }
+            std::cin.get(c);
+            std::unique_lock<pros::Mutex> lock(TerriBull::SerialController::input_mutex);
+            TerriBull::SerialController::input_buffer += c;
+            lock.unlock();
         }
     }
     SerialController(TerriBull::RoboController* _motherSys);
@@ -81,13 +76,13 @@ class TerriBull::SerialController {
     static std::string SerializeNumber( double f );
     static double DeserializeNumber( char *array, int *si );
     static std::string SerializeString( std::string s );
-    static std::string SerializeString( const char *s );
+    //static std::string SerializeString( const char *s );
     static std::string DeserializeString( char *array, int *si );
     void ExchangeTags();
     int RegisterCallback(std::string tag_name, PacketCallback callback);
     void DeserializePacket();
     void Update(float delta);
-    void ReadBuffer();
+    bool ReadBuffer();
     void processDataFromBuffer();
     void SendData(::std::string data);
     void updateExchangeTags();
diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 69234d1..a0092ae 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -2,105 +2,99 @@
  * @file SerialController.cpp
  * @author John Koch jkoch21@usf.edu, Bill Gate <Email Redacted>
  * @brief Manages Serial communication between the V5 and other devices
- *     
+ *
  * @version 0.1
  * @date 2023-03-28
- * 
+ *
  * @copyright Copyright (c) 2022
- * 
+ *
  */
 #include <stdio.h>
 #include <stdlib.h>
 #include <stddef.h>
 
 #include "../../include/Controllers/SerialController/SerialController.hpp"
-TerriBull::SerialController::SerialController(TerriBull::RoboController* _motherSys) : motherSys(_motherSys), tagExchange(false), isCollectingTags(false) {
-        ::pros::c::serctl(SERCTL_DISABLE_COBS, nullptr);
-        void* tsk_updater = malloc(sizeof(volatile bool*) + sizeof(std::string*));
-        int offset = 0;
-        volatile bool* mem_loc_a = &(this->buffer_has_data);
-        memcpy((char*)tsk_updater + offset, &mem_loc_a, sizeof(float));
-        offset += sizeof(volatile bool*);
-        std::string* mem_loc_b = &(this->input_buffer);
-        memcpy((char*)tsk_updater + offset, &mem_loc_b, sizeof(std::string*));
-        pros::Task input_task(SerialController::read_input_task, tsk_updater, "ogga_booga");
+
+std::string TerriBull::SerialController::input_buffer;
+pros::Mutex TerriBull::SerialController::input_mutex;
+
+TerriBull::SerialController::SerialController(TerriBull::RoboController *_motherSys) : motherSys(_motherSys), tagExchange(false), isCollectingTags(false)
+{
+    ::pros::c::serctl(SERCTL_DISABLE_COBS, nullptr);
+    pros::Task input_task(SerialController::read_input_task);
 }
 
 void TerriBull::SerialController::Update(float delta)
-{ 
-	this->ReadBuffer();
-    for (auto it = this->ScheduledCallbacks.begin(); it!= this->ScheduledCallbacks.end(); ++it) {
-        ScheduledCallback* callback = *it;
-        callback->sumTime+=delta;
-        if (callback->sumTime >= callback->frequency) {
+{
+    this->ReadBuffer();
+    for (auto it = this->ScheduledCallbacks.begin(); it != this->ScheduledCallbacks.end(); ++it)
+    {
+        ScheduledCallback *callback = *it;
+        callback->sumTime += delta;
+        if (callback->sumTime >= callback->frequency)
+        {
             callback->callbackItem->callback(this->motherSys, nullptr, 0, 0);
             callback->sumTime = 0;
         }
     }
 }
 
-bool TerriBull::SerialController::isInitialized() {
+bool TerriBull::SerialController::isInitialized()
+{
     return this->tagExchange;
 }
 
-bool TerriBull::SerialController::CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2) {
-    for(int i = start; i < end; i++)
+bool TerriBull::SerialController::ReadBuffer()
+{
+    std::string input;
+    std::unique_lock<pros::Mutex> lock(TerriBull::SerialController::input_mutex);
+    if (!TerriBull::SerialController::input_buffer.empty())
     {
-        if (buffer1[i] != buffer2[i-start])
-        {
-            return false;
-        }
+        input = TerriBull::SerialController::input_buffer;
+        TerriBull::SerialController::input_buffer.clear();
     }
-    return true;
-}
+    lock.unlock();
 
-void TerriBull::SerialController::ReadBuffer()
-{
-    if (!buffer_has_data) return;
-    std::string input = input_buffer;
-    pros::lcd::print(3, input.c_str());
-    input_buffer.clear();
-    buffer_has_data = false;
+    if (input.size() == 0)
+        return false;
 
-    if (input.empty()) return;
+    // return;
 
-    double last_length = 0;
     for (char _int : input)
     {
-        int packet_length = sizeof(__next_packet);
-        int lchar = __next_packet[packet_length-1];
-        if (packet_length - 1 < lchar)
+        int packet_length = __next_packet.size();
+        if (packet_length >= __header_length)
         {
-            if (packet_length >= __header_length){
-                __next_packet.push_back(_int);
-                /* Checks to see if Transmition is terminated */
-                if (this->CompareBuffer(__next_packet, packet_length-__footer_length, packet_length, __end_of_transmission))
-                {
-                    DeserializePacket();
-                    __next_packet.clear();
-                    __next_packet.push_back(1);
-                    continue;
-                }
-            }
-            else if (_int == __packet_header[packet_length-1])
+            __next_packet.push_back(_int);
+            /* Checks to see if Transmition is terminated */
+            if (std::equal(__next_packet.end() - __footer_length, __next_packet.end(), std::begin(__end_of_transmission),
+                           [](int a, char b)
+                           { return a == (int)b; }))
             {
-                __next_packet.push_back(_int);
-                continue;
+                this->DeserializePacket();
+                __next_packet.clear();
             }
+            continue;
+        }
+        else if (_int == __packet_header[packet_length])
+        {
+            __next_packet.push_back(_int);
+            continue;
         }
         __next_packet.clear();
-        __next_packet.push_back(_int);
     }
+    
+    return true;
 }
 
 // 0 and 10 unusable
 // 50-200 data types
 // 11-255 data length - 10
-// 11-255 integer - 11 
+// 11-255 integer - 11
 // 7 for decimal
 // 11 end of transmission
 // [start][function_id][data_type][data_length][data][end]
-std::string TerriBull::SerialController::SerializeNumber( double f )
+std::string TerriBull::SerialController::SerializeNumber(double f)
 {
     std::ostringstream stream;
     std::string ff = "";
@@ -110,17 +104,17 @@ std::string TerriBull::SerialController::SerializeNumber( double f )
     else
         ff = std::to_string(f);
 
-    for(int c = 0; c < ff.length(); c++)
+    for (int c = 0; c < ff.length(); c++)
     {
         unsigned char chr = ff[c];
-        
+
         if (chr >= 48 and chr <= 57)
         {
-            if (c+1 < ff.length() && ff[c+1] >= 48 && ff[c+1] <= 57)
+            if (c + 1 < ff.length() && ff[c + 1] >= 48 && ff[c + 1] <= 57)
             {
-                unsigned char nchar = ((chr - 48) * 10) + (ff[c+1] - 37);
+                unsigned char nchar = ((chr - 48) * 10) + (ff[c + 1] - 37);
                 stream << (unsigned char)nchar;
-                c+=1;
+                c += 1;
             }
             else
             {
@@ -139,14 +133,15 @@ std::string TerriBull::SerialController::SerializeNumber( double f )
     return ret;
 }
 
-double TerriBull::SerialController::DeserializeNumber( char *array, int *si )
+double TerriBull::SerialController::DeserializeNumber(char *array, int *si)
 {
     int length = array[*si] - 10;
+    (*si)++;
     double number = 0;
     double decimal = 0;
     int decimal_length = 0;
     bool switch_to_decimal = false;
-    for(int i = *si+1; i < *si+length; i++)
+    for (int i = *si; i < *si + length; i++)
     {
         if (!switch_to_decimal)
         {
@@ -176,59 +171,56 @@ double TerriBull::SerialController::DeserializeNumber( char *array, int *si )
     }
     int divisor = pow(10, decimal_length);
     number += decimal / divisor;
-    (*si)+=length+1;
+    (*si) += length;
+
     return number;
 }
 
-char* LongerLength( int length )
+char *LongerLength(int length)
 {
     char first_byte = (length & 0xFF00) >> 8;
     char second_byte = length & 0xFF;
-    return new char[2] { first_byte, second_byte };
+    return new char[2]{first_byte, second_byte};
 }
 
 // MAX length 65,535 - 10
-std::string TerriBull::SerialController::SerializeString( std::string s )
-{
-    return SerializeString(s.c_str());
-}
-
-std::string TerriBull::SerialController::SerializeString( const char *s )
+std::string TerriBull::SerialController::SerializeString(std::string s)
 {
     std::ostringstream stream;
-    stream << (char)2;
-    char* string_length = LongerLength(sizeof(s)+10);
-    stream << (char)string_length[0];
-    stream << (char)string_length[1];
+    stream << (unsigned char)2;
+    char *string_length = LongerLength(s.size() + 12);
+    stream << (unsigned char)string_length[0];
+    stream << (unsigned char)string_length[1];
     stream << s;
     return stream.str();
 }
 
-std::string TerriBull::SerialController::DeserializeString( char *array, int *si )
+std::string TerriBull::SerialController::DeserializeString(char *array, int *si)
 {
     std::ostringstream stream;
-    int length = (array[*si] << 8) + array[(*si)+1];
-    (*si)+=2;
-    for (int i = *si; i < *si+length; i++){
+    int length = (array[*si] << 8) + array[(*si) + 1];
+    (*si) += 2;
+    for (int i = *si; i < *si + length; i++)
+    {
         stream << array[i];
     }
-    (*si)+=length;
+    (*si) += length;
     return stream.str();
 }
 
 void TerriBull::SerialController::ExchangeTags()
 {
-    int dfoklgjldkf = 0;
     while (!this->tagExchange)
     {
-        if (!(this->isCollectingTags))
+        if (!this->isCollectingTags)
         {
-            for (auto it = this->Callbacks.begin(); it!= this->Callbacks.end(); ++it)
+            for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
             {
-                CallbackItem* item = it->second;
-                if (item->callback!= nullptr) {
+                CallbackItem *item = it->second;
+                if (item->callback != nullptr)
+                {
                     std::stringstream s3;
-                    s3 << (char) 1;
+                    s3 << (char)1;
                     s3 << SerialController::SerializeNumber(this->Callbacks.size() - 1 == it->first ? 0 : 1);
                     s3 << SerialController::SerializeNumber(it->first);
                     s3 << SerialController::SerializeString(item->friendly_name);
@@ -236,27 +228,27 @@ void TerriBull::SerialController::ExchangeTags()
                 }
             }
         }
-        dfoklgjldkf++;
         this->ReadBuffer();
         pros::delay(5);
-        pros::lcd::print(2, to_string(dfoklgjldkf).c_str());
-    } 
+    }
 }
 
 int TerriBull::SerialController::RegisterCallback(std::string tag_name, PacketCallback callback)
 {
     bool found = false;
     int returnIndex = -1;
-    for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it) {
-        CallbackItem* item = it->second;
-        if (item->friendly_name == tag_name) {
+    for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
+    {
+        CallbackItem *item = it->second;
+        if (item->friendly_name == tag_name)
+        {
             found = true;
             break;
         }
     }
     if (!found)
     {
-        CallbackItem* item = new CallbackItem();
+        CallbackItem *item = new CallbackItem();
         item->friendly_name = tag_name;
         item->callback = callback;
         item->jetson_id = -1;
@@ -272,68 +264,98 @@ int TerriBull::SerialController::RegisterCallback(std::string tag_name, PacketCa
 
 void TerriBull::SerialController::DeserializePacket()
 {
-    CallbackItem* _callback;
     int func_id = __next_packet[__header_length];
-    int pSize = sizeof(__next_packet);
+    int pSize = __next_packet.size() - __header_length;
     int end = pSize - __footer_length;
-    char* buffer[end];
-    memcpy( &buffer[0], &__next_packet[__header_length], end );
-    if (func_id == 1) {
-       this->ProcessTagExchange((*buffer), 0, end);
+    char buffer[end];
+    memcpy(buffer, &__next_packet[__header_length + 1], end);
+
+    if (func_id == 1)
+    {
+        this->ProcessTagExchange(buffer, 0, end);
     }
-    else {
-        _callback = Callbacks[func_id - SerialController::__packet_index_offset];
-        if (_callback == nullptr) return; /*TODO: Create Custom Callback where this sends back an error msg. */
-        PacketCallback __function =  _callback->callback;
-        __function(this->motherSys, (*buffer), 0, end);
+    else if (this->tagExchange && !this->isCollectingTags)
+    {
+        auto itr = Callbacks.find(func_id - SerialController::__packet_index_offset);
+        if (itr == Callbacks.end())
+            return;
+        itr->second->callback(this->motherSys, buffer, 0, end);
     }
 }
 
 /**
- * @brief 
+ * @brief
  * DATA: [float step, int tag_id, string friendly_name]
- * @param array 
- * @param start_index 
- * @param length 
+ * @param array
+ * @param start_index
+ * @param length
  */
-void TerriBull::SerialController::ProcessTagExchange(char * array, int start_index, int length)
+void TerriBull::SerialController::ProcessTagExchange(char *array, int start_index, int length)
 {
-    int step = (int) SerialController::DeserializeNumber(array, &start_index);
-    int tag_id = (int) SerialController::DeserializeNumber(array, &start_index);
-    std::string friendly_name = SerialController::DeserializeString(array, &start_index);
+    this->isCollectingTags = true;
+    int ind = start_index + 1;
+
+    int step = (int)SerialController::DeserializeNumber(array, &ind);
+
+    ind++;
+    int tag_id = (int)SerialController::DeserializeNumber(array, &ind);
+
+    ind++;
+    std::string friendly_name = SerialController::DeserializeString(array, &ind);
+
     int our_tag_id = this->GetCallbackIndex(friendly_name) - SerialController::__packet_index_offset;
-    SerialController::CallbackItem* item = this->Callbacks[our_tag_id];
-    if (item != nullptr) {
+
+    SerialController::CallbackItem *item;
+    auto ret = this->Callbacks.find(our_tag_id);
+
+    if (ret != this->Callbacks.end())
+    {
+        item = ret->second;
         item->jetson_id = tag_id;
     }
-    if (step == 0) {/* Final Tag Exchange Packet*/
-        this->tagExchange = true;
+    else
+    {
+        item = new SerialController::CallbackItem();
+        item->jetson_id = tag_id;
+        item->friendly_name = friendly_name;
+        this->Callbacks[Callbacks.size()] = item;
+    }
+
+    if (step == 0)
+    {
         this->isCollectingTags = false;
+        this->tagExchange = true;
     }
-    else if(step == 1) this->isCollectingTags = true;
 }
 
-int TerriBull::SerialController::GetCallbackIndex(std::string tag_name) { 
-    for (auto it = this->Callbacks.begin(); it!= this->Callbacks.end(); ++it) {
-        SerialController::CallbackItem* item = it->second;
-        if (item->friendly_name == tag_name) {
+int TerriBull::SerialController::GetCallbackIndex(std::string tag_name)
+{
+    for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
+    {
+        SerialController::CallbackItem *item = it->second;
+        if (item->friendly_name == tag_name)
+        {
             if (item->jetson_id == -1) /* We have not initialized the Packet index*/
                 return it->first + SerialController::__packet_index_offset;
-            else  /* Packet Callback has been initialized on the Jetson */
+            else /* Packet Callback has been initialized on the Jetson */
                 return item->jetson_id + SerialController::__packet_index_offset;
         }
     }
-    return -1;
+    return this->Callbacks.size() + SerialController::__packet_index_offset;
 }
 
-/*
-
-*/
-void TerriBull::SerialController::SendData(std::string data) {
-    for(auto c : SerialController::__packet_header) std::cout << c;
+void TerriBull::SerialController::SendData(std::string data)
+{
+    for (auto c : SerialController::__packet_header)
+    {
+        std::cout << c;
+    }
     std::cout << data;
-    for(auto c : SerialController::__end_of_transmission) std::cout << c;
-    
-    //std::cout << (char)0 << (char)0 << (char)10 << (char)10;
-    //std::cout << SerialController::__end_of_transmission;
+    for (auto c : SerialController::__end_of_transmission)
+    {
+        std::cout << c;
+    }
+
+    // std::cout << (char)0 << (char)0 << (char)10 << (char)10;
+    // std::cout << SerialController::__end_of_transmission;
 }
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 9599c05..73da631 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -43,43 +43,83 @@ void on_center_button() {
 	// controlSys.getSystem()->resetDrive();
 }
 
-void TestFunction( RoboController* c, char * array, int start_index, int length )
+// trim from start (in place)
+static inline void ltrim(std::string &s) {
+    s.erase(s.begin(), std::find_if(s.begin(), s.end(), [](unsigned char ch) {
+        return !std::isspace(ch);
+    }));
+}
+
+// trim from end (in place)
+static inline void rtrim(std::string &s) {
+    s.erase(std::find_if(s.rbegin(), s.rend(), [](unsigned char ch) {
+        return !std::isspace(ch);
+    }).base(), s.end());
+}
+
+// trim from both ends (in place)
+static inline void trim(std::string &s) {
+    rtrim(s);
+    ltrim(s);
+}
+
+int kghkh = 1;
+void TestFunction( RoboController* c, char *array, int start_index, int length )
 {
-	int* i = new int(start_index);
-	double number0 = SerialController::DeserializeNumber(array, i);
-	double number1 = SerialController::DeserializeNumber(array, i);
-	std::string string0 = SerialController::DeserializeString(array, i);
-
-	char* cNumber0 = new char(number0 + 48);
-	char* cNumber1 = new char(number1 + 48);
-	pros::lcd::clear_line(0);
-	pros::lcd::print(0, cNumber0);
-	pros::lcd::clear_line(1);
-	pros::lcd::print(1, cNumber1);
-	pros::lcd::clear_line(2);
-	pros::lcd::print(2, string0.c_str());
+	int i = start_index + 1;
+
+	double number0 = SerialController::DeserializeNumber(array, &i);
+	i++;
+	double number1 = SerialController::DeserializeNumber(array, &i);
+	i++;
+	std::string string0 = SerialController::DeserializeString(array, &i);
+
+	pros::lcd::set_text(0, to_string(kghkh++).c_str());
+	pros::lcd::set_text(1, to_string(number0).c_str());
+	pros::lcd::set_text(2, to_string(number1).c_str());
+	
+	int u = 3;
+	std::string fmt;
+	for (int p = 0; p < string0.length(); p++)
+	{
+		//if (u > 7) u = 2;
+		fmt += string0[p];
+		if (fmt.length() > 26)
+		{
+			trim(fmt);
+			if (fmt.length() > 0)
+				pros::lcd::set_text(u++, fmt.c_str());
+			fmt.clear();
+		}
+	}
+	trim(fmt);
+	if (fmt.length() > 0)
+		pros::lcd::set_text(u++, fmt.c_str());
 }
 
+
 void initialize() {
 	pros::lcd::initialize();
 	// controlSys.Init();
 	SerialController* serialController = new SerialController(nullptr);
 	serialController->RegisterCallback("TestFunction", (SerialController::PacketCallback)TestFunction);
-	pros::lcd::print(0, "Exchanging Tags...");
 	serialController->ExchangeTags();
-	pros::lcd::print(0, "Finished            ");
+
+	int sent = 1;
 	while(true)
 	{
 		serialController->ReadBuffer();
-		
+
 		stringstream stream;
-		stream << serialController->GetCallbackIndex("test_function");
+		stream << (unsigned char) (serialController->GetCallbackIndex("test_function") - 2);
 		stream << SerialController::SerializeNumber(654);
 		stream << SerialController::SerializeString("this is a super long string that we are sending over serial that i hope will just be over 255 characters to ensure strings are working properly");
-		stream << SerialController::SerializeNumber(5);
-		stream << SerialController::SerializeNumber(6568964);
+		stream << SerialController::SerializeNumber(1);
+		stream << SerialController::SerializeNumber(7154);
 		stream << SerialController::SerializeNumber(7896786);
 		serialController->SendData(stream.str());
+		pros::lcd::set_text(7, to_string(sent++).c_str());
+		pros::delay(5);
 	}
 	//pros::lcd::register_btn1_cb(on_center_button);
 }

From 548533f150ba08559fe8c93ec24f9df611df09cf Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Sun, 23 Apr 2023 14:43:30 -0400
Subject: [PATCH 5/9] fix decimal

---
 src/Controllers/SerialController.cpp | 15 +++++++++++----
 src/main.cpp                         |  4 ++--
 2 files changed, 13 insertions(+), 6 deletions(-)

diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index a0092ae..7b0197a 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -148,7 +148,12 @@ double TerriBull::SerialController::DeserializeNumber(char *array, int *si)
             if ((int)array[i] != 7)
             {
                 number *= 10;
-                number += (int)(array[i] - 11);
+                auto k = (int)(array[i] - 11);
+                number += k;
+                if (k > 10 && i < length && array[i+1] - 11 > 10)
+                {
+                    number *= 10;
+                }
             }
             else
             {
@@ -159,9 +164,11 @@ double TerriBull::SerialController::DeserializeNumber(char *array, int *si)
         {
             if ((int)array[i] != 7)
             {
-                decimal_length++;
-                decimal *= 10;
-                decimal += (int)(array[i] - 11);
+                auto k = (int)(array[i] - 11);
+                auto p = to_string(k).length();
+                decimal *= pow(10, p);
+                decimal += k;
+                decimal_length += p;
             }
             else
             {
diff --git a/src/main.cpp b/src/main.cpp
index 73da631..a73d7c2 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -115,8 +115,8 @@ void initialize() {
 		stream << SerialController::SerializeNumber(654);
 		stream << SerialController::SerializeString("this is a super long string that we are sending over serial that i hope will just be over 255 characters to ensure strings are working properly");
 		stream << SerialController::SerializeNumber(1);
-		stream << SerialController::SerializeNumber(7154);
-		stream << SerialController::SerializeNumber(7896786);
+		stream << SerialController::SerializeNumber(715.4);
+		stream << SerialController::SerializeNumber(789.6786);
 		serialController->SendData(stream.str());
 		pros::lcd::set_text(7, to_string(sent++).c_str());
 		pros::delay(5);

From e9e66a3b01704dfb31b2f27012cf8c9e19238435 Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Sun, 23 Apr 2023 15:16:19 -0400
Subject: [PATCH 6/9] e

---
 src/Controllers/SerialController.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 7b0197a..8bf0431 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -348,7 +348,7 @@ int TerriBull::SerialController::GetCallbackIndex(std::string tag_name)
                 return item->jetson_id + SerialController::__packet_index_offset;
         }
     }
-    return this->Callbacks.size() + SerialController::__packet_index_offset;
+    return this->Callbacks.size() - 1 + SerialController::__packet_index_offset;
 }
 
 void TerriBull::SerialController::SendData(std::string data)

From 1d820f35704cf5628f6d9806dd80c2077332f44d Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Sun, 23 Apr 2023 15:17:10 -0400
Subject: [PATCH 7/9] e

---
 src/Controllers/SerialController.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 8bf0431..7b0197a 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -348,7 +348,7 @@ int TerriBull::SerialController::GetCallbackIndex(std::string tag_name)
                 return item->jetson_id + SerialController::__packet_index_offset;
         }
     }
-    return this->Callbacks.size() - 1 + SerialController::__packet_index_offset;
+    return this->Callbacks.size() + SerialController::__packet_index_offset;
 }
 
 void TerriBull::SerialController::SendData(std::string data)

From 32ea0c054a8f59a10fa750a8708dd9671c74236a Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Sun, 23 Apr 2023 21:07:05 -0400
Subject: [PATCH 8/9] e

---
 .../SerialController/SerialController.hpp     |  2 +
 src/Controllers/SerialController.cpp          | 45 ++++++++++++-------
 2 files changed, 32 insertions(+), 15 deletions(-)

diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp
index fa39d48..7710426 100644
--- a/include/Controllers/SerialController/SerialController.hpp
+++ b/include/Controllers/SerialController/SerialController.hpp
@@ -46,10 +46,12 @@ class TerriBull::SerialController {
     static const int __packet_index_offset = 15;
     bool isCollectingTags, tagExchange;
     map<int, CallbackItem*> Callbacks;
+    map<int, CallbackItem*> tmpCallbacks;
     vector<ScheduledCallback*> ScheduledCallbacks;
     TerriBull::RoboController* motherSys;
 
     bool CompareBuffer(vector<char> buffer1, int start, int end, char* buffer2);    
+    SerialController::CallbackItem* FindInternal(std::string tag_name);
 
     public:
     // struct UpdateArgs {
diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 7b0197a..0d58b88 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -299,7 +299,11 @@ void TerriBull::SerialController::DeserializePacket()
  */
 void TerriBull::SerialController::ProcessTagExchange(char *array, int start_index, int length)
 {
-    this->isCollectingTags = true;
+    if (!this->isCollectingTags)
+    {
+        this->isCollectingTags = true;
+    }
+    
     int ind = start_index + 1;
 
     int step = (int)SerialController::DeserializeNumber(array, &ind);
@@ -310,31 +314,42 @@ void TerriBull::SerialController::ProcessTagExchange(char *array, int start_inde
     ind++;
     std::string friendly_name = SerialController::DeserializeString(array, &ind);
 
-    int our_tag_id = this->GetCallbackIndex(friendly_name) - SerialController::__packet_index_offset;
-
     SerialController::CallbackItem *item;
-    auto ret = this->Callbacks.find(our_tag_id);
 
-    if (ret != this->Callbacks.end())
-    {
-        item = ret->second;
-        item->jetson_id = tag_id;
-    }
-    else
+    item = new SerialController::CallbackItem();
+    item->jetson_id = tag_id;
+    item->friendly_name = friendly_name;
+
+    SerialController::CallbackItem* tmp = this->FindInternal(friendly_name);
+    if (tmp != nullptr)
     {
-        item = new SerialController::CallbackItem();
-        item->jetson_id = tag_id;
-        item->friendly_name = friendly_name;
-        this->Callbacks[Callbacks.size()] = item;
+        item->callback = tmp->callback;
     }
 
+    this->tmpCallbacks[tag_id] = item;
+
     if (step == 0)
     {
+        // john fix
+        this->Callbacks = this->tmpCallbacks;
         this->isCollectingTags = false;
         this->tagExchange = true;
     }
 }
 
+SerialController::CallbackItem* TerriBull::SerialController::FindInternal(std::string tag_name)
+{
+    for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
+    {
+        SerialController::CallbackItem *item = it->second;
+        if (item->friendly_name == tag_name)
+        {
+            return item;
+        }
+    }
+    //return nullptr;
+}
+
 int TerriBull::SerialController::GetCallbackIndex(std::string tag_name)
 {
     for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
@@ -348,7 +363,7 @@ int TerriBull::SerialController::GetCallbackIndex(std::string tag_name)
                 return item->jetson_id + SerialController::__packet_index_offset;
         }
     }
-    return this->Callbacks.size() + SerialController::__packet_index_offset;
+    return -1;
 }
 
 void TerriBull::SerialController::SendData(std::string data)

From c02026cec7e18ce1c4209482c373c1ee92011170 Mon Sep 17 00:00:00 2001
From: TheGuy920 <53882381+TheGuy920@users.noreply.github.com>
Date: Mon, 24 Apr 2023 04:12:13 -0400
Subject: [PATCH 9/9] fucking string compre does not work what the fuck

---
 .../SerialController/SerialController.hpp       |  2 +-
 src/Controllers/SerialController.cpp            | 17 +++++++++++++----
 src/main.cpp                                    |  2 +-
 3 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/include/Controllers/SerialController/SerialController.hpp b/include/Controllers/SerialController/SerialController.hpp
index 7710426..62edbca 100644
--- a/include/Controllers/SerialController/SerialController.hpp
+++ b/include/Controllers/SerialController/SerialController.hpp
@@ -28,7 +28,7 @@ class TerriBull::SerialController {
     typedef struct {
         PacketCallback callback;
         std::string friendly_name;
-        int jetson_id;
+        int jetson_id = -1;
     } CallbackItem;
 
     typedef struct {
diff --git a/src/Controllers/SerialController.cpp b/src/Controllers/SerialController.cpp
index 0d58b88..69ee07e 100644
--- a/src/Controllers/SerialController.cpp
+++ b/src/Controllers/SerialController.cpp
@@ -297,11 +297,13 @@ void TerriBull::SerialController::DeserializePacket()
  * @param start_index
  * @param length
  */
+int k =1;
 void TerriBull::SerialController::ProcessTagExchange(char *array, int start_index, int length)
 {
     if (!this->isCollectingTags)
     {
         this->isCollectingTags = true;
+        this->tmpCallbacks.clear();
     }
     
     int ind = start_index + 1;
@@ -326,12 +328,18 @@ void TerriBull::SerialController::ProcessTagExchange(char *array, int start_inde
         item->callback = tmp->callback;
     }
 
+    // pros::lcd::set_text(k++, (to_string(tag_id)+":"+item->friendly_name).c_str());
+
     this->tmpCallbacks[tag_id] = item;
 
     if (step == 0)
     {
-        // john fix
-        this->Callbacks = this->tmpCallbacks;
+        this->Callbacks.clear();
+        for (auto it = this->tmpCallbacks.begin(); it!= this->tmpCallbacks.end(); ++it)
+        {
+            this->Callbacks[it->first] = it->second;
+        }
+        this->tmpCallbacks.clear();
         this->isCollectingTags = false;
         this->tagExchange = true;
     }
@@ -342,7 +350,7 @@ SerialController::CallbackItem* TerriBull::SerialController::FindInternal(std::s
     for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
     {
         SerialController::CallbackItem *item = it->second;
-        if (item->friendly_name == tag_name)
+        if (item->friendly_name.find(tag_name) != std::string::npos)
         {
             return item;
         }
@@ -355,7 +363,8 @@ int TerriBull::SerialController::GetCallbackIndex(std::string tag_name)
     for (auto it = this->Callbacks.begin(); it != this->Callbacks.end(); ++it)
     {
         SerialController::CallbackItem *item = it->second;
-        if (item->friendly_name == tag_name)
+
+        if (item->friendly_name.find(tag_name) != std::string::npos )
         {
             if (item->jetson_id == -1) /* We have not initialized the Packet index*/
                 return it->first + SerialController::__packet_index_offset;
diff --git a/src/main.cpp b/src/main.cpp
index a73d7c2..3435a55 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -111,7 +111,7 @@ void initialize() {
 		serialController->ReadBuffer();
 
 		stringstream stream;
-		stream << (unsigned char) (serialController->GetCallbackIndex("test_function") - 2);
+		stream << (unsigned char) (serialController->GetCallbackIndex("test_function"));
 		stream << SerialController::SerializeNumber(654);
 		stream << SerialController::SerializeString("this is a super long string that we are sending over serial that i hope will just be over 255 characters to ensure strings are working properly");
 		stream << SerialController::SerializeNumber(1);