// Copyright (c) Acconeer AB, 2020-2022
// 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 <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include "sys_app.h"
#include "acc_detector_distance.h"
#include "acc_hal_definitions.h"
#include "acc_hal_integration.h"
#include "acc_rss.h"
#include "acc_version.h"
#include "yunhorn_sts_sensors.h"
/** \example example_detector_distance.c
 * @brief This is an example on how the distance detector can be used
 * @n
 * The example executes as follows:
 *   - Activate Radar System Software (RSS)
 *   - Create a distance detector configuration
 *   - Update the distance detector configuration
 *   - Create a distance detector using the previously updated configuration
 *   - Destroy the distance detector configuration
 *   - Activate the distance detector
 *   - Get the result and print it 5 times
 *   - Deactivate and destroy the distance detector
 *   - Deactivate Radar System Software (RSS)
 */

#define EXAMPLE_START_M  (0.8f)		//0.2f
#define EXAMPLE_LENGTH_M (1.4f)		//0.4f
#define EXAMPLE_PROFILE  ACC_SERVICE_PROFILE_2		//2
#define EXAMPLE_HWAAS    (63)		//63

volatile distance_measure_cfg_t distance_cfg={1.0, 2.0, 5, 63, 2, 10, 0.5, 1.3, 0.2};
extern float sts_distance_rss_distance;

static void update_configuration(acc_detector_distance_configuration_t distance_configuration);


static void print_distances(acc_detector_distance_result_t *result, uint16_t reflection_count);


int acc_example_detector_distance(int argc, char *argv[]);


int acc_example_detector_distance(int argc, char *argv[])
{
	(void)argc;
	(void)argv;
	//APP_LOG(TS_OFF, VLEVEL_M, "Acconeer software version %s\n", acc_version_get());

	const acc_hal_t *hal = acc_hal_integration_get_implementation();

	if (!acc_rss_activate(hal))
	{
		APP_LOG(TS_OFF, VLEVEL_M, "acc_rss_activate() failed\n");
		return EXIT_FAILURE;
	}
	acc_rss_override_sensor_id_check_at_creation(true);

	acc_detector_distance_configuration_t distance_configuration = acc_detector_distance_configuration_create();

	if (distance_configuration == NULL)
	{
		APP_LOG(TS_OFF, VLEVEL_M, "acc_detector_distance_configuration_create() failed\n");
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}
///
//	acc_detector_distance_handle_t distance_handle = acc_detector_distance_create(distance_configuration);
/*
	if (distance_handle == NULL)
	{
		APP_LOG(TS_OFF, VLEVEL_M, "acc_detector_distance_create() failed\n");
		acc_detector_distance_configuration_destroy(&distance_configuration);
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}
*/
///
	update_configuration(distance_configuration);
	acc_detector_distance_handle_t distance_handle = acc_detector_distance_create(distance_configuration);

	if (distance_handle == NULL)
	{
		APP_LOG(TS_OFF, VLEVEL_M, "acc_detector_distance_create() failed\n");
		acc_detector_distance_configuration_destroy(&distance_configuration);
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}

	acc_detector_distance_configuration_destroy(&distance_configuration);

	if (!acc_detector_distance_activate(distance_handle))
	{
		APP_LOG(TS_OFF, VLEVEL_M, "acc_detector_distance_activate() failed\n");
		acc_detector_distance_destroy(&distance_handle);
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}

	bool                                success         = true;
	const int                           iterations      = 6; //5;
	uint16_t                            number_of_peaks = 5;
	acc_detector_distance_result_t      result[number_of_peaks];
	acc_detector_distance_result_info_t result_info;
	sts_distance_rss_distance			= 0.0;

	for (int i = 0; i < iterations; i++)
	{
		success = acc_detector_distance_get_next(distance_handle, result, number_of_peaks, &result_info);

		if (!success)
		{
			APP_LOG(TS_OFF, VLEVEL_M, "acc_detector_distance_get_next() failed\n");
			break;
		}
		sts_distance_rss_distance += result->distance_m;
		print_distances(result, result_info.number_of_peaks);
	}

	sts_distance_rss_distance = 1000.0f*sts_distance_rss_distance/iterations;

	bool deactivated = acc_detector_distance_deactivate(distance_handle);

	acc_detector_distance_destroy(&distance_handle);

	acc_rss_deactivate();

	if (deactivated && success)
	{
		APP_LOG(TS_OFF, VLEVEL_H, "Application finished OK\n");
		return EXIT_SUCCESS;
	}

	return EXIT_FAILURE;
}


static void update_configuration(acc_detector_distance_configuration_t distance_configuration)
{
	acc_detector_distance_configuration_requested_start_set(distance_configuration, distance_cfg.start_m); //EXAMPLE_START_M);
	acc_detector_distance_configuration_requested_length_set(distance_configuration, distance_cfg.length_m); //EXAMPLE_LENGTH_M);
	acc_detector_distance_configuration_service_profile_set(distance_configuration, distance_cfg.acc_profile); //EXAMPLE_PROFILE);
	acc_detector_distance_configuration_hw_accelerated_average_samples_set(distance_configuration, distance_cfg.hwaas);//EXAMPLE_HWAAS);

	acc_detector_distance_configuration_threshold_sensitivity_set(distance_configuration, distance_cfg.threshold);
	acc_detector_distance_configuration_sweep_averaging_set(distance_configuration,distance_cfg.sweep_average);
	acc_detector_distance_configuration_receiver_gain_set(distance_configuration,distance_cfg.gain);
	acc_detector_distance_configuration_downsampling_factor_set(distance_configuration, distance_cfg.downsampling);

	acc_detector_distance_configuration_threshold_type_set(distance_configuration, ACC_DETECTOR_DISTANCE_THRESHOLD_TYPE_CFAR);
	acc_detector_distance_configuration_record_background_sweeps_set(distance_configuration, 16);

}


static void print_distances(acc_detector_distance_result_t *result, uint16_t reflection_count)
{
	APP_LOG(TS_OFF, VLEVEL_H, "Found %u peaks:\n", (unsigned int)reflection_count);

	for (uint16_t i = 0; i < reflection_count; i++)
	{
		APP_LOG(TS_OFF, VLEVEL_H, "Amplitude %u at %u mm\n", (unsigned int)result[i].amplitude,
		       (unsigned int)(result[i].distance_m * 1000));
	}
}