% nlsS = getNLSStruct( extra, dispOn, zoom)
%
% extra.tVec    : defining TIs
%               (not called TIVec because it looks too much like T1Vec)
% extra.T1Vec   : defining T1s
% dispOn        : 1 - display the struct at the end
%                 0 (or omitted) - no display
% zoom          : 1 (or omitted) - do a non-zoomed search
%                 x>1 - do an iterative zoomed search (x-1) times (slower search)
%                 NOTE: When zooming in, convergence is not guaranteed.
%
% Data Model    : a + b*exp(-TI/T1)
%
% written by J. Barral, M. Etezadi-Amoli, E. Gudmundson, and N. Stikov, 2009
%  (c) Board of Trustees, Leland Stanford Junior University

function nlsS = getNLSStruct(extra, dispOn, zoom)

nlsS.tVec = extra.tVec(:);
nlsS.N = length(nlsS.tVec);
nlsS.T1Vec = extra.T1Vec(:);
nlsS.T1Start = nlsS.T1Vec(1);
nlsS.T1Stop = nlsS.T1Vec(end);
nlsS.T1Len = length(nlsS.T1Vec);

% The search algorithm to be used
nlsS.nlsAlg = 'grid'; % Grid search

% Display the struct so that the user can see it went ok
if nargin < 2
  dispOn = 0;
end

% Set the number of times you zoom the grid search in, 1 = no zoom
% Setting this greater than 1 will reduce the step size in the grid search
% (and slow down the fit significantly)

if nargin < 3
	nlsS.nbrOfZoom = 2;
else
	nlsS.nbrOfZoom = zoom;
end

if nlsS.nbrOfZoom > 1
    nlsS.T1LenZ = 21; % Length of the zoomed search
end

if dispOn
  % Display the structure for inspection
  nlsS
end

% Set the help variables that can be precomputed:
% alpha is 1/T1,
% theExp is a matrix of exp(-TI/T1) for different TI and T1,
% rhoNormVec is a vector containing the norm-squared of rho over TI,
% where rho = exp(-TI/T1), for different T1's.
switch(nlsS.nlsAlg)
  case{'grid'}
    alphaVec = 1./nlsS.T1Vec;
    nlsS.theExp = exp( -nlsS.tVec*alphaVec' );
    nlsS.rhoNormVec = ...
        sum( nlsS.theExp.^2, 1)' - ...
        1/nlsS.N*(sum(nlsS.theExp,1)').^2;
end


function [tgrad,fgrad,c]=gabphasegrad(method,varargin)
%GABPHASEGRAD   Phase gradient of the DGT
%   Usage:  [tgrad,fgrad,c] = gabphasegrad('dgt',f,g,a,M);
%           [tgrad,fgrad]   = gabphasegrad('phase',cphase,a);
%           [tgrad,fgrad]   = gabphasegrad('abs',s,g,a);
%
%   `[tgrad,fgrad]=gabphasegrad(method,...)` computes the relative
%   time-frequency gradient of the phase of the |dgt| of a signal.
%   The derivative in time *tgrad* is the relative instantaneous
%   frequency while the frequency derivative *fgrad* is the negative
%   of the local group delay.
%
%   *tgrad* is a measure the deviation from the current channel frequency,
%   so a value of zero means that the instantaneous frequency is equal to
%   the center frequency of the considered channel, a positive value means
%   the true absolute intantaneous frequency is higher than the current
%   channel frequency and vice versa.
%   Similarly, *fgrad* is a measure of deviation from the current time
%   positions.
%
%   *fgrad* is scaled such that distances are measured in samples. Similarly,
%   *tgrad* is scaled such that the Nyquist frequency (the highest possible
%   frequency) corresponds to a value of L/2. The absolute time and
%   frequency positions can be obtained as
%
%      tgradabs = bsxfun(@plus,tgrad,fftindex(M)*L/M);
%      fgradabs = bsxfun(@plus,fgrad,(0:L/a-1)*a);
%
%   Please note that neither *tgrad* and *fgrad* nor *tgradabs* and
%   *fgradabs* are true derivatives of the |dgt| phase. To obtain the true
%   phase derivatives, one has to explicitly pass either 'freqinv' or
%   'timeinv' flags and scale both *tgrad* and *fgrad* by 2*pi/L.
%
%   The computation of *tgrad* and *fgrad* is inaccurate when the absolute
%   value of the Gabor coefficients is low. This is due to the fact the the
%   phase of complex numbers close to the machine precision is almost
%   random. Therefore, *tgrad* and *fgrad* may attain very large random values
%   when `abs(c)` is close to zero.
%
%   The computation can be done using four different methods.
%
%     'dgt'    Directly from the signal using algorithm by Auger and
%              Flandrin.
%
%     'phase'  From the phase of a DGT of the signal. This is the
%              classic method used in the phase vocoder.
%
%     'abs'    From the absolute value of the DGT. Currently this
%              method works only for Gaussian windows.
%
%     'cross'  Directly from the signal using algorithm by Nelson.
%
%   `[tgrad,fgrad]=gabphasegrad('dgt',f,g,a,M)` computes the time-frequency
%   gradient using a DGT of the signal *f*. The DGT is computed using the
%   window *g* on the lattice specified by the time shift *a* and the number
%   of channels *M*. The algorithm used to perform this calculation computes
%   several DGTs, and therefore this routine takes the exact same input
%   parameters as |dgt|.
%
%   The window *g* may be specified as in |dgt|. If the window used is
%   'gauss', the computation will be done by a faster algorithm.
%
%   `[tgrad,fgrad,c]=gabphasegrad('dgt',f,g,a,M)` additionally returns the
%   Gabor coefficients *c*, as they are always computed as a byproduct of the
%   algorithm.
%
%   `[tgrad,fgrad]=gabphasegrad('cross',f,g,a,M)` does the same as above
%   but this time using algorithm by Nelson which is based on computing
%   several DGTs.
%
%   `[tgrad,fgrad]=gabphasegrad('phase',cphase,a)` computes the phase
%   gradient from the phase *cphase* of a DGT of the signal. The original DGT
%   from which the phase is obtained must have been computed using a
%   time-shift of *a* using the default phase convention (`'freqinv'`) e.g.::
%
%        [tgrad,fgrad]=gabphasegrad('phase',angle(dgt(f,g,a,M)),a)
%
%   `[tgrad,fgrad]=gabphasegrad('abs',s,g,a)` computes the phase gradient
%   from the spectrogram *s*. The spectrogram must have been computed using
%   the window *g* and time-shift *a* e.g.::
%
%        [tgrad,fgrad]=gabphasegrad('abs',abs(dgt(f,g,a,M)),g,a)
%
%   `[tgrad,fgrad]=gabphasegrad('abs',s,g,a,difforder)` uses a centered finite
%   diffence scheme of order *difforder* to perform the needed numerical
%   differentiation. Default is to use a 4th order scheme.
%
%   Currently the `'abs'` method only works if the window *g* is a Gaussian
%   window specified as a string or cell array.
%
%   See also: resgram, gabreassign, dgt
%
%   References: aufl95 cmdaaufl97 fl65 ltfatnote042

% AUTHOR: Peter L. Søndergaard, 2008; Zdenek Průša 2015

%narginchk(4,6);

% If no phaseconv flag was passed, add 'relative'
definput = arg_gabphasederivconv;
if ~any(cellfun(@(el) any(strcmpi(el,varargin)),definput.flags.phaseconv))
    varargin{end+1} = 'relative';
end

if nargout<3
    phased = gabphasederiv({'t','f'},method,varargin{:});
else
    [phased,c] = gabphasederiv({'t','f'},method,varargin{:});
end

[tgrad,fgrad] = deal(phased{:});


classdef NMPCDCBF2 < handle
    % MPC with distance constraints
    properties
        system
        params
        x0
        x_curr
        time_curr = 0.0
        xlog = []
        ulog = []
        tt = 0
        distlog = []
        solvertime = []
        xopenloop = {}
        uopenloop = {}
        u_cost = 0
        obs
    end
    methods
        function self = NMPCDCBF2(x0, system, params)
            % Define MPC_CBF controller
            self.x0 = x0;
            self.x_curr = x0;
            self.system = system;
            self.params = params;
        end

        function sim(self, time)
            % Simulate the system until a given time
            tic;
            xk = self.x_curr;
            while self.time_curr < time
                [~, uk, tt] = self.solveNMPCDCBF1(self.x_curr);%if infeasiblility happens, stop
                if tt == -1
                    self.tt = tt;%record computing time for each time-step
                    return
                end
                xk = self.system.A * xk + [xk(4,1) * cos(xk(3,1))*self.system.dt;xk(4,1) * sin(xk(3,1))*self.system.dt;0;0] + self.system.C * uk;
                % update system
                self.x_curr = xk;
                self.time_curr = self.time_curr + self.system.dt;
                self.xlog = [self.xlog, xk];
                self.ulog = [self.ulog, uk];
                self.u_cost = self.u_cost + uk'*uk;
                tt = tt + toc;
                self.tt = tt;
                return
            end

        end

        function [xopt, uopt,tt] = solveNMPCDCBF1(self, xk)
            % Solve NMPC-DCBF
            [feas, x, u, J] = self.solve_cftoc1(xk);
            if ~feas
                xopt = [];
                uopt = [];
                tt = -1;
                return
            else
                xopt = x(:,2);
                uopt = u(:,1);
                tt = 0;
            end
        end

        function [feas, xopt, uopt, Jopt] = solve_cftoc1(self, xk)
            % Solve CFTOC
            % extract variables
            N = self.params.N;
            % define variables and cost
            x = sdpvar(4, N+1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            u = sdpvar(4, N);
            constraints = [];
            cost = 0;
            % initial constraint
            constraints = [constraints; x(:,1) == xk];
            % add constraints and costs
            AA=self.system.A;
            BB=self.system.B;
            CC=self.system.C;
            constraints = [constraints;
                    self.system.xl <= x(:,1) <= self.system.xu;
                    self.system.ul <= u(:,1) <= self.system.uu;
                    x(:,2) == AA * x(:,1) + [x(4,1)*cos(x(3,1))*self.system.dt;x(4,1)*sin(x(3,1))*self.system.dt;0; 0] + CC * u(:,1)];
                cost = cost + (x(:,1)-[3;0.01;0;0])'*self.params.Q*(x(:,1)-[3;0.01;0;0]) + (u(:,1)-[0;0;1;1])'*self.params.R*(u(:,1)-[0;0;1;1]); % self.params.S*(w(:,1)-1)^2;
            for i = 2:1:N
                constraints = [constraints;
                    self.system.xl <= x(:,i) <= self.system.xu;
                    self.system.ul <= u(:,i) <= self.system.uu;
                    x(:,i+1) == AA * x(:,i) + [x(4,i)*cos(x(3,i))*self.system.dt;x(4,i)*sin(x(3,i))*self.system.dt;0; 0] + CC * u(:,i)];
                cost = cost + (x(:,i)-[3;0.01;0;0])'*self.params.Q*(x(:,i)-[3;0.01;0;0]) + (u(:,i)-[0;0;1;1])'*self.params.R*(u(:,i)-[0;0;1;1]);% self.params.S*(w(:,i)-1)^2;
            end
            % add CBF constraints
            for i = 1:N-1
                pos = self.obs.pos1;
                r = self.obs.r1 ;
                b = (x([1:2],i)-pos)'*((x([1:2],i)-pos))-r^2;
                b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos))-r^2;
                b_next_next = (x([1:2],i+2)-pos)'*((x([1:2],i+2)-pos))-r^2;
                b1 = (b_next - b)/self.system.dt + self.params.gamma1/self.system.dt * (b);
                b1_next = (b_next_next - b_next)/self.system.dt + self.params.gamma1/self.system.dt * (b_next);
                constraints = [constraints; b1_next  >=u(4,i)*(1-self.params.gamma2)*b1;b_next  >= u(3,i)*(1-self.params.gamma1)*b];%Highest order mcbf=2
            end
            % add terminal cost
            cost = cost + (x(:,N+1)-[3;0.01;0;0])'*self.params.P*(x(:,N+1)-[3;0.01;0;0]);
            ops = sdpsettings('solver','ipopt','verbose',0);
            % solve optimization
            diagnostics = optimize(constraints, cost, ops);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt = [];
                uopt = [];
                Jopt = [];
            end
            pos = self.obs.pos1;
            r = self.obs.r1 ;
            self.distlog = [self.distlog, (r^2-(xk(1:2)-pos)'*(xk(1:2)-pos))];
            self.xopenloop{size(self.xopenloop,2)+1} = xopt;
            self.uopenloop{size(self.uopenloop,2)+1} = uopt;
            self.solvertime = [self.solvertime, diagnostics.solvertime];
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end
    end
end


%% FOURCURL3P2TEST
%
% Use the quadraticNedelec elements to approximate the velocity u and
% the stream function w.
%
% We solve the following equations:
%
%  -w + curl curl u = 0
%  curl curl w + u  = f
%
% with Dirichlet boundary condition
%
% u\times n = (curl u )\times n  = 0  on \partial \Omega
%
% Please check fourCurl3doc for details.
%
% Lin Zhong, May, 2013. Clean up. Long Chen, Oct 21, 2018
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

close all;
clear variables;

%% Problem Setting
pde = fourCurl3data;
[node,elem] = cubemesh([-1,1,-1,1,-1,1],2);
bdFlag = setboundary3(node,elem,'Dirichlet');

%% Parameters
maxIt = 3;
errwIwhL2 = zeros(maxIt,1);
errwIwhHcurl = zeros(maxIt,1);
errwL2 = zeros(maxIt,1);
errwwhHcurl = zeros(maxIt,1);
erruIuhL2 = zeros(maxIt,1);
erruIuhHcurl = zeros(maxIt,1);
erruL2 = zeros(maxIt,1);
erruuhHcurl = zeros(maxIt,1);
assembleTime = zeros(maxIt,1);
solverTime = zeros(maxIt,1);
N = zeros(maxIt,1);
h = zeros(maxIt,1);
option = [];

for k = 1:maxIt
    % refine grid
    [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    % solve the equation
    [w,u,eqn,info] = fourCurl3P2(node,elem,bdFlag,pde,option);
    % compute error
    uI = edgeinterpolate2(pde.exactu,node,eqn.edge,eqn.face,eqn.face2edge);
    wI = edgeinterpolate2(pde.curlcurlu,node,eqn.edge,eqn.face,eqn.face2edge);
    errwL2(k) = getL2error3ND2(node,elem,pde.curlcurlu,w);
    erruL2(k) = getL2error3ND2(node,elem,pde.exactu,u);
    errwwhHcurl(k) = getHcurlerror3ND2(node,elem,pde.curlcurlcurlu,w);
    erruuhHcurl(k) = getHcurlerror3ND2(node,elem,pde.curlu,u);
    errwIwhL2(k) = sqrt((w-wI)'*eqn.M*(w-wI));
    erruIuhL2(k) = sqrt((u-uI)'*eqn.M*(u-uI));
    errwIwhHcurl(k) = sqrt((w-wI)'*eqn.A*(w-wI));
    erruIuhHcurl(k) = sqrt((u-uI)'*eqn.A*(u-uI));
    % record information
    N(k) = length(u)+length(w);
    h(k) = 1./(size(node,1)^(1/3)-1);
    assembleTime(k) = info.assembleTime;
    solverTime(k) = info.solverTime;
end

%% Plot convergence rates
subplot(2,2,1)
showrateh2(h, errwIwhL2(1:maxIt), 1 , 'k-+', '||w_I-w_h||',...
           h, erruIuhL2(1:maxIt), 1 , 'r-*', '||u_I-u_h||' );
subplot(2,2,2)
showrateh2(h, errwL2(1:maxIt), 1 , 'k-+', '||w-w_h||',...
           h, erruL2(1:maxIt), 1 , 'r-*', '||u-u_h||' );
subplot(2,2,3)
showrateh2(h, errwIwhHcurl(1:maxIt), 1 , 'b-+', '||w_I-w_h||_1',...
           h, erruIuhHcurl(1:maxIt), 1 , 'g-*', '||u_I-u_h||_1' );
subplot(2,2,4)
showrateh2(h, errwwhHcurl(1:maxIt), 1 , 'b-+', '||w-w_h||_1',...
           h, erruuhHcurl(1:maxIt), 1 , 'g-*', '||u-u_h||_1' );

%% Output
err = struct('N', N, 'h', h, ...
             'wL2',errwL2(1:maxIt),'uL2',erruL2(1:maxIt),...
             'wIwhL2',errwIwhL2(1:maxIt), 'uIuhL2',erruIuhL2(1:maxIt),...
             'wIwhHcurl',errwIwhHcurl(1:maxIt),'uIuhHcurl',erruIuhHcurl(1:maxIt),...
             'wwhHcurl',errwwhHcurl(1:maxIt),'uuhHcurl',erruuhHcurl(1:maxIt));
time = struct('N',N,'assemble',assembleTime(1:maxIt),'solver',solverTime(1:maxIt));

%% Display error on screen
disp('Table: Error')
colname = {'#Dof','h','||w_I-w_h||','||w-w_h||','||w_I-w_h||_1','||w-w_h||_1'};
disptable(colname,err.N,[],err.h,'%0.3e',err.wIwhL2,'%0.5e',err.wL2,'%0.5e',...
              err.wIwhHcurl,'%0.5e',err.wwhHcurl,'%0.5e');

colname = {'#Dof','h','||u_I-u_h||','||u-u_h||','||u_I-u_h||_1','||u-u_h||_1'};
disptable(colname,err.N,[],err.h,'%0.3e',err.uIuhL2,'%0.5e',err.uL2,'%0.5e',...
              err.uIuhHcurl,'%0.5e',err.uuhHcurl,'%0.5e');

%     display('Table: CPU time')
colname = {'#Dof','Assemble','Solve'};
disptable(colname,time.N,[],time.assemble,'%0.2e',time.solver,'%0.2e');


function t=Tc(v,fc)

% t=Tc(v,fc)
%
% Return the channel coherence time for a mobile moving v m/s and communicating
% over carrier frequenc fc in Hz.

% Copyright 2012 Evrytania LLC (http://www.evrytania.com)
%
% Written by James Peroulas <james@evrytania.com>
%
% 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 3 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 should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

error(nargchk(2,2,nargin));
error(chk_param(v,'v','scalar','real','>=',0));
error(chk_param(fc,'fc','scalar','real','>=',0));

dop_spread=2*doppler_shift(fc,v);
t=1/4/dop_spread;


% Version 1.000
%
% Code provided by Ruslan Salakhutdinov and Geoff Hinton
%
% Permission is granted for anyone to copy, use, modify, or distribute this
% program and accompanying programs and documents for any purpose, provided
% this copyright notice is retained and prominently displayed, along with
% a note saying that the original programs are available from our
% web page.
% The programs and documents are distributed without any warranty, express or
% implied.  As the programs were written for research purposes only, they have
% not been tested to the degree that would be advisable in any important
% application.  All use of these programs is entirely at the user's own risk.

function [f, df] = CG_CLASSIFY(VV,Dim,XX,target);

l1 = Dim(1);
l2 = Dim(2);
l3= Dim(3);
l4= Dim(4);
l5= Dim(5);
N = size(XX,1);

% Do decomversion.
 w1 = reshape(VV(1:(l1+1)*l2),l1+1,l2);
 xxx = (l1+1)*l2;
 w2 = reshape(VV(xxx+1:xxx+(l2+1)*l3),l2+1,l3);
 xxx = xxx+(l2+1)*l3;
 w3 = reshape(VV(xxx+1:xxx+(l3+1)*l4),l3+1,l4);
 xxx = xxx+(l3+1)*l4;
 w_class = reshape(VV(xxx+1:xxx+(l4+1)*l5),l4+1,l5);

  XX = [XX ones(N,1)];
  w1probs = 1./(1 + exp(-XX*w1)); w1probs = [w1probs  ones(N,1)];
  w2probs = 1./(1 + exp(-w1probs*w2)); w2probs = [w2probs ones(N,1)];
  w3probs = 1./(1 + exp(-w2probs*w3)); w3probs = [w3probs  ones(N,1)];

  targetout = exp(w3probs*w_class);
  targetout = targetout./repmat(sum(targetout,2),1,10);
  f = -sum(sum( target(:,1:end).*log(targetout))) ;

IO = (targetout-target(:,1:end));
Ix_class=IO;
dw_class =  w3probs'*Ix_class;

Ix3 = (Ix_class*w_class').*w3probs.*(1-w3probs);
Ix3 = Ix3(:,1:end-1);
dw3 =  w2probs'*Ix3;

Ix2 = (Ix3*w3').*w2probs.*(1-w2probs);
Ix2 = Ix2(:,1:end-1);
dw2 =  w1probs'*Ix2;

Ix1 = (Ix2*w2').*w1probs.*(1-w1probs);
Ix1 = Ix1(:,1:end-1);
dw1 =  XX'*Ix1;

df = [dw1(:)' dw2(:)' dw3(:)' dw_class(:)']';


% add the path of RBM code
addpath('..');
addpath('~/work/Algorithms/liblinear-1.7/matlab');

use_whitening = 1;

% load natural image patches
load 'bsds500bw_patches_8.mat';
X = (Xbw / 255);

% shuffle the training data
perm_idx = randperm (size(X,1));

n_all = size(X, 1);
n_train = ceil(n_all * 3 / 4);
n_valid = floor(n_all /4);

X_valid = X(perm_idx(n_train+1:end), :);
X = X(perm_idx(1:n_train), :);

if use_whitening
    %% ZCA
    %[Z, Wsep, Wmix, mX] = zca(X, 0.1);
    load patch8_whiten.mat;
    X = zca_whiten(X, Wsep, Wmix, mX);
    X_valid = zca_whiten(X_valid, Wsep, Wmix, mX);
end

% construct RBM and use default configurations
D = default_dae (size(X, 2), 320);

if use_whitening
    D.do_normalize = 0;
    D.do_normalize_std = 0;
end

D.data.binary = 0;
D.hidden.binary = 0;

D.learning.lrate = 1e-1;
D.learning.lrate0 = 10000;
%D.learning.momentum = 0.5;
D.learning.weight_decay = 0;

D.adagrad.use = 1;

D.rica.cost = 0.01;

D.noise.drop = 0;
D.noise.level = 0;

D.sparsity.cost = 0;
D.sparsity.target = 0;

% max. 100 epochs
D.iteration.n_epochs = 500;

% set the stopping criterion
D.stop.criterion = 0;
D.stop.recon_error.tolerate_count = 1000;

% save the intermediate data after every epoch
D.hook.per_epoch = {@save_intermediate, {'rica_patch8.mat'}};

% print learining process
D.verbose = 0;

% display the progress
D.debug.do_display = 0;

fprintf(1, 'Training rICA\n');
tic;
D = dae (D, X, X_valid, 0.1);
fprintf(1, 'Training is done after %f seconds\n', toc);


function test_failed=test_dgt2

test_failed=0;

disp(' ===============  TEST_DGT2 ================');

% Run some fixed test to test the interface.
% This is not a thourough tester.

% --- first test

a=6;
M=8;

Lf=71;
L=72;
W=3;

f=tester_rand(Lf,Lf,W);

g=pgauss(L,a*M/L);
gd=gabdual(g,a,M);

[c,Ls]=dgt2(f,g,a,M);
r=idgt2(c,gd,a,Ls);

res=f-r;
nres=norm(res(:));

[test_failed,fail]=ltfatdiditfail(nres,test_failed);
%failed='';
%if nres>10e-10
%  failed='FAILED';
%  test_failed=test_failed+1;
%end;

s=sprintf('DGT2 Lf:%3i L:%3i %0.5g %s',Lf,L,nres,fail);
disp(s)

% --- second test

a1=6;
M1=8;

a2=5;
M2=10;

L1=a1*M1;
L2=a2*M2;

W=1;

f=tester_rand(L1,L2,W);

g1=pgauss(L1,a1*M1/L1);
g2=pgauss(L2,a2*M2/L2);

gd1=gabdual(g1,a1,M1);
gd2=gabdual(g2,a2,M2);

c=dgt2(f,g1,g2,[a1,a2],[M1,M2]);
c2=ref_dgt2(f,g1,g2,a1,a2,M1,M2);

rc=c-c2;
nres=norm(rc(:));

[test_failed,fail]=ltfatdiditfail(nres,test_failed);
%failed='';
%if nres>10e-10
%  failed='FAILED';
%  test_failed=test_failed+1;
%end;

s=sprintf('DGT2 REF L1:%3i L2:%3i %0.5g %s',L1,L2,nres,fail);
disp(s)

r=idgt2(c,gd1,gd2,[a1,a2]);

res=r-f;

nres=norm(res(:));

[test_failed,fail]=ltfatdiditfail(nres,test_failed);
%failed='';
%if nres>10e-10
%  failed='FAILED';
%  test_failed=test_failed+1;
%end;

s=sprintf('DGT2 INV L1:%3i L2:%3i %0.5g %s',L1,L2,nres,fail);
disp(s)


function y = upfilt(x, f, dim, extmod, shift)
% UPFILT   Upsample (by 2) and filter along a dimension
%
%       y = upfilt(x, f, dim, extmod, shift)
%
% Input:
%   x:      input signal
%   f:      1-D filter
%   dim:    the processing dimension
%   extmod: extension mode (e.g. 'per' or 'sym')
%   shift:  specifies the window over which filtering occurs
%
% Output:
%   y:      upsampled and filtered signal
%
% Note:
%   The origin of the filter f is assumed to be floor(size(f)/2) + 1.
%   Amount of shift should be no more than floor((size(f)-1)/2).

% Skip singleton dimension
if size(x, dim) == 1
    y = x;
    return
end

nd = ndims(x);
% Consider column vectors as 1-D signals
if nd == 2 & size(x, 2) == 1
    nd = 1;
end

% Cell array of indexes for each dimension
I = cell(1, ndims(x));
for d = 1:ndims(x)
    I{d} = 1:size(x,d);
end

% Upsample (by 2)
sx = size(x);
sx(dim) = 2*sx(dim);
y = zeros(sx);
I{dim} = 1:2:sx(dim);
y(I{:}) = x;

% Border extend
n = size(y, dim);
hlf = (length(f) - 1) / 2;
% Amount of extension at two ends
e1 = floor(hlf) + shift;
e2 = ceil(hlf) - shift;

switch extmod
    case 'per'
        I{dim} = [ly-e1+1:n , 1:n , 1:e2];

    case 'sym'
        I{dim} = [e1+1:-1:2 , 1:n , n-1:-1:e2];

    otherwise
        error('Invalid input for EXTMOD')

end
y = y(I{:});

% Filter and return only the 'valid' part
y = filter(f, 1, y, [], dim);

I{dim} = (1:n) + length(f) - 1;
y = y(I{:});


function varargout = circleAsPolygon(circle, varargin)
%CIRCLEASPOLYGON Convert a circle into a series of points
%
%   Note: this function is deprecated, use "circleToPolygon" instead
%
%   P = circleAsPolygon(CIRCLE, N);
%   convert circle given as [x0 y0 r], where x0 and y0 are coordinate of
%   center, and r is the radius, into an array of  [(N+1)x2] double,
%   containing x and y values of points.
%   The polygon is closed
%
%   P = circleAsPolygon(CIRCLE);
%   uses a default value of N=64 points
%
%   Example
%   circle = circleAsPolygon([10 0 5], 16);
%   figure;
%   drawPolygon(circle);
%
%   See also:
%   circles2d, circleToPolygon
%
%
% ---------
% author : David Legland
% created the 06/04/2005.
% Copyright 2010 INRA - Cepia Software Platform.
%

%   HISTORY
%   20/04/2007: return a closed polygon with N+1 vertices, use default N=64

warning('matGeom:deprecated', ...
    'function "circleAsPolygon" is deprecated, use "circleToPolygon" instead');

% format output
if nargout <= 1
    varargout = {circleToPolygon(circle, varargin{:})};
else
    [x, y] = circleToPolygon(circle, varargin{:});
    varargout = {x, y};
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script for using short-hand version of the meshing wrappers
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% preparation
% user must add the path of iso2mesh to matlab path list
% addpath('../');

% user need to add the full path to .../iso2mesh/bin directory
% to windows/Linux/Unix PATH environment variable

%% load the sample data
load rat_head.mat

% volimage is a volumetric image such as an X-ray or MRI image
%% v2m is the short-hand version of vol2mesh

% mesh volimage at threshold level 0.05, max surface element size 3,
% maximum tetrahedral element volume 2

[node,elem,face]=v2m(volimage,0.05,3,2);

%% visualize the resulting mesh
subplot(211);
plotmesh(node,face);
axis equal;

%% alternatively, one can call vol2surf and surf2mesh separately

% v2s: shorthand version of vol2surf, s2m: shorthand version of surf2mesh

[node,face,regions,holes]=v2s(volimage,0.05,3);
[node,elem,face]=s2m(node,face,1,2);

%% visualize the resulting mesh
subplot(212)
plotmesh(node,face);
axis equal;


%example_accel_grad_mtl

clear all;
clc

% add path
addpath(genpath('../../SLEP/'));

% load data and set regularization parameter
load('../../data/dmoz.mat');
lambda = 10^-4;

% center data
for i = 1:length(Xtrain)
        Xtrain{i} = CenterRowData(Xtrain{i});
        Ytrain{i} = CenterRowData(Ytrain{i});
end

% call the main function
[Wp,fval_vec,itr_counter] = accel_grad_mtl(Xtrain,Ytrain,lambda);


function [err,time,solver,eqn] = femPoisson3(node,elem,pde,bdFlag,option,varargin)
%% FEMPOISSON3 solve Poisson equation by various finite element methods
%
%   FEMPOISSON3 computes approximations to the Poisson equation on a
%   sequence of meshes obtained by uniform refinement of a input mesh.
%
% See also Poisson, crack, Lshape
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Check input arguments
if nargin >=1 && ischar(node)
    option.elemType = node;
    clear node
end
if ~exist('node','var') || ~exist('elem','var')
    [node,elem] = cubemesh([0,1,0,1,0,1],0.25); % default mesh is a cube
end
if ~exist('option','var'), option = []; end
if ~exist('pde','var')
    pde = sincosdata3;                          % default data
end
if ~exist('bdFlag','var')
    bdFlag = setboundary3(node,elem,'Dirichlet');
end

%% Parameters
option = femoption(option);
maxIt = option.maxIt;   maxN = option.maxN; L0 = option.L0;
elemType = option.elemType; refType = option.refType;

%% Generate an initial mesh
for k = 1:L0
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
end

%% Initialize err
errL2 = zeros(maxIt,1);   errH1 = zeros(maxIt,1);
erruIuh = zeros(maxIt,1); errMax = zeros(maxIt,1);
errTime = zeros(maxIt,1); solverTime = zeros(maxIt,1);
assembleTime = zeros(maxIt,1); meshTime = zeros(maxIt,1);
itStep = zeros(maxIt,1);  stopErr = zeros(maxIt,1); flag = zeros(maxIt,1);
N = zeros(maxIt,1);

%% Finite Element Method
for k = 1:maxIt
    % solve the equation
    switch elemType
        case 'P1'     % piecewise linear function P1 element
            [u,Du,eqn,info] = Poisson3(node,elem,pde,bdFlag,option);
        case 'CR'     % piecewise linear function CR element
            [u,Du,eqn,info] = Poisson3CR(node,elem,pde,bdFlag,option);
        case 'P2'     % piecewise quadratic function
            [u,Du,eqn,info] = Poisson3P2(node,elem,pde,bdFlag,option);
        case 'WG'     % weak Galerkin element
            [u,Du,eqn,info] = Poisson3WG(node,elem,pde,bdFlag,option);
    end
    % compute error
    tic;
    if isfield(pde,'Du')
        if ~isempty(Du)
            errH1(k) = getH1error3(node,elem,pde.Du,Du);
        else
            errH1(k) = getH1error3(node,elem,pde.Du,u);
        end
    end
    if isfield(pde,'exactu')
        errL2(k) = getL2error3(node,elem,pde.exactu,u);
        % interpolation
        switch elemType
            case 'P1'
                uI = Lagrangeinterpolate(pde.exactu,node,elem);
            case 'CR'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'CR',eqn.face);
            case 'P2'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'P2',eqn.edge);
            case 'WG'
                uI = Lagrangeinterpolate(pde.exactu,node,elem,'WG',eqn.face);
        end
        erruIuh(k) = sqrt((u-uI)'*eqn.A*(u-uI));
        errMax(k) = max(abs(u-uI));
    end
    errTime(k) = toc;
    % record time
    solverTime(k) = info.solverTime;
    assembleTime(k) = info.assembleTime;
    if option.printlevel>1
        fprintf('Time to compute the error %4.2g s \n H1 err %4.2g    L2err %4.2g \n',...
            errTime(k),errH1(k), errL2(k));
    end
    % record solver information
    itStep(k) = info.itStep;
    stopErr(k) = info.stopErr;
    flag(k) = info.flag;
    % plot
    N(k) = length(u);
    if option.plotflag && N(k) < 2e3 % show mesh and solution for small size
       figure(1);  showresult3(node,elem,u);
    end
    if N(k) > maxN
        break;
    end
    % refine mesh
    tic;
    if strcmp(refType,'red')
        [node,elem,bdFlag] = uniformrefine3(node,elem,bdFlag);
    elseif strcmp(refType,'bisect')
        [node,elem,bdFlag] = uniformbisect3(node,elem,bdFlag);
    end
    meshTime(k) = toc;
end

%% Plot convergence rates
if option.rateflag
    figure;
    set(gcf,'Units','normal');
    set(gcf,'Position',[0.25,0.25,0.55,0.4]);
    subplot(1,2,1)
    showrate2(N(1:k),errH1(1:k),1,'-*','||Du-Du_h||',...
              N(1:k),errL2(1:k),1,'k-+','||u-u_h||');
    subplot(1,2,2)
    showrate2(N(1:k),erruIuh(1:k),1,'m-+','||Du_I-Du_h||',...
              N(1:k),errMax(1:k),1,'r-*','||u_I-u_h||_{\infty}');
end

%% Output
err = struct('N',N,'H1',errH1(1:k),'L2',errL2(1:k),...
             'uIuhH1',erruIuh(1:k),'uIuhMax',errMax(1:k));
time = struct('N',N,'err',errTime(1:k),'solver',solverTime(1:k), ...
              'assmble',assembleTime(1:k),'mesh',meshTime(1:k));
solver = struct('N',N(1:k),'itStep',itStep(1:k),'time',solverTime(1:k),...
                'stopErr',stopErr(1:k),'flag',flag(1:k));

%% Display error
ts = zeros(k,3); ts = char(ts);
display(' #Dof   ||u-u_h||     ||Du-Du_h||   ||DuI-Du_h||  ||uI-u_h||_{max}');
display([num2str(err.N) ts num2str(err.L2,'%0.5e') ts num2str(err.H1,'%0.5e')...
         ts num2str(err.uIuhH1,'%0.5e') ts num2str(err.uIuhMax,'%0.5e')]);


function D = sldiff_pw(X1, X2, type)
%SLDIFF_PW Measures the pair-wise difference
%
% $ Synatx $
%   - D = sldiff_pw(X1, X2, type)
%
% $ Arguments $
%   - X1, X2:             the two sample matrices
%   - type:               the type of difference measurement
%                         default = 'abssum'
%
% $ Description $
%   - D = sldiff_pw(X1, X2, type) computes the measurment of differences
%     between the samples in X1 and those in X2 in a pairwise manner.
%     All samples should be stored in columns. And the samples in X1 and
%     X2 should be of the same dimension. If X1 and X2 are of sizes
%     dxn1 and dxn2 respectively. Then D is a matrix of size n1 x n2.
%
%   - The measurment types supported are listed below
%     \*
%     \t  Table 1. The difference measurement types                 \\
%     \h    name      &         description                         \\
%          'abssum'   &   sum of absolute values of differences     \\
%          'maxdiff'  &   maximum of absolute values of differences \\
%          'mindiff'  &   minimum of absolute values of differences \\
%     \*
%
% $ History $
%   - Created by Dahua Lin on Dec 06th, 2005
%   - Modified by Dahua Lin on Sep 10th, 2006
%       - Re-implement the core in C++: pwdiff_core
%       - The efficiency is increased by 10 times.
%

%% parse and verify input arguments

if nargin < 3 || isempty(type)
    type = 'abssum';
end

switch type
    case 'abssum'
        pdmcode = 1;
    case 'maxdiff'
        pdmcode = 2;
    case 'mindiff'
        pdmcode = 3;
    otherwise
        error('sltoolbox:invalidarg', ...
            'Invalid type of pwdiff computation: %s', type);
end

%% Compute

D = pwdiff_core(X1, X2, pdmcode);


function [] = visGaussianClassLearning(RBM);
%-----------------------------------------
%  [] =  visGaussianClassLearning(RBM);
%-----------------------------------------
% DES

nVis = floor(sqrt(size(RBM.W,1))).^2;

subplot(331);
scatter(RBM.auxVars.batchX(:,1),RBM.auxVars.batchX(:,2),'r.');

title('Batch Data');
xlim([-10, 10]); ylim([-20, 0]);
%  xlim([-5, 5]); ylim([-5, 5]);

subplot(332);
scatter(RBM.pVis(:,1),RBM.pVis(:,2),'b.');
title('Fantasies');
%  xlim([-5, 5]); ylim([-5, 5]);
xlim([-10, 10]); ylim([-20, 0]);

subplot(333);
visWeights(flipud(RBM.dW(1:nVis,:)));
title ('Weight Gradients');

subplot(334);
bar(RBM.b);
title('Visible Bias');

%
%  subplot(335)
%  bar((RBM.sigma2));
%  title('Visible Variance');

subplot(336);
visWeights(flipud(RBM.W(1:nVis,:)));
title('Connection Weights');

subplot(337);
plot(RBM.auxVars.error);
title('Reconstruction errors');

subplot(339);
semilogy(RBM.auxVars.lRate);
title('Learning Rate');

drawnow


%FxNLMS
%Marko Stamenovic
%April 28, 2016

mus = [0.01 0.05 0.1 0.5 1 2 2.5 3];% 0.5 1];
for j = 1:length(mus)
    for i = 1:100
        %%INITIALIZE VALUES%%
        % generate input signal
        muOG=mus(j); %learning rate
        M=128; %buffer size (num filter weights)
        x=randn(10000,1); %input signal
        x=x/max(x); %sample rate
        fs=8000; %number of samples of the input signal
        N=length(x); %length of input signal

        % generate known filter coefficients
        Pz=0.5*[0:127];  %linear coefficients
        %Pz=randn(128,1); %random coefficients
        ylim = max(Pz)*1.20;
        ymin = min(Pz)-.2*max(Pz);
        % generate filtered input signal == desired signal
        d=conv(Pz,x); %input signal filtered by known filter Pz (primary path)

        %% ESTIMATE SECONDARY PATH SIGNAL USING LMS %%
        %generate dummy secondary path response Sz
        Sz = Pz/2;
        %run known signal through filter
        xp=conv(Sz,x);
        %initalize Sz hat values
        Szh=zeros(M,1);

        for n=M:N
            xpvec=x(n:-1:n-M+1); %input has to be in reverse orxer has to be
            %update mu
            mu(n) = 1/(xpvec'*xpvec);
            e(n)=xp(n)-Szh'*xpvec; %update error
            %plot(e)
            Szh=Szh+mu(n)*xpvec*(e(n)); %update filter coefficient
        end
        Szh = abs(ifft(1./abs(fft(Szh))));

        %% LMS FOR MAIN ANC %%
        emean=zeros(N,1);
        %filter input by learned filter to get x prime
        xph = conv(Szh,x);
        %initalize Wz filter values
        Wz=zeros(M,1);
        %Make sure that x and d are column vectors
        x=x(:);
        d=d(:);
        %LMS
        for n=M:N
            xvec=x(n:-1:n-M+1); %input has to be in reverse orxer
            xpvec=xph(n:-1:n-M+1);
            %update mu
            mu(n) = muOG/(xvec'*xvec);
            e(n)=d(n)-Wz'*xvec; %update error
            %plot(e)
            Wz=Wz+mu(n)*xpvec*(e(n)); %update filter coefficient

            %draw the learned filter in realtime
            %         plot(Pz)
            %         hold on
            %         plot(Wz)
            %         axis([0 inf ymin ylim])
            %         title(sprintf('n=%f time=%fs error = %f',n-M, (n-M)/fs, e(n)))
            %         hold off
            %         legend('Input coefficients','Learned Coefficients')
            %         drawnow;
        end
        e=e(:);

        emean = (emean(:)+e);
    end
    emean=(emean)/i;
    if max(emean)>1e4
        for l = 1:length(emean)
            if abs(emean(l)) > 1e3
                emean(l)=1e3;
            end
        end
        eall(j,:)=emean;
    else
        try
            [eall(j,:),q]=(envelope(abs(emean),500,'peaks'));
        catch
            eall(j,:) = 1e3;
        end
    end
end
figure
for i = 1:length(mus)
    plot(10*log10(abs(eall(i,:))))
    hold on
end
title('Convergence Time in Cycles')
ylabel('Error (dB)');
xlabel('Cycles');
hleg=legend('0.01','0.05','0.1','0.5','1.0', '2.0');
htitle=get(hleg,'Title');
set(htitle,'String','mu');
% %% PLOT RESULTS %%
% figure
% subplot(2,1,1)
% plot(e)
% title('Convergence Time in Cycles')
% ylabel('Amplitude');
% xlabel('Cycles');
% legend('Error');
% subplot(2,1,2)
% stem(Pz)
% hold on
% stem(Wz, 'r*')
% title('Input Coefficients vs Learned Coefficients')
% ylabel('Amplitude');
% xlabel('Numbering of filter tap');
% legend('Input Coefficients', 'learned coefficients')


% Construct various DBNs and examine their clique structure.
% This was used to generate various figures in chap 3-4 of my thesis.

% Examine the cliques in the unrolled mildew net

%dbn = mk_mildew_dbn;
dbn = mk_chmm(4);
ss = dbn.nnodes_per_slice;
T = 7;
N = ss*T;
bnet = dbn_to_bnet(dbn, T);

constrained = 0;
if constrained
  stages = num2cell(unroll_set(1:ss, ss, T), 1);
else
  stages = { 1:N; };
end
clusters = {};
%[jtree, root, cliques, B, w, elim_order, moral_edges, fill_in_edges] = ...
%    dag_to_jtree(bnet, bnet.observed, stages, clusters);
[jtree, root, cliques] =  graph_to_jtree(moralize(bnet.dag), ones(1,N), stages, clusters);

flip=1;
clf;[dummyx, dummyy, h] = draw_dbn(dbn.intra, dbn.inter, flip, T, -1);
dir = '/home/eecs/murphyk/WP/Thesis/Figures/Inf/MildewUnrolled';
mk_ps_from_clqs(dbn, T, cliques, [])
%mk_collage_from_clqs(dir, cliques)

% Examine the cliques in the cascade DBN

% A-A
%  \
% B B
%  \
% C C
%  \
% D D
ss = 4;
intra = zeros(ss);
inter = zeros(ss);
inter(1, [1 2])=1;
for i=2:ss-1
  inter(i,i+1)=1;
end

% 2 coupled HMMs 1,3  and 2,4
ss = 4;
intra = zeros(ss);
inter = zeros(ss); % no persistent edges
%inter = diag(ones(ss,1)); % persitence edges
inter(1,3)=1; inter(3,1)=1;
inter(2,4)=1; inter(4,2)=1;

%bnet = mk_fhmm(3);
bnet = mk_chmm(4);
intra = bnet.intra;
inter = bnet.inter;

clqs = compute_minimal_interface(intra, inter);
celldisp(clqs)

% A A
%  \
% B B
%  \
% C C
%  \
% D-D
ss = 4;
intra = zeros(ss);
inter = zeros(ss);
for i=1:ss-1
  inter(i,i+1)=1;
end
inter(4,4)=1;

ns = 2*ones(1,ss);
dbn = mk_dbn(intra, inter, ns);
for i=2*ss
  dbn.CPD{i} = tabular_CPD(bnet, i);
end

T = 4;
N = ss*T;
bnet = dbn_to_bnet(dbn, T);

constrained = 1;
if constrained
  % elim first 3 slices first in any order
  stages = {1:12, 13:16};
  %stages = num2cell(unroll_set(1:ss, ss, T), 1);
else
  stages = { 1:N; };
end
clusters = {};
%[jtree, root, cliques, B, w, elim_order, moral_edges, fill_in_edges] = ...
%    dag_to_jtree(bnet, bnet.observed, stages, clusters);
[jtree, root, cliques] =  graph_to_jtree(moralize(bnet.dag), ones(1,N), stages, clusters);

% Examine the cliques in the 1.5 slice DBN

%dbn = mk_mildew_dbn;
dbn = mk_water_dbn;
%dbn = mk_bat_dbn;
ss = dbn.nnodes_per_slice;
int = compute_fwd_interface(dbn);
bnet15 = mk_slice_and_half_dbn(dbn, int);
N = length(bnet15.dag);
stages = {1:N};

% bat
%cl1 = [16 17 19 7 14];
%cl2 = [27 25 21 23 20];
%clusters = {cl1, cl2, cl1+ss, cl2+ss};

% water
%cl1 = 1:2; cl2 = 3:6; cl3 = 7:8;
%clusters = {cl1, cl2, cl3, cl1+ss, cl2+ss, cl3+ss};

%clusters = {};
clusters = {int, int+ss};
%[jtree, root, cliques, B, w, elim_order, moral_edges, fill_in_edges] = ...
%    dag_to_jtree(bnet15, bnet.observed, stages, clusters);
[jtree, root, cliques] =  graph_to_jtree(moralize(bnet15.dag), ones(1,N), stages, clusters);

clq_len = [];
for c=1:length(cliques)
  clq_len(c) = length(cliques{c});
end
hist(clq_len, 1:max(clq_len));
h=hist(clq_len, 1:max(clq_len));
axis([1 max(clq_len)+1 0 max(h)+1])
xlabel('clique size','fontsize',16)
ylabel('number','fontsize',16)


% MANIPULATOR TRAJECTORY GENERATION
% Generates Cartesian trajectories with independent
% linearly interpolated rotation trajectories.
%
% Copyright 2019 The MathWorks, Inc.

%% Setup
clear, clc, close all

% Define waypoint information
createWaypointData;

% Define IK
ik = inverseKinematics('RigidBodyTree',gen3);
ikWeights = [1 1 1 1 1 1];
ikInitGuess = gen3.homeConfiguration;

% Set up plot
plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames
show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false);
xlim([-1 1]), ylim([-1 1]), zlim([0 1.2])
hold on
if plotMode == 1
    hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);

%% Generate and follow trajectory
% Loop through segments one at a time
trajType = 'trap';
numWaypoints = size(waypoints,2);
for w = 1:numWaypoints-1
    % Get the initial and final rotations and times for the segment
    R0 = eul2quat(orientations(:,w)');
    Rf = eul2quat(orientations(:,w+1)');
    timeInterval = waypointTimes(w:w+1);
    trajTimes = timeInterval(1):ts:timeInterval(2);

    % Cartesian Motion only
    switch trajType
        case 'trap'
            [q,qd,qdd] = trapveltraj(waypoints(:,w:w+1),numel(trajTimes), ...
                'AccelTime',waypointAccelTimes(w), ...
                'EndTime',diff(waypointTimes(w:w+1)));

        case 'cubic'
            [q,qd,qdd] = cubicpolytraj(waypoints(:,w:w+1),waypointTimes(w:w+1),trajTimes, ...
                'VelocityBoundaryCondition',waypointVels(:,w:w+1));

        case 'quintic'
            [q,qd,qdd] = quinticpolytraj(waypoints(:,w:w+1),waypointTimes(w:w+1),trajTimes, ...
                'VelocityBoundaryCondition',waypointVels(:,w:w+1), ...
                'AccelerationBoundaryCondition',waypointAccels(:,w:w+1));

        case 'bspline'
            ctrlpoints = waypoints(:,idx:idx+1); % Can adapt this as needed
            [q,qd,qdd] = bsplinepolytraj(ctrlpoints,timeInterval,trajTimes);

        otherwise
            error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');
    end

    % Find the quaternions from trajectory generation
    [R, omega, alpha] = rottraj(R0, Rf, timeInterval, trajTimes);

    % Plot trajectory
    if plotMode == 1
        set(hTraj,'xdata',q(1,:),'ydata',q(2,:),'zdata',q(3,:));
    elseif plotMode == 2
        plotTransforms(q',R','FrameSize',0.05)
    end

    % Trajectory following loop
    for idx = 1:numel(trajTimes)
        % Solve IK
        tgtPose = trvec2tform(q(:,idx)') * quat2tform(R(:,idx)');
        [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
        ikInitGuess = config;

        % Show the robot
        show(gen3,config,'Frames','off','PreservePlot',false);
        title(['Trajectory at t = ' num2str(trajTimes(idx))])
        drawnow
    end

end


classdef AxAyComputerFromVolumeAndR < handle

    properties (Access = private)
        theta
        r
        cx
        cy
    end

    methods (Access = public)

        function obj = AxAyComputerFromVolumeAndR(cParams)
            obj.init(cParams)
        end

        function [ax,ay] = compute(obj)
            h  = obj.computeH(obj.theta,obj.r);
            Tx = obj.computeTx(obj.r,h,obj.cx);
            Ty = obj.computeTy(obj.r,h,obj.cx);
            ax = obj.computeAx(Tx,Ty,obj.cx);
            ay = obj.computeAy(Tx,Ty,obj.cx);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.theta = 1 - cParams.volume;
            obj.r     = cParams.r;
            obj.cx    = cParams.cx;
            obj.cy    = cParams.cy;
        end

    end

    methods (Access = private, Static)

        function Tx = computeTx(r,h,cx)
            n = r*h*(1+h);
            d = (1+r*h)*cx;
            Tx = n/d;
        end

        function Ty = computeTy(r,h,cx)
            n = cx*h*(1+r*h);
            d = (1+h);
            Ty = n/d;
        end

        function ax = computeAx(Tx,Ty,cx)
            ax = (cx - Ty)/(1-Tx*Ty);
        end

        function ay = computeAy(Tx,Ty,cx)
            ay = (1-Tx*cx)/(1-Tx*Ty);
        end

        function h = computeH(theta,r)
            n = -(1+r)*theta + sqrt(theta^2*(r-1)^2 + 4*r);
            d = 2*r*(1+theta);
            h = n/d;
        end

    end

end


function [ratio,initdl] = fitint(ratio);
%fitint   computes contraction/expansion ratio
%   [ratio,initdl] = fitint(ratio);
%   input
%          ratio      initial guess
%   output
%          ratio      converged value
%          initdl     associated first increment
%
%   called by subint
%   IFISS function: DJS; 1 April 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      global global_N global_INTL global_LASTDL
      if(global_N==1)
      initdl=global_INTL;
      ratio=global_LASTDL/initdl;
      return
      else
      x=ratio;
% call nonlinear equation solver
%     fprintf('\n      x            f   \n')
      ratio=fzero('fint',x,optimset('Display','off'));
      initdl=global_LASTDL/(ratio^global_N);
      end
% check solution validity
      if ratio <= 1,
         fprintf('\n\n\nInfeasible ratio computed for stretch grid.\n');
         fprintf('Try changing the initial value for "ratio" in function "subint.m"\n');
         error(' ');
      elseif ratio >= 2
         fprintf('\n\n*** warning \n computed ratio is large. \n')
      end
      return


classdef homochoricPlot < axisAnglePlot

  methods

    function oP = homochoricPlot(varargin)
      % create a 3d Euler angle plot

      oP = oP@axisAnglePlot(varargin{:});

     end

     function [x,y,z] = project(oP,ori,varargin)

      if ~check_option(varargin,'noBoundaryCheck')
        switch oP.fRMode
          case 'project2FundamentalRegion'
            ori = project2FundamentalRegion(ori);
          case 'restrict2FundamentalRegion'
            ori(~oP.oR.checkInside) = NaN;
        end
      end

      [x,y,z] = double(homochoric(ori));

    end

    function ori = iproject(oP,x,y,z,varargin)
      ori = orientation.id;
    end

    function ori = makeGrid(oP,varargin)

      [ori,S2G,omega] = makeGrid@axisAnglePlot(oP,varargin{:});

      [oP.plotGrid.x,oP.plotGrid.y,oP.plotGrid.z] = ...
        double( S2G .* (3./4 * (omega - sin(omega))).^(1/3));

    end
  end
end


function [sigma,mu,var_dirs,var_amts] = scm_ex(Y,A,R,sigma0,mu0,sigma_max)
%SCM_EX Summary of this function goes here
%   Detailed explanation goes here
[N,M] = size(A);
[~,B] = size(R);
sigma_min = 1e-6;

C = kron((A'*A),eye(B));
S = (mu0^2/(sigma0^2))*eye(M*B,M*B);

h = (Y-A*R)'*A;
h = h(:);
gamma = 1/(mu0^2);
lsq = sum(sum((Y-A*R).^2));
gamma0Inv = 1/(N*B)*lsq;
obj_fun_sigma = @(S,gamma) gamma*lsq - gamma*h'*((S+C)\h) ...
    + logdet(S+C) - logdet(S) - N*B*log(gamma);
calc_ignores = @(S,gamma) [(gamma*h'*((S+C)\h)) / (gamma*lsq), ...
    (logdet(S+C) - logdet(S)) / (gamma*lsq)];
disp(['Inital ignores are: ',num2str(calc_ignores(S,gamma))]);
if 0
    % U1'*N1 has a much smaller frobenius norm than that of N1
    global A_gt; global R_gt;
    N = Y-A*R; N1 = Y-A_gt*R_gt;
    [U,D,V] = svd(A,0); [U1,D1,V1] = svd(A_gt,0);
    sum(sum(N.^2)), sum(sum((U'*N).^2))
    sum(sum(N1.^2)), sum(sum((U1'*N1).^2))
end
Es = obj_fun_sigma(S,gamma);
delta_t0 = 1e-9;

T = S + C;
Th = T\h;

disp('Start estimating the uncertainty...');
for i = 1:200
    % update S
    S2_old = S;

    GammaTinv = gamma*Th*Th';
    Tinv = inv(T);
    E0 = obj_fun_sigma(S,gamma);
    delta_t = delta_t0;

    der = zeros(M*B,M*B);
    for j = 1:M
        inds = (j-1)*B+1:j*B;
        Sj = S(inds,inds);
        der(inds,inds) = GammaTinv(inds,inds) - inv(Sj) + Tinv(inds,inds);
    end

    while 1
        S2_new = S - delta_t*der;
        S2_new = (S2_new + S2_new')/2;
        for j = 1:M
            inds = (j-1)*B+1:j*B;
            S3 = S2_new(inds,inds);

            [V,D] = eig(S3);
            d = diag(D);

            epsilon = 1/(gamma*sigma_max^2);
            infinity = 1/(gamma*sigma_min^2);
            d(d<epsilon) = epsilon;
%                 d(d>infinity) = infinity;

            D = diag(d);
            S2_new(inds,inds) = V*D*V';
        end

        E0 = [E0; obj_fun_sigma(S2_new,gamma)];
        if E0(end) >= E0(end-1)
            S = S2_old;
            break;
        else
            S2_old = S2_new;
            delta_t = delta_t*10;
        end
    end
    % Update gamma
    T = S + C;
    Th = T\h;
    gamma = gamma0Inv - 1/(N*B)*h'*Th;
    gamma = 1/gamma;
    % Check the amounts of ignored terms for estimating A and R
    if 1
        total_amt = gamma*lsq;
        amt1 = gamma*h'*((S+C)\h);
        amt2 = logdet(S+C) - logdet(S);
        ratio1(i) = amt1/total_amt;
        ratio2(i) = amt2/total_amt;
    end
    % Check convergence
    Es = [Es;obj_fun_sigma(S,gamma)];
    if Es(end-1) - Es(end) <= 1e-6*(Es(1) - Es(2))
        disp(['Finishes at iteration ',num2str(i),', current energy: ',num2str(Es(end))]);
        disp(['Final ignores are: ',num2str(calc_ignores(S,gamma))]);
        break;
    end

    if mod(i,5) == 0
        disp(['Process iteration ',num2str(i),', current energy: ',num2str(Es(end))]);
    end
end

mu = gamma^(-1/2);
d = (1/mu) * ones(B,1);

[sigma,var_dirs,var_amts] = calc_uncertainty_range(S,d);


function [hrf, fit, e, param] = Fit_sFIR(tc,TR,Run,T,mode)
% function [hrf, fit, e, param] = Fit_sFIR(tc,TR,Runs,T,mode)
%
% Fits FIR and smooth FIR model
%
% INPUTS:
%
% tc    - time course
% TR    - time resolution
% Runs  - expermental design
% T     - length of estimated HRF
% mode  - FIR or smooth FIR
%   options:
%       0 - standard FIR
%       1 - smooth FIR
%
% OUTPUTS:
%
% hrf   - estimated hemodynamic response function
% fit   - estimated time course
% e     - residual time course
% param - estimated amplitude, height and width
%
% Created by Martin Lindquist on 10/02/09
% Last edited: 05/26/10 (ML)

numstim = length(Run);
len = length(Run{1});

Runs = zeros(len,numstim);
for i=1:numstim,
    Runs(:,i) = Run{i};
end;

[DX] = tor_make_deconv_mtx3(Runs,T,1);

% DX2 = DX(:,1:T);
% num = T;

if mode == 1

    C=(1:T)'*(ones(1,T));
    h = sqrt(1/(7/TR));                       % 7 seconds smoothing - ref. Goutte

    v = 0.1;
    sig = 1;

    R = v*exp(-h/2*(C-C').^2);
    RI = inv(R);
    MRI = zeros(numstim*T+1);
    for i=1:numstim,
        MRI(((i-1)*T+1):(i*T),((i-1)*T+1):(i*T)) = RI;
    end;

    b = inv(DX'*DX+sig^2*MRI)*DX'*tc;
    fit = DX*b;
    e = tc - DX*b;

elseif mode == 0

    b = pinv(DX)*tc;
    fit = DX*b;
    e = tc - DX*b;

end

hrf =zeros(T,numstim);
param = zeros(3,numstim);

for i=1:numstim,
    hrf(:,i) = b(((i-1)*T+1):(i*T))';
    param(:,i) = get_parameters2(hrf(:,i),T);
end;

end


% Example for the adjacency matrix of
%      0     0     1     1     1    -1     1     0
%      0     0     1    -1     0     1     1    -1
%      1     1     0     0     0     1     0     0
%      1    -1     0     0     0     0    -1     0
%      1     0     0     0     0     1    -1     1
%     -1     1     1     0     1     0     1    -1
%      1     1     0    -1    -1     1     0     0
%      0    -1     0     0     1    -1     0     0
% and 100 iterations of simulations

load adj2
heider(adj2,100);


%DEMO_LGCP  Demonstration for a log Gaussian Cox process
%           with inference via EP or Laplace approximation
%
%  Description
%    Log Gaussian Cox process (LGCP) is a model for non-homogeneous
%    point-process in which the log intensity is modelled using
%    Gaussian process. LGCP can be modelled using log GP and
%    Poisson observation model in a discretized space.
%
%    The model constructed is as follows:
%
%    The number of occurrences of the realised point pattern within cell w_i
%
%         y_i ~ Poisson(y_i| |w_i|exp(f_i))
%
%    where |w_i| is area of cell w_i and f_i is the log intensity.
%
%    We place a zero mean Gaussian process prior for f =
%    [f_1, f_2,...,f_n] ~ N(0, K),
%
%    where K is the covariance matrix, whose elements are given as
%    K_ij = k(x_i, x_j | th). The function k(x_i, x_j | th) is
%    covariance function and th its parameters. We place a
%    prior for parameters, p(th).
%
%    The inference is conducted via EP or Laplace, where we find
%    Gaussian approximation for p(f| th, data), where th is the
%    maximum a posterior (MAP) estimate for the parameters.
%
%  See also  LGCP, DEMO_SPATIAL2
%
% Copyright (c) 2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% =====================================
% 1D-example
% =====================================
fprintf(['Coal disaster data with EP integration over the latent values\n'])

% Coal disaster data
S = which('demo_lgcp');
L = strrep(S,'demo_lgcp.m','demodata/coal.txt');
x = load(L);
xt = (1850:1963)';
[p1,pq1] = lgcp(x,xt,'gpcf',@gpcf_exp);

figure()
hp=patch([xt; xt(end:-1:1)],[pq1(:,1); pq1(end:-1:1,2)],[.9 .9 .9]);
set(hp,'edgecolor',[.9 .9 .9])
xlim([min(x) max(x)])
line(xt,p1,'linewidth',2);
line([x x],[5 5.3],'color','k')
line(xlim,[5.15 5.15],'color','k')
xlim([1850 1963])
ylim([0 5.29])
title('The coal mine disaster data, estimated intensity, and 90% interval')
xlabel('Year')
ylabel('Intensity')

% =====================================
% 2D-example
% =====================================
fprintf(['Redwood data with Laplace integration over the latent\n' ...
         'values and MAP estimate for the parameters\n'])

% Redwood data
S = which('demo_lgcp');
L = strrep(S,'demo_lgcp.m','demodata/redwoodfull.txt');
x=load(L);
x1min=min(x(:,1));x1max=max(x(:,1));
x2min=min(x(:,2));x2max=max(x(:,2));
[xt1,xt2]=meshgrid(linspace(x1min,x1max,100),...
                   linspace(x2min,x2max,100));
xt=[xt1(:) xt2(:)];
p2 = lgcp(x,xt,'range',[0 1 0 1],'latent_method','Laplace','gridn',20);

figure()
G=zeros(size(xt1));
G(:)=p2;
pcolor(xt1,xt2,G);
shading flat
colormap('jet')
cx=caxis;
cx(1)=0;
caxis(cx);
colorbar
h=line(x(:,1),x(:,2),'marker','.','linestyle','none','color','k','markersize',10);
colorbar
axis square
set(gca,'xtick',[0:.2:1],'ytick',[0:.2:1])
title('Redwood data and intensity estimate')


function [slug] = amu2slug(amu)
% Convert mass from atomic mass units to slugs.
% Chad Greene 2012
slug = amu*(3.660864489409*1e-27)/32.17405;


% [R,Q] = vgg_rq(S)  Just like qr but the other way around.
%
% If [R,Q] = vgg_rq(X), then R is upper-triangular, Q is orthogonal, and X==R*Q.
% Moreover, if S is a real matrix, then det(Q)>0.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% MVTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with MVTB.  If not, see <http://www.gnu.org/licenses/>.

% By awf

function [U,Q] = rq(S)

S = S';
[Q,U] = qr(S(end:-1:1,end:-1:1));
Q = Q';
Q = Q(end:-1:1,end:-1:1);
U = U';
U = U(end:-1:1,end:-1:1);

if det(Q)<0
  U(:,1) = -U(:,1);
  Q(1,:) = -Q(1,:);
end

return


function patches = construct_grid_even(grid_size,overlap,dim,min_diff)

N = length(dim);
start_point = cell(N,1);
end_point = cell(N,1);

for i = 1:N
    start_point{i} = [1:grid_size(i):dim(i)-grid_size(i)-overlap(i)+1,max(dim(i)-grid_size(i)-overlap(i)+1,1)];
    end_point{i} = [grid_size(i)+overlap(i):grid_size(i):dim(i),dim(i)];
    if length(start_point{i}) > 1
        if start_point{i}(end) - start_point{i}(end-1) < min_diff(i)
            start_point{i}(end-1) = [];
            end_point{i}(end-1) = [];
        end
    end
end

start_grid = cell(1,N);
[start_grid{:}] = ndgrid(start_point{:});

end_grid = cell(1,N);
[end_grid{:}] = ndgrid(end_point{:});
patches = zeros(numel(start_grid{1}),2*N);

for i = 1:N
    patches(:,2*i-1) = start_grid{i}(:);
    patches(:,2*i) = end_grid{i}(:);
end


classdef OptimalSuperEllipsePrinter < handle

    properties (Access = private)
        mesh
        meshBackground
        levelSet
        mx
        my
        iter
    end

    properties (Access = private)
        mxV
        myV
        fileName
    end

    methods (Access = public)

        function obj = OptimalSuperEllipsePrinter()
            obj.init();
            obj.compute();
        end

    end

    methods (Access = private)

        function init(obj)
            obj.mxV = [0.8462 0.99 0.01 0.99 0.30108 0.1 0.2 0.2  0.85];
            obj.myV = [0.8462 0.2  0.01 0.99 0.72687 0.1 0.6 0.95 0.85];
            obj.fileName = 'OptimaSuperEllipseMicroEllipse';
          %  iter = 0;
        end

        function compute(obj)
            for iTest = 1:length(obj.mxV)
                obj.iter = iTest;

                rho = SuperEllipseParamsRelator.rho(obj.mxV(obj.iter),obj.myV(obj.iter),32)

                obj.createMeshBackground();
                obj.createLevelSet();
                obj.createMesh();
                obj.print();
            end
        end

        function createMeshBackground(obj)
            s.testName = 'RVE_Square_Triangle_FineFine';
            obj.meshBackground = Mesh().createFromFile(s);
        end

        function createLevelSet(obj)
            sM.coord  = obj.meshBackground.coord;
            sM.connec = obj.meshBackground.connec;
            s.mesh = Mesh_Total(sM);
            s.widthH = obj.mxV(obj.iter);
            s.widthV = obj.myV(obj.iter);
            %s.pnorm  = obj.computeSmoothingExponent();
            s.pnorm  = 2;
            s.type = 'smoothRectangle';
            s.levelSetCreatorSettings = s;
            s.type = 'LevelSet';
            s.scalarProductSettings.epsilon = 1;
            obj.levelSet = LevelSet(s);
        end

        function q = computeSmoothingExponent(obj)
            s.m1 = obj.mxV(obj.iter);
            s.m2 = obj.myV(obj.iter);
            s.type = 'Optimal';
            qComputer = SmoothingExponentComputer.create(s);
            q = qComputer.compute();
        end

        function createMesh(obj)
            s.fileName = obj.fileName;
            s.levelSet = obj.levelSet.value;
            s.meshBackground = obj.meshBackground;
            mCreator = MeshCreatorFromLevelSetWithMMG(s);
            obj.mesh = mCreator.create();
        end

        function print(obj)
            outputName = [obj.fileName,'Print',num2str(obj.iter)];
            s.mesh       = obj.mesh;
            s.outPutName = outputName;
            printer = SuperEllipsePrinter(s);
            printer.print();
            printer.captureImage();
        end

    end

end


function [LX, MX, PX] = mppca(X, no_dims, no_analyzers, tol, maxiter, minstd)
%MPPCA Runs EM algorithm and computes local factor analyzers
%
%   [LX, MX, PX] = mppca(X, no_dims, no_analyzers, tol, maxiter, minstd)
%
% Runs EM algorithm to determine coordinates of factor analyzers. The data
% is given in the DxN matrix X. no_dims indicates the number of dimensions that
% the local factor analyzers compute their embedding in. The number of
% factor analyzers that is used is given by no_analyzers. The variable tol indicates
% the tolreance in considering the EM as converged, whereas maxiter
% indicates the maximum number of iterations for the EM algorithm. The
% variable minstd sets the minimum standard deviation of the Gaussians that
% the EM algorithm fits.
% The function returns in LX the lowdimensional representations of X of all
% factor analyzers, in MX the means of the factors analyzers, and in PX the
% noise covariances.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    % Initialize some variables
    [D N] = size(X);                % size of data
    epsilon = 1e-9;                 % regularization parameter

    % Estimate minimum variance allowed
    minvar = minstd ^ 2;

    % Randomly initialize factor analyzers
    mm = mean(X, 2);
    ss = cov(X');
    try                                                 % for small problems
        cc = chol(ss);
        MX = bsxfun(@plus, cc' * randn(D, no_analyzers), mm);
        LX = minstd * randn(D, no_dims, no_analyzers);
        PX = 2 * mean(diag(cc)) * ones(D, 1);
    catch                                               % for large problems (or nearly singular covariance matrices)
        cc = std(X, [], 2);
        MX = bsxfun(@plus, bsxfun(@times, cc, randn(D, no_analyzers)), mm);
        LX = minstd * randn(D, no_dims, no_analyzers);
        PX = 2 * mean(cc) * ones(D, 1);
    end
    clear mm ss cc

    % Compute squared data
    X2 = X .^ 2;

    % Initialize some variables
    const = -D / 2 * log(2 * pi);
    lik = -Inf;
    R  = zeros(no_analyzers, N);
    czz = zeros(no_dims, no_dims, no_analyzers);
    zz  = zeros(no_dims, N, no_analyzers);

    % Run for maxiter iterations at max
    for i=1:maxiter

        % Progress bar
        if rem(i, 10) == 0
            fprintf('.');
        end

        % E step
        pii = 1 ./ PX;
        for k=1:no_analyzers
            l        = LX(:,:,k);                                                                       % select k-th local representation
            ltpi     = bsxfun(@times, pii, l)';
            ltpil    = ltpi * l;
            iltpil   = eye(no_dims) + ltpil;
            cc       = chol(iltpil);
            cci      = inv(cc);
            covz     = cci * cci';                                                                      % compute covariance
            delta    = X - MX(:,k * ones(1, N));
            meanz    = ((eye(no_dims) - ltpil * covz) * ltpi) * delta;
            czz(:,:,k) = covz;
            zz(:,:,k)  = meanz;                                                                         % update local representation
            R(k,:)    = -.5 * (pii' * (delta .* delta) - sum(meanz .* (iltpil * meanz), 1)) - ...       % update reponsibilie
                          sum(log(diag(cc)));
        end

        % Compute responsibilities of datapoints to the clusters
        R = R + const + .5 * sum(log(pi));
        R = exp(bsxfun(@minus, R, max(R, [], 1)));
        R = bsxfun(@rdivide, R, sum(R, 1));

        % Update likelihood of estimation
        oldlik = lik;
        lik = sum(max(R, [], 1) + log(sum(R, 1)));

        % Stop EM after convergence
        if abs(oldlik - lik) < tol
            break;
        end

        % M step
        PX = 0;
        for k=1:no_analyzers                         % Update all factor analyzers
            r    = R(k,:);
            z    = zz(:,:,k);
            rz   = bsxfun(@times, z, r);
            sr   = sum(r);
            srz  = sum(rz, 2);
            srxz = X * rz';
            srx  = X * r';
            m1   = [srxz srx];
            m2   = [sr * czz(:,:,k) + z * rz' srz; srz' sr];
            m1   = m1 / (m2 + (rand(size(m2)) * epsilon));     % random regularization to make sure that INV(m2) does not contain Infs
            LX(:,:,k) = m1(:,1:no_dims);
            MX(:,k) = m1(:,no_dims + 1);
            PX = PX + X2 * r' - sum(LX(:,:,k) .* srxz, 2) - MX(:,k) .* srx;
        end
        PX = max(minvar, PX / N);
        PX(:) = mean(PX);
    end

    % Done
    disp(' ');


function [ml] = ft32ml(ft3)
% Convert volume from cubic feet to milliliters.
% Chad Greene 2012
ml = ft3*28316.846592;


function [Q] = spm_P_clusterFDR(k,df,STAT,R,n,ui,Ps)
% Return the corrected FDR q-value
% FORMAT [Q] = spm_P_clusterFDR(k,df,STAT,R,n,ui,Ps)
%
% k        - extent {RESELS}
% df       - [df{interest} df{residuals}]
% STAT     - Statistical field
%            'Z' - Gaussian field
%            'T' - T - field
%            'X' - Chi squared field
%            'F' - F - field
% R        - RESEL Count {defining search volume}
% n        - Conjunction number
% ui       - feature-inducing threshold
% Ps       - Vector of sorted (ascending) p-values

% Q        - FDR q-value
%__________________________________________________________________________
%
% References
%
% J.R. Chumbley and K.J. Friston, "False discovery rate revisited: FDR and
% topological inference using Gaussian random fields". NeuroImage,
% 44(1):62-70, 2009.
%
% J.R. Chumbley, K.J. Worsley, G. Flandin and K.J. Friston, "Topological
% FDR for NeuroImaging". Under revision.
%__________________________________________________________________________
% Copyright (C) 2009 Wellcome Trust Centre for Neuroimaging

% Justin Chumbley & Guillaume Flandin
% $Id: spm_P_clusterFDR.m 2764 2009-02-19 15:30:03Z guillaume $

% Compute uncorrected p-values based on k using Random Field Theory
%--------------------------------------------------------------------------
[P, Z] = spm_P_RF(1, k, ui, df, STAT, R, n);

% q value using the  Benjamini & Hochberch False Discovery Rate procedure
%--------------------------------------------------------------------------
Q = spm_P_FDR(Z, df, 'P', n, Ps);


function [ r,flag ] =  reward_grid_world_continuous(s,x,beta,s_goal,x_goal,x_bad,r_bad,r_goal)
%
%
%
if s == s_goal

    r    = r_goal;
    flag = true;

elseif ismember(s,x_bad)

    r    = r_bad;
    flag = true;

else
    r    = r_goal .* exp(-beta * pdist([x;x_goal]).^2);
    flag = false;

end

end


function traffic ( cycle_num )

%*****************************************************************************80
%
%% TRAFFIC simulates the cars waiting at one traffic light.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2009
%
%  Author:
%
%    Original MATLAB version by Brian Hahn, Dan Valentine.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Brian Hahn, Dan Valentine,
%    Essential MATLAB for Engineers and Scientists,
%    Academic Press, 2009,
%    ISBN13: 978-0123748836,
%    LC: TA345.V34.
%
%  Parameters:
%
%    Input, integer CYCLE_NUM, the number of 10-second time cycles to model.
%
%  Local Parameters:
%
%    Local, integer CARS, the number of cars waiting at the light.
%
%    Local, integer CARS_IN, the total number of cars that have come.
%
%    Local, integer CARS_OUT, the total number of cars that have left.
%
%    Local, integer CYCLE, the number of time cycles that have elapsed.
%
%    Local, integer CYCLE_LENGTH, the number of seconds in one time cycle.
%
%    Local, integer GREEN_CYCLES, the number of 10-second time cycles that
%    a green light lasts.
%
%    Local, integer GREEN_TIMER, keeps track of the number of time cycles the
%    green light has been on.
%
%    Local, integer LIGHT, the state of the light.
%    'r', the light is now red.
%    'g', the light is now green.
%
%    Local, real P, the probability that a new car will come to the light
%    in the next second.
%
%    Local, integer RED_CYCLES, the number of 10-second time cycles that
%    a red light lasts.
%
%    Local, integer RED_TIMER, keeps track of the number of time cycles the
%    red light has been on.
%

%
%  Initialize.
%
  cars = 0;
  cars_in = 0;
  cars_out = 0;
  car_wait_cycles = 0;
  cycle = 0;
  cycle_length = 10;
  green_cycles = 2;
  green_timer = 0;
  light = 'r';
  p = 0.3;
  red_cycles = 4;
  red_timer = 0;
%
%  Set up the plot data.
%
  plot_data = zeros(2,cycle_num+1);
%
%  Handle the "0"-th cycle.
%
  plot_data(1,cycle+1) = cycle;
  plot_data(2,cycle+1) = cars;

  prq ( cars, light, cycle );
%
%  Handle cycles 1 through CYCLE_NUM.
%
  for cycle = 1 : cycle_num
%
%  Each second of the cycle, choose a random number.
%  If it is less than P, then a new car appeared at the light at that second.
%
    r = rand ( cycle_length, 1 );
    cars_new = sum ( r < p );
    cars = cars + cars_new;
    cars_in = cars_in + cars_new;
%
%  Handle this time cycle depending on whether the light is green or red.
%
    if ( light == 'g' )
      [ cars, cars_out, light, green_timer ] = go ( green_cycles, cars, ...
        cars_out, light, green_timer );
    else
      [ cars, light, red_timer ] = stop ( red_cycles, cars, light, red_timer );
    end
%
%  At the end of this cycle, how many cars are waiting?
%
    car_wait_cycles = car_wait_cycles + cars;
%
%  Print the current status.
%
    prq ( cars, light, cycle );

    plot_data(1,cycle+1) = cycle;
    plot_data(2,cycle+1) = cars;

  end

  plot ( plot_data(1,1:cycle_num+1), plot_data(2,1:cycle_num+1) )
  xlabel ( 'Time Cycles' )
  ylabel ( 'Cars Waiting' )
  title ( 'Traffic waiting at a Light' )

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of cycles =       %d\n', cycle_num );
  fprintf ( 1, '  Simulated time =         %d seconds\n', cycle_num * cycle_length );
  fprintf ( 1, '  Number of cars in =      %d\n', cars_in );
  fprintf ( 1, '  Number of cars waiting = %d\n', cars );
  fprintf ( 1, '  Number of cars out =     %d\n', cars_out );
  fprintf ( 1, '  Percentage Out/In = %7.1f%%\n', 100 * cars_out / cars_in );
  wait_average_seconds = car_wait_cycles * cycle_length / cars_in;
  fprintf ( 1, '  Average wait = %7.2f seconds\n', wait_average_seconds );
  wait_average_lights = car_wait_cycles / cars_in / ( red_cycles + green_cycles );
  fprintf ( 1, '  Average wait = %7.2f light cycles\n', wait_average_lights );

  return
end
function [ cars, cars_out, light, green_timer ] = go ( green_cycles, cars, ...
  cars_out, light, green_timer )

%*****************************************************************************80
%
%% GO simulates traffic when the light is green.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 November 2009
%
%  Author:
%
%    Original MATLAB version by Brian Hahn, Dan Valentine.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Brian Hahn, Dan Valentine,
%    Essential MATLAB for Engineers and Scientists,
%    Academic Press, 2009,
%    ISBN13: 978-0123748836,
%    LC: TA345.V34.
%
%  Parameters:
%
%    Input, integer GREEN_CYCLES, the number of 10-second time cycles that
%    a green light lasts.
%
%    Input, integer CARS, the number of cars stopped at the light.
%
%    Input, integer CARS_OUT, the total number of cars that have gone
%    through the light.
%
%    Input, integer LIGHT, the state of the light.
%    'r', the light is now red.
%    'g', the light is now green.
%
%    Input, integer GREEN_TIMER, keeps track of the number of time cycles the
%    green light has been on.
%
%    Output, integer CARS, the number of cars stopped at the light.
%
%    Output, integer CARS_OUT, the total number of cars that have gone
%    through the light.
%
%    Output, integer LIGHT, the state of the light.
%    'r', the light is now red.
%    'g', the light is now green.
%
%    Output, integer GREEN_TIMER, keeps track of the number of time cycles the
%    green light has been on.
%

%
%  In one 10-second time cycle, we estimate 8 cars can move out.
%
  cars_through = min ( 8, cars );

  cars = cars - cars_through;
  cars_out = cars_out + cars_through;
%
%  Advance the timer.  If the green light has timed out, reset the timer
%  and switch to red.
%
  green_timer = green_timer + 1;

  if ( green_cycles <= green_timer )
    light = 'r';
    green_timer = 0;
  end

  return
end
function [ cars, light, red_timer ] = stop ( red_cycles, cars, light, ...
  red_timer )

%*****************************************************************************80
%
%% STOP simulates the traffic when the light is red.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2009
%
%  Author:
%
%    Original MATLAB version by Brian Hahn, Dan Valentine.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Brian Hahn, Dan Valentine,
%    Essential MATLAB for Engineers and Scientists,
%    Academic Press, 2009,
%    ISBN13: 978-0123748836,
%    LC: TA345.V34.
%
%  Parameters:
%
%    Input, integer RED_CYCLES, the number of 10-second time cycles that
%    a red light lasts.
%
%    Input, integer CARS, the number of cars stopped at the light.
%
%    Input, integer LIGHT, the state of the light.
%    'r', the light is now red.
%    'g', the light is now green.
%
%    Input, integer RED_TIMER, keeps track of the number of time cycles the
%    red light has been on.
%
%    Output, integer CARS, the number of cars stopped at the light.
%
%    Output, integer LIGHT, the state of the light.
%    'r', the light is now red.
%    'g', the light is now green.
%
%    Output, integer RED_TIMER, keeps track of the number of time cycles the
%    red light has been on.
%

%
%  Advance the timer.
%  If the red light has timed out, reset the timer and switch to green.
%
  red_timer = red_timer + 1;

  if ( red_cycles <= red_timer )
    light = 'g';
    red_timer = 0;
  end

  return
end
function prq ( cars, light, cycle )

%*****************************************************************************80
%
%% PRQ prints the current traffic waiting at the light.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2009
%
%  Author:
%
%    Original MATLAB version by Brian Hahn, Dan Valentine.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Brian Hahn, Dan Valentine,
%    Essential MATLAB for Engineers and Scientists,
%    Academic Press, 2009,
%    ISBN13: 978-0123748836,
%    LC: TA345.V34.
%
%  Parameters:
%
%    Input, integer CARS, the number of cars stopped at the light.
%
%    Input, integer LIGHT, the state of the light.
%    'r', the light is now red.
%    'g', the light is now green.
%
%    Input, integer CYCLE, the current 10-second time cycle.
%
  fprintf ( 1, '%4d ', cycle );
  if ( light == 'r' )
    fprintf ( 'R  ' );
  else
    fprintf ( 'G  ' );
  end
  i = cars;
  c = floor ( i / 100 );
  i = i - 100 * c;
  for j = 1 : c
    fprintf ( 'C' );
  end
  x = floor ( i / 10 );
  i = i - 10 * x;
  for j = 1 : x
    fprintf ( 'X' );
  end
  for j = 1 : i
    fprintf ( 'I' );
  end
  fprintf ( 1, '\n' );

  return
end


function edge = createEdge3d(varargin)
%CREATEEDGE3D Create an edge between two 3D points, or from a 3D line.
%
%   E = createEdge3d(P1, P2)
%   Creates the 3D edge joining the two points P1 and P2.
%
%   E = createEdge3d(LIN)
%   Creates the 3D edge with same origin and same direction vector as the
%   3D line LIN.
%
%   Example
%     p1 = [1 1 1];
%     p2 = [3 4 5];
%     edge = createEdge3d(p1, p2);
%     edgeLength3d(edge)
%     ans =
%         5.3852
%
%   See also
%     edges3d, drawEdge3d, clipEdge3d, edgelength3d

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2018-08-29, using Matlab 9.4.0.813654 (R2018a)
% Copyright 2018-2022 INRA - Cepia Software Platform

if nargin == 1
    % Only one input parameter. Assumes it corresponds to a 3D line with
    % 6 params.
    var = varargin{1};

    if size(var, 2) ~= 6
        error('single input must have 6 columns');
    end

    % converts 3D line into 3D edge
    edge = zeros(size(var));
    edge(:, 1:3) = var(:, 1:3);
    edge(:, 4:6) = edge(:, 1:3) + var(:,4:6);

elseif nargin == 2
    % 2 input parameters correspond to two 3D points

    % extract the two arguments
    v1 = varargin{1};
    v2 = varargin{2};

    if size(v1, 2) ~= 3 || size(v2, 2) ~= 3
        error('Input points must be arrays with 3 columns');
    end

    % first input parameter is first point, and second input is the
    % second point. Allows multiple points.
    n1 = size(v1, 1);
    n2 = size(v2, 1);
    if n1 == n2
        edge = [v1 v2];
    elseif n1 == 1 || n2 == 1
        edge = [repmat(v1, n2, 1) repmat(v2, n1, 1)];
    end

else
    error('Wrong number of arguments in ''%s''', mfilename);
end


%% Interpolating EBSD Data
%
%%
% In the section <EBSDDenoising.html Denoising> and <EBSDFilling.html
% Filling Missing Data> we have discussed how to work with noisy EBSD data
% the contained non indexed pixels. Hereby, we made the assumption that the
% grid before and after the operations is the same.
%
% In this section we explain how to interpolate an EBSD map at positions
% that do not belong to the grid. Lets us consider a simple example

mtexdata twins;

[grains, ebsd.grainId] = calcGrains(ebsd('indexed'));

% this command here is important :)
ebsd = ebsd.project2FundamentalRegion(grains);

plot(ebsd('indexed'),ebsd('indexed').orientations)

%%
% Now we can use the command <EBSD.interp.html |interp|> to interpolate the
% orientation at arbitrary coordinates |x| and |y|.

x = 30.5; y = 5.5;
e1 = interp(ebsd,x,y)

%%
% By default the command <EBSD.interp.html |interp|> performs inverse
% distance interpolation. This is different to

e2 = ebsd('xy',x,y)

%%
% which returns the nearest neighbour EBSD measurement. Lets have a look at
% the difference

angle(e1.orientations,e2.orientations)./degree

%% Change of the measurement grid
% The command <EBSD.interp.html |interp|> can be used to evaluate the EBSD
% map on a different grid, which might have higher or lower resolution or
% might even be rotated. Lets demonstrate this

% define a rotated coarse grid
omega = 5*degree;
[xmin, xmax, ymin, ymax] = ebsd.extent;
x = linspace(xmin-cos(omega)*ymax,xmax,100);
y = linspace(ymin-sin(omega)*xmax,ymax,50);
[x,y] = meshgrid(x,y);

xy = [cos(omega) -sin(omega); sin(omega) cos(omega) ] * [x(:),y(:)].';

% define the EBSD data set on this new grid
ebsdNewGrid = interp(ebsd,xy(1,:),xy(2,:))

% plot the regridded EBSD data set
plot(ebsdNewGrid('indexed'),ebsdNewGrid('indexed').orientations)

%%
% Note, that we have not rotated the EBSD data but only the grid. All
% orientations as well as the position of all grains remains unchanged.
%
% Another example is the change from a square to an hexagonal grid or vice
% versa. In this case the command <EBSD.interp.html |interp|> is
% implicitely called by the command <EBSD.gridify.html |gridify|>. In order
% to demonstrate this functionality we start by EBSD data on a hex grid

mtexdata ferrite silent

plot(ebsd,ebsd.orientations)

%%
% and resample the data on a square grid. To do so we first define a
% smaller square unit cell corresponding to the hexagonal unit cell

% define a square unit cell
hexUnitCell = abs(round(ebsd.unitCell,4));
minUnit = min(hexUnitCell(hexUnitCell>0));
squnitCell = minUnit * [-1 -1;-1 1; 1 1; 1 -1];

% use the square unit cell for gridify
ebsd = ebsd.gridify('unitCell',squnitCell);

plot(ebsd,ebsd.orientations)


%%

cs = crystalSymmetry('m-3m');
ss = specimenSymmetry('1');

odf = unimodalODF(quaternion.id,cs,ss,'halfwidth',1.5*degree);

%%

[~,q] = max(odf)

%%
v = [];
r = linspace(1*degree,10*degree,10);
for i = 1:length(r)
  fprintf('.');
  v(i,1) = volume(odf,q,r(i));
  v(i,2) = volume(odf,quaternion.id,r(i));
  %v(i,1) = volume(uniformODF(cs,ss),quaternion.id,r(i));
end
fprintf('\n');
%v(:,2) = length(cs)*(r - sin(r))./pi;

plot(r/degree,v);

%%
clear v;
cs = crystalSymmetry('m-3m');
ss = specimenSymmetry('mmm');
r = linspace(0*degree,60*degree,20);
omega = linspace(0,45*degree,4);
for i = 1:length(r)
  for j = 1:length(omega)
    %disp(r(i));
    %tic
    v(i,j,1) = volume(uniformODF(cs,ss),axis2quat(vector3d(1,1,1),omega(j)),r(i));
    %toc
    %tic
    %v(i,j,2) = volume(uniformODF(cs,ss),axis2quat(xvector,omega(j)),r(i),'local');
    %toc
    fprintf('.');
  end
end
fprintf('\n');
plot(r/degree,reshape(v,size(v,1),[]));

%%

%r = linspace(0,100*degree,20);
%for i = 1:length(r)
%  v(i,1) = volume(odf,calcModes(odf),r(i));
%  v(i,2) = volume(uniformODF(cs,ss),quaternion.id,r(i));
%end

%plot(r/degree,v);

%%
cs = crystalSymmetry('m-3m');
ss = specimenSymmetry('mmm');
S3G = SO3Grid(1*degree,cs,ss);
r = plotS2Grid('resolution',10*degree,'hemisphere','upper','maxrho',90*degree,'RESTRICT2MINMAX');
rv = vector3d(r);
d1 = zeros(size(rv));
d2 = zeros(size(rv));
for i = 1:length(rv)
  q = axis2quat(rv(i),20*degree);
  d1(i) = length(find(dot_outer(S3G,q,'epsilon',20*degree)));
  d2(i) = volume(uniformODF(cs,ss),q,15*degree);
  fprintf('.');
end
fprintf('\n');
figure(1)
plot(r,d1./d1(1),'smooth')
figure(2)
plot(r,d2./d2(1),'smooth')
colorbar
%%


function J=calcSpherJacob(x,systemType,useHalfRange,lTx,lRx,M)
%%CALCSPHERJACOB Calculate the Jacobian for a monostatic or bistatic
%           spherical measurement with respect to 3D Cartesian position.
%
%INPUTS: x The 3X1 position of the target in Cartesian coordinates in the
%          order [x;y;z].
% systemType An optional parameter specifying the axis from which the
%          angles are measured in radians. Possible values are
%          0 (The default if omitted) Azimuth is measured
%            counterclockwise from the x-axis in the x-y plane. Elevation
%            is measured up from the x-y plane (towards the z-axis). This
%            is consistent with common spherical coordinate systems for
%            specifying longitude (azimuth) and geocentric latitude
%            (elevation).
%          1 Azimuth is measured counterclockwise from the z-axis in the
%            z-x plane. Elevation is measured up from the z-x plane
%            (towards the y-axis). This is consistent with some spherical
%            coordinate systems that use the z axis as the boresight
%            direction of the radar.
%          2 This is the same as 0 except instead of being given
%            elevation, one desires the angle away from the z-axis, which
%            is (pi/2-elevation).
%          3 This is the same as 0 except azimuth is measured clockwise
%            from the y-axis in the x-y plane instead of counterclockwise
%            from the x-axis. This coordinate system often arises when
%            given "bearings" in a local East-North-Up coordinate system,
%            where the bearing directions are measured East of North.
% useHalfRange An optional boolean value specifying whether the bistatic
%          (round-trip) range value has been divided by two. This normally
%          comes up when operating in monostatic mode (the most common
%          type of spherical coordinate system), so that the range
%          reported is a one-way range (or just half a bistatic range).
%          The default if this parameter is not provided is false if lTx
%          is provided and true if it is omitted (monostatic).
%      lTx The 3X1 transmitter position in the global coordinate system
%          with [x;y;z] components. If omitted or an empty matrix is
%          passed, then a vector of zeros is used.
%      lRx The 3X1 receiver position in the global coordinate system
%          with [x;y;z] components. If omitted or an empty matrix is
%          passed, then a vector of zeros is used.
%        M A 3X3 rotation matrices to go from the alignment of the global
%          coordinate system to that at the receiver. If omitted, then it
%          is assumed that the local coordinate system is aligned with the
%          global and M=eye(3) --the identity matrix is used.
%
%OUTPUTS: J A 3X3 Jacobian matrix where the rows are
%          [bistatic range;azimuth;elevation] in that order and the columns
%          take the derivative of the row component with respect to
%          [x,y,z] in that order.
%
%This function just calls rangeGradient and spherAngGradient.
%
%February 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6||isempty(M))
    M=eye(3,3);
end

if((nargin<4||isempty(lTx))&&(nargin<3||isempty(useHalfRange)))
    useHalfRange=true;
elseif(nargin<3||isempty(useHalfRange))
    useHalfRange=false;
end

if(nargin<5||isempty(lRx))
    lRx=zeros(3,1);
end

if(nargin<4||isempty(lTx))
    lTx=zeros(3,1);
end

if(nargin<2||isempty(systemType))
    systemType=0;
end

J=[rangeGradient(x(1:3,:),useHalfRange,lTx(1:3),lRx(1:3));
   spherAngGradient(x(1:3,:),systemType,lRx(1:3),M)];
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


% Chapter 8 - Planar Systems.
% Program_8a - Program to Plot a Vector Field.
% Save M-file as vectorfield.m. Do NOT run this function file.
% Copyright Birkhauser 2013. Stephen Lynch.

% See the phase protraits in the book.
function vectorfield(deqns,xval,yval,t)
if nargin==3;
    t=0;
end
m=length(xval);
n=length(yval);
x1=zeros(n,m);
y1=zeros(n,m);
for a=1:m
  for b=1:n
    pts = feval(deqns,t,[xval(a);yval(b)]);
    x1(b,a) = pts(1);
    y1(b,a) = pts(2);
  end
end
arrow=sqrt(x1.^2+y1.^2);
quiver(xval,yval,x1./arrow,y1./arrow,.5,'r');
axis tight;

% End of Program_8a.


classdef RegularizedSuperEllipseExponentPlotter < handle

    properties (Access = private)
        vademecum
        mesh
        errorF
        qAveraged
        qRegularized
        qComputer
    end

    methods (Access = public)

        function obj = RegularizedSuperEllipseExponentPlotter()
            obj.createQcomputer();
            obj.loadVademecum();
            obj.createMesh();
            obj.createErrorFunction();
            obj.computeNumericalAveragedValue();
            obj.computeQregularized();
            obj.plotQforSymmetricHoles();
            obj.plotSuperEllipseOptimalExponent();
        end

    end

    methods (Access = private)

        function createQcomputer(obj)
            s.m1 = [];
            s.m2 = [];
            obj.qComputer = SmoothingExponentComputerOptimal(s);
        end

        function loadVademecum(obj)
            obj.vademecum = VademecumReader();
        end

        function createMesh(obj)
            xi = obj.vademecum.xiV;
            rho = obj.vademecum.rhoV;
            obj.mesh = obj.obtainMesh(xi,rho);
        end

        function computeNumericalAveragedValue(obj)
            s.vademecum = obj.vademecum;
            p = PonderatedOptimalSuperEllipseComputer(s);
            p.compute();
            obj.qAveraged = p.qMean;
        end

        function error = computeError(obj,a,b,r,qS)
            q = obj.computeQ(a,b,r,qS);
            error = obj.errorF(q);
        end

        function q = computeQ(obj,a,b,r,q)
             obj.qComputer.setParamsValues(a,b,r,q)
             xi  = obj.vademecum.xiV;
             rho = obj.vademecum.rhoV;
             q = obj.qComputer.computeQ(xi,rho);
        end

        function [alpha,beta,rhoQmin,qSoptMin] = computeBestParameters(obj)
            s.errorComputer = @(a,b,c,d) obj.computeError(a,b,c,d);
            r = RegularizedExponentBestCoeffComputer(s);
            [alpha,beta,rhoQmin,qSoptMin] = r.compute();
        end

        function createErrorFunction(obj)
            s.type              = 'SIMPLE';
            s.mesh              = obj.mesh;
            s.backgroundMesh    = obj.mesh;
            s.globalConnec      = obj.mesh.connec;
            s.npnod = obj.mesh.nnodes;
            int = Integrator.create(s);
            int.computeLHS();
            p = 2;
            normF = @(x) ((int.computeL2norm(x)).^(p))^(1/p);
            obj.errorF = @(x) normF(abs(x - obj.qAveraged))/normF(obj.qAveraged);
        end

        function computeQregularized(obj)
            [alpha,beta,rhoQmin,qSoptMin] = obj.computeBestParameters();
            q = obj.computeQ(alpha,beta,rhoQmin,qSoptMin);
            obj.qRegularized = q;
        end

        function plotQforSymmetricHoles(obj)
            xi     = obj.vademecum.xiV;
            isSym  = abs(xi-pi/4) < 0.01;
            isSym  = isSym(2:end);
            rhoSym = obj.vademecum.rhoV(isSym);
            qAsym  = obj.qAveraged(isSym);
            qRsym  = obj.qRegularized(isSym);
            f = figure();
            hold on
            h{1} = plot(rhoSym,qAsym,'-+');
            h{2} = plot(rhoSym,qRsym,'-+');
            xlabel('$\rho$','Interpreter','Latex')
            ylabel('$q(\xi = \pi/4,\rho)$','Interpreter','Latex')
            legN = '$\textrm{Numerical smoothing exponent} \, q_N$';
            legA = '$\textrm{Analytical smoothing exponent} \, q_A$';
            legend({legN,legA},'Interpreter','Latex','Location','Best');
            outPutPath = '/home/alex/git-repos/MicroStructurePaper/';
            outputName = [outPutPath,'qMaxM1M2AnalyticalNumerical'];
            printer = plotPrinter(f,h);
            printer.print(outputName);
        end

        function plotSuperEllipseOptimalExponent(obj)
            s.title = 'Proposed ';
            s.fileName = '/home/alex/git-repos/MicroStructurePaper/ProposedOptimal';
            s.rhoV = rho;
            s.xiV = xi;
            s.value = qA;
            s.qMean = qA;
            p = SuperEllipseExponentPlotter(s);
            p.plot();
        end

        function m = obtainMesh(obj,x,y)
            s.coord = [x,y];
            s.connec = obj.obtainConnec(x,y);
            m = Mesh().create(s);
        end

        function connec = obtainConnec(obj,x,y)
            connec = delaunay(x,y);
            connec = obj.obtainQualityElements(connec,x,y);
        end

    end

    methods (Access = private, Static)

        function connec = obtainQualityElements(connec,x,y)
            s.coord = [x,y];
            s.connec = connec;
            m = Mesh().create(s);
            qua = m.computeElementQuality';
            isQ = qua > 0.02;
            connec = connec(isQ,:);
        end

    end
end


function [data,units] = compute_dwing_angle_imbalance(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  imbalancer = trx(fly).wing_angler+trx(fly).wing_anglel;
  imbalancel = -imbalancer;
  dimbalancer = diff(imbalancer);
  dimbalancel = diff(imbalancel);
  data{i} = dimbalancel;
  idx = imbalancer(1:end-1) > imbalancel(1:end-1);
  data{i}(idx) = dimbalancer(idx);

  data{i} = data{i} ./ trx(fly).dt;

end
units = parseunits('rad/s');


function[varargout]=arrayify(varargin)
%ARRAYIFY  Converts a set of scalars or arrays into column arrays.
%
%   [O1,O2,O3,...,ON]=ARRAYIFY(I1,I2,I3,...,IN) where each of the IN is
%   either a scalar or an array with M elements, converts each input
%   variable into a length M column vector.
%
%   The input variables that are M-element arrays are resized to column
%   vectors, while scalars are replicated to have M identical elements.
%
%   ARRAYIFY will return an error if any of the input variables that are
%   not scalars have different numbers of elements from each other.
%
%   ARRAYIFY(I1,I2,...IN); with no output arguments overwrites the original
%   input variables.
%
%   'arrayify --t' runs a test.
%
%   Usage: [o1,o2,o3]=arrayify(i1,i2,i3);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2013--2016 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
    arrayify_test,return
end

M=zeros(length(varargin),1);
for i=1:length(varargin)
    varargin{i}=varargin{i}(:);
    M(i)=length(varargin{i});
end

if any(M>1)
    if any(M(M~=1)~=max(M))
        error('Input parameters must either be scalars or arrays having the same number of elements.')
    end
    M=max(M);
    for i=1:length(varargin)
        varargin{i}=varargin{i}+0*ones(M,1);
    end
    varargout=varargin;
    eval(to_overwrite(nargin));
else
    %Do nothing.  No need to overwrite in this case.
    varargout=varargin;
end

function[]=arrayify_test

[o1,o2,o3]=arrayify(4,1:10,[1:10]');

reporttest('ARRAYIFY',aresame(o1,4+0*[1:10]')&aresame(o2,[1:10]')&aresame(o3,[1:10]'))

i1=4;
i2=1:10;
i3=[1:10]';

arrayify(i1,i2,i3);

reporttest('ARRAYIFY with overwriting',aresame(i1,4+0*[1:10]')&aresame(i2,[1:10]')&aresame(i3,[1:10]'))


maxmag = ceil(10*max(newt2.Magnitude))/10;
mima = min(newt2.Magnitude);
if mima > 0 ; mima = 0 ; end

[bval,xt2] = hist(newt2.Magnitude,(mima:0.1:maxmag));
% normalise to annula rates
bval = bval/(max(newt2.Date)-min(newt2.Date));
bvalsum = cumsum(bval); % N for M <=
bval2 = bval(length(bval):-1:1);
bvalsum3 = cumsum(bval(length(bval):-1:1));    % N for M >= (counted backwards)
xt3 = (maxmag:-0.1:mima);

backg_ab = log10(bvalsum3);

figure_w_normalized_uicontrolunits(bfig);delete(gca);delete(gca); delete(gca); delete(gca)
rect = [0.22,  0.3, 0.65, 0.6];           % plot Freq-Mag curves
axes('position',rect);

%%
% plot the cum. sum in each bin  %%
%%

%pl =semilogy(xt3,bvalsum3,'sb');
%set(pl,'LineWidth',1.0,'MarkerSize',6,...
%    'MarkerFaceColor','w','MarkerEdgeColor','k');
%hold on
pl1 =semilogy(xt3,bval2,'^b');
set(pl1,'LineWidth',1.0,'MarkerSize',4,...
    'MarkerFaceColor',[0.7 0.7 .7],'MarkerEdgeColor','k');

bv2 = [];bv3 = [] ; me = [];BV = [];
ni2 = 50;

for i = 1:ni2/1:length(newt2)-ni2
    [bv magco stan ] =  bvalca2(newt2(i:i+ni2,:));
    l = newt2(i:i+ni2,:) >= magco;
    nn2 = newt2(i:i+ni2,l);

    [bval,xt2] = hist(nn2(:,6),(mima:0.1:maxmag));
    % normalise to annula rates
    bval = bval/(max(nn2(:,3))-min(nn2(:,3)));
    bvalsum = cumsum(bval); % N for M <=
    bval2 = bval(length(bval):-1:1);
    bvalsum3 = cumsum(bval(length(bval):-1:1));    % N for M >= (counted backwards)

    hold on
    pl1 =semilogy(xt3,bval2,'^b');
    set(pl1,'LineWidth',1.0,'MarkerSize',4,...
        'MarkerFaceColor',[0.7 0.7 .7],'MarkerEdgeColor','k');
    pause

end

bv2 = [bv2 ; magco newt2(i+ni2/2,3)];
BV = [BV ; magco newt2(i,3) ; magco newt2(i+ni2,3) ; inf inf];


function grains = smooth(grains,iter,varargin)
% constraint laplacian smoothing of grain boundaries
%
% Input
%  grains - @grain2d
%  iter   - number of iterations (default: 1)
%
% Output
%  grains - @grain2d
%
% Options
%  moveTriplePoints  - do not exclude triple/quadruple points from smoothing
%  moveOuterBoundary - do not exclude outer boundary from smoothing
%  second_order, S2  - second order smoothing
%  rate              - default smoothing kernel
%  gauss             - gaussian smoothing kernel
%  exp               - exponential smoothing kernel
%  umbrella          - umbrella smoothing kernel

if nargin < 2 || isempty(iter), iter = 1; end

% compute incidence matrix vertices - faces
I_VF = [grains.boundary.I_VF,grains.innerBoundary.I_VF];

% compute vertice adjacency matrix
A_V = I_VF * I_VF';
t = size(A_V,1);

% do not consider triple points
if check_option(varargin,'moveTriplePoints')
  ignore = false(size(A_V,1),1);
else
  ignore = full(diag(A_V)) > 2;
end

% ignore outer boundary
if ~check_option(varargin,'moveOuterBoundary')
  ignore(grains.boundary.F(any(grains.boundary.grainId==0,2),:)) = true;
end

if check_option(varargin,{'second order','second_order','S','S2'})
  A_V = logical(A_V + A_V*A_V);
  A_V = A_V - diag(diag(A_V));
end

weight = get_flag(varargin,{'gauss','expotential','exp','umbrella','rate'},'rate');
lambda = get_option(varargin,weight,.5);

V = full(grains.V);
isNotZero = ~all(~isfinite(V) | V == 0,2) & ~ignore;

for l=1:iter
  if ~strcmpi(weight,'rate')
    [i,j] = find(A_V);
    d = sqrt(sum((V(i,:)-V(j,:)).^2,2)); % distance
    switch weight
      case 'umbrella'
        w = 1./(d);
        w(d==0) = 1;
      case 'gauss'
        w = exp(-(d./lambda).^2);
      case {'expotential','exp'}
        w = lambda*exp(-lambda*d);
    end

    A_V = sparse(i,j,w,t,t);
  end

  % take the mean over the neigbours
  Vt = A_V*V;

  m = sum(A_V,2);

  dV = V(isNotZero,:)-bsxfun(@rdivide,Vt(isNotZero,:),m(isNotZero,:));

  isZero = any(~isfinite(dV),2);
  dV(isZero,:) = 0;

  V(isNotZero,:) = V(isNotZero,:) - lambda*dV;

end

grains.V = V;


function [D]=patchEdgeLengths(F,V)

% function [D]=patchEdgeLengths(F,V)
% -----------------------------------------------------------------------
% Computes the edge lengths (D) for the patch data specified by the faces
% (F) and vertices (V) arrays. If size(F,2)>2 it is assumed that F indeed
% represents faces. If however size(F,2)==2 it is instead assumed that F is
% an array representing edges. As such it skips the computation of the
% edges array. The edges array used is non-unique by default. See the
% |patchEdges| function for more details if the lengths of a unique set of
% edges is desired.
%
%
% See also: |patchEdges|
%
% Kevin Mattheus Moerman
% gibbon.toolbox@gmail.com
%
% 2014/03/17
%------------------------------------------------------------------------

%%

%Derive edge array
if size(F,2)>2 %The input is assumed to represent faces hence an edge array is derived
    E=patchEdges(F);
else %It is assumed that the input array represents an edges array
    E=F;
end

%Derive edge vertex arrays
V_E1=V(E(:,1),:);
V_E2=V(E(:,2),:);

%Derive difference vectors
VD=(V_E1-V_E2);

%Compute the edge lengths
D=sqrt(sum(VD.^2,2));

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2017  Kevin Mattheus Moerman
%
% 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 3 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 should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.