diff --git a/include/constants.hpp b/include/constants.hpp
new file mode 100644
index 0000000..887e20b
--- /dev/null
+++ b/include/constants.hpp
@@ -0,0 +1,15 @@
+/** @file constants.hpp
+ *
+ *  @author Cory Alexander Balaton (coryab)
+ *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
+ *
+ *  @version 1.0
+ *
+ *  @brief Library of constants
+ *
+ *  @bug No known bugs
+ * */
+#ifndef __CONST__
+#define __CONST__
+
+#endif
diff --git a/include/testlib.hpp b/include/testlib.hpp
new file mode 100644
index 0000000..9498df1
--- /dev/null
+++ b/include/testlib.hpp
@@ -0,0 +1,58 @@
+/** @file testlib.hpp
+ *
+ *  @author Cory Alexander Balaton (coryab)
+ *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
+ *
+ *  @version 1.0
+ *
+ *  @brief Function prototypes and macros that are useful.
+ *
+ *  This a small testing library that is tailored for the needs of the project.
+ *
+ *  @bug No known bugs
+ * */
+#ifndef __TESTLIB__
+#define __TESTLIB__
+
+#include <armadillo>
+#include <string>
+
+#include "utils.hpp"
+
+/** @def ASSERT(expr)
+ *  @brief A prettier assertion function.
+ *
+ *  This macro calls the m_assert function which is a more informative
+ *  assertion function than the regular assert function from cassert.
+ * */
+#define ASSERT(expr, msg)                                                      \
+    m_assert(expr, #expr, __METHOD_NAME__, __FILE__, __LINE__, msg)
+
+/** @brief Test an expression, confirm that test is ok, or abort execution.
+ *
+ *  @details This function takes in an expression and prints an OK message if
+ *  it's true, or it prints a fail message and aborts execution if it fails.
+ *
+ *  @param expr The expression to be evaluated
+ *  @param expr_str The stringified version of the expression
+ *  @param func The function name of the caller
+ *  @param file The file of the caller
+ *  @param line The line number where this function is called from
+ *  @param msg The message to be displayed
+ * */
+void m_assert(bool expr, std::string expr_str, std::string func,
+              std::string file, int line, std::string msg);
+
+/** @brief Test if two armadillo vectors are close to each other.
+ *
+ *  @details This function takes in 2 vectors and checks if they are
+ *  approximately equal to each other given a tolerance.
+ *
+ *  @param a Vector a
+ *  @param b Vector b
+ *  @param tol The tolerance
+ *
+ *  @return bool
+ * */
+bool close_to(arma::vec &a, arma::vec &b, double tol = 1e-8);
+#endif
diff --git a/include/typedefs.hpp b/include/typedefs.hpp
new file mode 100644
index 0000000..90bc0f5
--- /dev/null
+++ b/include/typedefs.hpp
@@ -0,0 +1,18 @@
+/** @file typedefs.hpp
+ *
+ *  @author Cory Alexander Balaton (coryab)
+ *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
+ *
+ *  @version 1.0
+ *
+ *  @brief Useful typedefs for cleaner code.
+ *
+ *  @details These typedefs make the code more readable and easy to follow
+ *  along.
+ *
+ *  @bug No known bugs
+ * */
+#ifndef __TYPEDEFS__
+#define __TYPEDEFS__
+
+#endif
diff --git a/include/utils.hpp b/include/utils.hpp
new file mode 100644
index 0000000..9283735
--- /dev/null
+++ b/include/utils.hpp
@@ -0,0 +1,103 @@
+/** @file utils.hpp
+ *
+ *  @author Cory Alexander Balaton (coryab)
+ *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
+ *
+ *  @version 1.0
+ *
+ *  @brief Function prototypes and macros that are useful.
+ *
+ *  These utility function are mainly for convenience and aren't directly
+ *  related to the project.
+ *
+ *  @bug No known bugs
+ * */
+#ifndef __UTILS__
+#define __UTILS__
+
+#include <armadillo>
+#include <iomanip>
+#include <sstream>
+#include <string>
+#include <sys/stat.h>
+#include <vector>
+
+/** @def DEBUG(msg)
+ *  @brief Writes a debug message
+ *
+ *  This macro writes a debug message that includes the filename,
+ *  line number, and a custom message. The function is wrapped in an ifdef
+ *  that checks if DBG is defined, so one can choose to display the debug
+ *  messages by adding the -DDBG flag when compiling.
+ * */
+#ifdef DBG
+#define DEBUG(msg)                                                             \
+    std::cout << __FILE__ << " " << __LINE__ << ": " << msg << std::endl
+#else
+#define DEBUG(msg)
+#endif
+
+/** @def __METHOD_NAME__
+ *  @brief Get the name of the current method/function without the return type.
+ * */
+#define __METHOD_NAME__ methodName(__PRETTY_FUNCTION__)
+
+/** @brief Turns a double into a string written in scientific format.
+ *
+ *  @details The code is stolen from https://github.com/anderkve/FYS3150.
+ *
+ *  @param d The number to stringify
+ *  @param width The reserved width of the string
+ *  @param prec The precision of the stringified number
+ *
+ *  @return std::string
+ * */
+std::string scientific_format(double d, int width = 20, int prec = 10);
+
+/** @brief Turns a vector of doubles into a string written in scientific
+ * format.
+ *
+ *  @details The code is stolen from https://github.com/anderkve/FYS3150.
+ *
+ *  @param v The vector to stringify
+ *  @param width The reserved width of the string
+ *  @param prec The precision of the stringified number
+ *
+ *  @return std::string
+ * */
+std::string scientific_format(const std::vector<double> &v, int width = 20,
+                              int prec = 10);
+
+
+/** @brief Takes in the __PRETTY_FUNCTION__ string and removes the return type.
+ *
+ *  @details This function should only be used for the __METHOD_NAME__ macro,
+ *  since it takes the output from __PRETTY_FUNCTION__ and strips the return
+ *  type.
+ *
+ *  @param pretty_function The string from __PRETTY_FUNCTION__
+ *
+ *  @return std::string
+ * */
+static inline std::string methodName(const std::string &pretty_function)
+{
+    size_t colons = pretty_function.find("::");
+    size_t begin = pretty_function.substr(0, colons).rfind(" ") + 1;
+    size_t end = pretty_function.rfind("(") - begin;
+
+    return pretty_function.substr(begin, end) + "()";
+}
+
+/** @brief Make path given.
+ *
+ *  @details This tries to be the equivalent to "mkdir -p" and creates a new
+ *  directory whenever it needs to.
+ *
+ *  @param path The path to be created
+ *  @param mode The mode/permissions for all the new directories
+ *
+ *  @return bool Success/Fail
+ * */
+bool mkpath(std::string path, int mode = 0777);
+
+#endif
diff --git a/src/Makefile b/src/Makefile
new file mode 100644
index 0000000..622c9c1
--- /dev/null
+++ b/src/Makefile
@@ -0,0 +1,54 @@
+CC=g++
+
+LIBSRCS=utils.cpp
+LIBOBJS=$(LIBSRCS:.cpp=.o)
+
+CLASSSRCS=PenningTrap.cpp Particle.cpp
+CLASSOBJS=$(CLASSSRCS:.cpp=.o)
+
+INCLUDE=../include
+
+CFLAGS=-Wall -larmadillo -std=c++11 -O3 -fomit-frame-pointer
+#CFLAGS=-Wall -larmadillo -lblas -llapack -std=c++11 -O3 -fomit-frame-pointer
+OPENMP=-fopenmp
+
+# Add a debug flag when compiling (For the DEBUG macro in utils.hpp)
+DEBUG ?= 0
+ifeq ($(DEBUG), 1)
+	DBGFLAG=-DDBG
+else
+	DBGFLAG=
+endif
+
+# Add profiling for serial code
+PROFILE ?= 0
+ifeq ($(PROFILE), 1)
+	PROFFLAG=-pg -fno-inline-functions
+else
+	PROFFLAG=
+endif
+
+.PHONY: clean
+
+all: test_suite main frequency_narrow_sweeps_long
+
+# Instrumentation using scorep for parallel analysis
+instrument:
+	scorep $(CC) -c utils.cpp -o utils.o $(CFLAGS) $(DBGFLAG) $(PROFFLAG) -I$(INCLUDE) $(OPENMP)
+	scorep $(CC) -c main.cpp -o main.o $(CFLAGS) $(DBGFLAG) $(PROFFLAG) -I$(INCLUDE) $(OPENMP)
+	scorep $(CC) $(LIBOBJS) $(CLASSOBJS) main.o -o main $(CFLAGS) $(DBGFLAG) $(PROFFLAG) -I$(INCLUDE) $(OPENMP)
+	
+# Rules for executables
+main: main.o $(LIBOBJS) $(CLASSOBJS)
+	$(CC) $^ -o $@ $(CFLAGS) $(DBGFLAG) $(PROFFLAG) -I$(INCLUDE) $(OPENMP)
+
+test_suite: test_suite.o $(LIBOBJS) $(CLASSOBJS)
+	$(CC) $^ -o $@ $(CFLAGS) $(DBGFLAG) $(PROFFLAG) -I$(INCLUDE) $(OPENMP)
+
+# Rule for object files
+%.o: %.cpp
+	$(CC) -c $^ -o $@ $(CFLAGS) $(DBGFLAG) $(PROFFLAG) -I$(INCLUDE) $(OPENMP)
+
+clean:
+	rm *.o
+	rm test_suite main 
diff --git a/src/testlib.cpp b/src/testlib.cpp
new file mode 100644
index 0000000..126f5bc
--- /dev/null
+++ b/src/testlib.cpp
@@ -0,0 +1,54 @@
+/** @file utils.cpp
+ *
+ *  @author Cory Alexander Balaton (coryab)
+ *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
+ *
+ *  @version 1.0
+ *
+ *  @brief Implementation of the testing library
+ *
+ *  @bug No known bugs
+ * */
+
+#include "testlib.hpp"
+
+static void print_message(std::string msg)
+{
+    if (msg.size() > 0) {
+        std::cout << "message: " << msg << "\n\n";
+    } else {
+        std::cout << "\n";
+    }
+}
+
+void m_assert(bool expr, std::string expr_str, std::string f, std::string file,
+              int line, std::string msg)
+{
+    std::string new_assert(f.size() + (expr ? 4 : 6), '-');
+    std::cout << "\x1B[36m" << new_assert << "\033[0m\n";
+    std::cout << f << ": ";
+    if (expr) {
+        std::cout << "\x1B[32mOK\033[0m\n";
+        print_message(msg);
+    } else {
+        std::cout << "\x1B[31mFAIL\033[0m\n";
+        print_message(msg);
+        std::cout << file << " " << line << ": Assertion \"" << expr_str
+                  << "\" Failed\n\n";
+        abort();
+    }
+}
+
+bool close_to(arma::vec &a, arma::vec &b, double tol)
+{
+    if (a.n_elem != b.n_elem) {
+        return false;
+    }
+
+    for (size_t i = 0; i < a.n_elem; i++) {
+        if (std::abs(a(i) - b(i)) >= tol) {
+            return false;
+        }
+    }
+    return true;
+}
diff --git a/src/utils.cpp b/src/utils.cpp
new file mode 100644
index 0000000..bf602fc
--- /dev/null
+++ b/src/utils.cpp
@@ -0,0 +1,54 @@
+/** @file utils.cpp
+ *
+ *  @author Cory Alexander Balaton (coryab)
+ *  @author Janita Ovidie Sandtrøen Willumsen (janitaws)
+ *
+ *  @version 1.0
+ *
+ *  @brief Implementation of the utils
+ *
+ *  @bug No known bugs
+ * */
+
+#include "utils.hpp"
+
+std::string scientific_format(double d, int width, int prec)
+{
+    std::stringstream ss;
+    ss << std::setw(width) << std::setprecision(prec) << std::scientific << d;
+    return ss.str();
+}
+
+std::string scientific_format(const std::vector<double> &v, int width, int prec)
+{
+    std::stringstream ss;
+    for (double elem : v) {
+        ss << scientific_format(elem, width, prec);
+    }
+    return ss.str();
+}
+
+bool mkpath(std::string path, int mode)
+{
+    std::string cur_dir;
+    std::string::size_type pos = -1;
+    struct stat buf;
+
+    if (path.back() != '/') {
+        path += '/';
+    }
+    while (true) {
+        pos++;
+        pos = path.find('/', pos);
+        if (pos != std::string::npos) {
+            cur_dir = path.substr(0, pos);
+            if (mkdir(cur_dir.c_str(), mode) != 0
+                && stat(cur_dir.c_str(), &buf) != 0) {
+                return -1;
+            }
+        } else {
+            break;
+        }
+    }
+    return 0;
+}