%% Joshua Mack, Sam Bellestri, Nia Simmonds, IREECE 2015
%% Implements CORDIC algorithm for use in calculating sinh, cosh, and atanh
% inputs: mode, M, N, x0, y0, and z0
% 
% NOTE: Region of Convergence depends on M.
% M = -1 corresponds to the default CORDIC with convergence in [-1.118,
% 1.118] for cosh(z0) and sinh(z0), and [-0.80694, 0.80694] for atanh(y0/x0)
% See "Expanding the Range of Convergence of the CORDIC Algorithm" (Hu et al)
% For more details about choosing the correct value of M.

function [u, v] = Quantized_FloP_CORDIC_Model(mode, M, N, ExpWidth, FracWidth, x0, y0, z0)
    %Preallocate memory and initialize initial conditions.
    q = quantizer('float', [ExpWidth+FracWidth+1 ExpWidth]);
    [x, y, z] = deal(zeros(M+N+1,1));
    x(1) = quantize(q, x0); y(1) = quantize(q, y0); z(1) = quantize(q, z0);
    %If the mode is rotation...
    if mode == 'r'
        % Formulas: 
        % See Hu et al for specific details.
        % Description of values:
        %  - x and y are, in the original CORDIC, essentially rectangular
        %    coordinates
        %  - z forms a sequence whose sum converges to 0.
        %  - "sign(z(x))" is commonly written as d(i), delta(i), etc.
        %  - "atanh(2^(-i))" or "atanh(1-2^(i-2))" are simply sequences
        %    that fulfill some properties necessary for CORDIC to work.
        
        %Begin with the sum from -M to 0 (which deals with expanding the
        %interval of convergence), and then continue on with the original
        %set of iterations from 1 to N.
        i = -M;
        %Counter to keep track of special cases where i = (3^(j+1)-1)/2
        j = 1;
        while i < N
            if i <= 0
                %Taken directly from Hu et al. Another form was presented
                %that yielded higher regions of convergence in less
                %iterations, but this one was chosen instead for simplicity.
                x(i+1+M+1) = quantize(q, x(i+M+1) + quantize(q, sign(z(i+M+1))*y(i+M+1)*quantize(q,(1-2^(i-2)))));
                y(i+1+M+1) = quantize(q, y(i+M+1) + quantize(q, sign(z(i+M+1))*x(i+M+1)*quantize(q,(1-2^(i-2)))));
                z(i+1+M+1) = quantize(q, z(i+M+1) - quantize(q, sign(z(i+M+1))*quantize(q, atanh(1-2^(i-2)))));
            else
                x(i+1+M+1) = quantize(q, x(i+M+1) + quantize(q, sign(z(i+M+1))*y(i+M+1)*quantize(q, 2^(-i))));
                y(i+1+M+1) = quantize(q, y(i+M+1) + quantize(q, sign(z(i+M+1))*x(i+M+1)*quantize(q, 2^(-i))));
                z(i+1+M+1) = quantize(q, z(i+M+1) - quantize(q, sign(z(i+M+1))*quantize(q, atanh(2^(-i)))));

                %Special case that needs to be taken care of to ensure
                %convergence.
                if (i+M+1) == ((3^(j+1) - 1)/2)
                   x(i+1+M+1) = quantize(q, x(i+1+M+1) + quantize(q, sign(z(i+M+1))*y(i+1+M+1)*quantize(q, 2^(-i))));
                   y(i+1+M+1) = quantize(q, y(i+1+M+1) + quantize(q, sign(z(i+M+1))*x(i+1+M+1)*quantize(q, 2^(-i))));
                   z(i+1+M+1) = quantize(q, z(i+1+M+1) - quantize(q, sign(z(i+M+1))*quantize(q, atanh(2^(-i)))));
                   j = j + 1;
                end %end special case.
            end %end if i <= 0
            i = i + 1;
        end %end while
        % This modification changes the scaling factor to depend on M, so
        % the method of "set x0 = 1/K" doesn't work anymore unless you
        % calculate K beforehand (and remove the division below by K).
        % Again, see Hu et al.
        % vv--Determines scaling from -M to 0--vv  vv--Scaling on 1 to N--vv
        K = quantize(q, prod(sqrt(1-(1-2.^((-M:1:0)-2)).^2))) * quantize(q, prod(sqrt(1-2.^(-2.*(1:1:N)))));
        %u = K*(x0*cosh(z0) + y0*sinh(z0))/K = cosh(z0) if x0 = 1, y0 = 0.
        u = quantize(q, x(M+N+1)/K);
        %v = K*(x0*sinh(z0) - y0*cosh(z0))/K = sinh(z0) if x0 = 1, y0 = 0.
        v = quantize(q, y(M+N+1)/K);      
        
        % And we're done here...
%         fprintf(['Cosh(%g) = ', num2str(u), '\n'], z0);
%         fprintf(['Sinh(%g) = ', num2str(v), '\n'], z0);
        return;
        
    %Else if the mode is vectoring...
    elseif mode == 'v'
        %Iterate those other formulas N times.
        i = -M;
        j = 1;
        d = zeros(M+N+1,1);
        while i < N
            if i <= 0
                % vv--This is the one big change in vectoring mode. --vv
                d(i+M+1) = quantize(q, -1*sign(y(i+M+1)));
                % ^^--Now d(i) depends on the sign of y, not z--^^
                %This stuff's basically the same, otherwise.
                x(i+1+M+1) = quantize(q, x(i+M+1) + quantize(q, d(i+M+1)*y(i+M+1)*(quantize(q,1)-quantize(q, 2^(i-2)))));
                y(i+1+M+1) = quantize(q, y(i+M+1) + quantize(q, d(i+M+1)*x(i+M+1)*(quantize(q,1)-quantize(q, 2^(i-2)))));
                z(i+1+M+1) = quantize(q, z(i+M+1) - quantize(q, d(i+M+1)*quantize(q, atanh(1-2^(i-2)))));
            else
                %Oh look! A change again!
                d(i+M+1) = quantize(q, -1*sign(y(i+M+1)));
                %And...more of the same.
                x(i+1+M+1) = quantize(q, x(i+M+1) + quantize(q, d(i+M+1)*y(i+M+1)*quantize(q, 2^(-i))));
                y(i+1+M+1) = quantize(q, y(i+M+1) + quantize(q, d(i+M+1)*x(i+M+1)*quantize(q, 2^(-i))));
                z(i+1+M+1) = quantize(q, z(i+M+1) - quantize(q, d(i+M+1)*quantize(q, atanh(2^(-i)))));

                if (i+M+1) == ((3^(j+1) - 1)/2)
                   % vv--One of the last changes!--vv
                   d(i+M+1) = quantize(q, -1*sign(y(i+1+M+1))); 
                   % ^^--Right here!--^^
                   x(i+1+M+1) = quantize(q, x(i+1+M+1) + quantize(q, d(i+M+1)*y(i+1+M+1)*quantize(q, 2^(-i))));
                   y(i+1+M+1) = quantize(q, y(i+1+M+1) + quantize(q, d(i+M+1)*x(i+1+M+1)*quantize(q, 2^(-i))));
                   z(i+1+M+1) = quantize(q, z(i+1+M+1) - quantize(q, d(i+M+1)*quantize(q, atanh(2^(-i)))));
                   j = j + 1;
                end
            end
            i = i + 1;
        end
        % And then, *this* is the last change.
        % These values converge differently, so this time, z converges to
        % arctanh(y0/x0) + z0. So, subtract the initial value.
        u = quantize(q, z(N+M+1) - z(1));
        % And this guy's useless. Or it could be used for square root I suppose
        % but we're not using it for anything.
        v = 'N/A';
        ratio = y0 / x0;
        
%         fprintf(['arctanh(%g) = ', num2str(u), '\n'], ratio);
    end %End mode selection
    return;
end

function x = sign(z)
    if (z >= 0)
        x = 1;
    else
        x = -1;
    end
end