/*
 * Copyright (c) 2001-2003 Shiman Associates Inc. All Rights Reserved.
 * 
 * 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.
 *
 */
/*
 * $Id: mas_ppm_device.c,v 1.2 2003/06/30 01:59:08 rocko Exp $
 *
 * Copyright (c) 2000, 2001 by Shiman Associates Inc. and Sun
 * Microsystems, Inc. All Rights Reserved.
 *
 * 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.
 *
 * Except as contained in this notice, the names of the authors or
 * copyright holders shall not be used in advertising or otherwise to
 * promote the sale, use or other dealings in this Software without
 * prior written authorization from the authors or copyright holders,
 * as applicable.
 *
 * * All trademarks and registered trademarks mentioned herein are the
 * property of their respective owners. No right, title or interest in
 * or to any trademark, service mark, logo or trade name of the
 * authors or copyright holders or their licensors is granted.
 *
 */

/* 2 OCT 2002 - rocko - verified reentrant
 * 2 OCT 2002 - rocko - verified timestamp clean
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include "mas/mas_dpi.h"
#include "profile.h"


/* defaults */
#define PPM_DB_CUTOFF 60.0   /* meter cutoff point, dB below zero */
#define PPM_DECAY_TIME 0.4   /* seconds */
#define PPM_INTEGRATION 200  /* 1/PPM_INTEGRATION ms integration time */
#define PPM_REFRESH_CLIENT 4 /* multiple of 5ms */

/************************************************************************
 * ppm_state
 *
 * State memory structure for this device instance.
 *
 ************************************************************************/
struct ppm_state
{
    int32 reaction;
    int32 data_sink;
    int32 data_source;
    int32 meter_source;
    int32 rate;
    int8  got_source, got_sink, got_meter, ismono;
    
    uint32 samples; /* total # of samples that have flown through device */

    uint32 last_refresh;
    uint32 fivems;  /* number of samples for 5 ms */
    uint32 refresh_client; /* */
    int8   count;
    double db_cutoff;
    
    double l, r, lhistory, rhistory, decay, decay_time;
    
    char * buffer;
    uint32 buffer_size;
    uint32 buffer_start;
    
};

/*************************************************************************
 * ACTIONS
 *************************************************************************/


/* standard actions ****************************************************/
int32 mas_dev_init_instance( int32 , void* );
int32 mas_dev_show_state( int32 device_instance, void* predicate );
int32 mas_dev_terminate( int32 , void* );

/* device specific actions *********************************************/
int32 mas_ppm_convert( int32, void* );
int32 mas_ppm_set_properties( int32, void* );


/* local functions */
static void integrate_5ms     ( struct ppm_state *s, double *l, double *r );
static void mono_integrate_5ms( struct ppm_state *s, double *l, double *r );


int32
mas_dev_init_instance( int32 device_instance, void* predicate )
{
    struct ppm_state*  state;
    
    /* Allocate state holder and cast it so we can work on it */
    state       = masc_rtalloc(sizeof(struct ppm_state));
    if ( state == 0 )
	return mas_error(MERR_MEMORY);

    masd_set_state(device_instance, state); /* set device state */
    memset( state, 0, sizeof (struct ppm_state) );
    
    masd_get_port_by_name( device_instance, "ppm_sink",
			   &state->data_sink );
    masd_get_port_by_name( device_instance, "ppm_source",
			   &state->data_source );
    masd_get_port_by_name( device_instance, "ppm_meter",
			   &state->meter_source );
    masd_get_port_by_name( device_instance, "reaction",
			   &state->reaction );

    return 0;
}


int32
mas_dev_configure_port( int32 device_instance, void* predicate )
{
    struct ppm_state*  state;
    struct mas_data_characteristic* dc;
    int32*                dataflow_port_dependency;
    int32                 rate, chan;
    int32 err;
        
    masd_get_state(device_instance, (void**)&state);
    err = masd_get_data_characteristic( *(int32*)predicate, &dc );

    if ( *(int32*)predicate == state->data_sink )
    {
        state->got_sink = 1;    
        /* source and sink must have same data characteristic. I'm
         * not checking this here! */
        
        rate = masc_get_index_of_key(dc, "sampling rate");
        state->rate = atol(dc->values[rate]);
        
        state->fivems = state->rate / PPM_INTEGRATION;
        state->refresh_client = PPM_REFRESH_CLIENT;
        state->db_cutoff = PPM_DB_CUTOFF;
        state->decay_time = PPM_DECAY_TIME;
        
        
        state->decay = PPM_REFRESH_CLIENT / PPM_DECAY_TIME / PPM_INTEGRATION;
        state->decay = pow(0.1, state->decay); 
/*         printf("DECAY is %f\n", state->decay); */
        
        
        chan = masc_get_index_of_key(dc, "channels");
        state->ismono = ( strcmp( dc->values[chan], "1" ) == 0 )? 1:0;

        /* let's make at least 100ms buffer... */
        if (state->ismono)
        {
            state->buffer_size = (state->rate * 2) / 10;
        }
        else
        {
            state->buffer_size = (state->rate * 4) / 10;
        }
        state->buffer = masc_rtalloc( state->buffer_size );
    }
    

    if ( *(int32*)predicate == state->data_source )
    {
        state->got_source = 1;    
    }

    if ( *(int32*)predicate == state->meter_source )
    {
        state->got_meter = 1;
    }


    if ( (state->got_source) && (state->got_sink) && (state->got_meter) )
    {
        /* schedule our dataflow dependency on data_sink */
        dataflow_port_dependency = masc_rtalloc( sizeof (int32) );
        *dataflow_port_dependency = state->data_sink;
        err = masd_reaction_queue_action(state->reaction, device_instance, 
                                         "mas_ppm_convert", 0, 0, 0, 0, 0,
                                         MAS_PRIORITY_DATAFLOW, 1, 1, 
                                         dataflow_port_dependency);
        if ( err < 0 ) return err;
    }

    return 0;
}

int32
mas_dev_disconnect_port( int32 device_instance, void* predicate )
{
    return 0;
}

int32 mas_dev_terminate( int32 device_instance, void* predicate )
{
    return 0;
}

int32
mas_dev_exit_instance( int32 device_instance, void* predicate )
{
    struct ppm_state*  state;
    
    masd_get_state(device_instance, (void**)&state);

    masc_rtfree( state->buffer );
    masc_rtfree( state );
    
    return 0;
}

int32
mas_dev_show_state( int32 device_instance, void* predicate )
{
    struct ppm_state*  state;
    
    masd_get_state(device_instance, (void**)&state);

    masc_log_message( 0, "ppm device... yes I'm here doing work.", device_instance );
    return 0;
}

int32
mas_ppm_convert( int32 device_instance, void* predicate )
{
    struct ppm_state*   s;
    struct mas_data*    data;
    struct mas_data*    meter_data;
    struct mas_package package;
    uint32 tmp, samples_just_in;
    double left, right;
    
        
    masd_get_state(device_instance, (void**)&s);
    masd_get_data( s->data_sink, &data );
    


    /* put data in buffer */
    
    if ( (s->buffer_size - s->buffer_start) > data->length )
    {
        memcpy(s->buffer+s->buffer_start, data->segment, data->length);
        s->buffer_start += data->length;
    }
    else
    {
        tmp = s->buffer_size - s->buffer_start;
        
        memcpy( s->buffer+s->buffer_start, data->segment, tmp );
        memcpy( s->buffer, data->segment + tmp, data->length - tmp );
        
        s->buffer_start = data->length - tmp ;
        
    }


    
    /* if it's time for a new integration, do it and send package */

    samples_just_in = (s->ismono) ? data->length / 2 : data->length / 4;
    
    while ( (s->samples + samples_just_in) > (s->last_refresh + s->fivems) ) 
    {
        if (s->ismono)
        {
            mono_integrate_5ms( s, &left, &right );
        }
        else
        {
            integrate_5ms( s, &left, &right );
        }

        s->count += 1;
        s->count = s->count % s->refresh_client;

        if (left > s->l)
            s->l = left;
        if (right > s->r)
            s->r = right;
        
        if (s->count==0)
        {
            left = s->l;
            right = s->r;

            s->l = s->r = 0.0;

            /* evolve history */
            s->lhistory *= s->decay;
            s->rhistory *= s->decay;

            if (s->lhistory > left)
                left = s->lhistory;
            else
                s->lhistory = left;

            if (s->rhistory > right)
                right = s->rhistory;
            else
                s->rhistory = right;
            
            
            /* is linear, want decibels */
            left = 20 * log10(left);
            if (left < -s->db_cutoff) {
                left = -s->db_cutoff;
            }
            /* map back to a 0..1 scale */
            left = left / s->db_cutoff + 1;
            
            right = 20 * log10(right);
            if (right < -s->db_cutoff) {
                right = -s->db_cutoff;
            }
            right = right / s->db_cutoff + 1;
            
            masc_setup_package( &package, NULL, 0, MASC_PACKAGE_NOFREE );
            masc_push_double( &package, left );
            masc_push_double( &package, right );
            masc_finalize_package( &package ); 
            
            /* must put the package in a data struct. This data struct is
             * stripped off again (a 'transform' happens into RTP packets)
             * before the thing goes through the channel. On the other
             * end, out comes a  package. */
            meter_data = MAS_NEW( meter_data );
            meter_data->length = package.size;
            meter_data->allocated_length = package.allocated_size;

            memcpy( &meter_data->header, &data->header, sizeof data->header );
                        
            meter_data->segment = package.contents;
        
            masd_post_data( s->meter_source, meter_data );
            
            masc_strike_package( &package );
        }
    }
    
    s->samples += samples_just_in;
    
    masd_post_data( s->data_source, data );
    
    return 0;
}


/***************************************************************************
 * mas_ppm_get_properties
 *
 *
 *
 ***************************************************************************/
int32 mas_ppm_get_properties( int32 device_instance, void* predicate )
{
    
    struct ppm_state*   s;
    struct mas_package package;
    double tmp;
    uint32 ms;
    
    masd_get_state(device_instance, (void**)&s);
    
    masc_setup_package( &package, NULL, 0, MASC_PACKAGE_NOFREE );

    /* icky */
    tmp = rint( 1000 * s->fivems / (double)s->rate );
    ms = tmp;
        
    masc_push_uint32( &package, ms );
    masc_push_uint32( &package, s->refresh_client );
    masc_push_double( &package, s->decay_time );
    masc_push_double( &package, s->db_cutoff );
    
    masc_finalize_package( &package );

    masd_reaction_queue_response( s->reaction, package.contents, package.size );

    masc_strike_package( &package );
    
    return 0;
}



/***************************************************************************
 * mas_ppm_set_properties
 *
 *
 *
 ***************************************************************************/
int32 mas_ppm_set_properties( int32 device_instance, void* predicate )
{
    struct ppm_state*  s;
    struct mas_package package;
    double db_cutoff, decay_time;
    uint32  integration, refresh_client;
    
    masd_get_state(device_instance, (void**)&s);

    masc_setup_package( &package, predicate, 0, MASC_PACKAGE_STATIC|MASC_PACKAGE_EXTRACT );
    masc_pull_uint32( &package, &integration );
    masc_pull_uint32( &package, &refresh_client );
    masc_pull_double( &package, &decay_time );
    masc_pull_double( &package, &db_cutoff );

    masc_strike_package( &package );

    s->db_cutoff = db_cutoff;
    s->fivems = s->rate * integration *.001;
    s->refresh_client = refresh_client;
    s->decay_time = decay_time;
  
    decay_time = .001 * refresh_client * integration / decay_time;
    s->decay = pow(0.1, decay_time);
  
    return 0;
}



void integrate_5ms( struct ppm_state *s, double *l, double *r )
{
    uint32 pos, i, tmp, bufsize;
    uint32 lsum, rsum;
    
    int16 *in;

    bufsize = s->buffer_size/4;
    
    s->last_refresh += s->fivems;
    pos = (s->last_refresh - s->fivems) % bufsize ;

    in = (int16*) s->buffer;
    lsum = 0;
    rsum = 0;
    
    for (i=pos; i < pos+s->fivems; i++)
    {
        tmp = 2*(i % bufsize);
        
        lsum += ( in[tmp]   < 0 )? -in[tmp]   : in[tmp];
        rsum += ( in[tmp+1] < 0 )? -in[tmp+1] : in[tmp+1];
    }

    *l = (double)lsum / (double)s->fivems / 32768.0;
    *r = (double)rsum / (double)s->fivems / 32768.0;

    /* printf("r %f, pos %lu\n",*r, pos); */
    
    return;
}

void mono_integrate_5ms( struct ppm_state *s, double *l, double *r )
{
    uint32 pos, i, tmp, bufsize;
    uint32 lsum;
    
    int16 *in;

    bufsize = s->buffer_size/2;
    
    s->last_refresh += s->fivems;
    pos = (s->last_refresh - s->fivems) % bufsize ;

    in = (int16*) s->buffer;
    lsum = 0;
    
    for (i=pos; i < pos+s->fivems; i++)
    {
        tmp = i % bufsize;
        lsum += ( in[tmp] < 0 )? -in[tmp]   : in[tmp];
    }

    *l = (double)lsum / (double)s->fivems / 32768.0;
    *r = 1;
    
    return;
}
