function TestP3

clc
disp('          gamma        a           b         mu        r   ExitFlag  f-evals')
disp('-----------------------------------------------------------------------------')


% Example 1 No Sine
gamma = -1; a = 2; b = 0; mu = 4; noise = .1;
t = linspace(0,20,100)';
y = MakeProb(gamma,a,b,mu,t,noise);
fprintf('Exact: %8.4f    %8.4f    %8.4f  %8.4f  \n',gamma,a,b,mu)
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('CVL    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n',r,EXITFLAG,OUTPUT.funcCount);
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfit(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('You    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n\n',r,EXITFLAG,OUTPUT.funcCount);

% Example 2 No Cosine
gamma = -1; a = 0; b = 1; mu = 4; noise = .0;
t = linspace(0,20,100)';
y = MakeProb(gamma,a,b,mu,t,noise);
fprintf('Exact: %8.4f    %8.4f    %8.4f  %8.4f  \n',gamma,a,b,mu)
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('CVL    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n',r,EXITFLAG,OUTPUT.funcCount);
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfit(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('You    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n\n',r,EXITFLAG,OUTPUT.funcCount);

% Example 3  Easy Mix
gamma = -1; a = 2; b = -3; mu = 4; noise = .1;
t = linspace(0,5,100)';
y = MakeProb(gamma,a,b,mu,t,noise);
fprintf('Exact: %8.4f    %8.4f    %8.4f  %8.4f  \n',gamma,a,b,mu)
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('CVL    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n',r,EXITFLAG,OUTPUT.funcCount);
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfit(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('You    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n\n',r,EXITFLAG,OUTPUT.funcCount);


% Example 4  Slow decay, rapid oscillation
gamma = -.1; a = 2; b = -3; mu = 40; noise = .1;
t = linspace(0,50,100)';
y = MakeProb(gamma,a,b,mu,t,noise);
fprintf('Exact: %8.4f    %8.4f    %8.4f  %8.4f  \n',gamma,a,b,mu)
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('CVL    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n',r,EXITFLAG,OUTPUT.funcCount);
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfit(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('You    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n\n',r,EXITFLAG,OUTPUT.funcCount);

% Example 5 Rapid decay and oscillation
gamma = -5; a = 2; b = 2; mu = 40; noise = .1;
t = linspace(0,1,1000)';
y = MakeProb(gamma,a,b,mu,t,noise);
fprintf('Exact: %8.4f    %8.4f    %8.4f  %8.4f  \n',gamma,a,b,mu)
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('CVL    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n',r,EXITFLAG,OUTPUT.funcCount);
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfit(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('You    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n\n',r,EXITFLAG,OUTPUT.funcCount);

% Example 6 Slow oscillation
gamma = -1; a = 2; b = 2; mu = .1; noise = .1;
t = linspace(0,5,1000)';
y = MakeProb(gamma,a,b,mu,t,noise);
fprintf('Exact: %8.4f    %8.4f    %8.4f  %8.4f  \n',gamma,a,b,mu)
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('CVL    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n',r,EXITFLAG,OUTPUT.funcCount);
[gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfit(t,y);
r = NormOfResid(t,y,gamma,a,b,mu);
fprintf('You    %8.4f    %8.4f    %8.4f  %8.4f  ',gamma,a,b,mu)
fprintf('  %5.3f     %1d      %3d\n\n',r,EXITFLAG,OUTPUT.funcCount);


disp(' ')
disp('F (5pts):')
disp(' ')
disp('Correct use of lsqcurvefit (5pts):')
disp(' ')
disp('Starting value effort (5pts):')









function [gamma,a,b,mu,EXITFLAG,OUTPUT] = NLSfitCV(t,y)
% Fits the function f(t) = exp(gamma*t)*(a*cos(mu*t) + b*sin(mu*t))
% to the data (t(i),y(i)), i=1:m

x0 = xInitial(t,y);

% Set bounds so gamma is nonpositive and everything else is unrestricted...
LB = [-inf  -inf]; UB = [0 inf];
% Set tolerances...
options = optimset('display','off');
[x,resnorm,residual,EXITFLAG,OUTPUT] = lsqcurvefit(@(x,t) F(x,t,y),x0,t,y,LB,UB,options);
% Decipher x...
gamma = x(1); mu = x(2);
[a,b] = BestAB(gamma,mu,t,y);

function z = F(x,t,y);
gamma = x(1); mu = x(2);
[a,b] = BestAB(gamma,mu,t,y);
z = exp(gamma*t).*(a*cos(mu*t) + b*sin(mu*t));

function [a,b] = BestAB(gamma,mu,t,y)
% Since we want to minimize the 2-norm of
% [ exp(gamma*t).*cos(mu*t)  exp(gamma*t).*sin(mu*t)][a;b] - y
% it follows that for a given gamma and mu the best a and b are
% given by...
e = exp(gamma*t);
w = [e.*cos(mu*t) e.*sin(mu*t)]\y;
a = w(1); b = w(2);

function x0 = xInitial(t,y)
% Returns a column 2-vector x0 where x0(1) is an estimate
% of the time constant gamma and x0(2) estimates mu

n = length(t);

% Estimate mu...

% Find the times where y changes sign...
tZeros = [];
for i = 1:n-1
    if y(i)*y(i+1)<0
        tZeros = [tZeros t(i)];
    end
end
nz = length(tZeros);
if nz <= 1
    % Too few zeros to estimate mu. Longish period. Assume mu*t(n) = 2*pi
    mu = 2*pi/t(n);
else
    % If the underlying function was exp(gamma*t)*cos(mu*t) then 
    % deltaT = pi/mu would be the spacing of the zeros. So let's
    % let mu solve Average_deltaT = pi/mu...
    Ave_deltaT = mean(tZeros(2:nz)-tZeros(1:nz-1));
    mu = pi/Ave_deltaT;
end

% Find the local maximums of abs(y)...
iMax = [];
yp = abs(y);
for i = 2:length(yp)-1
    if yp(i) > yp(i-1) && yp(i) > yp(i+1)
        iMax = [iMax i];
    end
end
m = length(iMax);
if m > 1
    t1 = t(iMax(1)); y1 = yp(iMax(1));
    t2 = t(iMax(m)); y2 = yp(iMax(m));
    % If alfa*exp(gamma*t) fits (t1,y1), (t2,y2) then
    gamma = log(y1/y2)/(t1-t2);
end
x0 = [gamma;mu];


function r = NormOfResid(t,y,gamma,a,b,mu)
r = norm(exp(gamma*t).*(a*cos(mu*t) + b*sin(mu*t)) - y);
function y = MakeProb(gamma,a,b,mu,t,noise)
y = exp(gamma*t).*(a*cos(mu*t) + b*sin(mu*t))+ noise*randn(length(t),1);




