#include "ramp_ctrl.h"

#define RAMP_INTVAL 50 //ms
#define min_step 0.5f

void ramp_timer_handler(shark_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) {
	shark_timer_cancel(&ramp->timer);
	ramp->start_point = 0;
	ramp->step_ms = RAMP_INTVAL;
	ramp->interpolation = 0;
	ramp->final_point = 0;
	ramp->duration_ms = 0;
	ramp->step_val = 0;
}

void ramp_set_points(ramp_t *ramp, float start, float target) {
	ramp->start_point = start;
	ramp->final_point = target;
	ramp->interpolation = target;
}


void ramp_set_target(ramp_t *ramp, float target) {
	ramp->final_point = target;
}

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


void ramp_set_step_value(ramp_t *ramp, float step) {
	ramp->step_val = step;
}

void ramp_set_step_time(ramp_t *ramp, u32 ms) {
	ramp->step_ms = ms;
}

float ramp_get_interpolation(ramp_t *ramp) {
	return ramp->interpolation;
}

void ramp_calc_step(ramp_t *ramp) {
	float delta = fabs(ramp->final_point - ramp->start_point);
	float steps = delta/(ramp->duration_ms / ramp->step_ms);
	if (steps < min_step) {
		u32 step_ms = min_step * ramp->duration_ms / delta;
		ramp->step_val = min_step;
		ramp->step_ms = step_ms;
	}else {
		ramp->step_val = steps;
	}
	if (ramp->final_point < ramp->start_point) {
		ramp->step_val = - ramp->step_val;
	}
}


void ramp_set_target_duration(ramp_t *ramp, float start, float final, u32 duration_ms) {
	shark_timer_cancel(&ramp->timer);
	ramp->start_point = start;
	ramp->final_point = final;
	ramp->duration_ms = duration_ms;
	
	if (duration_ms == 0) {
		ramp->step_val = (final - ramp->start_point);
		ramp->interpolation = final;
	}else {
		ramp_calc_step(ramp);		
		ramp_exc(ramp);
	}
}



void ramp_exc(ramp_t *ramp){
	if (shark_timer_stopped(&ramp->timer)) {
		shark_timer_post(&ramp->timer, ramp->step_ms);
	}
}


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

void ramp_timer_handler(shark_timer_t *timer) {
	ramp_t *ramp = (ramp_t *)timer;
	float target = ramp->interpolation + ramp->step_val;
	if (ramp->step_val < 0) {
		if (target < ramp->final_point) {
			target = ramp->final_point;
		}
	}else {
		if (target > ramp->final_point) {
			target = ramp->final_point;
		}
	}
	ramp->interpolation = target;
	if (target != ramp->final_point) {
		shark_timer_post(&ramp->timer, ramp->step_ms);
	}else {
		shark_timer_cancel(&ramp->timer);
	}
}