#include <stdio.h>
#include <stdint.h>
#include <string.h>

void uint16_to_bcd(uint16_t input, uint8_t *bcd) {
    uint8_t i, j;
    bcd[0] = 0; bcd[1] = 0; bcd[2] = 0; bcd[3] = 0; bcd[4] = 0;
    for (i = 0; i < 16; i++) {
        // Коррекция.
        for (j = 0; j < 5; j++) {
            bcd[j] = (bcd[j] >= 0x05) ? (bcd[j] + (uint8_t) 0x03) : bcd[j];
        }
        // Сдвиг.
        bcd[4] = (bcd[4] << 1) | (bcd[3] >> 3);
        bcd[3] = (bcd[3] << 1) | (bcd[2] >> 3);
        bcd[2] = (bcd[2] << 1) | (bcd[1] >> 3);
        bcd[1] = (bcd[1] << 1) | (bcd[0] >> 3);
        bcd[0] = (bcd[0] << 1) | (input >> 15);
        input <<= 1;
        for (j = 0; j < 5; j++) {
            bcd[j] &= 0x0F;
        }
    }
}

int test_bcd() {
    uint16_t i;
    uint8_t bcd[5];
    char str1[10];
    char str2[10];

    for (i = 0; i < 0xFFFF; i++) {
        uint16_to_bcd(i, bcd);
        sprintf(str1, "%X%X%X%X%X", bcd[4], bcd[3], bcd[2], bcd[1], bcd[0]);
        sprintf(str2, "%05d", i);
        if (strcmp(str1, str2))
            return -1;
    }
    return 0;
}

int main() {
    uint8_t bcd[3];
    uint16_to_bcd(12345, bcd);
    printf("example 1: %X %X %X %X %X\n", bcd[4], bcd[3], bcd[2], bcd[1], bcd[0]);

    uint16_to_bcd(54679, bcd);
    printf("example 2: %X %X %X %X %X\n", bcd[4], bcd[3], bcd[2], bcd[1], bcd[0]);

    if (test_bcd())
        printf("BCD test: FAILED");
    else
        printf("BCD test: OK");
}