Added more support functions and translated most functions from pseudo code to working code