// Статические переменные

funcprot(0);

Fd = 168/9*1e6; // Частота дискретизации
NADC = 8; // Количество бит АЦП

nmem1 = 2^16;  // количество ячеек памяти
nmem2 = nmem1 -5; // после медианного фильтра

// определим время выборки
Td = 1/Fd;
Tm = nmem * Td

// Медианный фильтр по пяти точкам
function s2 = Median5(s1, nn)
    s2 = zeros(1:nn-5); // Обнуление выходного массива
    for ii = 1:(nn-5)
        v1(1)=s1(ii);
        v1(2)=s1(ii+1);
        v1(3)=s1(ii+2);
        v1(4)=s1(ii+3);
        v1(5)=s1(ii+4);
        v2 = gsort(v1);
        s2(ii) = v2(3);
    end
endfunction

// Скользящее среднее по пяти точкам
function s2 = Mov_avg(s1, nn)
  s2 = zeros(1:nn); // Обнуление выходного массива
  x1 = 0;
  for ii = 1:5
    x1 = x1 + s1(ii);
    s2(ii) = s1(ii);
  end
  for ii = 1:(nn-5)
    s2(ii) = int(x1/5);
    x1 = x1 - s1(ii+1);
    x1 = x1 + s1(ii+2); 
  end
endfunction

// Получение оценки максимума и минимума по гистограмме
function [ymin, ymax] = Histo1(s2)
  // Формируем гистограмму из массива
  // знаем, что разрядность АЦП 8 бит - 256 значений
  //  тип данных gg unsigned short
  gg = zeros(1:256);  // Обнуление массива
  for ii = 1:nmem2
    gg(s2(ii)) = gg(s2(ii)) + 1;
  end

  // Поиск максимумов в гистограмме

  ymin = 0;
  ymax = 0;  
  y = 0;  
  for ii = 1:124  // Серединку не обрабатываем
    if gg(ii) > y then
      y = gg(ii);
      ymin = ii;
    end
  y = 0;
  for ii = 130:256
    if gg(ii) > y then
      y = g(ii);
      ymax = ii;
    end
endfunction

// Последовательный поиск максимумов и минимумов, оценка пиков
// и вычисление среднего размаха











// Средний размах + Оценка периода
function [Fest, Sum1, s3] = Filter2(s2)
    nx = int((2/F11)/Td);
    nx2 = int((1/4/F11)/Td);
//    disp(nx);
//    disp(nx2);

    if nx2<2 then
        nx2 = 2;
    end
    s5 = zeros(nn);
//    for ii=1:nn-nx+1
//        s4(1:nx) = s2(ii:ii+nx-1)
//        vmax=max(s4);
//        vmin=min(s4);
//        vref=(vmax+vmin)/2;
//        s5(ii) = s2(ii) - vref;
//        s3(ii) = vmax-vmin;
//    end

    for ii=0:int(nn/nx)-1
        s4(1:nx) = s2(1+ii*nx:ii*nx+nx)
        vmax=max(s4);
        vmin=min(s4);
        vref=(vmax+vmin)/2;
        s5(1+ii*nx:ii*nx+nx) = s2(1+ii*nx:ii*nx+nx) - vref;
        s3(1+ii*nx:ii*nx+nx) = vmax-vmin;
    end

    Sum1=0;
    for ii=1:nn-nx;
        Sum1=Sum1+s3(ii);
    end
    Sum1=Sum1/(nn-nx);
    // Считаем периоды и оцениваем частоту
    nper = 0;
    jj=1;
    while s5(jj)>0
        jj=jj+1
    end
    while s5(jj)<0
      jj=jj+1
    end
    kk1 = jj
    while jj<nn-nx
      jj = jj + nx2;
      while (jj<nn-nx) && (s5(jj)<0)
        jj=jj+1;
      end
      nper = nper +1;
      kk2 = jj;
      jj = jj + nx2;
      while (jj<nn-nx) &&(s5(jj)>0)
        jj=jj+1;
      end
   end
   Fest = 1/(((kk2-kk1)*Td)/nper);
endfunction




function [gg, Fest, Vest]=Estim2(s1)
  // !!!!!!! Обработка целочисленных данных !!!!!!!!
  // Размер окна примерно 10 периодов на минимальной частоте
  wn = int(Fd/F11)*10;
  // Объём входных данных
  nmax = size(s1,1);
  ymin = 256;
  ymax = 0;
  ypp = 0;
  yhist = zeros(1:256);
  xhist = zeros(1:256);
  prlag = 8; // Защитный интервал
  prtmp = 8;
  Tr2 = 0;
  xzero = 0;
  xn1 = 1;
  xn2 = wn;

  // Первый цикл - инициализация
  for i=xn1:xn2
    if ymin>s1(i) then ymin = s1(i) end;
    if ymax<s1(i) then ymax = s1(i) end;
  end
  ypp = ymax-ymin;  
  yhist(ypp) = yhist(ypp)+1;
  trh = int(ypp/2)+ymin;
  // Первый перход через 0
  Tr3 = 0;
  for i=xn1:xn2
    // Поиск переходов через 0
    stmp = s1(i);
    if prtmp >= prlag then
      if Tr2 == 1 then
        if stmp>trh then
          prtmp = 0;
          if Tr3 == 1 then
            xhist(i-xzero)=xhist(i-xzero)+1;
          end
          xzero = i;
          Tr3 = 1;
          Tr2 = 0;
        end
      else
        if stmp<trh then
          prtmp = 0;
          Tr2 = 1;
        end
      end
    end
    prtmp = prtmp+1;
  end;

  while xn2<nmax
      xn1 = xn1+1;
      xn2 = xn2+1;
      stmp = s1(xn2);
      Tr1 = 0;
      if s1(xn2)>ymax then
        ymax = stmp;
      elseif s1(xn1-1)==ymax then
        Tr1=1;
      end
      if s1(xn2)<ymin then
        ymin = stmp;
      elseif s1(xn1-1)==ymin then
        Tr1 = 1;
      end
      if Tr1==1 then
        ymin = 256;
        ymax = 0; 
        for i=xn1:xn2
          if ymin>s1(i) then ymin = s1(i) end;
          if ymax<s1(i) then ymax = s1(i) end;
        end
      end
    ypp = ymax-ymin;  
    yhist(ypp) = yhist(ypp)+1;

    // Поиск переходов через 0
    trh = int(ypp/2)+ymin;
    if prtmp >= prlag then
      if Tr2 == 1 then
        if stmp>trh then
          prtmp = 0;
          xhist(xn2-xzero)=xhist(xn2-xzero)+1;
          xzero = xn2;
          Tr2 = 0;
        end
      else
        if stmp<trh then
          prtmp = 0;
          Tr2 = 1;
        end
      end
    end
    prtmp = prtmp+1;
 
  end

  gg = yhist;
  // Оценка гистограмм
  x=0;
  y=0;
  for i=1:256
    if yhist(i)>y then
      y=yhist(i);
      Vest=i;
    end
    if xhist(i)>x then
      x=xhist(i);
      Fest=i;
    end
  end
endfunction

rand("uniform");
rand("seed");
//Fi1 = 0.1570796-%pi/1000;
//Fi1 = 0
//for rkk=1:100
//  disp(rkk);
//  Fi1=Fi1+%pi/1000;  
//  s1 = ADC1();
//  s2 = Filter1(s1);
//  [Fest, Sum1, s3] = Filter2(s2);
//  disp((Sum1/2-A1)/A1*100);
//  disp((Fest-F1)/F1*100);
//end

//s1 = ADC1();
//sw1 = s1(1:1024);
//gg = MyHist1(sw1);
//plot2d(gg);

fl1(1)="c:\TestADC\10khz_809mv_without_R.txt";
fl1(2)="c:\TestADC\10khz_809mv_with_R.txt";

fl1(3)="c:\TestADC\300khz_201mv_without_R.txt";
fl1(4)="c:\TestADC\300khz_201mv_with_R.txt";
fl1(5)="c:\TestADC\300khz_802mv_without_R.txt";
fl1(6)="c:\TestADC\300khz_802mv_with_R.txt";
fl1(7)="c:\TestADC\400khz_201mv_without_R.txt";
fl1(8)="c:\TestADC\400khz_201mv_with_R.txt";
fl1(9)="c:\TestADC\400khz_802mv_without_R.txt";
fl1(10)="c:\TestADC\400khz_802mv_with_R.txt";
fl1(11)="c:\TestADC\500khz_201mv_without_R.txt";
fl1(12)="c:\TestADC\500khz_201mv_with_R.txt";
fl1(13)="c:\TestADC\500khz_802mv_without_R.txt";
fl1(14)="c:\TestADC\500khz_802mv_with_R.txt";


uinp(1) = 809;
uinp(2) = 809;
uinp(3) = 201;
uinp(4) = 201;
uinp(5) = 802;
uinp(6) = 802;
uinp(7) = 201;
uinp(8) = 201;
uinp(9) = 802;
uinp(10) = 802;
uinp(11) = 201;
uinp(12) = 201;
uinp(13) = 802;
uinp(14) = 802;


for fff=3:14
  s1 = read(fl1(fff),-1,1);
  s2 = Mov_avg(s1);
  h=scf(1);
  clf;
  plot2d(s2);
  [gg, Fest, Vest]=Estim2(s2);
  Err1 = Vest/255*1000;
  Err1 = (Err1-uinp(fff))/uinp(fff)*100;
  disp(fl1(fff), Err1);
  h=scf(2);
  clf;
  plot2d(gg);
  pause;
end






//sw1 = s1(1:1500);
//gg = MyHist2(sw1);
//plot2d(s1);

//s1 = ADC2();
