//
// Created by xemon on 02.05.23.
//

#include "GonetsCrcs.h"

uint16_t GonetsCrc16(uint8_t *data, size_t data_len) {
    //   uint16_t xor_in = 0x0000;
    uint16_t xor_out = 0xFFFF;
    uint16_t poly = 0x1021;
    uint16_t reg = 0xFFFF; //xor_in;
    for (size_t octet_idx = 0; octet_idx < data_len; octet_idx++) {
        uint8_t octet = data[octet_idx];
        for (uint8_t bit_idx = 0; bit_idx < 8; bit_idx++) {
            uint16_t topbit = reg & 0x8000;
            if (octet & (0x80 >> bit_idx)) {
                topbit &= 0x8000;
            }
            reg <<= 1;
            reg |= (octet >> (7 - bit_idx)) & 0x1;
            if (topbit) {
                reg ^= poly;
            }
        }
        reg &= 0xFFFF;
    }

    return reg ^ xor_out;
}