diff --git a/.gitignore b/.gitignore
index 186f843..2134aae 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,4 @@
 doxygen/html
 doxygen/DoxyWarnLogfile.txt
+build
+.vscode
diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000..b5f7549
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1,60 @@
+# This file is a template, and might need editing before it works on your project.
+# This is a sample GitLab CI/CD configuration file that should run without any modifications.
+# It demonstrates a basic 3 stage CI/CD pipeline. Instead of real tests or scripts,
+# it uses echo commands to simulate the pipeline execution.
+#
+# A pipeline is composed of independent jobs that run scripts, grouped into stages.
+# Stages run in sequential order, but jobs within stages run in parallel.
+#
+# For more information, see: https://docs.gitlab.com/ee/ci/yaml/index.html#stages
+#
+# You can copy and paste this template into a new `.gitlab-ci.yml` file.
+# You should not add this template to an existing `.gitlab-ci.yml` file by using the `include:` keyword.
+#
+# To contribute improvements to CI/CD templates, please follow the Development guide at:
+# https://docs.gitlab.com/ee/development/cicd/templates.html
+# This specific template is located at:
+# https://gitlab.com/gitlab-org/gitlab/-/blob/master/lib/gitlab/ci/templates/Getting-Started.gitlab-ci.yml
+
+# This file is a template, and might need editing before it works on your project.
+# This is a sample GitLab CI/CD configuration file that should run without any modifications.
+# It demonstrates a basic 3 stage CI/CD pipeline. Instead of real tests or scripts,
+# it uses echo commands to simulate the pipeline execution.
+#
+# A pipeline is composed of independent jobs that run scripts, grouped into stages.
+# Stages run in sequential order, but jobs within stages run in parallel.
+#
+# For more information, see: https://docs.gitlab.com/ee/ci/yaml/index.html#stages
+#
+# You can copy and paste this template into a new `.gitlab-ci.yml` file.
+# You should not add this template to an existing `.gitlab-ci.yml` file by using the `include:` keyword.
+#
+# To contribute improvements to CI/CD templates, please follow the Development guide at:
+# https://docs.gitlab.com/ee/development/cicd/templates.html
+# This specific template is located at:
+# https://gitlab.com/gitlab-org/gitlab/-/blob/master/lib/gitlab/ci/templates/Getting-Started.gitlab-ci.yml
+
+variables:
+  GIT_SUBMODULE_STRATEGY: recursive
+default:
+  image: rikorose/gcc-cmake
+stages:
+- test
+unit-test-job:
+  stage: test
+  script:
+  - mkdir build
+  - cd build
+  - cmake ..
+  - cmake --build .
+  - export GTEST_OUTPUT="xml:report.xml"
+  - ls -la
+  - "./RoboidControlTest"
+  artifacts:
+    when: always
+    reports:
+      junit: build/report.xml
+sast:
+  stage: test
+include:
+- template: Security/SAST.gitlab-ci.yml
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7cbff70..ee14052 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,16 +1,70 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+if(ESP_PLATFORM)
+    set(sourcedirs
+        .
+        VectorAlgebra/src
+    )
 
-set(sourcedirs
-  .
-  VectorAlgebra/src
+    set(includedirs
+        .
+        VectorAlgebra/include
+    )
+
+    idf_component_register(
+        SRC_DIRS ${sourcedirs}
+        INCLUDE_DIRS ${includedirs}
+        REQUIRES arduino
+    )
+endif()
+
+project(RoboidControl)
+add_subdirectory(VectorAlgebra)
+#add_subdirectory(test)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
 )
 
-set(includedirs
-  .
-  VectorAlgebra/include
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+include_directories(
+    .
+    VectorAlgebra/include
 )
 
-idf_component_register(
-    SRC_DIRS ${sourcedirs}
-    INCLUDE_DIRS ${includedirs}
-    REQUIRES arduino
+add_library(RoboidControl STATIC 
+    "Roboid.cpp"
+    "Perception.cpp"
+    "Propulsion.cpp"
+    "Motor.cpp"
+    "DifferentialDrive.cpp"
+    "DistanceSensor.cpp"
+    "Placement.cpp"
+    "Sensor.cpp"
+    "Switch.cpp"
+    "Thing.cpp"
 )
+
+enable_testing()
+
+add_executable(
+    RoboidControlTest
+    "test/Dummy_test.cc"
+)
+target_link_libraries(
+    RoboidControlTest
+    gtest_main
+    RoboidControl
+    VectorAlgebra
+)
+
+include(GoogleTest)
+gtest_discover_tests(RoboidControlTest)
diff --git a/RoboidControl.code-workspace b/RoboidControl.code-workspace
new file mode 100644
index 0000000..876a149
--- /dev/null
+++ b/RoboidControl.code-workspace
@@ -0,0 +1,8 @@
+{
+	"folders": [
+		{
+			"path": "."
+		}
+	],
+	"settings": {}
+}
\ No newline at end of file
diff --git a/doxygen/Doxyfile b/doxygen/Doxyfile
index 4d8f3c1..a4ff993 100644
--- a/doxygen/Doxyfile
+++ b/doxygen/Doxyfile
@@ -1420,7 +1420,7 @@ HTML_EXTRA_FILES       =
 # The default value is: AUTO_LIGHT.
 # This tag requires that the tag GENERATE_HTML is set to YES.
 
-HTML_COLORSTYLE        = AUTO_LIGHT
+HTML_COLORSTYLE        = LIGHT
 
 # The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
 # will adjust the colors in the style sheet and background images according to
diff --git a/test/.gitkeep b/test/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/test/CMakeLists_disabled.txt b/test/CMakeLists_disabled.txt
new file mode 100644
index 0000000..5cf6e5a
--- /dev/null
+++ b/test/CMakeLists_disabled.txt
@@ -0,0 +1,30 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+Project(test)
+
+set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+include_directories(include)
+
+enable_testing()
+
+add_executable(
+    VectorAlgebraTest
+    "test/Angle_test.cc"
+    "test/FloatSingle_test.cc"
+    "test/Vector2_test.cc"
+    "test/Vector3_test.cc"
+    "test/Quaternion_test.cc"
+)
+
+include(GoogleTest)
\ No newline at end of file
diff --git a/test/Dummy_test.cc b/test/Dummy_test.cc
new file mode 100644
index 0000000..abba183
--- /dev/null
+++ b/test/Dummy_test.cc
@@ -0,0 +1,25 @@
+#if GTEST
+#include <gtest/gtest.h>
+
+#include "DifferentialDrive.h"
+#include "DistanceSensor.h"
+#include "Roboid.h"
+
+TEST(Dummy, Foo) {
+  Motor motorLeft = Motor();
+  Motor motorRight = Motor();
+
+  DistanceSensor sensorLeft = DistanceSensor();
+  DistanceSensor sensorRight = DistanceSensor();
+
+  Placement sensors[] = {Placement(&sensorLeft, -30),
+                         Placement(&sensorRight, 30)};
+  Perception *perception = new Perception(sensors);
+
+  DifferentialDrive *propulsion =
+      new DifferentialDrive(Placement(&motorLeft, Vector3(-1, 0, 0)),
+                            Placement(&motorRight, Vector3(1, 0, 0)));
+
+  Roboid *roboid = new Roboid(perception, propulsion);
+}
+#endif