Finish initial implementation

Still really ugly
Refactor
Clean up code a bit
Simplify further
Fix bug