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


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 = 3;//используемый канал
	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->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 = false; // реально стимулируем низковольтно
	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->min_energy 	= MIN_ENERGY;
	icd_str->max_energy 	= MAX_ENERGY;
	icd_str->cap_pol		= cap_poz;

	icd_str->hv_blind_time	= (HV_BLIND_TIME_MS * DATA_RATE)/1000;//время которое мы слепы после 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;
}

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->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 комплексов
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)
				{
					//предварительная насторойка включение повышайки инициализация DAC
				}
			}

			//тут по идее нужно стимулировать если это разрешено
			if(icd_str->LV_on && icd_str->mode_cnt < icd_str->lv_start_time)
			{
				//начало стимуляции
			}
			//тут по идее нужно стимулировать если это разрешено
			if(icd_str->LV_on && icd_str->mode_cnt > icd_str->lv_stop_time)
			{
				//конец стимуляции дальше ждём восстановления сигнала
			}

			if(icd_str->mode_cnt > icd_str->lv_mode_time)
			{
				//	сбросили счётчик
				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
		//if(icd_str->filt_period > 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)
			TIM3->CCR1 = 120;
//			TIM3->CCR1 = 150;так было в начале
//			TIM3->CCR1 = 675;
		#else
			TIM2->CCR2 = 900;
		#endif
	}
	else
	{
		#if (MK_TYPE == F412)
			//TIM3->CCR1 = 749;
			TIM3->CCR1 = 0;
		#else
			TIM2->CCR2 = 999;
		#endif
	}
}

void ll_shock(bool state)
{
	if(state)//Если хотим генерировать
	{
		#if (MK_TYPE == F412)
			ll_h_off();//fixme
//			HAL_GPIO_WritePin(HV_N_1_GPIO_Port, HV_N_1_Pin, GPIO_PIN_RESET);
		#else
			HAL_GPIO_WritePin(HV_SHOCK_GPIO_Port, HV_SHOCK_Pin, GPIO_PIN_RESET);
		#endif
	}
	else
	{
		#if (MK_TYPE == F412)
			ll_h_off();//fixme
//			HAL_GPIO_WritePin(HV_N_1_GPIO_Port, HV_N_1_Pin, GPIO_PIN_SET);
		#else
			HAL_GPIO_WritePin(HV_SHOCK_GPIO_Port, HV_SHOCK_Pin, GPIO_PIN_SET);
		#endif
	}
}


//функция заряда Конденсатора
void hv_charge(icd_str * icd_str)
{
	ll_h_off();//fixme

	icd_str->sub_mode = sub_cap_ch;//
	//питание выключено
	HAL_GPIO_WritePin(HV_PWR_EN_GPIO_Port, HV_PWR_EN_Pin, GPIO_PIN_SET);
	//плавный разряд выключен
	HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_SET);
	//шок выключен
	ll_shock(false);

	hv_pwm(true);
	//подали питание на плату только если не в режиме мониторинга
	if(icd_str->work_set_mode == Monitor)
	{	//питания нет
		HAL_GPIO_WritePin(HV_PWR_EN_GPIO_Port, HV_PWR_EN_Pin, GPIO_PIN_SET);
		// и генерации нет
		hv_pwm(false);
	}
	else
	{
		if(DANGER_HV)
			HAL_GPIO_WritePin(HV_PWR_EN_GPIO_Port, HV_PWR_EN_Pin, GPIO_PIN_RESET);
		else
			HAL_GPIO_WritePin(HV_PWR_EN_GPIO_Port, HV_PWR_EN_Pin, GPIO_PIN_SET);

	}
	//заряд через транзистор
	//включили шим 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 && (MK_TYPE == F412))
	{
		int i = 0;
		float voltage = sqrt(icd_str->now_energy*0.2/CAPACITY);
		uint16_t voltage_i= (uint16_t)(voltage*11.0);//умножаем на 10.5 а не на 10 т.к. напряжение после выключения заряда слегка просаживается.
		while(i < 80)
		{
			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);
	//питание выключено
	HAL_GPIO_WritePin(HV_PWR_EN_GPIO_Port, HV_PWR_EN_Pin, GPIO_PIN_SET);
	//отключили заряд конденсатора
}

//функция плавного разряда Конденсатора
void hv_discharge(icd_str * icd_str)
{
#if (CAP_VOLT_CTRL && (MK_TYPE == F412))
	1;//ничего не нужно делать так как мы контролируем напряжение
	//плавный разряд включен
	HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_RESET);
	//включили разяд конденсатора
	osDelay(icd_str->now_energy*20);
#else
	//плавный разряд включен
	HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_RESET);
	//включили разяд конденсатора
	osDelay(icd_str->now_energy*20);
	//отключили разяд конденсатора
#endif
}

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

	//включили режим слепоты
	osDelay(10);
	//подали питание на плату только если не в режиме мониторинга
	if(icd_str->work_set_mode != Monitor)//fixme
	{
//		//шок включен
//		ll_shock(true);
//		osDelay(20);
//		if(LAMP)
//			osDelay(200);//доп задержка при использовании лампы
//		ll_shock(false);

		ll_bi_dis();
		//плавный разряд включен
		//HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_RESET);
	}
	else
	{
		ll_bi_dis();
		//плавный разряд включен
		HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_RESET);
		//ll_shock(false);//шок выключен
	}
	//плавный разряд включен
	//HAL_GPIO_WritePin(HV_DIS_GPIO_Port, HV_DIS_Pin, GPIO_PIN_RESET);
	// ничего не анализируем сколько-то времени
	osDelay(icd_str->hv_blind_time*5);
	//отключили режим слепоты
	icd_str->search_set_mode = Mode_search_SQUARE;
}

//быстрое определение нужно ли бить током
bool quick_anal(icd_str * icd_str)
{
	osDelay(icd_str->hv_blind_time*5);
	//сбросили счётчики редетекции
	//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->min_energy + (icd_str->hv_step_cnt-1) * icd_str->hv_step_energy;
		icd_str->now_energy = icd_str->max_energy;
		//активировать терапию низковольтную если задана (у нас такого пока нет) fixme
		//затем производим зарядку конденсатора
		hv_charge(icd_str);
		osDelay(1000);//fixme для того чтобы было видно кончное напряжение.
		//ждём пока наберётся Х событий для анализа
		if(!quick_anal(icd_str))//если всё ОК
		{
			hv_discharge(icd_str);
			//сброс параметров и переход в режим
			icd_str->terapy_set = Terapy_off;
			//выходим нафиг из цикла
			icd_str->sub_mode = sub_off;
			icd_str->now_energy = 0;
			icd_str->hv_step_cnt = 0;
			return;
		}
		else
		{
			//должны ещё вырубить анализ с АЦП и пихать в него среднее чтоб скользящее нахер не улетело
			hv_shock(icd_str);
			//опять проверяем на нормальность цикла если всё ок возвращаемся в норм режим
			if(!quick_anal(icd_str))//если всё ОК
			{
				hv_discharge(icd_str);
				//сброс параметров и переход в режим
				icd_str->terapy_set = Terapy_off;
				//выходим нафиг из цикла
				icd_str->sub_mode = sub_off;
				icd_str->now_energy = 0;
				icd_str->hv_step_cnt = 0;
				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;
	}

}

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;
	}
}



void ll_h_off(void)
{
#if (MK_TYPE == F412)
	HAL_GPIO_WritePin(HV_N_1_GPIO_Port, HV_N_1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(HV_N_2_GPIO_Port, HV_N_2_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(HV_P_1_GPIO_Port, HV_P_1_Pin, GPIO_PIN_RESET);
	HAL_GPIO_WritePin(HV_P_2_GPIO_Port, HV_P_2_Pin, GPIO_PIN_RESET);
#endif
}

void ll_h_1_side(half_br state)
{
#if (MK_TYPE == F412)
	//Замкнут верхний ключ
	if(state == low)
	{
		HAL_GPIO_WritePin(HV_N_1_GPIO_Port, HV_N_1_Pin, GPIO_PIN_SET);//N1
		HAL_GPIO_WritePin(HV_P_1_GPIO_Port, HV_P_1_Pin, GPIO_PIN_RESET);//P1
	}
	//Замкнут нижний ключ
	else if(state == high)
	{
		HAL_GPIO_WritePin(HV_P_1_GPIO_Port, HV_P_1_Pin, GPIO_PIN_SET);//P1
		HAL_GPIO_WritePin(HV_N_1_GPIO_Port, HV_N_1_Pin, GPIO_PIN_RESET);//N1
	}
	// Разомкнуты оба ключа
	else
	{
		HAL_GPIO_WritePin(HV_P_1_GPIO_Port, HV_P_1_Pin, GPIO_PIN_RESET);//P1
		HAL_GPIO_WritePin(HV_N_1_GPIO_Port, HV_N_1_Pin, GPIO_PIN_RESET);//N1
	}
#endif
}

void ll_h_2_side(half_br state)
{
#if (MK_TYPE == F412)
	//Замкнут нижний ключ
	if(state == low)
	{
		HAL_GPIO_WritePin(HV_N_2_GPIO_Port, HV_N_2_Pin, GPIO_PIN_SET);//N2
		HAL_GPIO_WritePin(HV_P_2_GPIO_Port, HV_P_2_Pin, GPIO_PIN_RESET);//P2
	}
	//Замкнут верхний ключ
	else if(state == high)
	{
		HAL_GPIO_WritePin(HV_P_2_GPIO_Port, HV_P_2_Pin, GPIO_PIN_SET);//P2
		HAL_GPIO_WritePin(HV_N_2_GPIO_Port, HV_N_2_Pin, GPIO_PIN_RESET);//N2
	}
	// Разомкнуты оба ключа
	else
	{
		HAL_GPIO_WritePin(HV_P_2_GPIO_Port, HV_P_2_Pin, GPIO_PIN_RESET);//P2
		HAL_GPIO_WritePin(HV_N_2_GPIO_Port, HV_N_2_Pin, GPIO_PIN_RESET);//N2
	}
#endif
}

////разряд биполярный
void ll_bi_dis(void)
{

//	//чтобы не пробило
//	ll_h_1_side(z_state);
//	ll_h_2_side(z_state);
//	delay_ms(2);
//	//Обе на земле
//	ll_h_1_side(low);
//	ll_h_2_side(low);
//	osDelay(10);

	//чтобы не пробило
	ll_h_1_side(z_state);
	ll_h_2_side(z_state);
	//ll_h_2_side(low);//новое пробую сделать так, чтоб хотябы 1 драйвер был включен.
	//
	delay_ms(2);
	//Сторона 1 +
	HAL_GPIO_WritePin(HV_N_2_GPIO_Port, HV_N_2_Pin, GPIO_PIN_SET);//N2
	HAL_GPIO_WritePin(HV_P_1_GPIO_Port, HV_P_1_Pin, GPIO_PIN_SET);//P1
//	ll_h_2_side(low);
//	ll_h_1_side(high);
	delay_ms(4);

	//чтобы не пробило
	ll_h_1_side(z_state);
	ll_h_2_side(z_state);
	delay_ms(2);//если 1 поставить на 300В выбивает.
	//Сторона 2 +
	ll_h_1_side(low);
	ll_h_2_side(high);
	delay_ms(3);
	delay_ms(50);//fixme

	//чтобы не пробило
	ll_h_1_side(z_state);
	ll_h_2_side(z_state);
	delay_ms(20);

//	//Обе на земле
//	ll_h_1_side(low);
//	ll_h_2_side(low);
	osDelay(3000);

//	//чтобы не пробило
//	ll_h_1_side(z_state);
//	ll_h_2_side(z_state);
//	osDelay(2);
//	//Обе на земле
//	ll_h_1_side(low);
//	ll_h_2_side(low);
//	osDelay(10);
//
//	//чтобы не пробило
//	ll_h_1_side(z_state);
//	ll_h_2_side(z_state);
//	osDelay(2);
//	//Сторона 1 +
//	ll_h_1_side(high);
//	ll_h_2_side(low);
//	osDelay(7);
//
//	//чтобы не пробило
//	ll_h_1_side(z_state);
//	ll_h_2_side(z_state);
//	osDelay(2);//если 1 поставить на 300В выбивает.
//	//Сторона 2 +
//	ll_h_1_side(low);
//	ll_h_2_side(high);
//	osDelay(7);
//
//	//чтобы не пробило
//	ll_h_1_side(z_state);
//	ll_h_2_side(z_state);
//	osDelay(2);
//	//Обе на земле
//	ll_h_1_side(low);
//	ll_h_2_side(low);
//	osDelay(3000);

//	//всё вырублено
//	ll_h_off();
//	osDelay(2);
//	//после задержки включили сторону 1
//	ll_h_1_side(true);
//	ll_h_2_side(false);
//	osDelay(7);
//	//всё вырублено
//	ll_h_off();
//	osDelay(2);
//	//после задержки включили сторону 2
//	ll_h_1_side(false);
//	ll_h_2_side(true);
//	osDelay(7);
//	ll_h_off();
}


