function [visitedNodes, f, cameFrom] = aStar(workSpace, startIndx, goalIndx, nNodes, h, g)

dim = sqrt(nNodes);
node = startIndx;

cameFrom(nNodes, 1) = 0;
cameFrom(node) = node;

closedSet(nNodes, 1) = false;
openSet(nNodes, 1) = false;

costSoFar(nNodes, 1) = 0;

f = inf(nNodes, 1);

openSet(node) = true;
costSoFar(node) = 0;
f(node) = 0;

visitedNodes = 0;

%preallocate neighbours -- this only makes sense if we expect to explore
%most of them. If the workspace is not very maze like, dynamic computation
%of the neighbours might be faster
% further this code section is 'optimized' for readability
neighbours = zeros(nNodes,4);
%infinite space movement: up, down, right, left
neighbours(:,1) = (1:nNodes)+dim; % go up
neighbours(:,2) = (1:nNodes)-dim; % go down
neighbours(:,3) = (1:nNodes)+1; % go right
neighbours(:,4) = (1:nNodes)-1; % go left

%add a border around the work space
neighbours(1:dim,1) = 1:dim;                            %nodes on the top cant co up
neighbours(nNodes-dim:nNodes,2) = nNodes-dim:nNodes;    %nodes on the bottom can't go down
neighbours(1:dim:nNodes,3) = 1:dim:nNodes;              %nodes on the right side cant go right
neighbours(dim:dim:nNodes,4) = dim:dim:nNodes;          %nodes on the left can't go left

%add the obstacles
for idx = 1:numel(workSpace)
    if workSpace(idx)
        edges = neighbours(idx,:); % we can only reach the cell from these 4 elements
        neighbours(edges(1),2) = edges(1); % above node can't go down
        neighbours(edges(2),1) = edges(2); % below node can't go up
        neighbours(edges(3),4) = edges(3); % right node can't go left
        neighbours(edges(4),3) = edges(4); % left node can't go right
    end
end

% main A* loop
while any(openSet)
    [~, minFIndx] = min(f);
    f(minFIndx) = inf;
    currentNode = minFIndx;

    if currentNode == goalIndx
        disp('goal Found')
        return
    end

    openSet(currentNode) = false;
    closedSet(currentNode) = true;

    for node = neighbours(currentNode,:)

        if closedSet(node)
            % depending on cost of g(x) it might be wise to not continue
            % jumping around in the code is painfully slow, maybe even
            % slower than evaluating g(x).
            % below if will skip this node regardless

            % further if we remove this continue, there is no need for the
            % variable closedSet anymore.
            continue
        end

        % if node == currentNode, tentativeGScore > costSoFar(node)
        tentativeGScore = costSoFar(currentNode) + g(currentNode);

        if ~openSet(node) || tentativeGScore < costSoFar(node)
            cameFrom(node) = currentNode;
            costSoFar(node) = tentativeGScore;
            f(node) = costSoFar(node) + h(node);
            if ~openSet(node)
                openSet(node) = true;
            end
        end
    end
end
end