// Kalman Filter
//
// kalman_filter
//
// Kalman Filter
//
// Optimal state estimation via predict-update cycle
//
// Inputs:    1
// Outputs:   1
// Constants: 2


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

typedef struct {
    fp_t Q;
    fp_t R;
} ts_kalman_filter_params_t;

ts_parameter_t const ts_kalman_filter_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 2, .params_size = sizeof(ts_kalman_filter_params_t) };

uint_t ts_kalman_filter_offset( ts_constant_t const *c )
{
	( void )c;
	return ( 0 );
}

int ts_kalman_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_kalman_filter_p );
	if ( !c ) return -1;
	TS_CHECK_INPUT(x, x_n, n);
    fp_t Q = ((ts_kalman_filter_params_t const *)c)->Q;
    fp_t R = ((ts_kalman_filter_params_t const *)c)->R;

    if (Q < 0 || R < 0) return -1;
    if (isnan(Q) || isnan(R)) return -1;
    fp_t x_est = x[0][0];
    /* NOTE: P=1.0 is a reasonable default for normalized data.
     * For price data in the range of 100-3000, consider initializing
     * P to the variance of the first few samples. */
    fp_t P = 1.0;

    y[0][0] = x_est;
    for ( uint_t i = 1; i < n; ++i )
    {
        P = P + Q;
        fp_t denom = P + R;
        fp_t K = denom != 0 ? P / denom : 0;
        x_est = x_est + K * (x[0][i] - x_est);
        P = (1 - K) * P;
        y[0][i] = x_est;
    }
	return 0;
}