From 0326e1e36e35015d7da9d0b9d7cd10ab40e41c1b Mon Sep 17 00:00:00 2001
From: GHOSCHT <31184695+GHOSCHT@users.noreply.github.com>
Date: Fri, 4 Feb 2022 09:42:15 +0100
Subject: [PATCH] Implement communication

---
 .../lib/Communicator/Communicator.cpp         | 25 +++++++++++++++++++
 .../Firmware/lib/Communicator/Communicator.h  |  6 ++++-
 .../lib/Communicator/I2CCommunicator.cpp      | 16 ++++++++++--
 .../lib/Communicator/I2CCommunicator.h        |  2 +-
 .../lib/Communicator/SerialCommunicator.cpp   | 15 ++++++++++-
 .../lib/Communicator/SerialCommunicator.h     |  2 +-
 6 files changed, 60 insertions(+), 6 deletions(-)

diff --git a/Devices/Control/Firmware/lib/Communicator/Communicator.cpp b/Devices/Control/Firmware/lib/Communicator/Communicator.cpp
index c6048df..42fbea3 100644
--- a/Devices/Control/Firmware/lib/Communicator/Communicator.cpp
+++ b/Devices/Control/Firmware/lib/Communicator/Communicator.cpp
@@ -3,9 +3,34 @@
 Communicator::Communicator(__SIZE_TYPE__ bufferSize)
 {
     this->messageBuffer = new char[bufferSize];
+    this->bufferSize = bufferSize;
 }
 
 char *Communicator::getBuffer()
 {
     return messageBuffer;
+}
+
+int Communicator::getBufferSize()
+{
+    return this->bufferSize;
+}
+
+int Communicator::calculateMessageOutSize(int numberOfValues)
+{
+    return numberOfValues + (numberOfValues - 1) + 1;
+}
+
+void Communicator::parseIDs(int *values, int numberOfValues, char *output)
+{
+    __SIZE_TYPE__ outputSize = calculateMessageOutSize(numberOfValues);
+    __SIZE_TYPE__ outputCharPointer = 0;
+    output[outputSize - 1] = '\0';
+
+    for (__SIZE_TYPE__ i = 0; i < numberOfValues; i++)
+    {
+        output[outputCharPointer++] = values[i] + '0';
+        if (outputCharPointer < outputSize - 1)
+            output[outputCharPointer++] = ',';
+    }
 }
\ No newline at end of file
diff --git a/Devices/Control/Firmware/lib/Communicator/Communicator.h b/Devices/Control/Firmware/lib/Communicator/Communicator.h
index 21ae783..91d59a3 100644
--- a/Devices/Control/Firmware/lib/Communicator/Communicator.h
+++ b/Devices/Control/Firmware/lib/Communicator/Communicator.h
@@ -2,11 +2,15 @@ class Communicator
 {
 private:
     char *messageBuffer;
+    __SIZE_TYPE__ bufferSize;
 
 public:
     Communicator(__SIZE_TYPE__ bufferSize);
-    virtual void sendMessage(int *values);
+    virtual void sendMessage(int *values, int numberOfValues);
     virtual void sendMessage(char *message);
     virtual char *receiveMessage();
     char *getBuffer();
+    int getBufferSize();
+    void parseIDs(int *values, int numberOfValues, char *out);
+    int calculateMessageOutSize(int numberOfValues);
 };
\ No newline at end of file
diff --git a/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.cpp b/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.cpp
index 2135b00..48837fd 100644
--- a/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.cpp
+++ b/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.cpp
@@ -4,14 +4,26 @@
 I2CCommunicator::I2CCommunicator(int slaveAddr, __SIZE_TYPE__ bufferSize) : Communicator(bufferSize)
 {
     Wire.begin(slaveAddr);
-    //Wire.onReceive(receiveEvent);
 }
 
-void I2CCommunicator::sendMessage(int *values)
+void I2CCommunicator::sendMessage(int *values, int numberOfValues)
 {
+    char message[calculateMessageOutSize(numberOfValues)];
+    parseIDs(values, numberOfValues, message);
+    Serial.println(message);
 }
 
 void I2CCommunicator::sendMessage(char *message)
 {
     Wire.println(message);
+}
+
+char *I2CCommunicator::receiveMessage()
+{
+    if (Wire.available())
+    {
+        memset(getBuffer(), '\0', getBufferSize());
+        Wire.readBytesUntil('\n', getBuffer(), getBufferSize());
+    }
+    return getBuffer();
 }
\ No newline at end of file
diff --git a/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.h b/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.h
index ce86e71..ac1ce45 100644
--- a/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.h
+++ b/Devices/Control/Firmware/lib/Communicator/I2CCommunicator.h
@@ -5,7 +5,7 @@ class I2CCommunicator : public Communicator
 private:
 public:
     I2CCommunicator(int slaveAddr, __SIZE_TYPE__ bufferSize);
-    void sendMessage(int *values);
+    void sendMessage(int *values, int numberOfValues);
     void sendMessage(char *message);
     char *receiveMessage();
 };
\ No newline at end of file
diff --git a/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.cpp b/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.cpp
index 0b790b9..5faf53a 100644
--- a/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.cpp
+++ b/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.cpp
@@ -7,11 +7,24 @@ SerialCommunicator::SerialCommunicator(int baudRate, int timeout, __SIZE_TYPE__
     Serial.setTimeout(timeout);
 }
 
-void SerialCommunicator::sendMessage(int *values)
+void SerialCommunicator::sendMessage(int *values, int numberOfValues)
 {
+    char message[calculateMessageOutSize(numberOfValues)];
+    parseIDs(values, numberOfValues, message);
+    Serial.println(message);
 }
 
 void SerialCommunicator::sendMessage(char *message)
 {
     Serial.println(message);
+}
+
+char *SerialCommunicator::receiveMessage()
+{
+    if (Serial.available())
+    {
+        memset(getBuffer(), '\0', getBufferSize());
+        Serial.readBytesUntil('\n', getBuffer(), getBufferSize());
+    }
+    return getBuffer();
 }
\ No newline at end of file
diff --git a/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.h b/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.h
index 5769dfc..d92abef 100644
--- a/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.h
+++ b/Devices/Control/Firmware/lib/Communicator/SerialCommunicator.h
@@ -5,7 +5,7 @@ class SerialCommunicator : public Communicator
 private:
 public:
     SerialCommunicator(int baudRate, int timeout, __SIZE_TYPE__ bufferSize);
-    void sendMessage(int *values);
+    void sendMessage(int *values, int numberOfValues);
     void sendMessage(char *message);
     char *receiveMessage();
 };