% Perform Nonlinear sequential function specification method
% using a FEM based forward solver
%   -geometry is the insulated finite plate
%   -uses function Tdot_func.m
% dt is the time step in the data
%  x is a scalar measurement location
%  r is a scalar number of future times for the estimation
%  Y is the vector of measurements beginning at time=dt
% function [qhat,Tsur] = NonlinearSFSM( dt, x, r, Y )
function [qhat,Tsur, That] = NonlinearSFSM( dt, x, r, Y )
if( nargin == 0 )
   dt = 0.06;
   x = 0.5;
   r = 3;
   Y = [ 0	0	0	0	0	0.0004	0.0035	0.0103	0.0212	0.0365	0.0569 ...
        0.0829	0.1152	0.1548	0.2025	0.256	0.3063	0.3526	0.3949	... 
        0.4325	0.4645	0.4903	0.5095	0.5216	0.5267	0.5273	0.5272	... 
        0.5272	0.5272	0.5272	0.5272	0.5272	0.5272	0.5272	0.5272 ]';

end

L = 1.0;            % we use a unit depth domain
n_nodes = 31;
tol = 1.e-4;        % relative error tolerance for convergence 
dx = L / (n_nodes - 1);
xvec = [ 0: dx : L ];


T = zeros(n_nodes,1);
q_M = 1.0;         % initial guess

N = size(Y,1);   % the total number of observations
qhat = zeros( N, 1 );
Tsur = zeros( N, 1 );
That = zeros( N, 1 );
if( N == 1 )
   error('Y must be a column vector');
end
time = [dt:dt:N*dt]';
for M = 1 : N-r       
    Tsave = T;    % save the initial conditions
    relerr = 1.0;
    it = 0;
    % begin the iterative loop
    if( M == 1)
        tspan = [ 0:dt:time(M+r-1) ];
    else
        tspan = [ time(M-1):dt:time(M+r-1) ];
    end
    while ( relerr > tol )
        T = Tsave;       % always start from the beginning
        TX  = [ T; zeros( n_nodes, 1 ) ];  % solve T and X together
        % integrate 'r' times ahead with current q_M
        [ t  TX ] = ode15s( @TdotXdot_func, tspan, TX, [], L, q_M );
        T = TX( 2:r+1, 1:n_nodes );
        Xq = TX( 2:r+1 , n_nodes+1:2*n_nodes );
        % need the T_x and Xq_x at the x location
        T_trans = interp1( xvec, T', x );
        Xq_trans = interp1( xvec, Xq', x );
        T = T_trans';     % values at the times for location 'x'
        Xq = Xq_trans';
    
        % now the method is about the same as Beck's
    
        denom = Xq(1:r)'*Xq(1:r);
        K = Xq(1:r)/denom;
        dq_M = ( Y(M:(M+r-1)) - T(1:r) )'*K;
        q_M = q_M + dq_M;             % go ahead and add the change
        relerr = min( abs(dq_M/q_M), abs(q_M) );      % for convergence check
    end
    % now converged - integrate one time step     
    TX = [ Tsave; zeros(n_nodes,1 ) ];
    [t, TX] = ode15s( @TdotXdot_func, tspan(1:2), TX, [], L, q_M );
    T = TX( size(TX,1), 1:n_nodes )';   % get the last row 
    qhat(M) = q_M;
    Tsur(M) = T(1);
    That(M) = interp1( xvec, T, x );  % value at the sensor location
%    fprintf(1,' time = %f   q_M = %f h_M = %f\n',time(M), qhat(M), qhat(M)/(1-T(M)) );
end

end



   


