// Angle Normalize
//
// angle_normalize - normalize angle to [-PI, PI]
//
// y[t] = fmod( x[t], 2*PI ), adjusted to [-PI, PI]
//
// X_n - 1
// Y_n - 1
// C_n - 0

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



ts_parameter_t const ts_angle_normalize_p = { .n_inputs = 1, .n_outputs = 1, .n_constants = 0, .params_size = 0 };


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

int ts_angle_normalize(
	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_angle_normalize_p );
	(void)c;
	for( uint_t i = 0; i < n; ++i )
	{
		fp_t r = fmod( x[0][i], 2.0 * PI );
		if( r > PI )
			r -= 2.0 * PI;
		if( r < -PI )
			r += 2.0 * PI;
		y[0][i] = r;
	}
	return 0;
}