Some fixes.
Float -> double.
Some tools and refactor.
Start to split the logic into other files.
Working, partly.
Selection only keeps positions now.
Almost working fully.
And finished!
Merge branch 'split_logic'