#include "KernelParams.h"

float linear_interpolate(float output, QList<float> x, QList<float> y, qint32 num)
{
    float slop = 0.0f;
    if(output <= x[0])
    {
      output = y[0];
    }
    else if(output >= x[num - 1])
    {
      output = y[num - 1];
    }
    else
    {
      for(qint32 i = 1; i < num; i++)
      {
        if(output < x[i])
        {
          slop = (y[i] - y[i - 1]) / (x[i] - x[i - 1]);
          output = y[i - 1] + ((output - x[i - 1]) * slop);
          break;
        }
      }
    }
    return output;
}

float DynamicContrast(float input, DynamicContrastParameters &params)
{
    float output = input + params.gain;

    QList<float> log_input_level = {0, params.app_min_log_input_level, params.app_max_log_input_level, params.max_log_input_level};
    QList<float> output_level = {0, params.min_output_level, params.max_output_level, 255};

    output = linear_interpolate(output, log_input_level, output_level, ARRAY_SIZE(output_level));

    output = linear_interpolate(output, params.const_piecewise_x, params.const_piecewise_y, ARRAY_SIZE(params.const_piecewise_x));

    return output;
}

QPair<qint32, qint32> readScanConversionIndex(float x, float z, scanConversionParameters &params)
{
    QPair<qint32, qint32> index;

    if(abs(x - params.gridX.at(static_cast<qint32>(params.gridX_size - 1))) < EPSILSON)
    {
        index.first = static_cast<qint32>(params.gridX_size - 2);
    }
    else
    {
        index.first = static_cast<qint32>(floorf((x - params.gridX.at(0)) / params.dx));
    }

    if(abs(z - params.gridZ.at(static_cast<qint32>(params.gridZ_size - 1))) < EPSILSON)
    {
        index.second = static_cast<qint32>(params.gridZ_size - 2);
    }
    else
    {
        index.second = static_cast<qint32>(floorf((z - params.gridZ.at(0)) / params.dz));
    }

    return index;
}

float scanConversionFmaCalc(scanConversionMatrix &field)
{
    return (fmaf(field.c11, (fmaf(field.a11, field.b11, (field.a12 * field.b21))),          \
           (field.c12 * (fmaf(field.a11, field.b12, (field.a12 * field.b22))))) / (field.dx * field.dz));
}