diff --git a/src/cluster.c b/src/cluster.c
index 446062b26356704582109731c76b0cf7abc38c13..e9bb226a2903cfa9e9db44305a2d4be7525fcd03 100644
--- a/src/cluster.c
+++ b/src/cluster.c
@@ -1,10 +1,11 @@
 #include "cluster.h"
+#include <assert.h>
+#include <math.h>
 #include <stdbool.h>
 #include "vector.h"
 
-#ifdef DEBUG
-#include <assert.h>
-#endif
+
+#define EPSILON 0.001
 
 
 cluster_int_t* cluster_create_int(vector_int_t* center) {
@@ -58,6 +59,8 @@ bool cluster_update_center_int(cluster_int_t* cluster) {
 bool cluster_update_center_fpt(cluster_fpt_t* cluster) {
 	// save old center
 	vector_fpt_t* old_center = cluster->center;
+	assert(old_center != NULL);
+	assert(cluster != NULL);
 	// create new center
 	list_points_node_fpt_t* node = cluster->points->head;
 	// if cluster is empty
@@ -75,7 +78,7 @@ bool cluster_update_center_fpt(cluster_fpt_t* cluster) {
 		// check whether center has changed
 		bool changed = false;
 		for (size_t p = 0; p < cluster->center->dim; ++p) {
-			if (cluster->center->data[p] != old_center->data[p]) {
+			if (fabs(cluster->center->data[p] - old_center->data[p]) < EPSILON) {
 				changed = true;
 				break;
 			}
diff --git a/src/kmeans.c b/src/kmeans.c
index cb572cd884ac3311fbb81df7b992221876cbb073..9d596fb081e12c53c0f6dafb0135ee82e6204929 100644
--- a/src/kmeans.c
+++ b/src/kmeans.c
@@ -1,17 +1,6 @@
 #include "kmeans.h"
-#include "vector.h"
-
-#ifdef DEBUG
 #include <assert.h>
-#include "io.h"
-#endif
-
-#define EPSILON 0.001
-
-
-fpt_t abs_fpt(const fpt_t x) {
-	return x >= 0 ? x : -x;
-}
+#include "vector.h"
 
 
 cluster_int_t** kmeans_init_clusters_int(const vector_int_t** points, const size_t point_count, const size_t nclusters) {
@@ -88,7 +77,7 @@ void kmeans_fpt(vector_fpt_t** points, const size_t point_count, cluster_fpt_t**
 			// find closest cluster
 			cluster_fpt_t* cmin = clusters[0];
 			fpt_t dmin = distance_function(point, cmin->center);
-			for (size_t k = 1; k < nb_clusters; ++k) {
+			for (size_t k = 0; k < nb_clusters; ++k) {
 				cluster_fpt_t* current_cluster = clusters[k];
 				fpt_t dist = distance_function(point, current_cluster->center);
 				if (dist < dmin) {
@@ -98,15 +87,14 @@ void kmeans_fpt(vector_fpt_t** points, const size_t point_count, cluster_fpt_t**
 			}
 			// add point to closest cluster
 			cluster_add_point_fpt(cmin, point);
-			// update all cluster centers
-			for (size_t k = 0; k < nb_clusters; ++k) {
-#ifdef DEBUG
-				assert(clusters[k] != NULL);
-				assert(clusters[k]->points != NULL);
-#endif
-				if (cluster_update_center_fpt(clusters[k])) {
-					changed = true;
-				}
+		}
+		// update all cluster centers
+		for (size_t k = 0; k < nb_clusters; ++k) {
+			assert(clusters[k] != NULL);
+			assert(clusters[k]->points != NULL);
+			if (cluster_update_center_fpt(clusters[k])) {
+				changed = true;
+				printf("%lud  \n<%lf  %lf  %lf>\n\n", nb_clusters, clusters[k]->center->data[0], clusters[k]->center->data[1], clusters[k]->center->data[2]);
 			}
 		}
 	}
diff --git a/src/main.c b/src/main.c
index 5df68eedea209d5d183f2f70f7f91a502139454a..86e9f2afba7199ddb89eb3703bca09f84f834a59 100644
--- a/src/main.c
+++ b/src/main.c
@@ -9,10 +9,6 @@
 #include "linkedlist.h"
 #include "vector.h"
 
-#ifdef DEBUG
-#include <assert.h>
-#endif
-
 
 enum DistanceFunctionType {
 	EUCLID = 0, MANHATTAN = 1, CHEBYSHEV = 2
@@ -105,16 +101,13 @@ int main_fpt(const char* ipath, const char* opath, const enum DistanceFunctionTy
 	printf("INIT: ...   ");
 	cluster_fpt_t** clusters = kmeans_init_clusters_fpt((const vector_fpt_t**) points, point_count, nb_clusters);
 	printf("DONE\n");
-#ifdef DEBUG
-	for(size_t i = 0; i < nb_clusters; ++i) assert(clusters[i] !=NULL);
-#endif
 	printf("STARTING KMEANS ALGORITHM: ...\n");
 	kmeans_fpt(points, point_count, clusters, nb_clusters, DIST_FUNC_FPT[dist_func_type]);
 	printf("KMEANS DONE !\n");
 	// WRITE
 	FILE* ofile = opath != NULL ? fopen(opath, "w") : stdout;
 	fprintf(ofile, "%lud\n%lud\n", dim, nb_clusters);
-	io_write_clusters_to_file_fpt(ofile, clusters, point_count);
+	io_write_clusters_to_file_fpt(ofile, clusters, nb_clusters);
 	fclose(ofile);
 	return EXIT_SUCCESS;
 }