#include <iostream>
#include<cmath>
#include "stdafx.h"
#include <stdlib.h>
#include <stdio.h>
#include "interpolation.h"

using namespace alglib;
using namespace std;

//I am scanning a cylindrical object with a pointlaser and I get points that can be fitted with a circle function:
//y=(r^2-(x-xm)^2)^(1/2)+ym  upper half of a circle
//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],  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 = sqrt(pow(c[2],2)-pow(x[0]-c[0],2))+c[1]
    // where x is a position on X-axis and c is adjustable parameter
    func=sqrt(c[2]*c[2]-pow(x[0]-c[0],2))+c[1] ;
}

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=sqrt(pow(c[2],2)-(x[0]-c[0])*(x[0]-c[0]))+c[1] 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=sqrt(c[2]*c[2]-pow(x[0]-c[0],2))+c[1] ;
    grad[0] = (c[0]-x[0])/(sqrt(c[2]*c[2]-pow(x[0]-c[0],2)));
    grad[1] = 1.0;
    grad[2] = c[2]/(sqrt(c[2]*c[2]-pow(x[0]-c[0],2))) ;
}

int main(int argc, char **argv)
{
    //
    // In this example we demonstrate exponential fitting
    // f(c,x)=sqrt(pow(c[2],2)-pow(x[0],2)+2*x[0]*c[0]+pow(c[0],2))+c[1]
    // using function value and gradient (with respect to c).
    //
    real_2d_array x = "[[5.0],[5.5],[6.0],[6.4],[7.2],[7.6],[8.0],[8.4],[8.8],[9.2],[9.6],[10.0],[10.4],[10.8],[11.2],[11.6],[12.0],[12.4],[12.8],[13.6],[14.0],[14.5],[15]]";
    real_1d_array y = "[10.0, 12.17744, 13.004, 13.46787, 14.14646, 14.38234, 14.58297, 14.73408, 14.85586, 14.93958, 14.98297, 15.003, 14.997997, 14.93658, 14.85186, 14.73408, 14.58657, 14.38634, 14.14146, 13.47087, 12.998, 12.18144, 10.0]";
    real_1d_array c = "[8,8,4]";
    real_1d_array bndl = "[9.0,9.0,4.0]";
    real_1d_array bndu = "[11.0,11.0,6.0]";
    real_1d_array s = "[10, 10, 1]";
    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] 
    
    
  
    system("pause");
    return 0;
        
    
}  
  /*
  
************************************************************************
lsfitresults function
  
Nonlinear least squares fitting results.

Called after return from LSFitFit().

INPUT PARAMETERS:
    State   -   algorithm state

OUTPUT PARAMETERS:
    Info    -   completetion code:
                    *  1    relative function improvement is no more than
                            EpsF.
                    *  2    relative step is no more than EpsX.
                    *  4    gradient norm is no more than EpsG
                    *  5    MaxIts steps was taken
                    *  7    stopping conditions are too stringent,
                            further improvement is impossible
    C       -   array[0..K-1], solution
    Rep     -   optimization report. Following fields are set:
                * Rep.TerminationType completetion code:
                * RMSError          rms error on the (X,Y).
                * AvgError          average error on the (X,Y).
                * AvgRelError       average relative error on the non-zero Y
                * MaxError          maximum error
                                    NON-WEIGHTED ERRORS ARE CALCULATED
                * WRMSError         weighted rms error on the (X,Y).
                
                
                
*************************************************************************
lsfitsetcond function
/*************************************************************************
Stopping conditions for nonlinear least squares fitting.

INPUT PARAMETERS:
    State   -   structure which stores algorithm state
    EpsF    -   stopping criterion. Algorithm stops if
                |F(k+1)-F(k)| <= EpsF*max{|F(k)|, |F(k+1)|, 1}
    EpsX    -   >=0
                The subroutine finishes its work if  on  k+1-th  iteration
                the condition |v|<=EpsX is fulfilled, where:
                * |.| means Euclidian norm
                * v - scaled step vector, v[i]=dx[i]/s[i]
                * dx - ste pvector, dx=X(k+1)-X(k)
                * s - scaling coefficients set by LSFitSetScale()
    MaxIts  -   maximum number of iterations. If MaxIts=0, the  number  of
                iterations   is    unlimited.   Only   Levenberg-Marquardt
                iterations  are  counted  (L-BFGS/CG  iterations  are  NOT
                counted because their cost is very low compared to that of
                LM).

NOTE

Passing EpsF=0, EpsX=0 and MaxIts=0 (simultaneously) will lead to automatic
stopping criterion selection (according to the scheme used by MINLM unit).


  -- ALGLIB --
     Copyright 17.08.2009 by Bochkanov Sergey
*************************************************************************
void alglib::lsfitsetcond(
    lsfitstate state,
    double epsf,
    double epsx,
    ae_int_t maxits);
*************************************************************************