vfh guess implemented, ready to test