//
// Created by zemon on 24.07.2022.
//

#include "GonetsReedSalmon.h"
#include "PolyGalois256.h"
#include <memory.h>

int32_t vGonetsReedSalmonAddDegree(tGonetsReedSalmon *env, int32_t fieldNumber, int32_t degreeAddon) {
    int32_t numDegree = iGalois256LogValue(env->gf, fieldNumber);
    return iGalois256PowValue(env->gf, numDegree + degreeAddon);
}


void vGonetsReedSalmonGenGenerativePoly(tGonetsReedSalmon *env) {

    memset(env->generative_polynomial, 0, env->generative_polynomial_len * sizeof(int8_t));


    env->generative_polynomial[0] = 2;
    env->generative_polynomial[1] = 1;


    for (uint8_t i = 2; i < env->generative_polynomial_len; i++) {
        env->generative_polynomial[i] = 1;

        for (uint8_t j = i - 1; j > 0; j--) {
            if (env->generative_polynomial[j] == 0) {
                env->generative_polynomial[j] = env->generative_polynomial[j - 1];
            } else {
                env->generative_polynomial[j] = iGalois256Add(
                        env->gf,
                        env->generative_polynomial[j - 1],
                        vGonetsReedSalmonAddDegree(env, env->generative_polynomial[j], i)
                );
            }
        }

        env->generative_polynomial[0] = vGonetsReedSalmonAddDegree(env, env->generative_polynomial[0], i);
    }

}

void vGonetsReedSalmonInit(tGonetsReedSalmon *env, tGalois256 *galoisField, uint8_t parity_length) {
    env->gf = galoisField;
    env->parity_length = parity_length;
    env->generative_polynomial_len = env->parity_length + 1;
    env->max_fixable_err = env->parity_length / 2;

    vGonetsReedSalmonGenGenerativePoly(env);
}


static void vGonetsReedSalmonDividePolynomialsR(
        tGalois256 *gf,

        uint8_t dividend_len,

        const uint8_t *divisor,
        uint8_t divisor_len,

        uint8_t *remainder
) {


    --dividend_len;
    --divisor_len;


    uint8_t top_value_of_divisor = divisor[divisor_len];

    int16_t remainder_len = dividend_len;


    size_t err_break = dividend_len;
    do {

        for (; remainder_len >= 0; --remainder_len) {
            if (remainder[-remainder_len] > 0) {
                break;
            }
        }

        if (remainder_len < divisor_len) {
            return;
        }

        uint8_t product_value = iGalois256Div(gf, remainder[-remainder_len], top_value_of_divisor);
        uint8_t product_idx = remainder_len - divisor_len;


        for (int16_t sub_idx = divisor_len; sub_idx >= 0; --sub_idx) {

            uint8_t element = iGalois256Mul(gf, divisor[sub_idx], product_value);
            remainder[-(product_idx + sub_idx)] = iGalois256Sub(gf, remainder[-(product_idx + sub_idx)], element);

        }

        --err_break;
    } while (err_break);
}

void vGonetsReedSalmonEncode(
        tGonetsReedSalmon *env,

        const uint8_t *message,
        uint8_t message_len,

        uint8_t *encoded_message
) {

    memcpy(encoded_message, message, message_len);
    memset(encoded_message + message_len, 0, env->parity_length);

    vGonetsReedSalmonDividePolynomialsR(
            env->gf,
            message_len + env->parity_length,
            env->generative_polynomial,
            env->generative_polynomial_len,
            encoded_message + message_len + env->parity_length - 1
    );

    memcpy(encoded_message, message, message_len);
}

void vGonetsReedSalmonGetLocatorFromErrPos(
        tGonetsReedSalmon *env,
        tPolyGalois256 *erasures,
        tPolyGalois256 *locator_result
) {

    uint8_t prev_buff[1024] = {0};
    uint8_t erasure_sub_buf[2] = {0};
    tPolyGalois256 prev = {.data = prev_buff, .length = 0,};
    tPolyGalois256 erasure_sub = {.data = erasure_sub_buf, .length = 2,};
    erasure_sub.data[0] = 1;


    locator_result->data[0] = 1;
    locator_result->length = 1;

    for (size_t erasures_idx = 0; erasures_idx < erasures->length; ++erasures_idx) {
        uint8_t erasure_pow_value = iGalois256PowValue(env->gf, erasures->data[erasures_idx]);
        erasure_sub.data[1] = erasure_pow_value;

        vPolyGalois256Mul(env->gf, locator_result, &erasure_sub, &prev);
        vPolyGalois256Copy(&prev, locator_result);

    }

}

void vGonetsReedSalmonFindLocatorInSyndomes(
        tGonetsReedSalmon *env,
        tPolyGalois256 *syndromes,
        tPolyGalois256 *locator_out
) {

    uint8_t locator_perv_buf[env->parity_length + 1];
    tPolyGalois256 locator_perv = {
            .data =locator_perv_buf,
            .length = 0,
    };


    uint8_t locator_next_buf[env->parity_length + 1];
    tPolyGalois256 locator_next = {
            .data =locator_next_buf,
            .length = 0,
    };

    uint8_t locator_next2_buf[env->parity_length + 1];
    tPolyGalois256 locator_next2 = {
            .data =locator_next2_buf,
            .length = 0,
    };

    locator_out->data[0] = 1;
    locator_out->length = 1;

    locator_perv.data[0] = 1;
    locator_perv.length = 1;

    uint8_t syndrome_shift = 0;

    for (uint8_t parity_idx = 0; parity_idx < env->parity_length; parity_idx++) {

        uint8_t k = parity_idx + syndrome_shift;
        uint8_t delta = syndromes->data[k];

        for (size_t locator_idx = 1; locator_idx < locator_out->length; ++locator_idx) {
            uint8_t mux = iGalois256Mul(env->gf, locator_out->data[locator_idx],
                                        syndromes->data[parity_idx - locator_idx]);
            delta = iGalois256Add(env->gf, delta, mux);
        }

        vPolyGalois256MulNumberSelf(env->gf, &locator_perv, 1);

        if (delta != 0) {

            if (locator_perv.length > locator_out->length) {
                vPolyGalois256Scale(env->gf, &locator_perv, delta, &locator_next);
                vPolyGalois256Scale(env->gf, locator_out, iGalois256Div(env->gf, 1, delta), &locator_perv);
                vPolyGalois256Copy(&locator_next, locator_out);
            }

            vPolyGalois256Scale(env->gf, &locator_perv, delta, &locator_next2);
            vPolyGalois256Add(env->gf, locator_out, &locator_next2, &locator_next);
            vPolyGalois256Copy(&locator_next, locator_out);

        }
    }
}

void vGonetsReedSalmonGetErrPosFromLocator(
        tGonetsReedSalmon *env,
        tPolyGalois256 *locator,
        tPolyGalois256 *err_pos_out
) {
    err_pos_out->length = 0;

    for (uint16_t arg = 0; arg < 256; ++arg) {
        uint8_t value_in_arg = iPolyGalois256Value(env->gf, locator, arg);
        if (value_in_arg == 0) {
            err_pos_out->data[err_pos_out->length] = iGalois256LogValue(env->gf, iGalois256Div(env->gf, 1, arg));
            ++err_pos_out->length;
        }
    }
}

void vGonetsReedSalmonFindMagnitudes(
        tGonetsReedSalmon *env,
        tPolyGalois256 *syndromes,
        tPolyGalois256 *locator,
        tPolyGalois256 *err_pos,
        tPolyGalois256 *magnitudes_path_out
) {

    uint8_t product_buf[env->parity_length + env->parity_length + 2];
    tPolyGalois256 product = {
            .data = product_buf,
            .length = 0,
    };

    uint8_t err_buf[env->parity_length + 1];
    tPolyGalois256 err = {
            .data = err_buf,
            .length = 0,
    };

    uint8_t loc_derivative_buf[env->parity_length + 1];
    tPolyGalois256 loc_derivative = {
            .data = loc_derivative_buf,
            .length = 0,
    };


    vPolyGalois256Mul(env->gf, syndromes, locator, &product);


    memcpy(err.data, product.data, env->parity_length);
    err.length = env->parity_length;

    vPolyGalois256FormalDerivative(locator, &loc_derivative);

    magnitudes_path_out->length = err_pos->length;
    memset(magnitudes_path_out->data, 0, magnitudes_path_out->length);


    for (size_t i = 0; i < err_pos->length; ++i) {

        uint8_t err_pos_pow = iGalois256PowValue(env->gf, err_pos->data[i]);

        uint8_t rev = iGalois256Div(env->gf, 1, err_pos_pow);

        uint8_t err_val = iPolyGalois256Value(env->gf, &err, rev);
        uint8_t loc_val = iPolyGalois256Value(env->gf, &loc_derivative, rev);

        uint8_t val = iGalois256Div(env->gf, err_val, loc_val);
        magnitudes_path_out->data[i] = val;
    }

//    printf("product \n");
//    print_arr(product.data, product.length);
//
//    printf("err \n");
//    print_arr(err.data, err.length);
//
//    printf("loc_derivative \n");
//    print_arr(loc_derivative.data, loc_derivative.length);

}

void vGonetsReedSalmonGetSyndromes(
        tGonetsReedSalmon *env,
        tPolyGalois256 *message,
        tPolyGalois256 *syndromes_out
) {
    syndromes_out->length = env->parity_length;

    for (uint8_t i = 0; i < env->parity_length; ++i) {
        uint8_t pow = iGalois256PowValue(env->gf, i + 1);
        syndromes_out->data[i] = iPolyGalois256Value(env->gf, message, pow);
    }

}

void vGonetsReedSalmonRecovery(
        tGonetsReedSalmon *env,

        uint8_t *encoded_message,
        uint8_t encoded_message_len,

        uint8_t *recovered_message
) {

    tPolyGalois256 message = {
            .data=encoded_message,
            .length = encoded_message_len,
    };

    tPolyGalois256 recovered = {
            .data=recovered_message,
            .length = 0,
    };

    uint8_t syndromes_buf[env->parity_length];
    tPolyGalois256 syndromes = {
            .data=syndromes_buf,
            .length = 0,
    };

    uint8_t locator_buf[env->parity_length + 1];
    tPolyGalois256 locator = {
            .data=locator_buf,
            .length = 0,
    };


    uint8_t err_pos_buf[env->max_fixable_err + 1];
    tPolyGalois256 err_pos = {
            .data=err_pos_buf,
            .length = 0,
    };

    uint8_t magnitudes_buf[env->max_fixable_err + 1];
    tPolyGalois256 magnitudes_path = {
            .data=magnitudes_buf,
            .length = 0,
    };


    vPolyGalois256CopyRevers(&message, &recovered);

    vGonetsReedSalmonGetSyndromes(env, &recovered, &syndromes);

    vGonetsReedSalmonFindLocatorInSyndomes(env, &syndromes, &locator);
    vGonetsReedSalmonGetErrPosFromLocator(env, &locator, &err_pos);


    vGonetsReedSalmonFindMagnitudes(env, &syndromes, &locator, &err_pos, &magnitudes_path);


    for (size_t i = 0; i < err_pos.length; ++i) {
        recovered.data[err_pos.data[i]] = iGalois256Add(
                env->gf,
                recovered.data[err_pos.data[i]],
                magnitudes_path.data[i]
        );
    }


    vPolyGalois256Revers(&recovered);

//    printf("data in\n");
//    print_arr(message.data, message.length);
//
//    printf("syndromes\n");
//    print_arr(syndromes.data, syndromes.length);
//
//    printf("locator\n");
//    print_arr(locator.data, locator.length);
//
//    printf("err_pos_rec\n");
//    print_arr(err_pos.data, err_pos.length);
//
//    printf("magnitudes \n");
//    print_arr(magnitudes.data, magnitudes.length);
//
//    printf("recovered in\n");
//    print_arr(recovered.data, recovered.length);

}

void vGonetsReedSalmonEncode_1410_3430(tGonetsReedSalmon *salmonParity4, uint8_t *data1410) {
    uint8_t in[30];
    uint8_t out[34];

    memset(in, 0, sizeof(in));
    memset(out, 0, sizeof(out));
    memcpy(in, data1410, 10);

    vGonetsReedSalmonEncode(salmonParity4, in, sizeof(in), out);

    memcpy(data1410, out, 10);
    memcpy(data1410 + 10, out + 30, 4);
}


void vGonetsReedSalmonRecovery_1410_3430(tGonetsReedSalmon *salmonParity4, uint8_t *data1410) {
    uint8_t in[34];
    uint8_t out[34];

    memset(in, 0, sizeof(in));
    memset(out, 0, sizeof(out));

    memcpy(in, data1410, 10);
    memcpy(in + 30, data1410 + 10, 4);

    vGonetsReedSalmonRecovery(salmonParity4, in, sizeof(in), out);

    memcpy(data1410, out, 10);
    memcpy(data1410 + 10, out + 30, 4);
}