#pragma once

#include <stdint.h>
#include <fixpoint/fp248.h>
#include <fixpoint/fp4816.h>

#define KI_FROM_TN(tn) (1/tn)
#define TN_FROM_KI(ki) (1/ki)

struct _pi_float
{
	float	limit_min,
			limit_max;
	float	kp,
			ki;
	float	integral;
};
typedef struct _pi_float PI_FLOAT;

static inline float pi_float_cycle(PI_FLOAT *pi,float value)
{
	pi->integral += pi->ki * value;
	if (pi->integral > pi->limit_max)
		pi->integral = pi->limit_max;
	if (pi->integral < pi->limit_min)
		pi->integral = pi->limit_min;
	return pi->integral + (pi->kp * value);
};

struct _pi_fp248
{
	fp248_t	limit_min,
			limit_max;
	fp248_t	kp,
			ki;
	fp248_t	integral;
};
typedef struct _pi_fp248 PI_FP248;

static inline fp248_t pi_fp248_cycle(PI_FP248 *pi,fp248_t value)
{
	pi->integral += fp248_mul(pi->ki, value);
	if (pi->integral > pi->limit_max)
		pi->integral = pi->limit_max;
	if (pi->integral < pi->limit_min)
		pi->integral = pi->limit_min;
	return pi->integral + fp248_mul(pi->kp, value);
};


struct _pi_fp4816
{
	fp4816_t	limit_min,
				limit_max;
	fp4816_t	kp,
				ki;
	fp4816_t	integral;
};
typedef struct _pi_fp4816 PI_FP4816;

static inline fp4816_t pi_fp4816_cycle(PI_FP4816 *pi,fp4816_t value)
{
	pi->integral += fp4816_mul(pi->ki, value);
	if (pi->integral > pi->limit_max)
		pi->integral = pi->limit_max;
	if (pi->integral < pi->limit_min)
		pi->integral = pi->limit_min;
	return pi->integral + fp4816_mul(pi->kp, value);
};