Preliminary works, added small test files

TODO:
Make output look correct
Calculate total path cost
Test large files
Make test code?