#include <math.h>

#include "filt.h"

float FilterGPS::Limit(float Value, float Min, float Max)
{
	if (Value > Max) return Max;
	if (Value < Min) return Min;
	return Value;
}

float FilterGPS::Minimum(float Value1, float Value2)
{
	if (Value1 < Value2) return Value1;
	return Value2;
}

void FilterGPS::Update(bool valid, float gps, float acc)
{
	const float ga = 9.80665f;

	Delay.Acc[Delay.Index] = acc;
	Delay.Pos[Delay.Index] = Position.Value;
	Delay.Spd[Delay.Index++] = Speed.Value;
	if (Delay.Index >= Delay.Size) Delay.Index = 0;

	float prev = Delay.Acc[Delay.Index];
	float shift = gps - Delay.Pos[Delay.Index];

	float coef_a = 1;
	if (Position.EnAcc) coef_a = Position.Acc / fabsf(prev);
	float coef_p = 1;
	if (Position.EnGPS) coef_p = powf(fabsf(shift * Position.Length), Position.Width);

	float coef1 = Minimum(coef_a, coef_p);
	coef1 = Limit(coef1, Position.Min, Position.Max);

	Speed.Count++;
	if (valid)
	{
		Speed.ValueGPS = (gps - Speed.Last_GPS) * Freq / Speed.Count;
		Speed.Last_GPS = gps;
		Speed.ValueAccGPS= (Speed.ValueGPS- Speed.Speed_GPS) * Freq / Speed.Count;
		Speed.Speed_GPS = Speed.ValueGPS;
		Speed.Count = 0;
	}

	for (int a = 0; a < Delay.Size; a++) Delay.Pos[a] += shift * coef1;
	Position.Value += shift * coef1 + (Speed.Value / Freq) * (1 - coef1);

	// SPEED

	shift = Speed.ValueGPS - Delay.Spd[Delay.Index];

	coef_a = 1;
	if (Speed.EnAcc) coef_a = Speed.Acc / fabsf(prev);
	float coef_g = 1;
	if (Speed.EnAcc) coef_g = Speed.AccGPS / fabsf(Speed.ValueAccGPS);
	float coef_s = 1;
	if (Speed.EnGPS) coef_s = powf(fabsf(shift * Speed.Length), Speed.Width);

	float coef2 = Minimum(coef_a, Minimum(coef_s, coef_g));
	coef2 = Limit(coef2, Speed.Min, Speed.Max);

	for (int a = 0; a < Delay.Size; a++) Delay.Spd[a] += shift * coef2;
	Speed.Value += shift * coef2 + (acc * ga / Freq) * (1 - coef2);
}