% Vypocet Position Error
function [ev_position_error,ev_hdg_error] = Position_HDG_Error (iv_data_e,iv_data_n,iv_data_hs,iv_data_hdg,iv_twy_e1,iv_twy_n1,iv_twy_e2,iv_twy_n2)

ls_twy_vector = [(iv_twy_e1 - iv_twy_e2) (iv_twy_n1 - iv_twy_n2)];      % vektor TWY
lv_twy_length = sqrt(ls_twy_vector(1,1)^2 + ls_twy_vector(1,2)^2);      % velikost vektoru TWY
ls_twy_one_vector = [(ls_twy_vector(1,1) / lv_twy_length) (ls_twy_vector(1,2) / lv_twy_length)];  % jednotkovy vektor TWY

lv_twy_hdg = (ls_twy_vector(1,1) * 0 + ls_twy_vector(1,2) * 1) / (lv_twy_length * 1);
lv_twy_hdg1 = acosd(lv_twy_hdg);     % HDG TWY
lv_twy_hdg2 = lv_twy_hdg1 + 180;
if lv_twy_hdg2 > 360;
    lv_twy_hdg2 = lv_twy_hdg2 - 360;
end;
% Vypocet konstant kolme primky k TWY
lv_c1 = -(ls_twy_one_vector(1,1) * iv_twy_e1 + ls_twy_one_vector(1,2) * iv_twy_n1);
lv_c2 = -(ls_twy_one_vector(1,1) * iv_twy_e2 + ls_twy_one_vector(1,2) * iv_twy_n2);

% Vypocet pricne hranice sledovaneho useku v okoli TWY
lv_border1 = (abs(ls_twy_one_vector(1,1) * iv_data_e + ls_twy_one_vector(1,2) * iv_data_n+lv_c1)) / (sqrt(ls_twy_one_vector(1,1)^2 + ls_twy_one_vector(1,2)^2));
lv_border2 = (abs(ls_twy_one_vector(1,1) * iv_data_e+ls_twy_one_vector(1,2) * iv_data_n+lv_c2)) /(sqrt(ls_twy_one_vector(1,1)^2+ls_twy_one_vector(1,2)^2));

% Vypocet Position Error v pripade splnenych podminek hranice sektoru
if lv_border1<lv_twy_length && lv_border2<lv_twy_length   % Pricna hranice
    ls_norm(1,1) = +(iv_twy_n1 - iv_twy_n2);
    ls_norm(1,2) = -(iv_twy_e1 - iv_twy_e2);
    lv_c3 = -(ls_norm(1,1) * iv_twy_e1 + ls_norm(1,2) * iv_twy_n1);
    
    ev_position_error = (ls_norm(1,1) * iv_data_e + ls_norm(1,2) * iv_data_n + lv_c3) / sqrt(ls_norm(1,1)^2 + ls_norm(1,2)^2);
    
    % Vypocet HDG Error v pripade splnenych podminek hranice sektoru
    if iv_data_hs == 1;       % HDG Status 
        iv_hdg_error1 = abs(lv_twy_hdg1 - iv_data_hdg);   % HDG Error
        iv_hdg_error2 = abs(lv_twy_hdg2 - iv_data_hdg);
        if iv_hdg_error1 < iv_hdg_error2;
            ev_hdg_error = iv_hdg_error1;
        else;
            ev_hdg_error = iv_hdg_error2;
        end;
    else;
        ev_hdg_error = 999;     % Nesplnene podminky, dosadime hodnotu, ktera nemuze nastat
    end;   
    
    if ev_position_error > 50;          % Podelna hranice (100m na kazdou stranu)
        ev_position_error = 99999;      % Priradime 99999 (vime, ze s takovouto odchylkou neni ve sledovanem useku)
        ev_hdg_error = 999;             % Nesplnene podminky, dosadime hodnotu, ktera nemuze nastat
    elseif ev_position_error < -50;
        ev_position_error = 99999;      % Priradime 99999 (vime, ze s takovouto odchylkou neni ve sledovanem useku)
        ev_hdg_error = 999;             % Nesplnene podminky, dosadime hodnotu, ktera nemuze nastat
    end;
else
    ev_position_error = 99999;                            % Priradime 99999 (vime, ze s takovouto odchylkou neni ve sledovanem useku)
    ev_hdg_error = 999;         % Nesplnene podminky, dosadime hodnotu, ktera nemuze nastat
end    