/*
* 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 "math_helper.h"
#include "car_connection_manager.h"
#include <math.h>

float math_helper_range_map(float value, float input_min, float input_max, float output_min, float output_max)
{
	float result = ((value - input_min) / (input_max - input_min)) * (output_max - output_min) + output_min;

	if (result < output_min) {
		result = output_min;
	} else if (result > output_max) {
		result = output_max;
	}

	return result;
}

float vector_length(float *vector, int size)
{
	float len = 0;
	int i = 0;

	for (i = 0; i < size; ++i) {
		len += vector[i] * vector[i];
	}

	return sqrtf(len);
}

void vector_scalar_multiplication(float *vector_in, float scalar, float *vector_out, int size)
{
	for (int i = 0; i < size; ++i) {
		vector_out[i] = vector_in[i] * scalar;
	}
}

void vector_diff(float *vector_1, float *vector_2, float *vector_out, int size)
{
	for (int i = 0; i < size; ++i) {
		vector_out[i] = vector_1[i] - vector_2[i];
	}
}

void vector_sum(float *vector_1, float *vector_2, float *vector_out, int size)
{
	for (int i = 0; i < size; ++i) {
		vector_out[i] = vector_1[i] + vector_2[i];
	}
}

void vector_normalize(float *vector_in, float *vector_out, int size)
{
	vector_scalar_multiplication(vector_in, 1.0f / vector_length(vector_in, size), vector_out, size);
}

float vector_dot_product(float *vector_1, float *vector_2, int size)
{
	float sum = 0;
	for (int i = 0; i < size; ++i) {
		sum += vector_1[i] * vector_2[i];
	}
	return sum;
}

void vector3_cross_product(float *vector_1, float *vector_2, float *vector_out)
{
	vector_out[0] = vector_1[1] * vector_2[2] - vector_1[2] * vector_2[1];
	vector_out[1] = vector_1[2] * vector_2[0] - vector_1[0] * vector_2[2];
	vector_out[2] = vector_1[0] * vector_2[1] - vector_1[1] * vector_2[0];
}

static void __vector_matrix_multiply(float *vector_in, float *vector_out, int size, float matrix[size][size])
{
	for (int i = 0; i < size; ++i) {
		vector_out[i] = 0;
		for (int j = 0; j < size; ++j) {
			vector_out[i] += matrix[i][j] * vector_in[j];
		}
	}
}

void vector_matrix_multiply(float *matrix, float *vector_in, float *vector_out, int size)
{
	__vector_matrix_multiply(vector_in, vector_out, size, (float (*)[size]) matrix);
}

static void __matrix_scalar_multiply(int rows, int columns, float matrix_in[][columns], float scalar, float matrix_out[][columns])
{
	for (int i = 0; i < rows; ++i) {
		for (int j = 0; j < columns; ++j) {
			matrix_out[i][j] = matrix_in[i][j] * scalar;
		}
	}
}

void matrix_scalar_mulitply(float *matrix_in, float scalar, float *matrix_out, int rows, int columns)
{
	__matrix_scalar_multiply(rows, columns, (float (*)[columns]) matrix_in, scalar, (float (*)[columns]) matrix_out);
}

static void __matrix_multiply(int matrix1_rows, int common_dimension, int matrix2_columns, float matrix_1[][common_dimension], float matrix_2[][matrix2_columns], float matrix_out[][matrix2_columns])
{
	for (int i = 0; i < matrix1_rows; ++i) {
		for (int j = 0; j < matrix2_columns; ++j) {
			matrix_out[i][j] = 0;
			for (int k = 0; k < common_dimension; ++k) {
				matrix_out[i][j] += matrix_1[i][k] * matrix_2[k][j];
			}
		}
	}
}

void matrix_mulitply(float *matrix_1, float *matrix_2, float *matrix_out, int matrix1_rows, int common_dimension, int matrix2_columns)
{
	__matrix_multiply(matrix1_rows, common_dimension, matrix2_columns, (float (*)[common_dimension]) matrix_1, (float (*)[matrix2_columns]) matrix_2, (float (*)[matrix2_columns]) matrix_out);
}

static void _vector3_skew_matrix(float *vector_in, float matrix_out[3][3])
{
	matrix_out[0][0] = 0;
	matrix_out[0][1] = -vector_in[2];
	matrix_out[0][2] = vector_in[1];
	matrix_out[1][0] = vector_in[2];
	matrix_out[1][1] = 0;
	matrix_out[1][2] = -vector_in[0];
	matrix_out[2][0] = -vector_in[1];
	matrix_out[2][1] = vector_in[0];
	matrix_out[2][2] = 0;
}

static void __matrix_add(int rows, int columns, float matrix_1[rows][columns], float matrix_2[rows][columns], float matrix_out[rows][columns])
{
	for (int i = 0; i < rows; ++i) {
		for (int j = 0; j < columns; ++j) {
			matrix_out[i][j] = matrix_1[i][j] + matrix_2[i][j];
		}
	}
}

void matrix_add(float *matrix_1, float *matrix_2, float *matrix_out, int rows, int columns)
{
	__matrix_add(rows, columns, (float (*)[columns]) matrix_1, (float (*)[columns]) matrix_2, (float (*)[columns]) matrix_out);
}

void vector3_rotation_matrix(float *vector_1, float *vector_2, float matrix_out[3][3])
{
	float v[3];
	float v1[3];
	float v2[3];
	vector_normalize(vector_1, &v1[0], 3);
	vector_normalize(vector_2, &v2[0], 3);
	for (int i = 0; i < 3; ++i) {
		for (int j = 0; j < 3; ++j) {
			matrix_out[i][j] = i == j ? 1.0f : 0.0f;
		}
	}
	vector3_cross_product(v1, v2, &v[0]);

	float s = vector_length(&v[0], 3);
	if (s == 0) {
		if (v1[0] != v2[0]) {
			for (int i = 0; i < 3; ++i) {
				matrix_out[i][i] = -1;
			}
		}
		return;
	}
	float c = vector_dot_product(v1, v2, 3);
	float vs[3][3];
	float vs2[3][3];
	_vector3_skew_matrix(v, vs);
	matrix_mulitply(&vs[0][0], &vs[0][0], &vs2[0][0], 3, 3, 3);
	matrix_scalar_mulitply(&vs2[0][0], 1 / (1 + c), &vs2[0][0], 3, 3);
	matrix_add(&matrix_out[0][0], &vs[0][0], &matrix_out[0][0], 3, 3);
	matrix_add(&matrix_out[0][0], &vs2[0][0], &matrix_out[0][0], 3, 3);
}