Initial working code

Needs code review and testing.