% clear all;
addpath('../msckf/utils');
load('../datasets/dataset3.mat')

rng(42);

%Set up appropriate structs
calibParams.c_u = cu;
calibParams.c_v = cv;
calibParams.f_u = fu;
calibParams.f_v = fv;
calibParams.b = b;

%Noise parameters
y_var = [1,1,1,1];
mu = zeros(4,1);
sigma = diag(y_var);

%Generate new landmarks
newLmNum = 500;

if exist('newLmPos', 'var')
    oldLmNum = size(newLmPos,2);
    oldLmPos = newLmPos;
else
    oldLmNum = 0;
    oldLmPos = [];
end

% xRange = linspace(min(rho_i_pj_i(1,:)), max(rho_i_pj_i(1,:)), newLmNum);
% yRange = linspace(min(rho_i_pj_i(2,:)), max(rho_i_pj_i(2,:)), newLmNum);
% zRange = linspace(min(rho_i_pj_i(3,:)), max(rho_i_pj_i(3,:)), newLmNum);
% [newLmPosX, newLmPosY, newLmPosZ ] = meshgrid(xRange, yRange, zRange);

xRange = [min(rho_i_pj_i(1,:)) - 5, max(rho_i_pj_i(1,:)) + 5];
yRange = [min(rho_i_pj_i(2,:)) - 5, max(rho_i_pj_i(2,:)) + 5];
zRange = [min(rho_i_pj_i(3,:)) - 5, max(rho_i_pj_i(3,:)) + 5];

newLmPosX = range(xRange)*rand(1,newLmNum - oldLmNum) + xRange(1);
newLmPosY = range(yRange)*rand(1,newLmNum - oldLmNum) + yRange(1);
newLmPosZ = range(zRange)*rand(1,newLmNum - oldLmNum) + zRange(1);

newLmPos = [newLmPosX(:)'; newLmPosY(:)'; newLmPosZ(:)'];
newLmPos = [oldLmPos, newLmPos];

rho_i_pj_i = newLmPos;
T_cv = [C_c_v -C_c_v*rho_v_c_v; 0 0 0 1];
y_k_j = [];

for k = 1:length(t)
    C_vi = axisAngleToRotMat(theta_vk_i(:,k));
    T_vi = [C_vi -C_vi*r_i_vk_i(:,k); 0 0 0 1];
    T_ci = T_cv*T_vi;

    %Add observations
    for lm_i = 1:size(newLmPos, 2)
        p_li_i = newLmPos(:,lm_i);
        p_lc_c = homo2cart(T_ci*cart2homo(p_li_i));
        [yMeas] = stereoCamProject(p_lc_c, calibParams);
        if all(yMeas > 0) ...
            && all(yMeas([1,3]) <= 640) && all(yMeas([2,4]) <= 480)...
            && p_lc_c(3) > 0    % in view and in front of camera
                noise = mvnrnd(mu, sigma)';
                y_k_j(:,k, lm_i) = yMeas + noise;
        else
                y_k_j(:,k, lm_i) = -1*ones(4,1);
        end
    end
end

save(['../datasets/dataset3_fresh2_',num2str(newLmNum),'lessnoisy.mat']);


function [Population,FrontNo,CrowdDis] = EnvironmentalSelection(Population,N,Points,W,delta)
% The environmental selection of r-NSGA-II

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    %% Non-r-dominated sorting
    [FrontNo,MaxFNo] = NrDSort(Population.objs,N,Points,W,delta);
    Next = FrontNo < MaxFNo;

    %% Calculate the crowding distance of each solution
    CrowdDis = CrowdingDistance(Population.objs,FrontNo);

    %% Select the solutions in the last front by their crowding distances
    Last     = find(FrontNo==MaxFNo);
    [~,Rank] = sort(CrowdDis(Last),'descend');
    Next(Last(Rank(1:N-sum(Next)))) = true;

    %% Population for next generation
    Population = Population(Next);
    FrontNo    = FrontNo(Next);
    CrowdDis   = CrowdDis(Next);
end


% DPRIME  --  Signal-detection theory sensitivity measure.
%
%  d = dprime(pHit,pFA)
%  [d,beta] = dprime(pHit,pFA)
%
%  PHIT and PFA are numerical arrays of the same shape.
%  PHIT is the proportion of "Hits":        P(Yes|Signal)
%  PFA is the proportion of "False Alarms": P(Yes|Noise)
%  All numbers involved must be between 0 and 1.
%  The function calculates the d-prime measure for each <H,FA> pair.
%  The criterion value BETA can also be requested.
%  Requires MATLAB's Statistical Toolbox.
%
%  References:
%  * Green, D. M. & Swets, J. A. (1974). Signal Detection Theory and
%    Psychophysics (2nd Ed.). Huntington, NY: Robert Krieger Publ.Co.
%  * Macmillan, Neil A. & Creelman, C. Douglas (2005). Detection Theory:
%    A User's Guide (2nd Ed.). Lawrence Erlbaum Associates.
%
%  See also NORMINV, NORMPDF.

% Original coding by Alexander Petrov, Ohio State University.
% $Revision: 1.2 $  $Date: 2009-02-09 10:49:29 $
%
% Part of the utils toolbox version 1.1 for MATLAB version 5 and up.
% http://alexpetrov.com/softw/utils/
% Copyright (c) Alexander Petrov 1999-2006, http://alexpetrov.com
% Please read the LICENSE and NO WARRANTY statement:

% GNU Public License for the UTILS Toolbox
%
% ==============================================================================
%
% IN BRIEF
% ========
%
% This document refers to all Matlab scripts and documentation (referred
% to collectively here as "the software") contained in the "utils toolbox"
% by Alexander Petrov 1999-2006, http://alexpetrov.com/softw/utils/
%
% The software is freely available and freely redistributable, according
% to the conditions of the Gnu General Public License (below). You may not
% distribute the software, in whole or in part, in conjunction with
% proprietary code. That means you ONLY have my permission to distribute a
% program that uses my code IF you also make freely available (under the
% terms of the Gnu GPL) the source code for your whole project. You may
% not pass on the software to another party in its current form or any
% altered, embellished or reduced form, without acknowledging the author
% and including a copy of this license.
%
% The software 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 (reproduced below), and the Free Software Foundation
% website (http://www.fsf.org) for more details.
%
% Please notify the author, via the website, of any bugs, notes, comments
% or suggested changes, particularly of any useful changes you may have
% made to your own copy of the software.
%
% Alex Petrov, December 2006
%
% ==============================================================================
%
% GNU GENERAL PUBLIC LICENSE
% ==========================
%
% Version 2, June 1991
% Copyright (C) 1989, 1991 Free Software Foundation, Inc.
% 	59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
% Everyone is permitted to copy and distribute verbatim copies
% of this license document, but changing it is not allowed.
%
%
% Preamble
% ========
%
%   The licenses for most software are designed to take away your
% freedom to share and change it.  By contrast, the GNU General Public
% License is intended to guarantee your freedom to share and change free
% software--to make sure the software is free for all its users.  This
% General Public License applies to most of the Free Software
% Foundation's software and to any other program whose authors commit to
% using it.  (Some other Free Software Foundation software is covered by
% the GNU Library General Public License instead.)  You can apply it to
% your programs, too.
%
%   When we speak of free software, we are referring to freedom, not
% price.  Our General Public Licenses are designed to make sure that you
% have the freedom to distribute copies of free software (and charge for
% this service if you wish), that you receive source code or can get it
% if you want it, that you can change the software or use pieces of it
% in new free programs; and that you know you can do these things.
%
%   To protect your rights, we need to make restrictions that forbid
% anyone to deny you these rights or to ask you to surrender the rights.
% These restrictions translate to certain responsibilities for you if you
% distribute copies of the software, or if you modify it.
%
%   For example, if you distribute copies of such a program, whether
% gratis or for a fee, you must give the recipients all the rights that
% you have.  You must make sure that they, too, receive or can get the
% source code.  And you must show them these terms so they know their
% rights.
%
%   We protect your rights with two steps: (1) copyright the software, and
% (2) offer you this license which gives you legal permission to copy,
% distribute and/or modify the software.
%
%   Also, for each author's protection and ours, we want to make certain
% that everyone understands that there is no warranty for this free
% software.  If the software is modified by someone else and passed on, we
% want its recipients to know that what they have is not the original, so
% that any problems introduced by others will not reflect on the original
% authors' reputations.
%
%   Finally, any free program is threatened constantly by software
% patents.  We wish to avoid the danger that redistributors of a free
% program will individually obtain patent licenses, in effect making the
% program proprietary.  To prevent this, we have made it clear that any
% patent must be licensed for everyone's free use or not licensed at all.
%
%   The precise terms and conditions for copying, distribution and
% modification follow.
%
%
% GNU GENERAL PUBLIC LICENSE
% ==========================
% TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
% ===============================================================
%
%   0. This License applies to any program or other work which contains
% a notice placed by the copyright holder saying it may be distributed
% under the terms of this General Public License.  The "Program", below,
% refers to any such program or work, and a "work based on the Program"
% means either the Program or any derivative work under copyright law:
% that is to say, a work containing the Program or a portion of it,
% either verbatim or with modifications and/or translated into another
% language.  (Hereinafter, translation is included without limitation in
% the term "modification".)  Each licensee is addressed as "you".
%
% Activities other than copying, distribution and modification are not
% covered by this License; they are outside its scope.  The act of
% running the Program is not restricted, and the output from the Program
% is covered only if its contents constitute a work based on the
% Program (independent of having been made by running the Program).
% Whether that is true depends on what the Program does.
%
%   1. You may copy and distribute verbatim copies of the Program's
% source code as you receive it, in any medium, provided that you
% conspicuously and appropriately publish on each copy an appropriate
% copyright notice and disclaimer of warranty; keep intact all the
% notices that refer to this License and to the absence of any warranty;
% and give any other recipients of the Program a copy of this License
% along with the Program.
%
% You may charge a fee for the physical act of transferring a copy, and
% you may at your option offer warranty protection in exchange for a fee.
%
%   2. You may modify your copy or copies of the Program or any portion
% of it, thus forming a work based on the Program, and copy and
% distribute such modifications or work under the terms of Section 1
% above, provided that you also meet all of these conditions:
%
%     a) You must cause the modified files to carry prominent notices
%     stating that you changed the files and the date of any change.
%
%     b) You must cause any work that you distribute or publish, that in
%     whole or in part contains or is derived from the Program or any
%     part thereof, to be licensed as a whole at no charge to all third
%     parties under the terms of this License.
%
%     c) If the modified program normally reads commands interactively
%     when run, you must cause it, when started running for such
%     interactive use in the most ordinary way, to print or display an
%     announcement including an appropriate copyright notice and a
%     notice that there is no warranty (or else, saying that you provide
%     a warranty) and that users may redistribute the program under
%     these conditions, and telling the user how to view a copy of this
%     License.  (Exception: if the Program itself is interactive but
%     does not normally print such an announcement, your work based on
%     the Program is not required to print an announcement.)
%
% These requirements apply to the modified work as a whole.  If
% identifiable sections of that work are not derived from the Program,
% and can be reasonably considered independent and separate works in
% themselves, then this License, and its terms, do not apply to those
% sections when you distribute them as separate works.  But when you
% distribute the same sections as part of a whole which is a work based
% on the Program, the distribution of the whole must be on the terms of
% this License, whose permissions for other licensees extend to the
% entire whole, and thus to each and every part regardless of who wrote it.
%
% Thus, it is not the intent of this section to claim rights or contest
% your rights to work written entirely by you; rather, the intent is to
% exercise the right to control the distribution of derivative or
% collective works based on the Program.
%
% In addition, mere aggregation of another work not based on the Program
% with the Program (or with a work based on the Program) on a volume of
% a storage or distribution medium does not bring the other work under
% the scope of this License.
%
%   3. You may copy and distribute the Program (or a work based on it,
% under Section 2) in object code or executable form under the terms of
% Sections 1 and 2 above provided that you also do one of the following:
%
%     a) Accompany it with the complete corresponding machine-readable
%     source code, which must be distributed under the terms of Sections
%     1 and 2 above on a medium customarily used for software interchange; or,
%
%     b) Accompany it with a written offer, valid for at least three
%     years, to give any third party, for a charge no more than your
%     cost of physically performing source distribution, a complete
%     machine-readable copy of the corresponding source code, to be
%     distributed under the terms of Sections 1 and 2 above on a medium
%     customarily used for software interchange; or,
%
%     c) Accompany it with the information you received as to the offer
%     to distribute corresponding source code.  (This alternative is
%     allowed only for noncommercial distribution and only if you
%     received the program in object code or executable form with such
%     an offer, in accord with Subsection b above.)
%
% The source code for a work means the preferred form of the work for
% making modifications to it.  For an executable work, complete source
% code means all the source code for all modules it contains, plus any
% associated interface definition files, plus the scripts used to
% control compilation and installation of the executable.  However, as a
% special exception, the source code distributed need not include
% anything that is normally distributed (in either source or binary
% form) with the major components (compiler, kernel, and so on) of the
% operating system on which the executable runs, unless that component
% itself accompanies the executable.
%
% If distribution of executable or object code is made by offering
% access to copy from a designated place, then offering equivalent
% access to copy the source code from the same place counts as
% distribution of the source code, even though third parties are not
% compelled to copy the source along with the object code.
%
%   4. You may not copy, modify, sublicense, or distribute the Program
% except as expressly provided under this License.  Any attempt
% otherwise to copy, modify, sublicense or distribute the Program is
% void, and will automatically terminate your rights under this License.
% However, parties who have received copies, or rights, from you under
% this License will not have their licenses terminated so long as such
% parties remain in full compliance.
%
%   5. You are not required to accept this License, since you have not
% signed it.  However, nothing else grants you permission to modify or
% distribute the Program or its derivative works.  These actions are
% prohibited by law if you do not accept this License.  Therefore, by
% modifying or distributing the Program (or any work based on the
% Program), you indicate your acceptance of this License to do so, and
% all its terms and conditions for copying, distributing or modifying
% the Program or works based on it.
%
%   6. Each time you redistribute the Program (or any work based on the
% Program), the recipient automatically receives a license from the
% original licensor to copy, distribute or modify the Program subject to
% these terms and conditions.  You may not impose any further
% restrictions on the recipients' exercise of the rights granted herein.
% You are not responsible for enforcing compliance by third parties to
% this License.
%
%   7. If, as a consequence of a court judgment or allegation of patent
% infringement or for any other reason (not limited to patent issues),
% conditions are imposed on you (whether by court order, agreement or
% otherwise) that contradict the conditions of this License, they do not
% excuse you from the conditions of this License.  If you cannot
% distribute so as to satisfy simultaneously your obligations under this
% License and any other pertinent obligations, then as a consequence you
% may not distribute the Program at all.  For example, if a patent
% license would not permit royalty-free redistribution of the Program by
% all those who receive copies directly or indirectly through you, then
% the only way you could satisfy both it and this License would be to
% refrain entirely from distribution of the Program.
%
% If any portion of this section is held invalid or unenforceable under
% any particular circumstance, the balance of the section is intended to
% apply and the section as a whole is intended to apply in other
% circumstances.
%
% It is not the purpose of this section to induce you to infringe any
% patents or other property right claims or to contest validity of any
% such claims; this section has the sole purpose of protecting the
% integrity of the free software distribution system, which is
% implemented by public license practices.  Many people have made
% generous contributions to the wide range of software distributed
% through that system in reliance on consistent application of that
% system; it is up to the author/donor to decide if he or she is willing
% to distribute software through any other system and a licensee cannot
% impose that choice.
%
% This section is intended to make thoroughly clear what is believed to
% be a consequence of the rest of this License.
%
%   8. If the distribution and/or use of the Program is restricted in
% certain countries either by patents or by copyrighted interfaces, the
% original copyright holder who places the Program under this License
% may add an explicit geographical distribution limitation excluding
% those countries, so that distribution is permitted only in or among
% countries not thus excluded.  In such case, this License incorporates
% the limitation as if written in the body of this License.
%
%   9. The Free Software Foundation may publish revised and/or new versions
% of the General Public License from time to time.  Such new versions will
% be similar in spirit to the present version, but may differ in detail to
% address new problems or concerns.
%
% Each version is given a distinguishing version number.  If the Program
% specifies a version number of this License which applies to it and "any
% later version", you have the option of following the terms and conditions
% either of that version or of any later version published by the Free
% Software Foundation.  If the Program does not specify a version number of
% this License, you may choose any version ever published by the Free Software
% Foundation.
%
%   10. If you wish to incorporate parts of the Program into other free
% programs whose distribution conditions are different, write to the author
% to ask for permission.  For software which is copyrighted by the Free
% Software Foundation, write to the Free Software Foundation; we sometimes
% make exceptions for this.  Our decision will be guided by the two goals
% of preserving the free status of all derivatives of our free software and
% of promoting the sharing and reuse of software generally.
%
% NO WARRANTY:
%
%   11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
% FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
% OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
% PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
% OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
% TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
% PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
% REPAIR OR CORRECTION.
%
%   12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
% WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
% REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
% INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
% OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
% TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
% YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
% PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGES.
%
% END OF TERMS AND CONDITIONS
%
%
% How to Apply These Terms to Your New Programs
% =============================================
%
%   If you develop a new program, and you want it to be of the greatest
% possible use to the public, the best way to achieve this is to make it
% free software which everyone can redistribute and change under these terms.
%
%   To do so, attach the following notices to the program.  It is safest
% to attach them to the start of each source file to most effectively
% convey the exclusion of warranty; and each file should have at least
% the "copyright" line and a pointer to where the full notice is found.
%
%     <one line to give the program's name and a brief idea of what it does.>
%     Copyright (C) <year>  <name of author>
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program; if not, write to the Free Software
%     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
%
%
% Also add information on how to contact you by electronic and paper mail.
%
% If the program is interactive, make it output a short notice like this
% when it starts in an interactive mode:
%
%     Gnomovision version 69, Copyright (C) year name of author
%     Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
%     This is free software, and you are welcome to redistribute it
%     under certain conditions; type `show c' for details.
%
% The hypothetical commands `show w' and `show c' should show the appropriate
% parts of the General Public License.  Of course, the commands you use may
% be called something other than `show w' and `show c'; they could even be
% mouse-clicks or menu items--whatever suits your program.
%
% You should also get your employer (if you work as a programmer) or your
% school, if any, to sign a "copyright disclaimer" for the program, if
% necessary.  Here is a sample; alter the names:
%
%   Yoyodyne, Inc., hereby disclaims all copyright interest in the program
%   `Gnomovision' (which makes passes at compilers) written by James Hacker.
%
%   <signature of Ty Coon>, 1 April 1989
%   Ty Coon, President of Vice
%
% This General Public License does not permit incorporating your program into
% proprietary programs.  If your program is a subroutine library, you may
% consider it more useful to permit linking proprietary applications with the
% library.  If this is what you want to do, use the GNU Library General
% Public License instead of this License.
%
% ==============================================================================

function [d,beta] = dprime(pHit,pFA)

%-- Convert to Z scores, no error checking
zHit = norminv(pHit) ;
zFA  = norminv(pFA) ;

%-- Calculate d-prime
d = zHit - zFA ;

%-- If requested, calculate BETA
if (nargout > 1)
  yHit = normpdf(zHit) ;
  yFA  = normpdf(zFA) ;
  beta = yHit ./ yFA ;
end

%%  Return DPRIME and possibly BETA
%%%%%% End of file DPRIME.M


function kernel_new = dsKernel(kernel, tsub)
%% downsample/upsample the convolution kernel
%% inputs:
%   kernel: struct variable with fields {'kernel_type', 'pars', 'nMax', 'lb', 'ub', 'bound_pars'}
%       kernel_type: string, convolution kernel type. now support {'exp',
%       'exp2', 'vector'}
%       pars: parameters for the selected kernel type
%       nMax: length of the kernel
%       lb:     lower bound for each parameter
%       ub:     upper bound for each parameter
%       bound_pars: logical variable, bound the parameters or not {1, 0}
%% outputs
%   kernel: struct variable

%% Author: Pengcheng Zhou, Carnegie Mellon University, 2016

kernel_new = kernel;
if nargin<2 || isempty(tsub)
    return;
end

%% kernel size
kernel_new.nMax = ceil(kernel.nMax/tsub);

%% kernel type
kernel_type = kernel.type;
if strcmpi(kernel_type, 'exp')
    % single exponential function: ~ exp(-t/tau)
    kernel_new.pars = kernel.pars/tsub;
elseif strcmpi(kernel_type, 'vector')
    % single vector
len_kernel = length(kernel.pars);
    kernel_new.pars = resample(kernel.pars, ceil(len_kernel/tsub), len_kernel);
else
    % differencing of two exponential function:
    % ~  exp(-t/tau_d)-exp(-t/tau_r)
    kernel_new.pars = kernel.pars/tsub;
end

%% lower and upper bounds for parameters
kernel_new.lb = kernel.lb / tsub;
kernel_new.ub = kernel.ub / tsub;


function average_velocity(nlp, bounds)
    % constraints for impact velocities

    domain = nlp.Plant;
    x = domain.States.x;

    % average step velocity
    v_lb = bounds.constrBounds.averageVelocity.lb;
    v_ub = bounds.constrBounds.averageVelocity.ub;
    x0 = x;
    xf = SymVariable('xf',size(x));
    T  = SymVariable('t',[2,1]);
    v_avg = [(xf(1)-x0(1))./(T(2)-T(1))
        (xf(2)-x0(2))./(T(2)-T(1))];
    v_avg_fun = SymFunction(['avgStepVelocity_',domain.Name],v_avg,{T, x0, xf});
    x0_var = nlp.OptVarTable.x(1);
    xf_var = nlp.OptVarTable.x(end);
    t_var  = nlp.OptVarTable.T(1);
    v_avg_cstr = NlpFunction('Name',v_avg_fun.Name,...
        'Dimension',2,'lb',v_lb, 'ub',v_ub,'Type','Nonlinear',...
        'SymFun',v_avg_fun,'DepVariables',[t_var;x0_var;xf_var]);
    addConstraint(nlp, 'avgStepVelocity', 'first', v_avg_cstr);

end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KUKA_KR_16_ARC_HW(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR5 ARC robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KUKA_KR_16_ARC_HW returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KUKA_KR_16_ARC_HW');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 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 Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function q = inversekinematic_kuka_kr_16_arc_HW(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

% %Evaluate the parameters
% theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
L6=abs(d(6));

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
W = T(1:3,3);

% Pm: wrist position
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
q1=atan2(Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);
%the other possible solution is q1 + pi
q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);
%solver for q3 for both cases
q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));
% solve for the last three joints
% for any of the possible combinations (theta1, theta2, theta3)
for i=1:2:size(q,2),
    qtemp = solve_spherical_wrist(robot, q(:,i), T, 1,'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_spherical_wrist(robot, q(:,i), T, -1, 'geometric'); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%The inverse kinematic problem can be solved as in the IRB 140 (for example)

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = atan2(-p1(2), p1(1));
gamma = real(acos((L2^2+r^2-L4^2)/(2*r*L2)));

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta3
q2(1) = pi/2 - beta - gamma; %elbow up
q2(2) = pi/2 - beta + gamma; %elbow down

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

%See geometry
L2=abs(a(2));
L3=abs(d(4));

A2 = abs(a(3));

%See geometry of the robot
%compute L4
L4 = sqrt(A2^2 + L3^2);

%the angle phi is fixed
phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4));

%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2);

beta = real(acos((L2^2 + L4^2 - r^2)/(2*L2*L4)));

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - beta;
q3(2) = pi - phi + beta;


function [ a x e ] = sdr_ialm(A, B, y, beta, gamma, epsilon, maxIter)

%-------------------------------------------------------------------------------------
% Dec 2014
%
% by Jian Lai, jlai1@ntu.edu.sg
% solve the equation (10) Sparse and Dense Hybrid Representation in our PAMI paper
% min ||a||_1 + \gamma ||x||_2^2 + \beta ||e||_1 s.t. y = Aa + Bx + e.

% input
% A: class-specific dictionary, each column is a atom
% B: nonclass-specific dictionary, each column is a atom
% beta: the balacing parameter of e
% gamma: the balancing parameter of x
% epsilon: the tolerance of the convergent checking
% maxIter: max iteration limit

% output
% a: the coefficients of A
% x: the coefficinets of B
% e: the error
%-------------------------------------------------------------------------------------

% check input
if nargin < 5
    error('Too few arguments') ;
end

if nargin < 6
    epsilon = 1e-6;
end

if nargin < 7
    maxIter = 1000;
end

% initialization
a=zeros(size(A,2),1);
x=zeros(size(B,2),1);
e=zeros(size(y));
phi=zeros(size(y));
[m, n]=size(A);
mu=1;
mu_bar = 1e+6;
rho=1.5;
iter = 0;
converged = false;
BTB=B'*B;

while ~converged

    iter = iter + 1;

	% optimize e
    Resi_e = y - A*a - B*x + phi/mu;
    e = max(Resi_e - beta/mu, 0)+min(Resi_e + beta/mu, 0);

    %optimize x
    Resi_x = B'*( phi + mu * (y-A*a-e) );
    x=(2*gamma*eye(n)+mu*BTB)\Resi_x;

    %optmize a
    Resi_a=y - B * x - e + phi/mu;
    a=SolveHomotopy(A, Resi_a, 'lambda', 1/mu, 'tolerance', 1e-5, 'stoppingcriterion', 3);

    %update \phi and \mu
    Z = y - A*a - B*x - e ;
    phi = phi + mu*Z;
    mu = min(mu*rho, mu_bar);

    %check convergence
    if norm(Z) < epsilon
        converged = true;
    end

    if ~converged && iter >= maxIter
        disp('Maximum iterations reached') ;
        converged = 1 ;
    end

end

end


function P_density = getInputPowerDensity(t,t0,tf,x,param,extra)
%	getInputPowerDensity returns the value of the input current density as a function of time.
%
%       P = getInputPowerDensity(t,t0,tf,extra)
%
%       Inputs:
%               - t     : value of the current time step
%               - t0    : initial integration time
%               - tf    : final integration time
%               - extra : extra parameters
%       Outputs:
%               - P_density     : Applied power density [W/m^2]

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

% Define your own linear/nonlinear function of time for the applied power density

% P = (t-t0)/(tf-t0) *(-30) + 0;
% P_density = -90*sin(t+100/100);

% P_density = -10*sin(omega*t+phi);
frequency = 0.1;
P_density = -300*sin((2*pi*frequency)*t+(1/100));
if P_density == 0
    P_density = 1e-2;
end

end


function [feat, x, y, wid, hgt] = extract_sift(img, c)
%
% Copyright Aditya Khosla http://mit.edu/khosla
%
% Please cite this paper if you use this code in your publication:
%   A. Khosla, J. Xiao, A. Torralba, A. Oliva
%   Memorability of Image Regions
%   Advances in Neural Information Processing Systems (NIPS) 2012
%

if(~exist('c', 'var'))
  c = conf();
end

feature = 'sift';
p = c.feature_config.(feature);

if(size(img, 3)==3)
  img = rgb2gray(img);
end

[hgt wid] = size(img);
grid_spacing = p.grid_spacing;
x = cell(length(p.patch_sizes), 1);
y = cell(length(p.patch_sizes), 1);
feat = cell(length(p.patch_sizes), 1);

for i=1:length(p.patch_sizes)
  patch_size = p.patch_sizes(i);
  [x{i}, y{i}, gridX, gridY] = create_grid(hgt, wid, grid_spacing, patch_size);
  feat{i} = sp_find_sift_grid(img, gridX, gridY, patch_size, 0.8);
  feat{i} = sp_normalize_sift(feat{i});
end

x = cell2mat(x);
y = cell2mat(y);
feat = cell2mat(feat);


classdef PSO < ALGORITHM
% <single> <real/integer> <large/none> <constrained/none>
% Particle swarm optimization
% W --- 0.4 --- Inertia weight

%------------------------------- Reference --------------------------------
% R. Eberhart and J. Kennedy, A new optimizer using particle swarm theory,
% Proceedings of the International Symposium on Micro Machine and Human
% Science, 1995, 39-43.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            W = Algorithm.ParameterSet(0.4);

            %% Generate random population
            Population = Problem.Initialization();
            Pbest      = Population;
            [~,best]   = min(FitnessSingle(Pbest));
            Gbest      = Pbest(best);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                Population     = OperatorPSO(Problem,Population,Pbest,Gbest,W);
                replace        = FitnessSingle(Pbest) > FitnessSingle(Population);
                Pbest(replace) = Population(replace);
                [~,best]       = min(FitnessSingle(Pbest));
                Gbest          = Pbest(best);
            end
        end
    end
end


function [A,B,C,D,E] = bay_lssvm(model,level,type, nb, bay)
% Compute the posterior cost for the 3 levels in Bayesian inference
%
% >> cost = bay_lssvm({X,Y,type,gam,sig2}, level, type)
% >> cost = bay_lssvm(model              , level, type)
%
% Description
% Estimate the posterior probabilities of model (hyper-) parameters
% on the different inference levels:
%     - First level: In the first level one optimizes the support values alpha 's and the bias b.
%     - Second level: In the second level one optimizes the regularization parameter gam.
%     - Third level: In the third level one optimizes the kernel
%                    parameter. In the case of the common 'RBF_kernel' the kernel
%                    parameter is the bandwidth sig2.
%
% By taking the negative logarithm of the posterior and neglecting all constants, one
% obtains the corresponding cost. Computation is only feasible for
% one dimensional output regression and binary classification
% problems. Each level has its different in- and output syntax.
%
%
% Full syntax
%
%     1. Outputs on the first level
%
% >> [costL1,Ed,Ew,bay] = bay_lssvm({X,Y,type,gam,sig2,kernel,preprocess}, 1)
% >> [costL1,Ed,Ew,bay] = bay_lssvm(model, 1)
%
%       costL1 : Cost proportional to the posterior
%       Ed(*)  : Cost of the fitting error term
%       Ew(*)  : Cost of the regularization parameter
%       bay(*) : Object oriented representation of the results of the Bayesian inference
%
%     2. Outputs on the second level
%
% >> [costL2,DcostL2, optimal_cost, bay] = bay_lssvm({X,Y,type,gam,sig2,kernel,preprocess}, 2)
% >> [costL2,DcostL2, optimal_cost, bay] = bay_lssvm(model, 2)
%
%       costL2     : Cost proportional to the posterior on the second level
%       DcostL2(*) : Derivative of the cost
%       optimal_cost(*) : Optimality of the regularization parameter (optimal = 0)
%       bay(*)     : Object oriented representation of the results of the Bayesian inference
%
%     3. Outputs on the third level
%
% >> [costL3,bay] = bay_lssvm({X,Y,type,gam,sig2,kernel,preprocess}, 3)
% >> [costL3,bay] = bay_lssvm(model, 3)
%
%       costL3 : Cost proportional to the posterior on the third level
%       bay(*) : Object oriented representation of the results of the Bayesian inference
%
%     4. Inputs using the functional interface
%
% >> bay_lssvm({X,Y,type,gam,sig2,kernel,preprocess}, level)
% >> bay_lssvm({X,Y,type,gam,sig2,kernel,preprocess}, level, type)
% >> bay_lssvm({X,Y,type,gam,sig2,kernel,preprocess}, level, type, nb)
%
%         X            : N x d matrix with the inputs of the training data
%         Y            : N x 1 vector with the outputs of the training data
%         type         : 'function estimation' ('f') or 'classifier' ('c')
%         gam          : Regularization parameter
%         sig2         : Kernel parameter (bandwidth in the case of the 'RBF_kernel')
%         kernel(*)    : Kernel type (by default 'RBF_kernel')
%         preprocess(*) : 'preprocess'(*) or 'original'
%         level        : 1, 2, 3
%         type(*)      : 'svd'(*), 'eig', 'eigs', 'eign'
%         nb(*)        : Number of eigenvalues/eigenvectors used in the eigenvalue decomposition approximation
%
%     5. Inputs using the object oriented interface
%
% >> bay_lssvm(model, level, type, nb)
%
%         model    : Object oriented representation of the LS-SVM model
%         level    : 1, 2, 3
%         type(*)  : 'svd'(*), 'eig', 'eigs', 'eign'
%         nb(*)    : Number of eigenvalues/eigenvectors used in the eigenvalue decomposition approximation
%
%
% See also:
%   bay_lssvmARD, bay_optimize, bay_modoutClass, bay_errorbar

% Copyright (c) 2002,  KULeuven-ESAT-SCD, License & help @ http://www.esat.kuleuven.ac.be/sista/lssvmlab

%
% initiate and ev. preprocess
%
if ~isstruct(model), model = initlssvm(model{:}); end
model = prelssvm(model);
if model.y_dim>1,
  error(['Bayesian framework restricted to 1 dimensional regression' ...
	 ' and binary classification tasks']);
end

%
% train with the matlab routines
%model = adaptlssvm(model,'implementation','MATLAB');

eval('nb;','nb=ceil(sqrt(model.nb_data));');

if ~(level==1 | level==2 | level==3),
  error('level must be 1, 2 or 3.');
end

%
% delegate functions
%
if level==1,

  eval('type;','type=''train'';');
  %[cost, ED, EW, bay, model] = lssvm_bayL1(model, type);
  eval('[A,B,C,D,E] = lssvm_bayL1(model,type,nb,bay);','[A,B,C,D,E] = lssvm_bayL1(model,type,nb);');

elseif level==2,

  % default type
  eval('type;','type=''svd'';');
  %[costL2, DcostL2, optimal, bay, model] = lssvm_bayL2(model, type);

  eval('[A,B,C,D,E] = lssvm_bayL2(model,type,nb,bay);',...
       '[A,B,C,D,E] = lssvm_bayL2(model,type,nb);')

elseif level==3,

  % default type
  eval('type;','type=''svd'';');
  %[cost, bay, model] = lssvm_bayL3(model, bay);
  [A,B,C] = lssvm_bayL3(model,type,nb);

end

%
%
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  FIRST LEVEL                   %
%                                %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [cost, Ed, Ew, bay, model] = lssvm_bayL1(model, type, nb, bay)
%
% [Ed, Ew, cost,model] = lssvm_bayL1(model)
% [bay,model] = lssvm_bayL1(model)
%
% type = 'retrain', 'train', 'svd'
%
%

if ~(strcmpi(type,'train') | strcmpi(type,'retrain') | strcmpi(type,'eig') | strcmpi(type,'eigs')| strcmpi(type,'svd')| strcmpi(type,'eign')),
  error('type should be ''train'', ''retrain'', ''svd'', ''eigs'' or ''eign''.');
end
%type(1)=='t'
%type(1)=='n'

N = model.nb_data;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute Ed, Ew en costL1 based on training solution %
% TvG, Financial Timeseries Prediction using LS-SVM, 27-28 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (type(1)=='t'), % train
  % find solution of ls-svm
  model = trainlssvm(model);
  % prior %
  if model.type(1) == 'f',
    Ew = .5*sum(model.alpha.*  (model.ytrain(1:model.nb_data,:) - model.alpha./model.gam - model.b));
  elseif model.type(1) == 'c',
    Ew = .5*sum(model.alpha.*model.ytrain(1:model.nb_data,:).*  ...
		((1-model.alpha./model.gam)./model.ytrain(1:model.nb_data,:) - model.b));
  end

  % likelihood
  Ed = .5.*sum((model.alpha./model.gam).^2);

  % posterior
  cost = Ew+model.gam*Ed;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compute Ed, Ew en costL1 based on SVD or nystrom %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
else

  if nargin<4,
    [bay.eigvals, bay.scores, ~, omega_r] = kpca(model.xtrain(model.selector,1:model.x_dim), ...
                                                  model.kernel_type, model.kernel_pars, [],type,nb,'original');

    bay.eigvals = bay.eigvals.*(N-1);
    bay.tol = 1000*eps;
    bay.Peff = find(bay.eigvals>bay.tol);
    bay.Neff = length(bay.Peff);
    bay.eigvals = bay.eigvals(bay.Peff);
    bay.scores = bay.scores(:,bay.Peff);
    %Zc = eye(N)-ones(model.nb_data)/model.nb_data;

    %disp('rescaling the scores');
    for i=1:bay.Neff,
      bay.Rscores(:,i) = bay.scores(:,i)./sqrt(bay.scores(:,i)'*bay.eigvals(i)*bay.scores(:,i));
    end
  end
  Y = model.ytrain(model.selector,1:model.y_dim);

  %%% Ew %%%%
  % (TvG: 4.75 - 5.73))
  YTM = (Y'-mean(Y))*bay.scores;
  Ew = .5*(YTM*diag(bay.eigvals)*diag((bay.eigvals+1./model.gam).^-2))*YTM';

  %%% cost %%%
  YTM = (Y'-mean(Y));
  %if model.type(1) == 'c', % 'classification'  (TvG: 5.74)
  %  cost = .5*YTM*[diag(bay.eigvals); zeros(model.nb_data-bay.Neff,bay.Neff)]*diag((bay.eigvals+1./model.gam).^-1)*bay.scores'*YTM';
  %elseif model.type(1) == 'f', % 'function estimation' % (TvG: 4.76)
			       % + correctie of zero eignwaardes
    cost = .5*(YTM*model.gam*YTM')-.5*YTM*bay.scores*diag((1+1./(model.gam.*bay.eigvals)).^-1*model.gam)*bay.scores'*YTM';
  %end

  %%% Ed %%%
  Ed = (cost-Ew)/model.gam;

end

bay.costL1 = cost;
bay.Ew = Ew;
bay.Ed = Ed;
bay.mu = (N-1)/(2*bay.costL1);
bay.zeta = model.gam*bay.mu;

% SECOND LEVEL
%
%
function [costL2, DcostL2, optimal, bay, model] = lssvm_bayL2(model,type,nb,bay)
%
%
%

if ~(strcmpi(type,'eig') | strcmpi(type,'eigs')| strcmpi(type,'svd')| strcmpi(type,'eign')),
  error('The used type needs to be ''svd'', ''eigs''  or ''eign''.')
end

  N = model.nb_data;
  % bayesian interference level 1

  eval('[cost, Ed, Ew, bay, model] = bay_lssvm(model,1,type,nb,bay); ',...
       '[cost, Ed, Ew, bay, model] = bay_lssvm(model,1,type,nb);');

  all_eigvals = zeros(N,1); all_eigvals(bay.Peff) = bay.eigvals;

  % Number of effective parameters
  bay.Geff = 1 + sum(model.gam.*all_eigvals ./(1+model.gam.*all_eigvals));
  bay.mu = .5*(bay.Geff-1)/(bay.Ew);
  bay.zeta = .5*(N-bay.Geff)/bay.Ed;
  % ideally: bay.zeta = model.gam*bay.mu;

  % log posterior (TvG: 4.73 - 5.71)
  costL2 = sum(log(all_eigvals+1./model.gam)) + (N-1).*log(bay.Ew+model.gam*bay.Ed);

  % gradient (TvG: 4.74 - 5.72)
  DcostL2 = -sum(1./(all_eigvals.*(model.gam.^2)+model.gam)) ...
	    + (N-1)*(bay.Ed/(bay.Ew+model.gam*bay.Ed));

  % endcondition fullfilled if optimal == 0;
  optimal = model.gam  - (N-bay.Geff)/(bay.Geff-1) * bay.Ew/bay.Ed;

  % update structure bay
  bay.optimal = optimal;
  bay.costL2 = costL2;
  bay.DcostL2 = DcostL2;

% THIRD LEVEL
%
%
function [costL3, bay, model] = lssvm_bayL3(model,type,nb)
%
% costL3 = lssvm_bayL3(model, type)
%

if ~(strcmpi(type,'svd') | strcmpi(type,'eigs') | strcmpi(type,'eign')),
  error('The used type needs to be ''svd'', ''eigs'' or ''eign''.')
end

% lower inference levels;
[model,~, bay] = bay_optimize(model,2,type,nb);

% test Neff << N
N = model.nb_data;
if sqrt(N)>bay.Neff,
  %model.kernel_pars
  %model.gam
  warning on;
  warning(['Number of degrees of freedom not tiny with respect to' ...
	   ' the number of datapoints. The approximation is not very good.']);
  warning off
end

% construct all eigenvalues
all_eigvals = zeros(N,1);
all_eigvals(bay.Peff) = bay.eigvals;

% L3 cost function
%costL3 = sqrt(bay.mu^bay.Neff*bay.zeta^(N-1)./((bay.Geff-1)*(N-bay.Geff)*prod(bay.mu+bay.zeta.*all_eigvals)));
%costL3 = .5*bay.costL2 - log(sqrt(2/(bay.Geff-1))) - log(sqrt(2/(N-bay.Geff)))
costL3 = -(bay.Neff*log(bay.mu) + (N-1)*log(bay.zeta)...
	 - log(bay.Geff-1) -log(N-bay.Geff) - sum(log(bay.mu+bay.zeta.*all_eigvals)));
bay.costL3 = costL3;


function i4vec_histogram_test ( )

%*****************************************************************************80
%
%% I4VEC_HISTOGRAM_TEST tests I4VEC_HISTOGRAM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_HISTOGRAM_TEST\n' );
  fprintf ( 1, '  I4VEC_HISTOGRAM histograms an integer vector.\n' );

  [ a, seed ] = i4vec_uniform_ab ( n, 0, 25, seed );

  histo_num = 20;

  histo_gram = i4vec_histogram ( n, a, histo_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Histogram of data from 0 to %d\n', histo_num );
  fprintf ( 1, '\n' );

  for i = 0 : histo_num
    if ( 0 < histo_gram(i+1) )
      fprintf ( 1, '  %6d  %6d\n', i, histo_gram(i+1) );
    end
  end

  return
end


%MAPEX Train and execute arbitrary untrained mapping
%
%    B = A*(W*MAPEX) = A*(A*W)
%    B = A*MAPEX(W,PAR1,PAR2, ...)
%    B = A*MAPEX(UMAP,PAR1,PAR2, ...)
%
% INPUT
%    A        Dataset or datafile or double
%    W        Untrained mapping
%    UMAP     String with name of untrained mapping
%    PAR1     Parameters of untrained mapping W or UMAP
%
% OUTPUT
%    B               Resulting dataset, datafile or double array
%
% DESCRIPTION
% This routine facilitates the construction of shortcuts by training and
% executing an untrained mapping by the same data. It is typically useful
% for mappings like PROXM and SCALEM that are often trained and exectuted
% with the same data.
%
% EXAMPLE
% mink1 = proxm('m',1)*mapex
% mink1 = mapex(proxm,'m',1)    % the same
% mink1 = mapex('proxm','m',1)  % the same
% Herewith D = A*mink1 computes a dissimilarity matrix based on the
% Minkowsky_1 metric between all objects in A.
%
% SEE ALSO (PRTools Guide)
% DATASETS, MAPPINGS, PROXM, SCALEM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com

function out = mapex(p1,varargin)

  if nargin == 0
    out = prmapping(mfilename,'combiner');
  elseif nargin == 1 && ismapping(p1)
    out = prmapping(mfilename,'fixed',p1);
  elseif nargin >= 1 && ischar(p1)
    out = prmapping(mfilename,'fixed',{p1,varargin{:}});
  elseif nargin > 1 && ismapping(p1)
    out = prmapping(mfilename,'fixed',{getmapping_file(p1),varargin{:}});
  elseif (isdataset(p1) || isa(p1,'double')) && ismapping(varargin{1})
    out = p1*(p1*varargin{1});
  elseif (isdataset(p1) || isa(p1,'double')) && nargin == 2
    out = p1*feval(varargin{1},p1);
  elseif (isdataset(p1) || isa(p1,'double')) && nargin > 1
    out = p1*feval(varargin{1},p1,varargin{2:end});
  else
    error('Illegal input')
  end


function [eparams, errors] = calibrateBdtClassifier(data, eclassifier, lab, ncv)
% [eparams, errors] = calibrateEdgeClassifier(efeatures, adjlist, imsegs,
% eclassifier, ncv)
lab(lab == -1 ) = 0;
nfeat = size(data, 1);
for k = 1:ncv
    if ncv > 1
        testind = [(k-1)*nfeat/ncv+1:k*nfeat/ncv];
        trainind = setdiff([1:nfeat], testind);
    else
        trainind = (1:nfeat);
    end
    edata{k} = data(trainind,:);
    elab{k} = lab(trainind);
    econf{k} = test_boosted_dt_mc(eclassifier, edata{k});
%    econf{k} = 1 ./ (1+exp(-econf{k}));
end

for k = 1:ncv
    disp(['iter: ' num2str(k)])
    if ncv>1
        traink = setdiff([1:ncv], k);
    else
        traink = k;
    end
    eparams{k} = fminunc(@(x) objective(x, cat(1, econf{traink}), cat(1, elab{traink})), [-1 0], optimset('TolFun', 0.001));
end

for k = 1:ncv
    econf{k} = 1 ./ (1+exp(eparams{k}(1)*econf{k}+eparams{k}(2)));
end

elab = cat(1, elab{:});
econf = cat(1, econf{:});

eerror = mean((econf>0.5)~=elab);

econf2 = 1-abs(elab-econf);

ind1 = find(elab==0);
ind2 = find(elab==1);
px = [0.025:0.05:0.975];
f1 = ksdensity(econf(ind1), px, 'support', [0 1]);
f2 = ksdensity(econf(ind2), px, 'support', [0 1]);
fc = ksdensity(econf2, px, 'support', [0 1]);
%fc = fc;

errors.err = eerror;
errors.pneg = f1;
errors.ppos = f2;
errors.conf = fc;
errors.px = px;

medFS = 18;
bigFS = 20;

% figure(1), hold on, plot(px, fc, 'y', 'LineWidth', 2);
% %axis([0 1 0 1])
% xlabel('Confidence in True Label', 'FontSize', medFS)
% ylabel('Frequency', 'FontSize', medFS)
% title('Same Label Confidence', 'FontSize', bigFS)
% set(gca, 'FontSize', medFS)
%
% figure(2), hold on, plot(px, f2 ./ (f1+f2), 'y', 'LineWidth', 2)
% hold on, plot(px, px, '--k')
% axis([0 1 0 1])
% xlabel('Estimated Probability', 'FontSize', medFS)
% ylabel('Empirical Probability', 'FontSize', medFS)
% %title('Same Label Confidence', 'FontSize', bigFS)
% set(gca, 'FontSize', medFS)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function err = objective(param, econf, elab)
econf = 1./(1+exp(param(1)*econf+param(2)));
px = [0.025:0.05:0.975];
f1 = ksdensity(econf(elab==0), px, 'support', [0 1+eps])+eps;
f2 = ksdensity(econf(elab==1), px, 'support', [0 1+eps])+eps;
f1 = f1 / sum(f1+f2);
f2 = f2 / sum(f1+f2);
err = sum((f1+f2).*(px - f2./(f1+f2)).^2);
disp(num2str([sum((f1+f2).*abs(px - f2./(f1+f2))) param]))


function [im, gx, gy, gz, midx] = scimat_intersect_plane(scimat, m, v, interp)
% SCIMAT_INTERSECT_PLANE  Intersection of a plane with an image volume.
%
% [IM, GX, GY, GZ, MIDX] = scimat_intersect_plane(SCIMAT, M, V)
%
%   SCIMAT is a struct with the 3D volume that we want to intersect. We use
%   SCIMAT structs widely in Gerardus, because that way we have the image
%   data and metainformation (e.g. voxel size) together in the same
%   variable. For details on SCIMAT structs, see "help scimat".
%
%   IM is an image that displays the intersection of the plane with the
%   image volume. Voxels that fall outside the image volume are returned as
%   NaN.
%
%   GX, GY, GZ are matrices of the same size as IM, and contain the
%   Cartesian coordinates of the voxels in IM (i.e. the coordinates of the
%   points at which the SCIMAT volume was sampled to obtain IM). You can
%   visualize the resulting plane using
%
%     >> surf(gx, gy, gz, im, 'EdgeColor', 'none')
%
%   Note: When INTERP='nn' (see below), coordinates in the plane are
%   rounded to the nearest image voxel centre. Hence, if you plot the plane
%   as above, it will in general look slightly "jagged" instead of flat.
%
%   The plane is uniquely defined in 3D space using a point and a vector:
%
%     * M is a 3-vector with the coordinates of a point contained in the
%       plane. It is assumed that M is within the image boundaries
%
%     * V is a 3-vector that represents a normalized vector orthogonal to
%       the plane
%
%   MIDX is a 2-vector with the row and colum of M in the intersecting
%   plane. That is, the X, Y, Z coordinates of M are
%
%     [gx(midx(1), midx(2)), ...
%      gy(midx(1), midx(2)), ...
%      gz(midx(1), midx(2))]
%
% ... = scimat_intersect_plane(..., INTERP)
%
%   INTERP is a string with the interpolation method when sampling the
%   volume with the plane:
%
%     'nn' (default): nearest neighbour. Good for binary segmentation masks
%     'linear': linear interpolation. Good for grayscale images

% Authors: Ramon Casero <rcasero@gmail.com>,
% Pablo Lamata <pablo.lamata@dpag.ox.ac.uk>
% Copyright © 2010-2015 University of Oxford
% Version: 0.4.4
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

% check arguments
narginchk(3, 4);
nargoutchk(0, 5);

% defaults
if (nargin < 4 || isempty(interp))
    interp = 'nn';
end
% remove dummy dimension of data if necessary
scimat = scimat_squeeze(scimat, true);

% the longest distance within the image volume is the length of the largest
% diagonal. We compute it in voxel units
lmax = ceil(sqrt(sum(([scimat.axis.size] - 1).^2)));

% create horizontal grid for the plane sampling points. We are going to have one
% sampling point per voxel. The grid is centered on 0
[gc, gr] = meshgrid(-lmax:lmax, -lmax:lmax);
gs = 0 * gc;

% compute a rotation matrix to map the Z-axis to v. Note that v is given in
% x, y, z coordinates, but we want to work with r, c, s indices, i.e. y, x,
% z coordinates
v = v([2 1 3]);
rotmat = vec2rotmat(v(:));

% rotate the grid sampling points accordingly
grcs = rotmat * [gr(:) gc(:) gs(:)]';

% compute index coordinates of real world coordinates of the plane point
idxm = scimat_world2index(m, scimat);

% center rotated grid on the plane point m
grcs(1, :) = grcs(1, :) + idxm(1);
grcs(2, :) = grcs(2, :) + idxm(2);
grcs(3, :) = grcs(3, :) + idxm(3);

% sampling points that are outside the image domain
idxout = (grcs(1, :) < 1) | (grcs(1, :) > scimat.axis(1).size) ...
    | (grcs(2, :) < 1) | (grcs(2, :) > scimat.axis(2).size) ...
    | (grcs(3, :) < 1) | (grcs(3, :) > scimat.axis(3).size);

% sampling points that are inside
idxin = ~idxout;
im = nan(size(grcs, 2), 1);
switch interp
    case 'nn' % nearest neighbour
        % round coordinates, so that we are sampling at voxel centers and don't
        % need to interpolate
        grcs = round(grcs);
        % rcs indices => linear indices
        idx = sub2ind(size(scimat.data), grcs(1, idxin), grcs(2, idxin), ...
        grcs(3, idxin));
        % sample image volume with the rotated and translated plane
        im(idxin) = scimat.data(idx);
    case 'linear' % linear interpolation
        xi = grcs(1, idxin);
        yi = grcs(2, idxin);
        zi = grcs(3, idxin);
        im(idxin) = interp3(scimat.data, yi, xi, zi, interp);
    otherwise
        error('Interpolation method not implemented')
end

% compute real world coordinates for the sampling points
gxyz = scimat_index2world(grcs', scimat)';

% reshape the sampled points to get again a grid distribution
im = reshape(im, size(gc));
gx = reshape(gxyz(1, :), size(gr));
gy = reshape(gxyz(2, :), size(gc));
gz = reshape(gxyz(3, :), size(gs));

% keep track of where the rotation point is using a boolean vector for the
% row position, and another one for the column position. The rotation point
% is at the center of the grid
v0row = false(size(gr, 1), 1);
v0row((size(gr, 1) + 1) / 2) = true;
v0col = false(1, size(gc, 2));
v0col((size(gc, 2) + 1) / 2) = true;

% find columns where all elements are NaNs
idxout = all(isnan(im), 1);

% remove those columns
im = im(:, ~idxout);
gx = gx(:, ~idxout);
gy = gy(:, ~idxout);
gz = gz(:, ~idxout);
v0col = v0col(~idxout);

% find rows where all elements are NaNs
idxout = all(isnan(im), 2);

% remove those columns
im = im(~idxout, :);
gx = gx(~idxout, :);
gy = gy(~idxout, :);
gz = gz(~idxout, :);
v0row = v0row(~idxout);

% find row, column coordinates of the rotation point in the intersection
% plane
midx = [find(v0row) find(v0col)];


% This function calculates the ground truth images for a given dataset by
% averaging multiple consecutive frames at the stop-motion time steps.
function groundTruthSequence = createGroundTruthImages(datasetDir, type)

    % Get all files in the dataset directory.
    files = dir([datasetDir, '/*.', type]);
    numFiles = length(files);
    fileIdx = 1;
    groundTruthIdx = 1;
    while fileIdx <= numFiles
        % Read the first frame of the k-th time step.
        filename = files(fileIdx).name;
        frames(:,:,1) = im2double( imread([datasetDir, '/', filename]) );

        % Read all remaining frames of the k-th time step.
        for frameIdx = 1:30
            % Get filename and extract the frame number.
            if fileIdx + frameIdx > numFiles
                % We reached the end of the file list.
                break;
            end
            filename = files(fileIdx + frameIdx).name;
            [~, filenameWithoutEx, ~] = fileparts(filename);
            frameNumber = str2num( filenameWithoutEx(end-1:end) ); %#ok<ST2NM>

            if frameNumber ~= 0
                % New frame for the current time step.
                frames(:,:,frameIdx+1) = im2double( imread([datasetDir, '/', filename]) ); %#ok<AGROW>
            else
                % We reached the last frame of the current time step.
                break;
            end
        end

        % Calculate ground truth image of the k-th time step by the
        % temporal mean (or median) of all corresponding frames.
        groundTruthSequence(:,:,groundTruthIdx) = mean(frames, 3); %#ok<AGROW>

        fileIdx = fileIdx + frameIdx;
        groundTruthIdx = groundTruthIdx + 1;
    end


function [b]=real(a)
%Compute a real part of TT-tensor
%   [B]=REAL(A)
%
% TT-Toolbox 2.2, 2009-2013
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%
% For details see Thm4 in Dolgov, Khoromskij, Savostyanov,
% "Superfast Fourier transform using the QTT format"
% http://dx.doi.org/10.1007/s00041-012-9227-4
%
%---------------------------

if (a.d==1)
    b=a;
    b.core=real(b.core);
    exit;
end
d=a.d;

%Determine the size and preallocate the result
b=tt_tensor;
b.r=a.r; b.r(2:d)=2*a.r(2:d);
b.d=a.d; b.n=a.n;
sz=dot(b.n.*b.r(1:d),b.r(2:d+1));
pos=(b.n.*b.r(1:d)).*b.r(2:d+1);
b.ps=cumsum([1;pos]);
b.core=zeros(sz,1);
apos=a.ps; bpos=b.ps;

% Fill cores
for i=1:d
    bb=zeros(b.r(i),b.n(i),b.r(i+1));
    aa=reshape(a.core(apos(i):apos(i+1)-1), [a.r(i),a.n(i),a.r(i+1)]);
    if (i==1)
        bb(1:a.r(i),:,1:a.r(i+1)           )=real(aa);
        bb(1:a.r(i),:,1+a.r(i+1):2*a.r(i+1))=imag(aa);
    elseif (i==d)
        bb(1:a.r(i),         :,1:a.r(i+1))= real(aa);
        bb(1+a.r(i):2*a.r(i),:,1:a.r(i+1))=-imag(aa);
    else
        bb(1:a.r(i),         :,1:a.r(i+1)           )= real(aa);
        bb(1:a.r(i),         :,1+a.r(i+1):2*a.r(i+1))= imag(aa);
        bb(1+a.r(i):2*a.r(i),:,1:a.r(i+1)           )=-imag(aa);
        bb(1+a.r(i):2*a.r(i),:,1+a.r(i+1):2*a.r(i+1))= real(aa);
    end
    b.core(bpos(i):bpos(i+1)-1)=bb(:);
end
return
end


% DEMCMU35GPLVMFGPLVM1 Learn a GPLVM on CMU 35 data set.

% FGPLVM

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

% Get the sequence numbers.
[Y, lbls] = lvmLoadData('cmu35WalkJog');
seq = cumsum(sum(lbls)) - [1:31];

dataSetName = 'cmu35gplvm';
experimentNo = 1;

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

% Set up model
options = fgplvmOptions('fitc');
options.optimiser = 'conjgrad';
options.back = 'mlp';
options.backOptions = mlpOptions(10);
options.numActive = 200;
options.fixInducing = 1;
options.fixIndices = round(linspace(1, size(Y, 1), options.numActive));
latentDim = 4;

d = size(Y, 2);
model = fgplvmCreate(latentDim, d, Y, options);
model.bias = mean(model.y);
model.scale = std(model.y);

% Add dynamics model.
optionsDyn = gpOptions('fitc');
optionsDyn.numActive = 200;
optionsDyn.fixInducing = 1;
optionsDyn.kern = kernCreate(model.X, {'rbf', 'white'});
optionsDyn.kern.comp{1}.inverseWidth = 0.2;
% This gives signal to noise of 0.1:5e-3 or 20:1.
optionsDyn.kern.comp{1}.variance = 0.01;
optionsDyn.kern.comp{2}.variance = 0.95;
diff = 1;
learn = 1;
optionsDyn.fixIndices = round(linspace(1, size(Y, 1)-length(seq), options.numActive));
model = fgplvmAddDynamics(model, 'gp', optionsDyn, diff, learn, seq);

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

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);


% Fig. 5.14   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
% script to initiate the RLTOOL like in Example 5.7
% click on Compensator Editor tab
% then right click in Dynamics Box
% add pole, set value to -12. That will produce Fig. 5.14 and the locus
%    in Fig. 5.11
% drag the pole from -12 to -9.   That will produce the RL in Fig. 5.13
% drag the pole from -9 to -4.   That will produce the RL in Fig. 5.12
% Play with the pole location more for your amusement
% If you had added the compensator pole and zero through the compensartor editor,
%   you could have dragged either one around to see how it affected the RL

clf
numL=[1 1];
denL=[1 0 0];
sysL=tf(numL,denL);
rltool(sysL)


function varargout = volumization(varargin)
% GUI for visualizing 3D data
%
% To run type:
% >>volumization (data,'n')
% for example:voumization (data, '2')
% where data are of format [n x m x p]  and  'n' - which slice to display:
% '1'- all, '2' - every second, '3' - every third, etc.
%
% For users with PLS_Toolbox 3.5 can type
% >> volumization
% and then load data interactively using lddlgpls routine.
%
% Includes different types of visualization:
% 1. Orthogonal slices - each slice individually, as well as three orthogonal slices on one plot
% 2. Rendered volume through showing series of z-slices. Enter the number of slices to display:
% ('1' - all, '2' - every second, etc.)
% 3. Isosurfaces:
%	- one for a single value
%	- multiple for multiple values of interest on one figure (each with different color)
%
% Options to change for orthogonal slices (1) and volumes (2):
% - transparency;
% - color scheme.
%
% Options to changes for all three types of visualization (1-3):
% - top/bottom, left/right
% - aspect ratio.
%
% To display isosurface on top of rendered volume or orthogonal slices, use controls to
% display desired view and then use Multiple isosurface slider to display the isosurface
% of value of interest. (Single isosurface slider clears the figure on execution).
%
% Use Clear figure button to start over.
%
% created by K.Artyushkova
% November 2004
%
% Kateryna Artyushkova
% Postdoctoral Scientist
% Department of Chemical and Nuclear Engineering
% The University of New Mexico
% (505) 277-0750
% kartyush@unm.edu
%
% Last Modified by GUIDE v2.5 02-Feb-2006 11:38:27

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @volumization_OpeningFcn, ...
                   'gui_OutputFcn',  @volumization_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin & isstr(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT

% --- Executes just before volumization is made visible.
function volumization_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to volumization (see VARARGIN)

% Choose default command line output for volumization
handles.output = hObject;

% Update handles structure
if (nargin <4)
        H.Position=[181 477 332 275];
figure(H)
msgbox('Please load the data through the File Load menu')
    else
    data= varargin{1};
    display=varargin{2};
    data=double(data);
[m,n,p]=size(data);
set(handles.xmin,'string',1);
set(handles.xmax,'string',n);
set(handles.xi,'string',1);
set(handles.ymin,'string',1);
set(handles.ymax,'string',m);
set(handles.yi,'string',1);
set(handles.zmin,'string',1);
set(handles.zmax,'string',p);
set(handles.zi,'string',1);
s=data;
handles.s=s;
H.Position=[181 477 332 275];
figure(H)
step=str2double(display);
hz=slice(s,[],[],[1:step:p]);
alpha('color')
axis tight
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
alphamap('rampdown')
daspect([1 1 0.5])
handles.aspect=[1 1 0.5];
handles.count=0;
colormap(jet)

end
handles.v={'0'};
guidata(hObject, handles);

% UIWAIT makes volumization wait for user response (see UIRESUME)
% uiwait(handles.figure1);

% --- Outputs from this function are returned to the command line.
function varargout = volumization_OutputFcn(hObject, eventdata, handles)
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;

% --------------------------------------------------------------------
function load_data_Callback(hObject, eventdata, handles)
% hObject    handle to load_data (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
data =lddlgpls;
data=double(data);
[m,n,p]=size(data);
set(handles.xmin,'string',1);
set(handles.xmax,'string',n);
set(handles.xi,'string',1);
set(handles.ymin,'string',1);
set(handles.ymax,'string',m);
set(handles.yi,'string',1);
set(handles.zmin,'string',1);
set(handles.zmax,'string',p);
set(handles.zi,'string',1);
s=data;
handles.s=data;
figure(1)
hz=slice(s,[],[],[1:3:p]);
alpha('color')
axis tight
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
alphamap('rampdown')
daspect([1 1 0.5])
handles.aspect=[1 1 0.5];
handles.count=0;
colormap(jet)
guidata(hObject, handles);

% --- Executes on button press in alphaincrease.
function alphaincrease_Callback(hObject, eventdata, handles)
% hObject    handle to alphaincrease (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
figure(1)
alphamap('increase',.1)

% --- Executes on button press in alphadecrease.
function alphadecrease_Callback(hObject, eventdata, handles)
% hObject    handle to alphadecrease (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
figure(1)
alphamap('decrease',.1)

% --- Executes during object creation, after setting all properties.
function colormap_CreateFcn(hObject, eventdata, handles)
% hObject    handle to colormap (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: popupmenu controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes on selection change in colormap.
function colormap_Callback(hObject, eventdata, handles)
% hObject    handle to colormap (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = get(hObject,'String') returns colormap contents as cell array
%        contents{get(hObject,'Value')} returns selected item from colormap

val = get(hObject,'Value');
figure(1)
switch val
    case 1
   colormap(jet)
case 2
   colormap(hsv)
case 3
   colormap(hot)
case 4
   colormap(gray)
case 5
    colormap(bone)
case 6
    colormap(copper)
case 7
    colormap(pink)
case 8
    colormap(white)
case 9
    colormap(colorcube)
case 10
    colormap(vga)
case 11
    colormap(jet)
case 12
    colormap(prism)
case 13
    colormap(cool)
case 14
    colormap(autumn)
case 15
    colormap(spring)
case 16
    colormap(winter)
case 17
    colormap(summer)
end

% --- Executes on button press in aspect_ratio.
function aspect_ratio_Callback(hObject, eventdata, handles)
% hObject    handle to aspect_ratio (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

prompt={'X:','Y:','Z:'};
def={'1','1','0.5'};
dlgTitle='Input for Aspect ratio';
lineNo=1;
answer=inputdlg(prompt,dlgTitle,lineNo,def);
M=str2double(answer);
figure(1)
daspect([M(1) M(2) M(3)])
handles.aspect=[M(1) M(2) M(3)];
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function reverse_axes_CreateFcn(hObject, eventdata, handles)
% hObject    handle to reverse_axes (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: listbox controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes on selection change in reverse_axes.
function reverse_axes_Callback(hObject, eventdata, handles)
% hObject    handle to reverse_axes (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: contents = get(hObject,'String') returns reverse_axes contents as cell array
%        contents{get(hObject,'Value')} returns selected item from reverse_axes

val = get(hObject,'Value');
switch val
case 1
    figure(1)
    set(gca,'Xdir','reverse')
case 2
    figure(1)
    set(gca,'Ydir','reverse')
case 3
    figure(1)
    set(gca,'Zdir','reverse')
end

% --- Executes during object creation, after setting all properties.
function display_CreateFcn(hObject, eventdata, handles)
% hObject    handle to display (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

function display_Callback(hObject, eventdata, handles)
% hObject    handle to display (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of display as text
%        returns contents of display as a double

step=str2double(get(hObject,'String'));
s=handles.s;
figure(1)
[m,n,p]=size(s);
hz=slice(s,[],[],[1:step:p]);
alpha('color')
axis tight
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
alphamap('rampdown')
aspect=handles.aspect;
daspect(aspect)
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function sliderx_CreateFcn(hObject, eventdata, handles)
% hObject    handle to sliderx (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change
%       'usewhitebg' to 0 to use default.  See ISPC and COMPUTER.
usewhitebg = 1;
if usewhitebg
    set(hObject,'BackgroundColor',[.9 .9 .9]);
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes on slider movement.
function sliderx_Callback(hObject, eventdata, handles)
% hObject    handle to sliderx (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

s=handles.s;
[m,n,p]=size(s);
step=1/n;
slider_step(1)=step;
slider_step(2)=step;
set(handles.sliderx, 'SliderStep', slider_step, 'Max', n, 'Min',0)
ix=get(hObject,'Value');
ix=round(ix);
set(handles.xi,'string',ix);
set(handles.ix,'string',ix);
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hx=slice(x,y,z, s,ix,[],[]);
alpha('color')
set(hx,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
handles.IX=ix;
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function slidery_CreateFcn(hObject, eventdata, handles)
% hObject    handle to slidery (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change
%       'usewhitebg' to 0 to use default.  See ISPC and COMPUTER.
usewhitebg = 1;
if usewhitebg
    set(hObject,'BackgroundColor',[.9 .9 .9]);
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes on slider movement.
function slidery_Callback(hObject, eventdata, handles)
% hObject    handle to slidery (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
s=handles.s;
[m,n,p]=size(s);
step=1/m;
slider_step(1)=step;
slider_step(2)=step;
set(handles.slidery, 'SliderStep', slider_step, 'Max', m, 'Min',0)
iy=get(hObject,'Value');
iy=round(iy);
set(handles.yi,'string',iy);
set(handles.iy,'string',iy);
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hy=slice(x,y,z, s,[],iy,[]);
alpha('color')
set(hy,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
handles.IY=iy;
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function sliderz_CreateFcn(hObject, eventdata, handles)
% hObject    handle to sliderz (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change
%       'usewhitebg' to 0 to use default.  See ISPC and COMPUTER.
usewhitebg = 1;
if usewhitebg
    set(hObject,'BackgroundColor',[.9 .9 .9]);
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes on slider movement.
function sliderz_Callback(hObject, eventdata, handles)
% hObject    handle to sliderz (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider

s=handles.s;
[m,n,p]=size(s);
step=1/p;
slider_step(1)=step;
slider_step(2)=step;
set(handles.sliderz, 'SliderStep', slider_step, 'Max', p, 'Min',0)
iz=get(hObject,'Value');
iz=round(iz);
set(handles.zi,'string',iz);
set(handles.iz,'string',iz);
figure(1)
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hz=slice(x,y,z, s,[],[],iz);
alpha('color')
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
handles.IZ=iz;
guidata(hObject, handles);

% --- Executes on button press in all_three.
function all_three_Callback(hObject, eventdata, handles)
% hObject    handle to all_three (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

s=handles.s;
ix=handles.IX;
iy=handles.IY;
iz=handles.IZ;
[m,n,p]=size(s);
figure(1)
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hz=slice(x,y,z, s,ix,iy,iz);
alpha('color')
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
aspect=handles.aspect;
daspect(aspect)
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function ix_CreateFcn(hObject, eventdata, handles)
% hObject    handle to ix (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

function ix_Callback(hObject, eventdata, handles)
% hObject    handle to ix (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of ix as text
%        str2double(get(hObject,'String')) returns contents of ix as a double
ix=str2double(get(hObject,'String')) ;
handles.IX=ix;
s=handles.s;
iy=handles.IY;
iz=handles.IZ;
[m,n,p]=size(s);
figure(1)
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hz=slice(x,y,z, s,ix,iy,iz);
alpha('color')
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
aspect=handles.aspect;
daspect(aspect)
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function iy_CreateFcn(hObject, eventdata, handles)
% hObject    handle to iy (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

function iy_Callback(hObject, eventdata, handles)
% hObject    handle to iy (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of iy as text
%        str2double(get(hObject,'String')) returns contents of iy as a double

iy=str2double(get(hObject,'String')) ;
handles.IY=iy;
s=handles.s;
ix=handles.IX;
iz=handles.IZ;
[m,n,p]=size(s);
figure(1)
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hz=slice(x,y,z, s,ix,iy,iz);
alpha('color')
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
aspect=handles.aspect;
daspect(aspect)
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function iz_CreateFcn(hObject, eventdata, handles)
% hObject    handle to iz (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc
    set(hObject,'BackgroundColor','white');
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

function iz_Callback(hObject, eventdata, handles)
% hObject    handle to iz (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of iz as text
%        str2double(get(hObject,'String')) returns contents of iz as a double

iz=str2double(get(hObject,'String')) ;
handles.IZ=iz;
s=handles.s;
ix=handles.IX;
iy=handles.IY;
[m,n,p]=size(s);
figure(1)
[x,y,z] = meshgrid([1:n],[1:m],[1:p]);
figure(1)
hz=slice(x,y,z, s,ix,iy,iz);
alpha('color')
set(hz,'EdgeColor','none','FaceColor','interp', 'FaceAlpha','interp')
axis tight
alphamap('rampdown')
aspect=handles.aspect;
daspect(aspect)
guidata(hObject, handles);

% --------------------------------------------------------------------
function smooth_Callback(hObject, eventdata, handles)
% hObject    handle to smooth (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

figure(1)
clf
s=handles.s;
size=inputdlg('Enter the size of smoothing kernel','Smoothing filter');
size=str2double(size);
s_sm = smooth3(s, 'gaussian',[size size size]);
handles.s=s_sm;
guidata(hObject, handles);

% --- Executes on slider movement.
function isosurface_Callback(hObject, eventdata, handles)
% hObject    handle to isosurface (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
s=handles.s;
aspect=handles.aspect;
Min=round(min(min(min(s))));
Max=round(max(max(max(s))));
v=[Min:1:Max];
K=Max-Min;
[m,n]=size(v);
step=1/n;
slider_step(1)=step;
slider_step(2)=step;
set(handles.isosurface, 'SliderStep', slider_step, 'Max', K, 'Min',0)
i=get(hObject,'Value');
i=round(i);
if i<=Min
    i=Min;
elseif i>=Max
    i=Max
else
end
i=i+Min;
handles.i=i;
set(handles.iso_value,'string',i);
[n,m,p]=size(s);
a=100*ones([n m p]);
s_inv=a-s;
figure(1)
clf
hiso=patch(isosurface(s,i),'FaceColor',[1,0.75,0.65], 'EdgeColor', 'none','FaceAlpha',0.7);
hcap=patch(isocaps(s_inv,(100-i)),'FaceColor',[1,0.75,0.65], 'Edgecolor','none','FaceAlpha',0.7);
view(45,30);
axis tight
daspect(aspect)
lightangle(45,30)
lighting phong
set(hcap,'AmbientStrength', 0.6)
set(hiso,'SpecularColorReflectance',0.3,'SpecularExponent', 50)
handles.count=0;
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function isosurface_CreateFcn(hObject, eventdata, handles)
% hObject    handle to isosurface (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change
%       'usewhitebg' to 0 to use default.  See ISPC and COMPUTER.
usewhitebg = 1;
if usewhitebg
    set(hObject,'BackgroundColor',[.9 .9 .9]);
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes during object creation, after setting all properties.
function isosurf_multiple_CreateFcn(hObject, eventdata, handles)
% hObject    handle to isosurf_multiple (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: slider controls usually have a light gray background, change
%       'usewhitebg' to 0 to use default.  See ISPC and COMPUTER.
usewhitebg = 1;
if usewhitebg
    set(hObject,'BackgroundColor',[.9 .9 .9]);
else
    set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'));
end

% --- Executes on slider movement.
function isosurf_multiple_Callback(hObject, eventdata, handles)
% hObject    handle to isosurf_multiple (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'Value') returns position of slider
%        get(hObject,'Min') and get(hObject,'Max') to determine range of slider
count=handles.count;
s=handles.s;
value=handles.v;
aspect=handles.aspect;
Min=round(min(min(min(s))));
Max=round(max(max(max(s))));
v=[Min:1:Max];
K=Max-Min;
[m,n]=size(v);
step=1/n;
slider_step(1)=step;
slider_step(2)=step;
set(handles.isosurf_multiple, 'SliderStep', slider_step, 'Max', K, 'Min',0)
i=get(hObject,'Value');
i=round(i);
if i<=Min
    i=Min;
elseif i>=Max
    i=Max
else
    i=i;
end
i=i+Min;
handles.i=i;
set(handles.multiple_iso_value,'string',i);
[n,m,p]=size(s);
figure(1)
hold on
hiso=patch(isosurface(s,i),'FaceColor',[(0.85-count*0.1),(0.25+count*0.1),(0.35+count*0.1)], 'EdgeColor', 'none','FaceAlpha',0.7);
view(45,30);
axis tight
daspect(aspect)
set(hiso,'SpecularColorReflectance',0.3,'SpecularExponent', 50)
count=count+1;
handles.count=count;
value{count}=num2str(i);
outstring=textwrap(value,3);
set(handles.value,'string',outstring)
handles.v=value;
guidata(hObject, handles);

% --- Executes on button press in Clear.
function Clear_Callback(hObject, eventdata, handles)
% hObject    handle to Clear (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
figure(1)
clf
handles.count=0;
handles.v={'0'};
set(handles.value,'string',{})
guidata(hObject, handles);

% --------------------------------------------------------------------
function about_Callback(hObject, eventdata, handles)
% hObject    handle to about (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
msgbox('This a GUI for visualizing 3D data. Created by K.Artyushkova. kartyush@unm.edu. December 2004','About GUI Volumization')

% --- Executes during object creation, after setting all properties.
function iso_value_multi_CreateFcn(hObject, eventdata, handles)
% hObject    handle to iso_value_multi (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end

count=handles.count;
s=handles.s;
value=handles.v;
aspect=handles.aspect;
Min=round(min(min(min(s))));
Max=round(max(max(max(s))));
v=[Min:1:Max];
K=Max-Min;
[m,n]=size(v);
i=str2double(get(hObject,'String'));
handles.i=i;
[n,m,p]=size(s);
figure(1)
hold on
hiso=patch(isosurface(s,i),'FaceColor',[(0.85-count*0.1),(0.25+count*0.1),(0.35+count*0.1)], 'EdgeColor', 'none','FaceAlpha',0.7);
view(45,30);
axis tight
daspect(aspect)
set(hiso,'SpecularColorReflectance',0.3,'SpecularExponent', 50)
count=count+1;
handles.count=count;
value{count}=num2str(i);
outstring=textwrap(value,3);
set(handles.value,'string',outstring)
handles.v=value;
guidata(hObject, handles);

function multiple_iso_value_Callback(hObject, eventdata, handles)
% hObject    handle to multiple_iso_value (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of multiple_iso_value as text
%        str2double(get(hObject,'String')) returns contents of multiple_iso_value as a double
count=handles.count;
s=handles.s;
value=handles.v;
aspect=handles.aspect;
Min=round(min(min(min(s))));
Max=round(max(max(max(s))));
v=[Min:1:Max];
K=Max-Min;
[m,n]=size(v);
i=str2double(get(hObject,'String'));
handles.i=i;
[n,m,p]=size(s);
figure(1)
hold on
hiso=patch(isosurface(s,i),'FaceColor',[(0.85-count*0.1),(0.25+count*0.1),(0.35+count*0.1)], 'EdgeColor', 'none','FaceAlpha',0.7);
view(45,30);
axis tight
daspect(aspect)
set(hiso,'SpecularColorReflectance',0.3,'SpecularExponent', 50)
count=count+1;
handles.count=count;
value{count}=num2str(i);
outstring=textwrap(value,3);
set(handles.value,'string',outstring)
handles.v=value;
guidata(hObject, handles);

% --- Executes during object creation, after setting all properties.
function multiple_iso_value_CreateFcn(hObject, eventdata, handles)
% hObject    handle to multiple_iso_value (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


 function [Af, Aa, dif] = test_adjoint(A, varargin)
%function [Af, Aa, dif] = test_adjoint(A, varargin)
%|
%| test that the adjoint of a 'matrix object' A really is its transpose
%|
%| in
%|	A		Fatrix or fatrix2 typically
%|
%| option
%|	'tolre'	dbl	tolerance (relative) for equality (default: 0)
%|	'big'	0|1	for big objects, use random data
%|	'tol'	dbl	tolerance for adjoint for 'big' option (mpd)
%|	'nrep'	int	multiple realizations for 'big' option
%|	'complex' 0|1	if 1, test with complex data (default: 0)
%|	'warn'	0|1	if 1, just print warning. 0 (default), fail if >tol
%|	'chat'	0|1	default 0
%|
%| Copyright 2005-8-2, Jeff Fessler, University of Michigan

if nargin < 1, ir_usage, end
if streq(A, 'test'), test_adjoint_test, return, end

arg.big = 0;
arg.tolre = 0;
arg.tol = 1e-5;
arg.nrep = 1;
arg.chat = 0;
arg.warn = false;
arg.complex = 0;
arg = vararg_pair(arg, varargin);

test_adjoint_big(A, arg.nrep, arg.tol, arg.complex, arg.warn, arg.chat);
if arg.big, return, end % stop here if 'big'

[nd, np] = size(A);

try
	% must be very careful here because ob(:,:) calls full(ob) by default
	% which in turns calls fatrix2_subsref_colon() which is "too" clever
	% because it uses the smaller dimension, so transpose has no effect.
	% 2013-07-16 added 'col' option to 'full' to force using the column
	% dimension for the loop (for both ob and ob') thereby truly testing.

	if arg.chat, printm 'A(:,:)', end
	if isa(A, 'Fatrix')
		Af = A(:,:); % not for fatrix2! calls full()
	else
		if isa(A, 'fatrix2')
			Af = full(A, 'col'); % trick
		else
			Af = full(A);
		end
	end
	if arg.chat, printm 'done', end

	if arg.chat, printm 'A''(:,:)', end
	Aa = A';
	if isa(Aa, 'Fatrix')
		Aa = Aa(:,:); % not for fatrix2! calls full()
	else
		if isa(A, 'fatrix2')
			Aa = full(Aa, 'col'); % trick
		else
			Af = full(A);
		end
	end
	if arg.chat, printm 'done', end

catch
	warn 'A(:,:) or its adjoint failed!?'

	Af = zeros(nd,np);
	Aa = A';
	% forward projection
	for jj=1:np
		x = zeros(np,1);
		x(jj) = 1;
		Af(:,jj) = A * x(:);
	end

	% back projection
	for ii=1:nd
		y = zeros(nd, 1);
		y(ii) = 1;
		Aa(:,ii) = A' * y(:);
	end
end

dif = Af - Aa';
relerr = max(abs(dif(:))) / max(abs(Af(:)));
if relerr > arg.tolre
	printm('adjoint of %s is imperfect (relerr=%g):', inputname(1), relerr)
	printm('adjoint real minmax: %g %g', minmax(real(dif)).')
	printm('adjoint imag minmax: %g %g', minmax(imag(dif)).')
	printm('adjoint real max diff = %g%%', max_percent_diff(real(Af), real(Aa')))
	printm('adjoint imag max diff = %g%%', max_percent_diff(imag(Af), imag(Aa')))
	printm('adjoint class=%s range: %g %g', class(Aa), minmax(Aa(:)).')
else
	tmp = class(A);
	if streq(tmp, 'Fatrix') || streq(tmp, 'fatrix2')
		tmp = sprintf('%s:%s', tmp, A.caller);
	end
	if arg.tolre
		printm('adjoint of %s matches within %g < %g, %s', ...
			inputname(1), relerr, arg.tolre, tmp)
	else
		printm('adjoint of %s appears numerically exact, %s', ...
			inputname(1), tmp)
	end
end

end % test_adjoint

% test_adjoint_big()
% test for big operators using random vectors
function test_adjoint_big(A, nrep, tol, do_complex, do_warn, chat)
rng(0)
for ii=1:nrep
	x = rand(size(A,2),1) - 0.5;
	y = rand(size(A,1),1) - 0.5;
	if do_complex
		x = x + 1i * rand(size(A,2),1) - 0.5;
		y = y + 1i * rand(size(A,1),1) - 0.5;
	end
	Ax = A * x;
	if ~isreal(Ax) && ~do_complex
		fail 'must test complex systems with ''complex'' option'
	end
%	v1 = y' * (A * x);
	v1 = ir_dot_double(conj(y), A * x);
%	v2 = (x' * (A' * y))';
	v2 = ir_dot_double(conj(x), A' * y)';

	mpd = max_percent_diff(v1, v2);
	if mpd/100 > tol
		pr v1
		pr v2
		pr '[mpd/100 tol]'
		if do_warn
			warn 'adjoint mismatch'
		else
			fail 'adjoint mismatch'
		end

	elseif chat
		pr v1
		pr v2
	end
end

end % test_adjoint_big

function test_adjoint_test
	B = randn(4,3);
	test_adjoint(B);
	test_adjoint(B, 'big', true);
	B = B + 2i;
	test_adjoint(B, 'complex', true);
end


% MatrixUser, a multi-dimensional matrix analysis software package
% https://sourceforge.net/projects/matrixuser/
%
% The MatrixUser is a matrix analysis software package developed under Matlab
% Graphical User Interface Developing Environment (GUIDE). It features
% functions that are designed and optimized for working with multi-dimensional
% matrix under Matlab. These functions typically includes functions for
% multi-dimensional matrix display, matrix (image stack) analysis and matrix
% processing.
%
% Author:
%   Fang Liu <leoliuf@gmail.com>
%   University of Wisconsin-Madison
%   Aug-30-2014

function MU_funcCos(Temp,Event,handles)
handles = guidata(handles.MU_matrix_display);

MergeM=get(handles.Matrix_name_edit,'String');
if isfloat(handles.TMatrix(1))
    set(handles.Matrix_name_edit,'String',['cos([' MergeM '])']);
else
    set(handles.Matrix_name_edit,'String',['cos(double([' MergeM ']))']);
end
MU_calc_matrix(handles);

end