% Perform Nonlinear sequential function specification method
%   -geometry is the insulated finite plate
%   -uses function TdotXdot_func_h.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 [hhat,Tsur,That] = NonlinearSFSM_h( dt, x, r, Y, T_inf )
if( nargin == 0 )
   dt = 0.06;
   x = 0.5;
   r = 2;
   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 ]';
    T_inf = 1.0;

end

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


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

N = size(Y,1);   % the total number of observations
hhat = zeros( N, 1 );
Tsur = zeros(N,1);
That = zeros(N,1);
if( N == 1 )
   error('Y must be a column vector');
end
time = [tfirst:dt:(N-1)*dt+tfirst]'

for M = 1 : N-r
    Tsave = T;    % save the initial conditions
    relerr = 1.0;
    it = 0;
    % begin the iterative loop
    if( M == 1)
        tspan = [ time(1)-dt: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
%        tic
        [ t  TX ] = ode15s( @TdotXdot_func_h, tspan, TX, [], L, h_M, T_inf );
%        toc
        if( r == 1 )
            nlast = length(t);
            T = TX( nlast, 1:n_nodes );
            Xh = TX( nlast , n_nodes+1:2*n_nodes );    
        else
            T = TX( 2:r+1, 1:n_nodes );
            Xh = TX( 2:r+1 , n_nodes+1:2*n_nodes );
        end
        % need the T_x and Xh_x at the x location
        T_trans = interp1( xvec, T', x );
        Xh_trans = interp1( xvec, Xh', x );
        T = T_trans';     % values at the times for location 'x'
        Xh = Xh_trans';
    
        % now the method is about the same as Beck's
    
        denom = Xh(1:r)'*Xh(1:r);
        K = Xh(1:r)/denom;
        dh_M = ( Y(M:(M+r-1)) - T(1:r) )'*K;
        h_M = h_M + damp*dh_M;             % go ahead and add the change
        relerr = min( abs(dh_M/h_M), abs(h_M) );      % for convergence check
    end
    % now converged - integrate one time step     
    TX = [ Tsave; zeros(n_nodes,1 ) ];
    [t, TX] = ode15s( @TdotXdot_func_h, tspan(1:2), TX, [], L, h_M, T_inf );
    T = TX( size(TX,1), 1:n_nodes )';   % get the last row 
    hhat(M) = h_M;
    Tsur(M) = T(1);
    That(M) = interp1( xvec, T, x );  % value at the sensor location

    fprintf(1,' time = %f   h_M = %f \n',time(M), hhat(M) );
end
if( nargin == 0 )
    load hexact_triangle_01   % has dt=0.01 intervals and corresponding h's
    figure(97); plot(t01, hexact_tri, 'k--', time(2:N-r)-dt/2, hhat(2:N-r), 'ro', 'linewidth', 1.0 ); figure(97)
    xlabel('time'); ylabel('Heat Transfer Coefficient');
    figure(98); plot( time(1:N-r), Y(1:N-r), time(1:N-r), That(1:N-r), 'ro'); figure(98)
    xlabel('time'); ylabel('Temperature');
    figure(99); plot( time(1:N-r), Y(1:N-r)-That(1:N-r), 'ro'); figure(99)
    xlabel('time'); ylabel('Residuals');
    
%    fig = figure(97);
%    print(fig,fullfile(pwd, 'Figure3_16.tif'),'-dtiff','-r600')

end
end


   


