Fixed some errors.
First working (?) implementation.