First stab at implementing algorithm in C++ from matlab.