// Scilab ( http://www.scilab.org/ ) - This file is part of Scilab
// Copyright (C) INRIA
// Copyright (C) Samuel GOUGEON - 2010 : data weights added (option)

// This file must be used under the terms of the CeCILL.
// This source file is licensed as described in the file COPYING, which
// you should have received as part of this distribution.  The terms
// are also available at    
// http://www.cecill.info/licences/Licence_CeCILL_V2-en.txt

// Support for weigthed data added: 2010-10-28 (S.G.)
//     => full backward compatibility

function [p,err] = datafit(imp,G,varargin)
//
//         [p,err]=datafit([imp,] G [,DG],Z [,zW] [,W],...)
//
//         Function used for fitting data to a model.
// For a given function G(p,z), this function finds the best vector 
// of parameters p for approximating G(p,z_i)=0 for a set of measurement
// vectors z_i weighted by zw_i. Vector p is found by minimizing
//    G(p,z_1)'WG(p,z_1)+G(p,z_2)'WG(p,z_2)+...+G(p,z_n)'WG(p,z_n)
//
//      G: Scilab function (e=G(p,z [,zw])
//         e: nex1, p: npx1, z: nXi x 1) zw:1
//     p0: initial guess (size npx1)
//      Z: matrix [z_1,z_2,...z_n] where z_i (nXx1) is the ith measurement
//     wZ: Data weights (optionnal). Row of length size(Z,2)
//      W: Criteria weights (optional). Matrix of size ne x ne
//     DG: partial of G wrt p (optional; S=DG(p,z), S: nexnp)
//
//                     Examples
//
//deff('y=FF(x)','y=a*(x-b)+c*x.*x')
//X=[];Y=[];
//a=34;b=12;c=14;for x=0:.1:3, Y=[Y,FF(x)+100*(rand()-.5)];X=[X,x];end
//Z=[Y;X];
//deff('e=G(p,z)','a=p(1),b=p(2),c=p(3),y=z(1),x=z(2),e=y-FF(x)')
//[p,err]=datafit(G,Z,[3;5;10])
//xset('window',0)
//clf();
//plot2d(X',Y',-1) 
//plot2d(X',FF(X)',5,'002')
//a=p(1),b=p(2),c=p(3);plot2d(X',FF(X)',12,'002')
//
//a=34;b=12;c=14;
//deff('s=DG(p,z)','y=z(1),x=z(2),s=-[x-p(2),-p(1),x*x]')
//[p,err]=datafit(G,DG,Z,[3;5;10])
//xset('window',1)
//clf(); 
//plot2d(X',Y',-1) 
//plot2d(X',FF(X)',5,'002')
//a=p(1),b=p(2),c=p(3);plot2d(X',FF(X)',12,'002')
//

[lhs,rhs] = argn(0)

_format = format()
format("v",23)        // for string()

// Processing the 2 first argin imp and G:
if type(imp)<>1 then  // 1st argin imp is G  (no imp directive)
    varargin(0) = G     // so the argin G is the first arg after the true G
    G = imp             // G gets its true value
    imp = 0             // imp gets its true value
end

if type(G)==15 then  // G = list(G, params)
    Gparams = G;
    Gparams(1) = null();
    G = G(1)
else
    Gparams = list()    // no explicit parameter
end

// Next argins: optional dG (Jacobian), or mandatory points Z
DG = varargin(1)
if type(DG)==10 | type(DG)==11 | type(DG)==13 then   // is a function
    GR = %t  //Jacobian provided
    varargin(1) = null()
elseif type(DG)==15 then    // list(..) provided (Jacobian with parameters)
    format(_format(2),_format(1))
    error(msprintf(gettext('%s: Jacobian cannot be a list, parameters must be set in G.'),'datafit'));
else
    GR = %f  // Jacobian not provided
end

Z = varargin(1);        // Data points to fit
varargin(1) = null()
if type(Z)<>1 then 
    format(_format(2),_format(1))
    error(msprintf(gettext('%s: Wrong measurement matrix.'),'datafit'));
end
[mz,nz] = size(Z);    // nz: Number of points

// Data weights: Must be a line vector of length nz (W can _neither_ be so long)
if size(varargin(1),2)==nz
    wZ = varargin(1)
    varargin(1) = null()
else
    wZ = 0    // = flag: wZ won't be provided to G()
end

if size(varargin(1),2)==1 then // p0 ou 'b'
    W = 1
else    // W
    W = varargin(1);
    varargin(1) = null()
    if size(W,1)~=size(W,2) then 
        format(_format(2),_format(1))
        error(msprintf(gettext('%s: Matrix of criteria Weights must be square.'),'datafit'));
    end
end

if type(varargin(1))==1 then // p0
    p0 = varargin(1)
else                // "b",lowerBound,upperBound forthcoming series => skipped
    p0 = varargin(4)
end
if size(p0,2)>1 then
    if size(p0,1)==1
        p0 = p0'        // parameters row => column vector
    else
        format(_format(2),_format(1))
        error(msprintf(gettext('%s: Initial guess must be a column vector.'),'datafit'));
    end
end

np = size(p0,'*');    // Number of parameters

if type(G)==10 then  //form function to call hard coded external
    if size(Gparams)==0 then 
        format(_format(2),_format(1))
        error(msprintf(gettext('%s: With hard coded function, user must give output size of G.'),'datafit'));
    end
    m = Gparams(1); Gparams(1) = null()

    // foo(m,np,p,mz,nz,Z[,wZ],pars,f)
    tmp = 'f=call('''+G+''','+ ..
    'm,1,''i'', np,2,''i'', p,3,''d'', mz,4,''i'', nz,5,''i'','+ ..
    'Z,6,''d'','
    if wZ==0
        tmp = tmp + 'pars,7,''out'',['+string(m)+',1],8,''d'')'
        deff('f = G(p,Z)',tmp)
    else 
        tmp = tmp + "wZ,7,''d'', pars,8,''out'',["+string(m)+',1],9,''d'')'
        deff('f = G(p,Z,wZ)',tmp)
    end

    pars=[];
    for k=1:size(Gparams)
        p = Gparams(k)
        pars = [pars;p(:)]
    end
    Gparams = list()
end

if type(DG)==10 then //form function to call hard coded external
    // dfoo(m,np,p,mz,nz,Z[,wZ],pars,f)
    tmp = 'f=call('''+G+''','+ ..
    'm,1,''i'', np,2,''i'', p,3,''d'', mz,4,''i'', nz,5,''i'','+ ..
    'Z,6,''d'','
    if wZ==0
        tmp = tmp + 'pars,7,''out'',['+string(m)+','+string(np)+'],8,''d'')'
        deff('f = DG(p,Z)',tmp)
    else 
        tmp = tmp + "wZ,7,''d'', pars,8,''out'',["+string(m)+','+string(np)+'],9,''d'')'
        deff('f = DG(p,Z,wZ)',tmp)
    end
end

// form square cost gradient function DGG
if wZ~=0, wZt = ",wZ(i)", else wZt="", end

if Gparams==list() then
    GP   = 'G(p,Z(:,i)'+wZt+')'
    GPPV = 'G(p+v,Z(:,i)'+wZt+')'
    DGP  = 'DG(p,Z(:,i)'+wZt+')'
else
    GP   = 'G(p,Z(:,i)'+wZt+',Gparams(:))'
    GPPV = 'G(p+v,Z(:,i)'+wZt+',Gparams(:))'
    DGP  = 'DG(p,Z(:,i)'+wZt+',Gparams(:))'  
end

if ~GR then // finite difference
    DGG=['g=0*p';
    'pa=sqrt(%eps)*(1+1d-3*abs(p))'
    'f=0;'
    'for i=1:'+string(nz)
    '  g1='+GP
    '  f=f+g1''*W*g1'
    'end'
    'for j=1:'+string(np)
    '  v=0*p;v(j)=pa(j),'
    '  e=0;'
    '  for i=1:'+string(nz)
    '    g1='+GPPV
    '    e=e+g1''*W*g1'
    '  end'
    '  g(j)=e-f;'
    'end;'
    'g=g./pa;']
else // using Jacobian of G
    DGG='g=0;for i=1:nz,g=g+2*'+DGP+'''*W*'+GP+',end'
end

// form cost function for optim
deff('[f,g,ind] = costf(p,ind)',[
    'if ind==2 | ind==4 then '
    '   f = 0;'
    '   for i=1:'+string(nz)
    '     g1 = '+GP
    '     f = f+g1''*W*g1'
    '   end'
    'else '
    '   f = 0;'
    'end';
    'if ind==3 | ind==4 then' 
    DGG
    'else' 
    ' g = 0*p;'
    'end'])

format(_format(2),_format(1))

[err,p] = optim(costf,varargin(:),imp=imp)
 
endfunction