Implemented pseudocode, not working atm