Velocity RMS calculation

static uint32_t CalculateIntegralRms(uint32_t num_of_adc_samples, bool *is_overflow)
{
	const int32_t vel_max = 65000000;	// if vel_maxx > vel >vel_max, devided by 10
	const int32_t vel_maxx = 650000000; // / 100
	int32_t cons;
	int16_t *acc; // FRAM1_Start+2
	int32_t *ampAcc;
	const int16_t amp = 1000;
	const int32_t amp1 = 1000000;
	int32_t sum;
	int64_t x;
	int16_t sum_cnt;
	int16_t i;
	int64_t z;
	int32_t *vel;
	uint64_t m;
	int64_t veloffset1, veloffset;

	int32_t temp_pk;
	int32_t temp_val;
	RESLO = 0x00;
	RESHI = 0x00;
	ampAcc = (int32_t *)FRAM3_START;
	acc = (int16_t *)(FRAM1_START + 2);

	for (i = num_of_adc_samples; i > 0; i--) // Signed 16 x 16, not accumulate  1.5S
	{
		MPYS = *acc++;
		OP2 = amp;
		*ampAcc = RESHI; // read result
		*ampAcc = (*ampAcc << 16) | (RESLO);
		ampAcc++;
	}

	// remove DC from Accel
	veloffset = 0;
	ampAcc = (int32_t *)FRAM3_START;
	for (i = num_of_adc_samples; i > 0; i--)
	{
		veloffset += *ampAcc++;
	}
	veloffset = veloffset / (int32_t)num_of_adc_samples;

	// check avg if in normal range
	if (veloffset < 2184000 || veloffset > 2526000)
	{
		return (0xFFFFFFFE);
	}

	//    peak_rms_value = peak_rms_value - (veloffset/1000);
	ampAcc = (int32_t *)FRAM3_START;
	for (i = num_of_adc_samples; i > 0; i--)
	{
		*ampAcc = (*ampAcc - (int32_t)veloffset);
		ampAcc++;
	}

	sum = 0;
	for (i = num_of_adc_samples; i > 0; i--) // calculate Weight Cn
	{
		sum += i;
	}

	// calculate  dotprod SUM
	RES0 = 0;
	RES1 = 0;
	RES2 = 0;
	RES3 = 0;
	MPY32CTL0 |= 0x0040 + MPYM_3; // 32 x 16 bits signed accu
	ampAcc = (int32_t *)FRAM3_START;
	for (i = num_of_adc_samples; i > 0; i--)
	{
		MACS32L = (int16_t)*ampAcc;
		MACS32H = (int16_t)((*ampAcc++) >> 16);
		OP2L = i;
		OP2H = 0;
	}

	z = 0;
	z = RES0;
	z += ((int64_t)RES1 << 16);
	z += ((int64_t)RES2 << 32);
	z += ((int64_t)RES3 << 48);
	z = z / sum;
	// calculate new accel
	ampAcc = (int32_t *)FRAM3_START;
	for (i = num_of_adc_samples; i > 0; i--)
	{
		*ampAcc = ((*ampAcc) + z);
		ampAcc++;
	}

	// integral
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	ampAcc = (int32_t *)(FRAM3_START);
	*vel++ = *ampAcc; // vel 0

	for (i = num_of_adc_samples; i > 0; i--)
	{
		*vel = *(vel - 1) + *ampAcc;
		ampAcc++;
		vel++;
	}

	// remove DC from vel
	veloffset1 = 0;
	veloffset = 0;
	sum_cnt = (num_of_adc_samples >> 1);
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	while (sum_cnt--)
	{
		veloffset1 += *vel++;
	}
	veloffset1 = veloffset1 / (int64_t)(num_of_adc_samples >> 1);
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = num_of_adc_samples; i > 0; i--)
	{
		veloffset += *vel++;
	}
	veloffset = veloffset / (int64_t)num_of_adc_samples;
	veloffset = veloffset1 + veloffset1 - veloffset;
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = num_of_adc_samples; i > 0; i--)
	{
		*vel = (*vel - (int32_t)veloffset);
		vel++;
	}

	// cal mean of vel
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = num_of_adc_samples; i > 0; i--)
	{
		veloffset1 += *vel++;
	}
	veloffset1 = veloffset1 / (int64_t)num_of_adc_samples;
	// x =  (-2) * veloffset1/ (int64_t)num_of_adc_samples;
	x = (veloffset1 << 1) / (int64_t)num_of_adc_samples;

	// cal new vel
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = 0; i < num_of_adc_samples; i++)
	{
		*vel = *vel - i * x; // MPY?
		vel++;
	}

	vel = (int32_t *)(FRAM1_START + 2); // was no +2

	if (*vel > 0)
	{
		temp_pk = *vel;
	}
	else
	{
		temp_pk = (*vel ^ (*vel >> 31)) - (*vel >> 31);
	}

	for (i = num_of_adc_samples; i > 0; i--)
	{
		*vel++;
		if (*vel > 0)
		{
			temp_val = *vel++;
		}
		else
		{
			temp_val = (*vel ^ (*vel >> 31)) - (*vel >> 31);
		}
		if (temp_pk < temp_val)
		{
			temp_pk = temp_val;
		}
	}
	temp_pk = temp_pk / amp;
	peak_rms_value = (uint16_t)temp_pk;

	vel = (int32_t *)(FRAM1_START + 2); // was no +2

	cons = 1;
	for (i = 0; i < num_of_adc_samples; i++)
	{
		if (*vel > vel_max)
		{
			cons = 10;
			i = num_of_adc_samples;
		}
		vel++;
	}
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = 0; i < num_of_adc_samples; i++)
	{
		if (*vel > vel_maxx)
		{
			cons = 100;
			i = num_of_adc_samples;
		}
		vel++;
	}
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = 0; i < num_of_adc_samples; i++)
	{
		*vel = *vel / cons; // MPY?
		// *vel=  *vel>>7;
		vel++;
	}

	// 32 x 32 signed accumu multiplication
	MPY32CTL0 |= 0x0040 + MPYM_3 + 0x0080; // // 32 x 32 bits signed accu
	RES3 = 0;
	RES2 = 0;
	RES1 = 0;
	RES0 = 0;
	vel = (int32_t *)(FRAM1_START + 2); // was no +2
	for (i = (num_of_adc_samples); i > 0; i--)
	{
		MACS32L = (int16_t)*vel;
		MACS32H = (int16_t)((*vel) >> 16);
		OP2L = (int16_t)*vel;
		OP2H = (int16_t)((*vel) >> 16);
		vel++;
	}
	m = 0;
	m = RES0;
	m += ((uint64_t)RES1 << 16);
	m += ((uint64_t)RES2 << 32);
	m += ((uint64_t)RES3 << 48);
	m = m / num_of_adc_samples;
	m = m / (uint64_t)amp1;
	m = sqrt430(m);
	if (cons == 10)
		m = m * 10;
	if (cons == 100)
		m = m * 100;

	// Set re-measure flag if velocity value is too high
	if (m > VRMS_HIGH_LIMIT) // 1874195
	{
		*is_overflow = true;
		// m = 0;     // if very high unnormal value, set m=0 to remeasure it
		// add some delay
		__delay_cycles(8000000); // around 1s
	}
	else
	{
		*is_overflow = false;
	}

	return (uint32_t)m;
}