#include <stdio.h>
#include <math.h>
#include "pslib_strict.h"
#include "psTest.h"

static psS32 testPolynomial1DEval_01(void);
static psS32 testPolynomial1DEvalVector_01(void);
static psS32 testPolynomial1DEvalVector_02(void);
static psS32 testVectorFitPolynomial1D_01(void);
static psS32 testVectorFitPolynomial1D_02(void);
static psS32 testVectorClipFitPolynomial1D_01(void);

/* list of test functions in test suite */
testDescription tests[] = {
                              {testPolynomial1DEval_01,1,"psPolynomial1DEval_01",0,false},
                              {testPolynomial1DEvalVector_01,2,"psPolynomial1DEvalVector_01",0,false},
                              {testPolynomial1DEvalVector_02,2,"psPolynomial1DEvalVector_02",0,false},
                              {testVectorFitPolynomial1D_01,2,"psVectorFitPolynomial1D_01",0,false},
                              {testVectorFitPolynomial1D_02,2,"psVectorFitPolynomial1D_02",0,false},
                              {testVectorClipFitPolynomial1D_01,2,"psVectorFitClipPolynomial1D_01",0,false},
                              {NULL}
                          };

/* basic main function: just need to define the test suite name */
int main(int argc, char* argv[])
{
    //psTraceSetLevel("VectorFitPolynomial1DOrd", 6);
    psLogSetFormat("HLNM");
    psLogSetLevel(PS_LOG_INFO);
    return !runTestSuite(stderr,"psPolynomialExamples", tests, argc, argv);
}

# define X0 +5.0
# define X1 +3.0
# define X2 -1.0
# define X3 -2.0


static psPolynomial1D *makePolynomial1D(int Nord, psPolynomialType type, int ordMask) {

    // define the polynomial
    psPolynomial1D *poly = psPolynomial1DAlloc(Nord, type);
    switch (Nord) {
      case 3:
	poly->coeff[3] = (ordMask==3) ?  0:X3;
      case 2:
	poly->coeff[2] = (ordMask==2) ?  0:X2;
      case 1:
	poly->coeff[1] = (ordMask==1) ?  0:X1;
      case 0:
	poly->coeff[0] = (ordMask==0) ?  0:X0;
	break;
      default:
	break;
    }
    return (poly);
}

static void fillVectors(psVector *x, psVector *y, int Nord, int Npts, psPolynomialType type) {

    psF32 X;//, P1, P2, P3;
    double c[] = {(double)X0,(double)X1,(double)X2,(double)X3};
    //double sv,d,dd;
    double sum, Tn, Tn_1, Tn_2;

    for (int i = 0; i < Npts; i++) {
	X = (float)i/Npts - 0.5;
	x->data.F32[i] = X;
	switch (type) {
	  case PS_POLYNOMIAL_ORD:
	    switch (Nord) {
	      case 0:
		y->data.F32[i] = c[0];
		break;
	      case 1:
		y->data.F32[i] = c[0] + c[1]*X;
		break;
	      case 2:
		y->data.F32[i] = c[0] + c[1]*X + c[2]*X*X;
		break;
	      case 3:
		y->data.F32[i] = c[0] + c[1]*X + c[2]*X*X + c[3]*X*X*X;
		break;
	      default:
		y->data.F32[i] = 0;
	    }
	    break;
	  case PS_POLYNOMIAL_CHEB:
	    switch (Nord) {
	      case 0:
		y->data.F32[i] = c[0];
		break;
	      case 1:
		y->data.F32[i] =c[0] + c[1]*X;
		break;
	      default:
		//Clenshaw's formula approach (NR 5.5)
		//d=0.0;
		//dd=0.0;
		//for (int j=Nord; j>=1; j--) {
		//    sv = d;
		//    d = (double)2.0*(double)X*d - dd + c[j];
		//    dd = sv;
		//}
		//y->data.F32[i] = (double)X*d-dd+(double)0.5*c[0];

		//Straightforward approach
		Tn_1=X; //T1
		Tn_2=1; //T0
		sum=0.;
		for (int j=2; j<=Nord; j++) {
		    Tn = 2.0*X*Tn_1 - Tn_2;
		    sum = sum + c[j]*Tn;
		    Tn_2 = Tn_1;
		    Tn_1 = Tn;
		}
		y->data.F32[i] = sum + c[1]*X + c[0] - 0.5*c[0];
	    }
	    break;
	}
    }
    return;
}

# define NPTS 10

int testPolynomial1DEval_01(void) {
    
    //# define NORD 3
    int NORD;
    int maxNORD = 3;
    int retval = 0;
    psPolynomialType type[] = {PS_POLYNOMIAL_ORD, PS_POLYNOMIAL_CHEB};

    for (int k = 0; k<1; k++) {  //loop over the 2 polynomial types
	for (int j = 1; j <= maxNORD; j++) {  //loop over the polynomial orders
	    int nGood = 0, nBad = 0; 
	    NORD = j; 
	    //printf("Testing NORD=%d\n", NORD);
	    psPolynomial1D *poly = makePolynomial1D(NORD, type[k], -1); 
	    
	    //create x vector, known y vector: 
	    psVector *x = psVectorAlloc (NPTS, PS_TYPE_F32); 
	    psVector *y = psVectorAlloc (NPTS, PS_TYPE_F32); 
	    fillVectors (x, y, NORD, NPTS, type[k]); 
	    
	    psF32 xV, yV; 
	    
	    for (int i = 0; i < NPTS; i++) { 
		xV = x->data.F32[i]; 
		yV = psPolynomial1DEval (poly, xV); 
		//Change some values around to make it fail!
		//if (i>90 && NORD==2) {yV = yV + 0.1;}
		if (yV == y->data.F32[i]) { 
		    nGood++; 
		} else { 
		    nBad++; 
		    printf("%.3f   -->  %.10f  & %.10f\tdel=%e\n",xV,yV,y->data.F32[i],y->data.F32[i]-yV); 
		} 
	    } 
	    psFree (x); 
	    psFree (y); 
	    psFree (poly); 
	    
	    if (nBad > 0) { 
		psError(PS_ERR_UNKNOWN,true,"%d incorrect polynomial evaluations for NORD=%d\n", nBad, NORD); 
		retval = retval + pow(2,NORD); 
	    } 
	}
    }
    return retval;
}


int testPolynomial1DEvalVector_01(void) {
    
    //# define NORD 3
    int NORD;
    int maxNORD = 3;
    int retval = 0;
    psPolynomialType type[] = {PS_POLYNOMIAL_ORD, PS_POLYNOMIAL_CHEB};
    
    for (int k = 0; k<1; k++) {  //loop over the 2 polynomial types
	for (int j = 1; j <= maxNORD; j++) {  //loop over the polynomial orders
	    int nGood = 0, nBad = 0; 
	    NORD = j; 
	    //printf("Testing NORD=%d\n", NORD);
	    psPolynomial1D *poly = makePolynomial1D(NORD, type[k], -1); 
	    
	    // create x vector, known y vector:
	    psVector *x = psVectorAlloc (NPTS, PS_TYPE_F32); 
	    psVector *y = psVectorAlloc (NPTS, PS_TYPE_F32); 
	    fillVectors (x, y, NORD, NPTS, type[k]); 
	    
	    psVector *xV = x; 
	    psVector *yV = psPolynomial1DEvalVector(poly, xV); 
	    
	    for (int i = 0; i < NPTS; i++) { 
		//Change some values around to make it fail!
		//if (i>90 && NORD==2) {yV->data.F32[i] = yV->data.F32[i] + 0.1;}
		if (yV->data.F32[i] == y->data.F32[i]) { 
		    nGood++; 
		} else { 
		    nBad++; 
		    printf("%.3f   -->  %.3f  & %.3f\tdel=%e\n",xV->data.F32[i],yV->data.F32[i],y->data.F32[i],y->data.F32[i]-yV->data.F32[i]); 
		} 
	    } 
	    psFree (x); 
	    psFree (y); 
	    psFree (yV); 
	    psFree (poly); 
	    
	    if (nBad > 0) { 
		psError(PS_ERR_UNKNOWN,true,"%d incorrect polynomial evaluations for NORD=%d\n", nBad, NORD); 
		retval = retval + pow(2,NORD); 
	    } 
	}
    }
    return retval;
}
int testPolynomial1DEvalVector_02(void) {
    //  check that psPolynomial1DEvalVector obeys the psPolynomial1D.mask 
    int NORD;
    int maxNORD = 3;
    int retval = 0;
    psPolynomialType type[] = {PS_POLYNOMIAL_ORD, PS_POLYNOMIAL_CHEB};
    
    for (int k = 0; k<1; k++) {  //loop over the 2 polynomial types
	for (int imask=0; imask<=maxNORD; imask++) {  // mask one order at a time
	    int j= maxNORD;
	    NORD = j;
	    int nGood = 0, nBad = 0; 
	    psPolynomial1D *poly       = makePolynomial1D(NORD, type[k], -1);
	    psPolynomial1D *polynomask = makePolynomial1D(NORD, type[k], imask);
	    poly->mask[imask]=1;

	    // create x vector, known y vector:
	    psVector *x = psVectorAlloc (NPTS, PS_TYPE_F32); 
	    psVector *y = psVectorAlloc (NPTS, PS_TYPE_F32); 
	    fillVectors (x, y, NORD, NPTS, type[k]); 
	    
	    psVector *xV = x; 
	    psVector *yV = psPolynomial1DEvalVector(poly, xV); 
	    psVector *yVnm = psPolynomial1DEvalVector(polynomask, xV); 
	    
	    for (int i = 0; i < NPTS; i++) { 
		if (yV->data.F32[i] == yVnm->data.F32[i]) { 
		    nGood++; 
		} else { 
		    nBad++; 
		    printf("x:%.3f  y:%.3f , %.3f , %.3f\tdel=%e\n",xV->data.F32[i],y->data.F32[i],yV->data.F32[i],yVnm->data.F32[i],yVnm->data.F32[i]-yV->data.F32[i]); 
		} 
	    } 
	    psFree (x); 
	    psFree (y); 
	    psFree (yV); 
	    psFree (yVnm); 
	    psFree (poly); 
	    psFree (polynomask); 
	    
	    if (nBad > 0) { 
		psError(PS_ERR_UNKNOWN,true,"%d incorrect polynomial evaluations for NORD=%d\n", nBad, NORD); 
		retval = retval + pow(2,NORD); 
	    } 
	}
    }
    return retval;
}



int testVectorFitPolynomial1D_01(void) { 
    int retval=0;
    int NORD, maxNORD=3;
    float tolerance = 1.0E-5;
    psPolynomialType type[] = {PS_POLYNOMIAL_ORD, PS_POLYNOMIAL_CHEB};
    
    for (int k = 0; k<1; k++) {  //loop over the 2 polynomial types   XXX: not using Chebyshevs right now!!!
	for (int j = 1; j <= maxNORD; j++) {  //loop over the polynomial orders 
	    int nGood = 0, nBad = 0; 
	    NORD = j;
	    printf("Testing NORD=%d\n", NORD);
	    psPolynomial1D *poly = makePolynomial1D(NORD, type[k], -1);
	    psPolynomial1D *polycopy = makePolynomial1D(NORD, type[k], -1);
	    psVector *f = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *fErr = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *x = psVectorAlloc(NPTS, PS_TYPE_F32);
	    
	    fillVectors(x, f, NORD, NPTS, type[k]);
	    psTime *now = psTimeGetNow(PS_TIME_UTC);
	    const psRandom *r = psRandomAlloc(PS_RANDOM_TAUS, now->sec);
	    for (int i = 0; i < NPTS; i++) {
		double errgen = psRandomGaussian(r);
		double amp = 1.0E-3;
		fErr->data.F32[i] = amp*errgen;
		//fErr->data.F32[i] = 0.;
	    }
	    psFree(now);
	    psFree(r);

	    //f->data.F32[NPTS-1] = X0 + 10.*X1*x->data.F32[NPTS-1];
	    //fErr->data.F32[NPTS-1] = 0.0000001;


	    psVectorFitPolynomial1D(poly, NULL, 0, f, fErr, x);
	    for (int i = 0; i <= NORD; i++) { 
		if (poly->coeff[i] <= polycopy->coeff[i]+tolerance 
		      &&
		    poly->coeff[i] >= polycopy->coeff[i]-tolerance) { 
		    nGood++; 
		} else { 
		    nBad++; 
		    printf("polycopy->coeff[%d]=%.20f    poly->coeff[%d]=%.20f\n",i,polycopy->coeff[i],i,poly->coeff[i]); 
		}
		if (polycopy->coeffErr[i] != poly->coeffErr[i]) { 
		    printf("polycopy->coeffErr[%d]=%.20f    poly->coeffErr[%d]=%.20f\n",i,polycopy->coeffErr[i],i,poly->coeffErr[i]); 
		}		
	    } 

	    if (nBad > 0) { 
		psError(PS_ERR_UNKNOWN,true,"%d incorrect polynomial evaluations for NORD=%d\n", nBad, NORD); 
		retval = retval + pow(2,NORD); 
	    } 

	    printf("NORD: %d   retval: %d\n",NORD,retval);
	    psFree(poly);
	    psFree(polycopy);
	    psFree(f);
	    psFree(fErr);
	    psFree(x);	    
	}
    }
    return retval;
}


int testVectorFitPolynomial1D_02(void) { 
    // this time test that the masking in psPolynomial1D works correctly
    int retval=0;
    int NORD, maxNORD=3;
    float tolerance = 1.0E-5;
    psPolynomialType type[] = {PS_POLYNOMIAL_ORD, PS_POLYNOMIAL_CHEB};
    
    for (int k = 0; k<1; k++) {  //loop over the 2 polynomial types   XXX: not using Chebyshevs right now!!!
	for (int imask=0; imask<=maxNORD; imask++) {  // mask one order at a time
	    int j= maxNORD;
	    NORD = j;
	    int nGood = 0, nBad = 0; 
	    psPolynomial1D *poly       = makePolynomial1D(NORD, type[k], -1);
	    psPolynomial1D *polynomask = makePolynomial1D(NORD, type[k], imask);
	    poly->mask[imask]=1;
	    //poly and polynomask look the same now.

	    psVector *mask = psVectorAlloc(NPTS, PS_TYPE_U8);
	    psVector *f = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *fErr = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *x = psVectorAlloc(NPTS, PS_TYPE_F32);
	    
	    fillVectors(x, f, NORD, NPTS, type[k]);
	    
	    // add random errors to the vector
	    psTime *now = psTimeGetNow(PS_TIME_UTC);
	    const psRandom *r = psRandomAlloc(PS_RANDOM_TAUS, now->sec);
	    for (int i = 0; i < NPTS; i++) {
		double errgen = psRandomGaussian(r);
		double amp = 1.0E-3;
		fErr->data.F32[i] = amp*errgen;
		//fErr->data.F32[i] = 0.;
	    }
	    psFree(now);
	    psFree(r);
	    
	    
	    //mask->data.U8[imask] = 0xff;
	    //for (int ii=0; ii<=mask->n; ii++) {
	    //printf("  %d",mask->data.U8[ii]);
	    //}
	    //printf("\n");

	    poly->mask[imask] = 1;
	    for (int ii=0; ii<=poly->nX; ii++) {
		printf("%x  ", poly->mask[ii]);
	    }
	    printf("\n");

	    psVectorFitPolynomial1D(poly, NULL, 0xff, f, fErr, x);
	    psVectorFitPolynomial1D(polynomask, NULL, 0xff, f, fErr, x);

	    psVector *xx = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *yy = psPolynomial1DEvalVector(poly, xx);
	    psVector *del;
	    psBinaryOp(del, f, "-", yy);
	    psPolynomial1D *polydel = makePolynomial1D(NORD, type[k], -1);
	    psVectorFitPolynomial1D(polydel, NULL,0xff, f, fErr, x);
	    


	    //mask->data.U8[imask] = 0;
	    poly->mask[imask] = 0;
	    for (int i = 0; i <= NORD; i++) { 
		printf("polynomask->coeff[%d]=%.8f    poly->coeff[%d]=%.8f    polydel->coeff[%d]=%.8f\n",i,polynomask->coeff[i],i,poly->coeff[i],i,polydel->coeff[i]); 
		if (poly->coeff[i] <= polynomask->coeff[i]+tolerance 
		    &&
		    poly->coeff[i] >= polynomask->coeff[i]-tolerance) { 
		    nGood++; 
		} else { 
		    nBad++; 
		    //printf("polynomask->coeff[%d]=%.8f    poly->coeff[%d]=%.8f\n",i,polynomask->coeff[i],i,poly->coeff[i]); 
		}
		if (polynomask->coeffErr[i] != poly->coeffErr[i]) { 
		    //printf("polynomask->coeffErr[%d]=%.8f    poly->coeffErr[%d]=%.8f\n",i,polynomask->coeffErr[i],i,poly->coeffErr[i]); 
		}		
	    }
	    
	    if (nBad > 0) { 
		psError(PS_ERR_UNKNOWN,true,"%d incorrect polynomial evaluations for NORD=%d\n", nBad, NORD); 
		retval = retval + pow(2,NORD); 
	    } 
	    
	    printf("Mask Ord: %d   retval: %d\n", imask, retval);
	    psFree(poly);
	    psFree(polynomask);
	    psFree(mask);
	    psFree(f);
	    psFree(fErr);
	    psFree(x);	    
	    psFree(xx);
	    psFree(yy);
	    psFree(del);
	    psFree(polydel);

	}
    }
    return retval;
}





int testVectorClipFitPolynomial1D_01(void) { 
    int retval=0;
    int NORD, maxNORD=3;
    double Nsigma = 3.;
    int Niter= 3.;
    float tolerance = 1.0E-5;
    psPolynomialType type[] = {PS_POLYNOMIAL_ORD, PS_POLYNOMIAL_CHEB};
    
    for (int k = 0; k<1; k++) {  //loop over the 2 polynomial types  XXX: psVectorClipFirPolynomial1D does not
	                          //                                  work with Chebyshev now
	for (int j = 1; j <= maxNORD; j++) {  //loop over the polynomial orders
	    int nGood = 0, nBad = 0; 
	    NORD = j;
	    printf("Testing NORD=%d\n", NORD);
	    psPolynomial1D *poly = makePolynomial1D(NORD, type[k], -1);
	    psPolynomial1D *polycopy = makePolynomial1D(NORD, type[k], -1);
	    psStats *stats = psStatsAlloc(PS_STAT_CLIPPED_MEAN | PS_STAT_CLIPPED_STDEV);
	    stats->clipSigma = Nsigma;
	    stats->clipIter  = Niter;
	    psVector *f = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *fErr = psVectorAlloc(NPTS, PS_TYPE_F32);
	    psVector *x = psVectorAlloc(NPTS, PS_TYPE_F32);
	    
	    fillVectors(x, f, NORD, NPTS, type[k]);
	    psTime *now = psTimeGetNow(PS_TIME_UTC);
	    const psRandom *r = psRandomAlloc(PS_RANDOM_TAUS, now->sec);
	    for (int i = 0; i < NPTS; i++) {
		double errgen = psRandomGaussian(r);
		double amp = 1.0E-3;
		fErr->data.F32[i] = amp*errgen;
		//fErr->data.F32[i] = 0.;
	    }
	    psFree(now);
	    psFree(r);

	    //f->data.F32[NPTS-1] = X0 + 10.*X1*x->data.F32[NPTS-1];
	    //fErr->data.F32[NPTS-1] = 0.0000001;


	    psVectorClipFitPolynomial1D(poly, stats, NULL, 2, f, fErr, x);
	    for (int i = 0; i <= NORD; i++) { 
		if (poly->coeff[i] <= polycopy->coeff[i]+tolerance 
		      &&
		    poly->coeff[i] >= polycopy->coeff[i]-tolerance) { 
		    nGood++; 
		} else { 
		    nBad++; 
		    printf("polycopy->coeff[%d]=%.20f    poly->coeff[%d]=%.20f\n",i,polycopy->coeff[i],i,poly->coeff[i]); 
		}
		if (polycopy->coeffErr[i] != poly->coeffErr[i]) { 
		    printf("polycopy->coeffErr[%d]=%.20f    poly->coeffErr[%d]=%.20f\n",i,polycopy->coeffErr[i],i,poly->coeffErr[i]); 
		}		
	    } 

	    if (nBad > 0) { 
		psError(PS_ERR_UNKNOWN,true,"%d incorrect polynomial evaluations for NORD=%d\n", nBad, NORD); 
		retval = retval + pow(2,NORD); 
	    } 

	    printf("NORD: %d   retval: %d\n",NORD,retval);
	    psFree(poly);
	    psFree(polycopy);
	    psFree(stats);
	    psFree(f);
	    psFree(fErr);
	    psFree(x);	    
	}
    }
    return retval;
}







//test psStats

//test psGaussian

//test psMinimization (and psMinimizeLMChi2 and psMinimizeChi2Powell?)

//test psRandomUniform / psRandomGausian / psRandomPoisson

//test psSpline?
