Almost final version of indoors navigation.
Some bug fixing and refactoring still needed.