Программа поиска пути в матрице по первому лучшему совпадению

Дана матрица с рандомными цифрами от 1 до 9
G = randi([1 9], 7);
Выбирается стартовая точка и конечная точка и необходимо построить путь и найти сумму пути(сумму значений каждой клетки, по которому прошелся путь) по алгоритму
Алгоритм простой, перебор:берете точку, у нее есть вес. Сравниваете веса соседей между собой.выбираете минимальный. Переходите туда. Суммируете с накопленной суммой весов. Дополнительно храните координаты точек. И при сравнении сосеседей условие - сравниваем только те на которые еще не ходили, т.е. те которых нет в переменной хранящей координаты точек которые прошли. И еще одно условие при сравнении соседей- если среди них конечная точка, если есть сразу туда переходите, если нет то сравниваете веса и координаты.