Issue #76 implementation

Implementation of #76 + some small fixes and code optimization