function [imageSeg,Rot]= getPlanSeg(XYZworldframeTest,SpaceTest,imgSize)
% input XYZworldframeTest is alinged in Z
%load([ '/n/fs/modelnet/NYUdataSet/NYUdatafeatureNew/' 'feature_' num2str(imageNum) '.mat'],'XYZworldframeTest','rgbTest','SpaceTest','imgDepth');
onPlaneThreshold =0.055;
sizethr =500;
normalAgreeThreshold =0.8;
removeNaN = find(~sum(isnan(XYZworldframeTest),2));
XYZworldframeTest = XYZworldframeTest(removeNaN,:);
[zhist,zc]=hist(XYZworldframeTest(:,3),round(20*(SpaceTest.Rz(2)-SpaceTest.Rz(1))));
[~,zpks] =findpeaks([0,zhist,0],'minpeakdistance',3);
zpks = zpks-1;
% figure,
% bar(bincenter,count);
% hold on;
% plot(bincenter(pks),count(pks),'rx');
%remove
range =[max(min(XYZworldframeTest(:,3)),zc(max(1,zpks-1))'),...
        min(zc(min(size(zc,2),zpks+1)),max(XYZworldframeTest(:,3)))';
        zc(end-2),max(XYZworldframeTest(:,3));
        min(XYZworldframeTest(:,3)),zc(3)];
removepts = inrange(XYZworldframeTest(:,3),range);
XYZworldframeTestRemove=XYZworldframeTest;
XYZworldframeTestRemove(removepts>0,:) =[];

% hough lines
[H,T,R] = votelines(XYZworldframeTestRemove(:,1),XYZworldframeTestRemove(:,2));
% blur it
allangleVote = max(H);
allangleVote = [allangleVote,allangleVote];
shift= 0;
clear rightAngle
for i =1:length(shift)
    rightAngle(i,:) = allangleVote(91+shift(i):180+shift(i))+allangleVote(1:90);
end
[~,AnglepickedLin]= max(rightAngle(:));
[s,Anglepicked]=ind2sub(size(rightAngle),AnglepickedLin);
[~,Rpicked1] = max(H(:,Anglepicked));
[~,Rpicked2] = max(H(:,Anglepicked+90+shift(s)));
P = [R(Rpicked1),T(Anglepicked);R(Rpicked2),T(Anglepicked)+90+shift(s)];

rotation = min(P(:,2));
Rot = getRotationMatrix('z',-1*rotation/180*pi);
XYZworldframeTestNew = [[Rot(1:2,1:2)*XYZworldframeTest(:,[1,2])']', XYZworldframeTest(:,3)];
normals = points2normals(XYZworldframeTestNew);

% find  corner
[xhist,xc]=hist(XYZworldframeTestNew(:,1),round((SpaceTest.Rx(2)-SpaceTest.Rx(1))/0.05));
[~,xpks] =findpeaks([0,xhist,0],'MINPEAKHEIGHT',50);
xpks = xpks-1;
[yhist,yc]=hist(XYZworldframeTestNew(:,2),round((SpaceTest.Ry(2)-SpaceTest.Ry(1))/0.05));
[~,ypks] =findpeaks([0,yhist,0],'MINPEAKHEIGHT',50);
ypks = ypks-1;
[zhist,zc]=hist(XYZworldframeTestNew(:,3),round((SpaceTest.Rz(2)-SpaceTest.Rz(1))/0.05));
[~,zpks] =findpeaks([0,zhist,0],'MINPEAKHEIGHT',50);
zpks = zpks-1;

%project points onto this plan and caculate conected component

gid =1;
imageSeg = zeros(imgSize);
for p =1:size(xpks,2)
    inLine =find(abs(XYZworldframeTestNew(:,1)-xc(xpks(p)))<onPlaneThreshold&abs(normals(1,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            imageSeg(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end

for p =1:size(ypks,2)
    inLine =find(abs(XYZworldframeTestNew(:,2)-yc(ypks(p)))<onPlaneThreshold&abs(normals(2,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            imageSeg(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end

for p =1:size(zpks,2)
    inLine =find(abs(XYZworldframeTestNew(:,3)-zc(zpks(p)))<onPlaneThreshold&abs(normals(3,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            imageSeg(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end
%%

% rotate back
%{
corner_r = [xc(xpks(1)) yc(ypks(1));xc(xpks(end)) yc(ypks(end))];
corner_r = get4corner(corner_r);
corner = [[Rot(1:2,1:2)'*corner_r(:,[1:2])']'];
D1 = cos(P(1,2)*pi/180)*corner(1,1)+sin(P(1,2)*pi/180)*corner(1,2);
D2 = cos(P(2,2)*pi/180)*corner(1,1)+sin(P(2,2)*pi/180)*corner(1,2);
D3 = cos(P(1,2)*pi/180)*corner(3,1)+sin(P(1,2)*pi/180)*corner(3,2);
D4 = cos(P(2,2)*pi/180)*corner(3,1)+sin(P(2,2)*pi/180)*corner(3,2);
P_new = [D1,P(1,2);D2,P(2,2);D3,P(1,2);D4,P(2,2)];
f = figure,
vis_point_cloud(XYZworldframeTest,rgbTest,10,5000);hold on;
for j =1:3
    plot3(corner([j,j+1],1),corner([j,j+1],2),[max(XYZworldframeTest(:,3));max(XYZworldframeTest(:,3))],'-xr','LineWidth',10)
end
plot3(corner([1,4],1),corner([1,4],2),[max(XYZworldframeTest(:,3));max(XYZworldframeTest(:,3))],'-xr','LineWidth',10)

for i =1:gid
    hold on;
    plot3(XYZworldframeTestOrg(imageSeg(:)==i,1),XYZworldframeTestOrg(imageSeg(:)==i,2),XYZworldframeTestOrg(imageSeg(:)==i,3),'+','Color',rand([1,3]));
end

axis equal;
axis tight;
view(30,50)
saveas(f,['./result/' num2str(imageNum) '.fig']);
saveas(f,['./result/' num2str(imageNum) '.jpg']);

for gid =1:length(gropuind)
    imageSeg(removeNaN(gropuind{gid})) = gid;
end
%}

%figure(1),imagesc(imageSeg)
%Boudary = [D1 D2 D3 D4];
%{
if imageNum> 0,
    im = getImagesc(imageSeg);
    mkdir(segpath);
    imwrite(im,sprintf('%s/%04d.jpg',segpath,imageNum));
end
%}
end
function [hough_transform,T,R] = votelines(X,Y)
         thetaResolution = 1;
         rhoResolution = 0.1;
         T = [1:180];
         theta = T/180*pi;
         rho = X(:)*cos(theta)+ Y(:)*sin(theta);
         % quantize the rho
         rhoNorm = max(1,round((rho-min(rho(:)))/rhoResolution));
         R =[1:max(rhoNorm(:))]*rhoResolution+min(rho(:));
         hough_transform = zeros(max(rhoNorm(:)),size(T,2));
         TT = repmat(T,[size(rhoNorm,1),1]);
         hough_transform = accumarray([rhoNorm(:),TT(:)],1,size(hough_transform));
end


% MP_SOLVE_MAXWELL_SRC_2D: Solve the 2d Maxwell source problem with a B-spline discretization, in a multipatch domain.
%
% Example to solve the problem
%
%    curl ( epsilon(x) curl (u)) + mu u = f      in Omega
%              (epsilon(x) curl(u)) x n = g      on Gamma_N
%                                 u x n = h x n  on Gamma_D
%
% where the domain \Omega is formed by several patches of the form F((0,1)^n).
%
% USAGE:
%
%  [geometry, msh, space, u] = mp_solve_maxwell_src (problem_data, method_data)
%
% INPUT:
%
%  problem_data: a structure with data of the problem. It contains the fields:
%    - geo_name:     name of the file containing the geometry
%    - nmnn_sides:   sides with Neumann boundary condition (may be empty)
%    - drchlt_sides: sides with Dirichlet boundary condition
%    - c_elec_perm:  electric permittivity (epsilon in the equation)
%    - c_magn_perm:  magnetic permeability (mu in the equation)
%
%  method_data : a structure with discretization data. Its fields are:
%    - degree:     degree of the spline functions.
%    - regularity: continuity of the spline functions.
%    - nsub:       number of subelements with respect to the geometry mesh
%                   (nsub=1 leaves the mesh unchanged)
%    - nquad:      number of points for Gaussian quadrature rule
%
% OUTPUT:
%
%  geometry:  array of geometry structures (see mp_geo_load)
%  msh:       multipatch mesh, consisting of several Cartesian meshes (see msh_multipatch)
%  space:     multipatch space, formed by several tensor product spaces plus connectivity and orientation (see sp_multipatch)
%  u:         the computed degrees of freedom
%
% Copyright (C) 2010-2015 Rafael Vazquez
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function [geometry, msh, space, u] = mp_solve_maxwell_src (problem_data, method_data)

% Extract the fields from the data structures into local variables
data_names = fieldnames (problem_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= problem_data.(data_names{iopt});']);
end
data_names = fieldnames (method_data);
for iopt  = 1:numel (data_names)
  eval ([data_names{iopt} '= method_data.(data_names{iopt});']);
end

% Construct geometry structure
[geometry, boundaries, interfaces, ~, boundary_interfaces] = mp_geo_load (geo_name);
npatch = numel (geometry);

msh = cell (1, npatch);
sp = cell (1, npatch);
for iptc = 1:npatch
  [knots, zeta] = ...
         kntrefine (geometry(iptc).nurbs.knots, nsub-1, degree, regularity);
  [knots_hcurl, degree_hcurl] = knt_derham (knots, degree);

% Construct msh structure
  rule      = msh_gauss_nodes (nquad);
  [qn, qw]  = msh_set_quad_nodes (zeta, rule);
  msh{iptc} = msh_cartesian (zeta, qn, qw, geometry(iptc));

% Construct space structure
  scalar_spaces = cell (msh{iptc}.ndim, 1);
  for idim = 1:msh{iptc}.ndim
    scalar_spaces{idim} = sp_bspline (knots_hcurl{idim}, degree_hcurl{idim}, msh{iptc});
  end
  sp{iptc} = sp_vector (scalar_spaces, msh{iptc}, 'curl-preserving');
  clear scalar_spaces
end

msh = msh_multipatch (msh, boundaries);
space = sp_multipatch (sp, msh, interfaces, boundary_interfaces);
clear sp

stiff_mat = op_curlu_curlv_mp (space, space, msh, c_stiff);
mass_mat = op_u_v_mp (space, space, msh, c_mass);
rhs = op_f_v_mp (space, msh, f);

% Apply Neumann boundary conditions
Nbnd = cumsum ([0, boundaries.nsides]);
for iref = nmnn_sides
  iref_patch_list = Nbnd(iref)+1:Nbnd(iref+1);
  gref = @(varargin) g(varargin{:},iref);
  rhs_nmnn = op_f_v_mp (space.boundary, msh.boundary, gref, iref_patch_list);
  rhs(space.boundary.dofs) = rhs(space.boundary.dofs) + rhs_nmnn .* space.boundary.boundary_orientation.';
end

% Apply Dirichlet boundary conditions
u = zeros (space.ndof, 1);
[u_drchlt, drchlt_dofs] = sp_drchlt_l2_proj (space, msh, h, drchlt_sides);
u(drchlt_dofs) = u_drchlt;
int_dofs = setdiff (1:space.ndof, drchlt_dofs);

% Solve the linear system
rhs(int_dofs) = rhs(int_dofs) - stiff_mat(int_dofs, drchlt_dofs)*u_drchlt ...
                              - mass_mat(int_dofs, drchlt_dofs)*u_drchlt;

u(int_dofs) = (stiff_mat(int_dofs, int_dofs) + ...
                mass_mat(int_dofs, int_dofs)) \ rhs(int_dofs);

end

%!demo
%! ex_maxwell_src_Lshaped_mp


function order = lyness_order ( rule )

%*****************************************************************************80
%
%% LYNESS_ORDER returns the order of a Lyness quadrature rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    James Lyness, Dennis Jespersen,
%    Moderate Degree Symmetric Quadrature Rules for the Triangle,
%    Journal of the Institute of Mathematics and its Applications,
%    Volume 15, Number 1, February 1975, pages 19-32.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Output, integer ORDER, the order of the rule.
%
  if ( rule == 0 )
    order = 1;
  elseif ( rule == 1 )
    order = 3;
  elseif ( rule == 2 )
    order = 4;
  elseif ( rule == 3 )
    order = 4;
  elseif ( rule == 4 )
    order = 7;
  elseif ( rule == 5 )
    order = 6;
  elseif ( rule == 6 )
    order = 10;
  elseif ( rule == 7 )
    order = 9;
  elseif ( rule == 8 )
    order = 7;
  elseif ( rule == 9 )
    order = 10;
  elseif ( rule == 10 )
    order = 12;
  elseif ( rule == 11 )
    order = 16;
  elseif ( rule == 12 )
    order = 13;
  elseif ( rule == 13 )
    order = 13;
  elseif ( rule == 14 )
    order = 16;
  elseif ( rule == 15 )
    order = 16;
  elseif ( rule == 16 )
    order = 21;
  elseif ( rule == 17 )
    order = 16;
  elseif ( rule == 18 )
    order = 19;
  elseif ( rule == 19 )
    order = 22;
  elseif ( rule == 20 )
    order = 27;
  elseif ( rule == 21 )
    order = 28;
  else
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LYNESS_ORDER - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized rule index.\n' );
    error ( 'LYNESS - Fatal error!' );
  end

  return
end


% DEMROBOTWIRELESS1 Wireless Robot data from University of Washington, without dynamics and without back constraints.

% FGPLVM

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

dataSetName = 'robotWireless';
experimentNo = 1;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('ftc');
latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

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

% Load the results and display dynamically.
fgplvmResultsDynamic(dataSetName, experimentNo, 'vector')


function op = linop_subsample( sz, omega, SYMMETRIC )
%LINOP_SUBSAMPLE Subsampling linear operator.
%OP = LINOP_SUBSAMPLE( SZ, OMEGA )
%   vector and matrix subsampling. Depending on SZ and OMEGA,
%   this can do row-sampling (e.g. a partial FFT)
%   or it can sample specific entries of a matrix (e.g. matrix completion)
%   To specify matrix subsampling, set sz = { [n1,n2], [length(omega),1] }
%       where the input matrix has size n1 x n2
%
%OP = LINOP_SUBSAMPLE( OMEGA )
%   works if OMEGA is a sparse matrix, whose nonzero entries
%   specify the entries to sample.
%
%OP = LINOP_SUBSAMPLE( ..., SYMMETRIC )
%   If SYMMETRIC is true, then
%   forces the domain to be the space of symmetric matrices
%
% Example with FFT:
%   FFT = linop_handles([N,N], @(x)fft(x)/sqrt(N), @(x)sqrt(N)*real(ifft(x)) ,'R2C');
%   and to make a row sub-sampled FFT (M <= N):
%   rows = randperm(M);
%   A   = linop_compose( linop_subsample([M,N], rows ), FFT );

% Designed to be used with a fft or dct
% Do we need a separate oversampling version?
% Also, make linop_DCT and linop_FFT ?
%   The reason we might want this is that for linop_FFT,
%   people will probably use idct as the transpose -- this is not
%   correct, due to scaling

% Help documentation: TBD
%    Constructs a TFOCS-compatible linear operator from separate function
%    handles that compute the forward and adjoint operations. The first
%    argument, SZ, gives the size of the linear operator; and the forward
%    and adjoint handles are AF and AT, respectively.
%
%    If the inputs and outputs are simple vectors, then SZ can take the
%    standard Matlab form [N,M], where N is the input length and M is the
%    output length. If the input or output is a matrix or array, then SZ
%    should take the form { S_in, S_out }, where S_in is the size of the
%    input and S_out is the size of the output.
%

error(nargchk(1,3,nargin));
if nargin < 3
    SYMMETRIC = false;
end
if nargin == 1
    omega = sz;
    sz = { size(omega), [nnz(omega),1] };
elseif nargin ==2 && issparse(sz)
    SYMMETRIC = omega;
    omega = sz;
    sz = { size(omega), [nnz(omega),1] };
end

if numel( sz ) ~= 2,
    error( 'Size must have two elements.' );
elseif isnumeric( sz ),
    sz = { [sz(2),1], [sz(1),1] };
elseif ~isa( sz, 'cell' ),
    error( 'Invalid operator size specification.' );
end

dom = sz{1};
n1 = dom(1); n2 = dom(2);
ran = sz{2};

% There are several possibilities.  Let x be a point in the domain
%{
    x is a vector.  Then omega should be a vector.
                    We return x(omega), resp.

    x is a matrix.
        omega is a vector
            This is ambiguous: does the user want x(omega,:) or x(omega)?
        omega is a matrix with 2 columns, then assume it is [I,J]
            We convert it to linear indices.
        omega is a general matrix, more than 2 columns
            Not sure what the user means; report an error.
        omega is a sparse matrix
            We find it's entries, and use those

%}

if n2 == 1
    % x is a vector, not a matrix.  Simple.
%     op = @(x,mode) linop_subsample_vector( sz, omega, x, mode );
    % Allow it to vectorize along rows:
    op = @(x,mode) linop_subsample_row( sz, omega, x, mode );
else
    % trickier case.
    if issparse(omega)
        ind = find(omega);
        [I,J] = ind2sub( sz{1}, ind );
%         op = @(x,mode) linop_subsample_matrix( sz, ind, I, J, SYMMETRIC, x, mode );
    elseif isvector(omega)
        ind     = omega;
        [I,J]   = ind2sub( sz{1}, ind );
%         op = @(x,mode) linop_subsample_matrix( sz, omega, SYMMETRIC,x, mode );
    elseif size(omega,2) == 2
        ind = sub2ind( sz{1}, omega(:,1), omega(:,2) );
        I   = omega(:,1);
        J   = omega(:,2);
%         op = @(x,mode) linop_subsample_matrix( sz, ind, SYMMETRIC,x, mode );
    else
        error('omega is not an acceptable size; perhaps you meant it to be sparse?');
    end
    % Make sure 'ind' is a column vector
    ind     = ind(:);
    op = @(x,mode) linop_subsample_matrix( sz, ind, I, J, SYMMETRIC, x, mode );

end

function y = linop_subsample_vector(sz, omega, x, mode )
switch mode,
    case 0, y = sz;
    case 1,
        if ~isequal( sz{1}, size(x) ), error('input wrong size for vector subsampling'); end
        y = x(omega);
    case 2,
        y = zeros( sz{1} );
        y(omega) = x;
end
function y = linop_subsample_row(sz, omega, x, mode )
switch mode,
    case 0, y = sz;
    case 1,
        if ~isequal( sz{1}(1), size(x,1) ), error('input wrong size for vector subsampling'); end
        y = x(omega,:);
    case 2,
        y = zeros( sz{1} );
        y = zeros( sz{1}(1), size(x,2) );
        y(omega,:) = x;
end
function y = linop_subsample_matrix(sz, omega, indI, indJ,SYMMETRIC, x, mode )
switch mode,
    case 0, y = sz;
    case 1
        S = [];
        S.type = '()';
        S.subs = {omega};
        y = subsref(x,S);
    case 2,
        dom = sz{1}; n1 = dom(1); n2 = dom(2);
        y = sparse( indI, indJ, x, n1, n2 );
        if SYMMETRIC
            % in future, might update this
            % e.g. force omega to only refer to lower part of matrx,
            % and then do the update y = y + tril(y,-1)'
            y = (y+y')/2;
        end
end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function opts = SRPrior(varargin)
%SRPRIOR Super-resolution image prior.
%   SRPRIOR returns a parameter structure to represent an image prior for
%   resolution using default settings.
%
%   The parameter structure consist of the following parameters:
%       - function:     Function handle to provide a regularization term
%                       for the prior.
%       - gradient:     Function handle to gradient of the regularizer.
%       - weight:       Regularization/prior weight.
%       - parameters:   Additional parameters passed to the regularizer and
%                       the gradient function.

opts = struct('function',   @huberPrior, ...        % Function handle to prior (default: Huber)
              'gradient',   @huberPrior_grad, ...   % Function handle to gradient
              'weight',     0, ...                  % Regularization weight
              'parameters', [], ...                 % Additional parameters passed to prior function/gradient
              'weightEvaluationFunction', []);

% Update with user-defined parameters
for k = 1:2:(nargin - 1)

    param = varargin{k};
    value = varargin{k+1};
    if isfield(opts, param)
        opts = setfield(opts, param, value);
    else
        error( sprintf('Invalid parameter %s', param) );
    end

end


function shape_scaled = scaleshape(shape, scale)
%SCALESHAPE Summary of this function goes here
%   Function: scale input shape using a scale ratio
%   Detailed explanation goes here
%   Input:
%        bbox: the bbox of current sample
%        shape: input shape
%        scale: scale ratio
%   Output:
%        shape_scaled: scaled shape

shape_scaled = bsxfun(@plus, scale*(bsxfun(@minus, shape, mean(shape))), mean(shape));

end


function U = spm_mvb_U(Y,priors,X0,xyz,vox,nu)
% Constructs patterns U for Multivariate Bayesian inversion of a linear model
% FORMAT U = spm_mvb_U(Y,priors,X0,xyz,vox,nu)
% Y      - data-feature matrix
% priors - 'null'      % no patterns
%        - 'compact'   % reduced (ns/3); using SVD on local compact support
%        - 'sparse'    % a pattern is a voxel
%        - 'smooth'    % patterns are local Gaussian kernels
%        - 'singular'  % patterns are global singular vectors
%        - 'support'   % the patterns are the images
%
% X0     - confounds
% xyz    - location in mm
% vox    - voxel size in mm
% nu     - number of patterns (for 'compact')
%
% U      - pattern or mode weights
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_mvb_U.m 7654 2019-08-25 20:09:35Z karl $

% defaults
%--------------------------------------------------------------------------
try, X0;  catch, X0  = [];   end
try, xyz; catch, xyz = [];   end

% get orders
%--------------------------------------------------------------------------
ns     = size(Y,1);                      % number of samples
nv     = size(Y,2);                      % number of voxels
if nargin < 6
    nu = min(ceil(ns/3),nv);             % number of patterns
end

% confounds
%--------------------------------------------------------------------------
if isempty(X0); X0 = zeros(ns,1); end

% get U: X = Y*P + X0*Q + R
%        P = U*E;
%--------------------------------------------------------------------------
% assemble empirical priors
%==========================================================================
switch priors

    case 'null'
        %------------------------------------------------------------------
        U     = sparse(nv,0);

    case 'sparse'
        %------------------------------------------------------------------
        U     = speye(nv,nv);

    case 'smooth'
        %------------------------------------------------------------------
        sm    = 4;                            % smoothness fixed at 4mm std
        dlim  = (4*sm)^2;                     % spatial limit (mm)^2
        s     = sm^2;                         % Smoothness variance
        xyz   = xyz';
        nlim  = 256;                          % voxel limit
        Vvx   = prod(vox);                    % volume of a voxel
        Vlr   = 4/3*pi*dlim^(3/2);            % VOI around a voxel
        Nlr   = round(Vlr/Vvx*.85);           % # of voxel in VOI; keep 85%
        U     = spalloc(nv,nv,nv*Nlr);        % pre-allocate memory
        fprintf('Creating smooth patterns - please wait\n')
        kk    = floor(nv/4);
        fprintf('\n0%%')
        for i = 1:nv
            if ~rem(i,kk), fprintf('.....%2i%%',25*i/kk); end
            u      = (xyz(:,1) - xyz(i,1)).^2;
            j      = find(u < dlim);
            u(j)   = u(j) + (xyz(j,2) - xyz(i,2)).^2;
            j      = j((u(j) < dlim));
            u(j)   = u(j) + (xyz(j,3) - xyz(i,3)).^2;
            j      = j((u(j) < dlim));
            if length(j)>nlim
                [q,k]  = sort(u(j));
                k      = k(1:nlim);
                j      = j(k);
            end
            U(j,i) = exp(-u(j)/(2*s));
        end
        fprintf('\nThank you\n')

    case 'singular'

        % get kernel (singular vectors)
        %------------------------------------------------------------------
        Y       = Y - X0*(pinv(X0)*Y);         % remove confounds
        [u,s,v] = spm_svd(Y,1/4);              % c.f., Kaiser criterion
        U       = v/s;

    case 'support'

        % get kernel (image vectors)
        %------------------------------------------------------------------
        if nv > ns
            R  = speye(size(X0,1)) - X0*pinv(X0);
            U  = (R*Y)';
        else
            U  = speye(nv,nv);
        end

    case 'compact'

        % get kernel (compact vectors)
        %------------------------------------------------------------------
        nc    = max(fix(nv/nu),1);           % voxels in compact support
        X0    = spm_svd(X0);                 % confounds
        Y     = Y - X0*(X0'*Y);              % remove confounds
        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

    otherwise
        disp('unknown prior')
        return
end


% L1 Filtering (Liu et al. 2011)
% process_video('RPCA', 'L1F', 'dataset/demo.avi', 'output/demo_L1F.avi');
[L,S] = rpca_l1f(M);


function a = wishart_unit_sample ( m, df )

%*****************************************************************************80
%
%% WISHART_UNIT_SAMPLE samples the unit Wishart distribution.
%
%  Discussion:
%
%    This function requires functions from the PDFLIB and RNGLIB libraries.
%
%    The "initialize()" function from RNGLIB must be called before using
%    this function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Patrick Odell, Alan Feiveson,
%    A numerical procedure to generate a sample covariance matrix,
%    Journal of the American Statistical Association,
%    Volume 61, Number 313, March 1966, pages 199-203.
%
%    Stanley Sawyer,
%    Wishart Distributions and Inverse-Wishart Sampling,
%    Washington University,
%    30 April 2007, 12 pages.
%
%  Parameters:
%
%    Input, integer M, the order of the matrix.
%
%    Input, integer DF, the number of degrees of freedom.
%    M <= DF.
%
%    Output, real A(M,M), the sample matrix from the unit Wishart distribution.
%
  if ( df < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'WISHART_UNIT_SAMPLE - Fatal error!\n' );
    fprintf ( 1, '  DF = %d < M = %d.\n', df, m );
    error ( 'WISHART_UNIT_SAMPLE - Fatal error!\n' );
  end

  c = zeros ( m, m );

  for i = 1 : m
    df_chi = df - i + 1;
    c(i,i) = sqrt ( r8_chi_sample ( df_chi ) );
    for j = i + 1 : m
      c(i,j) = r8_normal_01_sample ( );
    end
  end

  a = c' * c;

  return
end


function [pnt, tri] = read_off(fn)

% READ_OFF reads vertices and triangles from a OFF format triangulation file
%
% [pnt, tri] = read_off(filename)
%
% See also READ_TRI, READ_BND

% Copyright (C) 1998, Robert Oostenveld
%
% 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$

fid = fopen_or_error(fn, 'rt');

% scan the file type
[s, count] = fscanf(fid, '%s\n', 1);
if ~strcmp(s,'OFF')
  msg = sprintf('wrong file type %s', s);
  ft_error(msg);
end

% read the number of vertex points and triangles
[val, count] = fscanf(fid, '%d', 3);
Npnt = val(1)
Ntri = val(2)

% read the vertex points
pnt  = fscanf(fid, '%f', [3, Npnt]);
pnt  = pnt(1:3,:)';

% read the triangles
tri = fscanf(fid, '%d', [4, Ntri]);
tri = (tri(2:4,:)+1)';
fclose(fid);


%Copyright (c) October,15 2008 by Varsha Hedau, UIUC.  All rights reserved.
function [vp p All_lines]=getVP(imdir,imagename,DO_DISPLAY,savedir)
% getVP Get a triplet of orthogonal vanishing points for an image.
%For details see [1] Varsha Hedau, Derek Hoiem, David Forsyth, Recovering the Spatial
%     Layout of Cluttered Rooms, in the Twelfth IEEE International Conference
%     on Computer Vision, 2009.

% INPUT:
%    image -imagename and image directory

%OUTPUT:
% vp - three orthognal vanishing points
% p - vote for each line and each vanishing point
% All_lines - detected line segments in the image [x1 x2 y1 y2 theta r]

vp=[];p=[];lines=[];
img=imread([imdir imagename]);
%img=imresize(img,500/size(img,1));
[h w k]=size(img);
grayIm=rgb2gray(img);
[All_lines] = getLargeConnectedEdges(grayIm,30);

% chucking out the lines near image boundaries imaging artifacts
inds = find(sum(double(All_lines(:,1:2)>10),2) & sum(double(All_lines(:,1:2)<w-10),2) & ...
    sum(double(All_lines(:,3:4)>10),2) & sum(double(All_lines(:,3:4)<h-10),2));
All_lines = All_lines(inds,:);
All_lines=[All_lines sqrt(((All_lines(:,1)-All_lines(:,2)).^2+(All_lines(:,3)-All_lines(:,4)).^2))];
maxl=max(All_lines(:,7));
imsize = size(grayIm);

%Computing intersections of all the lines
lines = All_lines;
Xpnts = ComputeIntersectionPoints(lines);
inds = find(~isnan(Xpnts(:,1)) & ~isnan(Xpnts(:,2)) & ...
    ~isinf(Xpnts(:,1)) & ~isinf(Xpnts(:,2)));
Xpnts = Xpnts(inds,:);

%Computing votes for every point from all lines
VoteArr = ComputeLinePtVote(lines,Xpnts);
Vote=sum(VoteArr,1);

%get the first point & remove the lines of this point
[vv ii]=sort(Vote,'descend');
vp(1:2)=Xpnts(ii(1),1:2);
Vote1 = VoteArr(:,ii(1));
active_lines = find((Vote1*maxl./All_lines(:,7))<0.8);
inactive_lines = find((Vote1*maxl./All_lines(:,7))>=0.8);
Vote1 = [Vote1(active_lines);Vote1(inactive_lines)];
lines = All_lines(active_lines,:);

%work with the remaining lines
Xpnts = ComputeIntersectionPoints(lines);
inds = find(~isnan(Xpnts(:,1)) & ~isnan(Xpnts(:,2)) & ...
    ~isinf(Xpnts(:,1)) & ~isinf(Xpnts(:,2)));
Xpnts = Xpnts(inds,:);
VoteArr = ComputeLinePtVote([lines;All_lines(inactive_lines,:)],Xpnts);
Vote=sum(VoteArr(1:size(lines,1),:),1);
[vv ii]=sort(Vote,'descend');
Vote = vv(:);
Xpnts=Xpnts(ii,:);
VoteArr = VoteArr(:,ii);
%Remove some of the points
[Xpnts,Vote,VoteArr] = RemoveRedundantPoints2(Xpnts,Vote,VoteArr,w,h);

% Vectorized orthogonality check
[pts2,pts1]=find(~triu(ones(length(Vote))));
npts=length(pts1);
orthochks=[];
for pt=1:100000:npts
    tempinds = [pt:min(pt+100000-1,npts)];
    temp_orthochks=chckothrogonalityvector(...
        ones(length(tempinds),1)*vp(1:2),...
        Xpnts(pts1(tempinds),:),...
        Xpnts(pts2(tempinds),:),w,h);
    orthochks = [orthochks;temp_orthochks(:)];
end
orthos = find(orthochks);
pts1 = pts1(orthos);
pts2 = pts2(orthos);
npts=length(pts1);

% Total vote computation for these points
totVote = zeros(npts,1);
for ln=1:length(Vote1)
    Votes = [Vote1(ln)*ones(npts,1) VoteArr(ln,pts1)' VoteArr(ln,pts2)'];
    Votes = max(Votes,[],2);
    totVote = totVote+Votes;
end
totVote = [pts1(:) pts2(:) totVote(:)];
%     lines = All_lines;

if size(totVote,1) > 0
    [vv ii]=sort(totVote(:,3),'descend');
    vp(3:4) = Xpnts(totVote(ii(1),1),:);
    vp(5:6) = Xpnts(totVote(ii(1),2),:);

    VoteArrTemp = ComputeLinePtVote(All_lines,[vp(1) vp(2);vp(3) vp(4);vp(5) vp(6)]);
    p=[VoteArrTemp.*maxl./repmat(All_lines(:,7),[1 3]) zeros(size(All_lines,1),1)];%4th vp is outliers
    ind=find(max(p(:,1:3),[],2)< 0.5);
    p(ind,4)=1;
    p=p./repmat(sum(p,2),[1 4]);
    %     [vv linemem] = max(VoteArrTemp,[],2);
    [vv linemem] = max(p,[],2);
    %Plot three vps
    if DO_DISPLAY
        figure(1000);plot(vp(1),vp(2),'r*');hold on;
        imagesc(img);hold on;
        plot(vp(1),vp(2),'r*');
        plot(vp(3),vp(4),'g*');
        plot(vp(5),vp(6),'b*');
        % linemem(vv==0) = 4;
        grp1=find(linemem==1);
        grp2=find(linemem==2);
        grp3=find(linemem==3);
        grp4=find(linemem==4);
        plot(All_lines(grp1, [1 2])', All_lines(grp1, [3 4])','r');
        plot(All_lines(grp2, [1 2])', All_lines(grp2, [3 4])','g');
        plot(All_lines(grp3, [1 2])', All_lines(grp3, [3 4])','b');
        plot(All_lines(grp4, [1 2])', All_lines(grp4, [3 4])','c');
        axis ij;axis equal;
        saveas(1000,[savedir imagename(1:end-3) 'fig']);
        close all

    end
    filename=fullfile(savedir,[imagename(1:end-4) '_vp.mat']);
    save(filename,'vp','p','VoteArrTemp','All_lines');
end
return


% .^  Array power.
%    Z = X.^Y denotes element-by-element powers. X and Y must have
%    compatible sizes. In the simplest cases, they can be the same size or
%    one can be a scalar. Two inputs have compatible sizes if, for every
%    dimension, the dimension sizes of the inputs are either the same or one
%    of them is 1.
%
%    C = POWER(A,B) is called for the syntax 'A .^ B' when A or B is an
%    object.
%
%    See also MPOWER, NTHROOT, REALPOW.
%
%    Reference page in Doc Center
%       doc power
%
%    Other functions named power
%
%       codistributed/power    gpuArray/power    sym/power    ts/power
%       fints/power
%


function res = imRescale(img, varargin)
%IMRESCALE Rescale gray levels of image to get better dynamic
%
%   This function has been replaced by function 'imAdjustDynamic', and is
%   now deprecated.
%
%   RES = imRescale(IMG, [GMIN GMAX]);
%   all values below GMIN will be set to 0, all values greater than GMAX
%   will be set to 255, all values in between will be equally spaced
%   between 0 and 255.
%   The result is a 255 grayscale image.
%
%   RES = imRescale(IMG);
%   rescale using min and max values found in image.
%
%   See Also:
%   imLUT, imGrayscaleExtent, imadjust, mat2gray
%
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 21/03/2005.
%

%   HISTORY
%   25/03/2005 correct bug, due to computation with unsigned values
%   21/08/2009 extends to manage double images
%   05/11/2011 deprecated and replace by imAdjustDynamic

warning('matImage:deprecatedFunction', ...
    'function "imRescale" has been deprecated and replaced by "imAdjustDynamic"');

% process input arguments
if ~isempty(varargin)
    % use min and max values given as parameter
    var = varargin{1};
    min1 = double(var(1));
    max1 = double(var(end));
else
    % use min and max values computed from input image
    min1 = double(min(img(:)));
    max1 = double(max(img(:)));
end

% compute slope of linear transformation
a = double(255 / (max1 - min1));

% compute result image
% values below 0 or greater than 255 are automatically clipped when
% casting to uint8
res = uint8((img - min1) * a);


function verify_caffe_crops()

CONTEXT_PAD = 16;
CROP_SIZE = 227;
opts.data_mean = '~/working/caffe-rbg/matlab/caffe/ilsvrc_2012_mean.mat';
d = load(opts.data_mean);
off = floor((256 - 227)/2)+1;
IMAGE_MEAN = d.image_mean(off:off+227-1, off:off+227-1, :);
clear d;
%IMAGE_MEAN(:) = 0;

s = dir('dump/*.txt');
for i = 1:2:length(s)

  info_file = s(i+1).name;
  data_file = s(i).name;
  info = textread(['dump/' info_file], '%s');
  im_path = info{1};
  x1 = str2num(info{2});
  y1 = str2num(info{3});
  x2 = str2num(info{4});
  y2 = str2num(info{5});
  flip = str2num(info{6});
  label = str2num(info{7});
  is_fg = str2num(info{8});
  ov = 0; %str2num(info{9});

  %M1 = textread(['dump/' data_file], '%f');
  fid = fopen(['dump/' data_file], 'rb');
  M1 = fread(fid, 227*227*3, 'single');
  fclose(fid);

  if length(M1) ~= 227*227*3
    warning('skipping wrong size image');
    continue;
  end

  im1 = reshape(M1, [227 227 3]);
  im1 = permute(im1, [2 1 3]);
  im1 = im1(:,:,[3 2 1]);
  im1 = (im1 - min(im1(:))) / (max(im1(:)) - min(im1(:)));
  subplot(1,2,1);
  imagesc(im1);
  axis image;
  title(sprintf('max %.2f min %.2f  label %d  ov %.3f', max(M1), min(M1), label, ov));

  im = imread(im_path);
  im = single(im(:,:,[3 2 1]));
  if CONTEXT_PAD > 0
    scale = CROP_SIZE/(CROP_SIZE - CONTEXT_PAD*2);
    bbox = [x1 y1 x2 y2];
    %figure(1); showboxesc(im/256, bbox, 'b', '-');
    height = bbox(4)-bbox(2)+1;
    width = bbox(3)-bbox(1)+1;
    center = [bbox(1)+width/2 bbox(2)+height/2];
    bbox = round([center center] + [-width -height width height]/2*scale);
    height = bbox(4)-bbox(2)+1;
    width = bbox(3)-bbox(1)+1;
    %figure(1); showboxesc([], bbox, 'r', '-');
    pad_x1 = max(0, 1 - bbox(1));
    pad_y1 = max(0, 1 - bbox(2));
    pad_x2 = max(0, bbox(3) - size(im,2));
    pad_y2 = max(0, bbox(4) - size(im,1));
    bbox(1) = max(1, bbox(1));
    bbox(2) = max(1, bbox(2));
    bbox(3) = min(size(im,2), bbox(3));
    bbox(4) = min(size(im,1), bbox(4));
    window = im(bbox(2):bbox(4), bbox(1):bbox(3), :);

    window_width = round((bbox(3)-bbox(1)+1)*CROP_SIZE/width);
    window_height = round((bbox(4)-bbox(2)+1)*CROP_SIZE/height);
    pad_x1 = round(pad_x1*CROP_SIZE/width);
    pad_y1 = round(pad_y1*CROP_SIZE/height);
    pad_x2 = round(pad_x2*CROP_SIZE/width);
    pad_y2 = round(pad_y2*CROP_SIZE/height);

    pad_h = pad_y1;
    pad_w = pad_x1;
    if flip
      pad_w = pad_x2;
    end

    if pad_h + window_height > CROP_SIZE
      window_height = CROP_SIZE - pad_h;
    end
    if pad_w + window_width > CROP_SIZE
      window_width = CROP_SIZE - pad_w;
    end
    tmp = imresize(window, [window_height window_width], 'bilinear', 'antialiasing', false);
    if flip
      tmp = tmp(:, end:-1:1, :);
    end
    tmp = tmp - IMAGE_MEAN(1+pad_h:window_height+pad_h, 1+pad_w:window_width+pad_w, :);
    %figure(2); window_ = tmp; imagesc((window_-min(window_(:)))/(max(window_(:))-min(window_(:)))); axis image;
    window = zeros(CROP_SIZE, CROP_SIZE, 3, 'single');
    window(1+pad_h:window_height+pad_h, 1+pad_w:window_width+pad_w, :) = tmp;
    %figure(3); imagesc((window-min(window(:)))/(max(window(:))-min(window(:)))); axis image; pause;
    window = permute(window, [2 1 3]);
  else
    window = im(y1:y2, x1:x2, :);
    fprintf('im: %s\n', im_path);
    fprintf('box: %d %d %d %d\n', x1, y1, x2, y2);
    window = imresize(window, [CROP_SIZE CROP_SIZE], 'bilinear', 'Antialiasing', false);
    if flip
      window = window(:, end:-1:1, :);
    end
    window = window - IMAGE_MEAN;
    % permute to make width the fastest dimension (for caffe)
    window = permute(window, [2 1 3]);
  end

  M2 = window(:);

  im2 = reshape(M2, [227 227 3]);
  im2 = permute(im2, [2 1 3]);
  im2 = im2(:,:,[3 2 1]);
  im2 = (im2 - min(im2(:))) / (max(im2(:)) - min(im2(:)));
  subplot(1,2,2);
  imagesc(im2);
  axis image;
  title(sprintf('max %.3f min %.3f', max(M2), min(M2)));

  fprintf('max diff: %f\n', max(abs(M1 - M2)));

  pause;
end


mask = 5*double(instances(:,:,1))+5*double(labels(:,:,1));
maskColor = zeros(size(image,1)*size(image,2),3);
uniquemask = unique(mask(:));
for i =1:length(uniquemask)

    sel = mask(:)==uniquemask(i);

    maskColor(sel, :) = repmat(ObjectColor(uniquemask(i)),sum(sel),1);

end
maskColor(find(mask(:)==0),:) = 1;
maskColor = reshape(maskColor,[size(mask,1),size(mask,2),3]);

figure

imshow(maskColor);
imwrit(maskColor,'NYUmask.png')
%%
load('/n/fs/modelnet/SUN3DV2/prepareGT/cls.mat')

addpath('/n/fs/modelnet/SUN3DV2/roomlayout/')
fullname = '/n/fs/sun3d/data/rgbd_voc/000414_2014-06-04_19-49-13_260595134347_rgbf000044-resize'
data = readframe(fullname);
groundTruthBbs  = data.groundtruth3DBB;

sequenceName = getSequenceName(fullname);
gtRoom3D = GroundTruthBox(sequenceName,0);
cameraXYZ = data.anno_extrinsics'*gtRoom3D;
cameraXYZ([2 3],:) = cameraXYZ([3 2],:);
cameraXYZ(3,:) = - cameraXYZ(3,:);
cameraXYZ = data.Rtilt * cameraXYZ;

my_mhCorner3D = cameraXYZ; %data.Rtilt*data.anno_extrinsics'*gtCorner3D;
visulize_wholeroom(groundTruthBbs,cls,fullname,my_mhCorner3D)


function jac = jacmodhs52(p)
  m=5;

  jac(1, 1:m)=[4.0, -1.0, 0.0, 0.0, 0.0];
  jac(2, 1:m)=[0.0, 1.0, 1.0, 0.0, 0.0];
  jac(3, 1:m)=[0.0, 0.0, 0.0, 1.0, 0.0];
  jac(4, 1:m)=[0.0, 0.0, 0.0, 0.0, 1.0];


function s = i4_to_s_zero ( intval, s_len )

%*****************************************************************************80
%
%% I4_TO_S_ZERO converts an I4 to a string, with zero padding.
%
%  Discussion:
%
%    An I4 is an integer.
%
%  Example:
%
%    Assume that S is 6 characters long:
%
%    INTVAL  S
%
%         1  000001
%        -1  -00001
%         0  000000
%      1952  001952
%    123456  123456
%   1234567  ******  <-- Not enough room%
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 December 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer INTVAL, an integer to be converted.
%
%    Input, integer S_LEN, the length of the string to be used.
%
%    Output, string S, the representation of the integer.
%    The integer will be right justified, and zero padded.
%    If there is not enough space, the string will be filled with stars.
%
  s = [];

  if ( s_len <= 0 )
    return
  end

  ilo = 1;
%
%  Make a copy of the integer.
%
  ival = intval;
%
%  Handle the negative sign.
%
  if ( ival < 0 )

    if ( s_len <= 1 )
      s(1) = '*';
      s = char ( s );
      return
    end

    ival = - ival;
    s(1) = '-';
    ilo = 2;

  end
%
%  Working from right to left, strip off the digits of the integer
%  and place them into S(ILO:S_LEN).
%
  ipos = s_len;

  while ( ival ~= 0 | ipos == s_len )

    idig = mod ( ival, 10 );
    ival = floor ( ival / 10 );

    if ( ipos < ilo )
      s(1:s_len) = '*';
      s = char ( s );
      return
    end

    c = digit_to_ch ( idig );

    s(ipos) = c;
    ipos = ipos - 1;

  end
%
%  Fill the empties with zeroes.
%
  s(ilo:ipos) = '0';
  s = char ( s );

  return
end


%function [varargout] = date_(varargin)
function     [dt_]  = ...
    date_(x, time, strfreq, t0, t1, options)
%    date_(x, time, freq, ndfy, ndfm, ndly, ndlm, turnphase, phase, cycle, thresh, nrep, complete)
% st,trinary,notentp,dura_,ampl_,cumm_,excc_,durcv_,amplcv_,exccv_
%***********************************************************************************************/
%*                                                                                */
%*  Modified BBQ program                                                                       */
%*                                                                                */
%*  Computes turning points and imposes restrictions in one step                               */
%*                                                                                */
%*    NB: rule is peak greater than turnphase on either side,                                  */
%*             trough less than turnphase on either side.           (can be modified)          */
%*    Restriction: Phase quarters/months, Cycle  quarters/months Alternating Troughs and Peaks,*/
%*                  if two peaks(troughs) in a row chooses highest(lowest), Trough must be     */
%*                  lower then preceeding peak, and No turning point within phase length       */
%*                  of end points                                                              */
%*                                                                                */
%*                                                                                */
%*                                                                                */
%*    Date: 15th November 2005                                                                  */
%*     Author: James Engel - Code modified from Adrian Pagan and Don Harding's BBQ code     */
%*
%*     further adjustments  made  by  F. Canova
%***********************************************************************************************/
%

ndfy = t0(1); % year start
ndfm = t0(2); % month/quarter start,
ndly = t1(1); % year start,
ndlm = t1(2); % month/quarter start,

if length(x) ~= length(time)
    error('data (x) and time must have the same length');
end

if strcmp(strfreq,'q') == 1
    freq =1;
    % default settings cycle parameters q data
    turnphase   = 2;
    phase       = 2;        % censoring rules
    cycle       = 5;        % lenght  of cycle

    if ndfm>4 || ndlm>4
        error('Something went wrong. Frequency is quarterly; therefore, starting and ending quarters cannot be larger than 4!');
    end

elseif strcmp(strfreq,'m') == 1
    freq =2;
    % default settings cycle parameters m data
    turnphase   = 6;
    phase       = 6;        % censoring rules
    cycle       = 15;        % lenght  of cycle

else
    error('strfreq must be ''q'' (quarterly) or ''m'' (monthly)');
end

notentp=0;

if freq==1 % q
    nd= 4*(ndly-1-ndfy) + (5-ndfm) + (ndlm);     % Number of data points %
    stepsize = 1/4;
elseif freq==2 % m
    nd= 12*(ndly-1-ndfy) + (13-ndfm) + (ndlm);
    stepsize = 1/12;
end

thresh      = 10.4;     % bypasses phase and cycle restriction if peak to trough is > than thresh
nrep        = 1;        % 1 if analyze real data
complete    = 1;        % if= 1- use complete cycles,if =0 -use incomplete cycles (excess still on complete cycle)

if nargin > 5
    if isfield(options,'turnphase')==1
        turnphase = options.turnphase;
    end
    % censoring rules
    if isfield(options,'phase')==1
        phase = options.phase;
    end
    % lenght of cycle
    if isfield(options,'cycle')==1
        cycle = options.cycle;
    end
    % bypasses phase and cycle restriction if peak to trough is > than thresh
    if isfield(options,'thresh')==1
        thresh = options.thresh;
    end
    % 1 if analyze real data
    if isfield(options,'nrep')==1
        nrep = options.nrep;
    end
    % if= 1- use complete cycles,if =0 -use incomplete cycles (excess still on complete cycle)
    if isfield(options,'complete')==1
        complete = options.complete;
    end

end

pdcv=0;tdcv=0;pacv=0;tacv=0;pdm=0;pdma=0;tdm=0;tdma=0;pdcm=0;tdcm=0;
pdem=0;tdem=0;tdema=0;pdema=0;epcv=0;etcv=0;

[bcp5, bct5, nbp, nbt] = rawall(x(1:nd),turnphase,nd,phase,cycle,thresh);   % calculates turning points with restrictions %

if nbp+nbt<=2
    notentp=notentp+1;
else

    %ntr=nbt;npk=nbp;

    nr  = [nbt;nbp];
    nv  = max(nr);

    %               pdc=zeros(nv,1);tdc=zeros(nv,1);
    %               pda=zeros(nv,1);tda=zeros(nv,1);td=zeros(nv,1);pd=zeros(nv,1);
    pdc = zeros(nv,1);
    tdc = zeros(nv,1); % pde=zeros(nv,1);tde=zeros(nv,1);
    %               tdea=zeros(nv,1);pdea=zeros(nv,1);

    % calculate peak to trough durations & amps
    % p st&s for peaks,t for troughs,p gives cntractions,t expansions
    % code is pd,td is durations; pda,tda is amps; pdc,tdc is cum move pde, tde exces
    % excess is measured differently to avoid case that amps is close to zero in part cycle so that denom can become neg.so use cum movements as denom
    % now vs triangle in early paper

    if bcp5(1,1) < bct5(1,1)      % Peaks are first

        nr  = [nbt;nbp];
        r   = nbt;
        pd  = bct5(1:r,1)-bcp5(1:r,1);
        pda = x(bct5(1:r,1))-x(bcp5(1:r,1));
        k   = 1;

        while k<=r
            %pdc(k)=sumc(x(bcp5(k,1):bct5(k,1),1)-x(bcp5(k,1),1));
            pdc(k)=sum(x(bcp5(k,1):bct5(k,1),1)-x(bcp5(k,1),1));
            k=k+1;
        end
    else                      % troughs are First

        r   = nbt-1;
        pd  = bct5(2:r+1,1)-bcp5(1:r,1);
        pda = x(bct5(2:r+1,1))-x(bcp5(1:r,1));

        k=1;
        while k<=r
            %pdc(k)=sumc(x(bcp5(k,1):bct5(k+1,1),1)-x(bcp5(k,1),1));
            pdc(k)=sum(x(bcp5(k,1):bct5(k+1,1),1)-x(bcp5(k,1),1));
            k=k+1;
        end

        %r1 = r;

    end

    % calculate trough to peak durations & amplitudes

    if bct5(1,1) < bcp5(1,1)        %  Troughs are first
        r   = nbp;
        td  = bcp5(1:r,1)-bct5(1:r,1);
        tda = x(bcp5(1:r,1))-x(bct5(1:r,1));
        k   = 1;
        while k<=r
            %tdc(k)=sumc(x(bct5(k,1):bcp5(k,1),1)-x(bct5(k,1),1));
            tdc(k)=sum(x(bct5(k,1):bcp5(k,1),1)-x(bct5(k,1),1));
            k=k+1;
        end

    else                      % peaks are First
        r   = nbp-1;
        td  = bcp5(2:r+1,1)-bct5(1:r,1);
        tda = x(bcp5(2:r+1,1))-x(bct5(1:r,1));

        k=1;
        while k<=r

            %tdc(k)=sumc(x(bct5(k,1):bcp5(k+1,1),1)-x(bct5(k,1),1));
            tdc(k)=sum(x(bct5(k,1):bcp5(k+1,1),1)-x(bct5(k,1),1));
            k=k+1;
        end

    end
    pdc=pdc(1:size(pd,1));
    tdc=tdc(1:size(td,1));

    % compute excesses
    %excess is percentage of triangle area
    za      = (pd.*pda)/2;
    pde     = 100*(pdc-za-.5*pda)./za;

    pdea    = 100*(pdc-((pd.*pda)/2))./za;
    za      = (td.*tda)/2;
    tde     = 100*(tdc-za-.5*tda)./za;

    tdea    = 100*(tdc-((td.*tda)/2))./za;

    %***********************************************************************************************/

    if complete == 0    % switch: 1-only use complete cycles,0-use incomplete cycles (excess still on complete cycle)%

        bct5u   = bct5;
        bcp5u   = bcp5;

        if bcp5(1,1) < bct5(1,1)     % modifies code to include incomplete cycles %

            bct5u  = [1;bct5];

        elseif 	bct5(1,1) < bcp5(1,1)

            bcp5u = [1;bcp5];

        end

        nbtu = size(bct5u,1);
        nbpu = size(bcp5u,1);

        if bcp5u(nbpu,1) < bct5u(nbtu,1)    % modifies code to include incomplete cycles %

            bcp5u  = [bcp5u;nd];

        elseif 	bct5u(nbt,1) < bcp5u(nbp,1)

            bct5u = [bct5u;nd];

        end

        ntr     = size(bct5u,1);
        npk     = size(bcp5u,1);
        % nr      = [ntr;npk];
        % nv=max(nr);

        %        pdc=zeros(nv,1);tdc=zeros(nv,1);
        %        pda=zeros(nv,1);tda=zeros(nv,1);td=zeros(nv,1);pd=zeros(nv,1);
        %        pdc=zeros(nv,1);tdc=zeros(nv,1);

        if bcp5u(1,1) < bct5u(1,1)       % Peaks are first

            %nr  = [ntr;npk];
            r   = ntr;
            pd  = bct5u(1:r,1)-bcp5u(1:r,1);
            pda = x(bct5u(1:r,1))-x(bcp5u(1:r,1));
            k   = 1;
            while k<=r
                %pdc(k)=sumc(x(bcp5u(k,1):bct5u(k,1),1)-x(bcp5u(k,1),1));
                pdc(k)=sum(x(bcp5u(k,1):bct5u(k,1),1)-x(bcp5u(k,1),1));
                k=k+1;
            end
        else                      % troughs are First
            r   = ntr-1;
            pd  = bct5u(2:r+1,1)-bcp5u(1:r,1);
            pda = x(bct5u(2:r+1,1))-x(bcp5u(1:r,1));
            k   = 1;
            while k<=r
                pdc(k)=sum(x(bcp5u(k,1):bct5u(k+1,1),1)-x(bcp5u(k,1),1));
                %pdc(k)=sumc(x(bcp5u(k,1):bct5u(k+1,1),1)-x(bcp5u(k,1),1));
                k=k+1;
            end

%             r1=r;

        end

        % calculate trough to peak durations & amplitudes

        if bct5u(1,1) < bcp5u(1,1)       %  Troughs are first

            r   = npk;
            td  = bcp5u(1:r,1)-bct5u(1:r,1);
            tda = x(bcp5u(1:r,1))-x(bct5u(1:r,1));
            k   = 1;
            while k<=r
                %tdc(k)=sumc(x(bct5u(k,1):bcp5u(k,1),1)-x(bct5u(k,1),1));
                tdc(k)=sum(x(bct5u(k,1):bcp5u(k,1),1)-x(bct5u(k,1),1));
                k=k+1;
            end

        else                     % peaks are First
            r   = npk-1;
            td  = bcp5u(2:r+1,1)-bct5u(1:r,1);
            tda = x(bcp5u(2:r+1,1))-x(bct5u(1:r,1));

            k=1;
            while k<=r
                %tdc(k)=sumc(x(bct5u(k,1):bcp5u(k+1,1),1)-x(bct5u(k,1),1));
                tdc(k)=sum(x(bct5u(k,1):bcp5u(k+1,1),1)-x(bct5u(k,1),1));
                k=k+1;
            end

        end
        pdc=pdc(1:size(pd,1));
        tdc=tdc(1:size(td,1));

    end

    %***********************************************************************************************/

    % cumulate.when nrep=1 then gives raw data,otherwsise sums over monte carlo amounts

    pdcv    = pdcv + stdc(pd)/meanc(pd);       % compute cv's
    tdcv    = tdcv + stdc(td)/meanc(td);
    pacv    = pacv + stdc(pda)/meanc(pda);
    tacv    = tacv + stdc(tda)/meanc(tda);
    epcv    = epcv + stdc(pde)/meanc(pde);  %  cv of xss
    etcv    = etcv + stdc(tde)/meanc(tde);

    pdm     = pdm+meanc(pd);          % durations
    pdma    = pdma+meanc(pda);

    tdm     = tdm+meanc(td);          % durations
    tdma    = tdma+meanc(tda);

    pdcm    = pdcm+meanc(pdc);       % cumulative
    tdcm    = tdcm+meanc(tdc);

    % pdem=pdem+meanc(pde);
    tdem    = tdem+meanc(tde);       % xss
    tdema   = tdema+meanc(tdea);
    pdem    = pdem+meanc(pde);
    pdema   = pdema+meanc(pdea);

end

if nrep==1&&(nbp+nbt>2)

    %nbp and nbt=number of peaks and number of troughs %

    disp('peaks at')
    fprintf('%.2f\n', time(bcp5))
    %disp(bcp5(1:nbp))

    disp('troughs at')
    fprintf('%.2f\n', time(bct5))
    %disp(bct5(1:nbt))

    % total  number of  turning  points
    ttp=length(bcp5)+length(bct5);

    % collecting  turning  points in  trinary  indicator
    trinary=zeros(length(x),1);
    trinary(bcp5)=1;
    trinary(bct5)=-1;

end

if nrep==1&&(nbp+nbt>2)
    %remove below if want states ...
    %st=zeros(nd,1);

    [st] = states(bcp5,bct5,nbp,nbt,nd);
    dt_.st(:,nrep) = st;

    %determine obs in which states have been completed%...
    na  = min([bct5(1);bcp5(1)])';
    nb  = max([bct5(nbt);bcp5(nbp)])';
    %nb-na+1;
    z   = [x(na:nb) st(na:nb)];

    %disp('dated series')
    %z1= [time(time_start+1:time_end)' x st];
    %format  short  g
    %round(z1, 2)
    %%fprintf('%.2f\n', '%.2f\n', '%.2f\n', z1)

end
nrep1   = nrep-notentp;

% Compute final statistics
% duration contractions/duration expansions
dt_.dura = [pdm/nrep1 tdm/nrep1];
% amplitudes contractions/amplitude expansions
dt_.ampl  = 100*[pdma/nrep1 tdma/nrep1];
% cumulative contractions/cumulative expansions
dt_.cumm  = 100*[pdcm/nrep1 tdcm/nrep1];
% excess movements percent of triangle area
% contractions/expansions
dt_.excc  = [pdem/nrep1 tdem/nrep1];
% cv of durations contractions/expansions
dt_.durcv = [pdcv/nrep1 tdcv/nrep1];
% cv of amplitude contractions/expansions
dt_.amplcv = [pacv/nrep1 tacv/nrep1];
% cv of excess movements contractions/expansions
dt_.exccv = [epcv/nrep1 etcv/nrep1];
%
dt_.trinary = trinary;
%
dt_.notentp = notentp;

function m=meanc(x)
% get mean of each column
m=mean(x);
if size(m,2)>1
   m=m';
end

function m=stdc(x)
% get standard deviation of each column
m=std(x);
if size(m,2)>1
   m=m';
end


% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------
function [ bdry ]  = seg2bdry_wt(seg, fmt)
if nargin<2, fmt = 'doubleSize'; end;

if ~strcmp(fmt,'imageSize') && ~strcmp(fmt,'doubleSize'),
    error('possible values for fmt are: imageSize and doubleSize');
end

[tx, ty, nch] = size(seg);

if nch ~=1,
    error('seg must be a scalar image');
end

bdry = zeros(2*tx+1, 2*ty+1);

edgels_v = abs( seg(1:end-1, :) - seg(2:end, :) );
edgels_v(end+1, :) = 0;
edgels_h = abs( seg(:, 1:end-1) - seg(:, 2:end) );
edgels_h(:, end+1) = 0;

bdry(3:2:end, 2:2:end) = edgels_v;
bdry(2:2:end, 3:2:end) = edgels_h;
bdry(3:2:end-1, 3:2:end-1)= max ( max(edgels_h(1:end-1, 1:end-1), edgels_h(2:end, 1:end-1)), max(edgels_v(1:end-1,1:end-1), edgels_v(1:end-1,2:end)) );

bdry(1, :) = bdry(2, :);
bdry(:, 1) = bdry(:, 2);
bdry(end, :) = bdry(end-1, :);
bdry(:, end) = bdry(:, end-1);

if strcmp(fmt,'imageSize'),
    bdry = bdry(3:2:end, 3:2:end);
end


function L = probitLikelihood(noise, mu, varsigma, y)

% PROBITLIKELIHOOD Likelihood of data under probit noise model.

% NOISE

D = size(y, 2);
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
L = cumGaussian((y.*mu)./(sqrt(noise.sigma2+varsigma)));


function [model, top] = rbmBB(X, numhid, varargin)
%Learn RBM with Bernoulli hidden and visible units
%This is not meant to be applied to image data
%code by Andrej Karpathy
%based on implementation of Kevin Swersky and Ruslan Salakhutdinov

%INPUTS:
%X              ... data. should be binary, or in [0,1] to be interpreted
%               ... as probabilities
%numhid         ... number of hidden layers

%additional inputs (specified as name value pairs or in struct)
%method         ... CD or SML
%eta            ... learning rate
%momentum       ... momentum for smoothness amd to prevent overfitting
%               ... NOTE: momentum is not recommended with SML
%maxepoch       ... # of epochs: each is a full pass through train data
%avglast        ... how many epochs before maxepoch to start averaging
%               ... before. Procedure suggested for faster convergence by
%               ... Kevin Swersky in his MSc thesis
%penalty        ... weight decay factor
%batchsize      ... The number of training instances per batch
%verbose        ... For printing progress
%anneal         ... Flag. If set true, the penalty is annealed linearly
%               ... through epochs to 10% of its original value

%OUTPUTS:
%model.type     ... Type of RBM (i.e. type of its visible and hidden units)
%model.W        ... The weights of the connections
%model.b        ... The biases of the hidden layer
%model.c        ... The biases of the visible layer
%model.top      ... The activity of the top layer, to be used when training
%               ... DBN's
%errors         ... The errors in reconstruction at every epoch

%Process options
%if args are just passed through in calls they become cells
if (isstruct(varargin))
    args= prepareArgs(varargin{1});
else
    args= prepareArgs(varargin);
end
[   method        ...
    eta           ...
    momentum      ...
    maxepoch      ...
    avglast       ...
    penalty       ...
    batchsize     ...
    verbose       ...
    anneal        ...
    ] = process_options(args    , ...
    'method'        ,  'CD'     , ...
    'eta'           ,  0.1      , ...
    'momentum'      ,  0.5      , ...
    'maxepoch'      ,  50       , ...
    'avglast'       ,  5        , ...
    'penalty'       , 2e-4      , ...
    'batchsize'     , 100       , ...
    'verbose'       , false     , ...
    'anneal'        , false);
avgstart = maxepoch - avglast;
oldpenalty= penalty;
[N,d]=size(X);

if (verbose)
    fprintf('Preprocessing data...\n');
end

%Create batches
numcases=N;
numdims=d;
numbatches= ceil(N/batchsize);
groups= repmat(1:numbatches, 1, batchsize);
groups= groups(1:N);
perm=randperm(N);
groups = groups(perm);
for i=1:numbatches
    batchdata{i}= X(groups==i,:);
end

%train RBM
W = 0.1*randn(numdims,numhid);
c = zeros(1,numdims);
b = zeros(1,numhid);
ph = zeros(numcases,numhid);
nh = zeros(numcases,numhid);
phstates = zeros(numcases,numhid);
nhstates = zeros(numcases,numhid);
negdata = zeros(numcases,numdims);
negdatastates = zeros(numcases,numdims);
Winc  = zeros(numdims,numhid);
binc = zeros(1,numhid);
cinc = zeros(1,numdims);
Wavg = W;
bavg = b;
cavg = c;
t = 1;
errors=zeros(1,maxepoch);

for epoch = 1:maxepoch

	errsum=0;
    if (anneal)
        %apply linear weight penalty decay
        penalty= oldpenalty - 0.9*epoch/maxepoch*oldpenalty;
    end

    for batch = 1:numbatches
		[numcases numdims]=size(batchdata{batch});
		data = batchdata{batch};

%         %go up
% 		ph = logistic(data*W + repmat(b,numcases,1));
% 		phstates = ph > rand(numcases,numhid);
%         if (isequal(method,'SML'))
%             if (epoch == 1 && batch == 1)
%                 nhstates = phstates;
%             end
%         elseif (isequal(method,'CD'))
%             nhstates = phstates;
%         end

        %%%%%%%%% START POSITIVE PHASE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        poshidprobs = 1./(1 + exp(-data*W - repmat(b,numcases,1)));
        posprods    = data' * poshidprobs;
        poshidact   = sum(poshidprobs);
        posvisact = sum(data);

        %%%%%%%%% END OF POSITIVE PHASE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        poshidstates = poshidprobs > rand(numcases,numhid);

%         %go down
% 		negdata = logistic(nhstates*W' + repmat(c,numcases,1));
% 		negdatastates = negdata > rand(numcases,numdims);
%
%         %go up one more time
% 		nh = logistic(negdatastates*W + repmat(b,numcases,1));
% 		nhstates = nh > rand(numcases,numhid);

        %%%%%%%%% START NEGATIVE PHASE  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        negdata = 1./(1 + exp(-poshidstates*W' - repmat(c,numcases,1)));
        neghidprobs = 1./(1 + exp(-negdata*W - repmat(b,numcases,1)));
        negprods  = negdata'*neghidprobs;
        neghidact = sum(neghidprobs);
        negvisact = sum(negdata);

        %%%%%%%%% END OF NEGATIVE PHASE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        err= sum(sum( (data-negdata).^2 ));
        errsum = err + errsum;

        if epoch>5,
            this_momentum=1.8*momentum;
        else
            this_momentum=momentum;
        end;

%         %update weights and biases
%         dW = (data'*ph - negdatastates'*nh);
%         dc = sum(data) - sum(negdatastates);
%         db = sum(ph) - sum(nh);
% 		Winc = momentum*Winc + eta*(dW/numcases - penalty*W);
% 		binc = momentum*binc + eta*(db/numcases);
% 		cinc = momentum*cinc + eta*(dc/numcases);
% 		W = W + Winc;
% 		b = b + binc;
% 		c = c + cinc;

        %%%%%%%%% UPDATE WEIGHTS AND BIASES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        Winc = this_momentum*Winc + eta*( (posprods-negprods)/numcases - penalty*W);
        binc = this_momentum*binc + (eta/numcases)*(poshidact-neghidact);
        cinc = this_momentum*cinc + (eta/numcases)*(posvisact-negvisact);

        W = W + Winc;
        c = c + cinc;
        b = b + binc;

        %%%%%%%%%%%%%%%% END OF UPDATES %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%         if (epoch > avgstart)
%             %apply averaging
% 			Wavg = Wavg - (1/t)*(Wavg - W);
% 			cavg = cavg - (1/t)*(cavg - c);
% 			bavg = bavg - (1/t)*(bavg - b);
% 			t = t+1;
% 		else
			Wavg = W;
			bavg = b;
			cavg = c;
%         end

        %accumulate reconstruction error
        err= sum(sum( (data-negdata).^2 ));
		errsum = err + errsum;

    end

    errors(epoch)=errsum;
    if (verbose)
        fprintf('Ended epoch %i/%i. Reconstruction error is %f\n', ...
            epoch, maxepoch, errsum);
    end
end

model.type= 'BB';
top = logistic(X*Wavg + repmat(bavg,N,1));
model.W= Wavg;
model.b= bavg;
model.c= cavg;


function []=uw_interp();
%UW_INTERP Interpolate grid using nearest neighbour
%
%   Andy Hooper May 2007
%
%   ============================================================================
%   01/2012 AH: Speed up read/write for triangle
%   01/2013 AH: Replace dsearch by dsearchn only for versions 2012 onwards
%   08/2014 DB: Suppress command line output
%   09/2015 AH: use matlab triangulation if triangle program not installed
%   ============================================================================

fprintf('Interpolating grid...\n')

uw=load('uw_grid','n_ps','n_ifg','nzix');

arch=computer('arch');
if strcmpi(arch(1:3),'win')
    use_triangle='n';
else
    tripath=system('which triangle >& /dev/null');
    if tripath==0
        use_triangle='y';
    else
        use_triangle='n';
    end
end

[y,x]=find(uw.nzix);
xy=[[1:uw.n_ps]',x,y];

if use_triangle=='y'
    nodename=['unwrap.1.node'];
    fid=fopen(nodename,'w');
    fprintf(fid,'%d 2 0 0\n',uw.n_ps);

    fprintf(fid,'%d %d %d\n',xy');
    fclose(fid);

    [a,b] = system('triangle -e unwrap.1.node > triangle.log');

    fid=fopen('unwrap.2.edge','r');
    header=str2num(fgetl(fid));
    N=header(1);
    edgs=fscanf(fid,'%d %d %d %d\n',[4,N])';
    fclose(fid);
    n_edge=size(edgs,1);
    if n_edge~=N
        error('missing lines in unwrap.2.edge')
    end

    fid=fopen('unwrap.2.ele','r');
    header=str2num(fgetl(fid));
    N=header(1);
    ele=fscanf(fid,'%d %d %d %d\n',[4,N])';
    fclose(fid);
    n_ele=size(ele,1);
    if n_ele~=N
        error('missing lines in unwrap.2.ele')
    end
else
    xy=double(xy);
    ele=delaunay(xy(:,2),xy(:,3));
    tr=triangulation(ele,xy(:,2),xy(:,3));
    edgs=edges(tr);
    n_edge=size(edgs,1);
    edgs=[[1:n_edge]',edgs];
    n_ele=size(ele,1);
    ele=[[1:n_ele]',ele];
end

z=[1:uw.n_ps];
[nrow,ncol]=size(uw.nzix);

[X,Y]=meshgrid(1:ncol,1:nrow);
matlab_version=version('-release');
if str2num(matlab_version(1:4))<2012
    Z=dsearch(x,y,ele(:,2:4),X,Y); % dsearch removed in MatlabR2012a
else
    Z=dsearchn([x,y],ele(:,2:4),[X(:),Y(:)]); %index from grid to pixel node
    Z = reshape(Z,nrow,ncol);
end
Zvec=Z(:);
grid_edges=[Zvec(1:end-nrow),Zvec(nrow+1:end)]; % col edges
Zvec=reshape(Z',nrow*ncol,1);
grid_edges=[grid_edges;[Zvec(1:end-ncol),Zvec(ncol+1:end)]]; % add row edges
[sort_edges,I_sort]=sort(grid_edges,2); % sort each edge to have lowest pixel node first
edge_sign=I_sort(:,2)-I_sort(:,1);
[alledges,I,J]=unique(sort_edges,'rows'); % grid_edges=alledges(J)
sameix=(alledges(:,1)==alledges(:,2));
alledges(sameix,:)=0; % set edges connecting identical nodes to (0,0)
[edgs,I2,J2]=unique(alledges,'rows');
n_edge=size(edgs,1)-1;
edgs=[[1:n_edge]',edgs(2:end,:)]; % drop (0,0)
gridedgeix=(J2(J)-1).*edge_sign; % index to edges
colix=reshape(gridedgeix(1:nrow*(ncol-1)),nrow,ncol-1);
rowix=reshape(gridedgeix(nrow*(ncol-1)+1:end),ncol,nrow-1)';

fprintf('   Number of unique edges in grid: %d\n',n_edge);

save('uw_interp','edgs','n_edge','rowix','colix','Z');


function [Rnew,Ki]=finvupdate(a,R,k_newvsold,knew)

k=k_newvsold;
k22=knew;
e=R'*R*k;
g= 1/   sqrt(k22-k'*R'*R*k);
Rnew=[ R,zeros(length(k),1);-g*e',g];
if(nargout==2)
    Ki=Rnew'*Rnew;
end


function L = cmpndNoiseLogLikelihood(noise, mu, varsigma, y)

% CMPNDNOISELOGLIKELIHOOD Log likelihood of the data under the CMPND noise model.
% FORMAT
% DESC returns the log likelihood of a data set under the  compound noise model.
% ARG noise : the noise structure for which the log likelihood is required.
% ARG mu : input mean locations for the log likelihood.
% ARG varSigma : input variance locations for the log likelihood.
% ARG y : target locations for the log likelihood.
%
% SEEALSO : cmpndNoiseParamInit, cmpndNoiseLikelihood, noiseLogLikelihood
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

L = 0;
for i = 1:length(noise.comp)
  L = L + noiseLogLikelihood(noise.comp{i}, ...
			     mu(:, i), ...
			     varsigma(:, i), ...
			     y(:, i));
end


function []=olsss(Y,X,n,m,p,Bhat,stringdates1,decimaldates1,endo,pref)

% function []=olsss(Y,X,n,m,p,Bhat,stringdates1,decimaldates1,endo,datapath)
% calculates and displays the steady-state for the OLS VAR
% inputs:  - matrix 'Y': matrix of regressands for the VAR model (defined in 1.1.8)
%          - matrix 'X': matrix of regressors for the VAR model (defined in 1.1.8)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'm': number of exogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'p': number of lags included in the model (defined p 7 of technical guide)
%          - matrix 'Bhat': OLS VAR coefficients, in non vectorised form (defined in 1.1.9)
%          - cell 'stringdates1': date strings for the sample period
%          - vector 'decimaldates1': dates converted into decimal values, for the sample period
%          - cell 'endo': list of endogenous variables of the model
%          - string 'datapath': user-supplied path to excel data spreadsheet
% outputs: none

% this function estimates the steady-state, using (a.7.6)

% first compute the steady-state values

% recover the coefficient matrices A1,...,Ap and C, as defined in (1.1.2)
% first, calculate B and take its transpose BT
BT=Bhat';
% estimate the summation term I-A1-...-Ap in
summation=eye(n);
for jj=1:p
    summation=summation-BT(:,(jj-1)*n+1:jj*n);
end
% recover C
C=BT(:,end-m+1:end);
% now calculate the product of the inverse of the summation with C
product=summation\C;
% keep only the exogenous regressor part of X
X_exo=X(:,end-m+1:end)';
% compute the steady-state values
ssvalues=product*X_exo;

% create steady-state figure
if pref.plot
    sstate=figure('Tag','BEARresults');
    set(sstate,'Color',[0.9 0.9 0.9]);
    set(sstate,'name','steady-state');
    ncolumns=ceil(n^0.5);
    nrows=ceil(n/ncolumns);
    for ii=1:n
        subplot(nrows,ncolumns,ii);
        hold on
        ss=plot(decimaldates1,ssvalues(ii,:),'Color',[0.4 0.4 1],'LineWidth',2);
        actual=plot(decimaldates1,Y(:,ii),'Color',[0 0 0],'LineWidth',2);
        plot([decimaldates1(1,1),decimaldates1(end,1)],[0 0],'k--');
        hold off
        minband=min(min(ssvalues(ii,:)),min(Y(:,ii)));
        maxband=max(max(ssvalues(ii,:)),max(Y(:,ii)));
        space=maxband-minband;
        Ymin=minband-0.2*space;
        Ymax=maxband+0.2*space;
        set(gca,'XLim',[decimaldates1(1,1) decimaldates1(end,1)],'YLim',[Ymin,Ymax],'FontName','Times New Roman');
        title(endo{ii,1},'FontName','Times New Roman','FontSize',10,'interpreter','none');
        if ii==1
            plotlegend=legend([ss,actual],'steady-state','actual');
            set(plotlegend,'FontName','Times New Roman');
        end
    end
end

% finally, save in excel

% create the cell that will be saved on excel
sscell={};
% build preliminary elements: space between the tables
vertspace=repmat({''},size(stringdates1,1)+3,1);
% loop over variables (horizontal dimension)
for ii=1:n
    % create cell of steady-state record for variable ii
    temp=['steady-state and actual: ' endo{ii,1}];
    ss_i=[temp {''} {''};{''} {''} {''};{''} {'sample'} {'median'};stringdates1 num2cell(Y(:,ii)) num2cell(ssvalues(ii,:))'];
    sscell=[sscell ss_i vertspace];
end
% trim
sscell=sscell(:,1:end-1);
% write in excel
if pref.results==1
    bear.xlswritegeneral(fullfile(pref.results_path, [pref.results_sub '.xlsx']),sscell,'steady state','B2');
end


function xform = mrBaseXform(base, map);
%
% xform = mrBaseXform(base, map);
%
% Compute a 4x4 transform matrix to translate from
% data coords in a map volume to data coords in a
% base volume.
%
% base and map are both MR objects (see mrLoad). Will
% prompt if either is not specified.
%
%
% ras 07/05.
if ~exist('base', 'var') | isempty(base),  base = mrLoad; end
if ~exist('map', 'var') | isempty(map),  map = mrLoad; end

% first,  check if the two MR volumes cover the same space
% at the same resolution: then they're already in the same
% coordinate space and the xform is just an identity matrix:
if isequal(base.dims(1:3), map.dims(1:3)) & ...
        isequal(base.voxelSize(1:3), map.voxelSize(1:3))
    disp('Base and map volumes are coregistered')
    xform = eye(4);
    return
end

% next,  check that they don't specify the same extent
% in 3D space,  even if they may be at different resolutions:
if isequal(base.extent(1:3), map.extent(1:3)) | ...
        abs(base.extent(1:3)-map.extent(1:3)) < [1 1 1]
    disp('Base and map cover same extent -- scaling map to fit base')
    szRatio = map.voxelSize(1:3) ./ base.voxelSize(1:3);
    xform = affineBuild([0 0 0], [0 0 0], szRatio);
    return
end

% otherwise,  check if they share a common coordinate space.
% First,  remove the standard spaces (raw data in pix/mm,  L/R
% flipped -- see mrStandardSpaces):
spacesA = {base.spaces.name};
spacesB = {map.spaces.name};
std = {'Raw Data in Pixels' 'Raw Data in mm' 'L/R Flipped'};
spacesA = setdiff(spacesA, std);
spacesB = setdiff(spacesB, std);
common = intersect(spacesA, spacesB);

if isempty(common)
    error('base and map do not share any common coordinate spaces!');
end

% if we're here,  this means there is a common coord system.
% use the first common space (it shouldn''t matter which one
% is used) to derive the xform:
common = common{1};
fprintf('Coregistering via common space %s \n', common);
spacesA = {base.spaces.name};
spacesB = {map.spaces.name};
iA = cellfind(spacesA, common);
iB = cellfind(spacesB, common);
xform = inv(base.spaces(iA).xform) * map.spaces(iB).xform;

return


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function writeG2oConstraint(file_id, from_id, to_id, T_from_to, covariance)
% Adapted from Luca Carlone's
% https://bitbucket.org/lucacarlone/pgo3d-duality-opencode/src/ebb6e1b8cebaad7f2aaf581b1d0c0bad737faebb/lib/writeG2oDataset3D.m?at=master&fileviewer=file-view-default

dt = T_from_to(1:3, 4);
dx = dt(1); dy = dt(2); dz = dt(3);
dR = T_from_to(1:3, 1:3);
dq = rot2quat(dR);
assert(all(imag(dq)==0));
if norm(dq)>1e-3
    dq = dq/norm(dq);
else
    error('norm close to zero for unit quaternion (2)')
end
dqw = dq(1); dqx = dq(2); dqy = dq(3); dqz = dq(4);
I = covariance;

fprintf(file_id,'EDGE_SE3:QUAT %d %d   %f %f %f   %.7f %.7f %.7f %.7f   %f %f %f %f %f %f   %f %f %f %f %f   %f %f %f %f   %f %f %f   %f %f   %f\n', ...
    from_id, to_id, dx, dy, dz, dqx, dqy, dqz, dqw, ...
    I(1,1), I(1,2), I(1,3), I(1,4), I(1,5), I(1,6), ...
    I(2,2), I(2,3), I(2,4), I(2,5), I(2,6), ...
    I(3,3), I(3,4), I(3,5), I(3,6), ...
    I(4,4), I(4,5), I(4,6), ...
    I(5,5), I(5,6), ...
    I(6,6));

end


% test for drawing a spherical triangulation

rep = '../toolbox_graph_data/wrl/';
name = 'pawn';
name = 'test2';
name = 'test1';
[vertex,face] = read_wrl([rep name '.wrl']);

% project on sphere [should use here a spherical parameterization instead]
d = sqrt(sum(vertex.^2));
svertex = vertex./repmat(d, [3 1]);

% draw simply the projected mesh
clf;
plot_mesh(svertex,face);
shading faceted;

% now draw nice curve on the sphere
options.target_face = round(size(svertex,2)/2);
clf;
plot_spherical_triangulation(svertex,face, options);


function cvx_optval = huber_pos( x, M, t ) %#ok

%HUBER   Internal cvx version.

%
% Check arguments
%

error( nargchk( 1, 3, nargin ) );
if ~cvx_isconvex( x ),
    error( 'Disciplined convex programming error:\n    HUBER_POS is convex and nondecreasing in X, so X must be convex.', 1 ); %#ok
end
if nargin < 2,
    M = 1;
elseif isa( M, 'cvx' ),
    error( 'Second argument must be numeric.' );
elseif ~isreal( M ) || any( M( : ) <= 0 ),
    error( 'Second argument must be real and positive.' );
end
if nargin < 3,
    t = 1;
elseif ~isreal( t ),
    error( 'Third argument must be real.' );
elseif cvx_isconstant( t ) && nnz( cvx_constant( t ) <= 0 ),
    error( 'Third argument must be real and positive.' );
elseif ~cvx_isconcave( t ),
    error( 'Disciplined convex programming error:\n    HUBER_POS is convex and nonincreasing in T, so T must be concave.', 1 ); %#ok
end
sz = cvx_size_check( x, M, t );
if isempty( sz ),
    error( 'Sizes are incompatible.' );
end

%
% Compute result
%

cvx_begin separable
    variables v( sz ) w( sz )
    minimize( quad_over_lin( w, t, 0 ) + 2 .* M .* v )
    x <= w + v; %#ok
    w <= M * t; %#ok
    v >= 0; %#ok
cvx_end

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


function [jointTorques_star, LDot_tilde, L_tilde, intL_tilde, xiDes, f_des, xiDot_star] = momentumJerkControlYoga(intLTilde_angular, nu, J_CoM, JL, JR, xCoM, w_H_l_sole, w_H_r_sole, L, M, h, JDotL_nu, JDotR_nu, x_dx_ddx_dddx_CoM_des, ...
                                                                                gainsPCOM, gainsDCOM, LEFT_RIGHT_FOOT_IN_CONTACT, qj, qj_dqj_ddqj_des, impedances, dampings, ...
                                                                                contactForces_est, xi_star, xi, contactForces_star, Config, Gain, Reg)

    % MOMENTUMJERKCONTROL implements a momentum-based jerk controller.
    %
    % REFERENCES: IEEE-RAL 2018, "Momentum Control of an Underactuated Flying Humanoid Robot";
    %

    %% ------------Initialization----------------

    % parameters
    ndof            = size(M(7:end,7:end),1);

    % ROBUSTNESS w.r.t. modeling errors (inertial parameters)
    if Config.testRobustness

        M           = M * Config.robustnessFactor;
        h           = h * Config.robustnessFactor;
    end

    % get the robot total mass, and gravity forces, and feet positions
    m               = M(1,1);
    f_grav          = m*Config.GRAVITY_ACC*[0; 0; 1; zeros(3,1)];
    posLeftFoot     = w_H_l_sole(1:3,4);
    posRightFoot    = w_H_r_sole(1:3,4);

    % compute momentum references
    [LDDot_des, LDot_des, L_des, intL_des] = computeMomentumReferences(x_dx_ddx_dddx_CoM_des, m);

    %% %%%%%%%%%%%%%%% COMPUTE THE MOMENTUM ACCELERATION %%%%%%%%%%%%%%% %%
    %
    % WHILE BALANCING: the momentum acceleration is defined as follows:
    %
    %   LDDot = Ac * Beta* xiDot + AcDot * contactForces. (1)
    %
    % The contact forces derivative have been parametrized as follows:
    %
    %   contactForcesDot = Beta * xiDot
    %
    % xiDot is then used as control input. Beta is an invertible matrix.
    % It is therefore necessary to compute AcDot and Ac.

    % compute matrix Ac for both feet
    r_left          = posLeftFoot  - xCoM;
    r_right         = posRightFoot - xCoM;

    Ac_leftFoot     = [eye(3),           zeros(3);
                       wbc.skew(r_left), eye(3)];

    Ac_rightFoot    = [eye(3),            zeros(3);
                       wbc.skew(r_right), eye(3)];

    % compute matrix Beta_left and Beta_right
    Beta_left       = computeParametrizationGradient(xi(1:6),   Config);
    Beta_right      = computeParametrizationGradient(xi(7:end), Config);

    % multiplier of contact forces derivatives
    Ac              = [Ac_leftFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(1), ...
                       Ac_rightFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(2)];
    Ac_Beta         = [Ac_leftFoot * Beta_left .* LEFT_RIGHT_FOOT_IN_CONTACT(1), ...
                       Ac_rightFoot * Beta_right .* LEFT_RIGHT_FOOT_IN_CONTACT(2)];

    % compute the time derivative of r_left and r_right
    nu_leftFoot     = JL * nu;
    nu_rightFoot    = JR * nu;
    v_CoM           = J_CoM(1:3,:) * nu;
    rDot_left       = nu_leftFoot(1:3)  - v_CoM;
    rDot_right      = nu_rightFoot(1:3) - v_CoM;

    % compute AcDot
    AcDot_leftFoot  = [zeros(3),             zeros(3);
                       wbc.skew(rDot_left),  zeros(3)];

    AcDot_rightFoot = [zeros(3),             zeros(3);
                       wbc.skew(rDot_right), zeros(3)];

    AcDot           = [AcDot_leftFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(1), ...
                       AcDot_rightFoot .* LEFT_RIGHT_FOOT_IN_CONTACT(2)];

    %% %%%%%%%%%%%%%%% MOMENTUM CONTROLLER DEFINITION  %%%%%%%%%%%%%%%%% %%
    %
    % The control input of Eq. (1) is:
    %
    %  u1 = xiDot
    %
    % For the purpose of this documentation, it suffices to know that the
    % robot momentum converges to the desired values if the following
    % equation is verified:
    %
    %  ATilde * u1 + deltaTilde = 0 (2)
    %
    % where: ATilde and deltaTilde must be properly calculated.
    % For details on the controller implementation, see the RAL 2018 paper
    % at: https://ieeexplore.ieee.org/document/7997895.

    % Compute the momentum error derivative/integral
    LDot_estimated  = Ac * [contactForces_est(1:6) .* LEFT_RIGHT_FOOT_IN_CONTACT(1); ...
                            contactForces_est(7:end) .* LEFT_RIGHT_FOOT_IN_CONTACT(2)] - f_grav;
    LDot_tilde      = LDot_estimated - LDot_des;
    L_tilde         = L - L_des;
    intL_tilde      = [(m * xCoM - intL_des(1:3)); intLTilde_angular];

    % Get the gains matrices
    KP_momentum     = blkdiag(diag(gainsPCOM), Gain.KP_AngularMomentum .*eye(3));
    KD_momentum     = blkdiag(diag(gainsDCOM), Gain.KD_AngularMomentum .*eye(3));

    % CONTROL LAW: momentum-based control with Lyapunov stability (IEEE-RAL 2018)
    KTilde          = KP_momentum + inv(Gain.KO_momentum) + KD_momentum;
    deltaTilde      = KTilde * L_tilde + (KD_momentum + eye(6)) * LDot_tilde + ...
                      KP_momentum * intL_tilde - LDDot_des + AcDot * contactForces_est;

    % Primary task control input
    pinvAc_Beta       = [wbc.pinvDamped(Ac_leftFoot * Beta_left,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(2)); zeros(6)] + ...
                        [zeros(6); wbc.pinvDamped(Ac_rightFoot * Beta_right,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(1))] + ...
                         wbc.pinvDamped(Ac_Beta,1e-5).* LEFT_RIGHT_FOOT_IN_CONTACT(1).* LEFT_RIGHT_FOOT_IN_CONTACT(2);

    xiDot_primaryTask = -pinvAc_Beta * deltaTilde;

    % Null space projector (forces)
    Null_Ac_Beta      = (eye(12) - pinvAc_Beta*Ac_Beta)*LEFT_RIGHT_FOOT_IN_CONTACT(1)*LEFT_RIGHT_FOOT_IN_CONTACT(2);

    %% %%%%%%%%%%%%%%%%% TORQUE CONTROLLER DEFINITION  %%%%%%%%%%%%%%%%% %%

    % Get the required matrices
    Jc                = [JL .*LEFT_RIGHT_FOOT_IN_CONTACT(1); JR .*LEFT_RIGHT_FOOT_IN_CONTACT(2)];
    JcDot_nu          = [JDotL_nu .*LEFT_RIGHT_FOOT_IN_CONTACT(1); JDotR_nu .*LEFT_RIGHT_FOOT_IN_CONTACT(2)];
    B                 = [zeros(6,ndof); eye(ndof)];
    Lambda            = Jc/M*B;
    pinvLambda        = wbc.pinvDamped(Lambda, Reg.pinvDamp);
    NLambda           = eye(ndof) - pinvLambda*Lambda;

    % terms required for calculating u_0
    qjDot             = nu(7:end);
    KP_torqueControl  = diag(impedances);
    KD_torqueControl  = diag(dampings);
    jointPos_err      = qj    - qj_dqj_ddqj_des(:,1);
    jointVel_err      = qjDot - qj_dqj_ddqj_des(:,2);

    % consitency with the current gain tuning
    USE_MBAR          = false;
    USE_ACC           = 0;

    if USE_MBAR

        MBar          = (M(7:end,7:end) - M(7:end,1:6)/M(1:6,1:6)*M(1:6,7:end)); %#ok<UNRCH>
    else
        MBar          = eye(ndof);
    end

    u_0               = MBar*(USE_ACC*qj_dqj_ddqj_des(:,3) - KD_torqueControl * jointVel_err - KP_torqueControl * jointPos_err);

    % terms required for calculating tau_0
    tau_0             = h(7:end) -M(7:end,1:6)/M(1:6,1:6)*h(1:6) ...
                       -transpose(Jc(:,7:end))*contactForces_est + ...
                        M(7:end,1:6)/M(1:6,1:6)*transpose(Jc(:,1:6))*contactForces_est + u_0;

    jointTorques_star = pinvLambda*(Jc/M*(h-Jc'*contactForces_star) -JcDot_nu) + NLambda*tau_0;

    %% %%%%%%%%%%%%%%%%%%%%% NULL SPACE OF MOMENTUM %%%%%%%%%%%%%%%%%%%% %%

    % Now, rewrite the joint torques equations as:
    %
    %    tau = Sigma * f_star + gamma_full
    %
    % where we isolated the terms multiplied by the forces in the
    % joint torques equations. Finally we compute the time derivative of tau:
    %
    %    tauDot = SigmaDot * f_star + Sigma * fDot_star + gamma_fullDot
    %    tauDot ~ Sigma * fDot_star (approximated) + gamma_redDot
    %
    % Then, consider the following Lyapunov function candidate:
    %
    %    V    = 1/2*|tau|^2
    %    VDot = tau^T*tauDot
    %
    % It is clear that choosing tauDot = -K_tau*tau will ensures the
    % convergence of the joint torques to zero.
    %

    % theoretically, the forces inside tau_0 are the measured ones, not the
    % desired. Therefore only the derivative of the desired forces should
    % be used to minimize the joint torques, however I am not really sure
    % about this.
    DEACTIVATE_NULL_SPACE_FORCES = 0;

    Sigma               = -pinvLambda*(Jc/M*Jc') -DEACTIVATE_NULL_SPACE_FORCES.*NLambda*(transpose(Jc(:,7:end)) - M(7:end,1:6)/M(1:6,1:6)*transpose(Jc(:,1:6)));
    Sigma_Beta          =  Sigma * Null_Ac_Beta;
    gamma_redDot        =  Sigma * xiDot_primaryTask -Gain.useVelFeedbackForMinTorques*NLambda*MBar*KP_torqueControl*jointVel_err;
    K_tau               =  Gain.K_tau;

    % Secondary task
    pinvSigma_Beta      =  wbc.pinvDamped(Sigma_Beta, Reg.pinvDamp);
    xiDot_secondaryTask = -pinvSigma_Beta*(K_tau*jointTorques_star + gamma_redDot);

    % FINAL control input (Parametrized)
    pinvAc              = [wbc.pinvDamped(Ac_leftFoot,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(2)); zeros(6)] + ...
                          [zeros(6); wbc.pinvDamped(Ac_rightFoot,1e-5) .* (1-LEFT_RIGHT_FOOT_IN_CONTACT(1))] + ...
                           wbc.pinvDamped(Ac,1e-5).* LEFT_RIGHT_FOOT_IN_CONTACT(1).* LEFT_RIGHT_FOOT_IN_CONTACT(2);

    f_des               = pinvAc*(LDot_des + f_grav);

    if LEFT_RIGHT_FOOT_IN_CONTACT(1) > 0.5

        xi_des_left = fromForcesToParametrization(f_des(1:6),Config);
    else
        xi_des_left = zeros(6,1);
    end
    if LEFT_RIGHT_FOOT_IN_CONTACT(2) > 0.5

        xi_des_right = fromForcesToParametrization(f_des(7:end),Config);
    else
        xi_des_right = zeros(6,1);
    end

    xiDes               = [xi_des_left;xi_des_right];
    Ke                  = Gain.Ke;
    xiDot_star          = xiDot_primaryTask + Null_Ac_Beta*xiDot_secondaryTask -Ke*(xi_star-[xiDes(1:6)*LEFT_RIGHT_FOOT_IN_CONTACT(1);xiDes(7:end)*LEFT_RIGHT_FOOT_IN_CONTACT(2)]);
end


function logPr = calcLogJointPr_RGS_BPHMM( Psi, data_struct, hyperparams, model, objIDs, Xstats )
% Calculate logarithm of the joint probability
%   of the state Psi output by RESTRICTED scan
% OUTPUT
%   logPr := struct with fields
%              .F
%              .z
%              .obs

logPr.eta = 0;
logPr.z = 0;
logPr.theta = 0;

featIDs = unique(Psi.activeFeatIDs);

alpha0 = hyperparams.alpha;
kappa0 = hyperparams.kappa;

logPr.F = calcLogPrFeatureMatrix( Psi.F, hyperparams.gamma, hyperparams.c );

Zstats = getZSuffStats( Psi.F, Psi.stateSeq, model, objIDs );
switch model.HMMmodel.transType % ---------------------------------------------- log p( z | F, alph, kappa)
    case 'byObject'
        for ii = objIDs
            Pz = Psi.TS.obj(ii).pi_z;
            Pi = bsxfun( @rdivide, Pz, sum(Pz,2) );
            logPr.eta = logPr.eta + calcLogPrGamma( Pz, [], [], alpha0, kappa0 );
            logPr.z = logPr.z + sum(sum( Zstats.obj(ii).Nz .* log(Pi) ) );
        end
    otherwise
        error( 'To Do.' );
end

if ~exist( 'Xstats', 'var' )
    Xstats = getXSuffStats( Psi.F, Psi.stateSeq, data_struct, model );
end
logPr.obs = 0;
switch model.obsModel.type
    case 'Multinomial'
        lambda = model.obsModel.params.lambda';
        logp = Psi.theta.logp;
        %logPr.theta = calcLogPrDirichlet( Psi.theta.logp( featIDs,: ), lambda, 1 );
        %logPr.obs = sum( sum( Xstats.Nkv .* logp ) );
        %Xall = vertcat(  data_struct(:).obsHist );
        %Zall = horzcat( Psi.stateSeq(:).z  );
        logPr.obs = sum( sum( Xstats.Nkv .* logp ) );
        logPr.theta = 0;
        for kk = 1:size( Psi.F, 2 )
           if sum( Psi.F(:,kk) ) > 0
              logPr.theta = logPr.theta + calcLogPrDirichlet( logp(kk,:), lambda, 1 );
           end
        end
    case 'Gaussian'
        PP = model.obsModel.params;
        Mu = Psi.theta.Mu;
        invSigma = Psi.theta.invSigma;
        logPr.theta = 0;
        logPr.obs = 0;
        Xall = vertcat(  data_struct(:).obs );
        Zall = horzcat( Psi.stateSeq(:).z  );
        for kk = 1:size( Psi.F, 2 )
           Xkk = Xall( Zall==kk, : );
           logPr.obs = logPr.obs + sum( calcLogPrGaussian( Xkk, Mu(kk,:), invSigma(:,:,kk) ) );
           if sum( Psi.F(:,kk) ) > 0
              logPr.theta = logPr.theta + calcLogPrNormalInvWishart( Mu(kk,:), invSigma(:,:,kk), PP );
           end
        end
    case 'AR-Gaussian'
        PP = model.obsModel.params;
        A = Psi.theta.A;
        invSigma = Psi.theta.invSigma;
        logPr.theta = 0;
        logPr.obs = 0;
        Xall = vertcat(  data_struct(:).obs );
        XallR = vertcat(  data_struct(:).obsPrevR );
        Zall = horzcat( Psi.stateSeq(:).z  );
        for kk = 1:size( Psi.F, 2)
            keepINDS = Zall==kk;
            Xkk = Xall(   keepINDS, : );
            XkkR = XallR( keepINDS, : );
            logPr.obs = logPr.obs + sum( calcLogPrGaussian( Xkk, XkkR*A(:,:,kk)', invSigma(:,:,kk)  ) );

            if sum( Psi.F(:,kk) ) > 0
                logPr.theta = logPr.theta + calcLogPrMatrixNormalInvWishart( A(:,:, kk), invSigma(:,:,kk), PP );
            end
        end
    otherwise
        error( 'to do.' );
end

logPr.all = logPr.obs + logPr.z + logPr.F + logPr.eta + logPr.theta;

end


function U_out = updateSubspace(U_in, residual, w, param)
%  update U

    eta = param.eta;
    residualNorm = norm(residual);
    wNorm  = norm(w);

    sigma = param.lambda * residualNorm * wNorm;

    p = residual / residualNorm;

    q = w / wNorm;

  %  t = eta * sigma; % dynamic size

     t =eta;

    U_out = U_in + (cos(t)-1) * U_in * (q * q')  - sin(t) * p * q';

%       normU = norm(U_out)
%       diffI = norm(U_out'*U_out - eye(size(U_out,2)))
end


function varargout = sound_field_mono_localwfs_sbl(X,Y,Z,xs,src,f,conf)
%SOUND_FIELD_MONO_LOCALWFS_SBL sound field of local WFS using spatial bandwith
%limitation
%
%   Usage: [P,x,y,z,x0] = sound_field_mono_localwfs_sbl(X,Y,Z,xs,src,f,conf)
%
%   Input options:
%       X           - x-axis / m; single value or [xmin,xmax] or nD-array
%       Y           - y-axis / m; single value or [ymin,ymax] or nD-array
%       Z           - z-axis / m; single value or [zmin,zmax] or nD-array
%       xs          - position of point source or direction of plane
%                     wave / m [1x3]
%       src         - source type of the virtual source
%                         'pw' - plane wave
%                         'ps' - point source
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output options:
%       P           - simulated sound field
%       x           - corresponding x values / m
%       y           - corresponding y values / m
%       z           - corresponding z values / m
%       x0          - secondary sources / m
%
%   SOUND_FIELD_MONO_LOCALWFS_SBL(X,Y,Z,xs,src,f,conf) simulates a monochromatic
%   sound field of the given source type (src) synthesized with local wave field
%   synthesis using spatial bandwidth limitation for the frequency f.
%
%   To plot the result use:
%   plot_sound_field(P,X,Y,Z,x0,conf);
%   or simple call the function without output argument:
%   sound_field_mono_localwfs_sbl(X,Y,Z,xs,src,f,conf)
%   For plotting you may also consider to display the result in dB, by setting
%   the following configuration option before:
%   conf.plot.usedB = true;
%
%   See also: plot_sound_field, driving_function_mono_localwfs_sbl

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 7;
nargmax = 7;
narginchk(nargmin,nargmax);
isargnumeric(X,Y,Z);
isargxs(xs);
isargchar(src);
isargscalar(f);
isargstruct(conf);

%% ===== Configuration ==================================================
if strcmp('2D',conf.dimension)
    greens_function = 'ls';
else
    greens_function = 'ps';
end

%% ===== Computation =====================================================
% Get secondary sources
x0 = secondary_source_positions(conf);
% Get driving signals
D = driving_function_mono_localwfs_sbl(x0,xs,src,f,conf);
% Calculate sound field
[varargout{1:min(nargout,4)}] = ...
    sound_field_mono(X,Y,Z,x0,greens_function,D,f,conf);
% Return secondary sources if desired
if nargout==5, varargout{5}=x0; end


function mColormap = gui_Colormap_Rastafari(nSize)

% If size is not specified, set it to 256
if nargin < 1
  nSize = 256;
end

mColormap = [];

nSteps = floor(nSize / 2);
mColormap = [mColormap; gui_Interpolate([0.024169, 0.287879, 0.012572], [0.206233, 1.000000, 0.148478], nSteps)];

mColormap = [mColormap; [0.75 0.75 0.75]];

nSteps = floor(nSize / 2);
mColormap = [mColormap; gui_Interpolate([1.000000, 0.235294, 0.235294], [0.621212, 0.017977, 0.035956], nSteps)];


report_this_filefun(mfilename('fullpath'));

bz2 = [];
p1 = []; p1b=[];p99b=[];p50b=[];pmib=[];pmab=[];
p99 = [];
p50 = [];pma=[]; pmi=[];
niv = 1:0.2:4;

tdiff = round((teb - t0b)*365/par1);
ni = str2double(prmptdlg('Number of events in each window?','100'));
na = str2double(prmptdlg('Number of random samples drawn ?','30'));
nr = str2double(prmptdlg('Number of repeats ?','30'));
iwl = str2double(prmptdlg('windowlength ?','2'));
iwl = iwl*365/par1;
zr = (-15:0.1:15)*0;
wai = waitbar(0,' Please Wait ...  ');
set(wai,'NumberTitle','off','Name','Makegrid  -Percent done');;
zr = [];
for k=1:nr
    l = ceil(rand([ni na])*a.Count);
    [cumu, xt] = hist(reshape(a(l,3),[ni,na]),(t0b:par1/365:teb));
    for ti = 1:ni
        mean1 = mean([cumu(1:ti-1,:) ; cumu(ti+iwl+1:ni,:)]);
        mean2 = mean(cumu(ti:ti+iwl,:));
        var1 = cov([cumu(1:ti-1,:) ; cumu(ti+iwl+1:ni,:)]);
        var2 = cov(cumu(ti:ti+iwl,:));
    end     % for i
    as = (mean1 - mean2)./(sqrt(var1/(len-iwl)+var2/iwl));

    pmab = [pmab max(pma) ];
    pma=[];
    waitbar(k/nr);
end

close(wai)
figure
histogram(pmab)
set(gca,'box','on',...
    'SortMethod','childorder','TickDir','out','FontWeight',...
    'bold','FontSize',ZmapGlobal.Data.fontsz.m,'Linewidth',1.2)
grid
xlabel('Windowlength in [years]')
ylabel('Range of z')
title(['ni  =  ' num2str(ni) 'events, ' num2str(na) ' random samples'])

matdraw


function [hpe] = Btuph2hpe(Btuph)
% Convert power from British thermal units per hour to electrical horsepower.
% Chad A. Greene 2012
hpe = Btuph*0.000392857;


function test_failed=test_wmdct
% Test the algorithm using LONG windows.

which comp_dwiltiii
which comp_idwiltiii

disp(' ===============  TEST_WMDCT ================');

Lr=[4, 6, 8,12,16,12,18,32,30];
Mr=[2, 3, 2, 3, 4, 2, 3, 4, 3];

test_failed=0;

for ii=1:length(Lr);
  for W=1:3
    for ftype=1:2
      for wtype=1:2
	L=Lr(ii);
	M=Mr(ii);

	a=M;

	if wtype==1
	  % Full length window
	  g=pgauss(L);
	  gd=wildual(g,M);
          wtype='LONG';
	else
	  g=firwin('sqrthann',2*M,'2');
	  gd=g;
          wtype='FIR ';
	end;

	if ftype==1
	  % Complex-valued test case
	  f=tester_crand(L,W);
	  S='CMPLX';
	else
	  % Real-valued tes
	  f=tester_rand(L,W);
	  S='REAL ';
	end;

	c=wmdct(f,g,M,L);

	a=M;

	c2=ref_dwiltiii(f,g,a,M);
	r=iwmdct(c,gd);

	res=norm(c(:)-c2(:));

        [test_failed,fail]=ltfatdiditfail(res,test_failed);
	s=sprintf('REF  %s %s L:%3i W:%2i a:%3i M:%3i %0.5g %s',S,wtype,L,W,a,M,res,fail);
	disp(s);

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

        [test_failed,fail]=ltfatdiditfail(res,test_failed);

	s=sprintf('REC  %s %s L:%3i W:%2i a:%3i M:%3i %0.5g %s',S,wtype,L,W,a,M,res,fail);
	disp(s);

        g=wilorth(M,L);
        c=wmdct(f,g,M);
        r=iwmdct(c,g);
        rdiff=f-r;

	res=norm(rdiff(:));

        [test_failed,fail]=ltfatdiditfail(res,test_failed);
	s=sprintf('ORTH %s %s L:%3i W:%2i a:%3i M:%3i %0.5g %s',S,wtype,L,W,a,M,res,fail);
	disp(s);

      end;
    end;
  end;
end;