Алгебраические операции в конечном поле

#define uint8_t u8
#define FIELD_POLY 0x07
#define HIGH_BIT_ON_FIELD 0x02
#include <cstdint>
#include <cstdio>
#include <iostream>
#include <memory>
#include <random>
 
using namespace std;
u8 gf_add(u8 a, u8 b)
{
    return a ^ b;
}
u8 multiply_gf(u8 a, u8 b)
{
    u8 acc = 0x00;
    u8 msb = 0;
    for(size_t i = 0; i < 8; i++)
    {
        if(b & 0x01)
        {
            acc^=a;
        }
        msb = a & HIGH_BIT_ON_FIELD;
    a <<= 1;
    if(msb)
    {
        a = gf_add(a, FIELD_POLY);
    }
    b>>=1;
    }
    return acc;
}
void printf_m(u8 *a, size_t row, size_t cols)
{
    for(int i = 0; i < row*cols; i++)
        if(i%cols==0)cout <<endl << (int) a[i] << " ";
        else cout << (int) a[i] << " ";
    cout << endl;
}
u8 gf_pow(u8 a, u8 b)
{
    if(0==b)
    {
        return 1;
    } else if (1 == b)
    {
        return a;
    }
    u8 z = a;
    for(size_t i = 0; i < b - 1; ++i)
        z = multiply_gf(z,a);
    return z;
}
void gf_divide(u8 a, u8 b, u8 *q, u8 *r, u8 field_poly)
{
    u8 i = 0;
    u8 quotient = 0x00;
    u8 rem = b;
    u8 msb=0x00;
    msb = a & HIGH_BIT_ON_FIELD;
    while(0 == msb && i < 8)
    {
        a <<=1;
        msb = a & HIGH_BIT_ON_FIELD;
        i++;
    }
    if(field_poly)
    {
        quotient |= (1<<(i+1));
        rem^=(a << 1);
    }
    for(size_t j = 0; j < i+1; ++j)
    {
        if((rem<<j) & HIGH_BIT_ON_FIELD)
        {
            quotient  |= (1<<(i - j));
            rem^=a;
        }
        a >>= 1;
    }
    *q = quotient;
    *r = rem;
}
u8 gf_inverse(u8 a, u8 b)
{
    if(a == 0x00)
    {
        return 0;
    }
    u8 rem[8];
    u8 aux[8];
    u8 q, r;
    size_t i = 1;
    rem[0] = b;
    rem[1] = a;
    aux[0] = 0;
    aux[1] = 1;
    while(rem[i] > 0x01)
    {
        i++;
        if(i == 2)
        {
            gf_divide(rem[i-1], rem[i-2], &q, &r, 1);
        } else {
            gf_divide(rem[i-1], rem[i-2], &q, &r, 0);
        }
        rem[i] = r;
        aux[i] = multiply_gf(q, aux[i-1])^aux[i-2];
    }
    return aux[i];
}
    
int main()
{
    u8 row = 4, col = 4;
    u8 matrix[16] = {
                        0,0,2,1,
                        0,0,1,1,
                        3,0,1,2,
                        2,0,0,1
                    };      
    printf_m(matrix, row, col);
    u8 rs = 0;
    u8 swap = 0;
    u8 inverse = 0;
    for(size_t k=0,i,j,im; k < row-1; k++)
    {
        im = k;
        for(i = k + 1; i < row; i++)
        {
            if(matrix[im*col+k] < matrix[i*col+k])
            {
                    im = i;
            }
        }
        if(im != k)
        {
            for(j = 0; j < col; j++)
            {
                swap = matrix[im*col+j];
                matrix[im*col+j] = matrix[k*col+j];
                matrix[k*col+j]=swap;
            }
        }
        printf_m(matrix, row, col);
        if(matrix[k*col+k] != 0 && matrix[k*col+k] !=1)
        {
            inverse = gf_inverse(matrix[k*col+k], FIELD_POLY);
            for(j = k; j < col; j++)
            {
                matrix[k*col+j]=multiply_gf(inverse, matrix[k*col+j]);
            }
        }
        printf_m(matrix, row, col);
        for(i = k + 1; i < row; i++)
        {
            if(matrix[i*col+k] != 0)
            {
                inverse = gf_inverse(matrix[i*col+k], FIELD_POLY);
                for(j = 0; j < col; j++)
                {
                    matrix[i*col+j] = multiply_gf(inverse, matrix[i*col+j]);
                    matrix[i*col+j]  = gf_add(matrix[i*col+j],matrix[k*col+j]);
                }
            printf_m(matrix, row, col);
            }
            
        }
    }
    printf_m(matrix, row, col);
    return 0;
}