Added initial version of fib heap A*

Also move drawing/open list generation out of critical perf section

Fixes #22, fixes #8