Step 2 of algorithm added, TODO update centroids