#include "IQmathLib.h"
#include "efficiency_compensation.h"


static int DetectCommandRegion(_iq *power);

/***********************************************************/
/**
*
*Increases the power demand depending on the machine efficiency
*
*@param p_zad - pointer to demanded power
*@param i_q - pointer to value of active current
*
*@return return value throw the pointer
*
*************************************************************/
void EfficiencyCompensation(_iq *p_zad, _iq *i_q)
{
	static _iq efficiency_nominal = 16139681L; //96.2%
	static _iq i_q_nom_levels[] = {3635063L, 4395630L, 4977240L, 5536481L, 6011835L, 6487190L,
			6934582L, 7381975L, 7751073L, 8220835L};

	/**
	 * Coefficients of a square trinomial calculating the correction
	 */
	static _iq a0[10] = {17585441L, 17401328L, 17206712L, 17060750L, 17074172L, 16904722L,
			17129537L, 17698285L, 17188257L, 17740228L};
	static _iq a1[10] = {-2030043L, -1358954L, -905969L, -587202L, -553648L, -318767L,
			-536870L, -1224736L, -570425L, -1224736L};
	static _iq a2[10] = {-385875L, -385875L, -385875L, -402653L, -335544L,
			 -348966L, -234881L, 0L, -167772L, 50331L};
	_iq efficiency, p_local;
	_iq i_q_abs;
	int region = 0;
	region = DetectCommandRegion(p_zad);
	efficiency = efficiency_nominal;
	i_q_abs = _IQabs(*i_q);
	p_local = *p_zad;

	if (i_q_abs <= i_q_nom_levels[region]) {
		*p_zad = _IQdiv(*p_zad, efficiency_nominal);
		return;
	}
	 /**
	  * This is because of all electric values are normalized by 3000,
	  * but in a model in this place the current is normalized by 1000.
	  */
	i_q_abs = i_q_abs * 3;
	efficiency = _IQmpy(i_q_abs, i_q_abs);
	efficiency = a0[region] + _IQmpy(a1[region], i_q_abs) + _IQmpy(a2[region], efficiency);
	p_local = _IQdiv(p_local, efficiency);
	*p_zad = p_local;
}



#define BOUND_1p5_MWT 2796202L
#define BOUND_2p5_MWT 4660337L
#define BOUND_3p5_MWT 6524472L
#define BOUND_4p5_MWT 8388608L
#define BOUND_6p5_MWT 12116878L
#define BOUND_7p5_MWT 13981013L
#define BOUND_8p5_MWT 15845148L
#define BOUND_5p5_MWT 10252743L
#define BOUND_9p5_MWT 17709283L

/**************************************************************/
/**
 * Detects command mode:
 * 0 corresponds to 1 MWt
 * 1 - 2 MWt
 * ...
 * 9 - 10 MWt
 *
 * @param power - pointer to demanded power in watts
 *
 * @return corresponding mode
 */
int DetectCommandRegion(_iq *power)
{
	int region = 0;
    if (*power < BOUND_1p5_MWT)
	{
		region = 0;
    } else if (*power < BOUND_2p5_MWT) {
		region = 1;
    } else if (*power < BOUND_3p5_MWT) {
		region = 2;
    } else if (*power < BOUND_4p5_MWT) {
		region = 3;
    } else if (*power < BOUND_5p5_MWT) {
		region = 4;
    } else if (*power < BOUND_6p5_MWT) {
		region = 5;
    } else if (*power < BOUND_7p5_MWT) {
		region = 6;
    } else if (*power < BOUND_8p5_MWT) {
		region = 7;
    } else if (*power < BOUND_9p5_MWT) {
		region = 8;
	} else {
		region = 9;
	}
	
	return region;
}