tomwalters@0: /*
tomwalters@0:     integrate.c
tomwalters@0:     ===========
tomwalters@0: 
tomwalters@0: */
tomwalters@0: 
tomwalters@0: #include <math.h>
tomwalters@0: 
tomwalters@0: #include "stitch.h"
tomwalters@0: #include "source.h"
tomwalters@0: #include "calc.h"
tomwalters@0: 
tomwalters@0: #include "integrate.h"
tomwalters@0: 
tomwalters@0: 
tomwalters@0: struct _integral_state { ScalarType state, upward, downward, igain ; } ;
tomwalters@0: 
tomwalters@0: static int new_integral_callback( state, buffer, end, input, skip )
tomwalters@0: struct _integral_state *state ;
tomwalters@0: DataType *buffer, *end, *input ;
tomwalters@0: int skip ;
tomwalters@0: {
tomwalters@0:     register struct _integral_state *sptr = state ;
tomwalters@0:     register DataType *iptr = input ;
tomwalters@0:     register DataType *optr = buffer ;
tomwalters@0:     register DataType *eptr = end ;
tomwalters@0: 
tomwalters@0:     while( optr < eptr ) {
tomwalters@0: 
tomwalters@0: 	    /* the more accurate later version */
tomwalters@0: 
tomwalters@0: 	    *optr = DESCALE( sptr->state ) ;
tomwalters@0: 
tomwalters@0: 	    sptr->state += sptr->upward * DESCALE( *iptr * sptr->igain - sptr->state ) ;
tomwalters@0: 
tomwalters@0: 	    iptr += skip ;
tomwalters@0: 	    optr += skip ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     return ( sizeof ( DataType ) ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: static int old_integral_callback( state, buffer, end, input, skip )
tomwalters@0: struct _integral_state *state ;
tomwalters@0: DataType *buffer, *end, *input ;
tomwalters@0: int skip ;
tomwalters@0: {
tomwalters@0:     register struct _integral_state *sptr = state ;
tomwalters@0:     register DataType *iptr = input ;
tomwalters@0:     register DataType *optr = buffer ;
tomwalters@0:     register DataType *eptr = end ;
tomwalters@0: 
tomwalters@0:     while( optr < eptr ) {
tomwalters@0: 
tomwalters@0: 	    /* the "interesting" one */
tomwalters@0: 
tomwalters@0: 	    *optr = sptr->state ;
tomwalters@0: 	    sptr->state += DESCALE( sptr->upward * ( DESCALE( *iptr * sptr->igain ) - sptr->state ) ) ;
tomwalters@0: 
tomwalters@0: 	    iptr += skip ;
tomwalters@0: 	    optr += skip ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     return ( sizeof ( DataType ) ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: static void integral_close( state )
tomwalters@0: struct _integral_state *state ;
tomwalters@0: {
tomwalters@0:     Delete( state ) ;
tomwalters@0: 
tomwalters@0:     return ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: Source LowpassDataTypeSource( source, stages, channels, upward, downward, igain )
tomwalters@0: Source source ;
tomwalters@0: int stages, channels ;
tomwalters@0: double upward, downward, igain ;
tomwalters@0: {
tomwalters@0:     struct _integral_state **states ;
tomwalters@0:     int stage, chan ;
tomwalters@0: 
tomwalters@0:     for( stage=0 ; stage < abs( stages ) ; stage++ ) {
tomwalters@0: 
tomwalters@0: 	states = NewArray( struct _integral_state *, channels, "for states" ) ;
tomwalters@0: 
tomwalters@0: 	for( chan=0 ; chan<channels ; chan++ ) {
tomwalters@0: 
tomwalters@0: 	    states[chan] = New( struct _integral_state * ) ;
tomwalters@0: 
tomwalters@0: 	    states[chan]->state    = 0 ;
tomwalters@0: 	    states[chan]->upward   = SCALAR( 1. - exp( -1. /  upward  ) ) ;
tomwalters@0: 	    states[chan]->downward = SCALAR( 1. - exp( -1. / downward ) ) ;
tomwalters@0: 	    states[chan]->igain    = SCALAR(                  igain     ) ;
tomwalters@0: 
tomwalters@0: 	}
tomwalters@0: 
tomwalters@0: 	if( stages > 0 )
tomwalters@0: 	    source = NewMultiplexedSource( states, new_integral_callback, integral_close, channels, source, "integrate.c integration" ) ;
tomwalters@0: 	else
tomwalters@0: 	    source = NewMultiplexedSource( states, old_integral_callback, integral_close, channels, source, "integrate.c integration" ) ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     return ( source ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: 
tomwalters@0: struct _blocksample_state { struct _fillable_source parent ; Source source ; int spacing, channels ; } ;
tomwalters@0: 
tomwalters@0: static Pointer blocksample_callback( state, bytes, buffer )
tomwalters@0: struct _blocksample_state *state ;
tomwalters@0: ByteCount *bytes ;
tomwalters@0: DataType *buffer ;
tomwalters@0: {
tomwalters@0:     register int last = *bytes == 0 ;
tomwalters@0:     register StoreType sum ;
tomwalters@0:     register DataType *iptr ;
tomwalters@0:     register DataType *input = (DataType *) Pull( state->source, *bytes * state->spacing ) ;
tomwalters@0:     register DataType *end   = (DataType *) ( (Pointer) input  + *bytes * state->spacing ) ;
tomwalters@0:     int channel ;
tomwalters@0: 
tomwalters@0:     for( channel=0 ; channel<state->channels ; channel++ ) {
tomwalters@0: 
tomwalters@0: 	sum=0 ;
tomwalters@0: 
tomwalters@0: 	for( iptr=input+channel ; iptr < end ; iptr += state->channels )
tomwalters@0: 	    sum += *iptr ;
tomwalters@0: 
tomwalters@0: 	buffer[channel] = sum / state->spacing ;
tomwalters@0:     }
tomwalters@0: 
tomwalters@0:     if( !last )
tomwalters@0: 	return ( (Pointer) buffer ) ;
tomwalters@0:     else
tomwalters@0: 	return ( DeleteFillableSource( state ) ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: Source BlockSampleSource( source, spacing, channels )
tomwalters@0: Source source ;
tomwalters@0: int spacing, channels ;
tomwalters@0: {
tomwalters@0:     DeclareNew( struct _blocksample_state *, state ) ;
tomwalters@0: 
tomwalters@0:     state->source   = source ;
tomwalters@0:     state->spacing  = spacing ;
tomwalters@0:     state->channels = channels ;
tomwalters@0: 
tomwalters@0:     source = SetFillableSource( state, blocksample_callback, "integrate.c block sampling" ) ;
tomwalters@0: 
tomwalters@0:     source = NewSegmentingSource( source, channels * sizeof ( DataType ) ) ;
tomwalters@0: 
tomwalters@0:     return ( source ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: 
tomwalters@0: struct _downsample_state { struct _pullable_source parent ; Source source ; int spacing, channels ; } ;
tomwalters@0: 
tomwalters@0: static Pointer downsample_callback( state, bytes )
tomwalters@0: struct _downsample_state *state ;
tomwalters@0: ByteCount *bytes ;
tomwalters@0: {
tomwalters@0:     register int last = *bytes == 0 ;
tomwalters@0:     Pointer output = Pull( state->source, *bytes * state->spacing ) + *bytes * state->spacing - state->channels * sizeof ( DataType ) ;
tomwalters@0: 
tomwalters@0:     if( !last )
tomwalters@0: 	return ( output ) ;
tomwalters@0:     else
tomwalters@0: 	return ( DeletePullableSource( state ) ) ;
tomwalters@0: }
tomwalters@0: 
tomwalters@0: Source DownSampleSource( source, spacing, channels )
tomwalters@0: Source source ;
tomwalters@0: int spacing, channels ;
tomwalters@0: {
tomwalters@0:     DeclareNew( struct _downsample_state *, state ) ;
tomwalters@0: 
tomwalters@0:     state->source   = source ;
tomwalters@0:     state->spacing  = spacing ;
tomwalters@0:     state->channels = channels ;
tomwalters@0: 
tomwalters@0:     source = SetPullableSource( state, downsample_callback, "integrate.c down sampling" ) ;
tomwalters@0: 
tomwalters@0:     source = NewSegmentingSource( source, channels * sizeof ( DataType ) ) ;
tomwalters@0: 
tomwalters@0:     return ( source ) ;
tomwalters@0: }
tomwalters@0: