/*
 * agc_mprf_att_prediction.cpp
 *
 */

#include "agc_mprf_att_prediction.h"

#include <string.h>

void AGC_att_prediction_initialize(agc_att_prediction_state_t& state, float att_est, float att_est_dot, float alpha, float beta)
{
	memset(&state, 0, sizeof state);

	state.ATT_EST_DOT_k_1=att_est_dot;
	state.ATT_EST_k_1=att_est;

	state.ATT_EST_DOT_k=att_est_dot;
	state.ATT_EST_k=att_est;

	state.ALPHA=alpha;
	state.BETA=beta;
}

float AGC_att_prediction(float ATT_k, float dt, agc_att_prediction_state_t& state)
{
	const float A_MAX=63.75;
	const float A_MIN=0;

	//-----------------------------------------------------------------------
	// 1.1.6	Next Batch Prediction
	//-----------------------------------------------------------------------

	state.ATT_EST_k_1 = state.ATT_EST_k;			// position
	state.ATT_EST_DOT_k_1 = state.ATT_EST_DOT_k;	// velocity

	float Tb_k = dt;

	state.DELTA_T=Tb_k;

	state.ATT_PRED_k = state.ATT_EST_k_1 + Tb_k * state.ATT_EST_DOT_k_1;

	state.DELTA_ATT = ATT_k;		// ATT_k computed at 1.1.5 = prediction error

	float ALPHA=state.ALPHA;
	float BETA=state.BETA;

	// Compute next batch values
	state.ATT_EST_k = state.ATT_PRED_k + ALPHA * state.DELTA_ATT;
	state.ATT_EST_DOT_k = state.ATT_EST_DOT_k_1 + BETA * (state.DELTA_ATT / Tb_k);

	state.ATT_PRED = state.ATT_EST_k + Tb_k * state.ATT_EST_DOT_k;

	state.ATT_PROX=state.ATT_PRED;

	//-----------------------------------------------------------------------
	// 1.1.7	Attenuation Limitation
	//-----------------------------------------------------------------------

	if (state.ATT_PROX>A_MAX)
	{
		state.ATT_PROX = A_MAX;
		state.ATT_EST_k = A_MAX;
		state.ATT_EST_DOT_k = 0.0;
	}
	if (state.ATT_PROX < A_MIN)
	{
		state.ATT_PROX = A_MIN;
		state.ATT_EST_k = A_MIN;
		state.ATT_EST_DOT_k = 0.0;
		state.DELTA_ATT = 0.0;
	}

	return state.ATT_PROX;
}




