Rework directory structure
Encoder testing now giving distance