/*
 * utilities.h
 *
 * Created: 20/04/2021 19:59:00
 *  Author: Nick-XMG
 */ 


#ifndef UTILITIES_H_
#define UTILITIES_H_


#define ENABLED(V...)       DO(ENA,&&,V)
#define DISABLED(V...)      DO(DIS,&&,V)


#define DEBUG_1_PORT PORT_PC01
#define DEBUG_2_PORT PORT_PC03
#define DEBUG_3_PORT PORT_PC02
#define DEBUG_4_PORT PORT_PC30

inline void tic(const uint8_t pin)
{
	gpio_set_pin_level(pin, true);
}

inline void toc(const uint8_t pin)
{
	gpio_set_pin_level(pin, false);
}

inline void tic_port(const uint32_t port)
{
	REG_PORT_OUTSET2 = port;
}

inline void toc_port(const uint32_t port)
{
	REG_PORT_OUTCLR2 = port;
}


//! \brief     Saturates the input value between the minimum and maximum values
//! \param[in] in   The input value
//! \param[in] max  The maximum value allowed
//! \param[in] min  The minimum value allowed
//! \return    The saturated value
static inline float32_t f_clamp(const float32_t in, const float32_t min, const float32_t max)
{
	float32_t out = in;


	if(in < min)
	{
		out = min;
	}
	else if(in > max)
	{
		out = max;
	}
	else
	{
		// do nothing as of now
		;
	}

	return(out);
}

//! \brief     Finds the absolute value
//! \param[in] in   The input value
//! \return    The absolute value
static inline float32_t f_abs(const float32_t in)
{
	float32_t out = in;
	if(in < (float32_t)0.0)
	{
		out = -in;
	}

	return(out);
} 

// the typedefs

//! \brief Defines the first-order filter (FILTER_FO) object
//!
typedef struct _FILTER_FO_Obj_
{
	float32_t  a1;  //the denominator filter coefficient value for z^(-1)

	float32_t  b0;  //the numerator filter coefficient value for z^0
	float32_t  b1;  //the numerator filter coefficient value for z^(-1)

	float32_t  x1;  //the input value at time sample n=-1

	float32_t  y1;  //the output value at time sample n=-1
} FILTER_FO_Obj;


//! \brief     Runs a first-order filter of the form
//!            y[n] = b0*x[n] + b1*x[n-1] - a1*y[n-1]
//!
//! \param[in] handle      The filter handle
//! \param[in] inputValue  The input value to filter
//! \return    The output value from the filter

static inline float32_t FILTER_FO_run(volatile FILTER_FO_Obj *handle, const float32_t inputValue)
{
	FILTER_FO_Obj *obj = (FILTER_FO_Obj *)handle;

	float32_t a1 = obj->a1;
	float32_t b0 = obj->b0;
	float32_t b1 = obj->b1;
	float32_t x1 = obj->x1;
	float32_t y1 = obj->y1;


	// compute the output
	float32_t y0 = (b0 * inputValue) + (b1 * x1) - (a1 * y1);


	// store values for next time
	obj->x1 = inputValue;
	obj->y1 = y0;

	return(y0);
} // end of FILTER_FO_run() function


//! \brief     Runs a first-order filter of the form
//!            y[n] = b0*x[n] - a1*y[n-1]
//!
//! \param[in] handle      The filter handle
//! \param[in] inputValue  The input value to filter
//! \return    The output value from the filter
static inline float32_t FILTER_FO_run_form_0(volatile FILTER_FO_Obj *handle, const float32_t inputValue)
{
	FILTER_FO_Obj *obj = (FILTER_FO_Obj *)handle;

	float32_t a1 = obj->a1;
	float32_t b0 = obj->b0;
	float32_t y1 = obj->y1;


	// compute the output
	float_t y0 = (b0 * inputValue) - (a1 * y1);


	// store values for next time
	obj->y1 = y0;

	return(y0);
} // end of FILTER_FO_run_form_0() function

#endif /* UTILITIES_H_ */