First attempts for policy_iteration

Some working version of policy_iteration, although results seem to be a
bit wrong.
Some updates for help functions are also made to make stuff a bit more
clear and consistent