G726.cpp

Go to the documentation of this file.

/*
This program is distributed under the terms of the 'MIT license'. The text
of this licence follows...

Copyright (c) 2004 J.D.Medhurst (a.k.a. Tixy)

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "common.h"
#include "G726.h"
#include "G711.h"

#define CHECK_SM(x,bits) ASSERT_DEBUG(((x)>>bits)==0)

#define CHECK_UM(x,bits) ASSERT_DEBUG(((x)>>bits)==0)

#define CHECK_TC(x,bits) ASSERT_DEBUG(((x)>>(bits-1))==((x)<0?-1:0))

#define CHECK_FL(x,bits) ASSERT_DEBUG(((x)>>bits)==0)

#define CHECK_UNSIGNED(x,bits) ASSERT_DEBUG(((x)>>bits)==0)
 // End of group


static void EXPAND(unsigned S,unsigned LAW,int& SL)
    {
    CHECK_UNSIGNED(S,8);
    CHECK_UNSIGNED(LAW,1);
    int linear;
    if(LAW)
        linear = G711::ALawDecode(S);
    else
        linear = G711::ULawDecode(S);
    SL = linear>>2;
    CHECK_TC(SL,14);
    }


inline static void SUBTA(int SL,int SE,int& D)
    {
    CHECK_TC(SL,14);
    CHECK_TC(SE,15);
    D = SL-SE;
    CHECK_TC(D,16);
    }


static void LOG(int D,unsigned& DL,int& DS)
    {
    CHECK_TC(D,16);

    DS = D>>15;

    unsigned DQM = (D<0) ? -D : D;
    DQM &= 0x7fff;

    unsigned EXP = 0;
    unsigned x = DQM;
    if(x>=(1<<8))
        {
        EXP |= 8;
        x >>= 8;
        }
    if(x>=(1<<4))
        {
        EXP |= 4;
        x >>= 4;
        }
    if(x>=(1<<2))
        {
        EXP |= 2;
        x >>= 2;
        }
    EXP |= x>>1;

    unsigned MANT = ((DQM<<7)>>EXP)&0x7f;
    DL = (EXP<<7) + MANT;

    CHECK_UM(DL,11);
    CHECK_TC(DS,1);
    }


static void QUAN(unsigned RATE,int DLN,int DS,unsigned& I)
    {
    CHECK_TC(DLN,12);
    CHECK_TC(DS,1);

    int x;

    if(RATE==2)
        x = (DLN>=261);
    else
        {
        static const int16_t quan3[4] = {8,218,331,0x7fff};
        static const int16_t quan4[8] = {3972-0x1000,80,178,246,300,349,400,0x7fff};
        static const int16_t quan5[16] = {3974-0x1000,4080-0x1000,68,139,198,250,298,339,378,413,445,475,502,528,553,0x7fff};
        static const int16_t* const quan[3] = {quan3,quan4,quan5};

        const int16_t* levels = quan[RATE-3];
        const int16_t* levels0 = levels;

        while(DLN>=*levels++) {}

        x = levels-levels0-1;
        if(!x)
            x = ~DS;
        }
    int mask = (1<<RATE)-1;
    I = (x^DS)&mask;

    CHECK_UNSIGNED(I,RATE);
    }


inline static void SUBTB(unsigned DL,unsigned Y,int& DLN)
    {
    CHECK_UM(DL,11);
    CHECK_UM(Y,13);
    DLN = DL-(Y>>2);
    CHECK_TC(DLN,12);
    }


inline static void ADDA(int DQLN,unsigned Y,int& DQL)
    {
    CHECK_TC(DQLN,12);
    CHECK_UM(Y,13);
    DQL = DQLN+(Y>>2);
    CHECK_TC(DQL,12);
    }


inline static void ANTILOG(int DQL,int DQS,unsigned& DQ)
    {
    CHECK_TC(DQL,12);
    CHECK_TC(DQS,1);
    unsigned DEX = (DQL >> 7) & 15;
    unsigned DMN = DQL & 127;
    unsigned DQT = (1 << 7) + DMN;

    unsigned DQMAG;
    if(DQL>=0)
        DQMAG = (DQT << 7) >> (14 - DEX);
    else
        DQMAG = 0;

    DQ = DQS ? DQMAG+(1<<15) : DQMAG;

    CHECK_SM(DQ,16);
    }


inline static void RECONST(unsigned RATE,unsigned I,int& DQLN,int& DQS)
    {
    CHECK_UNSIGNED(I,RATE);

    // Tables 11-14
    static const int16_t reconst2[2] = {116,365};
    static const int16_t reconst3[4] = {2048-4096,135,273,373};
    static const int16_t reconst4[8] = {2048-4096,4,135,213,273,323,373,425};
    static const int16_t reconst5[16] = {2048-4096,4030-4096,28,104,169,224,274,318,358,395,429,459,488,514,539,566};
    static const int16_t* const reconst[4] = {reconst2,reconst3,reconst4,reconst5};

    int x = I;
    int m = 1<<(RATE-1);
    if(x&m)
        {
        DQS = -1;
        x = ~x;
        }
    else
        DQS = 0;
    DQLN = reconst[RATE-2][x&(m-1)];

    CHECK_TC(DQLN,12);
    CHECK_TC(DQS,1);
    }


inline static void FILTD(int WI,unsigned Y,unsigned& YUT)
    {
    CHECK_TC(WI,12);
    CHECK_UM(Y,13);
    int DIF = (WI<<5)-Y;
    int DIFSX = DIF>>5;
    YUT = (Y+DIFSX); // & 8191
    CHECK_UM(YUT,13);
    }


inline static void FILTE(unsigned YUP,unsigned YL,unsigned& YLP)
    {
    CHECK_UM(YUP,13);
    CHECK_UM(YL,19);
    int DIF = (YUP<<6)-YL;
    int DIFSX = DIF>>6;
    YLP = (YL+DIFSX); // & 524287
    CHECK_UM(YLP,19);
    }


inline static void FUNCTW(unsigned RATE,unsigned I,int& WI)
    {
    CHECK_UNSIGNED(I,RATE);

    static const int16_t functw2[2] = {4074-4096,439};
    static const int16_t functw3[4] = {4092-4096,30,137,582};
    static const int16_t functw4[8] = {4084-4096,18,41,64,112,198,355,1122};
    static const int16_t functw5[16] = {14,14,24,39,40,41,58,100,141,179,219,280,358,440,529,696};
    static const int16_t* const functw[4] = {functw2,functw3,functw4,functw5};

    unsigned signMask = 1<<(RATE-1);
    unsigned n = (I&signMask) ? (2*signMask-1)-I : I;
    WI = functw[RATE-2][n];

    CHECK_TC(WI,12);
    }


inline static void LIMB(unsigned YUT,unsigned& YUP)
    {
    CHECK_UM(YUT,13);
    unsigned GEUL = (YUT+11264)&(1<<13);
    unsigned GELL = (YUT+15840)&(1<<13);
    if(GELL)
        YUP = 544;
    else if (!GEUL)
        YUP = 5120;
    else
        YUP = YUT;
    CHECK_UM(YUP,13);
    }


inline static void MIX(unsigned AL,unsigned YU,unsigned YL,unsigned& Y)
    {
    CHECK_UM(AL,7);
    CHECK_UM(YU,13);
    CHECK_UM(YL,19);
    int DIF = YU-(YL>>6);
    int PROD = DIF*AL;
    if(DIF<0) PROD += (1<<6)-1; // Force round towards zero for following shift
    PROD >>= 6;
    Y = ((YL>>6)+PROD); // & 8191;
    CHECK_UM(Y,13);
    }


inline static void FILTA(unsigned FI,unsigned DMS,unsigned& DMSP)
    {
    CHECK_UM(FI,3);
    CHECK_UM(DMS,12);
    int DIF = (FI<<9)-DMS;
    int DIFSX = (DIF>>5);
    DMSP = (DIFSX+DMS); // & 4095;
    CHECK_UM(DMSP,12);
    }


inline static void FILTB(unsigned FI,unsigned DML,unsigned& DMLP)
    {
    CHECK_UM(FI,3);
    CHECK_UM(DML,14);
    int DIF = (FI<<11)-DML;
    int DIFSX = (DIF>>7);
    DMLP = (DIFSX+DML); // & 16383;
    CHECK_UM(DMLP,14);
    }


inline static void FILTC(unsigned AX,unsigned AP,unsigned& APP)
    {
    CHECK_UM(AX,1);
    CHECK_UM(AP,10);
    int DIF = (AX<<9)-AP;
    int DIFSX = (DIF>>4);
    APP = (DIFSX+AP); // & 1023;
    CHECK_UM(APP,10);
    }


inline static void FUNCTF(unsigned RATE,unsigned I,unsigned& FI)
    {
    CHECK_UNSIGNED(I,RATE);

    static const int16_t functf2[2] = {0,7};
    static const int16_t functf3[4] = {0,1,2,7};
    static const int16_t functf4[8] = {0,0,0,1,1,1,3,7};
    static const int16_t functf5[16] = {0,0,0,0,0,1,1,1,1,1,2,3,4,5,6,6};
    static const int16_t* const functf[4] = {functf2,functf3,functf4,functf5};

    unsigned x = I;
    int mask=(1<<(RATE-1));
    if(x&mask)
        x = ~x;
    x &= mask-1;
    FI = functf[RATE-2][x];

    CHECK_UM(FI,3);
    }


inline static void LIMA(unsigned AP,unsigned& AL)
    {
    CHECK_UM(AP,10);
    AL = (AP>256) ? 64 : AP>>2;
    CHECK_UM(AL,7);
    }


inline static void SUBTC(unsigned DMSP,unsigned DMLP,unsigned TDP,unsigned Y,unsigned& AX)
    {
    CHECK_UM(DMSP,12);
    CHECK_UM(DMLP,14);
    CHECK_UNSIGNED(TDP,1);
    CHECK_UM(Y,13);
    int DIF = (DMSP<<2)-DMLP;
    unsigned DIFM;
    if(DIF<0)
        DIFM = -DIF;
    else
        DIFM = DIF;
    unsigned DTHR = DMLP >> 3;
    AX = (Y>=1536 && DIFM<DTHR) ? TDP : 1;
    CHECK_UM(AX,1);
    }


inline static void TRIGA(unsigned TR,unsigned APP,unsigned& APR)
    {
    CHECK_UNSIGNED(TR,1);
    CHECK_UM(APP,10);
    APR = TR ? 256 : APP;
    CHECK_UM(APR,10);
    }

inline static void ACCUM(int WAn[2],int WBn[6],int& SE,int& SEZ)
    {
    CHECK_TC(WAn[0],16);
    CHECK_TC(WAn[1],16);
    CHECK_TC(WBn[0],16);
    CHECK_TC(WBn[1],16);
    CHECK_TC(WBn[2],16);
    CHECK_TC(WBn[3],16);
    CHECK_TC(WBn[4],16);
    CHECK_TC(WBn[5],16);
    int16_t SEZI = (int16_t)(WBn[0]+WBn[1]+WBn[2]+WBn[3]+WBn[4]+WBn[5]);
    int16_t SEI = (int16_t)(SEZI+WAn[0]+WAn[1]);
    SEZ = SEZI >> 1;
    SE = SEI >> 1;
    CHECK_TC(SE,15);
    CHECK_TC(SEZ,15);
    }


inline static void ADDB(unsigned DQ,int SE,int& SR)
    {
    CHECK_SM(DQ,16);
    CHECK_TC(SE,15);
    int DQI;
    if(DQ&(1<<15))
        DQI = (1<<15)-DQ;
    else
        DQI = DQ;
    SR = (int16_t)(DQI+SE);
    CHECK_TC(SR,16);
    }


inline static void ADDC(unsigned DQ,int SEZ,int& PK0,unsigned& SIGPK)
    {
    CHECK_SM(DQ,16);
    CHECK_TC(SEZ,15);
    int DQI;
    if(DQ&(1<<15))
        DQI = (1<<15)-DQ;
    else
        DQI = DQ;
    int DQSEZ = (int16_t)(DQI+SEZ);
    PK0 = DQSEZ>>15;
    SIGPK = DQSEZ ? 0 : 1;
    CHECK_TC(PK0,1);
    CHECK_UNSIGNED(SIGPK,1);
    }


static void MagToFloat(unsigned mag,unsigned& exp,unsigned& mant)
    {
    unsigned e = 0;
    unsigned m = mag<<1;
    if(m>=(1<<8))
        {
        e |= 8;
        m >>= 8;
        }
    if(m>=(1<<4))
        {
        e |= 4;
        m >>= 4;
        }
    if(m>=(1<<2))
        {
        e |= 2;
        m >>= 2;
        }
    e |= m>>1;
    exp = e;
    mant = mag ? (mag<<6)>>e : 1<<5;
    }


inline static void FLOATA(unsigned DQ, unsigned& DQ0)
    {
    CHECK_SM(DQ,16);
    unsigned DQS = (DQ>>15);
    unsigned MAG = DQ&32767;
    unsigned EXP;
    unsigned MANT;
    MagToFloat(MAG,EXP,MANT);
    DQ0 = (DQS<<10) + (EXP<<6) + MANT;
    CHECK_FL(DQ0,11);
    }


inline static void FLOATB(int SR, unsigned& SR0)
    {
    CHECK_TC(SR,16);
    unsigned SRS = (SR>>15)&1;
    unsigned MAG = SRS ? (-SR)&32767 : SR;
    unsigned EXP;
    unsigned MANT;
    MagToFloat(MAG,EXP,MANT);
    SR0 = (SRS<<10) + (EXP<<6) + MANT;
    CHECK_FL(SR0,11);
    }


static void FMULT(int An,unsigned SRn,int& WAn)
    {
    CHECK_TC(An,16);
    CHECK_FL(SRn,11);
    unsigned AnS = (An>>15)&1;
    unsigned AnMAG = AnS ? (-(An>>2))&8191 : An>>2;
    unsigned AnEXP;
    unsigned AnMANT;
    MagToFloat(AnMAG,AnEXP,AnMANT);

    unsigned SRnS = SRn>>10;
    unsigned SRnEXP = (SRn>>6) & 15;
    unsigned SRnMANT = SRn&63;

    unsigned WAnS = SRnS^AnS;
    unsigned WAnEXP = SRnEXP+AnEXP;
    unsigned WAnMANT = ((SRnMANT*AnMANT)+48)>>4;
    unsigned WAnMAG;
    if(WAnEXP<=26)
        WAnMAG = (WAnMANT<<7) >> (26-WAnEXP);
    else
        WAnMAG = ((WAnMANT<<7) << (WAnEXP-26)) & 32767;
    WAn = WAnS ? -(int)WAnMAG : WAnMAG;
    CHECK_TC(WAn,16);
    }


inline static void LIMC(int A2T,int& A2P)
    {
    CHECK_TC(A2T,16);
    const int A2UL = 12288;
    const int A2LL = 53248-65536;
    if(A2T<=A2LL)
        A2P = A2LL;
    else if(A2T>=A2UL)
        A2P = A2UL;
    else
        A2P = A2T;
    CHECK_TC(A2P,16);
    }


inline static void LIMD(int A1T,int A2P,int& A1P)
    {
    CHECK_TC(A1T,16);
    CHECK_TC(A2P,16);
    const int OME = 15360;
    int A1UL = (int16_t)(OME-A2P);
    int A1LL = (int16_t)(A2P-OME);
    if(A1T<=A1LL)
        A1P = A1LL;
    else if(A1T>=A1UL)
        A1P = A1UL;
    else
        A1P = A1T;
    CHECK_TC(A1P,16);
    }


inline static void TRIGB(unsigned TR,int AnP,int& AnR)
    {
    CHECK_UNSIGNED(TR,1);
    CHECK_TC(AnP,16);
    AnR = TR ? 0 : AnP;
    CHECK_TC(AnR,16);
    }


inline static void UPA1(int PK0,int PK1,int A1,unsigned SIGPK,int& A1T)
    {
    CHECK_TC(PK0,1);
    CHECK_TC(PK1,1);
    CHECK_TC(A1,16);
    CHECK_UNSIGNED(SIGPK,1);
    int UGA1;
    if(SIGPK==0)
        {
        if(PK0^PK1)
            UGA1 = -192;
        else
            UGA1 = 192;
        }
    else
        UGA1 = 0;
    A1T = (int16_t)(A1+UGA1-(A1>>8));
    CHECK_TC(A1T,16);
    }


inline static void UPA2(int PK0,int PK1,int PK2,int A1,int A2,unsigned SIGPK,int& A2T)
    {
    CHECK_TC(PK0,1);
    CHECK_TC(PK1,1);
    CHECK_TC(PK2,1);
    CHECK_TC(A1,16);
    CHECK_TC(A2,16);
    CHECK_UNSIGNED(SIGPK,1);

    unsigned UGA2;
    if(SIGPK==0)
        {
        int UGA2A = (PK0^PK2) ? -16384 : 16384;

        int FA1;
        if(A1<-8191)     FA1 = -8191<<2;
        else if(A1>8191) FA1 = 8191<<2;
        else             FA1 = A1<<2;

        int FA = (PK0^PK1) ? FA1 : -FA1;

        UGA2 = (UGA2A+FA) >> 7;
        }
    else
        UGA2 = 0;
    A2T = (int16_t)(A2+UGA2-(A2>>7));

    CHECK_TC(A2T,16);
    }


inline static void UPB(unsigned RATE,int Un,int Bn,unsigned DQ,int& BnP)
    {
    CHECK_TC(Un,1);
    CHECK_TC(Bn,16);
    CHECK_SM(DQ,16);
    int UGBn;
    if(DQ&32767)
        UGBn = Un ? -128 : 128;
    else
        UGBn = 0;
    int ULBn = (RATE==5) ? Bn>>9 : Bn>>8;
    BnP = (int16_t)(Bn+UGBn-ULBn);
    CHECK_TC(BnP,16);
    }


inline static void XOR(unsigned DQn,unsigned DQ,int& Un)
    {
    CHECK_FL(DQn,11);
    CHECK_SM(DQ,16);
    Un = -(int)((DQn>>10)^(DQ>>15));
    CHECK_TC(Un,1);
    }


inline static void TONE(int A2P,unsigned& TDP)
    {
    CHECK_TC(A2P,16);
    TDP = A2P<(53760-65536);
    CHECK_UNSIGNED(TDP,1);
    }


inline static void TRANS(unsigned TD,unsigned YL,unsigned DQ,unsigned& TR)
    {
    CHECK_UNSIGNED(TD,1);
    CHECK_UM(YL,19);
    CHECK_SM(DQ,16);
    unsigned DQMAG = DQ&32767;
    unsigned YLINT = YL>>15;
    unsigned YLFRAC = (YL>>10) & 31;
    unsigned THR1 = (32+YLFRAC)<<YLINT;
    unsigned THR2;
    if(YLINT>9)
        THR2 = 31<<10;
    else
        THR2 = THR1;
    unsigned DQTHR = (THR2+(THR2>>1)) >> 1;
    TR = DQMAG>DQTHR && TD;
    CHECK_UNSIGNED(TR,1);
    }


inline static void COMPRESS(int SR,unsigned LAW,unsigned& SP)
    {
    CHECK_TC(SR,16);
    CHECK_UNSIGNED(LAW,1);
    int x = SR;

#ifdef IMPLEMENT_G191_BUGS
    // reproduce bugs in G191 reference implementation...
    if(x==-0x8000)
        x=-1;
    else
        if(!LAW && x<0) x--;
#endif

    // Clamp to 14 bits...
    if(x>=(1<<13))
        x = (1<<13)-1;
    else if(x<-(1<<13))
        x = -(1<<13);

    if(LAW)
        SP = G711::ALawEncode(x<<2);
    else
        SP = G711::ULawEncode(x<<2);

    CHECK_UNSIGNED(SP,8);
    }


inline static void SYNC(unsigned RATE,unsigned I,unsigned SP,int DLNX,int DSX,unsigned LAW,unsigned& SD)
    {
    CHECK_UNSIGNED(SP,8);
    CHECK_TC(DLNX,12);
    CHECK_TC(DSX,1);
    CHECK_UNSIGNED(LAW,1);

    unsigned ID;
    unsigned IM;
    QUAN(RATE,DLNX,DSX,ID);
    unsigned signMask = 1<<(RATE-1);
    ID = ID^signMask;
    IM = I^signMask;

    unsigned s;
    if(LAW)
        {
        s = SP^0x55;
        if(!(s&0x80))
            s = s^0x7f;
        }
    else
        {
        s = SP;
        if(s&0x80)
            s = s^0x7f;
        }
    // s = ALAW/uLAW code converted to range 0x00..0xff
    // where 0x00 is most-negative and 0xff is most-positive

    if(ID<IM)
        {
        if(s<0xff)
            ++s;
        }
    else if(ID>IM)
        {
        if(s>0x00)
            {
            --s;
            if(s==0x7f && !LAW) --s; // TABLE 20/G.726 says uLaw 0xFF decrements to 0x7e and not 0x7f (!?)
            }
        }

    // convert s back to ALAW/uLAW code
    if(LAW)
        {
        if(!(s&0x80))
            s = s^0x7f;
        s = s^0x55;
        }
    else
        {
        if(s&0x80)
            s = s^0x7f;
        }
    SD = s;
    CHECK_UNSIGNED(SD,8);
    }


inline static void LIMO(int SR,int& SO)
    {
    CHECK_TC(SR,16);
    if(SR>=(1<<13))
        SO = (1<<13)-1;
    else if(SR<-(1<<13))
        SO = -(1<<13);
    else
        SO = SR;
    CHECK_TC(SO,14);
    }

 // End of group


inline void G726::InputPCMFormatConversionAndDifferenceSignalComputation(unsigned S,int SE,int& D)
    {
    int SL;
    EXPAND(S,LAW,SL);
    SUBTA(SL,SE,D);
    }


inline void G726::AdaptiveQuantizer(int D,unsigned Y,unsigned& I)
    {
    unsigned DL;
    int DS;
    LOG(D,DL,DS);

    int DLN;
    SUBTB(DL,Y,DLN);

    QUAN(RATE,DLN,DS,I);
    }


inline void G726::InverseAdaptiveQuantizer(unsigned I,unsigned Y,unsigned& DQ)
    {
    int DQLN;
    int DQS;
    RECONST(RATE,I,DQLN,DQS);

    int DQL;
    ADDA(DQLN,Y,DQL);

    ANTILOG(DQL,DQS,DQ);
    }


inline void G726::QuantizerScaleFactorAdaptation1(unsigned AL,unsigned& Y)
    {
    MIX(AL,YU,YL,Y);
    }
inline void G726::QuantizerScaleFactorAdaptation2(unsigned I,unsigned Y)
    {
    int WI;
    FUNCTW(RATE,I,WI);

    unsigned YUT;
    FILTD(WI,Y,YUT);

    unsigned YUP;
    LIMB(YUT,YUP);

    unsigned YLP;
    FILTE(YUP,YL,YLP);

    YU = YUP; // Delay
    YL = YLP; // Delay
    }


inline void G726::AdaptationSpeedControl1(unsigned& AL)
    {
    LIMA(AP,AL);
    }
inline void G726::AdaptationSpeedControl2(unsigned I,unsigned Y,unsigned TDP,unsigned TR)
    {
    unsigned FI;
    FUNCTF(RATE,I,FI);

    FILTA(FI,DMS,DMS); // Result 'DMSP' straight to delay storage 'DMS'
    FILTB(FI,DML,DML); // Result 'DMSP' straight to delay storage 'DMS'

    unsigned AX;
    SUBTC(DMS,DML,TDP,Y,AX); // DMSP and DMLP are read from delay storage 'DMS' and 'DML'

    unsigned APP;
    FILTC(AX,AP,APP);

    TRIGA(TR,APP,AP); // Result 'APR' straight to delay storage 'AP'
    }


inline void G726::AdaptativePredictorAndReconstructedSignalCalculator1(int& SE,int& SEZ)
    {
    int WBn[6];
    for(int i=0; i<6; i++)
        FMULT(Bn[i],DQn[i],WBn[i]);
    int WAn[2];
    FMULT(A1,SR1,WAn[0]);
    FMULT(A2,SR2,WAn[1]);
    ACCUM(WAn,WBn,SE,SEZ);
    }
inline void G726::AdaptativePredictorAndReconstructedSignalCalculator2(unsigned DQ,unsigned TR,int SE,int SEZ,int& SR,int& A2P)
    {
    int PK0;
    unsigned SIGPK;
    ADDC(DQ,SEZ,PK0,SIGPK);

    ADDB(DQ,SE,SR);
    SR2 = SR1; // Delay
    FLOATB(SR,SR1);  // Result 'SR0' straight to delay storage 'SR1'

    unsigned DQ0;
    FLOATA(DQ,DQ0);

    int i;
    for(i=0; i<6; i++)
        {
        int Un;
        XOR(DQn[i],DQ,Un);
        int BnP;
        UPB(RATE,Un,Bn[i],DQ,BnP);
        TRIGB(TR,BnP,Bn[i]); // Result 'BnR' straight to delay storage 'Bn'
        }

    int A2T;
    UPA2(PK0,PK1,PK2,A1,A2,SIGPK,A2T);
    LIMC(A2T,A2P);
    TRIGB(TR,A2P,A2); // Result 'A2R' straight to delay storage 'A2'

    int A1T;
    UPA1(PK0,PK1,A1,SIGPK,A1T);
    int A1P;
    LIMD(A1T,A2P,A1P);
    TRIGB(TR,A1P,A1); // Result 'A1R' straight to delay storage 'A1'

    PK2 = PK1; // Delay
    PK1 = PK0; // Delay

    for(i=5; i>0; i--)
        DQn[i] = DQn[i-1]; // Delay
    DQn[0] = DQ0; // Delay
    }


inline void G726::ToneAndTransitionDetector1(unsigned DQ,unsigned& TR)
    {
    TRANS(TD,YL,DQ,TR);
    }
inline void G726::ToneAndTransitionDetector2(int A2P,unsigned TR,unsigned& TDP)
    {
    TONE(A2P,TDP);
    TRIGB(TR,TDP,(int&)TD);  // Result 'TDR' straight to delay storage 'TD'
    }


inline void G726::OutputPCMFormatConversionAndSynchronousCodingAdjustment(int SR,int SE,unsigned Y,unsigned I,unsigned& SD)
    {
    unsigned SP;
    COMPRESS(SR,LAW,SP);
    int SLX;
    EXPAND(SP,LAW,SLX);
    int DX;
    SUBTA(SLX,SE,DX);
    unsigned DLX;
    int DSX;
    LOG(DX,DLX,DSX);
    int DLNX;
    SUBTB(DLX,Y,DLNX);
    SYNC(RATE,I,SP,DLNX,DSX,LAW,SD);
    }


inline void G726::DifferenceSignalComputation(int SL,int SE,int& D)
    {
    SUBTA(SL,SE,D);
    }


inline void G726::OutputLimiting(int SR,int& SO)
    {
    LIMO(SR,SO);
    }

 // End of group


unsigned G726::EncodeDecode(unsigned input,bool encode)
    {
    unsigned AL;
    AdaptationSpeedControl1(AL);
    unsigned Y;
    QuantizerScaleFactorAdaptation1(AL,Y);
    int SE;
    int SEZ;
    AdaptativePredictorAndReconstructedSignalCalculator1(SE,SEZ);

    unsigned I;
    if(encode)
        {
        int D;
        if(LAW==G726::PCM16)
            {
            int SL = (int16_t)input;
            SL >>= 2; // Convert input from 16bit to 14bit
            DifferenceSignalComputation(SL,SE,D);
            }
        else
            InputPCMFormatConversionAndDifferenceSignalComputation(input,SE,D);
        AdaptiveQuantizer(D,Y,I);
        }
    else
        I = input;

    unsigned DQ;
    InverseAdaptiveQuantizer(I,Y,DQ);
    unsigned TR;
    ToneAndTransitionDetector1(DQ,TR);
    int SR;
    int A2P;
    AdaptativePredictorAndReconstructedSignalCalculator2(DQ,TR,SE,SEZ,SR,A2P);
    unsigned TDP;
    ToneAndTransitionDetector2(A2P,TR,TDP);
    AdaptationSpeedControl2(I,Y,TDP,TR);
    QuantizerScaleFactorAdaptation2(I,Y);

    if(encode)
        return I;

    if(LAW==G726::PCM16)
        {
        int SO;
        OutputLimiting(SR,SO);
        return SO<<2; // Convert result from 14bit to 16 bit
        }
    else
        {
        unsigned SD;
        OutputPCMFormatConversionAndSynchronousCodingAdjustment(SR,SE,Y,I,SD);
        return SD;
        }
    }


/*
Public members of class G726
*/


void G726::Reset()
    {
    int i;
    for(i=0; i<6; i++)
        {
        Bn[i] = 0;
        DQn[i] = 32;
        }
    A1 = 0;
    A2 = 0;
    AP = 0;
    DML = 0;
    DMS = 0;
    PK1 = 0;
    PK2 = 0;
    SR1 = 32;
    SR2 = 32;
    TD = 0;
    YL = 34816;
    YU = 544;
    }


void G726::SetLaw(Law law)
    {
    ASSERT_DEBUG((unsigned)law<=PCM16);
    LAW = law;
    }


void G726::SetRate(Rate rate)
    {
    ASSERT_DEBUG((unsigned)(rate-Rate16kBits)<=(unsigned)(Rate40kBits-Rate16kBits));
    RATE = rate;
    }


unsigned G726::Encode(unsigned S)
    {
    return EncodeDecode(S,true);
    }


unsigned G726::Decode(unsigned I)
    {
    I &= (1<<RATE)-1; // Mask off un-needed bits
    return EncodeDecode(I,false);
    }


unsigned G726::Encode(void* dst, int dstOffset, const void* src, size_t srcSize)
    {
    // convert pointers into more useful types
    uint8_t* out = (uint8_t*)dst;
    union
        {
    const uint8_t* ptr8;
    const uint16_t* ptr16;
        }
    in;
    in.ptr8 = (const uint8_t*)src;

    // use given bit offset
    out += dstOffset>>3;
    unsigned bitOffset = dstOffset&7;

    unsigned bits = RATE;           // bits per adpcm sample
    unsigned mask = (1<<bits)-1;    // bitmask for an adpcm sample

    // calculate number of bits to be written
    unsigned outBits;
    if(LAW!=PCM16)
        outBits = bits*srcSize;
    else
        {
        outBits = bits*(srcSize>>1);
        srcSize &= ~1; // make sure srcSize represents a whole number of samples
        }

    // calculate end of input buffer
    const uint8_t* end = in.ptr8+srcSize;

    while(in.ptr8<end)
        {
        // read a single PCM value from input
        unsigned pcm;
        if(LAW==PCM16)
            pcm = *in.ptr16++;
        else
            pcm = *in.ptr8++;

        // encode the pcm value as an adpcm value
        unsigned adpcm = Encode(pcm);
        // shift it to the required output position
        adpcm <<= bitOffset;

        // write adpcm value to buffer...
        unsigned b = *out;          // get byte from ouput
        b &= ~(mask<<bitOffset);    // clear bits which we want to write to
        b |= adpcm;                 // or in adpcm value
        *out = (uint8_t)b;          // write value back to output

        // update bitOffset for next adpcm value
        bitOffset += bits;

        // loop if not moved on to next byte
        if(bitOffset<8)
            continue;

        // move pointers on to next byte
        ++out;
        bitOffset &= 7;

        // write any left-over bits from the last adpcm value
        if(bitOffset)
            *out = (uint8_t)(adpcm>>8);
        }

    // return number bits written to dst
    return outBits;
    }


unsigned G726::Decode(void* dst, const void* src, int srcOffset, unsigned srcSize)
    {
    // convert pointers into more useful types
    union
        {
    uint8_t* ptr8;
    uint16_t* ptr16;
        }
    out;
    out.ptr8 = (uint8_t*)dst;
    const uint8_t* in = (const uint8_t*)src;

    // use given bit offset
    in += srcOffset>>3;
    unsigned bitOffset = srcOffset&7;

    unsigned bits = RATE;       // bits per adpcm sample

    while(srcSize>=bits)
        {
        // read adpcm value from input
        unsigned adpcm = *in;
        if(bitOffset+bits>8)
            adpcm |= in[1]<<8;  // need bits from next byte as well

        // allign adpcm value to bit 0
        adpcm >>= bitOffset;

        // decode the adpcm value into a pcm value
        unsigned pcm = Decode(adpcm);

        // write pcm value to output
        if(LAW==PCM16)
            *out.ptr16++ = (int16_t)pcm;
        else
            *out.ptr8++ = (uint8_t)pcm;

        // update bit values for next adpcm value
        bitOffset += bits;
        srcSize -= bits;

        // move on to next byte of input if required
        if(bitOffset>=8)
            {
            bitOffset &= 7;
            ++in;
            }
        }

    // return number of bytes written to dst
    return out.ptr8-(uint8_t*)dst;
    }