%#codegen
function [valid, ed, xfo, yfo, cm] = fxpc_corner_detection(data_in)

%   Copyright 2011-2015 The MathWorks, Inc.

[~, ed, xfo, yfo] = fxpc_sobel(data_in);

cm = compute_corner_metric(xfo, yfo);

% compute valid signal
persistent cnt
if isempty(cnt)
  cnt = 0;
end
cnt = cnt + 1;
valid = cnt > 3*80+3 && cnt <= 80*80+3*80+3;


end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bm = compute_corner_metric(gh, gv)

cmh = make_buffer_matrix_gh(gh);
cmv = make_buffer_matrix_gv(gv);
bm = compute_harris_metric(cmh, cmv);

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bm = make_buffer_matrix_gh(gh)

[b1p, b2p, b3p, b4p] = chained_delays_80_1(gh);

cc = [b4p b3p b2p b1p gh];

[h1p, h2p, h3p, h4p] = chained_delays_80_2(cc);

bm = [h4p h3p h2p h1p cc];

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function bm = make_buffer_matrix_gv(gv)

[b1p, b2p, b3p, b4p] = chained_delays_80_3(gv);

cc = [b4p b3p b2p b1p gv];

[h1p, h2p, h3p, h4p] = chained_delays_80_4(cc);

bm = [h4p h3p h2p h1p cc];

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function cm = compute_harris_metric(gh, gv)

[g1, g2, g3] = gaussian_filter(gh, gv);
[s1, s2, s3] = reduce_matrix(g1, g2, g3);

cm = (((s1*s3) - (s2*s2)) - (((s1+s3) * (s1+s3)) * 0.04));

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [g1, g2, g3] = gaussian_filter(gh, gv)

%g=fspecial('gaussian',[5 5],1.5);
g = [0.0144    0.0281    0.0351    0.0281    0.0144
     0.0281    0.0547    0.0683    0.0547    0.0281
     0.0351    0.0683    0.0853    0.0683    0.0351
     0.0281    0.0547    0.0683    0.0547    0.0281
     0.0144    0.0281    0.0351    0.0281    0.0144];

g1 = (gh .* gh) .* g(:)';
g2 = (gh .* gv) .* g(:)';
g3 = (gv .* gv) .* g(:)';

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [s1, s2, s3] = reduce_matrix(g1, g2, g3)

s1 = sum(g1);
s2 = sum(g2);
s3 = sum(g3);

end

function [v1p, v2p, v3p, v4p] = chained_delays_80_1(v)
    persistent b1 b2 b3 b4 idx
    if isempty(b1)
        b1 = zeros(80, numel(v));
        b2 = zeros(80, numel(v));
        b3 = zeros(80, numel(v));
        b4 = zeros(80, numel(v));
        idx = 1;
    end

    v1p = b1(idx, :);
    b1(idx, :) = v;

    v2p = b2(idx, :);
    b2(idx, :) = v1p;

    v3p = b3(idx, :);
    b3(idx, :) = v2p;

    v4p = b4(idx, :);
    b4(idx, :) = v3p;

    if idx < 80
        idx = idx + 1;
    else
        idx = 1;
    end
end

function [v1p, v2p, v3p, v4p] = chained_delays_80_2(v)
    persistent b1 b2 b3 b4 idx
    if isempty(b1)
        b1 = zeros(80, numel(v));
        b2 = zeros(80, numel(v));
        b3 = zeros(80, numel(v));
        b4 = zeros(80, numel(v));
        idx = 1;
    end

    v1p = b1(idx, :);
    b1(idx, :) = v;

    v2p = b2(idx, :);
    b2(idx, :) = v1p;

    v3p = b3(idx, :);
    b3(idx, :) = v2p;

    v4p = b4(idx, :);
    b4(idx, :) = v3p;

    if idx < 80
        idx = idx + 1;
    else
        idx = 1;
    end
end

function [v1p, v2p, v3p, v4p] = chained_delays_80_3(v)
    persistent b1 b2 b3 b4 idx
    if isempty(b1)
        b1 = zeros(80, numel(v));
        b2 = zeros(80, numel(v));
        b3 = zeros(80, numel(v));
        b4 = zeros(80, numel(v));
        idx = 1;
    end

    v1p = b1(idx, :);
    b1(idx, :) = v;

    v2p = b2(idx, :);
    b2(idx, :) = v1p;

    v3p = b3(idx, :);
    b3(idx, :) = v2p;

    v4p = b4(idx, :);
    b4(idx, :) = v3p;

    if idx < 80
        idx = idx + 1;
    else
        idx = 1;
    end
end

function [v1p, v2p, v3p, v4p] = chained_delays_80_4(v)
    persistent b1 b2 b3 b4 idx
    if isempty(b1)
        b1 = zeros(80, numel(v));
        b2 = zeros(80, numel(v));
        b3 = zeros(80, numel(v));
        b4 = zeros(80, numel(v));
        idx = 1;
    end

    v1p = b1(idx, :);
    b1(idx, :) = v;

    v2p = b2(idx, :);
    b2(idx, :) = v1p;

    v3p = b3(idx, :);
    b3(idx, :) = v2p;

    v4p = b4(idx, :);
    b4(idx, :) = v3p;

    if idx < 80
        idx = idx + 1;
    else
        idx = 1;
    end
end
