/* USER CODE BEGIN Header */
/**
 ******************************************************************************
 * @file    yunhorn_sts_distance_rss.c									   *
 * @author  Yunhorn (r) Technology Limited Application Team	               *
 * @brief   Yunhorn (r) SmarToilets (r) Product configuration file.		   *
 ******************************************************************************
 * @attention
 *
 * Copyright (c) 2022 Yunhorn Technology Limited.
 * Copyright (c) 2022 Shenzhen Yunhorn Technology Co., Ltd.
 * All rights reserved.
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */
/* USER CODE END Header */

/* Includes ------------------------------------------------------------------*/ 
#include <stdbool.h>
#include <stdio.h>

/* USER CODE BEGIN Includes */
#include <stdlib.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 "sys_app.h"
#include "yunhorn_sts_prd_conf.h"
#include "yunhorn_sts_sensors.h"
// NEW ADD 2024-06-17
#define DEFAULT_FAR_RANGE_MUR                   ACC_SERVICE_MUR_6
#define DEFAULT_FAR_RANGE_SERVICE_PROFILE       ACC_SERVICE_PROFILE_2
#define DEFAULT_FAR_MAXIMIZE_SIGNAL_ATTENUATION false
// ===================================
#define STS_DISTANCE_START_M  (0.8f)
#define STS_DISTANCE_LENGTH_M (1.4f)
#define STS_DISTANCE_PROFILE  ACC_SERVICE_PROFILE_2
#define STS_DISTANCE_HWAAS    (63)
#define DEFAULT_FAR_RANGE_CFAR_THRESHOLD_GUARD  0.12f
#define DEFAULT_FAR_RANGE_CFAR_THRESHOLD_WINDOW 0.03f
#ifndef MIN
#define MIN( a, b ) ( ( ( a ) < ( b ) ) ? ( a ) : ( b ) )
#endif

#ifndef MAX
#define MAX( a, b ) ( ( ( a ) > ( b ) ) ? ( a ) : ( b ) )
#endif

//volatile distance_measure_cfg_t distance_cfg={1.5, 2.0, 1, 63, 2, 10, 0.5, 1.3, 0.2};
// SPARK FUN EXAMPLE
// GOOD --- volatile distance_measure_cfg_t distance_cfg={0.4, 3.5, 4, 63, 0, 10, 0.5, 1.3, 0.2};
volatile distance_measure_cfg_t distance_cfg={0.8, 3.5, 2, 63, 2, 10, 0.5, 1.3, 0.2};
//volatile distance_measure_cfg_t distance_cfg={1.5, 3.3, 2, 63, 4, 10, 0.8182f, 0.4, 0.2};
extern volatile uint16_t sts_distance_rss_distance, sts_sensor_install_height;

static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration);


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


int sts_distance_rss_detector_distance(void);

int sts_distance_rss_detector_distance(void)
{
	const acc_hal_t *hal = acc_hal_integration_get_implementation();

	if (!acc_rss_activate(hal))
	{
		APP_LOG(TS_OFF, VLEVEL_L, "Failed to activate RSS\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_L, "acc_detector_distance_configuration_create() failed\n");
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}

	sts_distance_rss_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_L, "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_L, "acc_detector_distance_activate() failed\n");
		acc_detector_distance_destroy(&distance_handle);
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}

	bool                                success         = true;
	const int                           iterations      = 2; //5;
	uint16_t                            number_of_peaks = 5;
	acc_detector_distance_result_t      result[number_of_peaks];
	acc_detector_distance_result_info_t result_info;
	float tmp_distance			= 0.0f;

	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_L, "acc_detector_distance_get_next() failed\n");
			break;
		}
		for(uint8_t j=0; j< result_info.number_of_peaks; j++)
		tmp_distance += result[j].distance_m;
		print_distances(result, result_info.number_of_peaks);
	}

	sts_distance_rss_distance = (uint16_t)(1000*(tmp_distance/result_info.number_of_peaks)/iterations);

	APP_LOG(TS_OFF, VLEVEL_M, "\r\nAverage Distance= %u mm\r\n", (uint16_t)sts_distance_rss_distance);
	// ensure it's a valid installation height
	//sts_sensor_install_height = (uint16_t)MAX(sts_distance_rss_distance,2000);
	sts_sensor_install_height = (uint16_t)sts_distance_rss_distance;
	APP_LOG(TS_OFF, VLEVEL_M, "\r\nAssume Sensor Install Height = %u mm\r\n", (uint16_t)sts_sensor_install_height);

	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_L, "Application finished OK\n");
		return EXIT_SUCCESS;
	}

	return EXIT_FAILURE;
}

static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration)
{
	acc_detector_distance_configuration_mur_set(distance_configuration, DEFAULT_FAR_RANGE_MUR);    // NEW ADD
	acc_detector_distance_configuration_requested_start_set(distance_configuration, distance_cfg.start_m);
	acc_detector_distance_configuration_requested_length_set(distance_configuration, distance_cfg.length_m);
	acc_detector_distance_configuration_receiver_gain_set(distance_configuration,distance_cfg.gain);
	acc_detector_distance_configuration_maximize_signal_attenuation_set(distance_configuration, DEFAULT_FAR_MAXIMIZE_SIGNAL_ATTENUATION); // NEW ADD
	acc_detector_distance_configuration_service_profile_set(distance_configuration, distance_cfg.acc_profile);
	acc_detector_distance_configuration_downsampling_factor_set(distance_configuration, distance_cfg.downsampling);
	acc_detector_distance_configuration_sweep_averaging_set(distance_configuration,distance_cfg.sweep_average);
	acc_detector_distance_configuration_threshold_type_set(distance_configuration, ACC_DETECTOR_DISTANCE_THRESHOLD_TYPE_CFAR);
	acc_detector_distance_configuration_threshold_sensitivity_set(distance_configuration, distance_cfg.threshold);
	//acc_detector_distance_configuration_hw_accelerated_average_samples_set(distance_configuration, distance_cfg.hwaas);
	//acc_detector_distance_configuration_threshold_type_set(distance_configuration, DEFAULT_FAR_RANGE_THRESHOLD_TYPE);  //NEW ADD
	acc_detector_distance_configuration_cfar_threshold_guard_set(distance_configuration, DEFAULT_FAR_RANGE_CFAR_THRESHOLD_GUARD);
	acc_detector_distance_configuration_cfar_threshold_window_set(distance_configuration,DEFAULT_FAR_RANGE_CFAR_THRESHOLD_WINDOW);
	acc_detector_distance_configuration_record_background_sweeps_set(distance_configuration, 16);
}

  //backup. ... previous setting ----don't delete
/*
static void sts_distance_rss_update_configuration(acc_detector_distance_configuration_t distance_configuration)
{
	acc_detector_distance_configuration_requested_start_set(distance_configuration, distance_cfg.start_m);
	acc_detector_distance_configuration_requested_length_set(distance_configuration, distance_cfg.length_m);
	acc_detector_distance_configuration_service_profile_set(distance_configuration, distance_cfg.acc_profile);
	acc_detector_distance_configuration_hw_accelerated_average_samples_set(distance_configuration, distance_cfg.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_M, "Found %u peaks:\n", (unsigned int)reflection_count);

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