#include <fixpoint/fixpoint.h>

#include <stdlib.h>
#include <string.h>


fp4816_pid_t*	pid4816_create	(void){
	fp4816_pid_t *pid = malloc( sizeof( fp4816_pid_t) );
	
	if (!pid)
		return NULL;
		
	memset(pid, 0x00, sizeof(fp4816_pid_t));
		
	return pid;
};

int				pid4816_set		(fp4816_pid_t *pid,fp4816_t Kp,fp4816_t Ki,fp4816_t Kd){
	if (!pid)
		return -ENULLPTR;
		
	pid->kp = Kp;
	pid->ki = Ki;
	pid->kd = Kd;
	
	return ESUCCESS;
};

int				pid4816_set_kp	(fp4816_pid_t *pid,fp4816_t Kp){
	if (!pid)
		return -ENULLPTR;
		
	pid->kp = Kp;

	return ESUCCESS;
};

int				pid4816_set_ki	(fp4816_pid_t *pid,fp4816_t Ki){
	if (!pid)
		return -ENULLPTR;
		
	pid->ki = Ki;
	
	return ESUCCESS;
};
int				pid4816_set_kd	(fp4816_pid_t *pid,fp4816_t Kd){
	if (!pid)
		return -ENULLPTR;
		
	pid->kd = Kd;
	
	return ESUCCESS;
};

int				pid4816_get_kp	(fp4816_pid_t *pid,fp4816_t *Kp){
	if (!pid)
		return -ENULLPTR;
		
	*Kp = pid->kp;
	
	return ESUCCESS;
};
int				pid4816_get_ki	(fp4816_pid_t *pid,fp4816_t *Ki){
	if (!pid)
		return -ENULLPTR;
		
	*Ki = pid->ki;
	
	return ESUCCESS;
};
int				pid4816_get_kd	(fp4816_pid_t *pid,fp4816_t *Kd){
	if (!pid)
		return -ENULLPTR;
		
	*Kd = pid->kd;
	
	return ESUCCESS;
};

int pid4816_set_limits(fp4816_pid_t *pid,fp4816_t *omin,fp4816_t *omax,fp4816_t *imin,fp4816_t *imax){
	if (!pid)
		return -ENULLPTR;
		
	if (omin)
		pid->lim_omin = *omin;
	if (omax)
		pid->lim_omax = *omax;
	if (imin)
		pid->lim_imin = *imin;
	if (imax)
		pid->lim_imax = *imax;
		
	return ESUCCESS;
};

int pid4816_get_limits(fp4816_pid_t *pid,fp4816_t *omin,fp4816_t *omax,fp4816_t *imin,fp4816_t *imax){
	if (!pid)
		return -ENULLPTR;
		
	if (omin)
		*omin = pid->lim_omin;
	if (omax)
		*omax = pid->lim_omax;
	if (imin)
		*imin = pid->lim_imin;
	if (imax)
		*imax = pid->lim_imax;
		
	return ESUCCESS;	
};



int				pid4816_cycle	(fp4816_pid_t *pid,fp4816_t value){
	fp4816_t	t;
	
	if (!pid)
		return -ENULLPTR;
		
	pid->integral += fp4816_mul(pid->ki, value);
	if (pid->integral < pid->lim_imin)
		pid->integral = pid->lim_imin;
	if (pid->integral > pid->lim_imax)
		pid->integral = pid->lim_imax;
	
	t = pid->integral + fp4816_mul( pid->kp, value ) + fp4816_mul( pid->kd, ( value - pid->last_value ) );
	pid->last_value = value;
	
	if (t < pid->lim_omin)
		t = pid->lim_omin;
	if (t > pid->lim_omax)
		t = pid->lim_omax;
	
	pid->out = t;
	
	return ESUCCESS;
};


/*
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);
};
 * 
*/