%% ************************************************************************************************
%  File: weight_center.m
%
%  Brief: subor obsahuje algoritmus vahovania stredu pre detekciu stredu 
%         interferencnych pruzkov
% 
%  Version: 1.0  Revision: 0.0   Name: peter novotny
% 
%  Date: 2011-01-15
%
%  Matlab version: R2009a
% *************************************************************************************************

% clear all; close all; clc; 

%% *************************************************************************************************
%  Normalizacia - V tomto kroku sa vytvory z vektoru intenzity I novy vektor I_norm ktoreho celkova 
%  priemerna hodnota je rovna nule.   
%  I_norm - normalizovany vektor intenzity
% **************************************************************************************************

I_norm = I_1D - mean(I_1D); 
% figure,plot(z,I_norm);

%% -------------------------------------------------------------------------------------------------
%  Automaticke zistenie hodnoty S(pocet vzoriek/pruzok) - v praxi by malo byt zname z rychlosti posuvania
%  stolika a rychlosti snimania kamery
I_temp = sgolayfilt(I_norm,1,3);
% figure; plot(I_temp);
% max_S_pol = 20; % polovica maximalneho poctu vzoriek kde sa musi nachadzat hodnota S 
[max_I_value, max_I_position] = max(I_temp);
[min_I_left, min_I_left_position] = min(I_temp(1:max_I_position));
[min_I_right, min_I_right_position] = min(I_temp(max_I_position:N));
S = (min_I_right_position) + (max_I_position - min_I_left_position);
%---------------------------------------------------------------------------------------------------

%% *************************************************************************************************
%  Prisposobene filtrovanie - sluzi na odstranenie sumoveho signalu z oboch stran interferencnych
%  pruzkov(obalky)
% **************************************************************************************************

%---------------------------------------------------------------------------------------------------
%  Krok 1: najdenie pruzkov so zodpovedajucou sirkou S s toleranciou +- t
%  K hladaniu pruzkov so zodpovedajucov sirkov sa najskor rozdeli vektor Intenzit na kladne
%  a zaporne hodnoty funkciou sign. Nasledne sa pocita pocet rovnakych po sebe nasledujucich hodnot 
%  a zistuje sa ci je napociane mnoztvo v rozmedi odpovedajucemu sirke pruzku. Ak nie su tieto
%  hodnoty vznulovane.
%  S - pocet vzoriek na pruzok(sirka pruzku)
%  t - tolerancia sirky pruzkov
%  sign_sort_I - vektor intenzity rozlisenej podla znamieka
%  fringe_width - pocita pocet vzoriek s rovnakym znamienkom(co odpoveda sirke pruzku)
%---------------------------------------------------------------------------------------------------

% nastavenie hodnoty t je experimentalne zistene
t = 4; 

sign_sort_I = sign(I_norm);

k = 1;
while k < N 
    fringe_width = 0;
    
    % nasledujuci cyklus pocita pocet vzoriek s rovnakym znamienkom(meria sirku pruzku)
    
    while sign_sort_I(k) == sign_sort_I(k+1) && k < N-1 % (N-1 je kvoli pouzitiu k+1 v podmienke)
        fringe_width = fringe_width + 1;
        k = k + 1;
    end
    
    % ak sirka pruzku nespada do pozadovanych hranic su hodnoty vynulovane
    
    if (fringe_width > (S/2 + t)) || (fringe_width < (S/2 - t))
        I_norm(k-fringe_width:k) = 0;
    end
    
    k = k + 1;
end

I_norm(end) = 0;  % vynulovanie posledneho vzorku(pretoze ho cyklus while neosetri)
% figure; plot(I_norm);

%% -------------------------------------------------------------------------------------------------
%  Krok 2: vynulovanie pruzkov ktore niesu spojite prilahle  - Nasledujuci cyklus pocita kladne 
%  a zaporne polopruzky ktore su vzajomne prilahle a ak je ich pocet < Lc odstarani ich.
%  Lc - koherencna dlzka v tomto pripade znamena priblizny(min.) pocet interferencych pruzkov
%  max_zero_gap - urcuje maximalnu povolenu medzeru medzi pruzkami sposobenu prechodom nulov
% --------------------------------------------------------------------------------------------------

% nastavenie hodnot "Lc" a "max_zero_gap" je experimentalne zistene
Lc = 4;    
max_zero_gap = 10;

% % Ak pred hladanim spojitych pruzkov pouzijem filter mozem podstatne znizit honotu max_zero_gap 
% % lebo budu odfiltrovane nulove hodnoty zo signalu medzi spojitymi pruzkami
I_filt = smoothts(I_norm, 'b', 3);  %!! nahradit sgolayom

% figure;
% plot(I_norm,'r'); grid; hold
% plot(I_filt,'b','linewidth',2);

sign_sort_I = sign(I_filt); % opat rozdelenie pruzkov na kladne a zaporne polvlny    
% figure, plot(I_filt,'b'), grid, hold
 

counter_zeros = 0;          % pocitadlo nulovych hodnot    
counter_half_fringes = 0;   % pocitadlo polovicnych pruzkov(kladne a zaporne polopruzky)
start_position = 1;         % pociatocna poloha pruzkov kvoli nasledovnemu mazaniu
sign_sort_I(end-1) = 1;     % kvoli fungovaniu nasledujuceho algoritmu(inak by sa nezmazal posledny sumovy pruzok)
sign_sort_I(end) = 0;                       

k = 1;
while k < N
    if sign_sort_I(k) == 0 && k < N
        k = k + 1;
        counter_zeros = counter_zeros + 1;
    elseif(counter_zeros <= max_zero_gap)
        counter_zeros = 0;
        temp = sign_sort_I(k); % temp obsahuje len hodnoty 1 a -1 
        if counter_half_fringes < 1
            start_position = k;
        end
        counter_half_fringes = counter_half_fringes + 1; 
        while sign_sort_I(k) == temp %&& k <= N
            k = k + 1;
        end
        if sign_sort_I(k) == (-temp)    % tento if je preto ze nie vzdy sa ide cez nulu
            counter_half_fringes = counter_half_fringes + 1;
            while sign_sort_I(k) == (-temp) %&& k <= N
                k = k + 1;
            end
        end
    elseif counter_half_fringes < 2*Lc    % 2*Lc pretoze ratam polovice pruzkov 
        I_filt(start_position:k) = 0;
        counter_half_fringes = 0;
        counter_zeros = 0;
        k = k + 1; 
    else
        pocet_pruzkov = counter_half_fringes/2;
        counter_half_fringes = 0;
        counter_zeros = 0;
        I_filt(k:N)= 0; k=N; % najdem pruzky a koncim - len docasne riesenie bude treba osetrit napriklad najdenie viaccerych pruzkov
    end
end
% koniec cyklu - while k < N 
% figure; plot(I_filt,'r')

%% -------------------------------------------------------------------------------------------------
%  Krok 3: najdenie suradnic urcujucich zaciatok a koniec pruzkov !?! a nasledne vlozenie povodnych 
%  hodnot intenzity(kvoli nulovaniu niektorych dat predchadzajucimi algoritmami pri prechodoch nulov) 
%  start_of_fringes - cislo vzorku kde zacinaju pruzky
%  end_of_fringes - cislo vzorku kde koncia pruzky
% --------------------------------------------------------------------------------------------------

start_of_fringes = 1;
end_of_fringes = N;

% detekcia polohy pruzkov
for k = 1:N
    if I_filt(k) ~= 0
        start_of_fringes = k;
        break
    end
end
for k = N:-1:1
    if I_filt(k) ~= 0
        end_of_fringes = k;
        break
    end
end

% % Ak by sa nenasli ziadne pruzky tak sa nic nestane lebo na kamere vsade byt nemusia
% if start_of_fringes == 1 || end_of_fringes == N
%     error('No fringes detected!');
% end

%% -------------------------------------------------------------------------------------------------
% Pred dalsou upravou nahradim najdene filtrovane pruzky povodnymi a vyhladim ich(filter smothts)
% aby som obmedzil vplyv vynulovanych hodnot(vzniklich predchadyajucimi filtrami) v oblasti pruzkov. 
% --------------------------------------------------------------------------------------------------

I_final = zeros(1,N);
I_final(start_of_fringes:end_of_fringes) = smoothts(I_norm(start_of_fringes:end_of_fringes), 'b', 5);
% figure, plot(I_final), grid;

%% *************************************************************************************************
%  Urcenie taziska  - nasledujuci vztah urci polohu taziska interferogramu a pruzok leziaci
%  najblizsie k tomuto bodu je povazovany za nulty pruzok
% **************************************************************************************************

if sum(I_final) ~= 0
    ii = 1:N;
    iixI_final = ii.*abs(I_final);

    c_I_final = sum(iixI_final)/sum(abs(I_final));
    fringe_center = x(round(c_I_final)); 
else
    fringe_center = 0;
end
    
% **************************************************************************************************
%  Zobrazenie 
% **************************************************************************************************
% figure; 
% subplot(3,1,1), plot(I_norm), title('I_norm'), grid
% subplot(3,1,2), plot(I_norm), title('I_norm'), grid
% subplot(3,1,3), plot(sign_sort_I), title('sign_sort_I'), grid

% **************************************************************************************************
%  END OF FILE: weight_center.m
% **************************************************************************************************