Path planning ported to Java

Initial port. Has bugs