perform a lot of TODOs
remove the PathPlan.java, integrate it into RobotState