#include "nolinearfilter.h"
#include <math.h>
#include "abaxCommon.h"
/* 非线性变换 
 * 实现变换公式 a1(sin(2 * Pi *b* d +c))  */
float nonLinearTrans(float d,  int i, double *pa, double *pb, double *pc)
{
    int k;
    float result = 0;
    double a, b, c;
    double (*pav)[SCREEN_PIXEL];
    double (*pbv)[SCREEN_PIXEL];
    double (*pcv)[SCREEN_PIXEL];
    pav = (double (*)[SCREEN_PIXEL])pa;
    pbv = (double (*)[SCREEN_PIXEL])pb;
    pcv = (double (*)[SCREEN_PIXEL])pc;

    for (k = 0; k < 10; k++)
    {
        a = *(*(pav+k)+i);
        b = *(*(pbv+k)+i);
        c = *(*(pcv+k)+i);
        result += a * sin(d * b * 2* PI + c);
    }
    result += d;
    result = abax_ModeF<float>(ABAX_LASER_FREQUENCE, result);
    return result;
}

int nonLinearFilterFun(float *src, float *dst, double *pa, double *pb, double *pc)
{
    int i, j;
    int offset, index;
    const int width = TOF_WIDTH;
    const int height = TOF_HEIGHT;

    for (i = 0; i < height; i++)
    {
        offset = i * width;
        for (j = 0; j < width; j++)
        {
            index=offset + j;
            *(dst+index) = nonLinearTrans(*(src+index), index, pa, pb, pc);
        }
    }
    return 0;
}