%% Joshua Mack, Sam Bellestri, Nia Simmonds, IREECE 2015
%  Implements CORDIC algorithm for use in calculating sinh, cosh, and atanh
%  Has "mode" and "N" as parameters and x0, y0, and z0 as inputs.
%  
%  NOTE: Converges properly within roughly [-1.118, 1.118]
%  Thus, it must be that the sum from 0 to infinity of
%  arctanh(2^(-sigma(n))) is roughly 1.118.

function [u, v] = Improved_CORDIC_Model_OG(mode, N, x0, y0, z0)
    % Initialize initial values.
    xOld = x0; yOld = y0; zOld = z0;
    
    %If the mode is rotation...
    if mode == 'r'
        %Formulas: 
        % x[n+1] = x[n] - m * d[n] * y[n] * 2^(-sigma(n))
        % y[n+1] = y[n] + d[n] * x[n] * 2^(-sigma(n))
        % z[n+1] = z[n] - d[n] * w_omega[n]
        %Description of values:
        % x = x-coordinate, y = y-coordinate, z = angle adjustment, 
        % d[i] = sign(z[i]), w_omega[n] = arctanh(2^-k), m = -1 for
        % hyperbolic calculations, 0 for linear, and 1 for circular
        % sigma(i) = n - k, w/ k max int s.t. 3^(k+1) + 2k - 1 <= 2N
        %Results (for hyperbolic):
        % As n gets large, x[n] -> K'*(x1*cosh(z1) + y1*sinh(z1)),
        % y[n] -> K'*(y1*cosh(z1) + x1*sinh(z1)), and z[n] -> 0
        %Iterate those formulas N times.
        i = 1;
        j = 1;
        while i <= N
            %TODO: Replace this with precomputed atanh values.
            xNew = xOld + sign(zOld)*yOld*2^(-i);
            yNew = yOld + sign(zOld)*xOld*2^(-i);
            zNew = zOld - sign(zOld)*atanh(2^(-i));
            if i == ((3^(j+1) - 1)/2)
               xNew = xNew + sign(zNew)*yNew*2^(-i);
               yNew = yNew + sign(zNew)*xNew*2^(-i);
               zNew = zNew - sign(zNew)*atanh(2^(-i));             
               j = j + 1;
            end
            xOld = xNew; yOld = yNew; zOld = zNew;
            i = i + 1;
        end
          u = xNew; v = yNew;
    %Else if the mode is vectoring...
    elseif mode == 'v'
        % Change to "-sign(yNew)" and calculate atanh instead.
        % As n >> 1, x approaches K*sqrt(x0^2 - y0^2)
        %            y approaches 0
        %            and z approaches z0 + atanh(y0/x0)
        % We personally care about that last one.
        i = 1; j = 1;
        while i <= N
            xNew = xOld - sign(yOld)*yOld*2^(-i);
            yNew = yOld - sign(yOld)*xOld*2^(-i);
            zNew = zOld + sign(yOld)*atanh(2^(-i));
            if i == ((3^(j+1) - 1)/2)
               xNew = xNew - sign(yNew)*yNew*2^(-i);
               yNew = yNew - sign(yNew)*xNew*2^(-i);
               zNew = zNew - sign(yNew)*atanh(2^(-i));
               j = j + 1;
            end
            xOld = xNew; yOld = yNew; zOld = zNew;
            i = i + 1;
        end
          u = zNew - z0;
          v = 'N/A';
    end
    return;
end

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