%Sigma Delta E3

clear all
close all
pkg load all

%sim param
Vr = 10^(4/20)*sqrt(2)*0.5
Vrms = 1*sqrt(2);
Pn = 10^(-92/20)
Pnq = Pn/3
Pnt = Pn*2/3

%osr = nthroot(((Vr/2)^2*pi^6)/(Pnq^2*12*7),7)
osr = 37.4;
n_sim = 2^15;
B = 10e6;
fs = osr*2*B
f_sin = fs*20/n_sim;
M = round(3*B/f_sin);

Vnq=10^(-102/20);
res_bits = log10((Vr/2)^2/(12*Pnq^2))/(2*log10(2))

%vin
Ain = sqrt(2)*0.001;
np = 1:n_sim;

vin = Ain*sin(2*pi*np*f_sin*1/fs)+Pnt*rand;

x1 = zeros(1,size(vin)(2));
x2 = zeros(1,size(vin)(2));
x3 = zeros(1,size(vin)(2));
x4 = zeros(1,size(vin)(2));
n1 = zeros(1,size(vin)(2));
n2 = zeros(1,size(vin)(2));
n3 = zeros(1,size(vin)(2));
s1 = zeros(1,size(vin)(2));
s2 = zeros(1,size(vin)(2));
s3 = zeros(1,size(vin)(2));
s4 = zeros(1,size(vin)(2));
d1 = zeros(1,size(vin)(2));
d2 = zeros(1,size(vin)(2));
d3 = zeros(1,size(vin)(2));
d4 = zeros(1,size(vin)(2));
dout = zeros(1,size(vin)(2));
dout1 = zeros(1,size(vin)(2));
dout2 = zeros(1,size(vin)(2));
dout1_aux = zeros(1,size(vin)(2));
dout2_aux1 = zeros(1,size(vin)(2));
dout2_aux2 = zeros(1,size(vin)(2));
vfeed = zeros(1,size(vin)(2));

b1=1*Vr;
b2=1*Vr;
k=1;


%2nd order sigma delta sim
%for i=2:size(dout)(2)-2
%  dout(i) = sign(x2(i-1));
%  n1(i) = vin(i)-dout(i)*b2;
%  x1(i) = n1(i) + x1(i-1);
%  n2(i) = x1(i)-dout(i)*b1;
%  x2(i) = n2(i) + x2(i-1);
%endfor

%1st order sigma delta sim
%for i=2:size(dout)(2)-2
%  dout(i) = sign(x3(i-1));
%  n3(i) = vin(i)-dout(i)*Vr; %x1[n-1]
%  x3(i) = n3(i) + x3(i-1);
%endfor

n_sinad = 2;
SNR_d = zeros(1,n_sinad);
SNR_dec = zeros(1,n_sinad);
amp_in = zeros(1, n_sinad);

%MASH 2+1
for n=0:n_sinad-1

    Ain = sqrt(2)*((0.1-0.0001)/(n_sinad-1)*(n+1)-((0.1-0.0001)/(n_sinad-1)-0.0001));

    vin = Ain*sin(2*pi*np*f_sin*1/fs)+Pnt*rand(1, max(size(vin)));

  for i=3:size(dout)(2)
    %2nd order modulator
    dout1(i) = sign(x2(i-1));
    n1(i) = vin(i)-dout1(i)*b2;
    x1(i) = n1(i) + x1(i-1);
    n2(i) = x1(i)-dout1(i)*b1;
    x2(i) = n2(i) + x2(i-1);

    vfeed(i) = x2(i-1)-dout1(i)*Vr;

    %first order modulator
    dout2(i) = sign(x3(i-1))*k;
    n3(i) = vfeed(i)*Vr-dout2(i)*Vr;
    x3(i) = n3(i) + x3(i-1);

    %output
    dout(i) = dout1(i-1) - (dout2(i)-2*dout2(i-1)+dout2(i-2));

    %decimation filter (4th order synk filter)
    if(i>M)
      s1(i) = (dout(i)/M)+s1(i-1);
      d1(i) = s1(i)-s1(i-M);
      s2(i) = (d1(i)/M)+s2(i-1);
      d2(i) = s2(i)-s2(i-M);
      s3(i) = (d2(i)/M)+s3(i-1);
      d3(i) = s3(i)-s3(i-M);
      s4(i) = (d3(i)/M)+s4(i-1);
      d4(i) = s4(i)-s4(i-M);
    endif
  endfor

  aux = fft(dout.*blackmanharris(max(size(dout)))')/max(size(dout));
  fft_x = (aux .*conj(aux));

  [x, center_d] = max(fft_x(1:100));

  Ps_d = sum(fft_x((center_d-3):(center_d+3)));
  Pn_d = sum(fft_x(4:200))-Ps_d;
  SNDR_d(n+1) = 10*log10(Ps_d/Pn_d)

  aux1 = fft(dout1.*blackmanharris(max(size(dout1)))')/max(size(dout1));
  fft_x_1 = (aux1 .*conj(aux1));

  aux2 = fft(dout2.*blackmanharris(max(size(dout2)))')/max(size(dout2));
  fft_x_2 = (aux2 .*conj(aux2));

  aux_dec = fft(d4.*blackmanharris(max(size(d4)))')/max(size(d4));
  fft_x_dec = (aux_dec .*conj(aux_dec));

  Ps_dec = sum(fft_x_dec((center_d-5):(center_d+5)));
  Pn_dec = sum(fft_x_dec(1:100))-Ps_dec;
  SNDR_dec(n+1) = 10*log10(Ps_dec/Pn_dec)
endfor

figure(1)
semilogx(10*log10(fft_x_1), 'color', 'b')
hold on
semilogx(10*log10(fft_x_2), 'color', 'g')
title('1st and 2nd order modulators FFT')
grid on

figure(2)
semilogx(10*log10(fft_x), 'color', 'b')
title('3rd order modulator FFT')
grid on

figure(3)
semilogx(10*log10(fft_x_dec), 'color', 'b')
title('Filtered 3rd order modulator FFT')
grid on

figure(4)
plot([SNDR_dec' amp_in'])
title('Post filter SINAD')