refactor: reorganize & add debugging

10 files changed, 322 insertions, 0 deletions

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/utils/time.cpp b/src/utils/time.cpp
new file mode 100644
index 0000000..adc33db
--- /dev/null
+++ b/src/utils/time.cpp
@@ -0,0 +1,37 @@
+#include "time.hpp"
+#include "Arduino.h"
+
+Time::Time(unsigned long time_micros)
+{
+	_time_micros = time_micros;
+}
+
+void Time::update()
+{
+	_time_micros = micros();
+}
+
+Time Time::diff(Time prev_time)
+{
+	return Time(_time_micros - prev_time.microsecs());
+}
+
+unsigned long Time::microsecs()
+{
+	return _time_micros;
+}
+
+unsigned long Time::millisecs()
+{
+	return _time_micros * 0.001;
+}
+
+float Time::secs()
+{
+	return _time_micros * 0.000001;
+}
+
+Time time_now()
+{
+	return Time(micros());
+}
diff --git a/src/utils/time.hpp b/src/utils/time.hpp
new file mode 100644
index 0000000..e0385ef
--- /dev/null
+++ b/src/utils/time.hpp
@@ -0,0 +1,53 @@
+#ifndef TIME_HPP
+#define TIME_HPP
+
+/**
+ * A representation of time.
+ */
+class Time
+{
+public:
+	/**
+	 * A representation of time.
+	 *
+	 * @param time_micros Time in microseconds
+	 */
+	Time(unsigned long time_micros);
+
+	/**
+	 * Updates the time to the current time.
+	 */
+	void update();
+
+	/**
+	 * Returns the difference between two points in time.
+	 *
+	 * @param prev_time A previous point in time
+	 */
+	Time diff(Time prev_time);
+
+	/**
+	 * Returns the time in seconds.
+	 */
+	float secs();
+
+	/**
+	 * Returns the time in milliseconds.
+	 */
+	unsigned long millisecs();
+
+	/**
+	 * Returns the time in microseconds.
+	 */
+	unsigned long microsecs();
+
+private:
+	unsigned long _time_micros;
+};
+
+/**
+ * Returns a time object for the time since the program started.
+ */
+Time time_now();
+
+#endif