#include "rotation_speed.h"
#include "xp_rotation_sensor.h"
#include "IQmathLib.h"
#include "params.h"
#include "adc_tools.h"
#include "rmp_cntl_my1.h"
#include "my_filter.h"
#include "vector.h"
#include "TuneUpPlane.h" //светодиодик
#include "errors.h"
#include "log_to_memory.h"
#include "DSP281x_Device.h"	//for int16

#pragma DATA_SECTION(rotor,".fast_vars1");
ROTOR_VALUE rotor = ROTOR_VALUE_DEFAULTS;

RMP_MY1 rmp_wrot = RMP_MY1_DEFAULTS; 

_iq koef_Wout_filter = _IQ(0.2); //_IQ(0.15);
_iq koef_Wout_filter_long = _IQ(0.12);//_IQ(0.03);


static _iq impulses_To_iqF(unsigned int time, unsigned int impulses);
static _iq counter_To_iqF(unsigned int count, unsigned int freq_mode);
static _iq delta_Angle_To_iqF(unsigned long delta, unsigned int period);
static void sort_F_array(_iq *array, unsigned int size);

void rotorInit(void)
{
	unsigned int i = 0, size = 0, *pint = 0;

	rmp_wrot.RampLowLimit = 0;
	rmp_wrot.RampHighLimit = _IQ(1);
	rmp_wrot.RampPlus = _IQ(0.0015/NORMA_FROTOR);
	rmp_wrot.RampMinus = _IQ(-0.0015/NORMA_FROTOR);
	rmp_wrot.DesiredInput = 0;
	rmp_wrot.Out = 0;

	pint = (unsigned int*)&rotor;
	size = sizeof(rotor) / sizeof(unsigned int);
	for(i = 0; i < size; i++)
	{
		*(pint + i) = 0;
	}

	koef_Wout_filter = _IQ(0.2); //_IQ(0.15);
	koef_Wout_filter_long = _IQ(0.001);	//_IQ(0.0005);
}

#pragma CODE_SECTION(update_rot_sensors,".fast_run");
void update_rot_sensors()
{
//	rotation_sensor.update_registers(&rotation_sensor);
}

#pragma CODE_SECTION(Rotor_measure,".fast_run");
void Rotor_measure(void)
{
	static unsigned long PrevAngle1 = 0, PrevAngle2 = 0, PrevAngle3 = 0, PrevAngle4 = 0;
	static unsigned int peroid_shim_mks = (unsigned int)(1000000L / FREQ_PWM / 2.0);
	static _iq prev_wrot = 0;
	static unsigned int prev_wrot_count = 0;
	static int direct_accum = 0;
	static int sens1_err_count = 0;
	static int sens2_err_count = 0;


// 	unsigned int s_number = 0, begin_data = 0, end_data = 0, i;
// 	_iq accumulator = 0, deltaF = 0, deltaAngle = 0;
// 	rotation_sensor.read_sensors(&rotation_sensor);
// 	if(rotation_sensor.in_plane.read.regs.comand_reg.bit.update_registers)
// 	{
// 		rotor.error_update_count ++;
// 	}
// //	logpar.log22 = rotation_sensor.in_plane.out.CountOne1;
// //	logpar.log23 = rotation_sensor.in_plane.out.CountZero1;
// 	if(rotation_sensor.use_sensor1)
// 	{
// 		if((rotation_sensor.in_plane.out.CountOne1 <= 200)// && !rotation_sensor.in_plane.out.counter_freq1) 
// 						|| rotation_sensor.in_plane.out.CountOne1 == 65535)
// 		{ rotation_sensor.in_plane.out.CountOne1 = 0; }
// 		if((rotation_sensor.in_plane.out.CountZero1 <= 200)//  && !rotation_sensor.in_plane.out.counter_freq1) 
// 						|| rotation_sensor.in_plane.out.CountZero1 == 65535)
// 		{ rotation_sensor.in_plane.out.CountZero1 = 0; }
		
// 		//Когда импульсов мало считаем по периоду
// 		if (rotation_sensor.in_plane.out.Impulses1 < 5) {
//             if(rotation_sensor.in_plane.out.CountOne1) {
//                 rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne1,
//                                                             rotation_sensor.in_plane.out.counter_freq1);
//             }
//             if(rotation_sensor.in_plane.out.CountZero1) {
//                 rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero1,
//                                                             rotation_sensor.in_plane.out.counter_freq1);
//             }
// 		}

// 		if(rotation_sensor.in_plane.out.Impulses1 > 2)
// 		{
// 			rotor.iqFsensors[s_number++] = impulses_To_iqF(rotation_sensor.in_plane.out.Time1,
// 															rotation_sensor.in_plane.out.Impulses1);
// //			logpar.log11 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]);
// 		} else {
// //			logpar.log11 = 0;
// 		}

// 		if (rotor.iqF > 139810L)//10 rpm
// 		{
// 			if (rotation_sensor.in_plane.out.CountOne1 == 0 && rotation_sensor.in_plane.out.CountZero1 == 0
// 					&& rotation_sensor.in_plane.out.Impulses1 == 0) {
// 				sens1_err_count += 1;
// 			} else {
// 				sens1_err_count = 0;
// 			}
// 			if (sens1_err_count > 50) {
// 				sens1_err_count = 50;
// 				faults.faults4.bit.Speed_Datchik_1_Off |= 1;
// 			}
// 		} else {
// 			sens1_err_count = 0;
// 		}
// 	}

// //	logpar.log4 = rotation_sensor.in_plane.out.CountOne2;
// //	logpar.log20 = rotation_sensor.in_plane.out.CountZero2;
// 	if(rotation_sensor.use_sensor2)
// 	{
// 		if((rotation_sensor.in_plane.out.CountOne2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) 
// 						|| rotation_sensor.in_plane.out.CountOne2 == 65535)
// 		{ rotation_sensor.in_plane.out.CountOne2 = 0; }
// 		if((rotation_sensor.in_plane.out.CountZero2 <= 200)// && !rotation_sensor.in_plane.out.counter_freq2) 
// 						|| rotation_sensor.in_plane.out.CountZero2 == 65535)
// 		{ rotation_sensor.in_plane.out.CountZero2 = 0; }

// 		//Кодда импульсов мало, считаем по периоду
// 		if (rotation_sensor.in_plane.out.Impulses2 < 5) {
//             if(rotation_sensor.in_plane.out.CountOne2) {
//                 rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountOne2,
//                                                             rotation_sensor.in_plane.out.counter_freq2);
//             }
//             if(rotation_sensor.in_plane.out.CountZero2) {
//                 rotor.iqFsensors[s_number++] = counter_To_iqF(rotation_sensor.in_plane.out.CountZero2,
//                                                             rotation_sensor.in_plane.out.counter_freq2);
//             }
// 		}

// 		if(rotation_sensor.in_plane.out.Impulses2 > 2)
// 		{
// 			rotor.iqFsensors[s_number++] = impulses_To_iqF(rotation_sensor.in_plane.out.Time2,
// 															rotation_sensor.in_plane.out.Impulses2);
// //			logpar.log16 = (int16) _IQtoIQ15(rotor.iqFsensors[s_number - 1]);
// 		}

// 		if (rotor.iqF > 139810L)//10 rpm
// 		{
// 			if (rotation_sensor.in_plane.out.CountOne2 == 0 && rotation_sensor.in_plane.out.CountZero2 == 0
// 					&& rotation_sensor.in_plane.out.Impulses2 == 0) {
// 				sens2_err_count += 1;
// 			} else {
// 				sens2_err_count = 0;
// 			}
// 			if (sens2_err_count > 50) {
// 				sens2_err_count = 50;
// 				faults.faults4.bit.Speed_Datchik_2_Off |= 1;
// 			}
// 		} else {
// 			sens2_err_count = 0;
// 		}

// 	}

// 	if(rotation_sensor.use_angle_plane && (s_number < SENSORS_NUMBER))
// 	{
// 		if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel1_enable && rotor.iqFsensors[0] && (s_number < (SENSORS_NUMBER - 1)))
// 		{
// 			if(rotation_sensor.rotation_plane.out.Delta_angle1 < 1500)
// 			{
// 				rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle1,
// 														rotation_sensor.rotation_plane.out.survey_time_mks);
			
// 			}
			
// 			deltaAngle = _IQabs(rotation_sensor.rotation_plane.out.Current_angle1 - PrevAngle1);
// 			if(deltaAngle > 1500) 
// 			{
// 				deltaAngle = 262144 - deltaAngle;
// 			}
// 			if((deltaAngle < 291) && (deltaAngle >= 0))
// 			{
// 				rotor.iqFsensors[s_number++] = 
// 					delta_Angle_To_iqF(deltaAngle, peroid_shim_mks);
// 			}
// 			PrevAngle1 = rotation_sensor.rotation_plane.out.Current_angle1;
// 		}
// //		else
// //		{
// //			rotor.iqFsensors[s_number++] = 0;
// //			rotor.iqFsensors[s_number++] = 0;
// //		}
// 		if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel2_enable  && (s_number < (SENSORS_NUMBER - 1)))
// 		{
// 			rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle2,
// 														rotation_sensor.rotation_plane.out.survey_time_mks);
// 			rotor.iqFsensors[s_number++] = 
// 				delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle2 - PrevAngle2),
// 									peroid_shim_mks);
// 			PrevAngle2 = rotation_sensor.rotation_plane.out.Current_angle2;
// 		}
// 		if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel3_enable  && (s_number < (SENSORS_NUMBER - 1)))
// 		{
// 			rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle3,
// 														rotation_sensor.rotation_plane.out.survey_time_mks);
// 			rotor.iqFsensors[s_number++] = 
// 				delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle3 - PrevAngle3),
// 									peroid_shim_mks);
// 			PrevAngle3 = rotation_sensor.rotation_plane.out.Current_angle3;
// 		}
// 		if(rotation_sensor.rotation_plane.cds_rs->read.sbus.config.bit.channel4_enable  && (s_number < (SENSORS_NUMBER - 1)))
// 		{
// 			rotor.iqFsensors[s_number++] = delta_Angle_To_iqF(rotation_sensor.rotation_plane.out.Delta_angle4,
// 														rotation_sensor.rotation_plane.out.survey_time_mks);
// 			rotor.iqFsensors[s_number++] = 
// 				delta_Angle_To_iqF(_IQabs(rotation_sensor.rotation_plane.out.Current_angle4 - PrevAngle4),
// 									peroid_shim_mks);
// 			PrevAngle2 = rotation_sensor.rotation_plane.out.Current_angle4;
// 		}
// 	}
// //	logpar.log22 = (int16) _IQtoIQ15(rotor.iqFsensors[0]);
// //    logpar.log23 = (int16) _IQtoIQ15(rotor.iqFsensors[1]);
// //    logpar.log24 = (int16) _IQtoIQ15(rotor.iqFsensors[2]);
// //    logpar.log25 = (int16) _IQtoIQ15(rotor.iqFsensors[3]);
// //    logpar.log29 = (int16) _IQtoIQ15(rotor.iqFsensors[4]);
// //    logpar.log30 = (int16) _IQtoIQ15(rotor.iqFsensors[5]);

// 	if(s_number > SENSORS_NUMBER_ONLY_IN) 	{s_number = SENSORS_NUMBER_ONLY_IN;}	//TODO set SENSORS_NUMBER when tune angle measure
// 	if(s_number > 3)
// 	{
// 		sort_F_array(rotor.iqFsensors, s_number);
// 		deltaF = rotor.iqFout >> 2;
// 		if(deltaF < 43000) // ~3 ob/min
// 		{
// 			deltaF = 43000;
// 		}
// 		i = 0;
// 		begin_data = 0;
// 		end_data = s_number;	//TODO test, as usial
// 		while(i < s_number)
// 		{
// 			if(_IQabs(rotor.iqFout - rotor.iqFsensors[i]) >= deltaF)
// 			{ 
// 				i++;
// 			}
// 			else
// 			{
// 				break;
// 			}
// 		}
// 		if(i < s_number) { begin_data = i; }
// 		else {begin_data = 0;}
// 		while((i < s_number) && (_IQabs(rotor.iqFout - rotor.iqFsensors[i]) < deltaF)) 
// 		{ 
// 			i++; 
// 		}
// 		if(i <= SENSORS_NUMBER)
// 		{
// 			end_data = i;
// 		}
// 		else
// 		{
// 			end_data = SENSORS_NUMBER;
// 		}

// 	}
// 	else
// 	{
// 		begin_data = 0;
// 		end_data = s_number;
// 	}
// 	if (begin_data >= end_data) {	//This part to prevent freeze of speed on some level if signal lost
// 		begin_data = 0;
// 		end_data = s_number;
// 	}
// 	for(i = begin_data; i < end_data; i++)
// 	{
// 		accumulator += rotor.iqFsensors[i];
// 	}

// 	if(end_data != begin_data)
// 	{
// 		rotor.iqF = accumulator / (end_data - begin_data);
// 		prev_wrot_count = 0;
// 	}
// 	else
// 	{
// 		rotor.iqF = prev_wrot;
// 		prev_wrot_count += 1;
// 	}

// //	logpar.log19 = (int16)(_IQtoIQ15(rotor.iqF));

// 	if (prev_wrot != rotor.iqF || rotor.iqF==0 )
// 	{
// 		rmp_wrot.DesiredInput = rotor.iqF;
// 		rmp_wrot.calc(&rmp_wrot);
// 		rotor.iqF = rmp_wrot.Out;
// 	}
//     prev_wrot=rotor.iqF;
//     //От зависаниЙа старого значениЙа.
// 	if (prev_wrot_count > 10) {
// 		prev_wrot = 0;
// 		prev_wrot_count = 10;
// 	}

// 	rotor.iqFout = exp_regul_iq(koef_Wout_filter, rotor.iqFout, rotor.iqF);
// 	rotor.iqFlong = exp_regul_iq(koef_Wout_filter_long, rotor.iqFlong, rotor.iqF);

// 	rotor.direct_rotor_in1 = rotation_sensor.in_plane.out.direction1;
// 	rotor.direct_rotor_in2 = rotation_sensor.in_plane.out.direction2;
// 	rotor.direct_rotor_angle = rotation_sensor.rotation_plane.out.direction;

// 	rotor.error.sens_err1 = rotation_sensor.in_plane.read.pbus.direction.bit.sens_err1;
// 	rotor.error.sens_err2 = rotation_sensor.in_plane.read.pbus.direction.bit.sens_err2;

// //	rotor.direct_rotor = (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0 ? 1 :	// + rotor.direct_rotor_angle
// //						(rotor.direct_rotor_in1 + rotor.direct_rotor_in2) < 0 ? -1 :	// + rotor.direct_rotor_angle
// //						0;
// 	if(rotor.iqFout >139810L)	//10ob/min
// 	{
// 		if((rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0)
// 		{
// 			direct_accum++;
// 		}
// 		else if((rotor.direct_rotor_in1 + rotor.direct_rotor_in2) < 0)
// 		{
// 			direct_accum--;
// 		}
// 		else
// 		{
// 			if(direct_accum > 0) {direct_accum--;}
// 			if(direct_accum < 0) {direct_accum++;}
// 		}
// 		if(direct_accum > 60) { direct_accum = 60; }
// 		if(direct_accum < -60) { direct_accum = -60; }
// 		rotor.direct_rotor = direct_accum > 0 ? 1 :
// 							direct_accum < 0 ? -1 :
// 								0;
// //		if (f.flag_second_PCH) {
// //			rotor.direct_rotor = - rotor.direct_rotor;
// //		}
// 	}
// 	else
// 	{
// 		rotor.direct_rotor = (rotor.direct_rotor_in1 + rotor.direct_rotor_in2) > 0 ? 1 :	// + rotor.direct_rotor_angle
// 								(rotor.direct_rotor_in1 + rotor.direct_rotor_in2) < 0 ? -1 :	// + rotor.direct_rotor_angle
// 								0;
// //		if (f.flag_second_PCH) {
// //			rotor.direct_rotor = - rotor.direct_rotor;
// //		}
// 		direct_accum = rotor.direct_rotor;
// 	}

//	if(rotation_sensor.in_plane.write.regs.comand_reg.bit.set_sampling_time)	//Импульсы короткие
//	{
//		rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0x5C;
//	}
//	else	//Импульсы длинные, фильтр усиливаем
//	{
//		rotation_sensor.in_plane.write.regs.comand_reg.bit.filter_sensitivity = 0xA8;
//	}
}

#pragma CODE_SECTION(impulses_To_iqF,".fast_run");
_iq impulses_To_iqF(unsigned int time, unsigned int impulses) //time mks. impulses count
{
	//Flong = (impulses / time / IMPULSES_PER_TURN) * _IQ(1) / NORMA_FROTOR;
	static unsigned long long koeff_to_F = 16777216LL / IMPULSES_PER_TURN * 1000000LL / NORMA_FROTOR;
	long long Flong = (long long)impulses * koeff_to_F / time;
	return (_iq)Flong;
}

//Frot = Fimp / IMPULSES_PER_TURN / NORMA_FROTOR = 
// = counter_freq / counter / 2 / IMPULSES_PER_TURN / NORMA_FROTOR
//делю на 2, т.к. считаем длительность половины периода
//дромножаю на (1LL << 24) чтобы привести к iq24
#pragma CODE_SECTION(counter_To_iqF,".fast_run");
_iq counter_To_iqF(unsigned int count, unsigned int freq_mode)
{
	static long long koeff_to_F = 60000000LL * _IQ(1) / IMPULSES_PER_TURN / 2 / NORMA_FROTOR;
	long long Flong = 0;
	if(freq_mode == 0) // 60Mhz
	{
		Flong = koeff_to_F / count / 100;
	}
	else	//600KHz
	{
		Flong = koeff_to_F / count;
	}
	return Flong;
}

#pragma CODE_SECTION(delta_Angle_To_iqF,".fast_run");
_iq delta_Angle_To_iqF(unsigned long delta, unsigned int period)
{
	// iqF = (delta / ANGLE_RESOLUTION) / (period * 10^-6) * (1LL << 24) / NORMA_FROTOR;
	// iqF = delta * 10^6 * (1LL << 24) / ANGLE_RESOLUTION / period / NORMA_FROTOR;
	static long long koeff_to_F = 1000000LL * (1LL << 24) / ANGLE_RESOLUTION / NORMA_FROTOR;
	return (_iq)(koeff_to_F * delta / period);
}

#pragma CODE_SECTION(sort_F_array,".fast_run2");
void sort_F_array(_iq *array, unsigned int size)
{
	unsigned int i, j;
	_iq tmp = 0;
	for(i = size; i > 0; i--)
	{
		for(j = 1; j < size; j++)
		{
			if(array[j - 1] > array[j])
			{
				tmp = array[j];
				array[j] = array[j - 1];
				array[j - 1] = tmp;
			}
		}
	}
}