%  Figure 10.6      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% fig10_06.m is a script to generate Fig. 10.6
% frequency response for the PD design for the satellite
clf;
% parameters for the two-mass spring model
m=[1 .1]; k0=[0 .091] ; d0=[0 .0036]; k1=[0 .4];

% call function
[f,g,h,j]=twomass(m,k0,d0);
nc1=0.25*[2 1];
dc1=[1/40 1];

% convert to state-space
[ac,bc,cc,dc]=tf2ss(nc1, dc1);

% series of controller and plant
[aol,bol,col,dol]= series(ac, bc,cc,dc,f,g,h,j);
[acl]=aol-bol*col;
w=logspace(-1,1);w(26)=1;
[magcl1, phcl1]= bode(aol,bol,col,dol,1,w);
subplot(211) ;
loglog(w,[magcl1, ones(size(magcl1))]);
xlabel('\omega (rad/sec)');
ylabel('Magnitude, |KD_1(s)G(s)|');
title('Fig. 10.6 Frequency response for the satellite PD design')
grid
subplot(212);
semilogx(w, [phcl1, -180*ones(size(phcl1))]); grid
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');

%Bode grid
bodegrid;


function [x,g,j,gg] = kmeans(d,k,x0,l)
%KMEANS Vector quantisation using K-means algorithm [X,ESQ,J]=(D,K,X0,L)
%
%  Inputs:
%
%    D(N,P)  contains N data vectors of dimension P
%    K       is number of centres required
%    X0(K,P) are the initial centres (optional)
%
%      or alternatively
%
%    X0      gives the initialization method
%            'f'   pick K random elements of D as the initial centres [default]
%            'p'   randomly divide D into K sets and choose the centroids
%    L       gives max number of iterations (use 0 if you just want to calculate G and J)
%
%  Outputs:
%
%    X(K,P)  is output row vectors (omitted if L=0)
%    G       is mean square error
%    J(N)    indicates which centre each data vector belongs to
%    GG(L)   gives the mean square error at the start of each iteration (omitted if L=0)
%
% It is often a good idea to scale the input data so that it has equal variance in each
% dimension before calling KMEANS.

%  Originally based on a routine by Chuck Anderson, anderson@cs.colostate.edu, 1996

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

memsize=voicebox('memsize');
[n,p] = size(d);
nb=min(n,max(1,floor(memsize/(8*p*k))));    % block size for testing data points
nl=ceil(n/nb);                  % number of blocks
if nargin<4
    l=300;                  % very large max iteration count
    if nargin<3
        x0='f';             % use 'f' initialization mode
    end
end
if ischar(x0)
    if k<n
        if any(x0)=='p'                  % Initialize using a random partition
            ix=ceil(rand(1,n)*k);       % allocate to random clusters
            ix(rnsubset(k,n))=1:k;      % but force at least one point per cluster
            x=zeros(k,p);
            for i=1:k
                x(i,:)=mean(d(ix==i,:),1);
            end
        else                                % Forgy initialization: choose k random points [default]
            x=d(rnsubset(k,n),:);         % sample k centres without replacement
        end
    else
        x=d(mod((1:k)-1,n)+1,:);    % just include all points several times
    end
else
    x=x0;
end
m=zeros(n,1);           % minimum distance to a centre
j=zeros(n,1);           % index of closest centre
gg=zeros(l,1);
wp=ones(1,p);
kk=1:p;
kk=kk(ones(n,1),:);
kk=kk(:);

if l>0
    for ll=1:l                 % loop until x==y causes a break

        % find closest centre to each data point [m(:),j(:)] = distance, index

        ix=1;
        jx=n-nl*nb;
        for il=1:nl
            jx=jx+nb;        % increment upper limit
            ii=ix:jx;
            z = disteusq(d(ii,:),x,'x');
            [m(ii),j(ii)] = min(z,[],2);
            ix=jx+1;
        end
        y = x;              % save old centre list

        % calculate new centres as the mean of their assigned data values (or zero for unused centres)

        nd=full(sparse(j,1,1,k,1));         % number of points allocated to each centre
        md=max(nd,1);                       % remove zeros
        jj=j(:,wp);
        x=full(sparse(jj(:),kk,d(:),k,p))./md(:,wp);    % calculate the new means
        fx=find(nd==0);

        % if any centres are unused, assign them to data values that are not exactly on centres
        % choose randomly if there are more such points than needed

        if ~isempty(fx)
            q=find(m~=0);
            if length(q)<=length(fx)
                x(fx(1:length(q)),:)=d(q,:);
            else
                if length(fx)>1
                    [rr,ri]=sort(rand(length(q),1));
                    x(fx,:)=d(q(ri(1:length(fx))),:);
                else
                    x(fx,:) = d(q(ceil(rand(1)*length(q))),:);
                end
            end
        end

        % quit if the centres are unchanged

        gg(ll)=sum(m,1);
        if x==y
            break
        end
    end
    gg=gg(1:ll)/n;
%     ll % *** DEBUG ***
%     gg' % *** DEBUG ***
    g=gg(end);
else            % if l==0 then just calculate G and J (but rename as X and G)
    ix=1;
    jx=n-nl*nb;
    for il=1:nl
        jx=jx+nb;        % increment upper limit
        ii=ix:jx;
        z = disteusq(d(ii,:),x,'x');
        [m(ii),j(ii)] = min(z,[],2);
        ix=jx+1;
    end
    x=sum(m,1)/n;
    g=j;
end


function x = stft_iw(Sx, opt)
% ISTFT  Inverse short-time Fourier transform
%
% Very closely based on Steven Schimmel's stft.m and istft.m from
% his SPHSC 503: Speech Signal Processing course at Univ. Washington.

if nargin<2, opt = struct(); end

if ~isfield(opt, 'window'), opt.window = round(size(Sx,2)/16); end
if length(opt.window) == 1, opt.window = hamming(opt.window); end
opt.overlap = length(opt.window)-1;

window = opt.window / norm(opt.window, 2); % Unit norm

Nwin = length(window);
n = size(Sx, 2);

% regenerate the full spectrum 0...2pi (minus zero Hz value)
Sx = [Sx; conj(Sx(floor((Nwin+1)/2):-1:2,:))];

% take the inverse fft over the columns
xbuf = real(ifft(Sx,[],1));

% apply the window to the columns
xbuf = xbuf .* repmat(window(:),1,size(xbuf,2));

% overlap-add the columns
x = unbuffer(xbuf,Nwin,opt.overlap);

%%% subfunction
function y = unbuffer(x,w,o)
% UNBUFFER  undo the effect of 'buffering' by overlap-add (see BUFFER)
%    A = UNBUFFER(B,WINDOWLEN,OVERLAP) returns the signal A that is
%    the unbuffered version of B.

y    = [];
skip = w - o;
N    = ceil(w/skip);
L    = (size(x,2) - 1) * skip + size(x,1);

% zero pad columns to make length nearest integer multiple of skip
if size(x,1)<skip*N, x(skip*N,end) = 0; end;

% selectively reshape columns of input into 1-d signals
for i = 1:N
    t = reshape(x(:,i:N:end),1,[]);
    l = length(t);
    y(i,l+(i-1)*skip) = 0;
    y(i,[1:l]+(i-1)*skip) = t;
end;

% overlap-add
y = sum(y,1);
y = y(1:L);


% DEMCLASSIFICATION3 IVM for classification on a data-set sampled from a GP with null category.

% IVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'classificationThree';
experimentNo = 3;

% load data
[X, y] = mapLoadData(dataSetName);

% Set up model
options = ivmOptions;
options.display = 2;
options.kern = {'rbf', 'white'};

model = ivmCreate(size(X, 1), size(y, 2), X, y, options);

if options.display > 1
  ivm3dPlot(model, 'ivmContour', i);
end
for i = 1:options.extIters;

  % Select the active set.
  model = ivmOptimiseIVM(model, options.display);
  % Plot the data.
  if options.display > 1
    ivm3dPlot(model, 'ivmContour', i);
  end
  % Optimise the kernel parameters.
  model = ivmOptimiseKernel(model, options.display, options.kernIters);
end
model = ivmOptimiseIVM(model, options.display);
if options.display > 1
  ivm3dPlot(model, 'ivmContour', i);
end
% display active points.
model = ivmOptimiseIVM(model, options.display);

% Display the final model.
ivmDisplay(model);

% Save the results.
capName = dataSetName;;
capName(1) = upper(capName(1));
[kern, noise, ivmInfo] = ivmDeconstruct(model);
save(['dem' capName num2str(experimentNo) '.mat'], ...
     'kern', ...
     'noise', ...
     'ivmInfo');

if exist('printDiagram') & printDiagram
  ivmPrintPlot(model, 'ivmContour', [], [], [], capName, experimentNo);
end


function [sys,x0,str,ts]=ADRC_2(t,x,u,flag,h,TD,ESO,NLSEF,b0)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2
        sys=mdlUpdate(x,u,h,TD,ESO,b0);
    case 3
        sys=mdlOutputs(x,NLSEF,b0);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=5;
    sizes.NumOutputs=2;
    sizes.NumInputs=3;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdate(x,u,h,TD,ESO,b0)
    e1=x(1)-u(1);
    fh=fhan(e1,x(2),TD(1),TD(2));
    sys(1)=x(1)+h*x(2);
    sys(2)=x(2)+h*fh;
    e2=x(3)-u(2);
    sys(3)=x(3)+h*(x(4)-ESO(1)*e2);
    sys(4)=x(4)+h*(x(5)-ESO(2)*fal(e2,0.5,ESO(4))+b0*u(3));
    sys(5)=x(5)+h*(-ESO(3)*fal(e2,0.25,ESO(4)));
%     fe=fal(e,0.5,Delta);
%     fe1=fal(e,0.25,Delta);
%     sys(1,1)=x(1)+h2*(x(2)-BB(1)*e);
%     sys(2,1)=x(2)+h2*(x(3)-BB(2)*fe+u(1));
%     sys(3,1)=x(3)+h2*(-BB(3)*fe1);
function sys=mdlOutputs(x,NLSEF,b0)
    e3=x(1)-x(3);
    e4=x(2)-x(4);
    sys(1)=-fhan(e3,NLSEF(1)*e4,NLSEF(2),NLSEF(3))-x(5)/b0;
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;

function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end

function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function results = lowrank_sparse_fista( data, params, funs )
% FISTA implementation for solving
%   min 0.5*||AY - T|| + \lambda_*\sum_i||Xi,(i)||_* + \lambda1*||E||_1
% where Z = [ X1,...,X_N,E ]', Y is the FISTA auxiliary variable,
% A is a linear operator, T is given observation.

% tic;
% function handles
eval_f = funs.f;
eval_grad_f = funs.grad_f;
Aprod = funs.Aprod;
Atprod = funs.Atprod;
solve_prox_grad = funs.prox_grad_solver;

T = data.T;
if params.IsTC
    T = data.b;
end
N = length( size(params.X0) );
Y = cell(1,N+1);
for i = 1:N
    Y{i} = params.X0;
end
Y{N+1} = params.E0;
Z = Y;

use_cont = isfield( params, 'use_cont' ) && params.use_cont;
lambda1 = params.lambda;
lambdaS = params.lambdaS;
if use_cont
    if lambdaS > 0
        lambdaS_min = lambdaS;
        r = lambda1 / lambdaS;      lambdaS = 0.99*norm(T);     lambda1 = lambdaS*r;
    else
        r = params.rRatio /sqrt( max(size(data.T)) );
        lambdaS = 0.99*norm(T);     lambda1 = lambdaS*r;
        lambdaS_min = lambdaS * 1e-5;
    end
    eta = 0.97;
end

mu = params.mu0;
beta = 0.5;
t = 0;

fZ = eval_f( Z, Aprod, T );
trnorm = 0;
oneNorm = 0;

for iter = 1:params.max_iter

    % compute gradient at Y
    grad_f = eval_grad_f( Y, Aprod, Atprod, T );

    Zp = Z;     trnorm_p = trnorm;    fZp = fZ;     oneNorm_p = oneNorm;
    [ Z, trnorm ] = solve_prox_grad( Y, mu, grad_f, lambdaS, lambda1 );
    % backtrack linesearch
    fY = eval_f( Y, Aprod, T );
    ZYdiff = tensor_array_diff( Z, Y );
    approx = fY + tensor_array_innerprod( grad_f, ZYdiff ) + tensor_array_norm(ZYdiff)^2 / (2*mu);
    fZ = eval_f( Z, Aprod, T );
    while fZ > approx
        mu = mu * beta;
        [ Z, trnorm ] = solve_prox_grad( Y, mu, grad_f, lambdaS, lambda1 );
        ZYdiff = tensor_array_diff( Z, Y );
        approx = fY + tensor_array_innerprod( grad_f, ZYdiff ) + tensor_array_norm(ZYdiff)^2 / (2*mu);
        fZ = eval_f( Z, Aprod, T );
    end
    oneNorm = tensor_1norm(Z{N+1});

    % FISTA acceleration step
    tp = t;
    t = (1 + sqrt(1+4*tp^2))/2;
    Zdiff = tensor_array_diff( Z, Zp );
    Y = tensor_array_add( Zp, tensor_array_scale(Zdiff,(tp-1)/t) );

    % compute optimality stats
    Fp = fZp + lambdaS*trnorm_p + lambda1*oneNorm_p;
    F = fZ + lambdaS*trnorm + lambda1*oneNorm;
    rel_F = abs(F-Fp)/Fp;
    denom = tensor_array_norm(Zp);
    rel_X = tensor_array_norm(Zdiff) / denom;   if denom == 0; rel_X = 1; end
    rel_err = norm(ten_sum_all(Z(1:N))-data.X) / norm(data.X);

    % print
    if params.verbose && rem( iter, 50 ) == 0
        fprintf('Iter: %d,   fit: %3.2e,   rel_F: %3.2e,    rel_X: %3.2e,   rel_err: %3.2e, S: %3.2e\n', ...
            iter, fZ, rel_F, rel_X, rel_err, lambdaS);
    end

    if max(rel_F,rel_X) < params.opt_tol
        break;
    end

    % update lambda's
    if use_cont
        lambdaS = max( lambdaS*eta, lambdaS_min );
        lambda1 = lambdaS*r;
    end
end

results.vars = Z;
results.V = params.V0;
results.T = data.T;
results.iter = iter;
% results.cpu = toc;
results.mu = mu;
results.lambda1 = lambda1;
results.lambdaS = lambdaS;
results.IsTC = params.IsTC;

end


function [newTS,newTBase] = mrSliceTiming(ts,frameAdjustment,method)
% Adjust the time series of a slice/scan based on the slice position
%
%   [newTS,newTBase] = mrSliceTiming(ts,frameAdjustment,method)
%
% This function takes in the time series (ts) and recreates a new time
% series at each voxel.  It calculates the time at which the ts was
% actually measured and then interpolates to a new time frame.  All of the
% different slices are interpolated to the same time frame, which is the
% basis of slice timing adjustment.  The adjustment to all of the slices is
% done by calling this routine repeated from AdjustSliceTiming.
%
% This is  the core routine for time series slice timing adjustment.
% AdjustSliceTiming has overhead associated with creating the data type and
% so forth.  This routine is just the calculation.
%
% ts:               Time series from a single slice within a scan
% frameAdjustment:  The fraction of the frame (TR) this time series must
%                   be resampled (not in seconds)
% method:           'linear' or 'spline'
%
% Slice timing adjustment smooths noisy time series a little bit.  The
% linear choice smooths more than the spline choice.
%
% Example:
%   We don't call this routine on its own.  We call it from the
%   wrapper AdjustSliceTiming().
%

if notDefined('ts'), error('Time series required'); end
if notDefined('frameAdjustment'), error('frameAdjustment value required'); end
if notDefined('method'), method = 'spline'; end

% get nFrames, the number of time samples, from data
nFrames    = size(ts,1);

% Pad the with a replication of the first and last frames to deal with
% extrapolation:
ts = [ts(1, :); ts; ts(nFrames, :)];

% deal w/ NaNs. These may occur due to motion correction, for example.
% For now, replace with zero. We should probably replace with the mean of
% the neighbors at some point.
%
% nanInds = find(isnan(ts));
% ts(nanInds) = 0;
ts(isnan(ts)) = 0;

% These are the time samples for the amended (padded) ts.  We don't
% bother multiplying by TR, though of course we could.
tBaseRef = (0:nFrames+1);

% If the timing between frames is deltaFrame, and the difference in slice
% between this one and the standard slice is refSlice - slice, then we need
% to adjust the times between this slice and the reference as here.  This
% calculation should be replaced by a routine that includes the slice
% ordering; the slice ordering should be saved!
% newTBase = (1:nFrames) + deltaFrame * (refSlice - slice);
newTBase = (1:nFrames) + frameAdjustment;

switch lower(method)
    case 'spline'
        % Ress used spline temporal interpolation routine
        newTS = spline(tBaseRef, ts', newTBase)';
    case 'linear'
        % This should be an option, as well as others.
        newTS = interp1(tBaseRef(:), ts, newTBase(:));
    otherwise
        error('Undefined method %s\n',method);
end

return


function DataSet = prtDataGenSwissRoll
% prtDataGenSwissRoll  Generates data from the Swiss Roll data set.
%
%   DataSet = prtDataGenSwissRoll generates a prtDataSetRegress from the
%   swiss roll data set. This data is drawn from a 2-D manifold embedded in
%   3-dimensions. For more information on ths data set see the following:
%
%   http://isomap.stanford.edu/code/Readme
%
%   Example:
%
%   ds = prtDataGenSwissRoll;
%
%   See also: prtDataSetClass, prtDataGenBiModal, prtDataGenIris,
%   prtDataGenManual, prtDataGenMary, prtDataGenNoisySinc,
%   prtDataGenOldFaithful,prtDataGenProtate, prtDataGenSprial,
%   prtDataGenSpiral3Regress, prtDataGenUnimodal, prtDataGenSwissRoll,
%   prtDataGenUnimodal, prtDataGenXor

% The 2-D manifold locations are in ds.getTargets, and the 3-dimensional
% embedding is in ds.getObservations.

swissRollFile = fullfile(prtRoot,']beta','dataGen','swissRoll','swiss_roll_data.mat');
swiss = load(swissRollFile);
X = swiss.X_data';
Y = swiss.Y_data';

DataSet = prtDataSetRegress(X,Y,'name','Standard Swiss Roll Data');


clear all; close all; clc;

% Define path to a urdf file
path_to_urdf = 'ur10e.urdf';

% Generate functions for dynamics based on Lagrange method
% Note that it might take some time
% generate_rb_dynamics(path_to_urdf);
generate_friction_eq();

% Generate regressors for inverse dynamics of the robot, friction and load
% Note that it might take some time
% generate_rb_regressor(path_to_urdf);
% generate_load_regressor(path_to_urdf);

% Run tests
test_rb_inverse_dynamics()
test_base_params()

% Perform QR decompostion in order to get base parameters of the robot
include_motor_dynamics = 1;
[pi_lgr_base, baseQR] = base_params_qr(include_motor_dynamics);

% Estimate drive gains
drive_gains = estimate_drive_gains(baseQR, 'PC-OLS');
% Or use those found in the paper by De Luca
% drive_gains = [14.87; 13.26; 11.13; 10.62; 11.03; 11.47];

% Estimate dynamic parameters
path_to_est_data = 'ur-20_02_10-30sec_12harm.csv';      idxs = [635, 3510];
% path_to_data = 'ur-20_02_12-40sec_12harm.csv';    idxs = [500, 4460];
% path_to_data = 'ur-20_02_05-20sec_8harm.csv';     idxs = [320, 2310];
% path_to_data = 'ur-20_02_12-50sec_12harm.csv';    idxs = [355, 5090];
sol = estimate_dynamic_params(path_to_est_data, idxs, ...
                              drive_gains, baseQR, 'PC-OLS');

% Validate estimated parameters
path_to_val_data = 'ur-20_01_17-ptp_10_points.csv';     idxs = [700, 4200];

rre = validate_dynamic_params(path_to_val_data, idxs, ...
                              drive_gains, baseQR, sol.pi_b, sol.pi_fr)


% some variables needed
u = P.x_trim(4);
v = P.x_trim(5);
w = P.x_trim(6);
Va_trim = sqrt(u^2 + v^2 + w^2);
theta_trim = P.x_trim(8);
alpha_trim = atan(w/u);
delta_e_trim = P.u_trim(1);
delta_t_trim = P.u_trim(4);

% Roll attitude hold
P.delta_a_max = 45 * pi / 180;
P.phi_max = 15 * pi / 180;
P.roll_max = 45 * pi / 180;
a_phi1 = -0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_p * P.b / (2 * Va_trim);
a_phi2 = 0.5 * P.rho * Va_trim^2 * P.S_wing * P.b * P.C_p_delta_a;
P.kp_phi = P.delta_a_max / P.phi_max * sign(a_phi2);
omega_phi = sqrt(abs(a_phi2) * P.delta_a_max / P.phi_max);
zeta_phi = 1.2;   % design parameter
P.kd_phi = (2 * zeta_phi * omega_phi - a_phi1) / a_phi2;
P.ki_phi = 0.2;    % design parameter

% Course hold
W_chi = 8;  % design parameter
omega_chi = 1 / W_chi * omega_phi;
zeta_chi = 0.707;   % design parameter
Vg = P.Va0;
P.kp_chi = 2 * zeta_chi * omega_chi * Vg / P.gravity;
P.ki_chi = omega_chi^2 * Vg / P.gravity;

% Pitch attitude hold
P.delta_e_max = 45 * pi / 180;
P.e_theta_max = 10 * pi / 180;
a_theta1 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_q * P.c / (2 * P.Jy * 2 * Va_trim);
a_theta2 = -P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_alpha / (2 * P.Jy);
a_theta3 = P.rho * Va_trim^2 * P.c * P.S_wing * P.C_m_delta_e / (2 * P.Jy);
P.kp_theta = P.delta_e_max / P.e_theta_max * sign(a_theta3);
omega_theta = sqrt(a_theta2 + P.delta_e_max / P.e_theta_max * abs(a_theta3));
zeta_theta = 0.707;     % design parameter
P.kd_theta = (2 * zeta_theta * omega_theta - a_theta1) / a_theta3;
P.theta_max = 20 * pi / 180;
P.pitch_max = 40 * pi / 180;

% Airspeed hold using Throttle
a_V1 = P.rho * Va_trim * P.S_wing / P.mass ...
       * (P.C_D_0 + P.C_D_alpha * alpha_trim + P.C_L_delta_e * delta_e_trim) ...
       + P.rho * P.S_prop / P.mass * P.C_prop * Va_trim;
a_V2 = P.rho * P.S_prop / P.mass * P.C_prop * P.k_motor^2 * delta_t_trim;
a_V3 = P.gravity * cos(theta_trim - alpha_trim);
omega_v = 5;   % design parameter
zeta_v = 0.707;     % design parameter
P.delta_t_max = 1;
P.delta_t_min = 0;
P.ki_v = omega_v^2 / a_V2;
P.kp_v = (2 * zeta_v * omega_v - a_V1) / a_V2;

% Airspeed hold using Pitch
W_v2 = 7;   % design parameter
zeta_v2 = 0.707;    % design parameter
omega_v2 = 1 / W_v2 * omega_theta;
K_theta_dc = P.kp_theta * a_theta3 / (a_theta2 + P.kp_theta * a_theta3);
P.ki_v2 = -omega_v2^2/(K_theta_dc * P.gravity);
P.kp_v2 = (a_V1 - 2 * zeta_v2 * omega_v2) / (K_theta_dc * P.gravity);

% Altitude hold using Pitch
W_h = 10;   % design parameter
omega_h = 1 / W_h * omega_theta;
Va = P.Va0;
zeta_h = 1.2;     % design parameter
P.h_max = 1000;
P.h_min = 0;
P.ki_h = omega_h^2 / (K_theta_dc * Va);
P.kp_h = 2 * zeta_h * omega_h / (K_theta_dc * Va);


%Simple function to test the cross method
d=10;
%elem_fun=@(x) sum(x); %Just sum of everything
p=0:d-1; p = 2.^p;
a=-5;b=5;
n=2^d;
h=(b-a)/(n-1);
%mv=@(x) x.^3;
%elem_fun=@(x) 1.0./(dot((x-1),p)+1e-3); %Just sum of everything
%elem_fun=@(x) mv(1e-12+dot(x-1,p)*h);

%Compare functions of TT-tensors
x=tt_x(d,2); x=tt_tensor(x);
e=tt_ones(d,2);e=tt_tensor(e);
x=a*e+h*x; x=round(x,1e-13);
rs=x;
%fun=@(x) 1.0./sqrt(x);
%fun=@(x) 1.0./x;
fun=@(x) exp(-(x).^4) + 1;
elem_fun = @(ind) fun(rs(ind));
%elem_fun=@(ind) rs(ind);
%elem_fun=@(x) sqrt(x(1))+x(2);

eps=1e-6;
%y=tt_rc2(2*d,2,elem_fun,1e-12);
y=tt_rc(d,2,elem_fun,1e-6,'nswp',40,'change_dir_on',false);

z=funcrs2(rs,fun,1e-12,rs,20);


function [AtA,A] = corrMatrix4D(obj,i)
% calucate 4D correlation matrix
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

nCha = size(obj.kCalib{i},4);

% A = [];

if(isreal(obj.kCalib))
    A = zeros(prod(obj.calibSize{i} - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision);
else
    A = complex(zeros(prod(obj.calibSize{i} - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision),zeros(prod(obj.calibSize{i} - obj.kernelSize + 1), prod(obj.kernelSize)*nCha,obj.measPara.precision));
end
if(isempty(A))
    error('corrMatrix4D(): Unable to create GRAPPA kernel. Check kernel and calibration size dimensionality');
end
counter = 1;
for n=1:nCha
    if(isreal(obj.kCalib{i}(:,:,:,n)))
        if(strcmp(obj.measPara.precision,'single'))
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colRSingle(obj.kCalib{i}(:,:,:,n),obj.kernelSize).';
        else
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colR(obj.kCalib{i}(:,:,:,n),obj.kernelSize).'; % before: tmp =
        end
    else
        if(strcmp(obj.measPara.precision,'single'))
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colCSingle(obj.kCalib{i}(:,:,:,n),obj.kernelSize).';
        else
            A(:,counter:counter+prod(obj.kernelSize)-1) = im3colC(obj.kCalib{i}(:,:,:,n),obj.kernelSize).';
        end
    end
    counter = counter + prod(obj.kernelSize);
end

AtA = A'*A;

end


function S=PSNR(sss,aaa)

[m n p]=size(sss);
A=double(sss);
B=double(aaa);
sumaDif=0;
maxI=m*n*max(max(A.^2));
for u=1:m
    for v=1:n
        sumaDif=sumaDif+(A(u,v)-B(u,v))^2;
    end
end
if  (sumaDif==0)
    sumaDif=1;
end
S=maxI/sumaDif;
S=10*log10(S);


function tests = test_ft_preproc_online_filter

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_preproc_online_filter_init ft_preproc_online_filter_apply

if nargout
  % assume that this is called by RUNTESTS
  tests = functiontests(localfunctions);
else
  % assume that this is called from the command line
  fn = localfunctions;
  for i=1:numel(fn)
    feval(fn{i});
  end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function testOptions(testCase)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nchan   = 2;
nsample = 100;

result = {};

[B,A] = butter(6, 0.05, 'high');
state = ft_preproc_online_filter_init(B, A, zeros(nchan,1));

[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);

[B,A] = butter(6, 0.20, 'low');
state = ft_preproc_online_filter_init(B, A, zeros(nchan,1));

[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);

[B,A] = butter(6, [0.05 0.20]);
state = ft_preproc_online_filter_init(B, A, zeros(nchan,1));

[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);
[state, result{end+1}] = ft_preproc_online_filter_apply(state, randn(nchan, nsample) + 10);

% the first  part is high-pass filtered
% the second part is low-pass  filtered
% the third  part is band-pass filtered
plot(cat(2, result{:})');

% all iterations were done with (slightly) different options, hence the results should not be equal
for i=1:numel(result)
  for j=(i+1):numel(result)
    assert(~isequal(result{i}, result{j}), 'the results %d and %d should not be equal', i, j);
  end
end


function [net, sName] = addOneLoop_forMeanShiftGrouping(net, sName, loopIdx, GaussianBandwidth)

if ~exist('GaussianBandwidth', 'var')
    GaussianBandwidth = 0.1;
end
%%
pre_input_layer = sName;

lName = sprintf('loop%d_meanshift_S_is_XX', loopIdx);
net.addLayer(lName, ...
    meanshift_S_is_XX(), ...
    {sName}, lName);
sName = lName;

lName = sprintf('loop%d_meanshift_G_is_Gaussian', loopIdx);
net.addLayer(lName, ...
    meanshift_G_is_Gaussian('delta', GaussianBandwidth), ...
    {sName}, lName);
G_layer = lName;
sName = lName;

lName = sprintf('loop%d_meanshift_d_is_sumG', loopIdx);
net.addLayer(lName, ...
    meanshift_d_is_sumG(), ...
    {sName}, lName);
sName = lName;

lName = sprintf('loop%d_meanshift_q_is_inv_d', loopIdx);
net.addLayer(lName, ...
    meanshift_q_is_inv_d(), ...
    {sName}, lName);
sName = lName;

lName = sprintf('loop%d_meanshift_P_is_G_diag_q', loopIdx);
net.addLayer(lName, ...
    meanshift_P_is_G_diag_q(), ...
    {G_layer, sName}, lName);
sName = lName;

lName = sprintf('loop%d_meanshift_Y_is_XP', loopIdx);
net.addLayer(lName, ...
    meanshift_Y_is_XP(), ...
    {pre_input_layer, sName}, lName);
sName = lName;


function [U, Theta, V, numiter ] = OR1MP(m, n, r, Known, data, opts )
%ORTHOGONAL-RANK-1-MATRIX-PURSUIT Infinit dimension matching pursuit for
%low rank matrix
%
%For the problem           min    L(X) = ||X - Y||^2
%                          s.t.   rank(X) <= r
%                          X = Theta * M = sum_i theta_i * M_i = U Theta V'
%   Detail variable
%   Input:
%         m ---- row number
%         n ---- column number
%         r ---- number of basis
%         Known ---- index of the known spot in the matrix
%         data ---- the content of the known spot in the matrix
%         opts ---- parameters for the algorithm
%   Output:
%         U ---- output matrix: U
%         Theta ---- output matrix: Theta
%         V ---- output matrix: V
%         numiter ---- number of iterations
%
%   Copyright Zheng Wang @ Arizona State University
%   $Date: 2013/01/29$

fn = mfilename;
error(nargchk(5, 6, nargin));
if nargin < 6
    epsilon = 1e-4; % convergence threshold
else
    epsilon = opts.epsilon;
end

%addpath('PROPACK/');
%addpath('largescale_ops/');
%addpath('SLEP_package_4.1/');

% initialization,
[indm, indn] = ind2sub([m, n], Known);
data( data == 0 )= eps;
res = sparse(indm, indn, data, m, n);
[indm, indn, data] = find(res);
U = [];
V = [];
Msup = [];

verbosity = 1;
printsyb = ['-', 'X', '|'];
if verbosity == 1
    fprintf('\nIteration:        ');
end

i = 0;
W = 0;
oldresnorm = 0;
gresnorm = 1;
yy = [];
nnorm = norm(data, 'fro');
% main iteration
% In OR1MP, the stop criterion is small gradient of residual
while (i < r) && (gresnorm > epsilon )
    % 1. find the top singular pair of the residual and update the gresnorm
    resvec = data - W;
    sparse_update(res, resvec); % sparse update the res using resvec

    [u, ~, v] = topsvd(res, 20); % run our power method for 10 iterations
    %[u, ~, v] = topsvd(res, 1);
    %[u, ~, v] = lansvd(res, 1, 'L'); % fast sparse svd using PROPACK
    %[u, s, v] = svds(res, 1); % use matlab sparse top svd

    resnorm = normest(res, 'fro')/nnorm;
    gresnorm = abs(resnorm - oldresnorm);
    oldresnorm = resnorm;

    % 2. update the weight Theta, the pursuit basis is uv', its weight is s.
    Mi = sparse_inp(u', v', indm, indn)';

    % b) use incremental inverse to solve the least sqare problem
    if i~=0
        Minv = inverse_incremental(Minv, Msup'*Mi, Mi'*Mi);
    else
        Minv = 1/(Mi'*Mi);
    end
    yy = [yy; Mi'*data];
    Theta = Minv*yy;
    Msup = [Msup Mi];

    U = [U u];
    V = [V v];

    % 3. update the learned matrix W = U' * diag(Theta) * V;
    W = Msup * Theta;

    if verbosity == 1
        fprintf('\b\b\b\b\b\b\b\b  %c:%4d', printsyb(1+mod(i,length(printsyb))), i);
    end
    i = i + 1;
end
% V = diag(Theta)*V;
numiter = i;
% fprintf( '\n OR1MP run %d rounds! \n', numiter);

% %* Sparse selection: for sharpe low rank problem, we may need the lasso
% %fine selection. Use lasso to learn a more sparse weights.
% % Starting point
% opts.init=2;        % starting from a zero point
% % termination criterion
% opts.tFlag=5;       % run .maxIter iterations
% opts.maxIter=100;   % maximum number of iterations
% % normalization
% opts.nFlag=0;       % without normalization
% % regularization
% opts.rFlag=1;       % the input parameter 'rho' is a ratio in (0, 1)
% opts.mFlag=0;       % treating it as compositive function
% opts.lFlag=0;       % Nemirovski's line search
% % get the final sparse Theta by lasso in SLEP toolbox
% [Theta, ~, ~] = LeastR(Msup, data, 0.00001, opts); %  [W, funVal, ValueL] 00001

function Ninv = inverse_incremental(Minv, MMi, d)
% calculate the inverse of the blocked matrix of
P = MMi' * Minv; % vector
q = 1/(d - P*MMi); % scaler
y = q*P;
Ninv = [Minv+P'*y, -y'; -y, q];

function [u, s, v] = topsvd(A, round)
% calculate the top svd of matrix A using round iterations
stopeps = 1e-3;
[m,n] = size(A);
u = ones(m,1); % this is the sigma*u
vo = 0;
for i=1:round
    v = u'*A/(norm(u))^2;
    u = A*v'/(norm(v))^2;
    if norm(v-vo) < stopeps
        break
    end
    vo = v;
end
nu = norm(u);
nv = norm(v);
u = u/nu;
v = v'/nv;
% v = v/nv;
s = nu*nv;


function [rc,fval,it] = ARhat_misd(ng,xg,rcinit,rc0,lag_max)

%ARHAT_MISD AR model from measurements with missing data
%  [rc,fval,it] = ARhat_misd(ng,xg,rcinit,rc0,lag_max) estimates
%  reflection coefficients from missing data (ng,xg). ng contains
%  measurement times and xg the corresponding measurements.
%
%  The starting value for the reflection coefficients is given by
%  rc_start = [1 rc0 rcinit]
%  An AR model is estimated using approximate Maximum Likelihood (ML)
%  estimation, where rc0 is fixed and the ML is sought over the last
%  reflection coefficients, starting in rcinit.
%
%  See also ARMLFIT, ARHAT_MISD.

nseg = length(ng);
n_obs = 0;
for seg = 1:nseg
   n_obs = n_obs+length(ng{seg});
end

opties = optimset('Display','iter','TolX',.001/sqrt(n_obs),'TolFun',.0001);

[rc_tan,fval,exitflag,output]= fminunc('ARMLfit',tan(.5*pi*rcinit),opties,ng,xg,rc0,lag_max);
rc = 2/pi*atan(rc_tan);
it = output.iterations;


function loadings = computeLoadingPercentPower(V,W,H)
    loadings = [];
    K = size(H,1);
    varv = sum(V(:).^2);
    for fi = 1:K
        WH = helper.reconstruct(W(:,fi,:),H(fi,:));
        loadings(fi) = sum(2*V(:).*WH(:) - WH(:).^2)/varv;
    end
    loadings(loadings<0)=0;
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   ABB IRB2400.
%
%   Author: Arturo Gil. Universidad Miguel Hern�ndez de Elche.
%   email: arturo.gil@umh.es date:   09/01/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'ABB_IRB2400';

%Path where everything is stored for this robot
robot.path = 'robots/abb/IRB2400';

robot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)+pi]';
robot.DH.d='[0.615 0 0 0.755 0 0.085]';
robot.DH.a='[0.100 0.705 0.135 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';

robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_irb2400(robot, T)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-180) deg2rad(180); %Axis 1, minimum, maximum
                deg2rad(-105) deg2rad(105); %Axis 2, minimum, maximum
                deg2rad(-62.5) deg2rad(62.5); %Axis 3
                deg2rad(-200) deg2rad(200); %Axis 4:
                deg2rad(-120) deg2rad(120); %Axis 5
                deg2rad(-400) deg2rad(400)]; %Axis 6:

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(150); %Axis 1, rad/s
                deg2rad(150); %Axis 2, rad/s
                deg2rad(150); %Axis 3, rad/s
                deg2rad(360); %Axis 4, rad/s
                deg2rad(360); %Axis 5, rad/s
                deg2rad(450)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time
            % end effectors maximum velocity
robot.linear_velmax = 1.0; %m/s, unavailable from datasheet

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [255 20 20]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=1;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot
robot.axis=[-2 2 -2 2 0 2.5];
%read graphics files
robot = read_graphics(robot);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DYNAMIC PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

robot.has_dynamics=1;

%link masses (kg)
robot.dynamics.masses=[0 269 60 30 20 1];

%COM of each link with respect to own reference system
robot.dynamics.r_com=[0       0.05          0; %(rx, ry, rz) link 1
    -0.550	0	 0.020; %(rx, ry, rz) link 2
    0       0       0;  %(rx, ry, rz) link 3
    0       -0.3775       0;%(rx, ry, rz) link 4
    0       0           0;%(rx, ry, rz) link 5
    0       0         0.032];%(rx, ry, rz) link 6

%Inertia matrices of each link with respect to its D-H reference system.
% Ixx	Iyy	Izz	Ixy	Iyz	Ixz, for each row
robot.dynamics.Inertia=[0      0.35	0   	0	0	0;
    .13     .524	.539	0	0	0;
    .066	.086	.0125	0	0	0;
    1.8e-3	1.3e-3	1.8e-3	0	0	0;
    .3e-3	.4e-3	.3e-3	0	0	0;
    .15e-3	.15e-3	.04e-3	0	0	0];

robot.motors=load_motors([5 5 5 4 4 4]);
%Speed reductor at each joint
robot.motors.G=[300 300 300 300 300 300];


% Take in a vector of complex samples representing individual QPSK constellation points with the constellation rotated
% such that the points are ideally at 1+i, -1+i, -1-i, and 1-i.
%
% This function uses hard decision, so it's not what you want to use in low SNR environments
%
% The constellation mapping is:
%     1+i == 0b00
%     1-i == 0b01
%    -1+i == 0b10
%    -1-i == 0b11
%
% Which comes from https://github.com/ttsou/openphy/blob/master/src/lte/qam.c#L35
%
% @param data_carriers Row or column vector of complex samples
% @return quantized_bits Vector of 1/0 values that make up the bits demapped from the provided sample vector
function [quantized_bits] = quantize_qpsk(data_carriers)
    assert(iscolumn(data_carriers) || isrow(data_carriers), "Data carriers must be row/column vector");

    quantized_bits = zeros(length(data_carriers), 1);

    % Track where in the `quantized_bits` vector the new bits should be placed
    bits_offset = 1;

    % Walk through each complex sample in the input vector
    for sample_idx = 1:length(data_carriers)
        sample = data_carriers(sample_idx);

        % Determine bit mapping based on the quadrant that the sample is located
        if (real(sample) > 0 && imag(sample) > 0)
            bits = [0, 0];
        elseif (real(sample) > 0 && imag(sample) < 0)
            bits = [0, 1];
        elseif (real(sample) < 0 && imag(sample) > 0)
            bits = [1, 0];
        elseif (real(sample) < 0 && imag(sample) < 0)
            bits = [1, 1];
        else
            bits = [0, 0];
        end

        % Save off the quatized bits and move the counter ahead by 2
        quantized_bits(bits_offset:bits_offset+1) = bits;
        bits_offset = bits_offset + 2;
    end
end


function mObject3 = waveformMorphing(mObject1,mObject2,mRate);
%   Morphing with minimum information
%   (Actually this is not real morphing.
%   It is simply blending two waveform.)
%   mObject3 = waveformMorphing(mObject1,mObject2,mRate);

%   Designed and coded by Hideki Kawahara
%   27/Feb./2005
%   Copyright(c) 2005, Hideki Kawahara

nLength = max(length(mObject1.waveform),length(mObject2.waveform));
if mObject1.samplingFrequency ~= mObject2.samplingFrequency
    mObject3 = [];
    return
end;
x = zeros(nLength,1);
x(1:length(mObject1.waveform)) = (1-mRate)*mObject1.waveform;
x(1:length(mObject2.waveform)) = mRate*mObject2.waveform + x(1:length(mObject2.waveform));

mObject3=createMobject;
mObject3.waveform = x;
mObject3.samplingFrequency = mObject1.samplingFrequency;


function y = centroid(M, x)
% Attempts the computation of a centroid of a set of points on a manifold.
%
% function y = centroid(M, x)
%
% M is a structure representing a manifold.
% x is a cell of points on that manifold.

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:

    % For now, just apply a few steps of gradient descent for Karcher means

    n = numel(x);

    problem.M = M;

    problem.cost = @cost;
    function val = cost(y)
        val = 0;
        for i = 1 : n
            val = val + M.dist(y, x{i})^2;
        end
        val = val/2;
    end

    problem.grad = @grad;
    function g = grad(y)
        g = M.zerovec(y);
        for i = 1 : n
            g = M.lincomb(y, 1, g, -1, M.log(y, x{i}));
        end
    end

    % This line can be uncommented to check that the gradient is indeed
    % correct. This should always be the case if the dist and the log
    % functions in the manifold are correct.
    % checkgradient(problem); pause;

    query = warning('query', 'manopt:getHessian:approx');
    warning('off', 'manopt:getHessian:approx');
    options.verbosity = 0;
    options.maxiter = 15;
    y = trustregions(problem, x{randi(n)}, options);
    warning(query.state, 'manopt:getHessian:approx');

end


%ASSEMBLEPROB Assembly of system matrix and right hand side/load vector.
%
%   [ M, A, F, T_M, T_A, T_F, T_SP ] = ASSEMBLEPROB( PROB, VARARGIN ) Calls the
%   assembly routines to compute and assemble a monolithic sparse system
%   matrix and a right hand side/load vector as specified in the PROB struct.
%   Accepts optional propery value pairs in VARARGIN.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       prob        struct                 Problem definition struct
%       icub        scalar/{auto}          Numnerical integration rule used in assembly
%                                                Default 1+max(shape function order)
%       imass       scalar {1}             Mass matrix lumping:  1 = Full mass matrix
%                                          2 = row sum lumping,  3 = diagonal lumping
%                                          4 = HRZ diagonal lumping
%       n_cmax      scalar {50000}         Max number of cells to assemble
%                                          for at once (to limit memory consumption)
%       f_m         logical {false}        Assembly flag for mass matrix
%       f_a         logical {false}        Assembly flag for system matrix
%       f_f         logical {false}        Assembly flag for load vector
%       f_c         logical {true}         Assembly flag for integral constraints
%       f_sparse    logical {false}        Return sparse/struct matrix format
%       solcomp     {all dvars/subd}       Dependent variables/subdomains to assemble for
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       M           sparse [n_M]           Assembled mass matrix
%       A           sparse [n_A]           Assembled system matrix
%       f           [n_A,1]                Assembled rhs/load vector
%       t_m         scalar                 Time spent assembling mass matrix
%       t_a         scalar                 Time spent assembling system matrix
%       t_f         scalar                 Time spent assembling rhs/load vector
%       t_sp        scalar                 Time for sparse matrix conversion
%
%   See also ASSEMBLEA, ASSEMBLEF, ASSEMMAT

% Copyright 2013-2022 Precise Simulation, Ltd.


function determ = conex3_determinant ( n )

%*****************************************************************************80
%
%% CONEX3_DETERMINANT returns the determinant of the CONEX3 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  determ = - 1.0;

  return
end


function [ edge_pointer, edge_data, xy ] = grf_example ( node_num, edge_num )

%*****************************************************************************80
%
%% GRF_EXAMPLE sets up a GRF example.
%
%  Discussion:
%
%    The example is known as the Coxeter graph.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 January 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Skiena,
%    Implementing Discrete Mathematics,
%    Combinatorics and Graph Theory with Mathematica,
%    Addison-Wesley, 1990.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, integer EDGE_NUM, the number of edges.
%
%    Output, integer EDGE_POINTER(NODE_NUM+1), pointers to
%    the beginning of edge data for each node.
%
%    Output, integer EDGE_DATA(EDGE_NUM), the edge data.
%
%    Output, real XY(2,NODE_NUM), the node coordinates.
%
  edge_pointer = [ ...
    1,  4,  7, 10, 13, 16, 19, 22, 25, 28, ...
   31, 34, 37, 40, 43, 46, 49, 52, 55, 58, ...
   61, 64, 67, 70, 73, 76, 79, 82, 85 ]';

  edge_data = [ ...
     8,   2,   3, ...
    14,   1,   5, ...
     9,   4,   1, ...
    10,   7,   3, ...
    13,   2,   6, ...
    12,   5,   7, ...
    11,   6,   4, ...
    25,  20,   1, ...
    24,  21,   3, ...
    23,  15,   4, ...
    22,  16,   7, ...
    28,  17,   6, ...
    27,  18,   5, ...
    26,  19,   2, ...
    10,  18,  19, ...
    11,  19,  20, ...
    12,  21,  20, ...
    13,  15,  21, ...
    14,  16,  15, ...
     8,  17,  16, ...
     9,  18,  17, ...
    11,  27,  24, ...
    10,  28,  25, ...
     9,  26,  22, ...
     8,  23,  27, ...
    14,  24,  28, ...
    13,  25,  22, ...
    12,  26,  23 ]';

  xy = [ ...
    0.412,   0.984; ...
    0.494,   0.984; ...
    0.366,   0.926; ...
    0.388,   0.862; ...
    0.546,   0.926; ...
    0.518,   0.860; ...
    0.458,   0.818; ...
    0.152,   0.684; ...
    0.264,   0.682; ...
    0.354,   0.680; ...
    0.458,   0.670; ...
    0.554,   0.672; ...
    0.658,   0.668; ...
    0.774,   0.692; ...
    0.164,   0.450; ...
    0.228,   0.448; ...
    0.274,   0.390; ...
    0.242,   0.330; ...
    0.194,   0.278; ...
    0.146,   0.328; ...
    0.102,   0.390; ...
    0.668,   0.472; ...
    0.638,   0.416; ...
    0.656,   0.334; ...
    0.714,   0.270; ...
    0.798,   0.326; ...
    0.830,   0.408; ...
    0.754,   0.466 ]';

  return
end


%  Script file: plotsinc.m
%
%  Purpose:
%    This program illustrates the use of handle graphics
%    commands by creating a plot of sinc(x) from -3*pi to
%    3*pi, and modifying the characteristics of the figure,
%    axes, and line using the "set" function.
%
%  Record of revisions:
%      Date       Programmer          Description of change
%      ====       ==========          =====================
%    04/02/07    S. J. Chapman        Original code
%
% Define variables:
%   hndl         -- Handle of line
%   x            -- Independent variable
%   y            -- sinc(x)

% Calculate sinc(x)
x = -3*pi:pi/10:3*pi;
y = sin(x) ./ x;

% Find the zero value and fix it up.  The zero is
% located in the middle of the x array.
index = fix(length(y)/2) + 1;
y(index) = 1;

% Plot the function.
hndl = plot(x,y);

% Now modify the figure to create a pink background,
% modify the axis to turn on y-axis grid lines, and
% modify the line to be a 2-point wide orange line.
set(gcf,'Color',[1 0.8 0.8]);
set(gca,'YGrid','on');
set(hndl,'Color',[1 0.5 0],'LineWidth',3);


function [a_opt,dt_opt] = refine(params,ref,tgt,varargin)
% function [anew,dtnew] = refine(params,ref,tgt,{cons,projmodel})
%
% Optimize initial estimates (from Hough accumulator) using non-linear
% minimization.
%
% For a variable alpha: params = [dt0,alpha0].
% For a fixed alpha: params = [dt0] and cons = [alpha].

% read in variable arguments
cons	= [];
	if (length(varargin)>0), cons = varargin{1}; end
projmodel	= 'affine';
	if (length(varargin)>1), projmodel = varargin{2}; end

% parameters+constraints must equal 2
if (size(params,2)+size(cons,2) ~= 2)
	error('Incorrect number of parameters/constraints');
end

% extract alpha and dt from params/cons
dt = params(:,1);
if isempty(cons),	a = params(:,2);
else,							a = cons(:,1);
end

% perform non-linear optimization of initial estimates using sub-frame cost
% function
cmax = inf;
for hyp	= 1:length(a)
	fprintf('  (a=%0.3f,dt=%0.3f) -> ',a(hyp),dt(hyp));
	if isempty(cons)
		[pmin,c]	=	fminsearch(@subframecost,[dt(hyp),a(hyp)],[],ref,tgt,[],projmodel);
		dtnew	= pmin(1);
		anew	= pmin(2);
	else
		[pmin,c]	=	fminsearch(@subframecost,[dt(hyp)],[],ref,tgt,[a(hyp)],projmodel);
		dtnew	= pmin(1);
		anew	= a(hyp);
	end
	fprintf('(a=%0.3f,dt=%0.3f)\n',anew,dtnew);

	if (c < cmax)
		cmax		= c;
		dt_opt	= dtnew;
		a_opt		= anew;
	end
end


function [f] = spm_SHC_fx(x,v,P,varargin)
% equations of motion for Lotka-Volterra dynamics
% FORMAT [f] = spm_SHC_fx(x,v,P)
%
% x   - hidden states
% v   - exogenous inputs
% P.f - lateral connectivity
% P.k - rate [default 1]
%
% returns f = dx/dt = P.f*S(x) - x/8 + 1;
%              S(x) = 1./(1 + exp(-x))
%
% where C determines the order of unstable fixed points visited in the
% stable heteroclinic channel.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_SHC_fx.m 3265 2009-07-10 14:02:22Z karl $

% intialise
%==========================================================================

% SHC states
%--------------------------------------------------------------------------
f.x  = P.f*spm_phi(x.x) - x.x/32 + 1;
f.c  = (spm_phi(x.x) - x.c)/64;

f    = spm_vec(f)*16;


function I = domIntegral4(data)
%+========================================================================+
%|                                                                        |
%|              OPENDOM - LIBRARY FOR NUMERICAL INTEGRATION               |
%|           openDom is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal & Francois Alouges (c) 2017-2018.          |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%|             francois.alouges@polytechnique.edu                         |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : domIntegral4.m                                |
%|    #    |   VERSION    : 0.40                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.03.2018                                    |
%| ( === ) |   SYNOPSIS   : Numerical integation with 4 input arguments   |
%|  `---'  |                                                              |
%+========================================================================+

%%% FINITE ELEMENT OPERATOR --> \int_{mesh(x)} psi(x)' f(x) psi(x) dx
if isa(data{1},'dom') && isa(data{2},'fem')
    % Domain with quadrature
    Xdom   = data{1};
    [X,Wx] = Xdom.qud;
    Nx     = size(X,1);
    Wx     = spdiags(Wx,0,Nx,Nx);

    % Integrated finite element matrix
    u  = data{2};
    Mu = u.uqm(Xdom);
    if iscell(Mu)
        Mu{1} = Mu{1}' * Wx;
        Mu{2} = Mu{2}' * Wx;
        Mu{3} = Mu{3}' * Wx;
    else
        Mu = Mu' * Wx;
    end

    % Function evaluation on quadrature
    F = data{3};
    if iscell(F)
        Fx{1} = spdiags(F{1}(X),0,Nx,Nx);
        Fx{2} = spdiags(F{2}(X),0,Nx,Nx);
        Fx{3} = spdiags(F{3}(X),0,Nx,Nx);
    else
        Fx = spdiags(F(X),0,Nx,Nx);
    end

    % Finite element matrix
    v  = data{4};
    Mv = v.uqm(Xdom);

    % Integration
    I = femMultiplyCell(Mu,Fx,Mv);

%%% BOUNDARY ELEMENT INTEGRATION --> \int_{mesh(y)} f(x,y) psi(y) dy
elseif isnumeric(data{1}) && isa(data{2},'dom')
    % Evaluation points
    X  = data{1};
    Nx = size(X,1);

    % Domain with quadrature
    Ydom   = data{2};
    [Y,Wy] = Ydom.qud;
    Ny     = size(Y,1);
    Wy     = spdiags(Wy,0,Ny,Ny);

    % Function evaluation on quadrature
    F = data{3};
    if iscell(F)
        Fxy = {zeros(Nx,Ny),zeros(Nx,Ny),zeros(Nx,Ny)};
        for i = 1:3
            for j = 1:Ny
                Fxy{i}(:,j) = F{i}(X,Y(j,:));
            end
        end
    else
        Fxy = zeros(Nx,Ny);
        for j = 1:Ny
            Fxy(:,j) = F(X,Y(j,:));
        end
    end

    % Integrated finite element matrix
    v  = data{4};
    Mv = v.uqm(Ydom);
    if iscell(Mv)
        Mv{1} = Wy * Mv{1};
        Mv{2} = Wy * Mv{2};
        Mv{3} = Wy * Mv{3};
    else
        Mv = Wy * Mv;
    end

    % Integration
    I = femMultiplyCell(Fxy,Mv);

%%% BOUNDARY ELEMENT INTEGRATION --> \int_{mesh(x)} psi(x)' f(x,y) dx
elseif isa(data{1},'dom') && isnumeric(data{2})
    % Domain with quadrature
    Xdom   = data{1};
    [X,Wx] = Xdom.qud;
    Nx     = size(X,1);
    Wx     = spdiags(Wx,0,Nx,Nx);

    % Evaluation points
    Y  = data{2};
    Ny = size(Y,1);

    % Integrated finite element matrix
    u  = data{3};
    Mu = u.uqm(Xdom);
    if iscell(Mu)
        Mu{1} = Mu{1}' * Wx;
        Mu{2} = Mu{2}' * Wx;
        Mu{3} = Mu{3}' * Wx;
    else
        Mu = Mu' * Wx;
    end

    % Function evaluation on quadrature
    F = data{4};
    if iscell(F)
        Fxy = {zeros(Nx,Ny),zeros(Nx,Ny),zeros(Nx,Ny)};
        for i = 1:3
            for j = 1:Ny
                Fxy{i}(:,j) = F{i}(X,Y(j,:));
            end
        end
    else
        Fxy = zeros(Nx,Ny);
        for j = 1:Ny
            Fxy(:,j) = F(X,Y(j,:));
        end
    end

    % Integration
    I = femMultiplyCell(Mu,Fxy);

else
    error('domIntegral4.m : unavailable case')
end
end


function H = mnorm_entropy(U)
warning('This function is deprecated')

% H = mnorm_entropy(U)
%
% U is the Cholesky factor of a covariance matrix C, that is, a lower or
% upper triangylar matrix such that C=U*U' or C=U'*U.

d = size(U,1);

H = d/2*log(2*pi) + 0.5*logdet_chol(U) + d/2;


function [fin_im] =  main_function(or_im)

    % PREPROCESSING
        or_im = pre_pro(or_im);

    % NORMALIZE
        or_im = normal(or_im,1);

    % SEGMENTATION
        blksze = 15; thresh = 0.08;
        [norm_im, mask] = segmentation(or_im, blksze, thresh);

    % DIFFUSION
        norm_im = diffusion(norm_im);

    % ORIENTATION
        [orientim, G_xx, G_yy, G_xy, cos2theta, sin2theta, denom] = orient(norm_im, 1, 5, 6);
    %    showorient(orientim, 20);

    % RELIABILTY
        reliability = reliability_f(G_xx, G_yy, G_xy, cos2theta, sin2theta, denom);

    % FREQUENCY
        [freq] = frequency(norm_im, mask, orientim, 33, 5, 4, 14);

    % GABOR FILTERS
        gabor_im = gabor(freq);

    % MORE FILTERS FOR RIDGE PATTERNS
        new_im = more_filter(norm_im, orientim, gabor_im, 0.5, 0.5);

    % BINARISE
        thres = 0;
        binim = binarise(new_im, thres);

    % Applying reliabilty factor to the final enhanced image
        rel = 0.5;  % reliability factor (above 0.5 is considered to be reliable)
        %binim = binim.*mask.*(reliability>rel);

    fin_im = imcomplement(binim);


function out = PeubChannel(P, n, L)
%upper bound for the error probability of the AWGN channel
%P - channel SNR
%n - block length (scalar)
%L - length of messages transmitter between the source encoder and the channel encoder

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

%dP/dQ is bounded by a constant. Find that constant.
[~, gamma2] = fminbnd(@(x)-ncx2pdf(n*x, n, n*P)./chi2pdf(n*x/(1+P), n)*(1+P), 0, 10);
gamma2 = -gamma2;

A = n/2*log(1+P) + n/2 - L - log(gamma2);

Finner = @(v) ncx2pdf(n*v, n, n/P).*n.*exp(P/2/(1+P)*n*(v - thresv()));
tail = 1 - ncx2cdf(thresv()*n,n,n/P);
out = quad(Finner, 0, thresv())...                      %exponent < 1
    + tail;                                             %exponent = 1

    function out = thresv()
        %threshold for v as a function of v0
        out = max( 0, 2*(1+P)/P/n*A );
    end
end


% Fig. 5.28  Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
% script for right side of Figure 5.28

clf
n=[1 2];
d=conv([1 1 0],[1 13]);
nc=[1 .05];
dc=[1 .01];
nol=conv([0 0 n],nc);
dol=conv(d,dc);
sysOL=tf(nol,dol);
K=0:.001:10;
rlocus(sysOL,K);
 hold on
 title('Fig.5.28b Root locus for lead plus lag')
  axis([-.15 .05 -.1 .1])
 z=0:.1:.9;
 wn=2:2:19;
 sgrid(z, wn)
 dcl=91*nol+dol;
 r=roots(dcl) % shows that extra  root from Lag comp is almost right on the zero.
 plot(r,'*')
 hold off


function [gray_enhanced]=gray_level_images(current_image);
 %% trying dc coefficients
[mrows,ncolumns]=size(current_image);
 current_image=mat2gray(current_image);
        H = fspecial('disk',10);
        gray_filtering=current_image;
        blurred_gray = imfilter(gray_filtering,H,'replicate');
        k=2;
    for i=1:1:mrows
        for j=1:1:ncolumns
gray_enhanced(i,j)=mat2gray((gray_filtering(i,j)+(k.*(gray_filtering(i,j)-blurred_gray(i,j)))));
        end
    end
%figure(10001);
imshow(gray_enhanced);


% Demonstrates the projection of x^2 function
clear all; close all; clc;

% The function to be projected.
func = @(x) (x-2).^2;

% epsilon
polarity = 0;
step = 0.01;
epsilon = 0.2;
stop = 2;
x = 0:step:stop;
func_x = func(x);
projection = spx.wavelet.lcs.project_0_right(func, step, stop, epsilon, polarity);
subplot(311);
plot(x, func_x, 'r');
subplot(312);
plot(x, projection, 'b');
residual = func_x - projection;
subplot(313);
plot(x, residual, 'g');


function [ FV ] = mesh_refine(FV,Nface)

% mesh_refine - creates smaller triangles from a triangle mesh
%
% [ FV ] = mesh_refine( FV, Nface )
%
% FV.vertices   - vertex matrix (Nx3)
% FV.faces      - face matrix (Mx3), indices into vertex matrix rows
%
% Nface         - subdivide faces into 4 or 6 faces,
%                 the default 4 provides an even subdivision
%
% This function calls mesh_refine_tri4 or mesh_refine_tri6.  See
% these for more details.
%

% This can be done until some minimal distance (D) of the mean
% distance between vertices of all triangles is achieved.  If
% no D argument is given, the function refines the mesh once.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:57 $

% Licence:  GNU GPL, no implied or express warranties
% History:  08/2002, Darren.Weber_at_radiology.ucsf.edu, created
%                    adapted this function as a wrapper to
%                    mesh_refine_tri4 & mesh_refine_tri6
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if ~exist('FV','var'),
    error('MESH_REFINE: NO input FV struct');
elseif isempty(FV),
    error('MESH_REFINE: NO input FV struct');
end

if ~exist('Nface','var'),
    Nface = 4;
elseif isempty(Nface),
    Nface = 4;
end

switch Nface,

case 4,
    FV = mesh_refine_tri4(FV);
case 6,
    FV = mesh_refine_tri6(FV);
otherwise
    FV = mesh_refine_tri4(FV);
end

return


% Author: Relja Arandjelovic (relja@relja.info)

function Y= relja_l1normalize_col( X )
    Y= bsxfun(@rdivide, X, sum(abs(X),1) + 1e-12 );
end


function value = p14_gu ( u )

%*****************************************************************************80
%
%% P14_GU computes the auxilliary function G(U).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real U, the argument of the function.
%
%    Output, real VALUE, the value of the function.
%
  beta = 0.1;
  gamma = 0.1;

  value = u / ( beta * ( u + gamma ) );

  return
end


function [ a, seed ] = r8but_random ( n, mu, seed )

%*****************************************************************************80
%
%% R8BUT_RANDOM randomizes a R8BUT matrix.
%
%  Discussion:
%
%    The R8BUT storage format is used for a banded upper triangular matrix.
%    The matrix is assumed to be zero above the MU-th superdiagonal.
%    The matrix is stored in an MU+1 by N array.
%    Columns are preserved.
%
%    The diagonal is stored in row MU+1 of the array.
%    The first superdiagonal in row MU, columns 2 through N.
%    The second superdiagonal in row MU-1, columns 3 through N.
%    The MU-th superdiagonal in row 1, columns MU+1 through N.
%
%  Example:
%
%    N = 5, MU = 2
%
%    A11 A12 A13   0   0
%      0 A22 A23 A24   0
%      0   0 A33 A34 A35
%      0   0   0 A44 A45
%      0   0   0   0 A55
%                --- ---
%                    ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, integer MU, the upper bandwidth.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real A(MU+1,N), the R8BUT matrix.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  for i = 1 : mu + 1

    for j = 1 : mu + 1 - i
      a(i,j) = 0.0;
    end

    for j = max ( 1, mu + 2 - i ) : n
      [ a(i,j), seed ] = r8_uniform_01 ( seed );
    end

  end

  return
end


function [Vdraw, phi_Vdraw]=sampleV(Psi,phi_V,Vold,priorValues,dataValues)
% This function takes a draw from the conditional posterior of the variance
% V of the SV in the local mean.

%% Initialize
offset_c=priorValues.offset_c;

p = find(isnan(Psi(:,1)),1,'last');
[T,M] = size(Psi);
% Ppsi=dataValues.Ppsi;

% obtain prior data
startMeanVector=priorValues.mean_ln_v0;
startVarVector=priorValues.var_ln_v0;

priorPhi_V=priorValues.phi_v;
priorD_V  =priorValues.d_v;

%% Prepare Sampling

Vvars=zeros(T,M);
phi_Vdraw=zeros(M,1);

% construct VvarsOld
% VvarsOld=zeros(T,M);
%
% for i=1:M
%     VvarsOld(:,i)=reshape(Vold(i,i,:),T,1,1);
% end

%% Sample Vvars
for i=1:M

    % prepare for each i
        zEntry=p+1;

        PsiDiff=Psi(zEntry+1:T,i)-Psi(zEntry:T-1,i);

        residsTemp=PsiDiff;
        yStar=log(residsTemp.^2+offset_c);
        phi=phi_V(i);
        lnSigma2=log(Vold(zEntry+1:T,i)); % note log => h = ln sigma2

        startMean=startMeanVector(i);
        startVar=startVarVector(i);

        % sample states
        [yStarAdj, Ht] = bear.statesMix(yStar,lnSigma2);

        % sample log variances
        [logVarsDraw_Vi]=bear.KF_CKsimSV(yStarAdj,Ht,phi,startMean,startVar);
        Vvars(:,i)=[0.00000001*ones(zEntry,1) ;exp(logVarsDraw_Vi)];

        % sample phi
        [phiDraw_Vi]=bear.samplePhi(logVarsDraw_Vi,priorD_V,priorPhi_V);
        phi_Vdraw(i)=phiDraw_Vi;

end

%% Construct V
% Vdraw=zeros(M,M,T);
%
% for i=1:M
%     Vdraw(i,:)=Vvars(:,i);
% end
Vdraw=Vvars;
end


% Test file for trigtech/circconv

function pass = test_circconv(pref)

% Get preferences.
if ( nargin < 1 )
    pref = trigtech.techPref();
end

testclass = trigtech();

% Generate a random point to use as test values.
seedRNG(6178);
x = 2 * rand - 1;

%%
% Check operation in the face of empty arguments.

f = testclass.make();
g = testclass.make(@(x) sin(pi*x), [], pref);
pass(1) = (isempty(circconv(f,g)) && isempty(circconv(g,f)));

%%
% Simple checks

f_op = @(x) tanh(5*cos(pi*(x)));
g_op = @(x) ones(size(x));
f = testclass.make(f_op, [], pref);
g = testclass.make(g_op, [], pref);

hfg = circconv(f,g);
hgf = circconv(g,f);
% Answer should be zero since the functions are odd.
approx_fg = feval(hfg,x);
approx_gf = feval(hgf,x);
expected = 0;
err = abs(approx_fg - expected);
tol = 1e2*eps;
pass(2) = err < tol;

f_op = @(x) tanh(cos(pi*(x)));
a = x;
fa_op = @(x) tanh(cos(pi*(x-a)));

% Harder tests
f = testclass.make(f_op, [], pref);
g = circconv(f,f);

% Computed answer at zero:
approx = feval(g,0);
% Expected answer:
expected = sum(f.*f);
err = abs(approx - expected);
tol = 1e2*eps*vscale(g);
pass(3) = err < tol;

% Computed answer at x:
approx = feval(g,x);
% Expected answer:
h = testclass.make(fa_op, [], pref);
expected = sum(f.*h);
err = abs(approx - expected);
tol = 1e2*eps*vscale(g);
pass(4) = err < tol;

end


function [K, Kbase, n2]  = gaussianKernCompute(kern, x, x2)

% GAUSSIANKERNCOMPUTE Compute the Gaussian kernel given the parameters and X.
% FORMAT
% DESC computes the kernel parameters for the Gaussian kernel given
% inputs associated with rows and columns.
% RETURN K : the kernel matrix computed at the given points.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG X : the input matrix associated with the rows of the kernel.
% ARG X2 : the input matrix associated with the columns of the kernel.
%
% FORMAT
% DESC computes the kernel matrix for the Gaussian kernel given a design matrix of inputs.
% RETURN K : the kernel matrix computed at the given points.
% ARG kern : the kernel structure for which the matrix is computed.
% ARG x : input data matrix in the form of a design matrix.
%
% SEEALSO : gaussianKernParamInit, kernCompute, kernCreate, gaussianKernDiagCompute
%
% COPYRIGHT : Mauricio Alvarez and Neil D. Lawrence, 2008
%
% MODIFICATIONS: Mauricio Alvarez, 2009

% KERN

if kern.isArd
    sqrtP = sqrt(kern.precisionU);
    sqrtPx = x*sparseDiag(sqrtP);
    if nargin < 3
        n2 = dist2(sqrtPx, sqrtPx);
    else
        sqrtPx2 = x2*sparseDiag(sqrtP);
        n2 = dist2(sqrtPx, sqrtPx2);
    end
    Kbase = exp(-0.5*n2);
else
    if nargin < 3
        n2 = dist2(x, x);
    else
        n2 = dist2(x, x2);
    end
    Kbase = exp(-0.5*kern.precisionU*n2);
end
K = kern.sigma2Latent*Kbase;


function X = normrow(X)
if nargin < 1
    error('Not enough input arguments.'); end

C = size(X,2);
if C == 1
    X = X ./ abs(X);
else
    X = sqrt(ones./(sum(X.*X,2)'))'*ones(1,C).*X;
end


function [ceq,ceqJac] = autoGen_cst_steplength(q1m,q2m,q4m,q5m,l1,l2,l4,l5,stepLength)
%AUTOGEN_CST_STEPLENGTH
%    [CEQ,CEQJAC] = AUTOGEN_CST_STEPLENGTH(Q1M,Q2M,Q4M,Q5M,L1,L2,L4,L5,STEPLENGTH)

%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    22-Oct-2015 19:14:35

t2 = cos(q1m);
t3 = l1.*t2;
t4 = cos(q2m);
t5 = l2.*t4;
t6 = cos(q4m);
t7 = cos(q5m);
t8 = sin(q1m);
t9 = sin(q2m);
t10 = sin(q4m);
t11 = l4.*t10;
t12 = sin(q5m);
t13 = l5.*t12;
ceq = [-stepLength+t11+t13-l1.*t8-l2.*t9;t3+t5-l4.*t6-l5.*t7];
if nargout > 1
    ceqJac = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-t3,-l1.*t8,-t5,-l2.*t9,0.0,0.0,l4.*t6,t11,l5.*t7,t13,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0],[2, 22]);
end


function test26
%TEST26 test cs_dmsol and cs_dmspy
%
% Example:
%   test26
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

clear functions

randn ('state', 0) ;
rand ('state', 0) ;

clf

ntrials = 1000 ;
e1 = zeros (ntrials,1) ;
e2 = zeros (ntrials,1) ;

for trials = 1:ntrials

    m = fix (100 * rand (1)) ;
    n = fix (100 * rand (1)) ;
    % d = 0.1 * rand (1) ;
    d = rand (1) * 4 * max (m,n) / max (m*n,1) ;
    A = sprandn (m,n,d) ;
    % S = sprandn (m,m,d) + speye (m) ;

    if (~ispc)
        if (rand ( ) > .5)
            A = A + 1i * sprand (A) ;
        end
    end

    subplot (1,3,2) ; spy (A) ;
    subplot (1,3,3) ; cs_dmspy (A) ;

    b = rand (m,1) ;

    if (~ispc)
        if (rand ( ) > .5)
            b = b + 1i * rand (size (b)) ;
        end
    end
    % MATLAB cannot do A\b when A is sparse and rectangular and either
    % A or b are complex
    if (m ~= n & isreal (A) & ~isreal (b))                                  %#ok
        x1 = (A\real(b)) + 1i * (A\imag(b)) ;
        err1 = norm (A*x1-b) ;
    elseif ((m ~= n) & ~isreal (A))                                         %#ok
        err1 = 1 ;
    else
        x1 = A\b ;
        err1 = norm (A*x1-b) ;
    end

    x2 = cs_dmsol (A,b) ;

    err2 = norm (A*x2-b) ;

    lerr1 = log10 (max (err1, eps)) ;
    lerr2 = log10 (max (err2, eps)) ;

    fprintf ('rank: %3d %3d err %6.2e  %6.2e  :   %6.1f\n', ...
        sprank(A), rank(full(A)), err1, err2, lerr1 - lerr2) ;

    if (isnan (err1))
        lerr1 = 10 ;
    end
    if (isnan (err2))
        lerr2 = 10 ;
    end

    if (lerr2 > lerr1 + 5)
        % pause
    end

    e1 (trials) = lerr1 ;
    e2 (trials) = lerr2 ;

    subplot (1,3,1) ; plot (e1, e2, 'o', [-16 10], [-16 10], 'r') ;
    xlabel ('MATLAB error') ;
    ylabel ('dmsol error') ;

    drawnow
    % pause
end


function [ p, t ] = cvt_square_uniform ( n, sample_num, delaunay_display )

%*****************************************************************************80
%
%% CVT_SQUARE_UNIFORM demonstrates how a CVT can be computed and displayed in MATLAB.
%
%  Discussion:
%
%    This simple example carries out an iterative CVT calculation in the
%    unit square, with a uniform density.  The initial placement of the
%    generators is random.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Qiang Du, Vance Faber, Max Gunzburger,
%    Centroidal Voronoi Tessellations: Applications and Algorithms,
%    SIAM Review,
%    Volume 41, 1999, pages 637-676.
%
%  Parameters:
%
%    Input, integer N, the number of generators.
%
%    Input, integer SAMPLE_NUM, the number of sample points.
%
%    Input, logical DELAUNAY_DISPLAY, is TRUE (nonzero) if the Delaunay
%    triangulation is to be displayed.
%
%    Output, real P(N,2), the location of the generators.
%
%    Output, integer T(NT,3), information defining the Delaunay
%    triangulation of the generators.  NT is the number of triangles,
%    which varies depending on the arrangement of the generators.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_SQUARE_UNIFORM:\n' );
  fprintf ( 1, '  A simple demonstration of a CVT computation\n' );
  fprintf ( 1, '  (Centroidal Voronoi Tessellation)\n' );
  fprintf ( 1, '  in a square, with a uniform density.\n' );

  if ( nargin < 1 )
    n = 100;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_SQUARE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of N was supplied.\n' );
    fprintf ( 1, '  N is the number of generators.\n' );
    fprintf ( 1, '  A default value N = %d will be used.\n', n );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of generators = %d\n',  n );
  end

  if ( nargin < 2 )
    sample_num = 1000 * n;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_SQUARE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of SAMPLE_NUM was supplied.\n' );
    fprintf ( 1, '  SAMPLE_NUM is the number of sample points.\n' );
    fprintf ( 1, '  A default value SAMPLE_NUM = %d will be used.\n', ...
      sample_num );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified number of sample points = %d\n', ...
      sample_num );
  end

  if ( nargin < 3 )
    delaunay_display = 0;
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CVT_SQUARE_UNIFORM - Note:\n' );
    fprintf ( 1, '  No value of DELAUNAY_DISPLAY was supplied.\n' );
    fprintf ( 1, '  DELAUNAY_DISPLAY is TRUE (nonzero) if the\n' );
    fprintf ( 1, '  Delaunay triangulation is also to be displayed.\n' );
    fprintf ( 1, '  A default value DELAUNAY_DISPLAY = %d will be used.\n', ...
      delaunay_display );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  User specified DELAUNAY_DISPLAY = %d\n', ...
      delaunay_display );
  end
%
%  This switch is set to 1 (TRUE) if the ACCUMARRAY command is available.
%  That speeds up the calculation a lot.  If you don't have the ACCUMARRAY
%  command, just set this to 0.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_SQUARE_UNIFORM:\n' );
  fprintf ( 1, '  MATLAB''s ACCUMARRAY command can be used for faster\n' );
  fprintf ( 1, '  computation.  This command is not available in\n' );
  fprintf ( 1, '  some versions of MATLAB.  If ACCUMARRAY is available,\n' );
  fprintf ( 1, '  simply make sure that the ACCUMARAY_AVAILABLE variable\n' );
  fprintf ( 1, '  is set to 1!\n' );

  accumarray_available = 1;

  if ( accumarray_available )
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will be used.\n' );
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, '  The ACCUMARRAY command will NOT be used.\n' );
  end
%
%  Clear the figure screen, if already open.
%
  clf
%
%  Randomize the initial locations of the generators in the unit square.
%  If another region is used, then this initialization should be changed.
%
  p = rand ( n, 2 );
  plot ( p(:,1), p(:,2), 'b.' );
  axis ( [ 0.0, 1.0, 0.0, 1.0 ] )
  line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ], ...
    'Color', 'r', 'LineWidth', 2 );
  title_string = sprintf ( 'Initial Generators' );
  title ( title_string );
  axis equal
  drawnow

  string = input ( 'RETURN, or Q to quit: ', 's' );

  if ( string == 'q' | string == 'Q' )
    return
  end

  it = 0;

  while ( 1 )
%
%  Compute the Delaunay triangle information T for the current nodes.
%
    t = delaunay ( p(:,1), p(:,2) );
%
%  Display the Delaunay triangulation, if requested.
%
    if ( delaunay_display )
      subplot ( 1, 2, 2 )
      trimesh ( t, p(:,1), p(:,2), zeros(n,1), 'EdgeColor', 'b' )
      hold on
      plot ( p(:,1), p(:,2), 'b.' );
      axis ( [ -0.0, 1.0, -0.0, 1.0 ] )
      line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ], ...
        'Color', 'r', 'LineWidth', 2 );
      title_string = sprintf ( 'Delaunay, step %d', it );
      title ( title_string );
      axis equal
      view ( 2 )
      hold off
    end
%
%  Display the CVT generators, and the associated Voronoi diagram.
%
    if ( delaunay_display )
      subplot ( 1, 2, 1 )
    end

    voronoi ( p(:,1), p(:,2), t );
    axis ( [ -0.0, 1.0, -0.0, 1.0 ] )
    line ( [ 0.0, 1.0, 1.0, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 1.0, 0.0 ], ...
      'Color', 'r', 'LineWidth', 2 );
    title_string = sprintf ( 'Voronoi, step %d', it );
    title ( title_string );
    axis equal
    axis tight
    drawnow
%
%  Generate sample points.
%
%  These sample points implicitly define the geometry of the region.
%  If the region is not a unit square, then the range of the sample
%  data must be changed.
%
%  The data is sampled uniformly.  If a nonuniform density is desired,
%  then the sampling must be done in a biased way.
%
    xs = rand ( sample_num, 1 );
    ys = rand ( sample_num, 1 );
%
%  For each sample point, find K, the index of the nearest generator.
%  We do this efficiently by using the Delaunay information with
%  Matlab's DSEARCH command, rather than a brute force nearest neighbor
%  computation.
%
    k(1:sample_num,1) = dsearch ( p(:,1), p(:,2), t, xs, ys );
%
%  The centroid of the Voronoi region associated with each generator
%  is approximated by the average of the sample points it was closest to.
%
    if ( accumarray_available )

      count(1:n) = accumarray ( k, ones(sample_num,1) );
      centroid(1,1:n) = accumarray ( k, xs );
      centroid(2,1:n) = accumarray ( k, ys );

    else

      count(1:n) = 0;
      centroid(1,1:n) = 0.0;
      centroid(2,1:n) = 0.0;

      for i = 1 : sample_num
        j = k(i);
        count(j) = count(j) + 1;
        centroid(1,j) = centroid(1,j) + xs(i);
        centroid(2,j) = centroid(2,j) + ys(i);
      end

    end
%
%  Replace the generators by the centroids.
%
    p(1:n,1) = ( centroid(1,1:n) ./ count(1:n) )';
    p(1:n,2) = ( centroid(2,1:n) ./ count(1:n) )';

    string = input ( 'RETURN, or Q to quit: ', 's' );

    if ( string == 'q' | string == 'Q' )
      break
    end

    it = it + 1;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CVT_SQUARE_UNIFORM:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function p = square_uniform ( n )

%*****************************************************************************80
%
%% SQUARE_UNIFORM returns sample points from the unit square.
%
%  Discussion:
%
%    This routine returns N points sampled uniformly at random
%    from within the unit square.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 May 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points to generate.
%
%    Output, real P(N,2), the sample points.
%
  p(1:n,1:2) = rand(n,2);

  return
end


%% Mean-Shift Video Tracking
% by Sylvain Bernhardt
% July 2008
%% Description
% Estimate the density of data samples
% (here colour histogram) in a patch T
% with a kernel profile k. Lmap is the
% colormap length and H,W the patch size.

function q = Density_estim(T,Lmap,k,H,W,graph)
q = zeros(Lmap,1);
colour = linspace(1,Lmap,Lmap);
for x=1:W
    for y=1:H
        q(T(y,x)+1) = q(T(y,x)+1)+k(y,x);
    end
end

% Normalizing
C = 1/sum(sum(k));
q = C.*q;

% Plotting the estimated densities
if graph==1
    figure
    plot(colour,q);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   demo script for mesh generation from binary volumetric image
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 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
% A,b are registration matrix and vector, respectively
%% perform mesh generation

%% use the alternative 'cgalmesh' method. This will call
% cgalmesher to process labled volume to produce surfaces
% and tetrahedral mesh in a single run.
clear opt
opt.radbound=2;
[node,elem,face]=v2m(uint8(volimage),0.5,opt,100,'cgalmesh');

%% visualize the resulting mesh

plotmesh(node,face(:,1:3));
axis equal;


%Test weka's Naive Bayes implementation on the iris data.

load fisheriris;    %built in to matlab

%Shuffle the data
rand('twister',0);
perm = randperm(150);
meas = meas(perm,:);
species = species(perm,:);

featureNames = {'sepallength','sepalwidth','petallength','petalwidth','class'};

%Prepare test and training sets.
data = [num2cell(meas),species];
train = data(1:120  ,:);
test  = data(121:end,:);

classindex = 5;

%Convert to weka format
train = matlab2weka('iris-train',featureNames,train,classindex);
test =  matlab2weka('iris-test',featureNames,test);

%Train the classifier
nb = trainWekaClassifier(train,'bayes.NaiveBayes');

%Test the classifier
predicted = wekaClassify(test,nb);

%The actual class labels (i.e. indices thereof)
actual = test.attributeToDoubleArray(classindex-1); %java indexes from 0

errorRate = sum(actual ~= predicted)/30


clear;
noOfNodes  = 100;
%rand('state', 0);
plot_flag = 1;
if plot_flag==1
    figure(1);
    clf;
    hold on;
end
L = 1000; % size of the whole area
R = 300; % maximum range;

s = 1; % source id
d = 10; % destination id

netXloc = double(rand(1,noOfNodes)*L);
netYloc = double(rand(1,noOfNodes)*L);
%netXloc = [0 1 0 1];
%netYloc = [0 0 1 1];
Astar_connect = zeros(noOfNodes, noOfNodes);
Astar_coord = zeros(noOfNodes, 2);
for i = 1:noOfNodes
    if plot_flag==1
        plot(netXloc(i), netYloc(i), '.');
    end
    Astar_coord(i,1) = netXloc(i);
    Astar_coord(i,2) = netYloc(i);
    if plot_flag == 1
        %text(netXloc(i), netYloc(i), num2str(i));
    end
    for j = 1:noOfNodes
        distance = sqrt((netXloc(i) - netXloc(j))^2 + (netYloc(i) - netYloc(j))^2);
        if distance <= R
            matrix(i, j) = distance;   % if set to '1', Dijkstra computes Spath in terms of hops; if set to 'distance', it is the real shortest path
            if i~=j % must be satisfied
                Astar_connect(i, j) = 1;
            else
                Astar_connect(i, j) = 0;
            end
            if plot_flag==1
                %line([netXloc(i) netXloc(j)], [netYloc(i) netYloc(j)], 'LineStyle', ':');
            end
        else
            matrix(i, j) = inf;
            Astar_connect(i, j) = 0;
        end;
    end;
end;

activeNodes = [];
for i = 1:noOfNodes,
    % initialize the farthest node to be itself;
    farthestPreviousHop(i) = i;     % used to compute the RTS/CTS range;
    farthestNextHop(i) = i;
end;

Astar_coord;
Astar_connect;

[path, totalCost, farthestPreviousHop, farthestNextHop] = dijkstra(noOfNodes, matrix, s, d, farthestPreviousHop, farthestNextHop);
combo = [noOfNodes s-1 d-1 R/2];
[Astar_path, Astar_search] = Astar(Astar_coord', Astar_connect, combo); % notice, we must put Astar_coord' rather than Astar_coord
path
totalCost
Astar_path
Astar_search
if length(path) ~= 0
    for i = 1:(length(path)-1)
        if plot_flag==1
            line([netXloc(path(i)) netXloc(path(i+1))], [netYloc(path(i)) netYloc(path(i+1))], 'Color','g','LineWidth', 2.50, 'LineStyle', '-.');
            text(netXloc(i), netYloc(i), num2str(i));
        end
    end;
end;

if length(Astar_path) ~= 0
    for i = 1:(length(Astar_path)-1)
        if plot_flag==1
            line([netXloc(Astar_path(i)) netXloc(Astar_path(i+1))], [netYloc(Astar_path(i)) netYloc(Astar_path(i+1))], 'Color','r','LineWidth', 2.50, 'LineStyle', '-.');
            text(netXloc(i), netYloc(i), num2str(i));
        end
    end;
end;
if plot_flag == 1
    hold off;
end
return;


function [ fea, out ] = ex_planestress2( varargin )
%EX_PLANESTRESS2 NAFEMS benchmark challenge 1 plane stress example.
%
%   [ FEA, OUT ] = EX_PLANESTRESS2( VARARGIN ) NAFEMS benchmark example to calculate von Mieses stress
%   thin plate under plane stress assumption with loads all around.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       E           scalar {210e9}         Modulus of elasticity
%       nu          scalar {0.3}           Poissons ratio
%       igrid       scalar 1/{0}           Cell type (0=quadrilaterals, 1=triangles)
%       hmax        scalar {1/20}          Max grid cell size
%       sfun        string {sflag1}        Shape function for displacements
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'E',        210e9; ...
  'nu',       0.3; ...
  'igrid',    0; ...
  'hmax',     1/20; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry definition.
gobj1 = gobj_rectangle( 0, 1, 0, 1, 'R1' );
fea.geom.objects = { gobj1 };
fea.sdim = { 'x' 'y' };

% Grid generation.
if( opt.igrid==1 )
  fea.grid = gridgen(fea,'hmax',opt.hmax,'fid',fid);
else
  nx = 1/opt.hmax;
  fea.grid = rectgrid( nx, nx );
end

% Boundary conditions.
dtol = 0.1;
if( opt.igrid==1 )
  lbdr = findbdr( fea, ['x<',num2str(dtol)] );     % Left boundary number.
  rbdr = findbdr( fea, ['x>',num2str(1-dtol)] );   % Right boundary number.
  tbdr = findbdr( fea, ['y>',num2str(1-dtol)] );   % Top boundary number.
  bbdr = findbdr( fea, ['y<',num2str(dtol)] );     % Bottom boundary number.
else
  lbdr = 4;
  rbdr = 2;
  tbdr = 3;
  bbdr = 1;
end

% Add plane stress physics mode.
fea = addphys(fea,@planestress);
fea.phys.pss.eqn.coef{1,end} = { opt.nu };
fea.phys.pss.eqn.coef{2,end} = { opt.E  };
fea.phys.pss.sfun            = { opt.sfun opt.sfun };

% Set boundary condition types.
bctype = mat2cell( zeros(2,4), [1 1], [1 1 1 1] );
fea.phys.pss.bdr.coef{1,5}   = bctype;

% Set loads on boundary.
bccoef = mat2cell( zeros(2,4), [1 1], [1 1 1 1] );
bccoef{1,tbdr} = '-x';
bccoef{2,tbdr} = 'x';
bccoef{1,bbdr} = '-(1-x)';
bccoef{2,bbdr} = '1-x';
bccoef{1,lbdr} = '1-y';
bccoef{2,lbdr} = '-(1-y)';
bccoef{1,rbdr} = 'y';
bccoef{2,rbdr} = '-y';
fea.phys.pss.bdr.coef{1,end} = bccoef;

% Parse and solve problem.
fea       = parsephys(fea);             % Check and parse physics modes.
fea       = parseprob(fea);             % Check and parse problem struct.
fea.sol.u = solvestat(fea,'fid',fid);   % Call to stationary solver.

% Postprocessing.
s_vm = fea.phys.pss.eqn.vars{1,2};
if ( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', s_vm, 'isoexpr', s_vm )
  title('von Mieses stress')
end

% Error checking.
dtol = sqrt(eps);
sm  = evalexpr( s_vm, [0.5;0.5], fea );
s01 = evalexpr( s_vm, [dtol;1],    fea );
s10 = evalexpr( s_vm, [1;dtol],    fea );
s00 = evalexpr( s_vm, [dtol;dtol], fea );
s11 = evalexpr( s_vm, [1;1],       fea );

sol = [ sm s01 s10 s00 s11 ];
ref = [  0   0   0   2   2 ];
out.err    = norm(sol-ref)/norm(ref);
out.pass   = out.err < 0.1;

if ( nargout==0 )
  clear fea out
end


function n=grid2n(i,j,Nj)
n=i+(j-1)*Nj;


function [error_x,error_y] = stokespost_q1q1_bc(aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound)
%stokespost_q1q1_bc   postprocesses Poisson error estimator
%   [error_x,error_y] = stokespost_q1q1_bc(aez,fezx,fezy,elerrorx,elerrory,xy,ev,ebound);
%   input
%          aez                  elementwise Poisson problem matrices
%          fezx,fezy            elementwise rhs vectors
%          elerrorx, elerrory   elementwise error estimate (without BC imposition)
%          xy                   vertex coordinate vector
%          ev                   element mapping matrix
%          ebound               element edge boundary matrix
%   output
%          error_x, error_y     component elementwise error estimate
%
%   calls function localbc_xy
%   IFISS function: DJS; 8 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      x=xy(:,1); y=xy(:,2);
      nel=length(ev(:,1));
      lev=[ev,ev(:,1)];
	  error_x=elerrorx;  error_y=elerrory;
%
% recompute contributions from elements with Dirichlet boundaries
      nbde=length(ebound(:,1));
      ebdy = zeros(nel,1);
      edge = zeros(nel,1);
% isolate boundary elements
      for el = 1:nbde
      ee = ebound(el,1);
      ebdy(ee) = ebdy(ee)+1; edge(ee)=ebound(el,2);
      end
%
% two edge elements
      k2=find(ebdy==2);
      nel2b=length(k2);
% loop over two edge elements
      for el = 1:nel2b
      el2e=k2(el);
      kk=find(ebound(:,1) == el2e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
	  ae=squeeze(aez(el2e,1:5,1:5));
      fex=fezx(el2e,:)'; fey=fezy(el2e,:)';
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el2e,:)); yl=y(lev(el2e,:));
	  [bae,fex,fey] = localbc_xy(ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=bae\fex;  erry=bae\fey;
	  error_x(el2e,1) = errx'*ae*errx; error_y(el2e,1) = erry'*ae*erry;
	  end
% end of element loop
%
% one edge elements
      k1=find(ebdy==1);
      nel1b=length(k1);
% loop over one edge elements
      for el = 1:nel1b
      el1e=k1(el);
      kk=find(ebound(:,1) == el1e);
      edges=ebound(kk,2);
% set up original matrix and RHS vector
      fex=fezx(el1e,:)'; fey=fezy(el1e,:)';
	  ae=squeeze(aez(el1e,1:5,1:5));
% set up local coordinates and impose interpolated error as Dirichlet bc
      xl=x(lev(el1e,:)); yl=y(lev(el1e,:));
	  [bae,fex,fey] = localbc_xy(ae,fex,fey,edges,xl,yl);
% solve local problems
      errx=bae\fex;  erry=bae\fey;
	  error_x(el1e,1) = errx'*ae*errx; error_y(el1e,1) = erry'*ae*erry;

      end
% end of element loop
%
      err_x = sqrt(sum(error_x)); error_x = sqrt(error_x);
	  err_y = sqrt(sum(error_y)); error_y = sqrt(error_y);
      fprintf('estimated velocity error (in energy):  (%10.6e,%10.6e) \n',err_x,err_y)
 return


function [x_sil, y_sil] = removeSilentFrames(x, y, range, N, K)

x       = x(:);
y       = y(:);

frames  = 1:K:(length(x)-N);
w       = ml_hanning(N);
msk     = zeros(size(frames));

for j = 1:length(frames)
    jj      = frames(j):(frames(j)+N-1);
    msk(j) 	= 20*log10(norm(x(jj).*w)./sqrt(N));
end

msk     = (msk-max(msk)+range)>0;
count   = 1;

x_sil   = zeros(size(x));
y_sil   = zeros(size(y));

for j = 1:length(frames)
    if msk(j)
        jj_i            = frames(j):(frames(j)+N-1);
        jj_o            = frames(count):(frames(count)+N-1);
        x_sil(jj_o)     = x_sil(jj_o) + x(jj_i).*w;
        y_sil(jj_o)  	= y_sil(jj_o) + y(jj_i).*w;
        count           = count+1;
    end
end

x_sil = x_sil(1:jj_o(end));
y_sil = y_sil(1:jj_o(end));


function[z]=celldiv(x,y)
%CELLDIV  Division acting on each element in a cell array.
%
%   Z=CELLDIV(X,Y) where X and Y are both cell arrays of N arrays, with the
%   corresponding elements in X and in Y having the same size, returns the
%   cell array Z containing their ratios,
%
%       Z{1}=Y{1}./X{1}, Z{2}=Y{2}./X{2},..., Z{N}=Y{N}./X{N}.
%
%   One of X or Y may also be a scalar or a numeric array of the same
%   length as the other input argument.
%
%   Usage: z=celldiv(x,y);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2015--2019 J.M. Lilly --- type 'help jlab_license' for details

if ~iscell(x)&&iscell(y)
    z=y;
    if length(x)==1
        x=x.*ones(length(y),1);
    end
    if length(x)==length(y)
        for i=1:length(x)
            z{i}=y{i}./x(i);
        end
    else
        error('LENGTH(X) and LENGTH(Y) must be the same.')
    end
elseif iscell(x)&&~iscell(y)
    z=x;
    if length(y)==1
        y=y.*ones(length(x),1);
    end
    if length(x)==length(y)
        for i=1:length(x)
            z{i}=y(i)./x{i};
        end
    else
        error('LENGTH(X) and LENGTH(Y) must be the same.')
    end
elseif iscell(x)&&iscell(y)
    z=y;
    for i=1:length(x)
        z{i}=y{i}./x{i};
    end
end


function wishart_test04 ( )

%*****************************************************************************80
%
%% WISHART_TEST04 demonstrates the Wishart sampling function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 July 2013
%
%  Author:
%
%    John Burkardt
%

%
%  Access the PDFLIB and RNGLIB libraries.
%
  addpath ( '../pdflib' );
  addpath ( '../rnglib' );
%
%  Initialize the RNGLIB library.
%
  initialize ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'WISHART_TEST04:\n' );
  fprintf ( 1, '  We can compute sample Wishart matrices by:\n' );
  fprintf ( 1, '    W = wishart_sample ( n, df, sigma );\n' );
%
%  Set the parameters and call.
%
  n = 5;
  df = 8;
  sigma = eye ( 5, 5 );
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 5, 8, Identity ):' );
%
%  Calling again yields a new matrix.
%
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 5, 8, Identity ):' );
%
%  Try a diagonal matrix.
%
  sigma = diag ( [1.0, 2.0, 3.0, 4.0, 5.0 ] );
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 5, 8, diag(1,2,3,4,5) ):' );
%
%  Try a smaller matrix.  Sigma must be positive definite symmetric.
%
  n = 3;
  df = 3;
  r = [ 5.0, 1.0, 3.0; ...
        0.0, 4.0, 2.0; ...
        0.0, 0.0, 6.0 ];
  sigma = r' * r;
  r8mat_print ( n, n, sigma, '  Set covariance SIGMA:' );
  w = wishart_sample ( n, df, sigma );
  r8mat_print ( n, n, w, '  wishart_sample ( 3, 3, sigma ):' );
%
%  What is the eigendecomposition of this matrix?
%
  [ v, lambda ] = eigs ( w );
  r8mat_print ( n, n, v, '  Eigenvectors of previous matrix:' );
  r8mat_print ( n, n, lambda, '  Eigenvalues of previous matrix:' );

  rmpath ( '../pdflib' );
  rmpath ( '../rnglib' );

  return
end


function p00_start_test ( problem_num )

%*****************************************************************************80
%
%% P00_START_TEST prints the norm of the starting point and its function value.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROBLEM_NUM, the number of problems.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'P00_START_TEST\n' );
  fprintf ( 1, '  Get norms of starting point X0 and F(X0)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   Problem    Option        ||X0||          ||F(X0)||\n' );
  fprintf ( 1, '\n' );

  for problem = 1 : problem_num

    option_num = p00_option_num ( problem );

    fprintf ( 1, '\n' );

    for option = 1 : option_num

      nvar = p00_nvar ( problem, option );

      x0 = p00_start ( problem, option, nvar );

      x0_norm = norm ( x0(1:nvar) );

      fx0 = p00_fun ( problem, option, nvar, x0 );

      fx0_norm = norm ( fx0(1:nvar-1) );

      fprintf ( 1, '  %8d  %8d  %14f  %14f\n', ...
        problem, option, x0_norm, fx0_norm )

    end
  end

  return
end


function [ m, d ] = mothers_day ( y )

%*****************************************************************************80
%
%% MOTHERS_DAY computes the date of Mother's Day (US) for a Common year.
%
%  Discussion:
%
%    Mother's Day occurs on the second Sunday in May.
%
%  Example:
%
%    Input:
%
%      Y = 2003
%
%    Output:
%
%      M = 5
%      D = 11
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year.
%
%    Output, integer M, D, the month and day of Mother's Day.
%

%
%  Determine the day of the week for 8 May, the earliest day
%  that Mother's day can occur.
%
  m = 5;
  d = 8;
  f = 0.0;

  w = ymdf_to_weekday_common ( y, m, d, f );
%
%  W = 1 means this day is Sunday, and day D is Mother's day.
%  Otherwise, figure out how to increment W to 8 (Sunday again);
%  The same increment makes D the correct day number.
%
  if ( w ~= 1 )
    d = d + 8 - w;
  end

  return
end


function [Sf,AI,AIrows,AIcols] = imageStats1(f)
%imageStats1 Sample function used in Chapter 2.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt
Sf = size(f);
AI = mean2(f);
AIrows = mean(f,2);
AIcols = mean(f,1);


function test_stepsize_alg_demo()
% demonstration file for original stepsize algorithm.
%
% This file illustrates how to set user's own stepsize algorithm in case of linear
% regression problem. This demonstrates SGD and SVRG algorithms.
%
% This file is part of SGDLibrary.
%
% Created by H.Kasai on Sep. 25, 2017

    clc;
    clear;
    close all;

    %% generate synthetic data
    % set number of dimensions
    d = 10;
    % set number of samples
    n = 1000;
    % generate data
    data = logistic_regression_data_generator(n, d);

    %% define problem definitions
    problem = logistic_regression(data.x_train, data.y_train, data.x_test, data.y_test);

%     %% define user-defined stepsize algorithm
%     function step = my_stepalg(iter, options)
%         step = options.step_init / (10 + iter*0.5);
%     end
%

    %% perform algorithms SGD and SVRG
    options.w_init = data.w_init;
    options.step_init = 0.01;
    options.verbose = 2;

    options.step_alg = 'fix';
    [w_sgd_fix, info_sgd_fix] = sgd(problem, options);

    options.step_alg = 'decay';
    [w_sgd_decay, info_sgd_decay] = sgd(problem, options);

    options.step_alg = 'decay-2';
    [w_sgd_decay2, info_sgd_decay2] = sgd(problem, options);

    options.stepsizefun = @my_stepalg;  % set my_stepalg (user-defined stepsize algorithm)
    [w_sgd_my, info_sgd_my] = sgd(problem, options);

    %% display cost/optimality gap vs number of gradient evaluations
    display_graph('grad_calc_count','cost', {'SGD (fix)','SGD (decay)', 'SGD (decay-2)', 'SGD (My stepsize algorithm)'}, ...
            {w_sgd_fix, w_sgd_decay w_sgd_decay2, w_sgd_my}, {info_sgd_fix, info_sgd_decay, info_sgd_decay2, info_sgd_my});

end

    %% define user-defined stepsize algorithm
    function step = my_stepalg(iter, options)
        step = options.step_init / (10 + iter*0.5);
    end


function asa136_test01 ( )

%*****************************************************************************80
%
%% TEST01 tries out the ASA136 routine.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  k = 5;
  m = 100;
  n = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Test the KMNS algorithm.\n' );
  fprintf ( 1, '  Applied Statistics Algorithm 136\n' );
%
%  Read the data.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reading the data.\n' );

  input_unit = fopen ( 'points_100.txt', 'rt' );

  for i = 1 : m
    x(i,1:n) = fscanf ( input_unit, '%f', n );
  end

  fclose ( input_unit );
%
%  Print a few data values.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  First 5 data values:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 5
    fprintf ( 1, '  %8d', j );
    for j = 1 : n
      fprintf ( 1, '  %14f', x(i,j) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Initialize the cluster centers.
%  Here, we arbitrarily make the first K data points cluster centers.
%
  for i = 1 : k
    for j = 1 : n
      c(i,j) = x(i,j);
    end
  end
%
%  Compute the clusters.
%
  iter = 50;

 [ c, ic1, nc, wss, ifault ] = kmns ( x, m, n, c, k, iter );

  if ( ifault ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST01 - Fatal error!\n' );
    fprintf ( 1, '  KMNS returned IFAULT = %d\n', ifault );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Cluster  Population  Energy\n' );
  fprintf ( 1, '\n' );

  for i = 1 : k
    fprintf ( 1, '  %8d  %8d  %14f\n', i, nc(i), wss(i) );
  end

  nc_sum = sum ( nc(1:k) );
  wss_sum = sum ( wss(1:k) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     Total  %8d  %14f\n', nc_sum, wss_sum );

  return
end