First version of Grounded

Implemented as described by the pseudocode.
Added a few debug messages
Changed Grounded signature

Now uses the class' internal af and C
Manual merge of 'grounded' into 'master'