/*!

* @file qinterp1.c

* @author J. Camilo Gomez C.

* @note This file is part of the qLibs distribution.

**/

#include "qinterp1.h"

#include "qffmath.h"

typedef float (*qInterp1Fcn_t)( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_next( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_previous( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_nearest( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_linear( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_sine( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_cubic( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_hermite( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_spline( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float qInterp1_cSpline( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize );

static float slope( const float * const tx,

const float * const ty,

const size_t i );

float firstDerivate( const float * const tx,

const float * const ty,

const size_t n,

const size_t i );

float leftSecondDerivate( const float * const tx,

const float * const ty,

const size_t n,

const size_t i );

float rightSecondDerivate( const float * const tx,

const float * const ty,

const size_t n,

const size_t i );

/*cstat -CERT-INT30-C_a*/

/*============================================================================*/

int qInterp1_Setup( qInterp1_t * const i,

const float * const xTable,

const float * const yTable,

const size_t sizeTable )

{

int retVal = 0;

if ( ( NULL != i ) && ( sizeTable > 2U ) && ( NULL != xTable ) && ( NULL != yTable ) ) {

i->xData = xTable;

i->yData = yTable;

i->dataSize = sizeTable;

i->method = &qInterp1_linear;

retVal = 1;

}

return retVal;

}

/*============================================================================*/

int qInterp1_SetData( qInterp1_t * const i,

const float * const xTable,

const float * const yTable,

const size_t sizeTable )

{

return qInterp1_Setup( i, xTable, yTable, sizeTable );

}

/*============================================================================*/

int qInterp1_SetMethod( qInterp1_t * const i,

const qInterp1Method_t m )

{

int retVal = 0;

static const qInterp1Fcn_t im[ QINTERP1_MAX ] = {

&qInterp1_next,

&qInterp1_previous,

&qInterp1_nearest,

&qInterp1_linear,

&qInterp1_sine,

&qInterp1_cubic,

&qInterp1_hermite,

&qInterp1_spline,

&qInterp1_cSpline,

};

if ( ( NULL != i ) && ( m < QINTERP1_MAX ) ) {

i->method = im[ m ];

retVal = 1;

}

return retVal;

}

/*============================================================================*/

float qInterp1_Get( qInterp1_t * const i,

const float x )

{

return i->method( x, i->xData, i->yData, i->dataSize );

}

/*============================================================================*/

static float qInterp1_next( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 2U ) && ( NULL != tx ) && ( NULL != ty ) ) {

size_t nearestIndex = tableSize - 1U;

for ( size_t i = 0U ; i < ( tableSize - 1U ) ; ++i ) {

if ( x < tx[ i + 1U ] ) {

nearestIndex = i;

break;

}

}

if ( x >= tx[ tableSize - 1U ] ) {

y = ty[ tableSize - 1U ];

}

else {

y = ty[ nearestIndex + 1U ];

}

}

return y;

}

/*============================================================================*/

static float qInterp1_previous( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 2U ) && ( NULL != tx ) && ( NULL != ty ) ) {

if ( x <= tx[ 0 ] ) {

y = ty[ 0 ];

}

else {

size_t nearestIndex = 0U;

for ( size_t i = 1U ; i < tableSize; ++i ) {

if ( x < tx [ i ] ) {

break;

}

nearestIndex = i;

}

y = ty[ nearestIndex ];

}

}

return y;

}

/*============================================================================*/

static float qInterp1_nearest( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 2U ) && ( NULL != tx ) && ( NULL != ty ) ) {

size_t nearestIndex = 0U;

float minDistance = qFFMath_Abs( x - tx[ 0 ] );

for ( size_t i = 1U ; i < tableSize; ++i ) {

const float distance = qFFMath_Abs( x - tx[ i ] );

if ( distance <= minDistance) {

minDistance = distance;

nearestIndex = i;

}

}

y = ty[ nearestIndex ];

}

return y;

}

/*============================================================================*/

static float qInterp1_linear( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 2U ) && ( NULL != tx ) && ( NULL != ty ) ) {

if ( x < tx[ 0 ] ) {

const float x0 = tx[ 0 ];

const float x1 = tx[ 1 ];

const float y0 = ty[ 0 ];

const float y1 = ty[ 1 ];

y = y0 + ( ( ( y1 - y0 )/( x1 - x0 ) )*( x - x0 ) );

}

else if ( x > tx[ tableSize - 1U ] ) {

const float x0 = tx[ tableSize - 2U ];

const float x1 = tx[ tableSize - 1U ];

const float y0 = ty[ tableSize - 2U ];

const float y1 = ty[ tableSize - 1U ];

y = y1 + ( ( ( y0 - y1 )/( x0 - x1 ) )*( x - x1 ) );

}

else {

const int maxIndex = (int)tableSize - 1;

for ( int i = 0; i < maxIndex; ++i ) {

if ( ( x >= tx[ i ] ) && ( x <= tx[ i + 1 ] ) ) {

const float x0 = tx[ i ];

const float x1 = tx[ i + 1 ];

const float y0 = ty[ i ];

const float y1 = ty[ i + 1 ];

y = y0 + ( ( ( y1 - y0 )/( x1 - x0 ) )*( x - x0 ) );

break;

}

}

}

}

return y;

}

/*============================================================================*/

static float qInterp1_sine( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 2U ) && ( NULL != tx ) && ( NULL != ty ) ) {

if ( x < tx[ 0 ] ) {

const float x0 = tx[ 0 ];

const float x1 = tx[ 1 ];

const float y0 = ty[ 0 ];

const float y1 = ty[ 1 ];

const float w = 0.5F - ( 0.5F*qFFMath_Cos( QFFM_PI*( x - x0 )/( x1 - x0 ) ) );

y = y0 + ( w*( y1 - y0 ) );

}

else if ( x > tx[ tableSize - 1U ] ) {

const float x0 = tx[ tableSize - 2U ];

const float x1 = tx[ tableSize - 1U ];

const float y0 = ty[ tableSize - 2U ];

const float y1 = ty[ tableSize - 1U ];

const float w = 0.5F - ( 0.5F*qFFMath_Cos( QFFM_PI*( x - x1 )/( x0 - x1 ) ) );

y = y1 + ( w*( y0 - y1 ) );

}

else {

for ( size_t i = 1; i < tableSize; ++i ) {

if ( x <= tx[ i ] ) {

const float x0 = tx[ i - 1U ];

const float x1 = tx[ i ];

const float y0 = ty[ i - 1U ];

const float y1 = ty[ i];

const float w = 0.5F - ( 0.5F*qFFMath_Cos( QFFM_PI*( x - x0 )/( x1 - x0 ) ) );

y = y0 + ( w*( y1 - y0 ) );

}

}

}

}

return y;

}

/*============================================================================*/

static float qInterp1_cubic( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 4U ) && ( NULL != tx ) && ( NULL != ty ) ) {

if ( x < tx[ 0 ] ) {

const float x0 = tx[ 0 ];

const float x1 = tx[ 1 ];

const float y0 = ty[ 0 ];

const float y1 = ty[ 1 ];

const float h = x1 - x0;

const float t = ( x - x0 )/h;

const float t2 = t*t;

const float t3 = t2*t;

y = ( ( ( 2.0F*t3 ) - ( 3.0F*t2 ) + 1.0F )*y0 ) +

( ( t3 - ( 2.0F*t2 ) + t )*h*( y0 - y1 ) ) +

( ( -( 2.0F*t3 ) + ( 3.0F*t2 ) )*y1 ) +

( ( t3 - t2 )*h*( y1 - y0 ) );

}

else if ( x > tx[ tableSize - 1U ] ) {

const float x0 = tx[ tableSize - 2U ];

const float x1 = tx[ tableSize - 1U ];

const float y0 = ty[ tableSize - 2U ];

const float y1 = ty[ tableSize - 1U ];

const float h = x1 - x0;

const float t = ( x - x1 )/h;

const float t2 = t*t;

const float t3 = t2*t;

y = ( ( ( 2.0F*t3 ) - ( 3.0F*t2 ) + 1.0F )*y1 ) +

( ( t3 - ( 2.0F*t2 ) + t )*h*( y0 - ty[ tableSize - 3U ] ) ) +

( ( -( 2.0F*t3 ) + ( 3.0F*t2 ) )*y0 ) +

( ( t3 - t2 )*h*( y1 - y0 ) );

}

else {

for ( size_t i = 1U; i < tableSize; ++i ) {

if ( x <= tx[ i ] ) {

const float x0 = tx[ i - 1U ];

const float x1 = tx[ i ];

const float y0 = ty[ i - 1U ];

const float y1 = ty[ i ];

const float h = x1 - x0;

const float t = ( x - x0 )/h;

const float t2 = t*t;

const float t3 = t2*t;

y = ( ( ( 2.0F*t3 ) - ( 3.0F*t2 ) + 1.0F )*y0 ) +

( ( t3 - ( 2.0F*t2 ) + t )*h*( y0 - ty[ i - 2U ] ) ) +

( ( -( 2.0F*t3 ) + ( 3.0F*t2 ) )*y1 ) +

( ( t3 - t2 )*h*( y1 - y0 ) );

break;

}

}

}

}

return y;

}

/*============================================================================*/

static float qInterp1_hermite( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 2U ) && ( NULL != tx ) && ( NULL != ty ) ) {

if ( x < tx[ 0 ] ) {

const float x0 = tx[ 0 ];

const float x1 = tx[ 1 ];

const float y0 = ty[ 0 ];

const float y1 = ty[ 1 ];

y = y0 + ( ( ( y1 - y0 )/( x1 - x0 ) )*( x - x0 ) );

}

else if ( x > tx[ tableSize -1U ] ) {

const float x0 = tx[ tableSize - 2U ];

const float x1 = tx[ tableSize - 1U ];

const float y0 = ty[ tableSize - 2U ];

const float y1 = ty[ tableSize - 1U ];

y = y1 + ( ( ( y0 - y1 )/( x0 - x1 ) )*( x - x1 ) );

}

else {

y = 0.0F;

for ( size_t i = 0U ; i < tableSize; ++i ) {

float term = ty[ i ];

for ( size_t j = 0U ; j < tableSize; ++j ) {

if ( i != j ) {

term *= ( x - tx[ j ] )/( tx[ i ] - tx[ j ] );

}

}

y += term;

}

}

}

return y;

}

/*============================================================================*/

static float qInterp1_spline( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 4U ) && ( NULL != tx ) && ( NULL != ty ) ) {

size_t i = 0U;

/* Extrapolation for x beyond the range*/

if ( x <= tx[ 0 ] ) {

i = 0U;

}

else if ( x >= tx[ tableSize - 1U ] ) {

i = tableSize - 2U; /* Use the last interval for extrapolation*/

}

else {

while ( x >= tx[ i + 1U ] ) {

i++;

}

}

if ( qFFMath_IsEqual( x , tx[ i + 1U ] ) ) {

y = ty[ i + 1U ];

}

else {

const float t = ( x - tx[ i ] )/( tx[ i + 1U ] - tx[ i ] );

const float t_2 = t*t;

const float t_3 = t_2*t;

const float tt_3 = 2.0F*t_3;

const float tt_2 = 3.0F*t_2;

const float h01 = tt_2 - tt_3;

const float h00 = 1.0F - h01;

const float h10 = t_3 - ( 2.0F*t_2 ) + t;

const float h11 = t_3 - t_2;

const float x1_x0 = tx[ i + 1U ] - tx[ i ];

const float y0 = ty[ i ];

const float y1 = ty[ i + 1U ];

float m0;

float m1;

if ( 0U == i ) {

m0 = ( ty[ 1 ] - ty[ 0 ] )/( tx[ 1 ] - tx[ 0 ] );

m1 = ( ty[ 2 ] - ty[ 0 ] )/( tx[ 2 ] - tx[ 0 ] );

}

else if ( ( tableSize - 2U ) == i ) {

m0 = ( ty[ tableSize - 1U ] - ty[ tableSize - 3U ] )/( tx[ tableSize - 1U ] - tx[ tableSize - 3U ] );

m1 = ( ty[ tableSize - 1U ] - ty[ tableSize - 2U ] )/( tx[ tableSize - 1U ] - tx[ tableSize - 2U ] );

}

else {

m0 = slope( tx, ty, i );

m1 = slope( tx, ty, i + 1U );

}

y = ( h00*y0 ) + ( h01*y1 ) + ( h10*x1_x0*m0 ) + ( h11*x1_x0*m1 );

}

}

return y;

}

/*============================================================================*/

static float qInterp1_cSpline( const float x,

const float * const tx,

const float * const ty,

const size_t tableSize )

{

float y = QFFM_NAN;

if ( ( tableSize >= 4U ) && ( NULL != tx ) && ( NULL != ty ) ) {

size_t i = 0U;

/* Extrapolation for x beyond the range*/

if ( x <= tx[ 0 ] ) {

i = 0U;

}

else if ( x >= tx[ tableSize - 1U ] ) {

i = tableSize - 2U; /* Use the last interval for extrapolation*/

}

else {

while ( x >= tx[ i + 1U ] ) {

i++;

}

}

if ( qFFMath_IsEqual( x , tx[ i + 1U ] ) ) {

y = ty[ i + 1U ];

}

else {

const float x0 = tx[ i ];

const float x1 = tx[ i + 1U ];

const float y0 = ty[ i ];

const float y1 = ty[ i + 1U ];

const float fd2i_xl1 = leftSecondDerivate( tx, ty, tableSize - 1U, i + 1U );

const float fd2i_x = rightSecondDerivate( tx, ty, tableSize - 1U, i + 1U );

const float x0_x1 = x0 - x1;

const float x1_2 = x1*x1;

const float x1_3 = x1_2*x1;

const float x0_2 = x0*x0;

const float x0_3 = x0_2*x0;

const float inv_x0_x1 = 1.0F/x0_x1;

const float d = ( fd2i_x - fd2i_xl1 )*( 0.166666667F*inv_x0_x1 );

const float c = ( ( x0*fd2i_xl1 ) - ( x1*fd2i_x ) )*( 0.5F*inv_x0_x1 );

const float b = ( y0 - y1 - ( c*( x0_2 - x1_2 ) ) - ( d*( x0_3 - x1_3 ) ) )*inv_x0_x1;

const float a = y1 - ( b*x1 ) - ( c*x1_2 ) - ( d*x1_3 );

y = a + ( x*( b + ( x*( c + ( x*d ) ) ) ) );

}

}

return y;

}

/*============================================================================*/

static float slope( const float * const tx,

const float * const ty,

const size_t i )

{

float m;

if ( qFFMath_IsEqual( tx[ i + 1U ], tx[ i - 1U ] ) ) {

m = 0.0F;

}

else {

m = ( ty[ i + 1U ] - ty[ i - 1U ] )/( tx[ i + 1U ] - tx[ i - 1U ] );

}

return m;

}

/*============================================================================*/

float firstDerivate( const float * const tx,

const float * const ty,

const size_t n,

const size_t i )

{

float fd1_x;

if ( 0U == i ) {

const float dx = tx[ 1 ] - tx[ 0 ];

const float dy = ty[ 1 ] - ty[ 0 ];

fd1_x = 1.5F*( dy/dx );

fd1_x -= 1.0F/( ( ( tx[ 2 ] - tx[ 0 ] )/( ty[ 2 ] - ty[ 0 ] ) ) + ( dx/dy ) );

}

else if ( n == i ) {

const float dx = tx[ n ] - tx[ n - 1U ];

const float dy = ty[ n ] - ty[ n - 1U ];

fd1_x = 1.5F*( dy/dx );

fd1_x -= 1.0F/( ( ( tx[ n ] - tx[ n - 2U ] )/( ty[ n ] - ty[ n - 2U ] ) ) + ( dx/dy ) );

}

else {

const float tmp1 = ( tx[ i + 1U ] - tx[ i ] )/( ty[ i + 1U ] - ty[ i ] );

const float tmp2 = ( tx[ i ] - tx[ i - 1U ] )/( ty[ i ] - ty[ i - 1U ] );

if ( ( tmp1*tmp2 ) < 0.0F ) {

fd1_x = 0.0F;

}

else {

fd1_x = 2.0F/( tmp1 + tmp2 );

}

}

return fd1_x;

}

/*============================================================================*/

float leftSecondDerivate( const float * const tx,

const float * const ty,

const size_t n,

const size_t i )

{

const float fdi_x = firstDerivate( tx, ty, n, i );

const float fdi_xl1 = firstDerivate( tx, ty, n, i - 1U );

const float xi_delta = tx[ i ] - tx[ i - 1U ];

float fd2l_x = -2.0F*( fdi_x + ( 2.0F*fdi_xl1 ) )/xi_delta;

fd2l_x += 6.0F*( ty[ i ] - ty[ i - 1U ] )/( xi_delta*xi_delta );

return fd2l_x;

}

/*============================================================================*/

float rightSecondDerivate( const float * const tx,

const float * const ty,

const size_t n,

const size_t i )

{

const float fdi_x = firstDerivate( tx, ty, n, i );

const float fdi_xl1 = firstDerivate( tx, ty, n, i - 1U );

const float xi_delta = tx[ i ] - tx[ i - 1U ];

float fd2r_x = 2.0F*( ( 2.0F*fdi_x ) + fdi_xl1 )/xi_delta;

fd2r_x -= 6.0F*( ty[ i ] - ty[ i - 1U ] )/( xi_delta*xi_delta );

return fd2r_x;

}

/*============================================================================*/

/*cstat +CERT-INT30-C_a*/