function blas1_z_test16 ( )

%*****************************************************************************80
%
%% TEST16 tests ZSIGN2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2006
%
%  Author:
%
%    John Burkardt
%
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST16\n' );
  fprintf ( 1, '  ZSIGN2 ( C1, C2 ) transfers the sign of complex C2\n' );
  fprintf ( 1, '  to the ZABS2 magnitude of C1.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '           C1                    C2                    C3\n' );
  fprintf ( 1, ...
    '  --------------------  --------------------  --------------------\n' );
  fprintf ( 1, '\n' );

  for j = 1 : 10

    [ c1, seed ] = c8_uniform_01 ( seed );
    c1 = 5.0 * c1;
    [ c2, seed ] = c8_uniform_01 ( seed );
    c2 = 5.0 * c2;
    c3 = zsign2 ( c1, c2 );

    fprintf ( 1, '  %10f  %10f  %10f  %10f  %10f  %10f\n', ...
      real ( c1 ), imag ( c1 ), real ( c2 ), imag ( c2 ), ...
      real ( c3 ), imag ( c3 ) );

  end

  return
end


function r = isempty(a)
%ISEMPTY      Returns 1 if input is empty, i.e. [], in the sense of Matlab
%
%   r = isempty(a)
%
%There are no empty intervals in the mathematical sense in INTLAB. For details,
%  please see Readme.txt
%

% written  10/15/99     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
% modified 10/12/05     S.M. Rump  functionality exchanged with @intval\isempty_
% modified 04/22/09     S.M. Rump  isempty_ removed
%

  if a.complex
    r = isempty(a.mid) | isempty(a.rad);
  else
    r = isempty(a.inf) | isempty(a.sup);
  end


function outface=outersurf(node,face)
%
% outface=outersurf(node,face)
%
% extract the out-most shell of a complex surface mesh
%
% author: Qianqian Fang, <q.fang at neu.edu>
%
% input:
%    node:  node coordinates
%    face:  surface triangle list
%
% output:
%    outface: the out-most shell of the surface mesh
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%

face=face(:,1:3);

ed=surfedge(face);
if(~isempty(ed))
   error('open surface is detected, you have to close it first, consider meshcheckrepair() with meshfix option');
end

[no,el]=fillsurf(node,face);

outface=volface(el);

[no,outface]=removeisolatednode(no,outface);
maxfacenode=max(outface(:));

[I,J]=ismember(round(no(1:maxfacenode,:)*1e10),round(node*1e10),'rows');

% if(sum(I)~=maxfacenode)
%     error('mesh tessellation failed');
% end
outface=J(outface);
[ii,jj]=find(outface==0);
outface(ii,:)=[];


function tests = test_ft_specest_irasa

% MEM 1gb
% WALLTIME 00:10:00
% DEPENDENCY ft_specest_irasa

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

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

fsample = 1000;
nchan   = 2;
nsample = 1000;

dat     = randn(nchan, nsample) + 1 + linspace(0,1,nsample);
time    = (0:(nsample-1))/fsample;
freqoi  = 1:150;

result = {};
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'original');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi(2:2:end), 'output', 'original');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi(2:2:end), 'output', 'fractal');

% try out another padding
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'original', 'pad', 2);
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal',  'pad', 2);

% try out the various padding types, don't remove the polynomial fit
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', -1, 'pad', 2, 'padtype', 'zero');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', -1, 'pad', 2, 'padtype', 'mean');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', -1, 'pad', 2, 'padtype', 'localmean');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', -1, 'pad', 2, 'padtype', 'edge');
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', -1, 'pad', 2, 'padtype', 'mirror');

% the default polyorder 1 is already covered earlier
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', 0);
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', 2);
result{end+1} = ft_specest_irasa(dat, time, 'verbose', false, 'freqoi', freqoi, 'output', 'fractal', 'polyorder', 3);

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


%%% Setup
% restoredefaultpath;
close, clear, clc;

%%% Path
addpath(fullfile(pwd,'utils'));

%%% Params
params.sequences_path = fullfile(pwd,'sequences');
params.sequences_mat = fullfile(pwd,'sequences_mat');
params.sequences_name = get_sequences_name(params);
params.sequences_format = 'PNG';
params.sequences_ext = strcat('*.',params.sequences_format);
params.debug = 0;
%params.current_sequence = 1;
clc;

%% Frame selection
displog('Performing frame selection');
for seq = 1:size(params.sequences_name,2)
    %seq = 1;
    displog(['Processing sequence: ' params.sequences_name(seq).name]);
    params.current_sequence = seq;
    perform_frame_selection(params);
    %break;
end
clear seq;

%% Matrix Completion
clc;
params.algs_path = 'algs_mc';
params.algs_name = get_algs_name(params);
params.results_path = 'results_mc';
displog('--- Matrix Completion ---');
for alg = 1:size(params.algs_name,2)
  %alg = 1;
  displog(['Current algorithm: ' params.algs_name(alg).name]);
  params.current_algorithm = alg;
  for seq = 1:size(params.sequences_name,2)
    %seq = 1;
    displog(['Processing sequence: ' params.sequences_name(seq).name]);
    params.current_sequence = seq;
    perform_matrix_completion(params);
    %break;
  end
  %break;
end
clear alg seq;

%% Tensor Completion
clc;
params.algs_path = 'algs_tc';
params.algs_name = get_algs_name(params);
params.results_path = 'results_tc';
displog('--- Tensor Completion ---');
for alg = 1:size(params.algs_name,2)
  %alg = 1;
  displog(['Current algorithm: ' params.algs_name(alg).name]);
  params.current_algorithm = alg;
  for seq = 1:size(params.sequences_name,2)
    %seq = 1;
    displog(['Processing sequence: ' params.sequences_name(seq).name]);
    params.current_sequence = seq;
    perform_tensor_completion(params);
    %break;
  end
  %break;
end
clear alg seq;


%IOPEN Morphological opening
%
% OUT = IOPEN(IM, SE, OPTIONS) is the image IM after morphological opening
% with the structuring element SE.  This is a morphological erosion followed
% by dilation.
%
% OUT = IOPEN(IM, SE, N, OPTIONS) as above but the structuring element
% SE is applied N times, that is N erosions followed by N dilations.
%
% Notes::
% - For binary image an opening operation can be used to eliminate small white
%   noise regions.
% - It is cheaper to apply a smaller structuring element multiple times than
%   one large one, the effective structuring element is the Minkowski sum
%   of the structuring element with itself N times.
% - Windowing options of IMORPH can be passed.  By default output image is
%   same size as input image.
%
% See also ICLOSE, IDILATE, IERODE, IMORPH.

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

function b = iopen(a, se, varargin)

    b = ierode(a, se, varargin{:});
    b = idilate(b, se, varargin{:});


%--- help for dsge/accuracy ---
%
%  Solves dsge models
%
%  ::
%
%    Euler_errors=accuracy(m)
%    Euler_errors=accuracy(m,nxcuts)
%    Euler_errors=accuracy(m,nxcuts,nsims)
%    Euler_errors=accuracy(m,nxcuts,nsims,nburn)
%    Euler_errors=accuracy(m,nxcuts,nsims,nburn,reset_params)
%    [Euler_errors,outsims]=accuracy(...)
%
%  Args:
%
%     m (rise | dsge): scalar or vector of model objects.
%
%     nxcuts ({10} | integer): Number of quantiles per discretized
%       continuous shock
%
%     nsims ({1000} | integer| 1x2 cell): Number of simulations to generate
%       the state of the endogenous variables. Alternatively, if the input
%       is a cell, no simulations will be performed. In this case nsims is
%       of the form nsims={Y,regs}, where:
%       - Y : simulations of endogenous variables
%       - regs : corresponding simulations for regimes
%
%     nburn ({100} | integer): Number of burned simulations
%
%     reset_params ({true} | false): reset the parameters to their correct
%       values in evaluating the model equations.
%
%  Returns:
%     :
%
%     - **Euler_errors** [cell | struct]: if the number of models is greater
%       that one, the output is a cell array of structures. Each structure
%       has the following fields:
%       - EE [1 x nsols cell] : Euler equation errors for all the solutions
%       with each cell for one solution
%       - eqtn1, eqtn2,... : structures containing "mean" (mean of the euler
%       error across all simulations), "max" (maximum of the euler error
%       across all simulations) and "min" (minimum of the euler error across
%       all simulations)
%
%     - **outsims** [1 x 2 x nsols cell]: Simulations for the endogenous
%       variables (first element) and the regimes (second element) for each
%       solution (3rd dimension)
%
%  Note:
%
%     - The errors are log10(abs(error))
%
%     - The smaller the standard deviation of the shocks the higher the
%       accuracy even if the solution is not very accurate in the first
%       place.
%
%     - The accuracy of the solution may critically depend on the tolerance
%       level used for solving the model in the first place. For instance,
%       if an iterative algorithm (e.g. mfi) is used, the solution might not
%       be 100% accurate even in a linear model
%
%     - The errors are provided in absolute value for all equations, rather
%       than being normalized as is customarily done in the literature
%
%


function visInferredShapes(class, jobID)
% Visualize fitted basis shapes cached in 'inferredShapes'

% Setup directories
globals;
inferredShapesDir = fullfile(cachedir, class, sprintf('inferredShapes%s',jobID));
meshDir = fullfile(cachedir, class, sprintf('meshes%s',jobID));
dmapDir = fullfile(cachedir, class, sprintf('depthMap%s',jobID));
fnames = getFileNamesFromDirectory(inferredShapesDir,'types',{'.mat'});
hFig = figure;
load(fullfile(datadir,'partNames',class));
for i = 1:length(fnames)
    fprintf('%s: VOC ID = %s\n',class, fnames{i}(1:end-4));
    load(fullfile(inferredShapesDir,fnames{i}));
    try
        mesh = load(fullfile(meshDir,fnames{i}));
    catch
        mesh = state2mesh(state);
    end

    subplot(231)
    imshow(state.im);
    title('Image');

    subplot(232)
    im = (color_seg(state.mask,state.im));
    %im = insertText(im,state.kps(1:2,:)',partNames,'FontSize',8);
    imshow(im);
    hold on;
    cmap = distinguishable_colors(size(state.kps,2),[0 1 1]);
    scatter(state.kps(1,:), state.kps(2,:),30,cmap,'Filled');
    hold off;
    title('Image Mask (and Keypoints)');

    subplot(233)
    imVerts = mesh.vertices;
    vertNormals = state.normals*state.cameraRot';

    %Flip z axis to be in the image frame
    imVerts(:,3) = -imVerts(:,3);
    imVerts(:,3) = imVerts(:,3)-min(0,min(imVerts(:,3)));
    zDepth = imVerts(:,3);
    vertNormals(:,3) = -vertNormals(:,3);

    imshow(state.im); hold on;

    %patch('vertices', imVerts, 'faces', mesh.faces, 'CData', zDepth,'VertexNormals',vertNormals,...
    %    'CDataMapping','scaled', 'FaceColor', 'interp', 'FaceAlpha', ...
    %     0.7, 'EdgeColor', 'None');

     patch('vertices', imVerts, 'faces', state.tri, 'FaceColor', 'red', 'VertexNormals',...
         vertNormals, 'FaceAlpha', 0.7, 'EdgeColor','None','FaceLighting','gouraud',...
         'BackFaceLighting','lit');

    camlight; lighting p;
    camproj('orthographic');
    hold off;
    title('Mesh overlayed on Image');

    subplot(234)
    showMeshTri(mesh);view(0,-90);
    title('Inferred Mesh');

    subplot(235)
    % Colored Point Cloud
    imsize = size(state.im); imsize = imsize(1:2);
    imVerts = mesh.vertices;

    % Mask project vertices only in mask
    maskInds = find(state.mask(:));
    imVerts(:,1) = max(1,min(imVerts(:,1),imsize(2)));
    imVerts(:,2) = max(1,min(imVerts(:,2),imsize(1)));
    linInds = sub2ind(imsize,round(imVerts(:,2)),round(imVerts(:,1)));
    [linInds, IA] = intersect(linInds,maskInds);
    imVerts = imVerts(IA,:);
    % Find colors for the points
    rCh = state.im(:,:,1); rCh = rCh(:);
    gCh = state.im(:,:,2); gCh = gCh(:);
    bCh = state.im(:,:,3); bCh = bCh(:);
    ptColors = [rCh(linInds) gCh(linInds) bCh(linInds)];
    % Plot
    scatter3(imVerts(:,1),imVerts(:,2),imVerts(:,3),15,single(ptColors)/255,'filled');
    axis off vis3d equal; view(0,-90);
    title('Colored Point Cloud');

    subplot(236)
    % Texture mapped depth map / Depth map
    try
        load(fullfile(dmapDir,fnames{i}));
    catch
        mesh = reducepatch(mesh,2000);
        dmap = meshToDepth(mesh.vertices,mesh.faces,size(state.mask));
        dmap(isinf(dmap)) = nan;
        dmap(~state.mask) = nan;
    end

    hIm = imagesc(imcrop(visualizeDEM(dmap),state.bbox));
    %set(hIm,'AlphaData',repmat(~isnan(imcrop(dmap,state.bbox)),[1 1 3]));
    %warp(dmap,state.im); view(0,90);
    axis equal off;
    title('Depth map');

    %subplotsqueeze(hFig, 1.2);
    pause; clf
end


function springDampers = springDampersVisualise(springDamperVals, startPoints, ...
                                         staticPoints, widths, springConst, ...
                                         damperConst)

% SPRINGDAMPERSVISUALISE Helper code for showing an spring dampers during 2-D visualisation.
% FORMAT
% DESC is a helper function for plotting spring damper locations using latent
% variable models.
% ARG springDamperValues : the values to set the springDamper data to.
% ARG startPoints : the fixed points of the spring damers.
% ARG widths : the widths of the spring dampers.
% ARG springConst : the spring constants of the spring dampers.
% ARG damperConst : the damper constants of the spring dampers.
% RETURN springDampers : contains array of spring damper objects.
%
% COPYRIGHT : Neil D. Lawrence, 2007
%
% SEEALSO : springDampersModify, fgplvmResultsDynamic

% MLTOOLS

for i = 1:length(springDamperVals)
  springDampers(i) = springDamperCreate(startPoints(i, :), ...
                                        staticPoints(i, :), ...
                                        widths(i), springConst(i), ...
                                        damperConst(i));
  springDampers(i).selected = 0;
end
springDampers = springDampersModify(springDampers, springDamperVals, ...
                                       startPoints, staticPoints, widths, ...
                                       springConst, damperConst);
for i = 1:length(springDamperVals)
  set(springDampers(i).handle, 'linewidth', 2)
  set(springDampers(i).spring.handle, 'linewidth', 2)
  set(springDampers(i).damper.handle, 'linewidth', 2)
end


function options = svmRunOptions(varargin)
% options = svmRunOptions(varargin)
% SUPPORT VECTOR MACHINE - RUN OPTIONS
% ---------------------------------------------------------
% Produces structure with parameters describing how to perform a
% classification.
%
% INPUTS
%	OPTIONS
%       'Procedure' -
%           'KFolds' - Split trials into k approximately equal folds,
%               removing each fold one by one, training on the remainder,
%               and testing on the withheld fold.
%           'ScanFolds' - KFolds where the folds are scans/runs.
%           'LeaveOneOut' - KFolds where the folds are individual trials.
%       'Verbose' - Whether or not to print MATLAB generated output.
%           DEFAULT: false
%       'Options' - Pre-existing options structure.
%       'OptionsFile' - Path to file containing pre-existing options
%           structure.
%       'k' - K parameter for KFolds. DEFAULT: 10
%       's' | 'SVM_Type' - See libSVM help.
%       't' | 'Kernel_Type' - See libSVM help.
%       'd' | 'Degree' - See libSVM help.
%       'g' | 'Gamma' - See libSVM help.
%       'r' | 'Coef0' - See libSVM help.
%       'c' | 'Cost' - See libSVM help.
%       'n' | 'Nu' - See libSVM help.
%       'p' | 'Epsilon_Loss' - See libSVM help.
%       'm' | 'CacheSize' - See libSVM help.
%       'e' | 'Epsilon' - See libSVM help.
%       'h' | 'Shrinking' - See libSVM help.
%       'b' | 'Probability_Estimates' - See libSVM help.
%       'w' | 'Weight' - See libSVM help.
%       'q' | 'Quiet' - See libSVM help.
%
% OUTPUTS
%   options - Structure containing all parameters to be used in svmRun, or
%       elsewhere.
%
% USAGE
%   Setting up the parameters to run a classification with KFolds, k = 20.
%   This options structure is then passed to svmRun.
%       options = svmRunOptions('Procedure', 'KFolds', 'k', 20);
%
%   Often, this function is called from within another function, such as
%   svmRun, which will accept the same inputs.
%       svmRun(..., 'Procedure', 'KFolds', 'k', 20);
%
% See also SVMINIT, SVMRUN, SVMEXPORTMAP, SVMBOOTSTRAP, SVMRELABEL,
% SVMREMOVE, SLINIT.
%
% renobowen@gmail.com [2010]
%

    % Lib SVM options
    options.svm_type = [];
    options.kernel_type = 0;
    options.degree = [];
    options.gamma = [];
    options.coef0 = [];
    options.cost = [];
    options.nu = [];
    options.epsilon_loss = [];
    options.cachesize = [];
    options.epsilon = [];
    options.shrinking = [];
    options.probability_estimates = [];
    options.weight = [];
    options.quiet = 1;
    options.subsample = false;

    % MATLAB side options
    options.procedure = 'scanfolds';
    options.verbose = false;
    options.k = 10;

    i = 1;
    while (i <= length(varargin))
        if (isempty(varargin{i})), break; end
        switch (lower(varargin{i}))
            case {'options'} % careful with this one and the next - we assume you know what you're doing with the options struct
                options = varargin{i + 1};
            case {'optionsfile'}
                load(varargin{i + 1});
            case {'procedure'}
                options.procedure = varargin{i + 1};
            case {'k'}
                options.k = varargin{i + 1};
            case {'log2cvector'}
                options.log2cvector = varargin{i + 1};
            case {'log2gvector'}
                options.log2gvector = varargin{i + 1};
            case {'svm_type' 's'}
                options.svm_type = varargin{i + 1};
            case {'kernel_type' 't'}
                options.kernel_type = varargin{i + 1};
            case {'degree' 'd'}
                options.degree = varargin{i + 1};
            case {'gamma' 'g'}
                options.gamma = varargin{i + 1};
            case {'coef0' 'r'}
                options.coef0 = varargin{i + 1};
            case {'cost' 'c'}
                options.coef = varargin{i + 1};
            case {'nu' 'n'}
                options.nu = varargin{i + 1};
            case {'epsilon_loss' 'p'}
                options.epsilon_loss = varargin{i + 1};
            case {'cachesize' 'm'}
                options.cachesize = varargin{i + 1};
            case {'epsilon' 'e'}
                options.epsilon = varargin{i + 1};
            case {'shrinking' 'h'}
                options.shrinking = varargin{i + 1};
            case {'probability_estimates', 'b'}
                options.probability_estimates = varargin{i + 1};
            case {'weight' 'w'}
                options.weight = [varargin{i + 1} varargin{i + 2}];
                i = i + 1;
            case {'quiet' 'q'} % Silence LIBSVM side outputs
                options.quiet = 1;
                i = i - 1;
            case {'verbose'}
                options.verbose = true;
                i = i - 1;
            case {'silent'} % Silence MATLAB side outputs
                options.verbose = false;
                i = i - 1;
            case {'subsample'}
                subsample = varargin{i+1};
            otherwise
                fprintf(1, 'Unrecognized option: ''%s''\n', varargin{i});
                return;
        end
        i = i + 2;
    end

end


function Q1 = mpc1(T_meas,T_sp)

s = 'http://byu.apmonitor.com';
c = 'mpc';

% input measurement
apm_meas(s,c,'TC',T_meas);

% input setpoint with deadband +/- DT
DT = 0.1;
apm_option(s,c,'TC.sphi',T_sp+DT);
apm_option(s,c,'TC.splo',T_sp-DT);

% solve MPC
output = apm(s,c,'solve');

% test for successful solution
if (apm_tag(s,c,'apm.appstatus')==1)
    % retrieve the first Q value
    Q1 = apm_tag(s,c,'Q1.Newval');
else
    % display output for debugging
    print(output)
    % not successful, set voltage to zero
    Q1 = 0;
end

end


function problem = inputValidation(problem)
%
% This function runs through the problem struct and sets any missing fields
% to the default value. If a mandatory field is missing, then it throws an
% error.
%
% INPUTS:
%   problem = a partially completed problem struct
%
% OUTPUTS:
%   problem = a complete problem struct, with validated fields
%

%%%% Check the function handles:

if ~isfield(problem,'func')
    error('Field ''func'' cannot be ommitted from ''problem''');
else
    if ~isfield(problem.func,'dynamics')
        error('Field ''dynamics'' cannot be ommitted from ''problem.func'''); end
    if ~isfield(problem.func,'pathObj'), problem.func.pathObj = []; end
    if ~isfield(problem.func,'bndObj'), problem.func.bndObj = []; end
    if ~isfield(problem.func,'pathCst'), problem.func.pathCst = []; end
    if ~isfield(problem.func,'bndCst'), problem.func.bndCst = []; end
end

%%%% Check the initial guess (also compute nState and nControl):
if ~isfield(problem, 'guess')
    error('Field ''guess'' cannot be ommitted from ''problem''');
else
    if ~isfield(problem.guess,'time')
        error('Field ''time'' cannot be ommitted from ''problem.guess'''); end
    if ~isfield(problem.guess, 'state')
        error('Field ''state'' cannot be ommitted from ''problem.guess'''); end
    if ~isfield(problem.guess, 'control')
        error('Field ''control'' cannot be ommitted from ''problem.guess'''); end

    % Compute the size of the time, state, and control based on guess
    [checkOne, nTime] = size(problem.guess.time);
    [nState, checkTimeState] = size(problem.guess.state);
    [nControl, checkTimeControl] = size(problem.guess.control);

    if nTime < 2 || checkOne ~= 1
        error('guess.time must have dimensions of [1, nTime], where nTime > 1');
    end

    if checkTimeState ~= nTime
        error('guess.state must have dimensions of [nState, nTime]');
    end
    if checkTimeControl ~= nTime
        error('guess.control must have dimensions of [nControl, nTime]');
    end

end

%%%% Check the problem bounds:
if ~isfield(problem,'bounds')
    problem.bounds.initialTime = [];
    problem.bounds.finalTime = [];
    problem.bounds.state = [];
    problem.bounds.initialState = [];
    problem.bounds.finalState = [];
    problem.bounds.control = [];
else

    if ~isfield(problem.bounds,'initialTime')
        problem.bounds.initialTime = []; end
    problem.bounds.initialTime = ...
        checkLowUpp(problem.bounds.initialTime,1,1,'initialTime');

    if ~isfield(problem.bounds,'finalTime')
        problem.bounds.finalTime = []; end
    problem.bounds.finalTime = ...
        checkLowUpp(problem.bounds.finalTime,1,1,'finalTime');

    if ~isfield(problem.bounds,'state')
        problem.bounds.state = []; end
    problem.bounds.state = ...
        checkLowUpp(problem.bounds.state,nState,1,'state');

    if ~isfield(problem.bounds,'initialState')
        problem.bounds.initialState = []; end
    problem.bounds.initialState = ...
        checkLowUpp(problem.bounds.initialState,nState,1,'initialState');

    if ~isfield(problem.bounds,'finalState')
        problem.bounds.finalState = []; end
    problem.bounds.finalState = ...
        checkLowUpp(problem.bounds.finalState,nState,1,'finalState');

    if ~isfield(problem.bounds,'control')
        problem.bounds.control = []; end
    problem.bounds.control = ...
        checkLowUpp(problem.bounds.control,nControl,1,'control');

end

end

function input = checkLowUpp(input,nRow,nCol,name)
%
% This function checks that input has the following is true:
%   size(input.low) == [nRow, nCol]
%   size(input.upp) == [nRow, nCol]

if ~isfield(input,'low')
    input.low = -inf(nRow,nCol);
end

if ~isfield(input,'upp')
    input.upp = inf(nRow,nCol);
end

[lowRow, lowCol] = size(input.low);
if lowRow ~= nRow || lowCol ~= nCol
    error(['problem.bounds.' name ...
        '.low must have size = [' num2str(nRow) ', ' num2str(nCol) ']']);
end

[uppRow, uppCol] = size(input.upp);
if uppRow ~= nRow || uppCol ~= nCol
    error(['problem.bounds.' name ...
        '.upp must have size = [' num2str(nRow) ', ' num2str(nCol) ']']);
end

if sum(sum(input.upp-input.low < 0))
    error(...
        ['problem.bounds.' name '.upp must be >= problem.bounds.' name '.low!']);
end

end


if ~exist('namecore','var')
    namecore = 'D10ptBMrandPSFgauss_StretchCoef2_N10_offset100_nc';
end
chall=2*ones(length(iteration),length(ncvec),(ncvec(end)-1)^2);
for ii=1:length(ncvec)
    for jj=1:length(iteration)
%         r=load (['D10ptBMrandPSFgauss_StretchCoef2_N10_offset100_nc' num2str(ncvec(ii)) '/results_iter' num2str(iteration(jj)) '.mat']);
        r=load ([namecore num2str(ncvec(ii)) '/results_iter' num2str(iteration(jj)) '.mat']);
        if isfield(r.res,'a')
            r.res.h=r.res.a;
        end
        %     r=load (['results_updates_nmfclassic_nc' num2str(ncvec(ii)) '/results_iter' num2str(iteration) '.mat']);
        if isfield(r.peval,'data_dir')
            d = load(['~/' r.peval.data_path '/' r.peval.data_dir '/' r.peval.data_file]);
        elseif isfield (r.peval,'data_file')
            d=load(['~/' peval.data_path '/' peval.data_file]);
        else
            if exist('dpixc', 'var')
                d.dpixc = dpixc;
            else
                d = load ('dpixc');
            end
        end
        resid = (r.res.w*r.res.h - reshape(d.dpixc, r.peval.nx*r.peval.ny, r.peval.nt));
        % pixels correlations:
        cp = (corrcoef(resid'));
        cpmax(jj,ii) = max(cp(cp<1));
        cpmin(jj,ii) = min(cp(cp<1));
        cpmean(jj,ii) = mean(cp(cp<1));
        cpmeanpos(jj,ii) = mean(cp(and(cp>0,cp<1)));
        cpmeanneg(jj,ii) = mean(cp(cp<0));
        % intensity correlations:
        if size(r.res.h,1)>2
            ch = corr(r.res.h(1:end-1,:)');
            chall(jj,ii,1:numel(ch))=ch(:);
            chmax(jj,ii) = max(ch(ch<1));
            %     chmin(ii) = min(min(corr(r.res.h(1:end-1,:)')-eye(size(r.res.h,1)-1)));
            chmin(jj,ii) = min(ch(:));

            chmean(jj,ii)=mean(ch(ch<1));
            chmeanpos(jj,ii)=mean(ch(and(ch<1,ch>0)));
            chmeanneg(jj,ii)=mean(ch(ch<0));
        end
%         h=hinton(ch);
        ll(jj,ii)=loglikelihoodPoisson(reshape(d.dpixc,r.peval.nx*r.peval.ny,r.peval.nt),r.res.w*r.res.h);
        if isfield(r.res, 'lb')
            lb(jj,ii) = r.res.lb;
        end

    end
end

% figure
% plot(ncvec, chmax.*(abs(chmin)),'s-r')


clc;
clear all;
for i=1:200
    I=imread(strcat('\',num2str(i),'.tif'));
    Imatch_L=imread(strcat('',num2str(i),'.tif'));

    I1=I(:,:,1);
    I2=I(:,:,2);
    I3=I(:,:,3);
    I4=I(:,:,4);

    Imatch_1=imresize(Imatch_L(:,:,1),4);
    Imatch_2=imresize(Imatch_L(:,:,2),4);
    Imatch_3=imresize(Imatch_L(:,:,3),4);
    Imatch_4=imresize(Imatch_L(:,:,4),4);

    Jmatch1=imhist(Imatch_1);
    Iout1=histeq(I1,Jmatch1);

    Jmatch2=imhist(Imatch_2);
    Iout2=histeq(I2,Jmatch2);

    Jmatch3=imhist(Imatch_3);
    Iout3=histeq(I3,Jmatch3);

    Jmatch4=imhist(Imatch_4);
    Iout4=histeq(I4,Jmatch4);

    I_out=cat(3,Iout1,Iout2,Iout3,Iout4);

    imwrite(I_out,strcat('',num2str(i),'.tif'))

end


function plot(catalogObject, varargin)
    %CATALOG.PLOT Plot hypocenters in map view and cross-sections
    %
    %
    %   Optional name/value pairs:
    %     'nsigma' - controls how zoomed-in the axes are (default
    %     5)

    % Glenn Thompson 2014/06/01
    if all(isnan(catalogObject.lat))
        warning('No hypocenter data to plot');
        return
    end
    p = inputParser;
    p.addParamValue('nsigma', '5', @isstr);
    p.parse(varargin{:});
    nsigma = p.Results.nsigma;

    % change region
    region = get_region(catalogObject, nsigma);

    % Compute Marker Size
    symsize = get_symsize(catalogObject);

    figure;
    set(gcf,'Color', [1 1 1]);

    % lon-lat plot
    axes('position',[0.05 0.45 0.5 0.5]);
    scatter(catalogObject.lon, catalogObject.lat, symsize);
    grid on;
    %set(gca, 'XLim', [region(1) region(2)]);
    %set(gca, 'YLim', [region(3) region(4)]);
    xlabel('Longitude');

    % depth-longitude
    axes('position',[0.05 0.05 0.5 0.35]);
    scatter(catalogObject.lon, catalogObject.depth, symsize);
    ylabel('Depth (km)');
    xlabel('Longitude');
    grid on;
    set(gca, 'YDir', 'reverse');
    %set(gca, 'XLim', [region(1) region(2)]);

    % depth-lat
    axes('position',[0.6 0.45 0.35 0.5]);
    scatter(catalogObject.depth, catalogObject.lat, symsize);
    xlabel('Depth (km)');
    %set(gca, 'XDir', 'reverse');
    ylabel('Latitude');
    grid on;
    %set(gca, 'YLim', [region(3) region(4)]);

%             % world map
%             load plotEarthquakes/plates.mat
%             coast = load('coast');
%             figure
%             worldmap world
%             setm(gca,'mlabelparallel',-90,'mlabellocation',90)
%             plotm(coast.lat,coast.long,'Color','k')
%             plotm(lat,lon,'LineWidth',2)
%
%             %% Find the first plate
%             % Look for the first NaN and stop there.
%             ind = find(isnan(lat),1,'first')
%             plotm(lat(1:ind),lon(1:ind),'Color','red','Linewidth',3)

end


% NMFLAB for Signal Processing written by A. Cichocki and R. Zdunek
% in cooperation with other members of Laboratory for Advanced Brain Signal
% Processing, BSI, RIKEN, Saitama, JAPAN

function [A,X,Distance_output]=nmf_smart_alg(Y,r,Index_norm,A_true,S, mc_on, NoAlts,restart_mc_on,AL,Y_true,type_alg_A,type_alg_X, max_restart,no_iter, alpha, alpha_BE, omegaX, omegaA, alphaS)
%
%
% Non-negative Matrix Factorization (NMF) with Extended SMART algorithms
%
% [A,X]=nmf_smart_alg(Y, r, Index_norm, A_true, S, mc_on, NoAlts, restart_mc_on, AL, Y_true, type_alg_A, type_alg_X, max_restart, no_iter,
%       alpha, alpha_BE, omegaX, omegaA, alphaS)
%       returns mixing matrix A of dimension [m by r],
%       and source matrix X of dimension [r by T], for the linear mixing model: AX = Y,
%       where Y is an observation matrix [m by T].
% Note: > m: number of sensors,
%       > r: number of sources,
%       > T: number of samples,
%
% INPUTS:
%       > Index_norm:    vector of 13 binary entries indicating which number
%                        divergence measures are turned on in the View Options,
%
%       > A_true:        true mixing matrix (only for synthetic data),
%       > S:             true source matrix (only for synthetic data),
%       > mc_on:         1 - Monte Carlo analysis enabled, 0 - otherwise,
%       > NoAlts:        number of alternating steps (only for Monte Carlo
%                        analysis and the option "Fixed Alternatings" is selected)
%       > restart_mc_on: 1 - restarts in Monte Carlo analysis are enabled,
%                        0 - otherwise,
%       > AL:            mixing matrix estimaed from the preceeding layer,
%       > Y_true:        the first layer mixed signals (mixtures),
%       > type_alg_A:    indicates the selected algorithm for computation
%                        of the mixing matrix,
%       > type_alg_X:    indicates the selected algorithm for computation of the sources,
%       > no_iter:       number of inner iterations,
%       > alpha:         parameter "alpha" in the Amari alpha-divergence
%       > alpha_BE:      parameter "alpha" in the Bose-Einstein divergence
%       > omegaX:        relaxation parameter (omega) in computation of the sources,
%       > omegaA:        relaxation parameter (omega) in computation of
%                        the mixing matrix,
%       > alphaS:        parameter of non-linear projection in computation
%                        of the sources,
%
% OUTPUTS:
%       > A:               estimated mixing matrix,
%       > X:               estimated source matrix,
%       > Distance_output: structures of different divergences measured between "Y" and estimated "AX" versus iterations,
%
% #########################################################################
A = [];
X = [];
if (nargin < 19) | isempty(alphaS) | max(size(alphaS) > 1)
   disp('Incorrect parameter alphaS');
   return
end
if (nargin < 18) | isempty(omegaA) | max(size(omegaA) > 1)
   disp('Incorrect parameter alpha');
   return
end
if (nargin < 17) | isempty(omegaX) | max(size(omegaX) > 1)
   disp('Incorrect parameter omegaX');
   return
end
if (nargin < 16) | isempty(alpha_BE) | max(size(alpha_BE) > 1)
   disp('Incorrect parameter alpha in the Bose_Einstein divergence');
   return
end
if (nargin < 15) | isempty(alpha) | max(size(alpha) > 1)
   disp('Incorrect parameter alpha');
   return
end
if (nargin < 14) | isempty(no_iter) | (no_iter < 1) | max(size(no_iter) > 1)
   disp('Incorrect number of inner iterations');
   return
end
if (nargin < 13) | isempty(max_restart)  | (max_restart < 0) | max(size(max_restart) > 1)
   disp('Number of restarts must be given correctly');
   return
end
if (nargin < 12) | isempty(type_alg_X) | (type_alg_X < 1) | max(size(type_alg_X) > 1)
   disp('Incorrect algorithm for X');
   return
end
if (nargin < 11) | isempty(type_alg_A) | (type_alg_A < 1) | max(size(type_alg_A) > 1)
   disp('Incorrect algorithm for A');
   return
end
if (nargin < 10) | isempty(Y_true)
   disp('The first layer mixed signals are unknown');
   Y_true = zeros(size(Y_true));
end
if (nargin < 9) | isempty(AL)
   disp('Mixing matrix from the preceeding layer unknown');
   AL = eye(size(Y,1));
end
if (nargin < 8) | isempty(restart_mc_on) | max(size(restart_mc_on) > 1)
   disp('Index od restarts in MC analysis unknown');
   restart_mc_on = 0;
end
if (nargin < 7) | isempty(NoAlts) | max(size(NoAlts) > 1)
   disp('Adjustable number of alternatings');
   NoAlts = [];
end
if (nargin < 6) | isempty(mc_on) | max(size(mc_on) > 1)
   disp('No Monte Carlo Analysis');
   mc_on = 0;
end
if (nargin < 5) | isempty(S)
   disp('X_true not given');
end
if (nargin < 4) | isempty(A_true)
   disp('A_true not given');
   index_fixed_A = 1;
else
   index_fixed_A = 0;
end
if (nargin < 3) | isempty(Index_norm)
   '"Index_norm" must be specified'
   return
end
if (nargin < 2) | isempty(r)
   'Rank of factorization must be given'
   return
end
if isempty(Y) | isnan(Y)
   error('No data');
   return
end
% test for negative values in Y
if min(min(Y)) < 0
    disp('Some matrix entries are changed from negative to small positive');
    Y(Y< 0) = eps;
end
if min(sum(Y,2)) == 0
    disp('Not all entries in a row can be zero');
    return
end

if (alpha == 0)
    if (type_alg_X == 1)
        type_alg_X = 2;
    end
    if (type_alg_A == 1)
        type_alg_A = 2;
    end
end

if (type_alg_A == 12) & (size(A_true,1) ~= size(Y,1))
   disp('Multilayer technique cannot be used with A fixed');
   Distance_output = [];
   return
end

Y = Y + eps;

[m,T]=size(Y);
niter_selected = 2000;     % maximum number of iterations for the selected sample (can be adjusted)
niter_sample = 30; % maximum number of iterations for each random sample
epsil_normA = 1E-4; % tolerance for alternating

% Monte Carlo and alternatings adjustment
if mc_on & ~restart_mc_on
    max_restart = 0;
end
if ~isempty(NoAlts)
    niter_selected = NoAlts;
end

% Declaration for A and X
A=zeros(m,r);
Ap = A;
X=zeros(r,T);
Ainit = A;
Xinit = X;
nr_best = -1;
Z = zeros(m,T);
KL_outer_temp = 0;
nr = 0; restart_on = 0; norm_A = 10;
m_sx = 1:m; r_sx = 1:r; T_sx = 1:T; s_dist = 0;

while (nr <= max_restart)

     % Initialize random A and X
       if ~nr & (~mc_on | restart_mc_on)
          Ainit(m_sx',r_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,r) + repmat(.1*cos(2*pi*.1*r_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,r) + repmat(sin(2*pi*.471*r_sx),m,1));
          Ainit = Ainit/max(max(Ainit));

          Xinit(r_sx',T_sx) = abs(repmat(.1*sin(2*pi*.1*r_sx'),1,T) + repmat(.1*cos(2*pi*.1*T_sx),r,1) + repmat(cos(2*pi*.471*r_sx'),1,T) + repmat(sin(2*pi*.471*T_sx),r,1));
          Xinit = Xinit/max(max(Xinit));
       else
          Ainit=rand(m,r);
          Xinit=rand(r,T);
       end

        % Normalization of initial guess
        Ainit = Ainit*diag(1./sum(Ainit,1));

        if (nr == max_restart)&(max_restart > 0)
           A = A_best;
           X = X_best;
        else
           A = Ainit;
           X = Xinit;
        end % initial guess assignment

    Yx = zeros(m,T);
    n = 0; k = 0;

while ((k <= niter_sample)&(nr < max_restart)) | ((k <= niter_selected)&(nr == max_restart)&(norm_A > epsil_normA)& isempty(NoAlts)) | ((k <= niter_selected)&(nr == max_restart)& (NoAlts > 0))

k = k + 1;

if no_iter == 1

    switch type_alg_X

        case 1 % Asymmetric alpha-divergence

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 2 % EG-DKL

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 3 % EG-RAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 4 % EG-SAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 5 % EG-DJ

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 6 % EG-RJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 7 % EG-DRJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 8 % EG-SJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 9 % EG-Tri

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 10 % EG-BE

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1 + alphaS);

        case 11 % Pinv

            X = max(1E6*eps,pinv(A)*Y);

        case 12 % Fixed X

            X = S + eps;
            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            X = diag(sum(X,2))*X;

    end % switch for X

 switch type_alg_A

        case 1 % Asymmetric alpha-divergence

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 2 % EG-DKL

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 3 % EG-RAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 4 % EG-SAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 5 % EG-DJ

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 6 % EG-RJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 7 % EG-DRJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 8 % EG-SJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 9 % EG-Tri

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 10 % EG-BE

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 11 % Pinv

            Ap = A;
            A = max(1E6*eps,Y*pinv(X')');
            A = A*diag(1./sum(A,1));

          case 12 % Fixed A

            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            A = A_true + eps;
            A = A*diag(1./(sum(A,1) + eps));

    end % switch for A

else % no_iter

   for t = 1:no_iter

        switch type_alg_X

        case 1 % Asymmetric alpha-divergence

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 2 % EG-DKL

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 3 % EG-RAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 4 % EG-SAG

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 5 % EG-DJ

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 6 % EG-RJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 7 % EG-DRJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 8 % EG-SJS

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 9 % EG-Tri

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 10 % EG-BE

            gamma_m = sum(A,1);
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            X = (X.*exp(diag(omegaX./gamma_m)*(A'*Q))).^(1+alphaS);

        case 11 % Pinv

            X = max(1E6*eps,pinv(A)*Y);

        case 12 % Fixed X

            X = S + eps;
            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            X = diag(sum(X,2))*X;

    end % switch for X

   end % for t in X

   for t = 1:no_iter

       switch type_alg_A

        case 1 % Asymmetric alpha-divergence

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (1/alpha)*((Y./Z).^alpha - 1);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 2 % EG-DKL

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(Y./Z + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

        case 3 % EG-RAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*Y./(Z + Y) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 4 % EG-SAG

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(2*sqrt(Y.*Z)./(Z + Y) + eps) + (Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 5 % EG-DJ

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = .5*log(Y./Z + eps) + .5*(Y - Z)./Z ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 6 % EG-RJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (Y - Z)./(Y + Z) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 7 % EG-DRJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = 2*log(.5*(Y + Z)./Z) + (Z - Y)./(Y + Z);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

         case 8 % EG-SJS

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = log(.5*(Y + Z)./Z + eps) ;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 9 % EG-Tri

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = (2*Y./(Y + Z)).^2 - 1;
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 10 % EG-BE

            Ap = A;
            gamma_T = (sum(X,2))';
            Z = A*X + eps;
            Q = alpha_BE*log((Y + alpha_BE*Z)./((1 + alpha_BE)*Z) + eps);
            A = A.*exp(Q*X'*diag(omegaA./gamma_T));
            A = A*diag(1./(sum(A,1) + eps));

          case 11 % Pinv

            Ap = A;
            A = max(1E6*eps,Y*pinv(X')');
            A = A*diag(1./sum(A,1));

          case 12 % Fixed A

            niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
            A = A_true + eps;
            A = A*diag(1./(sum(A,1) + eps));

        end % switch for A

   end % for t in A

end % if no_iter

                if (nr == max_restart)&(mod(k,50)==0)& (~mc_on | restart_mc_on)
                    norm_A = norm(abs(A - Ap),'fro');
                    fprintf(1, 'Restart %d,  %d-th alternating step\n',nr_best+1,k);
                end

                if sum(Index_norm)
                   if (nr == max_restart) & (((k < 50) & (mod(k,5)==0)) | ((k>49) & ((mod(k,50)==0))))

                       s_dist = s_dist + 1;
                       k_select(s_dist) = k;
                       Z = A*X + eps;
                       Z = diag(1./sqrt(var(Z')))*Z;

                        dist_Fro(s_dist) = norm(Y - Z,'fro');
                        dist_KL(s_dist) = sum(sum(Y.*log(Y./Z + eps) - Y + Z));
                        dist_KL2(s_dist) = sum(sum(Z.*log(Z./Y + eps) + Y - Z));
                        dist_Pearson(s_dist) = sum(sum( ((Y - Z).^2)./Z ));
                        dist_Hellinger(s_dist) = sum(sum( (sqrt(Z) - sqrt(Y)).^2 ));
                        dist_JS_rel(s_dist) = sum(sum(2*Y.*log(2*Y./(Y + Z) + eps) + Z - Y));
                        dist_JS_rel2(s_dist) = sum(sum(2*Z.*log(2*Z./(Y + Z) + eps) - Z + Y));
                        Zy = Y + Z;
                        dist_JS(s_dist) = sum(sum(Y.*log(2*Y./Zy + eps) + Z.*log(2*Z./Zy + eps) ));
                        dist_AG_rel(s_dist) = sum(sum(Zy.*log(.5*Zy./Y + eps) + Y - Z));
                        dist_AG(s_dist) = sum(sum(.5*Zy.*log(.5*Zy./sqrt(Y.*Z) + eps)));
                        dist_J(s_dist) = sum(sum( .5*(Y - Z).*log(Y./Z + eps) ));
                        dist_Chi(s_dist) = sum(sum( ((Y + Z).*(Y - Z).^2)./(Y.*Z) ));
                        dist_Tria(s_dist) = sum(sum( ((Y - Z).^2)./(Y + Z) ));
                    end % if multiple
                end % if sum

end % while (k)

% Outer KL divergence
Z = AL*A*X;
Z_outer = norm(Z,'fro') + eps;
KL_outer = sum(sum(Y_true.*log((Y_true + eps)./(Z + eps)) - Y_true + Z))/Z_outer;

           if (nr == 0) | (KL_outer < KL_outer_temp)
              A_best = A; X_best = X; KL_outer_temp = KL_outer; nr_best = nr;
           end % multi-conditions

   nr = nr + 1;

   if nr <=max_restart
      fprintf(1, 'Restart %d, Kullback-Leibler divergence = %e\n',	nr, KL_outer);
   end

end % while (restarts)

X(X <= 0) = eps;

Distance_output = cell(length(s_dist),1);
Distance_output(1) = {[]};
Distance_output(2) = {[]};
Distance_output(3) = {[]};
Distance_output(4) = {[]};
Distance_output(5) = {[]};
Distance_output(6) = {[]};
Distance_output(7) = {[]};
Distance_output(8) = {[]};
Distance_output(9) = {[]};
Distance_output(10) = {[]};
Distance_output(11) = {[]};
Distance_output(12) = {[]};
Distance_output(13) = {[]};
Distance_output(14) = {[]};

if sum(Index_norm)
   Distance_output(1) = {k_select};
   Distance_output(2) = {dist_Fro};
   Distance_output(3) = {dist_KL};
   Distance_output(4) = {dist_KL2};
   Distance_output(5) = {dist_Pearson};
   Distance_output(6) = {dist_Hellinger};
   Distance_output(7) = {dist_JS_rel};
   Distance_output(8) = {dist_JS_rel2};
   Distance_output(9) = {dist_JS};
   Distance_output(10) = {dist_AG_rel};
   Distance_output(11) = {dist_AG};
   Distance_output(12) = {dist_J};
   Distance_output(13) = {dist_Chi};
   Distance_output(14) = {dist_Tria};
end


function [pgr] = create_grid(pts, follow_meridians, trim_final_grid_to_shape)
    % Interactively define a grid
    %
    % CREATE_GRID will create a ZmapGrid interactively. Upon closing the window or choosing "Set"
    % the global ZmapData.Grid will be updated.
    %
    % CREATE_GRID(PTS) PTS is a polygon. only grid points with PTS will be shown
    %  GR is all data points, as [lon,lat; ...]
    %  ZGR is a ZmapGrid.
    % CREATE_GRID(PTS, FOLLOW_MERIDIANS) if FOLLOW_MERIDIANS is false, then distances are constant.
    % if FOLLOW_MERIDIANS is TRUE, then degrees of longitude are constant.
    %
    % Choose an origin point
    % Choose a point to define initial spacing
    % Use scroll Wheel to change spacing
    % Drag Point to change origin
    %
    % tests:
    %  create_grid('testpoly')
    %  create_grid('testworld');
    %
    % If FOLLOW_MERIDIANS is true, then grid gets smaller as pole is approached.
    % Unfortunately, routines like pcolor this is necessary for pcolor, when Y axis is a lon.
    % This could be avoided if coordinate system was transformed into X-Y.
    %
    % When FOLLOW_MERIDIANS is false, then longitudes drift in order to keep consistent sized boxes.

    % TODO: add edit fields that allow grid to be further modified
    % TODO: add SAVE and LOAD buttons
    % TODO: make this work with gridfun

    FOLLOW_MERIDIANS = exist('follow_meridians','var') && follow_meridians;
    USEPOLY=exist('pts','var') && ~isempty(pts) && ~isnan(pts(1));
    name='grid';
    changed=false;

    ZG=ZmapGlobal.Data;

    f=figure('Name','Grid Selection','Units','pixels','Position',[200 75 730 700]);

    % DISPLAY EVENTS
    ax=subplot(4,4,[1,11]);
    ax.Units='points';
    ax.Position=fix(ax.Position);
    plot(ax,ZG.primeCatalog.Longitude,ZG.primeCatalog.Latitude,'.',...
        'color',[.75 .75 .8],'DisplayName','events');
    ax.XLabel.String='longitude';
    ax.YLabel.String='latitude';
    ax.NextPlot='add';

    if exist('pts','var') && ischar(pts)
        switch pts
            case 'testpoly'
                pts =[... % SAMPLE POLYGON, FOR TESTING
                    7.3500   47.8007;...
                    8.3187   47.5009;...
                    8.8392   46.6736;...
                    8.4633   46.0321;...
                    7.4801   45.7263;...
                    6.5548   46.7095;...
                    7.3500   47.8007...
                    ];
            case 'testworld'
                USEPOLY=false
                clear pts
                axis([0 90 -20 80])
        end
    end

    %DISPLAY SURFACE FEATURES
    copyobj(ZG.features('borders'),ax);

    % DISPLAY POLYGON
    if USEPOLY
        plot(ax, pts(:,1),pts(:,2),'k:','LineWidth',2,'DisplayName','polygon');
    end

    legend(ax,'show')

    % SHOW INSTRUCTIONS
    t = uicontrol('style','text','units','pixels','position',[10 10 300 20],'String','temporary');

    % FEEDBACK
    tp = uicontrol('style','text','Units','pixels','Position',[10 50 300 20],'String','N Points: ???');
    d = uicontrol('style','text','Units','pixels','Position',[10 30 300 20],'String','Dist: ???? (deg) [???? (km)]');

    % ADDITIONAL CONTROLS
    uicontrol('style','pushbutton','Units','pixels','Position',[310 30 50 20],'String','SET','Callback',@set_grid);
    uicontrol('style','edit','Units','pixels','Position',[310 70 50 20],...
        'String',name,'Callback',@(~,~)update_plot());

    uicontrol('style','checkbox','Units','pixels','Position',[310 100 80 20],'String','Follow Meridians? (adjusts for lat)','Callback',@toggle_parallels);
    % SELECT FIXED POINT
    write_string(t,'Enter a fixed point for the grid');
    [fixed_x,fixed_y]=ginput(1);
    fp=plot(ax,fixed_x,fixed_y,'b+','LineWidth',2,'DisplayName','Origin');
    instruction_end(t);

    % SELECT OTHER POINT
    write_string(t,'Click at a distance that will define a grid');
    [x2,y2]=ginput(1);
    %tmph=plot(ax,x2,y2,'bo','LineWidth',2)
    instruction_end(t);

    dx = abs(x2-fixed_x);
    dy = abs(y2-fixed_y);

    % SELECT INITIAL GRID
    gpts_h2=plot(nan,nan,'gx','DisplayName','grid');

    update_plot();

    f.WindowScrollWheelFcn=@adjust_grid;
    f.WindowButtonDownFcn=@mouse_down;
    f.WindowButtonUpFcn=@mouse_up;
    f.DeleteFcn=@check_for_save;

    write_string(t,'Scroll the mouse wheel to scale')
    if nargout > 0
        waitfor(f);
        pgr =ZG.Grid;
    end

    function adjust_grid(~,ev)
        % adjust grid spacing when mouse wheel is scrolled
        scale=1.05;
        %disp(ev)
        if ev.VerticalScrollCount > 0
            dx=dx.*scale;
            dy=dy.*scale;
        elseif ev.VerticalScrollCount < 0
            dx=dx./scale;
            dy=dy./scale;
        end
        update_plot();
    end

    function set_grid(~,~)
        ZG.Grid=ZmapGrid(name,pgr.xs, pgr.ys, 'deg');
        if FOLLOW_MERIDIANS
            ZG.gridopt = GridOptions(deg2km(dx),deg2km(dy),[],'deg', FOLLOW_MERIDIANS);
        else
            ZG.gridopt = GridOptions(dx, dy, [], 'km', FOLLOW_MERIDIANS);
        end
        changed=false;
    end

    function toggle_parallels(src,~)
        FOLLOW_MERIDIANS=src.Value==1;
        update_plot();
    end

    function update_plot()
        changed=true;
        [gx,gy]=get_eq_grid(fixed_x,fixed_y,dx,dy);
        if USEPOLY
            ll=polygon_filter(pts(:,1),pts(:,2),gx(:),gy(:),'inside');
        else
            ll=true(size(gx(:)));
        end
        % plot points inside polygon, but grid still covers entire map.
        gpts_h2.XData=gx(ll);
        gpts_h2.YData=gy(ll);

        pgr.xs=gx;
        pgr.ys=gy;
        disp(pgr)

        % trim pgr to polygon before returning. Maybe I shouldn't!
        if USEPOLY && exist('trim_final_grid_to_shape','var') && trim_final_grid_to_shape
            ll=polygon_filter(pts(:,1),pts(:,2),pgr.xs,pgr.ys,'inside');
            pgr.xs(~ll)=nan;
            pgr.ys(~ll)=nan;
        end

        pgr=trim_nans(pgr);

        tp.String=sprintf('N Points: %d',sum(ll));
    end

    function [lonMat,latMat] = get_eq_grid(lon0,lat0,dLon,dLat)
        % GET_EQ_GRID
        % input is the origin point and arclength between points
        % output is 2 matrices (lon, lat)

        % base grid on a single distance, so that instead of separate dx & dy, we use dd
        dist_arc = max([...
            distance(lat0,lon0,lat0,lon0+dLon,'degrees'),...
            distance(lat0,lon0,lat0+dLat,lon0,'degrees')]);

        d.String=sprintf('Dist: %.3f (deg) [%.3f (km)]',dist_arc,deg2km(dist_arc));

        % use the axes limits (assumed degrees) to control size of grid
        ylims_deg = ylim(ax);
        xlims_deg = xlim(ax);

        % pick out latitude spacing. Our grid will have this many rows.
        lats = vector_including_origin(lat0, dist_arc, ylims_deg);
        lonMat=[];
        latMat=[];

        if FOLLOW_MERIDIANS
            % when following the meridian lines, the longitude span covered by
            % the arc-distance at lat0 (along the rhumb!) remains constant.
            % that is, dLon 45 from origin (0,0) will always be 45, regardless of latitude.
            [~,dLon]=reckon('rh',lat0,0,dist_arc,90);

            % resulting in a rectangular matrix where, on a globe lines will converge, but on a graph
            lonValues = vector_including_origin(lon0, dLon, xlims_deg);

            %creates a meshgrid of size numel(lonValues) x numel(lats)
            [lonMat,latMat]=meshgrid(lonValues,lats);

        else
            % when ignoring meridian lines, and aiming for an approximately constant distance,
            % the dLon at each latitude will differ.

            % number of degrees longitude covered by the arclength at each latitude
            [~,dLon_per_lat]=reckon('rh',lats,0,dist_arc,90);

            for n=1:numel(lats)
                theseLonValues = vector_including_origin(lon0, dLon_per_lat(n), xlims_deg);
                lonMat=[lonMat;theseLonValues(:)]; %#ok<AGROW>
                latMat=[latMat;repmat(lats(n),size(theseLonValues(:)))]; %#ok<AGROW>
            end

            [lonMat,latMat] = cols2matrix(lonMat,latMat);
            % each gridx & gridy are vectors.
        end
    end

    function v = vector_including_origin(orig_deg, delta_deg, lims_deg)
        v = unique([orig_deg : -delta_deg : min(lims_deg) , orig_deg : delta_deg :max(lims_deg)]);
    end

    function [xs, ys] = cols2matrix(lonCol,latCol)
        % COLS2MATRIX convert columns of lats & lons into a matrix.
        % this takes lots of stuff into account
        %

        % assign pgrid
        ugy=unique(latCol); % lats in matrix
        nrows=numel(ugy); % number of latitudes in matrix
        [~,example]=min(abs(latCol(:))); % latitude closest to equator will have most number of lons in matrix
        mostCommonY=latCol(example); % account for the abs possibly flipping signs
        base_lon_idx=find(lonCol(latCol==mostCommonY)==fixed_x); % longitudes that must line up
        ncols=sum(latCol(:)==mostCommonY); % most number of lons in matrix
        ys=repmat(ugy(:),1,ncols);
        xs=nan(nrows,ncols);
        for n=1:nrows
            thislat=ugy(n); % lat for this row
            idx_lons=(latCol==thislat); % mask of lons in this row
            these_lons=lonCol(idx_lons); % lons in this row
            row_length=numel(these_lons); % number of lons in this row

            main_lon_idx=find(these_lons==fixed_x); % offset of X in this row
            offset=base_lon_idx - main_lon_idx;
            xs(n,(1:row_length)+offset)=these_lons;
        end
    end

    function mouse_down(src,~)
        %mouse down, so make origin follow mouse around
        %disp(ev)
        %disp(ax.CurrentPoint)
        src.WindowButtonMotionFcn=@mouse_move;
        %disp('  ');
        src.Pointer='cross';
    end

    function mouse_up(src,~)
        %mouse up, so ignore position
        %disp(ev)
        %disp(ax.CurrentPoint);
        src.WindowButtonMotionFcn='';
        %disp('  ');
        update_plot()
        src.Pointer='arrow';
    end

    function mouse_move(~,~)
        fixed_x=ax.CurrentPoint(1,1);
        fixed_y=ax.CurrentPoint(1,2);
        fp.XData=fixed_x;
        fp.YData=fixed_y;
    end

    function check_for_save(~,~)
        if changed
            dosave=questdlg('Save Changes?','Grid Creation','Yes','No','Yes');
            if strcmpi(dosave,'Yes')
                set_grid([],[]);
            end
        end
    end

end

function write_string(h,str)
    for i=1:length(str)-1
        h.String=[str(1:i),'..'];
        pause(.01);
    end
    h.String=str;
end

function instruction_end(h)
    h.ForegroundColor=[0 .5 0];
    pause(.3);
    h.String='';
    h.ForegroundColor='k';
end

function pgr=trim_nans(pgr)
    % REMOVE GRID POINTS BEYOND POLYGON (rows & cols of all nans)
    nanrows=all(isnan(pgr.xs),2);
    nancols=all(isnan(pgr.xs),1);
    pgr.xs(nanrows,:)=[];pgr.xs(:,nancols)=[];
    pgr.ys(nanrows,:)=[];pgr.ys(:,nancols)=[];
end

%{
function c=mycontext()
    c=uicontextmenu('Tag','GridContext')

    uimenu(c,'Label','Select Rectangle');
    uimenu(c,'Label','Select Circle');
    uimenu(c,'Label','Select Polygon');
    uimenu(c,'Separator','on','Label','Create Polygon');
end
%}


classdef math_model_opf_accs < mp.math_model_opf_acc
%MP.MATH_MODEL_OPF_ACCS  MATPOWER mathematical model for AC optimal power flow (OPF) problem.
%   ?
%
%   MP.MATH_MODEL_OPF_ACCS ... power flow ...
%
%   Properties
%       ? - ?
%
%   Methods
%       ?

%   MATPOWER
%   Copyright (c) 2021-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%     properties
%     end

    methods
        function tag = form_tag(obj)
            tag = 'accs';
        end

        function name = form_name(obj)
            name = 'AC-cartesian-power';
        end

        function add_node_balance_constraints(obj, nm, dm, mpopt)
            %% power balance constraints
            nn = nm.node.N;             %% number of nodes
            fcn_mis = @(x)obj.nodal_power_balance_fcn(x, nm);
            hess_mis = @(x, lam)obj.nodal_power_balance_hess(x, lam, nm);
            obj.add_nln_constraint({'Pmis', 'Qmis'}, [nn;nn], 1, fcn_mis, hess_mis);
        end

        function [lam_p, lam_q] = node_power_balance_prices(obj, nm)
            %% shadow prices on node power balance
            nne = obj.get_idx('nle');
            lambda = obj.soln.lambda;
            lam_p = lambda.eqnonlin(nne.i1.Pmis:nne.iN.Pmis);
            lam_q = lambda.eqnonlin(nne.i1.Qmis:nne.iN.Qmis);
        end
    end     %% methods
end         %% classdef


% RMSEANALYSIS  RMSE analysis for slamtb.
%   RMSEANALYSIS is a script for evaluating average RMSE performance of
%   slamtb. It uses a slave version of SLAMTB, called SLAMTBSLAVE, and runs
%   it for N times with different kinds of landmarks.
%
%   Specify the types of landmarks in variable:  lmkTypes.
%   Specify the number of runs for each lmk in:  numRuns.
%   Specify the length of each run in         :  numFrames.
%   Specify the destination of log files in   :  logsDir.
%   Specify the rendering period in           :  rendPeriod.
%   Specify lmk parametrization with the flag :  reparametrize.
%   Specify N random seeds in                 :  randSeeds.
%
%   The result of this process is a set of log files. The contents of these
%   files can be plotted with NEESPLOTS.
%
%   See also SLAMTBSLAVE, NEESPLOTS, NEES, ROBOTNEES.

errorAnalysisFunction = @robotRmse;
logsDir    = '~/SLAM/logs/pose6d/RMSE/test/';
logFileHeader = 'frame, err: x y z r p y, std: x y z r p y';

% lmkTypes   = {'ahmPnt'};
% lmkTypes   = {'hmgPnt','idpPnt','ahmPnt'};
% lmkTypes   = {'hmgPnt'};
lmkTypes   = {'idpPnt'};
% lmkTypes   = {'idpPnt','ahmPnt'};
% lmkTypes   = {'plkLin','aplLin','hmgLin','ahmLin','idpLin'};
% lmkTypes   = {'aplLin','idpLin','ahmLin'};
% lmkTypes   = {'idpLin','ahmLin'};
% lmkTypes   = {'aplLin'};
% lmkTypes   = {'plkLin','aplLin'};

numRuns    = 25;
numFrames  = 400;

rendPeriod = 40;
reparametrize = false;
pixelNoise = 1;
noiseFactor = 1;

% randSeeds  = round(10000*rand(1,numRuns));

randSeeds = [8687 8440 3999 2599 8001 4314 9106 1818 2638 1455 1361 8693 5797 5499 1450 8530 6221 3510 5132 4018 7600 2399 1233 1839 2400];
% save [logsDir 'randSeeds.log'] randSeeds -ascii

for l = 1:numel(lmkTypes)
    lmkType = lmkTypes{l};
    for nRun = 1:numRuns
        disp(' ')
        disp('==============================')
        fprintf('Lmk type: %s -- Run #: %d\n',lmkType, nRun);
        disp('==============================')
        logFileName = [logsDir lmkType '-rmse-' num2str(nRun,'%02d') '.log'];
%         logFileName = [lmkType '-' num2str(nRun,'%02d') '.log']
        slamtbSlave;
    end
end

rmsePlots;

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


% SCRIPT TO TEST THE DIRECT DYNAMICS OF THE PUMA 560 ROBOT

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Lesser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
fprintf('\nTHE SIMULATION PRESENTS THE ROBOT AT AN INITIAL POSITION WHEN NO TORQUES ARE APPLIED\n')

%load robot parameters
robot=load_robot('ABB', 'IRB140');

total_simulation_time = 0.5 %simulate for .5 second

%initial position and joint speeds
q0 = [0 0 0 0 0 0]';
qd0 = [0 0 0 0 0 0]';

g=[0   0 -9.81]'; %Z0 axis

drawrobot3d(robot, q0);
adjust_view(robot);

%try both
tau = [0 0 0 0 0 0]';%no torques applied%
%tau = [0 200 1 1 1 1]';
%tau = [20 20 21 21 21 21]';

%no friction
robot.motors.friction = 1;

fprintf('\nCOMPUTING FORWARD DYNAMICS (this may take a while)')

%this may take a while, since it requires integration
%of the acceleration at each time step
%[t q qd] = forwarddynamic(robot, total_simulation_time, q0, qd0, tau, [0 0 9.81]);
            %forwarddynamic(robot, time_end, q0, qd0, tau, g, torqfun, varargin)
[t q qd] = forwarddynamic(robot, total_simulation_time, q0, qd0, tau, g, []);

figure, plot(t, q), grid, title('Position vs. time')
xlabel('time (s)'), ylabel('Position (rad)')
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6');

figure, plot(t, qd), grid, title('Speed vs. time')
xlabel('time (s)'), ylabel('Speed (rad/s)')
legend('qd_1', 'qd_2', 'qd_3', 'qd_4', 'qd_5', 'qd_6');

%animate it!!
animate(robot, q)


function [teE, teW] = nfgCompareAllVolumeTrueError(fgG,g_bundleID,g_radius,fibersSel,iBundleSel,phantomDir)
%Compare volume of all fiber matches using trueError program.
%
%   [teE, teW] =
%   nfgCompareAllVolumeTrueError(fgG,g_bundleID,g_radius,fibersSel,iBundleS
%   el,phantomDir)
%
%
%
% NOTES:

% if ieNotDefined('projName'); projName = 'projectome'; end

% XXX THE BELOW TEST MUST BECOME GENERIC
% First lets find the optimum radius for the trueError test using a
% particular bundle
disp('Searching for near optimal test diameter ...');
selID = 4;
sMask = zeros(size(iBundleSel));
gMask = zeros(size(g_bundleID));
for ss=selID
    sMask = sMask | iBundleSel==ss;
    gMask = gMask | g_bundleID==ss;
end
vRadiusTE = 0.01:0.005:0.03;
fgSS = dtiNewFiberGroup();
fgGS = dtiNewFiberGroup();
fgSS.fibers = fibersSel(sMask);
fgGS.fibers = fgG.fibers(gMask);
[imgE] = nfgCompareVolumeTrueError(fgGS, fgSS, g_radius(1), vRadiusTE, phantomDir);
[mIE, iIE] = min(imgE);
te_radius = vRadiusTE(iIE);
% te_radius = 0.014;
disp(['Found optimal diameter = ' num2str(te_radius*2) 'mm.']);

% Calculate arclengths of all bundles found in projectome and in gold
teE = zeros(1,max(g_bundleID));
teW = zeros(1,max(g_bundleID));
fgSS = dtiNewFiberGroup();
fgGS = dtiNewFiberGroup();
for bb=1:max(g_bundleID)
    disp(['Computing volume for bundle ' num2str(bb) ' ...']);
    fgSS.fibers = fibersSel(iBundleSel==bb);
    fgGS.fibers = fgG.fibers(g_bundleID==bb);
    if isempty(fgSS.fibers)
        if ~isempty(fgGS.fibers)
            teE(bb) = 100;
        end
    else
        [teE(bb), teW(bb)] = nfgCompareVolumeTrueError(fgGS, fgSS, g_radius(bb), te_radius, phantomDir);
    end

end

return;


function value = r8_big ( )

%*****************************************************************************80
%
%% R8_BIG returns a "big" real number.
%
%  Discussion:
%
%    The value returned by this function is NOT required to be the
%    maximum representable R8.
%    We simply want a "very large" but non-infinite number.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VALUE, a huge number.
%
  value = 1.0E+30;

  return
end


classdef BOWImgDescriptorExtractor < handle
    %BOWIMGDESCRIPTOREXTRACTOR  Class to compute an image descriptor using the bag of visual words
    %
    % Such a computation consists of the following steps:
    %
    % 1. Compute descriptors for a given image and its keypoints set.
    % 2. Find the nearest visual words from the vocabulary for each keypoint
    %    descriptor.
    % 3. Compute the bag-of-words image descriptor as is a normalized
    %    histogram of vocabulary words encountered in the image. The i-th bin
    %    of the histogram is a frequency of i-th word of the vocabulary in
    %    the given image.
    %
    % ## Example
    %
    %     % create bag of visual words
    %     trainer = cv.BOWKMeansTrainer(K);
    %     dictionary = trainer.cluster(train_descs);
    %
    %     % Compute histogram of visual word occurrences of an image
    %     extractor = cv.BOWImgDescriptorExtractor('SIFT','BruteForce');
    %     extractor.Vocabulary = dictionary;
    %     descs = extractor.compute(im, keypoints);
    %
    % See also: cv.BOWImgDescriptorExtractor.BOWImgDescriptorExtractor,
    %  cv.BOWKMeansTrainer, bagOfFeatures, trainImageCategoryClassifier,
    %  imageCategoryClassifier, indexImages, retrieveImages
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    properties (Dependent)
        % Visual vocabulary
        %
        % Vocabulary (can be trained using cv.BOWKMeansTrainer). Each row of
        % the vocabulary is a visual word (cluster center).
        Vocabulary
    end

    methods
        function this = BOWImgDescriptorExtractor(dextractor, dmatcher)
            %BOWIMGDESCRIPTOREXTRACTOR  The constructor
            %
            %     extractor = cv.BOWImgDescriptorExtractor(dextractor, dmatcher)
            %     extractor = cv.BOWImgDescriptorExtractor({dextractor, 'key',val,...}, {dmatcher, 'key',val,...})
            %
            % ## Input
            % * __dextractor__ Descriptor extractor that is used to compute
            %   descriptors for an input image and its keypoints. It can be
            %   specified by a string containing the type of descriptor
            %   extractor, such as 'SIFT' or 'SURF'. See
            %   cv.DescriptorExtractor.DescriptorExtractor for possible types.
            % * __dmatcher__ Descriptor matcher that is used to find the
            %   nearest word of the trained vocabulary for each keypoint
            %   descriptor of the image. It can be spacified by a string
            %   specifying the type of descriptor extractor, such as
            %   'BruteForce' or 'BruteForce-L1'. See
            %   cv.DescriptorMatcher.DescriptorMatcher for possible types.
            %   default 'BruteForce'
            %
            % In the first variant, it creates descriptor extractor/matcher of
            % the given types using default parameters (by calling the default
            % constructors).
            %
            % In the second variant, it creates descriptor extractor/matcher
            % of the given types using the specified options. Each algorithm
            % type takes optional arguments. Each of the extractor/matcher are
            % specified by a cell-array that starts with the type name
            % followed by option arguments, as in:
            % `{'Type', 'OptionName',optionValue, ...}`. Refer to the
            % individual extractor/matcher functions to see a list of possible
            % options of each algorithm.
            %
            % ## Example
            %
            %     % first variant
            %     extractor = cv.BOWImgDescriptorExtractor('ORB', 'BruteForce');
            %
            %     % second variant
            %     extractor = cv.BOWImgDescriptorExtractor(...
            %         {'FastFeatureDetector', 'Threshold',10}, ...
            %         {'BFMatcher', 'NormType','L2'});
            %
            % See also: cv.DescriptorExtractor, cv.DescriptorMatcher
            %
            if nargin < 2, dmatcher = 'BruteForce'; end
            this.id = BOWImgDescriptorExtractor_(0, 'new', dextractor, dmatcher);
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     extractor.delete()
            %
            % See also: cv.BOWImgDescriptorExtractor
            %
            if isempty(this.id), return; end
            BOWImgDescriptorExtractor_(this.id, 'delete');
        end

        function sz = descriptorSize(this)
            %DESCRIPTORSIZE  Returns image discriptor size
            %
            %     sz = extractor.descriptorSize()
            %
            % ## Output
            % * __sz__ Returns an image discriptor size if the vocabulary is
            %   set. Otherwise, it returns 0.
            %
            % This is basically `size(Vocabulary,1)` (i.e number of clusters).
            %
            % See also: cv.BOWImgDescriptorExtractor.descriptorType
            %
            sz = BOWImgDescriptorExtractor_(this.id, 'descriptorSize');
        end

        function dtype = descriptorType(this)
            %DESCRIPTORTYPE  Returns image descriptor type
            %
            %     dtype = extractor.descriptorType()
            %
            % ## Output
            % * __dtype__ Returns an image descriptor type, one of numeric
            %   MATLAB class names.
            %
            % Always `single` for BOWImgDescriptorExtractor.
            %
            % See also: cv.BOWImgDescriptorExtractor.descriptorSize,
            %  cv.DescriptorExtractor.descriptorType
            %
            dtype = BOWImgDescriptorExtractor_(this.id, 'descriptorType');
        end

        function [bow,idx,kptDescs] = compute(this, img, keypoints)
            %COMPUTE  Computes an image descriptor using the set visual vocabulary
            %
            %     [bow,idx,kptDescs] = extractor.compute(img, keypoints)
            %
            % ## Input
            % * __img__ Image, for which the descriptor is computed.
            % * __keypoints__ Keypoints detected in the input image. It is a
            %   struct array that is returned by cv.FeatureDetector.detect.
            %
            % ## Output
            % * __bow__ Computed output image descriptor. A vector of the same
            %   length as the vocabulary dimension.
            % * __idx__ Indices of keypoints that belong to the cluster. A
            %   cell array of integer vectors. This means that `idx{i}` are
            %   keypoint indices that belong to the i-th cluster (word of
            %   vocabulary).
            % * __kptDescs__ Descriptors of the image keypoints, as returned
            %   by cv.DescriptorExtractor.compute.
            %
            % See also: cv.BOWImgDescriptorExtractor.compute2,
            %  cv.BOWImgDescriptorExtractor.compute1
            %
            if nargout > 2
                [bow,idx,kptDescs] = BOWImgDescriptorExtractor_(this.id, 'compute', img, keypoints);
            elseif nargout > 1
                [bow,idx] = BOWImgDescriptorExtractor_(this.id, 'compute', img, keypoints);
            else
                bow = BOWImgDescriptorExtractor_(this.id, 'compute', img, keypoints);
            end
        end

        function [bow,idx] = compute1(this, kptDescs)
            %COMPUTE1  Computes an image descriptor using keypoint descriptors
            %
            %     [bow,idx] = extractor.compute1(kptDescs)
            %
            % ## Input
            % * __kptDescs__ Computed descriptors to match with vocabulary. It
            %   is a numeric matrix that is returned by
            %   cv.DescriptorExtractor.compute.
            %
            % ## Output
            % * __bow__ Computed output image descriptor. A vector of the same
            %   length as the vocabulary dimension.
            % * __idx__ Indices of keypoints that belong to the cluster. A
            %   cell array of integer vectors. This means that `idx{i}` are
            %   keypoint indices that belong to the i-th cluster (word of
            %   vocabulary).
            %
            % See also: cv.BOWImgDescriptorExtractor.compute
            %
            if nargout > 1
                [bow,idx] = BOWImgDescriptorExtractor_(this.id, 'compute1', kptDescs);
            else
                bow = BOWImgDescriptorExtractor_(this.id, 'compute1', kptDescs);
            end
        end

        function bow = compute2(this, img, keypoints)
            %COMPUTE2  Computes an image descriptor using the set visual vocabulary
            %
            %     bow = extrctor.compute2(img, keypoints)
            %
            % ## Input
            % * __img__ Image, for which the descriptor is computed.
            % * __keypoints__ Keypoints detected in the input image. It is a
            %   struct array that is returned by cv.FeatureDetector.detect.
            %
            % ## Output
            % * __bow__ Computed output image descriptor. A vector of the same
            %   length as the vocabulary dimension.
            %
            % See also: cv.BOWImgDescriptorExtractor.compute
            %
            bow = BOWImgDescriptorExtractor_(this.id, 'compute2', img, keypoints);
        end
    end

    %% Getters/Setters
    methods
        function value = get.Vocabulary(this)
            value = BOWImgDescriptorExtractor_(this.id, 'get', 'Vocabulary');
        end
        function set.Vocabulary(this, value)
            BOWImgDescriptorExtractor_(this.id, 'set', 'Vocabulary', value);
        end
    end

end


function [h,a] = PlotDistribution2(var1,var2,varargin)

%PlotDistribution2 - Plot bivariate data along with respective distributions.
%
%  USAGE
%
%    [h,a] = PlotDistribution2(var1,var2,<options>)
%
%    Using cell arrays will overlay variable pairs.
%
%    var1           variable 1
%    var2           variable 2
%    <options>      optional list of property-value pairs (see table below)
%
%    =========================================================================
%     Properties    Values
%    -------------------------------------------------------------------------
%     'nBins'       number of bins for distributions (default = 100)
%     'xbins'       [m M n], lower and upper bounds, and number of bins,
%                   respectively, for x axis (default [min max 100])
%     'ybins'       [m M n], lower and upper bounds, and number of bins,
%                   respectively, for y axis (default [min max 100])
%     'smooth'      standard deviation of Gaussian kernel (default = 5)
%    =========================================================================
%
%  OUTPUT
%
%    h              handles to the plots and histograms (one line per set of
%                   variables)
%    a              axes for the plots and histograms: [main top right]

% Copyright (C) 2004-2013 by Michaël Zugaro
%
% 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.

colors = [1 0.2 0;0 0.4 1;0 1 0.2;1 0.6 0;0.8 0 1;0.8 1 0];
% Default values
nBins = 100;
smooth = 5;
xbins = [];
ybins = [];

% Check number of parameters
if nargin < 2 | mod(length(varargin),2) ~= 0,
  error('Incorrect number of parameters (type ''help PlotDistribution2'' for details).');
end

if isa(var1,'cell'),
	if ~isa(var2,'cell'),
		error('One parameter is a cell array and the other is not (type ''help PlotDistribution2'' for details).');
	end
	if length(var1) ~= length(var2),
		error('Variable lists have different lengths (type ''help PlotDistribution2'' for details).');
	end
else
	var1 = {var1};
	var2 = {var2};
end

% Check parameter sizes
for i = 1:length(var1),
	if ~isdvector(var1{i}) | ~isdvector(var2{i}) | any(size(var1{i})~=size(var2{i})),
		error('Individual variables must be vectors of identical length (type ''help PlotDistribution2'' for details).');
	end
end

% Parse parameter list
for i = 1:2:length(varargin),
	if ~ischar(varargin{i}),
		error(['Parameter ' num2str(i+2) ' is not a property (type ''help PlotDistribution2'' for details).']);
	end
	switch(lower(varargin{i})),
		case 'nbins',
		nBins = varargin{i+1};
		if ~isiscalar(nBins,'>0'),
			error('Incorrect value for property ''nBins'' (type ''help PlotDistribution2'' for details).');
		end
	case 'xbins',
		xbins = varargin{i+1};
		if ~isdvector(xbins,'#3') | xbins(1) > xbins(2) | ~isiscalar(xbins(3),'>0'),
			error('Incorrect value for property ''xbins'' (type ''help PlotDistribution2'' for details).');
		end
	case 'ybins',
		ybins = varargin{i+1};
		if ~isdvector(ybins,'#3') | ybins(1) > ybins(2) | ~isiscalar(ybins(3),'>0'),
			error('Incorrect value for property ''ybins'' (type ''help PlotDistribution2'' for details).');
		end
	case 'smooth',
		smooth = varargin{i+1};
		if ~isdscalar(smooth,'>=0'),
			error('Incorrect value for property ''smooth'' (type ''help PlotDistribution2'' for details).');
		end
	otherwise,
		error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help PlotDistribution2'' for details).']);
  end
end

% Resize current axes to make room for distribution plots
main = gca;
position = get(main,'position');
x = position(1);
h1 = position(3); % current width
hg = h1/10; % horizontal gap
h2 = h1 - hg; % new total width
y = position(2);
v1 = position(4); % current height
vg = v1/10; % vertical gap
v2 = v1 - vg; % new total height
%  set(main,'position',[x+h2/3+hg y+v2/3+vg h2*2/3 v2*2/3]);
set(main,'position',[x y h2*2/3 v2*2/3]);
right = axes('position',[x+h2*2/3+hg y h2/3 v2*2/3]);
top = axes('position',[x y+v2*2/3+vg h2*2/3 v2/3]);

% Axis limits
if isempty(xbins),
	m1 = min(var1{1});
	M1 = max(var1{1});
	for i = 2:length(var1),
		m1 = min([m1;var1{i}]);
		M1 = max([M1;var1{i}]);
	end
	nBinsX = nBins;
else
	m1 = xbins(1);
	M1 = xbins(2);
	nBinsX = xbins(3);
end
if isempty(ybins),
	m2 = min(var2{1});
	M2 = max(var2{1});
	for i = 2:length(var1),
		m2 = min([m2;var2{i}]);
		M2 = max([M2;var2{i}]);
	end
	nBinsY = nBins;
else
	m2 = ybins(1);
	M2 = ybins(2);
	nBinsY = ybins(3);
end
d1 = M1-m1;
m1 = m1-d1/10;
M1 = M1+d1/10;
d2 = M2-m2;
m2 = m2-d2/10;
M2 = M2+d2/10;

h = [];
a = [main top right];

for i = 1:length(var1),
	% Plot bivariate data
	axes(main);hold on;
	p = plot(var1{i},var2{i},'o');
	k = mod(i,size(colors,1)+1);
	set(p,'MarkerFaceColor',colors(k,:),'MarkerEdgeColor',colors(i,:));
	if i > 1,
		m1 = min([m1 pm1]);
		M1 = max([M1 pM1]);
		m2 = min([m2 pm2]);
		M2 = max([M2 pM2]);
	end
	pm1 = m1;
	pM1 = M1;
	pm2 = m2;
	pM2 = M2;
	xlim([m1 M1]);ylim([m2 M2]);

	% Distribution of var1
	axes(top);hold on;
	[dist1,x1] = hist(var1{i},m1:(M1-m1)/nBinsX:M1);
	dist1 = Smooth(dist1/sum(dist1),smooth);
	bv = bar(x1,dist1,1);
	k = mod(i,size(colors,1)+1);
	set(bv,'FaceColor',colors(k,:),'EdgeColor',colors(i,:));
	set(top,'xtick',[]);
	xlim([m1 M1]);

	% Distribution of var2
	axes(right);hold on;
	[dist2,x2] = hist(var2{i},m2:(M2-m2)/nBinsY:M2);
	dist2 = Smooth(dist2/sum(dist2),smooth);
	bh = barh(x2,dist2,1);
	k = mod(i,size(colors,1)+1);
	set(bh,'FaceColor',colors(k,:),'EdgeColor',colors(i,:));
	set(right,'ytick',[]);
	ylim([m2 M2]);

	% store
	h = [h;p bv bh];
end

% Adjust axis locations
set(main,'xaxislocation','bottom','yaxislocation','left');
set(right,'xaxislocation','bottom','yaxislocation','right');
set(top,'xaxislocation','top','yaxislocation','left');
axes(main);


function nntest6(action,flag);
%NNTEST6 View  and Control Neural network.
%
%
%	See also SIMUFF.

% Koos j. den Oudsten, 1-20-99
% koos@phil.uu.nl
% Copyright (c) 1998-99 by KoosSoft vof
% $Revision: 0.4 $  $Date: 1999/01/27 22:29:28 $

global Xx w1 b1 w2 b2 hidden mlp

if nargin < 1
    action = 'start';
end

%On recursive calls get all necessary handles and data.
if ~strcmp(action,'start')
   childList = allchild(0);
   nn_fig = childList(find(childList == gcf));
   ud = gco;

%   popupvalue = get(ud.popup,'Value');

   switch action
      case {'Sw1', 'Sw2', 'Sw3', 'Sw4', 'Sw5', 'Sw6', 'Sb1', 'Sb2', 'Sb3', 'Sb4'}
         val=get(ud,'value');b=allchild(nn_fig);
	 c=findall(b,'tag',strrep(action,'S',''));set(c,'string',val);
		switch action
			case 'Sw1',w1(1)=val;
			case 'Sw2',w1(2)=val;
			case 'Sw3',w1(3)=val;
			case 'Sb1',b1(1)=val;
			case 'Sb2',b1(2)=val;
			case 'Sb3',b1(3)=val;
			case 'Sw4',w2(1)=val;
			case 'Sw5',w2(2)=val;
			case 'Sw6',w2(3)=val;
			case 'Sb4',b2(1)=val;
		end
	 action='plot';
      case 'train',
         x=0:0.05:5';plot(x,exp((-0.1.*x.^2)).*sin(4.*x));
      case {'w1', 'w2', 'w3', 'w4', 'w5', 'w6', 'b1', 'b2', 'b3', 'b4'}
         val=str2num(get(ud,'String'));
	 if val > 15  val=15; end; if val < -15  val =-15; end;
	 set(ud,'String',val);
	 b=allchild(nn_fig);
	 c=findall(b,'tag',strcat('S',action));set(c,'value',val);
		switch action
			case 'w1',w1(1)=val;
			case 'w2',w1(2)=val;
			case 'w3',w1(3)=val;
			case 'b1',b1(1)=val;
			case 'b2',b1(2)=val;
			case 'b3',b1(3)=val;
			case 'w4',w2(1)=val;
			case 'w5',w2(2)=val;
			case 'w6',w2(3)=val;
			case 'b4',b2(1)=val;
		end
	 action='plot';
      case 'reset',
         close(gcf);action='start';
      case 'set',
	 if mlp == 1,
         w1=[0.72 -0.83 0.83]';b1=[-1.8 4 -0.15]';w2=[-2 -1.7 1.7];b2=[0];
	 else w1=[0.5 2.5 4.6]';b1=[1.65 1.35 1]';w2=[3.1 2.4 1.9];b2=[-1.65];
	 end;
	 action='update';
      case 'grid',
         grid;
      case 'mlprbf',
	 if mlp == 1, mlp=0;
	 set(nn_fig,'Name','Neural Network Viewer (rbf)                  by  Koos j. den Oudsten');
	 else mlp =1;set(nn_fig,'Name','Neural Network Viewer (mlp)                  by  Koos j. den Oudsten');
	 end;
	 action='plot';
      case 'hidden',
         if hidden ==0, hidden=1; else hidden =0; end;
	 action='plot';
      case 'hold',
         hold;
   end
 disp(action);disp(ud);% test

end
% -----------------------------------------------------------------------
if strcmp(action,'update')
	b=allchild(nn_fig);

	set(findall(b,'tag','Sw1'),'value',w1(1));
	set(findall(b,'tag','Sw2'),'value',w1(2));
	set(findall(b,'tag','Sw3'),'value',w1(3));
	set(findall(b,'tag','Sb1'),'value',b1(1));
	set(findall(b,'tag','Sb2'),'value',b1(2));
	set(findall(b,'tag','Sb3'),'value',b1(3));
	set(findall(b,'tag','Sw4'),'value',w2(1));
	set(findall(b,'tag','Sw5'),'value',w2(2));
	set(findall(b,'tag','Sw6'),'value',w2(3));
	set(findall(b,'tag','Sb4'),'value',b2(1));

	set(findall(b,'tag','w1'),'string',w1(1));
	set(findall(b,'tag','w2'),'string',w1(2));
	set(findall(b,'tag','w3'),'string',w1(3));
	set(findall(b,'tag','b1'),'string',b1(1));
	set(findall(b,'tag','b2'),'string',b1(2));
	set(findall(b,'tag','b3'),'string',b1(3));
	set(findall(b,'tag','w4'),'string',w2(1));
	set(findall(b,'tag','w5'),'string',w2(2));
	set(findall(b,'tag','w6'),'string',w2(3));
	set(findall(b,'tag','b4'),'string',b2(1));

action='plot';
end

if strcmp(action,'plot')
	set(nn_fig,'DefaultAxesColorOrder',[1 0 0;0 1 0;0 0 1;1 1 0;1 0 1;0 0 0;0 1 1]);
	if mlp ==1, [hid,out]=simuff(Xx,w1,b1,'tansig',w2,b2,'purelin');
	else [hid,out]=simurb(Xx,w1,b1,w2,b2);
	end
	if hidden ==1,plot(Xx,out,'k',Xx,0,'r',Xx,hid);
		else plot(Xx,out,'k',Xx,0);
	end
	axis([0 5 -2 2]);axis manual;
end

%  Initialize all GUI objects.
if strcmp(action,'start')
% Set positions of graphic objects

Xx=0:0.05:5;w1=[0 0 0]';b1=[0 0 0]';w2=[0 0 0];b2=[0];% init weights en biases
hidden =0;mlp=1;% mlp mode

a = figure('Color',[0.8 0.8 0.8], ...
	'Position',[23 27 756 504], ...
'Name','Neural Network Viewer (mlp)                  by  Koos j. den Oudsten', ...
        'NumberTitle','off','Tag','NN-fig');
b = uimenu('Parent',a, ...
	'Label','Options', ...
	'Tag','NNmenu');
uimenu(b,'label','mlp <-> rbf', 'callback','nntest6(''mlprbf'')');
uimenu(b,'label','grid', 'callback','nntest6(''grid'')');
uimenu(b,'label','test', 'callback','nntest6(''test'')');
uimenu(b,'label','train','callback','nntest6(''train'')');
uimenu(b,'label','reset','callback','nntest6(''reset'')');
uimenu(b,'label','hold', 'callback','nntest6(''hold'')');
b = axes('Parent',a, ...
	'Units','points', ...
	'CameraUpVector',[0 1 0], ...
	'CameraUpVectorMode','manual', ...
	'Color',[1 1 1], ...
	'Position',[40.9655 125.379 410.897 180.621], ...
	'Tag','NetFigure', ...
	'XColor',[0 0 0], ...
	'YColor',[0 0 0], ...
	'ZColor',[0 0 0]);
c = text('Parent',b, ...
	'Color',[0 0 0], ...
	'HandleVisibility','callback', ...
	'HorizontalAlignment','center', ...
	'Position',[0.498489 -0.0965517 0], ...
	'Tag','Axes1Text4', ...
	'VerticalAlignment','cap');
set(get(c,'Parent'),'XLabel',c);
c = text('Parent',b, ...
	'Color',[0 0 0], ...
	'HandleVisibility','callback', ...
	'HorizontalAlignment','center', ...
	'Position',[-0.0528701 0.496552 0], ...
	'Rotation',90, ...
	'Tag','Axes1Text3', ...
	'VerticalAlignment','baseline');
set(get(c,'Parent'),'YLabel',c);
c = text('Parent',b, ...
	'Color',[0 0 0], ...
	'HandleVisibility','callback', ...
	'HorizontalAlignment','right', ...
	'Position',[-0.0996979 1.03448 0], ...
	'Tag','Axes1Text2', ...
	'Visible','off');
set(get(c,'Parent'),'ZLabel',c);
c = text('Parent',b, ...
	'Color',[0 0 0], ...
	'HandleVisibility','callback', ...
	'HorizontalAlignment','center', ...
	'Position',[0.498489 1.02759 0], ...
	'Tag','Axes1Text1', ...
	'VerticalAlignment','bottom');
set(get(c,'Parent'),'Title',c);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sw1'')', ...
	'Position',[22.3448 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sw1','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sw2'')', ...
	'Position',[63.5862 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sw2','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sw3'')', ...
	'Position',[104.828 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sw3','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sb1'')', ...
	'Position',[146.069 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sb1','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sb2'')', ...
	'Position',[187.621 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sb2','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sb3'')', ...
	'Position',[229.483 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sb3','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sw4'')', ...
	'Position',[307.241 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sw4','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sw5'')', ...
	'Position',[348.483 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sw5','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sw6'')', ...
	'Position',[389.724 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sw6','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''Sb4'')', ...
	'Position',[430.966 31.1724 16.1379 63.931], ...
	'Style','slider','SliderStep',[0.0025 0.01], ...
	'Tag','Sb4','Min',-15.0,'Max',15.0, ...
	'Value',0.0);
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''w1'')', ...
	'BackgroundColor',[1 .7 .7], ...
	'Position',[11.7931 17.3793 37.2414 12.4138], ...
	'Style','edit','String','w1', ...
	'Tag','w1');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''w2'')', ...
	'BackgroundColor',[.7 1 .7], ...
	'Position',[53.0345 17.3793 37.2414 12.4138], ...
	'Style','edit','String','w2', ...
	'Tag','w2');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''w3'')', ...
	'BackgroundColor',[.7 .7 1], ...
	'Position',[94.2759 17.3793 37.2414 12.4138], ...
	'Style','edit','String','w3', ...
	'Tag','w3');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''b1'')', ...
	'BackgroundColor',[1 .7 .7], ...
	'Position',[135.517 17.3793 37.2414 12.4138], ...
	'Style','edit','String','b1', ...
	'Tag','b1');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''b2'')', ...
	'BackgroundColor',[.7 1 .7], ...
	'Position',[176.759 16.7586 37.8621 13.0345], ...
	'Style','edit','String','b2', ...
	'Tag','b2');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''b3'')', ...
	'BackgroundColor',[.7 .7 1], ...
	'Position',[218.621 16.7586 37.8621 13.0345], ...
	'Style','edit','String','b3', ...
	'Tag','b3');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''w4'')', ...
	'BackgroundColor',[1 1 1], ...
	'Position',[296.69 17.3793 37.2414 12.4138], ...
	'Style','edit','String','w4', ...
	'Tag','w4');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''w5'')', ...
	'BackgroundColor',[1 1 1], ...
	'Position',[337.931 17.3793 37.2414 12.4138], ...
	'Style','edit','String','w5', ...
	'Tag','w5');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''w6'')', ...
	'BackgroundColor',[1 1 1], ...
	'Position',[379.172 17.3793 37.2414 12.4138], ...
	'Style','edit','String','w6', ...
	'Tag','w6');
b = uicontrol('Parent',a, ...
	'Units','points', ...
	'Callback','nntest6(''b4'')', ...
	'BackgroundColor',[1 1 1], ...
	'Position',[420.414 16.7586 37.2414 13.0345], ...
	'Style','edit','String','b4', ...
	'Tag','b4');
b = uicontrol('Parent',a, ...
	'Units','points','string','hold', ...
	'Callback','nntest6(''hold'')', ...
	'Position',[260 80 34 16], ...
	'Tag','button1');
b = uicontrol('Parent',a, ...
	'Units','points','string','grid', ...
	'Callback','nntest6(''grid'')', ...
	'Position',[260 60 34 16], ...
	'Tag','button2');
b = uicontrol('Parent',a, ...
	'Units','points','string','set', ...
	'Callback','nntest6(''set'')', ...
	'Position',[260 40 34 16], ...
	'Tag','button3');
b = uicontrol('Parent',a, ...
	'Units','points','string','hidden', ...
	'Callback','nntest6(''hidden'')', ...
	'Position',[260 20 34 16], ...
	'Tag','button4');
end