Reorganise code and add Centroid function