Сжатие звука в IMA ADPCM

#include "ul_audio.h"

// 4->16 bit IMA ADPCM tables of index changes and quantizer step size lookup
#ifdef __AVR
    S8  PROGMEM adpcmima_0416_index_tab[16] = {-1, -1, -1, -1, 2, 4, 6, 8, -1, -1, -1, -1, 2, 4, 6, 8};
    U16 PROGMEM adpcmima_0416_stepsize_tab[89] = {
             7,     8,     9,    10,    11,    12,    13,    14,    16,    17,
            19,    21,    23,    25,    28,    31,    34,    37,    41,    45,
            50,    55,    60,    66,    73,    80,    88,    97,   107,   118,
           130,   143,   157,   173,   190,   209,   230,   253,   279,   307,
           337,   371,   408,   449,   494,   544,   598,   658,   724,   796,
           876,   963,  1060,  1166,  1282,  1411,  1552,  1707,  1878,  2066,
          2272,  2499,  2749,  3024,  3327,  3660,  4026,  4428,  4871,  5358,
          5894,  6484,  7132,  7845,  8630,  9493, 10442, 11487, 12635, 13899,
         15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767
    };
#else
    S8  adpcmima_0416_index_tab[16] = {-1, -1, -1, -1, 2, 4, 6, 8, -1, -1, -1, -1, 2, 4, 6, 8};
    U16 adpcmima_0416_stepsize_tab[89] = {
             7,     8,     9,    10,    11,    12,    13,    14,    16,    17,
            19,    21,    23,    25,    28,    31,    34,    37,    41,    45,
            50,    55,    60,    66,    73,    80,    88,    97,   107,   118,
           130,   143,   157,   173,   190,   209,   230,   253,   279,   307,
           337,   371,   408,   449,   494,   544,   598,   658,   724,   796,
           876,   963,  1060,  1166,  1282,  1411,  1552,  1707,  1878,  2066,
          2272,  2499,  2749,  3024,  3327,  3660,  4026,  4428,  4871,  5358,
          5894,  6484,  7132,  7845,  8630,  9493, 10442, 11487, 12635, 13899,
         15289, 16818, 18500, 20350, 22385, 24623, 27086, 29794, 32767
    };
#endif

U16 adpcmima_0416_dec(U8 sample_in, U16 sample_prev, U8 *index_prev) {
    U16 diffq, step;
    S8 index;

    sample_prev += 32768; /*convert to unsigned*/
    index = *index_prev;

    /* Find quantizer step size from lookup table using index */
    #ifdef __AVR
        step = pgm_read_word(&adpcmima_0416_stepsize_tab[index]);
    #else
        step = adpcmima_0416_stepsize_tab[index];
    #endif

    /* Inverse quantize the ADPCM code into a difference using the quantizer step size */
    if (sample_in & 4) diffq = step; else diffq = 0; step >>= 1;
    if (sample_in & 2) diffq += step; step >>= 1;
    if (sample_in & 1) diffq += step; step >>= 1;
    diffq += step;

    /* Add the difference to the predicted sample */
    if (sample_in & 8) {
        if (sample_prev > diffq) sample_prev -= diffq;
        else sample_prev = 0;
    } else {
        if ((0xffff - sample_prev) > diffq) sample_prev += diffq;
        else sample_prev = 0xffff;
    }
    sample_prev -= 32768;

    /* Find new quantizer step size by adding the old index and a table lookup using the ADPCM code */
    #ifdef __AVR
        index += pgm_read_byte(&adpcmima_0416_index_tab[sample_in & 0xf]);
    #else
        index += adpcmima_0416_index_tab[sample_in & 0xf];
    #endif

    /* Check for overflow of the new quantizer step size index */
    if (index < 0) index = 0;
        else if (index > 88) index = 88;
    *index_prev = index;
    
    return(sample_prev);
}

U8 adpcmima_0416_enc(U16 sample_in, U16 *sample_prev, S8 *index_prev) {
    S32 diff;        /* Difference between sample and the predicted sample */
    U16 step;        /* Quantizer step size */
    S16 sample_pred; /* Output of ADPCM predictor */
    S32 diffq;       /* Dequantized predicted difference */
    S8  index;       /* Index into step size table */
    U8 sample_out;   /* Result of encoding */

    /*restore previous values of predicted sample and quantizer step size index*/
    sample_pred = *sample_prev; /*convert predicted sample to unsigned format*/
    index = *index_prev;

    #ifdef __AVR
        step = pgm_read_word(&adpcmima_0416_stepsize_tab[index]);
    #else
        step = adpcmima_0416_stepsize_tab[index];
    #endif

    /*compute the difference between the input sample (sample_in) and the the predicted sample (sample_pred)*/
    diff = (S32)sample_in - (S32)sample_pred;
    if (diff >= 0) sample_out = 0; else {sample_out = 8; diff = -diff;}

    /* Quantize the difference into the 4-bit ADPCM code using the the quantizer step size	*/
    /* Inverse quantize the ADPCM code into a predicted difference using the quantizer step size */
    diffq = 0;
    if (diff >= step) {sample_out |= 4; diff -= step; diffq += step;}
    step >>= 1;
    if (diff >= step) {sample_out |= 2; diff -= step; diffq += step;}
    step >>= 1;
    if (diff >= step) {sample_out |= 1; diffq += step;}
    step >>= 1;
    diffq += step;

    /* Fixed predictor computes new predicted sample by adding the old predicted sample to predicted difference */
    if (sample_out & 8) sample_pred -= diffq; else sample_pred += diffq;

    /* Check for overflow of the new predicted sample */
    if (sample_pred > 32767) sample_pred = 32767; else if (sample_pred < -32767) sample_pred = -32767;

    /* Find new quantizer stepsize index by adding the old index to a table lookup using the ADPCM code */
    #ifdef __AVR
        index += pgm_read_byte(&adpcmima_0416_index_tab[sample_out]);
    #else
        index += adpcmima_0416_index_tab[sample_out];
    #endif

    /* Check for overflow of the new quantizer step size index */
    if (index < 0) index = 0; else if (index > 88) index = 88;

    /* Save the predicted sample and quantizer step size index for next iteration */
    *sample_prev = sample_pred;
    *index_prev = index;

    return(sample_out);
}

// uLaw encoder sample bias value
#define ULAW_0816_BIAS 0x84
// uLaw encoder sample clipping value
#define ULAW_0816_CLIP 32635

#ifdef __AVR
    U16 PROGMEM ulaw_0816_expdec[] = {0, 132, 396, 924, 1980, 4092, 8316, 16764};
    U8  PROGMEM ulaw_0816_expenc[] = {
        0,0,1,1,2,2,2,2,3,3,3,3,3,3,3,3,
        4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
        5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
        5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7
	};
#else
    const U16 ulaw_0816_expdec[] = {0, 132, 396, 924, 1980, 4092, 8316, 16764};
    const U8  ulaw_0816_expenc[] = {
        0,0,1,1,2,2,2,2,3,3,3,3,3,3,3,3,
        4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,4,
        5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
        5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,5,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,6,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,
        7,7,7,7,7,7,7,7,7,7,7,7,7,7,7,7
    };
#endif
// uLaw decoder (8->16 bit)
U16 ulaw_0816_dec(U8 sample_in) {
    U8  exponent;
    U16 sample_out;
    sample_in = ~sample_in;
    exponent = (sample_in >> 4) & 7;
#ifdef __AVR
	sample_out = pgm_read_word(&ulaw_0816_expdec[exponent]);
#else
    sample_out = ulaw_0816_expdec[exponent];
#endif
    sample_out += (((U16)((sample_in) & 0xf)) << (exponent + 3));
    if (sample_in & 0x80) sample_out = -((S16)sample_out);
    return sample_out;
}

// uLaw encoder (16->8 bit)
U8 ulaw_0816_enc(U16 sample_in) {
    U8 sign, exponent, mantissa, sample_out;
    if ((S16)sample_in < 0) {
        sign = 0x80;
        sample_in = -(S16)sample_in;
    } else sign = 0;
    if (sample_in > ulaw_0816_clip) sample_in = ulaw_0816_clip;
    sample_in += ulaw_0816_bias;
#ifdef __AVR
	exponent = pgm_read_word(&ulaw_0816_expenc[((U16)sample_in >> 7) & 0xff]);
#else
    exponent = ulaw_0816_expenc[((U16)sample_in >> 7) & 0xff];
#endif
    mantissa = (sample_in >> (exponent + 3)) & 0xf;
    sample_out = (sign | (exponent << 4) | mantissa);
    if (sample_out == 0) sample_out = 2;
    return sample_out;
}