#include "zf_common_headfile.h"
#include "zf_common_debug.h"

#include "DOG_filter.h"

// ˲
float karman_filter(struct DOG_KARMAN_FILTER* this, float input){
	// ԤЭ̣kʱϵͳЭ = k-1ʱ̵ϵͳЭ + Э
    this -> now_p = this -> last_p + this -> Kq;
    // 淽̣ = kʱϵͳЭ / kʱϵͳЭ + ۲Э
    this -> Kg = this -> now_p / (this -> now_p + this -> Kr);
    // ֵ̣kʱ״ֵ̬ = ״̬Ԥֵ +  * ֵ - ״̬Ԥֵ
    this -> value = this -> value + this -> Kg * (input - this -> value);//ΪһεԤֵһεֵ
    // Э: εϵͳЭ klm->LastP Ϊһ׼
    this -> last_p = (1-this -> Kg) * this -> now_p;
	
	return (this -> value);
}

// 캯
void karman(struct DOG_KARMAN_FILTER* this, float Kq, float Kr){
	/* Ա */
	this -> Kq = Kq;
	this -> Kr = Kr;
	
	this -> last_p = 0;
	this -> now_p = 0;
	this -> value = 0;
	this -> Kg = 0;
	
	/* Ա */
	this -> karman_filter = karman_filter;
}
// 
void _karman(struct DOG_KARMAN_FILTER* this){
	this -> Kq = 0;
	this -> Kr = 0;
	
	this -> last_p = 0;
	this -> now_p = 0;
	this -> value = 0;
	this -> Kg = 0;
}

// ͨ˲
float lowpass_filter(struct DOG_LOWPASS_FILTER* this, float input){
	this -> new_value = input;		
	this -> new_value = this -> K*this -> new_value+(1-this -> K)*this -> old_value;
	this -> old_value = this -> new_value;
	return (this -> new_value);
}

// 캯
void lowpass(struct DOG_LOWPASS_FILTER* this, float K){
	/* Ա */
	this -> K = K;
	
	this -> old_value = 0;
	this -> new_value = 0;

	/* Ա */
	this -> lowpass_filter = lowpass_filter;
}

// 
void _lowpass(struct DOG_LOWPASS_FILTER* this){
	this -> K = 0;
	this -> old_value = 0;
	this -> new_value = 0;
}