/**
 * @file    interpolation.cpp
 * @brief	Linear interpolation algorithms
 *
 * See test_interpolation_3d.cpp
 *
 *
 * @date Oct 17, 2013
 * @author Andrey Belomutskiy, (c) 2012-2020
 * @author Dmitry Sidin, (c) 2015
 */

#include "pch.h"

#include "efi_interpolation.h"

/** @brief	Linear interpolation by two points
 *
 * @param	x1 key of the first point
 * @param	y1 value of the first point
 * @param	x2 key of the second point
 * @param	y2 value of the second point
 * @param	X key to be interpolated
 *
 * @note	For example, "interpolateMsg("", engineConfiguration.tpsMin, 0, engineConfiguration.tpsMax, 100, adc);"
 * @see interpolateClamped
 */
float interpolateMsg(const char *msg, float x1, float y1, float x2, float y2, float x) {
	if (cisnan(x1) || cisnan(x2) || cisnan(y1) || cisnan(y2)) {
		warning(ObdCode::CUSTOM_ERR_INTERPOLATE_1, "interpolate%s: why param", msg);
		return NAN;
	}
	if (cisnan(x)) {
		warning(ObdCode::CUSTOM_ERR_INTERPOLATE_2, "interpolate%s: why X", msg);
		return NAN;
	}
	// todo: double comparison using EPS
	if (x1 == x2) {
		/**
		 * we could end up here for example while resetting bins while changing engine type
		 */
		warning(ObdCode::CUSTOM_ERR_INTERPOLATE_3, "interpolate%s: Same x1 and x2 in interpolate: %.2f/%.2f", msg, x1, x2);
		return NAN;
	}

	// a*x1 + b = y1
	// a*x2 + b = y2
//	efiAssertVoid(ObdCode::CUSTOM_ERR_ASSERT_VOID, x1 != x2, "no way we can interpolate");
	float a = INTERPOLATION_A(x1, y1, x2, y2);
	if (cisnan(a)) {
		warning(ObdCode::CUSTOM_ERR_INTERPOLATE_4, "interpolate%s: why a", msg);
		return NAN;
	}
	float b = y1 - a * x1;
	return a * x + b;
}

float interpolateClampedWithValidation(float x1, float y1, float x2, float y2, float x) {
	if (x1 >= x2) {
        criticalError("interpolateClamped %f has to be smaller than %f", x1, x2);
	}
	return interpolateClamped(x1, y1, x2, y2, x);
}

/**
 * todo: use 'interpolateClampedWithValidation' wider?
 * @see interpolateMsg
 */
float interpolateClamped(float x1, float y1, float x2, float y2, float x) {
	// note how we assume order of x1 and x2 here! see also interpolateClampedWithValidation
	if (x <= x1)
		return y1;
	if (x >= x2)
		return y2;

    // todo: do we care with code duplication with interpolateMsg above?
	float a = INTERPOLATION_A(x1, y1, x2, y2);
	float b = y1 - a * x1;
	return a * x + b;
}