function X = vgg_intersect_quadrics(A, B, C);
%
% function X = vgg_intersect_quadrics(A, B, C);
%
% Purpose:
%   Compute intersections of three quadrics in projective 3-space.
%
% Input:
%   A, B, C are symmetric 4x4 matrices.
%
% Output:
%   X is 4-by-8 complex. Each column is a root.
%
% The function works by converting the problem into an 8x8 generalized
% eigen-problem.
%
% fsm@robots.ox.ac.uk
%

if any(size(A) ~= [4 4])
  error('bad A')
end
if any(size(B) ~= [4 4])
  error('bad B')
end
if any(size(C) ~= [4 4])
  error('bad C')
end

% symmetrize and normalize.
A = A + A.'; A = A / norm(A, 'fro'); a = [A(1, 1) 2*A(1, 2) 2*A(1, 3) 2*A(1, 4) A(2, 2) 2*A(2, 3) 2*A(2, 4) A(3, 3) 2*A(3, 4) A(4, 4)].';
B = B + B.'; B = B / norm(B, 'fro'); b = [B(1, 1) 2*B(1, 2) 2*B(1, 3) 2*B(1, 4) B(2, 2) 2*B(2, 3) 2*B(2, 4) B(3, 3) 2*B(3, 4) B(4, 4)].';
C = C + C.'; C = C / norm(C, 'fro'); c = [C(1, 1) 2*C(1, 2) 2*C(1, 3) 2*C(1, 4) C(2, 2) 2*C(2, 3) 2*C(2, 4) C(3, 3) 2*C(3, 4) C(4, 4)].';

% make multiplication tables (for degrees 1+2=3, 2+2=4 and 3+1=4)
if 1
  ijk = [
    1
    22
    43
    64
    82
    105
    126
    147
    163
    186
    208
    229
    244
    267
    289
    310
    325
    351
    372
    393
    406
    432
    454
    475
    487
    513
    535
    556
    568
    594
    617
    638
    649
    675
    698
    719
    730
    756
    779
    800
    ];
  X12 = zeros(20, 4, 10); X12(ijk) = 1;

  ijk = [
    1
    37
    73
    109
    145
    181
    217
    253
    289
    325
    352
    390
    426
    462
    501
    537
    573
    609
    645
    681
    703
    741
    778
    814
    852
    889
    925
    962
    998
    1034
    1054
    1092
    1129
    1165
    1203
    1240
    1276
    1313
    1349
    1385
    1405
    1446
    1482
    1518
    1561
    1597
    1633
    1669
    1705
    1741
    1756
    1797
    1834
    1870
    1912
    1949
    1985
    2022
    2058
    2094
    2107
    2148
    2185
    2221
    2263
    2300
    2336
    2373
    2409
    2445
    2458
    2499
    2537
    2573
    2614
    2652
    2688
    2726
    2762
    2798
    2809
    2850
    2888
    2924
    2965
    3003
    3039
    3077
    3113
    3149
    3160
    3201
    3239
    3275
    3316
    3354
    3390
    3428
    3464
    3500
    ];
  X22 = zeros(35, 10, 10); X22(ijk) = 1;

  ijk = [
    1
    37
    73
    109
    145
    181
    217
    253
    289
    325
    361
    397
    433
    469
    505
    541
    577
    613
    649
    685
    702
    740
    776
    812
    851
    887
    923
    959
    995
    1031
    1071
    1107
    1143
    1179
    1215
    1251
    1287
    1323
    1359
    1395
    1403
    1441
    1478
    1514
    1552
    1589
    1625
    1662
    1698
    1734
    1772
    1809
    1845
    1882
    1918
    1954
    1991
    2027
    2063
    2099
    2104
    2142
    2179
    2215
    2253
    2290
    2326
    2363
    2399
    2435
    2473
    2510
    2546
    2583
    2619
    2655
    2692
    2728
    2764
    2800
    ];
  X31 = zeros(35, 20, 4); X31(ijk) = 1;

  clear ijk
end

% compose ideal in degree 2.
I2 = [a b c];

% compute ideal in degrees 3 and 4.
I3 = reshape(reshape(X12, [20* 4 10]) * I2, [20 3* 4]);
I4 = reshape(reshape(X22, [35*10 10]) * I2, [35 3*10]);

% the columns of C3 and C4 span complements of I3 and I4, respectively.
[U,S,V] = svd(I3.'); C3 = V(:, 13:20); %W3 = diag(S).'
[U,S,V] = svd(I4.'); C4 = V(:, 28:35); %W4 = diag(S).'

% make the four multiplication operators.
M = cell(1, 4);
for k=1:4
  M{k} = C4.' * X31(:, :, k) * C3;
end

% FIXME: here we only use M{1} and M{2} to compute the Schur
% decomposition although we use all four multiplication operators to
% extract the root coordinates.
[AA, BB, Q, Z] = qz(M{1}, M{2});
for k=1:4
  M{k} = Q*M{k}*Z;
end
%M{:}

X = zeros(4, 8);
X(1, :) = diag(M{1}).';
X(2, :) = diag(M{2}).';
X(3, :) = diag(M{3}).';
X(4, :) = diag(M{4}).';

% normalize
X = X ./ repmat(max(abs(X), [], 1), [4 1]);

return;


function [delta,err] = diffTracker( I0, I1, sig, ss )
% Fast, robust estimation of translation/scale change between two images.
%
% Approximates the translational offset between two images by assuming the
% images lie on linear manifold. Specifically, assumes that if I0 and I1
% are a pair of images related by a translation [dx dy], then (I0+I1)/2 is
% the image exactly halfway between I0 and I1 (ie I0 translated by [dx/2
% dy/2]). The above only holds for small translations and spatially smooth
% images. As such the input images typically need to be spatially smoothed
% first, the amount of necessary smoothing will increase as the size of
% translation increases (experiment for best results). The code is quite
% fast, the bottleneck is the spatial smoothing.
%
% The actual computation is performed as follows. First we generate an
% artificial translation of I0 by 1 pixel in x and y, and store the results
% in Tx and Ty. Also, if ss>1, we generate an artificial scaling Ts of I0
% by upsampling by a factor of ss. The linearity assumption tells us that:
%  I1 = I0 + (I0-Tx) * dx + (I0-Ty) * dy +  (I0-Ts) * ds
% Only dx, dy and possibly ds are unknown in the resulting overcomplete set
% of linear equations, least squares is then used. The error of the
% estimate can be used as a measure of the quality of the linear fit.
%
% This function was inspired by the beautiful work ok Yang et al.:
%   H. Yang, M. Pollefeys, G. Welch, J. Frahm, and A. Ilie. Differential
%   camera tracking through linearizing the local appearance manifold.
%   CVPR, 2007.
%
% USAGE
%  [delta,err] = diffTracker( I0, I1, [sig], [ss] )
%
% INPUTS
%  I0       - reference grayscale double image
%  I1       - translated version of I0
%  sig      - [0] amount of Gaussian spatial smoothing to apply
%  ss       - [0] scale step for artificial scaling (if >1)
%
% OUTPUTS
%  delta    - estimated dx/dy/ds
%  err      - squared error of estimate
%
% EXAMPLE - translation only
%  I = double(imread('cameraman.tif'))/255; dx=3; dy=5;
%  I0=I(1+dy:end,1+dx:end); figure(1); im(I0);
%  I1=I(1:end-dy,1:end-dx); figure(2); im(I1);
%  tic, [delta,err] = diffTracker( I0, I1, 10 ), toc
%
% EXAMPLE - translation and scale
%  I0 = double(imread('coins.png'))/255; dx=9; dy=-2; ds=1.10;
%  H1 = [eye(2)*ds -[dy; dx]; 0 0 1];
%  I1 = imtransform2(I0,H1);
%  tic, [ds,err] = diffTracker( I0, I1, 25, 1.05 ), toc
%  H2 = [eye(2)*ds(3) -[ds(2); ds(1)]; 0 0 1];
%  I2 = imtransform2(I0,H2);
%  figure(1); im(I0); figure(2); im(I1); figure(3); im(I2);
%
% See also
%
% Piotr's Image&Video Toolbox      Version 2.61
% Copyright 2010 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

% get inputs
if(nargin<3 || isempty(sig)), sig=0; end
if(nargin<4 || isempty(ss)), ss=0; end

% smooth images, keep only valid region
if( sig>0 ), f=filterGauss(2*ceil(sig*2.25)+1,[],sig^2);
  I0 = conv2(conv2(I0,f','valid'),f,'valid');
  I1 = conv2(conv2(I1,f','valid'),f,'valid');
end

% I0 translated by 1 pixel both in x and y, crop I0/I1 so dims match
if(ss>1), Ts=arrayToDims(imResample(I0,ss),size(I0)); end
Ty = I0(2:end,1:end-1); Tx = I0(1:end-1,2:end);
I0 = I0(1:end-1,1:end-1); I1 = I1(1:end-1,1:end-1);

% I1 = I0 + (I0-Tx)*dx + (I0-Ty)*dy + (I0-Ts)*ds, recover delta accordingly
dI1=I1(:)-I0(:); dIy=I0(:)-Ty(:); dIx=I0(:)-Tx(:);
if(ss>1), Ts=Ts(1:end-1,1:end-1); dIs=I0(:)-Ts(:); else dIs=[]; end
delta = -[dIx dIy dIs] \ dI1;

% compute squared error (if over certain threshold may wish to discard)
if(nargout>1), err=sum((-[dIx dIy dIs]*delta - dI1).^2) / length(dI1); end

% put scale delta into units independent of ss
if(ss>1), delta(3)=ss^delta(3); end

end


function [e1, e2, E1_l, E2_l] = hmgLinEndpoints(l,t1,t2)

% HMGLINENDPOINTS  HMG line endpoints.
%   HMGLINENDPOINTS(L,T) returns the 3D Euclidean endpoint of the
%   HMG line L at abscissa T.
%
%   [E1,E2] = HMGLINENDPOINTS(L,T1,T2) returns two 3D Euclidean endpoints
%   at abscissas T1 and T2.
%
%   [e1,e2,E1_l,E2_l] = HMGLINENDPINTS(...) returns the Jacobians of the
%   endopints wrt L.
%
%   See also HMGLINSEGMENT, HMGLIN2SEG, HMGLIN2IDPPNTS.

%   Copyright 2009 Teresa Vidal.

if nargout <= 2

    % support segment
    s  = hmgLin2seg(l);

    % support points
    p1 = s(1:3);
    p2 = s(4:6);

    % endpoints
    e1 = (1-t1)*p1 + t1*p2;
    e2 = (1-t2)*p1 + t2*p2;

else

    [s, S_l] = hmgLin2seg(l);       % support segment

    p1 = s(1:3);                    % support points
    p2 = s(4:6);

    P1_l = S_l(1:3,:);              % jacobians of sup points
    P2_l = S_l(4:6,:);

    e1 = (1-t1)*p1 + t1*p2;         % endpoints
    e2 = (1-t2)*p1 + t2*p2;

    E1_p1 = 1-t1;                   % jacobians of endpoints
    E1_p2 = t1;
    E2_p1 = 1-t2;
    E2_p2 = t2;

    E1_l = E1_p1*P1_l + E1_p2*P2_l; % chain rule
    E2_l = E2_p1*P1_l + E2_p2*P2_l;

end

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function lambda = rodman_eigenvalues ( n, alpha )

%*****************************************************************************80
%
%% RODMAN_EIGENVALUES returns the eigenvalues of the RODMAN matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Input, real ALPHA, the parameter.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1:n-1,1) = 1.0 - alpha;

  lambda(n,1) = 1.0 + alpha * ( n - 1 );

  return
end


% mean functions to be use by Gaussian process functions. There are two
% different kinds of mean functions: simple and composite:
%
% simple mean functions:
%
%   meanZero      - zero mean function
%   meanOne       - one mean function
%   meanConst     - constant mean function
%   meanLinear    - linear mean function
%   meanPoly      - polynomial mean function
%   meanDiscrete  - precomputed mean for discrete data
%   meanGP        - predictive mean of another GP
%   meanGPexact   - predictive mean of a regression GP
%   meanNN        - nearest neighbor mean function
%
% composite covariance functions (see explanation at the bottom):
%
%   meanScale     - scaled version of a mean function
%   meanPow       - power of a mean function
%   meanProd      - products of mean functions
%   meanSum       - sums of mean functions
%   meanMask      - mask some dimensions of the data
%   meanPref      - difference mean for preference learning
%
% Naming convention: all mean functions are named "mean/mean*.m".
%
%
% 1) With no or only a single input argument:
%
%    s = meanNAME  or  s = meanNAME(hyp)
%
% The mean function returns a string s telling how many hyperparameters hyp it
% expects, using the convention that "D" is the dimension of the input space.
% For example, calling "meanLinear" returns the string 'D'.
%
% 2) With two input arguments:
%
%    m = meanNAME(hyp, x)
%
% The function computes and returns the mean vector where hyp are the
% hyperparameters and x is an n by D matrix of cases, where D is the dimension
% of the input space. The returned mean vector is of size n by 1.
%
% 3) With three input arguments:
%
%    dm = meanNAME(hyp, x, i)
%
% The function computes and returns the n by 1 vector of partial derivatives
% of the mean vector w.r.t. hyp(i) i.e. hyperparameter number i.
%
% See also doc/usageMean.m.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2014-12-08.
%                                      File automatically generated using noweb.


function lambda = fourier_eigenvalues ( n )

%*****************************************************************************80
%
%% FOURIER_EIGENVALUES returns the eigenvalues of the FOURIER matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, complex LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  lambda(1,1) = 1.0;

  lambda(2:4:n,1) = - 1.0;
  lambda(3:4:n,1) =   i;
  lambda(4:4:n,1) =   1.0;
  lambda(5:4:n,1) = - i;

  return
end


function cvx_optpnt = complex_lorentz( sx, dim )

%COMPLEX_LORENTZ   Complex second-order cone.
%   COMPLEX_LORENTZ(N), where N is a positive integer, creates a column
%   variable of length N and a scalar variable, and constrains them
%   to lie in a second-order cone. That is, given the declaration
%       variable x(n) complex
%       variable y
%   the constraint
%       {x,y} == complex_lorentz(n)
%   is equivalent to
%       norm(x,2) <= y
%   The inequality form is more natural, and preferred in most cases. But
%   in fact, the COMPLEX_LORENTZ set form is used by CVX itself to convert
%   complex NORM()-based constraints to solvable form.
%
%   COMPLEX_LORENTZ(SX,DIM), where SX is a valid size vector and DIM is a
%   positive integer, creates an array variable of size SX and an array
%   variable of size SY (see below) and applies the second-order cone
%   constraint along dimension DIM. That is, given the declarations
%       sy = sx; sy(min(dim,length(sx)+1))=1;
%       variable x(sx) complex
%       variable y(sy)
%   the constraint
%       {x,y} == complex_lorentz(sx,dim)
%   is equivalent to
%       norms(x,2,dim) <= y
%   Again, the inequality form is preferred, but CVX uses the set form
%   internally. DIM is optional; if it is omitted, the first non-singleton
%   dimension is used.
%
%   LORENTZ(SX,DIM,CPLX) creates real second-order cones if CPLX is FALSE,
%   and complex second-order cones if CPLX is TRUE. The latter case is
%   equivalent to COMPLEX_LORENTZ(SX,DIM).
%
%   Disciplined convex programming information:
%       LORENTZ is a cvx set specification. See the user guide for
%       details on how to use sets.

error( nargchk( 1, 2, nargin ) );
if nargin == 1,
    cvx_optpnt = lorentz( sx, [], true );
else
    cvx_optpnt = lorentz( sx, dim, true );
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


% The following vectorized Matlab code implements Algorithm 1 from the
% paper
% Projection onto the probability simplex: An efficient algorithm with a
% simple proof, and an application, by W. Wang and M.A. Carreira-Perpinan,
% see https://arxiv.org/abs/1309.1541
%
% It projects each column vector in the D N matrix Y onto the probability
% simplex in D dimensions.

function X = SimplexColProj(Y)

Y = Y';
[N,D] = size(Y);
X = sort(Y,2,'descend');
Xtmp = (cumsum(X,2)-1)*diag(sparse(1./(1:D)));
X = max(bsxfun(@minus,Y,Xtmp(sub2ind([N,D],(1:N)',sum(X>Xtmp,2)))),0);
X = X';


function [est_map,cost,iter] = regionpixel(data,arr,col,map,te,c,d,A1,A2,A3,niter,cal)
% data = source image
% arr, col = pixel position
% map = initial field map (=0) at (arr,col)th pixel
% te = time
% c,d = cos(F_fw*te), sin(F_fw*te)
% A1 = according to Appendix A
% A2 = inv(A1'*A1)*A1'
% A3 = [1 exp(j*F_fw*te)]
% max = maximum for delta_field map
% niter = iteration number

%% initialize

iter = 0;
nsets = size(data,3); % number of te  = 3
delta_map = 11;
if cal == 1
    % cost function at (arr,col)th pixel
    cost = zeros(2,niter*2); % cost(1,:) = field map, cost(2,:) = cost value
end
% source images at (arr,col)th pixel
orig_s = reshape(data(arr,col,:),nsets,1);

%% Iteration code

% while (abs(delta_map)>1 && iter<niter && abs(delta_map)<230)
while (abs(delta_map)>1 && iter<niter)

    iter = iter+1; % number of iteration

    %% Apendix A

    s =  orig_s.*(exp(-j*2*pi*map*te).'); % shat (m is freq)
    shat = [real(s);imag(s)];
    % shat = [s1_R s2_R s3_R s1_I s2_I s3_I]
    % A2 = inv(A1'*A1)*A1'
    rowhat = A2*shat; % rowhat vector = [water_R water_I fat_R fat_I]

   if cal == 1
        %% cost function of Appendix A
        water = rowhat(1) + j*rowhat(2);
        fat = rowhat(3) + j*rowhat(4);
        cost(1,2*iter-1) = map;
        cost(2,2*iter-1) = norm(orig_s - A3*[water;fat].*(exp(j*2*pi*map*te).'));
        % cost(1,:) = field map values
        % cost(2,:) = cost function valuses
   end
    %% Apendix B

    % c_1n = 1, d_1n = 0 for all t_n, c_2n = c, d_2n = d
    f1 = -rowhat(1) -rowhat(3)*c +rowhat(4)*d;
    f2 = -rowhat(2) -rowhat(3)*d -rowhat(4)*c;

    shathat = shat + [f1;f2]; % according to  appendix B

    g = 2*pi*[te';te'].*[f2;-f1];

    B = [g A1];
    y = inv(B'*B)*B'*shathat; % y(1)is delta_fieldmap

    % update the fieldmap = prefieldmap + delta_field map
    delta_map = y(1);
    if abs(delta_map) > 30
        map = map + delta_map/abs(delta_map)*30;
    else
        map = map + delta_map;
    end

    if cal == 1
        % cost function of Appendix B
        cost(1,2*(iter)) = map;
        cost(2,2*(iter)) = norm(orig_s - A3*[water;fat].*(exp(j*2*pi*map*te).'));
    end
end
if cal == 1
    est_map = cost(1,iter*2-1);
else
    est_map = map;
    cost = 0;
end


function [x,VAR]=least_square(v,H,P)

%normal vector
N = (H'*P*H);

%cumpute unknown parameter
x = (N^-1)*H'*P*v;

%convariance of parameter
VAR = N^-1;

return


%  Figure 6.65      Feedback Control of Dynamic Systems, 5e
%                   Franklin, Powell, Emami
%

clear all
close all;

k=10;
num=k;
den=[1 1 0];
w=logspace(-3,1,100);
[m,p]=bode(num,den,w);
num=conv(num,[10 1]);
den=conv(den,[100 1]);
[mc,pc]=bode(num,den,w);
subplot(2,1,1)
w2=[.001 10];
m2=[1 1];
loglog(w,m,w,mc,w2,m2,'r');
grid;
%xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.65 Frequency response of lag-compensation design: magnitude.');
subplot(2,1,2)
semilogx(w,p,w,pc);
grid;
xlabel('\omega (rad/sec)');
ylabel('Phase (deg)');
title('Fig. 6.65 Frequency response of lag compensation design: phase.');
%return;
%numcl=[1 0.1];
%dencl=[1 1.01 1.01 0.1];
%t=0:.01:20;
%y=step(numcl,dencl,t);
%plot(t,y);
%grid;
%xlabel('sec');
%ylabel('y');


function [P_st, s] = flicker_sim(u, fs, f_line)
% flicker_sim - Flickermeter Simulator according IEC 61000-4-15
%
% [P_st, s, cpf] = flicker_sim(u, fs, f_line)
%
% This function implements a flickermeter according [1] and [2].
% Requires MATLAB with Signal Procesing Toolbox installed or Octave.
% For more information refer to [3].
%
% Inputs:
%   u:      vector of voltage samples
%   fs:     sampling frequency of u in Hz (should be >= 2000)
%   f_line: line frequency in Hz (must be 50 or 60 Hz)
%
% Outputs:
%   P_st:   short term flicker
%   s:      instantaneous flicker severity
%===============================================================================
% References:
% [1] IEC 61000-4-15, Electromagnetic compatibility (EMC), Testing and
%     measurement techniques, Flickermeter, Edition 1.1, 2003-02
% [2] Wilhelm Mombauer: "Messung von Spannungsschwankungen und Flickern mit
%     dem IEC-Flickermeter", ISBN 3-8007-2525-8, VDE-Verlag
% [3] http://www.solcept.ch/en/embedded-tools/flickersim.html
%===============================================================================

%% Configuration

SHOW_TIME_SIGNALS           = 0; % enable to plot internal signals of flickermeter
SHOW_CUMULATIVE_PROBABILITY = 0; % enable to plot the statistical evaluation of the instantaneous flicker severity
SHOW_FILTER_RESPONSES       = 0; % enable to plot the filter responses of all internal filter stages (for model verification)

IS_OCTAVE = exist('OCTAVE_VERSION') ~= 0;

%% Check inputs

if (nargin ~= 3)
  error('Invalid number of arguments');
end

if (~isvector(u))
  error('First input argument must be a vector');
end
% convert to row vector if needed
u = reshape(u, 1, length(u));

if ((f_line ~= 50) && (f_line ~= 60))
  error('Line frequency must be 50 or 60 Hz');
end

if (fs < 2000)
  warning('Sampling frequency should be >= 2000 Hz');
end

%% Block 1: Input voltage adaptor

% remove DC component
u = u - mean(u);

% normalize input (scale with peak-amplitude value)
u_rms = sqrt(mean(u.^2));
u = u / (u_rms * sqrt(2));

%% Block 2: Quadratic demodulator

u_0 = u .^ 2;

%% Block 3: Bandpass and weighting filter

% bandpass filter

HIGHPASS_ORDER  = 1;
HIGHPASS_CUTOFF = 0.05;

LOWPASS_ORDER = 6;
if (f_line == 50)
  LOWPASS_CUTOFF = 35;
end
if (f_line == 60)
  LOWPASS_CUTOFF = 42;
end

% subtract DC component to limit filter transients at start of simulation
u_0_ac = u_0 - mean(u_0);

[b_hp, a_hp] = butter(HIGHPASS_ORDER, HIGHPASS_CUTOFF / (fs / 2), 'high');
u_hp = filter(b_hp, a_hp, u_0_ac);

% smooth start of signal to avoid filter transient at start of simulation
smooth_limit = min(round(fs / 10), length(u_hp));
u_hp(1 : smooth_limit) = u_hp(1 : smooth_limit) .* linspace(0, 1, smooth_limit);

[b_bw, a_bw] = butter(LOWPASS_ORDER, LOWPASS_CUTOFF / (fs / 2), 'low');
u_bw = filter(b_bw, a_bw, u_hp);

% weighting filter

if (f_line == 50)
  K = 1.74802;
  LAMBDA = 2 * pi * 4.05981;
  OMEGA1 = 2 * pi * 9.15494;
  OMEGA2 = 2 * pi * 2.27979;
  OMEGA3 = 2 * pi * 1.22535;
  OMEGA4 = 2 * pi * 21.9;
end
if (f_line == 60)
  K = 1.6357;
  LAMBDA = 2 * pi * 4.167375;
  OMEGA1 = 2 * pi * 9.077169;
  OMEGA2 = 2 * pi * 2.939902;
  OMEGA3 = 2 * pi * 1.394468;
  OMEGA4 = 2 * pi * 17.31512;
end

num1 = [K * OMEGA1, 0];
den1 = [1, 2 * LAMBDA, OMEGA1.^2];
num2 = [1 / OMEGA2, 1];
den2 = [1 / (OMEGA3 * OMEGA4), 1 / OMEGA3 + 1 / OMEGA4, 1];
if (IS_OCTAVE)
  [b_w, a_w] = bilinear(conv(num1, num2), conv(den1, den2), 1 / fs);
else
  [b_w, a_w] = bilinear(conv(num1, num2), conv(den1, den2), fs);
end

u_w = filter(b_w, a_w, u_bw);

%% Block 4: Squaring and smoothing

LOWPASS_2_ORDER  = 1;
LOWPASS_2_CUTOFF = 1 / (2 * pi * 300e-3);  % time constant 300 msec
SCALING_FACTOR   = 1238400;  % scaling of output to perceptibility scale  (according [2])

u_q = u_w .^ 2;

[b_lp, a_lp] = butter(LOWPASS_2_ORDER, LOWPASS_2_CUTOFF / (fs / 2), 'low');
s = SCALING_FACTOR * filter(b_lp, a_lp, u_q);

%% Block 5: Statistical evaluation

NUMOF_CLASSES = 10000;

[bin_cnt, cpf.magnitude] = hist(s, NUMOF_CLASSES);
cpf.cum_probability = 100 * (1 - cumsum(bin_cnt) / sum(bin_cnt));

p_50s = mean([get_percentile(cpf, 30), get_percentile(cpf, 50), get_percentile(cpf, 80)]);
p_10s = mean([get_percentile(cpf, 6), get_percentile(cpf, 8), ...
  get_percentile(cpf, 10), get_percentile(cpf, 13), get_percentile(cpf, 17)]);
p_3s = mean([get_percentile(cpf, 2.2), get_percentile(cpf, 3), get_percentile(cpf, 4)]);
p_1s = mean([get_percentile(cpf, 0.7), get_percentile(cpf, 1), get_percentile(cpf, 1.5)]);
p_0_1 = get_percentile(cpf, 0.1);

P_st = sqrt(0.0314 * p_0_1 + 0.0525 * p_1s + 0.0657 * p_3s + ...
  0.28 * p_10s + 0.08 * p_50s);

%% Optional graphical output

% time signals
if (SHOW_TIME_SIGNALS)
  t = 0 : 1 / fs : (length(u) - 1) / fs;
  filter_len = round(10 / 1000 * fs);
  u_0_m = filter(ones(1, filter_len) / filter_len * 2, 1, u_0);

  figure
  clf
  subplot(2, 2, 1)
  hold on
  plot(t, u, 'b');
  plot(t, u_0, 'm');
  plot(t, u_hp, 'r');
  plot(t, u_0_m, 'c');
  hold off
  legend('u', 'u_0', 'u_h_p');
  grid on
  subplot(2, 2, 2)
  hold on
  plot(t, u_bw, 'b');
  plot(t, u_w, 'm');
  legend('u_b_w', 'u_w');
  hold off
  grid on
  subplot(2, 2, 3)
  plot(t, u_q, 'b');
  legend('u_q');
  grid on
  subplot(2, 2, 4)
  plot(t, s, 'b');
  legend('s');
  grid on
end

% cumulative probability function
if (SHOW_CUMULATIVE_PROBABILITY)
  figure
  clf
  plot(cpf.magnitude, cpf.cum_probability);
  grid
end

% frequency responses of filters
if (SHOW_FILTER_RESPONSES)
  [h_hp, f] = freqz(b_hp, a_hp, 4096, fs);
  [h_bw, f] = freqz(b_bw, a_bw, 4096, fs);
  [h_w, f] = freqz(b_w, a_w, 4096, fs);
  [h_lp, f] = freqz(b_lp, a_lp, 4096, fs);

  figure
  clf
  subplot(2, 1, 1)
  hold on
  plot(f, abs(h_hp), 'b')
  plot(f, abs(h_bw), 'r')
  plot(f, abs(h_w), 'g')
  plot(f, abs(h_lp), 'm')
  hold off
  grid
  axis([0, 35, 0, 1]);

  subplot(2, 1, 2)
  hold on
  plot(f, 180 / pi * unwrap(angle(h_hp)), 'b')
  plot(f, 180 / pi * unwrap(angle(h_bw)), 'r')
  plot(f, 180 / pi * unwrap(angle(h_w)), 'g')
  plot(f, 180 / pi * unwrap(angle(h_lp)), 'm')
  hold off
  grid
  axis([0, 35, -200, 300]);
end

end  % end of function flicker_sim

%% Subfunction: get_percentile
function val = get_percentile(cpf, limit)
  [dummy, idx] = min(abs(cpf.cum_probability - limit));
  val = cpf.magnitude(idx);
end


 function pot = potential_func(type, delta, param)
%function pot = potential_func(type, delta, param)
%|
%| Define roughness penalty potential functions (using function handles).
%|
%| The penalty will have the form
%|	R(x) = sum_k w_k * potential_k([Cx]_k, delta_k)
%| where w_k is provided elsewhere, not here!
%|
%| in
%|	type		quad, huber, hyper2, hyper3, cauchy, lange1, lange3, ...
%|			recommended: 'hyper3'
%|	delta		scalar or image-sized array;
%|			"cutoff" parameter for edge-preserving regularization
%|	param		optional additional parameter(s) for some choices
%| out
%|	pot.delta	local copy
%|	anonymous functions:
%|	pot.potk(pot, C*x)	potential function value
%|	pot.wpot(pot, C*x)	potential 'weights' (aka half-quad. curvatures)
%|	pot.dpot(pot, C*x)	potential derivative
%|
% Copyright 2004-5-18, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if streq(type, 'test') potential_func_test, return, end

pot.delta = [];
pot.param = [];
if isvar('delta')
	pot.delta = delta(:);
	clear delta
end
if isvar('param')
	pot.param = param(:);
	clear param
end

% trick: huber2 is just a huber with delta / 2
% so that weighting function drops to 1/2 at delta, like hyper3 etc.
if streq(type, 'huber2')
	type = 'huber';
	pot.delta = pot.delta / 2;
end

% trick: hyper3 is just a hyperbola with delta scaled by sqrt(3)
% to approximately "match" the properties of 'cauchy' (old erroneous 'hyper')
if streq(type, 'hyper3')
	type = 'hyper2';
	pot.delta = pot.delta / sqrt(3);
end

switch type

%
% quadratic potential function
%
case 'quad'
	potk = '(abs(t).^2)/2';
	wpot = 'ones(size(t))';
	dpot = 't';

%
% huber potential function
%
case 'huber'
	potk = 'huber_pot(t, pot.delta)';
	wpot = 'huber_wpot(t, pot.delta)';
	dpot = 'huber_dpot(t, pot.delta)';

%
% cauchy penalty: d^2 / 2 * log(1 + (t/d)^2)
% Not convex!
%
case 'cauchy'
	potk = 'pot.delta.^2 / 2 .* log(1 + abs(t ./ pot.delta).^2)';
	wpot = '1 ./ (1 + abs(t ./ pot.delta).^2)';
	dpot = 't ./ (1 + abs(t ./ pot.delta).^2)';

%
% Geman&McClure penalty: d^2 / 2 * (t/d)^2 / (1 + (t/d)^2)
% Not convex!
%
case 'geman&mcclure'
	potk = 'pot.delta.^2 / 2 .* (t/pot.delta).^2 ./ (1 + abs(t ./ pot.delta).^2)';
	wpot = '1 ./ (1 + abs(t ./ pot.delta).^2).^2';
	dpot = 't ./ (1 + abs(t ./ pot.delta).^2).^2';

%
% hyperbola penalty: d^2 * [ sqrt(1 + (t/d)^2) - 1 ]
%
case 'hyper2'
	potk = 'pot.delta.^2 .* (sqrt(1 + abs(t ./ pot.delta).^2) - 1)';
	wpot = '1 ./ sqrt(1 + abs(t ./ pot.delta).^2)';
	dpot = 't ./ sqrt(1 + abs(t ./ pot.delta).^2)';

case 'hyper'
	error 'use "cauchy" or "hyper3" not "hyper" now'

%
% Lange1 penalty
%
case 'lange1'
	potk = 't.^2 / 2 ./ (1+abs(t./pot.delta))';
	wpot = '(1 + abs(t ./ pot.delta) / 2) ./ (1 + abs(t ./ pot.delta)).^2';
	dpot = ['t .* (' wpot ')'];

%
% Lange3 penalty
%
case 'lange3'
	potk = 'pot.delta.^2 .* (abs(t./pot.delta) - log(1+abs(t./pot.delta)))';
	wpot = '1 ./ (1 + abs(t ./ pot.delta))';
	dpot = 't ./ (1 + abs(t ./ pot.delta))';

%
% li98cfs
%
case 'li98cfs'
	% f = @(x) atan(x) / x - 0.5; fsolve(f, 2.3)
	pot.delta = pot.delta / 2.3311;
	potk = 'pot.delta.^2 .* ((t ./ pot.delta) .* atan(t ./ pot.delta) - 0.5 * log(1 + (t ./ pot.delta).^2))';
	wpot = 'ir_li98cfs_wpot(t, pot.delta)';
	dpot = ['t .* ' wpot];

%
% qgg2: q-generalized gaussian for p=2, due to Thibault, Sauer, Bouman
% q = "param", same as lange1 when q=1
%
case 'qgg2'
	potk = 't.^2 / 2 ./ (1+abs(t./pot.delta).^(2-pot.param))';
	wpot = ['(1 + abs(t ./ pot.delta).^(2-pot.param) * pot.param / 2) ' ...
		' ./ (1 + abs(t ./ pot.delta).^(2-pot.param)).^2'];
	dpot = ['t .* (' wpot ')'];

%
% genhub : "generalized" Huber, same as huber when p=2 and q=1
% p = param(1), q = param(2)
%
case 'genhub'
	potk = ['0.5 * abs(t) .^ p .* (abs(t) <= d) + ' ...
		'0.5 * (p ./ q .* d .^ (p-q) .* abs(t) .^ q + (1 - p ./ q) .* d .^ p) .* (abs(t) > d)'];
	reps = {'p', 'zp', 'q', 'zq', 'd', 'zd', ...
		'zp', 'pot.param(1)', 'zq', 'pot.param(2)', 'zd', 'pot.delta'};
	potk = strreps(potk, reps{:});
	wpot = ['p / 2 .* (abs(t) .^ (p-2)) .* (abs(t) <= d) + ' ...
		'p / 2 .* (d .^ (p-q)) .* ((abs(t) + (abs(t)==0)) .^ (q-2)) .* (abs(t) > d)']; % trick to avoid 0^-:
	wpot = strreps(wpot, reps{:});
	dpot = ['t .* (' wpot ')'];

%
% stevenson:94:dpr
% p = param(1), q = param(2), same as huber when p=2 and q=1 ???
%
case 'stevenson94dpr'
	potk = ['0.5 * abs(t) .^ p .* (abs(t) <= d) + ' ...
		'0.5 * ( (p .* d .^ (p-1) .* abs(t) - p .* d .^ p + (1 ./ q) .^ (1 ./ (q-1)) ) .^ q + d .^ p - (1 ./ q) .^ (q ./ (q-1)) ) .* (abs(t) > d)'];
	reps = {'p', 'zp', 'q', 'zq', 'd', 'zd', ...
		'zp', 'pot.param(1)', 'zq', 'pot.param(2)', 'zd', 'pot.delta'};
	potk = strreps(potk, reps{:});
	wpot = 'ones(size(t))'; % fix: fake for now
	dpot = ['t .* (' wpot ')'];

otherwise
	try % trick: try new strum version
		p = potential_fun(type, pot.delta, pot.param);
		pot.potk = @(dum, t) p.potk(t);
		pot.wpot = @(dum, t) p.wpot(t);
		pot.dpot = @(dum, t) p.dpot(t);
	catch
		fail('Unknown potential "%s"', type)
	end
end

if ~isfield(pot, 'potk')
	pot.potk = ir_str2func(['@(pot,t) ' potk]);
	pot.wpot = ir_str2func(['@(pot,t) ' wpot]);
	pot.dpot = ir_str2func(['@(pot,t) ' dpot]);
end

% test routine
% examine potential functions after rescaling.
function potential_func_test

delta = 10; tmax = 4;
plist = {'quad', 'huber2', 'hyper3', 'lange1', 'lange3', ...
	'cauchy', 'qgg2', 'gf1'};
%plist = {'quad', 'li98cfs', 'hyper3', 'huber2'}; % show li98cfs roughly hyper3
%plist = {'huber', 'genhub', 'quad'};%, 'stevenson94dpr'};
%plist = {'hyper3', 'qgg2'}; delta = 20; tmax = 10;
%plist = {'huber', 'hyper3', 'qgg2'}; delta = 10; tmax = 8;
%plist = {'qgg2', 'gf1'};
t = tmax * linspace(-delta, delta, 401)';
for ii=1:length(plist)
	type = plist{ii};
	leg{ii} = [plist{ii} ' \delta = ' num2str(delta)];
	if streq(type, 'qgg2')
		param = 1.2;
		leg{ii} = [leg{ii} ' q = ' num2str(param)];
	elseif streq(type, 'gf1')
		param = [0.0558 1.6395];
	elseif streq(type, 'genhub')
		param = [1.9 1.1];
		leg{ii} = [leg{ii} sprintf('p=%g q=%g', param(1), param(2))];
	elseif streq(type, 'stevenson94dpr')
		param = [2 2.01];
		leg{ii} = [leg{ii} sprintf('p=%g q=%g', param(1), param(2))];
	else
		param = [];
	end
	pot = potential_func(type, delta, param);
	pp(:,ii) = pot.potk(pot, t);
	pw(:,ii) = pot.wpot(pot, t);
	pd(:,ii) = pot.dpot(pot, t);
end

if im
	clf
	subplot(411), plot(t, pp), title 'potk'
	axis tight
	axisy([-0.0 2.5] * delta^2)
	legend(leg, 'location', 'north')
	subplot(412), plot(t, pw), title 'wpot'
	axisy(0, 1.1)
	subplot(413), plot(t, pd), title 'dpot'
	axisy([-1 1] * 1.1 * delta)
	% check derivatives
	subplot(414)
	plot(t, pd)
	title 'dpot check'
	hold on
	d = diffc(pp) / (t(2)-t(1));
	plot(t(1:end-1), d(1:end-1,:), '--')
	hold off
	axisy([-1 1] * 1.1 * delta)
end


function pde = elli3DlinIntf2(bm,bp,delta)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 12/15/2020 by GRC

%% PDE Structure
pde = struct('intf',@intf,'f',@f,'fm',@fm,'fp',@fp,'exactu',@exactu,...
    'um',@um,'up',@up,'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,'gD',@gD);

pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = (x-delta);
    end

%% exact solution
    function u = exactu(x,y,z)
        u = um(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up(x(id),y(id),z(id));
    end
    function u = um(x,y,z)
        r = (1+delta)*sin((x-delta)*pi/(1+delta)).*sin(pi*y).*sin(pi*z);
        u = r/bm;
    end
    function u = up(x,y,z)
        r = (1-delta)*sin((x-delta)*pi/(1-delta)).*sin(pi*y).*sin(pi*z);
        u = r/bp;
    end
%% Boundary Function
    function u = gD(x,y,z)
        u = exactu(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        r = cos((x-delta)*pi/(1+delta)).*sin(pi*y).*sin(pi*z)*pi;
        u = r/bm;
    end
    function u = Dxup(x,y,z)
        r = cos((x-delta)*pi/(1-delta)).*sin(pi*y).*sin(pi*z)*pi;
        u = r/bp;
    end

    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dyum(x,y,z)
        r = (1+delta)*sin((x-delta)*pi/(1+delta)).*cos(pi*y).*sin(pi*z)*pi;
        u = r/bm;
    end
    function u = Dyup(x,y,z)
        r = (1-delta)*sin((x-delta)*pi/(1-delta)).*cos(pi*y).*sin(pi*z)*pi;
        u = r/bp;
    end

    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dzum(x,y,z)
        r = (1+delta)*sin((x-delta)*pi/(1+delta)).*sin(pi*y).*cos(pi*z)*pi;
        u = r/bm;
    end
    function u = Dzup(x,y,z)
        r = (1-delta)*sin((x-delta)*pi/(1-delta)).*sin(pi*y).*cos(pi*z)*pi;
        u = r/bp;
    end

%% right hand side function
    function u = f(x,y,z)
        u = fm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp(x(id),y(id),z(id));
    end
    function u = fm(x,y,z)
        r = sin((x-delta)*pi/(1+delta)).*sin(pi*y).*sin(pi*z)*pi^2;
        u = r*(1/(1+delta)+2*(1+delta));
    end
    function u = fp(x,y,z)
        r = sin((x-delta)*pi/(1-delta)).*sin(pi*y).*sin(pi*z)*pi^2;
        u = r*(1/(1-delta)+2*(1-delta));
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


% [thresh,lower,cost] = onedim2class(x,l,costmatrix,weights,lower)
% inputs:
% x is the N x 1 matrix of N unidimensional data points
% l is the N x 1 matrix of N corresponding labels, where l(i) can
% be either -1 or 1
% thresh is the best threshold such that almost everything on one
% side of the threshold is of one class and almost of everything on
% the other side is the other class.
% optional:
% costmatrix is the 2 x 2 matrix. the cost of misclassifying a point with
% label costmatrix(1,1) is costmatrix(1,2) and the cost of misclassifying a
% point with label costmatrix(2,1) is costmatrix(2,2). by default, this is
% [-1 1 ; 1 1]. set to [] to use the default.
% weights is a vector of length n, where w(n) is the weight of the nth
% sample. set to [] to use default w(n) = 1 for all n.
% lower is a scalar. if lower == 1, then class 1 must be below the
% threshold and class -1 must be above. if lower == -1, then class
% -1 must be below the threshold and class 1 must be above. if
% lower == 0, then the best ordering is chosen
function [thresh,lower,cost] = onedim2class(x,l,costmatrix,weights,lower)

% parse inputs
n = length(x);
if nargin < 3 || isempty(costmatrix),
  costmatrix = [-1 1 ; 1 1];
else
  if numel(costmatrix) ~= 4 || rows(costmatrix) ~= 2,
    error('costmatrix must be 2 x 2');
  end;
  if abs(costmatrix(1,1)) ~= 1 || abs(costmatrix(2,1)) ~= 1,
    error('costmatrix(1,1) and constmatrix(2,1) can either be 1 or -1');
  end;
  if costmatrix(1,1) == costmatrix(2,1),
    error('costmatrix(1,1) == costmatrix(2,1)');
  end;
end;
if nargin < 4 || isempty(weights),
  weights = ones(n,1);
end

if nargin < 5,
  lower = 0;
else
  if abs(lower) ~= 1 && lower ~= 0,
    error('lower can be -1, 0, or 1');
  end;
end;
x = x(:);
l = l(:);
if length(x) ~= length(l),
  error('x and l must both be N x 1 vectors');
end;
classes = unique(l);
if length(classes) ~= 2,
  error('l must contain exactly 2 unique values, -1 and 1');
end;

% sort the data
% Sort the data based on response value
[sortedx,sortedi] = sort(x);
sortedl = l(sortedi);
sortedweights = weights(sortedi);

% collapse entries of x that are identical
isid = [false;diff(sortedx) == 0];
if any(isid),
  [startid,endid] = get_interval_ends(isid);
  for i = 1:length(startid),
    t0 = startid(i)-1;
    t1 = endid(i)-1;
    tmp = sum(sortedweights(t0:t1).*sortedl(t0:t1));
    sortedweights(t0) = abs(tmp);
    sortedl(t0) = sign(tmp);
  end
  sortedx(isid) = [];
  sortedl(isid) = [];
  sortedweights(isid) = [];
end

if lower == 0,
  [thresh_lowerpos,cost_lowerpos] = main(sortedx, sortedl, costmatrix, sortedweights, 1);
  [thresh_lowerneg,cost_lowerneg] = main(sortedx, sortedl, costmatrix, sortedweights, -1);
  if cost_lowerpos < cost_lowerneg,
    thresh = thresh_lowerpos;
    cost = cost_lowerpos;
    lower = 1;
  else
    thresh = thresh_lowerneg;
    cost = cost_lowerneg;
    lower = -1;
  end;
else
  [thresh,cost] = main(sortedx, sortedl, costmatrix, sortedweights,lower);
end;

function [thresh,cost] = main(x, l, costmatrix, weights, lower)

N = length(x);
labels = l;

l(labels==costmatrix(1,1)) = costmatrix(1,1)*costmatrix(1,2).*weights(labels==costmatrix(1,1));
l(labels==costmatrix(2,1)) = costmatrix(2,1)*costmatrix(2,2).*weights(labels==costmatrix(2,1));
suml1 = cumsum([0;l]);
suml2 = [flipud(cumsum(flipud(l)));0];
if lower == -1,
  ncorrect = -suml1 + suml2;
else
  ncorrect = suml1 - suml2;
end
[maxncorrect,besti] = max(ncorrect);

% Choose threshold to be in the middle
if besti == 1,
  thresh = x(1)-eps;
elseif besti == N+1,
  thresh = x(N)+eps;
else
  thresh = mean(x(besti-1:besti));
end;
cost = -maxncorrect;


function [logp, yhat, res] = tapas_unitsq_sgm_mu3(r, infStates, ptrans)
% Calculates the log-probability of response y=1 under the unit-square sigmoid model
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Initialize returned log-probabilities as NaNs so that NaN is
% returned for all irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states and responses
mu1hat = infStates(:,1,1);
mu1hat(r.irr) = [];
mu3hat = infStates(:,3,1);
mu3hat(r.irr) = [];
y = r.y(:,1);
y(r.irr) = [];

% Decision temperature is exponential of log-volatility
% (i.e., inverse decision temperature is exponential of negative log-volatility)
ze = exp(-mu3hat);

% Avoid any numerical problems when taking logarithms close to 1
x = mu1hat;
logx = log(x);
log1pxm1 = log1p(x-1);
logx(1-x<1e-4) = log1pxm1(1-x<1e-4);
log1mx = log(1-x);
log1pmx = log1p(-x);
log1mx(x<1e-4) = log1pmx(x<1e-4);

% Calculate log-probabilities for non-irregular trials
reg = ~ismember(1:n,r.irr);
logp(reg) = y.*ze.*(logx -log1mx) +ze.*log1mx -log((1-x).^ze +x.^ze);
yhat(reg) = x;
res(reg) = (y-x)./sqrt(x.*(1-x));

return;


% VL_HOMKERMAP Homogeneous kernel map
%   V = VL_HOMKERMAP(X, N) computes a 2*N+1 dimensional approximated
%   kernel map for the Chi2 kernel. X is an array of data points. Each
%   point is expanded into a vector of dimension 2*N+1 and saved to
%   the output V. The expanded feature vectors are stacked along the
%   first dimension, so that the output array V has the same
%   dimensions of the input array X except for the first one, which is
%   2*N+1 times larger.
%
%   The function accepts the following options:
%
%   KChi2::
%     Compute the map for the Chi2 kernel.
%
%   KINTERS::
%     Compute the map for the intersection kernel.
%
%   KL1::
%     Same as KINTERS, but deprecated as the name is not fully
%     accurate.
%
%   KJS::
%     Compute the map for the JS (Jensen-Shannon) kernel.
%
%   Period:: [automatically tuned]
%     Set the period of the kernel specturm. The approximation is
%     based on periodicizing the kernel specturm. If not specified,
%     the period is automatically set based on the heuristic described
%     in [2].
%
%   Window:: [RECTANGULAR]
%     Set the window used to truncate the spectrum before The window
%     can be either RECTANGULAR or UNIFORM window. See [2] and the API
%     documentation for details.
%
%   Gamma:: [1]
%     Set the homogeneity degree of the kernel. The standard kernels
%     are 1-homogeneous, but sometimes smaller values perform better
%     in applications. See [2] for details.
%
%   Example::
%     The following code results in approximatively the same
%     similarities matrices between points X and Y:
%
%       x = rand(10,1) ;
%       y = rand(10,100) ;
%       psix = vl_homkermap(x, 3) ;
%       psiy = vl_homkermap(y, 3) ;
%       figure(1) ; clf ;
%       ker = vl_alldist(x, y, 'kchi2') ;
%       ker_ = psix' * psiy ;
%       plot([ker ; ker_]') ;
%
%   Note::
%     The homogeneous kernels K(X,Y) are normally defined for
%     non-negative data only. VL_HOMKERMAP defines them for both
%     positive and negative data by using the definition
%     SIGN(X)SIGN(Y)K(ABS(X),ABS(Y)) -- note that other extensions are
%     possible as well (see [2]).
%
%   REFERENCES::
%     [1] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     Proc. CVPR, 2010.
%
%     [2] A. Vedaldi and A. Zisserman
%     `Efficient Additive Kernels via Explicit Feature Maps',
%     PAMI, 2011 (submitted).
%
%   See also: VL_HELP().

% Authors: Andrea Vedaldi

% Copyright (C) 2007-12 Andrea Vedaldi and Brian Fulkerson.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).


function x = rationalBezierCurve(p,w,t,tSpan)
% x = rationalBezierCurve(p,w,t,tSpan)
%
% This function evaluates a rational bezier curve, defined by points p and
% weights w.
%
% INPUTS:
%   p = [nCurve x nPoint] = control points
%   w = [1 x nPoint] = control point weights
%   t = [1 x nTime] = times at which to evaluate curve
%   tSpan = [1 x 2] = tSpan(1) <= t <= tSpan(2)
%
% OUTPUTS:
%   x = [nCurve x nTime] = bezier curve, evaluated at t
%
% NOTES:
%   It is not advisable to use this function for high-order polynomials.
%
% See also: BEZIERCURVE, FITBEZIERCURVE
%

[nCurve, nPoint] = size(p);
nTime = length(t);

t = (t-tSpan(1))/diff(tSpan);

%%% Compute the numerator and denominator:
num = zeros(nCurve,nTime);
den = zeros(nCurve,nTime);
ONE = ones(nCurve,1);
n = nPoint - 1;
for i=0:n
    tt = (t.^i).*(1-t).^(n-i);
    binom = nchoosek(n,i);
    num = num + binom *w(i+1)*p(:,i+1)*tt;
    den = den + binom *w(i+1)*ONE*tt;
end
x = num./den;

end


clear all;
n=400000;
p=1000;
density=0.01;

% generate random data
format compact;
randn('seed',0);
rand('seed',0);
X=sprandn(p,n,density);
nrm=sqrt(sum(X.^2));
X=X/mean(nrm);
%mean_nrm=mean();
%X=X/mean_nrm;

% generate some true model
z=double(sign(full(sprandn(p,1,0.05))));
y=X'*z;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXPERIMENT 1: Lasso
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('EXPERIMENT FOR LASSO\n');
nrm=sqrt(sum(y.^2));
y=y+0.01*nrm*randn(n,1);    % add noise to the model
nrm=sqrt(sum(y.^2));
y=y*(sqrt(n)/nrm);

clear param;
param.regul='l1';        % many other regularization functions are available
param.loss='square';     % only square and log are available
param.numThreads=-1;    % uses all possible cores
param.normalized=false;  % if the columns of X have unit norm, set to true.
param.averaging_mode=0;  % no averaging, averaging was not really useful in experiments
param.weighting_mode=0;  % weights are in O(1/sqrt(n))  (see help mexStochasticProx)
param.optimized_solver=true;  % best is not to touch this option
param.verbose=false;

% set grid of lambda
max_lambda=max(abs(X*y))/n;
tablambda=max_lambda*(2^(1/4)).^(0:-1:-20);  % order from large to small
param.lambda=tablambda;    % best to order from large to small
tabepochs=[1 2 3 5 10];  % in this script, we compare the results obtained when varying the number of passes over the data.

%% The problem which will be solved is
%%   min_beta  1/(2n) ||y-X' beta||_2^2 + lambda ||beta||_1
fprintf('EXPERIMENT: ALL LAMBDAS IN PARALLEL\n');
% we try different experiments when varying the number of epochs.
% the problems for different lambdas are solve INDEPENDENTLY in parallel
obj=[];
objav=[];
for ii=1:length(tabepochs)
   param.iters=tabepochs(ii)*n;
   fprintf('EXP WITH %d PASS\n',tabepochs(ii));
   nlambdas=length(param.lambda);
   Beta0=zeros(p,nlambdas);
   tic
   [Beta tmp]=mexStochasticProx(y,X,Beta0,param);
   toc
   yR=repmat(y,[1 nlambdas]);
   fprintf('Objective functions: \n');
   obj=[obj; 0.5*sum((yR-X'*Beta).^2)/n+param.lambda.*sum(abs(Beta))];
   obj
   if param.averaging_mode
      objav=[objav; 0.5*sum((yR-X'*tmp).^2)/n+param.lambda.*sum(abs(tmp))];
      objav
   end
   fprintf('Sparsity: \n');
   sum(Beta ~= 0)
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXPERIMENT 2: L2 logistic regression
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('EXPERIMENT FOR LOGISTIC REGRESSION + l2\n');
y=sign(y);
param.regul='l2';
param.loss='logistic';
obj=[];
objav=[];
for ii=1:length(tabepochs)
   param.iters=tabepochs(ii)*n;   % one pass over the data
   fprintf('EXP WITH %d PASS\n',tabepochs(ii));
   nlambdas=length(param.lambda);
   Beta0=zeros(p,nlambdas);
   tic
   [Beta tmp]=mexStochasticProx(y,X,Beta0,param);
   toc
   yR=repmat(y,[1 nlambdas]);
   fprintf('Objective functions: \n');
   obj=[obj;sum(log(1.0+exp(-yR .* (X'*Beta))))/n+0.5*param.lambda.*sum(abs(Beta.^2))];
   obj
   if param.averaging_mode
      objav=[objav; sum(log(1.0+exp(-yR .* (X'*tmp))))/n+0.5*param.lambda.*sum(abs(tmp))];
      objav
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% EXPERIMENT 3: L1 logistic regression
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf('EXPERIMENT FOR LOGISTIC REGRESSION + l1\n');
y=sign(y);
param.regul='l1';        % many other regularization functions are available
param.loss='logistic';     % only square and log are available
obj=[];
objav=[];
for ii=1:length(tabepochs)
   param.iters=tabepochs(ii)*n;   % one pass over the data
   fprintf('EXP WITH %d PASS\n',tabepochs(ii));
   nlambdas=length(param.lambda);
   Beta0=zeros(p,nlambdas);
   tic
   [Beta tmp]=mexStochasticProx(y,X,Beta0,param);
   toc
   yR=repmat(y,[1 nlambdas]);
   fprintf('Objective functions: \n');
   obj=[obj; sum(log(1.0+exp(-yR .* (X'*Beta))))/n+param.lambda.*sum(abs(Beta))];
   obj
   if param.averaging_mode
      objav=[objav; sum(log(1.0+exp(-yR .* (X'*tmp))))/n+param.lambda.*sum(abs(tmp))];
      objav
   end
   fprintf('Sparsity: \n');
   sum(Beta ~= 0)
end


function beta = LpAnalysis(H, Ht, A, At, y, p, normfac, err, insweep, decfac, tol)

% Algorithm for solving problems of the form:
% min ||Ax||_p s.t. ||y-Hx||_2 < err

% Inputs
% A - Sparsifying Forward Transform
% At - Sparsifying Backward Transform
% H - Forward Measurement Operator
% Ht - Backward Measurement Operator
% y - collected data
% p - non-convex norm
% normfac - highest singular value of A (default 1)
% err - two norm mismatch (default 1e-6)
% insweep - number of sweeps for internal loop (default 50)
% decfac - lambda decrease factor for outside loop (default 0.5)
% tol - tolerance for convergence (default 1e-4)

% copyright (c) Angshul Majumdar 2010

c = 1.1;

if nargin < 6
    normfac=1;
end
if nargin < 7
    p = 1;
end
if nargin < 8
    err = 1e-6;
end
if nargin < 9
    insweep = 50;
end
if nargin < 10
    decfac = 0.5;
end
if nargin < 11
    tol = 1e-4;
end

alpha = 1.1*normfac;
x_initial = Ht(y);
z_initial = zeros(length(A(x_initial)),1);

x = x_initial; z = z_initial;

lambdaInit = decfac*max(abs(Ht(y))); lambda = lambdaInit;

f_current = norm(y-H(x_initial)) + lambda*norm(A(x),p);

while lambda > lambdaInit*tol
    % debug lambda
    for ins = 1:insweep
        f_previous = f_current;

        b = x + (1/alpha)*(Ht(y-H(x)));
        z = (c*z + A(b-At(z)))./((2*alpha/lambda)*(abs(A(x))).^(2-p)+c);
        x = b - At(z);

        f_current = norm(y-H(x)) + lambda*norm(A(x),p);

        if norm(f_current-f_previous)/norm(f_current + f_previous)<tol
            break;
        end
    end
    if norm(y-H(x))<err
        break;
    end
    lambda = decfac*lambda;
end
beta = x;


function y = ssrfunc(x)

% sun-synchronous repeating ground track
% orbit objective function

% required by ssrepeat.m

% input

%  x = array of current dependent variables

% output

%  y = function value array evaluated at x

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global j2 mu req omega xldot

global ecc xqp

y = zeros(2, 1);

% "unload" current solution

sma = x(1);
inc = x(2);

% determine current values of nonlinear system

mm = sqrt(mu / sma ^ 3);

slr = sma * (1.0 - ecc * ecc);

cinc = cos(inc);

% compute perturbations

raandot = -1.5 * j2 * mm * (req / slr) ^ 2 * cinc;

apdot = 0.75 * j2 * mm * (req / slr) ^ 2 * (5.0 * cinc ^ 2 - 1);

madot = mm + 0.75 * j2 * mm * (req / sma) ^ 2 * (3.0 * cinc ^ 2 - 1) ...
    / (1.0 - ecc * ecc) ^ 1.5;

% sun-synchronous inclination equation

y(1) = cinc + 2.0 * xldot * sma ^ 3.5 * (1 - ecc * ecc) ^ 2 ...
    / (3 * j2 * req * req * sqrt(mu));

% repeating groundtrack equation

y(2) = (1.0 / xqp) / (omega - raandot) - (1.0 / (apdot + madot));


function x = daub14_transform_inverse ( n, y )

%*****************************************************************************80
%
%% DAUB14_TRANSFORM_INVERSE inverts the DAUB14 transform of a vector.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the vector.
%    N must be a power of 2 and at least 4.
%
%    Input, real Y(N), the transformed vector.
%
%    Output, real X(N), the original vector.
%
  c = [ ...
     7.785205408500917e-02; ...
     3.965393194819173e-01; ...
     7.291320908462351e-01; ...
     4.697822874051931e-01; ...
    -1.439060039285649e-01; ...
    -2.240361849938749e-01; ...
     7.130921926683026e-02; ...
     8.061260915108307e-02; ...
    -3.802993693501441e-02; ...
    -1.657454163066688e-02; ...
     1.255099855609984e-02; ...
     4.295779729213665e-04; ...
    -1.801640704047490e-03; ...
     3.537137999745202e-04 ];
  p = 13;
  x(1:n,1) = y(1:n);
  m = 4;
  q = floor ( ( p - 1 ) / 2 );

  while ( m <= n )

    z(1:m,1) = 0.0;

    j = 1;

    mh = floor ( m / 2 );

    for i = - q + 1 : mh - q

      for k = 0 : 2 : p - 1
        i0 = i4_wrap ( i      + k / 2,     1,      mh );
        i1 = i4_wrap ( i + mh + k / 2,     mh + 1, m  );
        z(j,1)   = z(j,1)   + c(p-k)   * x(i0) + c(k+2) * x(i1);
        z(j+1,1) = z(j+1,1) + c(p-k+1) * x(i0) - c(k+1) * x(i1);
      end

      j = j + 2;

    end

    x(1:m,1) = z(1:m);

    m = m * 2;

  end

  return
end


function varargout = quiver(N, varargin)
% QUIVER   Draw phase plot diagrams, based on ODEs specified with CHEBOPS
%
% Calling sequence:
%   H = QUIVER(N, AXIS, 'OPT1', VAL1, ...)
%
% Here, the inputs are:
%   N    : A chebop, whose N.op arguments specifies a first or second order
%          scalar ODE, or a coupled system of two first order ODEs.
%   AXIS : A 4-vector with elements [XMIN XMAX YMIN YMAX] that specify the
%          rectangular region shown on the phase plot. If none is passed, the
%          default values [-1 1 -1 1] are used.
%
% It is possible to pass the method various option pairs of the form
% 'OPTIONNAME', OPTIONVALUE. The options supported are:
%   'XPTS'      : An integer, specifying the resolution of the x-axis for the
%                 quiver plot. Default value: 10.
%   'YPTS'      : An integer, specifying the resolution of the y-axis for the
%                 quiver plot. Default value: 10.
%   'NORMALIZE' : A Boolean, which determines whether the arrows on the quiver
%                 plot or normalized all to have the same length. Default value:
%                 false.
%   'SCALE':      By default, quiver automatically scales the arrows to fit
%                 within the grid. By passing a SCALE argument S, the arrows are
%                 fitted within the grid and then stretched by S. Use S = 0 to
%                 plot the arrows without the automatic scaling. See the
%                 documentation for the built-in MATLAB QUIVER for more
%                 information.
%   LINESPEC      Specifies options for the quiver plot. The LINESPEC argument
%                 can either be a string supported by the MATLAB PLOT command,
%                 such as 'ro', or a parameter/value pair to specify additional
%                 properties of the quiver lines, such as
%                   quiver(N,[0 2 0 2], 'k', 'linewidth', 1.4).
%
% The optional output is
%   H   : A quivergroup handle.
%
% Note: The CHEBOP QUIVER command works by reformulating higher order problems
% as coupled first order systems, evaluating the resulting first order system at
% grid that should be interpreted as values of u and u', then calling the
% built-ing MATLAB QUIVER method on the results. In the case of first order
% scalar problems, the grid should be interpreted as values of t and u.
%
% Example 1 -- van der Pol equation (second order ODE)
%   N = chebop(0,100);
%   N.op = @(t,u) diff(u, 2) - 3*(1-u^2)*diff(u) + u;
%   quiver(N,[-2.5 2.5 -5.5 5.5], 'r', 'xpts', 40, 'ypts', 40, 'scale', .5, ...
%       'normalize', true, 'linewidth', 1.5)
%   hold on % Plot a particular solution on top of quiver plot
%   N.lbc = [2; 0];
%   u = N\0;
%   plot(u, diff(u),'linewidth',2)
%
% Example 2 -- Lotka-Volterra (first order coupled system)
%   N = chebop(@(t,u,v) [diff(u)-2.*u+u.*v; diff(v)+v-u.*v], [0 4]);
%   quiver(N, [0 2.5 0 4])
%   hold on
%   N.lbc = @(u,v) [u - 0.5; v - 1]; % Initial populations
%   [u, v] = N\0;
%   plot(u, v, 'linewidth', 2)
%   plot(0.5, 1,'m*','markersize',15) % Mark initial condition
%
% Example 3 -- Slopefield for a first order problem
%   N = chebop(@(t,u) diff(u)-sin(t)*u);
%   quiver(N,[-1.2*pi 1.2*pi -1 1])

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

% Set default values:
scale = 1;
normalize = false;
u0 = [];
xpts = 20;
ypts = 20;
axisLims = [-1 1 -1 1];

% The limits for the axes have to appear in the first entry of varargin if
% they're specified. They'll always be a 4-vector.
if ( ~isempty(varargin) && length(varargin{1}) == 4 )
    axisLims = varargin{1};
    % Throw away the axisLims:
    varargin(1) = [];
end

% Cell for linespecs
linespec = {};

% Parse VARARGIN, go through all elements:
while ( ~isempty(varargin) )
    if ( ~ischar(varargin{1}) && ~isnumeric(varargin{2}) )
        error('followpath:inputArgument','Incorrect options input arguments');
    end

    throwAwayLength = 2;
    switch lower(varargin{1})
        case 'normalize'
            normalize = varargin{2};
        case 'xpts'
            xpts = varargin{2};
        case 'ypts'
            ypts = varargin{2};
        case 'scale'
            scale = varargin{2};
        otherwise
            % Must have gotten something to do with linespec
            findNorm = strcmpi(varargin, 'normalize');
            findXpts = strcmpi(varargin, 'xpts');
            findYpts = strcmpi(varargin, 'ypts');
            findScale = strcmpi(varargin, 'scale');
            optionPos = findNorm | findXpts | findYpts | findScale;
            nextOption = find(optionPos, 1, 'first');
            % If nextOption is empty, we only got linespec options left
            if ( isempty(nextOption) )
                nextOption = length(varargin) + 1;
            end
            linespec = [linespec, varargin{1:nextOption - 1}];
            throwAwayLength = nextOption - 1;
    end

    % Throw away option name and argument and move on:
    varargin(1:throwAwayLength) = [];
end

% Extract the x and y limits:
xl = axisLims(1:2);
yl = axisLims(3:4);

% If ylim is empty, we solve the problem to obtain a range for plotting on:
if ( isempty(xl) )
    u0 = N\0;
    xl = 1.1*minandmax(u0);
end

if ( isempty(yl) )
    if ( isempty(u0) )
        u0 = N\0;
    end
    yl = 1.1*minandmax(diff(u0));
end

% Convert the operator in N to first order.
[firstOrderFun, ~, ~, ~, diffOrd] = treeVar.toFirstOrder(N.op, 0, N.domain);

% Vectors for constructing a meshgrid:
y1 = linspace(xl(1), xl(end), xpts);
y2 = linspace(yl(1), yl(end), ypts);

% Get a meshgrid for points of interests in the phase plane.
[x,y] = meshgrid(y1, y2);
u = zeros(size(x));
v = zeros(size(x));

% Phase plane portraits really only make sense for autonomous systems, which
% shouldn't depend on t, hence, we simply take t = 0 for evaluating.
t = 0;

% Check if we got passed a system of too high order:
numEquations = length(strfind(func2str(firstOrderFun),';'));

if ( numEquations > 2 )
    error('CHEBFUN:CHEBOP:quiver:tooHighOrder', ...
        ['The ODE passed to chebop/quiver must either be a scalar, second ' ...
        'order ODE or a system of two first order equations.'])
end

% Are we plotting a slope field (cf. #2238), or a standard phase plane?
if( (length(diffOrd) == 1) && (diffOrd == 1))   % Slope field
    for i = 1:numel(x)
        res = firstOrderFun(x(i), y(i));
        u(i) = 1;
        v(i) = res(1);
    end
else                                            % Phase plane
    for i = 1:numel(x)
        res = firstOrderFun(t,[x(i); y(i)]);
        u(i) = res(1);
        v(i) = res(2);
    end
end

if ( normalize )
    % Make all arrows equal length:
    nrm = sqrt(u.^2 + v.^2);
    u = u./nrm;
    v = v./nrm;
end

h = quiver(x, y, u, v, scale,linespec{:});

% Set x and y limits:
xlim(xl);
ylim(yl);
if ( nargout > 0 )
    varargout{1} = h;
else

end


% created: Zoya Bylinskii, Aug 2014

% This finds the KL-divergence between two different saliency maps when
% viewed as distributions: it is a non-symmetric measure of the information
% lost when saliencyMap is used to estimate fixationMap.

function score = KLdiv(saliencyMap, fixationMap)
% saliencyMap is the saliency map
% fixationMap is the human fixation map

map1 = im2double(imresize(saliencyMap, size(fixationMap)));
map2 = im2double(fixationMap);

% make sure map1 and map2 sum to 1
if any(map1(:))
    map1 = map1/sum(map1(:));
end

if any(map2(:))
    map2 = map2/sum(map2(:));
end

% compute KL-divergence
score = sum(sum(map2 .* log(eps + map2./(map1+eps))));


function [bestEpsilon, bestF1] = selectThreshold(yval, pval)
    %% SELECTTHRESHOLD Find the best threshold (epsilon) to use for selecting
    %   outliers
    %   [bestEpsilon bestF1] = SELECTTHRESHOLD(yval, pval) finds the best
    %   threshold to use for selecting outliers based on the results from a
    %   validation set (pval) and the ground truth (yval).
    %

    bestEpsilon = 0;
    bestF1 = 0;
    F1 = 0;

    stepsize = (max(pval) - min(pval)) / 1000;
    for epsilon = min(pval) : stepsize : max(pval)
        % ====================== YOUR CODE HERE ======================
        % Instructions: Compute the F1 score of choosing epsilon as the
        %               threshold and place the value in F1. The code at the
        %               end of the loop will compare the F1 score for this
        %               choice of epsilon and set it to be the best epsilon if
        %               it is better than the current choice of epsilon.
        %
        % Note: You can use predictions = (pval < epsilon) to get a binary vector
        %       of 0's and 1's of the outlier predictions
        predictions = (pval < epsilon);  % entry = 1 if anomalous

        % calculate how well we did
        tp = sum(predictions == 1 & yval == 1);
        fp = sum(predictions == 1 & yval == 0);
        fn = sum(predictions == 0 & yval == 1);

        % calculate precision and recall
        precision = tp / (tp + fp);
        recall = tp / (tp + fn);
        F1 = (2 * precision * recall) / (precision + recall);

        % =============================================================

        if F1 > bestF1
           bestF1 = F1;
           bestEpsilon = epsilon;
        end

        %fprintf('tp=%d, fp=%d, fn=%d, precision=%f, recall=%f, best=%f was F1=%f\n', ...
        %    tp, fp, fn, precision, recall, bestEpsilon, bestF1);
    end
end


function linplus_test03 ( )

%*****************************************************************************80
%
%% TEST03 tests R83_CR_FA, R83_CR_SL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST03\n' );
  fprintf ( 1, '  For a real tridiagonal matrix,\n' );
  fprintf ( 1, '  using CYCLIC REDUCTION,\n' );
  fprintf ( 1, '  R83_CR_FA factors;\n' );
  fprintf ( 1, '  R83_CR_SL solves.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
  fprintf ( 1, '  The matrix is NOT symmetric.%d\n' );
  fprintf ( 1, '\n' );
%
%  Set the matrix values.
%
  a(1,1) = 0.0E+00;
  for j = 2 : n
    a(1,j) = j;
  end

  for j = 1 : n
    a(2,j) = 4.0E+00 * j;
  end

  for j = 1 : n - 1
    a(3,j) = j;
  end
  a(3,n) = 0.0E+00;

  r83_print ( n, a, '  The matrix:' );
%
%  Set the desired solution.
%
  x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
  b = r83_mxv ( n, a, x );
  x(1:n) = 0.0E+00;
%
%  Factor the matrix.
%
  a_cr = r83_cr_fa ( n, a );
%
%  Solve the linear system.
%
  x = r83_cr_sl ( n, a_cr, b );

  r8vec_print_some ( n, x, 10, '  Solution:' );

  return
end


function [Y,L,S,params] = loadSyntheticProblem(varargin)
% [Y,L,S,params] = loadSyntheticProblem(problemName, referenceSolutionDirectory)
%
% Returns parameters and solution for the following problems
%   (parameters are tuned so that each problem has the
%    the same true solution (L,S) )
% Or, call this with no input arguments to set the path...
%
% "Lag": min   lambdaL*||L||_* + lambdaS*||S||_1 + .5||L+S-Y||_F^2
%
% "Sum": min   ||L||_* + lambdaSum*||S||_1
%        subject to ||L+S-Y||_F <= epsilon
%
% "Max": min   max( ||L||_* , lambdaMax*||S||_1  )
%        subject to ||L+S-Y||_F <= epsilon
%
% "Dual-sum"  min  .5||L+S-Y||_F^2
%         subject to ||L||_* + lambdaSum*||S||_1  <= tauSum
%
% "Max-sum"  min  .5||L+S-Y||_F^2
%         subject to max( ||L||_*, lambdaMax*||S||_1)  <= tauMax
%
% so params has the following fields:
%   .lambdaL
%   .lambdaS
%   .lambdaSum
%   .lambdaMax
%   .tauSum
%   .tauMax
%   .epsilon
%
%   .objective  This is a function of (L,S) and is the objective
%               of the "Lagrangian" formulation
%
%   .L1L2   set to 0, 'rows', or 'sum' for using l1 or l1l2 block norms
%
% Stephen Becker, March 6 2014

% addpath for the proxes and solvers
Mfilename = mfilename('fullpath');
baseDirectory = fileparts(Mfilename);
addpath(genpath(baseDirectory));

if nargin==0
    % we just setup the path, nothing else...
    return;
end

if nargin >= 2
    baseDirectory = varargin{2};
else
    baseDirectory = fullfile(baseDirectory,'referenceSolutions');
end
norm_nuke = @(L) sum(svd(L,'econ'));
vec = @(X) X(:);
problemString = varargin{1};

%%
% -- Common problem strings --
% problemString = 'small_randn_v2';
% problemString = 'medium_randn_v1';
% problemString = 'medium_exponentialNoise_v1';
% problemString = 'large_exponentialNoise_square_v1';
% problemString = 'large_exponentialNoise_rectangular_v1';
% problemString = 'huge_exponentialNoise_rectangular_v1'; % not yet done!
% problemString = 'nsa500_SNR80_problem1_run1';

fprintf('%s\n** Problem %s **\n\n',char('*'*ones(60,1)), problemString );
fileName      = fullfile(baseDirectory,[problemString,'.mat']);
if exist(fileName,'file')
    fprintf('Loading reference solution from file %s\n', fileName);
    load(fileName); % should contain L, S and Y at least, and preferably lambdaL and lambdaS
else
    [L,S,Y] = deal( [] );
end
% Default:
L1L2  = 0;

% For problems from the NSA software package:
if length(problemString)>3 && strcmpi( problemString(1:3), 'nsa' )
    % This sscanf line is broken in versions of Matlab after R2010a
    tmp = sscanf(problemString,'nsa%d_SNR%d_problem%d_run%d');
    m       = tmp(1); % either 500, 1000 or 1500
    SNR     = tmp(2); % either 80 or 45
    problem_type = tmp(3);
    run_counter  = tmp(4);
    % e.g.,  'nsa500_SNR80_problem1_run1'
    n  = m;
%     % Variable parameters
%     m = 500; n = m;
%     problem_type = 1; % between 1 and 4. Controls rank/sparsity
%     SNR          = 80;
%     run_counter  = 1; % between 1 and 10. Only affects seed

%     % Possible sparsity amounts:
%     p_array = ceil( m*n*[ 0.05, 0.1, 0.05, 0.1 ] );
%     r_array = ceil(min(m,n)*[  0.05, 0.05, 0.1, 0.1 ] );

    % Changing the conventions, March 14. They had 4 cases, for p = {.05,.1}
    %   and r = {.05, .1 } (these are sparsity and rank percentages)
    % I don't want to do that many tests since we will display graphically
    %   and not in tables. I also want more values of p and r. So I will
    %   make p and r be linked.
    p_array = ceil( m*n*[ 0.05, 0.1, 0.2, 0.3 ] );
    r_array = ceil(min(m,n)*[  0.05, 0.1, 0.2, 0.3 ] );

    p = p_array(problem_type);
    r = r_array(problem_type);
    pwr = r+p/(m*n)*100^2/3;
    stdev = sqrt(pwr/10^(SNR/10));
    delta = sqrt(min(m,n)+sqrt(8*min(m,n)))*stdev;
    lambdaSum = 1/sqrt( max(m,n) ); % "xi" in NSA
else
    switch problemString
        case 'small_randn_v1'
            my_rng(101);
            m   = 40; n = 42; lambdaL = .5; lambdaS = 1e-1;
            if isempty(Y)
                Y   = randn(m,n);
                printEvery = 500; tol = 1e-12;  maxIts = 3e3;
            end

        case 'small_randn_v2'
            my_rng(102);
            m   = 40; n = 42; lambdaL = .5; lambdaS = 1e-1;
            if isempty(Y)
                Y   = randn(m,n);
                printEvery = 500; tol = 1e-12; maxIts = 3e3;
            end
        case 'medium_randn_v1'
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            if isempty(Y)
                Y   = randn(m,n);
                printEvery = 1; %tol = 1e-4; maxIts = 100;
                tol = 1e-10; maxIts = 1e3;
            end
        case 'medium_exponentialNoise_v1'
            % Converges much faster than 'medium_randn_v1'
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 1000;

        case 'medium_exponentialNoise_v2'
            % Converges much faster than 'medium_randn_v1'
            % our quasi-Newton scheme does well here
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.05*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 1000;

        case 'medium_exponentialNoise_v3'
            % Converges much faster than 'medium_randn_v1'
            my_rng(101);
            m   = 400; n = 500; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.1*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 1000;

        case 'large_exponentialNoise_square_v1'
            my_rng(201);
            m   = 1e3; n = m; lambdaL = .25; lambdaS = 1e-2;
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-8; maxIts = 250;

        case 'large_exponentialNoise_rectangular_v1'
            my_rng(201);
            m   = 5e3; n = 200; lambdaL = .25; lambdaS = 5e-3; % OK
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-8; maxIts = 100;

        case 'huge_exponentialNoise_rectangular_v1';
            disp('Warning: lambda values not yet tuned');
            my_rng(301);
            m   = 5e4; n = 200; lambdaL = .25; lambdaS = 5e-2;
            rk  = round(.5*min(m,n) );
            Q1  = haar_rankR(m,rk,false);
            Q2  = haar_rankR(n,rk,false);
            Y   = Q1*diag(.1+rand(rk,1))*Q2';
            mdn = median(abs(Y(:)));
            Y   = Y + exprnd( .1*mdn, m, n );

            printEvery = 5; tol = 1e-10; maxIts = 100;

        case 'escalator_small';
            % added March 29 2015
            load escalator_data_2015_small % has Y, mat, m, n, nFrames
            m   = m*n; % 64 x 79, 200 frames
            n   = nFrames;
            lambdaL     = 3;
            lambdaS     = 0.06;
            printEvery  = 1;
            tol         = 1e-9;
            maxIts      = 100;
            L0          = Y;
            S0          = 0*Y;

        case 'verySmall_l1'
            load verySmall_l1
            % Used CVX to generate solution

        case 'verySmall_l1l2'
            load verySmall_l1l2
            % Used CVX to generate solution
            L1L2 = 'rows';

        otherwise
            error('bad problem name');
    end
end

% Generate new reference soln if not precomputed
if isempty(L)
    fprintf('Generating reference solution...\n');
    if length(problemString)>3 && strcmpi( problemString(1:3), 'nsa' )
        % We solve Sum problem, which gives us a lot of parameters but not
        % the parameters for the Lagrangian problem unfortunately.

        % I am changing the seeds format
        seed = 601;
        my_rng( seed + run_counter );
        [Y, optimal_L, optimal_S, deltabar]=create_data_noisy_L2(m,n,p,r,stdev);

        tol = 0.0001;
        denoise_flag=0;

        %         [L,S,out] = nsa(Y,stdev,tol,denoise_flag,optimal_L,optimal_S);
        %         [L,S,out] = nsa(Y,-delta,tol,denoise_flag,optimal_L,optimal_S);

        [L,S,out] = nsa(Y,-delta,tol,denoise_flag,[],[],lambdaSum);
        fprintf('Discrepancy with intitial signal L is %.2e\n', ...
            norm(L-optimal_L,'fro')/norm(L,'fro') );
        fprintf('Discrepancy with intitial signal S is %.2e\n', ...
            norm(S-optimal_S,'fro')/norm(S,'fro') );

        lambdaL     = [];
        lambdaS     = [];
        maxIts      = [];
    else
        tic
        opts    = struct('printEvery',printEvery,'FISTA',true,'tol',tol,'maxIts',maxIts);
        %[L,S,errHist] = solver_RPCA_Lagrangian_simple(Y,lambdaL,lambdaS,[],opts);
        opts.quasiNewton  = true; opts.FISTA=false; opts.BB = false;
        opts.SVDstyle   = 1; % for highest accuracy
        if exist('L0','var')
            opts.L0     = L0;
        end
        if exist('S0','var')
            opts.S0     = S0;
        end
        [L,S,errHist] = solver_RPCA_Lagrangian(Y,lambdaL,lambdaS,[],opts);
        toc
    end

    save(fileName,'L','S','Y','lambdaL','lambdaS','tol','maxIts');
end

sigma   = svd(L,'econ');
rk      = sum( sigma > 1e-8 );
normL   = sum(sigma);

params = [];
params.L1L2        = L1L2;
if L1L2
    if strcmpi(L1L2,'rows')
        params.objective    = @(L,S) lambdaL*norm_nuke(L)+lambdaS*sum( sqrt( sum(S.^2,2) ) ) + ...
            .5*norm(vec(L+S-Y))^2;
        normS   = sum( sqrt( sum(S.^2,2) ) );
    elseif strcmpi(L1L2,'cols')
        params.objective    = @(L,S) lambdaL*norm_nuke(L)+lambdaS*sum( sqrt( sum(S.^2,1) ) ) + ...
            .5*norm(vec(L+S-Y))^2;
        normS   = sum( sqrt( sum(S.^2,1) ) );
    else error('bad value for L1L2');
    end
else
    params.objective    = @(L,S) lambdaL*norm_nuke(L)+lambdaS*norm(S(:),1) + ...
        .5*norm(vec(L+S-Y))^2;
    normS   = norm(S(:),1);
end
params.epsilon     = norm( L + S - Y, 'fro' );
params.lambdaL     = lambdaL;
params.lambdaS     = lambdaS;
if exist('lambdaSum','var')
    params.lambdaSum   = lambdaSum;
else
    params.lambdaSum   = lambdaS/lambdaL;
end
params.lambdaMax   = normL/normS;
params.tauSum      = normL + params.lambdaSum*normS;
params.tauMax      = max(normL,params.lambdaMax*normS );

fprintf('\tSize %d x %d\n\tL has nuclear norm %.3f and rank %d of %d possible\n', ...
    m,n,normL, rk, length(sigma) );
fprintf('\tS has l1 (or l1l2) norm %.3f and %.2f%% of its elements are nonzero\n', ...
    normS, 100*sum( abs(S(:)) > 1e-8)/numel(S) );
fprintf('\t||L+S-Y||/||Y|| is %.2e\n', params.epsilon/norm(Y,'fro' ) );

end % end of main function

function Q = haar_rankR(n, r, cmplx)
% Q = HAAR_RANKR( N , R)
%   returns a N x N unitary matrix Q
%   from the Haar measure on the
%   Circular Unitary Ensemble (CUE)
%
% Q = HAAR_RANKR( N, R, COMPLEX )
%   if COMPLEX is false, then returns a real matrix from the
%   Circular Orthogonal Ensemble (COE). By default, COMPLEX = true
%
% For more info, see http://www.ams.org/notices/200705/fea-mezzadri-web.pdf
% "How to Generate Random Matrices fromthe Classical Compact Groups"
% by Francesco Mezzadri (also at http://arxiv.org/abs/math-ph/0609050 )
%
% Other references:
%   "How to generate a random unitary matrix" by Maris Ozols
%
% Stephen Becker, 2/24/11. updated 11/1/12 for rank r version.
%
% To test that the eigenvalues are evenly distributed on the unit
% circle, do this:   a = angle(eig(haar(1000))); hist(a,100);
%
% This calls "randn", so it will change the stream of the prng

if nargin < 3, cmplx = true; end
if nargin < 2 || isempty(r) , r=n; end

if cmplx
    z = (randn(n,r) + 1i*randn(n,r))/sqrt(2.0);
else
    z = randn(n,r);
end
[Q,R] = qr(z,0); % time consuming
d = diag(R);
ph = d./abs(d);
% Q = multiply(q,ph,q) % in python. this is q <-- multiply(q,ph)
%   where we multiply each column of q by an element in ph

%Q = Q.*repmat( ph', n, 1 );
%  use bsxfun for this to make it fast...
Q = bsxfun( @times, Q, ph' );

end % end of haar_rankR

function varargout = my_rng(varargin)
%MY_RNG Control the random number generator used by RAND, RANDI, and RANDN (SRB version)
%   MY_RNG(SD) seeds the random number generator using the non-negative
%   integer SD so that RAND, RANDI, and RANDN produce a predictable
%   sequence of numbers.
% MODIFIED BY STEPHEN BECKER
%   See also RAND, RANDI, RANDN, RandStream, NOW.

%   See Choosing a Random Number Generator for details on these generators.

if exist('rng','file')
    [varargout{1:nargout}] = rng(varargin{:});
    return;
end
% Otherwise, imitate the functionality...

% -- SRB adding this --
error(nargchk(1,1,nargin));
error(nargoutchk(0,0,nargout));
arg1 = varargin{1};
% For R2008a, this doesn't work... (not sure what earliest version is)
if verLessThan('matlab','7.7')
    randn('state',arg1);
    rand('state',arg1);
else
    if verLessThan('matlab','8.2')
        RandStream.setDefaultStream(RandStream('mt19937ar', 'seed', arg1) );
    else
        RandStream.setGlobalStream(RandStream('mt19937ar', 'seed', arg1) );
    end
end
end % end of my_rng


%%***********************************************************
%% lmiexamp1: generate SDP data for the following LMI problem
%%
%%  max  -eta
%%  s.t. B*P + P*B'  <= 0
%%        -P         <= -I
%%         P - eta*I <= 0
%%         P(1,1)     = 1
%%***********************************************************
%% Here is an example on how to use this function to
%% find an optimal P.
%%
%% B = [-1  0  0; 5 -2  0; 1  1 -1];
%% [blk,At,C,b] = lmiexamp1(B);
%% [obj,X,y,Z] = sqlp(blk,At,C,b);
%% P = smat(blk(1,:),y);
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 20 Apr 02
%%***********************************************************

   function [blk,At,C,b] = lmiexamp1(B);

   n = length(B); n2 = n*(n+1)/2;
   I  = speye(n);
   z0 = sparse(n2,1);
   blktmp{1,1} = 's'; blktmp{1,2} = n;
%%
   blk{1,1} = 's'; blk{1,2} = n;
   blk{2,1} = 's'; blk{2,2} = n;
   blk{3,1} = 's'; blk{3,2} = n;
   blk{4,1} = 'u'; blk{4,2} = 1;
%%
   At{1,1} = [lmifun(B,I),     z0];
   At{2,1} = [lmifun(-I/2,I),  z0];
   At{3,1} = [lmifun(I/2,I),   svec(blktmp,-I,1)];
   At{4,1} = sparse([1, zeros(1,n2)]);
%%
   C{1,1} = sparse(n,n);
   C{2,1} = -speye(n);
   C{3,1} = sparse(n,n);
   C{4,1} = 1;
%%
   b = [zeros(n2,1); -1];
%%**********************************************************


function [samples, energies, diagn] = metrop2(f, x, opt, gradf, varargin)
%METROP2	Markov Chain Monte Carlo sampling with Metropolis algorithm.
%
%	Description
%	SAMPLES = METROP(F, X, OPT) uses the Metropolis algorithm to
%	sample from the distribution P ~ EXP(-F), where F is the first
%	argument to METROP.   The Markov chain starts at the point X and each
%	candidate state is picked from a Gaussian proposal distribution and
%	accepted or rejected according to the Metropolis criterion.
%
%	SAMPLES = METROP(F, X, OPT, [], P1, P2, ...) allows additional
%	arguments to be passed to F().  The fourth argument is ignored, but
%	is included for compatibility with HMC and the optimizers.
%
%	[SAMPLES, ENERGIES, DIAGN] = METROP(F, X, OPT) also returns a log
%	of the energy values (i.e. negative log probabilities) for the
%	samples in ENERGIES and DIAGN, a structure containing diagnostic
%	information (position and acceptance threshold) for each step of the
%	chain in DIAGN.POS and DIAGN.ACC respectively.  All candidate states
%	(including rejected ones) are stored in DIAGN.POS.
%
%	S = METROP('STATE') returns a state structure that contains the state
%	of the two random number generators RAND and RANDN. These are
%	contained in fields randstate,  randnstate.
%
%	METROP('STATE', S) resets the state to S.  If S is an integer, then
%	it is passed to RAND and RANDN. If S is a structure returned by
%	METROP('STATE') then it resets the generator to exactly the same
%	state.
%
%       See METROP2_OPT for the optional parameters in the OPTIONS
%       structure.
%
%	See also
%	HMC, METROP2_OPT
%

%	Copyright (c) Christopher M Bishop, Ian T Nabney (1996, 1997)
%	Copyright (c) 1998-2000 Aki Vehtari

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

% Global variable to store state of momentum variables: set by set_state
% Used to initialize variable if set
global HMC_MOM
if nargin <= 2
  if ~strcmp(f, 'state')
    error('Unknown argument to metrop2');
  end
  switch nargin
   case 1
    samples = get_state(f);
    return;
   case 2
    set_state(f, x);
    return;
  end
end

% Set empty omptions to default values
opt=metrop2_opt(opt);
% Refrence to structures is much slower, so...
opt_nsamples=opt.nsamples;
opt_display =opt.display;
stddev = opt.stddev;

% Set up string for evaluating potential function.
%f = fcnchk(f, length(varargin));

nparams = length(x);
samples = zeros(opt_nsamples, nparams);		% Matrix of returned samples.
if nargout >= 2
  en_save = 1;
  energies = zeros(opt_nsamples, 1);
else
  en_save = 0;
end
if nargout >= 3
  diagnostics = 1;
  diagn_pos = zeros(opt_nsamples, nparams);
  diagn_acc = zeros(opt_nsamples, 1);
else
  diagnostics = 0;
end

% Main loop.
k = - opt.nomit + 1;
Eold = f(x, varargin{:});	% Evaluate starting energy.
nreject = 0;				% Initialise count of rejected states.
while k <= opt_nsamples

  xold = x;
  % Sample a new point from the proposal distribution
  x = xold + randn(1, nparams)*stddev;

  % Now apply Metropolis algorithm.
  Enew = f(x, varargin{:});	% Evaluate new energy.
  a = exp(Eold - Enew);			% Acceptance threshold.
  if (diagnostics & k > 0)
    diagn_pos(k,:) = x;
    diagn_acc(k,:) = a;
  end
  if (opt_display > 1)
    fprintf(1, 'New position is\n');
    disp(x);
  end

  if a > rand(1)	% Accept the new state.
    Eold = Enew;
    if (opt_display > 0)
      fprintf(1, 'Finished step %4d  Threshold: %g\n', k, a);
    end
  else			% Reject the new state
    if k > 0
      nreject = nreject + 1;
    end
    x = xold;	% Reset position
    if (opt_display > 0)
      fprintf(1, '  Sample rejected %4d.  Threshold: %g\n', k, a);
    end
  end
  if k > 0
    samples(k,:) = x;			% Store sample.
    if en_save
      energies(k) = Eold;		% Store energy.
    end
  end
  k = k + 1;
end

if (opt_display > 0)
  fprintf(1, '\nFraction of samples rejected:  %g\n', ...
          nreject/(opt_nsamples));
end

if diagnostics
  diagn.pos = diagn_pos;
  diagn.acc = diagn_acc;
end

% Return complete state of the sampler.
function state = get_state(f)

state.randstate = rand('state');
state.randnstate = randn('state');
return

% Set state of sampler, either from full state, or with an integer
function set_state(f, x)

if isnumeric(x)
  rand('state', x);
  randn('state', x);
else
  if ~isstruct(x)
    error('Second argument to metrop must be number or state structure');
  end
  if (~isfield(x, 'randstate') | ~isfield(x, 'randnstate'))
    error('Second argument to metrop must contain correct fields')
  end
  rand('state', x.randstate);
  randn('state', x.randnstate);
end
return


function [estimateclasstotal,model,D]=adaboost(mode,X,labels,model,itt,weak_type)
% This function AdaBoost, consist of two parts a simpel weak classifier and
% a boosting part:
% The weak classifier tries to find the best treshold in one of the data
% dimensions to sepparate the data into two classes -1 and 1
% The boosting part calls the clasifier iteratively, after every classification
% step it changes the weights of miss-classified examples. This creates a
% cascade of "weak classifiers" which behaves like a "strong classifier"
%
%
%   input -----------------------------------------------------------------
%
%       o mode   : string,  'train' or 'apply'.
%
%       o X      : (N x D), dataset of N samples of dimension D.
%
%       o labels : (N x 1), class labels {-1,+1}
%
%       o model  : struct of model learned, set ot [] when training.
%
%       o itt    :  number of iterations to perform which is equivalent to
%                   the number of resulting weak classifiers.
%
%       o weak_type : string, weak classifier type:
%
%               - decision_stump : splits space along the dimension
%
%   output ----------------------------------------------------------------
%
%  inputs/outputs:
%    datafeatures : An Array with size number_samples x number_features
%    dataclass : An array with the class off all examples, the class
%                 can be -1 or 1
%    itt : The number of training itterations
%    model : A struct with the cascade of weak-classifiers
%    estimateclass : The by the adaboost model classified data
%

if ~exist('weak_type','var'), weak_type = 'decision_stump'; end

switch(mode)
    case 'train'
        % Train the adaboost model

        % Set the data class labels
        labels    = labels(:);
        model     = struct;

        % Weight of training samples, first every sample is even important
        % (same weight)
        D=ones(length(labels),1)/length(labels);

        % This variable will contain the results of the single weak
        % classifiers weight by their alpha
        predict_label   =   zeros(size(labels));

        % Calculate max min of the data
        boundary=[min(X,[],1) max(X,[],1)];
        % Do all model training itterations
        for t=1:itt
            % Find the best treshold to separate the data in two classes

            if strcmpi(weak_type,'decision_stump')

                [estimateclass,err,h] = WeightedThresholdClassifier(X,labels,D);

            elseif strcmpi(weak_type,'linear')

                [B_hat,mapping] = train_linear_classifier(X,labels,D);
                [estimateclass] = predictor_linear_classifier(X,B_hat,mapping);
                err             = sum(D(:) .* (estimateclass ~= labels));
            end

            % Weak classifier influence on total result is based on the current
            % classification error
            alpha=1/2 * log((1-err)/max(err,eps));

            % Store the model parameters
            model(t).alpha      = alpha;

            if strcmpi(weak_type,'decision_stump')
                model(t).dimension  = h.dimension;
                model(t).threshold  = h.threshold;
                model(t).direction  = h.direction;
                model(t).boundary   = boundary;
            elseif strcmpi(weak_type,'linear')
                model(t).B_hat   = B_hat;
                model(t).mapping = mapping;
            end

            % We update D so that wrongly classified samples will have more weight
            D = D.* exp(-model(t).alpha.*labels.*estimateclass);
            D = D./sum(D);

            % Calculate the current error of the cascade of weak
            % classifiers
            predict_label       =   predict_label + estimateclass * model(t).alpha;
            estimateclasstotal  =   sign(predict_label);

            model(t).error      =   sum(estimateclasstotal~=labels)/length(labels);

            if(model(t).error==0), break; end

        end

    case 'apply'
        % Apply Model on the test data

        if strcmpi(weak_type,'decision_stump')

            % Limit datafeatures to orgininal boundaries
            if(length(model)>1);
                minb=model(1).boundary(1:end/2);
                maxb=model(1).boundary(end/2+1:end);
                X=bsxfun(@min,X,maxb);
                X=bsxfun(@max,X,minb);
            end

            % Add all results of the single weak classifiers weighted by their alpha
            predict_label=zeros(size(X,1),1);
            for t=1:length(model);
                predict_label=predict_label+model(t).alpha*ApplyClassTreshold(model(t), X);
            end
            % If the total sum of all weak classifiers
            % is less than zero it is probablly class -1 otherwise class 1;
        elseif strcmpi(weak_type,'linear')

            predict_label = zeros(size(X,1),1);

            for t=1:length(model);
                estimateclass = predictor_linear_classifier(X,model(t).B_hat,model(t).mapping);
                predict_label = predict_label + model(t).alpha * estimateclass;
            end

        end

        estimateclasstotal = sign(predict_label);

    otherwise
        error('adaboost:inputs','unknown mode');
end


function [angleRate] = PTrc2deg(X,rcRate,rcExpo,superrate, C)
% raw RCcommand data to RCrate in deg/s, i.e. "set point"
%  with help from: https://github.com/betaflight/betaflight-configurator/blob/master/src/js/RateCurve.js
%   X is a vector containing a single axis of RCcommand data scaled from -500 to 500,
%   rcRate(0-255),rcExpo(0-100),superrate(0-100)

    expoPower=3;
    rcRateConstant=C;
    angleRate=[];

    rcRate=rcRate/100;
    rcExpo=(rcExpo/100);
    superrate=superrate/100;

    maxRC=500;
    rcCommandf = X / maxRC;% scales from -1 to 1 for the math
    rcCommandfAbs = abs(rcCommandf) / 1;%max(abs(rcCommandf));

    if (rcRate > 2)
        rcRate = rcRate + (rcRate - 2) * 14.54;
    end
    if (rcExpo > 0)
       %disp('rcExpo > 0')
        rcCommandf =  rcCommandf .* power(rcCommandfAbs, expoPower) * rcExpo + rcCommandf * (1-rcExpo);
    end
    if (superrate > 0)
        %disp('superrate > 0')
        rcFactor = 1 ./ (1 - rcCommandfAbs * superrate); % this creates the super expo curve needed to convert RCcommand to RCrate
        angleRate = (rcRateConstant * rcRate * rcCommandf);
        angleRate = angleRate .* rcFactor;
       % disp(['angleRate:' num2str(angleRate) ' rcFactor:' num2str(rcFactor)])
    else
        angleRate = (rcRateConstant * rcRate * rcCommandf);
    end

end


function heapsort_complexity ( )

%*****************************************************************************80
%
%% HEAPSORT_COMPLEXITY examines the time required to perform a heap sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 December 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HEAPSORT_COMPLEXITY\n' );
  fprintf ( 1, '  How does the time for heapsort increase with vector length N?\n' );
%
%  Get some data for N = 1 : 200.
%  Do the loop twice to avoid startup anomalies.
%
  for i = 1 : 2

    data_size = zeros ( 200, 1 );
    data_time = zeros ( 200, 1 );

    for n = 1 : 200
      a = rand ( n, 1 );
      tic;
      a_heap = r8vec_sort_heap_a ( n, a );
      data_size(n) = n;
      data_time(n) = toc;
    end

  end

  figure ( 1 )
  plot ( data_size, data_time, 'r-' )
  grid on
  xlabel ( 'Vector length N' );
  ylabel ( 'Elapsed time T' );
  title ( 'T(N), time to heap sort a vector of length N' );

  filename = 'heapsort_1to200.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );
%
%  Get some data for N = 1, 2, 4, ..., 2^12.
%  Do the loop twice to avoid startup anomalies.
%
  for i = 1 : 2

    data_size = zeros ( 13, 1 );
    data_time = zeros ( 13, 1 );

    n = 1;

    for nlog = 0 : 12
      a = rand ( n, 1 );
      tic;
      a_heap = r8vec_sort_heap_a ( n, a );
      data_size(nlog+1) = n;
      data_time(nlog+1) = toc;
      n = n * 2;
    end

  end

  figure ( 2 )
  loglog ( data_size, data_time, 'r-o' )
  grid on
  xlabel ( 'Vector length N' );
  ylabel ( 'Elapsed time T' );
  title ( 'T(N), time to heap sort a vector of length N' );

  filename = 'heapsort_powersoftwo.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HEAPSORT_COMPLEXITY:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end
function a_out = r8vec_heap_d ( n, a )

%*****************************************************************************80
%
%% R8VEC_HEAP_D reorders an R8VEC into an descending heap.
%
%  Discussion:
%
%    A descending heap is an array A with the property that, for every index J,
%    A(J) >= A(2*J) and A(J) >= A(2*J+1), (as long as the indices
%    2*J and 2*J+1 are legal).
%
%  Diagram:
%
%                  A(1)
%                /      \
%            A(2)         A(3)
%          /     \        /  \
%      A(4)       A(5)  A(6) A(7)
%      /  \       /   \
%    A(8) A(9) A(10) A(11)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the size of the input array.
%
%    Input, real A(N), an unsorted array.
%
%    Output, real A_OUT(N), the array has been reordered into a heap.
%
  a_out(1:n) = a(1:n);
%
%  Only nodes N/2 down to 1 can be "parent" nodes.
%
  for i = floor ( n/2 ) : -1 : 1
%
%  Copy the value out of the parent node.
%  Position IFREE is now "open".
%
    key = a_out(i);
    ifree = i;

    while ( 1 )
%
%  Positions 2*IFREE and 2*IFREE + 1 are the descendants of position
%  IFREE.  (One or both may not exist because they exceed N.)
%
      m = 2 * ifree;
%
%  Does the first position exist?
%
      if ( n < m )
        break;
      end
%
%  Does the second position exist?
%
      if ( m + 1 <= n )
%
%  If both positions exist, take the larger of the two values,
%  and update M if necessary.
%
        if ( a_out(m) < a_out(m+1) )
          m = m + 1;
        end

      end
%
%  If the large descendant is larger than KEY, move it up,
%  and update IFREE, the location of the free position, and
%  consider the descendants of THIS position.
%
      if ( a_out(m) <= key )
        break;
      end

      a_out(ifree) = a_out(m);
      ifree = m;

    end
%
%  Once there is no more shifting to do, KEY moves into the free spot IFREE.
%
    a_out(ifree) = key;

  end

  return
end
function a_sorted = r8vec_sort_heap_a ( n, a )

%*****************************************************************************80
%
%% R8VEC_SORT_HEAP_A ascending sorts an R8VEC using heap sort.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 April 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    A Nijenhuis and H Wilf,
%    Combinatorial Algorithms,
%    Academic Press, 1978, second edition,
%    ISBN 0-12-519260-6.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real A(N), the array to be sorted;
%
%    Output, real A_SORTED(N), the sorted array.
%
  if ( n < 1 )
    a_sorted = [];
    return
  end

  if ( n == 1 )
    a_sorted(1) = a(1);
    return
  end
%
%  1: Put A into descending heap form.
%
  a_sorted = r8vec_heap_d ( n, a );
%
%  2: Sort A.
%
%  The largest object in the heap is in A(1).
%  Move it to position A(N).
%
  temp = a_sorted(1);
  a_sorted(1) = a_sorted(n);
  a_sorted(n) = temp;
%
%  Consider the diminished heap of size N1.
%
  for n1 = n-1 : -1 : 2
%
%  Restore the heap structure of A(1) through A(N1).
%
    a_sorted(1:n1) = r8vec_heap_d ( n1, a_sorted );
%
%  Take the largest object from A(1) and move it to A(N1).
%
    temp = a_sorted(1);
    a_sorted(1) = a_sorted(n1);
    a_sorted(n1) = temp;

  end

  return
end


function [Neurons] = neural_gas(D,n,epochs,alpha0,lambda0)

%NEURAL_GAS Quantizes the data space using the neural gas algorithm.
%
% Neurons = neural_gas(D, n, epochs, [alpha0], [lambda0])
%
%   C = neural_gas(D,50,10);
%   sM = som_map_struct(sD);
%   sM.codebook = neural_gas(sD,size(sM.codebook,1),10);
%
%  Input and output arguments ([]'s are optional):
%   D          (matrix) the data matrix, size dlen x dim
%              (struct) a data struct
%   n          (scalar) the number of neurons
%   epochs     (scalar) the number of training epochs (the number of
%                       training steps is dlen*epochs)
%   [alpha0]   (scalar) initial step size, 0.5 by default
%   [lambda0]  (scalar) initial decay constant, n/2 by default
%
%   Neurons    (matrix) the neuron matrix, size n x dim
%
% See also SOM_MAKE, KMEANS.

% References:
%  T.M.Martinetz, S.G.Berkovich, and K.J.Schulten. "Neural-gas" network
%  for vector quantization and its application to time-series prediction.
%  IEEE Transactions on Neural Networks, 4(4):558-569, 1993.

% Contributed to SOM Toolbox vs2, February 2nd, 2000 by Juha Vesanto
% Copyright (c) by Juha Vesanto
% http://www.cis.hut.fi/projects/somtoolbox/

% juuso 101297 020200

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Check arguments and initialize

error(nargchk(3, 5, nargin));  % check the number of input arguments

if isstruct(D), D = D.data; end
[dlen,dim] = size(D);
Neurons = (rand(n,dim)-0.5)*10e-5; % small initial values
train_len = epochs*dlen;

if nargin<4 || isempty(alpha0) || isnan(alpha0), alpha0 = 0.5; end
if nargin<5 || isempty(lambda0) || isnan(lambda0), lambda0 = n/2; end

% random sample order
rand('state',sum(100*clock));
sample_inds = ceil(dlen*rand(train_len,1));

% lambda
lambda = lambda0 * (0.01/lambda0).^([0:(train_len-1)]/train_len);

% alpha
alpha = alpha0 * (0.005/alpha0).^([0:(train_len-1)]/train_len);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Action

for i=1:train_len,

  % sample vector
  x = D(sample_inds(i),:); % sample vector
  known = ~isnan(x);       % its known components
  X = x(ones(n,1),known);  % we'll need this

  % neighborhood ranking
  Dx = Neurons(:,known) - X;  % difference between vector and all map units
  [~, inds] = sort((Dx.^2)*known'); % 1-BMU, 2-BMU, etc.
  ranking(inds) = [0:(n-1)];
  h = exp(-ranking/lambda(i));
  H = h(ones(length(known),1),:)';

  % update
  Neurons = Neurons + alpha(i)*H.*(x(ones(n,1),known) - Neurons(:,known));

  % track
  fprintf(1,'%d / %d \r',i,train_len);
  if 0 && mod(i,50) == 0,
    hold off, plot3(D(:,1),D(:,2),D(:,3),'bo')
    hold on, plot3(Neurons(:,1),Neurons(:,2),Neurons(:,3),'r+')
    drawnow
  end
end

fprintf(1,'\n');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%