// Copyright (c) Acconeer AB, 2019-2021
// 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 "acc_definitions_common.h"
#include "acc_detector_presence.h"
#include "acc_hal_definitions.h"
#include "acc_hal_integration.h"
#include "acc_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"
/*
#define DEFAULT_START_M             (0.2f)
#define DEFAULT_LENGTH_M            (1.4f)
#define DEFAULT_POWER_SAVE_MODE	     ACC_POWER_SAVE_MODE_SLEEP
*/
#define DEFAULT_UPDATE_RATE	         (10)
#define DEFAULT_DETECTION_THRESHOLD	 (2.0f)
#define DEFAULT_NBR_REMOVED_PC_2      (0)

#define DEFAULT_PROFILE				  ACC_SERVICE_PROFILE_4
#define DEFAULT_UPDATE_RATE         				(10)
#define DEFAULT_POWER_SAVE_MODE     				ACC_POWER_SAVE_MODE_ACTIVE
#define DEFAULT_SENSOR_ID            				(1)

#define DEFAULT_START_M              				(0.80f)		//default 0.2 unit(meter)			[1]
#define DEFAULT_LENGTH_M             				(2.00f)		//default 1.0 unit(meter)			[2]
#define DEFAULT_ZONE_LENGTH          				(0.4f)		//default 0.4 unit(meter)
#define DEFAULT_UPDATE_RATE_WAKEUP   				(2.0f)		//default 80 unit(hz)
#define DEFAULT_UPDATE_RATE_TRACKING 				(10.0f)		//default 80 unit(hz)				[7]
#define DEFAULT_UPDATE_RATE_PRESENCE 				(65.0F)		//(65.0f)		//default 80 unit(hz)
#define DEFAULT_HWAAS								(63)		//default 10 unit(hz)
#define DEFAULT_THRESHOLD            				(1.5f)		//default 1.5 level float			[3]

//acc_detector_presence_configuration_filter_parameters_t
#define DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST	(0.5f)		//default 0.5 unit(seconds)			[6]
#define	DEFAULT_INTER_FRAME_FAST_CUTOFF				(10.0f)		//default 20.0 unit(hz)				[8]
#define	DEFAULT_INTER_FRAME_SLOW_CUTOFF				(0.01f)		//(0.01f) 0.2 hz unit(hz)			[9]
#define DEFAULT_INTRA_FRAME_TIME_CONST				(0)			//default 0.0 unit(seconds)
#define	DEFAULT_INTRA_FRAME_WEIGHT					(0)			//default 0.6
#define	DEFAULT_OUTPUT_TIME_CONST					(0.5f)		//default 0.5 unit(seconds)			[5]
#define DEFAULT_NBR_REMOVED_PC       				(0)			//default 0 int						[10]

#define	DEFAULT_DOWNSAMPLING_FACTOR					(2)			//default 1
#define	DEFAULT_RECEIVER_GAIN						(0.45f)		//default 0.9 gain mdB				[4]

extern volatile uint8_t sts_rss_result, sts_rss_config_updated_flag;
volatile STS_OO_RSS_SensorTuneDataTypeDef sts_presence_rss_config;
//static void update_configuration(acc_detector_presence_configuration_t presence_configuration);
static void print_result(acc_detector_presence_result_t result);

/**
 * @brief Set default values in presence configuration
 *
 * @param[in] presence_configuration The presence configuration to set default values in
 */
static void set_default_configuration(acc_detector_presence_configuration_t presence_configuration)
{
	acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID);
	acc_detector_presence_configuration_update_rate_set(presence_configuration, DEFAULT_UPDATE_RATE_PRESENCE);
	acc_detector_presence_configuration_detection_threshold_set(presence_configuration, DEFAULT_THRESHOLD);
	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	//acc_service_sparse_configuration_sweeps_per_frame_set(sparse_configuration, sweeps_per_frame);

	acc_detector_presence_configuration_start_set(presence_configuration, DEFAULT_START_M);
	acc_detector_presence_configuration_length_set(presence_configuration, DEFAULT_LENGTH_M);
	acc_detector_presence_configuration_downsampling_factor_set(presence_configuration,	DEFAULT_DOWNSAMPLING_FACTOR);
	acc_detector_presence_configuration_receiver_gain_set(presence_configuration, DEFAULT_RECEIVER_GAIN);

	acc_detector_presence_configuration_filter_parameters_t filter = acc_detector_presence_configuration_filter_parameters_get(
		presence_configuration);
	filter.inter_frame_deviation_time_const		= 	DEFAULT_INTER_FRAME_DEVIATION_TIME_CONST;
	filter.inter_frame_fast_cutoff				= 	DEFAULT_INTER_FRAME_FAST_CUTOFF;
	filter.inter_frame_slow_cutoff				=	DEFAULT_INTER_FRAME_SLOW_CUTOFF;
	filter.intra_frame_time_const				=	DEFAULT_INTRA_FRAME_TIME_CONST;
	filter.intra_frame_weight					=	DEFAULT_INTRA_FRAME_WEIGHT;
	filter.output_time_const 					= 	DEFAULT_OUTPUT_TIME_CONST;		//0.0f;
	acc_detector_presence_configuration_filter_parameters_set(presence_configuration, &filter);
	acc_detector_presence_configuration_nbr_removed_pc_set(presence_configuration, DEFAULT_NBR_REMOVED_PC);
	acc_detector_presence_configuration_power_save_mode_set(presence_configuration, ACC_POWER_SAVE_MODE_ACTIVE);
}


static void sts_rss_set_current_configuration(acc_detector_presence_configuration_t presence_configuration)
{
	acc_detector_presence_configuration_sensor_set(presence_configuration, DEFAULT_SENSOR_ID);

//	acc_service_profile_t presence_profile = sts_presence_rss_config.default_profile;
//	acc_detector_presence_configuration_service_profile_set(presence_configuration, presence_profile);

	acc_detector_presence_configuration_update_rate_set(presence_configuration, sts_presence_rss_config.default_update_rate_presence); //DEFAULT_UPDATE_RATE_2);
	acc_detector_presence_configuration_detection_threshold_set(presence_configuration, sts_presence_rss_config.default_threshold);//DEFAULT_DETECTION_THRESHOLD_2);

	acc_detector_presence_configuration_start_set(presence_configuration, sts_presence_rss_config.default_start_m);
	acc_detector_presence_configuration_length_set(presence_configuration, sts_presence_rss_config.default_length_m); //DEFAULT_LENGTH_M_2);

	acc_detector_presence_configuration_downsampling_factor_set(presence_configuration, sts_presence_rss_config.default_downsampling_factor);
	acc_detector_presence_configuration_receiver_gain_set(presence_configuration, sts_presence_rss_config.default_receiver_gain);

	acc_detector_presence_configuration_filter_parameters_t filter = acc_detector_presence_configuration_filter_parameters_get(
		presence_configuration);
	filter.inter_frame_deviation_time_const		= 	sts_presence_rss_config.default_inter_frame_deviation_time_const;
	filter.inter_frame_fast_cutoff				= 	sts_presence_rss_config.default_inter_frame_fast_cutoff;
	filter.inter_frame_slow_cutoff				=	sts_presence_rss_config.default_inter_frame_slow_cutoff;
	filter.intra_frame_time_const				=	sts_presence_rss_config.default_intra_frame_time_const;
	filter.intra_frame_weight					=	sts_presence_rss_config.default_intra_frame_weight;
	filter.output_time_const 					= 	sts_presence_rss_config.default_output_time_const;		//0.0f;
	acc_detector_presence_configuration_filter_parameters_set(presence_configuration, &filter);

    acc_detector_presence_configuration_nbr_removed_pc_set(presence_configuration, sts_presence_rss_config.default_nbr_removed_pc);
	acc_detector_presence_configuration_hw_accelerated_average_samples_set(presence_configuration, sts_presence_rss_config.default_hwaas);

}


void sts_rss_get_current_configuration(acc_detector_presence_configuration_t presence_configuration)
{


}

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

	if (!acc_rss_activate(hal))
	{
		APP_LOG(TS_OFF, VLEVEL_M,"Failed to activate RSS\n");
		return EXIT_FAILURE;
	}

	acc_rss_override_sensor_id_check_at_creation(true);

	acc_detector_presence_configuration_t presence_configuration = acc_detector_presence_configuration_create();
	if (presence_configuration == NULL)
	{
		APP_LOG(TS_OFF, VLEVEL_M,"Failed to create configuration\n");
		acc_rss_deactivate();
		return EXIT_FAILURE;
	}

	if (sts_rss_config_updated_flag == 0) {
		set_default_configuration(presence_configuration);
	{
		sts_rss_set_current_configuration(presence_configuration);
	}

	acc_detector_presence_handle_t handle = acc_detector_presence_create(presence_configuration);
	if (handle == NULL)
	{
			APP_LOG(TS_OFF, VLEVEL_M,"Failed to create detector\n");
			acc_detector_presence_configuration_destroy(&presence_configuration);
			acc_detector_presence_destroy(&handle);
			acc_rss_deactivate();
			return EXIT_FAILURE;
	}

	acc_detector_presence_configuration_destroy(&presence_configuration);

	if (!acc_detector_presence_activate(handle))
		{
			APP_LOG(TS_OFF, VLEVEL_M,"Failed to activate detector\n");
			acc_detector_presence_destroy(&handle);
			acc_rss_deactivate();
			return EXIT_FAILURE;
		}

	bool                           success    = true;
	const int                      iterations = 10;
	acc_detector_presence_result_t result;
	uint8_t	 average_result = 0;
	for (int i = 0; i < iterations; i++)
	{
		success = acc_detector_presence_get_next(handle, &result);
		if (!success)
		{
			APP_LOG(TS_OFF, VLEVEL_M,"acc_detector_presence_get_next() failed\n");
			break;
		}

		print_result(result);

		if (result.presence_detected) average_result++;
		acc_integration_sleep_ms(1000 / DEFAULT_UPDATE_RATE);
	}

	sts_rss_result = (average_result > (iterations/2))? 1: 0;

	bool deactivated = acc_detector_presence_deactivate(handle);

	acc_detector_presence_destroy(&handle);

	acc_rss_deactivate();

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

	return EXIT_FAILURE;
}



void STS_PRESENCE_RSS_update_configuration(acc_detector_presence_configuration_t presence_configuration)
{
	acc_detector_presence_configuration_update_rate_set(presence_configuration, DEFAULT_UPDATE_RATE);
	acc_detector_presence_configuration_detection_threshold_set(presence_configuration, DEFAULT_DETECTION_THRESHOLD);
	acc_detector_presence_configuration_start_set(presence_configuration, DEFAULT_START_M);
	acc_detector_presence_configuration_length_set(presence_configuration, DEFAULT_LENGTH_M);
	acc_detector_presence_configuration_power_save_mode_set(presence_configuration, DEFAULT_POWER_SAVE_MODE);
	acc_detector_presence_configuration_nbr_removed_pc_set(presence_configuration, DEFAULT_NBR_REMOVED_PC);
}


void print_result(acc_detector_presence_result_t result)
{
	if (result.presence_detected)
	{
		uint32_t detected_zone = (uint32_t)((float)(result.presence_distance - DEFAULT_START_M) / (float)DEFAULT_ZONE_LENGTH);

		APP_LOG(TS_OFF, VLEVEL_M,"Motion in zone: %u, distance: %d, score: %d\n", (unsigned int)detected_zone,
			       (int)(result.presence_distance * 1000.0f),
			       (int)(result.presence_score * 1000.0f));

		sts_rss_result = 1;
	}
	else
	{

		APP_LOG(TS_OFF, VLEVEL_M,"No motion, score: %d\n", (int)(result.presence_score * 1000.0f));

		sts_rss_result = 0;
	}


}