/*====================================================================
 * envmixer.c
 *
 * Copyright 1993, Silicon Graphics, Inc.
 * All Rights Reserved.
 *
 * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics,
 * Inc.; the contents of this file may not be disclosed to third
 * parties, copied or duplicated in any form, in whole or in part,
 * without the prior written permission of Silicon Graphics, Inc.
 *
 * RESTRICTED RIGHTS LEGEND:
 * Use, duplication or disclosure by the Government is subject to
 * restrictions as set forth in subdivision (c)(1)(ii) of the Rights
 * in Technical Data and Computer Software clause at DFARS
 * 252.227-7013, and/or in similar or successor clauses in the FAR,
 * DOD or NASA FAR Supplement. Unpublished - rights reserved under the
 * Copyright Laws of the United States.
 *====================================================================*/

#include <libaudio.h>
#include "synthInternals.h"
#include <math.h>
#include <em.h>
#include <os.h>
#include <stdio.h>

#define EQPOWER_LENGTH 128
short eqpower[ EQPOWER_LENGTH ] = {
	 32767,  32764,  32757,  32744,  32727,  32704,
	 32677,  32644,  32607,  32564,  32517,  32464,
	 32407,  32344,  32277,  32205,  32127,  32045,
	 31958,  31866,  31770,  31668,  31561,  31450,
	 31334,  31213,  31087,  30957,  30822,  30682,
	 30537,  30388,  30234,  30075,  29912,  29744,
	 29572,  29395,  29214,  29028,  28838,  28643,
	 28444,  28241,  28033,  27821,  27605,  27385,
	 27160,  26931,  26698,  26461,  26220,  25975,
	 25726,  25473,  25216,  24956,  24691,  24423,
	 24151,  23875,  23596,  23313,  23026,  22736,
	 22442,  22145,  21845,  21541,  21234,  20924,
	 20610,  20294,  19974,  19651,  19325,  18997,
	 18665,  18331,  17993,  17653,  17310,  16965,
	 16617,  16266,  15913,  15558,  15200,  14840,
	 14477,  14113,  13746,  13377,  13006,  12633,
	 12258,  11881,  11503,  11122,  10740,  10357,
	  9971,   9584,   9196,   8806,   8415,   8023,
	  7630,   7235,   6839,   6442,   6044,   5646,
	  5246,   4845,   4444,   4042,   3640,   3237,
	  2833,   2429,   2025,   1620,   1216,    810,
	   405,      0
};

/*
 * prototypes for private enveloper functions
 */
static  Acmd *_pullSubFrame(void *filter, short *inp, short *outp,
                            int outCount, int flags, Acmd *p) ;
static  short _getRate(float vol, float tgt, int count, unsigned short* ratel);

static  float _getVol(float ivol, int samples, short ratem, unsigned short ratel);

/***********************************************************************
 * Enveloper filter public interfaces
 ***********************************************************************/
void alEnvmixerNew(ALEnvMixer *e)
{
    alFilterNew((ALFilter *) e, alEnvmixerPull, alEnvmixerParam, AL_ENVMIX);
    e->first = 1;
    e->motion = AL_STOPPED;
    e->volume = 1;
    e->ltgt = 1;
    e->rtgt = 1;
    e->cvolL = 1;
    e->cvolR = 1;
    e->dryamt = 0;
    e->wetamt = 0;
    e->lratm = 1;
    e->lratl = 0;
    e->lratm = 1;
    e->lratl = 0;
    e->delta = 0;
    e->segEnd = 0;
    e->pan = 0;
    e->ctrlList = 0;
    e->ctrlTail = 0;
    e->sources = 0;
}

Acmd *alEnvmixerPull(void *filter, short *outp, int outCount, int flags,
                     Acmd *p) 
{
    Acmd        *ptr = p;
    ALEnvMixer	*e = (ALEnvMixer *)filter;
    ALFilter    **sources = e->sources;
    short       inp;
    int         lastOffset;
    int         thisOffset = 0;
    int         samples;
    short       loutp = 0;
    long        fVol;
    
    /*
     * Force the input to be the resampler output
     */
    inp = AL_RESAMPLER_OUT;
    
    e->ctrlTail = 0;
    for ( ;e->ctrlList != 0; e->ctrlList = e->ctrlList->next) {
            
        lastOffset = thisOffset;
        thisOffset = e->ctrlList->delta;
        samples    = thisOffset - lastOffset;

        switch (e->ctrlList->type) {
	  case (AL_FILTER_SET_FXAMT):
	  case (AL_FILTER_SET_PAN):
	  case (AL_FILTER_SET_VOLUME):
            ptr = _pullSubFrame(e, &inp, &loutp, samples, flags, ptr);
            e->delta += samples;
            
            if (e->delta >= e->segEnd){
                /*
                 * We should have reached our target, calculate
                 * target in case e->segEnd was 0
                 */
                e->ltgt = (e->volume * eqpower[e->pan]) >> 15;
                e->rtgt = (e->volume * eqpower[EQPOWER_LENGTH - e->pan - 1]) >> 15;
                e->delta = e->segEnd;   /* To prevent overflow */
                e->cvolL = e->ltgt;
                e->cvolR = e->rtgt;
            } else {
                /*
                 * Estimate the current volume
                 */
                e->cvolL = _getVol(e->cvolL, e->delta, e->lratm, e->lratl);
                e->cvolR = _getVol(e->cvolR, e->delta, e->rratm, e->rratl);
            }
            
            /*
             * We can't have volume of zero, because the
             * envelope would never go anywhere from there
             */
            if( e->cvolL == 0 ) e->cvolL = 1;
	    if( e->cvolR == 0 ) e->cvolR = 1;

            if (e->ctrlList->type == AL_FILTER_SET_PAN)

                /*
                 * This should result in a change to
                 * the current segment rate and target
                 */
                e->pan = (short) e->ctrlList->data.i;

            if (e->ctrlList->type == AL_FILTER_SET_VOLUME){

                /*
                 * Switching to a new segment
                 */
                e->delta = 0;
                
                /*
                 * Map volume non-linearly to give something close to
                 * loudness
                 */
                fVol = (e->ctrlList->data.i);
                fVol = (fVol*fVol)>>15;
                e->volume = (short) fVol;
                
                e->segEnd = e->ctrlList->moredata.i;

            }
            
            if (e->ctrlList->type == AL_FILTER_SET_FXAMT){
		e->dryamt = eqpower[e->ctrlList->data.i];
		e->wetamt = eqpower[EQPOWER_LENGTH - e->ctrlList->data.i - 1];
            }
            
            /*
             * Force a volume update
             */
	    e->first = 1;
	    break;

	  case (AL_FILTER_SET_STATE):
              if (e->ctrlList->data.i == AL_PLAYING) {                    
		/*
                 * Changing to PLAYING
                 */
                (*e->filter.setParam)(&e->filter, AL_FILTER_START, 0);
              } else {
		/*
                 * Changing to STOPPED and reset the filter
                 */
		ptr = _pullSubFrame(e, &inp, &loutp, samples, flags, ptr);
                (*e->filter.setParam)(&e->filter, AL_FILTER_RESET, 0);
              }
	    break;

	  default:
	    /*
	     * Pull the reuired number of samples and then pass the message
	     * on down the chain
	     */
	    ptr = _pullSubFrame(e, &inp, &loutp, samples, flags, ptr);
            e->delta += samples;
            (*e->filter.setParam)(&e->filter, e->ctrlList->type,
                                      (void *) e->ctrlList->data.i);
	    break;
        }
        loutp  += (samples<<1);
        outCount -= samples;
    }
    
    if (e->motion == AL_PLAYING){
        ptr = _pullSubFrame(e, &inp, &loutp, outCount, flags, ptr);
        e->delta += outCount;
    }

    /*
     * Prevent overflow in e->delta
     */
    if (e->delta > e->segEnd)
        e->delta = e->segEnd;
    
    return ptr;
}

int
alEnvmixerParam(void *filter, int paramID, void *param)
{
    ALFilter      *f = (ALFilter *) filter;
    ALEnvMixer	*e = (ALEnvMixer *) filter;
    
    switch (paramID) {

        case (AL_FILTER_ADD_UPDATE):
            if (e->ctrlTail) {
                e->ctrlTail->next = (ALParam *)param;
            } else {
                e->ctrlList = (ALParam *)param;
            }            
            e->ctrlTail = (ALParam *)param;
            break;
        
        case (AL_FILTER_RESET):
            e->first = 1;
            e->motion = AL_STOPPED;
            e->volume = 1;
            if (f->source)
                (*f->source->setParam)(f->source, AL_FILTER_RESET, param);
            break;

        case (AL_FILTER_START):
            e->motion = AL_PLAYING;
            if (f->source)
                (*f->source->setParam)(f->source, AL_FILTER_START, param);
            break;
            
        case (AL_FILTER_SET_SOURCE):
            f->source = (ALFilter *) param;
            break;

        default:
            if (f->source)
                (*f->source->setParam)(f->source, paramID, param);
    }
    return 0;
}

static
Acmd* _pullSubFrame(void *filter, short *inp, short *outp, int outCount,
                    int flags, Acmd *p) 
{
    Acmd        *ptr = p;
    ALEnvMixer	*e = (ALEnvMixer *)filter;
    ALFilter      *source;

    if (!outCount)
        return ptr;
    
    /*
     * ask all filters upstream from us to build their command
     * lists.
     */
    if (source = e->filter.source) {
	ptr = (*source->handler)(source, inp, outCount, flags, p);
    } else {
	/* error */
    }
    /*
     * construct our portion of the command list
     */
    aSetBuffer(ptr++, A_MAIN, *inp, AL_MAIN_L_OUT + *outp, outCount<<1);
    aSetBuffer(ptr++, A_AUX, AL_MAIN_R_OUT + *outp, AL_AUX_L_OUT + *outp,
	       AL_AUX_R_OUT + *outp);
    if (e->first){
	e->first = 0;

        /*
         * Calculate derived parameters
         */
        e->ltgt = (e->volume * eqpower[e->pan]) >> 15;
        e->lratm = _getRate((float)e->cvolL, (float)e->ltgt,
                            e->segEnd, &(e->lratl));
        e->rtgt = (e->volume *
                   eqpower[EQPOWER_LENGTH - e->pan - 1]) >> 15;
        e->rratm = _getRate((float)e->cvolR, (float)e->rtgt,
                            e->segEnd, &(e->rratl));

        aSetVolume(ptr++, A_LEFT | A_VOL, e->cvolL, 0, 0);
        aSetVolume(ptr++, A_RIGHT | A_VOL, e->cvolR, 0, 0);
	aSetVolume(ptr++, A_LEFT  | A_RATE, e->ltgt, e->lratm, e->lratl);
	aSetVolume(ptr++, A_RIGHT | A_RATE, e->rtgt, e->rratm, e->rratl);
	aSetVolume(ptr++, A_AUX, e->dryamt, 0, e->wetamt);
	aEnvMixer(ptr++, A_INIT | A_AUX,
                  osVirtualToPhysical(e->state.envstate));
    }
    else
	aEnvMixer(ptr++, A_CONTINUE | A_AUX,
                  osVirtualToPhysical(e->state.envstate));

    /*
     * bump the input buffer pointer
     */

    *inp += (outCount<<1);

    return ptr;
}

static
short _getRate(float vol, float tgt, int count, unsigned short* ratel)
{
    float	r, a;
    unsigned int	v, mask;
    int	      	i;
    short       s;
    
    if (count == 0){
        if (tgt >= vol){
            *ratel = 0xffff;
            return 0x7fff;
        }
        else{
            *ratel = 0;
            return 0;
        }
    }

    if (tgt < 1.0)
        tgt = 1.0;

    if (vol <= 0) vol = 1;	/* zero and neg values not allowed */
    r = sqrt(tgt / vol);
    v = 0xffffffff * (1.0/count);
    a = 1.0;
    mask = 0x80000000;

    /* the following loop calculates (tgt/vol)^(1/n) by accumulating
     * factors of a^(1/(2*i)), where i is the bit position with the
     * msb being position 0. The limit of i determines precision and
     * was empirically derived.
     */

    for (i=0; i<32; i++){
	if( v & mask )
	    a *= r;
	r = sqrt(r);
	mask = mask >> 1;
    }
    a *= (a *= (a *= a));
    s = (short) a;
    *ratel = (short)(0xffff * (a - (float) s));

    return (short)a;

}

static
float _getVol(float ivol, int samples, short ratem, unsigned short ratel)
{
    float	r, a;
    int	      	i;

    /*
     * Rate values are actually rate^8
     */
    samples >>=3;
    if (samples == 0)
        return ivol;
    
    r = ((float) (ratem<<16) + (float) ratel)/65536;
    a = 1.0;
    for (i=0; i<32; i++){
	if( samples & 1 )
	    a *= r;
        samples >>= 1;
	r *= r;
    }
    ivol *= a;
    return ivol;
}