Adding simple pseudocode for ARA*
Fixing pseudocode for FRA* so it will be easier to decouple from the interface