#include "ramp_ctrl.h"

#define RAMP_INTVAL 5 //ms
void ramp_timer_handler(timer_t *timer);

void ramp_ctrl_init(ramp_t *ramp){
	ramp_clear(ramp);
	ramp->timer.handler = ramp_timer_handler;
}

void ramp_clear(ramp_t *ramp) {
	timer_cancel(&ramp->timer);
	ramp->start_point = 0;
	ramp->target = 0;
	ramp->final_point = 0;
	ramp->duration_ms = 0;
	ramp->steps = 0;
}

void ramp_set_target(ramp_t *ramp, float start, float final, u32 duration_ms) {
	timer_cancel(&ramp->timer);
	ramp->start_point = start;
	ramp->final_point = final;
	ramp->duration_ms = duration_ms;
	ramp->steps = (final - ramp->start_point) / (duration_ms / RAMP_INTVAL);
}

void ramp_exc(ramp_t *ramp){
	timer_post(&ramp->timer, RAMP_INTVAL);
}

float ramp_get_target(ramp_t *ramp){
	return ramp->target;
}

bool ramp_complete(ramp_t *ramp) {
	return ramp->target == ramp->final_point;
}

void ramp_timer_handler(timer_t *timer) {
	ramp_t *ramp = (ramp_t *)timer;
	float target = ramp->target + ramp->steps;
	if (abs(target) > abs(ramp->final_point)) {
		target = ramp->final_point;
	}
	ramp->target = target;
	if (target != ramp->final_point) {
		timer_post(&ramp->timer, RAMP_INTVAL);
	}else {
		timer_cancel(&ramp->timer);
	}
}