Clean-up coordinate conversion and add tests