// Copyright (c) Acconeer AB, 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 <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include "acc_algorithm.h"
#include "acc_config.h"
#include "acc_definitions_a121.h"
#include "acc_definitions_common.h"
#include "acc_hal_integration_a121.h"
#include "acc_integration.h"
#include "acc_integration_log.h"
#include "acc_processing.h"
#include "acc_rss_a121.h"
#include "acc_sensor.h"
#include "ref_app_parking.h"

#define CAL_ITERATIONS (3U)
#define MAX_AMPLITUDE  (46341.0f) // sqrt((2^15)^2 + (2^15)^2)

typedef struct
{
	float energy;
	float weighted_distance;
} signature_t;

struct ref_app_parking_handle
{
	// config
	ref_app_parking_config_t parking_config;
	acc_config_t            *sensor_config;

	// sensor handling
	acc_sensor_id_t  sensor_id;
	acc_sensor_t    *sensor;
	void            *buffer;
	uint32_t         buffer_size;
	acc_cal_result_t cal_result;
	uint32_t         measure_timeout_ms;

	// noise calibration
	float  *noise_magnitudes;
	int16_t noise_cal_temp;
	float   noise_level;

	// obstruction calibration and processing
	float      *obstruction_distances;
	float      *obstruction_magnitudes;
	signature_t obstruction_center;
	float       obstruction_noise_level;
	signature_t obstruction_lp_signature;
	float       obstruction_lp_const;
	float       obstruction_energy_threshold;
	float       obstruction_weighted_distance_threshold;

	// processing and result
	float                    *distances;
	float                    *norm_distances;
	float                    *magnitudes;
	signature_t              *signature_history;
	signature_t              *signature_history_sorted;
	acc_processing_t         *proc;
	acc_processing_metadata_t proc_meta;
	acc_processing_result_t   proc_result;
};

/**
 * @brief Set sensor config using already set parking config
 *
 * @param[in] handle The parking handle
 */
static void set_sensor_config(ref_app_parking_handle_t *handle);

/**
 * @brief Log config
 *
 * @param[in] handle The parking handle
 */
static void log_config(ref_app_parking_handle_t *handle);

/**
 * @brief Allocate resources for application
 *
 * @param[in] handle The parking handle
 */
static bool allocate_application_resources(ref_app_parking_handle_t *handle);

/**
 * @brief Initialize resources for application using already set parking config
 *
 * @param[in] handle The parking handle
 */
static void initialize_application_resources(ref_app_parking_handle_t *handle);

/**
 * @brief Get timeout value, in ms, for specified update rate
 *
 * @param[in] update_rate_hz The update rate, in Hz
 * @return The timeout value, in ms
 */
static uint32_t get_measure_timeout_ms(float update_rate_hz);

/**
 * @brief Calculate noise level from noise data
 *
 * @param[in] noise_data The noise data
 * @param[in] noise_magnitudes Intermediate step in processing will be stored here
 * @param[in] noise_data_length The length of noise data and magnitudes
 * @return The noise level
 */
static float noise_process(acc_int16_complex_t *noise_data, float *noise_magnitudes, uint16_t noise_data_length);

/**
 * @brief Calculate noise mean from obstruction noise data
 *
 * @param[in] obs_noise_data The obstruction noise data
 * @param[in] obs_noise_data_length The length of obstruction noise data
 * @return The noise mean
 */
static float obstruction_noise(acc_int16_complex_t *obs_noise_data, uint16_t obs_noise_data_length);

/**
 * @brief Calculate obstruction signature from obstruction data
 *
 * @param[in] subsweep The obstruction data
 * @param[in] magnitudes Intermediate step in processing will be stored here
 * @param[in] distances Corresponding distance for each point in the obstruction data
 * @param[in] length The length of subsweep, magnitudes, and distances
 * @param[in] noise_level The current noise level to compensate for
 * @param[out] signature The calculated signature
 */
static void obstruction_signature(acc_int16_complex_t *subsweep,
                                  float               *magnitudes,
                                  float               *distances,
                                  uint16_t             length,
                                  float                noise_level,
                                  signature_t         *signature);

/**
 * @brief Calculate car signature from car data
 *
 * @param[in] magnitudes The car data
 * @param[in] distances Corresponding distance for each point in the car data
 * @param[in] length The length of magnitudes and distances
 * @param[out] signature The calculated signature
 */
static void car_signature(const float *magnitudes, const float *distances, uint16_t length, signature_t *signature);

/**
 * @brief Determine if there are any objects present using signature history and thresholds
 *
 * @param[in] handle The parking handle
 * @return true if objects present, false otherwise
 */
static bool objects_present(const ref_app_parking_handle_t *handle);

/**
 * @brief Determine if multiple signatures relates to same object
 *
 * @param[in] handle The parking handle
 * @return true if multiple signatures relates to same object, false otherwise
 */
static bool same_objects(ref_app_parking_handle_t *handle);

/**
 * @brief Function used to compare two signatures for use in qsort
 *
 * @param[in] a First signature
 * @param[in] b Second signature
 * @return -1 if a is closer to sensor than b
 *         1 if b is closer to sensor than a
 *         0 if a and b is at same distance from sensor
 */
static int compare_signature(const void *a, const void *b);

/**
 * @brief Make sensor enter hibernate
 *
 * @param[in] handle The parking handle
 */
static bool enter_hibernate(ref_app_parking_handle_t *handle);

/**
 * @brief Make sensor exit hibernate
 *
 * @param[in] handle The parking handle
 */
static bool exit_hibernate(ref_app_parking_handle_t *handle);

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

void ref_app_parking_set_config(ref_app_parking_parking_preset_t preset, ref_app_parking_config_t *parking_config)
{
	parking_config->preset = preset;

	switch (parking_config->preset)
	{
		case PARKING_PRESET_NONE:
			break;
		case PARKING_PRESET_GROUND:
			parking_config->frame_rate                    = 0.1f;
			parking_config->obstruction_detection_enabled = true;

			parking_config->car_config.range_start_m                  = 0.1f;
			parking_config->car_config.range_end_m                    = 0.4f;
			parking_config->car_config.hwaas                          = 24;
			parking_config->car_config.profile                        = ACC_CONFIG_PROFILE_1;
			parking_config->car_config.subsweep_index                 = 0U;
			parking_config->car_config.queue_length_n                 = 3U;
			parking_config->car_config.amplitude_threshold            = 8.0f;
			parking_config->car_config.weighted_distance_threshold_m  = 0.1f;
			parking_config->car_config.signature_similarity_threshold = 0.6f;

			parking_config->obstruction_config.range_start_m  = 0.03f;
			parking_config->obstruction_config.range_end_m    = 0.05f;
			parking_config->obstruction_config.hwaas          = 16;
			parking_config->obstruction_config.profile        = ACC_CONFIG_PROFILE_1;
			parking_config->obstruction_config.subsweep_index = 1U;
			parking_config->obstruction_config.threshold      = 0.06f;
			parking_config->obstruction_config.time_constant  = 0.8f;
			break;
		case PARKING_PRESET_POLE:
			parking_config->frame_rate                    = 6.0f;
			parking_config->obstruction_detection_enabled = false;

			parking_config->car_config.range_start_m                  = 0.2f;
			parking_config->car_config.range_end_m                    = 3.0f;
			parking_config->car_config.hwaas                          = 24;
			parking_config->car_config.profile                        = ACC_CONFIG_PROFILE_2;
			parking_config->car_config.subsweep_index                 = 0U;
			parking_config->car_config.queue_length_n                 = 20U;
			parking_config->car_config.amplitude_threshold            = 6.0f;
			parking_config->car_config.weighted_distance_threshold_m  = 0.9f;
			parking_config->car_config.signature_similarity_threshold = 0.6f;
			break;
		default:
			break;
	}

	parking_config->frame_rate_app_driven = true;
}

ref_app_parking_handle_t *ref_app_parking_handle_create(ref_app_parking_config_t *parking_config, acc_sensor_id_t sensor_id)
{
	ref_app_parking_handle_t *handle = acc_integration_mem_alloc(sizeof(*handle));

	if (handle != NULL)
	{
		handle->parking_config = *parking_config;
		handle->sensor_id      = sensor_id;

		handle->sensor_config = acc_config_create();
		if (handle->sensor_config == NULL)
		{
			ref_app_parking_handle_destroy(handle);
			return NULL;
		}

		set_sensor_config(handle);

		log_config(handle);

		if (!allocate_application_resources(handle))
		{
			ref_app_parking_handle_destroy(handle);
			return NULL;
		}

		initialize_application_resources(handle);
	}

	return handle;
}

void ref_app_parking_handle_destroy(ref_app_parking_handle_t *handle)
{
	if (handle != NULL)
	{
		acc_hal_integration_sensor_disable(handle->sensor_id);
		acc_hal_integration_sensor_supply_off(handle->sensor_id);

		if (handle->sensor != NULL)
		{
			acc_sensor_destroy(handle->sensor);
		}

		if (handle->signature_history_sorted != NULL)
		{
			acc_integration_mem_free(handle->signature_history_sorted);
		}

		if (handle->signature_history != NULL)
		{
			acc_integration_mem_free(handle->signature_history);
		}

		if (handle->magnitudes != NULL)
		{
			acc_integration_mem_free(handle->magnitudes);
		}

		if (handle->norm_distances != NULL)
		{
			acc_integration_mem_free(handle->norm_distances);
		}

		if (handle->distances != NULL)
		{
			acc_integration_mem_free(handle->distances);
		}

		if (handle->obstruction_magnitudes != NULL)
		{
			acc_integration_mem_free(handle->obstruction_magnitudes);
		}

		if (handle->obstruction_distances != NULL)
		{
			acc_integration_mem_free(handle->obstruction_distances);
		}

		if (handle->noise_magnitudes != NULL)
		{
			acc_integration_mem_free(handle->noise_magnitudes);
		}

		if (handle->buffer != NULL)
		{
			acc_integration_mem_free(handle->buffer);
		}

		if (handle->proc != NULL)
		{
			acc_processing_destroy(handle->proc);
		}

		if (handle->sensor_config != NULL)
		{
			acc_config_destroy(handle->sensor_config);
		}

		acc_integration_mem_free(handle);
	}
}

bool ref_app_parking_sensor_calibration(ref_app_parking_handle_t *handle)
{
	bool           status                      = false;
	bool           cal_complete                = false;
	const uint16_t calibration_retries         = 1U;
	const uint32_t calibration_step_timeout_ms = 500U;

	// Random disturbances may cause the calibration to fail. At failure, retry at least once.
	for (uint16_t i = 0; !status && (i <= calibration_retries); i++)
	{
		// Reset sensor before calibration by disabling/enabling it
		acc_hal_integration_sensor_disable(handle->sensor_id);
		acc_hal_integration_sensor_enable(handle->sensor_id);

		do
		{
			status = acc_sensor_calibrate(handle->sensor, &cal_complete, &handle->cal_result, handle->buffer, handle->buffer_size);

			if (status && !cal_complete)
			{
				status = acc_hal_integration_wait_for_sensor_interrupt(handle->sensor_id, calibration_step_timeout_ms);
			}
		} while (status && !cal_complete);
	}

	if (status)
	{
		// Reset sensor after calibration by disabling/enabling it
		acc_hal_integration_sensor_disable(handle->sensor_id);
		acc_hal_integration_sensor_enable(handle->sensor_id);
	}

	return status;
}

bool ref_app_parking_noise_calibration(ref_app_parking_handle_t *handle)
{
	// The config to be used for calibration is the same as the 'car config' but with TX off
	// and a frame rate corresponding to a total measurement time of ~1s.
	uint16_t noise_subsweep_index = handle->parking_config.car_config.subsweep_index;

	acc_config_subsweep_enable_tx_set(handle->sensor_config, false, noise_subsweep_index);
	acc_config_frame_rate_set(handle->sensor_config, (float)CAL_ITERATIONS);
	handle->measure_timeout_ms = get_measure_timeout_ms((float)CAL_ITERATIONS);

	if (!acc_sensor_prepare(handle->sensor, handle->sensor_config, &handle->cal_result, handle->buffer, handle->buffer_size))
	{
		printf("acc_sensor_prepare() failed\n");
		return false;
	}

	int16_t noise_cal_temps[CAL_ITERATIONS] = {0};
	float   noise_level_sum                 = 0.0f;

	uint16_t noise_data_offset = handle->proc_meta.subsweep_data_offset[noise_subsweep_index];
	uint16_t noise_data_length = handle->proc_meta.subsweep_data_length[noise_subsweep_index];

	for (uint16_t i = 0; i < CAL_ITERATIONS; i++)
	{
		if (!ref_app_parking_measure(handle, false))
		{
			return false;
		}

		// Only a few measurements and recently calibrated - assume indications ok
		noise_cal_temps[i] = handle->proc_result.temperature;

		noise_level_sum += noise_process(&handle->proc_result.frame[noise_data_offset], handle->noise_magnitudes, noise_data_length);
	}

	handle->noise_cal_temp = acc_algorithm_median_i16(noise_cal_temps, CAL_ITERATIONS);
	handle->noise_level    = noise_level_sum / (float)CAL_ITERATIONS;
	handle->noise_level    = (handle->noise_level == 0.0f) ? 1.0f : handle->noise_level;

	// Restore the 'car config'
	acc_config_subsweep_enable_tx_set(handle->sensor_config, true, noise_subsweep_index);
	if (handle->parking_config.frame_rate_app_driven)
	{
		acc_config_frame_rate_set(handle->sensor_config, 0.0f);
	}
	else
	{
		acc_config_frame_rate_set(handle->sensor_config, handle->parking_config.frame_rate);
	}

	handle->measure_timeout_ms = get_measure_timeout_ms(handle->parking_config.frame_rate);

	return true;
}

bool ref_app_parking_obstruction_calibration(ref_app_parking_handle_t *handle)
{
	acc_config_frame_rate_set(handle->sensor_config, (float)CAL_ITERATIONS);
	handle->measure_timeout_ms = get_measure_timeout_ms((float)CAL_ITERATIONS);

	uint16_t obs_cal_subsweep_index = handle->parking_config.obstruction_config.subsweep_index;
	uint16_t obs_cal_data_offset    = handle->proc_meta.subsweep_data_offset[obs_cal_subsweep_index];
	uint16_t obs_cal_data_length    = handle->proc_meta.subsweep_data_length[obs_cal_subsweep_index];

	// Noise calibration without TX
	acc_config_subsweep_enable_tx_set(handle->sensor_config, false, handle->parking_config.obstruction_config.subsweep_index);

	if (!acc_sensor_prepare(handle->sensor, handle->sensor_config, &handle->cal_result, handle->buffer, handle->buffer_size))
	{
		printf("acc_sensor_prepare() failed\n");
		return false;
	}

	float noise_levels[CAL_ITERATIONS] = {0.0f};
	float noise_level_sum              = 0.0f;

	for (uint16_t i = 0; i < CAL_ITERATIONS; i++)
	{
		if (!ref_app_parking_measure(handle, false))
		{
			return false;
		}

		// Only a few measurements and recently calibrated - assume indications ok

		noise_levels[i]  = obstruction_noise(&handle->proc_result.frame[obs_cal_data_offset], obs_cal_data_length);
		noise_level_sum += noise_levels[i];
	}

	handle->obstruction_noise_level = noise_level_sum / CAL_ITERATIONS;

	// Signature calibration with TX - sensitive to environment.
	// Must be done with sensor free from obstruction and no car present.
	acc_config_subsweep_enable_tx_set(handle->sensor_config, true, handle->parking_config.obstruction_config.subsweep_index);

	if (!acc_sensor_prepare(handle->sensor, handle->sensor_config, &handle->cal_result, handle->buffer, handle->buffer_size))
	{
		printf("acc_sensor_prepare() failed\n");
		return false;
	}

	signature_t signature             = {0};
	float       energy_sum            = 0.0f;
	float       weighted_distance_sum = 0.0f;

	for (uint16_t i = 0; i < CAL_ITERATIONS; i++)
	{
		if (!ref_app_parking_measure(handle, false))
		{
			return false;
		}

		// Only a few measurements and recently calibrated - assume indications ok

		acc_int16_complex_t *subsweep = &handle->proc_result.frame[obs_cal_data_offset];

		obstruction_signature(subsweep,
		                      handle->obstruction_magnitudes,
		                      handle->obstruction_distances,
		                      obs_cal_data_length,
		                      noise_levels[i],
		                      &signature);

		energy_sum            += signature.energy;
		weighted_distance_sum += signature.weighted_distance;
	}

	handle->obstruction_center.energy            = energy_sum / CAL_ITERATIONS;
	handle->obstruction_center.weighted_distance = weighted_distance_sum / CAL_ITERATIONS;

	handle->obstruction_lp_signature = handle->obstruction_center;

	// Restore the 'obstruction config'
	if (handle->parking_config.frame_rate_app_driven)
	{
		acc_config_frame_rate_set(handle->sensor_config, 0.0f);
	}
	else
	{
		acc_config_frame_rate_set(handle->sensor_config, handle->parking_config.frame_rate);
	}

	handle->measure_timeout_ms = get_measure_timeout_ms(handle->parking_config.frame_rate);

	return true;
}

bool ref_app_parking_sensor_prepare(ref_app_parking_handle_t *handle)
{
	if (!acc_sensor_prepare(handle->sensor, handle->sensor_config, &handle->cal_result, handle->buffer, handle->buffer_size))
	{
		printf("acc_sensor_prepare failed\n");
		return false;
	}

	if (!enter_hibernate(handle))
	{
		printf("enter_hibernate failed\n");
		return false;
	}

	return true;
}

bool ref_app_parking_measure(ref_app_parking_handle_t *handle, bool hibernate)
{
	if (hibernate)
	{
		if (!exit_hibernate(handle))
		{
			printf("exit_hibernate failed\n");
			return false;
		}
	}

	if (!acc_sensor_measure(handle->sensor))
	{
		printf("acc_sensor_measure failed\n");
		return false;
	}

	if (!acc_hal_integration_wait_for_sensor_interrupt(handle->sensor_id, handle->measure_timeout_ms))
	{
		printf("Sensor interrupt timeout\n");
		return false;
	}

	if (!acc_sensor_read(handle->sensor, handle->buffer, handle->buffer_size))
	{
		printf("acc_sensor_read failed\n");
		return false;
	}

	if (hibernate)
	{
		if (!enter_hibernate(handle))
		{
			printf("enter_hibernate failed\n");
			return false;
		}
	}

	acc_processing_execute(handle->proc, handle->buffer, &handle->proc_result);

	return true;
}

bool ref_app_parking_handle_indications(ref_app_parking_handle_t *handle, bool *data_reliable)
{
	bool status = true;

	*data_reliable = true;

	if (handle->proc_result.data_saturated)
	{
		*data_reliable = false;

		printf("Data saturated. Try to reduce the sensor gain.\n");
	}

	if (handle->proc_result.frame_delayed)
	{
		printf("Frame delayed. Could not read data fast enough.\n");
		printf("Try lowering the frame rate or call 'acc_sensor_read' more frequently.\n");
	}

	if (handle->proc_result.calibration_needed)
	{
		printf("The current calibration is not valid for the current temperature.\n");
		printf("Re-calibrating sensor...\n");

		if (!exit_hibernate(handle))
		{
			printf("exit_hibernate failed\n");
			status = false;
		}

		if (status)
		{
			if (!ref_app_parking_sensor_calibration(handle))
			{
				printf("ref_app_parking_sensor_calibration() failed\n");
				acc_sensor_status(handle->sensor);
				status = false;
			}
		}

		if (status)
		{
			if (!acc_sensor_prepare(handle->sensor, handle->sensor_config, &handle->cal_result, handle->buffer, handle->buffer_size))
			{
				printf("acc_sensor_prepare() failed\n");
				status = false;
			}
		}

		if (status)
		{
			if (!enter_hibernate(handle))
			{
				printf("enter_hibernate failed\n");
				status = false;
			}
		}

		*data_reliable = false;

		printf("The sensor was successfully re-calibrated.\n");
	}

	return status;
}

void ref_app_parking_obstruction_process(ref_app_parking_handle_t *handle, bool *obstruction_detected)
{
	ref_app_parking_config_t             *parking_config     = &handle->parking_config;
	acc_config_t                         *sensor_config      = handle->sensor_config;
	ref_app_parking_obstruction_config_t *obstruction_config = &parking_config->obstruction_config;

	acc_config_profile_t profile = acc_config_subsweep_profile_get(sensor_config, obstruction_config->subsweep_index);

	float signal_adjust_factor;
	float deviation_adjust_factor;

	acc_processing_get_temperature_adjustment_factors(handle->noise_cal_temp,
	                                                  handle->proc_result.temperature,
	                                                  profile,
	                                                  &signal_adjust_factor,
	                                                  &deviation_adjust_factor);

	float noise_level_adjusted = handle->obstruction_noise_level * deviation_adjust_factor;

	acc_int16_complex_t *subsweep       = &handle->proc_result.frame[handle->proc_meta.subsweep_data_offset[obstruction_config->subsweep_index]];
	uint16_t             num_points     = acc_config_subsweep_num_points_get(sensor_config, obstruction_config->subsweep_index);
	signature_t          curr_signature = {0};

	// Adjust for temperature changes
	for (uint16_t i = 0; i < num_points; i++)
	{
		subsweep[i].real = ((float)subsweep[i].real / signal_adjust_factor) + 0.5f;
		subsweep[i].imag = ((float)subsweep[i].imag / signal_adjust_factor) + 0.5f;
	}

	// Extract signature
	obstruction_signature(subsweep, handle->obstruction_magnitudes, handle->obstruction_distances, num_points, noise_level_adjusted, &curr_signature);

	// Low-pass filter of signature
	signature_t *lp_signature = &handle->obstruction_lp_signature;
	float        lp_const     = handle->obstruction_lp_const;

	lp_signature->energy            = lp_signature->energy * lp_const + (1.0f - lp_const) * curr_signature.energy;
	lp_signature->weighted_distance = lp_signature->weighted_distance * lp_const + (1.0f - lp_const) * curr_signature.weighted_distance;

	// Compare against thresholds
	float energy_diff            = fabsf(lp_signature->energy - handle->obstruction_center.energy);
	float weighted_distance_diff = fabsf(lp_signature->weighted_distance - handle->obstruction_center.weighted_distance);

	*obstruction_detected =
	    (energy_diff > handle->obstruction_energy_threshold) || (weighted_distance_diff > handle->obstruction_weighted_distance_threshold);
}

void ref_app_parking_process(ref_app_parking_handle_t *handle, bool *car_detected)
{
	ref_app_parking_config_t     *parking_config = &handle->parking_config;
	acc_config_t                 *sensor_config  = handle->sensor_config;
	ref_app_parking_car_config_t *car_config     = &parking_config->car_config;

	acc_int16_complex_t *subsweep = &handle->proc_result.frame[handle->proc_meta.subsweep_data_offset[car_config->subsweep_index]];

	uint16_t             num_points = acc_config_subsweep_num_points_get(sensor_config, car_config->subsweep_index);
	acc_config_profile_t profile    = acc_config_subsweep_profile_get(sensor_config, car_config->subsweep_index);

	float signal_adjust_factor;
	float deviation_adjust_factor;

	acc_processing_get_temperature_adjustment_factors(handle->noise_cal_temp,
	                                                  handle->proc_result.temperature,
	                                                  profile,
	                                                  &signal_adjust_factor,
	                                                  &deviation_adjust_factor);

	float noise_level_adjusted = handle->noise_level * deviation_adjust_factor;

	for (uint16_t i = 0; i < num_points; i++)
	{
		handle->magnitudes[i] = sqrtf((float)subsweep[i].real * (float)subsweep[i].real + (float)subsweep[i].imag * (float)subsweep[i].imag);

		// Compensate for temperature changes
		handle->magnitudes[i] /= noise_level_adjusted;

		// Remove noise
		handle->magnitudes[i] = (handle->magnitudes[i] - 1.0f < 0.0f) ? 0.0f : handle->magnitudes[i] - 1.0f;

		// Scale with normalized distance
		handle->magnitudes[i] *= handle->norm_distances[i];
	}

	for (uint16_t i = 1; i < car_config->queue_length_n; i++)
	{
		handle->signature_history[i - 1] = handle->signature_history[i];
	}

	car_signature(handle->magnitudes, handle->distances, num_points, &handle->signature_history[car_config->queue_length_n - 1]);

	bool obj_present = objects_present(handle);
	bool same_obj    = same_objects(handle);

	*car_detected = obj_present && same_obj;
}

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

static void set_sensor_config(ref_app_parking_handle_t *handle)
{
	ref_app_parking_config_t *parking_config = &handle->parking_config;
	acc_config_t             *sensor_config  = handle->sensor_config;

	// Common settings
	acc_config_sweeps_per_frame_set(sensor_config, 1U); // This should not be changed
	if (parking_config->frame_rate_app_driven)
	{
		acc_config_frame_rate_set(sensor_config, 0.0f);
	}
	else
	{
		acc_config_frame_rate_set(sensor_config, parking_config->frame_rate);
	}

	uint8_t num_subsweeps = parking_config->obstruction_detection_enabled ? 2U : 1U;

	acc_config_num_subsweeps_set(sensor_config, num_subsweeps);

	// Car settings
	ref_app_parking_car_config_t *car_config = &parking_config->car_config;

	int32_t start_point = (int32_t)roundf(car_config->range_start_m / ACC_APPROX_BASE_STEP_LENGTH_M);

	// Handling of step_length based on profile and constraint in RSS API
	uint16_t step_length       = 0U;
	float    fwhm              = acc_algorithm_get_fwhm(car_config->profile);
	uint16_t step_length_ideal = (uint16_t)roundf(fwhm / ACC_APPROX_BASE_STEP_LENGTH_M);

	if (step_length_ideal >= 24)
	{
		step_length = (step_length_ideal / 24U) * 24U;
	}
	else
	{
		step_length = step_length_ideal;
		while (24U % step_length != 0U)
		{
			step_length--;
		}
	}

	uint16_t num_points = (uint16_t)ceilf((car_config->range_end_m - car_config->range_start_m) / (step_length * ACC_APPROX_BASE_STEP_LENGTH_M));

	acc_config_subsweep_start_point_set(sensor_config, start_point, car_config->subsweep_index);
	acc_config_subsweep_step_length_set(sensor_config, step_length, car_config->subsweep_index);
	acc_config_subsweep_num_points_set(sensor_config, num_points, car_config->subsweep_index);
	acc_config_subsweep_hwaas_set(sensor_config, car_config->hwaas, car_config->subsweep_index);
	acc_config_subsweep_profile_set(sensor_config, car_config->profile, car_config->subsweep_index);

	// Obstruction settings
	if (parking_config->obstruction_detection_enabled)
	{
		ref_app_parking_obstruction_config_t *obstruction_config = &parking_config->obstruction_config;

		start_point = (int32_t)roundf(obstruction_config->range_start_m / ACC_APPROX_BASE_STEP_LENGTH_M);
		step_length = 2U;

		num_points =
		    (uint16_t)ceilf((obstruction_config->range_end_m - obstruction_config->range_start_m) / (step_length * ACC_APPROX_BASE_STEP_LENGTH_M));

		acc_config_subsweep_start_point_set(sensor_config, start_point, obstruction_config->subsweep_index);
		acc_config_subsweep_step_length_set(sensor_config, step_length, obstruction_config->subsweep_index);
		acc_config_subsweep_num_points_set(sensor_config, num_points, obstruction_config->subsweep_index);
		acc_config_subsweep_hwaas_set(sensor_config, obstruction_config->hwaas, obstruction_config->subsweep_index);
		acc_config_subsweep_profile_set(sensor_config, obstruction_config->profile, obstruction_config->subsweep_index);
	}
}

static void log_config(ref_app_parking_handle_t *handle)
{
	ref_app_parking_config_t *parking_config = &handle->parking_config;

	printf("\nParking config:\n");

	char *preset_string = NULL;

	switch (parking_config->preset)
	{
		case PARKING_PRESET_NONE:
			preset_string = "None";
			break;
		case PARKING_PRESET_GROUND:
			preset_string = "Ground";
			break;
		case PARKING_PRESET_POLE:
			preset_string = "Pole";
			break;
		default:
			preset_string = "";
			break;
	}

	printf("preset: %s\n", preset_string);
	printf("frame_rate: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->frame_rate));
	printf("obstruction_detection_enabled: %s\n", parking_config->obstruction_detection_enabled ? "true" : "false");

	printf("\nCar config:\n");
	printf("range_start_m: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->car_config.range_start_m));
	printf("range_end_m: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->car_config.range_end_m));
	printf("queue_length_n: %" PRIu16 "\n", parking_config->car_config.queue_length_n);
	printf("amplitude_threshold: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->car_config.amplitude_threshold));
	printf("weighted_distance_threshold_m: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->car_config.weighted_distance_threshold_m));
	printf("signature_similarity_threshold: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->car_config.signature_similarity_threshold));

	if (parking_config->obstruction_detection_enabled)
	{
		printf("\nObstruction config:\n");
		printf("range_start_m: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->obstruction_config.range_start_m));
		printf("range_end_m: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->obstruction_config.range_end_m));
		printf("threshold: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->obstruction_config.threshold));
		printf("time_constant: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(parking_config->obstruction_config.time_constant));
	}

	printf("\nSensor config:\n");
	acc_config_log(handle->sensor_config);
}

static bool allocate_application_resources(ref_app_parking_handle_t *handle)
{
	ref_app_parking_config_t *parking_config = &handle->parking_config;
	acc_config_t             *sensor_config  = handle->sensor_config;

	handle->proc = acc_processing_create(handle->sensor_config, &handle->proc_meta);
	if (handle->proc == NULL)
	{
		return false;
	}

	if (!acc_rss_get_buffer_size(sensor_config, &handle->buffer_size))
	{
		return false;
	}

	handle->buffer = acc_integration_mem_alloc(handle->buffer_size);
	if (handle->buffer == NULL)
	{
		return false;
	}

	uint16_t car_num_points = acc_config_subsweep_num_points_get(sensor_config, parking_config->car_config.subsweep_index);
	uint16_t obs_num_points = acc_config_subsweep_num_points_get(sensor_config, parking_config->obstruction_config.subsweep_index);

	handle->noise_magnitudes = acc_integration_mem_alloc(car_num_points * sizeof(*handle->noise_magnitudes));
	if (handle->noise_magnitudes == NULL)
	{
		return false;
	}

	handle->obstruction_magnitudes = acc_integration_mem_alloc(obs_num_points * sizeof(*handle->obstruction_magnitudes));
	if (handle->obstruction_magnitudes == NULL)
	{
		return false;
	}

	handle->obstruction_distances = acc_integration_mem_alloc(obs_num_points * sizeof(*handle->obstruction_distances));
	if (handle->obstruction_distances == NULL)
	{
		return false;
	}

	handle->distances = acc_integration_mem_alloc(car_num_points * sizeof(*handle->distances));
	if (handle->distances == NULL)
	{
		return false;
	}

	handle->norm_distances = acc_integration_mem_alloc(car_num_points * sizeof(*handle->norm_distances));
	if (handle->norm_distances == NULL)
	{
		return false;
	}

	handle->magnitudes = acc_integration_mem_alloc(car_num_points * sizeof(*handle->magnitudes));
	if (handle->magnitudes == NULL)
	{
		return false;
	}

	uint16_t queue_length_n = parking_config->car_config.queue_length_n;

	handle->signature_history = acc_integration_mem_alloc(queue_length_n * sizeof(*handle->signature_history));
	if (handle->signature_history == NULL)
	{
		return false;
	}

	handle->signature_history_sorted = acc_integration_mem_alloc(queue_length_n * sizeof(*handle->signature_history_sorted));
	if (handle->signature_history_sorted == NULL)
	{
		return false;
	}

	acc_hal_integration_sensor_supply_on(handle->sensor_id);
	acc_hal_integration_sensor_enable(handle->sensor_id);

	handle->sensor = acc_sensor_create(handle->sensor_id);
	if (handle->sensor == NULL)
	{
		return false;
	}

	return true;
}

static void initialize_application_resources(ref_app_parking_handle_t *handle)
{
	ref_app_parking_config_t             *parking_config     = &handle->parking_config;
	acc_config_t                         *sensor_config      = handle->sensor_config;
	ref_app_parking_obstruction_config_t *obstruction_config = &parking_config->obstruction_config;
	ref_app_parking_car_config_t         *car_config         = &parking_config->car_config;

	handle->measure_timeout_ms = get_measure_timeout_ms(parking_config->frame_rate);

	float base_step_length = acc_processing_points_to_meter(1);

	// Obstruction distances
	uint8_t subsweep_index = obstruction_config->subsweep_index;

	uint16_t num_points  = acc_config_subsweep_num_points_get(sensor_config, subsweep_index);
	int32_t  start_point = acc_config_subsweep_start_point_get(sensor_config, subsweep_index);
	uint16_t step_length = acc_config_subsweep_step_length_get(sensor_config, subsweep_index);

	for (uint16_t i = 0; i < num_points; i++)
	{
		handle->obstruction_distances[i] = (float)(start_point + i * step_length) * base_step_length;
	}

	float actual_distance_range = handle->obstruction_distances[num_points - 1U] - handle->obstruction_distances[0];

	handle->obstruction_lp_const = acc_algorithm_exp_smoothing_coefficient(parking_config->frame_rate, obstruction_config->time_constant);

	handle->obstruction_energy_threshold            = MAX_AMPLITUDE * obstruction_config->threshold;
	handle->obstruction_weighted_distance_threshold = actual_distance_range * obstruction_config->threshold;

	// Car distances
	subsweep_index = car_config->subsweep_index;

	num_points  = acc_config_subsweep_num_points_get(sensor_config, subsweep_index);
	start_point = acc_config_subsweep_start_point_get(sensor_config, subsweep_index);
	step_length = acc_config_subsweep_step_length_get(sensor_config, subsweep_index);

	for (uint16_t i = 0; i < num_points; i++)
	{
		handle->distances[i]      = (float)(start_point + i * step_length) * base_step_length;
		handle->norm_distances[i] = handle->distances[i] / handle->distances[0];
	}

	memset(handle->signature_history, 0, car_config->queue_length_n * sizeof(*handle->signature_history));
	memset(handle->signature_history_sorted, 0, car_config->queue_length_n * sizeof(*handle->signature_history_sorted));
}

static uint32_t get_measure_timeout_ms(float update_rate_hz)
{
	return (uint32_t)((1.0f / update_rate_hz) * 1000.0f) + 1000U;
}

static float noise_process(acc_int16_complex_t *noise_data, float *noise_magnitudes, uint16_t noise_data_length)
{
	float noise_mean = 0.0f;

	for (uint16_t i = 0; i < noise_data_length; i++)
	{
		noise_magnitudes[i]  = sqrtf((float)noise_data[i].real * (float)noise_data[i].real + (float)noise_data[i].imag * (float)noise_data[i].imag);
		noise_mean          += noise_magnitudes[i];
	}

	noise_mean /= noise_data_length;

	float pow_sum = 0.0f;

	for (uint16_t i = 0; i < noise_data_length; i++)
	{
		pow_sum += (noise_magnitudes[i] - noise_mean) * (noise_magnitudes[i] - noise_mean);
	}

	float noise_std = sqrtf(pow_sum / noise_data_length);

	float noise_level = noise_mean + 2.0f * noise_std;

	return noise_level;
}

static float obstruction_noise(acc_int16_complex_t *obs_noise_data, uint16_t obs_noise_data_length)
{
	float noise_mean = 0.0f;

	for (uint16_t i = 0; i < obs_noise_data_length; i++)
	{
		noise_mean +=
		    sqrtf((float)obs_noise_data[i].real * (float)obs_noise_data[i].real + (float)obs_noise_data[i].imag * (float)obs_noise_data[i].imag);
	}

	noise_mean /= obs_noise_data_length;

	return noise_mean;
}

static void obstruction_signature(acc_int16_complex_t *subsweep,
                                  float               *magnitudes,
                                  float               *distances,
                                  uint16_t             length,
                                  float                noise_level,
                                  signature_t         *signature)
{
	float energy_sum = 0.0f;

	for (uint16_t j = 0; j < length; j++)
	{
		magnitudes[j]  = sqrtf((float)subsweep[j].real * (float)subsweep[j].real + (float)subsweep[j].imag * (float)subsweep[j].imag);
		magnitudes[j] -= noise_level;
		energy_sum    += magnitudes[j];
	}

	signature->energy            = (energy_sum / length);
	signature->weighted_distance = acc_algorithm_weighted_mean(distances, magnitudes, length);
}

static void car_signature(const float *magnitudes, const float *distances, uint16_t length, signature_t *signature)
{
	signature->energy            = acc_algorithm_max_f32(magnitudes, length);
	signature->weighted_distance = acc_algorithm_weighted_mean(distances, magnitudes, length);
}

static bool objects_present(const ref_app_parking_handle_t *handle)
{
	uint16_t queue_length_n                 = handle->parking_config.car_config.queue_length_n;
	float    amplitude_threshold            = handle->parking_config.car_config.amplitude_threshold;
	float    signature_similarity_threshold = handle->parking_config.car_config.signature_similarity_threshold;

	uint16_t trig_count = 0;

	for (uint16_t i = 0; i < queue_length_n; i++)
	{
		if (handle->signature_history[i].energy > amplitude_threshold)
		{
			trig_count++;
		}
	}

	return (float)trig_count > (queue_length_n * signature_similarity_threshold);
}

static bool same_objects(ref_app_parking_handle_t *handle)
{
	uint16_t queue_length_n                 = handle->parking_config.car_config.queue_length_n;
	float    amplitude_threshold            = handle->parking_config.car_config.amplitude_threshold;
	float    weighted_distance_threshold_m  = handle->parking_config.car_config.weighted_distance_threshold_m;
	float    signature_similarity_threshold = handle->parking_config.car_config.signature_similarity_threshold;

	for (uint16_t i = 0; i < queue_length_n; i++)
	{
		handle->signature_history_sorted[i] = handle->signature_history[i];
	}

	qsort(handle->signature_history_sorted, queue_length_n, sizeof(*handle->signature_history_sorted), compare_signature);

	uint16_t active_cluster_count    = 0;
	float    active_cluster_ref_dist = 0.0f;
	uint16_t largest_cluster_count   = 0;

	for (uint16_t i = 0; i < queue_length_n; i++)
	{
		if (handle->signature_history_sorted[i].energy > amplitude_threshold)
		{
			// Start of active cluster
			if (active_cluster_count == 0)
			{
				active_cluster_count++;
				active_cluster_ref_dist = handle->signature_history_sorted[i].weighted_distance;
			}
			// End of active cluster
			else if (handle->signature_history_sorted[i].weighted_distance - active_cluster_ref_dist > weighted_distance_threshold_m)
			{
				if (active_cluster_count > largest_cluster_count)
				{
					largest_cluster_count = active_cluster_count;
				}

				active_cluster_count    = 0;
				active_cluster_ref_dist = 0.0f;
			}
			// Continuation of active cluster
			else
			{
				active_cluster_count++;
			}
		}
	}

	// End last active cluster
	if (active_cluster_count != 0)
	{
		if (active_cluster_count > largest_cluster_count)
		{
			largest_cluster_count = active_cluster_count;
		}
	}

	return (float)largest_cluster_count / (float)queue_length_n > signature_similarity_threshold;
}

static int compare_signature(const void *a, const void *b)
{
	const signature_t *sig_a = (const signature_t *)a;
	const signature_t *sig_b = (const signature_t *)b;

	if (sig_a->weighted_distance < sig_b->weighted_distance)
	{
		return -1;
	}
	else if (sig_a->weighted_distance > sig_b->weighted_distance)
	{
		return 1;
	}
	else
	{
		return 0;
	}
}

static bool enter_hibernate(ref_app_parking_handle_t *handle)
{
	bool status = true;

	if (!acc_sensor_hibernate_on(handle->sensor))
	{
		printf("acc_sensor_hibernate_on failed\n");
		acc_sensor_status(handle->sensor);
		status = false;
	}

	acc_hal_integration_sensor_disable(handle->sensor_id);
	return status;
}

static bool exit_hibernate(ref_app_parking_handle_t *handle)
{
	bool status = true;

	acc_hal_integration_sensor_enable(handle->sensor_id);
	if (!acc_sensor_hibernate_off(handle->sensor))
	{
		printf("acc_sensor_hibernate_off failed\n");
		acc_sensor_status(handle->sensor);
		status = false;
	}

	return status;
}
