#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 so 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 I inserted the values of the measurement. 
    real_2d_array x = "[[5.0,10.0],[5.5,12.17744],[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],[14.5,12.18144],[15,10.0]]";
    real_1d_array y = "[0.0,-8.75504e-3,-21.6689e-3,19.3763e-3, 39.4505e-3, -10.0099e-3, 30.009e-3, 0.13990, 9.82209e-3, -19.45454e-3, -2848655e-12, 8.68047e-3,0.0 ]";
    // 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 c = "[9,9,4.5]";
    real_1d_array bndl = "[5,5,3]";
    real_1d_array bndu = "[15,15,10]";
    real_1d_array s = "[10, 10, 5]";
    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;
        
    
}