Started work on A4.

Got the algo to work. Now work on cleaning up to hand in.