/* 
 * valueOfBcdDigits takes an array of unsigned chars that is in BCD format, and
 * convert the values into and int.
 * 
 * Inputs -> digits:    An unsigned char array
 *           digitSize: Size of digits
 * Output -> Value of the BCD digits, or -1 if the digits are invalid.
 *
 */
int valueOfBcdDigits(unsigned char *digits, int digitsSize)
{
    int value = 0;
    int multiplier = 1;
    for (int i = 0; i < digitsSize; i++)
    {
        // Get the two nibbles out
        unsigned char d = digits[i];
        unsigned char lo = d & 0xf;
        unsigned char hi = (d & 0xf0) >> 4;

        // If the lower nibble is not 0-9, the value is invalid -> return -1
        if (lo > 0x9)
            return -1;
        value += lo * multiplier;
        multiplier *= 10;

        // The higher niblble, however, might just be garbage, we can't check it
        // But in that case, we want to break the loop
        if (hi > 0x9)
            break;
        value += hi * multiplier;
        multiplier *= 10;
    }

    return value;
}