diff --git a/meanshift_declarations.c b/meanshift_declarations.c
new file mode 100644
index 0000000..4fbf8ed
--- /dev/null
+++ b/meanshift_declarations.c
@@ -0,0 +1,169 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <float.h>
+#include <string.h>
+
+#include "meanshift_declarations.h"
+
+void get_args(int argc, char **argv, int *h){
+    if (argc != 6) {
+        printf("Usage: %s h N D Pd Pl\nwhere:\n", argv[0]);
+        printf("\th is the variance\n");
+        printf("\tN is the the number of points\n");
+        printf("\tD is the number of dimensions of each point\n");
+        printf("\tPd is the path of the dataset file\n");
+        printf("\tPl is the path of the labels file\n");
+        exit(1);
+    }
+
+    *h = atoi(argv[1]);
+    NUMBER_OF_POINTS = atoi(argv[2]);
+    DIMENSIONS = atoi(argv[3]);
+    POINTS_FILENAME = argv[4];
+    LABELS_FILENAME = argv[5];
+}
+
+int meanshift(double **original_points, double ***shifted_points, int h
+    , parameters *opt, int iteration){
+
+    // allocates space and copies original points on first iteration
+    if (iteration == 1){
+        (*shifted_points) = alloc_2d_double(NUMBER_OF_POINTS, DIMENSIONS);
+        duplicate(original_points, NUMBER_OF_POINTS, DIMENSIONS, shifted_points);
+    }
+
+    // mean shift vector
+    double **mean_shift_vector;
+    mean_shift_vector = alloc_2d_double(NUMBER_OF_POINTS, DIMENSIONS);
+    // initialize elements of mean_shift_vector to inf
+    for (int i=0;i<NUMBER_OF_POINTS;i++){
+        for (int j=0;j<DIMENSIONS;j++){
+            mean_shift_vector[i][j] = DBL_MAX;
+        }
+    }
+
+    double **kernel_matrix = alloc_2d_double(NUMBER_OF_POINTS, NUMBER_OF_POINTS);
+    double *denominator = malloc(NUMBER_OF_POINTS * sizeof(double));
+    // create new y vector
+    double **new_shift = alloc_2d_double(NUMBER_OF_POINTS, DIMENSIONS);
+
+    // find pairwise distance matrix (inside radius)
+    // [I, D] = rangesearch(x,y,h);
+    for (int i=0; i<NUMBER_OF_POINTS; i++){
+        double sum = 0;
+        for (int j=0; j<NUMBER_OF_POINTS; j++){
+            double dist_sum = 0;
+            for (int p=0; p<DIMENSIONS; p++){
+                double dif = ((*shifted_points)[i])[p]-(original_points[j])[p];
+                dist_sum += dif * dif;
+            }
+            double dist = sqrt(dist_sum);
+
+            if (dist < h*h){
+                kernel_matrix[i][j] = dist * dist;
+                // compute kernel matrix
+                double pow = ((-1)*(kernel_matrix[i][j]))/(2*(h*h));
+                kernel_matrix[i][j] = exp(pow);
+            } else {
+                kernel_matrix[i][j] = 0;
+            }
+            if (i==j){
+                kernel_matrix[i][j] += 1;
+            }
+            sum = sum + kernel_matrix[i][j];
+        }
+        denominator[i] = sum;
+
+        // build nominator
+        for (int j=0; j<DIMENSIONS; j++){
+            new_shift[i][j] = 0;
+            for (int k=0; k<NUMBER_OF_POINTS; k++){
+                new_shift[i][j] += kernel_matrix[i][k] * original_points[k][j];
+            }
+            // divide element-wise
+            new_shift[i][j] = new_shift[i][j] / denominator[i];
+            // calculate mean-shift vector at the same time
+            mean_shift_vector[i][j] = new_shift[i][j] - (*shifted_points)[i][j];
+        }
+    }
+
+
+    // frees previously shifted points, they're now garbage
+    free((*shifted_points)[0]);
+    // updates shifted points pointer to the new array address
+    shifted_points = &new_shift;
+
+    save_matrix((*shifted_points), iteration);
+
+    double current_norm = norm(mean_shift_vector, NUMBER_OF_POINTS, DIMENSIONS);
+    printf("Iteration n. %d, error %f \n", iteration, current_norm);
+
+    // clean up this iteration's allocates
+    free(mean_shift_vector[0]);
+    free(mean_shift_vector);
+    free(kernel_matrix[0]);
+    free(kernel_matrix);
+    free(denominator);
+
+    /** iterate until convergence **/
+    if (current_norm > opt->epsilon) {
+        return meanshift(original_points, shifted_points, h, opt, ++iteration);
+    }
+
+    return iteration;
+}
+
+// TODO check why there's is a difference in the norm calculate in matlab
+double norm(double **matrix, int rows, int cols){
+    double sum=0, temp_mul=0;
+    for (int i=0; i<rows; i++) {
+        for (int j=0; j<cols; j++) {
+            temp_mul = matrix[i][j] * matrix[i][j];
+            sum = sum + temp_mul;
+        }
+    }
+    double norm = sqrt(sum);
+    return norm;
+}
+
+double **alloc_2d_double(int rows, int cols) {
+    double *data = (double *) malloc(rows*cols*sizeof(double));
+    double **array = (double **) malloc(rows*sizeof(double*));
+    for (int i=0; i<rows; i++)
+        array[i] = &(data[cols*i]);
+    return array;
+}
+
+void duplicate(double **source, int rows, int cols, double ***dest){
+    for (int i=0; i<rows; i++){
+        for (int j=0; j<cols; j++){
+            (*dest)[i][j] = source[i][j];
+        }
+    }
+}
+
+void print_matrix(double **array, int rows, int cols){
+    for (int i=0; i<cols; i++){
+        for (int j=0; j<rows; j++){
+            printf("%f ", array[j][i]);
+        }
+        printf("\n");
+    }
+}
+
+void save_matrix(double **matrix, int iteration){
+    char filename[18];
+    snprintf(filename, sizeof(filename), "%s%d", "output/output_", iteration);
+    FILE *file;
+    file = fopen(filename, "w");
+    for (int rows=0; rows<NUMBER_OF_POINTS; ++rows){
+        for (int cols=0; cols<DIMENSIONS; ++cols){
+            fprintf(file, "%f", matrix[rows][cols]);
+            if (cols != DIMENSIONS - 1){
+                fprintf(file, ",");
+            }
+        }
+        fprintf(file, "\n");
+    }
+}
\ No newline at end of file
diff --git a/meanshift_declarations.h b/meanshift_declarations.h
new file mode 100644
index 0000000..4c5e5f1
--- /dev/null
+++ b/meanshift_declarations.h
@@ -0,0 +1,44 @@
+#ifndef SERIAL_DECLARATIONS_H    /*    Include guard    */
+#define SERIAL_DECLARATIONS_H
+
+#include <stdbool.h>
+
+extern int NUMBER_OF_POINTS;
+extern int DIMENSIONS;
+extern char* POINTS_FILENAME;
+extern char* LABELS_FILENAME;
+
+typedef struct parameters {
+    double epsilon;
+    bool verbose;
+    bool display;
+} parameters;
+
+//Function get_args parses command line arguments.
+void get_args(int argc, char **argv, int *h);
+
+//Function meanshift recursively shifts original points according to th
+//mean-shift algorithm saving the result to shiftedPoints. Struct opt has user
+//options, h is the desirable deviation, iteration is this call's iteration
+//number.
+int meanshift(double **original_points, double ***shifted_points, int h
+	, parameters *opt, int iteration);
+
+//Function norm returns the second norm of matrix of dimensions rowsXcols.
+double norm(double **matrix, int rows, int cols);
+
+//Function alloc_2d_double allocates rows*cols bytes of continuous memory.
+double **alloc_2d_double(int rows, int cols);
+
+//Function duplicate copies the values of source array to dest array.
+void duplicate(double **source, int rows, int cols, double ***dest);
+
+//Function print_matrix prints array of dimensions rowsXcols to the console.
+void print_matrix(double **array, int rows, int cols);
+
+//Function save_matrix prints matrix in a csv file with path/filename
+//"output/output_iteration". If a file already exists new lines are concatenated.
+void save_matrix(double **matrix
+	, int iteration);
+
+#endif //SERIAL_DECLARATIONS_H
\ No newline at end of file
diff --git a/serial_declarations.c b/serial_declarations.c
index c922ddb..9665c9f 100644
--- a/serial_declarations.c
+++ b/serial_declarations.c
@@ -54,9 +54,7 @@ int meanshift(double **original_points, double ***shifted_points, int h
             double dist = calculateDistance((*shifted_points)[i]
                 , original_points[j]);
 
-            if (i == j){
-                kernel_matrix[i][j] = 1;
-            } else if (dist < h*h){
+            if (dist < h*h){
                 kernel_matrix[i][j] = dist * dist;
                 // compute kernel matrix
                 double pow = ((-1)*(kernel_matrix[i][j]))/(2*(h*h));
@@ -64,6 +62,9 @@ int meanshift(double **original_points, double ***shifted_points, int h
             } else {
                 kernel_matrix[i][j] = 0;
             }
+            if (i == j){
+                kernel_matrix[i][j] += 1;
+            }
             sum = sum + kernel_matrix[i][j];
         }
         denominator[i] = sum;
@@ -183,4 +184,4 @@ void save_matrix(double **matrix, int iteration){
         }
         fprintf(file, "\n");
     }
-}
\ No newline at end of file
+}