classdef BundleAdjust < matlab.mixin.Copyable

    properties
        % Cameras and landmarks are represented by graph nodes
        %   coord(camera) -> camera pose, 6 vector
        %   vdata(camera) -> index into state vector or [] if a fixed camera
        %   coord(landmark) -> landmark position, first 3 elements of 6 vector
        %   vdata(landmark) -> index into state vector
        %
        % An edge from camera to a landmark holds the observed projection as a
        % property
        %   edata(edge) -> uv*

        g         % PGraph object describing visibility

        camera    % camera projection model

        cameras   % list of graph nodes corresponding to cameras
        points    % list of graph nodes corresponding to landmarks

        fixedcam     % logical array of cameras that are fixed
        fixedpoint   % logical array of landmarks that are fixed
    end

    properties (Dependent=true)
        ncams       % number of cameras
        nlandmarks  % number of landmark points
        ndim        % size of the linear system
    end

    methods

        function ba = BundleAdjust(camera)
            %BundleAdjust.BundleAdjust Bundle adjustment problem constructor
            %
            % Notes::
            % - A cameraModel file must exist on the path to evaluate the point projection
            %   and Jacobians.

            ba.camera = camera;  % stash the camera model
            ba.g = PGraph(6);    % initialize the graph, nodes have 6D coordinates
        end

        function n = get.ncams(ba)
            n = length(ba.cameras);
        end

        function n = get.nlandmarks(ba)
            n = length(ba.points);
        end

        function n = get.ndim(ba)
            n = 6*(ba.ncams - sum(ba.fixedcam)) + 3*(ba.nlandmarks - sum(ba.fixedpoint));
        end

        function n = cameraIdx(ba, i)
            n = 6*i - 5;
        end

        function n = landmarkIdx(ba, j)
            n = ba.ncams*6 + 3*j-2;
        end

        function n = cameraIdx2(ba, i)
            if ba.fixedcam(i)
                n = 0;
            else
                n = 6*(i - sum(ba.fixedcam(1:i))) - 5;
            end
        end

        function n = landmarkIdx2(ba, j)
            if ba.fixedpoint(j)
                n = 0;
            else
                n = 6*(ba.ncams - sum(ba.fixedcam)) + 3 * (j - sum(ba.fixedpoint(1:j))) - 2;
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % METHODS TO SUPPORT BUILDING PROBLEMS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function vc = add_camera(ba, T, varargin)
            %BundleAdjust.add_camera Add camera to bundle adjustment problem
            %
            % VC = BA.add_camera(T, options) is the vertex id of a camera node added to
            % a bundle adjustment problem.  The camera has pose T which can be SE3 or a
            % vector (1x7) comprising translation and quaternion.
            %
            % VC = BA.add_camera() as above but the camera pose is the null pose in SE3.
            %
            % Options::
            % 'fixed'    This camera is fixed (anchored) and will not be adjusted in
            %            the optimization process.
            %
            % See also BundleAdjust.add_landmark, BundleAdjust.add_projection, PGraph.

            if nargin == 1
                T = SE3();
            end

            opt.fixed = false;
            opt = tb_optparse(opt, varargin);

            if isvec(T, 7)
                t = T(1:3); q = T(4:7);
            else
                [R,t] = tr2rt(T);
                q = UnitQuaternion(R).double;
                t = t';
            end

            if q(1) < 0
                q = -q;
            end
            x = [t q(2:4)];
            vc = ba.g.add_node(x);
            ba.cameras = [ba.cameras vc];
            ba.g.setvdata(vc, length(ba.cameras));

            ba.fixedcam = [ba.fixedcam opt.fixed];
        end

        function vp = add_landmark(ba, P, varargin)
            %BundleAdjust.add_landmark Add landmark to bundle adjustment problem
            %
            % VL = BA.add_landmark(P, options) is the vertex id of a landmark node added to
            % a bundle adjustment problem.  The landmark has position P (3x1).
            %
            % Options::
            % 'fixed'    This landmark is fixed (anchored) and will not be adjusted in
            %            the optimization process.
            %
            % See also BundleAdjust.add_camera, BundleAdjust.add_projection, PGraph.

            assert(isvec(P), 'P must be a 3-vector');

            opt.fixed = false;
            opt = tb_optparse(opt, varargin);

            x = [P(:); 0; 0; 0];
            vp = ba.g.add_node(x);
            ba.points = [ba.points vp];
            ba.g.setvdata(vp, length(ba.points));

            ba.fixedpoint = [ba.fixedpoint opt.fixed];

        end

        function e = add_projection(ba, c, p, uv)
            %BundleAdjust.add_projection Add camera to bundle adjustment problem
            %
            % EP = BA.add_projection(VC, VL, UV) is the edge id of an edge added to
            % a bundle adjustment problem.  it represents the observed projection UV (2x1) of the landmark node VL as
            % seen by the camera node VC.
            %
            % See also BundleAdjust.add_camera, BundleAdjust.add_landmark, PGraph.

            assert(isvec(uv, 2), 'uv must be a 2-vector');

            e = ba.g.add_edge(c, p);
            ba.g.setedata(e, uv(:));
        end

        function load_sba(ba, cameraFile, pointFile, calibFile)
            %BundleAdjust.load_sba Load bundle adjustment problem from files
            %
            % BA.load_sba(camfile, pointfile, calibfile) loads a bundle adjustment
            % problem from data files as distributed with the SBA package
            %
            % Example::
            %
            % To solve the 7-point bundle adjustment problem distributed with SBA:
            %
            %           ba = BundleAdjust();
            %           pth = 'sba-1.6/demo/'
            %           ba.load_sba([pth '7cams.txt'], [pth '7pts.txt'], [pth 'calib.txt']);
            %
            % Reference::
            % - Sparse Bundle Adjustment package by Manolis Lourakis
            %   http://users.ics.forth.gr/~lourakis/sba
            %
            % See also BundleAdjust.add_camera, BundleAdjust.add_landmark, BundleAdjust.add_projection, PGraph.

            % adopted from sba-1.6/matlab/eucsbademo.m

            % read in camera parameters
            [q1, q2, q3, q4, tx, ty, tz] = textread(cameraFile, '%f%f%f%f%f%f%f', 'commentstyle', 'shell');

            for i=1:length(q1)
                ba.add_camera( [tx(i) ty(i) tz(i) q1(i) q2(i) q3(i) q4(i)] );
            end

            % read points file line by line
            %
            % each line is a world point
            fid = fopen(pointFile);
            npts = 0;
            while ~feof(fid)
                line = fgets(fid);
                [A, count, errmsg, nextindex] = sscanf(line, '%f%f%f%f', [1, 4]); % read X, Y, Z, nframes
                if(size(A, 2)>0) % did we read anything?
                    npts=npts+1;

                    % create a node for this point
                    lm = ba.add_landmark(A(1:3));

                    % now find which cameras it was seen by
                    nframes = A(4);
                    for i=1:nframes % read "nframes" id, x, y triplets
                        [A, count, errmsg, j] = sscanf(line(nextindex:length(line)), '%f%f%f', [1, 3]); % read id, x, y
                        nextindex=nextindex+j; % skip the already read line prefix

                        % add a landmark projection
                        ba.add_projection( ba.cameras(A(1)+1), lm, A(2:3) );
                    end
                end
            end
            fclose(fid);

            % read in calibration parameters
            [a1, a2, a3] = textread(calibFile, '%f%f%f', 'commentstyle', 'shell');
            % a1(1)    a2(1)  a3(1)  a2(2)    a3(2)];
            % f/rho_u  skew   u0     f/rho_v  v0

            ba.camera.f = a1(1);
            ba.camera.rho = [1 a1(1)/a2(2)];
            ba.camera.pp = [a3(1) a3(2)];
        end

        function T = getcamera(ba, ii)
            assert(all(ii > 0), 'camera indices must be > 0');
            assert(all(ii <= ba.ncams), 'camera index out of range');
            if nargin == 1
                ii = [1:ba.ncams];
            end
            for k=1:length(ii)
                i = ii(k);
                node = ba.cameras(i);
                qv = ba.g.coord(node);
                T(k) = SE3(qv(1:3)') * UnitQuaternion.vec(qv(4:6)).SE3;
            end

        end

        function P = getlandmark(ba, jj)
            assert(all(jj > 0), 'landmark indices must be > 0');
            assert(all(jj <= ba.nlandmarks), 'landmark index out of range');
            if nargin == 1
                jj = [1:ba.nlandmarks];
            end
            for k=1:length(jj)
                j = jj(k);
                node = ba.points(j);
                qv = ba.g.coord(node);
                P(:,k) = qv(1:3);
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % METHODS TO SOLVE PROBLEMS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function [ba2,e] = optimize(ba, varargin)
            %BundleAdjust.optimize  Optimize the solution
            %
            % BA2 = BA.optimize(

            opt.iterations = 1000;
            opt.animate = false;
            opt.retain = false;
            opt.lambda = 0.1;
            opt.lambdamin = 1e-8;
            opt.dxmin = 1e-8;
            opt.tol = 0.05;

            [opt,args] = tb_optparse(opt, varargin);

            %g2 = PGraph(pg.graph);  % deep copy

            if length(args) > 0 && isvec(args{1}, ba.ndim)
                X = args{1};
            else
                X = ba.getstate();
            end

            lambda = opt.lambda;

                        X0 = X;

            t0 = cputime();

            fprintf('Initial cost %g\n', ba.errors(X));

            for i=1:opt.iterations
                if opt.animate
                    if ~opt.retain
                        clf
                    end
                    g2.plot();
                    pause(0.5)
                end

                tic

                % solve for the step
                [dX,energy] = ba.solve(X, lambda);
                % update the state
                Xnew = ba.updatestate(X, dX);
                % compute new value of cost
                enew = ba.errors(Xnew);

                dt = toc;
                fprintf('  total cost %g (solved in %.2g sec)', enew, dt);

                % are we there yet?
                if enew < opt.tol
                    break
                end

                % have we stopped moving
                if  norm(dX) < opt.dxmin
                    break
                end

                % do the Levenberg-Marquadt thing, was it a good update?
                if enew < energy
                    % step is accepted
                    X = Xnew;
                    if lambda > opt.lambdamin
                        lambda = lambda/sqrt(2);
                    end
                    if opt.verbose
                        fprintf(' -- step accepted: lambda = %g', lambda);
                    end
                else
                    % step is rejected
                    lambda = lambda*4;
                    if opt.verbose
                        fprintf(' -- step rejected: lambda = %g', lambda);
                    end
                end

                fprintf('\n');
            end

            tf = cputime();
            fprintf('\n * %d iterations in %.1f seconds\n', i, tf-t0);
            fprintf(' * %.2f pixels RMS error\n', sqrt(enew/ba.g.ne));

            % copy the object and update it
            ba2 = copy(ba);
            ba2.g = copy(ba.g);
            ba2.setstate(X);

            if nargout > 1
                e = enew;
            end
        end

        function [deltax,err] = solve(ba, X, lambda)

            if nargin < 3
                lambda =0;
            end

            %ba.init_tables();

            % create the Hessian and error vector
            [H,b,e] = ba.build_linear_system(X);

            % add damping term to the diagonal
            for i=1:ba.ndim
                H(i,i) = H(i,i) + lambda;
            end

            % solve for the state update
            % - could replace this with the Schur complement trick
            deltax = H \ b;

            if nargout > 1
                err = e;
            end
        end

        % build the Hessian and measurement vector
        function [H,b,etotal,J] = build_linear_system(ba, X)

            % allocate storage
            H = sparse(ba.ndim, ba.ndim);
            b = zeros(ba.ndim,1);

            etotal = 0;

            % loop over cameras
            for i=1:ba.ncams

                k = ba.cameraIdx(i);
                x = X(k:k+5);

                t = x(1:3); qv = x(4:6);

                % loop over all points viewed from this camera
                for p=ba.g.edges( ba.cameras(i) )
                    v = ba.g.vertices(p);
                    j = ba.g.vdata(v(2));

                    k = ba.landmarkIdx(j);
                    x = X(k:k+2);

                    P = x';

                    uv = ba.g.edata(p);

                    % compute Jacobians and projection

                    [uvhat,JA,JB] = ba.camera.derivs(t, qv, P);

                    % compute reprojection error
                    e = uvhat - uv;
                    etotal = etotal + e'*e;

                    ii = ba.cameraIdx2(i); jj = ba.landmarkIdx2(j);

                    % compute the block components of H and b for this edge

                    if ~ba.fixedcam(i) & ~ba.fixedpoint(j)
                        % adjustable point and camera
                        H_ii= JA'*JA;
                        H_ij= JA'*JB;
                        H_jj= JB'*JB;

                        H(ii:ii+5,ii:ii+5) = H(ii:ii+5,ii:ii+5) + H_ii;
                        H(ii:ii+5,jj:jj+2) = H(ii:ii+5,jj:jj+2) + H_ij;
                        H(jj:jj+2,ii:ii+5) = H(jj:jj+2,ii:ii+5) + H_ij';
                        H(jj:jj+2,jj:jj+2) = H(jj:jj+2,jj:jj+2) + H_jj;

                        b_i = -JA'*e;
                        b_j = -JB'*e;

                        b(ii:ii+5) = b(ii:ii+5) + b_i;
                        b(jj:jj+2) = b(jj:jj+2) + b_j;

                    elseif ba.fixedcam(i) & ~ba.fixedpoint(j)
                        % fixed camera and adjustable point

                        H_jj= JB'*JB;

                        H(jj:jj+2,jj:jj+2) = H(jj:jj+2,jj:jj+2) + H_jj;

                        b_j = -JB'*e;

                        b(jj:jj+2) = b(jj:jj+2) + b_j;

                    elseif ~ba.fixedcam(i) & ba.fixedpoint(j)
                        % adjustable camera and fixed point

                        H_ii= JA'*JA;

                        H(ii:ii+5,ii:ii+5) = H(ii:ii+5,ii:ii+5) + H_ii;

                        b_i = -JA'*e;

                        b(ii:ii+5) = b(ii:ii+5) + b_i;
                    end

                end
            end

        end

        function spyH(ba, X)
            H = build_linear_system(ba, X);
            spy(H);
        end

        function X = getstate(ba)
            %BundleAdjust.getstate Get the state vector
            %
            % X = BA.getstate() is a row vector containing the state vector of the
            % problem: all camera poses followed by all landmark coordinates.
            %
            % Notes::
            % - The length of the vector is given by BA.ndim.

            X = [];

            i = 0;
            for c=ba.cameras  % step through camera nodes
                X = [X  ba.g.coord(c)'];
            end

            for p=ba.points  % step through landmark nodes
                P = ba.g.coord( p );
                X = [X  P(1:3)'];
            end
        end

        function setstate(ba, X)
            %BundleAdjust.getstate Get the state vector
            %
            % X = BA.getstate() is a row vector containing the state vector of the
            % problem: all camera poses followed by all landmark coordinates.
            %
            % Notes::
            % - The length of the vector is given by BA.ndim.

            for i=1:ba.ncams  % step through camera nodes
                if ~ba.fixedcam(i)
                    k = ba.cameraIdx(i);
                    c = ba.cameras(i);
                    ba.g.setcoord(c, X(k:k+5));
                end
            end

            for j=1:ba.nlandmarks
                if ~ba.fixedpoint(j)
                    k = ba.landmarkIdx(j);
                    lm = ba.points(j);
                    ba.g.setcoord(lm, [X(k:k+2) 0 0 0]);
                end
            end
        end

        function Xnew = updatestate(ba, X, dX)
            dX = dX(:)';

            % for each camera we need to compound the camera pose with the
            % incremental relative pose
            for i=1:ba.ncams
                k = ba.cameraIdx(i);
                if ba.fixedcam(i)
                    Xnew(k:k+5) = X(k:k+5);
                else
                    x = X(k:k+5);
                    t = x(1:3); qv = x(4:6);  % get current pose

                    k2 = ba.cameraIdx2(i);
                    dx = dX(k2:k2+5);
                    dt = dx(1:3); dqv = dx(4:6); % get incremental pose
                    tnew = t+dt;  % assume translation in old frame
                    % compound the quaternion vector rotations
                    % - function qvmul is symbolically generated by ba
                    %qvnew = qvmul(qv(1), qv(2), qv(3), dqv(1), dqv(2), dqv(3));
                    qvnew = UnitQuaternion.qvmul(qv, dqv);

                    Xnew(k:k+5) = [tnew qvnew];
                end
            end

            % for each landmark we add the increment to its position
            for j=1:ba.nlandmarks
                k = ba.landmarkIdx(j);
                x = X(k:k+2);
                if ba.fixedpoint(j)
                    Xnew(k:k+2) = x;
                else
                    k2 = ba.landmarkIdx2(j);
                    dx = dX(k2:k2+2);
                    Xnew(k:k+2) = x + dx;
                end
            end
        end

        % Compute total squared reprojection error
        function etotal = errors(ba, X)

            if nargin < 2
                X = ba.getstate();
            end
            r = ba.getresidual(X);

            etotal = sum(r(:));
        end

        function r = getresidual(ba, X)
            % this is the squared reprojection errors

            if nargin == 1
                X = ba.getstate();
            end

            % loop over cameras
            for i=1:ba.ncams

                k = ba.cameraIdx(i);
                x = X(k:k+5);

                t = x(1:3); qv = x(4:6);

                % loop over all points viewed from this camera
                for p=ba.g.edges( ba.cameras(i) )
                    v = ba.g.vertices(p);
                    j = ba.g.vdata(v(2));

                    k = ba.landmarkIdx(j);
                    x = X(k:k+2);

                    P = x';

                    uv = ba.g.edata(p);

                    uvhat = ba.camera.derivs(t, qv, P);

                    % compute reprojection error
                    e = uvhat - uv;
                    r(i,j) =  e'*e;
                end
            end
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % SUPPORT METHODS
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function plot(ba, varargin)
            clf
            ba.g.plot('dims', 3, 'only', ba.points, 'EdgeColor', 0.8*[1 1 1], varargin{:});
            axis equal
            hold on

            colorOrder = get(gca, 'ColorOrder');
            for i=1:ba.ncams
                T = ba.getcamera(i);
                cam(i) = ba.camera.move(T);
                cidx = mod(i-1, numrows(colorOrder))+1;
                color = colorOrder(cidx,:);
                cam(i).plot_camera('scale', 0.2, 'color', color, 'persist')
            end
            hold off
            axis equal
            xlabel('X (m)')
            ylabel('Y (m)')
            zlabel('Z (m)')
            grid on
        end

        function display(ba)
            %BundleAdjust.display BundleAdjust parameters
            %
            % BA.display() displays the bundle adjustment parameters in compact format.
            %
            % Notes::
            % - This method is invoked implicitly at the command line when the result
            %   of an expression is a BundleAdjust object and the command has no trailing
            %   semicolon.
            %
            % See also BundleAdjust.char.
            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            disp( char(ba) );
        end % display()

        function s = char(ba)
            %BundleAdjust.char Convert to string
            %
            % BA.char() is a string showing bundle adjustment parameters in compact format.
            %
            %
            % See also BundleAdjust.display.
            s = 'Bundle adjustment problem:';
            s = strvcat(s, sprintf('  %d cameras', ba.ncams));
            if any(ba.fixedcam)
                s = strvcat(s, sprintf('    locked cameras: %s', num2str(find(ba.fixedcam))));
            end

            s = strvcat(s, sprintf('  %d landmarks', ba.nlandmarks));

            s = strvcat(s, sprintf('  %d projections', ba.g.ne));

            s = strvcat(s, sprintf('  %d dimension linear problem', ba.ndim));
            c = ba.g.connectivity(ba.cameras);
            s = strvcat(s, sprintf('  landmarks per camera: min=%.1f, max=%.1f, avg=%.1f', min(c), max(c), mean(c)));
            c = ba.g.connectivity(ba.points);
            s = strvcat(s, sprintf('  cameras per landmark: min=%.1f, max=%.1f, avg=%.1f', min(c), max(c), mean(c)));
        end

    end % methods

end % classdef


function [x,zo,xs]=v_estnoisem(yf,tz,pp)
%V_ESTNOISEM - estimate noise spectrum using minimum statistics
%
% Usage:    ninc=round(0.016*fs);   % frame increment [fs=sample frequency]
%           ovf=2;                  % overlap factor
%           f=v_rfft(v_enframe(s,v_windows(2,ovf*ninc,'l'),ninc),ovf*ninc,2);
%           f=f.*conj(f);           % convert to power spectrum
%           x=v_estnoisem(f,ninc/fs); % estimate the noise power spectrum
%
% Inputs:
%   yf      input power spectra (one row per frame)
%   tz      frame increment in seconds
%           Alternatively, the input state from a previous call (see below)
%   pp      algorithm parameters [optional]
%
% Outputs:
%   x       estimated noise power spectra (one row per frame)
%   zo      output state
%   xs      estimated std error of x (one row per frame)
%           xs seems often to be an underestimate by a factor of 2 or 3
%
% The algorithm parameters are defined in reference [1] from which equation
% numbers are given in parentheses. They are as follows:
%
%        pp.taca      % (11): smoothing time constant for alpha_c [0.0449 seconds]
%        pp.tamax     % (3): max smoothing time constant [0.392 seconds]
%        pp.taminh    % (3): min smoothing time constant (upper limit) [0.0133 seconds]
%        pp.tpfall    % (12): time constant for P to fall [0.064 seconds]
%        pp.tbmax     % (20): max smoothing time constant [0.0717 seconds]
%        pp.qeqmin    % (23): minimum value of Qeq [2]
%        pp.qeqmax    % max value of Qeq per frame [14]
%        pp.av        % (23)+13 lines: fudge factor for bc calculation  [2.12]
%        pp.td        % time to take minimum over [1.536 seconds]
%        pp.nu        % number of subwindows to use [3]
%        pp.qith      % Q-inverse thresholds to select maximum noise slope [0.03 0.05 0.06 Inf ]
%        pp.nsmdb     % corresponding noise slope thresholds in dB/second   [47 31.4 15.7 4.1]
%
% Example use:      y=v_enframe(s,w,ni);                  % divide speech signal s(n) into
%                                                       % overlapping frames using window w(n)
%                   yf=v_rfft(y,nf,2);                    % take fourier transform
%                   dp=v_estnoisem(yf.*conj(yf),tinc);    % estimate the noise
%
% If convenient, you can call v_estnoisem in chunks of arbitrary size. Thus the following are equivalent:
%
%                   (a) dp=v_estnoisem(yp(1:300),tinc);
%
%                   (b) [dp(1:100),z]=v_estnoisem(yp(1:100),tinc);
%                       [dp(101:200),z]=v_estnoisem(yp(101:200),z);
%                       [dp(201:300),z]=v_estnoisem(yp(201:300),z);

% This is intended to be a precise implementation of [1] with Table III
% replaced by the updated table 5 from [2]. The only deliberate algorithm
% change is the introduction of a minimum value for 1/Qeq in equation (23).
% This change only affects the first few frames and improves the
% convergence of the algorithm. A minor improveemnt was reported in [3] but
% this has not yet been included.
%
% Refs:
%    [1] Rainer Martin.
%        Noise power spectral density estimation based on optimal smoothing and minimum statistics.
%        IEEE Trans. Speech and Audio Processing, 9(5):504-512, July 2001.
%    [2] Rainer Martin.
%        Bias compensation methods for minimum statistics noise power spectral density estimation
%        Signal Processing, 2006, 86, 1215-1229
%    [3] Dirk Mauler and Rainer Martin
%        Noise power spectral density estimation on highly correlated data
%        Proc IWAENC, 2006

%	   Copyright (C) Mike Brookes 2008
%      Version: $Id: v_estnoisem.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nr,nrf]=size(yf);          % number of frames and freq bins
x=zeros(nr,nrf);            % initialize output arrays
xs=zeros(nr,nrf);           % will hold std error in the future
if isempty(yf) && isstruct(tz)             % no real data
    zo=tz;              % just keep the same state
else
    if isstruct(tz)       % take parameters from a previous call
        nrcum=tz.nrcum;
        p=tz.p;          % smoothed power spectrum
        ac=tz.ac;               % correction factor (9)
        sn2=tz.sn2;              % estimated noise power
        pb=tz.pb;               % smoothed noisy speech power (20)
        pb2=tz.pb2;
        pminu=tz.pminu;
        actmin=tz.actmin;   % Running minimum estimate
        actminsub=tz.actminsub;           % sub-window minimum estimate
        subwc=tz.subwc;                   % force a buffer switch on first loop
        actbuf=tz.actbuf;  % buffer to store subwindow minima
        ibuf=tz.ibuf;
        lminflag=tz.lminflag;      % flag to remember local minimum
        tinc=tz.tinc;     % frame increment
        qq=tz.qq;         % parameter structure
    else
        tinc = tz;          % second argument is frame increment
        nrcum=0;            % no frames so far
        % default algorithm constants

        qq.taca=0.0449;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
        qq.tamax=0.392;    % max smoothing time constant in (3) = -tinc/log(0.96)
        qq.taminh=0.0133;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
        qq.tpfall=0.064;   % time constant for P to fall (12)
        qq.tbmax=0.0717;   % max smoothing time constant in (20) = -tinc/log(0.8)
        qq.qeqmin=2;       % minimum value of Qeq (23)
        qq.qeqmax=14;      % max value of Qeq per frame
        qq.av=2.12;             % fudge factor for bc calculation (23 + 13 lines)
        qq.td=1.536;       % time to take minimum over
        qq.nu=8;           % number of subwindows
        qq.qith=[0.03 0.05 0.06 Inf]; % noise slope thresholds in dB/s
        qq.nsmdb=[47 31.4 15.7 4.1];

        if nargin>=3 && ~isempty(pp)
            qqn=fieldnames(qq);
            for i=1:length(qqn)
                if isfield(pp,qqn{i})
                    qq.(qqn{i})=pp.(qqn{i});
                end
            end
        end
    end

    % unpack parameter structure

    taca=qq.taca;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
    tamax=qq.tamax;    % max smoothing time constant in (3) = -tinc/log(0.96)
    taminh=qq.taminh;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
    tpfall=qq.tpfall;   % time constant for P to fall (12)
    tbmax=qq.tbmax;   % max smoothing time constant in (20) = -tinc/log(0.8)
    qeqmin=qq.qeqmin;       % minimum value of Qeq (23)
    qeqmax=qq.qeqmax;      % max value of Qeq per frame
    av=qq.av;             % fudge factor for bc calculation (23 + 13 lines)
    td=qq.td;       % time to take minimum over
    nu=qq.nu;           % number of subwindows
    qith=qq.qith; % noise slope thresholds in dB/s
    nsmdb=qq.nsmdb;   % maximum permitted +ve noise slope in dB/s

    % derived algorithm constants

    aca=exp(-tinc/taca); % smoothing constant for alpha_c in equ (11) = 0.7
    acmax=aca;          % min value of alpha_c = 0.7 in equ (11) also = 0.7
    amax=exp(-tinc/tamax); % max smoothing constant in (3) = 0.96
    aminh=exp(-tinc/taminh); % min smoothing constant (upper limit) in (3) = 0.3
    bmax=exp(-tinc/tbmax); % max smoothing constant in (20) = 0.8
    snrexp = -tinc/tpfall;
    nv=round(td/(tinc*nu));    % length of each subwindow in frames
    if nv<4            % algorithm doesn't work for miniscule frames
        nv=4;
        nu=max(round(td/(tinc*nv)),1);
    end
    nd=nu*nv;           % length of total window in frames
    [md,hd]=mhvals(nd); % calculate the constants M(D) and H(D) from Table III
    [mv,hv]=mhvals(nv); % calculate the constants M(D) and H(D) from Table III
    nsms=10.^(nsmdb*nv*tinc/10);  % [8 4 2 1.2] in paper
    qeqimax=1/qeqmin;  % maximum value of Qeq inverse (23)
    qeqimin=1/qeqmax; % minumum value of Qeq per frame inverse

    if isempty(yf)      % provide dummy initialization
        ac=1;               % correction factor (9)
        subwc=nv;                   % force a buffer switch on first loop
        ibuf=0;
        p=x;          % smoothed power spectrum
        sn2=p;              % estimated noise power
        pb=p;               % smoothed noisy speech power (20)
        pb2=pb.^2;
        pminu=p;
        actmin=repmat(Inf,1,nrf);   % Running minimum estimate
        actminsub=actmin;           % sub-window minimum estimate
        actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
        lminflag=zeros(1,nrf);      % flag to remember local minimum
    else

        if ~nrcum       % initialize values for first frame
            p=yf(1,:);          % smoothed power spectrum
            ac=1;               % correction factor (9)
            sn2=p;              % estimated noise power
            pb=p;               % smoothed noisy speech power (20)
            pb2=pb.^2;
            pminu=p;
            actmin=repmat(Inf,1,nrf);   % Running minimum estimate
            actminsub=actmin;           % sub-window minimum estimate
            subwc=nv;                   % force a buffer switch on first loop
            actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
            ibuf=0;
            lminflag=zeros(1,nrf);      % flag to remember local minimum
        end

        % loop for each frame

        for t=1:nr              % we use t instead of lambda in the paper
            yft=yf(t,:);        % noise speech power spectrum
            acb=(1+(sum(p)./sum(yft)-1).^2).^(-1);  % alpha_c-bar(t)  (9)
            ac=aca*ac+(1-aca)*max(acb,acmax);       % alpha_c(t)  (10)
            ah=amax*ac.*(1+(p./sn2-1).^2).^(-1);    % alpha_hat: smoothing factor per frequency (11)
            snr=sum(p)/sum(sn2);
            ah=max(ah,min(aminh,snr^snrexp));       % lower limit for alpha_hat (12)

            p=ah.*p+(1-ah).*yft;            % smoothed noisy speech power (3)
            b=min(ah.^2,bmax);              % smoothing constant for estimating periodogram variance (22 + 2 lines)
            pb=b.*pb + (1-b).*p;            % smoothed periodogram (20)
            pb2=b.*pb2 + (1-b).*p.^2;     	% smoothed periodogram squared (21)

            qeqi=max(min((pb2-pb.^2)./(2*sn2.^2),qeqimax),qeqimin/(t+nrcum));   % Qeq inverse (23)
            qiav=sum(qeqi)/nrf;             % Average over all frequencies (23+12 lines) (ignore non-duplication of DC and nyquist terms)
            bc=1+av*sqrt(qiav);             % bias correction factor (23+11 lines)
            bmind=1+2*(nd-1)*(1-md)./(qeqi.^(-1)-2*md);      % we use the simplified form (17) instead of (15)
            bminv=1+2*(nv-1)*(1-mv)./(qeqi.^(-1)-2*mv);      % same expression but for sub windows
            kmod=bc*p.*bmind<actmin;        % Frequency mask for new minimum
            if any(kmod)
                actmin(kmod)=bc*p(kmod).*bmind(kmod);
                actminsub(kmod)=bc*p(kmod).*bminv(kmod);
            end
            if subwc>1 && subwc<nv              % middle of buffer - allow a local minimum
                lminflag=lminflag | kmod;    	% potential local minimum frequency bins
                pminu=min(actminsub,pminu);
                sn2=pminu;
            else
                if subwc>=nv                    % end of buffer - do a buffer switch
                    ibuf=1+rem(ibuf,nu);     	% increment actbuf storage pointer
                    actbuf(ibuf,:)=actmin;    	% save sub-window minimum
                    pminu=min(actbuf,[],1);
                    i=find(qiav<qith);
                    nsm=nsms(i(1));          	% noise slope max
                    lmin=lminflag & ~kmod & actminsub<nsm*pminu & actminsub>pminu;
                    if any(lmin)
                        pminu(lmin)=actminsub(lmin);
                        actbuf(:,lmin)=repmat(pminu(lmin),nu,1);
                    end
                    lminflag(:)=0;
                    actmin(:)=Inf;
                    subwc=0;
                end
            end
            subwc=subwc+1;
            x(t,:)=sn2;
            qisq=sqrt(qeqi);
            % empirical formula for standard error based on Fig 15 of [2]
            xs(t,:)=sn2.*sqrt(0.266*(nd+100*qisq).*qisq/(1+0.005*nd+6/nd)./(0.5*qeqi.^(-1)+nd-1));
        end
    end
    if nargout>1    % we need to store the state for next time
        zo.nrcum=nrcum+nr;      % number of frames so far
        zo.p=p;          % smoothed power spectrum
        zo.ac=ac;               % correction factor (9)
        zo.sn2=sn2;              % estimated noise power
        zo.pb=pb;               % smoothed noisy speech power (20)
        zo.pb2=pb2;
        zo.pminu=pminu;
        zo.actmin=actmin;   % Running minimum estimate
        zo.actminsub=actminsub;           % sub-window minimum estimate
        zo.subwc=subwc;                   % force a buffer switch on first loop
        zo.actbuf=actbuf;  % buffer to store subwindow minima
        zo.ibuf=ibuf;
        zo.lminflag=lminflag;      % flag to remember local minimum
        zo.tinc=tinc;     % must be the last one
        zo.qq=qq;
    end
    if ~nargout
        clf;
        subplot(212);
        plot((1:nr)*tinc,10*log10([sum(yf,2) sum(x,2)]))
        ylabel('Frame Energy (dB)');
        xlabel(sprintf('Time (s)   [%d ms frame incr]',round(tinc*1000)));
        v_axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
        subplot(211);
        plot(1:nrf,10*log10([sum(yf,1)'/nr sum(x,1)'/nr]))
        ylabel('Power (dB)');
        xlabel('Frequency bin');
        v_axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
    end
end

function [m,h,d]=mhvals(d)
% Values are taken from Table 5 in [2]
%[2] R. Martin,"Bias compensation methods for minimum statistics noise power
%               spectral density estimation", Signal Processing Vol 86, pp1215-1229, 2006.

% approx: plot(d.^(-0.5),[m 1-d.^(-0.5)],'x-'), plot(d.^0.5,h,'x-')
persistent dmh
if isempty(dmh)
    dmh=[
        1   0       0;
        2   0.26    0.15;
        5   0.48    0.48;
        8   0.58    0.78;
        10  0.61    0.98;
        15  0.668   1.55;
        20  0.705   2;
        30  0.762   2.3;
        40  0.8     2.52;
        60  0.841   3.1;
        80  0.865   3.38;
        120 0.89    4.15;
        140 0.9     4.35;
        160 0.91    4.25;
        180 0.92    3.9;
        220 0.93    4.1;
        260 0.935   4.7;
        300 0.94    5];
end

if nargin>=1
    i=find(d<=dmh(:,1));
    if isempty(i)
        i=size(dmh,1);
        j=i;
    else
        i=i(1);
        j=i-1;
    end
    if d==dmh(i,1)
        m=dmh(i,2);
        h=dmh(i,3);
    else
        qj=sqrt(dmh(i-1,1));    % interpolate using sqrt(d)
        qi=sqrt(dmh(i,1));
        q=sqrt(d);
        h=dmh(i,3)+(q-qi)*(dmh(j,3)-dmh(i,3))/(qj-qi);
        m=dmh(i,2)+(qi*qj/q-qj)*(dmh(j,2)-dmh(i,2))/(qi-qj);
    end
else
    d=dmh(:,1);
    m=dmh(:,2);
    h=dmh(:,3);
end


% -------------------------------------------------------------------------
% Downloaded 10.08.2018 from:
% http://www.columbia.edu/~bsm2105/type2sdt/archive/index.html
% Author: Brian Maniscalco
% Contact: brian@psych.columbia.edu

% 10.08.2018: OKH added calculation for type 1 c to the results

% NOTE FROM WEBPAGE:
% If you use this analysis file, please reference the Consciousness &
% Cognition paper:
%       Maniscalco, B., & Lau, H. (2012). A signal detection
%       theoretic approach for estimating metacognitive sensitivity from
%       confidence ratings. Consciousness and Cognition, 21(1), 422–430.
%       doi:10.1016/j.concog.2011.09.021)
% And the website:
%       http://www.columbia.edu/~bsm2105/type2sdt/archive/index.html
% -------------------------------------------------------------------------

function fit = tapas_fit_meta_d_MLE(nR_S1, nR_S2, s, fncdf, fninv)

% fit = fit_meta_d_MLE(nR_S1, nR_S2, s, fncdf, fninv)
%
% Given data from an experiment where an observer discriminates between two
% stimulus alternatives on every trial and provides confidence ratings,
% provides a type 2 SDT analysis of the data.
%
% INPUTS
%
% * nR_S1, nR_S2
% these are vectors containing the total number of responses in
% each response category, conditional on presentation of S1 and S2.
%
% e.g. if nR_S1 = [100 50 20 10 5 1], then when stimulus S1 was
% presented, the subject had the following response counts:
% responded S1, rating=3 : 100 times
% responded S1, rating=2 : 50 times
% responded S1, rating=1 : 20 times
% responded S2, rating=1 : 10 times
% responded S2, rating=2 : 5 times
% responded S2, rating=3 : 1 time
%
% The ordering of response / rating counts for S2 should be the same as it
% is for S1. e.g. if nR_S2 = [3 7 8 12 27 89], then when stimulus S2 was
% presented, the subject had the following response counts:
% responded S1, rating=3 : 3 times
% responded S1, rating=2 : 7 times
% responded S1, rating=1 : 8 times
% responded S2, rating=1 : 12 times
% responded S2, rating=2 : 27 times
% responded S2, rating=3 : 89 times
%
% N.B. if nR_S1 or nR_S2 contain zeros, this may interfere with estimation of
% meta-d'.
%
% Some options for dealing with response cell counts containing zeros are:
%
% (1) Add a small adjustment factor, e.g. adj_f = 1/(length(nR_S1), to each
% input vector:
%
% adj_f = 1/length(nR_S1);
% nR_S1_adj = nR_S1 + adj_f;
% nR_S2_adj = nR_S2 + adj_f;
%
% This is a generalization of the correction for similar estimation issues of
% type 1 d' as recommended in
%
% Hautus, M. J. (1995). Corrections for extreme proportions and their biasing
%     effects on estimated values of d'. Behavior Research Methods, Instruments,
%     & Computers, 27, 46-51.
%
% When using this correction method, it is recommended to add the adjustment
% factor to ALL data for all subjects, even for those subjects whose data is
% not in need of such correction, in order to avoid biases in the analysis
% (cf Snodgrass & Corwin, 1988).
%
% (2) Collapse across rating categories.
%
% e.g. if your data set has 4 possible confidence ratings such that length(nR_S1)==8,
% defining new input vectors
%
% nR_S1_new = [sum(nR_S1(1:2)), sum(nR_S1(3:4)), sum(nR_S1(5:6)), sum(nR_S1(7:8))];
% nR_S2_new = [sum(nR_S2(1:2)), sum(nR_S2(3:4)), sum(nR_S2(5:6)), sum(nR_S2(7:8))];
%
% might be sufficient to eliminate zeros from the input without using an adjustment.
%
% * s
% this is the ratio of standard deviations for type 1 distributions, i.e.
%
% s = sd(S1) / sd(S2)
%
% if not specified, s is set to a default value of 1.
% For most purposes, we recommend setting s = 1.
% See http://www.columbia.edu/~bsm2105/type2sdt for further discussion.
%
% * fncdf
% a function handle for the CDF of the type 1 distribution.
% if not specified, fncdf defaults to @normcdf (i.e. CDF for normal
% distribution)
%
% * fninv
% a function handle for the inverse CDF of the type 1 distribution.
% if not specified, fninv defaults to @norminv
%
% OUTPUT
%
% Output is packaged in the struct "fit."
% In the following, let S1 and S2 represent the distributions of evidence
% generated by stimulus classes S1 and S2.
% Then the fields of "fit" are as follows:
%
% fit.d1        = mean(S2) - mean(S1), in room-mean-square(sd(S1),sd(S2)) units
% fit.c1        = type 1 criterion
% fit.s         = sd(S1) / sd(S2)
% fit.meta_d    = meta-d' in RMS units
% fit.M_diff    = meta_da - da
% fit.M_ratio   = meta_da / da
% fit.meta_c1   = type 1 criterion for meta-d' fit, RMS units
% fit.t2ca_rS1  = type 2 criteria of "S1" responses for meta-d' fit, RMS units
% fit.t2ca_rS2  = type 2 criteria of "S2" responses for meta-d' fit, RMS units
%
% fit.S1units   = contains same parameters in sd(S1) units.
%                 these may be of use since the data-fitting is conducted
%                 using parameters specified in sd(S1) units.
%
% fit.logL          = log likelihood of the data fit
%
% fit.est_HR2_rS1  = estimated (from meta-d' fit) type 2 hit rates for S1 responses
% fit.obs_HR2_rS1  = actual type 2 hit rates for S1 responses
% fit.est_FAR2_rS1 = estimated type 2 false alarm rates for S1 responses
% fit.obs_FAR2_rS1 = actual type 2 false alarm rates for S1 responses
%
% fit.est_HR2_rS2  = estimated type 2 hit rates for S2 responses
% fit.obs_HR2_rS2  = actual type 2 hit rates for S2 responses
% fit.est_FAR2_rS2 = estimated type 2 false alarm rates for S2 responses
% fit.obs_FAR2_rS2 = actual type 2 false alarm rates for S2 responses
%
% If there are N ratings, then there will be N-1 type 2 hit rates and false
% alarm rates.

% 2015/07/23 - fixed bug for output fit.meta_ca and fit.S1units.meta_c1.
%            - added comments to help section as well as a warning output
%              for nR_S1 or nR_S2 inputs containing zeros
% 2014/10/14 - updated discussion of "s" input in the help section above.
% 2010/09/07 - created

%% parse inputs

% check inputs
if ~mod(length(nR_S1),2)==0, error('input arrays must have an even number of elements'); end
if length(nR_S1)~=length(nR_S2), error('input arrays must have the same number of elements'); end

if any(nR_S1 == 0) || any(nR_S2 == 0)
    disp(' ')
    disp('WARNING!!')
    disp('---------')
    disp('Your inputs')
    disp(' ')
    disp(['nR_S1 = [' num2str(nR_S1) ']'])
    disp(['nR_S2 = [' num2str(nR_S2) ']'])
    disp(' ')
    disp('contain zeros! This may interfere with proper estimation of meta-d''.')
    disp('See ''help fit_meta_d_MLE'' for more information.')
    disp(' ')
    disp(' ')
end

% assign input default values
if ~exist('s','var') || isempty(s)
    s = 1;
end

if ~exist('fncdf','var') || isempty(fncdf)
    fncdf = @normcdf;
end

if ~exist('fninv','var') || isempty(fninv)
    fninv = @norminv;
end

nRatings = length(nR_S1) / 2;
nCriteria = 2*nRatings - 1;

%% set up constraints for MLE estimation

% parameters
% meta-d' - 1
% t2c     - nCriteria-1

A = [];
b = [];

% constrain type 2 criteria values,
% such that t2c(i) is always <= t2c(i+1)
% want t2c(i)   <= t2c(i+1)
% -->  t2c(i+1) >= c(i) + 1e-5 (i.e. very small deviation from equality)
% -->  t2c(i) - t2c(i+1) <= -1e-5
for i = 2 : nCriteria-1

    A(end+1,[i i+1]) = [1 -1];
    b(end+1) = -1e-5;

end

% lower bounds on parameters
LB = [];
LB = [LB -10];                         % meta-d'
LB = [LB -20*ones(1,(nCriteria-1)/2)]; % criteria lower than t1c
LB = [LB zeros(1,(nCriteria-1)/2)];    % criteria higher than t1c

% upper bounds on parameters
UB = [];
UB = [UB 10];                          % meta-d'
UB = [UB zeros(1,(nCriteria-1)/2)];    % criteria lower than t1c
UB = [UB 20*ones(1,(nCriteria-1)/2)];  % criteria higher than t1c

%% select constant criterion type

constant_criterion = 'meta_d1 * (t1c1 / d1)'; % relative criterion

%% set up initial guess at parameter values

ratingHR  = [];
ratingFAR = [];
for c = 2:nRatings*2
    ratingHR(end+1) = sum(nR_S2(c:end)) / sum(nR_S2);
    ratingFAR(end+1) = sum(nR_S1(c:end)) / sum(nR_S1);
end

t1_index = nRatings;
t2_index = setdiff(1:2*nRatings-1, t1_index);

d1 = (1/s) * fninv( ratingHR(t1_index) ) - fninv( ratingFAR(t1_index) );
meta_d1 = d1;
c = -0.5 .* (fninv(ratingHR(t1_index)) + fninv(ratingFAR(t1_index)));
c1 = (-1/(1+s)) * ( fninv( ratingHR ) + fninv( ratingFAR ) );
t1c1 = c1(t1_index);
t2c1 = c1(t2_index);

guess = [meta_d1 t2c1 - eval(constant_criterion)];

%% find the best fit for type 2 hits and FAs

% save fit_meta_d_MLE.mat nR_S1 nR_S2 t2FAR_rS2 t2HR_rS2 t2FAR_rS1 t2HR_rS1 nRatings t1c1 s d1 fncdf constant_criterion
save fit_meta_d_MLE.mat nR_S1 nR_S2 nRatings d1 t1c1 s constant_criterion fncdf fninv

op = optimset(@fmincon);
op = optimset(op,'MaxFunEvals',100000);

[x f] = fmincon(@fit_meta_d_logL,guess,A,b,[],[],LB,UB,[],op);

meta_d1  = x(1);
t2c1     = x(2:end) + eval(constant_criterion);
logL     = -f;

%% data is fit, now to package it...

%% find observed t2FAR and t2HR

% I_nR and C_nR are rating trial counts for incorrect and correct trials
% element i corresponds to # (in)correct w/ rating i
I_nR_rS2 = nR_S1(nRatings+1:end);
I_nR_rS1 = nR_S2(nRatings:-1:1);

C_nR_rS2 = nR_S2(nRatings+1:end);
C_nR_rS1 = nR_S1(nRatings:-1:1);

for i = 2:nRatings
    obs_FAR2_rS2(i-1) = sum( I_nR_rS2(i:end) ) / sum(I_nR_rS2);
    obs_HR2_rS2(i-1)  = sum( C_nR_rS2(i:end) ) / sum(C_nR_rS2);

    obs_FAR2_rS1(i-1) = sum( I_nR_rS1(i:end) ) / sum(I_nR_rS1);
    obs_HR2_rS1(i-1)  = sum( C_nR_rS1(i:end) ) / sum(C_nR_rS1);
end

%% find estimated t2FAR and t2HR

S1mu = -meta_d1/2; S1sd = 1;
S2mu =  meta_d1/2; S2sd = S1sd/s;

mt1c1 = eval(constant_criterion);

C_area_rS2 = 1-fncdf(mt1c1,S2mu,S2sd);
I_area_rS2 = 1-fncdf(mt1c1,S1mu,S1sd);

C_area_rS1 = fncdf(mt1c1,S1mu,S1sd);
I_area_rS1 = fncdf(mt1c1,S2mu,S2sd);

for i=1:nRatings-1

    t2c1_lower = t2c1(nRatings-i);
    t2c1_upper = t2c1(nRatings-1+i);

    I_FAR_area_rS2 = 1-fncdf(t2c1_upper,S1mu,S1sd);
    C_HR_area_rS2  = 1-fncdf(t2c1_upper,S2mu,S2sd);

    I_FAR_area_rS1 = fncdf(t2c1_lower,S2mu,S2sd);
    C_HR_area_rS1  = fncdf(t2c1_lower,S1mu,S1sd);

    est_FAR2_rS2(i) = I_FAR_area_rS2 / I_area_rS2;
    est_HR2_rS2(i)  = C_HR_area_rS2 / C_area_rS2;

    est_FAR2_rS1(i) = I_FAR_area_rS1 / I_area_rS1;
    est_HR2_rS1(i)  = C_HR_area_rS1 / C_area_rS1;

end

%% package output

fit.d1        = sqrt(2/(1+s^2)) * s * d1;
fit.c1        = c;
fit.s         = s;
fit.meta_d    = sqrt(2/(1+s^2)) * s * meta_d1;
fit.M_diff    = fit.meta_d - fit.d1;
fit.M_ratio   = fit.meta_d / fit.d1;

mt1c1         = eval(constant_criterion);
fit.meta_c1   = ( sqrt(2).*s ./ sqrt(1+s.^2) ) .* mt1c1;

t2ca          = ( sqrt(2).*s ./ sqrt(1+s.^2) ) .* t2c1;
fit.t2ca_rS1  = t2ca(1:nRatings-1);
fit.t2ca_rS2  = t2ca(nRatings:end);

fit.S1units.d1        = d1;
fit.S1units.meta_d1   = meta_d1;
fit.S1units.s         = s;
fit.S1units.meta_c1   = mt1c1;
fit.S1units.t2c1_rS1  = t2c1(1:nRatings-1);
fit.S1units.t2c1_rS2  = t2c1(nRatings:end);

fit.logL           = logL;

fit.est_HR2_rS1  = est_HR2_rS1;
fit.obs_HR2_rS1  = obs_HR2_rS1;

fit.est_FAR2_rS1 = est_FAR2_rS1;
fit.obs_FAR2_rS1 = obs_FAR2_rS1;

fit.est_HR2_rS2  = est_HR2_rS2;
fit.obs_HR2_rS2  = obs_HR2_rS2;

fit.est_FAR2_rS2 = est_FAR2_rS2;
fit.obs_FAR2_rS2 = obs_FAR2_rS2;

%% clean up
delete fit_meta_d_MLE.mat

end

%% function to find the likelihood of parameter values, given observed data
function logL = fit_meta_d_logL(parameters)

% set up parameters
meta_d1  = parameters(1);
t2c1     = parameters(2:end);

% loads:
% nR_S1 nR_S2 nRatings d1 t1c1 s constant_criterion fncdf fninv
load fit_meta_d_MLE.mat

% define mean and SD of S1 and S2 distributions
S1mu = -meta_d1/2; S1sd = 1;
S2mu =  meta_d1/2; S2sd = S1sd/s;

% adjust so that the type 1 criterion is set at 0
% (this is just to work with optimization toolbox constraints...
%  to simplify defining the upper and lower bounds of type 2 criteria)
S1mu = S1mu - eval(constant_criterion);
S2mu = S2mu - eval(constant_criterion);

t1c1 = 0;

%%% set up MLE analysis

% get type 2 response counts
for i = 1:nRatings

    % S1 responses
    nC_rS1(i) = nR_S1(i);
    nI_rS1(i) = nR_S2(i);

    % S2 responses
    nC_rS2(i) = nR_S2(nRatings+i);
    nI_rS2(i) = nR_S1(nRatings+i);

end

% get type 2 probabilities
C_area_rS1 = fncdf(t1c1,S1mu,S1sd);
I_area_rS1 = fncdf(t1c1,S2mu,S2sd);

C_area_rS2 = 1-fncdf(t1c1,S2mu,S2sd);
I_area_rS2 = 1-fncdf(t1c1,S1mu,S1sd);

t2c1x = [-Inf t2c1(1:nRatings-1) t1c1 t2c1(nRatings:end) Inf];

for i = 1:nRatings
    prC_rS1(i) = ( fncdf(t2c1x(i+1),S1mu,S1sd) - fncdf(t2c1x(i),S1mu,S1sd) ) / C_area_rS1;
    prI_rS1(i) = ( fncdf(t2c1x(i+1),S2mu,S2sd) - fncdf(t2c1x(i),S2mu,S2sd) ) / I_area_rS1;

    prC_rS2(i) = ( (1-fncdf(t2c1x(nRatings+i),S2mu,S2sd)) - (1-fncdf(t2c1x(nRatings+i+1),S2mu,S2sd)) ) / C_area_rS2;
    prI_rS2(i) = ( (1-fncdf(t2c1x(nRatings+i),S1mu,S1sd)) - (1-fncdf(t2c1x(nRatings+i+1),S1mu,S1sd)) ) / I_area_rS2;
end

% calculate logL
logL = 0;
for i = 1:nRatings

    logL = logL + nC_rS1(i)*log(prC_rS1(i)) + nI_rS1(i)*log(prI_rS1(i)) + ...
                  nC_rS2(i)*log(prC_rS2(i)) + nI_rS2(i)*log(prI_rS2(i));

end

if isnan(logL), logL=-Inf; end

logL = -logL;

end


function [ gpos ] = point_image_to_ground( im_points, sicd_meta, varargin )
%POINT_IMAGE_TO_GROUND Transforms pixel row, col to ground ECF coordinate
% This function implements the SICD Image Projections Description Document:
% http://www.gwg.nga.mil/ntb/baseline/docs/SICD/index.html
%
% gpos = point_image_to_ground(im_points, sicd_meta, 'PropertyName', PropertyValue, ...)
%
% Inputs:
%    im_points   - [2xN] (row; column) coordinates of N points in image (or
%                  subimage if FirstRow/FirstCol are nonzero).  Zero-based,
%                  following SICD convention (rather than MATLAB
%                  convention, which is one-based); that is, upper-left
%                  pixel is [0;0].
%    sicd_meta   - SICD meta data structure
%
%       Property name     Description
%       projection_type   'plane', 'hae', 'dem', 'best' (try DEM first
%                         then HAE).  Default plane.
%       gref              Ground plane reference point ECF coordinates (m).
%                         Default is SCP.  Only valid if projection_type is
%                         'plane'.
%       ugpn              Ground plane unit normal vector.  Default is
%                         tangent to the surface of constant geodetic
%                         height above the WGS-84 reference ellipsoid
%                         passing through GREF.  Only valid if
%                         projection_type is 'plane'.
%       hae0              Surface height (m) above the WGS-84 reference
%                         ellipsoid for projection point.  Only valid if
%                         projection_type is 'hae'.
%       delta_hae_max     Height threshold for convergence of iterative
%                         constant HAE computation (m).  Default 1 meter.
%                         Only valid if projection_type is 'hae'.
%       hae_nlim          Maximum number of iterations allowed for constant
%                         hae computation.  Default 3.  Only valid if
%                         projection_type is 'hae'.
%       dem               SRTM pathname or structure with
%                         lats/lons/elevations fields where are all are
%                         arrays of same size and elevation is height above
%                         WGS-84 ellipsoid.
%                         Only valid if projection_type is 'dem'.
%       del_DISTrrc       Maximum distance between adjacent points along
%                         the R/Rdot contour. Recommended value: 10.0 m.
%                         Only valid if projection_type is 'dem'.
%       del_HDlim         Height difference threshold for determining if a
%                         point on the R/Rdot contour is on the DEM
%                         surface (m).  Recommended value: 0.001 m.  Only
%                         valid if projection_type is 'dem'.
%       delta_arp         ARP position adjustable parameter (ECF, m).  Default 0.
%       delta_varp        VARP position adjustable parameter (ECF, m/s).  Default 0.
%       range_bias        Range bias adjustable parameter (m).  Default 0.
%       adj_params_frame  Coordinate frame used for expressing delta_arp
%                         and delta_varp adjustable parameters.  Allowed
%                         values: 'ECF', 'RIC_ECF', 'RIC_ECI'. Default ECF.
%
% Outputs:
%    gpos        - [3xN] ECF Ground Points along the R/Rdot contour
%
% Contributors: Thomas McDowall, Harris Corporation
%               Lisa Talbot, NGA/IB
%               Rocco Corsetti, NGA/IB
%               Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

%% Parse input parameters
if isvector(im_points), im_points = im_points(:); end % Assure orientation
scp = [sicd_meta.GeoData.SCP.ECF.X; ...
    sicd_meta.GeoData.SCP.ECF.Y; ...
    sicd_meta.GeoData.SCP.ECF.Z]; % Default values for some input arguments require this
scp_llh = ecf_to_geodetic(scp);  % Also should be available in sicd_meta.GeoData.SCP.LLH
scp_hae = scp_llh(3);
p = inputParser;
p.addParamValue('projection_type','plane', @(x) any(strcmp(x,{'plane','hae','hae_newton','dem','best'})));
p.addParamValue('gref',scp, @(x) (size(x,1)==3)||(numel(x)==3)); % ECF (meters)
if isfield(sicd_meta,'PFA')&&isfield(sicd_meta.PFA,'FPN')
    default_ugpn=[sicd_meta.PFA.FPN.X; sicd_meta.PFA.FPN.Y; sicd_meta.PFA.FPN.Z];
else
    default_ugpn=wgs_84_norm(scp);
end
p.addParamValue('ugpn',default_ugpn, @(x) (size(x,1)==3)||(numel(x)==3)); % ECF
p.addParamValue('hae0',scp_hae, @isscalar); % meters
p.addParamValue('delta_hae_max', 1, @(x) isscalar(x) && (x>0)); % meters
p.addParamValue('hae_nlim', 3, @isscalar);
p.addParamValue('dem',[]);
p.addParamValue('del_DISTrrc', 10, @isscalar);
p.addParamValue('del_HDlim', 0.001, @isscalar);
% Adjustable parameters
p.addParamValue('delta_arp',[0 0 0], @(x) numel(x)==3); % ECF? (meters)
p.addParamValue('delta_varp',[0 0 0], @(x) numel(x)==3); % ECF? (meters/s)
p.addParamValue('range_bias',0, @isscalar); % meters
p.addParamValue('adj_params_frame','ECF', @(x) any(strcmpi(x,{'ECF','RIC_ECF','RIC_ECI'})));
p.FunctionName = mfilename;
p.parse(varargin{:});

%% Chapter 4: Compute projection parameters
try
    % Computation of r/rdot is specific to image formation type
    [r, rdot, arp_coa, varp_coa] = coa_projection_set(sicd_meta, im_points);
catch % If full metadata not available, try to make the approximation of uniformly sampled grid
    sicd_meta.Grid.Type = 'PLANE';
    if ~isfield(sicd_meta.Grid, 'TimeCOAPoly') % Another approximation that may have to be made
        sicd_meta.Grid.TimeCOAPoly = sicd_meta.Timeline.CollectDuration/2;
    end
    [r, rdot, arp_coa, varp_coa] = coa_projection_set(sicd_meta, im_points);
    warning('point_image_to_ground:IncompleteMetadata',...
        'Unable to compute precise position due to incomplete metadata.  Resorting to approximation.');
end
% After r/rdot is computed, the rest is generic to all SAR.

%% Apply adjustable parameters
if strcmpi(p.Results.adj_params_frame,'ECF')
    % No transformation necessary
    delta_arp = p.Results.delta_arp(:);
    delta_varp = p.Results.delta_varp(:);
else % Translate from RIC frame to ECF frame
    % Use the RIC frame at SCP COA time, not at COA time for im_points
    ARP_SCP_COA = [sicd_meta.SCPCOA.ARPPos.X; ...
        sicd_meta.SCPCOA.ARPPos.Y; ...
        sicd_meta.SCPCOA.ARPPos.Z];
    VARP_SCP_COA = [sicd_meta.SCPCOA.ARPVel.X; ...
        sicd_meta.SCPCOA.ARPVel.Y; ...
        sicd_meta.SCPCOA.ARPVel.Z];
    if strcmpi(p.Results.adj_params_frame,'RIC_ECI')
        T_ECEF_RIC = ric_ecf_mat(ARP_SCP_COA, VARP_SCP_COA, 'eci');
    else % RIC_ECF
        T_ECEF_RIC = ric_ecf_mat(ARP_SCP_COA, VARP_SCP_COA, 'ecf');
    end
    delta_arp = T_ECEF_RIC * p.Results.delta_arp(:);
    delta_varp = T_ECEF_RIC * p.Results.delta_varp(:);
end
arp_coa = arp_coa + repmat(delta_arp,1,size(arp_coa,2));
varp_coa = varp_coa + repmat(delta_varp,1,size(arp_coa,2));
r = r + p.Results.range_bias;

%% Perform actual projection
switch p.Results.projection_type
    case 'plane' % Chapter 5 of SICD document
        gpos = point_to_ground_plane(r, rdot, arp_coa, varp_coa, ...
            p.Results.gref, p.Results.ugpn);
    case 'hae' % Chapter 9 of SICD projection document (draft)
        gpos = point_to_hae(r, rdot, arp_coa, varp_coa, scp, ...
            p.Results.hae0, p.Results.delta_hae_max, p.Results.hae_nlim);
    case 'hae_newton' % Spotlight Synthetic Aperture Radar (SAR) Sensor Model, NGA.SIG.0005_1.0, 2010-03-30
        if ~strcmp(sicd_meta.CollectionInfo.RadarMode.ModeType,'SPOTLIGHT')
            error('point_image_to_ground:invalid_projection_mode','HAE_NEWTON projection method only valid for spotlight data.');
        end
        gpos = point_to_hae_newton(r, rdot, arp_coa, varp_coa, scp, ...
            p.Results.hae0);
    case 'dem' % Chapter 10 of SICD document (draft)
        gpos = point_to_DEM(r, rdot, arp_coa, varp_coa, scp, ...
            p.Results.delta_hae_max, p.Results.hae_nlim, p.Results.dem, ...
            p.Results.del_DISTrrc, p.Results.del_HDlim);
    case 'best' % Best available.  Use DEM if valid, otherwise use HAE
        try
            gpos = point_to_DEM(r, rdot, arp_coa, varp_coa, scp, ...
                p.Results.delta_hae_max, p.Results.hae_nlim, p.Results.dem, ...
                p.Results.del_DISTrrc, p.Results.del_HDlim);
        catch
            warning('point_image_to_ground:invalid_dem','Best projection reverted to HAE.');
            gpos = point_to_hae(r, rdot, arp_coa, varp_coa, scp, ...
                p.Results.hae0, p.Results.delta_hae_max, p.Results.hae_nlim);
        end
    otherwise
        % Unrecognized projection type
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


function paracellularAbsorption = calculateParacellularAbsorption(DrugLogP, DrugMolecularWeight)
% calculate the paracellular absorption rate using the heuristics presented
% in Eq. 2 in:
% Peters, S. A. (2008). Evaluation of a generic physiologically based pharmacokinetic model for lineshape analysis. Clinical pharmacokinetics, 47(4), 261-75.
% Copyright 2012 The MathWorks, Inc.

paracellularAbsorption = 0;
if DrugLogP > 0.7,
    paracellularAbsorption = 0;
elseif DrugLogP < 0.1 && (DrugMolecularWeight > 200 && DrugMolecularWeight < 360),
    paracellularAbsorption = 0.1;
elseif DrugMolecularWeight < 200,
    paracellularAbsorption = -0.0045*DrugMolecularWeight+1;
end

%paracellularAbsorption = paracellularAbsorption*5;


classdef MMF5 < PROBLEM
% <multi> <real> <multimodal>
% Multi-modal multi-objective test function

%------------------------------- Reference --------------------------------
% C. Yue, B. Qu, and J. Liang, A multi-objective particle swarm optimizer
% using ring topology for solving multimodal multiobjective Problems, IEEE
% Transactions on Evolutionary Computation, 2018, 22(5): 805-817.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        POS;    % Pareto optimal set for IGDX calculation
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            obj.D = 2;
            obj.lower    = [1,-1];
            obj.upper    = [3,3];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            temp = X(:,2) <= 1;
            y    = zeros(size(X,1),1);
            y(temp)  = X(temp,2) - sin(6*pi*abs(X(temp,1)-2)+pi);
            y(~temp) = X(~temp,2) - 2 - sin(6*pi*abs(X(~temp,1)-2)+pi);
            PopObj(:,1) = abs(X(:,1)-2);
            PopObj(:,2) = 1 - sqrt(PopObj(:,1)) + 2*y.^2;
        end
        %% Generate Pareto optimal solutions
        function R = GetOptimum(obj,N)
            % Generate points in Pareto optimal set
            obj.POS(:,1) = linspace(1,3,N/2)';
            obj.POS(:,2) = sin(6*pi*abs(obj.POS(:,1)-2)+pi);
            obj.POS = [obj.POS;obj.POS(:,1),obj.POS(:,2)+2];
            % Generate points on Pareto front
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R(:,1) = linspace(0,1,100)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case 'IGDX'
                    score = feval(metName,Population,obj.POS);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Display a population in the objective space
        function DrawObj(obj,Population)
            PopDec = Population.decs;
            temp1  = PopDec(:,1)<=2;
            temp2  = PopDec(:,2)<=1;
            Draw(Population(temp1&temp2).objs,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 .5],'Markeredgecolor',[1 .2 .2],{'\it f\rm_1','\it f\rm_2',[]});
            Draw(Population(temp1&~temp2).objs+0.05,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 .5 1],'Markeredgecolor',[.2 .2 1]);
            Draw(Population(~temp1&temp2).objs+0.1,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[.5 1 .5],'Markeredgecolor',[.2 1 .2]);
            Draw(Population(~temp1&~temp2).objs+0.15,'o','MarkerSize',6,'Marker','o','Markerfacecolor',[1 .5 1],'Markeredgecolor',[1 .2 1]);
            Draw(obj.PF,'-','LineWidth',1,'Color',[1 .2 .2]);
            Draw(obj.PF+0.05,'-','LineWidth',1,'Color',[.2 .2 1]);
            Draw(obj.PF+0.1,'-','LineWidth',1,'Color',[.2 1 .2]);
            Draw(obj.PF+0.15,'-','LineWidth',1,'Color',[1 .2 1]);
        end
    end
end


function c = tapas_hgf_ar1_binary_mab_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF) for AR(1) processes in a
% multi-armded bandit situation for binary inputs in the absence of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_hgf_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% The phis are estimated in 'logit space' because they are confined to the interval from 0 to 1.
% 'Logit-space' is a logistic sigmoid transformation of native space with a variable upper bound
% a>0:
%
% tapas_logit(x) = ln(x/(a-x)); x = a/(1+exp(-tapas_logit(x)))
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_binary_mab_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.phi        row vector of phis (representing reversion slope to attractor; in ascending order of levels)
%         est.p_prc.m          row vector of ms (representing attractors; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels, 3rd dim: bandits)
%         est.traj.sa          sigma (rows: trials, columns: levels, 3rd dim: bandits)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels, 3rd dim: bandits)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels, 3rd dim: bandits)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and responses y and use
%
%   >> est = tapas_fitModel(y, u, 'tapas_hgf_ar1_binary_mab_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013-2017 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'hgf_ar1_binary_mab';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Number of bandits
c.n_bandits = 3;

% Coupling
% This may only be set to true if c.n_bandits is set to 2 above. If
% true, it means that the two bandits' winning probabilities are
% coupled in the sense that they add to 1 and are both updated on
% each trial even though only the outcome for one of them is observed.
c.coupled = false;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 1, 1];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          1,      1];

% Phis
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero (-Inf in logit space) to set to zero.
c.logitphimu = [NaN, tapas_logit(0.4,1), tapas_logit(0.2,1)];
c.logitphisa = [NaN,                  1,                   1];

% ms
% Format: row vector of length n_levels.
% This should be fixed for all levels where the omega of
% the next lowest level is not fixed because that offers
% an alternative parametrization of the same model.
c.mmu = [NaN, c.mu_0mu(2), c.mu_0mu(3)];
c.msa = [NaN,           0,           0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,    0.1];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,   -2,   -2];
c.omsa = [NaN,    1,    1];

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.logitphimu,...
    c.mmu,...
    c.logkamu,...
    c.ommu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.logitphisa,...
    c.msa,...
    c.logkasa,...
    c.omsa,...
         ];

% Check whether we have the right number of priors
expectedLength = 5*c.n_levels+(c.n_levels-1);
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_hgf_ar1_binary_mab;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_hgf_ar1_binary_mab_transp;

return;


classdef cheboppref < chebpref
%CHEBOPPREF   Class for managing preferences for the Chebfun ODE suite.
%   CHEBOPPREF is a class for managing CHEBOP construction-time and solver
%   preferences, such as what solver is used for linear problem, the error or
%   residual tolerance for nonlinear problems, whether damped Newton iteration
%   should be performed for nonlinear problems, and how much information is to
%   be printed to the console during while the solver is active.
%
% Available Preferences:
%
%   bvpTol                      - Error tolerance for boundary value problems
%     [5e-13]
%
%     This is the relative tolerance used to test convergence during the
%     adaptive solution of linear boundary value problems. For nonlinear
%     boundary value problems the Newton convergence tolerance is set to
%     200*bvpTol.
%
%   domain                     - Construction domain.
%     [-1, 1]
%
%     This sets the default domain that will be used for CHEBOP construction if
%     no domain argument is explicitly passed to the constructor.
%
%   discretization             - Discretization of linear problems
%     ['values']
%     'coeffs'
%     @chebcolloc1
%     @chebcolloc2
%     @ultraS
%     @trigcolloc
%     @trigspec
%
%     This options determines whether linear operators are discretized using
%     rectangular collocation methods or the ultraspherical method. Please
%     observe that
%         * 'values' and 'coeffs' are convenient ways of specifying the
%           @chebcolloc2 and @ultraS options respectively (when the boundary
%           conditions are not periodic), and @trigcolloc and @trigspec
%           respectively (when the boundary conditions are periodic).
%         * The @trigcolloc/@trigspec options are only supported for problems
%           that are specified to have periodic boundary conditions.
%         * Specifying the @chebcolloc1 option causes the CHEBFUN solution
%           returned to be based on the @chebtech1 tech. The @chebtech2/@ultraS
%           option causes the CHEBFUN solution returned to be based on the
%           @chebtech2 tech. The @trigcolloc/@trigspec option causes the
%           CHEBFUN solution to be periodic, based on the @trigtech tech.
%
%   damping                     - Should Newton's method be damped?
%     [true]
%     false
%
%     If true, damped Newton iteration in function space is performed for
%     nonlinear problems. If false, undamped Newton iteration is performed, that
%     is, the solver will always take full Newton steps.
%
%   display                     - How much information is to be printed
%     'final'
%     ['iter']
%      'off'
%
%     If 'final', information is only printed after the solver of BVPs has
%     finished. If 'iter', information is printed at every Newton step. If
%     'off', no information is printed.
%
%   happinessCheck              - Routine for checking that solution converged
%     [@standardCheck]
%     @basicCheck
%     @plateauCheck
%     @classicCheck
%     @looseCheck
%     @strictCheck
%     @happinessCheck
%     @linopV4Check
%
%     This options determines which routine is used to determine that the
%     approximate solution has converged. Any of the above options may be
%     used, as well as any user defined function handle that conforms to
%     the happinessCheck standards.
%
%   ivpAbsTol                    - Absolute tolerance for the ivpSolver
%     [1e5*eps]
%
%     This options specifies the option for the absolute tolerance passed as an
%     option to the built-in MATLAB ODE solver when solving IVPs.
%
%   ivpRelTol                    - Relavtive tolerance for the ivpSolver
%     [100*eps]
%
%     This options specifies the option for the relative tolerance passed as an
%     option to the built-in MATLAB ODE solver when solving IVPs.
%
%   ivpRestartSolver             - Restart IVP solvers at breakpoints
%     false
%     [true]
%
%     This option specifies whether the MATLAB built in solvers should be
%     restarted at breakpoints. That is, whether each subinterval of a piecewise
%     problem will get integrated separately. This can be very useful for e.g.
%     short forcing pulses, which otherwise might get overlooked.
%
%   ivpSolver                  - Solver for IVPs
%     ['ode113']
%     'ode15s'
%     'ode45'
%     'values'
%     'coeffs'
%
%     This options determines which of the MATLAB built-in IVP solvers is used
%     for solving IVPs posed with the CHEBOP class. Any option of
%     CHEBOPPREF.discretization (see above) is allowed, which causes IVPs to be
%     solved globally via spectral methods, rather than reformulating them as
%     first-order problems and then solved via time-stepping method.
%
%   lambdaMin                   - Minimum allowed step-size for Newton's method
%     [1e-6]
%
%     The value of lambdaMin determines the minimum allowed step-size that the
%     damped Newton iteration is allowed to take.
%
%   maxDimension
%     [4096]
%
%     The maximum number of gridpoints/coefficients used as linear operators are
%     discretized at finer and finer grids to resolve the solution. The
%     intermediate values for the discretization between cheboppref.minDimension
%     and cheboppref.maxDimension depend on the discretization used for the
%     operator.
%
%   maxIter                     - Maximum number of Newton steps
%     [25]
%
%   The maximum number of steps that the (damped) Newton iteration is allowed to
%   take, before it is considered to be non-convergent.
%
%   minDimension
%     [32]
%
%     The minimum number of gridpoints/coefficients used as linear operators are
%     discretized at finer and finer grids to resolve the solution. The
%     intermediate values for the discretization between cheboppref.minDimension
%     and cheboppref.maxDimension depend on the discretization used for the
%     operator.
%
%   plotting                    - Plotting of intermediate Newton steps
%     DELAY
%     'on'
%     ['off']
%     'pause'
%
%   If plotting = 'on', the current iterate in the Newton solution is plotted at
%   every step, as well as the current Newton correction. If plotting = DELAY,
%   where DELAY has a numerical value, the iteration is paused and the plots are
%   shown for the time DELAY seconds. If plotting = 'pause', the iteration is
%   paused and the plots are shown until the user presses a button. If plotting
%   = 'off', no plots are shown during the Newton iteration.
%
%   vectorize                   - Automatic vectorization of anon. functions
%     [true]
%     false
%
%   Determines whether the CHEBOP class should try to automatically try to
%   vectorize anonymous functions used for describing the differential equation
%   and boundary condition(s).
%
%
% The default values for any of these preferences may be globally overridden
% using CHEBOPPREF.SETDEFAULTS(); see the documentation for that function for
% further details.
%
% Constructor inputs:
%   P = CHEBOPPREF() creates a CHEBOPPREF object with the default values of the
%   preferences.  For a list of all available preferences, see above.
%
%   P = CHEBOPPREF(Q), where Q is a MATLAB structure uses the field/value pairs
%   in Q to set the properties of P.  If a field of Q has a name which matches
%   a property of P, the value of that property of P is set to the value
%   associated to that field in Q.  If a field of Q has a name that does not
%   correspond to a known preference, then an error is thrown.
%
%   P = CHEBOPPREF(Q), where Q is a CHEBOPPREF, sets P to be a copy of Q.
%
% See also CHEBFUNPREF.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% TODO:  Further documentation of CHEBOPPREF preferences.

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

        function outPref = cheboppref(inPref, varargin)
            if ( (nargin == 1) && isa(inPref, 'cheboppref') )
                outPref = inPref;
                return
            elseif ( nargin < 1 )
                inPref = struct();
            elseif ( ischar(inPref) )
                if ( nargin == 1 )
                    error('CHEBFUN:CHEBOPPREF:cheboppref:deprecated', ...
                        ['cheboppref() no longer supports queries of ', ...
                         'the form cheboppref(''prop'').\n', ...
                         'Please use cheboppref().prop.']);
                else
                    error('CHEBFUN:CHEBOPPREF:cheboppref:deprecated', ...
                        ['cheboppref() no longer supports assignment ', ...
                         'via cheboppref(''prop'', val).\n', ...
                         'Please use cheboppref.setDefaults(''prop'', val).']);
                end
            elseif ( nargin > 1 )
                error('CHEBFUN:CHEBOPPREF:cheboppref:inputs', ...
                    'Too many input arguments.')
            end

            % Initialize default preference values.
            outPref.prefList = cheboppref.manageDefaultPrefs('get');

            % Copy fields from q, merging incomplete substructures.
            for field = fieldnames(inPref).'
                field1 = field{1};
                if ( isfield(outPref.prefList, field1) )
                    if ( isstruct(outPref.prefList.(field1)) )
                        outPref.prefList.(field1) = ...
                            chebpref.mergePrefStructs(...
                                outPref.prefList.(field1), ...
                                inPref.(field1));
                    else
                        outPref.prefList.(field1) = inPref.(field1);
                    end
                else
                    error('CHEBFUN:CHEBOPPREF:cheboppref:badPref', ...
                        'Unrecognized preference name.');
                end
            end
        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

       function display(pref)
       %DISPLAY   Display a CHEBOPPREF object.
       %   DISPLAY(PREF) prints out a list of the preferences stored in the
       %   CHEBOPPREF object PREF.

            % Compute the screen column in which pref values start.
            valueCol = 24; % length('    blowup:   ');

            % A subfunction to pad strings for formatting.
            function s = padString(s)
            %PADSTRING   Add whitespace to string for formatting.
                s = [s repmat(' ', 1, valueCol - length(s))];
            end

            % Print values of "known" preferences.
            prefList = pref.prefList;

            fprintf('cheboppref object with the following preferences:\n');
            fprintf([padString('    bvpTol:') '%g\n'], ...
                prefList.bvpTol);
            fprintf([padString('    domain:') '[%g, %g]\n'], ...
                prefList.domain(1), prefList.domain(end));
            if ( isa(prefList.discretization,'function_handle') )
                fprintf([padString('    discretization:') '%s\n'], ...
                    func2str(prefList.discretization));
            elseif ( isa(prefList.discretization,'char') )
                fprintf([padString('    discretization:') '%s\n'], ...
                    prefList.discretization);
            end

            fprintf([padString('    damping:') '%d\n'], ...
                prefList.damping);
            fprintf([padString('    display:') '%s\n'], ...
                prefList.display);
            fprintf([padString('    happinessCheck:') '%s\n'], ...
                func2str(prefList.happinessCheck));
            fprintf([padString('    ivpAbsTol:') '%g\n'], ...
                prefList.ivpAbsTol);
            fprintf([padString('    ivpRelTol:') '%g\n'], ...
                prefList.ivpRelTol);
            fprintf([padString('    ivpRestartSolver:') '%d\n'], ...
                prefList.ivpRestartSolver);
            fprintf([padString('    ivpSolver:') '%s\n'], ...
                func2str(prefList.ivpSolver));
            fprintf([padString('    lambdaMin:') '%g\n'], ...
                prefList.lambdaMin);
            fprintf([padString('    maxDimension:') '%d\n'], ...
                prefList.maxDimension);
            fprintf([padString('    maxIter:') '%d\n'], ...
                prefList.maxIter);
            fprintf([padString('    minDimension:') '%d\n'], ...
                prefList.minDimension);
            fprintf([padString('    plotting:') '%s\n'], ...
                prefList.plotting);
            fprintf([padString('    vectorize:') '%i\n'], ...
                prefList.vectorize);
       end

        function pref = subsasgn(pref, ind, val)
        %SUBSASGN   Subscripted assignment for CHEBOPPREF.
        %   P.PROP = VAL, where P is a CHEBOPPREF object, assigns the value
        %   VAL to the CHEBOPPREF property PROP stored in P.  If PROP is not a
        %   CHEBOPPREF property, an error will be thrown.
        %
        %   CHEBOPPREF does not support any other subscripted assignment types,
        %   including '()' and '{}'.

            % Support user-friendlier syntax for specifying discretization
            % choice:
            val = cheboppref.parseDiscretization(val);

            % Support user-friendlier syntax for specifying IVP solver choice:
            val = cheboppref.parseIVPsolver(val);

            % Call the superclass method.
            pref = subsasgn@chebpref(pref, ind, val);
        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )

        function pref = getFactoryDefaults()
        %GETFACTORYDEFAULTS   Get factory default preferences.
        %   PREF = CHEBOPPREF.GETFACTORYDEFAULTS() returns a CHEBOPPREF
        %   object with the preferences set to their factory defaults,
        %   irrespective of the currently defined values of the default
        %   preferences.  This function is useful if the user wishes to
        %   solve ODEs with CHEBOP using the factory defaults when other
        %   user-set defaults are currently in force.
        %
        % See also SETDEFAULTS.

            fd = cheboppref.factoryDefaultPrefs();
            pref = cheboppref(fd);
        end

        function setDefaults(varargin)
        %SETDEFAULTS   Set default preferences.
        %   CHEBOPPREF.SETDEFAULTS(PREF1, VAL1, PREF2, VAL2, ...) sets the
        %   default values for the preferences whose names are stored in the
        %   strings PREF1, PREF2, ..., etc. to VAL1, VAL2, ..., etc.  All
        %   subsequently constructed CHEBOPPREF objects will use these values
        %   as the defaults.
        %
        %   CHEBOPPREF.SETDEFAULTS(PREF) sets the default values to the
        %   preferences stored in the CHEBOPPREF object PREF.  PREF can also
        %   be a MATLAB structure, in which case it is converted to a
        %   CHEBOPPREF as described in the documentation for the CHEBOPPREF
        %   constructor first.
        %
        %   CHEBOPPREF.SETDEFAULTS('factory') resets the default preferences to
        %   their factory values.
        %
        % See also GETFACTORYDEFAULTS.

            % The reason we don't just use manageDefaults as the second
            % argument to chebpref.setDefaults and wrap it in an additional
            % anonymous function instead is to get around what seems to be a
            % bug in MATLAB.  See commit messages for more information.
            manageDefaults = @cheboppref.manageDefaultPrefs;
            chebpref.setDefaults(@(inPref) cheboppref(inPref), ...
                @(varargin) manageDefaults(varargin{:}), varargin{:});
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% PRIVATE STATIC METHODS
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Static = true, Access = private )

        function varargout = manageDefaultPrefs(varargin)
        %MANAGEDEFAULTPREFS   Private method for handling default preferences.
        %   CHEBOPPREF.MANAGEDEFAULTPREFS('get') returns a structure suitable
        %   for storing in the prefList property of a CHEBOPPREF with all of
        %   the currently stored default preferences suitable for initializing
        %   a CHEBOPPREF object.
        %
        %   CHEBOPPREF.MANAGEDEFAULTPREFS('set-factory') restores the default
        %   preferences to their "factory" values.
        %
        %   CHEBOPPREF.MANAGEDEFAULTPREFS('set', PREFLIST) sets the default
        %   values to those stored in the structure PREFLIST.  PREFLIST should
        %   be a structure suitable for use as a CHEBOPPREF prefList.
        %
        %   CHEBOPPREF.MANAGEDEFAULTPREFS('set', PREF1, VAL1, PREF2, VAL2, ...)
        %   sets the default values for PREF1, PREF2, ..., etc. to VAL1, VAL2,
        %   ..., etc.

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % DEVELOPER NOTE:
        %  - MATLAB has no equivalent to what might be called a "static" class
        %    variable in other languages, so a persistent variable is the best
        %    we can do for providing this feature.  Persistent variables are
        %    local to a specific function, so we can have only a single
        %    function for managing it.  As a result, this function has a mildly
        %    awkward syntax and so is not user-facing.
        %  - More importantly, this function is also not user-facing because
        %    its inputs and outputs depend on the internal representation of a
        %    CHEBOPPREF as a MATLAB structure, and that's not something with
        %    which anyone outside of CHEBOPPREF should be concerned.
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

            persistent defaultPrefs;

            if ( isempty(defaultPrefs) )
                defaultPrefs = cheboppref.factoryDefaultPrefs();
            end

            if ( strcmp(varargin{1}, 'get') )
                varargout{1} = defaultPrefs;
            elseif ( strcmp(varargin{1}, 'set-factory') )
                defaultPrefs = cheboppref.factoryDefaultPrefs();
            elseif ( strcmp(varargin{1}, 'set') )
                    varargin(1) = [];
                if ( isstruct(varargin{1}) )
                    defaultPrefs = varargin{1};
                else
                    while ( ~isempty(varargin) )
                        prefName = varargin{1};
                        prefValue = varargin{2};

                        % Support user-friendlier syntax for specifying
                        % discretization choice:
                        prefValue = cheboppref.parseDiscretization(prefValue);
                        prefValue = cheboppref.parseHappinessCheck(prefValue);
                        prefValue = cheboppref.parseIVPsolver(prefValue);
                        if ( isfield(defaultPrefs, prefName) )
                            defaultPrefs.(prefName) = prefValue;
                        else
                            error('CHEBFUN:CHEBOPPREF:cheboppref:badPref', ...
                                'Unrecognized preference name.');
                        end
                        varargin(1:2) = [];
                    end
                end
            end
        end

        function factoryPrefs = factoryDefaultPrefs()
        %FACTORYDEFAULTPREFS   Get structure of factory default preferences.
        %   S = CHEBOPPREF.FACTORYDEFAULTPREFS() returns a structure suitable
        %   for storing in the prefList property of a CHEBOPPREF object that
        %   contains all of the "factory default" values of the CHEBOP
        %   preferences.

            factoryPrefs.bvpTol = 5e-13;
            factoryPrefs.domain = [-1 1];
            factoryPrefs.discretization = 'values';
            factoryPrefs.scale = NaN;
            factoryPrefs.damping = 1;
            factoryPrefs.display = 'off';
            factoryPrefs.happinessCheck = @standardCheck;
            factoryPrefs.ivpAbsTol = 1e5*eps;
            factoryPrefs.ivpRelTol = 100*eps;
            factoryPrefs.ivpRestartSolver = true;
            factoryPrefs.ivpSolver = @chebfun.ode113;
            factoryPrefs.lambdaMin = 1e-6;
            factoryPrefs.maxDimension = 4096;
            factoryPrefs.maxIter = 25;
            factoryPrefs.minDimension = 32;
            factoryPrefs.plotting = 'off';
            factoryPrefs.vectorize = true;
        end

        function val = parseDiscretization(val)
        %PARSEDISCRETIZATION    Allow different syntax for specifying
        %                       discretization.

            % We want to allow user-friendly syntax for specifying the
            % discretization (#433). So check whether we have some of the
            % strings we want to allow, and convert them to the correct function
            % handle:
            if ( any(strcmpi(val, {'ultraspherical', 'ultraS'})) )
                warning('CHEBOPPREF:PARSEDISCRETIZATION', ...
                    ['''ULTRAS''/''ULTRASPHERICAL'' is deprecated. \n' ...
                    'Please use ''COEFFS''/@ultraS.']);
                val = @ultraS;

            elseif ( any(strcmpi(val, {'chebcolloc2', 'collocation', 'colloc2'})) )
                warning('CHEBOPPREF:PARSEDISCRETIZATION', ...
                    ['''COLLOCATION''/''COLLOC2''/''CHEBCOLLOC2'' is deprecated. \n' ...
                    'Please use ''VALUES''/@chebcolloc2.']);
                val = @chebcolloc2;

            elseif ( any(strcmpi(val, {'chebcolloc1', 'colloc1'})) )
                warning('CHEBOPPREF:PARSEDISCRETIZATION', ...
                    ['''COLLOC1''/''CHEBCOLLOC1'' is deprecated. \n' ...
                    'Please use ''VALUES''/@chebcolloc2.']);
                val = @chebcolloc1;

            elseif ( any(strcmpi(val, {'trigcolloc', 'periodic'})) )
                warning('CHEBOPPREF:PARSEDISCRETIZATION', ...
                    ['''TRIGCOLLOC''/''PERIODIC'' is deprecated. \n' ...
                    'Please use ''VALUES''/@trigcolloc.']);
                val = @trigcolloc;
            end

        end

        function val = parseHappinessCheck(val)
        %PARSEHAPPINESSCHECK    Allow different syntax for specifying
        %                       happinessCheck.

            % handle:
            if ( any(strcmpi(val, {'classic', 'classicCheck'})) )
                val = @classicCheck;

            elseif ( any(strcmpi(val, {'plateau', 'plateauCheck'})) )
                val = @plateauCheck;

            elseif ( any(strcmpi(val, {'strict', 'strictCheck'})) )
                val = @strictCheck;

            elseif ( any(strcmpi(val, {'loose', 'looseCheck'})) )
                val = @looseCheck;

            elseif ( any(strcmpi(val, {'happiness', 'happinessCheck'})) )
                val = @happinessCheck;

            elseif ( any(strcmpi(val, {'linopV4', 'linopV4Check'})) )
                val = @linopV4Check;

            end

        end

        function val = parseIVPsolver(val)
        %PARSEIVPSOLVER   Allow different syntax for specifying the IVPsolver.

            % Check whether we got pref.ivpSolver = @ode113/@ode45/@ode15s, that
            % is, a function handle, but not the CHEBFUN overload of it.
            if ( isa(val, 'function_handle') && ...
                    any(strcmpi(func2str(val), {'ode113', 'ode15s', 'ode45'})) )
                val = eval(['@chebfun.', func2str(val)]);

            % Check whether we got a string argument, e.g.
            % pref.ivpSolver = 'ode113'.
            elseif ( any(strcmpi(val, {'ode113', 'ode15s', 'ode45'})) )
                val = eval(['@chebfun.', val]);
            end
        end

    end

end


function r = mpower(p,q)
% MEAS/MPOWER  Implement p^q for meas.

% make a meas called r
r = meas();

% give it the right entries
r.value = p.value^q;
r.error = r.value * q * (p.error / p.value);