#include "icd.h"
#include "filter.h"
#include "parse.h"
#include "delay.h"
#if MK_TYPE==F412
	#include <math.h>
#endif

extern TIM_HandleTypeDef htim11;
extern TIM_HandleTypeDef htim14;

icd_str ICD;
extern adc_struct adc_str;//структура ацп
void init_icd(icd_str * icd_str)
{
	icd_str->work_set_mode = Monitor;

	icd_str->search_set_mode = Mode_search_BASE; //режим
	icd_str->search_now_mode = Mode_start_UP; //режим
	icd_str->filter_on = true;//фильтр активен

	icd_str->active_ch = 2;//используемый канал
	icd_str->sd_card = false;//использование SD карты
	icd_str->dc_cut = true;//вычитание постоянной составляющей

	//минимальный и максимальный пороги с плав. точкой и инт в десятых миливольта
	icd_str->min_tres = MIN_TRES;
	icd_str->max_tres = MAX_TRES;
	//	время на весь qrs rjvgktrc
	icd_str->max_time = (ALL_TIME_MS * DATA_RATE)/1000;
	//	время слепоты при включении
	icd_str->start_up_time = (START_UP_TIME_MS * DATA_RATE)/1000;
	//	время поиска в базовом режиме с минимальным порогом
	icd_str->base_time = (SEARCH_BASE_TIME_MS * DATA_RATE)/1000;
	//	Время в мс которое алгоритм ищет максимум
	icd_str->max_search_time = (SEARCH_MAX_TIME_MS * DATA_RATE)/1000;
	//	Время в мс нахождения в режиме поиска квадратов
	icd_str->square_time = (SQUARE_TIME_MS * DATA_RATE)/1000;
	//	Время в мс нахождения в режиме поиска треугольников
	icd_str->triangle_time = (TRIANGLE_TIME_MS * DATA_RATE)/1000;
	//	время каждой ступени в режиме треугольников
	icd_str->triangle_step_time = (TRIANGLE_STEP_TIME_MS * DATA_RATE)/1000;
	//	Время в мс которое длится режим стимуляции низким напряжением
	icd_str->lv_mode_time = (LV_TIME_MS * DATA_RATE)/1000;
	//	это во моногом эрзац.
	icd_str->lv_start_time = (LV_START_TIME_MS * DATA_RATE)/1000; //во сколько начали выдавать импульс
	icd_str->lv_stop_time = (LV_STOP_TIME_MS* DATA_RATE)/1000; //во сколько закончили выдавать импульс

	//	коэффициент на который умножается сигнал при переход в режим квадратов
	icd_str->square_coef = SQUARE_COEF;
	//	коэффициент на который умножается сигнал на каждой ступени в режиме треугольников
	icd_str->triangle_coef = TRIANGLE_COEF;

	icd_str->lv_polarity = lv_bipolar;//полярность низковольтной стимуляции
	icd_str->lv_mode = lv_mode_none;//режим низковольтной стимуляции

	icd_str->lv_shock_time = 13;//время импульса низковольной стимуляции 1-20(0,1-2мс) одна единица 0,1мс шаг 0.1мс
	icd_str->lv_relax_time = 14;//время стабилизации после удара низковольной стимуляции 0-20(0-20мс) одна единица 1мс шаг 1мс
	icd_str->lv_voltage = 35;//какое напряжение у импульсов 10-80(1,0-8,0В) одна единица 0,1В шаг 0,1В

		//bool BURST_active;//параметр вводящий нас в режим burst посылка пачки импульсов для вызова фибрилляции
	icd_str->burst_cnt = 33;//сколько импульсов в одной пачке 5-50 одна единица 1 импульс шаг 1 импульс
	icd_str->burst_voltage = 34;//какое напряжение у импульсов 10-80(1,0-8,0В) одна единица 0,1В шаг 0,1В
	icd_str->burst_period = 500;//период следования импульсов в мс 150-500мс одна единица 1мс шаг 10 мс на ползунке
	icd_str->burst_end_period = 300;//период следования импульсов в конце стимуляции в мс 150-500мс одна единица 1мс шаг 10 мс на ползунке


	//	тип последнего определённого события
	icd_str->last_QRS = Vsense;
	//	время между последним и предпоследним событием
	icd_str->last_period = 1000;
	icd_str->counter = 0;

	icd_str->last_RR_amp = MIN_TRES; // переменная в которой храним амплитуды начала QRS
	icd_str->last_RR_poz = 0; //переменная в котором храним положение начала QRS
	icd_str->last_RR_poz_rel = 0; //переменная в которой храним относительное положение начала QRS

	//	Счётчики нужны для алгоритма терапии Михаила
	icd_str->Vs_cnt = 0; //счётчик собственных сокращений сердца
	icd_str->Vn_cnt = 0; //счётчик собственных сокращений которые в шуме(не детектируется пока fixme)
	icd_str->Vp_cnt = 0; //счётчик принудительных стимуляций сердца низким напряжением(низковольтная стимуляция)

	icd_str->fibr_cnt = 0; //счётчик фибриляции при обнаружении короткого RR он растёт
	icd_str->fibr_max_tres = TERAPY_TRES; //порог счётчика фибриляций при котором начинаем терапию

	icd_str->LV_on = true; // реально стимулируем низковольтно
	icd_str->HV_on = false; // реально стимулируем низковольтно

	icd_str->ecg_rv_din_treshold = 0.0;	//динамический порог


	// тут уже корзины
	//порог периода фибриляции если период меньше, чем это число это фибриляция
	icd_str->fibr_tres		= (FIBR_PERIOD * DATA_RATE)/1000;
	//порог периода Тахикардии 2 если период меньше, чем это число это Тахикардия 2
	icd_str->tachy_2_tres 	= (TACHY_2_PERIOD * DATA_RATE)/1000;
	//порог периода Тахикардии 1 если период меньше, чем это число это Тахикардия 1
	icd_str->tachy_1_tres 	= (TACHY_1_PERIOD * DATA_RATE)/1000;

	icd_str->vs_cnt_last = 0;//сбрасываемый счётчик нормальных событий
	icd_str->fibr_cnt = 0; //счётчик фибриляции при обнаружении короткого RR он растёт
	icd_str->tachy_2_cnt = 0; //счётчик Тахикардии 2 при обнаружении короткого RR он растёт
	icd_str->tachy_1_cnt = 0; //счётчик Тахикардии 1 при обнаружении короткого RR он растёт
	icd_str->norm_cnt = 0;//счётчик нормальных сокращений подряд

	icd_str->vp_cnt_last = 0;//сбрасываемый счётчик нормальных событий

	icd_str->terapy_set = sub_off;//заданный режим терапии
	icd_str->terapy_now = sub_off;//текущий режим терапии

	icd_str->sub_mode = sub_off;//подрежим для индикации

	icd_str->hv_step_number = HV_STEP_NUM;//сколько шагов ВВ терапии?
	icd_str->hv_step_cnt = 0;// какой сейчас шаг?

	icd_str->hv_polarity = rv_neg_can_poz; //полярность стимуляции
	icd_str->hv_mode = hv_mode_fixed; //тип способа задания длительности импульса
	//время константы времени стимуляции
	icd_str->hv_phase_1_duration = 100;		// время в десятых мс приходящееся на 1 фазу 30-120(3-12мс) одна единица 0,1мс
	icd_str->hv_phase_2_duration = 70;		// время в десятых мс приходящееся на 2 фазу 20-100(2-10мс) одна единица 0,1мс
	icd_str->hv_switch_duration = 30;	// время в десятых мс приходящееся на переключение между фазами 10-30(1-3мс) одна единица 0,1мс
	//процентные параметры стимуляции
	icd_str->hv_switching_voltage = 40;//процент напряжения при котором происходит завешение 1 фазы при адаптивном режиме (20-80) одна единица 1%
	icd_str->hv_cutoff_voltage = 20;//процент напряжения при котором происходит завешение 2 фазы при адаптивном режиме (5-50) одна единица 1%

	icd_str->min_energy 	= MIN_ENERGY;
	icd_str->max_energy 	= MAX_ENERGY;

	icd_str->hv_blind_time	= HV_BLIND_TIME_MS;//время которое мы слепы после HV разряда

	icd_str->redet_num = REDET_BUF_LEN;	//какой длинны мы заполныем буффер при редетекции
	icd_str->redet_bad = REDET_BAD_MAX;	//сколько нам нужно плохих событий для запуска терапии

	icd_str->standby_timer  = STANDBY_TIME_MS;//сколько отдыхать после неудачного подавления фибриляции
	icd_str->spi_pot_set	= 6;
}

//статические функции
void first_phase(hv_polarity polarity);
void second_phase(hv_polarity polarity);


float my_abs(float a)
{
	return (a<0)? -a:a;
}

//	первичный заход в режим fixme обернуть в функцию
bool mode_start(icd_str * icd_str)
{
	if (icd_str->search_now_mode != icd_str->search_set_mode)
	{
		icd_str->search_now_mode = icd_str->search_set_mode;
		//				обнулили счётчик режима
		icd_str->mode_cnt = 0;
		return true;
	}
	return false;
}

void get_data(icd_str * icd_str,uint16_t input)
{
	float raw = ((float)input) * VOLT_COEF;//перевод в милливольты
//	icd_str->ecg_rv_filt = iir_50hz_2or(raw);
//	icd_str->ecg_rv_filt = iir_4060(raw);

	if(icd_str->filter_on)
		icd_str->ecg_rv_filt = iir_lp30(raw);
	else
		icd_str->ecg_rv_filt = raw;
	icd_str->ecg_rv_mov_av = (1 - MOV_AV_COEF)*icd_str->ecg_rv_mov_av + MOV_AV_COEF * icd_str->ecg_rv_filt;
	icd_str->ecg_rv_pos_sig = my_abs(icd_str->ecg_rv_filt-icd_str->ecg_rv_mov_av);
}

void get_data_max30003(icd_str * icd_str,int32_t input)
{
//перрвичная обработка сигналов
	float raw = ((float)input/256.0)* VOLT_COEF_MAX30003;//перевод в милливольты
	icd_str->ecg_rv_filt = iir_50hz_2or(raw);
	icd_str->ecg_rv_mov_av = (1 - MOV_AV_COEF)*icd_str->ecg_rv_mov_av + MOV_AV_COEF * icd_str->ecg_rv_filt;
	icd_str->ecg_rv_pos_sig = my_abs(icd_str->ecg_rv_filt-icd_str->ecg_rv_mov_av);
}

// проверка длительности определённого режима
// указваем какой режим выставить
bool check_mode_len(icd_str * icd_str,uint16_t base_time, search_mode mode_to_set)
{
	//	Проверка превышения времени на режим
	//if(icd_str->mode_cnt > base_time)//fixme так было раньше
	if(icd_str->mode_cnt > base_time || icd_str->last_RR_poz_rel > icd_str->max_time)
	{
		icd_str->mode_cnt = 0;
		//	перешли в режим низковольтной стимуляции
		icd_str->search_set_mode = mode_to_set;
		if(mode_to_set == Mode_stimulation_LV)
		{
			icd_str->vp_cnt_last++;
			//	период чсс равен времени с прошлого события
			icd_str->last_period = icd_str->last_RR_poz_rel;
			//	стёрли счётчик до прошлого события т.к. оно только что произошло
			icd_str->last_RR_poz_rel = 0;

			icd_str->last_QRS = Vpace;
			//	стёрли счётчик до прошлого события т.к. оно только что произошло
			icd_str->last_RR_poz_rel = 0;
			// инкримент счётчика Vpace событий
			icd_str->Vp_cnt++;
		}
		return true;
	}
	else
	return false;
}

// проверка пересечения порога
bool check_tres(icd_str * icd_str)
{
	//	Проверка превышения порога
	if((icd_str->ecg_rv_pos_sig >= icd_str->ecg_rv_din_treshold) && icd_str->work_set_mode != Monitor
			&& (icd_str->lv_mode == lv_mode_VVI || icd_str->lv_mode == lv_mode_none))//&& icd_str->lv_mode == lv_mode_VVI
	{
		//	перешли в режим поисков пиков
		icd_str->search_set_mode = Mode_search_MAX;
		//	событие нормальное
		icd_str->last_QRS = Vsense;
		// тетрарная операция находим не выходит ли за границы наш экг сигнал
		icd_str->last_RR_amp = icd_str->ecg_rv_pos_sig >= icd_str->max_tres ? icd_str->max_tres : icd_str->ecg_rv_pos_sig;
		icd_str->ecg_rv_din_treshold = icd_str->last_RR_amp;//fixme новое борьба с неправильным динамическим порогом
		//	период чсс равен времени с прошлого события
		icd_str->last_period = icd_str->last_RR_poz_rel;
		//	стёрли счётчик до прошлого события т.к. оно только что произошло
		icd_str->last_RR_poz_rel = 0;

		// инкримент счётчика Vsense событий
		icd_str->Vs_cnt++;
		icd_str->vs_cnt_last++;
		//сейчас сокращение сердца
		icd_str->rr_now = true;
		//	Если мы в нормальном режиме не терапии
		if(icd_str->terapy_now == Terapy_off)
		{
			icd_str->filt_period = moving_avarage(icd_str->last_period);
			if(icd_str->vs_cnt_last >= 10)
			{
				basket_alg(icd_str);
			}
			return true;
		}
		else if(icd_str->terapy_now == Terapy_fibr && icd_str->sub_mode == sub_redet)
		{
			//инкрементируем счётчик Vs
			icd_str->redet_cnt++;
			//если период меньше положенного
			if(icd_str->last_period< icd_str->tachy_1_tres)
			{
				icd_str->redet_bad_cnt++;
			}
		}
		return true;
	}
	else
	return false;
}
//алгоритм поиска QRS комплексов
//алгоритм поиска QRS комплексов
void search_alg(icd_str * icd_str)
{
	//инкрементируем глобальный счётчик
	icd_str->counter++;//
	icd_str->last_RR_poz_rel++;//новое fixme
	//сейчас не сокращение сердца
	icd_str->rr_now = false;
	switch (icd_str->search_set_mode)
	{
		case Mode_start_UP:
		{

		}
			break;
		case Mode_search_BASE:
		{
			//	первичный заход в режим
			if(mode_start(icd_str))
				icd_str->ecg_rv_din_treshold = icd_str->min_tres;

			//	проверка длительности режима функцией если превышена вывалиться из функции
			if(check_mode_len(icd_str,icd_str->base_time,Mode_stimulation_LV))
				return;
			//	проверка пересечения порога
			if(check_tres(icd_str))
				return;
			icd_str->mode_cnt++;
		}
			break;
		case Mode_search_SQUARE:
		{
			//	первичный заход в режим fixme обернуть в функцию
			if(mode_start(icd_str))
				icd_str->ecg_rv_din_treshold = icd_str->ecg_rv_din_treshold * icd_str->square_coef > icd_str->min_tres ?
										icd_str->ecg_rv_din_treshold * icd_str->square_coef : icd_str->min_tres;
			//	проверка длительности режима функцией если превышена вывалиться из функции
			if (check_mode_len(icd_str, icd_str->square_time, Mode_search_TRIANGLE))
				return;
			//	проверка пересечения порога
			if (check_tres(icd_str))
				return;
			icd_str->mode_cnt++;
		}
			break;
		case Mode_search_TRIANGLE:
		{
			//	первичный заход в режим
			mode_start(icd_str);
			//	если время шага прошло
			if((icd_str->mode_cnt % icd_str->triangle_step_time) == 0)
			{
				icd_str->ecg_rv_din_treshold = icd_str->ecg_rv_din_treshold * icd_str->triangle_coef > icd_str->min_tres ?
				icd_str->ecg_rv_din_treshold * icd_str->triangle_coef : icd_str->min_tres;
			}

			//	проверка длительности режима функцией если превышена вывалиться из функции
			if (check_mode_len(icd_str, icd_str->triangle_time, Mode_search_BASE))
				return;
			//	проверка пересечения порога
			if (check_tres(icd_str))
				return;
			icd_str->mode_cnt++;
		}
			break;
		case Mode_search_MAX://FIXME криво считаем!!!!!!!!!!!!!!!
		{
			//	первичный заход в режим
			if(mode_start(icd_str))
			{
			}
			//если сигнал выше найденного максимума
			if(icd_str->ecg_rv_pos_sig > icd_str->last_RR_amp)
			{
				//выше верхнего порога//fixme тут косяк
				//if(icd_str->ecg_rv_pos_sig > icd_str->ecg_rv_din_treshold) fixme вроде тут косяк был
				if(icd_str->ecg_rv_pos_sig > icd_str->max_tres)
				{
					icd_str->ecg_rv_din_treshold = icd_str->max_tres;
					icd_str->last_RR_amp = icd_str->max_tres;
				}
				//в норм границах
				else
				{
					icd_str->ecg_rv_din_treshold = icd_str->ecg_rv_pos_sig;
					icd_str->last_RR_amp = icd_str->ecg_rv_pos_sig;
				}
			}

			if(icd_str->mode_cnt > icd_str->max_search_time)
			{
				//	сбросили счётчик
				icd_str->mode_cnt = 0;
				//	перешли в режим поиска квадратный
				icd_str->search_set_mode = Mode_search_SQUARE;
			}
			icd_str->mode_cnt++;
		}
			break;
		case Mode_stimulation_LV:
		{
			//	первичный заход в режим
			if(mode_start(icd_str))
			{
				//сбросили параметр так как не было события сердца
				icd_str->norm_cnt = 0;
				//сейчас сокращение сердца
				icd_str->rr_now = true;
				if(icd_str->LV_on && icd_str->lv_mode != lv_mode_none)//тут должны производить низковольтную стимуляцию
				{
					icd_str->lv_start = true;//надо начинать стимуляцию
				}
			}

			// второе условие добавил для burst режима
			if(icd_str->mode_cnt > icd_str->lv_mode_time && icd_str->lv_start == false)
			{
				//	сбросили счётчик
				icd_str->mode_cnt = 0;//по идее не нужно всё равно сбросим при первичном заходе
				//	перешли в режим поиска квадратный
				icd_str->search_set_mode = Mode_search_SQUARE;
			}
			icd_str->mode_cnt++;
		}
		break;
		case Mode_stimulation_HV:
		{
			if(mode_start(icd_str))
			{
				//сбросили параметр так как не было события сердца
				icd_str->norm_cnt = 0;
			}
			icd_str->ecg_rv_pos_sig = 0.0;
		}
			break;
	}
}

uint16_t moving_avarage(uint16_t data)
{
	static uint16_t buf[10];
	static uint8_t b_pointer = 0;

	buf[b_pointer] = data;
	b_pointer++;
	if(b_pointer >= 10)
	{
		b_pointer = 0;
	}
	uint16_t sum = 0;
	for(int i =0; i<10; i++)
		sum += buf[i];
	return sum/10;
}

// Алгоритм корзин по идее должен вызываться при обнаружении новго события
// Едиственный смысл в нём это переход в другой режим не простого поиска а стимуляции
void basket_alg(icd_str * icd_str)
{
	if(icd_str->filt_period < icd_str->fibr_tres)
	{
		icd_str->fibr_cnt++;
		icd_str->norm_cnt = 0;
		if(icd_str->fibr_cnt > icd_str->fibr_max_tres)
		{
			//fixme по идее тоже обнуляем т.к. уже в жопном режиме
			icd_str->fibr_cnt 		= 0;
			icd_str->tachy_2_cnt	= 0;
			icd_str->tachy_1_cnt	= 0;
			//	перешли в режим терапии
			icd_str->terapy_set = Terapy_fibr;
		}
	}
	else if((icd_str->filt_period >= icd_str->fibr_tres)&&(icd_str->last_period < icd_str->tachy_2_tres))
	{
		icd_str->tachy_2_cnt++;
		icd_str->norm_cnt = 0;
	}
	else if((icd_str->filt_period >= icd_str->tachy_2_tres)&&(icd_str->last_period < icd_str->tachy_1_tres))
	{
		icd_str->tachy_1_cnt++;
		icd_str->norm_cnt = 0;
	}
	else
	{
		icd_str->norm_cnt++;
		//5 нормальных сокращений подряд можно сбросить корзины
		if(icd_str->norm_cnt >= 5)
		{
			icd_str->fibr_cnt 		= 0;
			icd_str->tachy_2_cnt	= 0;
			icd_str->tachy_1_cnt	= 0;
		}
	}
}


bool terapy_start(icd_str * icd_str)
{
	if (icd_str->terapy_now != icd_str->terapy_set)
	{
		icd_str->terapy_now = icd_str->terapy_set;
		return true;
	}
	return false;
}


void hv_pwm(bool state)
{
	if (state)		//Если хотим генерировать
	{
		#if (MK_TYPE == F412)
				TIM1->CCR4 = 60;
				//TIM1->CCR4 = 60;
		//			TIM3->CCR1 = 150;так было в начале
		//			TIM3->CCR1 = 675;
		#endif
	}
	else
	{
		#if (MK_TYPE == F412)
				TIM1->CCR4 = 0;
		#endif
	}
}

void hv_sound(uint32_t presc)
{
	if (presc == 0)		//без звука
	{
		TIM9->PSC = 100;
		TIM9->CCR2 = 0;
	}
	else		//задаём частоту
	{
		TIM9->PSC = presc;
		TIM9->CCR2 = 100;
	}

}


//функция заряда Конденсатора
void hv_charge(icd_str *icd_str)
{
	//отключение высоковольтной части
	hv_ll_control(z_state, z_state, z_state);
	//подали питание на высоковольтную часть
	hv_power(true);
	//выставили подрежим
	icd_str->sub_mode = sub_cap_ch;
	//плавный разряд выключен
	hv_discharge(false);
	//подали питание на плату только если не в режиме мониторинга
	hv_sound(CHARDE_TONE);		//включили звук заряда конденсатора
	if (icd_str->work_set_mode == Monitor)
	{

	}
	else
	{
		if (DANGER_HV && icd_str->HV_on)
		{
			hv_pwm(true);		//включили шим для заряда конденсатора
		}
		else//эмуляция
		{
		}
	}
	//заряд через транзистор
	//включили шим 32кГц выключили любой разряд
	//энергия заряда пропорциональна времени
	//40 джоулей в нашем случае 400 т.к у нас десятые доли джоулей можно задавать
	//набирает примерно за 15 сек 40 Дж при 560мкФ соответственно 2.667 дж в сек округлим до 2.7 потом поправить
	//т.к. энергия в десятых джоулях 40дж = 400 т=(400/10)*(1000/2.667) и если упростить, нам просто нужно умножать на 37
	//время для заряда до 40 дж = (E*1000/k)
	// 5дж наберутся за 1850 мс, что норм.
//если плата с новым мк
#if (CAP_VOLT_CTRL)
	{
		int i = 0;
		float voltage = sqrt(icd_str->now_energy * 0.2 / CAPACITY);
		uint16_t voltage_i = (uint16_t) (voltage * 10.2);//умножаем на 10.2 а не на 10 т.к. напряжение после выключения заряда слегка просаживается.
		while (i < 80)
		{
//			тут были попытки менять  в динамике заполнение шим, но никак не помогло.
//			if (adc_str.hv_volt > 3500)
//			{
//				TIM1->CCR4 = 50;
//			}
//			else if(adc_str.hv_volt > 5000)
//			{
//				TIM1->ARR = 300;
//			}
			i++;
			osDelay(icd_str->now_energy);
			if (adc_str.hv_volt > voltage_i)
				break;
		}
	}
#else
		osDelay(icd_str->now_energy*40);
	#endif
	//отключили шим
	hv_pwm(false);
	hv_sound(READY_TONE);		//включили  звук готовности к разряду
}


//функция резкого разряда
void hv_shock(icd_str * icd_str)
{
	icd_str->sub_mode = sub_shock;
	icd_str->search_set_mode = Mode_stimulation_HV;

	hv_sound(SHOCK_TONE);		//включили  звук разряда
	//включили режим слепоты
	osDelay(10);
	//подали питание на плату только если не в режиме мониторинга
	if(icd_str->work_set_mode != Monitor)//fixme
	{
		ll_bi_shock_param(icd_str);
	}
	else
	{

	}
	// ничего не анализируем сколько-то времени
	osDelay(icd_str->hv_blind_time);
	//отключили режим слепоты
	icd_str->search_set_mode = Mode_search_SQUARE;
	hv_sound(NO_TONE);		//включили  звук разряда
	hv_power(false);
}

//быстрое определение нужно ли бить током
bool quick_analyse(icd_str * icd_str)
{
	osDelay(icd_str->hv_blind_time);
	//сбросили счётчики редетекции
	//icd_str->Vs_cnt = 0;
	icd_str->redet_cnt = 0;
	icd_str->redet_bad_cnt = 0;

	icd_str->sub_mode = sub_redet;
	icd_str->vs_cnt_last = 0;//сбросили количество нормальных событий перед анализом
	icd_str->vp_cnt_last = 0;//сбросили количество НЕнормальных событий перед анализом

	//если у нас принудителная стимуляция ждём чуть-чуть и фигачим
	if(icd_str->work_set_mode == Force)
	{
		osDelay(FORCE_DELAY);
		return true;
	}

	//очистка всех событий из памяти
//	ожидание с периодом 10мс нет ли достаточного кол-ва событий
	// Тут по идее можно добавить счётчик времени вдруг Vs нет.
//	fixme раньше учитывали только Vs события
	while((icd_str->vs_cnt_last + icd_str->vp_cnt_last) < icd_str->redet_num)
	{
		//do nothing
		osDelay(10);
	}

	//проверка не превысили ли мы порог плохих сокращений
	if(icd_str->redet_bad_cnt > icd_str->redet_bad)
	{
		return true;
	}
	else
	{
		return false;
	}
}

void fibr_terapy(icd_str * icd_str)
{
	for(icd_str->hv_step_cnt = 0; icd_str->hv_step_cnt < icd_str->hv_step_number; )
	{
		icd_str->hv_step_cnt++;
		//расчёт энергии шага
		//icd_str->hv_step_energy = (icd_str->max_energy-icd_str->min_energy)/(icd_str->hv_step_number-1);
		icd_str->now_energy = icd_str->max_energy;
		//активировать терапию низковольтную если задана (у нас такого пока нет) fixme

		//затем производим зарядку конденсатора
		hv_charge(icd_str);
		osDelay(1000);//fixme для того чтобы было видно кончное напряжение.
		//ждём пока наберётся Х событий для анализа
		if(!quick_analyse(icd_str))//если всё ОК
		{
			hv_discharge(true);
			//сброс параметров и переход в режим
			icd_str->terapy_set = Terapy_off;
			//выходим нафиг из цикла
			icd_str->sub_mode = sub_off;
			icd_str->now_energy = 0;
			icd_str->hv_step_cnt = 0;
			hv_power(false);
			hv_sound(NO_TONE);		//включили  звук разряда
			return;
		}
		else
		{
			//должны ещё вырубить анализ с АЦП и пихать в него среднее чтоб скользящее нахер не улетело
			hv_shock(icd_str);
			//опять проверяем на нормальность цикла если всё ок возвращаемся в норм режим
			if(!quick_analyse(icd_str))//если всё ОК
			{
				hv_discharge(true);
				//сброс параметров и переход в режим
				icd_str->terapy_set = Terapy_off;
				//выходим нафиг из цикла
				icd_str->sub_mode = sub_off;
				icd_str->now_energy = 0;
				icd_str->hv_step_cnt = 0;
				hv_power(false);
				hv_sound(NO_TONE);		//включили  звук разряда
				return;
			}
		}
	}
	//если прошлись по всем шагам а ритм всё равно говно
	if(icd_str->hv_step_cnt >= icd_str->hv_step_number)
	{
		icd_str->sub_mode = sub_stim_fail;
		icd_str->now_energy = 0;
		icd_str->hv_step_cnt = 0;
		hv_discharge(icd_str);
		if(icd_str->work_set_mode == Force)
		{
			icd_str->work_set_mode = Monitor;//перешли в режим Мониторинга
		}
		else
		{
			osDelay(icd_str->standby_timer);//сброс всего и долгая зажержка
		}
		//сброс параметров и переход в режим
		icd_str->terapy_set = Terapy_off;
		icd_str->sub_mode = sub_off;
		icd_str->now_energy = 0;
		icd_str->hv_step_cnt = 0;
		hv_power(false);
	}

}

void terapy_algorithm(icd_str * icd_str)
{
	//если выбран режим принудительный//fixme
	if(icd_str->work_set_mode == Force)
	{
		icd_str->terapy_set = Terapy_fibr;
	}
	switch (icd_str->terapy_set)
	{
		case Terapy_off:
		{
			//	первичный заход в режим
			if(terapy_start(icd_str))
			{
				icd_str->hv_step_cnt = 0;
				//подготовка к нормальному режиму разрядка Конденсатора на всякий случай
				// настройка режима нормамльного поиска пиков
			}
		}
		break;
		// самое жесткое лечение
		case Terapy_fibr:
		{
			//	первичный заход в режим
			if(terapy_start(icd_str))
			{
				//подготовка к терапии
			}
			fibr_terapy(icd_str);
		}
		break;

		case Terapy_tachy_2:
		{
			//	первичный заход в режим
			if(terapy_start(icd_str))
			{
				//пока не активен
			}
		}
		break;

		case Terapy_tachy_1:
		{
			//	первичный заход в режим
			if(terapy_start(icd_str))
			{
				//пока не активен
			}
		}
		break;
	}

	if(icd_str->work_set_mode == Monitor &&  adc_str.hv_volt < SAFE_VOLTAGE/2)
	{
		hv_discharge(false);//выключили разряд
	}
}

//единое управление реле по идее вызываем один раз когда собираемся ударить не важно тестово или по настоящему
void relay_all_control(bool RV_safe,bool RA_safe,bool CAN_safe)
{
	relay_rv_control(RV_safe);
	relay_ra_control(RA_safe);
	relay_can_control(CAN_safe);
}

void relay_ra_control(bool RA_safe)
{
	if(RA_safe)
		HAL_GPIO_WritePin(RA_RELAY_GPIO_Port, RA_RELAY_Pin, GPIO_PIN_SET);
	else
		HAL_GPIO_WritePin(RA_RELAY_GPIO_Port, RA_RELAY_Pin, GPIO_PIN_RESET);
}


void relay_rv_control(bool RV_safe)
{
	if(RV_safe)
		HAL_GPIO_WritePin(RV_RELAY_GPIO_Port, RV_RELAY_Pin, GPIO_PIN_SET);
	else
		HAL_GPIO_WritePin(RV_RELAY_GPIO_Port, RV_RELAY_Pin, GPIO_PIN_RESET);
}

void relay_can_control(bool CAN_safe)
{
	if(CAN_safe)
		HAL_GPIO_WritePin(CAN_RELAY_GPIO_Port, CAN_RELAY_Pin, GPIO_PIN_SET);
	else
		HAL_GPIO_WritePin(CAN_RELAY_GPIO_Port, CAN_RELAY_Pin, GPIO_PIN_RESET);
}

//единое управление всей высоковольтной частью
void hv_ll_control(half_br state_RV, half_br state_SCV, half_br state_CAN)
{
	hv_ll_rv_control(state_RV);
	hv_ll_scv_control(state_SCV);
	hv_ll_can_control(state_CAN);
}

//питание высоковольтной части источник на 12В и 3.3В
void hv_power(bool state)
{
	if(state)
	{
		HAL_GPIO_WritePin(V12_PWR_GPIO_Port, V12_PWR_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_LOGIC_PWR_GPIO_Port, HV_LOGIC_PWR_Pin, GPIO_PIN_RESET);
	}
	else
	{
		HAL_GPIO_WritePin(V12_PWR_GPIO_Port, V12_PWR_Pin, GPIO_PIN_SET);
		HAL_GPIO_WritePin(HV_LOGIC_PWR_GPIO_Port, HV_LOGIC_PWR_Pin, GPIO_PIN_SET);
	}
}

//катушка RV
void hv_ll_rv_control(half_br state)
{
	//Замкнут нижний ключ
	if(state == low)
	{
		HAL_GPIO_WritePin(HV_HS_RV_GPIO_Port, HV_HS_RV_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_LS_RV_GPIO_Port, HV_LS_RV_Pin, GPIO_PIN_SET);
	}
	//Замкнут верхний ключ
	else if (state == high)
	{
		HAL_GPIO_WritePin(HV_LS_RV_GPIO_Port, HV_LS_RV_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_HS_RV_GPIO_Port, HV_HS_RV_Pin, GPIO_PIN_SET);
	}
	// Разомкнуты оба ключа
	else
	{
		HAL_GPIO_WritePin(HV_LS_RV_GPIO_Port, HV_LS_RV_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_HS_RV_GPIO_Port, HV_HS_RV_Pin, GPIO_PIN_RESET);
	}
}

//катушка SCV
void hv_ll_scv_control(half_br state)
{
	//Замкнут нижний ключ
	if(state == low)
	{
		HAL_GPIO_WritePin(HV_HS_SCV_GPIO_Port, HV_HS_SCV_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_LS_SCV_GPIO_Port, HV_LS_SCV_Pin, GPIO_PIN_SET);
	}
	//Замкнут верхний ключ
	else if (state == high)
	{
		HAL_GPIO_WritePin(HV_LS_SCV_GPIO_Port, HV_LS_SCV_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_HS_SCV_GPIO_Port, HV_HS_SCV_Pin, GPIO_PIN_SET);
	}
	// Разомкнуты оба ключа
	else
	{
		HAL_GPIO_WritePin(HV_LS_SCV_GPIO_Port, HV_LS_SCV_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_HS_SCV_GPIO_Port, HV_HS_SCV_Pin, GPIO_PIN_RESET);
	}
}

//Корпус
void hv_ll_can_control(half_br state)
{
	//Замкнут нижний ключ
	if(state == low)
	{
		HAL_GPIO_WritePin(HV_HS_CAN_GPIO_Port, HV_HS_CAN_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_LS_CAN_GPIO_Port, HV_LS_CAN_Pin, GPIO_PIN_SET);
	}
	//Замкнут верхний ключ
	else if (state == high)
	{
		HAL_GPIO_WritePin(HV_LS_CAN_GPIO_Port, HV_LS_CAN_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_HS_CAN_GPIO_Port, HV_HS_CAN_Pin, GPIO_PIN_SET);
	}
	// Разомкнуты оба ключа
	else
	{
		HAL_GPIO_WritePin(HV_LS_CAN_GPIO_Port, HV_LS_CAN_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(HV_HS_CAN_GPIO_Port, HV_HS_CAN_Pin, GPIO_PIN_RESET);
	}
}


//функция плавного разряда Конденсатора
void hv_discharge(bool state)
{
	if (state)		//Если хотим генерировать
	{
		//плавный разряд включен
		HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_SET);
	}
	else
	{
		//плавный разряд включен
		HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_RESET);
	}
}



void first_phase(hv_polarity polarity)
{
	switch (polarity)
	{
		// Электрод RV отрицательный SCV положительный
		case rv_neg_scv_poz:
		{
			hv_ll_control(low, high, z_state);
		}
		break;
		// Электрод RV отрицательный корпус прибора положительный
		case rv_neg_can_poz:
		{
			hv_ll_control(low, z_state, high);
		}
		break;
		// Электрод RV отрицательный SCV и корпус прибора положительный
		case rv_neg_scv_can_poz:
		{
			hv_ll_control(low, high, high);
		}
		break;
	}
}

void second_phase(hv_polarity polarity)
{
	switch (polarity)
	{
		// Электрод RV отрицательный SCV положительный
		case rv_neg_scv_poz:
		{
			hv_ll_control(high, low, z_state);
		}
		break;
		// Электрод RV отрицательный корпус прибора положительный
		case rv_neg_can_poz:
		{
			hv_ll_control(high, z_state, low);
		}
		break;
		// Электрод RV отрицательный SCV и корпус прибора положительный
		case rv_neg_scv_can_poz:
		{
			hv_ll_control(high, low, low);
		}
		break;
	}
}

////разряд биполярный с выбором параметров
void ll_bi_shock_param(icd_str * icd_str)
{
	//сперва тупо парсим параметры из основной структуры
	//хватаем параметры в локальные переменные чтобы ничего не поменялось
	hv_polarity polarity = icd_str->hv_polarity;
	hv_mode mode = icd_str->hv_mode;

	// время в десятых мс приходящееся на 1 фазу 30-120(3-12мс) одна единица 0,1мс
	uint8_t hv_phase_1_duration = icd_str->hv_phase_1_duration;
	// время в десятых мс приходящееся на 2 фазу 20-100(2-10мс) одна единица 0,1мс
	uint8_t	hv_phase_2_duration = icd_str->hv_phase_2_duration;
	// время в десятых мс приходящееся на переключение между фазами 10-30(1-3мс) одна единица 0,1мс
	uint8_t	hv_switch_duration = icd_str->hv_switch_duration;

	//процент напряжения при котором происходит завешение 1 фазы при адаптивном режиме (20-80) одна единица 1%
	uint8_t	hv_switching_voltage = icd_str->hv_switching_voltage;
	//процент напряжения при котором происходит завешение 2 фазы при адаптивном режиме (5-50) одна единица 1%
	uint8_t	hv_cutoff_voltage = icd_str->hv_cutoff_voltage;

	bool HV_on = icd_str->HV_on;
	//защитили низковольтную часть
	relay_all_control(true,true,true);
	osDelay(20);
	if(HV_on)
	{
		//чтобы не пробило перевели в разамкнутое состояние все полумосты
		hv_ll_control(z_state, z_state, z_state);
		osDelay(2);//временно

		hv_en_control(true,true,false);
		osDelay(2);//временно
		//первая фаза
		first_phase(polarity);
		delay_critical(hv_phase_1_duration);
		//остановка
		//чтобы не пробило перевели в разамкнутое состояние все полумосты
		hv_ll_control(z_state, z_state, z_state);
		delay_critical(hv_switch_duration);
		//вторая фаза
		second_phase(polarity);
		delay_critical(hv_phase_2_duration);
		//отключение
		hv_ll_control(z_state, z_state, z_state);
		hv_en_control(false,false,false);
		osDelay(2);//временно
		osDelay(20);
	}
	relay_all_control(false,false,false);
	osDelay(2);//
}




void ra_lv_control(icd_str * icd_str,lv_sub_mode mode)
{
	switch (mode)
	{
		//Свободный без всякого вмешательства tip и coil свободны разряда\заряда не происходит
		case lv_sub_free:
		{
			HAL_GPIO_WritePin(RA_LV_TIP_TO_GND_GPIO_Port, RA_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_COIL_TO_GND_GPIO_Port, RA_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_DIS_GPIO_Port, RA_LV_DIS_Pin, GPIO_PIN_SET);
		}
		break;
		//Заряд без всякого вмешательства tip и coil идёт заряд
		case lv_sub_charge:
		{
			HAL_GPIO_WritePin(RA_LV_TIP_TO_GND_GPIO_Port, RA_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_COIL_TO_GND_GPIO_Port, RA_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_DIS_GPIO_Port, RA_LV_DIS_Pin, GPIO_PIN_SET);
			osDelay(2);
			TIM4->CCR1 = 500;
			TIM4->CCR2 = 500;
			TIM4->CCR3 = 500;
			uint32_t charge_time = (uint32_t) (icd_str->lv_voltage);
			osDelay(charge_time/5);
			TIM4->CCR1 = 0;
			TIM4->CCR2 = 0;
			TIM4->CCR3 = 0;
		}
		break;
		//Разряд coil на землю притягиваем tip к конденсатору заряда нет
		case lv_sub_shock:
		{
			HAL_GPIO_WritePin(RA_LV_TIP_TO_GND_GPIO_Port, RA_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_COIL_TO_GND_GPIO_Port, RA_LV_COIL_TO_GND_Pin, GPIO_PIN_SET);
//			HAL_GPIO_WritePin(RA_LV_COIL_TO_GND_GPIO_Port, RA_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_DIS_GPIO_Port, RA_LV_DIS_Pin, GPIO_PIN_SET);
			osDelay(2);
			uint32_t shock_time = (uint32_t) (icd_str->lv_shock_time);
			__HAL_TIM_SET_COUNTER(&htim14, 0);
			__HAL_TIM_SET_COMPARE(&htim14, TIM_CHANNEL_1, 30-shock_time); //установка задержки перед импульсом
			TIM14->CR1 |= TIM_CR1_CEN;
		}
		break;
		//Релаксация tip и coil притянуты к земле
		case lv_sub_relax:
		{
			HAL_GPIO_WritePin(RA_LV_TIP_TO_GND_GPIO_Port, RA_LV_TIP_TO_GND_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(RA_LV_COIL_TO_GND_GPIO_Port, RA_LV_COIL_TO_GND_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(RA_LV_DIS_GPIO_Port, RA_LV_DIS_Pin, GPIO_PIN_SET);
		}
		break;
		//Разряд без всякого вмешательства tip и coil свободны идёт разряд(необязательный режим)
		case lv_sub_discharge:
		{
			HAL_GPIO_WritePin(RA_LV_TIP_TO_GND_GPIO_Port, RA_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_COIL_TO_GND_GPIO_Port, RA_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RA_LV_DIS_GPIO_Port, RA_LV_DIS_Pin, GPIO_PIN_RESET);
		}
		break;
	}
}

void rv_lv_control(icd_str * icd_str,lv_sub_mode mode)
{
	switch (mode)
	{
		//Свободный без всякого вмешательства tip и coil свободны разряда\заряда не происходит
		case lv_sub_free:
		{
			HAL_GPIO_WritePin(RV_LV_TIP_TO_GND_GPIO_Port, RV_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_COIL_TO_GND_GPIO_Port, RV_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_DIS_GPIO_Port, RV_LV_DIS_Pin, GPIO_PIN_SET);
		}
		break;
		//Заряд без всякого вмешательства tip и coil идёт заряд
		case lv_sub_charge:
		{
			HAL_GPIO_WritePin(RV_LV_TIP_TO_GND_GPIO_Port, RV_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_COIL_TO_GND_GPIO_Port, RV_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_DIS_GPIO_Port, RV_LV_DIS_Pin, GPIO_PIN_SET);
			osDelay(2);
			TIM2->CCR2 = 500;
			TIM2->CCR3 = 500;
			TIM2->CCR4 = 500;
			uint32_t charge_time = (uint32_t) (icd_str->lv_voltage);
			osDelay(charge_time/5);
			TIM2->CCR2 = 0;
			TIM2->CCR3 = 0;
			TIM2->CCR4 = 0;
		}
		break;
		//Разряд coil на землю притягиваем tip к конденсатору заряда нет
		case lv_sub_shock:
		{
			HAL_GPIO_WritePin(RV_LV_TIP_TO_GND_GPIO_Port, RV_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_COIL_TO_GND_GPIO_Port, RV_LV_COIL_TO_GND_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(RV_LV_DIS_GPIO_Port, RV_LV_DIS_Pin, GPIO_PIN_SET);

			uint32_t shock_time = (uint32_t) (icd_str->lv_shock_time);
			__HAL_TIM_SET_COUNTER(&htim11, 0);
			__HAL_TIM_SET_COMPARE(&htim11, TIM_CHANNEL_1, 30-shock_time); //установка задержки перед импульсом
			TIM11->CR1 |= TIM_CR1_CEN;
		}
		break;
		//Релаксация tip и coil притянуты к земле
		case lv_sub_relax:
		{
			HAL_GPIO_WritePin(RV_LV_TIP_TO_GND_GPIO_Port, RV_LV_TIP_TO_GND_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(RV_LV_COIL_TO_GND_GPIO_Port, RV_LV_COIL_TO_GND_Pin, GPIO_PIN_SET);
			HAL_GPIO_WritePin(RV_LV_DIS_GPIO_Port, RV_LV_DIS_Pin, GPIO_PIN_SET);
		}
		break;
		//Разряд без всякого вмешательства tip и coil свободны идёт разряд(необязательный режим)
		case lv_sub_discharge:
		{
			HAL_GPIO_WritePin(RV_LV_TIP_TO_GND_GPIO_Port, RV_LV_TIP_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_COIL_TO_GND_GPIO_Port, RV_LV_COIL_TO_GND_Pin, GPIO_PIN_RESET);
			HAL_GPIO_WritePin(RV_LV_DIS_GPIO_Port, RV_LV_DIS_Pin, GPIO_PIN_RESET);
		}
		break;
	}
}

void one_stimul(icd_str *icd_str)
{
	//сперва тупо парсим параметры из основной структуры
	//хватаем параметры в локальные переменные чтобы ничего не поменялось
	uint8_t active_ch = icd_str->active_ch;

	if (active_ch == RV_CHANNEL)
	{
		rv_lv_control(&ICD, lv_sub_charge);
		rv_lv_control(&ICD, lv_sub_shock);
		rv_lv_control(&ICD, lv_sub_relax);
		rv_lv_control(&ICD, lv_sub_discharge);
	}
	else if (active_ch == RA_CHANNEL)
	{
		ra_lv_control(&ICD, lv_sub_charge);
		ra_lv_control(&ICD, lv_sub_shock);
		ra_lv_control(&ICD, lv_sub_relax);
		ra_lv_control(&ICD, lv_sub_discharge);
	}

	icd_str->lv_start = false;
}


void burst(icd_str *icd_str)
{
	//сперва тупо парсим параметры из основной структуры
	//хватаем параметры в локальные переменные чтобы ничего не поменялось
	uint8_t active_ch = icd_str->active_ch;
	uint8_t burst_cnt = icd_str->burst_cnt;
	float burst_start_period = (float) icd_str->burst_period;
	float burst_end_period = (float) icd_str->burst_end_period;
	float burst_now_period = burst_start_period;
	float burst_step = (burst_end_period - burst_start_period) / ((float) (burst_cnt-1));

	uint8_t i = 0;

	while (i < burst_cnt)
	{
		if (active_ch == RV_CHANNEL)
		{
			rv_lv_control(&ICD, lv_sub_charge);
			rv_lv_control(&ICD, lv_sub_shock);
			rv_lv_control(&ICD, lv_sub_relax);
			rv_lv_control(&ICD, lv_sub_discharge);
		}
		else if (active_ch == RA_CHANNEL)
		{
			ra_lv_control(&ICD, lv_sub_charge);
			ra_lv_control(&ICD, lv_sub_shock);
			ra_lv_control(&ICD, lv_sub_relax);
			ra_lv_control(&ICD, lv_sub_discharge);
		}
//	тут криво работает период не меняет
//		burst_now_period = burst_start_period + burst_step * (float) (i);
//		fixme try this!!
		burst_now_period += burst_step;
		if (burst_now_period > BURST_PERIOD_MAX_F)
			osDelay(BURST_PERIOD_MAX);
		else if (burst_now_period < BURST_PERIOD_MIN_F)
			osDelay(BURST_PERIOD_MIN);
		else
			osDelay((uint32_t) burst_now_period);
		i++;
	}

	icd_str->lv_mode = lv_mode_none;
	icd_str->lv_start = false;
}

//единое управление enable пинами
void hv_en_control(bool en_RV, bool en_SCV, bool en_CAN)
{
	hv_en_rv(en_RV);
	hv_en_scv(en_SCV);
	hv_en_can(en_CAN);
}


void hv_en_rv(bool state)
{
	if(state)
		HAL_GPIO_WritePin(HV_EN_RV_GPIO_Port, HV_EN_RV_Pin, GPIO_PIN_RESET);
	else
		HAL_GPIO_WritePin(HV_EN_RV_GPIO_Port, HV_EN_RV_Pin, GPIO_PIN_SET);
}

void hv_en_scv(bool state)
{
	if(state)
		HAL_GPIO_WritePin(HV_EN_SCV_GPIO_Port, HV_EN_SCV_Pin, GPIO_PIN_RESET);
	else
		HAL_GPIO_WritePin(HV_EN_SCV_GPIO_Port, HV_EN_SCV_Pin, GPIO_PIN_SET);
}

void hv_en_can(bool state)
{
	if(state)
		HAL_GPIO_WritePin(HV_EN_CAN_GPIO_Port, HV_EN_CAN_Pin, GPIO_PIN_RESET);
	else
		HAL_GPIO_WritePin(HV_EN_CAN_GPIO_Port, HV_EN_CAN_Pin, GPIO_PIN_SET);
}

void delay_critical(uint8_t delay)
{
	uint32_t shock_time = (uint32_t) (delay);
	taskENTER_CRITICAL();
	delay_us((shock_time*125)/10);
	taskEXIT_CRITICAL();
}
