 void computeMeans() {
   int dimension = 1;
   int clusters = 3;
-  double errorTolerance = 0.01;
+  double errorTolerance = 1;
   double** centroids = (double**)calloc(clusters, sizeof(double*));
 
+  Serial.println(dataPoints[0][0]);
+  Serial.println(dataPoints[numDataPoints - 1][0]);
+
   int* labels = k_means(dataPoints, numDataPoints, dimension, clusters, errorTolerance, centroids);
-  for (int i = 0; i < numDataPoints; i++) {
-    char string[100];
-    sprintf(string, "data point %f is in cluster %f\n", (float)dataPoints[i][0], (float)labels[i]);
-    Serial.println(string);
-  }
+  Serial.println("first:");
+  Serial.println((float)dataPoints[0][0]);
+  Serial.println(labels[0]);
+  Serial.println("last");
+  Serial.println(dataPoints[numDataPoints - 1][0]);
+  Serial.println(labels[numDataPoints - 1]);
+
+  // for (int i = 0; i < numDataPoints; i++) {
+  //   char string[100];
+  //   sprintf(string, "data point %f is in cluster %f\n", (float)dataPoints[i][0], (float)labels[i]);
+  //   Serial.println(string);
+  // }
   free(labels); 
   free(centroids);
 }
 
 void collectIntialData() {
   int* values = getSensorValues();
-  // dataPoints[collectedPoints][0] = (double) values[0];
-  // dataPoints[collectedPoints + 1][0] = (double) values[1];
-  // dataPoints[collectedPoints + 2][0] = (double) values[2];
-  // dataPoints[collectedPoints + 3][0] = (double) values[3];
+  int collectedPoints = collectedPointSets * 4;
+  dataPoints[collectedPoints][0] = (double) values[0];
+  dataPoints[collectedPoints + 1][0] = (double) values[1];
+  dataPoints[collectedPoints + 2][0] = (double) values[2];
+  dataPoints[collectedPoints + 3][0] = (double) values[3];
 }
 
 int* getSensorValues() {

int diod4Pin = A5;

bool collecting = true;
int collectedPoints = 0;
int numberOfCollections = 5;
int numDataPoints = numberOfCollections * 4;
double** dataPoints;


  digitalWrite(diod3Pin, HIGH);
  digitalWrite(diod4Pin, HIGH);

  dataPoints = (double**)calloc(numDataPoints, sizeof(double*));
  for (int i = 0; i < numDataPoints; i++)
  {
    dataPoints[i] = (double*) calloc(1, sizeof(double));
  }

  // Serial.println(dataPoints[0][0]);
  // Serial.println(dataPoints[numDataPoints - 1][0]);

  // dataPoints = (double **)malloc(numDataPoints * sizeof(double *));
  // for (int i = 0; i < numDataPoints; i++) {
  //   dataPoints[i] = (double *)malloc(1 * sizeof(double));    
  // }
  // Serial.println(dataPoints[0][0]);
  // Serial.println(dataPoints[numDataPoints - 1][0]);
  Serial.println("Car initialized, ready to go!");
  delay(1000);
}

void loop() {
  if (collecting && collectedPointSets <= numberOfCollections) {
    /*driveRandom();*/
    collectIntialData();
    collectedPointSets++;
  } else if (collecting) {
    computeMeans();
    collecting = false;
