Done with implementing the C functions, starting to speed up the kmeans