% eda11_05
% non-linear least squares fit of a sinusoid
% of unknown frequency, gradient method

% note: Bill Menke patched and error in this code 09/20/2020
clear all;

% load Black Rock Forest temperature data
D=load('brf_temp.txt');
t=D(:,1);
dobs=D(:,2);
Ns=size(D);

% clean the data by deleting outliers and zero (= no data) values
k = find( (dobs>-35) & (dobs<40) & (dobs~=0.0) );
t = D(k,1);
dobs = D(k,2);
N = length(dobs);

% some statistics
dmean = mean(dobs);
amp = 0.5*(max(dobs)-min(dobs));

% Annual period, frequency, angular frequency
Ta = 365;
fa = 1/Ta;
wa = 2*pi*fa;

% nonlinear theory and its derivative
% d = m1*dmean + m2*amp*sin(m4 wa t) + m3*amp*cos(m4 wa t)
% dd/dm1 =  dmean
% dd/dm2 =  amp*sin(m4 wa t)
% dd/dm3 =  amp*cos(m4 wa t)
% dd/dm4 =  amp*m2*wa*t*cos(m4 wa t) - amp*m3*wa*t*sin(m4 wa t)

% inital guesses
M=4;
m = zeros(M,1);
m(1) =  1;          % average level is mean
m(2) =  0;          % phase with max in summer
m(3) = -0.75;        % so only -cos() term
m(4) =  1;          % fraction of annual frequency wa

Lm = sqrt(m'*m); % length of model

s = amp*sin(m(4)*wa*t);
c = amp*cos(m(4)*wa*t);
dpre = dmean*m(1) + m(2)*s + m(3)*c;

figure(1);
clf;
set(gca,'LineWidth',2);
hold on;
axis([0,5000,-30,50]);
plot(t,dobs,'k.','LineWidth',2);
hold on;
% plot(t,dpre,'-','LineWidth',2,'Color',[0.7,0.7,0.7]);
xlabel('time (days)');
ylabel('temperature (deg C)');

% error and its gradient at the trial solution
mgo = m;
s = amp*sin(mgo(4)*wa*t);
c = amp*cos(mgo(4)*wa*t);
dprego = mgo(1)*dmean + mgo(2)*s + mgo(3)*c;
ego = dobs - dprego;
Ego = ego'*ego;
Estart = Ego;
Go = [ amp*ones(N,1), s, c, mgo(2)*wa*t.*c-mgo(3)*wa*t.*s ];
dEdmo = -2*Go'*ego;

alpha = 1;
c1 = 0.0001;
tau = 0.5;

Niter = 4000;
for k = [1:Niter]
    
    % downslope unit vector
    v = -dEdmo / sqrt(dEdmo'*dEdmo);
    
    % backstep
    for kk=[1:100]
        mg = mgo+alpha*v;
        s = amp*sin(mg(4)*wa*t);
        c = amp*cos(mg(4)*wa*t);
        dpreg = dmean*mg(1) + mg(2)*s + mg(3)*c;
        eg = dobs - dpreg;
        Eg = eg'*eg;
        G = [ dmean*ones(N,1), s, c, mg(2)*wa*t.*c-mg(3)*wa*t.*s ];
        dEdm = -2*G'*eg;
        
        % check that the error really went down
        if( Eg<=Ego )
            break;
        end
        % another possibility is to use Armijo's rule
        % for the tesr
        % if( (Eg<=(Ego + c1*alpha*v'*dEdmo)) )
        
        % if the error went up, decrease the step size
        alpha = tau*alpha;
    end

    % change in solution
    Dmg = sqrt( (mg-mgo)'*(mg-mgo) );
    
    % update
    mgo=mg;
    dprego = dpreg;
    ego = eg;
    Ego = Eg;
    Go = G;
    dEdmo = dEdm;

    if( (Dmg/Lm) < 1.0e-4 )
        break;
    end
end

m = mg;
dpre = dpreg;

plot(t,dpre,'-','LineWidth',2,'Color',[0.9,0.9,0.9]);

ffinal = wa*m(4)/(2*pi);
Tfinal = (2*pi)/(wa*m(4));

fprintf('Iterations: %d\n', k );
fprintf('mean: %f amp: %f %f  Dm: %f\n', m(1)*dmean, m(2)*amp, m(3)*amp, Dmg );
fprintf('Estimated period %.3f error ratio %.3f\n', Tfinal, Eg/Estart );

