Added in working solution as well as pseudo code.