function [idi, rt, rn, bb] = idis(pt, pn, y, z, t, varargin)
%IDIS Integrated Discrimination Improvement between two models
%
%  Description
%    [IDI,RT,RN,BB] = IDIS(PT,PN,Y,Z,T,OPTIONS) Returns Integrated
%    Discrimination Improvement (IDI) given two vectors of event probabilities
%    at time T, PT for traditional model and PN for new model, and the
%    observed event or censoring times Y and the corresponding censoring
%    indicators Z (0=event, 1=censored). With these inputs, the estimator
%    attributed to Pencina et al. in the reference is used, with
%    restriction to time T.
%
%    [IDI,RT,RN,BB] = IDIS(PT,PN,OPTIONS) Returns Integrated
%    Discrimination Improvement (IDI) given two vectors of event probabilities
%    at time T, PT for traditional model and PN for new model. Here, the
%    model-based estimator is used (the "new estimator" in the reference).
%
%    Ouputs RT and RN are the R^2 statistics for the two models. BB are
%    Bayesian bootstrap samples of the IDI distribution.
%
%    OPTIONS is optional parameter-value pair
%      rsubstream - number of a random stream to be used for
%                   simulating dirrand variables. This way same
%                   simulation can be obtained for different models.
%                   See doc RandStream for more information.
%
%  Reference
%    L. E. Chambless, C. P. Cummiskey, and G. Cui (2011). Several
%    methods to assess improvement in risk prediction models:
%    Extension to survival analysis. Statistics in Medicine
%    30(1):22-38.

% Copyright (C) 2012 Ernesto Ulloa, Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

ip=inputParser;
if nargin < 3 || ischar(y)
    % model-based estimator
    model_based_estimator = true;

    ip.addRequired('pt', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
    ip.addRequired('pn', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
    ip.addParamValue('rsubstream', 0, @(x) isreal(x) && isscalar(x) && isfinite(x) && x>0)
    if nargin > 2
        % if more than 2 input arguments, there must be four as only a
        % single optional parameter is implemented
        if nargin == 4
            ip.parse(pt, pn, y, z);
        else
            error('Invalid number of arguments.');
        end
    else
        ip.parse(pt, pn);
    end
else
    % Pencina et al. estimator
    model_based_estimator = false;

    ip.addRequired('pt', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
    ip.addRequired('pn', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
    ip.addRequired('y', @(x) isreal(x) && all(isfinite(x(:))))
    ip.addRequired('z', @(x) isreal(x) && all(isfinite(x(:))))
    ip.addRequired('t', @(x) isreal(x) && isscalar(x) && ~isnan(x))
    ip.addParamValue('rsubstream', 0, @(x) isreal(x) && isscalar(x) && isfinite(x) && x>0)
    ip.parse(pt, pn, y, z, t, varargin{:})
end
rsubstream=ip.Results.rsubstream;

if nargout < 4
    % without boostrap
    if model_based_estimator
        rt = rsqr(pt);
        rn = rsqr(pn);
    else
        rt = rsqr(pt, y, z, t);
        rn = rsqr(pn, y, z, t);
    end
else
    % with boostrap
    if model_based_estimator
        if rsubstream == 0
            [rt, bbt] = rsqr(pt);
            [rn, bbn] = rsqr(pn);
        else
            [rt, bbt] = rsqr(pt, 'rsubstream', rsubstream);
            [rn, bbn] = rsqr(pn, 'rsubstream', rsubstream);
        end
    else
        if rsubstream == 0
            [rt, bbt] = rsqr(pt, y, z, t);
            [rn, bbn] = rsqr(pn, y, z, t);
        else
            [rt, bbt] = rsqr(pt, y, z, t, 'rsubstream', rsubstream);
            [rn, bbn] = rsqr(pn, y, z, t, 'rsubstream', rsubstream);
        end
    end
    bb = bbn - bbt;
end

idi = rn - rt;

end


function ch = rocholhFactorise(v);

% ROCHOLHFACTORISE Rank one Cholesky factorise.

% ROCHOL

% Return structure which represents factorisation of I+vv^T

if size(v, 2) ~= 1
  error('v should be a row vector');
end
t = 0;
ch.n = size(v, 1);
ch.v = v;
ch.s = zeros(size(v));
ch.u = zeros(size(v));
t = ch.v(1)*ch.v(1);
ch.s(1) = inf;
ch.u(1) = 1/ch.v(1);
for i = 2:ch.n
  tnew = t + ch.v(i)*ch.v(i);
  ch.s(i) = sqrt(tnew/t);
  ch.u(i) = ch.v(i)/tnew;
  t = tnew;
end


function [U,S,output] = lmlra_minf(T,U0,S0,options)
%LMLRA_MINF LMLRA by unconstrained nonlinear optimization.
%   [U,S,output] = lmlra_minf(T,U0,S0) computes the factor matrices U{1},
%   ..., U{N} and core tensor S belonging to a low multilinear rank
%   approximation of the N-th order tensor T by minimizing
%   0.5*frob(T-lmlragen(U,S))^2. Each term U{r} is a cell array of N factor
%   matrices U{r}{n}, followed by a core tensor U{r}{N+1}. The algorithm is
%   initialized with the factor matrices U0{n} and core tensor S0. The
%   structure output returns additional information:
%
%      output.Name  - The name of the selected algorithm.
%      output.<...> - The output of the selected algorithm.
%
%   lmlra_minf(T,U0,S0,options) may be used to set the following options:
%
%      options.Algorithm =     - The desired optimization method.
%      [{@minf_lbfgsdl}|...
%       @minf_lbfgs|@minf_ncg]
%      options.<...>           - Parameters passed to the selected method,
%                                e.g., options.TolFun, options.TolX and
%                                options.LineSearchOptions. See also help
%                                [options.Algorithm].
%
%   See also lmlra_nls.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.
%   [2] L. Sorber, M. Van Barel, L. De Lathauwer, "Unconstrained
%       optimization of real functions in complex variables," SIAM J. Opt.,
%       Vol. 22, No. 3, 2012, pp. 879-898.

% Forward problem to btd_minf as a one-term BTD.
if nargin < 4, options = struct; end
[U,output] = btd_minf(T,{[U0(:).',S0]},options);
S = U{1}{end};
U = U{1}(1:end-1);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Returns vector of muscle activation forces for a single Lag.Pt.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Fm = give_3_Element_Muscle_Activation(v,LF,LFO,SK,a,b,Fmax,current_time,xPt,xLag)

% current_time: current time in simulation (s)
% xLag: vector of all x-Lagrangian Pts

% 3 ELEMENT HILL MUSCLE MODEL: contractile element (CE), series element
%                              (SE), parallel element (PE)
%
% NOTE:
%       (1) F_tot = F_PE + F_SE; F_CE = F_SE
%       (2) L = L_PE = L_CE + L_SE
%
% Fm = F_PE + F_CE = F_PE + FSE
%    = k_PE*(Lf - LR) + a_f * Fmax * F1(Lf) * F2(Vf)
%    = k_PE*(Lf - LR) + a_f * Fmax *exp( -( (Q-1)/SK )^2 ) * (1/P0)*(b*P0-a*v)/(v+b); Q = LF/LFO
%

% a_f: coefficient that triggers contraction (traveling square or Gaussian wave?)

% Length-Tension Model Parameters (F1)
% Fmax: maximum isometric force produced at the optimum length of the muscle fibers
% LF:   length of the muscle fibers
% LFO:  length at which the muscle fibers exert their maximum tension
% SK:   constant specific for each muscle where SK > 0.28.

% Hill Model (Force-Velocity) Parameters (F2)
% P0:   maximum load w/ NO contraction
% a:
% b:
% v:    velocity of muscle expansion/contraction

% Length Tension Model Parameters %
Q = LF/LFO;
F1 = exp( - ( (Q-1)/SK )^2 );

% Hill Model %
P0 = Fmax;   %Same as Fmax
F2 = (1/P0)*(b*P0-a*v)/(v+b);

% Get Activation Coefficient %
af_Val = give_Traveling_Triggering_Coefficient(current_time,xLag,xPt);

% Actually Compute Muscle Force %
for i=1:length(xLag)
   Fm = af_Val*Fmax*F1*F2;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: Returns value of Activation Trigger at specific time
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function af_Val = give_Traveling_Triggering_Coefficient(current_time,xLag,xPt)

% current_time: current_time in simulation
% xLag:         x-Lagrangian pts (both bottom and top of tube)
% xPt: x-Pt of interest

t = current_time;               % current time
freq = 10;                      % frequency of traveling wave down tube
Ltube = xLag(end/2) - xLag(1);  % length of heart tube
buff = 0.25;                    % percent-buff on each end of heart tube
L_AR = Ltube*(1-2*buff);        % length of the actual activation region of tube
SqWidth = L_AR/10;              % width of the square traveling activation wave
v = (L_AR-SqWidth) * freq;      % traveling wave velocity
k = 2*pi*freq/v;                % Wave-number for traveling wave

t = rem(t,1/freq);              % Gives remainder after "modular arithmetic" ("fmod" in C++)

xL = xLag(1) + (buff)*Ltube;
xR = xLag(1) + (buff)*Ltube + L_AR;
    x = xPt;
    if ( ( x >= xL ) && ( x <= xR ) )
        af_Val = 0.75*( sin( 1/(L_AR)*( 2*pi*freq*t + k*x) - pi/2 ) +1 );
    else
        af_Val = 0.0;
    end

xL = xLag(1) + (buff)*Ltube + (v*t);
xR = xLag(1) + (buff)*Ltube + SqWidth + (v*t);
xM = (xL+xR)/2;
c = SqWidth/1.5;
    x = xPt;
    if ( ( x >= xL ) && ( x <= xR ) )
        af_Val = 1;                         % Traveling Square Wave
        %af_Val = exp( -(x-xM)^2 / (2*c)^2 ); % Traveling Gaussian Wave
    else
        af_Val = 0.0;
    end


function [elem,HB,newHB,tree]= uniformcoarsen3(elem,HB)
%% UNIFORMCOARSEN3 uniform coarsening in 3-D
%
% [elem,HB,newHB] = uniformcarsen3(elem,HB) remove all good-to-coarsen nodes.
% It is mainly used to get multilevel decomposition in multigrid methods.
% The input matrix elem stands for the fine mesh and the output one for the
% coarse mesh, whose nodal indices are shifted and thus different with the
% fine mesh.
%
% Unlike the two dimensional case, an additional matrix HB is introduced.
% The local index of HB is 2 -- 1 -- 3, i.e., HB(:,1) is the middle point
% of the edge formed by HB(:,2:3). HB(:,4) is used to store the generation
% of the node HB(:,1).
%
% In the output, newHB(:,1) are all removed nodes and newHB(:,2:3) are two
% neighboring nodes whose indices are in the coarse mesh.
%
% See also: coarsen3, bisect3, uniformcoarsen, MGP1, MGP2, MG3P1, MG3P2
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Find good-to-coarsen nodes
N = max(elem(:));
NT = size(elem,1);
generation = zeros(N,1);
generation(HB(:,1)) = HB(:,4);
valence = accumarray(elem(:),ones(4*NT,1),[N 1]);
valenceNew = accumarray(elem(:,4),ones(NT,1), [N 1]); % for newest nodes only
isGoodNode = (valence == valenceNew) & (generation > 0);
if ~any(isGoodNode)
    newHB = []; tree = [];
    return
else
    nGoodNode = sum(isGoodNode);
    newHB = zeros(nGoodNode,3);
    newHB(:,1) = find(isGoodNode);
    newHB(:,2) = HB(newHB(:,1),2);
    newHB(:,3) = HB(newHB(:,1),3);
end

%% Remove good-to-coarsen nodes
t = find(isGoodNode(elem(:,4)));     % elements containing good nodes
leftNode = HB(elem(t,4),2);          % element-wise left node of good nodes
idx = (elem(t,1)==leftNode) | (elem(t,2)==leftNode) | (elem(t,3)==leftNode);
tl = t(idx);
tr = t(~idx);
Nr = length(tl); % number of refined elements
tree = zeros(Nr,3);
% sort tl and tr by the common vertices such that tl and tr matches
if nargout == 4
    leftNode = HB(elem(tl,4),2);
    rightNode = HB(elem(tr,4),3);
    temptl = zeros(Nr,2);
    idx = (elem(tl,1) == leftNode);
    temptl(idx,:) = elem(tl(idx),[2 3]);
    idx = (elem(tl,2) == leftNode);
    temptl(idx,:) = elem(tl(idx),[1 3]);
    idx = (elem(tl,3) == leftNode);
    temptl(idx,:) = elem(tl(idx),[1 2]);
    temptl = sort(temptl,2);
    temptr = zeros(Nr,2);
    idx = (elem(tr,1) == rightNode);
    temptr(idx,:) = elem(tr(idx),[2 3]);
    idx = (elem(tr,2) == rightNode);
    temptr(idx,:) = elem(tr(idx),[1 3]);
    idx = (elem(tr,3) == rightNode);
    temptr(idx,:) = elem(tr(idx),[1 2]);
    temptr = sort(temptr,2);
    [temptl, Il] = sortrows([temptl elem(tl,4)]);
    [temptr, Ir] = sortrows([temptr elem(tr,4)]);
    tl = tl(Il);
    tr = tr(Ir);
    tree(:,3) = tr;
    tree(:,1) = tl;
    tree(:,2) = tl;
end
% coarsen tl to t
elem(tl,4) = HB(elem(tl,4),3);       % replace new node by right node

%% Sort element nodes by generations
% the newest node is the node with maxmum generation
if (length(tl)==1)
    idx = max(transpose(generation(elem(t1,:))));
else
    [tempvar,idx] = max(generation(elem(tl,:)),[],2);  %#ok<*ASGLU>
end
elem(tl((idx==1)),1:4) = elem(tl((idx==1)),[2 4 3 1]);
elem(tl((idx==2)),1:4) = elem(tl((idx==2)),[3 4 1 2]);
elem(tl((idx==3)),1:4) = elem(tl((idx==3)),[4 2 1 3]);

%% Clean and shift index
elem(tr,:) = [];
inCoarse = true(NT,1);
inCoarse(tr) = false;
elemidxMap = zeros(NT,1);
elemidxMap(inCoarse) = 1:size(elem,1);
if nargout == 4
    tree(:,1) = elemidxMap(tree(:,1));
end
HB(isGoodNode,:) = [];
indexMap = zeros(N,1);
indexMap(~isGoodNode)= 1:N-nGoodNode;
elem = indexMap(elem);
HB(:,1:3) = indexMap(HB(:,1:3));
newHB(:,2:3) = indexMap(newHB(:,2:3));


function [V, converged, i] = newtonpf_S_cart(Ybus, Sbus, V0, ref, pv, pq, mpopt)
%NEWTONPF_S_CART  Solves power flow using full Newton's method (power/cartesian)
%   [V, CONVERGED, I] = NEWTONPF_S_CART(YBUS, SBUS, V0, REF, PV, PQ, MPOPT)
%
%   Solves for bus voltages using a full Newton-Raphson method, using nodal
%   power balance equations and cartesian coordinate representation of
%   voltages, given the following inputs:
%       YBUS  - full system admittance matrix (for all buses)
%       SBUS  - handle to function that returns the complex bus power
%               injection vector (for all buses), given the bus voltage
%               magnitude vector (for all buses)
%       V0    - initial vector of complex bus voltages
%       REF   - bus index of reference bus (voltage ang reference & gen slack)
%       PV    - vector of bus indices for PV buses
%       PQ    - vector of bus indices for PQ buses
%       MPOPT - (optional) MATPOWER option struct, used to set the
%               termination tolerance, maximum number of iterations, and
%               output options (see MPOPTION for details).
%
%   The bus voltage vector contains the set point for generator
%   (including ref bus) buses, and the reference angle of the swing
%   bus, as well as an initial guess for remaining magnitudes and
%   angles.
%
%   Returns the final complex voltages, a flag which indicates whether it
%   converged or not, and the number of iterations performed.
%
%   See also RUNPF, NEWTONPF, NEWTONPF_I_POLAR, NEWTONPF_I_CART.

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

%% default arguments
if nargin < 7
    mpopt = mpoption;
end

%% options
tol         = mpopt.pf.tol;
max_it      = mpopt.pf.nr.max_it;
lin_solver  = mpopt.pf.nr.lin_solver;

%% initialize
converged = 0;
i = 0;
V = V0;
Vm = abs(V);
Vmpv = Vm(pv);

%% set up indexing for updating V
npv = length(pv);
npq = length(pq);
j1 = 1;         j2 = npq;           %% j1:j2 - Vr of pq buses
j3 = j2 + 1;    j4 = j2 + npv;      %% j3:j4 - Vr of pv buses
j5 = j4 + 1;    j6 = j4 + npq;      %% j5:j6 - Vi of pq buses
j7 = j6 + 1;    j8 = j6 + npv;      %% j7:j8 - Vi of pv buses

%% evaluate F(x0)
mis = V .* conj(Ybus * V) - Sbus(Vm);
F = [   real(mis([pq; pv]));
        imag(mis(pq));
        V(pv) .* conj(V(pv)) - Vmpv.^2  ];

%% check tolerance
normF = norm(F, inf);
if mpopt.verbose > 1
    fprintf('\n it    max P & Q mismatch (p.u.)');
    fprintf('\n----  ---------------------------');
    fprintf('\n%3d        %10.3e', i, normF);
end
if normF < tol
    converged = 1;
    if mpopt.verbose > 1
        fprintf('\nConverged!\n');
    end
end

%% attempt to pick fastest linear solver, if not specified
if isempty(lin_solver)
    nx = length(F);
    if nx <= 10 || have_feature('octave')
        lin_solver = '\';       %% default \ operator
    else    %% MATLAB and nx > 10 or Octave and nx > 2000
        lin_solver = 'LU3';     %% LU decomp with 3 output args, AMD ordering
    end
end

%% do Newton iterations
while (~converged && i < max_it)
    %% update iteration counter
    i = i + 1;

    %% evaluate Jacobian
    [dSbus_dVr, dSbus_dVi] = dSbus_dV(Ybus, V, 1);
    dV2_dVr = sparse(1:npv, npq+(1:npv), 2*real(V(pv)), npv, npv+npq);
    dV2_dVi = sparse(1:npv, npq+(1:npv), 2*imag(V(pv)), npv, npv+npq);

    %% handling of derivatives for voltage dependent loads
    %% (not yet implemented) goes here

    j11 = real(dSbus_dVr([pq; pv], [pq; pv]));
    j12 = real(dSbus_dVi([pq; pv], [pq; pv]));
    j21 = imag(dSbus_dVr(pq, [pq; pv]));
    j22 = imag(dSbus_dVi(pq, [pq; pv]));
    j31 = dV2_dVr;
    j32 = dV2_dVi;

    J = [   j11 j12;
            j21 j22;
            j31 j32;    ];

    %% compute update step
    dx = mplinsolve(J, -F, lin_solver);

    %% update voltage
    if npv
        V(pv) = V(pv) + dx(j3:j4) + 1j * dx(j7:j8);
    end
    if npq
        V(pq) = V(pq) + dx(j1:j2) + 1j * dx(j5:j6);
    end

    %% evalute F(x)
    mis = V .* conj(Ybus * V) - Sbus(Vm);
    F = [   real(mis([pq; pv]));
            imag(mis(pq));
            V(pv) .* conj(V(pv)) - Vmpv.^2  ];

    %% check for convergence
    normF = norm(F, inf);
    if mpopt.verbose > 1
        fprintf('\n%3d        %10.3e', i, normF);
    end
    if normF < tol
        converged = 1;
        if mpopt.verbose
            fprintf('\nNewton''s method power flow (power balance, cartesian) converged in %d iterations.\n', i);
        end
    end
end

if mpopt.verbose
    if ~converged
        fprintf('\nNewton''s method power flow (power balance, cartesian) did not converge in %d iterations.\n', i);
    end
end


function [Jul1,Jul2]=UTC2TCG(Jul1,Jul2)
%%UTC2TCG  Convert from universal coordinated time (UTC) given as a
%          two-part pseudo-Julian date to geocentric coordinate time (TCG),
%          represented as a two-part Julian date.
%
%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.
%
%OUTPUTS: Jul1, Jul2 The time as a Julian date in TCG.
%
%The UTC date is only pseudo-Julian, because there is not a fixed number
%of seconds in a Julian day. The convention used in the IAU standard is
%that the Julian day matches the UTC day regardless of whether the UTC day
%is 86399, 86400 or 86401 SI seconds (depending on the presence of leap
%seconds).
%
%UTC began at 1960 January 1.0 (JD 2436934.5) and this function should not
%be called with an earlier date.
%
%This just calls a number of intermediate conversion functions out of the
%International Astronomical Union's (IAU) Standard's of Fundamental
%Astronomy library.
%
%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.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=UTC2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2TT(Jul1,Jul2);
[Jul1,Jul2]=TT2TCG(Jul1,Jul2);
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.


function mgdata = mg_ns_diff(Ap,domain,flowsol,qmethod,outbnd)
%mg_ns_diff   GMG for diffusion problem (Navier-Stokes)
%   mgdata = mg_ns_diff(Ap,domain,flowsol,qmethod,outbnd)
%   input
%          Ap           top level discrete diffusion operator
%          domain       domain information, 1 for cavity, 3 for step
%          flowsol      current velocity solution iterate
%          qmethod      discretization method, 0 for Q1-Q1, 2 for Q2-Q1
%          outbnd       location of outflow boundary (for step)
%   output
%          mgdata       structure containing GMG data at all levels
%             matrix    discrete diffusion operators
%             prolong   grid transfer operators
%             smoother  structure containing smoothing operators
%             nc        grid parameter identifying finest level
%
%   IFISS function: HCE; 18 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

if domain==1,      % Cavity
   np = sqrt(length(Ap));
   nc = log2(np-1);
   grid_type = 1;

% Smoothing strategy
   sweeps=1; smooth=2; stype=2;  % Means one (sweeps)-directional line (stype) GS (smooth)

% Top level
% Note (19 April 2004):  Fine level matrix created here is not pinned down
   [x,y,xy,mv,mp,bound] = mg_cavity_domain(nc+1,grid_type);
   [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
   mgdata(nc).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
   if qmethod==2,
      [x,y,xy,mv,mp,bound] = mg_cavity_domain(nc,grid_type);
      [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
      mgdata(nc).prolong = mg_prolong(2^nc,2^nc,x,y);
   else % qmethod=0
      mgdata(nc).prolong=mg_prolong(2^nc,2^nc,x,y);
   end
   mgdata(nc).smoother = mg_ns_smooth(mgdata(nc).matrix,sweeps,smooth,stype);
   mgdata(nc).nc = nc;  % Not used, saved to be consistent with domain=3

   % Loop over remaining levels
   for level = nc-1:-1:2;
      if qmethod==2,
         %%% construct matrix
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         %%% construct prolongation operator
         [x,y,xy,mv,mp,bound] = mg_cavity_domain(level,grid_type);
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).prolong = mg_prolong(2^level,2^level,x,y);
      else % qmethod=0
         [x,y,xy,mv,mp,bound] = mg_cavity_domain(level+1,grid_type);
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         mgdata(level).prolong=mg_prolong(2^level,2^level,x,y);
      end
      mgdata(level).smoother = mg_ns_smooth(mgdata(level).matrix,sweeps,smooth,stype);
      mgdata(level).nc = nc;
   end

elseif domain==3,     % Step
   nu = length(flowsol)/2;
   xi = (-(outbnd+3)+sqrt((outbnd+3)^2+4*(2*outbnd+1)*(nu-1))) ...
          / (2*(2*outbnd+1));
   nc = log2(2*xi);

% Smoothing strategy
   sweeps=1; smooth=3; stype=1; % Means one (sweeps)-directional point (stype) ILU (smooth)

% top level
   [x,y,xy,mv,mp,bound] = mg_step_domain(nc,outbnd);
   mgdata(nc).matrix = Ap;
   if qmethod==2,
      x=x(1:2:end)'; y=y(1:2:end)'; mgdata(nc).prolong=mg_ns_prolong_step(nc-1,x,y,outbnd);
   else % qmethod=0
      x=x'; y=y';
      mgdata(nc).prolong=mg_ns_prolong_step(nc,x,y,outbnd);
   end
   mgdata(nc).smoother = mg_ns_smooth(mgdata(nc).matrix,sweeps,smooth,stype);
   mgdata(nc).nc = nc;

%%% loop over remaining levels
   for level = nc-1:-1:2;
      if qmethod==2,
         [x,y,xy,mv,mp,bound] = mg_step_domain(level,outbnd);
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         if level>2,
            x=x(1:2:end)'; y=y(1:2:end)';
            mgdata(level).prolong=mg_ns_prolong_step(level-1,x,y,outbnd);
         end
      else % qmethod=0
         [x,y,xy,xyp] = mg_diff_q2q1grid(x,y,xy,mv,mp,bound);
         mgdata(level).matrix = mg_apsetup_q1(xy,xyp,mv,mp,domain);
         [x,y,xy,mv,mp,bound] = mg_step_domain(level,outbnd);
         x=x'; y=y';
         mgdata(level).prolong = mg_ns_prolong_step(level,x,y,outbnd);
      end
      mgdata(level).smoother = mg_ns_smooth(mgdata(level).matrix,sweeps,smooth,stype);
      mgdata(level).nc = nc;
   end
else
   error('Error, mg_ns_diff_data is defined only for cavity and step\n');
end


function [ position, velocity, acceleration ] = interpolate( s, nrl, target, t )
% This function differentiates and interpolates a set of Chebyshev coefficients to give position and velocity.
%
% void interpolate (double *buf, double *t, long int ncf, long int na,
%
%                   double *position, double *velocity)
%
% ------------------------------------------------------------------------
%
%    PURPOSE:
%       This function differentiates and interpolates a set of
%       Chebyshev coefficients to give position and velocity.
%
%    REFERENCES:
%       Standish, E.M. and Newhall, X X (1988). "The JPL Export
%          Planetary Ephemeris"; JPL document dated 17 June 1988.
%
%    INPUT
%    ARGUMENTS:
%       *buf (double)
%          Array of Chebyshev coefficients of position.
%       *t (double)
%          t[0] is fractional time interval covered by coefficients at
%          which interpolation is desired (0 <= t[0] <= 1).
%          t[1] is length of whole interval in input time units.
%       ncf (long int)
%          Number of coefficients per component.
%       na (long int)
%          Number of sets of coefficients in full array
%          (i.e., number of sub-intervals in full interval).
%
%    OUTPUT
%    ARGUMENTS:
%       *position (double)
%          Position array of requested object.
%       *velocity (double)
%          Velocity array of requested object.
%
%    RETURNED
%    VALUE:
%       None.
%
%    GLOBALS
%    USED:
%       NP                eph_manager.h
%       NV                eph_manager.h
%       PC                eph_manager.h
%       VC                eph_manager.h
%       TWOT              eph_manager.h
%
%    FUNCTIONS
%    CALLED:
%       fmod              math.h
%
%    VER./DATE/
%    PROGRAMMER:
%       V1.0/03-93/WTH (USNO/AA): Convert FORTRAN to C.
%       V1.1/07-93/WTH (USNO/AA): Update to C standards.
%       V1.2/07-98/WTH (USNO/AA): Modify to make position and velocity
%                                 two distinct vector arrays.
%       V1.3/11-07/WKP (USNO/AA): Updated prolog.
%       V1.4/12-07/WKP (USNO/AA): Changed ncf and na arguments from short
%                                 int to long int.
%       V1.5/10-10/WKP (USNO/AA): Renamed function to lowercase to
%                                 comply with coding standards.
%
%    NOTES:
%       None.
%
% ------------------------------------------------------------------------

  persistent PC VC AC NP TWOT;
  if isempty(PC)
    PC = zeros(1,15); % ncf <= 15
    VC = zeros(1,15); % ncf <= 15
    AC = zeros(1,15); % ncf <= 15
    TWOT = 0.0;
    PC(1) = 1.0; % T_0(x) = 1
    PC(2) = 0.0; % T_1(x) = x
    VC(1) = 0.0; % T'_0(x) = 0
    VC(2) = 1.0; % T'_1(x) = 1
    AC(1) = 0.0; % T"_0(x) = 0
    AC(2) = 0.0; % T"_1(x) = 0
    AC(3) = 4.0; % T"_2(x) = 4
    NP = 2;
  end
  if target == Ephem.TTState
    NDIM = 1;
  elseif target == Ephem.NutationsState
    NDIM = 2;
  else
    NDIM = 3;
  end

  buf = s.IPT(1,target) + nrl*s.indexInc + s.indexOffset;
  ncf = s.IPT(2,target);
  na  = s.IPT(3,target);
  table = s.scan;

  position = zeros(1,NDIM); % row vector
  velocity = zeros(1,NDIM); % row vector
  acceleration = zeros(1,NDIM); % row vector
  if ncf <= 0
    if s.output > 0
      fprintf(s.output,'ERROR: interpolate DE%s. No coefficients available for target %d\n', ...
        s.de_number,target);
    end
    return;
  end
  %   Get correct sub-interval number for this set of coefficients and
  %   then get normalized Chebyshev time within that subinterval.
  dna = double(na);
  temp = dna * double(t);

  %   'tc' is the normalized Chebyshev time (-1 <= tc <= 1).

  tc = 2.0 * mod(temp,1.0) - 1.0;

  %   Check to see whether Chebyshev time has changed, and compute new
  %   polynomial values if it has.  (The element PC[1] is the value of
  %   t1[tc] and hence contains the value of 'tc' on the previous call.)

  if tc ~= PC(2)
    NP = 3;
    TWOT = tc + tc;
    PC(2) = tc; % T_1(x) = x
    PC(3) = TWOT * PC(2) - PC(1); % T_2(x) = 2x^2-1
    VC(3) = 2.0 * TWOT; % T'_2(x) = 4x
  end

  %   Be sure that at least 'ncf' polynomials have been evaluated and
  %   are stored in the array 'PC'.
  %   Chebyshev polynomial recurrence relationship PC[i+1] = T_i(tc)

  if NP < ncf
    for i = (NP+1):ncf
      % T_i-1(x) = 2x T_i-2(x) - T_i-3(x)
      PC(i) = TWOT * PC(i-1) - PC(i-2);
      % T'_i-1(x) = 2x T'_i-2(x) + 2 T_i-2(x) - T'_i-3(x)
      VC(i) = TWOT * VC(i-1) + PC(i-1) + PC(i-1) - VC(i-2);
      % T"_i-1(x) = 2x T"_i-2(x) + 4 T'_i-2(x) - T"_i-3(x)
      AC(i) = TWOT * AC(i-1) + 4*VC(i-1) - AC(i-2);
    end
    NP = ncf;
  end

  vfac = 2.0 * dna;
  %   Interpolate to get position for each component.

  k = buf + int32(floor(temp)) * (NDIM * ncf);
  if k + NDIM*ncf > length(table)
    if s.output > 0
      fprintf(s.output,'interpolate: past end of coefficients by %d/%d\n',...
        k + NDIM*ncf-length(table),length(table));
    end
    return;
  end
  % p = sum( a_i * T_i(x) )
  % v = sum( a_i * T'_i(x) )*2*n
  % a = sum( a_i * T"_i(x) )*4*n^2
  for i = 1:NDIM
    p = 0.0;
    v = 0.0;
    a = 0.0;
    for j = ncf:-1:1
      p = p + PC(j) * table(k + j);
      v = v + VC(j) * table(k + j);
      a = a + AC(j) * table(k + j);
    end
    position(i) = p;
    velocity(i) = v*vfac;
    acceleration(i) = a*vfac*vfac;
    k = k + ncf;
  end

end


function [rc] = calc_ratechange(a,time_as,step,mintime, maxtime,timestep)
    % function [rc] = calc_ratechange(a,time_as,step,mintime,maxtime,timestep);
    % ----------------------------------------------------------------
    % Determines ratechanges within aftershock sequences
    %
    % Input parameters:
    %   a           earthquake catalog
    %   time_as     delay times (days)
    %   step        number of quakes to determine forecast period
    %   mintime     tart time
    %   maxtime     maximal time at which Omori parameters are calculated
    %   timestep    Timesteps between learning periods
    %
    % Output parameters:
    %   rc      Matrix containing: time, absdiff, sigma, numreal, nummod,
    %           pval, pvalstd, cval, cvalstd, kval, kvalstd, t_forecast, fMc
    %
    % Info: Version for ZMAP, original function by ratechange.m by S. Neukomm May 27, 2002
    % J. Woessner
    % last update: 09.07.03

    if min(time_as) >= 1
        rc = [];
        return
    end

    fMc0 = mcwithtime(a,time_as,7);

    rc = [];
    time = mintime;
    while time <= maxtime

        % determination of magnitude of completeness
        if time < 7
            fMc = mcwithtime(a,time_as,time);
        else
            fMc = fMc0;
        end

        % estimation of Omori parameters
        l = time_as <= time & a.Magnitude >= fMc;
        [pval, pvalstd, cval, cvalstd, kval, kvalstd, loopout] = bruteboot(time_as(l));

        if isnan(pval) == 0
            nummod = step; % forecasted number of aftershocks
            if pval == 1
                pv = 1-10^(-6);
            else
                pv = pval;
            end

            t_forecast = (-nummod*(pv-1)/kval+(time+cval)^(1-pv))^(1/(1-pv))-cval; % forecast interval
            if isreal(t_forecast) == 1  &&  t_forecast > time
                if t_forecast > max(time_as)
                    return
                end

                l = time_as <= t_forecast & a.Magnitude >= fMc & time_as >= time;
                numreal = sum(l); % observed number of aftershocks
                absdiff = numreal-nummod;

                % calculate uncertainty sigma in forecasted number of aftershocks
                time1 = t_forecast; mpm1 = 1-pv; t1c = time1+cval; t0c = time+cval;
                sigma = (((-t1c^mpm1+t0c^mpm1)/(pv-1)*kvalstd)^2+...
                    (kval/(pv-1)*(-t1c^mpm1*mpm1/t1c+t0c^mpm1*mpm1/t0c)*cvalstd)^2+...
                    (kval/(pv-1)*(t1c^mpm1*log(t1c)+t1c^mpm1/(pv-1)-t0c^mpm1*log(t0c)-t0c^mpm1/(pv-1))*pvalstd)^2)^0.5;

                rc = [rc; time absdiff sigma numreal nummod pval pvalstd cval cvalstd kval kvalstd t_forecast fMc];
            end
        end
        time = time+timestep;
    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   q = solve_spherical_wrist_flexion_n2(robot, q, T, wrist)
%   Solves the inverse kinematic problem for a spherical wrist of the robot
%   Epson Flexion N2
%   robot: robot structure.
%   q: vector containing the values of the joints 1, 2 and 3.
%   T: orientation of the last reference system.
%   wrist: select -1 or 1 for two possible solutions (wrist up, wrist down)
%
%	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 = solve_spherical_wrist_flexion_n2(robot, q, T, wrist, method)

switch method

     %algebraic solution
    case 'algebraic'
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);

        Q=inv(T23)*inv(T12)*inv(T01)*T;

        %detect the degenerate case when q(5)=0, this leads to zeros
        % in Q13, Q23, Q31 and Q32 and Q33=1
        thresh=1e-12;
        %detect if q(5)==0
        % this happens when cos(q5) in the matrix Q is close to 1
        if abs(Q(3,3)-1)>thresh
            %normal solution
            if wrist==1 %wrist up
                q(4)=atan2(-Q(2,3),-Q(1,3))+pi;
                q(6)=atan2(-Q(3,2),Q(3,1));
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            else %wrist down
                q(4)=atan2(-Q(2,3),-Q(1,3))-pi+pi;
                q(6)=atan2(-Q(3,2),Q(3,1))+pi;
                %q(5)=atan2(-Q(3,2)/sin(q(6)),Q(3,3));
            end
            if abs(cos(q(6)+q(4)))>thresh
                cq5=(Q(1,1)+Q(2,2))/cos(q(4)+q(6))-1;
            end
            if abs(sin(q(6)+q(4)))>thresh
                cq5=(-Q(1,2)+Q(2,1))/sin(q(4)+q(6))-1;
            end
            if abs(sin(q(6)))>thresh
                sq5=-Q(3,2)/sin(q(6));
            end
            if abs(cos(q(6)))>thresh
                sq5=Q(3,1)/cos(q(6));
            end
            q(5)=atan2(sq5,cq5)+pi;

        else %degenerate solution, in this case, q4 cannot be determined,
             % so q(4)=0 is assigned
            if wrist==1 %wrist up
                q(4)=0;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))-pi/2;
            else %wrist down
                q(4)=-pi;
                q(5)=0;
                q(6)=atan2(-Q(1,2)+Q(2,1),Q(1,1)+Q(2,2))+pi-pi/2;
            end

        end

       %geometric solution
    case 'geometric'
        % T is the noa matrix defining the position/orientation of the end
        % effector's reference system
        vx6=T(1:3,1);
        vz5=T(1:3,3); % The vector a z6=T(1:3,3) is coincident with z5

        % Obtain the position and orientation of the system 3
        % using the already computed joints q1, q2 and q3
        T01=dh(robot, q, 1);
        T12=dh(robot, q, 2);
        T23=dh(robot, q, 3);
        T03=T01*T12*T23;

        vx3=T03(1:3,1);
        vy3=T03(1:3,2);
        vz3=T03(1:3,3);

        % find z4 normal to the plane formed by z3 and a
        vz4=cross(vz3, vz5);	% end effector's vector a: T(1:3,3)

        % in case of degenerate solution,
        % when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
        if norm(vz4) <= 0.000001
            if wrist == 1 %wrist up
                q(4)=0;
            else
                q(4)=-pi; %wrist down
            end
        else
            %this is the normal and most frequent solution
            cosq4=wrist*dot(-vy3,vz4);
            sinq4=wrist*dot(vx3,vz4);
            q(4)=atan2(sinq4, cosq4)+pi;
        end
        %propagate the value of q(4) to compute the system 4
        T34=dh(robot, q, 4);
        T04=T03*T34;
        vx4=T04(1:3,1);
        vy4=T04(1:3,2);

        % solve for q5
        cosq5=dot(vy4,vz5);
        sinq5=dot(-vx4,vz5);
        q(5)=atan2(sinq5, cosq5)+pi;

        %propagate now q(5) to compute T05
        T45=dh(robot, q, 5);
        T05=T04*T45;
        vx5=T05(1:3,1);
        vy5=T05(1:3,2);

        % solve for q6
        cosq6=dot(vx6,vx5);
        sinq6=dot(vx6,vy5);
        q(6)=atan2(sinq6, cosq6)-pi/2;

    otherwise
        disp('no method specified in solve_spherical_wrist');
end


function M = getmassmatvec3(elem2dof,volume,Dlambda,elemType,K)
%% GETMASSMATVEC3 get a mass matrix of finite element spaces in 3D
%
% M = GETMASSMATVEC3(elem2dof,volume,Dlambda,elemType,K) get mass matrix of
% the finite element space specified by elemType. The coefficient K is
% piecewise constant.
%
% The elemType can be:
%
% - "RT0": The lowest order Raviart-Thomas element
% - "ND0": The lowest order Nedelec element
% - "ND1": The full linear Nedelec element
% - "ND2m": The incomplete quadratic Nedelec element (P\Lambda_2^{-})
% - "ND2": The full quadratic Nedelec element (P\Lambda_2)
%
% Note that for H(div) element, the coefficient is Mij/K while for H(curl)
% element, it is Mij*K.
%
% See also Maxwell, Maxwell1, Maxwell2, Poisson3RT0
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('K','var'), K = []; end
NT = size(elem2dof,1);

if contains(elemType, 'ND')
    DiDj = zeros(NT,4,4);
    for i = 1:4
        for j = i:4
            DiDj(:,i,j) = dot(Dlambda(:,:,i),Dlambda(:,:,j),2);
            DiDj(:,j,i) = DiDj(:,i,j);
        end
    end
end

%% ND0: the lowest order edge element
if strcmp(elemType,'ND0')
    NE = double(max(elem2dof(:)));
    locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
    ii = zeros(21*NT,1); jj = zeros(21*NT,1); sM = zeros(21*NT,1);
    index = 0;
    for i = 1:6
        for j = i:6
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            % mass matrix
            % locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               - (1+(i1==j2))*DiDj(:,i2,j1) ...
                               - (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),NE,NE);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),NE,NE);
    M = M + MU + MU';
end

%% ND1: The full linear Nedelec element
if strcmp(elemType,'ND1')
    NE = double(max(elem2dof(:)));
    Ndof = 2*NE;
    locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
    ii = zeros(21*NT,1); jj = zeros(21*NT,1);
    sMphi = zeros(21*NT,1); sMpsi = zeros(21*NT,1);
    index = 0;
    % two block diagonal matrix for phi and psi
    for i = 1:6
        for j = i:6
            % (phi_i,phi_j)
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            % mass matrix
            % locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               - (1+(i1==j2))*DiDj(:,i2,j1) ...
                               - (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            sMphi(index+1:index+NT) = Mij;
            % (psi_i,psi_j)
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                               + (1+(i1==j2))*DiDj(:,i2,j1) ...
                               + (1+(i2==j1))*DiDj(:,i1,j2) ...
                               + (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            sMpsi(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse([ii(diagIdx); ii(diagIdx)+NE], [jj(diagIdx); jj(diagIdx)+NE],...
               [sMphi(diagIdx); sMpsi(diagIdx)],Ndof,Ndof);
    MU = sparse([ii(upperIdx); ii(upperIdx)+NE], [jj(upperIdx); jj(upperIdx)+NE],...
               [sMphi(upperIdx); sMpsi(upperIdx)],Ndof,Ndof);
    M = M + MU + MU';
    % off-diagonal matrix (psi, phi)
    ii = zeros(36*NT,1); jj = zeros(36*NT,1); ss = zeros(36*NT,1);
    index = 0;
    for i = 1:6
        for j = 1:6
            % local to global index map and its sign
            i1 = locEdge(i,1); i2 = locEdge(i,2);
            j1 = locEdge(j,1); j2 = locEdge(j,2);
            Mij = 1/20*volume.*( (1+(i1==j1))*DiDj(:,i2,j2) ...
                       - (1+(i1==j2))*DiDj(:,i2,j1) ...
                       + (1+(i2==j1))*DiDj(:,i1,j2) ...
                       - (1+(i2==j2))*DiDj(:,i1,j1));
            if ~isempty(K)
                Mij = Mij.*K;
            end
            ii(index+1:index+NT) = double(elem2dof(:,i))+NE;
            jj(index+1:index+NT) = double(elem2dof(:,j));
            ss(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end
    ML = sparse(ii,jj,ss,Ndof,Ndof);
    M  = M  + ML + ML';
end

%% ND2m: the incomplete quadratic Nedelec element
% see Maxwell2
% elem2dof = [elem2edge elem2edge+NE elem2face+2*NE elem2face+2*NE+NF];

if strcmp(elemType,'ND2m')
    NE = double(max(elem2dof(:, 1:6), [], "all"));
    NF = double(max(elem2dof(:, 12:16)-2*NE, [], "all"));
    Ndof = 2*(NE+NF);
    locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % Nd0
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % grad(P2)
               3 2 4; 3 1 4; 2 1 4; 2 1 3; ...
               4 2 3; 4 1 3; 4 1 2; 3 1 2]; % face bubbles
    ii = zeros(210*NT,1); jj = zeros(210*NT,1);
    sM = zeros(210*NT,1);
    index = 0;
    for i = 1:20
        for j = i:20
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            i1 = locBasesIdx(i,1); i2 = locBasesIdx(i,2); i3 = locBasesIdx(i,3);
            j1 = locBasesIdx(j,1); j2 = locBasesIdx(j,2); j3 = locBasesIdx(j,3);
            Mij = zeros(NT,1);
            if (i<=6) && (j<=6)
                % block 1: (phi_i,phi_j)
                Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                    - (1+(i1==j2))*DiDj(:,i2,j1) ...
                    - (1+(i2==j1))*DiDj(:,i1,j2) ...
                    + (1+(i2==j2))*DiDj(:,i1,j1));
            end
            if (i<=6) && (7<=j) && (j<=12)
                % block 2: (psi_j,phi_i)
                Mij = 1/20*( (1+(j1==i1))*DiDj(:,j2,i2) ...
                    - (1+(j1==i2))*DiDj(:,j2,i1) ...
                    + (1+(j2==i1))*DiDj(:,j1,i2) ...
                    - (1+(j2==i2))*DiDj(:,j1,i1));

            end
            if (7<=i) && (i<=12) && (7<=j) && (j<=12)
                % block 3: (psi_j,psi_i)
                Mij = 1/20*((1+(i1==j1))*DiDj(:,i2,j2) ...
                    + (1+(i1==j2))*DiDj(:,i2,j1) ...
                    + (1+(i2==j1))*DiDj(:,i1,j2) ...
                    + (1+(i2==j2))*DiDj(:,i1,j1));
            end
            if (i<=6) && (j>12)
                % block 4: (chi_j,phi_i)
                Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                    -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                    -intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                    +intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
            end
            if (7<=i) && (i<=12) && (j>12)
                % block 5: (chi_j,psi_i)
                Mij = intlambda([j1,i1,j2],3)*DiDj(:,i2,j3) ...
                    -intlambda([j1,i1,j3],3)*DiDj(:,i2,j2) ...
                    +intlambda([j1,i2,j2],3)*DiDj(:,i1,j3) ...
                    -intlambda([j1,i2,j3],3)*DiDj(:,i1,j2);
            end
            if (i>12) && (j>12)
                % block 6: (chi_j,chi_i)
                Mij = intlambda([i1,j1,i2,j2],3)*DiDj(:,i3,j3) ...
                    -intlambda([i1,j1,i2,j3],3)*DiDj(:,i3,j2) ...
                    -intlambda([i1,j1,i3,j2],3)*DiDj(:,i2,j3) ...
                    +intlambda([i1,j1,i3,j3],3)*DiDj(:,i2,j2);
            end
            if ~isempty(K)
                Mij = Mij.*K;
            end
            Mij = Mij.*volume;
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end

    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
    M = M + MU + MU';
end

%% ND2: the complete quadratic Nedelec element
% elem2dof = [elem2edge elem2edge+NE elem2edge+2*NE ...
%             elem2face+3*NE elem2face+3*NE+NF elem2face+3*NE+2*NF]
% index 1 to 18: edge basis
% index 19 to 30: face basis
% phi_1 = lambda_i^2 \nabla lambda_j
% phi_2 = lambda_j^2 \nabla lambda_i
% phi_3 = lambda_i lambda_j \nabla (lambda_i - lambda_j)
% psi_{F_{ijk}; 1,2,3} = ijk \in cyc {\lambda_i \lambda_j \nabla \lambda_k}

if strcmp(elemType,'ND2')
    NE = double(max(elem2dof(:, 1:6), [], "all"));
    NF = double(max(elem2dof(:, 19:22)-3*NE, [], "all"));
    Ndof = 3*(NE+NF);
    locBasesIdx = [1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % phi_1
               2 1 0; 3 1 0; 4 1 0; 3 2 0; 4 2 0; 4 3 0; ... % psi_2
               1 2 0; 1 3 0; 1 4 0; 2 3 0; 2 4 0; 3 4 0; ... % psi_3
               2 3 4; 3 4 2; 4 2 3; ... % face 1
               1 3 4; 3 4 1; 4 1 3; ... % face 2
               1 2 4; 2 4 1; 4 1 2; ... % face 3
               1 2 3; 2 3 1; 3 1 2]; % face 4
    ii = zeros(465*NT,1); jj = zeros(465*NT,1);
    sM = zeros(465*NT,1);
    index = 0;
    for i = 1:30
        for j = i:30
            ii(index+1:index+NT) = double(elem2dof(:,i));
            jj(index+1:index+NT) = double(elem2dof(:,j));
            i1 = locBasesIdx(i,1); i2 = locBasesIdx(i,2); i3 = locBasesIdx(i,3);
            j1 = locBasesIdx(j,1); j2 = locBasesIdx(j,2); j3 = locBasesIdx(j,3);
            Mij = zeros(NT,1);
            if (i<=12) && (j<=12)
                % block 1: (phi_{1,2},phi_{1,2})
                Mij = intlambda([i1,i1,j1,j1],3)*DiDj(:,i2,j2);
            end
            if (i<=12) && (12<j) && (j<=18)
                % block 2: (phi_{1,2},phi_{3})
                Mij = intlambda([i1,i1,j1,j2],3)*...
                    (DiDj(:,i2,j1) - DiDj(:,i2,j2));
            end
            if (12<i) && (i<=18) && (12<j) && (j<=18)
                % block 3: (phi_{3},phi_{3})
                Mij = intlambda([i1,i2,j1,j2],3)* ...
                    (DiDj(:,i1,j1) + DiDj(:,i2,j2) ...
                    -DiDj(:,i1,j2) - DiDj(:,i2,j1));
            end
            if (i<=12) && (j>18)
                % block 4: (phi_{1,2}, psi_F)
                Mij = intlambda([i1,i1,j1,j2],3)*DiDj(:,i2,j3);
            end
            if (12<i) && (i<=18) && (j>18)
                % block 5: (phi_{3}, psi_F)
                Mij = intlambda([i1,i2,j1,j2],3)*...
                    (DiDj(:,i1,j3) - DiDj(:,i2,j3));
            end
            if (i>18) && (j>18)
                % block 6: (psi_F,psi_F)
                Mij = intlambda([i1,i2,j1,j2],3)*DiDj(:,i3,j3);
            end
            if ~isempty(K)
                Mij = Mij.*K;
            end
            Mij = Mij.*volume;
            sM(index+1:index+NT) = Mij;
            index = index + NT;
        end
    end

    diagIdx = (ii == jj);   upperIdx = ~diagIdx;
    M = sparse(ii(diagIdx),jj(diagIdx),sM(diagIdx),Ndof,Ndof);
    MU = sparse(ii(upperIdx),jj(upperIdx),sM(upperIdx),Ndof,Ndof);
    M = M + MU + MU';
end

%% RT0: the lowest order face element
if strcmp(elemType,'RT0')
    NF = double(max(elem2dof(:)));
    localFace = [2 3 4; 1 3 4; 1 2 4; 1 2 3]; % ascend ordering
    M = sparse(NF,NF);
    for i = 1:4
        for j = i:4
            % local to global index map
            ii = double(elem2dof(:,i));
            jj = double(elem2dof(:,j));
            i1 = localFace(i,1); i2 = localFace(i,2); i3 = localFace(i,3);
            j1 = localFace(j,1); j2 = localFace(j,2); j3 = localFace(j,3);
            % computation of mass matrix --- (phi_i, phi_j)
            Mij = 1/5*volume.*( ...
                  (1+(i1==j1))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i1==j2))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i1==j3))*dot(mycross(Dlambda(:,:,i2),Dlambda(:,:,i3),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i2==j1))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i2==j2))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i2==j3))*dot(mycross(Dlambda(:,:,i3),Dlambda(:,:,i1),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2)...
                 +(1+(i3==j1))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j2),Dlambda(:,:,j3),2),2)...
                 +(1+(i3==j2))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j3),Dlambda(:,:,j1),2),2)...
                 +(1+(i3==j3))*dot(mycross(Dlambda(:,:,i1),Dlambda(:,:,i2),2), ...
                                   mycross(Dlambda(:,:,j1),Dlambda(:,:,j2),2),2));
            if ~isempty(K)
                Mij = Mij./K;
            end
            if (j==i)
                M = M + sparse(ii,jj,Mij,NF,NF);
            else
                M = M + sparse([ii;jj],[jj;ii],[Mij; Mij],NF,NF);
            end
        end
    end
end


classdef FDV < ALGORITHM
% <multi/many> <real/integer> <large/none>
% Fuzzy decision variable framework with various internal optimizers
% Rate      --- 0.8 --- Fuzzy evolution rate. Default = 0.8
% Acc       --- 0.4 --- Step acceleration. Default = 0.4
% optimizer ---   5 --- Internal optimisation algorithm. 1 = NSGA-II, 2 = NSGA-III, 3 = MOEA/D, 4 = CMOPSO, 5 = LMOCSO
% type      ---   1 --- The type of aggregation function for MOEA/D

%------------------------------- Reference --------------------------------
% X. Yang, J. Zou, S. Yang, J. Zheng, and Y. Liu, A fuzzy decision
% variables framework for large-scale multiobjective optimization, IEEE
% Transactions on Evolutionary Computation, 2021.
%--------------------------------------------------------------------------

%  Copyright (C) 2021 Xu Yang
%  Xu Yang <xuyang.busyxu@qq.com> or <xuyang369369@gmail.com>

    methods
        function main(Algorithm,Problem)
            %% Set the default parameters
            [Rate,Acc,optimizer,type] = Algorithm.ParameterSet(0.8,0.4,5,1);

            %% NSGAII
            if optimizer==1
                % Generate random population
                Population = Problem.Initialization();
                [~,FrontNo,CrowdDis] = EnvironmentalSelection_NSGAII(Population,Problem.N);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    MatingPool = TournamentSelection(2,Problem.N,FrontNo,-CrowdDis);
                    OffDec     = OperatorGA(Problem,Population(MatingPool).decs);
                    %% FDV
                    if Problem.FE/Problem.maxFE <= Rate
                        Offspring = FDVOperator(Problem,Rate,Acc,OffDec);
                    else
                        Offspring = Problem.Evaluation(OffDec);
                    end
                    %%
                    [Population,FrontNo,CrowdDis] = EnvironmentalSelection_NSGAII([Population,Offspring],Problem.N);
                end
            end

            %% NSGAIII
            if optimizer==2
                % Generate the reference points and random population
                [Z,Problem.N] = UniformPoint(Problem.N,Problem.M);
                Population    = Problem.Initialization();
                Zmin          = min(Population(all(Population.cons<=0,2)).objs,[],1);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    MatingPool = TournamentSelection(2,Problem.N,sum(max(0,Population.cons),2));
                    OffDec     = OperatorGA(Problem,Population(MatingPool).decs);
                    %% FDV
                    if Problem.FE/Problem.maxFE <= Rate
                        Offspring = FDVOperator(Problem,Rate,Acc,OffDec);
                    else
                        Offspring = Problem.Evaluation(OffDec);
                    end
                    Zmin       = min([Zmin;Offspring(all(Offspring.cons<=0,2)).objs],[],1);
                    Population = EnvironmentalSelection_NSGAIII([Population,Offspring],Problem.N,Z,Zmin);
                end
            end

            %% MOEA/D
            if optimizer==3
                % Generate the weight vectors
                [W,Problem.N] = UniformPoint(Problem.N,Problem.M);
                T = ceil(Problem.N/10);

                % Detect the neighbours of each solution
                B = pdist2(W,W);
                [~,B] = sort(B,2);
                B = B(:,1:T);

                % Generate random population
                Population = Problem.Initialization();
                Z = min(Population.objs,[],1);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    % For each solution
                    for i = 1 : Problem.N
                        % Choose the parents
                        P = B(i,randperm(size(B,2)));

                        % Generate an offspring
                        OffDec = OperatorGAhalf(Problem,Population(P(1:2)).decs);
                        %% FDV
                        if Problem.FE/Problem.maxFE <= Rate
                            Offspring = FDVOperator(Problem,Rate,Acc,OffDec);
                        else
                            Offspring = Problem.Evaluation(OffDec);
                        end

                        % Update the ideal point
                        Z = min(Z,Offspring.obj);

                        % Update the neighbours
                        switch type
                            case 1
                                % PBI approach
                                normW   = sqrt(sum(W(P,:).^2,2));
                                normP   = sqrt(sum((Population(P).objs-repmat(Z,T,1)).^2,2));
                                normO   = sqrt(sum((Offspring.obj-Z).^2,2));
                                CosineP = sum((Population(P).objs-repmat(Z,T,1)).*W(P,:),2)./normW./normP;
                                CosineO = sum(repmat(Offspring.obj-Z,T,1).*W(P,:),2)./normW./normO;
                                g_old   = normP.*CosineP + 5*normP.*sqrt(1-CosineP.^2);
                                g_new   = normO.*CosineO + 5*normO.*sqrt(1-CosineO.^2);
                            case 2
                                % Tchebycheff approach
                                g_old = max(abs(Population(P).objs-repmat(Z,T,1)).*W(P,:),[],2);
                                g_new = max(repmat(abs(Offspring.obj-Z),T,1).*W(P,:),[],2);
                            case 3
                                % Tchebycheff approach with normalization
                                Zmax  = max(Population.objs,[],1);
                                g_old = max(abs(Population(P).objs-repmat(Z,T,1))./repmat(Zmax-Z,T,1).*W(P,:),[],2);
                                g_new = max(repmat(abs(Offspring.obj-Z)./(Zmax-Z),T,1).*W(P,:),[],2);
                            case 4
                                % Modified Tchebycheff approach
                                g_old = max(abs(Population(P).objs-repmat(Z,T,1))./W(P,:),[],2);
                                g_new = max(repmat(abs(Offspring.obj-Z),T,1)./W(P,:),[],2);
                        end
                        Population(P(g_old>=g_new)) = Offspring;
                    end
                end
            end

            %% CMOPSO
            if optimizer == 4
                % Generate random population
                Population = Problem.Initialization();

                % Optimization
                while Algorithm.NotTerminated(Population)
                    [OffDec,OffVel] = Operator_CMOPSO(Problem,Population);
                    %% FDV
                    if Problem.FE/Problem.maxFE <= Rate
                        Offspring = FDVOperator(Rate,Acc,OffDec,OffVel);
                    else
                        Offspring = Problem.Evaluation(OffDec,OffVel);
                    end
                    Population = EnvironmentalSelection_CMOPSO([Population,Offspring],Problem.N);
                end
            end

            %% LMOCSO
            if optimizer == 5
                 % Generate random population
                [V,Problem.N] = UniformPoint(Problem.N,Problem.M);
                Population    = Problem.Initialization();
                Population    = EnvironmentalSelection_LMOCSO(Population,V,(Problem.FE/Problem.maxFE)^2);

                % Optimization
                while Algorithm.NotTerminated(Population)
                    % Calculate the fitness by shift-based density   SDE (the shift-based density estimation strategy)
                    PopObj = Population.objs;
                    N      = size(PopObj,1);
                    fmax   = max(PopObj,[],1);
                    fmin   = min(PopObj,[],1);
                    PopObj = (PopObj-repmat(fmin,N,1))./repmat(fmax-fmin,N,1);
                    Dis    = inf(N);
                    for i = 1 : N
                        SPopObj = max(PopObj,repmat(PopObj(i,:),N,1));
                        for j = [1:i-1,i+1:N]
                            Dis(i,j) = norm(PopObj(i,:)-SPopObj(j,:));
                        end
                    end
                    Fitness = min(Dis,[],2);

                    if length(Population) >= 2
                        Rank = randperm(length(Population),floor(length(Population)/2)*2);
                    else
                        Rank = [1,1];
                    end
                    Loser  = Rank(1:end/2);
                    Winner = Rank(end/2+1:end);
                    Change = Fitness(Loser) >= Fitness(Winner);
                    Temp   = Winner(Change);
                    Winner(Change) = Loser(Change);
                    Loser(Change)  = Temp;

                    [OffDec,OffVel] = Operator_LMOCSO(Problem,Population(Loser),Population(Winner),Rate);
                    %% FDV
                    iter = Problem.FE/Problem.maxFE;
                    if iter <= Rate
                        Offspring = FDVOperator(Problem,Rate,Acc,OffDec,OffVel);
                    else
                        Offspring = Problem.Evaluation(OffDec,OffVel);
                    end
                    Population = EnvironmentalSelection_LMOCSO([Population,Offspring],V,(Problem.FE/Problem.maxFE)^2);
                end
            end
        end
    end
end


%SE2 Representation of 2D rigid-body motion
%
% This subclasss of RTBPose is an object that represents rigid-body motion in 2D.
% Internally this is a 3x3 homogeneous transformation matrix (3x3) belonging to
% the group SE(2).
%
% Constructor methods::
%  SE2          general constructor
%  SE2.exp      exponentiate an se(2) matrix
%  SE2.rand     random transformation
%  new          new SE2 object
%
% Display and print methods::
%  animate          ^graphically animate coordinate frame for pose
%  display          ^print the pose in human readable matrix form
%  plot             ^graphically display coordinate frame for pose
%  print            ^print the pose in single line format
%
% Group operations::
%  *            ^mtimes: multiplication (group operator, transform point)
%  /            ^mrdivide: multiply by inverse
%  ^            ^mpower: exponentiate (integer only):
%  inv          inverse
%  prod         ^product of elements
%
% Methods::
%  det          determinant of matrix component
%  eig          eigenvalues of matrix component
%  log          logarithm of rotation matrix
%  inv          inverse
%  simplify*    apply symbolic simplication to all elements
%  interp       interpolate between poses
%  theta        rotation angle
%
% Information and test methods::
%  dim          ^returns 2
%  isSE         ^returns true
%  issym        ^test if rotation matrix has symbolic elements
%  SE2.isa      test if matrix is SE(2)
%
% Conversion methods::
%  char*         convert to human readable matrix as a string
%  SE2.convert   convert SE2 object or SE(2) matrix to SE2 object
%  double        convert to rotation matrix
%  R             convert to rotation matrix
%  SE3           convert to SE3 object with zero translation
%  SO2           convert rotational part to SO2 object
%  T             convert to homogeneous transformation matrix
%  Twist         convert to Twist object
%  t             get.t: convert to translation column vector
%
% Compatibility methods::
%  isrot2       ^returns false
%  ishomog2     ^returns true
%  tr2rt        ^convert to rotation matrix and translation vector
%  t2r          ^convert to rotation matrix
%  transl2      ^translation as a row vector
%  trprint2     ^print single line representation
%  trplot2      ^plot coordinate frame
%  tranimate2   ^animate coordinate frame
%
% ^ inherited from RTBPose class.
%
% See also SO2, SE3, RTBPose.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

% Copyright (C) 1993-2017, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB 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.
%
% RTB 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 RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

classdef SE2 < SO2

    properties (Dependent = true)
        t
    end

    methods

        function obj = SE2(varargin)
            %SE2.SE2 Construct an SE(2) object
            %
            % Constructs an SE(2) pose object that contains a 3x3 homogeneous transformation
            % matrix.
            %
            % T = SE2() is the identity element, a null motion.
            %
            % T = SE2(X, Y) is an object representing pure translation defined by X and Y.
            %
            % T = SE2(XY) is an object representing pure translation defined by XY
            % (2x1). If XY (Nx2) returns an array of SE2 objects, corresponding to
            % the rows of XY.
            %
            % T = SE2(X, Y, THETA) is an object representing translation, X and Y, and
            % rotation, angle THETA.
            %
            % T = SE2(XY, THETA) is an object representing translation, XY (2x1), and
            % rotation, angle THETA.
            %
            % T = SE2(XYT) is an object representing translation, XYT(1) and XYT(2),
            % and rotation angle XYT(3). If XYT (Nx3) returns an array of SE2 objects, corresponding to
            % the rows of XYT.
            %
            % T = SE2(T) is an object representing translation and rotation defined by
            % the SE(2) homogeneous transformation matrix T (3x3).  If T (3x3xN) returns an
            % array (1xN) of SE2 objects, corresponding to the third index of T.
            %
            % T = SE2(R) is an object representing pure rotation defined by the
            % SO(2) rotation matrix R (2x2)
            %
            % T = SE2(R, XY) is an object representing rotation defined by the
            % orthonormal rotation matrix R (2x2) and position given by XY (2x1)
            %
            % T = SE2(T) is a copy of the SE2 object T. If T (Nx1) returns an array of SE2 objects,
            % corresponding to the index of T.
            %
            % Options::
            % 'deg'         Angle is specified in degrees
            %
            % Notes::
            % - Arguments can be symbolic
            % - The form SE2(XY) is ambiguous with SE2(R) if XY has 2 rows, the second form is assumed.
            % - The form SE2(XYT) is ambiguous with SE2(T) if XYT has 3 rows, the second form is assumed.
            % - R and T are checked to be valid SO(2) or SE(2) matrices.

            opt.deg = false;

            [opt,args] = tb_optparse(opt, varargin);

            if opt.deg
                scale = pi/180.0;
            else
                scale = 1;
            end

            % if any of the arguments is symbolic the result will be symbolic
            if any( cellfun(@(x) isa(x, 'sym'), args) )
                obj.data = sym(obj.data);
            end

            obj.data = eye(3,3);

            switch length(args)
                case 0
                    % null motion
                    return
                case 1
                    % 1 argument
                    a = args{1};

                    if isvec(a, 2)
                        % (t)
                        obj.data = [ 1 0 a(1); 0 1 a(2); 0 0 1];

                    elseif isvec(a, 3)
                        % ([x y th])
                        a = a(:);
                        obj.data(1:2,1:2) = rot2(a(3)*scale);
                        obj.t = a(1:2);

                    elseif SO2.isa(a)
                        % (R)
                        obj.data(1:2,1:2) = a;

                    elseif SE2.isa(a)
                        % (T)
                        for i=1:size(a, 3)
                            obj(i).data = a(:,:,i);
                        end
                    elseif isa(a, 'SE2')
                        % (SE2)
                        for i=1:length(a)
                            obj(i).data = a(i).data;
                        end

                    elseif any( numcols(a) == [2 3] )
                        for i=1:numrows(a)
                            obj(i) = SE2(a(i,:));
                        end
                        return
                    else
                        error('SMTB:SE2:badarg', 'unknown arguments');
                    end

                case 2
                    % 2 arguments
                    a = args{1}; b = args{2};
                    if isscalar(a) && isscalar(b)
                        % (x,y)
                        obj.data = [ 1 0 a; 0 1 b; 0 0 1];
                    elseif isvec(a,2) && isscalar(b)
                        % ([x y], th)
                        obj.data = [ rot2(b*scale) a(:); 0 0 1];
                    elseif SO2.isa(a) && isvec(b,2)
                        % (R, t)
                        obj.data = [a b(:); 0 0 1];
                    else
                        error('SMTB:SE3:badarg', 'unknown arguments');
                    end

                case 3
                    % 3 arguments
                    a = args{1}; b = args{2}; c = args{3};
                    if isscalar(a) && isscalar(b) && isscalar(c)
                        % (x, y, th)
                        obj.data = [ rot2(c*scale) [a b]'; 0 0 1];
                    else
                        error('SMTB:SE3:badarg', 'unknown arguments');
                    end
                otherwise
                    error('SMTB:SE3:badarg', 'unknown arguments');

            end

            % add the last row if required
%             if numrows(obj.data) == 2
%                 obj.data = [obj.data; 0 0 1];
%             end
            assert(all(size(obj(1).data) == [3 3]), 'SMTB:SE2:SE2', 'created wrong size data element');
            %% HACK
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %  GET AND SET
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function t = get.t(obj)
            %SE2.t  Get translational component
            %
            % P.t is a column vector (2x1) representing the translational component of
            % the rigid-body motion described by the SE2 object P.
            %
            % Notes::
            % - If P is a vector the result is a MATLAB comma separated list, in this
            %   case use P.transl().
            %
            % See also SE2.transl.
            t = obj.data(1:2,3);
        end

        function o = set.t(obj, t)
            %SE2.t  Set translational component
            %
            % P.t = TV sets the translational component of the rigid-body motion
            % described by the SE2 object P to TV (2x1).
            %
            % Notes::
            % - TV can be a row or column vector.
            % - If TV contains a symbolic value then the entire matrix becomes
            %   symbolic.

            if isa(t, 'sym') && ~isa(obj.data, 'sym')
                obj.data = sym(obj.data);
            end
            obj.data(1:2,3) = t;
            o = obj;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%  conversion methods
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function v = xyt(obj)
            %SE2.xyt  Extract configuration
            %
            % XYT = P.xyt() is a column vector (3x1) comprising the minimum three
            % configuration parameters of this rigid-body motion: translation (x,y)
            % and rotation theta.

            % TODO VECTORISE
            v = obj.t;
            v(3) = atan2(obj.data(2,1), obj.data(1,1));
        end

        function [tx,ty] = transl(obj)
            %SE2.t  Get translational component
            %
            % TV = P.transl() is a row vector (1x2) representing the translational component of
            % the rigid-body motion described by the SE2 object P.  If P is a vector of
            % objects (1xN) then TV (Nx2) will have one row per object element.

            if nargout == 1 || nargout == 0
                tx = [obj.t]';
            else
                t = obj.t;
                tx = t(1);
                ty = t(2);
            end
        end

        function T = T(obj)
            %SE2.T  Get homogeneous transformation matrix
            %
            % T = P.T() is the homogeneous transformation matrix (3x3) associated with the
            % SE2 object P, and has zero translational component.  If P is a vector
            % (1xN) then T (3x3xN) is a stack of homogeneous transformation matrices, with the third
            % dimension corresponding to the index of P.
            %
            % See also SO2.T.
            for i=1:length(obj)
                T(:,:,i) = obj(i).data;
            end
        end

        function t = SE3(obj)
            %SE2.SE3 Lift to 3D
            %
            % Q = P.SE3() is an SE3 object formed by lifting the rigid-body motion
            % described by the SE2 object P from 2D to 3D.  The rotation is about the
            % z-axis, and the translation is within the xy-plane.
            %
            % See also SE3.
            t = SE3();
            t.data(1:2,1:2) = obj.data(1:2,1:2);
            t.data(1:2,4) = obj.data(1:2,3);
        end

        function out = SO2(obj)
            %SE2.SO2  Extract SO(2) rotation
            %
            % Q = SO2(P) is an SO2 object that represents the rotational component of
            % the SE2 rigid-body motion.
            %
            % See also SE2.R.

            out = SO2( obj.R );
        end

        function tw = Twist(obj)
            %SE2.Twist  Convert to Twist object
            %
            % TW = P.Twist() is the equivalent Twist object.  The elements of the twist are the unique
            % elements of the Lie algebra of the SE2 object P.
            %
            % See also SE2.log, Twist.
            tw = Twist( obj.log );
        end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%  SE(2) OPERATIONS
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function it = inv(obj)
            %SE2.inv  Inverse of SE2 object
            %
            % Q = inv(P) is the inverse of the SE2 object P.
            %
            % Notes::
            %  - This is formed explicitly, no matrix inverse required.
            %  - This is a group operator: input and output in the SE(2) group.
            %  - P*Q will be the identity group element (zero motion, identity matrix).

            it = SE2( obj.R', -obj.R'*obj.t);
        end

        function S = log(obj)
            %SE2.log  Lie algebra
            %
            % se2 = P.log() is the Lie algebra corresponding to the SE2 object P. It is
            % an augmented skew-symmetric matrix (3x3).
            %
            % See also SE2.Twist, logm, skewa, vexa.
            S = logm(obj.data);
        end

        function Ti = interp(obj1, varargin)
            %SE2.interp Interpolate between SO2 objects
            %
            % P1.interp(P2, s) is an SE2 object which is an interpolation
            % between poses represented by SE2 objects P1 and P2.  s varies from 0
            % (P1) to 1 (P2). If s is a vector (1xN) then the result will be a vector
            % of SE2 objects.
            %
            % Notes::
            % - It is an error if S is outside the interval 0 to 1.
            %
            % See also SO2.angle.

            if isa(varargin{1}, 'SE2')
                % interp(SE2, SE2, s)  interpolate between given values
                obj2 = varargin{1};
                varargin = varargin(2:end);
                try
                    Ti = SE2( trinterp2(obj1.T, obj2.T, varargin{:}) );
                catch me
                    switch me.identifier
                        case 'SMTB:trinterp2:badarg'
                            throw( MException('SMTB:SE2:interp:badarg', 'value of S outside interval [0,1]') );
                        otherwise
                            rethrow(me);
                    end
                end
            else
                % interp(SE2, s)  interpolate between null and given value
                try
                    Ti = SE2( trinterp2( obj1.T, varargin{:}) );
                catch me
                    switch me.identifier
                        case 'SMTB:trinterp2:badarg'
                            throw( MException('SMTB:SE2:interp:badarg', 'value of S outside interval [0,1]') );
                        otherwise
                            rethrow(me);
                    end
                end
            end

        end

        function print(obj, varargin)
            for T=obj
                theta = atan2(T.data(2,1), T.data(1,1)) * 180/pi;
                fprintf('t = (%.4g, %.4g), theta = %.4g deg\n', T.t, theta);
            end
        end

        function n = new(obj, varargin)
            %SE2.new  Construct a new object of the same type
            %
            % P2 = P.new(X) creates a new object of the same type as P, by invoking the SE2 constructor on the matrix
            % X (3x3).
            %
            % P2 = P.new() as above but defines a null motion.
            %
            % Notes::
            %  - Serves as a dynamic constructor.
            %  - This method is polymorphic across all RTBPose derived classes, and
            %    allows easy creation of a new object of the same class as an existing
            %    one without needing to explicitly determine its type.
            %
            % See also SE3.new, SO3.new, SO2.new.

            n = SE2(varargin{:});
        end

    end

    methods (Static)
        % Static factory methods for constructors from exotic representations

        function obj = exp(s)
            %SE2.exp  Construct SE2 from Lie algebra
            %
            % SE2.exp(SIGMA) is the SE2 rigid-body motion corresponding to the se(2)
            % Lie algebra element SIGMA (3x3).
            %
            % SE3.exp(TW) as above but the Lie algebra is represented
            % as a twist vector TW (1x1).
            %
            % Notes::
            %  - TW is the non-zero elements of X.
            %
            % Reference::
            % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p25-31.
            %
            %
            % See also trexp2, skewa.

            obj = SE2( trexp2(s) );
        end

        function T = convert(tr)
            %SE2.check  Convert to SE2
            %
            % Q = SE2.convert(X) is an SE2 object equivalent to X where X is either
            % an SE2 object, or an SE(2) homogeneous transformation matrix (3x3).
            if isa(tr, 'SE2')
                T = tr;
            elseif SE2.isa(tr)
                T = SE2(tr);
            else
                error('expecting an SE2 or 3x3 matrix');
            end
        end

        function h = isa(tr, rtest)
            %SE2.ISA Test if matrix is SE(2)
            %
            % SE2.isa(T) is true (1) if the argument T is of dimension 3x3 or 3x3xN, else
            % false (0).
            %
            % SE2.isa(T, true) as above, but also checks the validity of the rotation
            % sub-matrix.
            %
            % Notes::
            %  - This is a class method.
            %  - The first form is a fast, but incomplete, test for a transform in SE(3).
            %  - There is ambiguity in the dimensions of SE2 and SO3 in matrix form.
            %
            % See also SO3.ISA, SE2.ISA, SO2.ISA, ishomog2.

            d = size(tr);
            if ndims(tr) >= 2
                h =  all(d(1:2) == [3 3]);

                if h && nargin > 1 && ~isa(tr, 'sym')
                    h = SO3.isa( tr(1:2,1:2) );
                    h = h && all(tr(4,:) == [0 0 0 1]);  % test the bottom row
                end
            else
                h = false;
            end
        end

        function T = rand()
            %SE2.rand Construct a random SE(2) object
            %
            % SE2.rand() is an SE2 object with a uniform random translation and a
            % uniform random orientation.  Random numbers are in the interval [-1 1]
            % and rotations in the interval [-pi pi].
            %
            % See also RAND.
            T = SE2(rand(1,3)*diag([2 2 2*pi]) + [-1 -1 -pi]);
        end
    end
end