// Copyright (c) Acconeer AB, 2024-2025
// 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 <stddef.h>
#include <stdio.h>

#include "acc_algorithm.h"
#include "acc_config.h"
#include "acc_detector_presence.h"
#include "acc_detector_presence_processing.h"
#include "acc_integration.h"
#include "acc_integration_log.h"
#include "acc_processing.h"
#include "acc_rss_a121.h"
#include "example_hand_motion_detection.h"

#define MODULE "example_hand_motion_detection"

struct hand_motion_detection_handle
{
	hand_motion_detection_config_t config;
	bool                           prepare_needed;

	bool     has_detected;
	uint32_t update_index;
	uint32_t update_index_at_detection;

	acc_sensor_id_t                            sensor_id;
	acc_config_t                              *hand_motion_detection_sensor_config;
	acc_processing_t                          *sensor_processing;
	acc_processing_metadata_t                  sensor_processing_metadata;
	acc_detector_presence_processing_config_t *hand_motion_detection_presence_processing_config;
	acc_detector_presence_processing_handle_t *hand_motion_detection_presence_processor;

	acc_detector_presence_handle_t  *presence_detector;
	acc_detector_presence_metadata_t presence_metadata;

	hand_motion_detection_app_mode_t app_mode;
	uint16_t                         hand_motion_detection_timeout_duration;
	uint16_t                         hand_motion_detection_timer;
	uint16_t                         detection_retention_duration;
};

static void reinit_hand_motion_detection_state(hand_motion_detection_handle_t *handle);

static void set_sensor_config(const hand_motion_detection_config_t *hand_motion_detection_config, acc_config_t *sensor_config);

static void set_processing_config(const hand_motion_detection_config_t      *hand_motion_detection_config,
                                  acc_detector_presence_processing_config_t *processing_config);

static hand_motion_detection_detection_state_t process_presence_processing_result(
    hand_motion_detection_handle_t                  *handle,
    const acc_detector_presence_processing_result_t *presence_processing_result);

static void determine_app_mode(hand_motion_detection_handle_t *handle, hand_motion_detection_result_t *result);

static void swap_app_mode(hand_motion_detection_handle_t *handle, hand_motion_detection_app_mode_t new_app_mode);

hand_motion_detection_config_t *hand_motion_detection_config_create(void)
{
	hand_motion_detection_config_t *config = acc_integration_mem_calloc(1U, sizeof(*config));

	if (config != NULL)
	{
		config->presence_config = acc_detector_presence_config_create();

		if (config->presence_config == NULL)
		{
			hand_motion_detection_config_destroy(config);
			config = NULL;
		}
	}

	return config;
}

void hand_motion_detection_config_destroy(hand_motion_detection_config_t *config)
{
	if (config != NULL)
	{
		if (config->presence_config != NULL)
		{
			acc_detector_presence_config_destroy(config->presence_config);
		}

		acc_integration_mem_free(config);
	}
}

void hand_motion_detection_set_config(hand_motion_detection_preset_t preset, hand_motion_detection_config_t *config)
{
	switch (preset)
	{
		case HAND_MOTION_DETECTION_PRESET_NONE:
			break;
		case HAND_MOTION_DETECTION_PRESET_FAUCET:
			// Hand motion algo settings
			config->algo_config.sensor_to_water_distance     = 0.12f;
			config->algo_config.water_jet_width              = 0.02f;
			config->algo_config.measurement_range_end        = 0.2f;
			config->algo_config.filter_time_const            = 0.3f;
			config->algo_config.threshold                    = 1.5f;
			config->algo_config.detection_retention_duration = 1.0f;

			// Sensor settings
			config->algo_config.hwaas            = 32U;
			config->algo_config.sweeps_per_frame = 64U;
			config->algo_config.sweep_rate       = 1000.0f;
			config->algo_config.frame_rate       = 10.0f;

			// Presence settings
			acc_detector_presence_config_reset_filters_on_prepare_set(config->presence_config, true);
			acc_detector_presence_config_frame_rate_set(config->presence_config, 2.0f);
			acc_detector_presence_config_start_set(config->presence_config, 0.7f);
			acc_detector_presence_config_end_set(config->presence_config, 0.7f);
			acc_detector_presence_config_sweeps_per_frame_set(config->presence_config, 32U);
			acc_detector_presence_config_inter_frame_deviation_time_const_set(config->presence_config, 0.01f);
			acc_detector_presence_config_inter_output_time_const_set(config->presence_config, 0.01f);
			acc_detector_presence_config_intra_frame_time_const_set(config->presence_config, 0.01f);
			acc_detector_presence_config_intra_output_time_const_set(config->presence_config, 0.01f);

			acc_detector_presence_config_frame_rate_app_driven_set(config->presence_config, true);

			// Application settings
			config->hand_detection_timeout = 5.0f;
			config->use_presence_detection = true;
			break;
		default:
			break;
	}
}

hand_motion_detection_handle_t *hand_motion_detection_handle_create(const hand_motion_detection_config_t *hand_motion_detection_config,
                                                                    acc_sensor_id_t                       sensor_id)
{
	hand_motion_detection_handle_t *handle = acc_integration_mem_calloc(1U, sizeof(*handle));

	bool status = handle != NULL;

	if (status)
	{
		handle->config         = *hand_motion_detection_config;
		handle->prepare_needed = true;
		handle->app_mode       = HAND_MOTION_DETECTION_APP_MODE_HAND_MOTION;
		handle->sensor_id      = sensor_id;

		handle->hand_motion_detection_timeout_duration =
		    (uint16_t)(roundf(hand_motion_detection_config->hand_detection_timeout * hand_motion_detection_config->algo_config.frame_rate));
		handle->hand_motion_detection_timer  = 0U;
		handle->detection_retention_duration = (uint16_t)(roundf(hand_motion_detection_config->algo_config.detection_retention_duration *
		                                                         hand_motion_detection_config->algo_config.frame_rate));

		reinit_hand_motion_detection_state(handle);

		handle->hand_motion_detection_sensor_config              = acc_config_create();
		handle->hand_motion_detection_presence_processing_config = acc_detector_presence_processing_config_create();

		status = handle->hand_motion_detection_sensor_config != NULL && handle->hand_motion_detection_presence_processing_config != NULL;
	}

	if (status)
	{
		set_sensor_config(hand_motion_detection_config, handle->hand_motion_detection_sensor_config);
		set_processing_config(hand_motion_detection_config, handle->hand_motion_detection_presence_processing_config);

		handle->sensor_processing = acc_processing_create(handle->hand_motion_detection_sensor_config, &handle->sensor_processing_metadata);
		handle->hand_motion_detection_presence_processor =
		    acc_detector_presence_processing_create(handle->hand_motion_detection_presence_processing_config,
		                                            handle->hand_motion_detection_sensor_config);

		status = handle->hand_motion_detection_presence_processor != NULL;
	}

	if (status)
	{
		if (handle->config.use_presence_detection)
		{
			handle->app_mode = HAND_MOTION_DETECTION_APP_MODE_PRESENCE;

			handle->presence_detector = acc_detector_presence_create(handle->config.presence_config, &handle->presence_metadata);

			status = handle->presence_detector != NULL;

			uint32_t sleep_time_ms = (uint32_t)(1000.0f / acc_detector_presence_config_frame_rate_get(handle->config.presence_config));
			acc_integration_set_periodic_wakeup(sleep_time_ms);
		}
		else
		{
			uint32_t sleep_time_ms = (uint32_t)(1000.0f / handle->config.algo_config.frame_rate);
			acc_integration_set_periodic_wakeup(sleep_time_ms);
		}
	}

	if (!status)
	{
		hand_motion_detection_handle_destroy(handle);
		handle = NULL;
	}

	return handle;
}

void hand_motion_detection_handle_destroy(hand_motion_detection_handle_t *handle)
{
	if (handle != NULL)
	{
		if (handle->presence_detector != NULL)
		{
			acc_detector_presence_destroy(handle->presence_detector);
		}

		if (handle->hand_motion_detection_presence_processor != NULL)
		{
			acc_detector_presence_processing_destroy(handle->hand_motion_detection_presence_processor);
		}

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

		if (handle->hand_motion_detection_presence_processing_config != NULL)
		{
			acc_detector_presence_processing_config_destroy(handle->hand_motion_detection_presence_processing_config);
		}

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

		acc_integration_mem_free(handle);
	}
}

void hand_motion_detection_config_log(hand_motion_detection_handle_t *handle)
{
	if (handle != NULL)
	{
		printf("App config:\n");
		printf("sensor_to_water_distance:     %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.sensor_to_water_distance));
		printf("water_jet_width:              %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.water_jet_width));
		printf("measurement_range_end:        %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.measurement_range_end));
		printf("filter_time_const:            %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.filter_time_const));
		printf("threshold:                    %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.threshold));
		printf("detection_retention_duration: %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.detection_retention_duration));
		printf("hwaas:                        %" PRIu16 "\n", handle->config.algo_config.hwaas);
		printf("sweeps_per_frame:             %" PRIu16 "\n", handle->config.algo_config.sweeps_per_frame);
		printf("sweep_rate:                   %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.sweep_rate));
		printf("frame_rate:                   %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.algo_config.frame_rate));
		printf("hand_detection_timeout:       %" PRIfloat "\n", ACC_LOG_FLOAT_TO_INTEGER(handle->config.hand_detection_timeout));
		printf("use_presence_detection:        %s\n", handle->config.use_presence_detection ? "True" : "False");

		printf("Sensor config:\n");
		acc_config_log(handle->hand_motion_detection_sensor_config);

		if (handle->config.use_presence_detection)
		{
			printf("Presence config:\n");
			acc_detector_presence_config_log(handle->config.presence_config);
		}
	}
}

bool hand_motion_detection_get_buffer_size(hand_motion_detection_handle_t *handle, uint32_t *buffer_size)
{
	uint32_t hand_motion_detection_buffer_size = 0U;
	uint32_t presence_detector_buffer_size     = 0U;

	bool status = acc_rss_get_buffer_size(handle->hand_motion_detection_sensor_config, &hand_motion_detection_buffer_size);

	if (status)
	{
		hand_motion_detection_buffer_size += acc_detector_presence_processing_get_buffer_size(handle->hand_motion_detection_sensor_config);

		if (handle->config.use_presence_detection)
		{
			status = acc_detector_presence_get_buffer_size(handle->presence_detector, &presence_detector_buffer_size);
		}
	}

	if (status)
	{
		*buffer_size =
		    (hand_motion_detection_buffer_size > presence_detector_buffer_size) ? hand_motion_detection_buffer_size : presence_detector_buffer_size;
	}

	return status;
}

bool hand_motion_detection_prepare(hand_motion_detection_handle_t *handle,
                                   acc_sensor_t                   *sensor,
                                   const acc_cal_result_t         *cal_result,
                                   void                           *buffer,
                                   uint32_t                        buffer_size,
                                   bool                            force_prepare)
{
	bool status = true;

	if (handle->prepare_needed || force_prepare)
	{
		if (handle->app_mode == HAND_MOTION_DETECTION_APP_MODE_PRESENCE)
		{
			status =
			    acc_detector_presence_prepare(handle->presence_detector, handle->config.presence_config, sensor, cal_result, buffer, buffer_size);
		}
		else // HAND_MOTION_DETECTION_APP_MODE_HAND_MOTION
		{
			status = acc_sensor_prepare(sensor, handle->hand_motion_detection_sensor_config, cal_result, buffer, buffer_size);
		}

		handle->prepare_needed = false;
	}

	return status;
}

void hand_motion_detection_process(hand_motion_detection_handle_t *handle, void *buffer, hand_motion_detection_result_t *hand_motion_detection_result)
{
	acc_detector_presence_result_t presence_result;

	if (handle->app_mode == HAND_MOTION_DETECTION_APP_MODE_PRESENCE)
	{
		acc_detector_presence_process(handle->presence_detector, buffer, &presence_result);

		hand_motion_detection_result->presence_result_available = true;
		hand_motion_detection_result->presence_result           = presence_result;
		hand_motion_detection_result->algo_result_available     = false;

		hand_motion_detection_result->proc_result = presence_result.processing_result;
	}
	else // HAND_MOTION_DETECTION_APP_MODE_HAND_MOTION
	{
		acc_detector_presence_processing_result_t presence_processing_result;
		acc_processing_execute(handle->sensor_processing, buffer, &hand_motion_detection_result->proc_result);

		acc_detector_presence_processing_process(handle->hand_motion_detection_presence_processor,
		                                         buffer,
		                                         hand_motion_detection_result->proc_result.frame,
		                                         &presence_processing_result);

		hand_motion_detection_result->algo_result_available       = true;
		hand_motion_detection_result->algo_result.detection_state = process_presence_processing_result(handle, &presence_processing_result);
		hand_motion_detection_result->presence_result_available   = false;
	}

	hand_motion_detection_result->app_mode = handle->app_mode;

	determine_app_mode(handle, hand_motion_detection_result);
}

static void reinit_hand_motion_detection_state(hand_motion_detection_handle_t *handle)
{
	handle->has_detected              = false;
	handle->update_index              = 0U;
	handle->update_index_at_detection = 0U;
}

static void set_sensor_config(const hand_motion_detection_config_t *hand_motion_detection_config, acc_config_t *sensor_config)
{
	const hand_motion_detection_algo_config_t *algo_config             = &hand_motion_detection_config->algo_config;
	acc_config_profile_t                       profile                 = ACC_CONFIG_PROFILE_1;
	uint16_t                                   step_length             = 6U;
	float                                      water_jet_half_width    = algo_config->water_jet_width / 2.0f;
	uint8_t                                    subsweep_idx            = 0U;
	float                                      profile_envelope_fwhm_m = acc_algorithm_get_fwhm(profile);

	// Determine if subsweep between sensor and water jet is feasible
	if (profile_envelope_fwhm_m < (algo_config->sensor_to_water_distance - water_jet_half_width))
	{
		int32_t start_point =
		    (int32_t)roundf((algo_config->sensor_to_water_distance - profile_envelope_fwhm_m - water_jet_half_width) / ACC_APPROX_BASE_STEP_LENGTH_M);
		acc_config_subsweep_start_point_set(sensor_config, start_point, subsweep_idx);
		acc_config_subsweep_num_points_set(sensor_config, 1U, subsweep_idx);
		acc_config_subsweep_step_length_set(sensor_config, step_length, subsweep_idx);
		acc_config_subsweep_profile_set(sensor_config, profile, subsweep_idx);
		acc_config_subsweep_hwaas_set(sensor_config, algo_config->hwaas, subsweep_idx);
		acc_config_subsweep_receiver_gain_set(sensor_config, 4U, subsweep_idx);

		subsweep_idx++;
	}

	// Determine if subsweep after water jet is required
	if (profile_envelope_fwhm_m + water_jet_half_width + algo_config->sensor_to_water_distance < algo_config->measurement_range_end)
	{
		float    start_m     = algo_config->sensor_to_water_distance + water_jet_half_width + profile_envelope_fwhm_m;
		int32_t  start_point = (int32_t)roundf(start_m / ACC_APPROX_BASE_STEP_LENGTH_M);
		uint16_t num_points =
		    (uint16_t)roundf(fmaxf(1.0f, (algo_config->measurement_range_end - start_m) / ((float)step_length * ACC_APPROX_BASE_STEP_LENGTH_M)));

		acc_config_subsweep_start_point_set(sensor_config, start_point, subsweep_idx);
		acc_config_subsweep_num_points_set(sensor_config, num_points, subsweep_idx);
		acc_config_subsweep_step_length_set(sensor_config, step_length, subsweep_idx);
		acc_config_subsweep_profile_set(sensor_config, profile, subsweep_idx);
		acc_config_subsweep_hwaas_set(sensor_config, algo_config->hwaas, subsweep_idx);
		acc_config_subsweep_receiver_gain_set(sensor_config, 4U, subsweep_idx);

		subsweep_idx++;
	}

	acc_config_num_subsweeps_set(sensor_config, subsweep_idx);
	acc_config_frame_rate_set(sensor_config, 0.0f);
	acc_config_sweep_rate_set(sensor_config, algo_config->sweep_rate);
	acc_config_sweeps_per_frame_set(sensor_config, algo_config->sweeps_per_frame);
	acc_config_inter_frame_idle_state_set(sensor_config, ACC_CONFIG_IDLE_STATE_SLEEP);
	acc_config_inter_sweep_idle_state_set(sensor_config, ACC_CONFIG_IDLE_STATE_SLEEP);
}

static void set_processing_config(const hand_motion_detection_config_t      *hand_motion_detection_config,
                                  acc_detector_presence_processing_config_t *processing_config)
{
	const hand_motion_detection_algo_config_t *algo_config = &hand_motion_detection_config->algo_config;

	acc_detector_presence_processing_config_frame_rate_set(processing_config, algo_config->frame_rate);
	acc_detector_presence_processing_config_intra_detection_set(processing_config, true);
	acc_detector_presence_processing_config_inter_detection_set(processing_config, true);
	acc_detector_presence_processing_config_intra_detection_threshold_set(processing_config, algo_config->threshold);
	acc_detector_presence_processing_config_inter_detection_threshold_set(processing_config, algo_config->threshold);
	acc_detector_presence_processing_config_inter_frame_presence_timeout_set(processing_config, 0U);
	acc_detector_presence_processing_config_inter_frame_fast_cutoff_set(processing_config, 3.0f);
	acc_detector_presence_processing_config_inter_frame_slow_cutoff_set(processing_config, 0.8f);
	acc_detector_presence_processing_config_inter_frame_deviation_time_const_set(processing_config, 0.5f);
	acc_detector_presence_processing_config_inter_output_time_const_set(processing_config, algo_config->filter_time_const);
	acc_detector_presence_processing_config_intra_frame_time_const_set(processing_config, 0.15f);
	acc_detector_presence_processing_config_intra_output_time_const_set(processing_config, algo_config->filter_time_const);
}

static hand_motion_detection_detection_state_t process_presence_processing_result(
    hand_motion_detection_handle_t                  *handle,
    const acc_detector_presence_processing_result_t *presence_processing_result)
{
	hand_motion_detection_detection_state_t detection_state = HAND_MOTION_DETECTION_STATE_NO_DETECTION;

	if (presence_processing_result->presence_detected)
	{
		handle->has_detected              = true;
		handle->update_index_at_detection = handle->update_index;
		detection_state                   = HAND_MOTION_DETECTION_STATE_DETECTION;
	}

	if (handle->has_detected && detection_state != HAND_MOTION_DETECTION_STATE_DETECTION &&
	    ((handle->update_index - handle->update_index_at_detection) < handle->detection_retention_duration))
	{
		detection_state = HAND_MOTION_DETECTION_STATE_RETENTION;
	}

	handle->update_index++;

	return detection_state;
}

static void determine_app_mode(hand_motion_detection_handle_t *handle, hand_motion_detection_result_t *result)
{
	hand_motion_detection_app_mode_t new_app_mode = handle->app_mode;

	if (handle->app_mode == HAND_MOTION_DETECTION_APP_MODE_PRESENCE)
	{
		if (result->presence_result.presence_detected)
		{
			new_app_mode = HAND_MOTION_DETECTION_APP_MODE_HAND_MOTION;
		}
	}
	else // HAND_MOTION_DETECTION_APP_MODE_HAND_MOTION
	{
		if (handle->config.use_presence_detection)
		{
			if (result->algo_result.detection_state != HAND_MOTION_DETECTION_STATE_NO_DETECTION)
			{
				handle->hand_motion_detection_timer = 0U;
			}
			else if (handle->hand_motion_detection_timeout_duration < handle->hand_motion_detection_timer)
			{
				new_app_mode = HAND_MOTION_DETECTION_APP_MODE_PRESENCE;
			}
			else
			{
				handle->hand_motion_detection_timer++;
			}
		}
	}

	if (new_app_mode != handle->app_mode)
	{
		swap_app_mode(handle, new_app_mode);
	}
}

static void swap_app_mode(hand_motion_detection_handle_t *handle, hand_motion_detection_app_mode_t new_app_mode)
{
	handle->prepare_needed = true;
	handle->app_mode       = new_app_mode;

	if (handle->app_mode == HAND_MOTION_DETECTION_APP_MODE_HAND_MOTION)
	{
		acc_detector_presence_processing_reset(handle->hand_motion_detection_presence_processor);
		reinit_hand_motion_detection_state(handle);
		handle->hand_motion_detection_timer = 0U;

		uint32_t sleep_time_ms = (uint32_t)(1000.0f / handle->config.algo_config.frame_rate);
		acc_integration_set_periodic_wakeup(sleep_time_ms);
	}
	else
	{
		uint32_t sleep_time_ms = (uint32_t)(1000.0f / acc_detector_presence_config_frame_rate_get(handle->config.presence_config));
		acc_integration_set_periodic_wakeup(sleep_time_ms);
	}
}
