function [A,B,C,D,E,F,ys,status]=Linearize(func,t0,x0,u0,d0,varargin)

%
%   Linearize - This function linearizes =f(t,x,u,d,data);
%
%    Written by: Atle C. Christiansen
%    Modified by: Truls Larsson change of syntax
%    Modified by: Marius S. Govatsmark included measurements (y=Cx+Du,ys)
%                                                       06.04.1999
%   Modified by Espen Storkaas 23.11.2006, changed syntax, removed global
%   variables, allow for extra data struct to be sent to f(t,...)
%
%     SYNTAX: [A,B,C,D,E,F,ys,status]=linearize('func',t0,Xinit,flag,Uinit,d);
%
%     INPUTS:
%     func  - nonlinear model; must be on the form: [xprime,y]=func(t,x,flag,u,d)
%     t     - time
%     x     - initial values for the states
%     u     - inputs
%     d     - disturbances
%     flag  - output flag sent to function
%     data  - Function data
%
%
%     OUTPUTS:
%     The outputs are the matrixes from the linear model:
%
%     xprime = A*x + B*u +E*d
%     y      = C*x + D*u +F*d
%
%     x - states
%     u - inputs
%     d - disturbances  (optional)
%     y - measurements
%
%     and the steadystate values of the measurements
%
%     ys- measurements - steadystate value
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%dxdt=Ax+Bu
%y=Cx+Du
if nargin==4

    %steadystate of measurements
    [fs,ys]=feval(func,t0,x0',u0);
    if abs(norm(fs))<1e-4
        ys=Y;
    else
        disp('You try to linearize around a point which is not in steadystate');
        disp(norm(fs));
        %       pause;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%
    n=max(size(x0));
    r=max(size(u0));
    delta=1e-6;

    for i=1:n
        xh=x0; xh(i)=xh(i)+delta;
        xl=x0; xl(i)=xl(i)-delta;
        [fh]=feval(func,t0,xh',u0);
        yh=Y;
        [fl]=feval(func,t0,xl',u0);
        yl=Y;
        A1(:,i)=(fh(:)-fl(:))/(2*delta);
        C1(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    for i=1:r
        uh=u0; uh(i)=uh(i)+delta;
        ul=u0; ul(i)=ul(i)-delta;
        [fh]=feval(func,t0,x0',uh);
        yh=Y;
        [fl]=feval(func,t0,x0',ul);
        yl=Y;
        B1(:,i)=(fh(:)-fl(:))/(2*delta);
        D1(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    delta=delta/2;

    for i=1:n
        xh=x0; xh(i)=xh(i)+delta;
        xl=x0; xl(i)=xl(i)-delta;
        [fh]=feval(func,t0,xh',u0);
        yh=Y;
        [fl]=feval(func,t0,xl',u0);
        yl=Y;
        A(:,i)=(fh(:)-fl(:))/(2*delta);
        C(:,i)=(yh(:)-yl(:))/(2*delta);
    end


    for i=1:r
        uh=u0; uh(i)=uh(i)+delta;
        ul=u0; ul(i)=ul(i)-delta;
        [fh]=feval(func,t0,x0',uh);
        yh=Y;
        [fl]=feval(func,t0,x0',ul);
        yl=Y;
        B(:,i)=(fh(:)-fl(:))/(2*delta);
        D(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    A=(4*A-A1)/3;
    B=(4*B-B1)/3;
    C=(4*C-C1)/3;
    D=(4*D-D1)/3;


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
elseif nargin>=5
    %dxdt=Ax+Bu+Ed
    %y=Cx+Du+Fd
    %disp('Hi')

    %steadystate of measurements
%     if nargin==5
%         varargin=[];
%     elseif nargin==6
%         flag=;
%     else
%         varargin(1)=flag;varargin(2)=data;
%     end
    [fs,ys]=feval(func,t0,x0,[u0;d0],varargin{:});
    norm(fs);
    if abs(norm(fs./x0))<1e-1
        status.status=1;
        status.errormsg='No errors encountered';
    else
        %disp('You try to linearize around a point which is not in steadystate');
        %pause;
        status.status=0;
        status.errormsg='Not steady state, cannot linearize';
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%
    n=max(size(x0));
    r=max(size(u0));
    q=length(d0);
    delta=1e-6;

    for i=1:n
        xh=x0; xh(i)=xh(i)+delta;
        xl=x0; xl(i)=xl(i)-delta;
        [fh,yh]=feval(func,t0,xh',[u0;d0],varargin{:});
        [fl,yl]=feval(func,t0,xl',[u0;d0],varargin{:});
        A1(:,i)=(fh(:)-fl(:))/(2*delta);
        C1(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    for i=1:r
        uh=u0; uh(i)=uh(i)+delta;
        ul=u0; ul(i)=ul(i)-delta;
        [fh,yh]=feval(func,t0,x0',[uh;d0],varargin{:});
        [fl,yl]=feval(func,t0,x0',[ul;d0],varargin{:});
        B1(:,i)=(fh(:)-fl(:))/(2*delta);
        D1(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    for i=1:q
        dh=d0; dh(i)=dh(i)+delta;
        dl=d0; dl(i)=dl(i)-delta;
        [fh,yh]=feval(func,t0,x0',[u0;dh],varargin{:});
        [fl,yl]=feval(func,t0,x0',[u0;dl],varargin{:});
        E1(:,i)=(fh(:)-fl(:))/(2*delta);
        F1(:,i)=(yh(:)-yl(:))/(2*delta);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    delta=delta/2;
    for i=1:n
        xh=x0; xh(i)=xh(i)+delta;
        xl=x0; xl(i)=xl(i)-delta;
        [fh,yh]=feval(func,t0,xh',[u0;d0],varargin{:});
        [fl,yl]=feval(func,t0,xl',[u0;d0],varargin{:});
        A(:,i)=(fh(:)-fl(:))/(2*delta);
        C(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    for i=1:r
        uh=u0; uh(i)=uh(i)+delta;
        ul=u0; ul(i)=ul(i)-delta;
        [fh,yh]=feval(func,t0,x0',[uh;d0],varargin{:});
        [fl,yl]=feval(func,t0,x0',[ul;d0],varargin{:});
        B(:,i)=(fh(:)-fl(:))/(2*delta);
        D(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    for i=1:q
        dh=d0; dh(i)=dh(i)+delta;
        dl=d0; dl(i)=dl(i)-delta;
        [fh,yh]=feval(func,t0,x0',[u0;dh],varargin{:});
        [fl,yl]=feval(func,t0,x0',[u0;dl],varargin{:});
        E(:,i)=(fh(:)-fl(:))/(2*delta);
        F(:,i)=(yh(:)-yl(:))/(2*delta);
    end

    A=(4*A-A1)/3;
    B=(4*B-B1)/3;
    C=(4*C-C1)/3;
    D=(4*D-D1)/3;
    E=(4*E-E1)/3;
    F=(4*F-F1)/3;
else
    disp('Usage: [A,B,C,D,E,F]=linearize(func,t0,x0,u0,d0);')
    return
end
