function [y_2,B,a,T_1,phi,values,nEvals] = bigGfit(y,t,seedSimplex);
%[y_2,B,a,T_1,phi,values,nEvals] = bigGfit(y,t,seedSimplex);
%          y --> displacement (mm) values on wall
%          t --> times (s) of displacement values
%seedSimplex --> seed simplex for optimization as [y_2 B a T_1 phi], with
%                      y_2 == equillibrium displacement y(inf) (mm)
%                        B == initial displacement y(0)-y(inf) (mm)
%                        a == damping constant (s)
%                      T_1 == oscilation period (s)
%                      phi == phase offset (radians)
%   y_2 <== simplex results for y_2
%     B <== simplex results for B
%     a <== simplex results for a
%   T_1 <== simplex results for T_1
%   phi <== simplex results for phi
%values <== function values on simplex vertices
%nEvals <== # of function valuations req'd to optimize
%
%This MATLAB function fits a curve to your data for the G lab based on a
%model function y = y_2 + B*exp(-t/a).*cos(2*pi*t/T_1-phi).  It performs
%the fit by minimizing the absolute value of the sum of deviations between
%your measured displacements and the model displacements.  This fit
%assumes that sig_t/|t| << 1.  The curve fit is done via the Nelder-Mead
%optimization technique, since we cannot easily describe the damped 
%sinusoid easily as a linear combination of functions.  The Nelder-Mead is
%one technique that can be used to accomplish nonlinear fitting tasks.  In
%order to perform a Nelder-mead optimization (here we define "optimize" to
%as finding the smallest possible absolute value of the sum of deviations 
%between measured displacements and model displacements), you must first
%define a seed simplex (a best guess) to initialize the optimization.  
%Since we're fitting 5 parameters, we really ought to have a 6-vertex
%simplex, of which what you provide is only one (i.e., six sets of
%variations on the best guesses for the five parameters).  To simplify
%matters, you only provide one vertex and a simplex about that best guess
%is constructed for you by this code.  You should be able to see from a 
%plot of your data a means to guess reasonable values for each of the 5 
%parameters we're fitting. After using it, take a look through the code 
%listing and see what makes sense to you.
%
% CRITICAL NOTE: this code assumes that only one parameter may be
% identically zero (phi) -- if you set the aparatus up so that y_2 == 0,
% then DO NOT use 0 for the seed value; rather, some small value such as
% 0.0025.

% (c) 17 Feb 2006, Curt A. L. Szuberla, 
%                 University of Alaska Fairbanks, all rights reserved

% extract individual seeds
y_2Seed = seedSimplex(1);
BSeed = seedSimplex(2);
aSeed = seedSimplex(3);
T_1Seed = seedSimplex(4);
phiSeed = seedSimplex(5);

% compact parameters into seed simplex x_o
% note, since we need to fit 5 parameters, we need a 6-pt simplex
widFac = 0.05;
if phiSeed == 0
    phiSeed = phiSeed+0.0025; % dither phiSeed
end
x_o = [y_2Seed BSeed aSeed T_1Seed phiSeed
    y_2Seed*(1-widFac) BSeed aSeed T_1Seed phiSeed
    y_2Seed BSeed*(1-widFac) aSeed T_1Seed phiSeed
    y_2Seed BSeed aSeed*(1-widFac) T_1Seed phiSeed
    y_2Seed BSeed aSeed T_1Seed*(1-widFac) phiSeed
    y_2Seed BSeed aSeed T_1Seed phiSeed*(1-widFac)
];

% define anonymous function call & handle
normFunction = @(x_o) dampedOscillatorDiffs(x_o,y,t);

% perform fit via minimizing sum of deviations (you can select either 
% abs(d) or d^2 to minimize, it doesn't make much difference, but we take
% the std. chi-squared route here and do d^2; check the code below to see
% where you can change this to abs(d)).  once we find a minimum, we
% re-start the mimimization at that point to ensure we didn't get trapped
% in a small local minima
[x,values,nEvals] = nmOpt(normFunction,x_o,1e-6);
[x,values,nEvals] = nmOpt(normFunction,x,1e-9);

% deconvolve output x (actually spits back out final simplex)
y_2 = x(:,1);
B = x(:,2);
a = x(:,3);
T_1 = x(:,4);
phi = x(:,5);

% plot results
clf
tPlot = linspace(min(t),max(t),1000);
plot(t,y,'bo',t,y,'gx',...
    tPlot,...
    dampedOscillator(mean(y_2),mean(B),tPlot,mean(a),mean(T_1),mean(phi)),...
    'b-');
xlabel('t (s)')
ylabel('displacement (mm)')

return

function val = dampedOscillatorDiffs(x,y,t,y_1);

% this function calculates the sum of absolute value of differences or the 
% chi-sqaures between measured displacements (assuming relatively small 
% errors in time) and the  model values

% deconvolve x
y_2 = x(1);
B = x(2);
a = x(3);
T_1 = x(4);
phi = x(5);

% calculate differences at all times, L1 & L2 norm given below
%val = sum(abs(y - dampedOscillator(y_2,B,t,a,T_1,phi))); %L1 norm
val = sum((y - dampedOscillator(y_2,B,t,a,T_1,phi)).^2);  %L2 norm

return

function y = dampedOscillator(y_2,B,t,a,T_1,phi);

% this is the model equation
y = y_2 + B*exp(-t/a).*cos(2*pi*t/T_1-phi);

return

function [simplex,values,nEvals]=nmOpt(normFunction,simplex_o,vTol,mxEvals);
%[simplex,values,nEvals] = nmOpt(normFunction,simplex_o,valuesTol,maxEvals);
% Nelder-Mead simplex optimization scheme
%normFunction --> function handle to be minimized (use anonymous functions
%                 to tackle functions w/ parameters: e.g.:
%                                normFunction = @(x) foo(x,param1,param2);
%   simplex_o --> initial [d+1,d] simplex in search space
%   valuesTol --> exit tolerance on function values (default == 0.01)
%    maxEvals --> exit tolerance on function evaluations (default == 491)
%     simplex <== final [d+1,d] simplex, whose vertices give function 
%                 values all within valuesTol of one another (unless 
%                 maxEvals exceeded)
%      values <== [d+1,1] function values on simplex vertices
%      nEvals <== # of function evaluations taken

% (c) 23 Sep 2005, Curt A. L. Szuberla, 
% (c)              University of Alaska Fairbanks, all rights reserved

verNo = 1.0;
fnName = 'nmOpt';
%zcode(fnName);
%% version check
if nargin == 0
    disp(['(' fnName '.m) current version: ' num2str(verNo)]);
    return
end

% ascertain dimensions & initialize variables, set defaults
[diM,dim] = size(simplex_o);
simplex = simplex_o;
values = zeros(diM,1);
vTolDef = 0.01;   % function value exit tolerance
mxEvalsDef = 491; % max function evaluations before forced exit
EpS = eps*10^6;   % avoid divide-by-zero in exit tolerance checks
tempTol = mxEvalsDef;

% assign defaults if req'd
if nargin == 2
    vTol = vTolDef;
    mxEvals = mxEvalsDef;
elseif nargin == 3
    if isempty(vTol),vTol = vTolDef;end
    mxEvals = mxEvalsDef;
elseif nargin == 4
    if isempty(vTol),vTol = vTolDef;end
    if isempty(mxEvals),mxEvals = mxEvalsDef;end
end

% initialize values & sort
for k = 1:diM
    values(k) = normFunction(simplex(k,:));
end
nEvals = diM;
[values,idx] = sort(values);
simplex = simplex(idx,:);

% search pattern
while vTol < tempTol && nEvals < mxEvals
    % reflect worst point in simplex through opposite face
    [valueTest,simplex,values,nEvals] = ...
        metaReflect(-1,diM,dim,simplex,values,normFunction,nEvals);
    if valueTest >= values(dim) % refl pt worse than the next-worst vertex?
        valueTemp = values(diM); % set the worst vertex aside for a bit
        % contract in 1D along reflection line
        [valueTest,simplex,values,nEvals] = ...
            metaReflect(0.5,diM,dim,simplex,values,normFunction,nEvals);
        if valueTest >= valueTemp % contr. pt. worse than worst vertex?
            % contract entire simplex toward best vertex
            simplex(2:diM,:) = ... % no call to cols/rows here
                0.5*(simplex(2:diM,:) - ones(dim,1)*simplex(1,:));
            for k = 2:diM
                values(k) = normFunction(simplex(k,:));
            end
            nEvals = nEvals + dim;
        end
    elseif valueTest <= values(1) % was it better than the best vertex?
        % expansion in direction of reflection
        [valueTest,simplex,values,nEvals] = ...
            metaReflect(2,diM,dim,simplex,values,normFunction,nEvals);
    end
    % re-sort on exit
    [values,idx] = sort(values);
    simplex = simplex(idx,:);
    % calculate exit condition
    tempTol = abs(values(diM)-values(1))/(mean(abs(values))+EpS);
end

return

%% private functions
function [valueTest,simplex,values,nEvals] = ...
    metaReflect(scaleFactor,diM,dim,simplex,values,normFunction,nEvals);

% calculate meta-reflection point
scaleA = (1-scaleFactor)/dim;
simplexTest = sum(simplex)*scaleA - ...
    simplex(diM,:)*(scaleA-scaleFactor);

% attempt metaReflection
valueTest = normFunction(simplexTest);
nEvals = nEvals + 1;

% check result for improvement
if values(diM) > valueTest
    simplex(diM,:) = simplexTest;
    values(diM) = valueTest;
end

return