diff --git a/epicardium/modules/display.c b/epicardium/modules/display.c
index afb0401d2ad0e97666972345d2e120e132f1cc48..b6bf36249f61aed4b60b3fcbb62900ac72f15756 100644
--- a/epicardium/modules/display.c
+++ b/epicardium/modules/display.c
@@ -1,13 +1,13 @@
-#include "epicardium.h"
-#include "tmr_utils.h"
-#include "gpio.h"
+#include "display.h"
 #include "Fonts/fonts.h"
-#include "tmr.h"
 #include "FreeRTOS.h"
-#include "task.h"
-#include "gfx.h"
-#include "display.h"
 #include "LCD_Driver.h"
+#include "epicardium.h"
+#include "gfx.h"
+#include "gpio.h"
+#include "task.h"
+#include "tmr.h"
+#include "tmr_utils.h"
 
 static TaskHandle_t lock = NULL;
 
@@ -73,7 +73,7 @@ int epic_disp_line(
 		return cl;
 	} else {
 		/* TODO add linestyle support to gfx code */
-		gfx_thick_line(
+		gfx_line(
 			&display_screen,
 			xstart,
 			ystart,
diff --git a/hw-tests/imutest/main.c b/hw-tests/imutest/main.c
index 9472d57f0a8b5f363935df3e05d8b03a8a2df615..a9496e05894668a370b28b9b08283501ed495f20 100644
--- a/hw-tests/imutest/main.c
+++ b/hw-tests/imutest/main.c
@@ -3,31 +3,32 @@
  ******************************************************************************/
 
 /***** Includes *****/
-#include "mxc_config.h"
-#include "led.h"
+#include "bhy_uc_driver.h"
 #include "board.h"
-#include "tmr_utils.h"
+#include "display.h"
+#include "gfx.h"
+#include "gpio.h"
 #include "i2c.h"
+#include "led.h"
+#include "mxc_config.h"
+#include "pmic.h"
 #include "rtc.h"
 #include "spi.h"
-#include "gpio.h"
-#include "bhy_uc_driver.h"
-#include "pmic.h"
-#include "gfx.h"
-#include "display.h"
+#include "tmr_utils.h"
 
 #include "card10.h"
 
 #include <math.h>
-#include <stdio.h>
+#include <stdbool.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <string.h>
-#include <stdbool.h>
 
 #define M_PI 3.1415
 /***** Definitions *****/
 
-/* should be greater or equal to 69 bytes, page size (50) + maximum packet size(18) + 1 */
+/* should be greater or equal to 69 bytes, page size (50) + maximum packet
+ * size(18) + 1 */
 #define FIFO_SIZE 300
 #define ROTATION_VECTOR_SAMPLE_RATE 10
 #define MAX_PACKET_LENGTH 18
@@ -49,7 +50,7 @@ void draw_arrow(int angle, int color)
 	int x2 = x1 - sin * 30;
 	int y2 = y1 - cos * 30;
 
-	gfx_thick_line(&display_screen, x1, y1, x2, y2, 2, color);
+	gfx_line(&display_screen, x1, y1, x2, y2, 2, color);
 
 	sin = sinf((angle - 140) * 2 * M_PI / 360.);
 	cos = cosf((angle - 140) * 2 * M_PI / 360.);
@@ -57,7 +58,7 @@ void draw_arrow(int angle, int color)
 	int x3 = x2 - sin * 10;
 	int y3 = y2 - cos * 10;
 
-	gfx_thick_line(&display_screen, x2, y2, x3, y3, 2, color);
+	gfx_line(&display_screen, x2, y2, x3, y3, 2, color);
 
 	sin = sinf((angle + 140) * 2 * M_PI / 360.);
 	cos = cosf((angle + 140) * 2 * M_PI / 360.);
@@ -65,7 +66,7 @@ void draw_arrow(int angle, int color)
 	int x4 = x2 - sin * 10;
 	int y4 = y2 - cos * 10;
 
-	gfx_thick_line(&display_screen, x2, y2, x4, y4, 2, color);
+	gfx_line(&display_screen, x2, y2, x4, y4, 2, color);
 }
 
 /***** Functions *****/
@@ -92,8 +93,8 @@ static void sensors_callback_orientation(
 		draw_arrow(sensor_data->data_vector.x * 360 / 32768, color);
 
 		char buf[128];
-		//sprintf(buf, "Azimuth: %3d", angle);
-		//Paint_DrawString_EN(0, 0, buf, &Font12, BLACK, color);
+		// sprintf(buf, "Azimuth: %3d", angle);
+		// Paint_DrawString_EN(0, 0, buf, &Font12, BLACK, color);
 
 		sprintf(buf, "%3d", angle);
 		gfx_puts(
@@ -220,10 +221,14 @@ int main(void)
     }
 #endif
 
-	//if(bhy_install_sensor_callback(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP, sensors_callback_vector))
-	//if(bhy_install_sensor_callback(VS_TYPE_GRAVITY, VS_WAKEUP, sensors_callback_vector))
-	//if(bhy_install_sensor_callback(VS_TYPE_ACCELEROMETER, VS_WAKEUP, sensors_callback_vector))
-	//if(bhy_install_sensor_callback(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED, VS_WAKEUP, sensors_callback_vector_uncalib))
+	// if(bhy_install_sensor_callback(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP,
+	// sensors_callback_vector))
+	// if(bhy_install_sensor_callback(VS_TYPE_GRAVITY, VS_WAKEUP,
+	// sensors_callback_vector))
+	// if(bhy_install_sensor_callback(VS_TYPE_ACCELEROMETER, VS_WAKEUP,
+	// sensors_callback_vector))
+	// if(bhy_install_sensor_callback(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+	// VS_WAKEUP, sensors_callback_vector_uncalib))
 	if (bhy_install_sensor_callback(
 		    VS_TYPE_ORIENTATION,
 		    VS_WAKEUP,
@@ -240,10 +245,14 @@ int main(void)
 #endif
 
 	/* enables the virtual sensor */
-	//if(bhy_enable_virtual_sensor(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
-	//if(bhy_enable_virtual_sensor(VS_TYPE_GRAVITY, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
-	//if(bhy_enable_virtual_sensor(VS_TYPE_ACCELEROMETER, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
-	//if(bhy_enable_virtual_sensor(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED, VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
+	// if(bhy_enable_virtual_sensor(VS_TYPE_GEOMAGNETIC_FIELD, VS_WAKEUP,
+	// ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
+	// if(bhy_enable_virtual_sensor(VS_TYPE_GRAVITY, VS_WAKEUP,
+	// ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
+	// if(bhy_enable_virtual_sensor(VS_TYPE_ACCELEROMETER, VS_WAKEUP,
+	// ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
+	// if(bhy_enable_virtual_sensor(VS_TYPE_MAGNETIC_FIELD_UNCALIBRATED,
+	// VS_WAKEUP, ROTATION_VECTOR_SAMPLE_RATE, 0, VS_FLUSH_NONE, 0, 0))
 	if (bhy_enable_virtual_sensor(
 		    VS_TYPE_ORIENTATION,
 		    VS_WAKEUP,
@@ -288,8 +297,10 @@ int main(void)
 				);
 			}
 
-			/* the logic here is that if doing a partial parsing of the fifo, then we should not parse  */
-			/* the last 18 bytes (max length of a packet) so that we don't try to parse an incomplete   */
+			/* the logic here is that if doing a partial parsing of the fifo, then we
+       * should not parse  */
+			/* the last 18 bytes (max length of a packet) so that we don't try to
+       * parse an incomplete   */
 			/* packet */
 		} while ((result == BHY_SUCCESS) &&
 			 (bytes_read >
diff --git a/hw-tests/ips/main.c b/hw-tests/ips/main.c
index 888185ed07e856e29d47b9559c59efeeee9e91ca..6aa51eae2c518e69219f7e16af89e39b33a4cb24 100644
--- a/hw-tests/ips/main.c
+++ b/hw-tests/ips/main.c
@@ -3,19 +3,19 @@
  ******************************************************************************/
 
 /***** Includes *****/
-#include "tmr_utils.h"
-#include "gpio.h"
-#include "gfx.h"
-#include "display.h"
+#include "DEV_Config.h"
 #include "Fonts/fonts.h"
+#include "display.h"
+#include "gfx.h"
+#include "gpio.h"
 #include "image/image.h"
 #include "tmr.h"
-#include "DEV_Config.h"
+#include "tmr_utils.h"
 
 #include "card10.h"
 
-#include <stdio.h>
 #include <stdint.h>
+#include <stdio.h>
 #include <string.h>
 
 // *****************************************************************************
@@ -33,8 +33,8 @@ int main(void)
 	gfx_puts(&Font24, &display_screen, 0, 0, "123", 0x000f, 0xfff8);
 	gfx_puts(&Font24, &display_screen, 23, 0, "ABC", 0x000f, 0xfff8);
 	gfx_rectangle(&display_screen, 70, 10, 100, 40, 2, red);
-	gfx_thick_line(&display_screen, 70, 10, 100, 40, 2, green);
-	gfx_thick_line(&display_screen, 100, 10, 70, 40, 2, yellow);
+	gfx_line(&display_screen, 70, 10, 100, 40, 2, green);
+	gfx_line(&display_screen, 100, 10, 70, 40, 2, yellow);
 	gfx_circle(&display_screen, 85, 25, 22, 2, green);
 
 	gfx_copy_region_raw(
diff --git a/lib/gfx/gfx.c b/lib/gfx/gfx.c
index e26ca9c14de0dfba61704a83c96b9a954651cb4b..eef9103e85f5c985c19eb626a8127011c491f9e5 100644
--- a/lib/gfx/gfx.c
+++ b/lib/gfx/gfx.c
@@ -1,8 +1,8 @@
 #include "gfx.h"
 #include "framebuffer.h"
+#include <math.h>
 #include <stddef.h>
 #include <stdlib.h>
-#include <math.h>
 
 const struct gfx_color_rgb gfx_colors_rgb[COLORS] = {
 	{ 255, 255, 255 }, /* WHITE */
@@ -157,17 +157,10 @@ void gfx_circle_fill(struct gfx_region *reg, int x, int y, int r, Color c)
 void gfx_rectangle(
 	struct gfx_region *reg, int x, int y, int w, int h, int t, Color c
 ) {
-	if (t > 1) {
-		gfx_thick_line(reg, x, y, x + w, y, t, c);
-		gfx_thick_line(reg, x, y + h, x + w, y + h, t, c);
-		gfx_thick_line(reg, x, y, x, y + h, t, c);
-		gfx_thick_line(reg, x + w, y, x + w, y + h, t, c);
-	} else {
-		gfx_line(reg, x, y, x + w, y, c);
-		gfx_line(reg, x, y + h, x + w, y + h, c);
-		gfx_line(reg, x, y, x, y + h, c);
-		gfx_line(reg, x + w, y, x + w, y + h, c);
-	}
+	gfx_line(reg, x, y, x + w, y, t, c);
+	gfx_line(reg, x, y + h, x + w, y + h, t, c);
+	gfx_line(reg, x, y, x, y + h, t, c);
+	gfx_line(reg, x + w, y, x + w, y + h, t, c);
 }
 
 void gfx_rectangle_fill(
@@ -179,37 +172,86 @@ void gfx_rectangle_fill(
 	}
 }
 
-void gfx_line(struct gfx_region *reg, int x1, int y1, int x2, int y2, Color c)
-{
-	float dx = x2 - x1;
-	float dy = y2 - y1;
-	float de = fabs(dy / dx);
-	float e  = .0f;
-	int y    = y1;
-	for (int x = x1; x < x2; x++) {
-		gfx_setpixel(reg, x, y, c);
-		e += de;
-		if (e >= .5f) {
-			y += dy >= .0f ? 1 : -1;
-			e -= 1.0f;
+/*
+ * For derivation of the algorithm, see:
+ * https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+ */
+static void plot_line_low(
+	struct gfx_region *reg, int x0, int y0, int x1, int y1, int t, Color c
+) {
+	int dx = x1 - x0;
+	int dy = y1 - y0;
+	int yi = 1;
+
+	if (dy < 0) {
+		yi = -1;
+		dy = -dy;
+	}
+
+	int d = 2 * dy - dx;
+	int y = y0;
+	for (int x = x0; x < x1; x++) {
+		if (t > 1) {
+			gfx_circle_fill(reg, x, y, t, c);
+		} else {
+			gfx_setpixel(reg, x, y, c);
 		}
+
+		if (d > 0) {
+			y += yi;
+			d -= 2 * dx;
+		}
+		d += 2 * dy;
 	}
 }
 
-void gfx_thick_line(
-	struct gfx_region *reg, int x1, int y1, int x2, int y2, int t, Color c
+/*
+ * For derivation of the algorithm, see:
+ * https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+ */
+static void plot_line_high(
+	struct gfx_region *reg, int x0, int y0, int x1, int y1, int t, Color c
 ) {
-	float dx = x2 - x1;
-	float dy = y2 - y1;
-	float de = fabs(dy / dx);
-	float e  = .0f;
-	int y    = y1;
-	for (int x = x1; x < x2; x++) {
-		gfx_circle_fill(reg, x, y, t, c);
-		e += de;
-		if (e >= .5f) {
-			y += dy >= .0f ? 1 : -1;
-			e -= 1.0f;
+	int dx = x1 - x0;
+	int dy = y1 - y0;
+	int xi = 1;
+
+	if (dx < 0) {
+		xi = -1;
+		dx = -dx;
+	}
+
+	int d = 2 * dx - dy;
+	int x = x0;
+	for (int y = y0; y < y1; y++) {
+		if (t > 1) {
+			gfx_circle_fill(reg, x, y, t, c);
+		} else {
+			gfx_setpixel(reg, x, y, c);
+		}
+
+		if (d > 0) {
+			x += xi;
+			d -= 2 * dy;
+		}
+		d += 2 * dx;
+	}
+}
+
+void gfx_line(
+	struct gfx_region *reg, int x0, int y0, int x1, int y1, int t, Color c
+) {
+	if (abs(y1 - y0) < abs(x1 - x0)) {
+		if (x0 > x1) {
+			plot_line_low(reg, x1, y1, x0, y0, t, c);
+		} else {
+			plot_line_low(reg, x0, y0, x1, y1, t, c);
+		}
+	} else {
+		if (y0 > y1) {
+			plot_line_high(reg, x1, y1, x0, y0, t, c);
+		} else {
+			plot_line_high(reg, x0, y0, x1, y1, t, c);
 		}
 	}
 }
diff --git a/lib/gfx/gfx.h b/lib/gfx/gfx.h
index fbfe916e53133b7a463e044813edd97793bad814..428b196c6a41465230538da60a6ff0ce3fccafb5 100644
--- a/lib/gfx/gfx.h
+++ b/lib/gfx/gfx.h
@@ -28,9 +28,8 @@ void gfx_rectangle(struct gfx_region *reg, int x, int y, int w, int h,
 						      int t, Color c);
 void gfx_rectangle_fill(struct gfx_region *reg, int x, int y, int w, int h,
 								  Color c);
-void gfx_line(struct gfx_region *reg, int x1, int y1, int x2, int y2, Color c);
-void gfx_thick_line(struct gfx_region *reg, int x1, int y1, int x2, int y2,
-							   int t, Color c);
+void gfx_line(struct gfx_region *reg, int x1, int y1, int x2, int y2,
+						     int t, Color c);
 void gfx_update(struct gfx_region *reg);
 
 enum gfx_color {