diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..2298bfb
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,6 @@
+IndentWidth: 4
+AllowShortFunctionsOnASingleLine: false
+BreakBeforeBraces: Custom
+BraceWrapping:
+  AfterFunction: true
+  BeforeElse: true
diff --git a/include/PenningTrap.hpp b/include/PenningTrap.hpp
index 1b1752e..3601634 100644
--- a/include/PenningTrap.hpp
+++ b/include/PenningTrap.hpp
@@ -13,9 +13,13 @@
 #define __PENNING_TRAP__
 
 #include <armadillo>
+#include <omp.h>
 
-#include "constants.hpp"
 #include "Particle.hpp"
+#include "constants.hpp"
+
+#pragma omp declare reduction( + : arma::vec : omp_out += omp_in ) \
+  initializer( omp_priv = omp_orig )
 
 /** @brief A class that simulates a Penning trap.
  *
diff --git a/include/utils.hpp b/include/utils.hpp
index c8d3e4a..7a03149 100644
--- a/include/utils.hpp
+++ b/include/utils.hpp
@@ -15,15 +15,16 @@
 #ifndef __UTILS__
 #define __UTILS__
 
-#include <string>
-#include <vector>
+#include <armadillo>
 #include <iomanip>
 #include <sstream>
+#include <string>
+#include <vector>
 
 /** @def DEBUG(msg)
  *  @brief Writes a debug message
  *
- *  This function writes a debug message that includes the filename,
+ *  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.
@@ -35,6 +36,18 @@
     #define DEBUG(msg)
 #endif
 
+/** @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)
+
+#define __METHOD_NAME__ methodName(__PRETTY_FUNCTION__)
+
+
 /** Code stolen from https://github.com/anderkve/FYS3150
  *  Header: https://github.com/anderkve/FYS3150/blob/master/code_examples/compilation_linking/example_1/include/utils.hpp
  *  Source: https://github.com/anderkve/FYS3150/blob/master/code_examples/compilation_linking/example_1/src/utils.cpp
@@ -62,4 +75,47 @@ std::string scientific_format(const std::vector<double>& v,
                               int width=20, 
                               int prec=10);
 
+/** @brief Test an expression, confirm that test is ok, or abort execution.
+ *
+ *  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.
+ *
+ *  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 Boolean
+ * */
+bool arma_vector_close_to(arma::vec &a, arma::vec &b, double tol=1e-8);
+
+
+static inline std::string methodName(const std::string& prettyFunction)
+{
+    size_t colons = prettyFunction.find("::");
+    size_t begin = prettyFunction.substr(0,colons).rfind(" ") + 1;
+    size_t end = prettyFunction.rfind("(") - begin;
+
+    return prettyFunction.substr(begin,end) + "()";
+}
+
 #endif
diff --git a/src/Makefile b/src/Makefile
index f0cd401..52c16b7 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -27,7 +27,7 @@ all: test_suite main
 main: main.o $(LIBOBJS) $(CLASSOBJS)
 	$(CC) $^ -o $@ $(CFLAGS) $(DBGFLAG) -I$(INCLUDE) $(OPENMP)
 
-test_suite: test_suite.o $(LIBOBJS)
+test_suite: test_suite.o $(LIBOBJS) $(CLASSOBJS)
 	$(CC) $^ -o $@ $(CFLAGS) $(DBGFLAG) -I$(INCLUDE) $(OPENMP)
 
 # Rule for object files
diff --git a/src/PenningTrap.cpp b/src/PenningTrap.cpp
index 59be75c..b4b2dd8 100644
--- a/src/PenningTrap.cpp
+++ b/src/PenningTrap.cpp
@@ -13,15 +13,9 @@
  *  @todo Implement evolve_forward_euler
  * */
 
-#include "utils.hpp"
 #include "PenningTrap.hpp"
 #include "constants.hpp"
-#include <algorithm>
-#include <stdexcept>
-#include <omp.h>
-
-#pragma omp declare reduction( + : arma::vec : omp_out += omp_in ) \
-  initializer( omp_priv = omp_orig )
+#include "utils.hpp"
 
 PenningTrap::PenningTrap(double B_0, double V_0, double d)
 {
@@ -38,20 +32,17 @@ void PenningTrap::add_particle(Particle particle)
 arma::vec PenningTrap::external_E_field(arma::vec r)
 {
     arma::vec::fixed<3> res;
-    double f = this->V_0/(this->d*this->d);
+    double f = this->V_0 / (this->d * this->d);
     res(0) = r(0);
     res(1) = r(1);
-    res(2) = -2.*r(2);
+    res(2) = -2. * r(2);
 
-    return f*res;
+    return f * res;
 }
 
 arma::vec PenningTrap::external_B_field(arma::vec r)
 {
-    arma::vec::fixed<3> res;
-    res(0) = 0.;
-    res(1) = 0.;
-    res(2) = this->B_0;
+    arma::vec::fixed<3> res{0., 0., this->B_0};
 
     return res;
 }
@@ -59,14 +50,14 @@ arma::vec PenningTrap::external_B_field(arma::vec r)
 arma::vec PenningTrap::force_on_particle(int i, int j)
 {
     // Calculate the difference between the particles' position
-    arma::vec::fixed<3> res = this->particles.at(i).r_vec 
-        - this->particles.at(j).r_vec;
+    arma::vec::fixed<3> res =
+        this->particles.at(i).r_vec - this->particles.at(j).r_vec;
 
     // Get the distance between the particles
     double norm = arma::norm(res);
 
     // Multiply res with p_j's charge divided by the norm cubed
-    res *= this->particles.at(j).q/(norm*norm*norm);
+    res *= this->particles.at(j).q / (norm * norm * norm);
 
     return res;
 }
@@ -75,16 +66,13 @@ arma::vec PenningTrap::total_force_external(int i)
 {
     Particle p = this->particles.at(i);
 
-    arma::vec::fixed<3> v_cross_B;
-
     arma::vec::fixed<3> B = this->external_B_field(p.r_vec);
 
-    v_cross_B(0) = p.v_vec(1)*B(2) - p.v_vec(2)*B(1);
-    v_cross_B(1) = p.v_vec(2)*B(0) - p.v_vec(0)*B(2);
-    v_cross_B(2) = p.v_vec(0)*B(1) - p.v_vec(1)*B(0);
+    arma::vec::fixed<3> v_cross_B{p.v_vec(1) * B(2) - p.v_vec(2) * B(1),
+                                  p.v_vec(2) * B(0) - p.v_vec(0) * B(2),
+                                  p.v_vec(0) * B(1) - p.v_vec(1) * B(0)};
 
-    arma::vec force = p.q
-        *(this->external_E_field(p.r_vec) + v_cross_B);
+    arma::vec force = p.q * (this->external_E_field(p.r_vec) + v_cross_B);
 
     return force;
 }
@@ -95,7 +83,7 @@ arma::vec PenningTrap::total_force_particles(int i)
 
     arma::vec res(3);
 
-    for (int j=0; j < this->particles.size(); j++) {
+    for (int j = 0; j < this->particles.size(); j++) {
         if (i == j) {
             continue;
         }
@@ -103,7 +91,7 @@ arma::vec PenningTrap::total_force_particles(int i)
         res += this->force_on_particle(i, j);
     }
 
-    res *= K_E*(p.q/p.m);
+    res *= K_E * (p.q / p.m);
 
     return res;
 }
@@ -115,7 +103,6 @@ arma::vec PenningTrap::total_force(int i)
 
 void PenningTrap::evolve_RK4(double dt)
 {
-
 }
 
 void PenningTrap::evolve_forward_euler(double dt)
@@ -124,11 +111,11 @@ void PenningTrap::evolve_forward_euler(double dt)
 
     Particle *p;
 
-    #pragma omp parallel for private(p)
-    for (int i=0; i < this->particles.size(); i++) {
+#pragma omp parallel for private(p)
+    for (int i = 0; i < this->particles.size(); i++) {
         p = &new_state.at(i);
-        p->v_vec += dt*this->total_force(i)/new_state.at(i).m;
-        p->r_vec += dt*this->particles.at(i).v_vec;
+        p->v_vec += dt * this->total_force(i) / new_state.at(i).m;
+        p->r_vec += dt * this->particles.at(i).v_vec;
     }
 
     this->particles = new_state;
diff --git a/src/main.cpp b/src/main.cpp
index b479649..14e8fa7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -10,9 +10,7 @@
  *  @bug No known bugs
  * */
 
-#include <filesystem>
 #include <fstream>
-#include <string>
 #include <omp.h>
 #include <sys/stat.h>
 
diff --git a/src/test_suite.cpp b/src/test_suite.cpp
index 41f8b9c..17f9c0e 100644
--- a/src/test_suite.cpp
+++ b/src/test_suite.cpp
@@ -9,7 +9,139 @@
  *
  *  @bug No known bugs
  * */
+
+#include "PenningTrap.hpp"
+#include "utils.hpp"
+
+#include <iomanip>
+#include <sstream>
+#include <string>
+
+class PenningTrapTest {
+public:
+    static void test_external_E_field()
+    {
+        PenningTrap trap;
+
+        // Vector containing inputs and expected results
+        std::vector<std::pair<arma::vec, arma::vec>> tests;
+
+        tests.push_back(std::make_pair(arma::vec{0.,0.,0.}, 
+                                       arma::vec{0.,0.,0.}));
+
+        tests.push_back(std::make_pair(arma::vec{10.,0.,0.}, 
+                                       arma::vec{96.4852558,0.,0.}));
+
+        tests.push_back(std::make_pair(arma::vec{10.,0.,0.}, 
+                                       arma::vec{96.4852558,0.,0.}));
+
+        tests.push_back(std::make_pair(arma::vec{0.,10.,0.}, 
+                                       arma::vec{0.,96.4852558,0.}));
+
+        tests.push_back(std::make_pair(arma::vec{0.,0.,10.}, 
+                                       arma::vec{0.,0.,-192.9705116}));
+
+
+        arma::vec result;
+        arma::vec v;
+        std::stringstream msg;
+        for (int i=0; i < tests.size(); i++) {
+            v = tests.at(i).first;
+            result = trap.external_E_field(v);
+
+            msg.str("");
+            msg << "Testing the external E field at (" << std::setprecision(2) 
+                << v(0) << "," << v(1) << "," << v(2) << ").";
+
+            ASSERT(arma_vector_close_to(result, tests.at(i).second), msg.str());
+        }
+    }
+
+
+    static void test_external_B_field()
+    {
+        // No point in testing at different points since it's not dependent on 
+        // position.
+        PenningTrap trap;
+        arma::vec expected{0.,0.,T};
+        arma::vec result = trap.external_B_field(arma::vec{0.,0.,0.});
+        ASSERT(arma_vector_close_to(expected, result),
+               "Testing the external B field at (0,0,0)");
+    }
+
+
+    static void test_force_on_particle()
+    {
+        PenningTrap trap;
+        arma::vec v{0.,0.,0.};
+
+        // Add particles to test
+        trap.add_particle(Particle(1.,40.,arma::vec{0.,0.,0.},v));
+        trap.add_particle(Particle(1.,40.,arma::vec{1.,0.,0.},v));
+        trap.add_particle(Particle(1.,40.,arma::vec{0.,3.,4.},v));
+
+        // Test p0 and p1
+        arma::vec expected{-1.,0.,0.};
+        arma::vec result = trap.force_on_particle(0, 1);
+        ASSERT(arma_vector_close_to(expected, result),
+               "Testing the force on a particle at (0,0,0) from a " 
+               "particle at (1,0,0)."); 
+
+        // Test p0 and p2
+        expected = arma::vec{0, -.024, -.032};
+        result = trap.force_on_particle(0, 2);
+        ASSERT(arma_vector_close_to(expected, result), 
+               "Testing the force on a particle at (0,0,0) from a "
+               "particle at (0,3,4).");
+    }
+
+    static void test_total_force_external()
+    {
+        PenningTrap trap;
+        trap.add_particle(Particle(1.,40.,arma::vec{1.,2.,3.},
+                                   arma::vec{3.,4.,5.}));
+
+        arma::vec expected{395.58954878,-270.15871624,-57.89115348};
+        arma::vec result = trap.total_force_external(0);
+        ASSERT(arma_vector_close_to(expected, result),
+               "Testing the total external force on a particle at "
+               "(1,2,3) with velocity (3,4,5)");
+    }
+
+    static void test_total_force_particles()
+    {
+        PenningTrap trap;
+        trap.add_particle(Particle(1.,40.,arma::vec{0.,0.,0.},
+                                   arma::vec{0.,0.,0.}));
+
+        arma::vec expected{0.,0.,0.};
+        arma::vec result = trap.total_force_particles(0);
+        ASSERT(arma_vector_close_to(expected, result),
+               "Testing the total force of all particles on particle 0 "
+               "with only a single particle");
+
+        trap.add_particle(Particle(1.,40.,arma::vec{1.,0.,0.},
+                                   arma::vec{0.,0.,0.}));
+        trap.add_particle(Particle(1.,40.,arma::vec{0.,1.,0.},
+                                   arma::vec{0.,0.,0.}));
+        trap.add_particle(Particle(1.,40.,arma::vec{0.,0.,1.},
+                                   arma::vec{0.,0.,0.}));
+
+        expected = arma::vec(3,arma::fill::value(-3473.383325));
+        result = trap.total_force_particles(0);
+        ASSERT(arma_vector_close_to(expected, result),
+               "Testing the total force of all particles on particle 0 "
+               "with 3 other particles.");
+    }
+};
+
+
 int main() 
 {
+    PenningTrapTest::test_external_E_field();
+    PenningTrapTest::test_external_B_field();
+    PenningTrapTest::test_force_on_particle();
+    PenningTrapTest::test_total_force_external();
+    PenningTrapTest::test_total_force_particles();
     return 0;
 }
diff --git a/src/utils.cpp b/src/utils.cpp
index 0e93a31..9694610 100644
--- a/src/utils.cpp
+++ b/src/utils.cpp
@@ -26,3 +26,50 @@ std::string scientific_format(const std::vector<double>& v, int width, int prec)
     }
     return ss.str();
 }
+
+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 arma_vector_close_to(arma::vec &a, arma::vec &b, double tol)
+{
+    if (a.n_elem != b.n_elem) {
+        return false;
+    }
+
+    for (int i=0; i < a.n_elem; i++) {
+        if (std::abs(a(i) - b(i)) >= tol) {
+            return false;
+        }
+    }
+    return true;
+}