refactor: reorganize & add debugging

16 files changed, 408 insertions, 141 deletions

diff --git a/src/calibration.cpp b/src/calibration.cpp
index f76d8f8..a772ae9 100644
--- a/src/calibration.cpp
+++ b/src/calibration.cpp
@@ -1,9 +1,11 @@
 #include "calibration.hpp"
-#include "time_utils.hpp"
 
-SensorCalibrator::SensorCalibrator(Sensor *sensor)
+#include "utils/general.hpp"
+#include "utils/time.hpp"
+
+SensorCalibrator::SensorCalibrator(unique_ptr<Sensor> &sensor, SerialStream sout)
+	: _sensor(sensor), _sout(sout)
 {
-	_sensor = sensor;
 	_resetValues();
 }
 
@@ -15,7 +17,9 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time)
 	while (!done)
 	{
 		if (time_now().diff(start_time).millisecs() >= CALIBRATION_TIMEOUT)
+		{
 			return false;
+		}
 
 		delay(throttle_time);
 
@@ -28,19 +32,12 @@ bool SensorCalibrator::calibrate(unsigned int throttle_time)
 
 		_adjustValues();
 
-		Serial.print("Accel X: ");
-		Serial.print(_accel_x);
-		Serial.print("	Accel Y: ");
-		Serial.print(_accel_y);
-		Serial.print("	Accel Z: ");
-		Serial.print(_accel_z);
-
-		Serial.print("	Gyro X: ");
-		Serial.print(_gyro_x);
-		Serial.print("	Gyro Y: ");
-		Serial.print(_gyro_y);
-		Serial.print("	Gyro Z: ");
-		Serial.println(_gyro_z);
+		_sout << "Accel X: " << _accel_x << "	"
+			  << "Accel Y: " << _accel_y << "	"
+			  << "Accel Z: " << _accel_z << "	"
+			  << "Gyro X: " << _gyro_x << "	"
+			  << "Gyro Y: " << _gyro_y << "	"
+			  << "Gyro Z: " << _gyro_z << endl;
 
 		if (_isValuesInRange())
 		{
diff --git a/src/calibration.hpp b/src/calibration.hpp
index 79b3758..e66b6fc 100644
--- a/src/calibration.hpp
+++ b/src/calibration.hpp
@@ -1,7 +1,9 @@
 #ifndef CALIBRATION_HPP
 #define CALIBRATION_HPP
 
-#include "sensor.hpp"
+#include "sensor/sensor.hpp"
+#include "utils/memory.hpp"
+#include "utils/serial.hpp"
 
 // Calibration precision
 #define ACCEL_CAL_X_MAX 0.006
@@ -37,8 +39,9 @@ public:
 	 * Sensor calibrator.
 	 *
 	 * @param sensor A sensor to calibrate
+	 * @param sout A Serial output stream
 	 */
-	SensorCalibrator(Sensor *sensor);
+	SensorCalibrator(unique_ptr<Sensor> &sensor, SerialStream sout);
 
 	/**
 	 * Calibrates the sensor.
@@ -57,7 +60,9 @@ private:
 
 	void _adjustCalibration();
 
-	Sensor *_sensor;
+	unique_ptr<Sensor> &_sensor;
+
+	SerialStream _sout;
 
 	float _accel_x;
 	float _accel_y;
diff --git a/src/gyronardo.cpp b/src/gyronardo.cpp
index f1e968e..53a1912 100644
--- a/src/gyronardo.cpp
+++ b/src/gyronardo.cpp
@@ -1,94 +1,87 @@
 #include "Arduino.h"
+
 #include "Wire.h"
 #include "calibration.hpp"
-#include "sensor.hpp"
+#include "sensor/sensor.hpp"
+#include "utils/memory.hpp"
+
+#include "utils/serial.hpp"
 
 #define LINE_CLEAR_LENGTH 30
 
+#define SENSOR_ADDRESS 0x68
 #define SENSOR_THROTTLE_TIME 50 // milliseconds
 
-Sensor sensor(0x68, &Wire, SENSOR_THROTTLE_TIME);
-
-char line_clear[LINE_CLEAR_LENGTH + 1];
+unique_ptr<SerialStream> sout_ptr = nullptr;
 
-/**
- * Stops code execution.
- */
-void stop()
-{
-	while (true) {}
-}
+unique_ptr<Sensor> sensor = nullptr;
 
 void setup()
 {
-	Serial.begin(9600);
+	sout_ptr = make_unique<SerialStream>(Serial, 9600);
 
-	// Wait for Serial because the Arduino Leonardo is weird
-	while (!Serial) {}
+	auto sout = *sout_ptr;
 
-	while (!sensor.begin())
+	sensor = make_unique<Sensor>(SENSOR_ADDRESS, &Wire, sout, SENSOR_THROTTLE_TIME);
+
+	while (!sensor->begin())
 	{
-		Serial.println("Error: Could not connect to the sensor. Retrying after 2000 "
-					   "milliseconds...");
+		sout << "Error: Could not connect to the sensor. Retrying after 2000 "
+				"milliseconds..."
+			 << endl;
+
 		delay(2000);
 	}
 
-	if (!sensor.setAccelSensitivity(2)) // 8g
+	sout << "Skit!" << endl;
+
+	if (!sensor->setAccelSensitivity(2)) // 8g
 	{
-		Serial.print(
-			"Error: Failed to set the sensor's accelerometer sensitivity. Status: ");
-		Serial.println(static_cast<int>(sensor.getStatus()));
+		sout << "Error: Failed to set the sensor's accelerometer sensitivity. Status: "
+			 << static_cast<int>(sensor->getStatus()) << endl;
+
 		stop();
 	}
 
-	if (!sensor.setGyroSensitivity(1)) // 500 degrees/s
+	if (!sensor->setGyroSensitivity(1)) // 500 degrees/s
 	{
-		Serial.print("Error: Failed to set the sensor's gyro sensitivity. Status: ");
-		Serial.println(static_cast<int>(sensor.getStatus()));
+		sout << "Error: Failed to set the sensor's gyro sensitivity. Status: "
+			 << static_cast<int>(sensor->getStatus()) << endl;
+
 		stop();
 	}
 
-	Serial.println("Calibrating sensor...");
-	SensorCalibrator sensor_calibrator(&sensor);
+	sout << "Calibrating sensor..." << endl;
+
+	SensorCalibrator sensor_calibrator(sensor, sout);
 
 	if (!sensor_calibrator.calibrate(SENSOR_THROTTLE_TIME))
 	{
-		Serial.print("Error: Sensor calibration timed out after ");
-		Serial.print(CALIBRATION_TIMEOUT);
-		Serial.println(" milliseconds");
+		sout << "Error: Sensor calibration timed out after " << CALIBRATION_TIMEOUT
+			 << " milliseconds" << endl;
 
 		stop();
 	}
-	Serial.println("Finished calibrating sensor");
-
-	Serial.println("Calibration values:");
-
-	Serial.print("Accelerometer X: ");
-	Serial.println(sensor.accel_cal_x);
-
-	Serial.print("Accelerometer Y: ");
-	Serial.println(sensor.accel_cal_y);
-
-	Serial.print("Accelerometer Z: ");
-	Serial.println(sensor.accel_cal_z);
 
-	Serial.print("Gyro X: ");
-	Serial.println(sensor.gyro_cal_x);
+	sout << "Finished calibrating sensor\n";
 
-	Serial.print("Gyro Y: ");
-	Serial.println(sensor.gyro_cal_y);
+	sout << "Calibration values:\n"
+		 << "Accelerometer X: " << sensor->accel_cal_x << "\n"
+		 << "Accelerometer Y: " << sensor->accel_cal_y << "\n"
+		 << "Accelerometer Z: " << sensor->accel_cal_z << "\n"
+		 << "Gyro X: " << sensor->gyro_cal_x << "\n"
+		 << "Gyro Y: " << sensor->gyro_cal_y << "\n"
+		 << "Gyro Z: " << sensor->gyro_cal_z << "\n";
 
-	Serial.print("Gyro Z: ");
-	Serial.println(sensor.gyro_cal_z);
+	sout << "Starting..." << endl;
 
-	Serial.println("\nStarting...");
-
-	size_t prev_len = strlen(line_clear);
-	memset(line_clear + prev_len, ' ', LINE_CLEAR_LENGTH - prev_len);
+	sout << "Pitch: " << sensor->getPitch() << "  "
+		 << "Roll: " << sensor->getRoll() << endl;
 }
 
 void loop()
 {
+	/*
 	delay(SENSOR_THROTTLE_TIME);
 
 	if (!sensor.read())
@@ -97,24 +90,25 @@ void loop()
 
 		if (status == SensorStatus::THROTTLED)
 		{
-			Serial.println("Warning: The sensor was read too frequently and throttled");
+			printSerial("Warning: The sensor was read too frequently and throttled");
+			endl();
+
 			return;
 		}
 
-		Serial.print("Error: Failed to read sensor. Status: ");
-		Serial.println(static_cast<int>(status));
+		printSerial("Error: Failed to read sensor. Status: %d", static_cast<int>(status));
+		endl();
+
 		stop();
 	}
 
-	float pitch = sensor.getPitch();
-	float roll = sensor.getRoll();
+	char *pitch = floatToStr(sensor.getPitch(), FLOAT_WIDTH, FLOAT_PRECISION);
+	char *roll = floatToStr(sensor.getRoll(), FLOAT_WIDTH, FLOAT_PRECISION);
 
-	Serial.print(line_clear);
-	Serial.print("\r");
-	Serial.print("Pitch: ");
-	Serial.print(pitch, 3);
-	Serial.print(" Roll: ");
-	Serial.print(roll, 3);
-	Serial.print("\r");
+	printSerial("\rPitch: %s	Roll: %s", pitch, roll);
 	Serial.flush();
+
+	free(pitch);
+	free(roll);
+	*/
 }
diff --git a/src/sensor_registers.hpp b/src/sensor/registers.hpp
index c1b19b4..c1b19b4 100644
--- a/src/sensor_registers.hpp
+++ b/src/sensor/registers.hpp
diff --git a/src/sensor.cpp b/src/sensor/sensor.cpp
index 9d4157e..e11a74b 100644
--- a/src/sensor.cpp
+++ b/src/sensor/sensor.cpp
@@ -1,9 +1,11 @@
 #include "sensor.hpp"
-#include "math.h"
-#include "sensor_registers.hpp"
-#include "time_utils.hpp"
+#include "../utils/serial.hpp"
+#include "../utils/time.hpp"
+#include "registers.hpp"
 
-Sensor::Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time) : _last_time(0)
+Sensor::Sensor(uint8_t address, TwoWire *wire, SerialStream sout,
+			   unsigned int throttle_time)
+	: _last_time(0), _sout(sout)
 {
 	_address = address;
 	_wire = wire;
@@ -100,14 +102,24 @@ bool Sensor::read()
 	}
 
 	// Accelerometer
-	_accel_raw_x = _readTwoBytes(); // ACCEL_XOUT_H  ACCEL_XOUT_L
-	_accel_raw_y = _readTwoBytes(); // ACCEL_YOUT_H  ACCEL_YOUT_L
-	_accel_raw_z = _readTwoBytes(); // ACCEL_ZOUT_H  ACCEL_ZOUT_L
+	_accel_raw_x = _readHighLow();
+	_accel_raw_y = _readHighLow();
+	_accel_raw_z = _readHighLow();
+
+	_sout << "\nAccel raw x: " << _accel_raw_x << "\n"
+		  << "Accel raw y: " << _accel_raw_y << "\n"
+		  << "Accel raw z: " << _accel_raw_z << "\n"
+		  << endl;
 
 	// Gyroscope
-	_gyro_raw_x = _readTwoBytes(); // GYRO_XOUT_H   GYRO_XOUT_L
-	_gyro_raw_y = _readTwoBytes(); // GYRO_YOUT_H   GYRO_YOUT_L
-	_gyro_raw_z = _readTwoBytes(); // GYRO_ZOUT_H   GYRO_ZOUT_L
+	_gyro_raw_x = _readHighLow();
+	_gyro_raw_y = _readHighLow();
+	_gyro_raw_z = _readHighLow();
+
+	_sout << "\nGyro raw x: " << _gyro_raw_x << "\n"
+		  << "Gyro raw y: " << _gyro_raw_y << "\n"
+		  << "Gyro raw z: " << _gyro_raw_z << "\n"
+		  << endl;
 
 	// Duration interval
 	now.update();
@@ -119,67 +131,93 @@ bool Sensor::read()
 	_accel_raw_y *= _accel_to_g_force;
 	_accel_raw_z *= _accel_to_g_force;
 
+	_sout << "\nAccel raw x g:s: " << _accel_raw_x << "\n"
+		  << "Accel raw y g:s: " << _accel_raw_y << "\n"
+		  << "Accel raw z g:s: " << _accel_raw_z << "\n"
+		  << endl;
+
 	// Error correct raw acceleration
 	_accel_raw_x += accel_cal_x;
 	_accel_raw_y += accel_cal_y;
 	_accel_raw_z += accel_cal_z;
 
+	_sout << "\nAccel raw x correct: " << _accel_raw_x << "\n"
+		  << "Accel raw y correct: " << _accel_raw_y << "\n"
+		  << "Accel raw z correct: " << _accel_raw_z << "\n"
+		  << endl;
+
 	// Prepare for Pitch Roll Yaw
-	float accel_x_pow_two = pow(_accel_raw_x, 2);
 	float accel_y_pow_two = pow(_accel_raw_y, 2);
 	float accel_z_pow_two = pow(_accel_raw_z, 2);
 
+	_accel_x = atan2(_accel_raw_y, _accel_raw_z) * 180 / PI;
+
+	_accel_y = atan2(-_accel_raw_x, sqrt(accel_y_pow_two + accel_z_pow_two)) * 180 / PI;
+
+	/*
 	_accel_x =
 		atan(_accel_raw_y / sqrt(accel_x_pow_two + accel_z_pow_two)) * RAD_TO_DEGREES;
 
 	_accel_y = atan(-1.0 * _accel_raw_x / sqrt(accel_y_pow_two + accel_z_pow_two)) *
 			   RAD_TO_DEGREES;
+	*/
 
 	// Convert raw Gyro to degrees/s
 	_gyro_raw_x *= _ang_rate_to_dps;
 	_gyro_raw_y *= _ang_rate_to_dps;
 	_gyro_raw_z *= _ang_rate_to_dps;
 
+	_sout << "\nGyro raw x dps: " << _gyro_raw_x << "\n"
+		  << "Gyro raw y dps: " << _gyro_raw_y << "\n"
+		  << "Gyro raw z dps: " << _gyro_raw_z << "\n"
+		  << endl;
+
 	// Error correct raw gyro measurements.
 	_gyro_raw_x += gyro_cal_x;
 	_gyro_raw_y += gyro_cal_y;
 	_gyro_raw_z += gyro_cal_z;
 
+	_sout << "\nGyro raw x correct: " << _gyro_raw_x << "\n"
+		  << "Gyro raw y correct: " << _gyro_raw_y << "\n"
+		  << "Gyro raw z correct: " << _gyro_raw_z << "\n"
+		  << endl;
+
 	_gyro_x += _gyro_raw_x * duration;
 	_gyro_y += _gyro_raw_y * duration;
 	_gyro_z += _gyro_raw_z * duration;
 
-	_yaw = _gyro_z;
+	_sout << "\nGyro raw x w/o time: " << _gyro_raw_x << "\n"
+		  << "Gyro raw y w/o time: " << _gyro_raw_y << "\n"
+		  << "Gyro raw z w/o time: " << _gyro_raw_z << "\n"
+		  << endl;
+
 	_pitch = 0.96 * _gyro_y + 0.04 * _accel_y;
 	_roll = 0.96 * _gyro_x + 0.04 * _accel_x;
+	_yaw = _gyro_z;
 
 	return true;
 }
 
 bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 {
-	_accel_sensitivity = sensitivity;
+	if (sensitivity > 3)
+		sensitivity = 3;
 
-	if (_accel_sensitivity > 3)
-		_accel_sensitivity = 3;
+	_accel_sensitivity = sensitivity;
 
-	uint8_t val = getRegister(SensorRegisters::ACCEL_CONFIG);
+	uint8_t accel_config = getRegister(SensorRegisters::ACCEL_CONFIG);
 
 	if (_status != SensorStatus::OK)
-	{
 		return false;
-	}
 
 	// No need to write same value
-	if (((val >> 3) & 3) != _accel_sensitivity)
+	if (((accel_config >> 3) & 3) != _accel_sensitivity)
 	{
-		val &= 0xE7;
-		val |= (_accel_sensitivity << 3);
+		accel_config &= 0xE7;
+		accel_config |= (_accel_sensitivity << 3);
 
-		if (!setRegister(SensorRegisters::ACCEL_CONFIG, val))
-		{
+		if (!setRegister(SensorRegisters::ACCEL_CONFIG, accel_config))
 			return false;
-		}
 	}
 
 	// Calculate conversion factor.
@@ -190,12 +228,12 @@ bool Sensor::setAccelSensitivity(uint8_t sensitivity)
 
 bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 {
-	_gyro_sensitivity = sensitivity;
+	if (sensitivity > 3)
+		sensitivity = 3;
 
-	if (_gyro_sensitivity > 3)
-		_gyro_sensitivity = 3;
+	_gyro_sensitivity = sensitivity;
 
-	uint8_t val = getRegister(SensorRegisters::GYRO_CONFIG);
+	uint8_t gyro_config = getRegister(SensorRegisters::GYRO_CONFIG);
 
 	if (_status != SensorStatus::OK)
 	{
@@ -203,12 +241,12 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 	}
 
 	// No need to write same value
-	if (((val >> 3) & 3) != _gyro_sensitivity)
+	if (((gyro_config >> 3) & 3) != _gyro_sensitivity)
 	{
-		val &= 0xE7;
-		val |= (_gyro_sensitivity << 3);
+		gyro_config &= 0xE7;
+		gyro_config |= (_gyro_sensitivity << 3);
 
-		if (!setRegister(SensorRegisters::GYRO_CONFIG, val))
+		if (!setRegister(SensorRegisters::GYRO_CONFIG, gyro_config))
 		{
 			return false;
 		}
@@ -222,52 +260,52 @@ bool Sensor::setGyroSensitivity(uint8_t sensitivity)
 float Sensor::getAccelX()
 {
 	return _accel_raw_x;
-};
+}
 
 float Sensor::getAccelY()
 {
 	return _accel_raw_y;
-};
+}
 
 float Sensor::getAccelZ()
 {
 	return _accel_raw_z;
-};
+}
 
 float Sensor::getAngleX()
 {
 	return _accel_x;
-};
+}
 
 float Sensor::getAngleY()
 {
 	return _accel_y;
-};
+}
 
 float Sensor::getGyroX()
 {
 	return _gyro_raw_x;
-};
+}
 
 float Sensor::getGyroY()
 {
 	return _gyro_raw_y;
-};
+}
 
 float Sensor::getGyroZ()
 {
 	return _gyro_raw_z;
-};
+}
 
 float Sensor::getPitch()
 {
 	return _pitch;
-};
+}
 
 float Sensor::getRoll()
 {
 	return _roll;
-};
+}
 
 bool Sensor::setRegister(uint8_t reg, uint8_t value)
 {
@@ -302,9 +340,7 @@ uint8_t Sensor::getRegister(uint8_t reg)
 		return 0;
 	}
 
-	uint8_t val = _wire->read();
-
-	return val;
+	return _wire->read();
 }
 
 SensorStatus Sensor::getStatus()
@@ -313,13 +349,12 @@ SensorStatus Sensor::getStatus()
 	_status = SensorStatus::OK;
 
 	return status;
-};
+}
 
-int16_t Sensor::_readTwoBytes()
+int16_t Sensor::_readHighLow()
 {
-	int16_t response = _wire->read();
-	response <<= 8;
-	response |= _wire->read();
+	int16_t high = _wire->read();
+	int16_t low = _wire->read();
 
-	return response;
+	return high << 8 | low;
 }
diff --git a/src/sensor.hpp b/src/sensor/sensor.hpp
index 72ff396..c982fac 100644
--- a/src/sensor.hpp
+++ b/src/sensor/sensor.hpp
@@ -1,13 +1,14 @@
 #ifndef SENSOR_HPP
 #define SENSOR_HPP
 
-#include "Arduino.h"
+#include "../utils/serial.hpp"
+#include "../utils/time.hpp"
+
 #include "Wire.h"
-#include "time_utils.hpp"
+#include <Arduino.h>
 
 #define SENSOR_WAKEUP 0x00
 
-#define RAD_TO_DEGREES (180.0 / PI)
 #define RAW_TO_DPS_FACTOR (1.0 / 131.0)
 #define RAW_TO_G_FACTOR (1.0 / 16384.0)
 
@@ -31,9 +32,10 @@ public:
 	 *
 	 * @param address The address of the sensor
 	 * @param wire A Wire instance
+	 * @param sout A serial output stream
 	 * @param throttle_time A minumum time between sensor reads for the sensor to throttle
 	 */
-	Sensor(uint8_t address, TwoWire *wire, unsigned int throttle_time);
+	Sensor(uint8_t address, TwoWire *wire, SerialStream sout, unsigned int throttle_time);
 
 	/**
 	 * Initializes communication with the sensor.
@@ -181,9 +183,11 @@ private:
 
 	float _pitch, _roll, _yaw;
 
-	int16_t _readTwoBytes();
+	int16_t _readHighLow();
 
 	TwoWire *_wire;
+
+	SerialStream _sout;
 };
 
 #endif
diff --git a/src/utils/general.cpp b/src/utils/general.cpp
new file mode 100644
index 0000000..bbcd4dd
--- /dev/null
+++ b/src/utils/general.cpp
@@ -0,0 +1,14 @@
+#include "general.hpp"
+
+void stop()
+{
+	while (true) {}
+}
+
+unique_ptr<SmartString> floatToStr(float num, unsigned int width, unsigned int precision)
+{
+	auto str = make_unique<SmartString>(width + precision);
+	dtostrf(num, width, precision, str->c_str);
+
+	return str;
+}
diff --git a/src/utils/general.hpp b/src/utils/general.hpp
new file mode 100644
index 0000000..9ad0655
--- /dev/null
+++ b/src/utils/general.hpp
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "memory.hpp"
+#include "smart_string.hpp"
+
+/**
+ * Stops code execution.
+ */
+void stop();
+
+/**
+ * Converts a floating point number to a string.
+ *
+ * @param num A floating point number
+ * @param width The desired float width
+ * @param precision The desired float precision
+ * @returns The float as a string.
+ */
+unique_ptr<SmartString> floatToStr(
+	float num,
+	unsigned int width = 3,
+	unsigned int precision = 2
+);
diff --git a/src/utils/memory.hpp b/src/utils/memory.hpp
new file mode 100644
index 0000000..81e3757
--- /dev/null
+++ b/src/utils/memory.hpp
@@ -0,0 +1,23 @@
+#pragma once
+
+template <typename memType>
+memType *malloc_s(unsigned int size);
+
+template <class Target>
+class unique_ptr
+{
+public:
+	unique_ptr(Target *target);
+	~unique_ptr();
+
+	Target operator *() const;
+	Target *operator ->() const;
+
+private:
+	Target *_target;
+};
+
+template<class Target, typename...Args>
+unique_ptr<Target> make_unique(Args&... args);
+
+#include "memory.tpp"
diff --git a/src/utils/memory.tpp b/src/utils/memory.tpp
new file mode 100644
index 0000000..276b0b4
--- /dev/null
+++ b/src/utils/memory.tpp
@@ -0,0 +1,48 @@
+#include "memory.hpp"
+
+#include "Arduino.h"
+#include "general.hpp"
+
+template <typename memType>
+memType *malloc_s(unsigned int size)
+{
+	auto *mem = malloc(size);
+
+	if (mem == nullptr)
+	{
+		Serial.println("Error: Memory allocation failed");
+		while (true) {}
+	}
+
+	return static_cast<memType *>(mem);
+}
+
+template <class Target>
+unique_ptr<Target>::unique_ptr(Target *target)
+{
+	this->_target = target;
+}
+
+template <class Target>
+unique_ptr<Target>::~unique_ptr()
+{
+	delete this->_target;
+}
+
+template <class Target>
+Target unique_ptr<Target>::operator*() const
+{
+	return *(this->_target);
+}
+
+template <class Target>
+Target *unique_ptr<Target>::operator->() const
+{
+	return this->_target;
+}
+
+template <class Target, typename... Args>
+unique_ptr<Target> make_unique(Args... args)
+{
+	return unique_ptr<Target>(new Target(args...));
+}
diff --git a/src/utils/serial.cpp b/src/utils/serial.cpp
new file mode 100644
index 0000000..11be550
--- /dev/null
+++ b/src/utils/serial.cpp
@@ -0,0 +1,57 @@
+#include "serial.hpp"
+
+#include "general.hpp"
+
+SerialStream::SerialStream(Serial_ serial, unsigned long baud_rate)
+{
+	this->_serial = serial;
+
+	this->_serial.begin(baud_rate);
+
+	while (!this->_serial) {}
+}
+
+SerialStream::~SerialStream()
+{
+	Serial.end();
+}
+
+SerialStream &SerialStream::operator<<(const char *str)
+{
+	this->write(str);
+	return *this;
+}
+
+SerialStream &SerialStream::operator<<(const SmartString &str)
+{
+	this->write(str.c_str);
+	return *this;
+}
+
+SerialStream &SerialStream::operator<<(const float num)
+{
+	this->write(floatToStr(num)->c_str);
+	return *this;
+}
+
+SerialStream &SerialStream::operator<<(void (*manipulator)(SerialStream *))
+{
+	manipulator(this);
+	return *this;
+}
+
+void SerialStream::write(const char *str)
+{
+	this->_serial.write(str);
+}
+
+void SerialStream::flush()
+{
+	this->_serial.flush();
+}
+
+void endl(SerialStream *serial_stream)
+{
+	serial_stream->write("\n");
+	serial_stream->flush();
+}
diff --git a/src/utils/serial.hpp b/src/utils/serial.hpp
new file mode 100644
index 0000000..f97e1d8
--- /dev/null
+++ b/src/utils/serial.hpp
@@ -0,0 +1,27 @@
+#pragma once
+
+#include "smart_string.hpp"
+
+#include <USBAPI.h>
+
+class SerialStream
+{
+public:
+	SerialStream(Serial_ serial, unsigned long baud_rate);
+	~SerialStream();
+
+	SerialStream &operator<<(const char *str);
+	SerialStream &operator<<(const SmartString &str);
+	SerialStream &operator<<(const float num);
+
+	SerialStream &operator<<(void (*manipulator)(SerialStream *));
+
+	void write(const char *str);
+
+	void flush();
+
+private:
+	Serial_ _serial;
+};
+
+void endl(SerialStream *serial_stream);
diff --git a/src/utils/smart_string.cpp b/src/utils/smart_string.cpp
new file mode 100644
index 0000000..5258ffe
--- /dev/null
+++ b/src/utils/smart_string.cpp
@@ -0,0 +1,26 @@
+#include "smart_string.hpp"
+
+#include "memory.hpp"
+
+#include <stdlib.h>
+
+SmartString::SmartString(char *c_string)
+{
+	this->c_str = c_string;
+}
+
+SmartString::SmartString(unsigned int size)
+{
+	this->c_str = malloc_s<char>(size + 1);
+}
+
+SmartString::~SmartString()
+{
+	if (this->c_str != nullptr)
+		free(this->c_str);
+}
+
+SmartString::operator char *() const
+{
+	return this->c_str;
+}
diff --git a/src/utils/smart_string.hpp b/src/utils/smart_string.hpp
new file mode 100644
index 0000000..fcaff98
--- /dev/null
+++ b/src/utils/smart_string.hpp
@@ -0,0 +1,14 @@
+#pragma once
+
+class SmartString
+{
+public:
+	SmartString(char *c_str);
+	SmartString(unsigned int size);
+
+	~SmartString();
+
+	operator char *() const;
+
+	char *c_str = nullptr;
+};
diff --git a/src/time_utils.cpp b/src/utils/time.cpp
index 84c7d96..adc33db 100644
--- a/src/time_utils.cpp
+++ b/src/utils/time.cpp
@@ -1,4 +1,4 @@
-#include "time_utils.hpp"
+#include "time.hpp"
 #include "Arduino.h"
 
 Time::Time(unsigned long time_micros)
diff --git a/src/time_utils.hpp b/src/utils/time.hpp
index f8f2eb3..e0385ef 100644
--- a/src/time_utils.hpp
+++ b/src/utils/time.hpp
@@ -1,5 +1,5 @@
-#ifndef TIME_UTILS_HPP
-#define TIME_UTILS_HPP
+#ifndef TIME_HPP
+#define TIME_HPP
 
 /**
  * A representation of time.