#include "functions.h"
#include "defines.h"
#define PPV_Encoder 2000
#define V_offset 0.3

//GENERAR VELOCIDAD DE REFERENCIA
float vel_reference(float vel_alcanzada, float acel, float dt){
		
	if(vel_alcanzada>V_offset){
		
		vel_alcanzada = vel_alcanzada - acel*dt;
	}
	if(vel_alcanzada<=V_offset && vel_alcanzada>=-V_offset){
		vel_alcanzada = 0;
	}
	if(vel_alcanzada<-V_offset){
		
		vel_alcanzada = vel_alcanzada + acel*dt;
	}
	
	return vel_alcanzada;

}

// FUNCIÓN PARA ESTIMAR LA VELOCIDAD
float speed_estimation(float pos_actual, float pos_anterior, float dt){

	// PPV_Encoder(Pulsos por vuelta) -> 2000 Pulsos -> 2Pi -> 1 vuelta
	//	   (					rad						 )/(  s	 )
	return ((pos_actual-pos_anterior)/dt);

}


// Función para pasar de pulsos por revolución a radianes
// tambien se tiene en cuenta el auto-reload
float PPR_to_Rad(int32_t encoder, int32_t encoder_previous){
	float aux;
	
	if(encoder-encoder_previous > 65500){
		aux = ((float)(encoder - 65536)*2*Pi)/PPV_Encoder;
		return aux;
	}
	
	else if(encoder-encoder_previous < -65500){
		aux = ((float)(encoder + 65536)*2*Pi)/PPV_Encoder;
		return aux;
	
	}
	else{
		return (float)((encoder*2.0*Pi)/PPV_Encoder);
	}
}

// Acomodor la señal de salida para circuito auxilar
int32_t escalado(float value)
{
	// DAC = 1743 -> tensión al epos= 2.5V //1570
	float scale = 697.2;
	int32_t max_value = 3486;
	int32_t min_value = 0;
	
	int32_t result;
	result = value*scale;
	
	if (result > max_value)
	{
		result = max_value;
	}
	else if (result < min_value)
	{
		result = min_value;
	}
		
	return result;
	
}