function out = cordicsqrt(u, varargin)
%CORDICSQRT  CORDIC-based square root.
%   CORDICSQRT(U, ...) returns the square root of the elements of U.

%   Copyright 2013-2017 The MathWorks, Inc.

%#codegen

coder.allowpcode('plain');

% Argument Checking for code Generation
if (nargin < 1) || (nargin > 4)
    eml_invariant(false, ...
        'fixed:fi:invalidNumberOfInputArgs');
elseif eml_ambiguous_types
    out = coder.ignoreConst(reshape(zeros(size(u)),size(u)));
    return; % EARLY RETURN
else
    coder.internal.prefer_const(u);
    if ~isreal(u) || ~isnumeric(u)
        eml_invariant(false, 'fixed:fi:realAndNumeric');
    elseif isfi(u) && ~isfloat(u)
        eml_invariant(isscalingbinarypoint(u), ...
            'fixed:fi:inputsMustBeIntOrFixPtBPSOrSDBPS');
        eml_invariant((u.WordLength >= 2), ...
            'fixed:cordic:inputWordLengthNotGTOne');
    end
end

if isempty(u)
    out = u;
elseif isfi(u) && isfloat(u)
    % Call CORDICSQRT with the equivalent builtin floating-point type
    % and then cast builtin results to FI Double/Single outputs.
    out_flt = cordicsqrt(storedInteger(u), varargin{:});
    out     = cast(out_flt, 'like', u);
elseif isinteger(u)
    % Builtin MATLAB integer (treat as FI)
    out = cordicsqrt(fi(u), varargin{:});
else
    % Compute default number of iterations constant
    if isa(u, 'double')
        numItersDefault = 52;
    elseif isa(u, 'single')
        numItersDefault = 23;
    elseif isfi(u)
        uFPNT   = numerictype(u);
        inputWL = uFPNT.WordLength;
        numItersDefault = inputWL - 1;
    else
        eml_invariant(isscalingbinarypoint(u), ...
                      'fixed:fi:inputsMustBeIntOrFixPtBPSOrSDBPS');
    end
    
    % Get actual number of iterations and doOutScaling to use
    [numIters_dbl, doOutScaling] = ...
        checkAndParseVarargin(numItersDefault, varargin{:});
    
    % Pre-compute constant gain value, based on the TOTAL number
    % of iterations, AND ALSO the repeated 3*k + 1 iterations.
    An_hp_dbl = coder.const(compute_An_hp_dbl(numIters_dbl));
    
    if isa(u, 'double') || isa(u, 'single') || (isfi(u) && isfloat(u))
        % Floating-point processing
        out = localFloatingPointCORDICSQRT( ...
            u, numIters_dbl, doOutScaling, An_hp_dbl);
    else
        % Fixed-point processing
        eml_invariant((inputWL <= 128), ...
                      'fixed:fi:maxWordLengthExceeded', inputWL, 128);
        
        out = localFixedPointCORDICSQRT( ...
            u, numIters_dbl, doOutScaling, An_hp_dbl, ...
            u.SignednessBool, u.WordLength, u.FractionLength);
    end
end

out = removefimath(out);

end % function


% =========================================================================
function [numIters_dbl, doOutScaling] = ...
    checkAndParseVarargin(numItersDefault, varargin)

if nargin > 1
    % CODE GENERATION checking and setting
    coder.internal.prefer_const(varargin);
    switch nargin
      case 2
        % varargin is {NITERS}
        numIters_dbl = checkNITERSArgCG(varargin{1}, numItersDefault);
        doOutScaling = true; % use default
      case 3
        % varargin is {'ScaleOutput', value}
        numIters_dbl = numItersDefault; % use default
        doOutScaling = checkScaleOutputPVPairCG(varargin{1}, varargin{2});
      otherwise
        % varargin is {NITERS, 'ScaleOutput', value}
        numIters_dbl = checkNITERSArgCG(varargin{1}, numItersDefault);
        doOutScaling = checkScaleOutputPVPairCG(varargin{2}, varargin{3});
    end
    
    % We allow specified niters values of [], Inf
    if isempty(numIters_dbl) || ~isfinite(numIters_dbl)
        numIters_dbl = numItersDefault;
    end
else
    % No optional arguments specified
    numIters_dbl = numItersDefault; % use default
    doOutScaling = true;            % use default
end

end % function


% =========================================================================
function An_hp_dbl = compute_An_hp_dbl(numIters_dbl)
    
coder.internal.prefer_const(numIters_dbl);

% Pre-compute constant gain value, based on the TOTAL number
% of iterations, AND ALSO the repeated 3*k + 1 iterations:
An_hp_dbl = prod(sqrt(1 - 2.^(-2*(1:(numIters_dbl)))));
k = 4;
while k <= numIters_dbl
    An_hp_dbl = An_hp_dbl * sqrt(1 - 2.^(-2*k));
    k = 3*k + 1;
end

end % function


% =========================================================================
function out = localFloatingPointCORDICSQRT( ...
    u, numIters_dbl, doOutScaling, An_hp_dbl)

coder.internal.prefer_const(u, numIters_dbl, doOutScaling, An_hp_dbl);
out = zeros(size(u), 'like', u);
isBuiltinDblSgl = ~isfi(u);

% --------------------------------------------------------------
% Floating-point processing (e.g. for Data Type Override) always
% uses input/output normalization on a value-by-value basis.
% --------------------------------------------------------------
for idx = 1:numel(u)
    uInput = u(idx);
    if (uInput <= 0)
        out(idx) = 0;
    elseif isfinite(uInput)
        % Floating-point: normBitsVal varies for each input value.
        % Normalize each value into the range [0.5, 2) for CORDIC.
        if isBuiltinDblSgl
            [uNorm, normBitsVal] = log2(uInput); % Double/Single
        else
            [uNorm, normBitsVal] = log2(double(uInput)); % FI Dbl/Sgl
        end
        normBitsDivTwo = normBitsVal/2;
        denormPow2Bits = floor(normBitsVal/2);
        isodd = (denormPow2Bits ~= normBitsDivTwo);
        if isodd
            uNorm = uNorm .* 2;
        end
        
        % Compute CORDICSQRT using normalized range
        % then denormalize back into correct range
        xNrm     = localCORDICSQRT(uNorm, numIters_dbl);
        out(idx) = pow2(xNrm, denormPow2Bits);
    else
        % Non-finite (nan, inf) value
        out(idx) = uInput;
    end
end

if doOutScaling
    % Scale ALL results by inverse of CORDIC gain constant
    out(:) = out .* cast((1/An_hp_dbl),'like',u);
end

end % function


% =========================================================================
function out = localFixedPointCORDICSQRT(u, ...
    numIters_dbl, doOutScaling, An_hp_dbl, ...
    inputSB, inputWL, inputFL)

% Pre-allocate type-specific outputs, temp vars, constants, etc.
% Output result type uses input signedness and word length along
% with best precision fraction length for SQRT(inputMaxRange).
% It is independent of the internal, intermediate CORDIC types.
coder.internal.prefer_const(u, numIters_dbl, doOutScaling, ...
                 An_hp_dbl, inputSB, inputWL, inputFL);
    
inputRangeVect = coder.const(double(range(fi([],numerictype(u)))));
inputMaxRange  = coder.const(inputRangeVect(2));
outputMaxRange = coder.const(sqrt(inputMaxRange));
maxOutputNT    = coder.const(numerictype(fi(outputMaxRange, inputSB, inputWL)));
outFL          = coder.const(maxOutputNT.FractionLength);

if isequal(u.DataType,'ScaledDouble')
    % Get results using builtin double
    out_dbl = cordicsqrt(double(u), numIters_dbl, ...
        'ScaleOutput', doOutScaling);

    % Cast builtin double results to ScaledDouble.
    % Note: cannot set 'DataType' after construction.
    tmp = fi(zeros(size(u)), inputSB, inputWL, outFL);
    out = coder.nullcopy(fi(tmp, 'DataType', 'ScaledDouble'));
    out(:) = out_dbl;
else
    out = coder.nullcopy(fi(zeros(size(u)), inputSB, inputWL, outFL));

    % Temporary variable for next input sample prior to normalization, etc.
    if (inputFL < 4)
        % Inputs need a minimum FractionLength to produce reasonable results.
        % Increase input precision in uTemp variable and grow extra WL bits.
        newWL = coder.const(inputWL + 4 - inputFL);
        uTmpT = numerictype(inputSB, newWL, 4);
        uTemp = fi(0, uTmpT);
    else
        % Keep input precision as it is for uTemp var
        newWL = inputWL;
        uTemp = fi(0, numerictype(u));
    end

    % X-Y : Worst-case normalized X-Y sum range between [0, 4.25]
    [xyWL, xyFL] = localGetXYWordLenFracLen(newWL, inputSB);
    xyNT = numerictype(1, xyWL, xyFL);
    xyFm = fimath( ...
        'RoundingMethod',    'Floor', ...
        'OverflowAction',    'Wrap', ...
        'ProductMode',       'FullPrecision', ...
        'SumMode',           'SpecifyPrecision', ...
        'SumWordLength',     xyWL, ...
        'SumFractionLength', xyFL, ...
        'CastBeforeSum',     true);
    
    % Initialize NORMALIZED value temp variable
    v = fi(0, xyNT, xyFm);
    
    % Constrain number of iterations to the X-Y sum word length.
    % There is no reason to go beyond that word length, since
    % the X-Y sum values will be right shifted to zeros and
    % have no impact on the final result. Also prevents error.
    numItersValue = min(xyWL, numIters_dbl);
    
    % Inverse of CORDIC gain constant, using signed type with best precision
    % fraction length (typical IGC value is ~ 1.2144775390625, i.e., the
    % fraction length is WL-2 for this best precision, signed constant).
    igc = fi(1/An_hp_dbl, true, xyWL, (xyWL-2));
    
    % For simplicity, handling both SIGNED and UNSIGNED inputs
    for idx = 1:numel(u)
        uTemp(1) = u(idx); % Get next input sample (cast to uTemp var)
        
        % Compute CORDICSQRT using normalized range, i.e., [0.5, 2)
        % with even normBitsVal power-of-two, then denormalize back
        % into correct range using normBitsVal/2 power-of-two factor.
        % The power-of-two factors may be implemented using bit shift.
        if uTemp <= 0
            out(idx) = 0;
        else
            [v(1), denormPow2Bits] = ...
                localNormalizeInput(uTemp, xyNT, xyFm);
            
            xNrm = localCORDICSQRT(v, numItersValue);
            
            % We may need extra bits if not enough shift bits available
            % for Denormalization of very small or very large values.
            % (Note: we cannot use reinterpretcast since codegen requires
            %  constant WL and FL values for calls to numerictype below,
            %  and use of reinterpretcast requires these to vary...)
            largeWL    = coder.const(min((2 .* xyWL), 128));
            largeValNT = numerictype(false, largeWL, xyFL);
            smallValNT = numerictype(false, largeWL, xyFL + xyWL);
            
            if (inputMaxRange < 2) || (denormPow2Bits <= 0)
                % SMALL-RANGE DENORMALIZATION:
                smlNormVal = fi(xNrm, smallValNT);
                if doOutScaling
                    % Scale output by inverse of CORDIC gain constant
                    x_cd = bitsra(smlNormVal, (-denormPow2Bits));
                    out(idx) = x_cd .* igc;
                else
                    out(idx) = bitsra(smlNormVal, (-denormPow2Bits));
                end
            else
                % LARGE RANGE DENORMALIZATION:
                lrgNormVal = fi(xNrm, largeValNT);
                if doOutScaling
                    % Scale output by inverse of CORDIC gain constant
                    x_cd = bitsll(lrgNormVal, denormPow2Bits);
                    out(idx) = x_cd .* igc;
                else
                    out(idx) = bitsll(lrgNormVal, denormPow2Bits);
                end
            end
        end
    end
end

end % function


% =========================================================================
function [uNorm, denormPow2Bits] = localNormalizeInput(uInput, xyNT, xyFm)

coder.internal.prefer_const(uInput, xyNT, xyFm);

uNorm = coder.nullcopy(fi(0, xyNT, xyFm));

[uNorm(1), normBitsVal] = fxpFREXP(uInput);

denormPow2Bits = bitsra(normBitsVal, 1);

if (bitsll(denormPow2Bits, 1) ~= normBitsVal)
    uNorm = bitsll(uNorm,1); % multiply by 2 for ODD normBitsVal
end

end % function


% =========================================================================
function [y, r] = fxpFREXP(u)
% fxpFREXP(u): Range normalization
%
% This function assumes:
%
%   1) u is a POSITIVE-valued
%   2) u is a FI fixed-point binary point scaled type
%   3) 4 < u.WordLength <= 128
%
% *** The CALLER must check for this before calling fxpFREXP ***

WL = u.WordLength;
v  = reinterpretcast(u, numerictype(false, WL, 0));
fm = fimath('RoundingMethod', 'Floor', ...
            'OverflowAction', 'Wrap',  ...
            'ProductMode',           'SpecifyPrecision', ...
            'ProductWordLength',     WL, ...
            'ProductFractionLength', 0,  ...
            'SumMode',               'SpecifyPrecision', ...
            'SumWordLength',         WL, ...
            'SumFractionLength',     0,  ...
            'CastBeforeSum',         true);
v  = setfimath(v, fm);

% Define constants for use in comparisons below
twoToThe02Minus1 = fi(3, false, WL, 0, fm); % (2^2 - 1)
twoToThe04Minus1 = fi(15, false, WL, 0, fm); % (2^4 - 1)
if WL > 8
    twoToThe08Minus1 = fi(intmax('uint8'), false, WL, 0, fm); % (2^8 - 1)
    if WL > 16
        twoToThe16Minus1 = fi(intmax('uint16'), false, WL, 0, fm); % (2^16 - 1)
        if WL > 32
            twoToThe32Minus1 = fi(intmax('uint32'), false, WL, 0, fm); % (2^32 - 1)
            if WL > 64
                twoToThe64Minus1 = fi(intmax('uint64'), false, WL, 0, fm); % (2^64 - 1)
            end
        end
    end
end

if WL > 64
    % Assuming 64 < WL <= 128 here
    if (v > twoToThe64Minus1)
        r = bitsll(int16(1), 6);
    else
        r = int16(0);
    end
    v = bitsra(v, r);
    
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe32Minus1, 5);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe16Minus1, 4);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe08Minus1, 3);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe04Minus1, 2);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe02Minus1, 1);
    
elseif WL > 32
    % Assuming 32 < WL <= 64 here
    if (v > twoToThe32Minus1)
        r = bitsll(int16(1), 5);
    else
        r = int16(0);
    end
    v = bitsra(v, r);
    
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe16Minus1, 4);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe08Minus1, 3);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe04Minus1, 2);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe02Minus1, 1);
    
elseif WL > 16
    % Assuming 16 < WL <= 32 here
    if (v > twoToThe16Minus1)
        r = bitsll(int16(1), 4);
    else
        r = int16(0);
    end
    v = bitsra(v, r);
    
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe08Minus1, 3);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe04Minus1, 2);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe02Minus1, 1);
    
elseif WL > 8
    % Assuming 8 < WL <= 16 here
    if (v > twoToThe08Minus1)
        r = bitsll(int16(1), 3);
    else
        r = int16(0);
    end
    v = bitsra(v, r);
    
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe04Minus1, 2);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe02Minus1, 1);
    
else
    % Assuming 4 < WL <= 8 here
    if (v > twoToThe04Minus1)
        r = bitsll(int16(1), 2);
    else
        r = int16(0);
    end
    v = bitsra(v, r);
    [v(:), r(:)] = doCompareLRShiftsBitor(v, r, twoToThe02Minus1, 1);
end

r(:) = bitor( r, int16(bitsra(v,1)) ) + 1;  % plus one for ceiling
r(:) = r - cast(u.FractionLength,'like',r); % signed integer value

% Need to temporarily grow bits prior to right or left shifts.
% Minimum number of WL bits to grow is R. However word length
% needs to remain constant for code generation (and sim must
% match code generation) so use WL + WL for temporary variable.
if r >= cast(0,'like',r)
    % R is NON-NEGATIVE -> RIGHT SHIFT to smaller range
    uTmp = fi(u, false, (WL + WL), (WL + u.FractionLength)); % avoid round
    y    = fi(bitsra(uTmp, r), false, WL, WL); % Downcast back to WL
else
    % R is NEGATIVE (input is a small fractional value < 0.5)
    uTmp = fi(u, false, (WL + WL), u.FractionLength);
    y    = fi(bitsll(uTmp, -r), false, WL, WL); % Downcast back to WL
end

end % function


% =========================================================================
function [v,r] = doCompareLRShiftsBitor(v,r,c,n)
coder.internal.prefer_const(c,n);
if (v > c)
    sh = bitsll(int16(1), n);
    v  = bitsra(v, sh);
    r  = bitor( r, sh);
end
end % function


% =========================================================================
function x = localCORDICSQRT(v,n)

coder.internal.prefer_const(v,n);

% Initialize and run CORDIC kernel for N iterations

x = v + cast(0.25, 'like', v); % v + 0.25 in same data type
y = v - cast(0.25, 'like', v); % v - 0.25 in same data type

k = 4; % Used for the repeated (3*k + 1) iteration steps

for idx = 1:n
    xtmp = bitsra(x, idx); % multiply by 2^(-idx)
    ytmp = bitsra(y, idx); % multiply by 2^(-idx)
    if y < 0
        x(:) = x + ytmp;
        y(:) = y + xtmp;
    else
        x(:) = x - ytmp;
        y(:) = y - xtmp;
    end
    
    if idx==k
        xtmp = bitsra(x, idx); % multiply by 2^(-idx)
        ytmp = bitsra(y, idx); % multiply by 2^(-idx)
        if y < 0
            x(:) = x + ytmp;
            y(:) = y + xtmp;
        else
            x(:) = x - ytmp;
            y(:) = y - xtmp;
        end
        k = 3*k + 1;
    end
end % idx loop

end % function


% =========================================================================
function [xyWL, xyFL] = localGetXYWordLenFracLen(inputWL, inputSB)
% Do not throw away LSBs after normalization (grow extra bits).
% Also may need to account for one extra bit for possible
% unsigned-to-signed input casts. Also need to handle maximum range
% using SIGNED arithmetic, using a minimum 8-bit WL for the X-Y sums
% (i.e., to handle worst case sum value 4.25, we need 4 extra bits),
% and a maximum of 128-bit WL for the X-Y sums (max alg supported WL).

intBits = 4; % Extra bits to handle max sum value
xyWL = min(max(8, inputWL + intBits + double(~inputSB)), 128);
xyFL = xyWL - intBits; % for X-Y in range [0, 4.25]

end % function


% =========================================================================
function numIters_dbl = checkNITERSArgCG(niters, numItersDefault)
coder.internal.prefer_const(niters, numItersDefault);
eml_cordic_check_niters_arg(niters);
if ~isempty(niters) && isfinite(niters)
    % Clip maximum NITERS to X-Y sum word length
    % to avoid code generation errors with use of
    % BITSRA when NITERS is too large to matter.
    numItersSpec = double(niters);
    nitersMAXVAL = numItersDefault + 4;
    if numItersSpec > nitersMAXVAL
        numIters_dbl = nitersMAXVAL;
    else
        numIters_dbl = numItersSpec;
    end
else
    numIters_dbl = numItersDefault;
end

end % function


% =========================================================================
function doOutScaling = checkScaleOutputPVPairCG(paramStr, paramVal)
coder.internal.prefer_const(paramStr, paramVal);

eml_invariant(~isempty(paramStr) && ischar(paramStr), ...
              'fixed:coder:invalidInputArg');

eml_invariant(coder.const(strcmp(coder.internal.toLower(paramStr),'scaleoutput')), ...
    'fixed:cordic:invalidParameter');

eml_invariant(isscalar(paramVal) && ...
              ( islogical(paramVal) || ...
                (isnumeric(paramVal) && isreal(paramVal) && isfinite(paramVal)) ), ...
              'fixed:cordic:invalidScaleOutputArg');

doOutScaling = logical(paramVal);

end % function
