Algorithm function

Started working on algorithm function