Fixed bugs, output equal to MATLAB