annotate model/integrate.c @ 0:5242703e91d3 tip

Initial checkin for AIM92 aimR8.2 (last updated May 1997).

/*
    integrate.c
    ===========

*/

#include <math.h>

#include "stitch.h"
#include "source.h"
#include "calc.h"

#include "integrate.h"


struct _integral_state { ScalarType state, upward, downward, igain ; } ;

static int new_integral_callback( state, buffer, end, input, skip )
struct _integral_state *state ;
DataType *buffer, *end, *input ;
int skip ;
{
    register struct _integral_state *sptr = state ;
    register DataType *iptr = input ;
    register DataType *optr = buffer ;
    register DataType *eptr = end ;

    while( optr < eptr ) {

	    /* the more accurate later version */

	    *optr = DESCALE( sptr->state ) ;

	    sptr->state += sptr->upward * DESCALE( *iptr * sptr->igain - sptr->state ) ;

	    iptr += skip ;
	    optr += skip ;
    }

    return ( sizeof ( DataType ) ) ;
}

static int old_integral_callback( state, buffer, end, input, skip )
struct _integral_state *state ;
DataType *buffer, *end, *input ;
int skip ;
{
    register struct _integral_state *sptr = state ;
    register DataType *iptr = input ;
    register DataType *optr = buffer ;
    register DataType *eptr = end ;

    while( optr < eptr ) {

	    /* the "interesting" one */

	    *optr = sptr->state ;
	    sptr->state += DESCALE( sptr->upward * ( DESCALE( *iptr * sptr->igain ) - sptr->state ) ) ;

	    iptr += skip ;
	    optr += skip ;
    }

    return ( sizeof ( DataType ) ) ;
}

static void integral_close( state )
struct _integral_state *state ;
{
    Delete( state ) ;

    return ;
}

Source LowpassDataTypeSource( source, stages, channels, upward, downward, igain )
Source source ;
int stages, channels ;
double upward, downward, igain ;
{
    struct _integral_state **states ;
    int stage, chan ;

    for( stage=0 ; stage < abs( stages ) ; stage++ ) {

	states = NewArray( struct _integral_state *, channels, "for states" ) ;

	for( chan=0 ; chan<channels ; chan++ ) {

	    states[chan] = New( struct _integral_state * ) ;

	    states[chan]->state    = 0 ;
	    states[chan]->upward   = SCALAR( 1. - exp( -1. /  upward  ) ) ;
	    states[chan]->downward = SCALAR( 1. - exp( -1. / downward ) ) ;
	    states[chan]->igain    = SCALAR(                  igain     ) ;

	}

	if( stages > 0 )
	    source = NewMultiplexedSource( states, new_integral_callback, integral_close, channels, source, "integrate.c integration" ) ;
	else
	    source = NewMultiplexedSource( states, old_integral_callback, integral_close, channels, source, "integrate.c integration" ) ;
    }

    return ( source ) ;
}





struct _blocksample_state { struct _fillable_source parent ; Source source ; int spacing, channels ; } ;

static Pointer blocksample_callback( state, bytes, buffer )
struct _blocksample_state *state ;
ByteCount *bytes ;
DataType *buffer ;
{
    register int last = *bytes == 0 ;
    register StoreType sum ;
    register DataType *iptr ;
    register DataType *input = (DataType *) Pull( state->source, *bytes * state->spacing ) ;
    register DataType *end   = (DataType *) ( (Pointer) input  + *bytes * state->spacing ) ;
    int channel ;

    for( channel=0 ; channel<state->channels ; channel++ ) {

	sum=0 ;

	for( iptr=input+channel ; iptr < end ; iptr += state->channels )
	    sum += *iptr ;

	buffer[channel] = sum / state->spacing ;
    }

    if( !last )
	return ( (Pointer) buffer ) ;
    else
	return ( DeleteFillableSource( state ) ) ;
}

Source BlockSampleSource( source, spacing, channels )
Source source ;
int spacing, channels ;
{
    DeclareNew( struct _blocksample_state *, state ) ;

    state->source   = source ;
    state->spacing  = spacing ;
    state->channels = channels ;

    source = SetFillableSource( state, blocksample_callback, "integrate.c block sampling" ) ;

    source = NewSegmentingSource( source, channels * sizeof ( DataType ) ) ;

    return ( source ) ;
}


struct _downsample_state { struct _pullable_source parent ; Source source ; int spacing, channels ; } ;

static Pointer downsample_callback( state, bytes )
struct _downsample_state *state ;
ByteCount *bytes ;
{
    register int last = *bytes == 0 ;
    Pointer output = Pull( state->source, *bytes * state->spacing ) + *bytes * state->spacing - state->channels * sizeof ( DataType ) ;

    if( !last )
	return ( output ) ;
    else
	return ( DeletePullableSource( state ) ) ;
}

Source DownSampleSource( source, spacing, channels )
Source source ;
int spacing, channels ;
{
    DeclareNew( struct _downsample_state *, state ) ;

    state->source   = source ;
    state->spacing  = spacing ;
    state->channels = channels ;

    source = SetPullableSource( state, downsample_callback, "integrate.c down sampling" ) ;

    source = NewSegmentingSource( source, channels * sizeof ( DataType ) ) ;

    return ( source ) ;
}