function stroud_test41 ( )

%*****************************************************************************80
%
%% TEST41 tests TRIANGLE_UNIT_SET and TRIANGLE_SUB.
%
%  Discussion:
%
%    Break up the triangle into NSUB**2 equal subtriangles.  Approximate
%    the integral over the triangle by the sum of the integrals over each
%    subtriangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_2D_INDEX;

  num = function_2d_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST41\n' );
  fprintf ( 1, '  TRIANGLE_UNIT_SET sets up a quadrature rule\n' );
  fprintf ( 1, '    on a triangle.\n' );
  fprintf ( 1, '  TRIANGLE_SUB applies it to subtriangles of an\n' );
  fprintf ( 1, '    arbitrary triangle.\n' );
  fprintf ( 1, '\n' );
%
%  Set the location of the triangle.
%
  xval(1) = 0.0;
  yval(1) = 0.0;

  xval(2) = 0.0;
  yval(2) = 1.0;

  xval(3) = 1.0;
  yval(3) = 0.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Triangle vertices:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %8f  %8f\n', xval(1), yval(1) );
  fprintf ( 1, '  %8f  %8f\n', xval(2), yval(2) );
  fprintf ( 1, '  %8f  %8f\n', xval(3), yval(3) );
%
%  Get the quadrature abscissas and weights for a unit triangle.
%
  rule = 3;
  order = triangle_unit_size ( rule );

  [ xtab, ytab, weight ] = triangle_unit_set ( rule, order );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Using unit triangle quadrature rule %d\n', rule );
  fprintf ( 1, '  Rule order = %d\n', order );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Function Nsub  Result\n' );
  fprintf ( 1, '\n' );
%
%  Set the function.
%
  for i = 1 : num

    FUNC_2D_INDEX = i;
%
%  Try an increasing number of subdivisions.
%
    for nsub = 1 : 5

      result = triangle_sub ( ...
        'function_2d', xval, yval, nsub, order, xtab, ytab, weight );

      fname = function_2d_name ( i );

      fprintf ( 1, '  %s  %2d  %12f\n', fname, nsub, result );

    end

  end

  return
end


function hidden_probability = visible_state_to_hidden_probabilities(rbm_w, visible_state)
% <rbm_w> is a matrix of size <number of hidden units> by <number of visible units>
% <visible_state> is a binary matrix of size <number of visible units> by <number of configurations that we're handling in parallel>.
% The returned value is a matrix of size <number of hidden units> by <number of configurations that we're handling in parallel>.
% This takes in the (binary) states of the visible units, and returns the activation probabilities of the hidden units conditional on those states.
    m_ = rbm_w * visible_state;
    hidden_probability = 1 ./ (1 + e .^ (-m_));
end


% Fig. 5.47  Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%  Figure 5.47 Root locus for the heat exchanger, with and without delay

clf
numG=1;
denG=conv([10 1],[60 1]);
sysG=tf(numG, denG);
rlocus(sysG);
axis([-.8 .8 -.6 .6]);
%pause;
hold on
gtext('locus without delay')
Td=5;
numD1 = 1;
denD1=[Td 1];
numGD1=conv(numG,numD1);
denGD1=conv(denG,denD1);
sysGD1= tf(numGD1, denGD1);
rlocus(sysGD1);

gtext('locus with (0,1) lag Pade')
title('Fig. 5.47 Root locus for heat exchanger with and without delay')
%pause;
numDe2=[Td^2/12 -Td/2 1];
denDe2=[Td^2/12 +Td/2 1];
numGD=conv(numG,numDe2);
denGD=conv(denG,denDe2);
sysGD= tf(numGD, denGD);
rlocus(sysGD);

gtext('locus with(2,2) Pade')
grid on
% title('Fig. 5.47 Root locus for heat exchanger with and without delay')
% z=0:.1:.9;
% wn= .1:.1:.6;
% sgrid(z, wn)
hold off


function [mps2] = ftps22mps2(ftps2)
% Convert acceleration from feet per square-second to meters per second
% squared.
% Chad A. Greene 2012
mps2 = ftps2*0.3048;
end


function som = SOMSimple(nfeatures, ndim, nepochs, ntrainingvectors, eta0, etadecay, sgm0, sgmdecay, showMode)
%SOMSimple Simple demonstration of a Self-Organizing Map that was proposed by Kohonen.
%   sommap = SOMSimple(nfeatures, ndim, nepochs, ntrainingvectors, eta0, neta, sgm0, nsgm, showMode)
%   trains a self-organizing map with the following parameters
%       nfeatures        - dimension size of the training feature vectors
%       ndim             - width of a square SOM map
%       nepochs          - number of epochs used for training
%       ntrainingvectors - number of training vectors that are randomly generated
%       eta0             - initial learning rate
%       etadecay         - exponential decay rate of the learning rate
%       sgm0             - initial variance of a Gaussian function that
%                          is used to determine the neighbours of the best
%                          matching unit (BMU)
%       sgmdecay         - exponential decay rate of the Gaussian variance
%       showMode         - 0: do not show output,
%                          1: show the initially randomly generated SOM map
%                             and the trained SOM map,
%                          2: show the trained SOM map after each update
%
%   For example: A demonstration of an SOM map that is trained by RGB values
%
%       som = SOMSimple(3,60,10,100,0.1,0.05,20,0.05,2);
%       % It uses:
%       %   3    : dimensions for training vectors, such as RGB values
%       %   60x60: neurons
%       %   10   : epochs
%       %   100  : training vectors
%       %   0.1  : initial learning rate
%       %   0.05 : exponential decay rate of the learning rate
%       %   20   : initial Gaussian variance
%       %   0.05 : exponential decay rate of the Gaussian variance
%       %   2    : Display the som map after every update

nrows = ndim;
ncols = ndim;

som = rand(nrows,ncols,nfeatures);

if showMode >= 1
    fig = figure;
    displaySOMmap(fig, 1, 'Randomly initialized SOM', som, nfeatures);
end

% Generate random training data
trainingData = rand(ntrainingvectors,nfeatures);

% Generate coordinate system
[x y] = meshgrid(1:ncols,1:nrows);

for t = 1:nepochs
    % Compute the learning rate for the current epoch
    eta = eta0 * exp(-t*etadecay);

    % Compute the variance of the Gaussian (Neighbourhood) function for the ucrrent epoch
    sgm = sgm0 * exp(-t*sgmdecay);

    % Consider the width of the Gaussian function as 3 sigma
    width = ceil(sgm*3);

    for ntraining = 1:ntrainingvectors
        % Get current training vector
        trainingVector = trainingData(ntraining,:);

        % Compute the Euclidean distance between the training vector and
        % each neuron in the SOM map
        dist = getEuclideanDistance(trainingVector, som, nrows, ncols, nfeatures);

        % Find the best matching unit (bmu)
        [~, bmuindex] = min(dist);

        % transform the bmu index into 2D
        [bmurow bmucol] = ind2sub([nrows ncols],bmuindex);

        % Generate a Gaussian function centered on the location of the bmu
        g = exp(-(((x - bmucol).^2) + ((y - bmurow).^2)) / (2*sgm*sgm));

        % Determine the boundary of the local neighbourhood
        fromrow = max(1,bmurow - width);
        torow   = min(bmurow + width,nrows);
        fromcol = max(1,bmucol - width);
        tocol   = min(bmucol + width,ncols);

        % Get the neighbouring neurons and determine the size of the neighbourhood
        neighbourNeurons = som(fromrow:torow,fromcol:tocol,:);
        sz = size(neighbourNeurons);

        % Transform the training vector and the Gaussian function into
        % multi-dimensional to facilitate the computation of the neuron weights update
        T = reshape(repmat(trainingVector,sz(1)*sz(2),1),sz(1),sz(2),nfeatures);
        G = repmat(g(fromrow:torow,fromcol:tocol),[1 1 nfeatures]);

        % Update the weights of the neurons that are in the neighbourhood of the bmu
        neighbourNeurons = neighbourNeurons + eta .* G .* (T - neighbourNeurons);

        % Put the new weights of the BMU neighbouring neurons back to the
        % entire SOM map
        som(fromrow:torow,fromcol:tocol,:) = neighbourNeurons;

        if showMode == 2
            displaySOMmap(fig, 2, ['Epoch: ',num2str(t),'/',num2str(nepochs),', Training Vector: ',num2str(ntraining),'/',num2str(ntrainingvectors)], som, nfeatures);
        end
    end
end

if showMode == 1
    displaySOMmap(fig, 2, 'Trained SOM', som, nfeatures);
end

function ed = getEuclideanDistance(trainingVector, sommap, nrows, ncols, nfeatures)

% Transform the 3D representation of neurons into 2D
neuronList = reshape(sommap,nrows*ncols,nfeatures);

% Initialize Euclidean Distance
ed = 0;
for n = 1:size(neuronList,2)
    ed = ed + (trainingVector(n)-neuronList(:,n)).^2;
end
ed = sqrt(ed);

function displaySOMmap(fig, nsubplot, description, sommap, nfeatures)
% Display given SOM map

figure(fig);
subplot(1,2,nsubplot);
if nfeatures >= 3
    imagesc(sommap(:,:,1:3));
else
    imagesc(sommap(:,:,1));
end
axis off;axis square;
title(description);


addpath('tools')
more off
close all
clear all

% Load laser scans and robot poses.
load("../data/laser")
%laser = read_robotlaser('../data/csail.log');

% Extract robot poses: Nx3 matrix where each row is in the form: [x y theta]
poses = [laser.pose];
poses = reshape(poses,3,size(poses,2)/3)';

% Initial cell occupancy probability.
prior = 0.50;
% Probabilities related to the laser range finder sensor model.
probOcc = 0.9;
probFree = 0.35;

% Map grid size in meters. Decrease for better resolution.
gridSize = 0.1;

% Set up map boundaries and initialize map.
border = 30;
robXMin = min(poses(:,1));
robXMax = max(poses(:,1));
robYMin = min(poses(:,2));
robYMax = max(poses(:,2));
mapBox = [robXMin-border robXMax+border robYMin-border robYMax+border];
offsetX = mapBox(1);
offsetY = mapBox(3);
mapSizeMeters = [mapBox(2)-offsetX mapBox(4)-offsetY];
mapSize = ceil([mapSizeMeters/gridSize]);

% Used when updating the map. Assumes that prob_to_log_odds.m
% has been implemented correctly.
logOddsPrior = prob_to_log_odds(prior);

% The occupancy value of each cell in the map is initialized with the prior.
map = logOddsPrior*ones(mapSize);
disp('Map initialized. Map size:'), disp(size(map))

% Map offset used when converting from world to map coordinates.
offset = [offsetX; offsetY];

% Main loop for updating map cells.
% You can also take every other point when debugging to speed up the loop (t=1:2:size(poses,1))
for(t=1:size(poses,1))
%for(t=1:50)
	t
	% Robot pose at time t.
	robPose = [poses(t,1);poses(t,2);poses(t,3)];

	% Laser scan made at time t.
	sc = laser(1,t);
	% Compute the mapUpdate, which contains the log odds values to add to the map.
	[mapUpdate, robPoseMapFrame, laserEndPntsMapFrame] = inv_sensor_model(map, sc, robPose, gridSize, offset, probOcc, probFree);

	mapUpdate -= logOddsPrior*ones(size(map));
	% Update the occupancy values of the affected cells.
	map += mapUpdate;

	% Plot current map and robot trajectory so far.
        plot_map(map, mapBox, robPoseMapFrame, poses, laserEndPntsMapFrame, gridSize, offset, t);
endfor


function v = makemodel(p,ng)
%MAKEMODEL Helper function to make model matrix
%    P = max model term size, NG = number of grouping variables
% or
%    P = vector of term codes

% We want a matrix with one row per term.  Each row has a 1 for
% the variables participating in the term.  There will be rows
% summing up to 1, 2, ..., p.

if numel(p)==1
   % Create model matrix from a scalar max order value
   vgen = 1:ng;
   v = eye(ng);                      % linear terms
   for j=2:min(p,ng)
      c = nchoosek(vgen,j);          % generate column #'s with 1's
      nrows = size(c,1);             % generate row #'s
      r = repmat((1:nrows)',1,j);    %    and make it conform with c
      m = zeros(nrows,ng);           % create a matrix to get new rows
      m(r(:)+nrows*(c(:)-1)) = 1;    % fill in 1's
      v = [v; m];                    % append rows
   end
else
   % Create model matrix from terms encoded as bit patterms
   nterms = length(p);
   v = zeros(nterms,ng);
   for j=1:nterms
      tm = p(j);
      while(tm)
         % Get last-numbered effect remaining
         lne = 1 + floor(log2(tm));
         tm = bitset(tm, lne, 0);
         v(j,lne) = 1;
      end
   end
end


function mfcc = mfccFeature(config, x)
%MFCCFEATURE    MFCC feature extraction.
%   Format: mfcc = mfccFeature(config, x)
%   Inputs:
%       config: Configuration for MFCC features.
%       x:      Input signal.
%   Output:
%       mfcc:   MFCC features, one column per frame.

    X = abs(spectrogram(config, x));
    frames = size(X,2);
    mfcc = zeros(config.totalDims, frames);

    mfcc(1:config.coefs, :) = config.D * log(config.M * X);
    if (config.energy == true)
        energy = zeros(1,frames);
        for f = 1:frames
            start = (f-1) * config.frameShift + 1;
            finish = start - 1 + config.frameLen;
            frame = x(start:finish) .* config.window;
            energy(f) = sum(frame.^2);
        end
        mfcc(config.staticDims, :) = log(energy);
    end

    % Cepstral mean subtraction or normalization
    switch upper(config.normalization)
        case 'CMS'
            for u = 1:config.staticDims
                mfcc(u,:) = (mfcc(u,:) - mean(mfcc(u,:)));
            end
        case 'CMN'
            for u = 1:config.staticDims
                mfcc(u,:) = (mfcc(u,:) - mean(mfcc(u,:))) / std(mfcc(u,:));
            end
    end

    % Differentials
    zeroCol = zeros(config.staticDims, 1);
    diff = mfcc(1:config.staticDims, :);
    for d = 1:config.diffs
        diff = [diff(:, 2:end), zeroCol] - [zeroCol, diff(:, 1:end-1)];
        mfcc(d * config.staticDims + 1 : (d+1) * config.staticDims, :) = diff;
    end
end


function U = spm_compact_svd(Y,xyz,nu)
% local SVD with compact support for large matrices
% FORMAT U = spm_compact_svd(Y,xyz,nu)
% Y     - matrix
% xyz   - location
% nu    - number of vectors
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_compact_svd.m 5219 2013-01-29 17:07:07Z spm $

% get orders
%--------------------------------------------------------------------------
ns     = size(Y,1);                % number of samples
nv     = size(Y,2);                % number of components (voxels/vertices)

% get kernel (compact vectors)
%--------------------------------------------------------------------------
nc    = max(fix(nv/nu),1);         % voxels in compact support
C     = sum(Y.^2);                 % variance of Y
U     = spalloc(nv,nu,nc*nu);
J     = 1:nv;
for i = 1:nu

    % find maximum variance voxel
    %----------------------------------------------------------------------
    [v,j] = max(C);
    d     = 0;
    for k = 1:size(xyz,1)
        d  = d + (xyz(k,:) - xyz(k,j)).^2;
    end
    [d,j] = sort(d);
    try
        j = j(1:nc);
    end

    % save principal eigenvector
    %----------------------------------------------------------------------
    k        = J(j);
    u        = spm_svd(Y(:,k)');
    U(k,i)   = u(:,1);

    % remove compact support voxels and start again
    %----------------------------------------------------------------------
    J(j)     = [];
    C(j)     = [];
    xyz(:,j) = [];

end


function distmsr=dtiFrenetDistance(curves1, curves2, samegroupflag)

%Computes distance between curves using frenet framework: curve matching
%method from Bakircioglu et al., HBM 6:329-333 (1998)

%Usage:
%dtiFrenetDistances(fibergroup1, fibergroup2, npoints)
%dtiFrenetDistances(fibergroup, npoints)

%ER 2008 04/2008 SCSNL

%1. Checks
if ~((samegroupflag==0)||(samegroupflag==1))
display('Same group flag should be 1 or zero');
    return;
end

if ~isequal(size(curves1,2), size(curves2,2))
    display('Curves must be resampled to the same number of nodes');
return
end

if (samegroupflag==1)&&~isequal(size(curves1,3), size(curves2,3))
    display('Are you sure your curves1 and curves2 are equivalent? curves2 will be ignored');
end

nfibers1=size(curves1, 3);

%actually if same group flag == 1 then curves2 is ignored

%2. Compute frenet representations for the curves
for i=1:nfibers1
[T(:, :, i),N(:, :, i),B(:, :,i ),v1(:, i),k1(:, i),t1(:, i)] = frenetAll(curves1(1, :, i),curves1(2, :, i),curves1(3, :, i));
end

if samegroupflag==0
nfibers2=size(curves2, 3);
    for i=1:nfibers2
[T(:, :, i),N(:, :, i),B(:, :,i ),v2(:, i),k2(:, i),t2(:, i)] = frenetAll(curves2(1, :, i),curves2(2, :, i),curves2(3, :, i));
end
else
    nfibers2=size(curves1, 3);
end

distmsr=zeros(nfibers1, nfibers2);

%CASE WHERE THE GROUPS OF FIBERS ARE NOT EQUIVALENT
if (samegroupflag==0)
display('2 different fiber groups');
    for i=1:nfibers1
        for j=1:nfibers2

            distmsr(i, j)=sum(3.*(k1(:, i).*v1(:, i)-k2(:, j).*v2(:, j)).^2+(t1(:, i).*v1(:, i)-t2(:, j).*v2(:, j)).^2);
        end
      end
elseif (samegroupflag==1)
display('2 equivalent fiber groups');
%A SHORTCUT-CASE, where the two fibergroups supplied are the same.

          for i=1:nfibers1
        for j=i:nfibers2
            distmsr(i, j)=sum(3.*(k1(:, i).*v1(:, i)-k1(:, j).*v1(:, j)).^2+(t1(:, i).*v1(:, i)-t1(:, j).*v1(:, j)).^2);
        end
          end

distmsr=distmsr+distmsr'- diag(diag(distmsr)); %To make a full matrix
end


%% Rotation Matrices for Real Spherical Harmonics. Direct Determination by Recursion
%% This MATLAB code was written based on the C++ code at link:
%% http://mathlib.zfx.info/html_eng/SHRotate_8h-source.html
%% which I believe was again based on the implementation from Don Williamson.
%% http://www.donw.co.uk/home/downloads/SHRotation.zip
%% The original algorithm was given in the following references:
%% [1] Joseph Ivanic and Klaus Ruedenberg
%%     J. Phys. Chem. 1996, 100, 6342-5347
%%
%% [2] Additions and Corrections (to previous paper)
%%     Joseph Ivanic and Klaus Ruedenberg
%%     J. Phys. Chem. A, 1998, Vol. 102, No. 45, 9099
%% Todo:
%% http://www.cgg.cvut.cz/~xkrivanj/papers/2005-rapport1728/rr1728-irisa-kr
%% ivanek-shrotation.pdf

function [R] = SHRotate(r, degree)

%%=============================================================
%% Project:   Spherical Harmonics
%% Module:    $RCSfile: SHRotate.m,v $
%% Language:  MATLAB
%% Author:    $Author: bjian $
%% Date:      $Date: 2007/12/27 06:23:35 $
%% Version:   $Revision: 1.4 $
%%=============================================================

% The first 1x1 sub-matrix is always 1
R{1} = 1;

% The l=1 band is rotated by a permutation of the original matrix
R1(-1+2,-1+2) = r(2,2);
R1(-1+2, 0+2) = r(3,2);
R1(-1+2, 1+2) = r(1,2);

R1( 0+2,-1+2) = r(2,3);
R1( 0+2, 0+2) = r(3,3);
R1( 0+2, 1+2) = r(1,3);

R1( 1+2,-1+2) = r(2,1);
R1( 1+2, 0+2) = r(3,1);
R1( 1+2, 1+2) = r(1,1);

R{1+1} = R1;

% Calculate each block of the rotation matrix for each subsequent band
for band=2:degree
    for m=-band:band
        for n=-band:band
            R{band+1}(m+band+1,n+band+1) = M(band,m,n,R);
        end
    end
end

function [ret] = M(l, m, n, R)

d = (m==0);
if (abs(n)==l)
    denom = (2*l)*(2*l-1);
else
    denom = (l*l-n*n);
end

u = sqrt((l*l-m*m)/denom);

v = sqrt((1+d)*(l+abs(m)-1)*(l+abs(m))/denom)*(1-2*d)*0.5;
w = sqrt((l-abs(m)-1)*(l-abs(m))/denom)*(1-d)*(-0.5);

if (u~=0)
    u = u*U(l,m,n,R);
end

if (v~=0)
    v = v*V(l,m,n,R);
end

if (w~=0)
    w = w*W(l,m,n,R);
end

ret = u+v+w;

function [ret] = U(l,m,n,R)

ret = P(0,l,m,n,R);

function [ret] = P(i,l,a,b,R)

ri1 = R{2}(i+2,1+2);
rim1 = R{2}(i+2,-1+2);
ri0 = R{2}(i+2,0+2);

if (b==-l)
    ret = ri1*R{l}(a+l,1) + rim1*R{l}(a+l, 2*l-1);
else
    if (b==l)
        ret = ri1*R{l}(a+l,2*l-1) - rim1*R{l}(a+l, 1);
    else
        ret = ri0*R{l}(a+l,b+l);
    end
end

function [ret] = V(l,m,n,R)
if (m==0)
    p0 = P(1,l,1,n,R);
    p1 = P(-1,l,-1,n,R);
    ret = p0+p1;
else
    if (m>0)
        d = (m==1);
        p0 = P(1,l,m-1,n,R);
        p1 = P(-1,l,-m+1,n,R);
        ret = p0*sqrt(1+d) - p1*(1-d);
    else
        d = (m==-1);
        p0 = P(1,l,m+1,n,R);
        p1 = P(-1,l,-m-1,n,R);
        ret = p0*(1-d) + p1*sqrt(1+d);
    end
end

function [ret] = W(l,m,n,R)
if (m==0)
    error('never gets called')
else
    if (m>0)
        p0 = P(1,l,m+1,n,R);
        p1 = P(-1,l,-m-1,n,R);
        ret = p0 + p1;
    else
        p0 = P(1,l,m-1,n,R);
        p1 = P(-1,l,-m+1,n,R);
        ret = p0 - p1;
    end
end


function [rfrac,isonfly] = center_of_rotation2(trx,fly,debug)
if ~exist('debug','var')
  debug = false;
end
N = 100;

% note that if dtheta = 0, this will produce 0

cost = cos(trx(fly).theta_mm);
sint = sin(trx(fly).theta_mm);
dacost = 2*diff(trx(fly).a_mm.*cost);
dbcost = 2*diff(trx(fly).b_mm.*cost);
dasint = 2*diff(trx(fly).a_mm.*sint);
dbsint = 2*diff(trx(fly).b_mm.*sint);
Z = dacost .* dbcost + dbsint.*dasint;
Minv = zeros(2,2,trx(fly).nframes-1);
Minv(1,1,:) = dbcost ./ Z;
Minv(1,2,:) = dbsint ./ Z;
Minv(2,1,:) = -dasint ./ Z;
Minv(2,2,:) = dacost ./ Z;
Minv = reshape(Minv,[4,trx(fly).nframes-1]);
x = [ diff(trx(fly).x_mm) ; diff(trx(fly).y_mm) ];
rfrac = -[Minv(1,:) .* x(1,:) + Minv(3,:) .* x(2,:);...
  Minv(2,:) .* x(1,:) + Minv(4,:) .* x(2,:)];
% when no rotation, set center of rotation to middle of fly by default
rfrac(isnan(rfrac)) = 0;
rfrac0 = rfrac;
isoutofbounds = sum(rfrac.^2,1) > 1;
psi = linspace(0,2*pi,N)';
cospsi = cos(psi);
sinpsi = sin(psi);
idx = find(isoutofbounds);
nout = length(idx);
if nout > 0,
  x1 = repmat(trx(fly).x_mm(idx),[N,1]) + repmat(2*trx(fly).a_mm(idx).*cost(idx),[N,1]).*repmat(cospsi,[1,nout]) - ...
    repmat(2*trx(fly).b_mm(idx).*sint(idx),[N,1]).*repmat(sinpsi,[1,nout]);
  y1 = repmat(trx(fly).y_mm(idx),[N,1]) + repmat(2*trx(fly).a_mm(idx).*sint(idx),[N,1]).*repmat(cospsi,[1,nout]) + ...
    repmat(2*trx(fly).b_mm(idx).*cost(idx),[N,1]).*repmat(sinpsi,[1,nout]);
  x2 = repmat(trx(fly).x_mm(idx+1),[N,1]) + repmat(2*trx(fly).a_mm(idx+1).*cost(idx+1),[N,1]).*repmat(cospsi,[1,nout]) - ...
    repmat(2*trx(fly).b_mm(idx+1).*sint(idx+1),[N,1]).*repmat(sinpsi,[1,nout]);
  y2 = repmat(trx(fly).y_mm(idx+1),[N,1]) + repmat(2*trx(fly).a_mm(idx+1).*sint(idx+1),[N,1]).*repmat(cospsi,[1,nout]) + ...
    repmat(2*trx(fly).b_mm(idx+1).*cost(idx+1),[N,1]).*repmat(sinpsi,[1,nout]);
  d = (x1 - x2).^2 + (y1 - y2).^2;
  [~,j] = min(d,[],1);
  rfrac(1,idx) = cospsi(j);
  rfrac(2,idx) = sinpsi(j);
end
isonfly = ~isoutofbounds;

if debug,
  ntry = 50;
  psitry = linspace(0,2*pi,ntry+2);
  psitry = psitry(2:end-1);
  [rhotry,psitry] = meshgrid(linspace(0,1,ntry),psitry);
  rtry = [rhotry(:).*cos(psitry(:)),rhotry(:).*sin(psitry(:))]';
  rtry = [rtry,[0;0]];
  colorsplot = 'rg';
  colorsplot2 = 'mc';
  [~,order] = sort(-abs(modrange(diff(trx(fly).theta_mm),-pi,pi)));
  for i = order,
    u = [0,0]; v = [0,0];
    uout = [0,0]; vout = [0,0];
    utry = zeros(2,length(rtry)); vtry = zeros(2,length(rtry));
    clf; hold on;
    for j = [i,i+1],
      u(j-i+1) = trx(fly).x_mm(j) + rfrac(1,i)*trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)) - rfrac(2,i)*trx(fly).b_mm(j)*2*sin(trx(fly).theta_mm(j));
      v(j-i+1) = trx(fly).y_mm(j) + rfrac(1,i)*trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)) + rfrac(2,i)*trx(fly).b_mm(j)*2*cos(trx(fly).theta_mm(j));
      uout(j-i+1) = trx(fly).x_mm(j) + rfrac0(1,i)*trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)) - rfrac0(2,i)*trx(fly).b_mm(j)*2*sin(trx(fly).theta_mm(j));
      vout(j-i+1) = trx(fly).y_mm(j) + rfrac0(1,i)*trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)) + rfrac0(2,i)*trx(fly).b_mm(j)*2*cos(trx(fly).theta_mm(j));
      utry(j-i+1,:) = trx(fly).x_mm(j) + rtry(1,:)*trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)) - rtry(2,:)*trx(fly).b_mm(j)*2*sin(trx(fly).theta_mm(j));
      vtry(j-i+1,:) = trx(fly).y_mm(j) + rtry(1,:)*trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)) + rtry(2,:)*trx(fly).b_mm(j)*2*cos(trx(fly).theta_mm(j));
      ellipsedraw(trx(fly).a_mm(j)*2,trx(fly).b_mm(j)*2,trx(fly).x_mm(j),trx(fly).y_mm(j),trx(fly).theta_mm(j),colorsplot(j-i+1));
      plot(trx(fly).x_mm(j)+trx(fly).a_mm(j)*2*cos(trx(fly).theta_mm(j)),trx(fly).y_mm(j)+trx(fly).a_mm(j)*2*sin(trx(fly).theta_mm(j)),'x','color',colorsplot(j-i+1));
      htest(j-i+1) = plot([trx(fly).x_mm(j),u(j-i+1),uout(j-i+1)],[trx(fly).y_mm(j),v(j-i+1),vout(j-i+1)],'o-','color',colorsplot(j-i+1),'markerfacecolor',colorsplot(j-i+1)); %#ok<AGROW>
    end
    d = (utry(1,:)-utry(2,:)).^2 + (vtry(1,:)-vtry(2,:)).^2;
    [dexp,k] = min(d);
    uexp = utry(:,k);
    vexp = vtry(:,k);
    dexp = sqrt(dexp);
    dtest = sqrt(diff(u).^2 + diff(v).^2);
    dout = sqrt(diff(uout).^2 + diff(vout).^2);
    plot(uexp,vexp,'k.-');
    plot(u,v,'b.-');
    plot(uout,vout,'b.-');
    for j = [i,i+1],
      hexp(j-i+1) = plot([uexp(j-i+1),trx(fly).x_mm(j)],[vexp(j-i+1),trx(fly).y_mm(j)],'o-','color',colorsplot2(j-i+1),'markerfacecolor',colorsplot2(j-i+1)); %#ok<AGROW>
    end
    ax = nan(1,4);
    ax1 = nan(1,4);
    for j = [i,i+1],
      [ax1(1),ax1(2),ax1(3),ax1(4)] = ellipse_to_bounding_box(trx(fly).x_mm(j),trx(fly).y_mm(j),2*trx(fly).a_mm(j),2*trx(fly).b_mm(j),trx(fly).theta_mm(j));
      ax([1,3]) = min(ax([1,3]),ax1([1,3]));
      ax([2,4]) = max(ax([2,4]),ax1([2,4]));
    end
    ax = ax + [-3,3,-3,3];
    legend([htest,hexp],'analytic1','analytic2','empirical1','empirical2');
    title(sprintf('red = frame %d, green = frame %d, dtest = %f, dout = %f, dexp = %f',i,i+1,dtest,dout,dexp));
    axis equal
    axis(ax)
    input('');
  end
end


function [An Bn] = mieLayeredTerms(mu, epsilon, radius, isPEC, frequency, nmax)

% Compute the sphere coefficients An and Bn for a sphere having a number
% of homogeneous dielectric layers, with or without a perfectly
% electrically conducting (PEC) core. Follows the treatment in
% Chapter 3 of
%
% Ruck, et. al. "Radar Cross Section Handbook", Plenum Press, 1970.
%
% The incident electric field is in the -z direction (theta = 0) and is
% theta-polarized. The time-harmonic convention exp(jwt) is assumed, and
% the Green's function is of the form exp(-jkr)/r.
%
% Inputs:
%   mu: Relative complex permeability for each dielectric region
%   epsilon: Relative complex permittivity for each dielectric region
%       (the unbounded (free space) region should be the first entry)
%   radius: Radius of each spherical dielectric interface, from outermost
%           to innermost
%   isPEC: A flag for when the innermost sphere is conducting
%   frequency: Operating frequency (Hz)
%   nmax: Maximum mode for computing Bessel functions
% Outputs:
%   An: Array of Mie solution constants (used in mieScatteredField)
%   Bn: Array of Mie solution constants (used in mieScatteredField)
%
%   Author: Walton C. Gibson, email: kalla@tripoint.org

% speed of light
c = 299792458.0;

% radian frequency
w = 2.0*pi*frequency;

% wavenumber
k0 = w/c;

% total number of dielectric interfaces
numInterfaces = length(mu);

% free space impedance
eta0 = 376.7303134617707;

% impedance of each dielectric region
eta = eta0 * sqrt(mu./epsilon);

% scale factor
m = sqrt(mu.*epsilon);

% mode numbers
mode = 1:nmax;

Z = zeros(numInterfaces, length(mode));
Y = zeros(numInterfaces, length(mode));

numIFC = numInterfaces - 1;

for L = numIFC:-1:1

    if L == numIFC
        if isPEC == 0
            x = k0 * m(L + 1) * radius(L);
            [J JP] = SphericalBessel_JJP(mode, x);
            % Ruck, et. al. (3.4-23)
            P = (J ./ JP);
            % Ruck, et. al. (3.4-26), at innermost interface
            Z(L,:) = eta(L + 1) * P;
            % Ruck, et. al. (3.4-26), at innermost interface
            Y(L,:) = P / eta(L + 1);
        end
    else
        x1 = k0 * m(L + 1) * radius(L);
        x2 = k0 * m(L + 1) * radius(L + 1);
        [J1 JP1] = SphericalBessel_JJP(mode, x1);
        [H1 HP1] = SphericalHankel_HHP(mode, 2, x1);
        [J2 JP2] = SphericalBessel_JJP(mode, x2);
        [H2 HP2] = SphericalHankel_HHP(mode, 2, x2);
        % Ruck, et. al. (3.4-24)
        U = JP2 .* HP1 ./ (JP1 .* HP2);
        % Ruck, et. al. (3.4-25)
        V = (J2 .* H1) ./ (J1 .* H2);
        % Ruck, et. al. (3.4-23)
        P1 = J1 ./ JP1;
        % Ruck, et. al. (3.4-23)
        P2 = J2 ./ JP2;
        % Ruck, et. al. (3.4-23)
        Q2 = H2 ./ HP2;
        if (L == numIFC - 1) && (isPEC == 1)
            % Ruck, et. al. (3.4-27) for PEC boundary condition
            Z(L,:) = eta(L + 1) * P1 .* (1.0 - V) ./ (1.0 - U .*  P2 ./ Q2);
            % Ruck, et. al. (3.4-28) for PEC boundary condition
            Y(L,:) = (P1 / eta(L + 1)) .* (1.0 - V .*  Q2 ./ P2) ./ (1.0 - U);
        else
            % Ruck, et. al. (3.4-27)
            Z(L,:) = eta(L + 1) * P1 .* (1.0 - V .*((1.0 - Z(L+1,:)./(eta(L+1)*P2))./(1.0 - Z(L+1,:)./(eta(L+1)*Q2)))) ./ ...
                (1.0 - U .* ((1.0 - eta(L+1)*P2./Z(L+1,:))./(1.0 - eta(L+1)*Q2./Z(L+1,:))));
            % Ruck, et. al. (3.4-28)
            Y(L,:) = (P1 / eta(L + 1)) .* (1.0 - V .*((1.0 - eta(L+1)* Y(L+1,:)./ P2)./(1.0 - eta(L+1)*Y(L+1,:)./Q2))) ./ ...
                (1.0 - U .* ((1.0 - P2./(eta(L+1)*Y(L+1,:)))./(1.0 - Q2./(eta(L + 1)*Y(L+1,:)))));
        end
    end

end

% Ruck, et. al. (3.4-29)
Zn = i*Z(1,:)/eta0;
% Ruck, et. al. (3.4-29)
Yn = i*eta0*Y(1,:);

x = k0*radius(1);
[J JP] = SphericalBessel_JJP(mode, x);
[H HP] = SphericalHankel_HHP(mode, 2, x);

% Ruck, et. al. (3.4-1)
An = -((i).^(mode)) .* (2*mode + 1) ./ (mode.*(mode + 1)) .* (J + i*Zn .* JP) ./ (H + i*Zn .* HP);

% Ruck, et. al. (3.4-2) - there is an error in Ruck which is fixed here
Bn = ((i).^(mode+1)) .* (2*mode + 1) ./ (mode.*(mode + 1)) .* (J + i*Yn .* JP) ./ (H + i*Yn .* HP);

    function [J JP] = SphericalBessel_JJP(mode, x)

        % compute spherical bessel functions and their derivatives

        s = sqrt(0.5*pi/x);

        [J] = besselj(mode + 1/2, x); J = J*s;
        [J2] = besselj(mode - 1/2, x); J2 = J2*s;

        JP = (x * J2 - mode .* J);
        J = x*J;

    end

    function [H HP] = SphericalHankel_HHP(mode, arg, x)

        % compute spherical hankel functions and their derivatives

        s = sqrt(0.5*pi/x);

        [H] = besselh(mode + 1/2, arg, x); H = H*s;
        [H2] = besselh(mode - 1/2, arg, x); H2 = H2*s;

        HP = (x * H2 - mode .* H);
        H = x*H;
    end

end


function c = tapas_squared_pe_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuraton for the squared-prediction error optimization of perceptual parameters
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The squared-prediction error optimization infers the perceptual parameter values that lead to the
% best predictions of input according to the (conditional) criterion of the sum of squared
% errors. The criterion is conditional in the sense that the priors on the perceptual parameters
% retain a certain weight determined by the parameter zeta, whose prior is defined below. Zeta can
% be interpreted as an inverse weight on the prediction errors: greater values of zeta lead to less
% influence of prediction errors as opposed to priors. One would usually leave zeta fixed.
%
% Usage:
%     tapas_fitModel([], inputs, '<perceptual_model>', 'tapas_squared_pe_config', ...)
%
% Note that the first argument (responses) is empty.
%
% --------------------------------------------------------------------------------------------------
% 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/>.

% Config structure
c = struct;

% Model name
c.model = 'tapas_gaussian_obs';

% Sufficient statistics of Gaussian parameter priors
%
% Zeta
c.logzemu = log(0.05);
c.logzesa = 0;

% Gather prior settings in vectors
c.priormus = [
    c.logzemu,...
         ];

c.priorsas = [
    c.logzesa,...
         ];

% Model filehandle
c.obs_fun = @tapas_squared_pe;

% Handle to function that transforms observation parameters to their native space
% from the space they are estimated in
c.transp_obs_fun = @tapas_squared_pe_transp;

return;


function x = box_behnken ( dim_num, x_num, range )

%*****************************************************************************80
%
%% BOX_BEHNKEN returns a Box-Behnken design for the given number of factors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 October 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    George Box, Donald Behnken,
%    Some new three level designs for the study of quantitative variables,
%    Technometrics,
%    Volume 2, pages 455-475, 1960.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer X_NUM, the number of elements of the design.
%    X_NUM should be equal to DIM_NUM * 2**(DIM_NUM-1) + 1.
%
%    Input, real RANGE(DIM_NUM,2), the minimum and maximum
%    value for each component.
%
%    Output, real X(DIM_NUM,X_NUM), the elements of the design.
%

%
%  Ensure that the range is legal.
%
  if ( any ( range(1:dim_num,2) <= range(1:dim_num,1) ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'BOX_BEHNKEN - Fatal error!\n' );
    fprintf ( 1, '  For some index I,\n' );
    fprintf ( 1, '  RANGE(I,2) <= RANGE(I,1).\n' );
    error ( 'BOX_BEHNKEN - Fatal error!' );
  end
%
%  The first point is the center.
%
  j = 1;

  x(1:dim_num,j) = ( range(1:dim_num,1) + range(1:dim_num,2) ) / 2.0;
%
%  For subsequent elements, one entry is fixed at the middle of the range.
%  The others are set to either extreme.
%
  for i = 1 : dim_num

    j = j + 1;

    x(1:dim_num,j) = range(1:dim_num,1);
    x(i,j) = ( range(i,1) + range(i,2) ) / 2.0;
%
%  The next element is made by finding the last low value, making it
%  high, and all subsequent high values low.
%
    while ( 1 )

      last_low = -1;

      for i2 = 1 : dim_num
        if ( x(i2,j) == range(i2,1) )
          last_low = i2;
        end
      end

      if ( last_low == -1 )
        break
      end

      j = j + 1;
      x(1:dim_num,j) = x(1:dim_num,j-1);
      x(last_low,j) = range(last_low,2);

      for i2 = last_low + 1 : dim_num
        if ( x(i2,j) == range(i2,2) )
          x(i2,j) = range(i2,1);
        end
      end

    end

  end

  return
end


function r8sp_print ( m, n, nz_num, row, col, a, title )

%*****************************************************************************80
%
%% R8SP_PRINT prints a R8SP matrix.
%
%  Discussion:
%
%    This version of R8SP_PRINT has been specifically modified to allow,
%    and correctly handle, the case in which a single matrix location
%    A(I,J) is referenced more than once by the sparse matrix structure.
%    In such cases, the routine prints out the sum of all the values.
%
%    The R8SP storage format stores the row, column and value of each nonzero
%    entry of a sparse matrix.
%
%    It is possible that a pair of indices (I,J) may occur more than
%    once.  Presumably, in this case, the intent is that the actual value
%    of A(I,J) is the sum of all such entries.  This is not a good thing
%    to do, but I seem to have come across this in MATLAB.
%
%    The R8SP format is used by CSPARSE ("sparse triplet"), DLAP/SLAP
%    ("nonsymmetric SLAP triad"), by MATLAB, and by SPARSEKIT ("COO" format).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, integer NZ_NUM, the number of nonzero elements in the matrix.
%
%    Input, integer ROW(NZ_NUM), COL(NZ_NUM), the row and column indices
%    of the nonzero elements.
%
%    Input, real A(NZ_NUM), the nonzero elements of the matrix.
%
%    Input, string TITLE, a title.
%
  r8sp_print_some ( m, n, nz_num, row, col, a, 1, 1, m, n, title );

  return
end


function geometry_test0366 ( )

%*****************************************************************************80
%
%% TEST0366 tests SEGMENT_POINT_DIST_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  test_num = 3;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST0366\n' );
  fprintf ( 1, '  SEGMENT_POINT_DIST_3D computes the distance\n' );
  fprintf ( 1, '    between a line segment and point in 3D.\n' );

  for test = 1 : test_num

    [ p1(1:3,1), seed ] = r8vec_uniform_01 ( 3, seed );
    [ p2(1:3,1), seed ] = r8vec_uniform_01 ( 3, seed );
    [ p(1:3,1), seed ] = r8vec_uniform_01 ( 3, seed );

    dist = segment_point_dist_3d ( p1, p2, p );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  TEST = %d', test );
    fprintf ( 1, '  P1 =   %12f  %12f  %12f\n', p1(1:3,1) );
    fprintf ( 1, '  P2 =   %12f  %12f  %12f\n', p2(1:3,1) );
    fprintf ( 1, '  P =    %12f  %12f  %12f\n', p(1:3,1) );
    fprintf ( 1, '  DIST = %12f\n', dist );

  end

  return
end


function lm = gallager_ach(n, epsil, P)
% returns log M achievable by gallager's random coding.
%
% Note: In the case of uncostrained input gallager's bound can be written as
%
%  P_e <= M^s   V(s)^n
%
% Where V(s) = (   Sum_y [ Sum_x Q(x) W(y|x)^{1/1+s} ]^(1+s)   )
%
% Thus we could rewrite the bound as a lower bound on $M$ directly.
%
% For AWGN we can do the same in the case when we do not restrict codewords to a narrow shell
% around the power sphere. I did that and here is the result:
%
%   gallager_ach(3000, 1e-6, 1) = 1225
%   -betaq_low_v2(1-1e-6, 3000, 1) = 1287
%
%   new_gallager() = 1206 --- much worse than good gallager!
%
% Here's how to compute new_gallager:
% mu = chi2cdf(n,n);
% f = @(x) -( (1+x)./x * log2(mu) + n/2 .* log2( 1 + A^2 ./ (1+x)) + log2(epsil/2) ./ x )
% [x_best f_best] = fminbnd(f, 0, 1); new_gal = -f - 1;
%
% Note epsil/2 and also that we should subtract one bit.

R_up = cap_awgn(P);
R_down = 0;

% Check if for this n gallager works at all
Pe = gallager_pe(P, R_down, n);
if Pe > epsil;
    lm = 0;
    return;
end

precision = 1e-3 * R_up / n;

while 1;
	if ((R_up - R_down) < precision)
		break;
	end
	R = (R_up + R_down) / 2;

	Pe = gallager_pe(P, R, n);
	if( Pe < epsil)
		R_down = R;
	else
		R_up = R;
	end
end

lm = n * R_down;

function [pe Er] = gallager_pe(P, R, n, deltap_force)
%
% This function returns upper bound (achievability) on P_e using
% Gallager's random coding error exponents
%
% TODO: use expurgated exponent for low rates!
%

% conversion A->P. Old versions are all in terms of ``amplitude'' A.
A = sqrt(P);

if (nargin < 4)
	deltap_force = [];
end

if (R == 0)
	pe = 0;
	return;
end

Rcr = 1/2 * log2( 1/2 + A^2/4 + 1/2 * sqrt(1+A^4/4) );

% convert rate to nats
R = R / log2(exp(1));
Rcrn = Rcr / log2(exp(1));

if (R >= Rcrn)
	beta = exp(2*R);
	ro = A^2/(2*beta) * (1 + sqrt(1+ 4*beta/A^2/(beta-1) ) ) - 1;

	Er = A^2/(4*beta) * ...
		( (beta+1) - (beta-1)*sqrt(1 + 4*beta/A^2/(beta-1)) ) + ...
		1/2 * log(  beta - A^2*(beta-1)/2 * ( sqrt(1 + 4*beta/A^2/(beta-1)) - 1 ) );
else
	beta = 1/2 * (1 + A^2/2 + sqrt(1+A^4/4) );
	ro = 1;

	Er = 1 - beta + A^2/2 + 1/2 * log(beta - A^2/2) + 1/2*log(beta) - R;
end;

s = ro*A^2/(2 * (1+ro)^2 * beta);

%%
%% Now instead of maximizing over delta's we choose delta = 1/s  (see Gallager 7.4.39)
%% This is suboptimal, but let it be...
%%

if (max(size(deltap_force)) > 0)
	deltap = deltap_force * n;
else
	if (s == 0)
		deltap = n;
	else
		deltap = min(1/s, n);
	end;
end

%disp(sprintf('ro = %g, deltap = %g, s = %g', ro, deltap, s));

ccdfs = chi2cdf([n-deltap n], n);

mu = ccdfs(2) - ccdfs(1);
if (mu < 1e-10 * ccdfs(1))
	mu_new = quad(@(x)( chi2pdf(x, n) ), n - deltap, n);
	disp(sprintf( [	'--- gallager_pe: computing mu using quad()\n' ...
			'    `--> mu_new = %.3g, mu_old = %.3g (diff = %.2f%%)'], ...
			mu_new, mu, 200*abs(mu_new - mu)/(mu_new + mu)));
	mu = mu_new;
end;

multip = 2*exp(s*deltap)/mu;
multip_approx = ro*A^2*exp(1)*sqrt(4*pi*n)/(1+ro)^2 /beta;

%	disp(sprintf( [	'--- gallager_pe: multiplier comparsion\n' ...
%			'    `--> multip = %.3g, approx = %.3g (diff = %.2f%%)'], ...
%			multip, multip_approx, 100*abs(multip - multip_approx)/(multip)));

pe = multip*exp(-n*Er);

if(pe > 1) pe = 1; end;


function out = DN_Quantile(y,p)
% DN_Quantile   Quantile of the data vector
%
% Calculates the quantile value at a specified proportion, p, using the
% Statistics Toolbox function, quantile.
%
%---INPUTS:
% y, the input data vector
% p, the quantile proportion

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

if nargin < 2
    fprintf(1,'Using quantile p = 0.5 (median) by default\n');
    p = 0.5;
end
if ~isnumeric(p) || (p < 0) || (p > 1)
    error('p must specify a proportion, in (0,1)');
end

out = quantile(y,p);

end


function [strings, perm] = vl_alphanum(strings)
% VL_ALPHANUM  Sort strings using the Alphanum algorithm
%   STRINGS = VL_ALPHANUM(STRINGS) sorts the cell array of strings
%   STRINGS by using the Alphanum algorithm [1]. [STRINGS,PERM] =
%   VL_ALPHANUM(...) returns the corresponding permutation PERM as
%   well.
%
%   Example::
%     Alphanum sorts strings in a way that 'makes sense'. For instance
%
%      strings = {'B1', 'B2', 'B12', 'A12', 'A1', 'A2'} ;
%      sorted = vl_alphanum(strings) ;
%
%     produces the sorted array {'A1', 'A2', 'A12', 'B1', B2',
%     'B12'}. By contrast, SORT() produces the array {'A1', 'A12',
%     'A2', 'B1', B12', 'B2'} (note the position of the elements
%     'A12', 'B12').
%
%   References:
%   [1] Dave Koelle, 'The Alphanum Algorithm',
%   http://www.davekoelle.com/alphanum.html

chunks = regexp(strings, '(\d+|\D+)', 'tokens') ;
for i = 1:length(strings)
  chunks{i} = [chunks{i}{:}] ;
  for j = 1:length(chunks{i})
    if isstrprop(chunks{i}{j},'digit')
      chunks{i}{j} = sprintf('%020.0f',sscanf(chunks{i}{j},'%d')) ;
    end
  end
  chunks{i} = [chunks{i}{:}] ;
end

[dorp,perm] = sort(chunks) ;
strings = strings(perm) ;


function test_suite = test_mdwt
initTestSuite;

function test_mdwt_1D
  x = makesig('LinChirp', 8);
  h = daubcqf(4, 'min');
  L = 2;  % For 8 values in x we would normally be L=2
  [y, L] = mdwt(x, h, L);
  y_corr = [1.1097 0.8767 0.8204 -0.5201 -0.0339 0.1001 0.2201 -0.1401];
  L_corr = 2;
assertVectorsAlmostEqual(y, y_corr, 'relative', 0.001);
assertEqual(L, L_corr);

function test_mdwt_2D
  x = [1 2 3 4; 5 6 7 8 ; 9 10 11 12; 13 14 15 16];
  h = daubcqf(4);
  y = mdwt(x, h);
  y_corr = [34.0000 -3.4641 0.0000 -2.0000; -13.8564 0.0000 0.0000 -2.0000; -0.0000 0.0000 -0.0000 -0.0000; -8.0000 -8.0000 0.0000 -0.0000];
assertVectorsAlmostEqual(y, y_corr, 'relative', 0.001);

function test_mdwt_compute_L1
  x = [1 2];
  h = daubcqf(4, 'min');
  [y, L] = mdwt(x, h);
assertEqual(L, 1);

function test_mdwt_compute_L2
  x = [1 2 3 4];
  h = daubcqf(4, 'min');
  [y, L] = mdwt(x, h);
assertEqual(L, 2);

function test_mdwt_compute_L3
  x = [1 2 3 4 5 6 7 8];
  h = daubcqf(4, 'min');
  [y, L] = mdwt(x, h);
assertEqual(L, 3);

function test_mdwt_compute_bad_L
  L = -1;
  x = [1 2 3 4 5 6 7 8 9];
  h = daubcqf(4, 'min');
  mdwtHandle = @() mdwt(x, h);
assertExceptionThrown(mdwtHandle, '');

function test_mdwt_empty_input
  mdwtHandle = @() mdwt([], [0 0 0 0]);
assertExceptionThrown(mdwtHandle, '');


% function acc = LRAccuracy(GroundTruth, Predictions) compares the
% vector of predictions with the vector of ground truth values,
% and returns the accuracy (fraction of predictions that are correct).
%
% Input:
% GroundTruth    (numInstances x 1 vector)
% Predictions    (numInstances x 1 vector)
%
% Output:
% err            (scalar between 0 and 1 inclusive)
%
% Copyright (C) Daphne Koller, Stanford Univerity, 2012

function acc = LRAccuracy(GroundTruth, Predictions)

    GroundTruth = GroundTruth(:);
    Predictions = Predictions(:);
    assert(all(size(GroundTruth) == size(Predictions)));

    acc = mean(GroundTruth == Predictions);

end


function [cm] = pm2cm(pm)
% Convert length from picometers to centimeters.
% Chad A. Greene 2012
cm = pm*1e-10;


function [pos vel] = DeDvKalman(z)
%
%
persistent A H Q R
persistent x P
persistent firstRun

if isempty(firstRun)
  firstRun = 1;

  dt = 0.1;

  A = [ 1 dt;
        0 1  ];
  H = [1 0];

  Q = [ 1 0;
        0 3 ];
  R = 10;

  x = [ 0 20 ]';
  P = 5*eye(2);
end

xp = A*x;
Pp = A*P*A' + Q;

K = 1 / (Pp(1,1) + R) * [Pp(1,1) Pp(2,1)]';  % Pp*H'*inv(H*Pp*H' + R);

x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;

pos = x(1);
vel = x(2);


function y=jinc(r)
% y=jinc(r) returns the jinc function [J1(2*pi*r)/(2*pi*r)] evaluated
% at r. Per this definition, the first zero of jinc function is at 0.61.

y=real(2*besselj(1,2*pi*r)./ (2*pi*r));
%Imaginary parts due to numerical error are of order 1E-16.

%Division by zero leads to NaN.
y(isnan(y))=1;

end


function Gimg = gaborConvF(img,G,gWinLen)
%GIMG = GABORCONV(IMG,G,GWINLEN) Computes Gabor transform using FFT
%   G is the generated FFTed Gabor kernels using GENGABORKERNELF, GIMG will
%   be a cell the same size w/ G. GWINLEN is the radius of the origin kernel
%	Yan Ke @ THUEE, xjed09@gmail.com

[scale_num angle_num] = size(G);
Gimg = cell(scale_num,angle_num);
[fftM fftN] = size(G{1});
[imgM imgN] = size(img);

f = padarray(img,[gWinLen gWinLen],'rep');
fimg = fft2(f,fftM,fftN);
for r = 1:scale_num
	for s = 1:angle_num
		filtered = ifft2(fimg.*G{r,s}); % conv in freq domain
		cropped = filtered(gWinLen*2+(1:imgM),gWinLen*2+(1:imgN));
		Gimg{r,s} = abs(cropped); % use magnitude
	end
end

end


function c = mvnvbcost(x,Covx,mu,Covmu,pCholCov,pLogDetCov)
% c = mvnvbcost(x,Covx,mu,Covmu,pCholCov,pLogDetCov)
%
% Calculates c = -KL(q||p) = <log p(X)> - <log q(X)>
% where the expectation <.> is taken over q(X). This is used for
% calculating the lower bound of the marginal loglikelihood in VB
% models.
%
% p(X) = N(M,T)
% q(X) = N(x,Covx)
%
% Rest of the parameters are defined as:
% q(M) = N(mu,Covmu)
% <inv T> = inv(pCholCov * pCholCov')
% <log det T> = pLogDetCov
%
% If Covx==[], then the term <log q(X)> is not calculated.
% This is useful when X is, e.g., observations.

c = 0;
d = length(x);

% Cost from q-posterior
if ~isempty(Covx)
  % TODO:
  % if ~isempty(pCholCov), entropy = mvnentropy(pCholCov,true); else.... end
%  entropy = mvnentropy(pCholCov,true);
  entropy = mvnentropy(Covx);
  c = entropy;
else
  Covx = 0;
end

% Use Cholesky of the prior covariance
% $$$ opts.UT = true;
% $$$ opts.TRANSA = true;
z = solve_tril(pCholCov, x-mu);

% Cost from prior

% Below: (x-mu)' * Cov^(-1) * (x-mu) + trace(Cov^(-1)*(Covx+Covmu))
err2 = z'*z + trace(solve_triu(pCholCov',solve_tril(pCholCov,Covx+Covmu)));
%err2 = (x-mu)'*pInvCov*(x-mu) + (Covx(:)+Covmu(:))'*pInvCov(:);
%((x-mu).^2 + diag(Covx) + diag(Covmu));
c = c - 0.5*pLogDetCov - 0.5*err2 - 0.5*d*log(2*pi);


function d = det3x3(x)

% DET3X3 computes determinant of matrix x, using explicit analytic definition
% if size(x) = [3 3 K M]

% Copyright (C) 2017, Donders Centre for Cognitive Neuroimaging, Nijmegen, NL
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

siz = size(x);
if all(siz(1:2)==3)
  d = x(1,1,:,:).*x(2,2,:,:).*x(3,3,:,:) - ...
      x(1,1,:,:).*x(2,3,:,:).*x(3,2,:,:) - ...
      x(1,2,:,:).*x(2,1,:,:).*x(3,3,:,:) + ...
      x(1,2,:,:).*x(2,3,:,:).*x(3,1,:,:) + ...
      x(1,3,:,:).*x(2,1,:,:).*x(3,2,:,:) - ...
      x(1,3,:,:).*x(2,2,:,:).*x(3,1,:,:);
else
  ft_error('not implemented');
  % write for loop for the higher dimensions, using normal inv
end


% Author:
% - Mehrtash Harandi (mehrtash.harandi at gmail dot com)
%
% This file is provided without any warranty of
% fitness for any purpose. You can redistribute
% this file and/or modify it under the terms of
% the GNU General Public License (GPL) as published
% by the Free Software Foundation, either version 3
% of the License or (at your option) any later version.

function outS = Stein_Divergence(X1,X2)
l1 = size(X1,3);
l2 = size(X2,3);

outS = zeros(l1,l2);
for tmpC1 = 1:l1
    for tmpC2 = 1:l2
        X = X1(:,:,tmpC1);
        Y = X2(:,:,tmpC2);

        %outS(tmpC1,tmpC2) = log(det(0.5*(X+Y))) -   0.5*log(det(X*Y)); % Comment outed by HK

        S = log(det(0.5*(X+Y))) -   0.5*log(det(X*Y));

        real_flag = isreal(S);
        if real_flag
            outS(tmpC1,tmpC2) = S;
        else
            outS(tmpC1,tmpC2) = real(S);
        end

        if  (outS(tmpC1,tmpC2) < 1e-10)
            outS(tmpC1,tmpC2) = 0.0;
        end
    end
end


function unitCell = calcUnitCell(xy,varargin)
% compute the unit cell for an EBSD data set
%
% Input
%  xy - spatial coordinates
%
% Output
%  unitCell - coordinates of the unit cell
%
% Options
%
% GridType - [automatic, hexagonal, rectangular]

% nothing to do -> skip
if isempty(xy)
  unitCell = [];
  return;
end

if size(xy,2) == 3
  unitCell = [calcUnitCell(xy(:,[1 2]), varargin{:});...
    calcUnitCell(xy(:,[1 3]), varargin{:}); ...
    calcUnitCell(xy(:,[2 3]), varargin{:})];
  return
end

% first estimate of the grid resolution
area = (max(xy(:,1))-min(xy(:,1)))*(max(xy(:,2))-min(xy(:,2)));
dxy = sqrt(area / length(xy));

% compensate for single line EBSD
if dxy==0
  lx = mean(diff(xy(:,1))); ly = mean(diff(xy(:,2)));
  if lx==0, lx=ly; else; ly=lx; end
  dxy= (ly+ly)/2;
end

% reduce data set
if length(xy)>10000, xy = subSample(xy,10000); end

% remove dublicates from the coordinates
xy = uniquetol(xy,0.01/sqrt(size(xy,1)),'ByRows',true);

try
  % compute Voronoi decomposition
  [v, c] = voronoin(xy,{'Qz'});

  % compute the area of all Voronoi cells
  areaf = @(x,y) abs(0.5.*sum(x(1:end-1).*y(2:end)-x(2:end).*y(1:end-1)));
  areaf = cellfun(@(c1) areaf(v([c1 c1(1)],1),v([c1 c1(1)],2)),c);

  % the unit cell should be the Voronoi cell with the smalles area
  [~, ci] = min(areaf);

  % compute vertices of the unit cell
  unitCell = [v(c{ci},1) - xy(ci,1),v(c{ci},2) - xy(ci,2)];
  % sometimes it happens that we have one point doubled, remove those
  ignore = [false;sqrt(sum(diff(unitCell,1).^2,2)) < max(sqrt(sum(diff(unitCell,1).^2,2)))/5];
  unitCell(ignore,:) = [];

  if isRegularPoly(unitCell,varargin)
    return
  end

  % second estimate of the grid resolution
  dxy2 = max(unitCell) - min(unitCell);

  if 100*dxy2 > dxy, dxy = dxy2;end

catch %#ok<CTCH>
end

% get grid options
dxy = get_option(varargin,'GridResolution',dxy);
cellType = get_option(varargin,'GridType','rectangular');
cellRot = get_option(varargin,'GridRotation',0*degree);

% otherwise take a regular unit cell
switch lower(cellType)

  case 'rectangular'

    unitCell = regularPoly(4,dxy,cellRot);

  case 'hexagonal'

    unitCell = regularPoly(6,dxy,cellRot);

  case 'circle'

    unitCell = regularPoly(16,dxy,cellRot);

  otherwise

    error('MTEX:plotspatial:UnitCell','Unknown unit cell type!')
end

% a regular polygon with s vertices, diameter d, and rotation rot
function unitCell = regularPoly(s,d,rot)

c = exp(1i*((pi/s:pi/(s/2):2*pi)+rot))./sqrt((s/2));
unitCell = [real(c(:)),imag(c(:))].*d;

function isRegular = isRegularPoly(unitCell,varargin)

sideLength = sqrt(sum((unitCell).^2,2));
sides      = numel(sideLength);

uC = complex(unitCell(:,1),unitCell(:,2));
nC = uC([2:end 1]);

enclosingAngle = uC./nC;
enclosingAngle = complex(abs(real(enclosingAngle)),...
  abs(imag(enclosingAngle)));

isRegular = any(sides == [4 6]) && ... % norm(sideLength - mean(sideLength))*dxy < 1e-5 && ...
  norm(enclosingAngle - mean(enclosingAngle)) < 0.05*degree;

% find a quare subset of about N points
function xy = subSample(xy,N)

xminmax = [min(xy(:,1));max(xy(:,1))];
yminmax = [min(xy(:,2));max(xy(:,2))];

% shrink range until only N points are inside
while length(xy) > N

  if diff(xminmax) > diff(yminmax)
    xminmax = [3 1;1 3] * xminmax ./ 4;
  else
    yminmax = [3 1;1 3] * yminmax ./ 4;
  end

  xy = xy(xy(:,1)>xminmax(1) & xy(:,1)<xminmax(2) & ...
    xy(:,2)>yminmax(1) & xy(:,2)<yminmax(2),:);

end


function x = calcAxisDistribution(S3F,varargin)
% axis distribution function
%
% Syntax
%
%   value = calcAxisDistribution(odf, a)
%   adf = calcAxisDistribution(odf)
%
% Input
%  S3F - orientation or misorientation distribution function, @SO3Fun
%  a   - rotational axis, @vector3d
%
% Output
%  afd   - axis distribution function, @S2Fun
%  value - value of axis distribution function for rotational axis a
%
% See also
% symmetry/calcAxisDistribution

[oR,dcs,nSym] = fundamentalRegion(S3F.CS,S3F.SS,varargin{:});

if nargin == 1 || ~isa(varargin{1},'vector3d')
  adf = @(h) calcAxisDistribution(S3F,h,varargin{:});
  x = S2FunHarmonicSym.quadrature(adf,dcs,'bandwidth',64,varargin{:});
  return
end

h = varargin{1};

maxOmega = oR.maxAngle(project2FundamentalRegion(h,dcs));
res = get_option(varargin,'resolution',2.5*degree);
nOmega = round(max(maxOmega(:))/res);

% define a grid for quadrature
omega = linspace(0,1,nOmega);
omega = maxOmega(:) * omega(:).';
h = repmat(h(:),1,nOmega);
S3G = orientation.byAxisAngle(h,omega,S3F.CS,S3F.SS);

% quadrature weights
weights = sin(omega./2).^2 ./ nOmega;

% eval ODF
f = eval(S3F,S3G,varargin{:});

% sum along axes
x = 2*nSym / pi * sum(f .* weights,2) .* maxOmega(:);


function zz=lpcss2zz(ss)
%LPCSS2ZZ Convert s-place poles to z-plane poles ZZ=(SS)
%the s-plane is in units of Normalized Hz and so the imaginary part
% of each ss() value is in the range +-0.5
%
% If you multiply ss by the sample frequency, a formant with
% frequency f and bandwidth b will give an s-plane pole-pair
% of approximately -b/2 +- j f

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

zz=exp(2*pi*ss);


% vgg_selfcalib_qaffine  Upgrading projective to quasi-affine reconstruction.
%
% Given projective reconstruction [P,X] with correct signs of P and X
% (the output of vgg_signsPX_from_x), it finds homography H transforming
% [P,X] to quasi-affine reconstruction [Pq,Xq] = [P*inv(H),H*X].
% Let Ainf=[0 0 0 1] be plane at infinity, then [Pq,Xq] has the property that :-
%
%   Ainf * Xq > 0                (all scene points in front of plane at infty)
%   Ainf * vgg_wedge(Pq{k}) > 0  (all camera centers in front of plane at infty)
%
% H = vgg_selfcalib_qaffine(P,X), where
%   P ... cell(K) of double(3,4), camera matrices. K is number of cameras.
%     P also can be 3x4xK array.
%   X ... double(4,N), scene points in homog. coordinates.
%   H ... cell{I} of double(4,4), homographies upgrading [P,X] to quasi-affine reconstruction.
%     There can be 0, 1, or 2 solution classes (corresponding to I=0,1,2) :-
%       - I==0 ... no solution, ie [P,X] cannot be transformed to any affine scene.
%       - I==1 ... 1 solution, ie camera centers and scene points
%           are not separable by a plane in the true scene.
%       - I==2 ... 2 solutions, ie camera centers and scene points are separable
%           by a plane in the true scene. Then there are two solutions for plane at infinity,
%           differing by sign(det(H{i})). The two reconstruction corresponding to H{1} and H{2}
%           have oppposite handedness and we cannot say which handedness is that of the true scene.
% (Note: by 'solution' we mean rather 'class of solutions' - indeed there are infinitely many
% solutions if I>0, and linear programming chooses a single solution out of them.)
%
% EXAMPLE: Let [P,X] be a projective reconstruciton from homogeneous image points x.
% Upgrade to quasi-affine reconstruction is done as follows:
%   [P,X] = vgg_signsPX_from_x(P,X,x);
%   H = vgg_selfcalib_qaffine(P,X);
%   H = H{1}; % single solution assumed
%   P = P*inv(H);
%   X = H*X;
% If either of rows 1 and 2 returns no solution, there's something wrong with
% the reconstruction, eg an outlier.

% T.Werner, Feb 2002, werner@robots.ox.ac.uk

function H = vgg_selfcalib_qaffine(P,X)

if ndims(P)==3
  for k = 1:size(P,3)
    Q{k} = P(:,:,k);
  end
  P = Q;
end

[D N] = size(X);
K = length(P);

for k = 1:K
  C(:,k) = vgg_wedge(P{k}); % oriented camera centers
end

% Solve chiral equalities:
%
% A := found plane at infinity
% detH := required det(H)
% (A and detH can be none, one, or two according to the number of solution classes)
detH = [];
A = [];
for detHa = [-1 1]
  Aa = sephplane([X detHa*C]);
  if ~isempty(Aa)
    A = [A; Aa];
    detH = [detH; detHa];
  end
end

if isempty(A)
  H = {};
  return
end

% compose final homography H
for i = 1:size(A,1)

  % find H{i} such that H{i}(4,:)==A
  [dummy,dummy,H{i}] = svd(A(i,:),0);
  H{i} = H{i}(end:-1:1,:);

  % make det(H{i}) the same sign as detH(i)
  if det(H{i})*detH(i) < 0
    H{i} = H{i}([2 1 3 4],:);
  end

  % 'beautifier' of X:
  % Do singular value equalization on the set X,
  % i.e., make mean(nhom(H{i}*X),2)==[0;0;0] and svd(nhom(H{i}*X))==[1;1;1].
  Xi = vgg_get_nonhomg(H{i}*X);
  c = mean(Xi,2); % centroid
  Xi = Xi - c*ones(1,N);
  [U,S] = eig(Xi*Xi'); % sv equalization
  S = diag(1./sqrt(diag(S)));
  K = S*U';
  if det(K) < 0 % we want the sv equalization to be parity-preserving
    K = -K;
  end
  H{i} = [ K -K*c; 0 0 0 1 ]*H{i};

end

return

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

% A = sephplane(X)  Finds separating hyperplane A such that all(A*X)>0.
% If no solution exists, A = [].
% Works for any dimension of X.
function A = sephplane(X)

[D,N] = size(X);

X = X ./ (ones(D,1)*sqrt(sum(X.*X)));
A = [-X' ones(N,1)];
b = zeros(size(A,1),1);
f = [zeros(1,D) -1]';
LB = [-ones(1,D) 0];
UB = [ones(1,D) Inf];
fprintf('vgg selfcalib_qaffine: linprog for %d %dd pts ... ', size(X,2), D);
options = optimset('linprog');
options.Display = 'off';
[res,FVAL,EXITFLAG] = linprog(f,A,b,[],[],LB,UB, [], options);
if isempty(res)
  fprintf('no feasible plane\n');
  A = [];
  return
end
A = res(1:D)';

if ~all(A*X > 0)
  fprintf('feasible plane returned, but is not in fact feasible\n');
  A = [];
end

fprintf('Got plane [%.2f %.2f %.2f %.2f]\n', A);

return

%i = k2i(k)
% Computes indices of joint point matrix rows corresponding to views k.
%% function i = k2i(k,step)
%% k = k(:)';
%% i = [1:3]'*ones(size(k)) + 3*(ones(3,1)*k-1);
%% i = i(:);


function [Aeq, beq] = getAbeq(n_seg, n_order, ts, start_cond, end_cond)
    n_coeff = n_order+1
    n_all_poly = n_seg*(n_coeff);
    %#####################################################
    % STEP 2.1 p,v,a constraint in start
    Aeq_start = zeros(3, n_all_poly);
    beq_start = zeros(3, 1);

    d1 = n_order;
    d2 = n_order * (n_order - 1);
    Aeq_start(1:1:3, 1:1:n_coeff) = [  1,    0,  0, 0, 0, 0, 0, 0;
                                       -d1,   d1,  0, 0, 0, 0, 0, 0;
                                       d2, -2*d2, d2, 0, 0, 0, 0, 0];
    beq_start = start_cond';% p,v,a

    %#####################################################
    % STEP 2.2 p,v,a constraint in end
    Aeq_end = zeros(3, n_all_poly);
    Aeq_end = zeros(3, 1);
    Aeq_end(1:1:3, n_all_poly-n_order:1:n_all_poly) = [0, 0, 0, 0, 0,  0,     0,   1;
                                                       0, 0, 0, 0, 0,  0,   -d1,  d1;
                                                       0, 0, 0, 0, 0, d2, -2*d2,  d2];
    beq_end = end_cond';% p,v,a

    %#####################################################
    % Init array for continuity constrains
    Aeq_con_p = zeros(n_seg-1, n_all_poly);
    beq_con_p = zeros(n_seg-1, 1);
    Aeq_con_v = zeros(n_seg-1, n_all_poly);
    beq_con_v = zeros(n_seg-1, 1);
    Aeq_con_a = zeros(n_seg-1, n_all_poly);
    beq_con_a = zeros(n_seg-1, 1);

    for con_index = 1:n_seg-1
        index = n_coeff + n_coeff * (con_index - 1);

        % STEP 2.3 position continuity constrain between 2 segments
        Aeq_con_p(con_index,index) = 1;% the end of previous segment
        Aeq_con_p(con_index,index+1) = -1;% the begin of next segment

        % STEP 2.4 velocity continuity constrain between 2 segments
        Aeq_con_v(con_index,index-1:index) = [-d1, d1];% the end of previous segment
        Aeq_con_v(con_index,index+1:index+2) = -[-d1, d1];% the begin of next segment

        % STEP 2.5 acceleration continuity constrain between 2 segments
        Aeq_con_a(con_index,index-2:index) = [d2, -2*d2, d2];% the end of previous segment
        Aeq_con_a(con_index,index+1:index+3) = -[d2, -2*d2, d2];% the begin of next segment
    end

    %#####################################################
    % combine all components to form Aeq and beq
    Aeq_con = [Aeq_con_p; Aeq_con_v; Aeq_con_a];
    beq_con = [beq_con_p; beq_con_v; beq_con_a];
    Aeq = [Aeq_start; Aeq_end; Aeq_con];
    beq = [beq_start; beq_end; beq_con];
end


% clear all; close all; clc;

img1=imread('lena.jpg');
imshow(img1,[]);
[m n]=size(img1);

img=sqrt(double(img1));      %伽马校正
% figure;hold on;
figure;
a = min(min(min(img)));
b = max(max(max(img)));
% can not directly operator on a matrix more than three dimensions
c = 255/(b-a);
for i = 1:3
    img2(:,:,i) =img(:,:,i) * c;
end

imshow(uint8(img2));

%下面是求边缘
fy=[-1 0 1];        %定义竖直模板,用于计算y方向上的梯度值，向右为正方向
fx=fy';             %定义水平模板,用于计算x方向上的梯度值
Iy=imfilter(img,fy,'replicate');    %竖直边缘/梯度
Ix=imfilter(img,fx,'replicate');    %水平边缘/梯度
Ied=sqrt(Ix.^2+Iy.^2);              %边缘强度/梯度
Iphase=Iy./Ix;              %边缘斜率，有些为inf,-inf,nan，其中nan需要再处理一下

%下面是求cell
step=16;                %step*step个像素作为一个cell
orient=9;               %方向直方图的方向个数
jiao=360/orient;        %每个方向包含的角度数
Cell=cell(1,1);              %所有的角度直方图,cell是可以动态增加的，所以先设了一个
ii=1;
jj=1;
for i=1:step:m          %如果处理的m/step不是整数，最好是i=1:step:m-step， no overlapping
    ii=1;
    for j=1:step:n      %注释同上
        tmpx=Ix(i:i+step-1,j:j+step-1);
        tmped=Ied(i:i+step-1,j:j+step-1);
        tmped=tmped/sum(sum(tmped));        %局部边缘强度归一化
        tmpphase=Iphase(i:i+step-1,j:j+step-1);
        Hist=zeros(1,orient);               %当前step*step像素块统计角度直方图,就是cell
        for p=1:step
            for q=1:step
                if isnan(tmpphase(p,q))==1  %0/0会得到nan，如果像素是nan，重设为0
                    tmpphase(p,q)=0;
                end
                ang=atan(tmpphase(p,q));    %atan求的是[-90 90]度之间
                ang=mod(ang*180/pi,360);    %全部变正，[0,360), -90变270
                if tmpx(p,q)<0              %根据x方向确定真正的角度
                    if ang<90               %如果是第一象限
                        ang=ang+180;        %移到第三象限
                    end
                    if ang>270              %如果是第四象限
                        ang=ang-180;        %移到第二象限
                    end
                end
                ang=ang+0.0000001;          %防止ang为0
                Hist(ceil(ang/jiao))=Hist(ceil(ang/jiao))+tmped(p,q);   %ceil向上取整，使用边缘强度加权
            end
        end
        Hist=Hist/sum(Hist);    %方向直方图归一化
        Cell{ii,jj}=Hist;       %放入Cell中
        ii=ii+1;                %针对Cell的y坐标循环变量
    end
    jj=jj+1;                    %针对Cell的x坐标循环变量
end

%下面是求feature,2*2个cell合成一个block,没有显式的求block
[m n]=size(Cell);
feature=cell(1,(m-1)*(n-1));%step = size(Cell,1),而且每个block由2*2的cell组成
for i=1:m-1
   for j=1:n-1
        f=[];
        f=[f Cell{i,j}(:)' Cell{i,j+1}(:)' Cell{i+1,j}(:)' Cell{i+1,j+1}(:)'];
        feature{(i-1)*(n-1)+j}=f;
   end
end

%到此结束，feature即为所求
%下面是为了显示而写的
l=length(feature);
f=[];
for i=1:l
    f=[f;feature{i}(:)'];
end
figure
mesh(f)


function [node,elem,interface] = interfacemeshdoc(box,phi,h)
%% INTERFACEMESHDOC doc for interfacemesh
%
% [node,elem,interfaceData] = INTERFACEMESH(box,phi,h) generates an
% interface-fitted mesh [node,elem]. The box is a rectangle containing the
% interface and the interface is defined as the zero level set of function
% phi. The parameter h is the size of the backgroud uniform mesh.
%
% Information on the discrete interface is stored in the output
% interface which includes:
%   - interface.vSign: sign of phi at vertices;
%        1: outside;  -1: inside;  0: on the interface
%   - interface.tElem: traingles near the interface;
%   - interface.sElem: traingles away from the interface;
%   - interface.edge: edges approximating the interface;
%   - interface.node: vertices on the interface;
%
%  Examples
%   % circle
%     box = [ -1, 1, -1, 1];
%     h = 0.1;
%     phi = @(p) sum(p.^2, 2) - 0.5.^2;
%     [node,elem,interfaceData] = interfacemeshdoc(box,phi,h);
%
%    % heart
%     box = [ -1, 1, -1, 1];
%    [node,elem,interface] = interfacemeshdoc(box,@phiheart,0.05);
%    showmesh(node,elem);
%    findedge(node,interface.edge,'all','noindex','draw');
%
%
% Author: Huayi Wei <weihuayi@xtu.edu.cn>, and Long Chen.
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Construct the initial structure mesh
[node,elem,T] = squarequadmesh(box,h);
edge = T.edge;
edge2elem = T.edge2elem;
clear T;
N = size(node,1);
NT = size(elem,1);
showmesh(node,elem);
pause;

%% Step 1: Find points near interface
% compute the phi value at each vertex
phiValue = phi(node);
phiValue(abs(phiValue) < eps*h) = 0;  % treat nearby nodes as on the interface
vSign = sign(phiValue);
findnode(node,vSign==0);

% Find the intersection points between edges and the interface
isCutEdge = (vSign(edge(:,1)).* vSign(edge(:,2))<0);
findedge(node,edge,isCutEdge,'noindex','draw');
A = node(edge(isCutEdge,1),:);
B = node(edge(isCutEdge,2),:);
cutNode = findintersectbisect(phi,A,B);
Ncut = size(cutNode, 1);
vSign(N+1:N+Ncut) = 0;
findnode(cutNode,'all','noindex','color','k','MarkerSize',12);
pause;

% find interface elem and nodes
isInterfaceElem = false(NT,1);
isInterfaceElem(edge2elem(isCutEdge,[1,2])) = true;
isInterfaceElem(sum(abs(vSign(elem)), 2) < 3) = true; % 2 vertices on interface
findelem(node,elem,isInterfaceElem,'noindex');

isInterfaceNode = false(N,1);
isInterfaceNode(elem(isInterfaceElem,:)) = true;
findnode(node,isInterfaceNode,'noindex');
pause;

% add centers of special elements (two vertices on the interface)
%  0 - -1    -1 - 0     0 -  0      1 - 0
%  1 -  0     0 - 1     1 - -1      1 - 0
isSpecialElem = (sum(abs(vSign(elem)),2) <= 2); % two or more vertices on interfaces
% isSpecialElem = (sum(vSign(elem),2) == 0) & (sum(abs(vSign(elem)),2) == 2);
specialElem = elem(isSpecialElem,:);
auxPoint = (node(specialElem(:,1),:) + node(specialElem(:, 3),:))/2.0;
Naux = size(auxPoint,1);
vSign(N+Ncut+1:N+Ncut+Naux) = sign(phi(auxPoint));
% find the first two cases
isDiagInterface = (vSign(specialElem(:,1)).*vSign(specialElem(:,3)) == -1) | ...
                  (vSign(specialElem(:,2)).*vSign(specialElem(:,4)) == -1);
vSign(N+Ncut+find(isDiagInterface)) = 0;
findnode(auxPoint,'all','noindex','color','m');
pause;

% add cut points and aux points
node = [node; cutNode; auxPoint];
nearInterfaceNode = [node(isInterfaceNode,:); cutNode; auxPoint];
interfaceNodeIdx = [find(isInterfaceNode); N+(1:Ncut)'; N+Ncut+(1:Naux)'];
% figure(2);
% showmesh(node,elem);
findnode(nearInterfaceNode,'all','noindex');
pause;

%% Step 2: generate a Delaunay triangulation of interface points
% construct the Delaunay triangulation of interfaceNode
% different versions of matlab using different delaunay triangulation
matlabversion = version();
if str2double(matlabversion(end-5:end-2)) <= 2013
    DT = DelaunayTri(nearInterfaceNode); %#ok<*DDELTRI>
    tElem = DT.Triangulation;
else
    DT = delaunayTriangulation(nearInterfaceNode);
    tElem = DT.ConnectivityList;
end
tElem = fixorder(nearInterfaceNode,tElem); % correct the orientation
% showmesh(node,tElem);

%% Step 3: Post-processing
% get rid of the unnecessary triangles
NI = sum(isInterfaceNode);  % number of pts of the background mesh near interface
haveNewPts = (sum(tElem > NI,2) > 0); % triangles containing new added vertices
% vSignNearInterface = zeros(size(nearInterfaceNode,1),1);
% vSignNearInterface(1:sum(isInterfaceNode)) = vSign(isInterfaceNode);
% haveInterfacePts = (sum(abs(vSignNearInterface(tElem)),2) < 3);
tElem = tElem(haveNewPts,:);
% tElem = tElem(haveInterfacePts,:);
tElem = interfaceNodeIdx(tElem);  % map interfaceNode index to node index
showmesh(node,tElem,'Facecolor','y');
findnode(node,vSign==0,'noindex');
pause;

% get the remainding quad elems
sElem = elem(~isInterfaceElem,:);
% merge into one triangulation
elem = [tElem; sElem(:,[2 3 1]); sElem(:,[4 1 3])];
T = auxstructure(tElem);
showmesh(node,elem); hold on;
showmesh(node,tElem,'Facecolor','y');
isInterfaceEdge = ((vSign(T.edge(:,1)) == 0) & vSign(T.edge(:,2)) == 0);
interfaceEdge = T.edge(isInterfaceEdge,:);
nearInterfaceNode = find(vSign == 0);
findedge(node,interfaceEdge,'all','noindex','draw');
pause;

%% Generate interfaceData
interface.vSign = vSign;
interface.tElem = tElem;
interface.sElem = sElem;
interface.edge = interfaceEdge;
interface.node = nearInterfaceNode;


function patches = img2patch_batch(img, patchSize, stride)

a = patchSize(1); b = patchSize(2); c = stride;

if stride > 1
	cut = mod(size(img), patchSize);
	img = img(1:end-cut(1), 1:end-cut(2));
end

nPatches = prod(size(img)/c);

% Extended image from the original image
img = [ img img(:, 1:b-c); img(1:a-c, :) img(1:a-c, 1:b-c) ];
imgSize = size(img);

% Linear indices for the 1st patch
stride = [1 imgSize(1)];
limit = (patchSize-1).*stride;
ind = 1;
for dim = 1:numel(imgSize)
	ind = bsxfun (@plus, ind(:), 0:stride(dim):limit(dim));
end

% Linear indices for all patches
slides = imgSize - patchSize;
limit = slides.*stride;
stride = stride*c;
for dim = 1:numel(imgSize)
	ind = bsxfun( @plus, ind(:), 0:stride(dim):limit(dim) );
end
patches = reshape(img(ind(:)), prod(patchSize), nPatches);

end % img2patch_batch


function r8poly_print ( n, a, title )

%*****************************************************************************80
%
%% R8POLY_PRINT prints out a polynomial.
%
%  Discussion:
%
%    The power sum form is:
%
%      p(x) = a(0) + a(1) * x + ... + a(n-1) * x^(n-1) + a(n) * x^(n)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of A.
%
%    Input, real A(1:N+1), the polynomial coefficients.
%    A(1) is the constant term and
%    A(N+1) is the coefficient of X^N.
%
%    Input, character TITLE(*), an optional title.
%
  if ( 0 < s_len_trim ( title ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, '%s\n', title );
  end

  fprintf ( 1, '\n' );

  n = r8poly_degree ( n, a );

  if ( a(n+1) < 0.0 )
    plus_minus = '-';
  else
    plus_minus = ' ';
  end

  mag = abs ( a(n+1) );

  if ( 2 <= n )
    fprintf ( 1, '  p(x) = %c%14f * x^%d\n', plus_minus, mag, n );
  elseif ( n == 1 )
    fprintf ( 1, '  p(x) = %c%14f * x\n', plus_minus, mag );
  elseif ( n == 0 )
    fprintf ( 1, '  p(x) = %c%14f\n', plus_minus, mag );
  end

  for i = n-1 : -1 : 0

    if ( a(i+1) < 0.0 )
      plus_minus = '-';
    else
      plus_minus = '+';
    end

    mag = abs ( a(i+1) );

    if ( mag ~= 0.0E+00 )

      if ( 2 <= i )
        fprintf ( 1, '         %c%14f * x^%d\n', plus_minus, mag, i );
      elseif ( i == 1 )
        fprintf ( 1, '         %c%14f * x\n', plus_minus, mag );
      elseif ( i == 0 )
        fprintf ( 1, '         %c%14f\n', plus_minus, mag );
      end
    end

  end

  return
end


function [assignment, cost] = assignmentsuboptimal2(distMatrix)
%ASSIGNMENTSUBOPTIMAL2    Compute suboptimal assignment
%   ASSIGNMENTSUBOPTIMAL2(DISTMATRIX) computes a suboptimal assignment
%   (minimum overall costs) for the given rectangular distance or cost
%   matrix, for example the assignment of tracks (in rows) to observations
%   (in columns). The result is a column vector containing the assigned
%   column number in each row (or 0 if no assignment could be done).
%
%   [ASSIGNMENT, COST] = ASSIGNMENTSUBOPTIMAL2(DISTMATRIX) returns the
%   assignment vector and the overall cost.
%
%   The algorithm searches the matrix for the minimum element and makes the
%   corresponding row-column assignment. After setting all elements in the
%   given row and column to infinity (i.e. forbidden assignment), the
%   search procedure is repeated until all assignments are done or only
%   infinite values are found.
%
%   This function and the corresponding mex-function can further be
%   improved by first sorting all elements instead of searching for the
%   minimum of all elements many times.
%
%   assignment.html  File Exchange  Donate via PayPal
%
%   Markus Buehren
%   Last modified 05.07.2011

% initialize
nOfRows    = size(distMatrix, 1);
assignment = zeros(nOfRows,1);
cost       = 0;

for n=1:nOfRows

  % find minimum distance observation-to-track pair
  [minDist, index1] = min(distMatrix, [], 1);
  [minDist, index2] = min(minDist);
  row = index1(index2);
  col = index2;

  if isfinite(minDist)

    % make the assignment
    assignment(row) = col;
    cost = cost + minDist;

    % delete observation-to-track pair
    distMatrix(row, :) = inf;
    distMatrix(:, col) = inf;

  else
    return
  end

end


function [x, resnorm, residual, exitflag, output, lambda] = cplexlsqlin (C, d, Aineq, bineq, Aeq, beq, lb, ub, x0, options)
%%
% Purpose
% Solve constrained least squares problems.
%
% Syntax
%    x = cplexlsqlin(C, d, Aineq, bineq)
%    x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq)
%    x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq, lb, ub)
%    x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq, lb, ub, x0)
%    x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq, lb, ub, x0, options)
%    x = cplexlsqlin(problem)
%    [x, resnorm] = cplexlsqlin(...)
%    [x, resnorm, residual] = cplexlsqlin(...)
%    [x, resnorm, residual, exitflag] = cplexlsqlin(...)
%    [x, resnorm, residual, exitflag, output] = cplexlsqlin(...)
%    [x, resnorm, residual, exitflag, output, lambda] = cplexlsqlin(...)
%
% Description
% Finds the minimum of a problem specified by
%    min      norm(C*x-d)^2
%    st.      Aineq*x <= bineq
%             Aeq*x    = beq
%             lb <= x <= ub
%
% d, bineq, beq, lb, and ub are column vectors.
% C, Aineq, and Aeq are matrices.
%
% x = cplexlsqlin(C, d, Aineq, bineq) solves the constrained least squares
% problem min norm(C*x-d)^2 such that Aineq*x <= bineq.
%
% x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq) solves the preceding
% problem with the additional equality constraints Aeq*x = beq.
% If no inequalities exist, set Aineq=[] and bineq=[].
%
% x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq, lb, ub) defines a set of
% lower and upper bounds on the design variables, x, so that the solution
% is always in the range lb <= x <= ub. If no equalities exist, set Aeq=[]
% and beq=[].
%
% x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq, lb, ub, x0) sets the
% starting point for the algorithm to x0. If no bounds exist, set lb=[]
% and ub=[].
%
% x = cplexlsqlin(C, d, Aineq, bineq, Aeq, beq, lb, ub, x0, options)
% minimizes with the default optimization options replaced by values in the
% structure options, which can be created using the function cplexoptimset.
% If you do not wish to give an initial point, set x0=[].
%
% x = cplexlsqlin(problem) where problem is a structure.
%
% [x, resnorm] = cplexlsqlin(...) returns the value of the objective
% function at the solution x: resnorm = norm(C*x-d)^2.
%
% [x, resnorm, residual] = cplexlsqlin(...) returns the residual at the
% solution: C*x-d.
%
% [x, resnorm, residual, exitflag] = cplexlsqlin(...) returns a value
% exitflag that describes the exit condition of cplexlsqlin.
%
% [x, resnorm, residual, exitflag, output] = cplexlsqlin(...) returns a
% structure output that contains information about the optimization.
%
% [x, resnorm, residual, exitflag, output, lambda] = cplexlsqlin(...)
% returns a structure lambda whose fields contain the Lagrange multipliers
% at the solution x.
%
% Input Arguments
% problem   Structure containing the following fields:
%           C         Double matrix for objective function
%           d         Double column vector for objective function
%           Aineq     Double matrix for linear inequality constraints
%           bineq     Double column vector for linear inequality
%                     constraints
%           Aeq       Double matrix for linear equality constraints
%           beq       Double column vector for linear equality constraints
%           lb        Double column vector of lower bounds
%           ub        Double column vector of upper bounds
%           x0        Double column vector of initial point of x
%           options   Options structure created with cplexoptimset
%
% Output Arguments
% x         Solution found by the optimization function. If exitflag > 0,
%           then x is a solution; otherwise, x is the value of the
%           optimization routine when it terminated prematurely.
% resnorm   Value of the objective function at the solution x
% residual  Residual at the solution
% exitflag  Integer identifying the reason the optimization algorithm
%           terminated
% output    Structure containing information about the optimization. The
%           fields of the structure are:
%           iterations         Number of iterations
%           algorithm          Optimization algorithm used
%           message            Exit message
%           time               Execution time of the algorithm
%           cplexstatus        Status code of the solution
%           cplexstatusstring  Status string of the solution
% lambda    Structure containing the Lagrange multipliers at the solution x
%           (separated by constraint type). The fields of the structure
%           are:
%           lower              Lower bounds lb
%           upper              Upper bounds ub
%           ineqlin            Linear inequalities
%           eqlin              Linear equalities
%
%
%  See also cplexoptimset
%

% ---------------------------------------------------------------------------
% File: cplexlsqlin.m
% ---------------------------------------------------------------------------
% Licensed Materials - Property of IBM
% 5725-A06 5725-A29 5724-Y48 5724-Y49 5724-Y54 5724-Y55
% Copyright IBM Corporation 2008, 2010. All Rights Reserved.
%
% US Government Users Restricted Rights - Use, duplication or
% disclosure restricted by GSA ADP Schedule Contract with
% IBM Corp.
% ---------------------------------------------------------------------------


function [Q, trace] = Update(Q , V , V2, knn , p , r , a, ap , trace, alpha, gamma, lambda, f )
% Update update de Qtable
% V: previous values from state before taking action (a)
% V2: current values after action (a)
% r: reward received from the environment after taking action (a) in state
%                                             s1 and reaching the state s2
% knn: the features in previous states
% p:  the features coeficients (probabilities a.k.a. unit activations)
% a:  the last executed action
% Q: the current Q-table
% alpha: learning rate
% gamma: discount factor
% lambda: eligibility trace decay factor
% trace : eligibility trace vector
% f: true if episode ends

trace(knn,:) = 0.0; %optional trace reset
trace(knn,a) = p;

if f==true
     delta  =  r - V(a);
else
    delta  =  ( r + gamma .* V2(ap) ) - V(a);
end

Q      =  Q  + alpha .* delta .* trace;
trace  =  gamma * lambda .* trace;
end


function visualizeBoundary(X, y, model, varargin)
%VISUALIZEBOUNDARY plots a non-linear decision boundary learned by the SVM
%   VISUALIZEBOUNDARYLINEAR(X, y, model) plots a non-linear decision
%   boundary learned by the SVM and overlays the data on it

% Plot the training data on top of the boundary
plotData(X, y)

% Make classification predictions over a grid of values
x1plot = linspace(min(X(:,1)), max(X(:,1)), 100)';
x2plot = linspace(min(X(:,2)), max(X(:,2)), 100)';
[X1, X2] = meshgrid(x1plot, x2plot);
vals = zeros(size(X1));
for i = 1:size(X1, 2)
   this_X = [X1(:, i), X2(:, i)];
   vals(:, i) = svmPredict(model, this_X);
end

% Plot the SVM boundary
hold on
contour(X1, X2, vals, [0 0], {'LineWidth', 1, 'LineColor', 'b'});
hold off;

end


function linplus_test56 ( )

%*****************************************************************************80
%
%% TEST56 tests R8SM_ML.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  m = 7;
  n = m;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST56' );
  fprintf ( 1, '  R8SM_ML computes A*x or A''*X\n' );
  fprintf ( 1, '    where A is a Sherman Morrison matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix rows M =    %d\n', m );
  fprintf ( 1, '  Matrix columns N = %d\n', n );

  for job = 0 : 1
%
%  Set the matrix.
%
    [ a, u, v, seed ] = r8sm_random ( m, n, seed );

    r8sm_print ( m, n, a, u, v, '  The Sherman Morrison matrix:' );
%
%  Set the desired solution.
%
    x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
    if ( job == 0 )
      b = r8sm_mxv ( m, n, a, u, v, x );
    else
      b = r8sm_vxm ( m, n, a, u, v, x );
    end
%
%  Factor the matrix.
%
    [ a_lu, pivot, info ] = r8ge_fa ( n, a );

    if ( info ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Fatal error!\n' );
      fprintf ( 1, '  R8GE_FA declares the matrix is singular!\n' );
      fprintf ( 1, '  The value of INFO is %d\n', info );
      return
    end
%
%  Now multiply factored matrix times solution to get right hand side again.
%
    b2 = r8sm_ml ( n, a_lu, u, v, pivot, x, job );

    if ( job == 0 )
      r8vec2_print_some ( n, b, b2, 10, '  A*x and PLU*x' );
    else
      r8vec2_print_some ( n, b, b2, 10, '  A''*x and (PLU)''*x' );
    end

  end

  return
end


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k)

%project_points.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k)
%
%Projects a 3D structure onto the image plane.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion coefficients (radial and tangential) (4x1 vector)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2.
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
%
%The left terms correspond to radial distortion, the right terms correspond to tangential distortion
%
%Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*xx + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 6,
   k = zeros(4,1);
   if nargin < 5,
      c = zeros(2,1);
      if nargin < 4,
         f = ones(2,1);
         if nargin < 3,
            T = zeros(3,1);
            if nargin < 2,
               om = zeros(3,1);
               if nargin < 1,
                  error('Need at least a 3D structure to project (in project_points.m)');
                  return;
               end;
            end;
         end;
      end;
   end;
end;

[m,n] = size(X);

[Y,dYdom,dYdT] = rigid_motion(X,om,T);

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

dxdom = zeros(2*n,3);
dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

dxdT = zeros(2*n,3);
dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);

r4 = r2.^2;

dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4;

dcdistdom = k(1) * dr2dom + k(2) * dr4dom;
dcdistdT = k(1) * dr2dT+ k(2) * dr4dT;
dcdistdk = [ r2' r4' zeros(n,2)];

xd1 = x .* (ones(2,1)*cdist);

dxd1dom = zeros(2*n,3);
dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
dxd1dom = dxd1dom + coeff.* dxdom;

dxd1dT = zeros(2*n,3);
dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
dxd1dT = dxd1dT + coeff.* dxdT;

dxd1dk = zeros(2*n,4);
dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk;
dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

delta_x = [k(3)*a1 + k(4)*a2 ;
   k(3) * a3 + k(4)*a1];

ddelta_xdx = zeros(2*n,2*n);
aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

ddelta_xdom = zeros(2*n,3);
ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);

ddelta_xdT = zeros(2*n,3);
ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);

ddelta_xdk = zeros(2*n,4);
ddelta_xdk(1:2:end,3) = a1';
ddelta_xdk(1:2:end,4) = a2';
ddelta_xdk(2:2:end,3) = a3';
ddelta_xdk(2:2:end,4) = a1';

xd2 = xd1 + delta_x;

dxd2dom = dxd1dom + ddelta_xdom ;
dxd2dT = dxd1dT + ddelta_xdT;
dxd2dk = dxd1dk + ddelta_xdk ;

% Pixel coordinates:
if length(f)>1,
    xp = xd2 .* (f * ones(1,n))  +  c*ones(1,n);
    coeff = reshape(f*ones(1,n),2*n,1);
    dxpdom = (coeff*ones(1,3)) .* dxd2dom;
    dxpdT = (coeff*ones(1,3)) .* dxd2dT;
    dxpdk = (coeff*ones(1,5)) .* dxd2dk;
    dxpdf = zeros(2*n,2);
    dxpdf(1:2:end,1) = xd2(1,:)';
    dxpdf(2:2:end,2) = xd2(2,:)';
else
    xp = f * xd2 + c*ones(1,n);
    dxpdom = f  * dxd2dom;
    dxpdT = f * dxd2dT;
    dxpdk = f  * dxd2dk;
    dxpdf = xd2(:);
end;

dxpdc = zeros(2*n,2);
dxpdc(1:2:end,1) = ones(n,1);
dxpdc(2:2:end,2) = ones(n,1);

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(4,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k);

% Test on om: OK

dom = 0.000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points(X,om2,T,f,c,k);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: OK!!

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points(X,om,T2,f,c,k);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points(X,om,T,f2,c,k);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points(X,om,T,f,c2,k);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.001 * norm(4)*randn(4,1);
k2 = k + dk;

[x2] = project_points(X,om,T,f,c,k2);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)


% SDP relaxation for structured total least squares (STLS)
%
% Let k,m,n integers (m<=n)
% Let SS: R^k -> R^{m x n} be an affine map
% Let u1 be a vector in R^k
% Consider the optimization problem
%
% min_u     |u - u1|^2
% s.t.      SS(u) is rank deficient
%
% More generally, the cost can be a quadratic function
%           (u - u1)' * W * (u - u1)
% for some symmetric weight matrix W of size k x k.
% If W is diagonal and the entry W_ii is equal to zero then
% the respective u1(i) is ignored (as if it was missing).
%
% The function also accepts problems with complex data.
% They are internally converted to real valued problems.
%
% Input:
% S - matrix of size (k+1)m x n describing the affine map SS
% u1 - a vector of length k, possibly containing 'nan' entries
%
% Optional inputs:
% W - weight matrix of size k x k (default W_ii=1 except for 'nan' entries)
% solver - SDP solver (default 'sdpt3')
% quiet - set to false to display information (default true)
%
% Output:
% opt - optimal value of SDP relaxation
% u - the minimizer of the problem
% U - the matrix SS(u)
% z - a vector in the left kernel of SS(u)
% X - the PSD matrix (relaxation is exact if rank(X)=1)
%
% Usage:
% [opt,u,U,z,X] = sdp_stls(S,u1,W,solver,quiet)
% The optional arguments can be omitted or set to empty values.
% For instance: sdp_stls(S,u1,[],solver)

function [opt,u,U,z,X] = sdp_stls(S,u1,W,solver,quiet)

narginchk(2,5);
if nargin<3||isempty(W); W = diag(~isnan(u1)); end
if nargin<4||isempty(solver); solver = 'sdpt3'; end
if nargin<5||isempty(quiet); quiet = false; end

k = length(u1);
n = size(S,2);
m = size(S,1)/(k+1);
if floor(m)~=m; error('mismatch in dimensions'); end
u1(isnan(u1)) = 0;
u1 = reshape(u1,[k,1]);

iscomplex = ~(isreal(S) && isreal(u1) && isreal(W));
if iscomplex; [k,m,n,S,u1,W] = data2real(k,m,n,S,u1,W); end
W = .5*(W+W');

% fprintf('PSD matrix size: %d\n',(k+1)*m)

g = [eye(k) -u1];
G0 = g'*W*g;
E0 = sparse(k+1,k+1,1);

Im = eye(m);
G = kron(G0,Im);
E = kron(E0,Im);

[opt, X] = primal_cvx(k,m,n,S,G,E,solver,quiet);

[~,E] = eig(full(X));
r=nnz(diag(E)>1e-4);
if (~iscomplex && r>1) || (iscomplex && r>2)
    warning('sdp might not be exact');
end

J0 = k*m+1:(k+1)*m;
z = recoverSol(X(J0,J0));
u = zeros(k,1);
for i = 1:k
    Ji = (i-1)*m+1:i*m;
    u(i) = trace(X(J0,Ji));
end
U = applyAffineMap(S,u);

if iscomplex
    [u,U,z] = data2complex(k,m,n,u,U,z);
end

function [k,m,n,S,u1,W] = data2real(k,m,n,S,u1,W)
toReal = @(a) [real(a) -imag(a); imag(a) real(a)];
u1 = [real(u1); imag(u1)];
if norm(W-W')>1e-4; warning('W forced to be hermitian'); end
W = toReal(W);
SS = mat2cell(S,m*ones(k+1,1),n);
B = SS(1:k); A = SS(k+1);
iB = cellfun(@(x) 1i*x, B,'Unif',0);
SS = cellfun(toReal, [B; iB; A],'Unif',0);
S = cell2mat(SS);
k = 2*k; m = 2*m; n = 2*n;

function [u,U,z] = data2complex(k,m,n,u,U,z)
k = k/2; m = m/2; n = n/2;
u = u(1:k) + 1i * u(k+1:2*k);
U = U(1:m,1:n) + 1i * U(m+1:2*m,1:n);
z = z(1:m) + 1i * z(m+1:2*m);

% dual sdp
function [opt, X] = dual_cvx(k,m,n,S,G,E,solver,quiet)
N = (k+1)*m;
SC = cell(n,1);

if quiet
cvx_begin sdp quiet
else
cvx_begin sdp
end
    cvx_solver(solver)
    variable t(1,1);
    variable Cvec(N,n)
    dual variable X
    for i=1:n
        Ci = Cvec(:,i);
        si = S(:,i);
        SC{i} = si*Ci';
    end
    sSC = sum(cat(3,SC{:}),3);
    sSC = blksym(k+1,m,sSC);
    maximize(t);
    Q = G - t*E + sSC;
    X: Q >= 0;
cvx_end

opt = cvx_optval;

% primal sdp
function [opt, X] = primal_cvx(k,m,n,S,G,E,solver,quiet)
N = (k+1)*m;

e = speye(N);
SC = zeros(N,N,n*N);
for i=1:n
    for j=1:N
        SC(:,:,(i-1)*N+j) = S(:,i)*e(j,:);
    end
end
SC = blksym(k+1,m,SC);
A = zeros(n*N,N*(N+1)/2);
for l=1:n*N
    A(l,:) = smat2vec(SC(:,:,l));
end
A = sparse(A);

if quiet
cvx_begin sdp quiet
else
cvx_begin sdp
end
    cvx_solver(solver)
    variable X(N,N) symmetric
    dual variable Q
    y = smat2vec(X);
    minimize(smat2vec(G)'*y);
    smat2vec(E)'*y == 1;
    A*y == 0;
    Q: X >= 0;
cvx_end

opt = cvx_optval;

% recover minimizer from moment matrix
function [x,e] = recoverSol(X)
N = size(X,1);
if any(isnan(X))
    x = nan(N,1);
    e = inf;
else
    [V,E] = eig(full(X));
    e=diag(E);
    x = sqrt(e(N))*V(:,N);
    e = e(N-1);
end

% vector to symmetric matrix
function M = vec2smat(v)

N = length(v);
k = (-1+sqrt(1+8*N))/2;

M = repmat(0*v(1:k),[1,k]);
I = triu(true(k,k),0);
I2 = triu(true(k,k),1);
M(I) = v/2;
M(I2) = M(I2)*sqrt(2);
M = M + M.';

% symmetric matrix to vector
function v = smat2vec(M)

k = size(M,1);
I = triu(true(k,k),0);
I2 = triu(true(k,k),1);
M(I2) = M(I2)*sqrt(2);
v = M(I);

% Block symmetrization of a matrix
% Input: mk x mk matrix A
% Output: symmetric matrix As such that
%         each m x m block is also symmetric (there are n^2 blocks)
function As = blksym(n,m,A)

As = mysym(A);
if isempty(A); return; end

for i=1:n
    for j=1:n
        I = m*(i-1) + (1:m);
        J = m*(j-1) + (1:m);
        As(I,J,:) = mysym(As(I,J,:));
    end
end

% symmetrize matrix
function As = mysym(A)
At = permute(A,[[2,1],3:ndims(A)]);
As = .5*(A+At);


function [vect] = normVector(vect)
%normVector Summary of this function goes here
%   Detailed explanation goes here
    vect = vect / norm(vect);
end