% 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


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 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 [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;


%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 [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)];


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


function [x,zo,xs]=estnoisem(yf,tz,pp)
%ESTNOISEM - estimate noise spectrum using minimum statistics
%
% Usage:    ninc=round(0.016*fs);   % frame increment [fs=sample frequency]
%           ovf=2;                  % overlap factor
%           f=rfft(enframe(s,hanning(ovf*ninc,'periodic'),ninc),ovf*ninc,2);
%           f=f.*conj(f);           % convert to power spectrum
%           x=estnoisem(f,ninc/fs); % estimate the noise power spectrum
%
% Inputs:
%   yf      input power spectra (one row per frame)
%   tz      frame increment in seconds
%           Alternatively, the input state from a previous call (see below)
%   pp      algorithm parameters [optional]
%
% Outputs:
%   x       estimated noise power spectra (one row per frame)
%   zo      output state
%   xs      estimated std error of x (one row per frame)
%           xs seems often to be an underestimate by a factor of 2 or 3
%
% The algorithm parameters are defined in reference [1] from which equation
% numbers are given in parentheses. They are as follows:
%
%        pp.taca      % (11): smoothing time constant for alpha_c [0.0449 seconds]
%        pp.tamax     % (3): max smoothing time constant [0.392 seconds]
%        pp.taminh    % (3): min smoothing time constant (upper limit) [0.0133 seconds]
%        pp.tpfall    % (12): time constant for P to fall [0.064 seconds]
%        pp.tbmax     % (20): max smoothing time constant [0.0717 seconds]
%        pp.qeqmin    % (23): minimum value of Qeq [2]
%        pp.qeqmax    % max value of Qeq per frame [14]
%        pp.av        % (23)+13 lines: fudge factor for bc calculation  [2.12]
%        pp.td        % time to take minimum over [1.536 seconds]
%        pp.nu        % number of subwindows to use [3]
%        pp.qith      % Q-inverse thresholds to select maximum noise slope [0.03 0.05 0.06 Inf ]
%        pp.nsmdb     % corresponding noise slope thresholds in dB/second   [47 31.4 15.7 4.1]
%
% Example use:      y=enframe(s,w,ni);                  % divide speech signal s(n) into
%                                                       % overlapping frames using window w(n)
%                   yf=rfft(y,nf,2);                    % take fourier transform
%                   dp=estnoisem(yf.*conj(yf),tinc);    % estimate the noise
%
% If convenient, you can call estnoisem in chunks of arbitrary size. Thus the following are equivalent:
%
%                   (a) dp=estnoisem(yp(1:300),tinc);
%
%                   (b) [dp(1:100),z]=estnoisem(yp(1:100),tinc);
%                       [dp(101:200),z]=estnoisem(yp(101:200),z);
%                       [dp(201:300),z]=estnoisem(yp(201:300),z);

% This is intended to be a precise implementation of [1] with Table III
% replaced by the updated table 5 from [2]. The only deliberate algorithm
% change is the introduction of a minimum value for 1/Qeq in equation (23).
% This change only affects the first few frames and improves the
% convergence of the algorithm. A minor improveemnt was reported in [3] but
% this has not yet been included.
%
% Refs:
%    [1] Rainer Martin.
%        Noise power spectral density estimation based on optimal smoothing and minimum statistics.
%        IEEE Trans. Speech and Audio Processing, 9(5):504-512, July 2001.
%    [2] Rainer Martin.
%        Bias compensation methods for minimum statistics noise power spectral density estimation
%        Signal Processing, 2006, 86, 1215-1229
%    [3] Dirk Mauler and Rainer Martin
%        Noise power spectral density estimation on highly correlated data
%        Proc IWAENC, 2006

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

[nr,nrf]=size(yf);          % number of frames and freq bins
x=zeros(nr,nrf);            % initialize output arrays
xs=zeros(nr,nrf);           % will hold std error in the future
if isempty(yf) && isstruct(tz)             % no real data
    zo=tz;              % just keep the same state
else
    if isstruct(tz)       % take parameters from a previous call
        nrcum=tz.nrcum;
        p=tz.p;          % smoothed power spectrum
        ac=tz.ac;               % correction factor (9)
        sn2=tz.sn2;              % estimated noise power
        pb=tz.pb;               % smoothed noisy speech power (20)
        pb2=tz.pb2;
        pminu=tz.pminu;
        actmin=tz.actmin;   % Running minimum estimate
        actminsub=tz.actminsub;           % sub-window minimum estimate
        subwc=tz.subwc;                   % force a buffer switch on first loop
        actbuf=tz.actbuf;  % buffer to store subwindow minima
        ibuf=tz.ibuf;
        lminflag=tz.lminflag;      % flag to remember local minimum
        tinc=tz.tinc;     % frame increment
        qq=tz.qq;         % parameter structure
    else
        tinc = tz;          % second argument is frame increment
        nrcum=0;            % no frames so far
        % default algorithm constants

        qq.taca=0.0449;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
        qq.tamax=0.392;    % max smoothing time constant in (3) = -tinc/log(0.96)
        qq.taminh=0.0133;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
        qq.tpfall=0.064;   % time constant for P to fall (12)
        qq.tbmax=0.0717;   % max smoothing time constant in (20) = -tinc/log(0.8)
        qq.qeqmin=2;       % minimum value of Qeq (23)
        qq.qeqmax=14;      % max value of Qeq per frame
        qq.av=2.12;             % fudge factor for bc calculation (23 + 13 lines)
        qq.td=1.536;       % time to take minimum over
        qq.nu=8;           % number of subwindows
        qq.qith=[0.03 0.05 0.06 Inf]; % noise slope thresholds in dB/s
        qq.nsmdb=[47 31.4 15.7 4.1];

        if nargin>=3 && ~isempty(pp)
            qqn=fieldnames(qq);
            for i=1:length(qqn)
                if isfield(pp,qqn{i})
                    qq.(qqn{i})=pp.(qqn{i});
                end
            end
        end
    end

    % unpack parameter structure

    taca=qq.taca;    % smoothing time constant for alpha_c = -tinc/log(0.7) in equ (11)
    tamax=qq.tamax;    % max smoothing time constant in (3) = -tinc/log(0.96)
    taminh=qq.taminh;    % min smoothing time constant (upper limit) in (3) = -tinc/log(0.3)
    tpfall=qq.tpfall;   % time constant for P to fall (12)
    tbmax=qq.tbmax;   % max smoothing time constant in (20) = -tinc/log(0.8)
    qeqmin=qq.qeqmin;       % minimum value of Qeq (23)
    qeqmax=qq.qeqmax;      % max value of Qeq per frame
    av=qq.av;             % fudge factor for bc calculation (23 + 13 lines)
    td=qq.td;       % time to take minimum over
    nu=qq.nu;           % number of subwindows
    qith=qq.qith; % noise slope thresholds in dB/s
    nsmdb=qq.nsmdb;   % maximum permitted +ve noise slope in dB/s

    % derived algorithm constants

    aca=exp(-tinc/taca); % smoothing constant for alpha_c in equ (11) = 0.7
    acmax=aca;          % min value of alpha_c = 0.7 in equ (11) also = 0.7
    amax=exp(-tinc/tamax); % max smoothing constant in (3) = 0.96
    aminh=exp(-tinc/taminh); % min smoothing constant (upper limit) in (3) = 0.3
    bmax=exp(-tinc/tbmax); % max smoothing constant in (20) = 0.8
    snrexp = -tinc/tpfall;
    nv=round(td/(tinc*nu));    % length of each subwindow in frames
    if nv<4            % algorithm doesn't work for miniscule frames
        nv=4;
        nu=max(round(td/(tinc*nv)),1);
    end
    nd=nu*nv;           % length of total window in frames
    [md,hd]=mhvals(nd); % calculate the constants M(D) and H(D) from Table III
    [mv,hv]=mhvals(nv); % calculate the constants M(D) and H(D) from Table III
    nsms=10.^(nsmdb*nv*tinc/10);  % [8 4 2 1.2] in paper
    qeqimax=1/qeqmin;  % maximum value of Qeq inverse (23)
    qeqimin=1/qeqmax; % minumum value of Qeq per frame inverse

    if isempty(yf)      % provide dummy initialization
        ac=1;               % correction factor (9)
        subwc=nv;                   % force a buffer switch on first loop
        ibuf=0;
        p=x;          % smoothed power spectrum
        sn2=p;              % estimated noise power
        pb=p;               % smoothed noisy speech power (20)
        pb2=pb.^2;
        pminu=p;
        actmin=repmat(Inf,1,nrf);   % Running minimum estimate
        actminsub=actmin;           % sub-window minimum estimate
        actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
        lminflag=zeros(1,nrf);      % flag to remember local minimum
    else

        if ~nrcum       % initialize values for first frame
            p=yf(1,:);          % smoothed power spectrum
            ac=1;               % correction factor (9)
            sn2=p;              % estimated noise power
            pb=p;               % smoothed noisy speech power (20)
            pb2=pb.^2;
            pminu=p;
            actmin=repmat(Inf,1,nrf);   % Running minimum estimate
            actminsub=actmin;           % sub-window minimum estimate
            subwc=nv;                   % force a buffer switch on first loop
            actbuf=repmat(Inf,nu,nrf);  % buffer to store subwindow minima
            ibuf=0;
            lminflag=zeros(1,nrf);      % flag to remember local minimum
        end

        % loop for each frame

        for t=1:nr              % we use t instead of lambda in the paper
            yft=yf(t,:);        % noise speech power spectrum
            acb=(1+(sum(p)./sum(yft)-1).^2).^(-1);  % alpha_c-bar(t)  (9)
            ac=aca*ac+(1-aca)*max(acb,acmax);       % alpha_c(t)  (10)
            ah=amax*ac.*(1+(p./sn2-1).^2).^(-1);    % alpha_hat: smoothing factor per frequency (11)
            snr=sum(p)/sum(sn2);
            ah=max(ah,min(aminh,snr^snrexp));       % lower limit for alpha_hat (12)

            p=ah.*p+(1-ah).*yft;            % smoothed noisy speech power (3)
            b=min(ah.^2,bmax);              % smoothing constant for estimating periodogram variance (22 + 2 lines)
            pb=b.*pb + (1-b).*p;            % smoothed periodogram (20)
            pb2=b.*pb2 + (1-b).*p.^2;     	% smoothed periodogram squared (21)

            qeqi=max(min((pb2-pb.^2)./(2*sn2.^2),qeqimax),qeqimin/(t+nrcum));   % Qeq inverse (23)
            qiav=sum(qeqi)/nrf;             % Average over all frequencies (23+12 lines) (ignore non-duplication of DC and nyquist terms)
            bc=1+av*sqrt(qiav);             % bias correction factor (23+11 lines)
            bmind=1+2*(nd-1)*(1-md)./(qeqi.^(-1)-2*md);      % we use the simplified form (17) instead of (15)
            bminv=1+2*(nv-1)*(1-mv)./(qeqi.^(-1)-2*mv);      % same expression but for sub windows
            kmod=bc*p.*bmind<actmin;        % Frequency mask for new minimum
            if any(kmod)
                actmin(kmod)=bc*p(kmod).*bmind(kmod);
                actminsub(kmod)=bc*p(kmod).*bminv(kmod);
            end
            if subwc>1 && subwc<nv              % middle of buffer - allow a local minimum
                lminflag=lminflag | kmod;    	% potential local minimum frequency bins
                pminu=min(actminsub,pminu);
                sn2=pminu;
            else
                if subwc>=nv                    % end of buffer - do a buffer switch
                    ibuf=1+rem(ibuf,nu);     	% increment actbuf storage pointer
                    actbuf(ibuf,:)=actmin;    	% save sub-window minimum
                    pminu=min(actbuf,[],1);
                    i=find(qiav<qith);
                    nsm=nsms(i(1));          	% noise slope max
                    lmin=lminflag & ~kmod & actminsub<nsm*pminu & actminsub>pminu;
                    if any(lmin)
                        pminu(lmin)=actminsub(lmin);
                        actbuf(:,lmin)=repmat(pminu(lmin),nu,1);
                    end
                    lminflag(:)=0;
                    actmin(:)=Inf;
                    subwc=0;
                end
            end
            subwc=subwc+1;
            x(t,:)=sn2;
            qisq=sqrt(qeqi);
            % empirical formula for standard error based on Fig 15 of [2]
            xs(t,:)=sn2.*sqrt(0.266*(nd+100*qisq).*qisq/(1+0.005*nd+6/nd)./(0.5*qeqi.^(-1)+nd-1));
        end
    end
    if nargout>1    % we need to store the state for next time
        zo.nrcum=nrcum+nr;      % number of frames so far
        zo.p=p;          % smoothed power spectrum
        zo.ac=ac;               % correction factor (9)
        zo.sn2=sn2;              % estimated noise power
        zo.pb=pb;               % smoothed noisy speech power (20)
        zo.pb2=pb2;
        zo.pminu=pminu;
        zo.actmin=actmin;   % Running minimum estimate
        zo.actminsub=actminsub;           % sub-window minimum estimate
        zo.subwc=subwc;                   % force a buffer switch on first loop
        zo.actbuf=actbuf;  % buffer to store subwindow minima
        zo.ibuf=ibuf;
        zo.lminflag=lminflag;      % flag to remember local minimum
        zo.tinc=tinc;     % must be the last one
        zo.qq=qq;
    end
    if ~nargout
        clf;
        subplot(212);
        plot((1:nr)*tinc,10*log10([sum(yf,2) sum(x,2)]))
        ylabel('Frame Energy (dB)');
        xlabel(sprintf('Time (s)   [%d ms frame incr]',round(tinc*1000)));
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
        subplot(211);
        plot(1:nrf,10*log10([sum(yf,1)'/nr sum(x,1)'/nr]))
        ylabel('Power (dB)');
        xlabel('Frequency bin');
        axisenlarge([-1 -1.05]);
        legend('input','noise','Location','Best');
    end
end

function [m,h,d]=mhvals(d)
% Values are taken from Table 5 in [2]
%[2] R. Martin,"Bias compensation methods for minimum statistics noise power
%               spectral density estimation", Signal Processing Vol 86, pp1215-1229, 2006.

% approx: plot(d.^(-0.5),[m 1-d.^(-0.5)],'x-'), plot(d.^0.5,h,'x-')
persistent dmh
if isempty(dmh)
    dmh=[
        1   0       0;
        2   0.26    0.15;
        5   0.48    0.48;
        8   0.58    0.78;
        10  0.61    0.98;
        15  0.668   1.55;
        20  0.705   2;
        30  0.762   2.3;
        40  0.8     2.52;
        60  0.841   3.1;
        80  0.865   3.38;
        120 0.89    4.15;
        140 0.9     4.35;
        160 0.91    4.25;
        180 0.92    3.9;
        220 0.93    4.1;
        260 0.935   4.7;
        300 0.94    5];
end

if nargin>=1
    i=find(d<=dmh(:,1));
    if isempty(i)
        i=size(dmh,1);
        j=i;
    else
        i=i(1);
        j=i-1;
    end
    if d==dmh(i,1)
        m=dmh(i,2);
        h=dmh(i,3);
    else
        qj=sqrt(dmh(i-1,1));    % interpolate using sqrt(d)
        qi=sqrt(dmh(i,1));
        q=sqrt(d);
        h=dmh(i,3)+(q-qi)*(dmh(j,3)-dmh(i,3))/(qj-qi);
        m=dmh(i,2)+(qi*qj/q-qj)*(dmh(j,2)-dmh(i,2))/(qi-qj);
    end
else
    d=dmh(:,1);
    m=dmh(:,2);
    h=dmh(:,3);
end


% EDICON Multi-edit and condense a training set
%
% 	J = EDICON(D,NSETS,NITERS,NTRIES)
%
% INPUT
%   D       Distance matrix dataset
%   NSETS   Number of subsets for editing, or [] for no editing (default: 3)
%   NITERS  Number of iterations for editing (default: 5)
%   NTRIES  Number of tries for condensing, or [] for no condensing (dflt: 10)
%
% OUTPUT
%   J       Indices of retained samples
%
% DESCRIPTION
% Returns the set of objects J such that the nearest neigbour gives zero error
% on the remaining objects. If MODE = 0, multi-edit the dataset represented by
% distance matrix D first. D can be computed from a dataset A by A*proxm(A).
%
% REFERENCES
% Devijver, P. and Kittler, J. "Pattern recognition", Prentice-Hall, 1982.
%
% SEE ALSO (PRTools Guide)
% PRDATASET, KNNC, PROXM

% Copyright: E. Pekalska and D. de Ridder, {E.Pekalska,D.deRidder}@ewi.tudelft.nl
% Faculty of Electrical Engineering, Mathematics and Computer Science
% Delft University of Technology, Mekelweg 4, 2628 CD Delft, The Netherlands

function J = edicon (D,no_subsets,max_iter,no_tries)

    if (nargin < 4 | isempty(no_tries))
        prwarning(4,'number of tries for condensing not specified, assuming 10');
        no_tries = 10;
    end;
    if (nargin < 3 | isempty(max_iter))
        prwarning(4,'number of iterations for editing not specified, assuming 5');
        max_iter = 5;
    end;
    if (nargin < 2 | isempty(no_subsets))
        prwarning(4,'number of subsets for editing not specified, assuming 3');
        no_subsets = 5;
    end;

    % Extract dataset information.

	nlab    = getnlab(D); lablist = getlablist(D);
	[m,k,c] = getsize(D); p       = getprior(D);

    if (~isdataset(D) | (m ~= k))
        error('require a square distance matrix dataset');
    end

    J = 1:m;            % Initialise index array.

    % If requested, apply the multi-edit algorithm.

    if (~isempty(no_subsets))
        iter = 1;
        while (iter <= max_iter)

      	    % 1. Create NO_SUBSETS distinct subsets out of the sample indices
            %    remaining in J. Store the subset indices in subset_ind{.}.
            %    Note: this is slightly more complicated than strictly
            %    necessary, to enforce that each class is represented equally
            %    in each subset.

    		m = length(J); J = J(randperm(m));
		    subset_size = floor(m/no_subsets);

            % Find the MC(J) indices of samples belonging to class J in CLASS_IND{J}.
            % Also create a random permutation for class J.

            for j = 1:c
			    class_ind{j} = find(nlab(J)==j);
                mc(j)        = length(class_ind{j});
			    perm{j}      = randperm(mc(j));
    		end;

            % Distribute the samples over the subsets, per class.

    		for i = 1:no_subsets
		        subset_ind{i} = [];
			    for j = 1:c
                    num           = min(floor(mc(j)/no_subsets),length(class_ind{j}));
        		    subset_ind{i} = [subset_ind{i}; class_ind{j}(1:num)];
				    class_ind{j}  = class_ind{j}(num+1:end);
    			end;
			    perm_subset_ind{i} = J(subset_ind{i});
    		end;

          	% 2. In turn, classify each subset I using 1-NN on subset I+1.

          	drop = [];
  	        for i = 1:no_subsets
    			L1 = perm_subset_ind{i};
                L2 = perm_subset_ind{mod(i,no_subsets)+1};
    			if (length(L2)>1)
	  	    	    [dummy,nearest] = min(D(L2,L1));
    			else
				    nearest = 1;
    			end;
                drop = [drop; subset_ind{i}(find(nlab(L1)~=nlab(L2(nearest))))];
            end;

         	% 3. Discard incorrectly classified samples from J.

  	        if (isempty(drop))
  		        iter = iter + 1;
         	else
   		        J(drop) = []; iter = 0;
          	end;
        end;

        D = D(J,J);
    end;

    % Condense.

    if (~isempty(no_tries))
        % Extract dataset information.

    	nlab    = getnlab(D); lablist = getlablist(D);
	    [m,k,c] = getsize(D); p       = getprior(D);

        D = +D;

        % The whole procedure starts from a random object and continues to add
        % objects, so it is not optimal. Therefore it is repeated NO_TRIES times
        % and the smallest set found is returned.

        K = zeros(m,no_tries); storesz = zeros(1,no_tries);
        for o = 1:no_tries

            % Start with 1 sample index in STORE, the others in GRABBAG.
            p = randperm(m); store = p(1); grabbag = p(2:m);

            % While there are changes...
            transfer = 0;
            while (~transfer) & (~isempty(grabbag))
    	        storelab    = nlab(store);
	            grabbaglab  = nlab(grabbag);
	            new_grabbag = [];

                % For all samples in GRABBAG...
            	transfer = 0;
	            for k = 1:length(grabbag)
                    % ... find the nearest sample in STORE...
   	                [dummy,z] = min(D(grabbag(k),store));

                    % ... if it has a different label, move it to STORE.
   	                if (storelab(z) ~= grabbaglab(k))
                        store    = [store; grabbag(k)];
                        storelab = [storelab; grabbaglab(k)];
                        transfer = 1;
               	    else
                        new_grabbag = [new_grabbag; grabbag(k)];
                   	end;
    	        end;

            	grabbag = new_grabbag;

              end

              % Remember the STORE and its size for this repetition.
              storesz(o)        = length(store);
              K(1:storesz(o),o) = store;

        end

        % Take the STORE with minimal size.
        [minsz,minszind] = min(storesz);
        J = J(K(1:minsz,minszind));
    end;

return


%DEMOMESHSHAPES Display various 3D demo shapes, together with surfacic mesh
%
%   output = demoImShapes(input)
%
%   Example
%   demoImShapes
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2011-06-29,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2011 INRA - Cepia Software Platform.

%% initialisations

% generate cubic images
lx = 1:100;
ly = 1:100;
lz = 1:100;

% choose a center not aligned with the grid
center = [50+sqrt(2)-1 50+sqrt(3)-1 50+sqrt(5)-2];

angles = [...
    0 0; ...
    30 0; ...
    30 30];

% set of ellipsoid orientations, in format YAW PITCH ROLL
orientations = [...
     0   0   0; ...
    30   0   0; ...
    30  30   0; ...
    30  30  30];

%% Ball

% sphere is defined by center and radius
sphere = [center 40];

% generation of 3D image
img = discreteBall(lx, ly, lz, sphere);

% display image isosurface
f = figure; set(gca, 'fontsize', 14);
isosurface(img, .5);

% setup display
hold on; axis equal; l = light;
axis ([0 100 0 100 0 100]);
view([40 20]);
snapnow;

% add surfacic mesh
drawSphere(sphere);

% decorate
title('Sphere, radius=40');
legend('isosurface', 'shape mesh', 'location', 'northeast');

%% Ellipsoid

clf;

% iterate on orientations
for i = 1:4
    elli = [center 50 30 10 orientations(i, :)];

    % generation of 3D image
    img = discreteEllipsoid(lx, ly, lz, elli);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 10 90  20 80]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Ellipsoid, ori=[%d %d %d]', orientations(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawEllipsoid(elli, 'FaceColor', 'r');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Cuboid

clf;

% iterate on orientations
for i = 1:4
    cubo = [center 90 40 10 orientations(i, :)];

    % generation of 3D image
    img = discreteCuboid(lx, ly, lz, cubo);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 10 90  20 80]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Cuboid, ori=[%d %d %d]', orientations(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawCuboid(cubo, 'FaceColor', 'r');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Cube

% iterate on orientations
for i = 1:4

    % cylinder representation
    cube = [center 60 orientations(i, :)];

    % generation of 3D image
    img = discreteCube(lx, ly, lz, cube);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 0 100 0 100]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Cube, ori=[%d %d %d]', orientations(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawCube(cube, 'FaceColor', 'r');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Torus

% iterate on orientations
for i = 1:3
    torus = [center 30 10 angles(i, :)];

    % generation of 3D image
    img = discreteTorus(lx, ly, lz, torus);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([0 100 10 90  20 80]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Torus, ori=[%d %d]', angles(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawTorus(torus, 'FaceColor', 'r', 'LineStyle', 'none');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end

%% Cylinder

clf;

% iterate on orientations
for i = 1:3

    % cylinder representation
    cart = sph2cart2d(angles(i, :));
    p1 = center - 30*cart;
    p2 = center + 30*cart;
    cyl = [p1 p2 10];

    % generation of 3D image
    img = discreteCylinder(lx, ly, lz, cyl);

    % display image isosurface
    clf; set(gca, 'fontsize', 14);
    patch(isosurface(img, .5), 'FaceColor', 'g', 'LineStyle', 'none');

    % setup display
    hold on; axis equal; l = light;
    axis ([20 80 20 80 0 100]);
    view([40 20]);

    % decorate
    strTitle = sprintf('Cylinder, ori=[%d %d]', angles(i, :));
    title(strTitle);

    snapnow;

    % add surfacic mesh
    drawCylinder(cyl, 'FaceColor', 'r', 'LineStyle', 'none');

    % decorate
    title(strTitle);
    legend('isosurface', 'shape mesh', 'location', 'northeast');
    snapnow;
end


function linpack_s_test27 ( )

%*****************************************************************************80
%
%% TEST27 tests SSPFA and SSPSL.
%
%  Discussion:
%
%    SSPFA and SSPSL are for packed symmetric indefinite matrices.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 100;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST27\n' );
  fprintf ( 1, '  For a symmetric indefinite packed matrix,\n' );
  fprintf ( 1, '  SSPFA factors the matrix,\n' );
  fprintf ( 1, '  SSPSL solves a factored linear system.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Assign values to the matrix A and the right hand side B.
%
  b(1:n-1) = 0.0;
  b(n)= n + 1;

  k = 0;
  for j = 1 : n
    for i = 1 : j
      k = k + 1;
      if ( i == j )
        a(k) = 2.0;
      elseif ( j == i+1 )
        a(k) = -1.0;
      else
        a(k) = 0.0;
      end
    end
  end
%
%  Factor the matrix.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Factor the matrix.\n' );

  [ a, ipvt, info ] = sspfa ( a, n );

  if ( info ~= 0 )
    fprintf ( 1, '  Error!  SSPFA returns INFO = %d\n', info );
    return
  end
%
%  Solve the linear system.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Solve the linear system.\n' );

  b = sspsl ( a, n, ipvt, b );
%
%  Print the result.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The first and last five entries of the solution:\n' );
  fprintf ( 1, '  (Should be 1,2,3,4,5,...,n-1,n):\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    if ( i <= 5 | n-5 < i )
      fprintf ( 1, '  %6d  %14f\n', i, b(i) );
    end
    if ( i == 5 )
      fprintf ( 1, '  ......  ..............\n' );
    end
  end

  return
end


function [A, b, x, ProbInfo] = PRtomo(varargin)
% IRtomo Generates data for X-ray tomographic reconstruction problems
%
% [A, b, x, ProbInfo] = PRtomo
% [A, b, x, ProbInfo] = PRtomo(n)
% [A, b, x, ProbInfo] = PRtomo(n, options)
% [A, b, x, ProbInfo] = PRtomo(options)
%
% This function uses the "line model" to create a 2D X-ray tomography test
% problem with an N-times-N pixel domain, using p rays for each angle in
% the vector theta.
%
% The severity of the problem is almost unaffected by the problem parameters.
%
% Input:
%  n : size of the image; a scalar such that the size is n x n.
%      Default: n = 256.
%  options : Structure containing the following optional fields:
%    phantomImage - user supplied test image of size n x n, of type numeric,
%             2-D only, or character string indicating
%             'shepplogan' : Shepp-Logan phantom
%             'smooth' : a smooth image
%             'binary' : a binary image
%             'threephases' : random image with pixel values 0, 0.5, 1
%                arranged in domains
%             'threephasessmooth' : similar to threephases, but the
%                domains have smoothly varying pixel values and there is
%                a smooth background
%             'fourphases' : similar to 'binary' but with three phases
%                separated by (thin) structures that form the fourth phase
%             'grains' : a random image with Voronoi cells
%             'ppower' : a random image with patterns of nonzero pixels
%             Default: 'shepplogan'.
%             This image is then stored in the output vector x.
%    CTtype - string that defines the type of CT problem:
%             'parallel'  : parallel beam geometry (default),
%             'fancurved' : fan beam with curved detector.
%    sm     - logical; if true (default )then A is a sparse matrix,
%             otherwise it is a function handle.
%    angles - vector of projection angles, in degrees.
%             Default: 0:1:179 for parallel beam, 0:2:358 for fan beam.
%    p      - number of rays for each source angle.
%             Default: p = round(sqrt(2)*n).
%    d      - Parallel beam only: scalar denoting the distance from the
%             first ray to the last; default: d = p-1.
%    R      - Fan beam only: the distance from the source to the center
%             of the phantom is R*N.  Default: R = 2.
%    span   - Fan beam only: scalar that determines the angular span of
%             the rays, in degrees. The default value is defined such
%             that from the source at (0,R*N) the first and last rays
%             hit the image corners (-n/2,n/2) and (n/2,n/2).
%
% Output:
%  A : Sparse matrix or function handle for forward/adjoint problem.
%  b : Vector with projection data (the sinogram).
%  x : Vector with image (i.e., exact image with stacked columns).
%  ProbInfo : structure containing some information about problem
%      problemType - kind of test problem (in this case: 'tomography')
%      xType       - solution type (in this case 'image2D')
%      bType       - data type (in this case 'image2D')
%      xSize       - size of image x
%      bSize       - size of sinogram b
%
% See also: PRblur, PRdiffusion, PRinvinterp2, PRnmr, PRseismic,
% PRspherical, PRtomo, PRnoise, PRshowb, PRshowx, radon

% Silvia Gazzola, University of Bath
% Per Christian Hansen, Technical University of Denmark
% James G. Nagy, Emory University
% April, 2018.

% This file is part of the IR Tools package and is distributed under the
% 3-Clause BSD Licence. A separate license file should be provided as part
% of the package.

% Initialization: set the default image size.
default_N = 256;

% Check for acceptable number of optional input arguments.
switch length(varargin)
    case 0
        N = default_N; options = [];
    case 1
        if isa(varargin{1}, 'double')
            N = varargin{1}; options = [];
        else
            N = default_N; options = varargin{1};
        end
    case 2
        if isa(varargin{1}, 'double')
            N = varargin{1}; options = varargin{2};
        else
            N = varargin{2}; options = varargin{1};
        end
    otherwise
        error('Too many input parameters')
end

% Set default values for options.
defaultopt = struct('phantomImage', 'shepplogan', 'CTtype', 'parallel', ...
    'sm', true, 'angles', 0:1:179, 'p', NaN, 'R', 2, 'd', NaN, 'span', NaN);

% If input is 'defaults,' return the default options in A.
if nargin == 1 && nargout <= 1 && strcmp(varargin,'defaults')
    A = defaultopt;
    return;
end

if isempty(options)
    options = defaultopt;
end

options = PRset(defaultopt, options);

phantom    = PRget(options, 'phantomImage', [], 'fast');
CTtype     = PRget(options, 'CTtype',       [], 'fast');
angles     = PRget(options, 'angles',       [], 'fast');
sm         = PRget(options, 'sm',           [], 'fast');
p          = PRget(options, 'p',            [], 'fast');
R          = PRget(options, 'R',            [], 'fast');
d          = PRget(options, 'd',            [], 'fast');
span       = PRget(options, 'span',         [], 'fast');

% If a phantom image is given, then this defines N and the other parametes.
if isnumeric(phantom)
    % Make sure user input image is a matrix
    if ~ismatrix(phantom)
        error('Expected user supplied phantom image to be a 2-D array of type numeric')
    else
        N = size(phantom,1);
        if size(phantom,2)~=N, error('phantomImage image must be square'); end
        x = double(phantom(:));
    end
else
    x = phantomgallery(phantom,N);
    x = x(:);
end
if isnan(p), p = round(sqrt(2)*N); end

switch CTtype
    case 'parallel'
        if isnan(d), d = p-1; end
        A = paralleltomo(N,angles,p,d,0,sm);
    case 'fancurved'
        if isnan(span), span = 2*atand(1/(2*R-1)); end
        A = fancurvedtomo(N,angles,p,R,span,0,sm);
   otherwise
        error('Type of CT problem not provided')
end
if sm
    b = A*x;
else
    b = A(x,'notransp');
end

% Providing information about the test problem.
ProbInfo.problemType = 'tomography';
ProbInfo.xType = 'image2D';
ProbInfo.bType = 'image2D';
ProbInfo.xSize = [N,N];
ProbInfo.bSize = [p,length(angles)];


function [P, Transf] = cs_convert(sMri, src, dest, P, isNanAllowed)
% CS_CONVERT: Convert 3D points between coordinates systems.
%
% USAGE:       P = cs_convert(sMri, src, dest, P, isNanAllowed=0)
%         Transf = cs_convert(sMri, src, dest)
%
% INPUT:
%     - sMri         : Brainstorm MRI structure
%     - src          : Current coordinate system {'voxel','mri','scs','mni','acpc','world','captrak'}
%     - dest         : Target coordinate system {'voxel','mri','scs','mni','acpc','world','captrak'}
%     - P            : a Nx3 matrix of point coordinates to convert
%     - isNanAllowed : If 0, throw an error and if the conversion of some points P leads to NaN,
%                      and if there is no linear MNI registration to be used instead
%                      (case of non-linear MNI registration with points outside of the defined area)
%
% DESCRIPTION:   https://neuroimage.usc.edu/brainstorm/CoordinateSystems
%     - voxel : X=left>right,  Y=posterior>anterior,   Z=bottom>top
%               Coordinate of the center of the first voxel at the bottom-left-posterior of the MRI volume: (1,1,1)
%     - mri   : Same as 'voxel' but in millimeters instead of voxels:  mriXYZ = voxelXYZ * Voxsize
%     - scs   : Based on: Nasion, left pre-auricular point (LPA), and right pre-auricular point (RPA).
%               Origin: Midway on the line joining LPA and RPA
%               Axis X: From the origin towards the Nasion (exactly through)
%               Axis Y: From the origin towards LPA in the plane defined by (NAS,LPA,RPA), and orthogonal to X axis
%               Axiz Z: From the origin towards the top of the head
%     - mni   : MNI coordinates based on SPM affine or non-linear registration
%     - acpc  : Based on: Anterior commissure (AC), Posterior commissure (PC) and an interhemisperic point (IH)
%               Origin: AC
%               Axis X: From the origin towards the right
%               Axis Y: Negative y-axis is passing from AC through PC
%               Axis Z: Passing through a mid-hemispheric point in the superior direction
%     - world : Transformation available in the initial file loaded as the default MRI (vox2ras/qform/world transformation)
%     - captrak: RAS orientation and the origin approximately between LPA and RPA
%               Axis X: From LPA through RPA exactly
%               Axis Y: Orthogonal to the X-axis through the nasion (NAS)
%               Axis Z: Orthogonal to the XY-plane through the vertex of the head

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2008-2022

% Throw errors by default if NaN
if (nargin < 5) || isempty(isNanAllowed)
    isNanAllowed = 0;
end
% Check matrices orientation
if (nargin < 4) || isempty(P)
    P = [];
elseif (size(P,2) ~= 3) && (size(P,1) == 3)
    P = P';
elseif (size(P,2) ~= 3)
    error('P must have 3 columns (X,Y,Z).');
end
% If the coordinate system didn't change
if strcmpi(src, dest)
    return;
end
% Keep track of points that cannot be transformed
iMissing = [];
isApplied = 0;
Porig = P;
% Transform to homogeneous coordinates
if ~isempty(P)
    P = [P'; ones(1,size(P,1))];
end

% ===== GET MRI=>WORLD TRANSFORMATION =====
if strcmpi(src, 'world') || strcmpi(dest, 'world') || (strcmpi(src, 'mni') && isfield(sMri,'NCS') && isfield(sMri.NCS,'y') && ~isempty(sMri.NCS.y))
    % Get the vox2ras transformation
    if isempty(sMri) || isempty(sMri.InitTransf)
        P = [];
        return;
    end
    iTransf = find(strcmpi(sMri.InitTransf(:,1), 'vox2ras'));
    if isempty(iTransf)
        P = [];
        return;
    end
    vox2ras = sMri.InitTransf{iTransf,2};
    % 2nd operation: Change reference from (0,0,0) to (1,1,1)
    vox2ras = vox2ras * [1 0 0 -1; 0 1 0 -1; 0 0 1 -1; 0 0 0 1];
    % 1st operation: Convert from MRI(mm) to voxels
    vox2ras = vox2ras * diag(1 ./ [sMri.Voxsize, 1]);
    % Convert to meters: transformation MRI=>WORLD (in meters)
    mri2world = vox2ras .* [ones(3,3), 1e-3.*ones(3,1); ones(1,4)];
    % Compute inverse transformation WORLD=>MRI (in meters)
    world2mri = inv(mri2world);
end

% ===== COMPUTE ACPC TRANSFORMATION =====
if strcmpi(src, 'acpc') || strcmpi(dest, 'acpc')
    tACPC = cs_compute(sMri, 'acpc');
    if isempty(tACPC) || isempty(tACPC.R)
        P = [];
        return;
    end
    scs2acpc = [tACPC.R, tACPC.T; 0 0 0 1];
end

% ===== COMPUTE CAPTRAK TRANSFORMATION =====
if strcmpi(src, 'captrak') || strcmpi(dest, 'captrak')
    tCapTrak = cs_compute(sMri, 'captrak');
    if isempty(tCapTrak) || isempty(tCapTrak.R)
        P = [];
        return;
    end
    scs2captrak = [tCapTrak.R, tCapTrak.T; 0 0 0 1];
end

% ===== CONVERT SRC => MRI =====
% Evaluate the transformation to apply
switch lower(src)
    case 'voxel'
        RT1 = diag([sMri.Voxsize(:) ./ 1000; 1]);
    case 'mri'
        RT1 = eye(4);
    case 'scs'
        if ~isfield(sMri,'SCS') || ~isfield(sMri.SCS,'R') || isempty(sMri.SCS.R) || ~isfield(sMri.SCS,'T') || isempty(sMri.SCS.T)
            P = [];
            return;
        end
        RT1 = inv([sMri.SCS.R, sMri.SCS.T./1000; 0 0 0 1]);
    case 'mni'
        % Transformation of each point by indirection in the deformation field y
        if ~isempty(P) && isfield(sMri,'NCS') && isfield(sMri.NCS,'y') && ~isempty(sMri.NCS.y)
            % Convert MNI => voxel space of the registration matrix
            P_reg = inv(sMri.NCS.y_vox2ras) * (P .* [1000;1000;1000;1]);
            % Convert from 0-based to 1-based??
            % => This solution was obtained empirically by minimizing:
            %    sqrt(sum((cs_convert(sMri, 'mri', 'mni', cs_convert(sMri, 'mni', 'mri', P)) - P).^2)).*1000 => around 0.003 with this adjustment
            P_reg = P_reg + [1;1;1;0];
            % Convert Voxel => World
            P_world = [...
                interp3(sMri.NCS.y(:,:,:,1), P_reg(2,:), P_reg(1,:), P_reg(3,:), 'linear', NaN); ...
                interp3(sMri.NCS.y(:,:,:,2), P_reg(2,:), P_reg(1,:), P_reg(3,:), 'linear', NaN); ...
                interp3(sMri.NCS.y(:,:,:,3), P_reg(2,:), P_reg(1,:), P_reg(3,:), 'linear', NaN)] ./ 1000;
            % Check if some points could not be converted
            if ~isNanAllowed
                iMissing = find(any(isnan(P_world),1));
            end
            % Convert World => MRI
            P = world2mri * [double(P_world); ones(1, size(P_world,2))];
            RT1 = eye(4);
        elseif isfield(sMri,'NCS') && isfield(sMri.NCS,'R') && ~isempty(sMri.NCS.R) && isfield(sMri.NCS,'T') && ~isempty(sMri.NCS.T)
            RT1 = inv([sMri.NCS.R, sMri.NCS.T./1000; 0 0 0 1]);
        else
            P = [];
            return;
        end
    case 'acpc'
        % ACPC => SCS => MRI
        RT1 = inv(scs2acpc);
    case 'captrak'
        % CapTrak => SCS => MRI
        RT1 = inv(scs2captrak);
    case 'world'
        RT1 = world2mri;
    otherwise
        error(['Invalid coordinate system: ' src]);
end

% ===== CONVERT MRI => DEST =====
% Evaluate the transformation to apply
switch lower(dest)
    case 'voxel'
        RT2 = diag([1000 ./ sMri.Voxsize(:); 1]);
    case 'mri'
        RT2 = eye(4);
    case 'scs'
        if ~isfield(sMri,'SCS') || ~isfield(sMri.SCS,'R') || isempty(sMri.SCS.R) || ~isfield(sMri.SCS,'T') || isempty(sMri.SCS.T)
            P = [];
            return;
        end
        RT2 = [sMri.SCS.R, sMri.SCS.T./1000; 0 0 0 1];
    case 'mni'
        % Using non-linear MNI normalization: Transformation of each point by indirection in the deformation field iy
        if ~isempty(P) && isfield(sMri,'NCS') && isfield(sMri.NCS,'iy') && ~isempty(sMri.NCS.iy)
            % Convert: src => MRI => voxel
            P_vox = diag([1000 ./ sMri.Voxsize(:); 1]) * RT1 * P;
            % Get values from the iy volumes
            Pmni = [interp3(sMri.NCS.iy(:,:,:,1), P_vox(2,:), P_vox(1,:), P_vox(3,:), 'linear', NaN); ...
                 interp3(sMri.NCS.iy(:,:,:,2), P_vox(2,:), P_vox(1,:), P_vox(3,:), 'linear', NaN); ...
                 interp3(sMri.NCS.iy(:,:,:,3), P_vox(2,:), P_vox(1,:), P_vox(3,:), 'linear', NaN)] ./ 1000;
            % Check if some points could not be converted
            if ~isNanAllowed
                iMissing = find(any(isnan(Pmni),1));
            end
            % Transpose the matrix back
            P = double(Pmni');
            Transf = [];
            isApplied = 1;
        elseif isfield(sMri,'NCS') && isfield(sMri.NCS,'R') && ~isempty(sMri.NCS.R) && isfield(sMri.NCS,'T') && ~isempty(sMri.NCS.T)
            RT2 = [sMri.NCS.R, sMri.NCS.T./1000; 0 0 0 1];
        else
            P = [];
            return;
        end
    case 'acpc'
        % MRI => SCS => ACPC
        RT2 = scs2acpc;
    case 'captrak'
        % MRI => SCS => CapTrak
        RT2 = scs2captrak;
    case 'world'
        RT2 = mri2world;
    otherwise
        error(['Invalid coordinate system: ' dest]);
end

% If the final transformation is not already applied
if ~isApplied
    % Compute the final transformation matrix
    Transf = RT2 * RT1;
    % Apply the transformation matrix to the points
    if ~isempty(P)
        % Apply rotation-translation
        P = Transf * P;
        % Remove the last coordinate and transpose the matrix back
        P = P(1:3,:)';
    else
        P = Transf;
    end
end

% If there are NaN points from non-linear MNI: Try to fix them with linear MNI
if ~isempty(iMissing)
    % If there is a linear transformation available as well, use it for missing points (same code as below)
    if isfield(sMri,'NCS') && isfield(sMri.NCS,'R') && ~isempty(sMri.NCS.R) && isfield(sMri.NCS,'T') && ~isempty(sMri.NCS.T)
        % Remove the non-linear transformation, so that it forces using the linear one
        sMri.NCS.iy = [];
        sMri.NCS.y = [];
        P(iMissing,:) = cs_convert(sMri, src, dest, Porig(iMissing,:), isNanAllowed);
    % If NaN not allowed and no linear transformation: check for missing values
    elseif ~isNanAllowed
        error(['Some points are outside the definition of the non-linear MNI registration.' 10 'Please compute a linear MNI normalization to convert these points.']);
    end
end


function [Jul1,Jul2]=UTC2TDB(Jul1,Jul2,deltaT,clockLoc)
%%UTC2TDB Convert from universal coordinated time (UTC) to barycentric
%         dynamical time (TDB) to an accuracy of nanoseconds (if deltaT
%         is accurate) using the routines from the International
%         Astronomical Union's library that do not require external
%         ephemeris data.
%
%INPUTS: Jul1,Jul2 Two parts of a pseudo-Julian date given in UTC. The
%                  units of the date are days. The full date is the sum of
%                  both terms. The date is broken into two parts to provide
%                  more bits of precision. It does not matter how the date
%                  is split.
%           deltaT An optional parameter specifying the offset between UTC
%                  and UT1 in seconds. If this parameter is omitted or an
%                  empty matrix is passed, then the value of the function
%                  deltaTTUT1 with an appropriate offset will be used.
%         clockLoc An optional 3X1 vector specifying the location of the
%                  clock in WGS-84 ECEF Cartesian [x;y;z] coordinates with
%                  units of meters. Due to relativistic effects, clocks
%                  that are synchronized with respect to UTC are not
%                  synchronized with respect to TDB. If this parameter is
%                  omitted, then a clock at the center of the Earth is
%                  used and the precision declines to microseconds.
%
%OUTPUTS:Jul1,Jul2 Two parts of a Julian date given in TDB.
%
%This function converts the date to a terrestrial time and then calls
%TT2TDB.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%March 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3)
    deltaT=[];
end

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

[Jul1,Jul2]=UTC2TT(Jul1,Jul2);
[Jul1,Jul2]=TT2TDB(Jul1,Jul2,deltaT,clockLoc);

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.