clear all; close all; clc;

% Generate 20 data points each (Rotation, Vectoring).
% Example on how to create an input file and then how to read output file:
% This example uses 20 data points for rotation

An = 1.6468;
xt = zeros(1,20);
yt = (1/An)*ones(1,20);
zt = linspace(-pi/2,pi/2,20); % 100 equally-spaced values between -pi/2 and pi/2

% For num2hex conversion, we use the 'quantizer' object:
%   This requires the Fixed-Point Toolbox.
q = quantizer ('fixed','round', [16 14]);

fod = fopen('in_benchR.txt', 'wt');
for i = 1:20
      stream = [num2hex(q,xt(i)) num2hex(q,yt(i)) num2hex(q,zt(i))];
          % we convert every input value to the FX representation [16 14]
          % (hexadecimals)
      fprintf (fod, '%s\n',stream);
end
 fclose (fod);
 
 xp = linspace(0, 0.5, 20);
 yp = ones(1,20);
 zp = zeros(1,20);

 fad = fopen('in_benchV.txt', 'wt');
 for i = 1:20
     stream = [num2hex(q,xp(i)) num2hex(q,yp(i)) num2hex(q,zp(i))];
     fprintf (fad, '%s\n',stream);
 end
 fclose(fad);
 
 % Getting the CORDIC results from the MATLAB model (Basic Circular CORDIC)
 N = 16; % Results for N=16 iterations
 for i = 1:20
     [xr{i}, yr{i}, zr{i}] = cordic_circular(xt(i), yt(i), zt(i), 0, N,0); % Rotation
     [xv{i}, yv{i}, zv{i}] = cordic_circular(xp(i), yp(i), zp(i), 0, N,0); % Vector
 end 
 
 for i=1:20
     xr_o(i) = xr{i}(N+1); yr_o(i) = yr{i}(N+1); zr_o(i) = zr{i}(N+1);
     xv_o(i) = xv{i}(N+1); yv_o(i) = yv{i}(N+1); zv_o(i) = zv{i}(N+1);
 end
 
 % Retrieving data from the Vivado simulation:
 
fid = fopen('out_bench.txt','r');
A = textscan (fid, '%s');
D = A{1};
L = length(D); % we expect L to be 40 (40 lines of data)
disp (D); % Prints data from text file
for i = 1:L
    x_FPGA(i) = hex2num(q,D{i}(1:4));
    % Rotation: first 20 values, Vectoring: second 20 values.
    y_FPGA(i) = hex2num(q,D{i}(5:8));
    z_FPGA(i) = hex2num(q,D{i}(9:12));
end

% Ploting Rotation results for 'x':
figure; plot (x_FPGA(1:20),'.:r')
figure; plot (xr_o,'.:b');