All functions implemented

Needs heavy test for functionality and comparison with Matlab code