Implement given pseduocode

Rao Fu 21