// Copyright (c) Acconeer AB, 2023-2024
// All rights reserved
// This file is subject to the terms and conditions defined in the file
// 'LICENSES/license_acconeer.txt', (BSD 3-Clause License) which is part
// of this source code package.

#include <complex.h>
#include <float.h>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>

#include "acc_alg_basic_utils.h"
#include "acc_algorithm.h"
#include "acc_definitions_a121.h"
#include "acc_definitions_common.h"

#define DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH 5

//-----------------------------
// Private declarations
//-----------------------------

/**
 * @brief Calculates the sum of elements of data located at (i * stride) for i in [0, num_steps)
 *
 * @param[in] data The data
 * @param[in] num_steps How many steps of length 'stride' should be taken
 * @param[out] out Single output location
 * @param[in] stride How many elements one step should "jump".
 *                   E.g. 1U for contiguous iteration.
 */
static void sum_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride);

/**
 * @brief Calculates the mean of elements of data located at (i * stride) for i in [0, num_steps)
 *
 * @param[in] data The data
 * @param[in] num_steps How many steps of length 'stride' should be taken
 * @param[out] out Single output location
 * @param[in] stride How many elements one step should "jump".
 *                   E.g. 1U for contiguous iteration.
 */
static void mean_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride);

static void rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);

static void fftshift(float *data, uint16_t data_length, uint16_t stride);

static void welch(const float complex *data,
                  uint16_t             data_length,
                  uint16_t             segment_length,
                  float complex       *data_buffer,
                  float complex       *fft_out,
                  float               *psd,
                  const float         *window,
                  uint16_t             length_shift,
                  float                fs,
                  uint16_t             stride);

static void filter_inplace_apply(uint16_t sample_idx, const float *b, const float *a, float state[5], float *data);

static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride);

static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride);

static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);

static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride);

static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride);

static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride);

static void sort_i16(int16_t *array, uint16_t array_length);

static void swap_i16(int16_t *array, uint16_t idx_a, uint16_t idx_b);

static void sort_f32(float *array, uint16_t array_length);

static void swap_f32(float *array, uint16_t idx_a, uint16_t idx_b);

/**
 * @brief Interpolate function for Double buffering
 *
 * This function will calculate the interpolated value from the sweep
 * before and the sweep two positions ahead.
 *
 * @param[in, out] frame Data frame to where the filter is applied
 * @param[in] sweeps_per_frame How many sweeps there are in the frame
 * @param[in] num_points The number of points in the frame
 * @param[in] sweep The sweep to generate with interpolatation
 * @param[in] point The point to generate with interpolatation
 */
static void double_buffering_interpolate(acc_int16_complex_t *frame,
                                         const uint16_t       sweeps_per_frame,
                                         const uint16_t       num_points,
                                         const uint16_t       sweep,
                                         const uint16_t       point);

/**
 * @brief Median function for Double buffering
 *
 * This function will calculate the median value of four complex values.
 *
 * @param[in, out] frame Data frame to where the filter is applied
 * @param[in] num_points The number of points in the frame
 * @param[in] sweep The sweep to generate with median filter
 * @param[in] point The point to generate with median filter
 * @param[in] median_start_sweep The start sweep for the median calculation
 */
static void double_buffering_median_filter(acc_int16_complex_t *frame,
                                           const uint16_t       num_points,
                                           const uint16_t       sweep,
                                           const uint16_t       point,
                                           const uint16_t       median_start_sweep);

/**
 * @brief Merge peak cluster
 *
 * @param[in] start_idx Start index
 * @param[in] num_peaks Number of peaks to merge
 * @param[in] velocities Velocities array
 * @param[in] energies Energies array
 * @param[in] peak_idxs Peak indexes array
 * @param[out] merged_velocities Merged velocities array
 * @param[out] merged_energies Merged energies array
 * @param[in] cluster_count Cluster count
 */
static void merge_peak_cluster(uint16_t        start_idx,
                               uint16_t        num_peaks,
                               const float    *velocities,
                               const float    *energies,
                               const uint16_t *peak_idxs,
                               float          *merged_velocities,
                               float          *merged_energies,
                               uint16_t        cluster_count);

/**
 * @brief Get max measurable distance for some PRF
 *
 * @param[in] prf Pulse repetition frequency (PRF)
 * @return Max measurable distance in meters
 */
static float max_measurable_dist(acc_config_prf_t prf);

/**
 * Get profile by value
 *
 * @param[in] value Integer value that corresponds to an ACC_CONFIG_PROFILE enum
 * @return An ACC_CONFIG_PROFILE enum
 */
static acc_config_profile_t get_profile(uint16_t value);

//-----------------------------
// Public definitions
//-----------------------------

void acc_algorithm_roll_and_push(float *data, uint16_t data_length, float element)
{
	for (uint16_t i = 1U; i < data_length; i++)
	{
		data[i - 1U] = data[i];
	}

	data[data_length - 1U] = element;
}

void acc_algorithm_roll_and_push_matrix_f32(float *data, uint16_t rows, uint16_t cols, const float *column, bool pos_shift)
{
	if (pos_shift)
	{
		for (uint16_t r = rows - 1U; r > 0U; r--)
		{
			for (uint16_t c = 0U; c < cols; c++)
			{
				data[(r * cols) + c] = data[((r - 1U) * cols) + c];
			}
		}

		for (uint16_t c = 0U; c < cols; c++)
		{
			data[c] = column[c];
		}
	}
	else
	{
		for (uint16_t r = 1U; r < rows; r++)
		{
			for (uint16_t c = 0U; c < cols; c++)
			{
				data[((r - 1U) * cols) + c] = data[(r * cols) + c];
			}
		}

		for (uint16_t c = 0U; c < cols; c++)
		{
			data[((rows - 1U) * cols) + c] = column[c];
		}
	}
}

void acc_algorithm_roll_and_push_matrix_f32_complex(float complex *data, uint16_t rows, uint16_t cols, const float complex *column, bool pos_shift)
{
	if (pos_shift)
	{
		for (uint16_t r = rows - 1U; r > 0U; r--)
		{
			for (uint16_t c = 0U; c < cols; c++)
			{
				data[(r * cols) + c] = data[((r - 1U) * cols) + c];
			}
		}

		for (uint16_t c = 0U; c < cols; c++)
		{
			data[c] = column[c];
		}
	}
	else
	{
		for (uint16_t r = 1U; r < rows; r++)
		{
			for (uint16_t c = 0U; c < cols; c++)
			{
				data[((r - 1U) * cols) + c] = data[(r * cols) + c];
			}
		}

		for (uint16_t c = 0U; c < cols; c++)
		{
			data[((rows - 1U) * cols) + c] = column[c];
		}
	}
}

void acc_algorithm_roll_and_push_mult_matrix_i16_complex(acc_int16_complex_t       *data,
                                                         uint16_t                   data_rows,
                                                         uint16_t                   cols,
                                                         const acc_int16_complex_t *matrix,
                                                         uint16_t                   matrix_rows,
                                                         bool                       pos_shift)
{
	for (uint16_t m_rows = 0U; m_rows < matrix_rows; m_rows++)
	{
		if (pos_shift)
		{
			for (uint16_t r = data_rows - 1U; r > 0U; r--)
			{
				for (uint16_t c = 0U; c < cols; c++)
				{
					data[(r * cols) + c].real = data[((r - 1U) * cols) + c].real;
					data[(r * cols) + c].imag = data[((r - 1U) * cols) + c].imag;
				}
			}

			for (uint16_t c = 0U; c < cols; c++)
			{
				data[c].real = matrix[(m_rows * cols) + c].real;
				data[c].imag = matrix[(m_rows * cols) + c].imag;
			}
		}
		else
		{
			for (uint16_t r = 1U; r < data_rows; r++)
			{
				for (uint16_t c = 0U; c < cols; c++)
				{
					data[((r - 1U) * cols) + c].real = data[(r * cols) + c].real;
					data[((r - 1U) * cols) + c].imag = data[(r * cols) + c].imag;
				}
			}

			for (uint16_t c = 0U; c < cols; c++)
			{
				data[((data_rows - 1U) * cols) + c].real = matrix[(m_rows * cols) + c].real;
				data[((data_rows - 1U) * cols) + c].imag = matrix[(m_rows * cols) + c].imag;
			}
		}
	}
}

void acc_algorithm_unwrap(float *data, uint16_t data_length)
{
	for (uint16_t i = 1U; i < data_length; i++)
	{
		float diff = data[i] - data[i - 1U];

		while ((diff > (float)M_PI) || (diff < -(float)M_PI))
		{
			if (diff > (float)M_PI)
			{
				data[i] -= 2.0f * (float)M_PI;
			}
			else
			{
				data[i] += 2.0f * (float)M_PI;
			}

			diff = data[i] - data[i - 1U];
		}
	}
}

uint16_t acc_algorithm_argmax(const float *data, uint16_t data_length)
{
	uint16_t idx = 0U;
	float    max = data[idx];

	for (uint16_t i = 1U; i < data_length; i++)
	{
		if (data[i] > max)
		{
			idx = i;
			max = data[i];
		}
	}

	return idx;
}

float acc_algorithm_interpolate_peaks(const float *y, const float *x)
{
	float a = ((x[0] * (y[2] - y[1])) + (x[1] * (y[0] - y[2])) + (x[2] * (y[1] - y[0]))) / ((x[0] - x[1]) * (x[0] - x[2]) * (x[1] - x[2]));
	float b = ((y[1] - y[0]) / (x[1] - x[0])) - (a * (x[0] + x[1]));

	return -b / (2.0f * a);
}

float acc_algorithm_interpolate_peaks_equidistant(const float *y, float x_start, float x_delta, uint16_t peak_idx)
{
	float peak_offset = (y[peak_idx - 1U] - y[peak_idx + 1U]) / ((2.0f * y[peak_idx - 1U]) - (4.0f * y[peak_idx]) + (2.0f * y[peak_idx + 1U]));

	return x_start + (((float)peak_idx + peak_offset) * x_delta);
}

void acc_algorithm_butter_lowpass(float freq, float fs, float *b, float *a)
{
	float factor = (2.0f * freq) / fs;

	// Values are centered around 0 to ensure an exactly real pole
	float complex p[2];

	p[0] = -cexpf(((-1.0f * (float)M_PI) / 4.0f) * I);
	p[1] = -cexpf(((1.0f * (float)M_PI) / 4.0f) * I);

	// Pre-wrap frequencies for digital filter design
	factor = 4.0f * tanf(((float)M_PI * factor) / 2.0f);

	// Scale all points radially from origin to shift cutoff frequency
	p[0] = (float complex)factor * p[0];
	p[1] = (float complex)factor * p[1];

	// Cancel out gain change from frequency scaling
	float k = factor * factor;

	// Compensate for gain change
	float complex real_four = (float complex)4.0f;
	float complex z_prod    = 1.0f;
	float complex p_prod    = (real_four - p[0]) * (real_four - p[1]);

	k = k * crealf(acc_algorithm_cdiv(z_prod, p_prod));

	// Bilinear transform the poles and zeros
	p[0] = acc_algorithm_cdiv(real_four + p[0], real_four - p[0]);
	p[1] = acc_algorithm_cdiv(real_four + p[1], real_four - p[1]);

	// Any zeroes that were at infinity get moved to the Nyquist Frequency
	float z_z[2] = {-1.0f, -1.0f};

	// Calculate polynomial transfer functions
	a[0] = -(p[0] + p[1]);
	a[1] = p[0] * p[1];
	b[0] = k;
	b[1] = -k * (z_z[0] + z_z[1]);
	b[2] = k * (z_z[0] * z_z[1]);
}

void acc_algorithm_butter_bandpass(float min_freq, float max_freq, float fs, float *b, float *a)
{
	float min_f = (2.0f * min_freq) / fs;
	float max_f = (2.0f * max_freq) / fs;

	// Values are centered around 0 to ensure an exactly real pole
	float complex p[2];

	p[0]    = -cexpf(((-1.0f * (float)M_PI) / 4.0f) * I);
	p[1]    = -cexpf(((1.0f * (float)M_PI) / 4.0f) * I);
	float k = 1.0f;

	// Pre-wrap frequencies for digital filter design
	min_f = 4.0f * tanf(((float)M_PI * min_f) / 2.0f);
	max_f = 4.0f * tanf(((float)M_PI * max_f) / 2.0f);

	// Transform lowpass filter prototype to a bandspass filter
	float         bw = max_f - min_f;
	float complex w0 = (float complex)sqrtf(min_f * max_f);

	// Scale poles and zeros to desired bandwidth
	float complex scale = (float complex)(bw / 2.0f);

	p[0] = scale * p[0];
	p[1] = scale * p[1];

	// Duplicate poles and zeros and shift from baseband to +w0 and -w0
	float complex p_bp[4];

	w0      *= w0;
	p_bp[0]  = p[0] + csqrtf((p[0] * p[0]) - w0);
	p_bp[1]  = p[1] + csqrtf((p[1] * p[1]) - w0);
	p_bp[2]  = p[0] - csqrtf((p[0] * p[0]) - w0);
	p_bp[3]  = p[1] - csqrtf((p[1] * p[1]) - w0);

	// Cancel out gain change from frequency scaling
	float k_bp = k * bw * bw;

	// Bilinear transform the poles and zeros
	float complex real_four = (float complex)4.0f;
	float complex p_z[4];

	p_z[0] = acc_algorithm_cdiv(real_four + p_bp[0], real_four - p_bp[0]);
	p_z[1] = acc_algorithm_cdiv(real_four + p_bp[1], real_four - p_bp[1]);
	p_z[2] = acc_algorithm_cdiv(real_four + p_bp[2], real_four - p_bp[2]);
	p_z[3] = acc_algorithm_cdiv(real_four + p_bp[3], real_four - p_bp[3]);

	// Any zeroes that were at infinity get moved to the Nyquist Frequency
	float z_z[4] = {1.0f, 1.0f, -1.0f, -1.0f};

	// Compensate for gain change
	float complex z_prod = 16.0f;
	float complex p_prod = (real_four - p_bp[0]) * (real_four - p_bp[1]) * (real_four - p_bp[2]) * (real_four - p_bp[3]);
	float         k_z    = k_bp * crealf(acc_algorithm_cdiv(z_prod, p_prod));

	// Calculate polynomial transfer functions
	a[0] = -(p_z[0] + p_z[1] + p_z[2] + p_z[3]);
	a[1] = (p_z[0] * p_z[1]) + (p_z[0] * p_z[2]) + (p_z[0] * p_z[3]) + (p_z[1] * p_z[2]) + (p_z[1] * p_z[3]) + (p_z[2] * p_z[3]);
	a[2] = -((p_z[0] * p_z[1] * p_z[2]) + (p_z[0] * p_z[1] * p_z[3]) + (p_z[0] * p_z[2] * p_z[3]) + (p_z[1] * p_z[2] * p_z[3]));
	a[3] = p_z[0] * p_z[1] * p_z[2] * p_z[3];
	b[0] = k_z;
	b[1] = -k_z * (z_z[0] + z_z[1] + z_z[2] + z_z[3]);
	b[2] = k_z * ((z_z[0] * z_z[1]) + (z_z[0] * z_z[2]) + (z_z[0] * z_z[3]) + (z_z[1] * z_z[2]) + (z_z[1] * z_z[3]) + (z_z[2] * z_z[3]));
	b[3] = -k_z * ((z_z[0] * z_z[1] * z_z[2]) + (z_z[0] * z_z[1] * z_z[3]) + (z_z[0] * z_z[2] * z_z[3]) + (z_z[1] * z_z[2] * z_z[3]));
	b[4] = k_z * (z_z[0] * z_z[1] * z_z[2] * z_z[3]);
}

void acc_algorithm_lfilter(const float *b, const float *a, float *data, uint16_t data_length)
{
	float filter_states[5] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};

	for (uint16_t i = 0U; i < data_length; i++)
	{
		filter_inplace_apply(i, b, a, filter_states, data);
	}
}

void acc_algorithm_lfilter_matrix(const float *b, const float *a, float *data, uint16_t rows, uint16_t cols)
{
	for (uint16_t i = 0U; i < rows; i++)
	{
		acc_algorithm_lfilter(b, a, &data[i * cols], cols);
	}
}

void acc_algorithm_apply_filter_f32(const float *a,
                                    const float *filt_data,
                                    uint16_t     filt_rows,
                                    uint16_t     filt_cols,
                                    const float *b,
                                    const float *data,
                                    uint16_t     data_rows,
                                    uint16_t     data_cols,
                                    float       *output,
                                    uint16_t     output_length)
{
	for (uint16_t i = 0U; i < output_length; i++)
	{
		output[i] = 0.0f;

		for (uint16_t r = 0U; r < filt_rows; r++)
		{
			output[i] -= a[r] * filt_data[i + (r * filt_cols)];
		}

		for (uint16_t r = 0U; r < data_rows; r++)
		{
			output[i] += b[r] * data[i + (r * data_cols)];
		}
	}
}

void acc_algorithm_apply_filter_f32_complex(const float         *a,
                                            const float complex *filt_data,
                                            uint16_t             filt_rows,
                                            uint16_t             filt_cols,
                                            const float         *b,
                                            const float complex *data,
                                            uint16_t             data_rows,
                                            uint16_t             data_cols,
                                            float complex       *output,
                                            uint16_t             output_length)
{
	for (uint16_t i = 0U; i < output_length; i++)
	{
		float real = 0.0f;
		float imag = 0.0f;

		for (uint16_t r = 0U; r < filt_rows; r++)
		{
			real -= a[r] * crealf(filt_data[i + (r * filt_cols)]);
			imag -= a[r] * cimagf(filt_data[i + (r * filt_cols)]);
		}

		for (uint16_t r = 0U; r < data_rows; r++)
		{
			real += b[r] * crealf(data[i + (r * data_cols)]);
			imag += b[r] * cimagf(data[i + (r * data_cols)]);
		}

		output[i] = real + (imag * I);
	}
}

void acc_algorithm_mean_sweep(const acc_int16_complex_t *frame,
                              uint16_t                   num_points,
                              uint16_t                   sweeps_per_frame,
                              uint16_t                   start_point,
                              uint16_t                   end_point,
                              float complex             *sweep)
{
	for (uint16_t n = start_point; n < end_point; n++)
	{
		mean_i16_complex(&frame[n], sweeps_per_frame, &sweep[n - start_point], num_points);
	}
}

void acc_algorithm_sum_sweep(const acc_int16_complex_t *frame,
                             uint16_t                   num_points,
                             uint16_t                   sweeps_per_frame,
                             uint16_t                   start_point,
                             uint16_t                   end_point,
                             float complex             *sum_sweep)
{
	for (uint16_t n = start_point; n < end_point; n++)
	{
		sum_i16_complex(&frame[n], sweeps_per_frame, &sum_sweep[n - start_point], num_points);
	}
}

void acc_algorithm_mean_i16_complex(const acc_int16_complex_t *data, uint16_t data_length, float complex *out)
{
	mean_i16_complex(data, data_length, out, 1U);
}

void acc_algorithm_mean_matrix_i16_complex(const acc_int16_complex_t *matrix, uint16_t rows, uint16_t cols, float complex *out, uint16_t axis)
{
	if (axis == 1U)
	{
		for (uint16_t i = 0U; i < rows; i++)
		{
			mean_i16_complex(&matrix[i * cols], cols, &out[i], 1U);
		}
	}
	else if (axis == 0U)
	{
		for (uint16_t i = 0U; i < cols; i++)
		{
			mean_i16_complex(&matrix[i], rows, &out[i], cols);
		}
	}
	else
	{
		// Do nothing
	}
}

void acc_algorithm_conj_f32(float complex *data, uint16_t data_length)
{
	for (uint16_t i = 0U; i < data_length; i++)
	{
		data[i] = conjf(data[i]);
	}
}

void acc_algorithm_normalize_f32_complex(float complex *data, uint16_t data_length)
{
	for (uint16_t i = 0U; i < data_length; i++)
	{
		data[i] = data[i] / cabsf(data[i]);
	}
}

void acc_algorithm_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output)
{
	rfft(data, data_length, length_shift, output, 1U);
}

void acc_algorithm_rfft_matrix(const float *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
{
	uint16_t full_cols = ((uint16_t)1U) << length_shift;

	if (axis == 1U)
	{
		uint16_t output_cols = (full_cols / 2U) + 1U;
		for (uint16_t i = 0U; i < rows; i++)
		{
			rfft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
		}
	}
	else if (axis == 0U)
	{
		for (uint16_t i = 0U; i < cols; i++)
		{
			rfft(&data[i], rows, length_shift, &output[i], cols);
		}
	}
	else
	{
		// Do nothing
	}
}

void acc_algorithm_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output)
{
	small_fft(data, data_length, length_shift, output, 1U);
}

void acc_algorithm_fft_matrix(const float complex *data, uint16_t rows, uint16_t cols, uint16_t length_shift, float complex *output, uint16_t axis)
{
	uint16_t full_cols = ((uint16_t)1U) << length_shift;

	if (axis == 1U)
	{
		uint16_t output_cols = full_cols;
		for (uint16_t i = 0U; i < rows; i++)
		{
			small_fft(&data[i * cols], cols, length_shift, &output[i * output_cols], 1U);
		}
	}
	else if (axis == 0U)
	{
		for (uint16_t i = 0U; i < cols; i++)
		{
			small_fft(&data[i], rows, length_shift, &output[i], cols);
		}
	}
	else
	{
		// Do nothing
	}
}

float acc_algorithm_fftfreq_delta(uint16_t n, float d)
{
	float df = NAN;

	if ((n > 0U) && (d > 0.0f))
	{
		df = 1.0f / ((float)n * d);
	}

	return df;
}

void acc_algorithm_rfftfreq(uint16_t n, float d, float *freqs)
{
	uint16_t n_freqs = (n / 2U) + 1U;
	float    df      = acc_algorithm_fftfreq_delta(n, d);

	for (uint16_t i = 0U; i < n_freqs; i++)
	{
		freqs[i] = (float)i * df;
	}
}

void acc_algorithm_fftfreq(uint16_t n, float d, float *freqs)
{
	float    df  = acc_algorithm_fftfreq_delta(n, d);
	uint16_t mid = n / 2U;

	for (uint16_t i = 0U; i < mid; i++)
	{
		freqs[i] = (float)i * df;
	}

	int32_t n_int32 = (int32_t)n;

	for (int32_t i = (int32_t)mid; i < n_int32; i++)
	{
		int32_t diff = i - n_int32;
		freqs[i]     = (float)diff * df;
	}
}

float acc_algorithm_exp_smoothing_coefficient(float fs, float tc)
{
	float res = NAN;

	if ((fs != 0.0f) && (tc != 0.0f))
	{
		float dt = 1.0f / fs;

		res = expf(-dt / tc);
	}

	return res;
}

float complex acc_algorithm_cdiv(float complex num, float complex denom)
{
	double a = (double)crealf(denom);
	double b = (double)cimagf(denom);
	double c = (double)crealf(num);
	double d = (double)cimagf(num);

	float real = (float)(((c * a) + (b * d)) / ((a * a) + (b * b)));
	float imag = (float)(((a * d) - (c * b)) / ((a * a) + (b * b)));

	return real + (imag * I);
}

void acc_algorithm_hamming(uint16_t n, float *window)
{
	const float a      = 0.54f;
	const float b      = 0.46f;
	const float factor = (2.0f * (float)M_PI) / ((float)n - 1.0f);

	for (uint16_t i = 0U; i < n; i++)
	{
		window[i] = a - (b * cosf((float)i * factor));
	}
}

void acc_algorithm_hann(uint16_t n, float *window)
{
	const float a = 0.5f;

	const float factor = (2.0f * (float)M_PI) / (float)n;

	for (uint16_t i = 0U; i < n; i++)
	{
		window[i] = a - (a * cosf((float)i * factor));
	}
}

float acc_algorithm_get_fwhm(acc_config_profile_t profile)
{
	float fwhm;

	switch (profile)
	{
		case ACC_CONFIG_PROFILE_1:
			fwhm = 0.04f;
			break;
		case ACC_CONFIG_PROFILE_2:
			fwhm = 0.07f;
			break;
		case ACC_CONFIG_PROFILE_3:
			fwhm = 0.14f;
			break;
		case ACC_CONFIG_PROFILE_4:
			fwhm = 0.19f;
			break;
		case ACC_CONFIG_PROFILE_5:
			fwhm = 0.32f;
			break;
		default:
			fwhm = 0.0f;
			break;
	}

	return fwhm;
}

void acc_algorithm_double_buffering_frame_filter(acc_int16_complex_t *frame,
                                                 const uint16_t       sweeps_per_frame,
                                                 const uint16_t       num_points,
                                                 int32_t             *work_buffer)
{
	int32_t first_diff_r[2U];
	int32_t first_diff_i[2U];

	if (sweeps_per_frame >= 32U)
	{
		for (uint16_t point = 0U; point < num_points; point++)
		{
			int32_t abs_mad_sum = 0;

			for (uint16_t sweep = 0U; sweep < (sweeps_per_frame - 2U); sweep++)
			{
				/* Calculate 1st discrete difference */
				for (uint16_t idx = 0U; idx < 2U; idx++)
				{
					uint16_t sweep_idx      = (sweep + idx) * num_points;
					uint16_t next_sweep_idx = (sweep + idx + 1U) * num_points;
					int32_t  diff1_r        = frame[(next_sweep_idx + point)].real;
					int32_t  diff1_i        = frame[(next_sweep_idx + point)].imag;
					int32_t  diff2_r        = frame[(sweep_idx + point)].real;
					int32_t  diff2_i        = frame[(sweep_idx + point)].imag;
					first_diff_r[idx]       = diff1_r - diff2_r;
					first_diff_i[idx]       = diff1_i - diff2_i;
				}

				/* Calculate 2nd discrete difference */
				int32_t second_diff_r = first_diff_r[1] - first_diff_r[0U];
				int32_t second_diff_i = first_diff_i[1] - first_diff_i[0U];

				/* Estimating magnitude using: abs(real) + abs(imag) */
				int32_t abs_r = (second_diff_r < 0) ? -second_diff_r : second_diff_r;
				int32_t abs_i = (second_diff_i < 0) ? -second_diff_i : second_diff_i;

				work_buffer[sweep] = abs_r + abs_i;

				/* Sum mean absolute deviation */
				abs_mad_sum += work_buffer[sweep];
			}

			/* Mean absolute deviation */
			int32_t nof_of_abs = (int32_t)sweeps_per_frame;
			nof_of_abs         = nof_of_abs - 2;
			int32_t diff_mad   = abs_mad_sum / nof_of_abs;
			int32_t threshold  = DOUBLE_BUFFERING_MEAN_ABS_DEV_OUTLIER_TH * diff_mad;

			for (uint16_t sweep = 1U; sweep < (sweeps_per_frame - 1U); sweep++)
			{
				if (work_buffer[sweep - 1U] <= threshold)
				{
					continue;
				}

				if (sweep == 1U)
				{
					/* First Sweep */
					double_buffering_median_filter(frame, num_points, 1U, point, 0U);
				}
				else if (sweep == (sweeps_per_frame - 2U))
				{
					/* Last Sweep */
					double_buffering_median_filter(frame, num_points, sweeps_per_frame - 2U, point, sweeps_per_frame - 4U - 1U);
				}
				else
				{
					double_buffering_interpolate(frame, sweeps_per_frame, num_points, sweep, point);
				}
			}
		}
	}
}

void acc_algorithm_fftshift_matrix(float *data, uint16_t rows, uint16_t cols)
{
	for (uint16_t i = 0U; i < cols; i++)
	{
		fftshift(&(data[i]), rows, cols);
	}
}

void acc_algorithm_fftshift(float *data, uint16_t data_length)
{
	fftshift(data, data_length, 1U);
}

void acc_algorithm_welch_matrix(const float complex *data,
                                uint16_t             rows,
                                uint16_t             cols,
                                uint16_t             segment_length,
                                float complex       *data_buffer,
                                float complex       *fft_out,
                                float               *psds,
                                const float         *window,
                                uint16_t             length_shift,
                                float                fs)
{
	for (uint16_t i = 0U; i < cols; i++)
	{
		welch(&(data[i]), rows, segment_length, data_buffer, fft_out, &(psds[i]), window, length_shift, fs, cols);
	}
}

void acc_algorithm_welch(const float complex *data,
                         uint16_t             data_length,
                         uint16_t             segment_length,
                         float complex       *data_buffer,
                         float complex       *fft_out,
                         float               *psd,
                         const float         *window,
                         uint16_t             length_shift,
                         float                fs)
{
	welch(data, data_length, segment_length, data_buffer, fft_out, psd, window, length_shift, fs, 1U);
}

float acc_algorithm_calculate_cfar(const float *data,
                                   uint16_t     data_length,
                                   uint16_t     window_length,
                                   uint16_t     half_guard_length,
                                   float        sensitivity,
                                   uint16_t     idx)
{
	const uint16_t start_idx = window_length + half_guard_length;
	const uint16_t end_idx   = data_length - start_idx;
	float          threshold = 0.0f;

	if ((idx < start_idx) || (idx >= end_idx))
	{
		threshold = FLT_MAX;
	}
	else
	{
		float    samples_sum             = 0.0f;
		uint16_t sample_count            = 0U;
		uint16_t take_window_close_start = idx - half_guard_length - window_length;
		uint16_t take_window_far_start   = idx + half_guard_length + 1U;

		for (uint16_t k = 0; k < window_length; k++)
		{
			samples_sum  += data[take_window_close_start + k];
			samples_sum  += data[take_window_far_start + k];
			sample_count += 2U;
		}

		threshold  = (sample_count > 0U) ? (samples_sum / (float)sample_count) : 0.0f;
		threshold += sensitivity;
	}

	return threshold;
}

float acc_algorithm_calculate_mirrored_one_sided_cfar(const float *data,
                                                      uint16_t     data_length,
                                                      uint16_t     middle_idx,
                                                      uint16_t     window_length,
                                                      uint16_t     half_guard_length,
                                                      float        sensitivity,
                                                      uint16_t     idx)
{
	uint16_t margin                        = window_length + half_guard_length;
	uint16_t half_sweep_len_without_margin = (uint16_t)rint(((double)data_length / 2.0) - (double)margin);

	float min = INFINITY;

	for (uint16_t i = 0U; i < data_length; i++)
	{
		min = fminf(data[i], min);
	}

	float sum = 0.0f;

	if (idx <= margin)
	{
		for (uint16_t j = 0U; j < window_length; j++)
		{
			sum += data[j];
		}
	}

	if ((idx > margin) && (idx < middle_idx))
	{
		for (uint16_t j = 0U; j < window_length; j++)
		{
			sum += data[j + (idx - margin)];
		}
	}

	if ((idx >= middle_idx) && (idx < (data_length - margin - 1U)))
	{
		for (uint16_t j = 0U; j < window_length; j++)
		{
			sum += data[data_length - half_sweep_len_without_margin - j + idx - middle_idx];
		}
	}

	if (idx >= (data_length - margin - 1U))
	{
		for (uint16_t j = 0U; j < window_length; j++)
		{
			sum += data[data_length - j - 1U];
		}
	}

	return ((sum / (float)window_length) + min) / sensitivity;
}

uint16_t acc_algorithm_get_distance_idx(const float *data, uint16_t cols, uint16_t rows, uint16_t middle_idx, uint16_t half_slow_zone)
{
	float max = -INFINITY;

	uint16_t idx = 0U;

	for (uint16_t i = 0U; i < rows; i++)
	{
		if ((i < (middle_idx + half_slow_zone)) && (i >= (middle_idx - half_slow_zone)))
		{
			continue;
		}

		for (uint16_t j = 0U; j < cols; j++)
		{
			if (data[(i * cols) + j] > max)
			{
				max = data[(i * cols) + j];
				idx = j;
			}
		}
	}

	return idx;
}

float acc_algorithm_get_peak_velocity(const float *velocities, const float *energies, const uint16_t *peak_idxs, uint16_t num_peaks, float limit)
{
	float slow_vs              = 0.0f;
	float valid_vs             = 0.0f;
	bool  has_valid            = false;
	float biggest_energy_slow  = -INFINITY;
	float biggest_energy_valid = -INFINITY;

	for (uint16_t i = 0U; i < num_peaks; i++)
	{
		uint16_t idx = (peak_idxs != NULL) ? peak_idxs[i] : i;

		if (energies[idx] > biggest_energy_slow)
		{
			if (fabsf(velocities[idx]) < limit)
			{
				slow_vs             = velocities[idx];
				biggest_energy_slow = energies[idx];
			}
		}

		if (energies[i] > biggest_energy_valid)
		{
			if (fabsf(velocities[idx]) >= limit)
			{
				valid_vs             = velocities[idx];
				biggest_energy_valid = energies[idx];
				has_valid            = true;
			}
		}
	}

	return has_valid ? valid_vs : slow_vs;
}

bool acc_algorithm_merge_peaks(float           max_peak_separation,
                               const float    *velocities,
                               const float    *energies,
                               const uint16_t *peak_idxs,
                               uint16_t        num_peaks,
                               float          *merged_velocities,
                               float          *merged_energies,
                               uint16_t        merged_peaks_length,
                               uint16_t       *num_merged_peaks)
{
	bool     status            = true;
	uint16_t cluster_count     = 0U;
	uint16_t cluster_start_idx = 0U;

	if (num_peaks > 1U)
	{
		for (uint16_t i = 0U; i < (num_peaks - 1U); i++)
		{
			uint16_t current_idx = peak_idxs[i];
			uint16_t next_idx    = peak_idxs[i + 1U];

			uint16_t num_peaks_in_cluster = i - cluster_start_idx + 1U;

			bool current_peak_is_in_cluster = (velocities[next_idx] - velocities[current_idx]) < max_peak_separation;

			if (current_peak_is_in_cluster)
			{
				continue;
			}

			status = cluster_count < merged_peaks_length;

			if (status)
			{
				merge_peak_cluster(
				    cluster_start_idx, num_peaks_in_cluster, velocities, energies, peak_idxs, merged_velocities, merged_energies, cluster_count);
			}

			if (!status)
			{
				break;
			}

			cluster_count++;
			cluster_start_idx = i + 1U;
		}
	}

	bool last_cluster_not_merged = cluster_start_idx < num_peaks;

	if (status && last_cluster_not_merged)
	{
		status = cluster_count < merged_peaks_length;

		if (status)
		{
			merge_peak_cluster(
			    cluster_start_idx, num_peaks - cluster_start_idx, velocities, energies, peak_idxs, merged_velocities, merged_energies, cluster_count);
		}

		if (status)
		{
			cluster_count++;
		}
	}

	if (status)
	{
		*num_merged_peaks = cluster_count;
	}

	return status;
}

float acc_algorithm_get_distance_m(uint16_t step_length, uint16_t start_point, float base_step_length_m, uint16_t idx)
{
	uint16_t steps = (idx * step_length) + start_point;

	return (float)steps * base_step_length_m;
}

acc_config_profile_t acc_algorithm_select_profile(int32_t start_point, float base_step_length)
{
	acc_config_profile_t profile = ACC_CONFIG_PROFILE_1;

	// Array with minimum start point for each profile without interference from direct leakage
	// Profile 1 has special case -1 to default to this if no other work
	float MIN_DIST_M[5] = {-1.0f, 0.07f * 2.0f, 0.14f * 2.0f, 0.19f * 2.0f, 0.32f * 2.0f};

	for (uint16_t i = (uint16_t)ACC_CONFIG_PROFILE_1; i <= (uint16_t)ACC_CONFIG_PROFILE_5; i++)
	{
		if ((MIN_DIST_M[i - 1U] == -1.0f) || (MIN_DIST_M[i - 1U] <= ((float)start_point * base_step_length)))
		{
			profile = get_profile(i);
		}
	}

	return profile;
}

acc_config_prf_t acc_algorithm_select_prf(int16_t breakpoint, acc_config_profile_t profile, float base_step_length)
{
	float            breakpoint_p = (float)breakpoint;
	float            breakpoint_m = breakpoint_p * base_step_length;
	acc_config_prf_t prf;

	if ((breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_19_5_MHZ)) && (profile == ACC_CONFIG_PROFILE_1))
	{
		prf = ACC_CONFIG_PRF_19_5_MHZ;
	}
	else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_15_6_MHZ))
	{
		prf = ACC_CONFIG_PRF_15_6_MHZ;
	}
	else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_13_0_MHZ))
	{
		prf = ACC_CONFIG_PRF_13_0_MHZ;
	}
	else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_8_7_MHZ))
	{
		prf = ACC_CONFIG_PRF_8_7_MHZ;
	}
	else if (breakpoint_m < max_measurable_dist(ACC_CONFIG_PRF_6_5_MHZ))
	{
		prf = ACC_CONFIG_PRF_6_5_MHZ;
	}
	else
	{
		prf = ACC_CONFIG_PRF_5_2_MHZ;
	}

	return prf;
}

bool acc_algorithm_find_peaks(const float    *abs_sweep,
                              const uint16_t  data_length,
                              const uint32_t *threshold_check,
                              uint16_t       *peak_idxs,
                              uint16_t        peak_idxs_length,
                              uint16_t       *num_peaks)
{
	bool     success     = true;
	uint16_t found_peaks = 0U;
	uint16_t i           = 1U;

	while (i < data_length)
	{
		/*
		 * Find a peak candidate.
		 */

		if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, ((size_t)i - 1U)))
		{
			i++;
			continue;
		}

		if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, i))
		{
			i += 2U;
			continue;
		}

		if (abs_sweep[i - 1U] >= abs_sweep[i])
		{
			i++;
			continue;
		}

		/*
		 * Peak candidate found at abs_sweep[d].
		 *
		 * We have two consecutive peaks above threshold: abs_sweep[d-1] and
		 * abs_sweep[d], where abs_sweep[d-1] < abs_sweep[d].
		 *
		 * Now search for an upper bound where abs_sweep[upper] < abs_sweep[d],
		 * but still above threshold.
		 */

		uint16_t d_upper    = i + 1U;
		bool     upper_done = false;

		while (!upper_done)
		{
			if (d_upper >= (data_length - 1U))
			{
				upper_done = true;
			}
			else if (!acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, d_upper))
			{
				upper_done = true;
			}
			else if (abs_sweep[d_upper] > abs_sweep[i])
			{
				// Growing slope; reset the peak candidate to the new larger value.
				i = d_upper;
				d_upper++;
			}
			else if (abs_sweep[d_upper] < abs_sweep[i])
			{
				/*
				 * Ensure that the value after a peak candidate (i) isn't below threshold; e.g:
				 *
				 * abs_sweep = [1, 2, 3, 4, 2, 2]
				 * threshold = [2, 2, 2, 2, 2, 2]
				 *
				 * Candidate: abs_sweep[i] = 4
				 */
				if (acc_alg_basic_utils_is_bit_set_bitarray_uint32(threshold_check, ((size_t)i + 1U)))
				{
					if (found_peaks < peak_idxs_length)
					{
						peak_idxs[found_peaks] = i;
						found_peaks++;
					}
					else
					{
						success = false;
					}
				}

				upper_done = true;
			}
			else
			{
				d_upper++;
			}
		}

		i = d_upper;
	}

	*num_peaks = found_peaks;

	return success;
}

void acc_algorithm_count_points_above_threshold(const float *matrix,
                                                uint16_t     rows,
                                                uint16_t     cols,
                                                const float  threshold,
                                                uint16_t    *count,
                                                uint16_t     offset,
                                                uint16_t     threshold_check_length,
                                                uint16_t     axis)
{
	if (axis == 0U)
	{
		for (uint16_t r = offset; r < (threshold_check_length + offset); r++)
		{
			count[r] = 0U;
			for (uint16_t c = 0U; c < cols; c++)
			{
				if (matrix[c + (r * cols)] > threshold)
				{
					count[r]++;
				}
			}
		}
	}
	else if (axis == 1U)
	{
		for (uint16_t c = offset; c < (threshold_check_length + offset); c++)
		{
			count[c] = 0U;
			for (uint16_t r = 0U; r < rows; r++)
			{
				if (matrix[c + (r * cols)] > threshold)
				{
					count[c]++;
				}
			}
		}
	}
	else
	{
		// Do nothing
	}
}

int16_t acc_algorithm_median_i16(int16_t *data, uint16_t length)
{
	sort_i16(data, length);

	int16_t result;

	if ((length % 2U) == 0U)
	{
		result = (data[(length / 2U) - 1U] + data[length / 2U]) / 2;
	}
	else
	{
		result = data[length / 2U];
	}

	return result;
}

float acc_algorithm_median_f32(float *data, uint16_t length)
{
	sort_f32(data, length);

	float result;

	if ((length % 2U) == 0U)
	{
		result = (data[(length / 2U) - 1U] + data[length / 2U]) / 2.0f;
	}
	else
	{
		result = data[length / 2U];
	}

	return result;
}

float acc_algorithm_max_f32(const float *data, uint16_t length)
{
	float max_value = 0.0f;

	for (uint16_t i = 0; i < length; i++)
	{
		if (data[i] > max_value)
		{
			max_value = data[i];
		}
	}

	return max_value;
}

float acc_algorithm_weighted_mean(const float *data, const float *weights, uint16_t length)
{
	float weight_sum        = 0.0f;
	float weighted_data_sum = 0.0f;
	float weighted_mean     = 0.0f;

	for (uint16_t i = 0; i < length; i++)
	{
		weight_sum        += weights[i];
		weighted_data_sum += weights[i] * data[i];
	}

	if (weight_sum > 0.0f)
	{
		weighted_mean = weighted_data_sum / weight_sum;
	}
	else
	{
		weighted_mean = data[0];
	}

	return weighted_mean;
}

float acc_algorithm_variance_f32(const float *data, uint16_t length)
{
	float sum        = 0.0f;
	float sq_err_sum = 0.0f;
	float variance   = 0.0f;

	if (length > 1U)
	{
		for (uint16_t i = 0U; i < length; i++)
		{
			sum += data[i];
		}

		float mean = sum / (float)length;

		for (uint16_t i = 0U; i < length; i++)
		{
			float error  = data[i] - mean;
			sq_err_sum  += error * error;
		}

		variance = sq_err_sum / (float)length;
	}

	return variance;
}

float acc_algorithm_stddev_f32(const float *data, uint16_t length)
{
	return sqrtf(acc_algorithm_variance_f32(data, length));
}

float acc_algorithm_clip_f32(float value, float min, float max)
{
	float res;

	if (value > max)
	{
		res = max;
	}
	else if (value < min)
	{
		res = min;
	}
	else
	{
		res = value;
	}

	return res;
}

//-----------------------------
// Private definitions
//-----------------------------

static void sum_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride)
{
	float real_sum = 0.0f;
	float imag_sum = 0.0f;

	for (uint16_t i = 0U; i < num_steps; i++)
	{
		real_sum += (float)data[i * stride].real;
		imag_sum += (float)data[i * stride].imag;
	}

	*out = real_sum + (imag_sum * I);
}

static void mean_i16_complex(const acc_int16_complex_t *data, uint16_t num_steps, float complex *out, uint16_t stride)
{
	sum_i16_complex(data, num_steps, out, stride);

	float real_mean = crealf(*out) / (float)num_steps;
	float imag_mean = cimagf(*out) / (float)num_steps;

	*out = real_mean + (imag_mean * I);
}

static void rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
{
	small_rfft(data, data_length, length_shift - 1U, output, stride);

	small_rfft_real_symmetry_conversion(output, length_shift - 1U, stride);

	output[(((uint16_t)1U) << (length_shift - 1U)) * stride] = cimagf(output[0]);
	output[0]                                                = crealf(output[0]);
}

static void fftshift(float *data, uint16_t data_length, uint16_t stride)
{
	uint16_t half_data_length = (data_length + 1U) / 2U;

	for (uint16_t i = 0U; i < half_data_length; i++)
	{
		float x = data[0];

		for (uint16_t j = 0U; j < (data_length - 1U); j++)
		{
			data[j * stride] = data[(j * stride) + stride];
		}

		data[(data_length * stride) - stride] = x;
	}
}

static void welch(const float complex *data,
                  uint16_t             data_length,
                  uint16_t             segment_length,
                  float complex       *data_buffer,
                  float complex       *fft_out,
                  float               *psd,
                  const float         *window,
                  uint16_t             length_shift,
                  float                fs,
                  uint16_t             stride)
{
	uint16_t num_segments = data_length / segment_length;
	float    scale        = 0.0f;

	for (uint16_t i = 0U; i < num_segments; i++)
	{
		scale              = 0.0f;
		float complex mean = 0.0f;

		for (uint16_t j = 0U; j < segment_length; j++)
		{
			mean += data[(i * segment_length * stride) + (j * stride)];
		}

		float complex adj_mean_real = crealf(mean) / (float)segment_length;
		float complex adj_mean_imag = cimagf(mean) / (float)segment_length;
		mean                        = adj_mean_real + (adj_mean_imag * I);

		for (uint16_t j = 0U; j < segment_length; j++)
		{
			data_buffer[j] = data[(i * segment_length * stride) + (j * stride)] - mean;

			float complex adj_buf_real = crealf(data_buffer[j]) * window[j];
			float complex adj_buf_imag = cimagf(data_buffer[j]) * window[j];
			data_buffer[j]             = adj_buf_real + (adj_buf_imag * I);

			scale += window[j] * window[j];
		}

		acc_algorithm_fft(data_buffer, segment_length, length_shift, fft_out);

		for (uint16_t j = 0U; j < segment_length; j++)
		{
			psd[j * stride] += cabsf(fft_out[j]) * cabsf(fft_out[j]);
		}
	}

	if (scale != 0.0f)
	{
		scale = 1.0f / (scale * fs * (float)num_segments);
	}

	for (uint16_t i = 0U; i < segment_length; i++)
	{
		psd[i * stride] *= scale;
	}
}

static void filter_inplace_apply(uint16_t sample_idx, const float *b, const float *a, float state[5], float *data)
{
	float x = data[sample_idx];
	float y;

	y = state[0] + (b[0] * x);

	state[0] = state[1] + (b[1] * x) - (a[0] * y);
	state[1] = state[2] + (b[2] * x) - (a[1] * y);
	state[2] = state[3] + (b[3] * x) - (a[2] * y);
	state[3] = (b[4] * x) - (a[3] * y);

	data[sample_idx] = y;
}

static float complex get_data_padded_f32_to_f32_complex(const float *data, uint16_t data_length, uint16_t index, uint16_t stride)
{
	float    real = 0.0f;
	float    imag = 0.0f;
	uint16_t i    = index * 2U;

	if (i < data_length)
	{
		real = data[i * stride];
	}

	i++;

	if (i < data_length)
	{
		imag = data[i * stride];
	}

	return real + (imag * I);
}

static float complex get_data_padded_f32_complex(const float complex *data, uint16_t data_length, uint16_t index, uint16_t stride)
{
	float complex res = 0.0f + 0.0f * I;

	if (index < data_length)
	{
		res = data[index * stride];
	}

	return res;
}

static void small_rfft(const float *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
{
	uint16_t full_data_length = ((uint16_t)1U) << length_shift;

	if (length_shift == 0U)
	{
		// Trivial 1-element FFT
		output[0] = get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride);
	}
	else if (length_shift == 1U)
	{
		// 2-element FFT
		output[0] =
		    get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride) + get_data_padded_f32_to_f32_complex(data, data_length, 1U, stride);
		output[1U * stride] =
		    get_data_padded_f32_to_f32_complex(data, data_length, 0U, stride) - get_data_padded_f32_to_f32_complex(data, data_length, 1U, stride);
	}
	else
	{
		// Perform element reordering
		uint16_t reverse_i = 0U;
		for (uint16_t i = 0U; i < full_data_length; i++)
		{
			if (i < reverse_i)
			{
				float complex tmp          = get_data_padded_f32_to_f32_complex(data, data_length, i, stride);
				output[i * stride]         = get_data_padded_f32_to_f32_complex(data, data_length, reverse_i, stride);
				output[reverse_i * stride] = tmp;
			}
			else if (i == reverse_i)
			{
				output[i * stride] = get_data_padded_f32_to_f32_complex(data, data_length, i, stride);
			}
			else
			{
				// Do nothing
			}

			uint16_t bit = full_data_length >> 1U;
			while ((bit & reverse_i) != 0U)
			{
				reverse_i  &= ~bit;
				bit       >>= 1U;
			}
			reverse_i |= bit;
		}

		small_fft_transformation(length_shift, output, stride);
	}
}

static void small_fft(const float complex *data, uint16_t data_length, uint16_t length_shift, float complex *output, uint16_t stride)
{
	uint16_t full_data_length = ((uint16_t)1U) << length_shift;

	if (length_shift == 0U)
	{
		// Trivial 1-element FFT
		output[0] = get_data_padded_f32_complex(data, data_length, 0U, stride);
	}
	else if (length_shift == 1U)
	{
		// 2-element FFT
		output[0]           = get_data_padded_f32_complex(data, data_length, 0U, stride) + get_data_padded_f32_complex(data, data_length, 1U, stride);
		output[1U * stride] = get_data_padded_f32_complex(data, data_length, 0U, stride) - get_data_padded_f32_complex(data, data_length, 1U, stride);
	}
	else
	{
		// Perform element reordering
		uint16_t reverse_i = 0U;
		for (uint16_t i = 0U; i < full_data_length; i++)
		{
			if (i < reverse_i)
			{
				float complex tmp          = get_data_padded_f32_complex(data, data_length, i, stride);
				output[i * stride]         = get_data_padded_f32_complex(data, data_length, reverse_i, stride);
				output[reverse_i * stride] = tmp;
			}
			else if (i == reverse_i)
			{
				output[i * stride] = get_data_padded_f32_complex(data, data_length, i, stride);
			}
			else
			{
				// Do nothing
			}

			uint16_t bit = full_data_length >> 1U;
			while ((bit & reverse_i) != 0U)
			{
				reverse_i  &= ~bit;
				bit       >>= 1U;
			}
			reverse_i |= bit;
		}

		small_fft_transformation(length_shift, output, stride);
	}
}

static void small_fft_transformation(uint16_t length_shift, float complex *output, uint16_t stride)
{
	uint16_t full_data_length = ((uint16_t)1U) << length_shift;

	// Perform 4-element base transformations
	for (uint16_t i = 0U; i < full_data_length; i += 4U)
	{
		float complex s0 = output[i * stride] + output[(i + 1U) * stride];
		float complex d0 = output[i * stride] - output[(i + 1U) * stride];
		float complex s1 = output[(i + 2U) * stride] + output[(i + 3U) * stride];
		float complex d1 = output[(i + 2U) * stride] - output[(i + 3U) * stride];

		d1 = cimagf(d1) - (I * crealf(d1)); // d1 = -I*d1;

		output[(i + 0U) * stride] = s0 + s1;
		output[(i + 2U) * stride] = s0 - s1;
		output[(i + 1U) * stride] = d0 + d1;
		output[(i + 3U) * stride] = d0 - d1;
	}

	// Main part of the FFT computation
	uint16_t      block_length = 4U;
	float complex phase_incr   = -I;

	while (block_length < full_data_length)
	{
		// Update the phase_incr unit vector to have half phase angle
		phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);

		float complex phase = 1.0f;
		for (uint16_t m = 0U; m < block_length; m++)
		{
			for (uint16_t i = m; i < full_data_length; i += block_length << 1U)
			{
				float complex delta = output[(i + block_length) * stride] * phase;

				output[(i + block_length) * stride] = output[i * stride] - delta;

				output[i * stride] += delta;
			}

			// This phase increment is the leading error source for large transforms
			phase = phase * phase_incr;
		}

		block_length <<= 1U;
	}
}

static void small_rfft_real_symmetry_conversion(float complex *output, uint16_t length_shift, uint16_t stride)
{
	uint16_t full_data_length = ((uint16_t)1U) << length_shift;

	output[0] = (1.0f + (1.0f * I)) * conjf(output[0]);

	if (length_shift > 0U)
	{
		float complex phase_incr = I;
		float complex z1_factor  = 0.5f * phase_incr;

		for (uint16_t i = 1U; i < length_shift; i++)
		{
			phase_incr = (phase_incr + 1.0f) / cabsf(phase_incr + 1.0f);
		}

		uint16_t mid = full_data_length / 2U;
		for (uint16_t i = 1U; i < mid; i++)
		{
			float complex t;
			float complex z0 = output[i * stride];
			float complex z1 = output[(full_data_length - i) * stride];

			t  = z0 + conjf(z1);
			z1 = conjf(z0) - z1;
			z0 = t;

			z0        *= 0.5f;
			z1_factor  = z1_factor * phase_incr;
			z1         = z1 * z1_factor;

			t  = z0 + conjf(z1);
			z1 = conjf(z0) - z1;
			z0 = t;

			output[i * stride]                      = z0;
			output[(full_data_length - i) * stride] = z1;
		}

		output[mid * stride] = conjf(output[mid * stride]);
	}
}

static void double_buffering_median_filter(acc_int16_complex_t *frame,
                                           const uint16_t       num_points,
                                           const uint16_t       sweep,
                                           const uint16_t       point,
                                           const uint16_t       median_start_sweep)
{
	/* Get the complex median value over an array of length 4 */
	int32_t point_r[4U];
	int32_t point_i[4U];
	int32_t point_abs[4U];

	/* Calculate abs value */
	for (uint16_t idx = 0U; idx < 4U; idx++)
	{
		point_r[idx]   = frame[((median_start_sweep + idx) * num_points) + point].real;
		point_i[idx]   = frame[((median_start_sweep + idx) * num_points) + point].imag;
		point_abs[idx] = (point_r[idx] * point_r[idx]) + (point_i[idx] * point_i[idx]);
	}

	uint16_t high_index = 0U;
	uint16_t low_index  = 0U;
	int32_t  high_val   = INT32_MIN;
	int32_t  low_val    = INT32_MAX;

	/* Find highest/lowest abs index */
	for (uint16_t idx = 0; idx < 4U; idx++)
	{
		if (point_abs[idx] > high_val)
		{
			high_val   = point_abs[idx];
			high_index = idx;
		}

		if (point_abs[idx] < low_val)
		{
			low_val   = point_abs[idx];
			low_index = idx;
		}
	}

	/* Clear highest and lowest */
	point_r[high_index] = 0;
	point_i[high_index] = 0;
	point_r[low_index]  = 0;
	point_i[low_index]  = 0;

	int32_t median_real = 0;
	int32_t median_imag = 0;

	/* Sum complex points */
	for (uint16_t idx = 0U; idx < 4U; idx++)
	{
		median_real += point_r[idx];
		median_imag += point_i[idx];
	}

	/* Update frame with median filtered value */
	median_real = median_real / 2;
	median_imag = median_imag / 2;

	frame[(sweep * num_points) + point].real = (int16_t)median_real;
	frame[(sweep * num_points) + point].imag = (int16_t)median_imag;
}

static void double_buffering_interpolate(acc_int16_complex_t *frame,
                                         const uint16_t       sweeps_per_frame,
                                         const uint16_t       num_points,
                                         const uint16_t       sweep,
                                         const uint16_t       point)
{
	/* 2/3 of the sweep value before */
	int32_t interpolate_real_i32 = frame[((sweep - 1U) * num_points) + point].real;
	int32_t interpolate_imag_i32 = frame[((sweep - 1U) * num_points) + point].imag;

	interpolate_real_i32 = interpolate_real_i32 * 2;
	interpolate_imag_i32 = interpolate_imag_i32 * 2;

	/* 1/3 of the sweep value two positions ahead */
	uint16_t sweep_idx = sweep + 2U;

	if (sweep_idx > (sweeps_per_frame - 1U))
	{
		sweep_idx = sweeps_per_frame - 1U;
	}

	interpolate_real_i32 += frame[((sweep_idx)*num_points) + point].real;
	interpolate_imag_i32 += frame[((sweep_idx)*num_points) + point].imag;

	interpolate_real_i32 = interpolate_real_i32 / 3;
	interpolate_imag_i32 = interpolate_imag_i32 / 3;

	/* Update frame with interpolated value */
	frame[(sweep * num_points) + point].real = (int16_t)interpolate_real_i32;
	frame[(sweep * num_points) + point].imag = (int16_t)interpolate_imag_i32;
}

static void merge_peak_cluster(uint16_t        start_idx,
                               uint16_t        num_peaks,
                               const float    *velocities,
                               const float    *energies,
                               const uint16_t *peak_idxs,
                               float          *merged_velocities,
                               float          *merged_energies,
                               uint16_t        cluster_count)
{
	float min = INFINITY;
	float max = -INFINITY;

	for (uint16_t i = 0U; i < num_peaks; i++)
	{
		merged_velocities[cluster_count] += velocities[peak_idxs[start_idx + i]];
		merged_energies[cluster_count]   += energies[peak_idxs[start_idx + i]];

		min = fminf(velocities[peak_idxs[start_idx + i]], min);

		max = fmaxf(velocities[peak_idxs[start_idx + i]], max);
	}

	merged_velocities[cluster_count] /= (float)num_peaks;
	merged_energies[cluster_count]   /= (float)num_peaks;
}

static float max_measurable_dist(acc_config_prf_t prf)
{
	float mmd;

	switch (prf)
	{
		case ACC_CONFIG_PRF_19_5_MHZ:
			mmd = 3.1f;
			break;
		case ACC_CONFIG_PRF_15_6_MHZ:
			mmd = 5.1f;
			break;
		case ACC_CONFIG_PRF_13_0_MHZ:
			mmd = 7.0f;
			break;
		case ACC_CONFIG_PRF_8_7_MHZ:
			mmd = 12.7f;
			break;
		case ACC_CONFIG_PRF_6_5_MHZ:
			mmd = 18.5f;
			break;
		case ACC_CONFIG_PRF_5_2_MHZ:
			mmd = 24.2f;
			break;
		default:
			mmd = 0.0f;
			break;
	}

	return mmd;
}

static acc_config_profile_t get_profile(uint16_t value)
{
	acc_config_profile_t profile = ACC_CONFIG_PROFILE_3;

	switch (value)
	{
		case 1U:
			profile = ACC_CONFIG_PROFILE_1;
			break;

		case 2U:
			profile = ACC_CONFIG_PROFILE_2;
			break;

		case 4U:
			profile = ACC_CONFIG_PROFILE_4;
			break;

		case 5U:
			profile = ACC_CONFIG_PROFILE_5;
			break;

		default:
			break;
	}

	return profile;
}

static void sort_i16(int16_t *array, uint16_t array_length)
{
	for (uint16_t i = 0; i < (array_length - 1U); i++)
	{
		for (uint16_t j = 0; j < (array_length - i - 1U); j++)
		{
			if (array[j] > array[j + 1U])
			{
				swap_i16(array, j, j + 1U);
			}
		}
	}
}

static void swap_i16(int16_t *array, uint16_t idx_a, uint16_t idx_b)
{
	int16_t tmp = array[idx_a];

	array[idx_a] = array[idx_b];
	array[idx_b] = tmp;
}

static void sort_f32(float *array, uint16_t array_length)
{
	for (uint16_t i = 0; i < (array_length - 1U); i++)
	{
		for (uint16_t j = 0; j < (array_length - i - 1U); j++)
		{
			if (array[j] > array[j + 1U])
			{
				swap_f32(array, j, j + 1U);
			}
		}
	}
}

static void swap_f32(float *array, uint16_t idx_a, uint16_t idx_b)
{
	float tmp = array[idx_a];

	array[idx_a] = array[idx_b];
	array[idx_b] = tmp;
}
