Added implementation and removes some pseudocode