Finished PSet2a

Passed both test cases with identicle results.
Could neaten the code up.