function p = unwrap(p,cutoff,dim)
%MATLAB Code Generation Library Function

%   Limitations:  Single precision results may differ from MATLAB due to
%   different rounding errors.  When the result is single precision,
%   MATLAB Code Generation does all intermediate calculations in single precision.

%   Copyright 1984-2015 The MathWorks, Inc.
%#codegen

eml_invariant(nargin > 0,'MATLAB:minrhs');
eml_invariant(isfloat(p),'Coder:toolbox:unwrap_2');
if nargin < 2 || eml_is_const(size(cutoff)) && isempty(cutoff)
    cut = cast(pi,'like',real(p));
else
    eml_invariant(isscalar(cutoff) && isa(cutoff,'float'), ...
        'Coder:toolbox:unwrap_3', ...
        'IfNotConst','Fail');
    cut = cutoff;
end
if nargin < 3
    dim = coder.internal.nonSingletonDim(p);
else
    coder.internal.assertValidDim(dim);
    coder.internal.prefer_const(dim);
    eml_invariant(eml_is_const(dim) || coder.const(~coder.internal.eml_option_eq('VariableSizing','DisableInInference')), ...
        'Coder:toolbox:dimNotConst');
end
vlen = size(p,dim);
vwork = coder.nullcopy(eml_expand(coder.internal.scalarEg(p),[vlen,1]));
if eml_is_const(isvector(p)) && isvector(p) && eml_is_const(dim) && ( ...
        (dim == 1 && eml_is_const(size(p,2)) && size(p,2) == 1) || ...
        (dim == 2 && eml_is_const(size(p,1)) && size(p,1) == 1))
    p = unwrap_vector(p,cut);
else
    vstride = coder.internal.prodsize(p,'below',dim);
    vspread = coder.internal.indexTimes(coder.internal.indexMinus(vlen,1),vstride);
    npages  = coder.internal.prodsize(p,'above',dim);
    i2 = zeros(coder.internal.indexIntClass);
    for i = 1:npages
        i1 = i2;
        i2 = coder.internal.indexPlus(i2,vspread);
        for j = 1:vstride
            i1 = coder.internal.indexPlus(i1,1);
            i2 = coder.internal.indexPlus(i2,1);
            % Copy p(i1:vstride:i2) to vwork.
            ip = i1;
            for k = 1:vlen
                vwork(k) = p(ip);
                ip = coder.internal.indexPlus(ip,vstride);
            end
            vwork = unwrap_vector(vwork,cut);
            % Copy vector into the output matrix.
            ip = i1;
            for k = 1:vlen
                p(ip) = vwork(k);
                ip = coder.internal.indexPlus(ip,vstride);
            end
        end
    end
end

%--------------------------------------------------------------------------

function p = unwrap_vector(p,cutoff)
% Unwrap phase angles.  Algorithm minimizes the incremental
% phase variation by constraining it to the range [-pi,pi]
ppi = cast(pi,'like',real(p));
m = numel(p);
cumsum_dp_corr = coder.internal.scalarEg(p);
% Seek first finite value.
k = 1;
while k < m && ~isfinite(p(k))
    k = k + 1;
end
if k < m
    pkm1 = p(k);
    while true
        % Seek next finite value.
        k = k + 1;
        while k <= m && ~isfinite(p(k))
            k = k + 1;
        end
        if k > m
            break
        end
        dp = p(k) - pkm1;
        
        dp_corr = dp / (2*ppi);

        % Round to nearest integer, but make the boundary cases round towards zero
        % instead of away from zero:
        if abs(rem(dp_corr, 1)) <= 0.5
            dp_corr = fix(dp_corr);
        else
            dp_corr = round(dp_corr);
        end
        if abs(dp) >= cutoff
            cumsum_dp_corr = cumsum_dp_corr + dp_corr;
        end
        % Save p(k) for next iteration.
        pkm1 = p(k);
        % Add to P to produce smoothed phase values
        p(k) = p(k) - (2*ppi)*cumsum_dp_corr;
    end
end

%--------------------------------------------------------------------------