policy iteration finally working

I might have merged stuff a bit wrong, but it was a little mess and I
had to fix it manually :(

Maybe some of the added things were already deleted and unnecessary, but
I had them in them in my previous version and wasn't sure what should be
there... sowwy