Separating State and CostMatrix into their own files.