#include <adc_tools.h>
#include <break_regul.h>
#include <edrk_main.h>
#include <params_pwm24.h>
#include <PWMTools.h>

#include "IQmathLib.h"
#include "pid_reg3.h"
#include "DSP281x_Examples.h"   // DSP281x Examples Include File
#include "DSP281x_Device.h"     // DSP281x Headerfile Include File


#pragma DATA_SECTION(break_result_1,".fast_vars");
_iq break_result_1 = 0;

#pragma DATA_SECTION(break_result_2,".fast_vars");
_iq break_result_2 = 0;

#pragma DATA_SECTION(break_result_3,".fast_vars");
_iq break_result_3 = 0;

#pragma DATA_SECTION(break_result_4,".fast_vars");
_iq break_result_4 = 0;

#pragma DATA_SECTION(koef_Krecup,".fast_vars");
_iq koef_Krecup = 0;

#pragma DATA_SECTION(koef_Min_recup,".fast_vars");
_iq koef_Min_recup = 0;

BREAK_REGUL break_reg = BREAK_REGUL_DEFAULTS;

_iq calc_recup(_iq voltage, _iq recup_level, _iq max_recup_level);
//#pragma DATA_SECTION(pidF,".fast_vars");
//PIDREG3 pidF=PIDREG3_DEFAULTS;

void break_resistor_managment_init(unsigned int freq_pwm)
{
	volatile float percent;//, percent2;
	_iq iq_percent1 = 0, iq_percent2 = 0;// iq_percent3 = 0, iq_percent4 = 0;

	break_reg.pwm_tics = (FREQ_INTERNAL_GENERATOR_XILINX_TMS / freq_pwm );
	break_reg.pwm_minimal_tics = (float)DEF_PERIOD_MIN_MKS*1000.0*FREQ_INTERNAL_GENERATOR_XILINX_TMS/1000000000.0;
	break_reg.pwm_max_tics = break_reg.pwm_tics - break_reg.pwm_minimal_tics;


	//Минимальный коэффициент модул-ции тормозных ключей. Количество тиков в IQ15.
	percent = (break_reg.pwm_max_tics * RECUP_PERCENT_MIN/100.0) * 32768.0;//Здась и далее вычислению в _IQ15
	iq_percent1 = percent;
	
	//Максимальный коэффициент модул-ции тормозных ключей. Количество тиков в IQ15.
	percent = (break_reg.pwm_max_tics * RECUP_PERCENT_MAX/100.0) * 32768.0;
	iq_percent2 = percent;

	//Коэффициент чтобы перевести напр-жению в ЗПТ во времю открытию ключей
	koef_Krecup = _IQdiv((iq_percent1 - iq_percent2),(IQ_RECUP_LEVEL_MIN - IQ_RECUP_LEVEL_MAX));
	koef_Min_recup = iq_percent1;	//Минимальное времю открытию тормозных ключей в тиках
}


//int break_resistor_managment_calc(int flag_on_off)
#pragma CODE_SECTION(break_resistor_managment_calc,".fast_run");
void break_resistor_managment_calc()
{
	static unsigned int break_counter = 0;//MAX_BREAK_IMPULSE + 1;

	if(edrk.Discharge && edrk.from_shema_filter.bits.QTV_ON_OFF==0
	        && edrk.from_shema_filter.bits.UMP_ON_OFF==0)// && edrk.to_shema.bits.QTV_ON == 0)
	{
		if (break_counter < MAX_BREAK_IMPULSE)
		{
			break_counter++;


			if ((filter.iqU_1_fast > BREAK_INSENSITIVE_LEVEL_MIN)
			        || edrk.ManualDischarge || edrk.NoDetectUZeroDischarge)
			{
			    if (edrk.Obmotka1 == 0)
			        break_result_1 = 300;
			    else
			        break_result_1 = 0;
			}
			else
			{
				break_result_1 = 0;
			}

			if ((filter.iqU_2_fast > BREAK_INSENSITIVE_LEVEL_MIN)
			        || edrk.ManualDischarge || edrk.NoDetectUZeroDischarge )
			{
                if (edrk.Obmotka2 == 0)
                    break_result_2 = 300;
                else
                    break_result_2 = 0;
			}
			else
			{
				break_result_2 = 0;
			}

//			if (filter.iqU_3_fast < BREAK_INSENSITIVE_LEVEL_MIN)
//			{
//				break_result_3 = 0;
//			}
//			else
//			{
//				break_result_3 = 300;
////				break_result_3 = calc_stop_run_recup(filter.iqU_3_fast);
//			}
//
//			if (filter.iqU_4_fast < BREAK_INSENSITIVE_LEVEL_MIN)
//			{
//				break_result_4 = 0;
//			}
//			else
//			{
//				break_result_4 = 300;
////				break_result_4 = calc_stop_run_recup(filter.iqU_4_fast);
//			}

			if((!break_result_1 && !break_result_2 && !break_result_3 && !break_result_4)
					|| (break_counter >= (MAX_BREAK_IMPULSE - 1)))
			{
				edrk.Discharge = 0;
				break_counter = 0;
			}
		}
		else
		{
			break_result_1 = 0;
			break_result_2 = 0;
			break_result_3 = 0;
			break_result_4 = 0;
			break_counter = 0;
			edrk.Discharge = 0;
		}
	}
	
}

#pragma CODE_SECTION(calc_recup,".fast_run");
_iq calc_recup(_iq voltage, _iq recup_level, _iq max_recup_level)
{
	if (voltage <= recup_level)
	{
		return 0;
	}
	else
	{
		voltage = _IQsat(voltage,max_recup_level,recup_level);
		voltage = _IQmpy(koef_Krecup,voltage - recup_level) + koef_Min_recup;
		return voltage >> 15;
	}
}

#pragma CODE_SECTION(break_resistor_recup_calc,".fast_run");
void break_resistor_recup_calc(_iq Uzpt_nominal)
{
	_iq Uzpt_start_recup = Uzpt_nominal + IQ_DELTA_U_START_RECUP;
	_iq Uzpt_max_open_break_keys = Uzpt_nominal + IQ_DELTA_U_MAX_OPEN_BREAK_KEYS;

    if (/*edrk.Go &&*/ (edrk.SborFinishOk || edrk.Status_Ready.bits.ImitationReady2) )
	{
		break_result_1 = calc_recup(filter.iqU_1_fast, Uzpt_start_recup, Uzpt_max_open_break_keys);
		break_result_2 = calc_recup(filter.iqU_2_fast, Uzpt_start_recup, Uzpt_max_open_break_keys);
	}
}

#pragma CODE_SECTION(break_resistor_managment_update,".fast_run");
void break_resistor_managment_update()
{
	if (break_result_1 < break_reg.pwm_minimal_tics)
		break_result_1 = 0;
		
	if (break_result_2 < break_reg.pwm_minimal_tics)
		break_result_2 = 0;
		
//	if (break_result_3 < break_reg.pwm_minimal_tics)
//		break_result_3 = 0;
//
//	if (break_result_4 < break_reg.pwm_minimal_tics)
//		break_result_4 = 0;

	if (break_result_1 > break_reg.pwm_max_tics)
		break_result_1 = break_reg.pwm_max_tics;

	if (break_result_2 > break_reg.pwm_max_tics)
		break_result_2 = break_reg.pwm_max_tics;

//	if (break_result_3 > break_reg.pwm_max_tics)
//		break_result_3 = break_reg.pwm_max_tics;
//
//	if (break_result_4 > break_reg.pwm_max_tics)
//		break_result_4 = break_reg.pwm_max_tics;
}

//#pragma CODE_SECTION(break_resistor_set_closed,".fast_run");
void break_resistor_set_closed()
{
	break_result_1 = 0;
	break_result_2 = 0;
	break_result_3 = 0;
	break_result_4 = 0;
}