Ugly commit. Ported matlab code to C++.
solved conflict. ported matlab to C++