/*********************************************

    SeamlessBufferManagement.c

**********************************************/



typedef long long INT64;



#define ALIGN32 __declspec(align(8)) // __align(32)

#define ALIGN32SFX // __attribute__ ((aligned(8)))



#define MAXWINLEN (1024*2*2)





#define ONE_Q16     (1<<16)

#define ONE_Q32     ((INT64)1<<32)



#define MINSPEED 0.2f

#define MAXSPEED 5.0f



union magic {

  struct {

    long lo;

    long hi;

  } halves;

  INT64 whole;

};



static __inline int SMULW(int x, int y)

{

    return (int)((INT64)((INT64)x * y)>>16);

}



static __inline int SMLAW(int acc,int x, int y)

{

    return acc+(int)((INT64)((INT64)x * y)>>16);

}



static __inline INT64 SMULL(int x, int y)

{

    return (INT64)((INT64)x * y);

}





static short buf[MAXWINLEN][2];



struct WinStruct {

    int winlen, winstep, wincoeff;

} Win;



static struct IdxStruct {

    int idxwr, idx, idx1;

} Idx;



static int SpeedPiV_bypass_flag=1;



// Multuthreading support

static char SBM_Init_flag=1;

static char SBM_SetPar_flag=1;

static int DJ_SpeedPiV_Fs=44100;

static float DJ_SpeedPiV_speed=1.0f;



static void SBM_Init_Apply(int fs);

static void SBM_SetPar_Apply(float level);



static void memfill32 (int* a, int b, int n)

{ do{ *a++=b; }while(--n>0); }



static void memcopy32 (int* a, int* b, int n)

{ do{ *a++=*b++; }while(--n>0); }



static int SBM_Exe_Frame(short (*out)[2], const short (*in)[2], short (*buf)[2],

                            struct IdxStruct* idxptr, struct WinStruct* win, int n)

{

    short (*out_orig)[2]=out;                           // store initial outptr position



    int idx  =idxptr->idx;

    int idx1 =idxptr->idx1;

    int idxwr=idxptr->idxwr;



    int wincoeff=win->wincoeff;

    int winlen  =win->winlen;

    int winstep =(winlen<<16)-win->winstep;



    do{

        int delta;



LoopSTART:

        // where am I ?

        delta=(idxwr<<16)-idx;

        if (delta<-(winlen<<(16-1))) delta+=(winlen<<16);

        if (delta>(winlen<<(16-1))) delta-=(winlen<<16);



        if (delta<0) //((idxwr<<16)>=idx)

            goto IDXWRINC;

        else

        {

            int win1, win2;

            int idx2;

            union magic tmp;

            int accleft,  accright;

            int dataleft, dataright;



            // window values

            win1=idx-(idx1<<16);

//            win1=(int)(((INT64)win1*wincoeff)>>32);

            tmp.whole = SMULL(win1, wincoeff);

            win1=tmp.halves.hi;



            if (win1<0) win1+=ONE_Q16<<1;                       // distance should be positive

            if (win1>ONE_Q16) win1=(ONE_Q16<<1)-win1;           // window value should be less than winlen/2



            // load data for idx1

            dataleft=buf[idx1][0];     dataright=buf[idx1][1];

            accleft=SMULW(win1, dataleft);

            accright=SMULW(win1, dataright);



            // second window value

            win2=ONE_Q16-win1;

            // compute idx2

            idx2=idx1-(winlen>>1); if (idx2<0) idx2+=winlen;



            // load data for idx2

            dataleft=buf[idx2][0];     dataright=buf[idx2][1];

            accleft =SMLAW(accleft, win2, dataleft);

            accright=SMLAW(accright,win2, dataright);



            // saturation (only for negative values required)

            if (accleft<-32768)  accleft=-32768;

            if (accright<-32768) accright=-32768;



            // write output

            (*out  )[0]=(short)accleft;

            (*out++)[1]=(short)accright;



            // modify pointers

            idx-=winstep;                           // winstep is fractional number

            if (idx<0) idx+=(winlen<<16);



            idx1+=1;        if (idx1>=winlen) idx1-=winlen;



            goto LoopSTART;



        }



IDXWRINC:



        buf[idxwr][0]=(*in)[0];  buf[idxwr][1]=(*in++)[1];      // fill buffer by input data



        idxwr++;        if (idxwr>=winlen) idxwr-=winlen;



    }while(--n>0);



    idxptr->idx  = idx  ;

    idxptr->idx1 = idx1 ;

    idxptr->idxwr= idxwr;



    return (out-out_orig);

}





#define LOGFRAME 7

#define FRAME (1<<LOGFRAME)



#define QTR         10

#define TRMAXLEVEL  (1<<(10))

#define TRDECR      1



//static short  Speed_buf[FRAME][2];

static int transition_flag, trcnt;

static int Speed_level;



enum{

    NO_TRANSITION=0,

    NORMAL_TO_EFFECT,

    EFFECT_TO_NORMAL

};



static void SBM_Exe_Transition(short (*out)[2], const short (*new_buf)[2], const short (*old_buf)[2],

                        int n, int trcnt)

{

    do{

        int new_data, old_data;



        int gain=trcnt; if (gain>TRMAXLEVEL) gain=TRMAXLEVEL;



        // left

        old_data=(*old_buf)[0];

        new_data=(*new_buf)[0];

        // saturation need?

        (*out)[0]  =(short) ( (((old_data-new_data)*gain)>>QTR)+new_data );



        // right

        old_data=(*old_buf++)[1];

        new_data=(*new_buf++)[1];

        // saturation need?

        (*out++)[1]=(short) ( (((old_data-new_data)*gain)>>QTR)+new_data );



        trcnt-=TRDECR; if (trcnt<0) trcnt=0;

    }while(--n>0);

}





static void SBM_InitSetPar_Apply(void)

{

    if (SBM_Init_flag)

    {

        SBM_Init_Apply(DJ_SpeedPiV_Fs);

        SBM_Init_flag=0;

    }

    if (SBM_SetPar_flag)

    {

        SBM_SetPar_Apply(DJ_SpeedPiV_speed);

        SBM_SetPar_flag=0;

    }

}



int SBM_Exe(short (*out)[2], const short (*in)[2], int n)

{

    int outnum;



    // Init and SetPar apply

	SBM_InitSetPar_Apply();



    if (SpeedPiV_bypass_flag){

    	//printf("vsp bypass mode");

        if (out!=(short (*)[2])in)

            memcopy32((int*) out, (int*) in, n);

        return(n);}



    switch (transition_flag)

    {

        case NORMAL_TO_EFFECT:

        {

            outnum=SBM_Exe_Frame( out/*Speed_buf*/, in, buf, &Idx, &Win, n );

//            if (out!=(short (*)[2])in)

//                memcopy32((int*) out, (int*) in, n);

            SBM_Exe_Transition(out,(const short (*)[2])out,in,n,trcnt);

            trcnt-=n; if (trcnt<=0) { trcnt=0; transition_flag=NO_TRANSITION; Win.winstep=Speed_level; }

        }

        break;



        case EFFECT_TO_NORMAL:

        {

            outnum=SBM_Exe_Frame( out/*Speed_buf*/, in, buf, &Idx, &Win, n );

//            if (out!=(short (*)[2])in)

//                memcopy32((int*) out, (int*) in, n);

            SBM_Exe_Transition(out,in,(const short (*)[2])out,n,trcnt);

            trcnt-=n; if (trcnt<=0) { trcnt=0; transition_flag=NO_TRANSITION; SpeedPiV_bypass_flag=1;}

        }

        break;



        default:

            outnum=SBM_Exe_Frame( out, in, buf, &Idx, &Win, n);

        break;

    }



    return( outnum );

}



void SBM_SetPar(float level)

{

    SBM_SetPar_flag=1;

    DJ_SpeedPiV_speed=level;

}



static void SBM_SetPar_Apply(float level)

{

    // check boundaries

    if ( ! ((level>=MINSPEED) && (level<=MAXSPEED)) )

        level=1.0f;



    // ptr increment will be set to this value

    Speed_level=(int)(level*(float)ONE_Q16);



    // default values

    transition_flag=NO_TRANSITION;

    SpeedPiV_bypass_flag=0;



    // check transition case

    if ((Speed_level!=ONE_Q16) && (Win.winstep!=ONE_Q16))

    {

        Win.winstep=Speed_level;

    }

    else if ((Speed_level==ONE_Q16) && (Win.winstep!=ONE_Q16))

    {

        transition_flag=EFFECT_TO_NORMAL;

        trcnt=TRMAXLEVEL;

        Win.winstep=Speed_level;

    }

    else if ((Speed_level!=ONE_Q16) && (Win.winstep==ONE_Q16))

    {

        transition_flag=NORMAL_TO_EFFECT;

        trcnt=TRMAXLEVEL+(Win.winlen);

        // reset internal buffer

        memfill32((int*)buf, 0, sizeof(buf)>>2/*MAXWINLEN*/);

        // 'Win.winstep' will be set after transiion period

    }

    else // ((Speed_level==ONE_Q16) && (Win.winstep==ONE_Q16))

        SpeedPiV_bypass_flag=1;



}



void SBM_Init(int fs)

{

    SBM_Init_flag=1;

    DJ_SpeedPiV_Fs=fs;

}



static void SBM_Init_Apply(int fs)

{



    // check samplerate boundaries

    if ((fs<8000) || (fs>96000))    fs = 44100;         // default samplerate



    // reset internal buffer

    memfill32((int*)buf, 0, MAXWINLEN);



    // window length

    Win.winlen=(int)((float)fs*(float)(MAXWINLEN)/96000.0f/*48000.0f*/);    Win.winlen&=-2;

    // coeff for window magnitude

    Win.wincoeff=(int)(ONE_Q32/(Win.winlen>>1));



    // init indices

    Idx.idx1=0;

    Idx.idxwr=Win.winlen>>1;

    Idx.idx=Idx.idxwr<<16;



    Win.winstep=ONE_Q16;

    Speed_level=ONE_Q16;



    SpeedPiV_bypass_flag=1;

}