#include <iostream>
#include<cmath>
#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include "interpolation.h"
using namespace alglib;
using namespace std;


//I'm using triangulation laser, so I move this Laser along the x-axis and measure the distance to a cylinder. 
//As result I get a profile  of the cylinder, that means I get points that can be fitted by a circle function.
//func=pow(x[0]-c[0],2)+pow(x[1]-c[1],2)-c[2]*c[2] 
//So I have to determine the three parameters r, xm, ym in the way, that the curve y=(r^2-(x-xm)^2)^(1/2)-ym fit the points best.
// Mathematical background: min(F(c) = ((f(c,x[0])-y[0]))^2 + ... + ((f(c,x[n-1])-y[n-1]))^2) 
//x=x[0], y=x[1], xm=c[0],  ym=c[1],  r=c[2]

void function_cx_1_func(const real_1d_array &c, const real_1d_array &x, double &func, void *ptr) 
{
   // this callback calculates func=pow(x[0]-c[0],2)+pow(x[1]-c[1],2)-c[2]*c[2] ;
    // where x is a position on X-axis and c is adjustable parameter
     func=pow(x[0]-c[0],2)+pow(x[1]-c[1],2)-c[2]*c[2] ;
}

void function_cx_1_grad(const real_1d_array &c, const real_1d_array &x, double &func, real_1d_array &grad, void *ptr) 
{
    // this callback calculates func=pow(x[0]-c[0],2)+pow(x[1]-c[1],2)-c[2]*c[2] ; and gradient G={df/dc[i]}
    // where x is a position on X-axis and c is adjustable parameter.
    // IMPORTANT: gradient is calculated with respect to C, not to X
    func=pow(x[0]-c[0],2)+pow(x[1]-c[1],2)-c[2]*c[2] ;
    grad[0] = -2*(x[0]-c[0]);
    grad[1] = -2*(x[1]-c[1]);
    grad[2] = -2*c[2];
}

int main(int argc, char **argv)
{
    //
    // In this example we demonstrate exponential fitting
    // func=pow(x[0]-c[0],2)+pow(x[1]-c[1],2)-c[2]*c[2] ;
    // using function value and gradient (with respect to c).
    //
    // in real_2d_array x habe ich jeweils die Messpunkte (x_i,y_i) eingefügt
    // The values in real_1d_array y I calculated by inserting measured points  (x_i,y_i) in (x_i-x_m)^2+(y_i-y_m)^2-r^2
    // real_1d_array bndl = "[5,5,3]" real_1d_array bndu = "[15,15,10]" Diese beide Befehle bndl, bndu definieren Grenzen für die c-werte, das erleichtert dem Algorithmus die Arbeit
    //real_1d_array s = "[5, 5, 3]" Dieser Befehl scaliert den vektor real_1d_array c. Wenn die  Werte c[0],c[1]... sich stark
    //von einander unterscheiden, also z.B. real_1d_array c = "[55,55,3]"; dann braucht der Algorithmus für die ersten beide werte länger
    //also mehr Schritte als füer den letzten. Damit aber die Genauigkeit von c1 und c2 nicht leidet, muss man real_1d_array s = "[50, 50, 1]" 
    //wählen
    
    
    real_2d_array x = "[[5.5,12.17744],[6.0,13.01],[6.4,13.46787],[7.2,14.13646],[7.6,14.3924],[8.4,14.73480],[8.8,14.85586],[9.2,14.93958],[9.6,14.98297],[10.0,15.003],[10.4,14.997997],[10.8,14.93658],[11.2,14.85186],[11.6,14.73408],[12.0,14.5965],[12.4,14.390],[12.8,14.1314],[13.6,13.46087],[14,12.988],[14.5,12.18144]]";
    real_1d_array y = "[-8.75504e-3, 60.1e-3, -83.6192e-3, 49.6986e-3, 53.1777e-3, -21.6689e-3,19.3763e-3, 39.4505e-3, -10.0099e-3, 30.009e-3, 0.13990, 9.82209e-3, -19.45454e-3, -28.48655e-3, 0.12781, 32.1e-3, -91.5340e-3,-62.3788e-3, -71.856e-3, 8.68047e-3]";
    real_1d_array c = "[5,5,3]";
    real_1d_array bndl = "[5,5,3]"; 
    real_1d_array bndu = "[15,15,10]";
    real_1d_array s = "[5, 5, 3]"; 
    double epsf =0 ;
    double epsx = 0.0000001;
    ae_int_t maxits =0;
    ae_int_t info;
    lsfitstate state;
    lsfitreport rep;


  
    // Fitting without weights
    //
    lsfitcreatefg(x, y, c, true, state);
    lsfitsetcond(state, epsf, epsx, maxits);
    lsfitsetbc(state, bndl, bndu);
    lsfitsetscale(state, s);
    alglib::lsfitfit(state, function_cx_1_func, function_cx_1_grad);
    lsfitresults(state, info, c, rep);
    printf("%d\n", int(info)); // EXPECTED: 2
    printf("%s\n", c.tostring(4).c_str()); // EXPECTED: [10,10,5] 
    
    
    

 /*  // Fitting with weights
    // (you can change weights and see how it changes result)
    //
    real_1d_array w = "[3,3,3,3,3,3,3,3,3,3,3,3,3]";
    lsfitcreatewfg(x, y, w, c, false, state);
    lsfitsetcond(state, epsf, epsx, maxits);
    alglib::lsfitfit(state, function_cx_1_func, function_cx_1_grad);
    lsfitresults(state, info, c, rep);
    printf("%d\n", int(info)); // EXPECTED: 2
    printf("%s\n", c.tostring(1).c_str()); // EXPECTED: [1.5]  */

    system("pause");
    return 0;
        
    
}