First "working" commit.

It produces more or less reasonable output. It is not symmetric and
very dependent on the time step choosen. But cost minimum is at the
goal and grows with distance from it.