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

#define printf _printf

const char *hexChars = "0123456789ABCDEF";

// Improved routine
int binaryToHex(char *hexString, uint8_t size, uint8_t count, uint16_t *value)
{
    int totalBytes = size * count;
    uint8_t *byteArray = (uint8_t *)value;

    for (int i = 0; i < totalBytes; i++) {
        uint8_t byte = byteArray[i];

        // Determine the number of hex characters to write per byte
        for (int j = 0; j < size; j++) {
            int shift = (size - 1 - j) * 4; // Calculate the shift amount (0 for last byte, 4 for second last, etc.)
            *hexString++ = hexChars[(byte >> shift) & 0x0F];
        }
    }

    return totalBytes * size;
}

void setup()
{
    Serial.begin(9600);

    // Example data for testing
    uint16_t values16[] = {0x3412};

    // Allocate enough space for hex string (2 characters per byte)
    char hexString[9]; // 2 bytes * 2 digits/byte + 1 for null terminator

    // Run the routine
    int length = binaryToHex(hexString, 2, 1, values16);

    // Add null terminator
    hexString[length] = '\0';

    // Print results
    printf("Original 16-bit Hex: %s\n", hexString);

    return 0;
}

void loop()
{
  
}

int _printf(char *format, ...) {
  char buffer[80];
  va_list aptr;
  int ret;

  va_start(aptr, format);
  ret = vsprintf(buffer, format, aptr);
  va_end(aptr);
  Serial.print(String(buffer));
  return (ret);
}