%% Matrix setup

clear all; close all; clc;

igrids = 80;
jgrids = 40;
start = [20,10];
goal = [60,25];

%% Adjecency matrix and Initial Conditions

[adjecency_matrix] = make_adjecency(igrids,jgrids);
[heurestic] = make_heurestic();
[h, g] = cost_heurestic(adjecency_matrix, heurestic, goal);
plot_surf(adjecency_matrix, start, goal);
[g, f, not_visited, visited] = initial_settings(start, g, h);

%% Execute

[flag, not_visited] = execute(g, h, f, not_visited, visited, adjecency_matrix, heurestic, igrids, jgrids, start, goal);

%% Display Path

disp_path(flag, not_visited);

%% utility functions

function [adjecency_matrix] = make_adjecency(igrids,jgrids)
    adjecency_matrix = zeros(igrids,jgrids);
    adjecency_matrix(40,15:25) = inf;
    adjecency_matrix(35:40,15) = inf;
    adjecency_matrix(35:40,25) = inf;
end

function [heurestic] = make_heurestic()
    heurestic = sqrt(2);
end

function [h, g] = cost_heurestic(adjecency_matrix, heurestic, goal)
    for x = 1:size(adjecency_matrix,1)
        for y = 1:size(adjecency_matrix,2)
            if(adjecency_matrix(x,y)~=inf)
                h(x,y) = heurestic*norm(goal-[x,y]);
                g(x,y) = inf;
            end
        end
    end
end

function plot_surf(adjecency_matrix, start, goal)
    surf(adjecency_matrix', 'FaceAlpha', 0.5); view(2); hold all;
    plot(start(1),start(2),'s','MarkerFaceColor','k');
    plot(goal(1),goal(2),'s','MarkerFaceColor','r','MarkerSize', 10);
end

function [g, f, not_visited, visited] = initial_settings(start, g, h)
    g(start(1),start(2)) = 0;
    f(start(1),start(2)) = h(start(1),start(2));
    not_visited = [];
    visited = [start g(start(1),start(2)) f(start(1),start(2)) 0];
end

function [result] = check(x, y, igrids, jgrids, adjecency_matrix, present)
    result1 = x<1||x>igrids||y<1||y>jgrids;
    result2 = isinf(adjecency_matrix(x,y));
    result3 = x==present(1)&&y==present(2);
    result = result1 || result2 || result3;
end

function [missed] = new_check(not_visited, x, y)
    missed = 0;
    for j = 1:size(not_visited,1)
        if(x == not_visited(j,1) && y==not_visited(j,2))
            missed = 1;
            break;
        end
    end
end

function disp_path(flag, not_visited)
    if (flag)
        j = size(not_visited,1);
        motion = [];
        while(j > 0)
            x = not_visited(j,1);
            y = not_visited(j,2);
            j = not_visited(j,5);
            motion = [x,y;motion];
        end
        for j = 1:size(motion,1)
            plot(motion(j,1),motion(j,2),'x','color','k');
            pause(0.01)
        end
    else
        disp('Path with given obstacles, start and goal points does not exist');
    end
end

function [flag, not_visited] = execute(g, h, f, not_visited, visited, adjecency_matrix, heurestic, igrids, jgrids, start, goal)
    while(~isempty(visited))
        [array,I] = min(visited(:,4));
        present = visited(I,:);
        flag = false;
        if(present(1:2)==goal)
            not_visited = [not_visited; present];
            flag = true;
            break;
        end
        visited(I,:) = [];
        not_visited = [not_visited;present];
        for x = present(1)-1:present(1)+1
            for y = present(2)-1:present(2)+1
                [result] = check(x, y, igrids, jgrids, adjecency_matrix, present);
                if (result)
                    continue
                end
                missed = new_check(not_visited, x, y);
                if(missed == 1)
                    continue
                end
                [array] = fill_array(visited, x, y);
                g1 = g(present(1),present(2)) + round(norm([present(1)-x,present(2)-y]),1);
                if(isempty(array))
                    g(x,y) = g1;
                    f1 = g(x,y) + h(x,y);
                    new_visited = [x y g(x,y) f1 size(not_visited,1)];
                    visited = [visited; new_visited];
                    continue
                end
                if (g1 >= g(x,y))
                    continue
                end
                g(x,y) = g1;
                f1 = g1 + h(x,y);
                visited(array,3:5) = [g1 f1 size(not_visited,1)];
            end
        end
    end
end

function [array] = fill_array(visited, x, y)
    array = [];
    if(~isempty(visited))
        for j = 1:size(visited,1)
            if(x == visited(j,1) && y==visited(j,2))
                array = j;
                break;
            end
        end
    end
end