diff --git a/.gitignore b/.gitignore
index 254bdcf..cb50d2d 100644
--- a/.gitignore
+++ b/.gitignore
@@ -54,4 +54,4 @@ dkms.conf
 
 main
 
-result.txt
\ No newline at end of file
+*result.txt
\ No newline at end of file
diff --git a/MMS24-25.c b/MMS24-25.c
index e210532..1f08442 100644
--- a/MMS24-25.c
+++ b/MMS24-25.c
@@ -17,12 +17,39 @@
 #include <stdlib.h>
 #include "MMS24-25.h"
 #include <stdio.h>
+#include <stdarg.h>
 
-// NULL Pointer
+// Define NULL if not already provided
 #ifndef NULL
 #define NULL 0
 #endif
 
+void _error(char* fmt,...) {
+    printf("<\x1B[31;1m ERROR \x1B[0m: \x1B[34m'");
+
+    va_list args;
+    va_start(args, fmt);
+    printf(fmt, args);
+    va_end(args);
+
+    printf("'\x1B[0m >\n");
+
+    exit(-1);
+}
+
+void _debug(char* fmt,...) {
+    printf("[\x1B[31;1m DEBUG \x1B[0m] \x1B[34m'");
+
+    va_list args;
+    va_start(args, fmt);
+    printf(fmt, args);
+    va_end(args);
+
+    printf("'\x1B[0m\n");
+
+    exit(-1);
+}
+
 double interpolateLine(
         double dX1,
         double dY1,
@@ -30,13 +57,18 @@ double interpolateLine(
         double dY2,
         double dXq
     ) {
-    double y = (
-        (dY2 - dY1) / (dX2 - dX1)
-    ) * (dXq + dX1);
+    if (dX2 == dX1)
+        _error("interpolateLine: dX1 and dX2 must differ!");
+
+    double y = dY1 * (
+        (dX2 - dXq) / (dX2 - dX1)
+    ) + dY2 * (
+        (dXq - dX1) / (dX2 - dX1)
+    );
 
     #ifdef _DEBUG_
         // DEBUG: print the parameters and result
-        printf("interpolateLine(%lf, %lf, %lf, %lf, %lf) -> %lf\n",
+        _debug("interpolateLine(%lf, %lf, %lf, %lf, %lf) -> %lf\n",
            dX1,dY1,dX2,dY2,dXq,y);
     #endif
 
@@ -77,10 +109,8 @@ double *generateSineValues(
     double* sineArray = NULL;
     sineArray = (double*) malloc(sizeof(double) * iNumValues);
 
-    if (sineArray == NULL) {
-        printf("Failed to allocate memory for SineArray\n");
-        exit(-1);
-    }
+    if (sineArray == NULL) 
+        _error("*generateSineValues: Failed to allocate memory for Sine Array\n");
 
     for (int i=0; i<iNumValues;i++){
         sineArray[i] = dAmp*sin(((2*M_PI)/iNumSamplesPerPeriod) * i);
@@ -94,10 +124,8 @@ void writeDoubleArray(double *pdOut, int iLen, char *pcOutName) {
      */
 
     FILE* datei = fopen(pcOutName, "w");
-    if (datei == NULL) {
-        printf("Error while opening file '%s'.\n", pcOutName);
-        exit(-1);
-    }
+    if (datei == NULL)
+        _error("writeDoubleArray: Error while opening file '%s'.\n", pcOutName);
 
     for (int i = 0; i < iLen; i++) {
         fprintf(datei, "%f\n", pdOut[i]);
diff --git a/Makefile b/Makefile
index 5da918e..b36eebb 100644
--- a/Makefile
+++ b/Makefile
@@ -2,7 +2,13 @@ CC=gcc
 SOURCE_FILE=MMS24-25.c
 OPTS=-lm
 
-all: testInterpolation
+all: clean tests run
 
-testInterpolation:
-	$(CC) testInterpolation.c $(SOURCE_FILE) -o testInterpolation $(OPTS)
\ No newline at end of file
+clean:
+	rm ./tests || true
+
+tests:
+	$(CC) tests.c $(SOURCE_FILE) -o tests $(OPTS)
+
+run:
+	./tests
\ No newline at end of file
diff --git a/testInterpolation b/testInterpolation
deleted file mode 100755
index af0685c..0000000
Binary files a/testInterpolation and /dev/null differ
diff --git a/testInterpolation.c b/testInterpolation.c
deleted file mode 100644
index e50bdb9..0000000
--- a/testInterpolation.c
+++ /dev/null
@@ -1,31 +0,0 @@
-//
-// Created by frederik on 1/11/25.
-//
-
-#include "MMS24-25.h"
-#include <stdio.h>
-#include <stdlib.h>
-
-
-int main (int iArgC, char** ppcArgV){
-    int iRes = -1;
-
-    if (iArgC != 6){
-        printf("Number of Arguments != 6\n");
-        printf("Usage testInterpolation <dX1> <dY1> <dX2> <dY2> <dX>\n");
-        exit(iRes);
-    };
-
-    double dX1 = atof(ppcArgV[1]);
-    double dY1 = atof(ppcArgV[2]);
-    double dX2 = atof(ppcArgV[3]);
-    double dY2 = atof(ppcArgV[4]);
-    double dX = atof(ppcArgV[5]);
-
-    for(int i=0; i<4; i++){
-        double dRes = interpolateLine(dX1, dY1, dX2, dY2, dX+(double)i);
-        printf ("%f\n",dRes);
-    }
-
-    return iRes;
-}
diff --git a/tests b/tests
new file mode 100755
index 0000000..0b0af51
Binary files /dev/null and b/tests differ
diff --git a/tests.c b/tests.c
new file mode 100644
index 0000000..f45de54
--- /dev/null
+++ b/tests.c
@@ -0,0 +1,77 @@
+//
+// Created by frederik on 1/11/25.
+//
+
+#include "MMS24-25.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <math.h>
+
+void info(char* fmt,...) {
+    printf("[\x1B[37;1m INFO \x1B[0m]\t\x1B[34m'");
+
+    va_list args;
+    va_start(args, fmt);
+    vprintf(fmt, args);
+    va_end(args);
+
+    printf("'\x1B[0m\n");
+}
+
+void success(char* fmt,...) {
+    printf("[\x1B[32;1m SUCCESS \x1B[0m]\t\x1B[34m'");
+
+    va_list args;
+    va_start(args, fmt);
+    vprintf(fmt, args);
+    va_end(args);
+
+    printf("'\x1B[0m\n");
+}
+
+void error(const char* fmt, ...) {
+    va_list args;
+
+    printf("<\x1B[31;1m ERROR \x1B[0m: \x1B[34m'");
+    va_start(args, fmt);
+    vprintf(fmt, args);
+    va_end(args);
+    printf("'\x1B[0m >\n");
+    exit(-1);
+}
+
+void testInterpolation() {
+    info("testInterpolation();");
+
+    double dRangeMin = -10;
+    double dRangeMax = 10;
+    double dRangeStep = 0.5;
+
+    for (double dX1=dRangeMin;dX1<dRangeMax;dX1+=dRangeStep) {
+        for (double dX2=dRangeMin;dX2<dRangeMax;dX2+=dRangeStep) {
+            for (double dY1=dRangeMin;dY1<dRangeMax;dY1+=dRangeStep) {
+                for (double dY2=dRangeMin;dY2<dRangeMax;dY2+=dRangeStep) {
+                    for (double dX=dRangeMin;dX<dRangeMax;dX+=dRangeStep) {
+                        if (dX1 == dX2 || dX1 == dX || dX2 == dX)
+                            continue;
+
+                        double dR1 = interpolateLine(dX1, dY1, dX2, dY2, dX);
+                        double dR2 = interpolateLine(dX1, dY1, dX, dR1, dX2);
+
+                        if (round(dR2*1e6) != round(dY2*1e6))
+                            error("Failed assertion for: (%lf, %lf), (%lf, %lf) -> (%lf, %lf); %lf", dX1, dY1, dX2, dY2, dX, dR1, dR2);
+                    }
+                }
+            }
+        }
+    }
+
+    success("testInterpolation();");
+}
+
+int main (int iArgC, char** ppcArgV){
+    testInterpolation();
+
+    return 0;
+}