// Vertical Horizontal Filter
//
// vertical_horizontal_filter
//
// Vertical Horizontal Filter
//
// 
//
// Inputs:    4
// Outputs:   1
// Constants: 1


#include <math.h>
#include "ts.h"

typedef struct {
    uint_t k;
} ts_vertical_horizontal_filter_params_t;

ts_parameter_t const ts_vertical_horizontal_filter_p = { .n_inputs = 4, .n_outputs = 1, .n_constants = 1, .params_size = sizeof(ts_vertical_horizontal_filter_params_t) };

uint_t ts_vertical_horizontal_filter_offset( ts_constant_t const *c )
{
	( void )c;
	return ( c ? ((ts_vertical_horizontal_filter_params_t const *)c)->k : 0 );
}

int ts_vertical_horizontal_filter(
	fp_t * const *y, uint_t y_n,
	fp_t const * const *x, uint_t x_n,
	uint_t n, ts_constant_t const *c
)
{
	TS_PROLOGUE( ts_vertical_horizontal_filter_p );
	if ( !c ) return -1;
    uint_t k = ((ts_vertical_horizontal_filter_params_t const *)c)->k;
    if (k == 0 || n < k) return -1;

    fp_t cum_den = 0;

    for ( uint_t j = 1; j <= k; ++j )
    {
        cum_den += fabs( ts_h( x, j ) - ts_h( x, j - 1 ) )
                 + fabs( ts_l( x, j ) - ts_l( x, j - 1 ) );
    }

    for ( uint_t i = k; i < n; ++i )
    {
        if ( i > k )
        {
            cum_den += fabs( ts_h( x, i ) - ts_h( x, i - 1 ) )
                     + fabs( ts_l( x, i ) - ts_l( x, i - 1 ) );
            cum_den -= fabs( ts_h( x, i - k ) - ts_h( x, i - k - 1 ) )
                     + fabs( ts_l( x, i - k ) - ts_l( x, i - k - 1 ) );
        }

        fp_t hh = ts_h( x, i - k + 1 );
        fp_t ll = ts_l( x, i - k + 1 );
        for ( uint_t j = i - k + 2; j <= i; ++j )
        {
            if ( ts_h( x, j ) > hh ) hh = ts_h( x, j );
            if ( ts_l( x, j ) < ll ) ll = ts_l( x, j );
        }

        fp_t range = hh - ll;

        if ( cum_den == 0 )
            y[0][i] = ( range == 0 ) ? 1 : 0;
        else
            y[0][i] = range / cum_den;
    }
	return 0;
}