#include "xc.h"
#include "../CommonCode/pl_interp.h"
#include "libq.h"

#ifdef FOO
typedef struct pl_interp {
    unsigned int N;  //Number of points
    int X[MAX_N];
    int Y[MAX_N];
    long Slope[MAX_N];
}  PL_Interp;
#endif

void pl_interp_init(PL_Interp *s, int N, int *X, int *Y) {
    int i;
    s->N = N;
    for (i = 0; i < N; i++) {
        s->X[i] = X[i];
        s->Y[i] = Y[i];
        if (i > 0)
          s->Slope[i - 1] = ((long) ((long) Y[i] - (long)Y[i - 1]) << 15) / ((long) X[i] - (long) X[i - 1]);
    }

    return;
    
}

int pl_interp(PL_Interp *s,int x)
{
    int i;
    int y;
   
    if (x >= s->X[s->N - 1])
        x = s->X[s->N - 1] - 1;  //Set result to max in table if outside range

    if (x <= s->X[0])
        x = s->X[0];  //Set result to min in table if outside range
    
    for (i = 0; i < s->N - 1; i++) {
        if ((x >= s->X[i]) && (x < s->X[i + 1]))
        { //Solution is in this segment
            y = s->Y[i] + (int) ((s->Slope[i]*((long) x - (long) s->X[i])) >> 15);
            break;
        }
    }
    
    return y;
}