/*
* Copyright (c) 2018 Samsung Electronics Co., Ltd.
*
* Licensed under the Flora License, Version 1.1 (the License);
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://floralicense.org/license/
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an AS IS BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include <sensor.h>
#include "gear-racing-controller.h"
#include "model/model_sensors.h"
#include "model/model_car_connection.h"
#include "model/model_device_to_connection.h"
#include "log.h"
#include "math_helper.h"

#define VALUE_LIST_SIZE 50
#define GRAVITY_LISTENER_INTERVAL 10
#define ACCELEROMETER_LISTENER_INTERVAL 10
#define LINEAR_LISTENER_INTERVAL 10

typedef struct _s_data_history {
	int current_index;
	float sum;
	float array[VALUE_LIST_SIZE];
} s_data_history;

typedef struct _s_axis_params {
	float deadzone;
	float input_range;
	float output_range;
	float multiplier;

	bool is_incremental;
	float incremental_min;
	float incremental_max;
	float incremental_value;
} s_axis_params;

typedef struct _s_model_sensors {
	t_model_sensors_update_cb sensors_update_cb;

	sensor_h sensor_accelerometer;
	sensor_listener_h listener_accelerometer;

	sensor_h sensor_gravity;
	sensor_listener_h listener_gravity;

	sensor_h sensor_linear;
	sensor_listener_h listener_linear;

	float initial_sensor_accelerometer_data[3];
	float initial_sensor_gravity_data[3];
	float last_linear_event_values[3];
	float velocity_reference_vector[3];
	s_model_sensors_cb_data last_model_data;

	s_data_history acceleration_sensor;
	s_data_history gravity_sensor;

	s_axis_params acceleration_sensor_params;
	s_axis_params gravity_sensor_params;
} s_model_sensors;

static s_model_sensors s_info = {
	.initial_sensor_accelerometer_data = { 0, },
	.initial_sensor_gravity_data = { 0, },
	.last_linear_event_values = { 0, },
	.velocity_reference_vector = { 0, },
};

static void _update_data_history(float value, s_data_history *parameter)
{
	parameter->sum = parameter->sum - parameter->array[parameter->current_index] + value;
	parameter->array[parameter->current_index] = value;
	++parameter->current_index;
	parameter->current_index %= VALUE_LIST_SIZE;
}

static inline float _get_average_parameter_from_history(float value, s_data_history *parameter)
{
	_update_data_history(value, parameter);
	return parameter->sum / VALUE_LIST_SIZE;
}

static float _get_position_in_deadzone(float value, float deadzone)
{
	if (fabsf(value) <= deadzone) {
		value = 0;
	} else if (value > 0) {
		value = value - deadzone;
	} else {
		value = value + deadzone;
	}

	return value;
}

static inline float _set_incremental(s_axis_params *axis, float value)
{
	axis->incremental_value += value;

	if (axis->incremental_value > axis->incremental_max) {
		axis->incremental_value = axis->incremental_max;
	} else if (axis->incremental_value < axis->incremental_min) {
		axis->incremental_value = axis->incremental_min;
	}

	return axis->incremental_value;
}

static void _sensor_accelerometer_event_cb(sensor_h sensor_accelerometer, sensor_event_s *event, void *data)
{
	float vector[3] = { 0,};

	vector_diff(event->values, s_info.initial_sensor_accelerometer_data, &vector[0], 3);

	float acceleration = vector[1];
	acceleration = _get_average_parameter_from_history(acceleration, &s_info.acceleration_sensor);
	acceleration = _get_position_in_deadzone(acceleration, s_info.acceleration_sensor_params.deadzone);

	s_info.last_model_data.axis_x = math_helper_range_map(acceleration,
			-(s_info.acceleration_sensor_params.input_range - s_info.acceleration_sensor_params.deadzone),
			(s_info.acceleration_sensor_params.input_range - s_info.acceleration_sensor_params.deadzone),
			-s_info.acceleration_sensor_params.output_range, s_info.acceleration_sensor_params.output_range);

	if (s_info.acceleration_sensor_params.is_incremental) {
		s_info.last_model_data.axis_x = _set_incremental(&s_info.acceleration_sensor_params, s_info.last_model_data.axis_x);
	}


	s_info.last_model_data.axis_x *= s_info.acceleration_sensor_params.multiplier;
	s_model_sensors_cb_data model_data = s_info.last_model_data;
	model_device_to_connection_axis_x(model_data.axis_x);

	_D("MODEL VALUES: acceleration:% .4f ranged:% .4f", acceleration, model_data.axis_x);
	if (s_info.sensors_update_cb) {
		s_info.sensors_update_cb(&model_data);
	}
}

void model_sensors_set_initial_values(void)
{
	model_sensors_sensor_accelerometer_set_initial_values();
	model_sensors_sensor_gravity_set_initial_values();

	s_info.last_model_data.type = MODEL_TYPE_UPDATE;
	s_info.last_model_data.axis_x = 0.0f;
	s_info.last_model_data.axis_y = 0.0f;
}

void model_sensors_sensor_accelerometer_set_initial_values(void)
{
	sensor_event_s event = { 0, };

	int ret = sensor_listener_read_data(s_info.listener_accelerometer, &event);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	memcpy(s_info.initial_sensor_accelerometer_data, event.values, sizeof(s_info.initial_sensor_accelerometer_data));

	_D("Initial sensor_accelerometer data: {%f, %f, %f}", s_info.initial_sensor_accelerometer_data[0], s_info.initial_sensor_accelerometer_data[1],
	   s_info.initial_sensor_accelerometer_data[2]);
}

static void _sensor_gravity_event_cb(sensor_h sensor_gravity, sensor_event_s *event, void *data)
{
	float vector[3] = { 0, 0, 0};

	//Adding linear acceleration to reduce noise appearing during move
	vector_sum(&event->values[0], &s_info.last_linear_event_values[0], &vector[0], 3);

	vector_normalize(&vector[0], &vector[0], 3);

	float gravity = vector_dot_product(vector, s_info.velocity_reference_vector, 3);
	gravity = _get_average_parameter_from_history(gravity, &s_info.gravity_sensor);
	gravity = _get_position_in_deadzone(gravity, s_info.gravity_sensor_params.deadzone);

	s_info.last_model_data.axis_y = math_helper_range_map(gravity,
			-(s_info.gravity_sensor_params.input_range - s_info.gravity_sensor_params.deadzone),
			 (s_info.gravity_sensor_params.input_range - s_info.gravity_sensor_params.deadzone),
			 -s_info.gravity_sensor_params.output_range, s_info.gravity_sensor_params.output_range);

	if (s_info.gravity_sensor_params.is_incremental) {
		s_info.last_model_data.axis_y = _set_incremental(&s_info.gravity_sensor_params, s_info.last_model_data.axis_y);
	}

	s_info.last_model_data.axis_x *= s_info.gravity_sensor_params.multiplier;
	s_model_sensors_cb_data model_data = s_info.last_model_data;

	model_device_to_connection_axis_y(model_data.axis_y);
	_D("MODEL VALUES: axis_y:% .4f ranged:% .4f", gravity, model_data.axis_y);
	if (s_info.sensors_update_cb) {
		s_info.sensors_update_cb(&model_data);
	}
}

static void _sensor_linear_event_cb(sensor_h sensor_linear, sensor_event_s *event, void *data)
{
	memcpy(s_info.last_linear_event_values, event->values, sizeof(s_info.last_linear_event_values));
}

void model_sensors_sensor_gravity_set_initial_values(void)
{
	sensor_event_s event = { 0, };

	int ret = sensor_listener_read_data(s_info.listener_gravity, &event);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	memcpy(s_info.initial_sensor_gravity_data, event.values, sizeof(s_info.initial_sensor_gravity_data));

	//In further calculation there is assumption, that normalized initial gravity vector was [0,0,1] (device facing sky)
	//However, user can start in different position, so we have to calculate rotation matrix to change base for reference vector
	float reference_vector[3] = {0, 0, vector_length(s_info.initial_sensor_gravity_data, 3)};
	_D("Caluclating rotation matrix from [% .4f, % .4f, % .4f] to [% .4f,% .4f,% .4f]", s_info.initial_sensor_gravity_data[0], s_info.initial_sensor_gravity_data[1],
	   s_info.initial_sensor_gravity_data[2], reference_vector[0], reference_vector[1], reference_vector[2]);
	float rotation_matrix[3][3];
	vector3_rotation_matrix(s_info.initial_sensor_gravity_data, reference_vector, rotation_matrix);

	//With previous assumption, the greatest speed should be achievable with vector [0,1,0] (device facing walls).
	reference_vector[0] = -1;
	reference_vector[1] = 0;
	reference_vector[2] = 0;
	vector_matrix_multiply(&rotation_matrix[0][0], &reference_vector[0], &s_info.velocity_reference_vector[0], 3);

	_D("Initial sensor_gravity data: {%f, %f, %f}", s_info.initial_sensor_gravity_data[0], s_info.initial_sensor_gravity_data[1],
	s_info.initial_sensor_gravity_data[2]);
	_D("Initial velovity refernce vector: {%f, %f, %f}", s_info.velocity_reference_vector[0], s_info.velocity_reference_vector[1], s_info.velocity_reference_vector[2]);
}

void model_sensors_init(void)
{
	bool supported = false;

	int ret = sensor_is_supported(SENSOR_ACCELEROMETER, &supported);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);
	ASSERT(!supported, "Sensor %d is not supported", SENSOR_ACCELEROMETER)

	ret = sensor_get_default_sensor(SENSOR_ACCELEROMETER, &s_info.sensor_accelerometer);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	ret = sensor_create_listener(s_info.sensor_accelerometer, &s_info.listener_accelerometer);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	ret = sensor_listener_set_event_cb(s_info.listener_accelerometer, ACCELEROMETER_LISTENER_INTERVAL, _sensor_accelerometer_event_cb, NULL);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	sensor_listener_start(s_info.listener_accelerometer);

	ret = sensor_is_supported(SENSOR_GRAVITY, &supported);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);
	ASSERT(!supported, "Sensor %d is not supported", SENSOR_GRAVITY)

	ret = sensor_get_default_sensor(SENSOR_GRAVITY, &s_info.sensor_gravity);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	ret = sensor_create_listener(s_info.sensor_gravity, &s_info.listener_gravity);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	ret = sensor_listener_set_event_cb(s_info.listener_gravity, GRAVITY_LISTENER_INTERVAL, _sensor_gravity_event_cb, NULL);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	sensor_listener_start(s_info.listener_gravity);

	ret = sensor_is_supported(SENSOR_LINEAR_ACCELERATION, &supported);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);
	ASSERT(!supported, "Sensor %d is not supported", SENSOR_LINEAR_ACCELERATION)

	ret = sensor_get_default_sensor(SENSOR_LINEAR_ACCELERATION, &s_info.sensor_linear);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	ret = sensor_create_listener(s_info.sensor_linear, &s_info.listener_linear);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	ret = sensor_listener_set_event_cb(s_info.listener_linear, LINEAR_LISTENER_INTERVAL, _sensor_linear_event_cb, NULL);
	ASSERT_FUNCTION(ret != SENSOR_ERROR_NONE);

	sensor_listener_start(s_info.listener_linear);

	model_sensors_set_initial_values();
}


static void _set_axis_params(s_axis_params *axis, float input_range, float output_range,
		float deadzone, float multiplier,
		bool incremental, float incremental_min, float incremental_max)
{
	axis->input_range = input_range;
	axis->output_range = output_range;
	axis->deadzone = deadzone;
	axis->multiplier = multiplier;

	axis->is_incremental = incremental;
	axis->incremental_min = incremental_min;
	axis->incremental_max = incremental_max;
	axis->incremental_value = 0;
}

void model_sensors_set_acceleration_sensor_params(float input_range, float output_range,
		float deadzone, float multiplier,
		bool incremental, float incremental_min, float incremental_max)
{
	_set_axis_params(&s_info.acceleration_sensor_params, input_range, output_range,
			deadzone, multiplier,
			incremental, incremental_min, incremental_max);
}

void model_sensors_set_gravity_sensor_params(float input_range, float output_range,
		float deadzone, float multiplier,
		bool incremental, float incremental_min, float incremental_max)
{
	_set_axis_params(&s_info.gravity_sensor_params, input_range, output_range,
			deadzone, multiplier,
			incremental, incremental_min, incremental_max);
}

void model_sensors_subscribe_event(t_model_sensors_update_cb model_update_cb)
{
	s_info.sensors_update_cb = model_update_cb;
}

void model_sensors_unsubscirbe_event(void)
{
	s_info.sensors_update_cb = NULL;
}