%% triBox
% Below is a demonstration of the features of the |triBox| function

%%
clear; close all; clc;

%%
% PLOT SETTINGS
fontSize=15;
faceAlpha1=0.5;

%% Creating a triangulated mesh of a box

%%
% Specifying dimensions and number of elements for each direction
boxDim=[4 5 6]; %Width in each direction
pointSpacing=1; %Desired point spacing

%%
% Using |triBox| to build the patch model

[F,V,faceBoundaryMarker]=triBox(boxDim,pointSpacing);

%%
% Visualisation
cFigure; hold on;
title('Box triangular faces and normals','FontSize',fontSize);

gpatch(F,V,faceBoundaryMarker,'k',1);
patchNormPlot(F,V);

axisGeom(gca,fontSize);
colormap(gjet(6)); icolorbar;
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.


% @TTENSOR
%
% Files
%   disp      - Command window display of a ttensor.
%   display   - Command window display of a ttensor.
%   double    - Convert ttensor to double array.
%   end       - Last index of indexing expression for ttensor.
%   full      - Convert a ttensor to a (dense) tensor.
%   innerprod - Efficient inner product with a ttensor.
%   isequal   - True if each component of two ttensor's is numerically equal.
%   mtimes    - Implement scalar multiplication for a ttensor.
%   mttkrp    - Matricized tensor times Khatri-Rao product for ttensor.
%   ndims     - Return the number of dimensions for a ttensor.
%   norm      - Norm of a ttensor.
%   nvecs     - Compute the leading mode-n vectors for a ttensor.
%   permute   - Permute dimensions for a ttensor.
%   size      - Size of a ttensor.
%   subsasgn  - Subscripted reference for a ttensor.
%   subsref   - Subscripted reference for a ttensor.
%   ttensor   - Tensor stored as a Tucker operator (decomposed).
%   ttm       - Tensor times matrix for ttensor.
%   ttv       - Tensor times vector for ttensor.
%   uminus    - Unary minus for ttensor.
%   uplus     - Unary plus for ttensor.


function [c]=findcntr(H)
    %
    %    [Center]=findcntr(Handle)
    %    Returns the geometric center of Handle
    %
    %    This can be used as the ORIGIN input in ROTATE to rotate
    %    an object about its geometric center.
    %
    %    See also ROTATE, TRNSLATE, SCALHAND
    %
    %    Richard G. Cobb    3/96
    %
    c=[0 0 0];

    tmp=(max(get(H,'Xdata'))+min(get(H,'Xdata')))/2;
    if tmp ~= []
        c(1)=tmp;
    else
        set(H,'Xdata',zeros(size(get(H,'Ydata'))))
    end
    tmp=(max(get(H,'Ydata'))+min(get(H,'Ydata')))/2;
    if tmp ~= []
        c(2)=tmp;
    else
        set(H,'Ydata',zeros(size(get(H,'Xdata'))))
    end

    tmp=(max(get(H,'Zdata'))+min(get(H,'Zdata')))/2;
    if tmp ~= []
        c(3)=tmp;
    else
        set(H,'Zdata',zeros(size(get(H,'Ydata'))))
    end

    %eof


clear all
close all
clc

load ../data/lc_data_20s_withpoints

lc_len = length(lc_data);

for i = 1: lc_len
    temp = lc_data{i};
    pp   = points(i,:);
    lat  = temp.veh_s.x;
    lon  = temp.veh_s.y;
    figure(1);hold off;plot(lon,lat,'b.');hold on;
    plot(lon(pp(1)),lat(pp(1)),'o','MarkerFaceColor','g');
    plot(lon(pp(2)),lat(pp(2)),'o','MarkerFaceColor','r');
end


function[d,x,y,t,z,w]=polysmooth_presort(varargin)
%POLYSMOOTH_PRESORT  Sort arguments to POLYSMOOTH in case of missing data.
%
%   POLYSMOOTH_PRESORT is a low-level function called by POLYSMOOTH.
%
%   [DS,XS,YS,TS,ZS,WS]=POLYSMOOTH_PRESORT(DS,XS,YS,TS,ZS,WS,P,TAU,STR),
%   where all the input fields are in the form output by TWODSORT or
%   SPHERESORT, performs several prelimary processing steps.
%
%   First, if the temporal bandwidth TAU is nonempty, any datapoints
%   outside of temporal window ABS(TS./TAU)>1 are set to NaNs.
%
%   After this, the data field, ZS, is checked to see if it contains any
%   NaNs. If it does, then all fields are sorted by distance DS along
%   third dimension, thus moving any interior NaN value to the bottom.
%
%   In the case that STR='pop', indicating a fixed population algorithm,
%   then finally all input fields are additionally truncated to length P
%   along their third dimension.
%
%   See also POLYSMOOTH.
%
%   Usage: [ds,xs,ys,zs,ws]=polysmooth_presort(ds,xs,ys,zs,ws,P,tau,str);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2018 J.M. Lilly --- type 'help jlab_license' for details

t=[];
z=[];
w=[];
B=[];
tau=[];
varstr='bandwidth';

d=varargin{1};
x=varargin{2};
y=varargin{3};
if nargin>=4
    t=varargin{4};
end
if nargin>=5
    z=varargin{5};
end
if nargin>=6
    w=varargin{6};
end
if nargin>=7
    B=varargin{7};
end
if nargin>=8
    tau=varargin{8};
end
if nargin>=9
    varstr=varargin{9};
end
%figure,jpcolor(squeeze(z))

if ~isempty(tau)
    if length(tau(:))~=1
        tau=vrep(tau,size(z,3),3);
    end
    z(abs(t./tau)>1)=nan;
    %d(abs(t./tau)>1)=nan;
end

%figure,jpcolor(squeeze(z))
%d(~isfinite(z))=nan;
%x(~isfinite(z))=nan;
%y(~isfinite(z))=nan;

if ~isempty(d)
    if ~strcmpi(varstr(1:3),'pop')
        if ~isempty(B)
            if length(B(:))~=1
                B=vrep(B,size(z,3),3);
            end
            z(abs(d./B)>1)=nan;
        end
    end
    %figure,jpcolor(squeeze(z))

    if ~isempty(z)
        z(~isfinite(d))=nan;  %make sure pattern of nans matches
        d(~isfinite(z))=nan;  %make sure pattern of nans matches
    end

    %bool will be true if current point is finite, but previous is not
    bool1=isnan(d);
    bool2=isnan(vshift(d,-1,3));bool2(:,:,1)=false;
    bool=(sum(~bool1&bool2,3)>0)&~(sum(bool1,3)==size(bool1,3));

    if anyany(bool)
        disp('POLYSMOOTH_PRESORT detecting interior NaNs; sorting to compress these.')
        [d,kk]=sort(d,3);
        ii=vrep([1:size(d,1)]',[size(d,2) size(d,3)],[2 3]);
        jj=vrep([1:size(d,2)],[size(d,1) size(d,3)],[1 3]);
        index=sub2ind(size(d),ii,jj,kk);
        %Can't use vindex here because i want to preserve the size
        x=x(index);
        y=y(index);
        if ~isempty(z),z=z(index);end
        if ~isempty(t),t=t(index);end
        if ~isempty(w),w=w(index);end
        %vindex(x,y,t,z,w,index,3);
        disp('Sorting complete.')
    end
    %1,vsize(d,x,y,t,z,w)

    if strcmpi(varstr(1:3),'pop')
        if ~isempty(B)
            vindex(d,x,y,t,z,w,1:min(maxmax(B),size(d,3)),3);
        end
    end
    %2,vsize(d,x,y,t,z,w)
    %figure,jpcolor(squeeze(d))
    numgood=squeeze(sum(sum(isfinite(d),2),1));
    %figure,plot(numgood)
    index=find(numgood~=0,1,'last');
    %index
    vindex(d,x,y,t,z,w,1:index,3);
    %3,vsize(d,x,y,t,z,w)
end


%ESTIMATEAFFINE2D  Computes an optimal affine transformation between two 2D point sets
%
%     H = cv.estimateAffine2D(from, to)
%     [H, inliers] = cv.estimateAffine2D(...)
%     [...] = cv.estimateAffine2D(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __from__ First input 2D point set containing `(X,Y)`. Cell array of
%   2-element vectors `{[x,y],...}` or Nx2/Nx1x2/1xNx2 numeric array.
% * __to__ Second input 2D point set containing `(x,y)`. Same size and type as
%   `from`.
%
% ## Output
% * __H__ Output 2D affine transformation matrix 2x3 or empty matrix if
%   transformation could not be estimated. The returned matrix has the
%   following form `[a11 a12 b1; a21 a22 b2]`.
% * __inliers__ Output vector of same length as number of points, indicating
%   which points are inliers (1-inlier, 0-outlier).
%
% ## Options
% * __Method__ Robust method used to compute transformation. RANSAC is the
%   default method. The following methods are possible:
%   * __Ransac__ RANSAC-based robust method.
%   * __LMedS__ Least-Median of squares robust method
% * __RansacThreshold__ Maximum reprojection error in the RANSAC algorithm to
%   consider a point as an inlier. Applies only to RANSAC. default 3.0.
% * __MaxIters__ The maximum number of robust method iterations. default 2000
% * __Confidence__ Confidence level, between 0 and 1, for the estimated
%   transformation. Anything between 0.95 and 0.99 is usually good enough.
%   Values too close to 1 can slow down the estimation significantly. Values
%   lower than 0.8-0.9 can result in an incorrectly estimated transformation.
%   default 0.99.
% * __RefineIters__ Maximum number of iterations of refining algorithm
%   (Levenberg-Marquardt). Passing 0 will disable refining, so the output
%   matrix will be output of robust method. default 10
%
% It computes:
%
%     [x; y] = [a11 a12; a21 a22] * [X; Y] + [b1; b2]
%
% The function estimates an optimal 2D affine transformation between two 2D
% point sets using the selected robust algorithm.
%
% The computed transformation is then refined further (using only inliers)
% with the Levenberg-Marquardt method to reduce the re-projection error even
% more.
%
% Note: The RANSAC method can handle practically any ratio of outliers but
% needs a threshold to distinguish inliers from outliers. The method LMedS
% does not need any threshold but it works correctly only when there are more
% than 50% of inliers.
%
% See also: cv.estimateAffinePartial2D, cv.getAffineTransform
%


%% Set Manopt options (if desired)
Manopt_opts.tolgradnorm = 1e-2;  % Stopping tolerance for norm of Riemannian gradient
Manopt_opts.rel_func_tol = 1e-5;  % Additional stopping criterion for Manopt: stop if the relative function decrease between two successive accepted iterates is less than this value
Manopt_opts.miniter = 1;  % Minimum number of outer iterations (i.e. accepted update steps) to perform
Manopt_opts.maxiter = 300;  % Maximum number of outer iterations (i.e. accepted update steps) to perform
Manopt_opts.maxinner = 500;  % Maximum number of iterations for the conjugate-gradient method used to compute approximate Newton steps
%manopt_options.maxtime = 60*60;  % Maximum computation time to allow, in seconds
%manopt_options.solver = @steepestdescent;  % Select Manopt solver to use: {trustregions (default), conjugategradient, steepestdescent}

%% Set SE-Sync options (if desired)
SE_Sync_opts.r0 = 5;  % Initial maximum-rank parameter at which to start the Riemannian Staircase
SE_Sync_opts.rmax = 10;  % Maximum maximum-rank parameter at which to terminate the Riemannian Staircase
SE_Sync_opts.eig_comp_rel_tol = 1e-4;  % Relative tolerance for the minimum-eigenvalue computation used to test for second-order optimality with MATLAB's eigs() function
SE_Sync_opts.min_eig_lower_bound = -1e-3;  % Minimum eigenvalue threshold for accepting a maxtrix as numerically positive-semidefinite
SE_Sync_opts.Cholesky = false;  % Select whether to use Cholesky or QR decomposition to compute orthogonal projections


% prettyMap 2.0
%--------------------------------------------------------------------------
%
% plot with projection an area of the world
%
% POSSIBLE SINTAXES:
%   prettyMap(map);
%
%   prettyMap(map, shape);
%   prettyMap(map, projection);
%   prettyMap(map, lineCol);
%
%   prettyMap(map, phiGrid, lambdaGrid);
%   prettyMap(map, shape, projection);
%   prettyMap(map, shape, lineCol);
%   prettyMap(map, projection, shape);
%   prettyMap(map, projection, lineCol);
%
%   prettyMap(map, phiGrid, lambdaGrid, shape);
%   prettyMap(map, phiGrid, lambdaGrid, projection);
%   prettyMap(map, phiGrid, lambdaGrid, lineCol);
%
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax)
%   prettyMap(map, phiMin, phiMax, shape, projection);
%   prettyMap(map, phiMin, phiMax, shape, lineCol);
%   prettyMap(map, phiMin, phiMax, projection, shape);
%   prettyMap(map, phiMin, phiMax, projection, lineCol);
%
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, shepe);
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, projection);
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, lineCol);
%
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax)
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, shape, projection);
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, shape, lineCol);
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape);
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, projection, lineCol);
%
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shepe);
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection);
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, lineCol);
%
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape, projection);
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape, lineCol);
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape);
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, lineCol);
%
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape, lineCol);
%   prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape, projection, lineCol);
%
% EXAMPLE:
%   prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, 'Miller Cylindrical');
%
% INPUT:
%   map             matrix containing data of the whole world to be shown
%   phiGrid         array [degree]
%   lambdaGrid      array [degree]
%   phiMin          minimum latitude    [degree]
%   phiMax          maximum latitude    [degree]
%   lambdaMin       minimum longitude   [degree]
%   lambdaMax       maximum longitude   [degree]
%   projection      type of projection to be used "standard" is the default
%   shape           shapefile to load as coast (or country) contour
%                       - fill          fill coasts coarse
%                       - coast         only coasts coarse
%                       - 50m           1:50000000 scale country contours
%                       - 30m           1:30000000 scale country contours
%                       - 10m           1:10000000 scale country contours
%   lineCol         [1 1 1] array of RGB component to draw the contour lines
%
% DEFAULT VALUES:
%    projection = 'Lambert'
%
% AVAILABLE PROJECTION:
%    * Lambert
%      Stereographic
%      Orthographic
%      Azimuthal Equal-area
%      Azimuthal Equidistant
%      Gnomonic
%      Satellite
%      Albers Equal-Area Conic
%      Lambert Conformal Conic
%      Mercator
%    * Miller Cylindrical
%    * Equidistant Cylindrical (world map)
%      Oblique Mercator
%      Transverse Mercator
%      Sinusoidal
%      Gall-Peters
%      Hammer-Aitoff
%      Mollweide
%      Robinson
%    * UTM
%
% SEE ALSO:
%   mapPlot, mapPlot3D, quiver
%
% REQUIREMENTS:
%   M_Map: http://www.eos.ubc.ca/~rich/map.html
%   shape files with contours
%
% VERSION: 2.1
%
% CREDITS:
%   http://www.eos.ubc.ca/~rich/map.html
%
%   Andrea Gatti
%   DIIAR - Politecnico di Milano
%   2013-12-19
%
function h = prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape, lineCol)

%shape = 'coast';
%shape = 'fill';
%shape = '10m';
%shape = '30m';
%shape = '50m';

% lineCol = [0 0 0];
limitsOk = false;

% Manage opening a new figure;
tohold = false;
if length(findall(0,'Type','figure'))>=1
    if ishold
        clf;
        tohold = true;
    else
        figure;
    end
end

switch (nargin)
    case 1                                                                % prettyMap(map);
        shape = 'coast';
        lineCol = [0 0 0];
        projection = 'Miller Cylindrical';
        phiMin = 90;
        phiMax = -90;
        lambdaMin = -180;
        lambdaMax = 180;

        deltaPhi = (phiMax-phiMin)/size(map,1);
        deltaLambda = (lambdaMax-lambdaMin)/size(map,2);

        phiGrid    = (phiMin + deltaPhi/2 : deltaPhi : phiMax - deltaPhi/2)';
        lambdaGrid = (lambdaMin + deltaLambda/2 :  deltaLambda :  lambdaMax  - deltaLambda/2)';
    case 2
        shape = 'coast';
        lineCol = [0 0 0];
        projection = 'Miller Cylindrical';
        if (ischar(phiGrid))
            if (sum(strcmp(phiGrid,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}]))) % prettyMap(map, shape);
                shape = phiGrid;
            else                                                          % prettyMap(map, projection);
                projection = phiGrid;
            end
        elseif (length(phiGrid) == 3)                                     % prettyMap(map, lineCol);
            lineCol = phiGrid;
        end

        phiMin = 90;
        phiMax = -90;
        lambdaMin = -180;
        lambdaMax = 180;

        deltaPhi = (phiMax-phiMin)/size(map,1);
        deltaLambda = (lambdaMax-lambdaMin)/size(map,2);

        phiGrid    = (phiMin + deltaPhi/2 : deltaPhi : phiMax - deltaPhi/2)';
        lambdaGrid = (lambdaMin + deltaLambda/2 :  deltaLambda :  lambdaMax  - deltaLambda/2)';
    case 3
        shape = 'coast';
        lineCol = [0 0 0];
        if (ischar(phiGrid))
            projection = 'Miller Cylindrical';
            if (sum(strcmp(phiGrid,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))
                shape = phiGrid;
                if (ischar(lambdaGrid))
                    projection = lambdaGrid;                              % prettyMap(map, shape, projection);
                else
                    lineCol = lambdaGrid;                                 % prettyMap(map, shape, lineCol);
                end
            else
                projection = phiGrid;
                if (ischar(lambdaGrid))
                    shape = lambdaGrid;                                   % prettyMap(map, projection, shape);
                else
                    lineCol = lambdaGrid;                                 % prettyMap(map, projection, lineCol);
                end
            end
            phiMin = 90;
            phiMax = -90;
            lambdaMin = -180;
            lambdaMax = 180;

            deltaPhi = (phiMax-phiMin)/size(map,1);
            deltaLambda = (lambdaMax-lambdaMin)/size(map,2);

            phiGrid    = (phiMin + deltaPhi/2 : deltaPhi : phiMax - deltaPhi/2)';
            lambdaGrid = (lambdaMin + deltaLambda/2 :  deltaLambda :  lambdaMax  - deltaLambda/2)';
        else                                                              % prettyMap(map, phiGrid, lambdaGrid);
            projection = 'Miller Cylindrical';
            phiMin = max(phiGrid);
            phiMax = min(phiGrid);
            lambdaMin = min(lambdaGrid);
            lambdaMax = max(lambdaGrid);
        end
    case 4
        shape = 'coast';
        lineCol = [0 0 0];
        projection = 'Miller Cylindrical';
        if (ischar(phiMin))
            if (sum(strcmp(phiMin,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))  % prettyMap(map, phiGrid, lambdaGrid, shape);
                shape = phiMin;
            else                                                          % prettyMap(map, phiGrid, lambdaGrid, projection);
                projection = phiMin;
            end
        elseif (length(phiMin) == 3)                                      % prettyMap(map, phiGrid, lambdaGrid, lineCol);
            lineCol = phiMin;
        end

        phiMin = max(phiGrid);
        phiMax = min(phiGrid);
        lambdaMin = min(lambdaGrid);
        lambdaMax = max(lambdaGrid);
    case 5
        shape = 'coast';
        lineCol = [0 0 0];
        projection = 'Miller Cylindrical';
        if (ischar(phiMin))
            if (sum(strcmp(phiMin,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))
                shape = phiMin;
                if (ischar(phiMax))
                    projection = phiMax;                                  % prettyMap(map, phiMin, phiMax, shape, projection);
                else
                    lineCol = phiMax;                                     % prettyMap(map, phiMin, phiMax, shape, lineCol);
                end
            else
                projection = phiMin;
                if (ischar(phiMax))
                    shape = phiMax;                                       % prettyMap(map, phiMin, phiMax, projection, shape);
                else
                    lineCol = phiMax;                                     % prettyMap(map, phiMin, phiMax, projection, lineCol);
                end
            end
            phiMin = max(phiGrid);
            phiMax = min(phiGrid);
            lambdaMin = min(lambdaGrid);
            lambdaMax = max(lambdaGrid);
        else                                                             %  prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax);
            limitsOk = true;
            lambdaMin = phiMin;
            lambdaMax = phiMax;
            phiMin = phiGrid;
            phiMax = lambdaGrid;

            if (phiMin < phiMax)
                tmp = phiMin;
                phiMin = phiMax;
                phiMax = tmp;
            end

            deltaPhi = (phiMax-phiMin)/size(map,1);
            deltaLambda = (lambdaMax-lambdaMin)/size(map,2);

            phiGrid    = (phiMin + deltaPhi/2 : deltaPhi : phiMax - deltaPhi/2)';
            lambdaGrid = (lambdaMin + deltaLambda/2 :  deltaLambda :  lambdaMax  - deltaLambda/2)';
         end
    case 6
        shape = 'coast';
        lineCol = [0 0 0];
        limitsOk = true;
        projection = 'Lambert';
        if (ischar(lambdaMin))
            if (sum(strcmp(lambdaMin,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))
                shape = lambdaMin;                                        % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, shape);
            else
                projection = lambdaMin;                                   % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, projection);
            end
        elseif (length(lambdaMin) == 3)
            lineCol = lambdaMin;                                          % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, lineCol);
        end

        lambdaMax = phiMax;
        lambdaMin = phiMin;
        phiMin = phiGrid;
        phiMax = lambdaGrid;

        if (phiMin < phiMax)
            tmp = phiMin;
            phiMin = phiMax;
            phiMax = tmp;
        end

        deltaPhi = (phiMax-phiMin)/size(map,1);
        deltaLambda = (lambdaMax-lambdaMin)/size(map,2);

        phiGrid    = (phiMin + deltaPhi/2 : deltaPhi : phiMax - deltaPhi/2)';
        lambdaGrid = (lambdaMin + deltaLambda/2 :  deltaLambda :  lambdaMax  - deltaLambda/2)';
    case 7
        shape = 'coast';
        lineCol = [0 0 0];
        limitsOk = true;
        projection = 'Lambert';
        if (ischar(lambdaMin))
            if (sum(strcmp(lambdaMin,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))
                shape = lambdaMin;
                if (ischar(lambdaMax))
                    projection = lambdaMax;                               % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, shape, projection);
                else
                    lineCol = lambdaMax;                                  % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, shape, lineCol);
                end
            else
                projection = lambdaMin;
                if (ischar(lambdaMax))
                    shape = lambdaMax;                                    % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape);
                else
                    lineCol = lambdaMax;                                  % prettyMap(map, phiMin, phiMax, lambdaMin, lambdaMax, projection, lineCol);
                end
            end

            lambdaMin = phiMin;
            lambdaMax = phiMax;
            phiMin = phiGrid;
            phiMax = lambdaGrid;

            if (phiMin < phiMax)
                tmp = phiMin;
                phiMin = phiMax;
                phiMax = tmp;
            end

            deltaPhi = (phiMax-phiMin)/size(map,1);
            deltaLambda = (lambdaMax-lambdaMin)/size(map,2);

            phiGrid    = (phiMin + deltaPhi/2 : deltaPhi : phiMax - deltaPhi/2)';
            lambdaGrid = (lambdaMin + deltaLambda/2 :  deltaLambda :  lambdaMax  - deltaLambda/2)';
        else
            projection = 'lambert';                                       % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax);
        end
    case 8
        shape = 'coast';
        lineCol = [0 0 0];
        limitsOk = true;
        if (ischar(projection))
            if (sum(strcmp(projection,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))
                shape = projection;                                       % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape);
                projection = 'Lambert';
            else
                                                                          % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection);
            end
        elseif (length(projection) == 3)
            lineCol = projection;                                         % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, lineCol);
            projection = 'Lambert';
        end
    case 9
        lineCol = [0 0 0];
        limitsOk = true;
        if (ischar(projection))
            if (sum(strcmp(projection,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))
                tmp = shape;
                shape = projection;
                if (ischar(tmp))
                    projection = tmp;                                     % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape, projection);
                else
                    lineCol = tmp;                                        % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape, lineCol);
                    projection = 'UTM';
                end
            else
                if (ischar(shape))
                                                                          % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape);
                else
                    lineCol = shape;                                      % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, lineCol);
                    shape = 'coast';
                end
            end
        end
    case 10                                                               % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, projection, shape, lineCol)
        limitsOk = true;
       if (sum(strcmp(projection,[{'none'},{'coast'},{'fill'},{'10m'},{'30m'},{'50m'}])))   % prettyMap(map, phiGrid, lambdaGrid, phiMin, phiMax, lambdaMin, lambdaMax, shape, projection, lineCol)
            tmp = shape;
            shape = projection;
            projection = tmp;
       end
end

if (phiMin < phiMax)
    tmp = phiMin;
    phiMin = phiMax;
    phiMax = tmp;
end

lambdaGrid = sort(lambdaGrid);

[val idMax] = max(diff(lambdaGrid));
if sum(diff(lambdaGrid) == val) == 1
    lambdaGrid(1:idMax) = lambdaGrid(1:idMax)+360;
    if ~limitsOk
        lambdaMax = lambdaGrid(idMax);
        lambdaMin = lambdaGrid(idMax+1);
    end
end
lambdaGrid = sort(lambdaGrid);

if(lambdaMax<lambdaMin)
    lambdaMax = lambdaMax+360;
end

% setup the projection
if (strcmpi(projection,[{'lambert'}]) && abs(phiMax==-phiMin))
    projection='Miller Cylindrical';
end

if (sum(strcmpi(projection,[{'lambert'},{'UTM'},{'Sinusoidal'},{'Transverse Mercator'},{'Mollweid'},{'Oblique Mercator'},{'Miller Cylindrical'}])))
    m_proj(projection,'long',[lambdaMin lambdaMax],'lat',[phiMax phiMin]);
else
    m_proj(projection);
end

% Printing projection
fprintf('Using projection: %s\n', projection);

if sum(diff(lambdaGrid)<-200)
    lambdaGrid(lambdaGrid<0)=lambdaGrid(lambdaGrid<0)+360;
end
% plot the map
h = m_pcolor(lambdaGrid, phiGrid, map);

% set the light
shading flat;

% read shapefile
if (~strcmp(shape,'none'))
	if (~strcmp(shape,'coast')) && (~strcmp(shape,'fill'))
    	if (strcmp(shape,'10m'))
        	M=m_shaperead('countries_10m');
	    elseif (strcmp(shape,'30m'))
    	    M=m_shaperead('countries_30m');
	    else
    	    M=m_shaperead('countries_50m');
	    end
    	[xMin,yMin] = m_ll2xy(lambdaMin,phiMin);
	    [xMax,yMax] = m_ll2xy(lambdaMax,phiMax);
    	for k=1:length(M.ncst)
        	lamC = M.ncst{k}(:,1);
	        ids = lamC < lambdaMin;
   	    	lamC(ids) = lamC(ids) + 360;
        	phiC = M.ncst{k}(:,2);
        	[x,y] = m_ll2xy(lamC,phiC);
        	if sum(~isnan(x))>1
            	x(find(abs(diff(x))>=abs(xMax-xMin)*0.90)+1) = nan; % Remove lines that occupy more than th 90% of the plot
	            line(x,y,'color', lineCol);
    	    end
        end
    else
        if (strcmp(shape,'coast'))
        	m_coast('line','color', lineCol);
        else
            m_coast('patch',lineCol);
        end
	end
end

m_grid('box','fancy','tickdir','in');
colorbar;

if tohold
    hold on;
end


function dat = interpolate(dat, varargin)
% Interpolate over missing values in image_vector object
%
% :Usage:
% ::
%
%    dat = interpolate(dat, varargin)
%
% :Input:
%    image_vector object (dat; e.g., an fmri_data object)
%
% Use when there are some missing values in the mask image
% Performs 3-D linear interpolation to fill in all values in the original
% mask.
%
% e.g., For a standard brain image space that is 91 x 109 x 91, you may
% have 300,000 in-mask values. Only 150,000 of these may be defined in the
% image, however, and the rest are missing (0 or NaN).
% This function will return a dat image with non-missing values for all
% 300,000 voxels (the "in-mask" space).
% It will not return values for all voxels in the 91 x 109 x 91 space,
% however.
%
% :Note:
% This function does not upsample the data now, but could be extended
% to do so fairly easily.
%

upsamplevalue = 1; % values > 1 would upsample the data

SPACE = define_sampling_space(dat.volInfo, upsamplevalue);

% voldata = reconstruct_image(dat);
% voldata(voldata(:) == 0 | isnan(voldata(:))) = 0;

xyzfull = dat.volInfo.xyzlist;

dat = remove_empty(dat);
wh = ~dat.removed_voxels;
x = xyzfull(wh, 1);
y = xyzfull(wh, 2);
z = xyzfull(wh, 3);

% interpolation of scattered data
% note: y and x are reversed in some Matlab functions, such as this one - OK
Vq = griddata(y, x, z, double(dat.dat),  SPACE.X, SPACE.Y, SPACE.Z, 'linear');

datnew = Vq(:);
datnew = datnew(dat.volInfo.wh_inmask);

dat.dat = datnew;
dat.removed_voxels = false;

if upsamplevalue > 1
    % Need to re-create image space and dims from scratch
    % up-sample mask as well

end

end % function

% view
%slice(SPACE.X, SPACE.Y, SPACE.Z, Vq,[6 30 60],2,[10 40 70]), shading flat

% resampled_dat = interp3(x, y, z, dat.dat, SPACE.X, SPACE.Y, SPACE.Z);
% resampled_dat = interp3(SPACE.Xo, SPACE.Yo, SPACE.Zo, voldata, SPACE.X, SPACE.Y, SPACE.Z);


function [isPos,reportStr]=istriequal(v1Mat,f1Mat,v2Mat,f2Mat,maxTol)
% ISTRIEQUAL checks if two triangulations are equal. Triangulations
% resulting from permunations of edge directions and vertices in
% faces are considered equal
%
% Input:
%   regular:
%       v1Mat: double[n1Verts,3] - vertices of the first triangualtion
%       f1Mat: double[n1Faces,3] - faces of the first triangulation
%       v2Mat: double[n1Verts,3] - vertices of the second triangulation
%       f2Mat: double[n1Faces,3] - faces of the second triangulation
%       maxTol: double[1,1] - tolerances used for comparing vertex
%          coordinates
%
% Output:
%   isPos: logical[1,1] - specifies if result of comparison is true
%   reportStr: char[1,] - describes a reason of negative result
%
% $Author: Peter Gagarinov, PhD  <pgagarinov@gmail.com> $
% $Copyright: Peter Gagarinov, PhD,
%            Moscow State University,
%            Faculty of Computational Mathematics and Computer Science,
%            System Analysis Department 2011-2016 $
%
n1Verts=size(v1Mat,1);
n2Verts=size(v2Mat,1);
isPos=n1Verts==n2Verts;
%
if isPos
    n1Faces=size(f1Mat,1);
    n2Faces=size(f2Mat,1);
    isPos=n1Faces==n2Faces;
    if isPos
        [v1Mat,~,indF1Vec]=sortrowstol(v1Mat,maxTol);
        [v2Mat,~,indF2Vec]=sortrowstol(v2Mat,maxTol);
        %
        f1Mat=indF1Vec(f1Mat);
        f2Mat=indF2Vec(f2Mat);
        realTol=max(max(abs(v1Mat-v2Mat)));
        isPos=realTol<=maxTol;
        %
        if isPos
            nF1Unique=size(unique(f1Mat,'rows'),1);
            nF2Unique=size(unique(f2Mat,'rows'),1);
            isPos=nF1Unique==nF2Unique;
            if isPos
                isPos=all(isface(v2Mat,f2Mat,f1Mat));
                if isPos
                    reportStr='';
                else
                    reportStr='faces are different';
                end
            else
                reportStr='numbers of unique faces are different';
            end
        else
            reportStr=sprintf(...
                'vertices are different, real tol=%f, exp tol=%f',...
                realTol,maxTol);
        end
    else
        reportStr='number of faces is different';
    end
else
    reportStr='numbers of vertices are different';
end


function rrd = jplephem (et, ntarg, ncent)

% reads the jpl planetary ephemeris and gives
% the position and velocity of the point 'ntarg'
% with respect to point 'ncent'

% input

%   et    = julian ephemeris date at which interpolation is wanted

%   ntarg = integer number of 'target' point

%   ncent = integer number of center point

%   the numbering convention for 'ntarg' and 'ncent' is:

%        1 = mercury           8 = neptune
%        2 = venus             9 = pluto
%        3 = earth            10 = moon
%        4 = mars             11 = sun
%        5 = jupiter          12 = solar-system barycenter
%        6 = saturn           13 = earth-moon barycenter
%        7 = uranus           14 = nutations (longitude and obliq)
%                             15 = librations, if on ephemeris file

%        if nutations are wanted, set ntarg = 14.
%        for librations, set ntarg = 15. set ncent = 0.

% output

%   rrd = output 6-word array containing position and velocity
%         of point 'ntarg' relative to 'ncent'. the units are au and
%         au/day. for librations the units are radians and radians
%         per day. in the case of nutations the first four words of
%         rrd will be set to nutations and rates, having units of
%         radians and radians/day.

%         the option is available to have the units in km and km/sec.
%         for this, set km = 1 via global in the calling program.

% Orbital Mechanics with MATLAB

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

global cval ss au emrat ncon ipt np nv twot pc vc

global iephem ephname bary pvsun nrl fid lpt

rrd = zeros(6, 1);

list = zeros(12, 1);

et2 = zeros(2, 1);

% load time array

et2(1) = et;

et2(2) = 0;

% first entry?

if (iephem == 1)

    pvsun = zeros(6, 1);

    % read header file data

    fid = fopen(ephname, 'r');

    ttl = fread(fid, 252);

    cnam = fread(fid, 2400);

    ss = fread(fid, 3, 'double');

    ncon = fread(fid, 1, 'int');

    % astronomical unit

    au = fread(fid, 1, 'double');

    % earth-moon ratio

    emrat = fread(fid, 1, 'double');

    ipt = fread(fid, [3 12], 'int');

    numde = fread(fid, 1, 'int');

    lpt = fread(fid, 3, 'int');

    % move to next record

    status = fseek(fid, 8144, 'bof');

    % read "constant" values

    cval = fread(fid, 400, 'double');

    % initialization

    nrl = 0;

    bary = 0;

    pc(1) = 1;
    pc(2) = 0;

    vc(2) = 1;

    np = 2;
    nv = 3;

    twot = 0;

    iephem = 0;
end

if (ntarg == ncent)

    return;

end

%%%%%%%%%%%%%%%%%%%%%%
% nutations requested?
%%%%%%%%%%%%%%%%%%%%%%

if (ntarg == 14)

    if (ipt(2, 12) > 0)

        list(11) = 2;

        [pv, rrd] = state(et2, list);

        list(11) = 0;

        return;

    else

        fprintf('\n\njplephem - no nutations on this ephemeris file \n');

        return;

    end
end

%%%%%%%%%%%%%%%%%%%%%%%
% librations requested?
%%%%%%%%%%%%%%%%%%%%%%%

if (ntarg == 15)

    if (lpt(2) > 0)

        list(12) = 2;

        [pv, rrd] = state(et2, list);

        list(12) = 0;

        for i = 1:1:6
            rrd(i) = pv(i, 11);
        end

        return

    else

        fprintf('\n\n no librations on this ephemeris file \n');

        return;

    end

end

% force barycentric output by function 'state'

bsave = bary;

bary = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% set up proper entries in 'list' array for state call
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:2

    k = ntarg;

    if (i == 2)
        k = ncent;
    end

    if (k <= 10)
        list(k) = 2;
    end

    if (k == 10)
        list(3) = 2;
    end

    if (k == 3)
        list(10) = 2;
    end

    if (k == 13)
        list(3) = 2;
    end

end

%%%%%%%%%%%%%%%%%%%%
% make call to state
%%%%%%%%%%%%%%%%%%%%

[pv, rrd] = state(et2, list);

if (ntarg == 11 || ncent == 11)
    for i = 1:1:6
        pv(i, 11) = pvsun(i);
    end
end

if (ntarg == 12 || ncent == 12)
    for i = 1:1:6
        pv(i, 12) = 0;
    end
end

if (ntarg == 13 || ncent == 13)
    for i = 1:1:6
        pv(i, 13) = pv(i, 3);
    end
end

if (ntarg * ncent == 30 && ntarg + ncent == 13)
    for i = 1:1:6
        pv(i, 3) = 0;
    end
else
    if (list(3) == 2)
        for i = 1:1:6
            pv(i, 3) = pv(i, 3) - pv(i, 10) / (1 + emrat);
        end
    end

    if (list(10) == 2)
        for i = 1:1:6
            pv(i, 10) = pv(i, 3) + pv(i, 10);
        end
    end
end

for i = 1:1:6
    rrd(i) = pv(i, ntarg) - pv(i, ncent);
end

bary = bsave;

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

function [pv, nut] = state(et2, list)

% reads and interpolates the jpl planetary ephemeris file

% input

%   et2    2-word julian ephemeris epoch at which interpolation
%          is wanted.  any combination of et2(1) + et2(2) which falls
%          within the time span on the file is a permissible epoch.

%          a. for ease in programming, the user may put the
%             entire epoch in et2(1) and set et2(2) = 0.

%          b. for maximum interpolation accuracy, set et2(1) equal
%             to the most recent midnight at or before interpolation
%             epoch and set et2(2) equal to fractional part of a day
%             elapsed between et2(1) and epoch.

%          c. as an alternative, it may prove convenient to set
%             et2(1) = some fixed epoch, such as start of integration,
%             and et2(2) = elapsed interval between and epoch.

%   list   12-word integer array specifying what interpolation
%          is wanted for each of the bodies on the file.
%
%          list(i) = 0 => no interpolation for body i
%                  = 1 => position only
%                  = 2 => position and velocity

%          the designation of the astronomical bodies by i is:

%              i =  1 => mercury
%                =  2 => venus
%                =  3 => earth-moon barycenter
%                =  4 => mars
%                =  5 => jupiter
%                =  6 => saturn
%                =  7 => uranus
%                =  8 => neptune
%                =  9 => pluto
%                = 10 => geocentric moon
%                = 11 => nutations in longitude and obliquity
%                = 12 => lunar librations (if on file)

% output

%   pv   6 x 11 array that will contain requested interpolated
%        quantities. the body specified by list(i) will have its
%        state in the array starting at pv(1, i). (on any given
%        call, only those words in 'pv' which are affected by the
%        first 10 'list' entries (and by list(12) if librations are
%        on the file) are set. the rest of the 'pv' array
%        is untouched).  the order of components starting in
%        pv(1, i) is x, y, z, dx, dy, dz.

%        all output vectors are referenced to the earth mean
%        equator and equinox of j2000 if the de number is 200 or
%        greater; of b1950 if the de number is less than 200.

%        the moon state is always geocentric; the other nine states
%        are either heliocentric or solar-system barycentric,
%        depending on the setting of common flags (see below).

%        lunar librations, if on file, are put into pv(k, 11) if
%        list(12) is 1 or 2.
%
%   nut  4-word array that will contain nutations and rates,
%        depending on the setting of list(11). the order of
%        quantities in nut is:

%        d psi  (nutation in longitude)
%        d epsilon (nutation in obliquity)
%        d psi dot
%        d epsilon dot

% global

%   km    logical flag defining physical units of the output states
%         = 1 => kilometers and kilometers/second
%         = 0 => au and au/day

%         default value = 0 (km determines time unit
%         for nutations and librations. angle unit is always radians.)

%   bary  logical flag defining output center.
%         only the 9 planets are affected.
%         bary = 1 => center is solar-system barycenter
%              = 0 => center is sun
%         default value = 0

%   pvsun 6-word array containing the barycentric position and
%         velocity of the sun

global ss au ipt lpt

global nrl fid km bary pvsun coef

nut = zeros(4, 1);

pv = zeros(6, 11);

if (et2(1) == 0)
    return
end

s = et2(1) - 0.5;

tmp = split(s);

pjd(1) = tmp(1);

pjd(2) = tmp(2);

tmp = split(et2(2));

pjd(3) = tmp(1);

pjd(4) = tmp(2);

pjd(1) = pjd(1) + pjd(3) + 0.5;

pjd(2) = pjd(2) + pjd(4);

tmp = split(pjd(2));

pjd(3) = tmp(1);

pjd(4) = tmp(2);

pjd(1) = pjd(1) + pjd(3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% error return for epoch out of range
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (pjd(1) + pjd(4) < ss(1) || pjd(1) + pjd(4) > ss(2))
    fprintf('\n\n error in state - epoch out of range \n');

    return;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate record number and relative time in interval
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

nr = fix((pjd(1) - ss(1)) / ss(3)) + 2;

if (pjd(1) == ss(2))
    nr = nr - 1;
end

t(1) = ((pjd(1) - ((nr-2) * ss(3) + ss(1))) + pjd(4)) / ss(3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% read correct record if not in core
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (nr ~= nrl)
    nrl = nr;

    status = fseek(fid, nr * 8144, 'bof');

    coef = fread(fid, 1018, 'double');
end

if (km == 1)
    t(2) = ss(3) * 86400;
    aufac = 1;
else
    t(2) = ss(3);
    aufac = 1 / au;
end

% interpolate barycentric state vector of sun

tmpv = zeros(3, 2);

ibuf = ipt(1, 11);
ncf = ipt(2, 11);
na = ipt(3, 11);

tmpv = interp(ibuf, t, ncf, 3, na, 2);

k = 0;

for j = 1:1:2
    for i = 1:1:3
        k = k + 1;
        pvsun(k) = tmpv(i, j) * aufac;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check and interpolate bodies requested
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tmpv = zeros(3, 2);

tempv = zeros(6, 1);

for i = 1:1:10
    if (list(i) ~= 0)

        ibuf = ipt(1, i);
        ncf = ipt(2, i);
        na = ipt(3, i);

        tmpv = interp(ibuf, t, ncf, 3, na, list(i));

        k = 0;

        for j = 1:1:2
            for m = 1:1:3
                k = k + 1;
                tempv(k) = tmpv(m, j);
            end
        end

        for j = 1:1:6
            if (i <= 9 && bary == 0)
                pv(j, i) = tempv(j) * aufac - pvsun(j);
            else
                pv(j, i) = tempv(j) * aufac;
            end
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% do nutations if requested (and if on file)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (list(11) > 0 && ipt(2, 12) > 0)

    ibuf = ipt(1, 12);
    ncf = ipt(2, 12);
    na = ipt(3, 12);

    nut = interp(ibuf, t, ncf, 2, na, list(11));
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get librations if requested (and if on file)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (list(12) > 0 && lpt(2) > 0)
    tmpv = interp(lpt(1), t, lpt(2), 3, lpt(3), list(12));

    for i = 1:1:3
        pv(i, 11) = tmpv(i, 1);

        pv(i + 3, 11) = tmpv(i, 2);
    end
end

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

function pv = interp(ibuf, t, ncf, ncm, na, ifl)

% this function differentiates and interpolates a set
% of chebyshev coefficients to give position and velocity

% input

%   buf   1st location of array of chebyshev coefficients of position

%   t     t(1) is fractional time in interval covered by
%         coefficients at which interpolation is wanted
%         (0 <= t(1) <= 1). t(2) is length of whole
%         interval in input time units.

%   ncf   number of coefficients per component

%   ncm   number of components per set of coefficients

%   na    number of sets of coefficients in full array
%         (i.e., number of sub-intervals in full interval)

%   ifl   integer flag
%         = 1 for positions only
%         = 2 for pos and vel

% output

%   pv    interpolated quantities requested.  dimension
%         expected is pv(ncm, ifl)

global coef np nv twot pc vc

pv = zeros(6, 12);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get correct sub-interval number for this set of
% coefficients and get normalized chebyshev time
% within that subinterval.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

dna = na;

dt1 = fix(t(1));

temp = dna * t(1);

ll = fix(temp - dt1) + 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% tc is the normalized chebyshev time (-1 <= tc <= 1)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

tc = 2 * (mod(temp, 1) + dt1) - 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% check to see whether chebyshev time has changed,
% and compute new polynomial values if it has.
% (the element pc(2) is the value of t1(tc) and hence
% contains the value of tc on the previous call)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (tc ~= pc(2))
    np = 2;
    nv = 3;
    pc(2) = tc;
    twot = tc + tc;
end

% be sure that at least 'ncf' polynomials have been evaluated
% and are stored in the array 'pc'.

if (np < ncf)
    for i = np + 1:1:ncf
        pc(i) = twot * pc(i - 1) - pc(i - 2);
    end

    np = ncf;
end

bcoef = ncf * na * ncm;

cbody = zeros(bcoef, 1);

n = ibuf;

for m = 1:1:bcoef
    cbody(m) = coef(n);
    n = n + 1;
end

cbuf = zeros(ncf, ncm, na);

n = 0;

for l = 1:1:na
    for i = 1:1:ncm
        for j = 1:1:ncf
            n = n + 1;
            cbuf(j, i, l) = cbody(n);
        end
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% interpolate to get position for each component
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:ncm
    pv(i, 1) = 0;

    for j = ncf:-1:1
        pv(i, 1) = pv(i, 1) + pc(j) * cbuf(j, i, ll);
    end
end

if (ifl <= 1)
    return
end

% if velocity interpolation is wanted, be sure enough
% derivative polynomials have been generated and stored.

vfac = (dna + dna) / t(2);

vc(3) = twot + twot;

if (nv < ncf)
    for i = nv + 1:1:ncf
        vc(i) = twot * vc(i - 1) + pc(i - 1) + pc(i - 1) - vc(i - 2);
    end

    nv = ncf;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% interpolate to get velocity for each component
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for i = 1:1:ncm
    pv(i, 2) = 0;

    for j = ncf:-1:2
        pv(i, 2) = pv(i, 2) + vc(j) * cbuf(j, i, ll);
    end

    pv(i, 2) = pv(i, 2) * vfac;
end

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

function fr = split(tt)

% this function breaks a number into a integer
% and a fractional part.

% input

%   tt = input number

% output

%   fr = 2-word output array
%        fr(1) contains integer part
%        fr(2) contains fractional part

%        for negative input numbers, fr(1) contains the next
%        more negative integer; fr(2) contains a positive fraction.

fr = zeros(2, 1);

fr(1) = fix(tt);

fr(2) = tt - fr(1);

if (tt >= 0 || fr(2) == 0)
    return
end

% make adjustments for negative input number

fr(1) = fr(1) - 1;

fr(2) = fr(2) + 1;


% By NG Hoi-Fung, Ivan
% Discription: Convert the NMEA sentence to time, llh data
% Output CSV format: gps time, latitude, longitude, MSL(m), Geoid(m), utc time(yyyymmddHHMMSS.FFF)
% csv file will output to the original path that nmea file directory

clc;
clear;
close all;

GPS_TIME_START = datetime(1980,1,6,0,0,0,0);
leap_second = 18;

nmea_path = 'E:\sparse and urban jiulongtang midundao Huawei VINS test\20200314\COM55_200314_093715.ubx'; % input nmea sentence directory
[nmea_dir, exp_name] = fileparts(nmea_path);

support_type = {'$G*GGA','$G*RMC','$G*GLL','$G*BWC'};

% GGA: UTC, Lat, NE, Lon, NE, Fix Quality, No Sat, HDOP, MSL(m), Geoid(m)
% RMC: UTC, Status, Lat, NE, Lon, NE, Speed, Track angle, Date, Magnetic, checksum
% GLL: Lat, NE, Lon, NE, UTC, Date active, checksum
% BWC: UTC, Lat, NE, Lon, NE,bearing(true), bearing(magnetic), dist to waypoint(NM), waypoint id, checksum
% RMB: data status, Cross-track error(NM), Origin waypoint ID, Destination waypoint ID

date_str = {};
S = textread(nmea_path,'%s');
nmea_pos = double.empty(0,4);

for i = 1:length(S)
    type = regexp(S{i}, regexptranslate('wildcard', support_type));
    sup = 0;
    sup = find(~cellfun(@isempty,type));
    k = strfind(S{i},'$GNRMC');
    if k
       disp('');
    end
    if ~isempty(sup)
        utc = nan; lat = nan; lon = nan; msl = nan; geoid = nan;
        lat_ = nan; lon_ = nan; date_ = nan;
        lat_factor = 1; lon_factor = 1;

        str = S{i};
        str = str(type{sup}:end)
        C = strsplit(str,',', 'CollapseDelimiters',false);
        %         disp('');
        switch(sup)
            case 1 % GGA
                utc = str2double(C{2});
                utc_ = C{2};
                try
                    lat_ = str2double(C{3});
                    if C{4}~='N'; lat_factor = -1; end
                catch
                end
                try
                    lon_ = str2double(C{5});
                    if C{6}~='E'; lon_factor = -1; end
                catch
                end
                try msl = str2double(C{10}); catch, end
                try geoid = str2double(C{12}); catch, end
            case 2 % RMC
                utc = str2double(C{2});
                utc_ = C{2};
                try
                    lat_ = str2double(C{4});
                    if C{5}~='N'; lat_factor = -1; end
                catch
                end
                try
                    lon_ = str2double(C{6});
                    if C{7}~='E'; lon_factor = -1; end
                catch
                end
                try
                    datetime(C{10},'InputFormat','ddMMyy');
                    if isnan(date_); date_ = C{10}; end
                catch
                end
            case 3 % GLL
                utc = str2double(C{6});
                utc_ = C{6};
                try
                    lat_ = str2double(C{2});
                    if C{3}~='N'; lat_factor = -1; end
                catch
                end
                try
                    lon_ = str2double(C{4});
                    if C{5}~='E'; lon_factor = -1; end
                catch
                end
            case 4 % BWC
                utc = str2double(C{2});
                utc_ = C{2};
                try
                    lat_ = str2double(C{3});
                    if C{4}~='N'; lat_factor = -1; end
                catch
                end
                try
                    lon_ = str2double(C{5});
                    if C{6}~='E'; lon_factor = -1; end
                catch
                end
        end

        try
            datetime(utc_,'InputFormat','HHmmss.S');
        catch
            utc_ = nan;
        end

        if ~isnan(lat_)
            degree = floor(lat_/100);
            minute = floor(lat_ - degree*100)/60;
            second = (lat_ - (degree*100 + floor(lat_ - degree*100)))*60/3600;
            lat = degree + minute + second;
        end

        if ~isnan(lon_)
            degree = floor(lon_/100);
            minute = floor(lon_ - degree*100)/60;
            second = (lon_ - (degree*100 + floor(lon_ - degree*100)))*60/3600;
            lon = degree + minute + second;
        end

        if isempty(nmea_pos)
            nmea_pos = [utc,nan,nan,nan,nan];
        end
        utc_idx = find(nmea_pos(:,1)==utc);
        if isempty(utc_idx)
            utc_idx = size(nmea_pos,1)+1;
            nmea_pos(utc_idx,:) = [utc,nan,nan,nan,nan];
        end
        if isnan(nmea_pos(utc_idx,2)) && ~isnan(lat); nmea_pos(utc_idx,2)=lat; end
        if isnan(nmea_pos(utc_idx,3)) && ~isnan(lon); nmea_pos(utc_idx,3)=lon; end
        if isnan(nmea_pos(utc_idx,4)) && ~isnan(msl); nmea_pos(utc_idx,4)=msl; end
        if isnan(nmea_pos(utc_idx,5)) && ~isnan(geoid); nmea_pos(utc_idx,5)=geoid; end
        if ~isnan(date_); date_str(utc_idx,1) = {date_}; end
        if ~isnan(utc_); date_str(utc_idx,2) = {utc_}; end
    end
end

empty_date_idx = find(cellfun(@isempty,date_str(:,1)));
date_idx = find(~cellfun(@isempty,date_str(:,1)));
for i = 1:size(empty_date_idx,1)
    [~, store_idx] = min(date_idx - empty_date_idx(i));
    date_str(empty_date_idx(i),1) = date_str(date_idx(store_idx),1);
end
validIdx = find(~isnan(nmea_pos(:,1)));
nmea_pos = nmea_pos(validIdx,:);
date_str = date_str(validIdx,:);
gps_time = zeros(size(date_str,1),1);
utc_time = zeros(size(date_str,1),1);
for i = 1:size(gps_time,1)
    tempt = datetime(strcat(date_str{i,:}),'InputFormat','ddMMyyHHmmss.S');
    gps_time(i) = seconds(tempt - GPS_TIME_START) + leap_second;
    utc_time(i) = str2double(datestr(tempt, 'yyyymmddHHMMSS.FFF'));
end
nmea_pos(:,1) = gps_time;
nmea_pos = [nmea_pos utc_time];

% if exist('plot_openstreetmap'); plot_openstreetmap(nmea_pos(:,[2:3])); end
if exist('plot_osm'); plot_osm(nmea_pos(:,[2:3])); end

% Output format: gps time, latitude, longitude, MSL(m), Geoid(m), utc time(yyyymmddHHMMSS.FFF)
nmea_output = [nmea_dir,'\',exp_name,'_nmea.csv'];
fid = fopen(nmea_output,'w+');
for i = 1:size(nmea_pos,1)
    fprintf(fid,'%d,%.7f,%.7f,%.1f,%.1f,%.2f\n',nmea_pos(i,:));
end
fclose(fid);

ublox_data = csvread('COM55_200314_093715_nmea.csv');
%
kmlwrite('COM55_200314_093715_nmea.kml',ublox_data(:,2),ublox_data(:,3),'Icon',...
    'http://maps.google.com/mapfiles/kml/shapes/shaded_dot.png','IconScale',0.5,'Color',[1,0,0],'Name','  ');


% dl = gp_dlogpdf(y_invK_dK_invK_y, trace_invK_dK)

% dl = gp_dlogpdf(dK, invK, invK_y)

function dl = gp_dlogpdf(y_invK_dK_invK_y, trace_invK_dK)
dl = 0.5*y_invK_dK_invK_y - 0.5*trace_invK_dK;
end
% $$$ function dl = gp_dlogpdf(dK, invK, invK_y)
% $$$ dl = 0.5*(invK_y'*dK*invK_y) - 0.5*traceprod(invK,dK);
% $$$ end


function [CD CLmax CDv qratio]= wingpolar(CL,AC,tState)
% inputs:
%   AC: see documentation
%   tState: see documentation
%   CL: wing lift coefficient; L = CL*q*Sref_wing
% outputs:
%   CD: wing drag coefficient; D = CD*q*Sref_wing
%   CLmax: WING maximum lift coefficient
%   CDv: drag coefficient of the wing due to the vertical downwash
%   qratio: ratio of downwash q at wing to 1/2*rho*(v_induced)^2

if AC.Wing.S
    e = .9;
    CD0 = .009;
    CD = CD0 + CL.^2./(pi*e*AC.Wing.AR);
else
    CD = .009;
end

if nargout >1
CLmax = 1.2; %this could be improved by adjusting CLmax to the AR
CDv = 1.3; %drag coefficient of the wing to the vertical downwash
qratio = .6; %ratio of downwash q at wing to 1/2*rho*(v_induced)^2
end


normX = AllFeature';
normX = bsxfun(@rdivide, normX, sqrt(sum(normX.^2,2)));
% [normX, PCAmap] = compute_mapping(normX, 'PCA', 319);
% fid = fopen('G:\face-CASIA-WebFace\sim_label_test.txt');
% C = textscan(fid,'%d');
% fclose(fid);
% sim = C{1};
% fid = fopen('G:\face-CASIA-WebFace\test_data1.txt');
% C = textscan(fid, '%s %d');
% fclose(fid);
% [mappedX, mapping] = JointBayesian(normX, C{2});
num = size(normX,1) / 2;

thresh = zeros(num,1);

% normX = AllFeature';
% normX = mappedX;
for i = 1:num
%     thresh(i) = normX(i,:) * mapping.A * normX(i,:)' + normX(num+i,:) * mapping.A * normX(num+i,:)' - 2 * normX(i,:) * mapping.G * normX(num+i,:)';
    thresh(i) = pdist2(normX(i,:), normX(num+i,:));
end;

num = size(AllFeature1,2);
F1 = AllFeature1';
F1 = bsxfun(@rdivide, F1, sqrt(sum(F1.^2,2)));
% F1 = bsxfun(@minus,F1,PCAmap.mean);
% F1 = F1 * PCAmap.M;
F2 = AllFeature2';
F2 = bsxfun(@rdivide, F2, sqrt(sum(F2.^2,2)));
% F2 = bsxfun(@minus,F2,PCAmap.mean);
% F2 = F2 * PCAmap.M;
% F1 = AllFeature1';
% F2 = AllFeature2';
thresh2 = zeros(num,1);
for i = 1:num
%     thresh2(i) = F1(i,:) * mapping.A * F1(i,:)' + F2(i,:) * mapping.A * F2(i,:)' - 2 * F1(i,:) * mapping.G * F2(i,:)';
    thresh2(i) = pdist2(F1(i,:),F2(i,:));
end;

% model = train(double(sim),sparse(thresh),'-s 2');
% predicted_label = predict(double(sim),sparse(thresh),model);
bestc=256;
lfw_label = ones(6000,1);
lfw_label(3001:6000) = 0;
% predicted_label = predict(double(lfw_label),sparse(thresh2),model);
cmd = [' -t 0 -h 0'];
model = svmtrain(double(sim),thresh,cmd);
[class] = svmpredict(lfw_label,thresh2,model);


function d = dpoly(obj,feat,varargin)
% INPUTS:
% obj contains the mesh points and the outer, mainland, inner polygons,
% bbox (the bounding box)
% feat contains inpoly_flip to check whether to flip the inpoly result or not
%
% OUTPUTS:
% d is the distance from point, p to closest point on polygon
% (d is  negative if inside the bounded polygon, pv and positive if outside)
% by Keith Roberts and William Pringle 2017-2018.
if nargin == 3
    p  = varargin{1};
else
    [xg,yg] = CreateStructGrid(obj);
    p = [xg(:),yg(:)];
    clearvars xg yg
end
%% Doing the distance + inpoly calc
if ~isempty(obj.lmsl)
    pv = [obj.lmsl.mainland; obj.lmsl.inner];
    pg = [obj.lmsl.outer; obj.lmsl.inner];
else
    pv = [feat.mainland; feat.inner];
    pg = [feat.outer; feat.inner];
end
edges = Get_poly_edges( pg );
pv1 = pv; % <-dup for inpoly to work
pv1(isnan(pv(:,1)),:) = []; clear pv;
d = 0*p(:,1); in = false(size(d));
noblks = ceil(length(p)*2*8*1e-9);
blklen = floor(length(p)/noblks);
ns = 1;
disp(['Memory management: number of loops in dpoly = ' num2str(noblks)])
tic
for blks = 1:noblks
    if blks == noblks
        ne = length(p);
    else
        ne = ns + blklen - 1;
    end
    %% Doing the distance check
    [~,d(ns:ne)] = WrapperForKsearch(pv1, p(ns:ne,:), 1);

    %% Doing the inpoly check
    in(ns:ne) = inpoly(p(ns:ne,:), pg, edges);

    ns = ne + 1;
end
toc

% d is negative if inside polygon and vice versa.
if feat.inpoly_flip
    d = (-1).^(~in).*d;
else
    d = (-1).^( in).*d;
end
end


function [x,fval,exitflag,info] = opti_nl2sol(fun,grad,x0,ydata,lb,ub,opts)
%OPTI_NL2SOL Solve a NLS using NL2SOL (Adaptive Nonlinear Least Squares)
%
%   min sum[ (F(x) - ydata)^2 ]       subject to:   lb <= x <= ub
%    x
%
%   x = opti_nl2sol(fun,grad,x0,ydata) solves a NLS where fun is the fitting
%   function. grad is an optional gradient of the fitting function and x0
%   is a starting guess. ydata is the data to fit the function to. Use
%   ydata = zeros() for solving a SNLE.
%
%   x = opti_nl2sol(fun,grad,x0,ydata,lb,ub) solves subject to decision
%   variables bounds lb <= x <= ub. Infinite bounds are OK.
%
%   x = opti_nl2sol(fun,grad,x0,ydata,lb,ub,opts) uses opts to pass optiset
%   options to the solver.
%
%   [x,fval,exitflag,info] = opti_nl2sol(...) returns the objective value at
%   the solution, together with the solver exitflag, and an information
%   structure.
%
%   THIS IS A WRAPPER FOR NL2SOL + NL2SNO
%   See referenced ACM License

%   Copyright (C) 2012 Jonathan Currie (IPL)

if(nargin < 7), opts = optiset; end
if(nargin < 6), ub = []; end
if(nargin < 5), lb = []; end
if(nargin < 4), error('NL2SOL requires at least 4 arguments'); end

%Setup display level
opts.display = dispLevel(opts.display);
opts.optiver = optiver;

%Check we have a valid x0
if(isempty(x0) || any(isnan(x0)))
    error('NL2SOL requires an initial guess, x0!');
end

t = tic;
% Run NL2SOL
[x, fval, exitflag, iter, feval] = nl2sol(fun,grad,x0,ydata,lb,ub,opts);

%Collect Results
info.Iterations = iter;
info.FuncEvals = feval;
info.Time = toc(t);
info.Algorithm = 'NL2SOL: Adaptive Nonlinear Least Squares';

switch(exitflag)
    case 1
        info.Status = 'Optimal';
    case 0
        info.Status = 'Exceeded Iterations';
    case -1
        info.Status = 'Infeasible / Could not Converge';
    case -2
        info.Status = 'NL2SOL Error';
    case -5
        info.Status = 'User Exited';
    otherwise
        info.Status = 'NL2SOL Error';
end


function [D,S] = dictlearn(varargin)

% References:
%     "Online Learning for Matrix Factorization and Sparse Coding"
%     by Julien Mairal, Francis Bach, Jean Ponce and Guillermo Sapiro
%     arXiv:0908.0050
%
%     "Online Dictionary Learning for Sparse Coding"
%     by Julien Mairal, Francis Bach, Jean Ponce and Guillermo Sapiro
%     ICML 2009.

args = arg_define(varargin, ...
    arg_nogui({'X','Data'},[],[]), ...
    arg({'whiten','Whiten'}, true, [], 'Whiten the data.'), ...
    arg({'rescale','Rescale'}, true, [], 'Rescale the data.'), ...
    arg({'shuffle','Shuffle'}, true, [], 'Shuffle the data.'), ...
    arg({'haveMemory','HaveMemory'}, true, [], 'Have lots of memory.'), ...
    arg({'D','InitialGuess'},[],[],'Initial guess.'), ...
    arg({'K','NumComponents'},256,[], 'Number of components.'), ...
    arg({'iter','MaxIterations'},500,[], 'Maximum number of iterations.'), ...
    arg({'mode','ObjectiveFunction'},'(1/2)||x_i-Dalpha_i||_2^2 + lambda||alpha_i||_1 + lambda_2||alpha_i||_2^2',{'(1/2)||x_i-Dalpha_i||_2^2  s.t. ||alpha_i||_1 <= lambda','||alpha_i||_1  s.t. ||x_i-Dalpha_i||_2^2 <= lambda','(1/2)||x_i-Dalpha_i||_2^2 + lambda||alpha_i||_1 + lambda_2||alpha_i||_2^2','(1/2)||x_i-Dalpha_i||_2^2  s.t. ||alpha_i||_0 <= lambda','||alpha_i||_0  s.t.  ||x_i-Dalpha_i||_2^2 <= lambda','0.5||x_i-Dalpha_i||_2^2 +lambda||alpha_i||_0'},'Objective function to impose.'), ...
    arg({'modeD','DictionaryConstraints'},'||d_j||_2^2 <= 1',{'||d_j||_2^2 <= 1','||d_j||_2^2 + gamma1||d_j||_1 <= 1','||d_j||_2^2 + gamma1||d_j||_1 + gamma2 FL(d_j) <= 1','(1-gamma1)||d_j||_2^2 + gamma1||d_j||_1 <= 1'},'Constraints to impose on dictionary.'), ...
    arg({'modeParam','OptimizationMode'},'parameter-free (ICML)',{'parameter-free (ICML)','using rho (arXiv)','exponential decay weights (rho as memory factor, arXiv)'},'Optimization mode to use.'), ...
    arg({'lambda','Lambda'},1,[], 'Main penalty parameter.'), ...
    arg({'lambda2','Lambda2'},1,[], 'Second penalty parameter.'), ...
    arg({'posAlpha','PositiveActivations'},false,[], 'Enforce positive activations. Not compatible with learning modes 3 and 4.'), ...
    arg({'posD','PositiveDictionary'},false,[], 'Enforce positive dictionary. Not compatible with constraint set 2.'), ...
    arg({'gamma1','GammaParam1'},[],[], 'Gamma parameter 1 for dictionary constraint set.'), ...
    arg({'gamma2','GammaParam2'},[],[], 'Gamma parameter 2 for dictionary constraint set.'), ...
    arg({'batchsize','MinibatchSize'},512,[], 'Size of the mini-batches to use.'), ...
    arg({'iter_updateD','NumBCDIterations'},1,[], 'Number of block-coordinate descent iterations. For the dictionary update step.'), ...
    arg({'rho','TuningParameterRho'},[],[], 'Rho tuning parameter. For online opt mode.'), ...
    arg({'t0','TuningParameterT0'},[],[], 'T0 tuning parameter. For online opt mode.'), ...
    arg({'clean','PruneUnused'},true,[], 'Prune unused elements.'), ...
    arg({'verbose','Verbose'},true,[], 'Verbose output.'), ...
    arg({'numThreads','NumThreads'},16,[], 'Number of threads.'), ...
    arg_nogui({'chanlocs','Chanlocs'},16,[], 'Channel locations.'));

% fix up arguments
if isempty(args.D)
    args = rmfield(args,'D'); end
if isempty(args.gamma1)
    args = rmfield(args,'gamma1'); end
if isempty(args.gamma2)
    args = rmfield(args,'gamma2'); end
if isempty(args.rho)
    args = rmfield(args,'rho'); end
args.modeParam = hlp_rewrite(args.modeParam,'parameter-free (ICML)',0, 'using rho (arXiv)',1, ...
    'exponential decay weights (rho as memory factor, arXiv)',3);
args.mode = hlp_rewrite(args.mode, ...
    '(1/2)||x_i-Dalpha_i||_2^2  s.t. ||alpha_i||_1 <= lambda',0, ...
    '||alpha_i||_1  s.t. ||x_i-Dalpha_i||_2^2 <= lambda',1, ...
    '(1/2)||x_i-Dalpha_i||_2^2 + lambda||alpha_i||_1 + lambda_2||alpha_i||_2^2',2, ...
    '(1/2)||x_i-Dalpha_i||_2^2  s.t. ||alpha_i||_0 <= lambda',3,...
    '||alpha_i||_0  s.t.  ||x_i-Dalpha_i||_2^2 <= lambda',4,...
    '0.5||x_i-Dalpha_i||_2^2 +lambda||alpha_i||_0',5);
args.modeD = hlp_rewrite(args.modeD, ...
    '||d_j||_2^2 <= 1',0, ...
    '||d_j||_2^2 + gamma1||d_j||_1 <= 1',1,...
    '||d_j||_2^2 + gamma1||d_j||_1 + gamma2 FL(d_j) <= 1',2, ...
    '(1-gamma1)||d_j||_2^2 + gamma1||d_j||_1 <= 1',3);

X = args.X;

%args.whiten = false;

% Whiten and rescale the data
if args.whiten
    sphere = inv(real(sqrtm(cov_robust(X'))));
else
    sphere = eye(size(X,1));
end
X = sphere*X;

if args.rescale
    m = sqrt(sum(X.^2) + (1e-8));
    X = bsxfunwrap(@rdivide,X,m);
end

if args.shuffle
    idx = shuffle(1:size(X,2));
    X = X(:,idx);
end

%% Run the dictionary learning
if args.haveMemory
    D = mexTrainDL_Memory(X,args);
else
    D = mexTrainDL(X,args);
end
S = sphere;
%Df = inv(S)*D;


function [ a, b] = p04_interval ( )

%*****************************************************************************80
%
%% P04_INTERVAL returns a starting interval for optimization for problem 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2002
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real A, B, two points defining an interval in which
%    the local minimizer should be sought.
%
  a = 0.0001;
  b = 1.0;

  return
end


% erpregoutfunc() - sub function of erpregout() used to regress
%                   out the ERP from the data
%
% Usage:
%   totdiff = erpregout(fact, data, erp);
%
% Inputs:
%   fact    - factor
%   data    - [float] 1-D data (time points).
%   erp     - [float] 1-D data (time points).
%
% Outputs:
%   totdif  - residual difference
%
% Author: Arnaud Delorme, Salk, SCCN, UCSD, CA, April 29, 2004

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2004 Arnaud Delorme
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function totdiff = erpregoutfunc(fact, data, erp);

    totdiff = mean(abs(data - fact*erp));


function feature = feature_pyramid(image, detect_max_scale, detect_min_scale)

%Make sure image is in double format
image = double(image);
if nargin==1
    detect_max_scale = 2.0;
    detect_min_scale = .25;
% detect_max_scale = 2.0;
% detect_min_scale = 0.1;
end

%Hardcoded maximum number of levels in the pyramid
MAXLEVELS = 200;

%Hardcoded minimum dimension of smallest (coarsest) pyramid level
MINDIMENSION = 5;

%Get the levels per octave from the parameters
interval = 5;

sc = 2 ^(1/interval);

% Start at detect_max_scale, and keep going down by the increment sc, until
% we reach MAXLEVELS or detect_min_scale
feature.scale = zeros(1,MAXLEVELS);
feature.feat = {};
for i = 1:MAXLEVELS
    scaler = detect_max_scale / sc^(i-1);

    if scaler < detect_min_scale
        return
    end

    feature.scale(i) = scaler;
    if feature.scale(i)>1
        scaled = imresize(image, feature.scale(i));
    else
        scaled = resize(image,feature.scale(i));
    end
    %if minimum dimensions is less than or equal to 5, exit
    if min([size(scaled,1) size(scaled,2)])<=MINDIMENSION
        feature.scale = feature.scale(feature.scale>0);
        return;
    end

    feature.feat{i} = HOGMe(scaled,8,8);

    %if we get zero size feature, backtrack one, and dont produce any
    %more levels
    if (size(feature.feat{i},1)*size(feature.feat{i},2)) == 0
        feature.feat = feature.feat(1:end-1);
        feature.scale = feature.scale(1:end-1);
        return;
    end

    %recover lost bin!!!
    feature.feat{i} = padarray(feature.feat{i}, [1 1 0], 0);

    %if the max dimensions is less than or equal to 5, dont produce
    %any more levels
    if max([size(feature.feat{i},1) size(feature.feat{i},2)])<=MINDIMENSION
        feature.scale = feature.scale(feature.scale>0);
        return;
    end
end


function christmas

% Anselm Ivanovas, anselm.ivanovas@student.unisg.ch

%Basically just a nice plot for some christmas fun.
%3D Plot of a hhristmas tree with some presents and snow

%% setup
snow=800;     % number of snow flakes [0 .. 5000]

%% draw tree
h=0:0.2:25; %vertical grid
[X,Y,Z] = cylinder(tree(h)); %produce a tree formed cylinder
Z=Z*25; %scale to the right heigth

%Add some diffusion to the surface of the tree to make it look more real

treeDiffusion=rand(126,21)-0.5;%some horizontal diffusion data

%add diffusion to the grid points
for cnt1=1:21

    for cnt2=16:126%starting above the trunk
        %get the angle to always diffuse in direction of the radius
        angle=atan(Y(cnt2,cnt1)/X(cnt2,cnt1));
        %split the diffusion in the two coordinates, depending on the angle
        X(cnt2,cnt1)=X(cnt2,cnt1)+cos(angle)*treeDiffusion(cnt2,cnt1);
        Y(cnt2,cnt1)=Y(cnt2,cnt1)+sin(angle)*treeDiffusion(cnt2,cnt1);
        %some Vertical diffusion for each point
        Z(cnt2,cnt1)=Z(cnt2,cnt1)+(rand-0.5)*0.5;
    end

end
%draw the tree
surfl(X,Y,Z,'light')

%% View and format

%Use as nice green color map (darker at the bottom, lighter at the top)
r=(0.0430:(0.2061/50):0.2491)';%red component
g=(0.2969:(0.4012/50):0.6981)';%green component
b=(0.0625:(0.2696/50):0.3321)';%blue component
map=[r,g,b];%join in a map
for cnt=1:6
    %change the lower part to brown for the trunk
    map(cnt,:)=[77,63,5]/265;
end

colormap(map)%set the map
view([-37.5,4])%Change the view to see a little more of the Actual 3D tree
lighting phong %some nice lighting
shading interp %remove grid and smoothen the surface color
axis equal %takes care of display in the right proportion
axis([-10 10 -10 10 0 30]) %give some more axis space (for the snow later)
axis off %but don't show axis
hold on %to draw the rest
title('HAPPY HOLIDAYS')%self explaining

%% Presents
%Draw some presents around the tree (each with random color)
drawPresent(2,-4,0,3,3,2);
drawPresent(-4,3,0,2,3,1.5);
drawPresent(5,3,0,4,3,3);
drawPresent(-14,-5,0,6,3,1);
drawPresent(-9,-10,0,2,2,2);
drawPresent(0,4,0,4,3,3);
drawPresent(-6,-13,0,3,3,3);

%% Snow

%create some random 3D coordinates for the snow (amount as in setup above)
snowX=(rand(snow,1)*25-12.5);
snowY=(rand(snow,1)*25-12.5);
snowZ=(rand(snow,1)*27);
%Note:Some flakes will end up IN the tree but just can't be seen then
plot3(snowX,snowY,snowZ,'w*')%plot coordinates as white snow flakes
hold off%Done
end % of function

%% ============= private functions

function r=tree(h)%Gives a profile for the tree
for cnt=1:length(h)

    if(h(cnt)==0)%no Width at the bottom. Ensures a "closed" trunk
        r(cnt)=0;
    end
    %smaller radius for the trunk
    if (h(cnt)>0 && h(cnt)<=3)
        r(cnt)=1.5;
    end

    %reduce radius gradually from 8 to 0. Note: will only work with a trunk heigth
    %of 3 and a whole tree heigth of 25. Scale the height of the tree in
    %the "draw tree" section, since the cylinder command will return a 1
    %unit high cylinder anyway
    if(h(cnt)>3)
        r(cnt)=8-(h(cnt)-3)*0.3636;
    end

end

end % of function

%Draws a present with the given coordinate + size in a random color
%Note:Given coordinates apply to the lower front + left corner of the
%present (the one closest to the viewer) as seen in the plot
function drawPresent(dx,dy,dz,scalex,scaley,scalez)

%the standard present coordinates
presentX=[0.5 0.5 0.5 0.5 0.5; 0 1 1 0 0; 0 1 1 0 0; 0 1 1 0 0; 0.5 0.5 0.5 0.5 0.5];
presentY=[0.5 0.5 0.5 0.5 0.5; 0 0 1 1 0; 0 0 1 1 0; 0 0 1 1 0; 0.5 0.5 0.5 0.5 0.5];
presentZ=[0 0 0 0 0; 0 0 0 0 0; 0.5 0.5 0.5 0.5 0.5; 1 1 1 1 1; 1 1 1 1 1];

%draw some presents with random colors
%scale present and move it to the right place and get the plot handle
myHandle=surf((presentX*scalex+dx),(presentY*scaley+dy), (presentZ*scalez+dz));
%some random color map
randColorMap(:,:,1)=repmat(rand,[5,5]);%r component
randColorMap(:,:,2)=repmat(rand,[5,5]);%g component
randColorMap(:,:,3)=repmat(rand,[5,5]);%b component
%Assign colormap just to the plot handle object of the present, so the tree
%does not change color
set(myHandle,'CData',randColorMap)
shading interp %Nice shding + without grid

end % of function


function pout = propagate_bounds_from_arbitrary_quadratics(p)

pout = p;
if p.bilinears~=0
    F_struc = p.F_struc;

    p.F_struc = [-p.F_struc(1:p.K.f,:);p.F_struc];
    p.K.f=2*p.K.f;

    InequalityConstraintState = [p.EqualityConstraintState;p.EqualityConstraintState;p.InequalityConstraintState];

    if p.K.l+p.K.f>0
        quadratic_variables = find(p.bilinears(:,2) == p.bilinears(:,3));
        if ~isempty(quadratic_variables)
            quadratic_variables = p.bilinears(quadratic_variables,1);
            for i = 1:length(quadratic_variables)
                k = quadratic_variables(i);

                if (p.lb(k) < p.ub(k)-1e-4)
                    x = p.bilinears(p.bilinears(:,1)==k,2);% x^2
                    candidates = find((InequalityConstraintState==1) & p.F_struc(1:p.K.f+p.K.l,1+k))';
                    for j = candidates
                        a = p.F_struc(j,2:end);
                        b = p.F_struc(j,1);
                        aij = a(k);

                        % rest + cij*x + (+-)x^2 >= 0
                        cij = a(x);
                        cij = cij/abs(aij);
                        b = b/abs(aij);
                        a = a/abs(aij);
                        %  aij = sign(aij);

                        a(k) = 0;
                        a(x) = 0;
                        indNEG = find(a < 0);
                        indPOS = find(a > 0);
                        LB = p.lb;
                        UB = p.ub;
                        LB(k) = 0;
                        UB(k) = 0;
                        LB(x) = 0;
                        UB(x) = 0;
                        a(k) = 0;
                        a(x) = 0;

                        if aij < 0
                            % rest + cij*x - x^2 >= 0

                            % Derive upper bound on rest
                            rest = (b+a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)]);
                            % Write as rest >= (x - center)^2-(center)^2
                            center = cij/2;
                            radii2 = rest + (cij/2)^2;
                            if radii2 > 0
                                newUB = center + sqrt(radii2);
                                newLB = center - sqrt(radii2);
                                p.lb(x) = max(p.lb(x),newLB);
                                p.ub(x) = min(p.ub(x),newUB);
                            end
                        else
                            % rest + cij*x + x^2 >= 0
                            % (x+cij/2)^2 - (cij/2)^2 + rest >= 0
                            % (x+cij/2)^2 >= radii
                            % Lower bound on rest
                            rest = (b+a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)]);
                            center = -cij/2;
                            radii2 = -rest + (cij/2)^2;
                            if radii2 > 0
                                left  = center-sqrt(radii2);
                                right = center+sqrt(radii2);
                                if p.ub(x) < right
                                    p.ub(x) = min(p.ub(x),left);
                                end
                                if p.lb(x) > left
                                    p.lb(x) = max(p.lb(x),right);
                                end
                            end
                        end
                    end

                elseif p.lb(k)<0

                end
            end
        end
    end

    if p.K.l+p.K.f>0
        bilinear_variables = find(p.bilinears(:,2) ~= p.bilinears(:,3));
        if ~isempty(bilinear_variables)
            bilinear_variables = p.bilinears(bilinear_variables,1);
            for i = 1:length(bilinear_variables)
                k = bilinear_variables(i);
                if p.lb(k) >= -5000000000 & (p.lb(k) < p.ub(k)-1e-4)
                    x = p.bilinears(p.bilinears(:,1)==k,2);% x^2
                    y = p.bilinears(p.bilinears(:,1)==k,3);% x^2
                    candidates = find((InequalityConstraintState==1) & p.F_struc(1:p.K.f+p.K.l,1+k))';
                    for j = candidates
                        a = p.F_struc(j,2:end);
                        aij = a(k);
                        if aij > 0
                            indNEG = find(a < 0);
                            indPOS = find(a > 0);
                            LB = p.lb;
                            UB = p.ub;
                            LB(k) = 0;
                            UB(k) = 0;
                            a(k) = 0;
                            newLB = (-p.F_struc(j,1)-a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)])/aij;
                            p.lb(k) = max(p.lb(k),newLB);

                        elseif aij < 0
                            indNEG = find(a < 0);
                            indPOS = find(a > 0);
                            LB = p.lb;
                            UB = p.ub;
                            LB(k) = 0;
                            UB(k) = 0;
                            a(k) = 0;
                            newUB = (p.F_struc(j,1)+a([indPOS(:);indNEG(:)])*[UB(indPOS);LB(indNEG)])/(-aij);
                            p.ub(k) = min(p.ub(k),newUB);
                        end
                    end

                    if p.lb(k)>0 & p.lb(x)>0 & p.lb(y)>0
                        p.ub(x) = min(p.ub(x), p.ub(k)/p.lb(y));
                        p.ub(y) = min(p.ub(y), p.ub(k)/p.lb(x));
                        p.lb(x) = max(p.lb(x), p.lb(k)/p.ub(y));
                        p.lb(y) = max(p.lb(y), p.lb(k)/p.ub(x));
                    end

                elseif p.lb(k)<0

                end
            end
        end
    end
end

if ~isequal([p.lb p.ub],[pout.lb pout.ub])
    quad_v = find(p.bilinears(:,2) == p.bilinears(:,3));
    quad_x = p.bilinears(quad_v,2);
    quad_v = p.bilinears(quad_v,1);
    if ~isempty(quad_v)
        % y = x^2, x>=0, y >= L means x >= sqrt(L)
        k = find(p.lb(quad_x)>=0 & p.lb(quad_v)>0);
        if ~isempty(k)
            p.lb(quad_x(k)) = max(p.lb(quad_x(k)),sqrt(p.lb(quad_v(k))));
        end
    end
end

if ~isequal([p.lb p.ub],[pout.lb pout.ub])
    pout.changedbounds = 1;
end
pout.lb = p.lb;
pout.ub = p.ub;


function y = UVtOmega(U,V,I,J,col);
% The values of U*V' in Omega, as a vector
% Omega is given by I,J, col

y = zeros(length(I), 1);
for k = 1:length(col)-1
    j = J(col(k)+1);
    Xj = U * V(j,:)';
    idx = [col(k)+1:col(k+1)];
    y(idx) = Xj(I(idx));
end

% for j = 1:length(col)-1
%     Xj = U * V(j,:)';
%     idx = [col(j)+1:col(j+1)];
%     y(idx) = Xj(I(idx));
% end


function varargout = min(varargin)
%MIN   Minimum value of a SPHEREFUN in one direction.
%   MIN(f) returns a chebfun representing the minimum of the SPHEREFUN along the
%   latitude direction, i.e, MIN(f) = @(lambda) max( f ( lambda, : ) )
%
%   MIN(f, [], dim) returns a CHEBFUN representing the minimum of f along the
%   DIM direction. If DIM = 1 is along the latitude-direction and DIM = 2 is along the
%   longitude-direction.
%
%   WARNING: This function is not always accurate to the expected precision.
%
%   For the global minimum use MIN2.
%
% See also SPHEREFUN/MAX, SPHEREFUN/MAX2, SPHEREFUN/MIN2, SPHEREFUN/MINANDMAX2.

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

[varargout{1:nargout}] = min@separableApprox(varargin{:});

end