#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include "Alglib/lsfit.h"

int main(int argc, char **argv) {
    int m;
    int n;
    int k;
    ap::real_1d_array y;
    ap::real_2d_array x;
    ap::real_1d_array c;
    lsfitreport rep;
    lsfitstate state;
    int info;
    double epsf;
    double epsx;
    int maxits;
    int i;
    int j;
    double a;
    double b;

    printf("Fitting Pointcloud from Laserscanner to ax^3+bx^2+cx+d\n");

    //
    // * without Hessian (gradient only)
    // * using alpha=1 as initial value
    // * using 1000 uniformly distributed points to fit to
    //
    // Notes:
    // * N - number of points
    // * M - dimension of space where points reside
    // * K - number of parameters being fitted
    //
    n = 1000;
    m = 1;
    k = 4;

    //
    // Prepare task matrix
    //
    y.setlength(n);
    x.setlength(n, m);
    c.setlength(k);
    for (i = 0; i <= n - 1; i++) {
        x(i, 0) = i;
        y(i) = 2 * i * i * i + i * i + 5 * i + 4;
    }
    c(0) = 1.0;
    c(1) = 1.0;
    c(2) = 3.0;
    c(3) = 4.0;
    epsf = 0.0;
    epsx = 0.0001;
    maxits = 0;

    //
    // Solve
    //
    lsfitnonlinearfg(x, y, c, n, m, k, true, state);
    lsfitnonlinearsetcond(state, epsf, epsx, maxits);
    while (lsfitnonlineariteration(state)) {

        //
        // F(x) =
        //
        state.f = state.c(0) * ap::sqr(state.x(0)) * state.x(0) + state.c(1)
                * ap::sqr(state.x(1)) + state.c(2) * state.x(2) + state.c(3);

        if (state.needfg ) {

            //
            // F(x)      = Exp(-alpha*x^2)
            // dF/dAlpha = (-x^2)*Exp(-alpha*x^2)
            //
//            state.f = state.c(0) * ap::sqr(state.x(0)) * state.x(0)
//                    + state.c(1) * ap::sqr(state.x(0)) + state.c(2)
//                    * state.x(0) + state.c(3);
            state.g(0) = 3 * state.c(0) * ap::sqr(state.x(0)) + 2 * state.c(1)
                    * state.x(0) + state.c(2);
            state.g(1)=state.c(1)*state.x(0);    //what should i do here?
            state.g(2)=state.c(2);
            state.g(3)=1.00;
        }

    }
    lsfitnonlinearresults(state, info, c, rep);
    printf("a:   %0.3lf\n", double(c(0)));
    printf("b:   %0.3lf\n", double(c(1)));
    printf("c:   %0.3lf\n", double(c(2)));
    printf("d:   %0.3lf\n", double(c(3)));
    printf("rms.err: %0.3lf\n", double(rep.rmserror));
    printf("max.err: %0.3lf\n", double(rep.maxerror));
    printf("Termination type: %0ld\n", long(info));
    printf("\n\n");
    return 0;
}

state.g(0) = ap::sqr(state.x(0)) * state.x(0);
state.g(1) = ap::sqr(state.x(0))
state.g(2) =  state.x(0);
state.g(3) = 1