Updated algorithm, added functions in c