Working code

Seems to work OK. Needs formatting, commenting, testing.