Switched to Mean Shift clustering, cleaned up code