function cmap = cmapExtendedHSV(numGrays,numColors,range)
%
% cmap = cmapExtendedHSV([numGrays=128],[numColors=96],[range=query user])
%
%The map created here are hsv maps where all of the colors
% can be placed in a subsection of the full color map.  In this way, the
% full range of colors spans less than 2pi, like in the double color map.
% Rather than compressing by a complete factor of 2, like the double color
% map, the compression factor can be a bit smaller.
%
%   There are numGrays gray scale entries.  They occupy the first part of the cmap, 1:numGrays
%   There are numColors hsv colors.  They fill the map entries following the gray,
%   from (numGrays+1):numGrays+numColors
%   When the range is 1, the hsv map is hsv(numColors) and we insert it
%   into the cmap.
%   When the range is 1.5, we compute tmp = hsv(numColors/1.5) and we
%   create [tmp,tmp(1:needed)] to fill up numColors entries.
%
% Examples:
%   cmap = cmapExtendedHSV(128,96,1.1);
%   cmap = cmapExtendedHSV(128,96);    -- 128 gray levels, 96 color levels,
%          query use for compression
%   cmap = cmapExtendedHSV;
%   cmap = cmapExtendedHSV(128,96,2);  -- Same as hsvDoubleCmap
%

function vw = cmapExtended(vw,range)
%
%   vw = cmapExtended(vw,range)
%
% Author: AAB, BW
% Purpose:
%   Compress the current color map to smaller range and add gray at the end.
%
%  vw = FLAT{1};

numGrays = viewGet(vw,'cmapcurnumgrays');
numColors = viewGet(vw,'cmapcurnumcolors');
mpColors = viewGet(vw,'cmapcurrent');

if ieNotDefined('vw'),  error('Must pass in the view.'); end
if ieNotDefined('range'), range = 1.2; end

if (range < 1) | (range > 2),  error('Range must be betweem 1 and 2.'); end

desiredColors = round(numColors/range);
extraColors = numColors - desiredColors;
newMap = [round(interp1(mpColors',(1:range:numColors)')')];
newMap = [newMap,ones(3,extraColors)*128];

vw = viewSet(vw,'cmapcurrent',newMap);

% cmap = zeros(numGrays+numColors,3);
%
% % If you want the map symmetric at the boundary, you should do this.
% % We could trap range == 2 and do it then ... which would be backwards
% % compatible?
% % cmap = [gray(numGrays); hsvMap; flipud(hsvMap(1:hsvColorsExtra,:))];
% cmap = [hsvMap; hsvMap(1:hsvColorsExtra,:)];
% shiftSize = round(hsvColorsExtra/2);
% hsvMap = circshift(cmap,shiftSize);
%
% cmap = [gray(numGrays); cmap];

return;

%----------------------------------------
function range = readRange

prompt={'Enter compression range for the hsv map (2 = double color map)'};
def={'1.2'};
dlgTitle='Color map compression factor';
lineNo=1;
range=inputdlg(prompt,dlgTitle,lineNo,def);
range = str2num(range{1});

return;


function c8_sqrt_test ( )

%*****************************************************************************80
%
%% C8_SQRT_TEST tests C8_SQRT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2013
%
%  Author:
%
%    John Burkardt
%
  test_num = 10;
  seed = 123456678;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'C8_SQRT_TEST\n' );
  fprintf ( 1, '  C8_SQRT computes the principal square root of a C8.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, ...
    '            C1=random            C2=C8_SQRT(C1)         C3=C2*C2\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

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

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

  end

  return
end


clear all; close all; clc

addpath('apm')

s = 'http://byu.apmonitor.com';
b = 'mhe';

% Connect to Arduino
tclab;

% Run time in minutes
run_time = 10.0;

% Number of cycles (1 cycle per 3 seconds)
loops = round(20*run_time);

% milli-volts input
Q1 = zeros(1,loops);
Q2 = zeros(1,loops);
Q1(3:end) = 100.0;
Q1(50:end) = 0.0;
Q1(100:end) = 80.0;

Q2(25:end) = 60.0;
Q2(75:end) = 100.0;
Q2(125:end) = 25.0;

for i = 130:180
    if mod(i,10)==0
        Q1(i:i+10) = rand(1) * 100;
    end
    if mod(i+5,10)==0
        Q2(i:i+10) = rand(1) * 100;
    end
end

% Temperature (degC)
T1 = ones(1,loops) * T1C(); % measured T
T2 = ones(1,loops) * T2C(); % measured T
T1mhe = ones(1,loops) * T1C(); % measured T
T2mhe = ones(1,loops) * T2C(); % measured T
Umhe = ones(1,loops) * 10.0;
taumhe = ones(1,loops) * 5.0;
amhe1 = ones(1,loops) * 0.01;
amhe2 = ones(1,loops) * 0.0075;
time = zeros(1,loops);

% time
tm = zeros(1,loops);

% moving horizon estimation
mhe_init();

start_time = clock;
prev_time = start_time;

% dynamic plot (note: subplots needs to be declared here first)
figure(1)
subplot(3,1,1)
hold on, grid on
anexp1 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
anpred1 = animatedline('LineStyle','--','Color', 'r','LineWidth', 2);
anexp2 = animatedline('LineStyle','-', 'Color', 'b', 'LineWidth', 2);
anpred2 = animatedline('LineStyle','--','Color', 'g','LineWidth', 2);
ylabel('Temperature \circC')
legend('T_1 Measured', 'T_1 Predicted', ...
    'T_2 Measured', 'T_2 Predicted', ...
    'Location', 'northwest')
title('Temperature Estimation')
subplot(3,1,2)
hold on, grid on
anQ1 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
anQ2 = animatedline('LineStyle','--', 'Color', 'b', 'LineWidth', 2);
ylabel('Power Level Q (%)')
legend('Q_1', 'Q_2', 'Location', 'northwest')
subplot(3,1,3)
hold on, grid on
anU = animatedline('LineStyle','-', 'Color', 'r', 'LineWidth', 2);
antau = animatedline('LineStyle','--', 'Color', 'b', 'LineWidth', 2);
ana1 = animatedline('LineStyle','-', 'Color', 'g', 'LineWidth', 2);
ana2 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
ylabel('Parameters')
legend('U', 'tau', 'a1 x 1000', 'a2 x 1000', 'Location', 'northwest')
xlabel('Time (sec)')

for ii = 1:loops
    % adjust power level
    h1(Q1(ii));
    h2(Q2(ii));

    % Pause Sleep time
    pause_max = 3.0;
    pause_time = pause_max - etime(clock,prev_time);
    if pause_time >= 0.0
        pause(pause_time - 0.01)
    else
        pause(0.01)
    end

    % Record time and change in time
    t = clock;
    dt = etime(t,prev_time);
    if ii>=2
        time(ii) = time(ii-1) + dt;
    end
    prev_time = t;

    % read and record from temperature controller
    T1(ii) = T1C();
    T2(ii) = T2C();

    % Start estimating parameters after 10 cycles (30 sec)
    if ii==10
       apm_option(s,b,'U.STATUS',1);
       apm_option(s,b,'tau.STATUS',1);
       apm_option(s,b,'a1.STATUS',1);
       apm_option(s,b,'a2.STATUS',1);
    end

    % non-linear energy balance
    jj = ii+1;
    params = mhe(T1(ii),T2(ii),Q1(ii),Q2(ii));
    Umhe(jj) = params(1);
    taumhe(jj) = params(2);
    a1(jj) = params(3);
    a2(jj) = params(4);
    T1mhe(jj) = params(5);
    T2mhe(jj) = params(6);

    % plot
    addpoints(anexp1,time(ii),T1(ii))
    addpoints(anpred1,time(ii),T1mhe(ii))
    addpoints(anexp2,time(ii),T2(ii))
    addpoints(anpred2,time(ii),T2mhe(ii))
    addpoints(anQ1,time(ii),Q1(ii))
    addpoints(anQ2,time(ii),Q2(ii))
    addpoints(anU,time(ii),Umhe(ii))
    addpoints(antau,time(ii),taumhe(ii))
    addpoints(ana1,time(ii),1000*a1(ii))
    addpoints(ana2,time(ii),1000*a2(ii))
    drawnow

    % open web-interface
    if ii==20
        apm_web(s,b);
    end
end

h1(0);
h2(0);
disp('Heaters off')
% turn off heater but keep LED on if T > 50
if (T1C() || T2C()) > 50
    led(1)
    disp(['Warning, heater temperature 1 =', num2str(T1C())])
    disp(['Warning, heater temperature 2 =', num2str(T2C())])
else
    led(0)
end

% save txt file with data
data = [time',Q1',Q2',T1',T2'];
csvwrite('data.txt',data);


function [crack_img] = seg2cracks(seg)
%
% [crack_img] = seg2cracks(seg)
%
%  Converts a segmentation image into a crack-coded image.
%

% Bits:
%       |
%       | 1
% 4 ----+---- 2
%       |
%       | 3

% Values:
% tic
UP    = 1;
RIGHT = 2;
DOWN  = 3;
LEFT  = 4;

JUNCTION = 5;

% dx = uint8(seg ~= image_right(seg));
% dy = uint8(seg ~= image_down(seg) );
% crack_img = dx + bitshift(dy,LEFT-1) + ...
%     bitshift(image_right(dy),RIGHT-1) + bitshift(image_down(dx),DOWN-1);

% Removed dependency on image_down and image_right, to make this more
% easily packaged:
dx = uint8(seg ~= seg(:,[2:end end]));
dy = uint8(seg ~= seg([2:end end],:));
crack_img = dx + bitshift(dy,LEFT-1) + ...
    bitshift(dy(:,[2:end end]),RIGHT-1) + bitshift(dx([2:end end],:),DOWN-1);

% Find interior junctions:
junction_map = (crack_img==11 | crack_img==7 | crack_img==14 | ...
    crack_img==13 | crack_img==15);

% Find the junctions along the borders:
junction_map([1 end],:) = junction_map([1 end],:) | bitget(crack_img([1 end],:),UP);
junction_map(:,[1 end]) = junction_map(:,[1 end]) | bitget(crack_img(:,[1 end]),LEFT);

% set the junction bit for all these junctions in the crack_img
crack_img(junction_map) = bitset(crack_img(junction_map), JUNCTION);

% % Also set all borders of the image to be seen as junctions (all bits set
% % == a value of 15) so that we will know to stop in the fragment chaining
% % process later
% crack_img(:,[1 end]) = 15;
% crack_img([1 end],:) = 15;

% fprintf('New method: %.3f seconds\n', toc);
%
% %% Old Method %%
% tic
% [nrows, ncols ] = size(seg);
% crack_img2 = zeros(nrows, ncols, 'uint8');
% % Bits:
% UP    = 1;
% RIGHT = 2;
% DOWN  = 3;
% LEFT  = 4;
%
% index = find(seg ~= image_right(seg));
% crack_img2(index) = bitset(crack_img2(index), UP);
%
% index = find(image_right(seg) ~= image_downright(seg));
% crack_img2(index) = bitset(crack_img2(index), RIGHT);
%
% index = find(image_down(seg) ~= image_downright(seg));
% crack_img2(index) = bitset(crack_img2(index), DOWN);
%
% index = find(seg ~= image_down(seg));
% crack_img2(index) = bitset(crack_img2(index), LEFT);
%
% fprintf('Old method: %.3f seconds\n', toc);
%
% if(all(crack_img(:)==crack_img2(:)))
%     disp('Results agree.')
% end
%


% PLOT2DKM - For a 2-D binary classification problem, plot2dkm plots the data,
%            the margin and error vectors and contours of constant margin
%            for the SVM classifier in memory.
%
% Syntax: plot2dkm
%
% Version 3.22e -- Comments to diehl@alumni.cmu.edu
%

function plot2dkm

% flags for example state
MARGIN    = 1;
ERROR     = 2;
RESERVE   = 3;
UNLEARNED = 4;

% define global variables
global ind;   % cell array containing indices of margin, error, reserve and unlearned vectors
global X;     % matrix of margin, error, reserve and unlearned vectors stored columnwise
global y;     % column vector of class labels (-1/+1) for margin, error, reserve and unlearned vectors

% plot examples with label -1
figure;
indn1 = find(y == -1);
scatter(X(1,indn1),X(2,indn1),40,'b','filled');
hold on;

% plot examples with label +1
ind1 = find(y == 1);
scatter(X(1,ind1),X(2,ind1),40,'r');

% plot margin vectors
scatter(X(1,ind{MARGIN}),X(2,ind{MARGIN}),120,'k');
scatter(X(1,ind{MARGIN}),X(2,ind{MARGIN}),150,'k');
scatter(X(1,ind{MARGIN}),X(2,ind{MARGIN}),200,'k');

% plot error vectors
scatter(X(1,ind{ERROR}),X(2,ind{ERROR}),120,'k');
scatter(X(1,ind{ERROR}),X(2,ind{ERROR}),150,'k');
scatter(X(1,ind{ERROR}),X(2,ind{ERROR}),200,'k');
scatter(X(1,ind{ERROR}),X(2,ind{ERROR}),120,'k','x');
scatter(X(1,ind{ERROR}),X(2,ind{ERROR}),150,'k','x');
scatter(X(1,ind{ERROR}),X(2,ind{ERROR}),200,'k','x');

% draw margin band
xl = xlim;
yl = ylim;
pd = min(xl(2)-xl(1),yl(2)-yl(1))/100;
x_range = xl(1):pd:xl(2);
y_range = yl(1):pd:yl(2);
f = zeros(length(y_range),length(x_range));
i = 1;
for xp = x_range
   j = 1;
   for yp = y_range
      f(j,i) = svmeval([xp ; yp]);
      j = j + 1;
   end;
   i = i + 1;
end;
contour(x_range,y_range,f,[-1 1]);


% Copyright (C) 2013 Quan Wang <wangq10@rpi.edu>,
% Signal Analysis and Machine Perception Laboratory,
% Department of Electrical, Computer, and Systems Engineering,
% Rensselaer Polytechnic Institute, Troy, NY 12180, USA

% this is a demo showing the use of our dynamic time warping package
% we provide both Matlab version and C/MEX version
% the C/MEX version is much faster and highly recommended

clear;clc;close all;

mex dtw_c.c;

a=rand(500,3);
b=rand(520,3);
w=50;

tic;
d1=DTWComputer.mddtw(a,b);
t1=toc;

tic;
d2=dtw_c(a,b,w);
t2=toc;

fprintf('Using Max version: distance=%f, running time=%f\n',d1,t1);
fprintf('Using C/MEX version: distance=%f, running time=%f\n',d2,t2);


% RPCA | MoG-RPCA | Mixture of Gaussians RPCA (Zhao et al. 2014)
% process_video('RPCA', 'MoG-RPCA', 'dataset/demo.avi', 'output/demo_MoG-RPCA.avi');

%{
clear, clc;
load('dataset/trafficdb/traffic_patches.mat');
V = im2double(imgdb{100});
[M,m,n,p] = convert_video3d_to_2d(V);
%}

r = 1;
param.mog_k = 3;
param.lr_init = 'SVD';
param.maxiter = 100;
param.initial_rank = 2*r;
param.tol = 1e-3;

lr_prior.a0 = 1e-6;
lr_prior.b0 = 1e-6;

mog_prior.mu0 = 0;
mog_prior.c0 = 1e-3;
mog_prior.d0 = 1e-3;
mog_prior.alpha0 = 1e-3;
mog_prior.beta0 = 1e-3;

[lr_model, mog_model, r] = mog_rpca(M, param, lr_prior, mog_prior);

L = lr_model.U*lr_model.V';
S = M - L;

%{
show_2dvideo(M,m,n);
show_2dvideo(L,m,n);
show_2dvideo(S,m,n);
show_2dvideo(hard_threshold(S),m,n);
%}


function MatingPool = MatingSelection(PopObj,div)
% The mating selection of GrEA

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

    N = size(PopObj,1);

    %% Calculate the grid location of each solution
    fmax = max(PopObj,[],1);
    fmin = min(PopObj,[],1);
    lb   = fmin-(fmax-fmin)/2/div;
    ub   = fmax+(fmax-fmin)/2/div;
    d    = (ub-lb)/div;
    lb   = repmat(lb,N,1);
    d    = repmat(d,N,1);
    GLoc = floor((PopObj-lb)./d);
    GLoc(isnan(GLoc)) = 0;

    %% Calculate the GD value of each solution
    GD = zeros(N)+inf;
    for i = 1 : N-1
        for j = i+1 : N
            GD(i,j) = sum(abs(GLoc(i,:)-GLoc(j,:)));
            GD(j,i) = GD(i,j);
        end
    end

    %% Calculate the GCD value of each solution
    GD  = max(size(PopObj,2)-GD,0);
    GCD = sum(GD,2);

    %% Binary tournament selection
    Parents1   = randi(N,1,N);
    Parents2   = randi(N,1,N);
    Dominate   = any(PopObj(Parents1,:)<PopObj(Parents2,:),2) - any(PopObj(Parents1,:)>PopObj(Parents2,:),2);
    GDominate  = any(GLoc(Parents1,:)<GLoc(Parents2,:),2) - any(GLoc(Parents1,:)>GLoc(Parents2,:),2);
    MatingPool = [Parents1(Dominate==1 | GDominate==1),...
                  Parents2(Dominate==-1 | GDominate==-1),...
                  Parents1(Dominate==0 & GDominate==0 & GCD(Parents1)<=GCD(Parents2)),...
                  Parents2(Dominate==0 & GDominate==0 & GCD(Parents1)>GCD(Parents2))];
end


function [ W, W1,S, index_mat,IDX_vote,S_tan,Data_PCA ] = createAffMatrix( feats,params,affinity_type )
%% W1 affinity matrix using Euclidean distances and RBF.

switch affinity_type
    case 'local_PCA'
       % [W1,index_mat, S] = RBF_affnity(feats,params);
        [W1,index_mat, S,IDX_vote,S_tan] = RBF_affnity_knn_fast(feats,params);
        %% W2 affinity using local tangent space with local PCA
        [Data_PCA,W2,theta] = locPCA_affnity_fast(feats, IDX_vote, params);
        %index_mat=IDX_vote;
        %% Connecting all the points
        Wc = W1.*W2;
        W = Wc.*Wc';

        %W_large=W_large.*W2;
        %W_large=W_large.*W_large;

    case 'Hillinger_affinity'
        %params.affinity_type='cosine';
      %  [W1, index_mat, S] = affinity.RBF_affnity(feats,params);
        [W1,index_mat, S,IDX_vote,S_tan] = RBF_affnity_knn(feats,params);
        params.affinity_type= 'Hillinger_affinity';

        [W2, W_l]= Hillinger_affinity( feats, IDX_vote,params);
        Wc = W1.*W2;
        W = Wc.*Wc';
    case 'sqeuclidean'
       % [W1,index_mat, S] = affinity.RBF_affnity(feats,params);
      %  [W1,index_mat, S,IDX_vote,S_tan] = affinity.RBF_affnity_knn(feats,params);
        [W1,index_mat, S,IDX_vote,S_tan] = RBF_affnity_knn_fast(feats,params);
        W=W1;
        case 'outliers'
        [W1,index_mat, S,IDX_vote,S_tan] = RBF_affnity_knn_fast(feats,params);
         W=W1;

    case 'cosine'
       % [W1,index_mat, S] = affinity.RBF_affnity(feats,params);
      % [W1,index_mat, S,IDX_vote,S_tan] = affinity.RBF_affnity_knn(feats,params);
       [W1,index_mat, S,IDX_vote,S_tan] = RBF_affnity_knn_fast(feats,params);

        W = W1;
    case 'Tensor_Voting'
        %[W1,index_mat, S] = affinity.RBF_affnity(feats,params);
        [W1,index_mat, S,IDX_vote,S_tan] = RBF_affnity_knn(feats,params);
        %% W2 affinity using the Tensor Voting Graph
        [Normal_Space,W2,theta] = TVG_affinity_knn(feats, IDX_vote, params,S_tan);
        %% Connecting all the points
        Wc = W1.*W2;
        %W=(Wc + Wc')./2;
        W = Wc.*Wc';
end


function [err,PatchHandles] = ShowPlane (EqPlane,Opt)
%
%   [err,PatchHandles] = ShowPlane (EqPlane,Opt)
%
%Purpose:
%   This function displays a plane with equation ax + by + cz +d = 0
%   The plane is defined by one sqaure patch
%   The planes span the current axis limits of the figure.
%
%Input Parameters:
%   EqPlane is a Nx4 matrix containing the equation of the plane containing each
%       triplet in  Triplets. The plane passing by triplet i is speicifed in
%       EqPlane(i,:) the plane would be
%       EqPlane(i,1)x + EqPlane(i,2)y + EqPlane(i,3)z + EqPlane(i,4) = 0
%   Opt is the options structure
%     .Fig is a handle to the figure you want the planes displayed in,
%         default is the current figure.
%     .WriteIV if this string is not empty, the planes that are displayed
%         on the graph are written to an inventor format file
%     .units 'mm' or 'tesscon'. default is tesscon
%       if you specify mm, then the xyz coordinates are transformed to tesscon
%       before writing them out. (*319.7). The idea is to write all inventor
%       files in tesscon units. This option will only be used if WriteIV is not empty.
%     .OvrWrite (0/1) default is 0, flag for overwriting existing .iv file
%
%Output Parameters:
%   err : 0 No Problem
%       : 1 Mucho Problems
%
%   PatchHandles : the handle to the patches displayed on the figure
%
%More Info :
%   see Plane_Equation
%
%
%
%     Author : Ziad Saad
%     Date : Thu Oct 22 20:19:36 CDT 1998

%Define the function name for easy referencing
FuncName = 'ShowPlane';

%initailize return variables
err = 1;

%check on the size of input data
if (nargin == 1),
	Opt.Fig = [];
	Opt.OvrWrite = 0;
	Opt.WriteIV = '';
end

if (~isfield(Opt,'OvrWrite') | isempty(Opt.OvrWrite)), Opt.OvrWrite = 0; end

if (size(EqPlane,2) ~= 4),	err = ErrEval(FuncName,'Err_Bad size for EqPlane');	return;	end

Nplanes = size(EqPlane,1);
Nnodes = 4.* Nplanes;

%pop up a figure
if (~isfield(Opt,'Fig') | isempty(Opt.Fig)),
	Opt.Fig = gcf;
end

figure(Opt.Fig);

%get the axis roperties of the figure
Xlim = get(gca,'Xlim');
Ylim = get(gca,'ylim');
Zlim = get(gca,'Zlim');

inode = 0;
Node = zeros(Nnodes,3);
Pat = zeros(Nplanes,4);
ztmp = zeros(1,4);

for (i=1:1:Nplanes),
	%using the XY limits, find the corrspondign z values
	if (EqPlane(i,3) ~= 0),
		ztmp(1) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(1) - EqPlane(i,2).*Ylim(1)) ./ EqPlane(i,3);
		ztmp(2) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(2) - EqPlane(i,2).*Ylim(1)) ./ EqPlane(i,3);
		ztmp(3) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(2) - EqPlane(i,2).*Ylim(2)) ./ EqPlane(i,3);
		ztmp(4) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(1) - EqPlane(i,2).*Ylim(2)) ./ EqPlane(i,3);
		%form the four points on the plane
		Node(inode+1,:) = [Xlim(1) Ylim(1) ztmp(1)];
		Node(inode+2,:) = [Xlim(2) Ylim(1) ztmp(2)];
		Node(inode+3,:) = [Xlim(2) Ylim(2) ztmp(3)];
		Node(inode+4,:) = [Xlim(1) Ylim(2) ztmp(4)];

	elseif (EqPlane(i,2) ~= 0),
		ytmp(1) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(1) - EqPlane(i,3).*Zlim(1)) ./ EqPlane(i,2);
		ytmp(2) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(2) - EqPlane(i,3).*Zlim(1)) ./ EqPlane(i,2);
		ytmp(3) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(2) - EqPlane(i,3).*Zlim(2)) ./ EqPlane(i,2);
		ytmp(4) = (-EqPlane(i,4) - EqPlane(i,1).*Xlim(1) - EqPlane(i,3).*Zlim(2)) ./ EqPlane(i,2);
		%form the four points on the plane
		Node(inode+1,:) = [Xlim(1) ytmp(1) Zlim(1)];
		Node(inode+2,:) = [Xlim(2) ytmp(2) Zlim(1)];
		Node(inode+3,:) = [Xlim(2) ytmp(3) Zlim(2)];
		Node(inode+4,:) = [Xlim(1) ytmp(4) Zlim(2)];
	elseif (EqPlane(i,1) ~= 0),
		xtmp(1) = (-EqPlane(i,4) - EqPlane(i,2).*Ylim(1) - EqPlane(i,3).*Zlim(1)) ./ EqPlane(i,1);
		xtmp(2) = (-EqPlane(i,4) - EqPlane(i,2).*Ylim(2) - EqPlane(i,3).*Zlim(1)) ./ EqPlane(i,1);
		xtmp(3) = (-EqPlane(i,4) - EqPlane(i,2).*Ylim(2) - EqPlane(i,3).*Zlim(2)) ./ EqPlane(i,1);
		xtmp(4) = (-EqPlane(i,4) - EqPlane(i,2).*Ylim(1) - EqPlane(i,3).*Zlim(2)) ./ EqPlane(i,1);
		%form the four points on the plane
		Node(inode+1,:) = [xtmp(1) Ylim(1) Zlim(1)];
		Node(inode+2,:) = [xtmp(2) Ylim(2) Zlim(1)];
		Node(inode+3,:) = [xtmp(3) Ylim(2) Zlim(2)];
		Node(inode+4,:) = [xtmp(4) Ylim(1) Zlim(2)];
	end

	%verify that all points are on plane for debugging only
	%sum(EqPlane.*[Node(inode+1,:) 1])
	%sum(EqPlane.*[Node(inode+2,:) 1])
	%sum(EqPlane.*[Node(inode+3,:) 1])
	%sum(EqPlane.*[Node(inode+4,:) 1])

	%form the faceset connection, for the patch
	Pat(i,:) = [inode+1 inode+2 inode+3 inode+4];

	inode = inode + 4;

end

%Now display those patches
vc = 0:1./Nnodes:(1-1./Nnodes);
tcolor = [vc' (1-vc)' vc'];

PatchHandles = patch('vertices',Node,'faces',Pat,...
          'FaceVertexCData',tcolor,'FaceColor','flat');

%write to file ?
if (isfield(Opt,'WriteIV') & ~isempty(Opt.WriteIV)),
	fprintf (1,'Saving patches to iv file %s ...\n',Opt.WriteIV);
   Opt.OptIV.BaseCol = tcolor;
   if (~isfield(Opt,'units') | isempty(Opt.units)),	Opt.units = 'tesscon'; end
	Opt.OptIV.units = Opt.units;
	Opt.OptIV.OvrWrite = Opt.OvrWrite;
	Opt.OptIV.verbose = 0;
   [err] = WriteInv21Surf(Opt.WriteIV,Node,Pat,Opt.OptIV);  %exclude redundant last node
end

err = 0;
return;


function fismat = genfis4(Xin, Xout, fistype, sig_par, varargin)

%GENFIS4 Generates a FIS using CART
%
%   Given separate sets of input and output data, GENFIS4 generates a fuzzy
%   inference system (FIS) using CART algorithm. GENFIS4 accomplishes this
%   by extracting a set of rules that models the data behavior. The rule
%   extraction method first uses TREEINFO to determine the number
%   of rules and membership functions for the antecedents and consequents.
%
%   FIS = GENFIS4(XIN, XOUT) returns a Sugeno-type FIS given input data XIN
%   and output data XOUT. The matrices XIN and XOUT have one column per FIS
%   input and output, respectively.
%
%   FIS = GENFIS4(XIN, XOUT, TYPE) returns FIS of type specified by the
%   argument TYPE. It can take one of two values: 'mamdani' or 'sugeno'.
%
%   FIS = GENFIS4(XIN, XOUT, TYPE, SIGM_PAR) allows you to specify the
%   sigmoid parameters in the argument SIGM_PAR. The parameter is used to
%   form input membership function. The larger SIGM_PAR the closer
%   behaviour of fuzzy CART to behaviour of "crisp" CART. It should be
%   positive scalar or vector. The latter should have size equal to number
%   of FIS inputs. This argument also takes the value 'auto' in which case
%   GENFIS4 uses additional information containing in data set to calculate
%   SIGM_PAR.
%
%   FIS = GENFIS4(XIN, XOUT, TYPE, SIGM_PAR, VARARGIN) allows you to
%   specify options for the CART algorithm. Type HELP CLASSREGTREE and
%   HELP TEST for a list of options that can be specified for the CART
%   algorithm.
%
%   Examples:
%
%       Xin1 = 7 * rand(50, 1);
%       Xin2 = 20 * rand(50, 1) - 10;
%       Xin = [Xin1 Xin2];
%       Xout = 5 * rand(50, 1);
%       fis = genfis4(Xin, Xout);
%
%       fis = genfis4(Xin, Xout, 'mamdani', [10, 1]);
%       specifies the type of FIS and the sigmoid parameters desired.
%
%       fis = genfis4(Xin, Xout, 'mamdani', 'auto', {'minparent', 15});
%       specifies the type of FIS, the sigmoid parameters desired and CART
%       options.
%
%   See also CLASSREGTREE, TEST, TREEINFO, GENFIS3, ANFISX

%   Per Konstantin A. Sidelnikov, 2009.

%%%%%%%%%%%%%%%%%%%%
% Some constants
%%%%%%%%%%%%%%%%%%%%

% Number of standard deviations (can be changed)
NSTD = 1;
% Area under the gauss curve in the
% [mu - NSTD * sigma, mu + NSTD * sigma]
AREA = erf(NSTD / sqrt(2));
% Scaling factor
SCALE = log((1 + AREA) / (1 - AREA)) / NSTD;

%%%%%%%%%%%%%%%%%%%%
% Number of input arguments checking
%%%%%%%%%%%%%%%%%%%%

if nargin < 2
    error('FuzzyLogic:missingparams', ...
        'genfis4 requires input and output data to build a FIS.');
end

if nargin < 3
    fistype = 'sugeno';
end

if nargin < 4
    sig_par = 'auto';
end

% hardcoded for now
in_mftype = 'sigmf';
out_mftype = 'gaussmf';

%%%%%%%%%%%%%%%%%%%%%%%%%
% IO checking
%%%%%%%%%%%%%%%%%%%%%%%%%

% Check Xin, Xout
[numData, numInp] = size(Xin);
[numData2, numOutp] = size(Xout);

if numData ~= numData2
    % There's a mismatch in the input and output data matrix dimensions
    if numData == numOutp
        % The output data matrix should have been transposed, we'll fix it
        Xout = Xout';
        numOutp = numData2;
    else
        error('FuzzyLogic:dimensionmismatch', ...
            'Mismatched input and output data matrices.');
    end
end

if numOutp > 1
    error('FuzzyLogic:outputmismatch', ...
        'Output data must be a vector.');
end

%%%%%%%%%%%%%%%%%%%%
% Checking sig_par
%%%%%%%%%%%%%%%%%%%%

% Convert sig_par (if necessary) to numeric array
arrayNaN = NaN(1, numInp);
if ~isnumeric(sig_par)
    if ~isequal(sig_par, 'auto')
        error('FuzzyLogic:sigmoid', ...
            'Set sigmoid parameter to ''auto'' or a value greater than 0');
    end
    sig_par = arrayNaN;
elseif ~isscalar(sig_par)
    tmp = arrayNaN;
    tmp(1 : length(sig_par)) = sig_par;
    sig_par = tmp;
else
    sig_par = repmat(sig_par, 1, numInp);
end
% Avoid negative sig_par
sig_par = abs(sig_par);
% Length of sig_par must be the same as number of inputs
if length(sig_par) ~= numInp
    error('FuzzyLogic:sigmoid', ...
        ['Set number of sigmoid parameters to ', ...
        '1 or %d (number of inputs)'], numInp);
end

%%%%%%%%%%%%%%%%%%%%
% Creating tree
%%%%%%%%%%%%%%%%%%%%
ti = treeinfo(Xin, Xout, varargin{:});

n_node = numel(ti.node);
n_leaf = numel(ti.leaf);

in_mf = zeros(1, n_node);

%%%%%%%%%%%%%%%%%%%%%%
% Building FIS
%%%%%%%%%%%%%%%%%%%%%

% Initialize a FIS
str = sprintf('%s%g%g', fistype, numInp, numOutp);
fismat = newfis(str, fistype);

% Loop through and add inputs
for ind = 1 : numInp
    fismat = addvar(fismat, ...
        'input', ['in' num2str(ind)], minmax(Xin(:, ind)'));
end

% Loop through and add mf's
for ind = 1 : n_node
    var = ti.node(ind).variable;
    cut = ti.node(ind).cutpoint;
    s = ti.node(ind).sample;
    mfparams = computemfparams(in_mftype, Xin(s, var), ...
        cut, sig_par(var), SCALE);

    fismat = addmf(fismat, 'input', var, ...
        ['larger', num2str(ind)], in_mftype, mfparams);

    in_mf(ind) = length(fismat.input(var).mf);
end

% Add output
fismat = addvar(fismat, 'output', 'out', minmax(Xout'));

switch fistype
    case 'sugeno'
        % Loop through and add mf's
        for ind = 1 : n_leaf
            s = ti.leaf(ind).sample;
            mfparams = computemfparams('linear', Xin(s, :), Xout(s));

            fismat = addmf(fismat, 'output', 1, ...
                ['class', num2str(ind)], 'linear', mfparams);
        end
    case 'mamdani'
        % Loop through and add mf's
        for ind = 1 : n_leaf
            s = ti.leaf(ind).sample;
            mfparams = computemfparams(out_mftype, [], Xout(s));

            fismat = addmf(fismat, 'output', 1, ...
                ['class', num2str(ind)], out_mftype, mfparams);
        end
    otherwise
        error('FuzzyLogic:unknownfistype', ...
            'Unknown fistype specified');
end

% Create rules
rulelist = cell(n_leaf, 4);
for ind = 1 : n_leaf
    n = ti.branch(ind).nodes;
    e = ti.branch(ind).ineqs;

    var = [ti.node(n).variable];

    rulelist{ind, 1} = [var; in_mf(n) .* e];
    rulelist{ind, 2} = ind;
    rulelist{ind, 3} = 1;
    rulelist{ind, 4} = 1;
end

fismat = addrulex(fismat, rulelist);

function mfparams = computemfparams(mf, X, y, sig_par, scale)
%   This subfunction computes parameters of input and
%   output membership functions dependeding on mf's value.

switch mf
    case 'sigmf'
        % NaN's value of sig_par means its automatic calculation
        if isnan(sig_par)
            sigma = std(X);
            sig_par = scale / sigma;
        end
        mfparams = [sig_par, y];
    case 'gaussmf'
        sigma = std(y);
        % Check if y is a scalar or consists of identical elements
        if sigma == 0
            sigma = sqrt(eps);
        end
        c = mean(y);
        mfparams = [sigma, c];
    case 'linear'
        numData = size(X, 1);
        A = [X, ones(numData, 1)];
        % Using pinv instead of ldivide avoids
        % warning if A is close to singular
        mfparams = (pinv(A) * y)';
    otherwise
        error('FuzzyLogic:invalidmftype', ...
            'Unknown type of membership function specified.');
end

function pr = minmax(p)
%MINMAX Ranges of matrix rows.
%
%  Syntax
%
%    pr = minmax(p)
%
%  Description
%
%    MINMAX(P) takes one argument,
%      P - RxQ matrix.
%    and returns the Rx2 matrix PR of minimum and maximum values
%    for each row of P.
%
%    Alternately, P can be an MxN cell array of matrices.  Each matrix
%    P{i,j} should have Ri rows and Q columns.  In this case, MINMAX returns
%    an Mx1 cell array where the mth matrix is an Rix2 matrix of the
%    minimum and maximum values of elements for the matrics on the
%    ith row of P.
%
%  Examples
%
%    p = [0 1 2; -1 -2 -0.5]
%    pr = minmax(p)
%
%    p = {[0 1; -1 -2] [2 3 -2; 8 0 2]; [1 -2] [9 7 3]};
%    pr = minmax(p)

if iscell(p)
    m = size(p, 1);
    pr = cell(m, 1);
    for i = 1 : m
        pr{i} = minmax([p{i, :}]);
    end
elseif isa(p, 'double')
    pr = [min(p, [], 2), max(p, [], 2)];
else
    error('Argument has illegal type.');
end


function [data, Ht] = rarch_simulate(T,C,parameters,p,q,type)
% Simulation of RARCH(p,q) multivariate volatility model of Noureldin, Shephard and Sheppard
%
% USAGE:
%  [DATA,HT] = rarch_simulate(T,C,PARAMETERS,P,Q,TYPE)
%
% INPUTS:
%   T          - Either a scalar containing the length of the series to simulate, or a T by K matrix
%                  of simulated random variables.  The default is to use standard normal random
%                  variables.  Providing a T by K matrix allows other distributions to be used.
%   C          - Unconditional covariance of the data
%   PARAMETERS - Vector of parameters governing the dynamics.  The form of the parameters depends on the TYPE.
%                  'Scalar':
%                  [a(1) ... a(p) b(1) ... b(q)]'  (all scalars)
%                  'CP' :
%                  [diag(A(:,:,1))' ... diag(A(:,:,p))' theta]' (theta is the scalar persistence)
%                  'Diagonal'
%                  [diag(A(:,:,1))' ... diag(A(:,:,p))' diag(B(:,:,1))' ... diag(B(:,:,p))']'
%   P          - Positive, scalar integer representing the number of symmetric innovations
%   Q          - Non-negative, scalar integer representing the number of conditional covariance
%                  lags.  When using 'CP' model, 0<=q<=1
%   TYPE       - String, one of :
%                  'Scalar' (Default)
%                  'CP' (Common Persistence)
%                  'Diagonal'
%
% OUTPUTS:
%   DATA   - A T by K matrix of simulated data
%   HT     - A [K K T] dimension matrix of conditional covariances
%
% COMMENTS:
%   The dynamics of a RARCH model are identical to that of a BEKK, except
%   that the model evolves in the rotated space.
%
%   G(:,:,t) = (eye(K) - sum(A.^2,3) - sum(B.^2,3)) +
%       A(:,:,1)*OP(:,:,t-1)*A(:,:,1) + ... A(:,:,p)*OP(:,:,t-1)*A(:,:,p) +
%       B(:,:,1)*G(:,:,t-1)*B(:,:,1) + ... B(:,:,p)*OP(:,:,t-1)*B(:,:,p)
%
%   where in the scalar model A(:,:,i) = a(i)*eye(K), B(:,:,j)=b(j)*eye(K)
%   and in the CP model, B(:,:,j) = theta - sum(A.^2,3).  OP is the outer product of the
%   unconditionally standardized data.
%
% EXAMPLES:
%   % Scalar with A.^2=.05 and B.^2=.93
%   [data,Ht] = rarch_simulate(1000,eye(2)+1,sqrt([.05,.93]),1,1,'Scalar')
%   % Diagonal
%   [data,Ht] = rarch_simulate(1000,eye(2)+1,sqrt([.05 .07 .93 .88]),1,1,'Diagonal')
%   % Common Persistence, note uses theta (sqrt(A.^2+B.^2) not B)
%   [data,Ht] = rarch_simulate(1000,eye(2)+1,sqrt([.05 .07 .99]),1,1,'CP')
%
% See also RARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 3/27/2012

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k = size(C,1);
if isscalar(T)
    e = randn(2*T,k);
else
    e = T;
    if size(e,2)~=k
        error('T must have K columns when providing simulated random numbers.')
    end
    T = size(e,1);
    e = [e(ceil(rand(T,1)*T),:);e];
end

if strcmpi(type,'Scalar')
    type = 1;
elseif strcmpi(type,'CP')
    type = 2;
elseif strcmpi(type,'Diagonal')
    type = 3;
else
    error('TYPE must be ''Scalar'', ''CP'' or  ''Diagonal''.')
end

if type==2 && q>1
    error('Q must be either 0 or 1 for the ''CP'' model.')
end

switch type
    case 1
        count = p+q;
    case 2
        count = p*k+q;
    case 3
        count = (p+q)*k;
end
if length(parameters)~=count
    error('PARAMETERS does not have the expected number of elements.')
end
[C,A,B] = rarch_parameter_transform(parameters,p,q,k,C,type,false);
if max(diag(sum(A.^2,3)+sum(B.^2,3)))>=1
    warning('MFE:nonstationary','The parameters do not correspond to the stationary region.')
end
if type==2 && q==1 && min(B(:))<0
    error('When using ''CP'', the common persistence parameter Theta must satisfy Theta^2>=sum(A.^2,3)')
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Gt = repmat(eye(k),[1 1 2*T]);
intercept = eye(k) - sum(A.^2,3) - sum(B.^2,3);
backCast = eye(k);
for i=1:(2*T)
    Gt(:,:,i) = intercept;
    for j=1:p
        if (i-j)<=0
            Gt(:,:,i) = Gt(:,:,i) + A(:,:,j)*backCast*A(:,:,j);
        else
            Gt(:,:,i) = Gt(:,:,i) + A(:,:,j)*(e(i-j,:)'*e(i-j,:))*A(:,:,j);
        end
    end
    for j=1:q
        if (i-j)<=0
            Gt(:,:,i) = Gt(:,:,i) + B(:,:,j)*backCast*B(:,:,j);
        else
            Gt(:,:,i) = Gt(:,:,i) + B(:,:,j)*Gt(:,:,i-j)*B(:,:,j);
        end
    end
    Gt12 = Gt(:,:,i)^(0.5);
    e(i,:) = e(i,:)*Gt12;
end

data = zeros(size(e));
Ht = zeros(k,k,T);
C12 = C^(0.5);
for i=1:length(e)
    data(i,:) = e(i,:)*C12;
    Ht(:,:,i) = C12*Gt(:,:,i)*C12;
end

data = data(T+1:2*T,:);
Ht = Ht(:,:,T+1:2*T);


%DEMO_SVI_REGRESSION  A toy data regression example for sparse SVI GP
%
%  Description
%    Demonstration of sochastic variational inference GP model regression.
%    The problem is similar as in the Hensman et. al (2013). The dataset is
%    made synthetically in two dimensions using sinusoidal functions.
%
%  See also
%    DEMO_SVI_CLASSIFIC
%
%  References:
%    Hensman, J., Fusi, N. and Lawrence, N. D. (2013). Gaussian processes
%    for big data. arXiv preprint arXiv:1309.6835.

% Copyright (c) 2014 Tuomas Sivula

% 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.

% Generate toy data
n = 2000;
s2 = 0.01;
% Training samples
x = rand(n,2)*2*pi - pi;
y = sin(x(:,1)).*sin(x(:,2)) + sqrt(s2).*randn(n,1);
% Test samples
nt = 100;
xt = rand(nt,2)*2*pi - pi;
yt = sin(xt(:,1)).*sin(xt(:,2));

% Prediction grid
[X1,X2] = meshgrid(-4:0.2:4, -4:0.2:4);
grid = [X1(:), X2(:)];

% Build gp structure
gp = gp_set('lik',lik_gaussian, 'cf', gpcf_sexp, 'latent_method', 'SVI');
% Optimise
nu = 50;    % The number of inducing inputs
maxi = 150; % The maximum number of iteration rounds
[gp, diagnosis] = svigp(gp,x,y,'xt',xt,'yt',yt,'nu',nu,'maxiter',maxi);
% Predict
Eft = gpsvi_pred(gp,x,y,grid);

% ------- Plot --------
% Prediction
figure()
contour(X1, X2, reshape(Eft, size(X1)), 16)
hold on
scatter(x(:,1), x(:,2), 20, y, 'filled')
scatter(gp.X_u(:,1), gp.X_u(:,2), 30)
legend('Eft', 'data', 'Z')
% Convergence analysis
figure()
subplot(3,1,1)
plot(mean(diagnosis.e,2))
title('energy')
subplot(3,1,2)
plot(diagnosis.mlpd)
title('mean log predictive density')
subplot(3,1,3)
plot(diagnosis.rmse)
title('root mean square error')
xlabel('iteration')


function varargout = warpvars_vbmc(varargin)
%WARPVARS Linear and nonlinear transformation of variables.
%
%  TRINFO = PDFTRANS(NVARS,LB,UB) returns the transformation structure
%  TRINFO for a problem with NVARS dimensions and lower/upper bounds
%  respectively LB and UB. LB and UB are either scalars or row arrays that
%  can contain real numbers and Inf's
%  The ordering LB <= UB needs to hold coordinate-wise.
%
%  Variables with lower or upper bounds are transformed via a log transform.
%  Variables with both lower and upper bounds are transformed via a logit
%  transform.
%
%  Y = TRANSVARS(X,'dir',TRINFO) performs direct transform of constrained
%  variables X into unconstrained variables Y according to transformation
%  encoded in structure TRINFO. X must be a N x NVARS array, where N is the
%  number of input data and NVARS is the number of dimensions.
%
%  X = TRANSVARS(Y,'inv',TRINFO) performs inverse transform of unconstrained
%  variables Y into constrained variables X.
%
%  P = TRANSVARS(Y,'prob',TRINFO) returns probability multiplier for the
%  original pdf evaluated at f^{-1}(Y), that is | df^{-1}(y) / dy |.
%
%  LP = TRANSVARS(Y,'logprob',TRINFO) returns log probability term for the
%  original log pdf evaluated at f^{-1}(Y).

%  Author: Luigi Acerbi
%  e-mail: luigi.acerbi@gmail.com

if nargin < 3
    error('TRANSVARS requires a minimum of three input arguments.');
end

%% Transform variables
if nargin == 3 && (isstruct(varargin{3}) || ischar(varargin{2}))

    Tol = sqrt(eps);    % Small number

    action = varargin{2};
    trinfo = varargin{3};

    if isempty(action)
        error('The transformation direction cannot be empty. Allowed values are direct (''dir'' or ''d'') and inverse (''inv'' or ''i'').');
    end

    if isempty(trinfo)
        % Empty TRINFO - consider as identity transformation

        x = varargin{1};

        switch lower(action(1))
            case {'d','i'}
                varargout{1} = x;
            case 'p'
                varargout{1} = ones(size(x,1),1);
            case 'l'
                varargout{1} = zeros(size(x,1),1);
            case {'m','f','g'}
                error('TRINFO is empty.');
            otherwise
                error(['Unkwnown transformation direction ''' action '''. Allowed values are direct (''dir'' or ''d'') and inverse (''inv'' or ''i'').']);
        end
    else

        scale = [];
        if isfield(trinfo,'scale') && ~isempty(trinfo.scale) && any(trinfo.scale ~= 1)
            scale = trinfo.scale;
        end

        if ~isfield(trinfo,'R_mat'); trinfo.R_mat = []; end

        nvars = numel(trinfo.lb_orig);  % Number of variables

        switch lower(action(1))
        %% DIRECT TRANSFORM
            case 'd'    % Direct transform
                x = varargin{1};
                y = x;
                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                mu = trinfo.mu;
                delta = trinfo.delta;

                % Unbounded scalars (possibly center and rescale)
                idx = trinfo.type == 0;
                if any(idx)
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,x(:,idx),mu(idx)),delta(idx));
                end

                % Lower bounded scalars
                idx = trinfo.type == 1;
                if any(idx)
                    y(:,idx) = log(bsxfun(@minus, x(:,idx), a(idx)));
                end

                % Upper bounded scalars
                idx = trinfo.type == 2;
                if any(idx)
                    y(:,idx) = log(bsxfun(@minus, b(idx), x(:,idx)));
                end

                % Lower and upper bounded scalars
                idx = trinfo.type == 3;
                if any(idx)
                    z = bsxfun(@rdivide, bsxfun(@minus, x(:,idx), a(idx)), ...
                        b(idx) - a(idx));
                    y(:,idx) = log(z./(1-z));
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Lower and upper bounded scalars with Beta CDF transform
                idx = trinfo.type == 4;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = betacdf((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii));
                        z = min(max(eps,betacdf((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii))),1-eps);
                        y(:,ii) = log(z./(1-z));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy CDF transform
                idx = trinfo.type == 5;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = kumarcdf((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii));
                        % p = min(max(eps,kumarcdf(z,alpha(ii),beta(ii))),1-eps);
                        % y(:,ii) = log(p./(1-p));
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        % p = 1 - (1 - z.^alpha(ii)).^beta(ii);
                        y(:,ii) = log1p(-(1 - z.^alpha(ii)).^beta(ii)) - beta(ii)*log1p(-z.^alpha(ii));
                    end
                     y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Lower and upper bounded scalars with Kumaraswamy-logistic-power transform
                idx = trinfo.type == 6;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    mu = trinfo.mu;
                    gamma = trinfo.gamma;
                    for ii = find(idx)
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        y(:,ii) = log1p(-(1 - z.^alpha(ii)).^beta(ii)) - beta(ii)*log1p(-z.^alpha(ii)) - mu(ii);
                        y(:,ii) = sign(y(:,ii)).*abs(y(:,ii)).^gamma(ii);
                    end
                end

                % Lower and upper bounded scalars with nonparametric CDF transform
                idx = trinfo.type == 7;
                if any(idx)
                    xspace = trinfo.xspace;
                    pspace = trinfo.pspace;
                    for ii = find(idx)
                        y(:,ii) = norminv(interp1(xspace(ii,:),pspace(ii,:),x(:,ii)));
                    end
                end

                % Lower and upper bounded scalars with GMM CDF transform
                idx = trinfo.type == 8;
                if any(idx)
                    for ii = find(idx)
                        gmm = trinfo.gmm{ii};
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        z = gmm.lambda*z + (1-gmm.lambda)*(gmm1cdf(z,gmm.w,gmm.Mu,gmm.Sigma)-gmm.Min)./gmm.Norm;
                        y(:,ii) = norminv(z);
                        % y(:,ii) = logiinv(z);
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                idx = trinfo.type == 9;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        u = z.^(alpha(ii));

                        % Small u (close to zero)
                        uzero = u < sqrt(eps);
                        y(uzero,ii) = log(beta(ii)) + alpha(ii)*log(z(uzero)) + 0.5*u(uzero)*(beta(ii)+1) + 0.5*((beta(ii)-1)^2/4-beta(ii))*u(uzero).^2;

                        % Large u (close to 1)
                        uone = u > (1 - sqrt(eps));
                        y(uone,ii) = log(1./(1 - u(uone)).^beta(ii) - 1);

                        % Other values
                        y(~uzero & ~uone,ii) = log1p(-(1 - z(~uzero & ~uone).^alpha(ii)).^beta(ii)) - beta(ii)*log1p(-z(~uzero & ~uone).^alpha(ii));
                    end
                end

                % Unbounded with logistic-Kumaraswamy-logit transform
                idx = trinfo.type == 10;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = (x(:,ii)-mu(:,ii)) ./ delta(:,ii);
                        %z = exp(z)./(exp(z)+1);
                        %y(:,ii) = log1p(-(1 - z.^alpha(:,ii)).^beta(:,ii)) - beta(:,ii).*log1p(-z.^alpha(:,ii));
                        s = 1./(1+exp(-z));
                        u = s.^alpha(ii);

                        % Small u (near zero)
                        uzero = u < Tol;
                        %y(uzero,ii) = log(beta(ii)) + alpha(ii)*log(s(uzero)) + beta(ii)*u(uzero);
                        y(uzero,ii) = log(beta(ii)) + alpha(ii)*z(uzero) + beta(ii)*u(uzero);

                        % Large u (near one)
                        uone = u > (1 - Tol);
                        w = 1./(1 + exp(z(uone)));
                        y(uone,ii) = -log(alpha(ii)) + beta(ii)*z(uone) - (alpha(ii)*w).^beta(ii);

                        % All other cases
                        urest = ~uzero & ~uone;
                        y(urest,ii) = log1p(-(1 - s(urest).^alpha(ii)).^beta(ii)) - beta(ii).*log1p(-s(urest).^alpha(ii));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit inverse CDF transform
                idx = trinfo.type == 11;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = kumarinv((x(:,ii) - a(ii)) / (b(ii) - a(ii)),alpha(ii),beta(ii));
                        % p = min(max(eps,kumarcdf(z,alpha(ii),beta(ii))),1-eps);
                        % y(:,ii) = log(p./(1-p));
                        z = (x(:,ii) - a(ii)) / (b(ii) - a(ii));
                        % p = 1 - (1 - z.^alpha(ii)).^beta(ii);
                        % y(:,ii) = 1./alpha(ii).*log1p(-(1 - z).^(1./beta(ii))) - log1p(-(1-(1-z).^(1./beta(ii))).^(1./alpha(ii)));
                        u = (1./beta(ii)).*log1p(-z);

                        uidx = u < log(eps)/2;
                        % u(uidx) = 1 - 1./alpha(ii).*exp(u(uidx));
                        y(uidx,ii) = -1./alpha(ii).*exp(u(uidx)) + log(alpha(ii)) - u(uidx);
                        % u(~uidx) = (1 - exp(u(~uidx))).^(1/alpha(ii));
                        y(~uidx,ii) = (1/alpha(ii)).*log1p(-exp(u(~uidx))) - log1p(-(1 - exp(u(~uidx))).^(1/alpha(ii)));
                        usmall = abs(u) < sqrt(eps);
                        y(usmall,ii) = (1/alpha(ii)).*log(-u(usmall)) - log1p(-u(usmall)/alpha(ii));

                        % y(:,ii) = log(expm1(-1./alpha(ii).*log1p(-(1-z).^(1./beta(ii)))));
                        y(:,ii) = bsxfun(@rdivide,bsxfun(@minus,y(:,ii),mu(ii)),delta(ii));
                    end
                end

                % Lower and upper bounded scalars (cumulative normal)
                idx = trinfo.type == 12;
                if any(idx)
                    z = bsxfun(@rdivide, bsxfun(@minus, x(:,idx), a(idx)), ...
                        b(idx) - a(idx));
                    y(:,idx) = -sqrt(2).*erfcinv(2*z);
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Lower and upper bounded scalars (Student-t, nu = 4)
                idx = trinfo.type == 13;
                if any(idx)
                    z = bsxfun(@rdivide, bsxfun(@minus, x(:,idx), a(idx)), ...
                        b(idx) - a(idx));
                    aa = sqrt(4*z.*(1-z));
                    q = cos(acos(aa)/3)./aa;
                    y(:,idx) = sign(z - 0.5).*(2.*sqrt(q-1));
                    y(:,idx) = bsxfun(@rdivide,bsxfun(@minus,y(:,idx),mu(idx)),delta(idx));
                end

                % Rotate output
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat; end

                % Rescale output
                if ~isempty(scale); y = bsxfun(@rdivide,y,scale); end

                varargout{1} = y;

            %% INVERSE TRANSFORM
            case 'i'    % Inverse transform
                y = varargin{1};
                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                x = y;
                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                mu = trinfo.mu;
                delta = trinfo.delta;

                % Unbounded scalars (possibly unscale and uncenter)
                idx = trinfo.type == 0;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                end

                % Lower bounded scalars
                idx = trinfo.type == 1;
                if any(idx)
                    x(:,idx) = bsxfun(@plus, exp(y(:,idx)), a(idx));
                end

                % Upper bounded scalars
                idx = trinfo.type == 2;
                if any(idx)
                    x(:,idx) = bsxfun(@minus, b(idx), exp(y(:,idx)));
                end

                % Lower and upper bounded scalars
                idx = trinfo.type == 3;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    x(:,idx) = bsxfun(@plus, a(:,idx), bsxfun(@times, ...
                        b(idx)-a(idx), 1./(1+exp(-x(:,idx)))));
                end

                % Lower and upper bounded scalars with Beta CDF transform
                idx = trinfo.type == 4;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = 1./(1+exp(-y(:,ii)));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*betainv(z,alpha(ii),beta(ii));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy CDF transform
                idx = trinfo.type == 5;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    for ii = find(idx)
                        % z = 1./(1+exp(-y(:,ii)));
                        % x(:,ii) = a(ii) + (b(ii)-a(ii))*kumarinv(z,alpha(ii),beta(ii));
                        z = exp(-y(:,ii))./(1+exp(-y(:,ii)));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1-z.^(1/beta(ii))).^(1/alpha(ii));
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logistic-power transform
                idx = trinfo.type == 6;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    mu = trinfo.mu;
                    gamma = trinfo.gamma;
                    for ii = find(idx)
                        z = sign(y(:,ii)).*abs(y(:,ii)).^(1/gamma(ii)) + mu(ii);
                        z = exp(-z)./(1+exp(-z));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1-z.^(1/beta(ii))).^(1/alpha(ii));
                    end
                end

                % Lower and upper bounded scalars with nonparametric CDF transform
                idx = trinfo.type == 7;
                if any(idx)
                    xspace = trinfo.xspace;
                    pspace = trinfo.pspace;
                    for ii = find(idx)
                        x(:,ii) = interp1(pspace(ii,:),xspace(ii,:),normcdf(y(:,ii)));
                    end
                end

                % Lower and upper bounded scalars with GMM CDF transform
                idx = trinfo.type == 8;
                if any(idx)
                    for ii = find(idx)
                        gmm = trinfo.gmm{ii};
                        z = normcdf(y(:,ii));
                        % z = logicdf(y(:,ii));
                        for j = 1:size(z,1)
                            z(j) = tgmminv(z(j),gmm);
                        end
                        x(:,ii) = a(ii) + (b(ii)-a(ii)).*z;
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                idx = trinfo.type == 9;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        z = exp(-y(:,ii))./(1+exp(-y(:,ii)));   % 1 - logistic(z)
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1-z.^(1/beta(ii))).^(1/alpha(ii));
                    end
                end

                % Unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = trinfo.type == 10;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    for ii = find(idx)
                        % z = exp(-y(:,ii))./(1+exp(-y(:,ii)));   % 1 - logistic(z)
                        % % z = (1-z.^(1/beta(ii))).^(1/alpha(ii));
                        % z = 1./alpha(:,ii).*log1p(-z.^(1./beta(:,ii))) - log1p(-(1-z.^(1./beta(:,ii))).^(1./alpha(:,ii)));

                        % Small u (~ zero)
                        uzero = y(:,ii) < log(Tol);
                        u = 1./(1 + exp(-y(uzero,ii)));
                        %x(uzero,ii) = 1/alpha(ii)*(-log1p(exp(-y(uzero,ii)))-log(beta(ii))) - log1p(-(u/beta(ii)).^(1/alpha(ii)));
                        x(uzero,ii) = 1/alpha(ii)*(y(uzero,ii)-log(beta(ii))) - log1p(-(u/beta(ii)).^(1/alpha(ii)));

                        % Large u (~ one)
                        uone = y(:,ii) > -log(Tol);
                        w = 1./(1 + exp(y(uone,ii))).^(1/beta(ii));
                        x(uone,ii) = 1/alpha(ii)*log1p(-w) + 1/beta(ii)*y(uone,ii) + log(alpha(ii));

                        % All other cases
                        urest = ~uzero & ~uone;
                        z = 1./(1+exp(y(urest,ii)));
                        x(urest,ii) = 1./alpha(:,ii).*log1p(-z.^(1./beta(:,ii))) - log1p(-(1-z.^(1./beta(:,ii))).^(1./alpha(:,ii)));

                        x(:,ii) = x(:,ii).*delta(:,ii) + mu(:,ii);
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy inverse CDF transform
                idx = trinfo.type == 11;
                if any(idx)
                    alpha = trinfo.alpha;
                    beta = trinfo.beta;
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    for ii = find(idx)
                        z = 1./(1+exp(-y(:,ii)));
                        x(:,ii) = a(ii) + (b(ii)-a(ii))*(1 - (1-z.^(alpha(ii))).^beta(ii));
                    end
                end

                % Lower and upper bounded scalars (cumulative normal)
                idx = trinfo.type == 12;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    x(:,idx) = bsxfun(@plus, a(:,idx), bsxfun(@times, ...
                        b(idx)-a(idx), 0.5 * erfc(-x(:,idx) ./ sqrt(2))));
                end

                % Lower and upper bounded scalars (Student-t, nu = 4)
                idx = trinfo.type == 13;
                if any(idx)
                    x(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    t2 = x(:,idx).^2;
                    f = 0.5 + 3/8*x(:,idx)./sqrt(1 + t2/4).*(1 - t2./(1 + t2/4)/12);
                    x(:,idx) = bsxfun(@plus, a(:,idx), bsxfun(@times, ...
                        b(idx)-a(idx), f));
                end

                % Force to stay within bounds
                a(isfinite(a)) = a(isfinite(a)) + eps(a(isfinite(a)));
                b(isfinite(b)) = b(isfinite(b)) - eps(b(isfinite(b)));
                x = bsxfun(@min,bsxfun(@max,x,a),b);
                varargout{1} = x;

            %% PDF (OR LOG PDF) CORRECTION
            case {'p','l','g'}  % pdf (or log pdf) correction
                y = varargin{1};
                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                logpdf_flag = strcmpi(action(1),'l');
                if logpdf_flag
                    p = zeros(size(y,1),nvars);
                else
                    p = ones(size(y,1),nvars);
                end
                grad_flag = strcmpi(action(1),'g');

                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                mu = trinfo.mu;
                delta = trinfo.delta;

                % Unbounded scalars
                idx = trinfo.type == 0;
                if any(idx)
                    p(:,idx) = repmat(log(delta(idx)),[size(p,1),1]);
                end

                % Lower or upper bounded scalars
                idx = trinfo.type == 1 | trinfo.type == 2;
                if any(idx)
                    p(:,idx) = y(:,idx);
                end

                % Lower and upper bounded scalars
                idx = trinfo.type == 3;
                if any(idx)
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    z = -log1p(exp(-y(:,idx)));
                    p(:,idx) = bsxfun(@plus, log(b(idx)-a(idx)), -y(:,idx) + 2*z);
                    p(:,idx) = bsxfun(@plus, p(:,idx), log(delta(idx)));
                end

                % Lower and upper bounded scalars with Beta CDF transform
                idx = trinfo.type == 4;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        z = -log1p(exp(-y(:,ii)));
                        x = min(max(eps,betainv(1./(1+exp(-y(:,ii))),alpha(ii),beta(ii))),1-eps);
                        logbeta = (alpha(ii)-1)*log(x) + (beta(ii)-1)*log1p(-x) ...
                            + gammaln(alpha(ii)+beta(ii)) - gammaln(alpha(ii)) - gammaln(beta(ii));
                        p(:,ii) = log(b(ii)-a(ii)) -logbeta -y(:,ii) + 2*z;

                        if any(~isfinite(p))
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy CDF transform
                idx = trinfo.type == 5;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        y(:,ii) = bsxfun(@plus,bsxfun(@times,y(:,ii),delta(ii)),mu(ii));

                        z = -log1p(exp(-y(:,ii)));
                        % x = kumarinv(1./(1+exp(-y(:,ii))),alpha(ii),beta(ii));

                        %p = 1./(1+exp(-y(:,ii)));
                        %x = (-((-(p-1)).^(1/beta(ii))-1)).^(1/alpha(ii));
                        %log(-(p-1)) = -y(,::) - log1p(exp(-y(:,ii))));

                        %(1/beta(ii)).*log(-(p-1))

                        % x = min(max(eps,kumarinv(1./(1+exp(-y(:,ii))),alpha(ii),beta(ii))),1-eps);
                        u = 1./(1 + exp(-y(:,ii)));
                        logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*(-y(:,ii)+z) ...
                            + log(alpha(ii)) + log(beta(ii));

                        % Special case for very small u
                        %idx_small = u < 1e12;
                        %logf(idx_small) = (1-1/alpha(ii))*(-y(idx_small,ii) + z - log(beta(ii))) + (1-1/beta(ii))*(-y(idx_small,ii)+z(idx_small)) ...
                        %    + log(alpha(ii)) + log(beta(ii));

                        %logf = (alpha(ii)-1)*log(x) + (beta(ii)-1)*log1p(-x.^alpha(ii)) ...
                        %    + log(alpha(ii)) + log(beta(ii));
                        p(:,ii) = log(b(ii)-a(ii)) -logf -y(:,ii) + 2*z;
                        p(:,ii) = bsxfun(@plus, p(:,ii), log(delta(ii)));

                        if any(~isfinite(p))
                            p(~isfinite(p)) = -Inf;
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logistic-power transform
                idx = trinfo.type == 6;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;
                        mu = trinfo.mu;
                        gamma = trinfo.gamma;

                        yl = sign(y(:,ii)).*abs(y(:,ii)).^(1/gamma(ii)) + mu(ii);

                        z = -log1p(exp(-yl));
                        u = 1./(1 + exp(-yl));
                        logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*(-yl+z) ...
                            + log(alpha(ii)) + log(beta(ii));
                        logf = logf + log(gamma(ii)) + (gamma(ii)-1)*log(abs(yl-mu(ii)));
                        p(:,ii) = log(b(ii)-a(ii)) -logf -yl + 2*z;
                    end
                end

                % Lower and upper bounded scalars with nonparametric CDF transform
                idx = trinfo.type == 7;
                if any(idx)
                    xspace = trinfo.xspace;
                    pspace = trinfo.pspace;
                    for ii = find(idx)
                        z = -0.5*log(2*pi) -0.5*y(:,ii).^2;

                        yinv = normcdf(y(:,ii));
                        [~,pos] = histc(yinv,pspace(ii,:));
                        dx = [-log(diff(xspace(ii,:))),Inf];
                        logf = dx(pos);

                        p(:,ii) = log(b(ii)-a(ii)) -logf(:) + z;
                    end
                end

                % Lower and upper bounded scalars with GMM CDF transform
                idx = trinfo.type == 8;
                if any(idx)
                    for ii = find(idx)
                        gmm = trinfo.gmm{ii};

                        z = -0.5*log(2*pi) -0.5*y(:,ii).^2;
                        % z = -y(:,ii) - 2*log1p(exp(-y(:,ii)));

                        yinv = normcdf(y(:,ii));
                        for j = 1:size(yinv,1); yinv(j) = tgmminv(yinv(j),gmm); end

                        logf = log(gmm.lambda + (1-gmm.lambda)*gmm1pdf(yinv,gmm.w,gmm.Mu,gmm.Sigma)./gmm.Norm);

                        p(ii) = log(b(ii)-a(ii)) - logf + z;

                        if any(~isfinite(p))
                            p(~isfinite(p)) = -Inf;
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                idx = trinfo.type == 9;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;
                        nf = (b(ii)-a(ii))/alpha(ii)/beta(ii);
                        k = 1./(1+exp(-y(:,ii)));

                        logk = -log1p(exp(-y(:,ii)));
                        log1mk = logk - y(:,ii);
                        logz = 1/alpha(ii)*log1p(-(1-k).^(1/beta(ii)));

                        % k small (close to zero)
                        kzero = k < sqrt(eps);
                        log1mk(kzero) = log1p(-k(kzero));
                        logz(kzero) = 1/alpha(ii) * (logk(kzero)-log(beta(ii))) + 1/alpha(ii)*log1p(0.5*(1-1/beta(ii)*k(kzero)));

                        %z = (1-(1-k).^(1/beta(ii))).^(1/alpha(ii));
                        % 1 - z^alpha = (1-k).^(1/beta(ii))
                        p(:,ii) = log(nf) + (1/beta(ii)-1) * log1mk + (1-alpha(ii))*logz -y(:,ii)+2*logk;
                    end
                end

                % Unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = trinfo.type == 10;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        lnf = log(delta(:,ii)./alpha(:,ii)./beta(:,ii));

                        if 0

                            u = 1./(1+exp(y(:,ii)));
                            logu = -log1p(exp(y(:,ii)));
                            z = (1-u.^(1./beta(:,ii))).^(1./alpha(:,ii));
                            p(:,ii) = lnf + y(:,ii) + (1+1./beta(:,ii)).*logu - log1p(-u.^(1./beta(:,ii))) - log1p(-z);

                        else

                            p(:,ii) = lnf -y(:,ii) - 2*log1p(exp(-y(:,ii)));

                            % Small u (~ zero)
                            uzero = y(:,ii) < log(Tol);
                            u = 1./(1 + exp(-y(uzero,ii)));
                            % p(uzero,ii) = p(uzero,ii) - log(u/beta(ii)) + (1/beta(ii)-1)*log1p(-u);
                            p(uzero,ii) = p(uzero,ii) - y(uzero,ii) + log(beta(ii)) + (1/beta(ii)-1)*log1p(-u);
                            t = (1 - (1-u).^(1/beta(ii))).^(1/alpha(ii));
                            p(uzero,ii) = p(uzero,ii) - log1p(-t);

                            % Large u (~ one)
                            uone = y(:,ii) > -log(Tol);
                            w = 1./(1 + exp(y(uone,ii)));
                            p(uone,ii) = p(uone,ii) - log1p(-w.^(1/beta(ii))) + y(uone,ii) + log(alpha(ii));

                            % All other cases
                            urest = ~uzero & ~uone;
                            u = 1./(1 + exp(-y(urest,ii)));
                            p(urest,ii) = p(urest,ii) - log1p(-(1-u).^(1/beta(ii))) + (1/beta(ii)-1)*log1p(-u);
                            t = (1 - (1-u).^(1/beta(ii))).^(1/alpha(ii));
                            p(urest,ii) = p(urest,ii) - log1p(-t);

                        end

                    end
                end

                % Lower and upper bounded scalars with inverse Kumaraswamy logit CDF transform
                idx = trinfo.type == 11;
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        y(:,ii) = bsxfun(@plus,bsxfun(@times,y(:,ii),delta(ii)),mu(ii));

                        y_large = y(:,ii) > -log(eps)/2;

                        z = 1./(1+exp(-y(:,ii)));
                        u = 1 - (1-z.^(alpha(ii))).^beta(ii);

                        u(y_large) = 1 - alpha(ii)*exp(-beta(ii)*y(y_large,ii))./(1+alpha(ii)*beta(ii)*exp(-y(y_large,ii)));

%                        z = -log1p(exp(-y(:,ii)));
                        %u = 1./(1 + exp(-y(:,ii)));
                        %u = a(ii) + (b(ii)-a(ii))*(1 - (1-u.^(alpha(ii))).^beta(ii));
%                        logf = log(alpha(ii)) + log(beta(ii)) + (alpha(ii)-1).*log(u) + (beta(ii)-1) .* log1p(-u.^alpha(ii));
                         logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*log1p(-u) ...
                             + log(alpha(ii)) + log(beta(ii));
                         logf(y_large) = (1-1/alpha(ii))*log1p(-alpha(ii)*exp(-y(y_large,ii))./(1+alpha(ii)*exp(-y(y_large,ii)))) ...
                             + (1-1/beta(ii))*log(alpha(ii)*exp(-beta(ii)*y(y_large,ii))./(1+alpha(ii)*beta(ii)*exp(-y(y_large,ii)))) ...
                             + log(alpha(ii)) + log(beta(ii));

%                         logf = (1-1/alpha(ii))*log1p(-(1-u).^(1/beta(ii))) + (1-1/beta(ii))*(-y(:,ii)+z) ...
%                             + log(alpha(ii)) + log(beta(ii));

                        w = log1p(-z);
                        w(y_large) = -y(y_large,ii) - log1p(exp(-y(y_large,ii)));

                        p(:,ii) = log(b(ii)-a(ii)) + logf + log(z) + w; %-y(:,ii) + 2*z;
                        p(:,ii) = bsxfun(@plus, p(:,ii), log(delta(ii)));

                        if any(~isfinite(p))
                            p(~isfinite(p)) = -Inf;
                            fprintf('aaaa!');
                        end
                    end
                end

                % Lower and upper bounded scalars (cumulative normal)
                idx = trinfo.type == 12;
                if any(idx)
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    z = -0.5*log(2*pi) - 0.5*y(:,idx).^2;
                    p(:,idx) = bsxfun(@plus, log(b(idx)-a(idx)), z);
                    p(:,idx) = bsxfun(@plus, p(:,idx), log(delta(idx)));
                end

                % Lower and upper bounded scalars (Student-t, nu = 4)
                idx = trinfo.type == 13;
                if any(idx)
                    y(:,idx) = bsxfun(@plus,bsxfun(@times,y(:,idx),delta(idx)),mu(idx));
                    z = log(3/8) - 5/2*log1p(y(:,idx).^2/4);
                    p(:,idx) = bsxfun(@plus, log(b(idx)-a(idx)), z);
                    p(:,idx) = bsxfun(@plus, p(:,idx), log(delta(idx)));
                end

                %if ~isempty(trinfo.R_mat) && lower(action(1)) == 'g'
                %    p = p*(trinfo.R_mat*diag();
                %end

                % Scale transform
                if ~isempty(scale) && ~grad_flag
                    p = bsxfun(@plus,p,log(scale));
                end

                if ~grad_flag; p = sum(p,2); end
                if ~logpdf_flag; p = exp(p); end

                varargout{1} = p;

            %% FIRST DERIVATIVE WRT TRANSFORMATION PARAMETERS (ignores final rotation and scaling)
            case {'f'}
                y = varargin{1};
                nvars = numel(trinfo.lb_orig);

                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                delta = trinfo.delta;

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                % and unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = (trinfo.type == 9 | trinfo.type == 10);
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        k = 1./(1+exp(-y(:,ii)));
                        talpha = 1 - (1-k).^(1/beta(ii));
                        logt = 1/alpha(ii) .* log1p(-(1-k).^(1/beta(ii)));

                        p(:,ii) = talpha.*beta(ii).*logt./(1-talpha)./k;
                        p(:,ii+nvars) = -log1p(-talpha)./k;
                    end
                end

                varargout{1} = p;

            %% MIXED DERIVATIVE WRT TRANSFORMATION PARAMETERS of FIRST DERIVATIVE
            % (ignores final rotation and scaling)
            case {'m'}
                y = varargin{1};
                nvars = numel(trinfo.lb_orig);

                % Rescale input
                if ~isempty(scale); y = bsxfun(@times,y,scale); end

                % Rotate input
                if ~isempty(trinfo.R_mat); y = y*trinfo.R_mat'; end

                a = trinfo.lb_orig;
                b = trinfo.ub_orig;
                delta = trinfo.delta;

                % Lower and upper bounded scalars with Kumaraswamy-logit transform
                % and unbounded scalars with logistic-Kumaraswamy-logit transform
                idx = (trinfo.type == 9 | trinfo.type == 10);
                if any(idx)
                    for ii = find(idx)
                        alpha = trinfo.alpha;
                        beta = trinfo.beta;

                        k = 1./(1+exp(-y(:,ii)));
                        talpha = 1 - (1-k).^(1/beta(ii));
                        logt = 1/alpha(ii) .* log1p(-(1-k).^(1/beta(ii)));
                        t = talpha.^(1/alpha(ii));

                        if trinfo.type(ii) == 9
                            nf = - 1./(b(ii) - a(ii)) ./ t;
                        else
                            nf = (t-1)./delta(ii);
                        end
                        den = 1 ./ k.^2 ./ (talpha-1) .* nf;
                        p(:,ii) = talpha.*beta(ii).*((-k.*(1+alpha(ii).*logt)) + talpha.*(1 + (1-k).*(-1+alpha(ii).*beta(ii).*logt))) ...
                            .* den ./ (talpha - 1);
                        p(:,ii+nvars) = talpha .* alpha(ii) .* (1 + (1-k).*(-1 - y(:,ii) - log1p(exp(-y(:,ii))))) ...
                            .* den;
                    end
                end

                varargout{1} = p;

            otherwise
                error(['Unkwnown transformation direction ''' action '''. Allowed values are direct (''dir'' or ''d'') and inverse (''inv'' or ''i'').']);
        end
    end

else
%% Create transform

    nvars = varargin{1};
    lb = varargin{2}(:)';
    ub = varargin{3}(:)';
    if nargin > 3
        plb = varargin{4}(:)';
        pub = varargin{5}(:)';
    else
        plb = []; pub = [];
    end
    if nargin > 5
        bounded_type = varargin{6};
    else
        % Default bounded type is logit
        bounded_type = 3;
    end

    % Empty LB and UB are Infs
    if isempty(lb); lb = -Inf; end
    if isempty(ub); ub = Inf; end

    % Empty plausible bounds equal hard bounds
    if isempty(plb); plb = lb; end
    if isempty(pub); pub = ub; end

    % Convert scalar inputs to row vectors
    if isscalar(lb); lb = lb*ones(1,nvars); end
    if isscalar(ub); ub = ub*ones(1,nvars); end
    if isscalar(plb); plb = plb*ones(1,nvars); end
    if isscalar(pub); pub = pub*ones(1,nvars); end

    % Check that the order of bounds is respected
    assert(all(lb <= plb & plb < pub & pub <= ub), ...
        'Variable bounds should be LB <= PLB < PUB <= UB for all variables.');

    % Transform to log coordinates
    trinfo.lb_orig = lb;
    trinfo.ub_orig = ub;

    trinfo.type = zeros(1,nvars);
    for i = 1:nvars
        if isfinite(lb(i)) && isinf(ub(i)); trinfo.type(i) = 1; end
        if isinf(lb(i)) && isfinite(ub(i)); trinfo.type(i) = 2; end
        if isfinite(lb(i)) && isfinite(ub(i)) && lb(i) < ub(i); trinfo.type(i) = bounded_type; end
    end

    % Centering (at the end of the transform)
    trinfo.mu = zeros(1,nvars);
    trinfo.delta = ones(1,nvars);

    % Get transformed PLB and PUB
    plb = warpvars_vbmc(plb,'d',trinfo);
    pub = warpvars_vbmc(pub,'d',trinfo);

    % Center in transformed space
    for i = 1:nvars
        if isfinite(plb(i)) && isfinite(pub(i))
            trinfo.mu(i) = 0.5*(plb(i)+pub(i));
            trinfo.delta(i) = (pub(i)-plb(i));
        end
    end

    varargout{1} = trinfo;

end

end

%--------------------------------------------------------------------------
function y = tgmminv(p,gmm)
    if p <= 0; p = eps; elseif p >= 1; p = 1 - eps; end    % Correct bounds
    fun = @(z) tgmmfzero(z,p,gmm.w,gmm.Mu,gmm.Sigma,gmm.Min,gmm.iMax,gmm.Norm,gmm.lambda);
    y = qfzero(fun,[-eps,1+eps]);
    if y <= 0; y = eps; elseif y >= 1; y = 1 - eps; end    % Correct bounds
end


function [kappa,k,theta0,P,b,ebuf,Zf,Zw] = celpana(x,L,M,c,cb,Pidx,bbuf,ebuf,Zf,Zw)
%  celpana --> CELP analyzer (coder).
%
%    [kappa,k,theta0,P,b,ebuf,Zf,Zw] = celpana(x,L,M,c,cb,Pidx,bbuf,ebuf,Zf,Zw)
%
%    The function implements a CELP coder using the following steps:
%
%    (1) Find the reflection coefficients, kappa, using M'th order
%    LP analysis on the signal frame x of length N.
%
%    (2) Find the coefficients of the filter function A(z/c) used in
%    the perceptual weighting filter.
%
%    (3) Determine the excitation parameters k, theta0, P, and b, used
%    to generate the excitation sequence, e(n). The parameters will be
%    estimated in blocks of length L, so N/L values are obtained for
%    the single input frame. Other inputs used here, are the Gaussian
%    codebook given by the L-by-K matrix cb, the pitch search range
%    Pidx(1) < P < Pidx(2), the last estimated b in bbuf, Pidx(2) previous
%    excitation samples buffered in the vector ebuf, and Zf and Zw which
%    are the memory hangover in the filters 1/A(z/c) and W(z) = A(z/c)/A(z),
%    respectively.

N = length(x);                          % Frame length.
J = N/L;                                % Number of sub-blocks.

k      = zeros(J,1);
theta0 = zeros(J,1);
P      = zeros(J,1);
b      = zeros(J,1);

[ar,xi,kappa,ehat] = lpcana(x,M);       % LP analysis of frame.
ac = lpcweight(ar,c);                   % Coefficients of filter A(z/c).

for (j=1:J)                             % Excitation sequence in blocks.
  n = (j-1)*L+1:j*L;
  [k(j),theta0(j),P(j),b(j),ebuf,Zf,Zw] = celpexcit(x(n),cb,Pidx,ar,ac,...
                                                              bbuf,ebuf,Zf,Zw);
  bbuf = b(j);                          % Last estimated b.
end


%  Figure 10.24      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
%   fig10_24.m is a script to generate Fig. 10.24,
%   the transient response of the LQR symmetric rootlocus compensator
%   of the satellite position control, non-colocated case WITH ESTIMATOR

% parameter values
m=[1, 0.1]; k=[0, 0.091] ; d=[0, 0.0036]; k1=[0, 0.4];
% call function
[f,g,h,j] = twomass(m,k,d);
s=[f, g;h, 0];r=[0*g;1]; n=s\r;nx=n(1:4);nu=n(5);
% call function
[f1,g,h,j] = twomass(m,k1,d);

% form G(s)G(-s) model
a=[f, 0*f;
-h'*h, -f'];
b=[g;0*g];
d=[0];
c=[0*h, g'];
hold off; clf
P=eig(a-b*c*0.1621);
pc=P(real(P<0)==1);
K=place(f,g,pc);
nbar=nu+K*nx;
% eig(f-g*K)
P=eig(a-b*c*3.056e7);
pe=P(real(P<0)==1);
L=place(f',h',pe)';
ac=f-g*K-L*h ;bc=L;cc=K;dc=0;
[Aol,Bol,Col,Dol]=series(ac,bc,cc,dc,f,g,h,j);
[acl,bcl,ccl,dcl]=feedback(f,g,h,j,ac,bc,cc,dc);
[acl1,bcl,ccl,dcl]=feedback(f1,g,h,j,ac,bc,cc,dc);
bcl= nbar*[g;g];
t=0:.25:30;
syscl=ss(acl,bcl,ccl,dcl);
step(syscl,t);
hold on;
gtext('nominal case')
syscl1=ss(acl1,bcl,ccl,dcl);
step(syscl1,t) ;
gtext('stiff-spring case');
title('Fig. 10.24 Closed-loop step response for the SRL design with an estimator')
%grid
nicegrid


function [err_N, sG] = ptv_nuclear_metric(volfix, voldef, pix_resolution, mask, singular_coefs, norm_type)
    volsz = [size(voldef, 1), size(voldef, 2), size(voldef, 3)];
    npix = prod(volsz);
    Nimgs = size(voldef, 5);

%     nuc_G = zeros(size(voldef), 'like', voldef);
    err_N = 0;

%     imagesc([voldef(:,:, round(end/2), 1, 1), volfix(:,:, round(end/2), 1, 1)]); pause(0.02);
%     imagesc([ mean(voldef(:,:, round(end/2), 1, :), 5)]); pause(0.02);

    if isempty(singular_coefs)
        singular_coefs = fl(sqrt(1:Nimgs));
        singular_coefs(1) = 0;
    end

    if isempty(volfix)
        X = reshape(voldef, [npix, Nimgs]);
    else
        X = reshape(cat(5, volfix, voldef), [npix, Nimgs+1]);
        if numel(singular_coefs > 1)
%             singular_coefs = [0, 1, singular_coefs(2:end)];
%             singular_coefs = [fl(singular_coefs(1:end)); singular_coefs(end)];
%             singular_coefs = fl(sqrt(1:(Nimgs+1)));
%             singular_coefs
        end
    end
    if ~isempty(mask)
        mask = logical(mask > 0.5);
        X = X(mask, :);
    end
    singular_coefs = singular_coefs(1 : min(numel(singular_coefs), size(X, 1)));
    [sU, sS, sV] = svdecon(X);
%     sS
    diagS = diag(sS);
    if false
        sGt = sU * ( (diag(singular_coefs)) * sV');
        err_N = sum(diagS(:) .* singular_coefs(:));
    else
        eps = 1e-4;
%         eps=0.1;
        G_sS = diagS ./ sqrt(diagS.^2 + eps);
        sGt = sU * ( (diag(singular_coefs(:) .* G_sS(:))) * sV');
        err_N = sum( sqrt(diagS.^2 + eps) .* singular_coefs(:));
    end

%     if true
%         sk1 = sqrt(1 : size(singular_coefs, 1));
%         sk2 = sk1;
%         sk2(1) = 0;
%
%         sk3 = ones(size(singular_coefs, 1), 1);
%         sk4 = sk3;
%         sk4(1) = 0;
%
%         size(sk1)
%         subplot(221);
%         itmp = reshape(sU * (diag(sk1) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         subplot(222);
%         itmp = reshape(sU * (diag(sk2) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         subplot(223);
%         itmp = reshape(sU * (diag(sk3) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         subplot(224);
%         itmp = reshape(sU * (diag(sk4) * sV'), [volsz, Nimgs]);
%         imagesc(itmp(:,:, 70)); colorbar;
%
%         pause(0.05);
%         pause();
%     end

    if isempty(volfix)
        if ~isempty(mask)
            sG = zeros([npix, Nimgs], 'like', voldef);
            sG(mask, :) = reshape(sGt, [nnz(mask), Nimgs]);
            sG = reshape(sG, [volsz, Nimgs]);
        else
            sG = reshape(sGt, [volsz, Nimgs]);
        end
    else
        if ~isempty(mask)
            sG = zeros([npix, Nimgs+1], 'like', voldef);
            sG(mask, :) = reshape(sGt, [nnz(mask), Nimgs+1]);
            sG = reshape(sG, [volsz, Nimgs+1]);
        else
            sG = reshape(sGt, [volsz, Nimgs+1]);
        end
        sG = sG(:,:,:, 2:end);
    end

end


function d = mg_q1cd_supg(xy,ev,expe,eph,epw)
%mg_q1cd_supg  streamline diffusion matrix generator for GMG
%   d = mg_q1cd_supg(xy,ev,expe,eph,epw)
%   input
%           xy         vertex coordinate vector
%           ev         element mapping matrix
%           expe       element peclet numbers
%           eph        flow specific element lengths
%           epw        centroid evaluated wind
%   output
%           d          discrete streamline diffusion operator
%
%   IFISS function: DJS; 26 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

%   Analogous to femq1_cd_supg.m
x=xy(:,1); y=xy(:,2);
nvtx=length(x);
nel=length(ev(:,1));
%
% find the elements where streamline diffusion is active
acte = find(isfinite(expe)); nacte=length(acte);
%
% initialise global matrices
      d = sparse(nvtx,nvtx);
%
% set up 2x2 Gauss points
      gpt=1.0e0/sqrt(3.0e0);
      s(1) = -gpt;  t(1) = -gpt;
      s(2) =  gpt;  t(2) = -gpt;
      s(3) =  gpt;  t(3) =  gpt;
      s(4) = -gpt;  t(4) =  gpt;
%
% loop over active and inactive elements
%      for iact = 1:nacte
% ielem = acte(iact);
% inner loop over elements
        for ivtx = 1:4
        xl_v(:,ivtx) = x(ev(:,ivtx));
        yl_v(:,ivtx) = y(ev(:,ivtx));
		end
        de = zeros(nel,4,4);
% loop over 2x2 Gauss points
         for igpt = 1:4
         sigpt=s(igpt);
         tigpt=t(igpt);
%  evaluate derivatives etc
         [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
         [flowx,flowy] = gauss_transprt(sigpt,tigpt,xl_v,yl_v);
		 for j = 1:4
               for i = 1:4
    de(:,i,j) = de(:,i,j) + flowx(:).*dphidx(:,i).*flowx(:).*dphidx(:,j).*invjac(:);
    de(:,i,j) = de(:,i,j) + flowy(:).*dphidy(:,i).*flowx(:).*dphidx(:,j).*invjac(:);
    de(:,i,j) = de(:,i,j) + flowx(:).*dphidx(:,i).*flowy(:).*dphidy(:,j).*invjac(:);
    de(:,i,j) = de(:,i,j) + flowy(:).*dphidy(:,i).*flowy(:).*dphidy(:,j).*invjac(:);
               end
	    end
% end of Gauss point loop
         end
%
% scale with the appropriate parameter
      acte = find(isfinite(expe));
      factor = expe(acte); flow_h=eph(acte); flow_l2=epw(acte);
	  lpe =zeros(nel,1); lpe(acte)= factor.*(flow_h./flow_l2);
		 for j = 1:4
               for i = 1:4
               de(:,i,j) = lpe(:) .* de(:,i,j);
		   end
	   end
%
% perform assembly of global matrix  and source vector
      for krow=1:4
	  nrow=ev(:,krow);
          for kcol=1:4
		  ncol=ev(:,kcol);
          d = d + sparse(nrow,ncol,de(:,krow,kcol),nvtx,nvtx);
          end
      end
%
return


function [aVals,aJacob,aHess,papt]=aSpiralSimp(xPoints,numRetDims)
%%ASPIRALSIMP The drift function for a non-ballistic spiraling target
%         motion model in 3 dimensions, formulated in a manner such that it
%         is simple to make a spiraling target follow a nominal trajectory.
%         This model is probably bad to design into a target tracking
%         algorithm, but might be good for designing spiraling trajectories
%         to use to test target tracking algorithms.
%
%INPUTS: xPoints The target state at time t. It consists of position
%                (3 elements), instantaneous velocity (3 elements), the
%                velocity vector (3 elements) of the overall direction of
%                motion of the spiraling model (ground speed) and the
%                spiral rate of the target. Thus xDim=10. If x is an
%                xDim X numStates matrix, then the spiraling model is
%                evaluated for all of the state vectors. The last 4
%                elements in xPoints are taken to be constants and thus
%                aVal entries will be 0 for them. If only aVals is
%                requested on the output, then xPoints can be a matrix of
%                points.
%     numRetDims If numRetDims=6, then the last 4 elements in xPoints (the
%                velocity vector of the overall spiraling model and the
%                spiral rate) are taken to be constants and the returned
%                aVals Vec is 6X1. Otherwise, numRetDims can be 10 and
%                aVals is 10X1. The default if omitted or an empty matrix
%                is passed is 6.
%
%OUTPUTS: aVals The 6X1 (or 10X1 depending on numRetDims) flat-Earth time-
%               derivative of the state. If xPoints was a matrix of N
%               points, then this will be 6XN (or 10XN).
%        aJacob The 6X6 (or 10X10) matrix of partial derivatives of aVals
%               such that aJacob(:,k) is the partial derivative of
%               aVals(:,k) with respect to
%               xPoints(k). xPoints can't be a matrix if this output is
%               requested.
%         aHess The 6X6X6 (or 10X10X10) matrix of second derivatives of
%               aVals such that aHess(:,k1,k2) is the second partial
%               derivative of aVals with respect to xPoints(k1) and
%               xPoints(k2).
%          papt The 6X1 or 10X1 derivative with resect to time of aVals.
%               This is all zeros, because the model is time invariant.
%
%A derivation of the simplified flat-Earth spiraling dynamic model is given
%in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<3||isempty(numRetDims))
        numRetDims=6;
    end

    if(numRetDims<6||numRetDims>10)
       error('numRetDims must be between 6 and 10.')
    end

    numPoints=size(xPoints,2);
    aVals=zeros(numRetDims,numPoints);

    vl=xPoints(7:9,:);
    omega=xPoints(10,:);

    vlMag2=sum(vl.*vl,1);
    vlMag=sqrt(vlMag2);

    aVals(1:3,:)=xPoints(4:6,:);
    vs=xPoints(4:6,:)-vl;
    ul=bsxfun(@rdivide,vl,vlMag);
    vsDot=omega.*bsxfun(@cross,ul,vs);
    aVals(4:6,:)=vsDot;

    if(nargout>1)
        rdotx=xPoints(4);
        rdoty=xPoints(5);
        rdotz=xPoints(6);

        vlx=vl(1);
        vly=vl(2);
        vlz=vl(3);

        vlx2=vlx*vlx;
        vly2=vly*vly;
        vlz2=vlz*vlz;

        vlMag2=vlMag*vlMag;
        vlMag3=vlMag2*vlMag;

        pXrdy=-(omega*vlz)/vlMag;
        pXrdz=(omega*vly)/vlMag;

        pYrdx=(omega*vlz)/vlMag;
        pYrdz=-((omega*vlx)/vlMag);

        pZrdx=-((omega*vly)/vlMag);
        pZrdy=(omega*vlx)/vlMag;

        if(numRetDims==6)
            aJacob=[0,0,0,1,        0,      0;
                    0,0,0,0,        1,      0;
                    0,0,0,0,        0,      1;
                    0,0,0,0,        pXrdy,  pXrdz;
                    0,0,0,pYrdx,    0,      pYrdz;
                    0,0,0,pZrdx,    pZrdy,  0];
        else
            pXvx=(omega*vlx*(-rdotz*vly+rdoty*vlz))/vlMag3;
            pXvy=(omega*(rdoty*vly*vlz+rdotz*(vlx2+vlz2)))/vlMag3;
            pXvz=-((omega*(rdoty*(vlx2+vly2)+rdotz*vly*vlz))/vlMag3);

            pXomega=(rdotz*vly-rdoty*vlz)/vlMag;

            pYvx=-((omega*(rdotx*vlx*vlz+rdotz*(vly2+vlz2)))/vlMag3);
            pYvy=(omega*vly*(rdotz*vlx-rdotx*vlz))/vlMag3;
            pYvz=(omega*(rdotx*(vlx2+vly2)+rdotz*vlx*vlz))/vlMag3;
            pYomega=(-rdotz*vlx+rdotx*vlz)/vlMag;

            pZvx=(omega*(rdotx*vlx*vly+rdoty*(vly2+vlz2)))/vlMag3;
            pZvy=-((omega*(rdoty*vlx*vly+rdotx*(vlx2+vlz2)))/vlMag3);
            pZvz=(omega*(-rdoty*vlx+rdotx*vly)*vlz)/vlMag3;
            pZomega=(rdoty*vlx-rdotx*vly)/vlMag;

            aJacob=[0,0,0,1,        0,      0,      0,      0,      0,      0;
                    0,0,0,0,        1,      0,      0,      0,      0,      0;
                    0,0,0,0,        0,      1,      0,      0,      0,      0;
                    0,0,0,0,        pXrdy,  pXrdz,  pXvx,   pXvy,   pXvz,   pXomega;
                    0,0,0,pYrdx,    0,      pYrdz,  pYvx,   pYvy,   pYvz,   pYomega;
                    0,0,0,pZrdx,    pZrdy,  0,      pZvx,   pZvy,   pZvz,   pZomega;
                    zeros(4,10)];
        end

        if(nargout>2)
            aHess=zeros(numRetDims,numRetDims,numRetDims);

            vlMag5=vlMag3*vlMag2;

            %Second deriavtives are all 0 if only
            if(numRetDims>6)
                pXrdyvx=(omega*vlx*vlz)/vlMag3;
                pXrdyvy=(omega*vly*vlz)/vlMag3;
                pXrdyvz=-((omega*(vlx2+vly2))/vlMag3);
                pXrdyOmega=-(vlz/vlMag);

                pXrdzvx=-((omega*vlx*vly)/vlMag3);
                pXrdzvy=(omega*(vlx2+vlz2))/vlMag3;
                pXrdzvz=-((omega*vly*vlz)/vlMag3);
                pXrdzOmega=vly/vlMag;

                pXvxrdx=0;
                pXvxrdy=pXrdyvx;
                pXvxrdz=pXrdzvx;
                pXvxvx=-((omega*(rdotz*vly-rdoty*vlz)*(-2*vlx2+vly2+vlz2))/vlMag5);
                pXvxvy=-((omega*vlx*(3*rdoty*vly*vlz+rdotz*(vlx2-2*vly2+vlz2)))/vlMag5);
                pXvxvz=(omega*vlx*(3*rdotz*vly*vlz+rdoty*(vlx2+vly2-2*vlz2)))/vlMag5;
                pXvxOmega=(-rdotz*vlx*vly+rdoty*vlx*vlz)/vlMag3;

                pXvyrdx=0;
                pXvyrdy=pXrdyvy;
                pXvyrdz=pXrdzvy;
                pXvyvx=pXvxvy;
                pXvyvy=(omega*(-3*rdotz*vly*(vlx2+vlz2)+rdoty*vlz*(vlx2-2*vly2+vlz2)))/vlMag5;
                pXvyvz=(omega*(rdoty*vly*(vlx2+vly2-2*vlz2)-rdotz*vlz*(vlx2-2*vly2+vlz2)))/vlMag5;
                pXvyOmega=(rdoty*vly*vlz+rdotz*(vlx2+vlz2))/vlMag3;

                pXvzrdx=0;
                pXvzrdy=pXrdyvz;
                pXvzrdz=pXrdzvz;
                pXvzvx=pXvxvz;
                pXvzvy=pXvyvz;
                pXvzvz=(omega*(3*rdoty*(vlx2+vly2)*vlz-rdotz*vly*(vlx2+vly2-2*vlz2)))/vlMag5;
                pXvzOmega=(-rdoty*(vlx2+vly2)-rdotz*vly*vlz)/vlMag3;

                pXOmegardx=0;
                pXOmegardy=pXrdyOmega;
                pXOmegardz=pXrdzOmega;
                pXOmegavx=pXvxOmega;
                pXOmegavy=pXvyOmega;
                pXOmegavz=pXvzOmega;
                pXOmegaOmega=0;

                pYrdxvx=-((omega*vlx*vlz)/vlMag3);
                pYrdxvy=-((omega*vly*vlz)/vlMag3);
                pYrdxvz=(omega*(vlx2+vly2))/vlMag3;
                pYrdxOmega=vlz/vlMag;

                pYrdzvx=-((omega*(vly2+vlz2))/vlMag3);
                pYrdzvy=(omega*vlx*vly)/vlMag3;
                pYrdzvz=(omega*vlx*vlz)/vlMag3;
                pYrdzOmega=-(vlx/vlMag);

                pYvxrdx=pYrdxvx;
                pYvxrdy=0;
                pYvxrdz=pYrdzvx;
                pYvxvx=(omega*(3*rdotz*vlx*(vly2+vlz2)-rdotx*vlz*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pYvxvy=(omega*vly*(3*rdotx*vlx*vlz+rdotz*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pYvxvz=(omega*(-rdotx*vlx*(vlx2+vly2-2*vlz2)+rdotz*vlz*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pYvxOmega=(-rdotx*vlx*vlz-rdotz*(vly2+vlz2))/vlMag3;

                pYvyrdx=pYrdxvy;
                pYvyrdy=0;
                pYvyrdz=pYrdzvy;
                pYvyvx=pYvxvy;
                pYvyvy=(omega*(rdotz*vlx-rdotx*vlz)*(vlx2-2*vly2+vlz2))/vlMag5;
                pYvyvz=-((omega*vly*(3*rdotz*vlx*vlz+rdotx*(vlx2+vly2-2*vlz2)))/vlMag5);
                pYvyOmega=(vly*(rdotz*vlx-rdotx*vlz))/vlMag3;

                pYvzrdx=pYrdxvz;
                pYvzrdy=0;
                pYvzrdz=pYrdzvz;
                pYvzvx=pYvxvz;
                pYvzvy=pYvyvz;
                pYvzvz=(omega*(-3*rdotx*(vlx2+vly2)*vlz+rdotz*vlx*(vlx2+vly2-2*vlz2)))/vlMag5;
                pYvzOmega=(rdotx*(vlx2+vly2)+rdotz*vlx*vlz)/vlMag3;

                pYOmegardx=pYrdxOmega;
                pYOmegardy=0;
                pYOmegardz=pYrdzOmega;
                pYOmegavx=pYvxOmega;
                pYOmegavy=pYvyOmega;
                pYOmegavz=pYvzOmega;
                pYOmegaOmega=0;

                pZrdxvx=(omega*vlx*vly)/vlMag3;
                pZrdxvy=-((omega*(vlx2+vlz2))/vlMag3);
                pZrdxvz=(omega*vly*vlz)/vlMag3;
                pZrdxOmega=-(vly/vlMag);

                pZrdyvx=(omega*(vly2+vlz2))/vlMag3;
                pZrdyvy=-((omega*vlx*vly)/vlMag3);
                pZrdyvz=-((omega*vlx*vlz)/vlMag3);
                pZrdyOmega=vlx/vlMag;

                pZvxrdx=pZrdxvx;
                pZvxrdy=pZrdyvx;
                pZvxrdz=0;
                pZvxvx=(omega*(-3*rdoty*vlx*(vly2+vlz2)+rdotx*vly*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pZvxvy=(omega*(rdotx*vlx*(vlx2-2*vly2+vlz2)-rdoty*vly*(-2*vlx2+vly2+vlz2)))/vlMag5;
                pZvxvz=-((omega*vlz*(3*rdotx*vlx*vly+rdoty*(-2*vlx2+vly2+vlz2)))/vlMag5);
                pZvxOmega=(rdotx*vlx*vly+rdoty*(vly2+vlz2))/vlMag3;

                pZvyrdx=pZrdxvy;
                pZvyrdy=pZrdyvy;
                pZvyrdz=0;
                pZvyvx=pZvxvy;
                pZvyvy=(omega*(3*rdotx*vly*(vlx2+vlz2)-rdoty*vlx*(vlx2-2*vly2+vlz2)))/vlMag5;
                pZvyvz=(omega*vlz*(3*rdoty*vlx*vly+rdotx*(vlx2-2*vly2+vlz2)))/vlMag5;
                pZvyOmega=(-rdoty*vlx*vly-rdotx*(vlx2+vlz2))/vlMag3;

                pZvzrdx=pZrdxvz;
                pZvzrdy=pZrdyvz;
                pZvzrdz=0;
                pZvzvx=pZvxvz;
                pZvzvy=pZvyvz;
                pZvzvz=-((omega*(rdoty*vlx-rdotx*vly)*(vlx2+vly2-2*vlz2))/vlMag5);
                pZvzOmega=(-rdoty*vlx*vlz+rdotx*vly*vlz)/vlMag3;

                pZOmegardx=pZrdxOmega;
                pZOmegardy=pZrdyOmega;
                pZOmegardz=0;
                pZOmegavx=pZvxOmega;
                pZOmegavy=pZvyOmega;
                pZOmegavz=pZvzOmega;
                pZOmegaOmega=0;

                aHess(:,:,4)=[zeros(3,10);
                              0,0,0,0,0,0,  pXvxrdx,   pXvyrdx,   pXvzrdx,   pXOmegardx;
                              0,0,0,0,0,0,  pYvxrdx,   pYvyrdx,   pYvzrdx,   pYOmegardx;
                              0,0,0,0,0,0,  pZvxrdx,   pZvyrdx,   pZvzrdx,   pZOmegardx;
                              zeros(4,10)];
                aHess(:,:,5)=[zeros(3,10);
                              0,0,0,0,0,0,  pXvxrdy,   pXvyrdy,   pXvzrdy,   pXOmegardy;
                              0,0,0,0,0,0,  pYvxrdy,   pYvyrdy,   pYvzrdy,   pYOmegardy;
                              0,0,0,0,0,0,  pZvxrdy,   pZvyrdy,   pZvzrdy,   pZOmegardy;
                              zeros(4,10)];
                aHess(:,:,6)=[zeros(3,10);
                              0,0,0,0,0,0,  pXvxrdz,   pXvyrdz,   pXvzrdz,   pXOmegardz;
                              0,0,0,0,0,0,  pYvxrdz,   pYvyrdz,   pYvzrdz,   pYOmegardz;
                              0,0,0,0,0,0,  pZvxrdz,   pZvyrdz,   pZvzrdz,   pZOmegardz;
                              zeros(4,10)];
                aHess(:,:,7)=[zeros(3,10);
                              0,0,0,0,          pXrdyvx,  pXrdzvx,  pXvxvx,   pXvyvx,   pXvzvx,   pXOmegavx;
                              0,0,0,pYrdxvx,    0,        pYrdzvx,  pYvxvx,   pYvyvx,   pYvzvx,   pYOmegavx;
                              0,0,0,pZrdxvx,    pZrdyvx,  0,        pZvxvx,   pZvyvx,   pZvzvx,   pZOmegavx;
                              zeros(4,10)];
                aHess(:,:,8)=[zeros(3,10);
                              0,0,0,0,          pXrdyvy,  pXrdzvy,  pXvxvy,   pXvyvy,   pXvzvy,   pXOmegavy;
                              0,0,0,pYrdxvy,    0,        pYrdzvy,  pYvxvy,   pYvyvy,   pYvzvy,   pYOmegavy;
                              0,0,0,pZrdxvy,    pZrdyvy,  0,        pZvxvy,   pZvyvy,   pZvzvy,   pZOmegavy;
                              zeros(4,10)];
                aHess(:,:,9)=[zeros(3,10);
                              0,0,0,0,          pXrdyvz,  pXrdzvz,  pXvxvz,   pXvyvz,   pXvzvz,   pXOmegavz;
                              0,0,0,pYrdxvz,    0,        pYrdzvz,  pYvxvz,   pYvyvz,   pYvzvz,   pYOmegavz;
                              0,0,0,pZrdxvz,    pZrdyvz,  0,        pZvxvz,   pZvyvz,   pZvzvz,   pZOmegavz;
                              zeros(4,10)];
               aHess(:,:,10)=[zeros(3,10);
                              0,0,0,0,             pXrdyOmega,  pXrdzOmega,  pXvxOmega,   pXvyOmega,   pXvzOmega,   pXOmegaOmega;
                              0,0,0,pYrdxOmega,    0,           pYrdzOmega,  pYvxOmega,   pYvyOmega,   pYvzOmega,   pYOmegaOmega;
                              0,0,0,pZrdxOmega,    pZrdyOmega,  0,           pZvxOmega,   pZvyOmega,   pZvzOmega,   pZOmegaOmega;
                              zeros(4,10)];
            end
            if(nargout>3)
                papt=zeros(10,1);
            end
        end
    end
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.