#clear all
#close all
#pkg load statistics
if false
PPS_phase=load("..\\PPS 6H phase.csv");
PPS=diff(PPS_phase(1:10001),1);
TCXO_phase = load("..\\TCXO 6h phase.csv");
TCXO = TCXO_phase(1:10000);
else # simulated data
TCXO = 1e-6*sin(0:0.0005:5)+5e-10*(rand(1,10001)-0.5);
PPS = 2e-8*(rand(1,10001)-0.5);
endif
graphics_toolkit("fltk")


close all
g = gpsdo(TCXO,PPS,0.01,0.0001);
subplot(2,2,3)
plot(PPS)
hold on
plot(TCXO/20)
#plot(10*g)
legend("PPS","TCXO/20")
title("input")
hold off
subplot(2,2,4)
plot_adev(TCXO, PPS,g)
legend("TCXO","PPS","GPSDO")

function retval=adev(data)
  retval = [];
  ADEV_x = [];
  ds = length(data);
  for tau =round(power(10,0:0.1:4))
    if tau< ds/4
      cnt = length(data) - 2*tau;
      retval = cat(1,retval, [tau, sqrt(sumsq(data(1:ds-2*tau) - data(1+tau:ds-tau)- data(1+tau:ds-tau) + data(1+2*tau:ds )) / (cnt*2))/tau]);
    endif
  endfor
#  retval = [ADEV_x,retval]
endfunction


function plot_adev(p1,p2,p3)
ap1 = adev(p1);
loglog(ap1(:,1), ap1(:,2));
hold on
ap2 = adev(p2);
loglog(ap2(:,1), ap2(:,2));
ap3 = adev(p3);
loglog(ap3(:,1), ap3(:,2));
hold off
title("ADEV")
endfunction

function phase = gpsdo(osc,pps, kp, ki)
  phase(1) = 0;
  freq(1) = 0;
  phase_predict(1) = 0;
  freq_predict(1) = 0;
  alpha = 0.01;
  beta = 0.001;
  f = 0;
  err = 0;
  sumerr = 0;
  len = min(length(pps),length(osc));
  for i = 2:len
    phase(i) = phase(i-1) + f + osc(i) - osc(i-1);
    freq(i) = phase(i) - phase(i-1);
    err = pps(i) - phase(i);
    if false   # false means no alpha/beta filter
      #update
      phase(i) = phase_predict(i-1) + alpha * (phase(i) + pps(i) - phase_predict(i-1));
      freq(i) = freq_predict(i-1) + beta * (phase(i) + pps(i) - phase_predict(i-1));
      #predict
      phase_predict(i) = phase(i) + freq(i);
      freq_predict(i) = freq(i);
      err = -phase_predict(i);
    end
    sumerr = sumerr + err;
    f =  kp * err + ki * sumerr;
  endfor
  subplot(2,2,1)
  plot(diff(osc,1))
  hold on
  plot(freq)
  legend("without" , "with")
  title("freq")
  hold off
  subplot(2,2,2)
  plot(osc)
  hold on
  plot(phase*10)
  title("phase")
  legend("without" , "with*10")
  hold off
endfunction




