Initial solution
Doc completion
Some refactoring
Add drawing for sanity check.