#include <iostream>
#include <cmath>

using namespace std;

#define DEF_RATE_LIMIT 100

int sgn(double d){
	if(d > 0)return 1;
	else return -1;
}

// Control surface class
// deflection = parameter*(input pulse width - neutral pulse width), with rate limitation
// rate limit = max deg of change / time step
// no saturation
class con_surface{
private:
	double lin_parameter, old_deflection, rate_limit;
	long int neutral_pulse;
public:
	con_surface(double lp, long int neutral, double rl = DEF_RATE_LIMIT){
		lin_parameter = lp, neutral_pulse = neutral, rate_limit = rl, old_deflection = 0.0;
	};
	con_surface(long int up_pulse, long int down_pulse, long int n_pulse,
							double up_deflection, double down_deflection, double rl = DEF_RATE_LIMIT);
	double deflection(long int pulse_width);
};

con_surface::	con_surface(long int up_pulse, long int down_pulse, long int n_pulse,
													double up_deflection, double down_deflection, double rl){
	neutral_pulse = n_pulse;
	lin_parameter = (up_deflection*(up_pulse-neutral_pulse) + down_deflection*(down_pulse-neutral_pulse))
	    /(pow((double)up_pulse-neutral_pulse,2)+pow((double)down_pulse-neutral_pulse,2));
	rate_limit = rl, old_deflection = 0.0;
}

double con_surface::deflection(long int pulse_width){

	double virtual_deflection = lin_parameter*(double)(pulse_width-neutral_pulse);
	if(fabs(virtual_deflection-old_deflection) > rate_limit){
		virtual_deflection = old_deflection + sgn(virtual_deflection - old_deflection) * rate_limit;
	}
	old_deflection = virtual_deflection;
	return virtual_deflection;
}

int main(void){

	con_surface cs(3868, 2387, 3047, -28.1, 25.9, 2);

	for(int i=0;i<20; i++){
		cout << cs.deflection(3200) << endl;
	}
	return 0;
}