C = [1, 1, 1, 1, 1
1, 5, 5, 6, 3
1, 4, 5, 7, 2
1, 7, 4, 2, 3
1, 9, 2, 9, 5];

clc;
ncols = size(C,1);
nrows = size(C,2);

if ncols ~= nrows
    fprintf("Матрица должна быть квадратной\n");
    return;
end

find_max = false;
cost = C;
stars = [];
dots = [];
plus_rows = [];
plus_cols = [];

iter_num = 1;


if find_max
    max_cost = max(max(cost));
    temp = max_cost - cost;
    cost = temp;

    fprintf(' (Max - Cij) \n');
    fprintf('  Матрица стоимостей после изменения задачи на задачу максимизации:\n');
    print_matrix(cost);
    fprintf('________________________\n\n');
else
    fprintf('Заданная матрица стоимостей :\n');
    print_matrix(cost);
    fprintf('________________________\n\n');
end


fprintf('Минимизированная матрица: \n')
cost = minimize_matrix(cost);
print_matrix(cost)

for i = 1:ncols
    for j = 1:nrows
        if cost(i,j) == 0
            if size(stars) ~= 0
                if  not(ismember(j,stars(:,2)))
                    stars = [stars; [i,j]];
                    break;
                end
            else
                stars = [stars; [i,j]];
                break;
            end
        end
    end
end

while size(stars, 1) ~= ncols

    plus_cols = stars(:,2);
    if size(plus_rows,1) == 0
        plus_rows = [];
    else
        plus_rows = dots(:,1);
    end


    fprintf(' Итерация %d.\n', iter_num);
    fprintf(' Матрица стоимостей с выделенными элементами:\n');
    fprintf(' k = %d\n', size(stars,1));
    print_pluses(cost, plus_cols);
    print_stars(cost,stars,dots,plus_rows);
    fprintf('________________________\n\n');



    dots = [];

    [flag, i, j] = zero_in_nonpluses(cost, plus_cols, plus_rows);

    while 1 == 1
        if flag
            dots = [dots; [i,j]];
            [retval,colnum] = two_zeros_one_line(stars, i, j);
            if retval
                plus_rows = [plus_rows;i];
                plus_cols(plus_cols == colnum) = [];

                fprintf(' Итерация %d.\n', iter_num);
                fprintf(" Матрица стоимостей после добавления 0':\n");
                print_pluses(cost, plus_cols);
                print_stars(cost,stars, dots, plus_rows);
                fprintf('________________________\n\n');
            else
                % l_chain

                fprintf(' Итерация %d.\n', iter_num);
                fprintf(' Матрица стоимостей до построения L-цепочки:\n');
                print_pluses(cost, plus_cols);
                print_stars(cost,stars, dots, plus_rows);
                fprintf('________________________\n\n');


                chain = L_chain(stars, dots);
                nchain = size(chain,1);

                for i = 1:nchain
                    [la, lb] = ismember(chain(i,:),stars, 'rows');
                    if la
                        stars(lb,:) = [];
                    else
                        stars = [stars;chain(i,:)];
                    end
                end
                [flag, i, j] = zero_in_nonpluses(cost, plus_cols, plus_rows);
                dots = [];
                plus_cols = [];
                plus_rows = [];

                fprintf(' Итерация %d.\n', iter_num);
                fprintf(' L-цепочка:\n');
                print_l_chain(chain);
                fprintf('\n Матрица стоимостей после построения L-цепочки:\n');
                print_pluses(cost, plus_cols);
                print_stars(cost,stars, dots, plus_rows);
                fprintf('________________________\n\n');
                break;
            end

            [flag, i, j] = zero_in_nonpluses(cost, plus_cols, plus_rows);
        else
            cost = manipulate(cost, plus_cols, plus_rows);

            fprintf(' Итерация %d.\n', iter_num);
            fprintf('\n После преобразований:\n');
            print_pluses(cost, plus_cols);
            print_stars(cost,stars, dots, plus_rows);
            fprintf('________________________\n\n');

            [flag, i, j] = zero_in_nonpluses(cost, plus_cols, plus_rows);
        end
    end
    iter_num = iter_num + 1;
end

fprintf(' Матрица стоимостей с выделеннымми элементами:\n');
fprintf(' k = %d\n', size(stars,1));
print_pluses(cost, plus_cols);
print_stars(cost,stars,dots,plus_rows);
fprintf('________________________\n\n');

print_opt(cost, C, stars);


function [new_mat] = minimize_matrix(mat)
    new_mat = minimize_columns(minimize_columns(mat).').';
end

function [new_mat] = minimize_columns(mat)
    new_mat = mat;
    [rows_count, columns_count] = size(new_mat);
    min_column_elements = min(new_mat);

    for column_index = 1:columns_count
        el = min_column_elements(column_index);
        for row_index = 1:rows_count
            new_mat(row_index, column_index) = new_mat(row_index, column_index) - el;
        end
    end
end

function print_matrix(mat)
      for i = 1:size(mat,1)
        for j = 1:size(mat,2)
            fprintf('%d  ',mat(i,j));
        end
        fprintf('\n');
      end

end

function print_stars(cost, stars, dots, plus_rows)
    for i = 1:size(cost,1)
        for j = 1:size(cost,2)
            if size(dots,2) > 0
                if ismember([i,j],dots, 'rows')
                    fprintf("%d' ", cost(i,j))
                    continue
                end
            end
            if ismember([i,j],stars, 'rows')
                fprintf('%d* ', cost(i,j))
            else
                fprintf('%d  ',cost(i,j));
            end
        end
        if ismember(i,plus_rows)
            fprintf(' +\n');
        else
            fprintf('\n');
        end
    end
end

function print_opt(cost, C, stars)

    fprintf('\n X_opt = \n');
    y = 0;

    for i = 1:size(cost,1)
        for j = 1:size(cost,2)
            if cost(i,j) == 0
                if ismember([i,j],stars, 'rows')
                    y = y + C(i,j);
                    fprintf("%d  ", 1)
                else
                    fprintf("%d  ", 0)
                end
            else
                fprintf("%d  ", 0)
            end
        end
        fprintf('\n');

    end
    fprintf("\n f_opt = %d\n", y)
end

function print_pluses(cost, plus_cols)
    for j = 1:size(cost,2)
        if ismember(j, plus_cols)
            fprintf("+  ");
        else
            fprintf("   ");
        end
    end
    fprintf("\n");
end

function chain = L_chain(stars, dots)
    chain = [];
    ndots = size(dots,1);
    nstars = size(stars,1);

    cur_point = dots(size(dots,1),:);
    chain = [chain;cur_point];
    flag = 0; % 0 - search nearest by col, 1 - search nearest by row
    end_flag = 1;
    while end_flag
        if flag == 0
            for i = 1:nstars
                point = stars(i,:);
                if cur_point(2) == point(2)
                    chain = [chain;point];
                    cur_point = point;
                    flag = 1;
                end
            end
            if flag == 0
                end_flag = 0;
            end
        else
            for i = 1:ndots
                point = dots(i,:);
                if cur_point(1) == point(1)
                    chain = [chain;point];
                    cur_point = point;
                    flag = 0;
                end
            end
        end
    end

    return;
end

function [flag,row,col] = zero_in_nonpluses(cost, plus_cols, plus_rows)
    flag = 0;
    for i = 1:size(cost,1)
        for j = 1:size(cost,2)
            if cost(i,j) == 0 && ~ismember(j, plus_cols) && ~ismember(i, plus_rows)
                flag = 1;
                row = i;
                col = j;
                return;
            end
        end
    end
    row = -1;
    col = -1;
    return;
end

function cost_matrix = manipulate(cost, plus_cols, plus_rows)
    %find min in unemphasized...
    temp = cost;
    min_elem = min_nonplus(cost, plus_cols, plus_rows);
    fprintf('Матрица стоимостей после вычитания минимального остаточного элемента из каждого не вычеркнутого столбца\n');

    for i = 1:size(cost,1)
        for j = 1:size(cost,2)
            if ~ismember(j,plus_cols)
                temp(i,j) = temp(i,j) - min_elem;
            end
            fprintf('%3d ', temp(i,j));
        end
        fprintf('\n');
    end

    fprintf('\n');
    fprintf('Матрица стоимостей после добавления минимального элемента к каждой вычеркнутой строке\n');

    for i = 1:size(cost,1)
        for j = 1:size(cost,2)
            if ismember(i,plus_rows)
                temp(i,j) = temp(i,j) + min_elem;
            end
            fprintf('%3d ', temp(i,j));
        end
        fprintf('\n');
    end
    fprintf('\n');

    cost_matrix = temp;
end

function min_value = min_nonplus(cost, plus_cols, plus_rows)
    min_elem = max(max(cost));
    for i = 1:size(cost,1)
        for j = 1:size(cost,2)
            if cost(i,j) < min_elem && ~ismember(i,plus_rows) && ~ismember(j, plus_cols)
                min_elem = cost(i,j);
            end
        end
    end
    min_value = min_elem;
    return;
end

function [retval,colnum] = two_zeros_one_line(stars, i, j)
    retval = 0;
    rownum = -1;
    colnum = -1;

    for k = 1:size(stars,1)
        if stars(k) == i
            star = stars(k,:);
            retval = 1;
            rownum = star(1);
            colnum = star(2);
            return;
        end
    end

end

function print_l_chain(chain)
    fprintf(" (%d,%d)", chain(1,1), chain(1,2));
    for i = 2:size(chain,1)
        fprintf("->(%d,%d)", chain(i,1), chain(i,2));
    end
end