%% 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] = CORDIC_Model_OG(mode, N, x0, y0, z0)
    %Preallocate memory
    %A nifty trick I just found that distributes ("deals") the assignment.
    %TODO: Fix the waste in memory allocation.
    [x, y, z, d, w_omega] = deal(zeros(2*N,1));
    %Prompt input
%     fprintf('Enter x0, y0, and z0:\n');
%     x(1) = input('x0: '); y(1) = input('y0: '); z(1) = input('z0: ');
    x(1) = x0; y(1) = y0; z(1) = 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;
%         for i = 1:1:N
        while i <= N
            w_omega(i) = atanh(2^(-i));
            d(i) = sign(z(i));
            
            x(i+1) = x(i) + d(i)*y(i)*2^(-i);
            y(i+1) = y(i) + d(i)*x(i)*2^(-i);
            z(i+1) = z(i) - d(i)*w_omega(i);
            
            if i == ((3^(j+1) - 1)/2)
               %display(i);
               d(i) = sign(z(i+1)); 
               %w_omega(i) = atanh(2^(-(i+1)));
               
               x(i+1) = x(i+1) + d(i)*y(i+1)*2^(-i);
               y(i+1) = y(i+1) + d(i)*x(i+1)*2^(-i);
               z(i+1) = z(i+1) - d(i)*w_omega(i);
               
               j = j + 1;
            end
            
            i = i + 1;
            %display(x(i));
            
        end
        u = x(N+1);
        v = y(N+1);
    %Else if the mode is vectoring...
    elseif mode == 'v'
        %Iterate those other formulas N times.
        i = 1;
        j = 1;
        while i <= N
            %TODO: Replace this with precomputed atanh values.
            w_omega(i) = atanh(2^(-i));
            d(i) = -1*sign(y(i));
            
            x(i+1) = x(i) + d(i)*y(i)*2^(-i);
            y(i+1) = y(i) + d(i)*x(i)*2^(-i);
            z(i+1) = z(i) - d(i)*w_omega(i);
            
            if i == ((3^(j+1) - 1)/2)
               d(i) = -1*sign(y(i+1)); 
               %w_omega(i) = atanh(2^(-(i+1)));
               
               x(i+1) = x(i+1) + d(i)*y(i+1)*2^(-i);
               y(i+1) = y(i+1) + d(i)*x(i+1)*2^(-i);
               z(i+1) = z(i+1) - d(i)*w_omega(i);
               
               j = j + 1;
            end
            
            i = i + 1;
        end
        u = z(N+1) - z(1);
        v = 'N/A';
    end
    return;
endfunction