function [v_INTERP,ind_Start,ind_End] = FitzHugh_Nagumo_1d(dt_IBM,T_final)
%function FitzHugh_Nagumo_1d(dt_IBM,T_final)

%
% This script solves the FitzHugh-Nagumo Equations in 1d, which are
% a reduced order model of the more complicated Hodgkin-Huxley Equations.
%
% Author:  Nick Battista
% Created: 09/11/2015
% University: UNC-CH
%
% Equations:
% dv/dt = D*Laplacian(v) + v*(v-a)*(v-1) - w - I(t)
% dw/dt = eps*(v-gamma*w)
%
% Inputs:
% dt_IBM: time-step from immersed boundary code
% T_final: final time for immersed boundary simulation
%
% Variables & Parameters:
% v(x,t): membrane potential
% w(x,t): blocking mechanism
% D:      diffusion rate of potential
% a:      threshold potential
% gamma:  resetting rate
% eps:    strength of blocking
% I(t):   initial condition for applied activation
%

%USER-INPUT FROM MODEL:
numPtsAlongTube = 78;    % # of Lag. Muscle Pts Along Tube
ind_Start = 155;         % first index for muscle between tube in spring count
ind_End = 232;           % last index for muscle between tube in spring count
dt_IBM = dt_IBM*2500;    % scale time-info
T_final_Ca = T_final;    % Stays on time-scale of IBM
T_final = T_final * 2500;% scale time-info

% Save Movie?
PLAY_MOVIE = 0; % (1 for YES, 0 for NO)

% Parameters in model %
D = 10.0;       % Diffusion coefficient
a = 0.3;        % Threshold potential (Note: a=0.3 is traveling wave value, a=0.335 is interesting)
gamma = 1.0;    % Resetting rate (Note: large values give 'funky thick' traveling wave, gamma = 1.0 is desired)
eps = 0.001;    % Blocking strength (Note: eps = 0.001 is desired)
I_mag = 0.05;   % Activation strength

% Discretization/Simulation Parameters %
factor = 4;     % 4x the resolution of the FHN model than IBM for tube
N = factor*numPtsAlongTube-1; % # of discretized points for FHN model (finer resolution than IBM mesh)
L = 500;        % Length of domain, [0,L]
dx = L/N;       % Spatial Step
x = 0:dx:L;     % Computational Domain

% Temporal  Parameters %
Np = 10;              % Set the number of pulses
pulse = T_final/Np;   % determines the length of time between pulses.
numFHN = 10;          % # that relates # of time-steps of FHN to IBM
dt = dt_IBM / numFHN; % Time-step for FitzHugh-Nagumo (fraction of IBM time-step)
NT = T_final / dt;    % Total # of time-steps to be taken for FHN
NT_IBM = T_final / dt_IBM; % Total # of time-steps for IBM
i1 = 0.25;%0.475;     % fraction of total length where current starts
i2 = 0.3;%0.525;      % fraction of total length where current ends
dp = pulse/50;        % Set the duration of the current pulse
pulse_time = 0;       % pulse time is used to store the time that the next pulse of current will happen
IIapp=zeros(1,N+1);   % this vector holds the values of the applied current along the length of the neuron
dptime = T_final/100; % This sets the length of time frames that are saved to make a movie.

% Initialization %
v = zeros(1,N+1);
w = v;
t=0;
ptime = 0;
store = 1;       % counter for storing data to match time-steps from IBM
tVec = 0:dt:T_final;
Nsteps = length(tVec);
vStore = zeros(NT_IBM,N+1); vStore(store,:) = v;
wStore = zeros(NT_IBM,N+1); wStore(store,:) = w;
IIappStore=vStore;
store = store+1; % update storage counter

% Compute Calcium-Dynamics a-priori for activation wave!
% (returns Ca: free calcium ions, Caf: bound to filaments Ca-ions)
fprintf('     --> Solving Calcium Dynamics Model\n');
[Ca,Caf] = Calcium_Dynamics(Nsteps,T_final_Ca);
fprintf('     --> Finished calculating Calcium Dynamics\n');

%
% **** % **** BEGIN SIMULATION! **** % **** %
%
for i=2:Nsteps;

     % Update the time
    t = t+dt;

    % Give Laplacian
    DD_v_p = give_Me_Laplacian(v,dx);

    % Gives activation wave (either from prescribed or Calcium-Dynamics model)
    %[IIapp,pulse_time] = Iapp(pulse_time,i1,i2,I_mag,N,pulse,dp,t,IIapp);
    IIapp = Iapp_from_Calcium_Dynamics(i,i1,i2,N,Caf);

    % Update potential and blocking mechanism, using Forward Euler
    vN = v + dt * ( D*DD_v_p - v.*(v-a).*(v-1) - w + IIapp );
    wN = w + dt * ( eps*( v - gamma*w ) );

    % Update time-steps
    v = vN;
    w = wN;

    % Store time-step values
    if mod(i-1,numFHN) == 0
        vStore(store,:) = v;
        wStore(store,:) = w;
        IIappStore(store,:) = IIapp;
        store = store + 1;
    end

    % PLAY MOVIE?
    if PLAY_MOVIE == 1
        %This is used to determine if the current time step will be a frame in the movie
        if t > ptime,
            figure(1)
            plot(x, v);
            axis([0 L -0.5 1.5]);
            xlabel('Distance (x)');
            ylabel('Electropotenital (v)');
            ptime = ptime+dptime;
            fprintf('Time(s): %d\n',t);
            pause(0.01);
        end
    end

end %END TIME-STEPPING LOOP

% Compute electro-potential at associated Lagrangian Pts.
v_INTERP = compute_IBM_Potential_At_IBM_Lag_Pts(factor,numPtsAlongTube,vStore);
%IIapp_INTERP = compute_IBM_Potential_At_IBM_Lag_Pts(factor,numPtsAlongTube,IIappStore);

% TEST SOLUTION
%  pause();
%  x=1:1:78;
%  for i=1:10:length(v_INTERP(:,1))
%     plot(x,v_INTERP(i,:),'-');
%     %plot(x,IIapp_INTERP(i,:),'-');
%     axis([0 79 -0.5 1.5]);
%     pause(0.01);
%  end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: computes the electro-potential, v(x,t), at the correct points
% along the IBM's Lagrangian Structure
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [v_INTERP] = compute_IBM_Potential_At_IBM_Lag_Pts(factor,numPtsAlongTube,vStore)

% v: row: electro-potential
% factor: resolution of FHN:IBM
% vStore: membrane potential along tube

v_INTERP = zeros(length(vStore(:,1)),numPtsAlongTube);

ct = 1;
for j=1:length(vStore(1,:))
   if ( mod(j-1,factor) == 0 )
       v_INTERP(:,ct) = vStore(:,j);
       ct = ct+1;
   end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: the CALCIUM inspired injection function, Iapp = activation wave
% for system, and returns the activation signal between i1 and i2 in
% geometry along tube
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function IIapp = Iapp_from_Calcium_Dynamics(ith_step,i1,i2,N,Caf)

    % Resets activation to zero
    IIapp = zeros(1,N+1);

    % Activates the proper region based on Calcium-Dynamics
    for j=(floor(i1*N):floor(i2*N))
        coeff = 0.5;                 % Coefficient to scale activation wave accordingly
        IIapp(j) = coeff*Caf(ith_step); % Activation of bound Calcium to filaments
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: the injection function, Iapp = activation wave for system, and
% returns both the activation as well as updated pulse_time
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [app,pulse_time] = Iapp(pulse_time,i1,i2,I_mag,N,pulse,dp,t,app)

    %Check to see if there should be a pulse
    if t > (pulse_time),

        % Sets pulsing region to current amplitude of I_mag x\in[i1*N,i2*N]
        for j=(floor(i1*N):floor(i2*N)),
            app(j) = I_mag;
        end

        % Checks if the pulse is over & then resets pulse_time to the next pulse time.
        if t > (pulse_time+dp),
            pulse_time = pulse_time+pulse;
        end

    else

        % Resets to no activation
        app = zeros(1,N+1);

    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% FUNCTION: gives Laplacian of the membrane potential, note: assumes
% periodicity and uses the 2nd order central differencing operator.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function DD_v = give_Me_Laplacian(v,dx)

Npts = length(v);
DD_v = zeros(1,Npts);

for i=1:Npts
   if i==1
       %DD_v(i) = ( v(i+1) - 2*v(i) + v(end) ) / dx^2;
       DD_v(i) = ( v(i+1) - 2*v(i) + 0 ) / dx^2;
   elseif i == Npts
       %DD_v(i) = ( v(1) - 2*v(i) + v(i-1) ) / dx^2;
       DD_v(i) = ( 0 - 2*v(i) + v(i-1) ) / dx^2;
   else
       DD_v(i) = ( v(i+1) - 2*v(i) +  v(i-1) ) /dx^2;
   end

end


function laguerre_polynomial_test06 ( )

%*****************************************************************************80
%
%% LAGUERRE_POLYNOMIAL_TEST06 tests LM_POLYNOMIAL_COEFFICIENTS.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 March 2012
%
%  Author:
%
%    John Burkardt
%
  n = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LAGUERRE_POLYNOMIAL_TEST06\n' );
  fprintf ( 1, '  LM_POLYNOMIAL_COEFFICIENTS determines polynomial coefficients of Lm(n,m,x).\n' );

  for m = 0 : 4

    c = lm_polynomial_coefficients ( n, m );

    for i = 0 : n
      fprintf ( 1, '\n' );
      fprintf ( 1, '  Lm(%d,%d) = \n', i, m );
      fprintf ( 1, '\n' );
      for j = i : -1 : 0
        if ( c(i+1,j+1) == 0.0 )

        elseif ( j == 0 )
          fprintf ( 1, '  %g\n', c(i+1,j+1) );
        elseif ( j == 1 )
          fprintf ( 1, '  %g * x\n', c(i+1,j+1) );
        else
          fprintf ( 1, '  %g * x^%d\n', c(i+1,j+1), j );
        end
      end
    end

  end

  return
end


function zc = zerocross(x,wintype,winamp,winlen)
%ENERGY   Short-time energy computation.
%   y = ZEROCROSS(X,WINTYPE,WINAMP,WINLEN) computes the short-time enery of
%   the sequence X.
%
%   WINTYPE defines the window type. RECTWIN, HAMMING, HANNING, and
%   BLACKAMN are the possible choices. WINAMP sets the amplitude of the
%   window and the length of the window is WINLEN.
%
%   See also RECTWIN, HAMMING, HANNING, BARTLETT, BLACKMAN.
%
%   Author: Nabin Sharma
%   Date: 2009/03/15

error(nargchk(1,4,nargin,'struct'));

% generate x[n] and x[n-1]
x1 = x;
x2 = [0, x(1:end-1)];

% generate the first difference
firstDiff = sgn(x1)-sgn(x2);

% magnitude only
absFirstDiff = abs(firstDiff);

% lowpass filtering with window
zc = winconv(absFirstDiff,wintype,winamp,winlen);


%This Matlab script can be used to reproduce Figure 2.8 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BS antennas
M = 100;

%Angular standard deviation in the local scattering model (in degrees)
ASDs = [10 30];

%Set the nominal angle of the desired UE
varphiDesired = pi/6;

%Set range of nominal angles of the interfering UE
varphiInterfererDegrees = -180:1:180;
varphiInterfererRadians = varphiInterfererDegrees*(pi/180);

%Define the antenna spacing (in number of wavelengths)
antennaSpacing = 1/2; %Half wavelength distance

%Preallocate matrix for storing the simulation results
variance = zeros(length(varphiInterfererRadians),length(ASDs));

%% Go through the range of ASDs
for n = 1:length(ASDs)

    %Output simulation progress
    disp([num2str(n) ' ASDs out of ' num2str(length(ASDs))]);

    %Compute spatial correlation matrix of the desired UE
    R1 = functionRlocalscattering(M,varphiDesired,ASDs(n),antennaSpacing);

    %Go through all angles of the interfering UE
    for r = 1:length(varphiInterfererRadians)

        %Compute spatial correlation matrix of the interfering UE
        R2 = functionRlocalscattering(M,varphiInterfererRadians(r),ASDs(n),antennaSpacing);

        %Compute variance of favorable propagation according to (2.19)
        variance(r,n) = real(trace(R1*R2)/(trace(R1)*trace(R2)));

    end

end

%% Plot the simulation results
figure;
hold on; box on;

plot(varphiInterfererDegrees,variance(:,1),'r--','LineWidth',1);
plot(varphiInterfererDegrees,variance(:,2),'b-.','LineWidth',1);
plot(varphiInterfererDegrees,1/M*ones(length(varphiInterfererDegrees),1),'k-','LineWidth',1);

xlabel('Angle of interfering UE [degree]');
ylabel('Variance in (2.19)');
xlim([-180 180]);

legend('Gaussian, ASD 10^o','Gaussian, ASD 30^o','Uncorrelated','Location','NorthWest');


function c=counts(x,v)
% COUNTS counts the number of elements of x that fall within v
% COUNTS(x,v) returns the number of elements such that
% v(1)<=x, ....v(n-1)<x<=v(n)
c=[];
nv=[-Inf; v];
for i=2:rows(nv);
   t=sum(x<=nv(i) & x>nv(i-1));
   c(i-1,1)=t;
end


% magnitude of velocity of tail
function [data,units] = compute_velmag_tail(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  % location of tail
  tailx = trx(fly).x_mm + 2*cos(-trx(fly).theta).*trx(fly).a_mm;
  taily = trx(fly).y_mm + 2*sin(-trx(fly).theta).*trx(fly).a_mm;
  dx = diff(tailx);
  dy = diff(taily);

  if trx(fly).nframes < 2,
    data{i} = [];
  else
    % magnitude of velocity vector
    data{i} = sqrt(dx.^2 + dy.^2)./trx(fly).dt;
  end
end
units = parseunits('mm/s');


function [vertexIndices,distSq] = GetPointsWithinSphere(point,radius,xyzData)

% CHECK INPUTS
assert(numel(point) == 3,   'Point must be specified as a 3D point [3x1]');
assert(size(xyzData,2) == 3,'Vertex data must be specified a list of 3D vertices [nx3]');
assert(numel(radius) == 1,  'The radius is a scalar comparitor.');

% DEFAULT VALUES
% internalPoints = xyzData;               % Return the full set
vertexIndices = 1:1:size(xyzData,1);      % Return a substitute index vector
distSq = inf(1,3);

% DEFAULT RESPONSE
if isinf(radius)
    return
end
% VECTORISED SQUARED DISTANCE CALCULATION
distSq = (xyzData(:,1) - point(1)).^2 + (xyzData(:,2) - point(2)).^2 + (xyzData(:,3) - point(3)).^2;
% SQUARED DISTANCE COMPARITOR
ind = distSq < radius^2;
% THE REDUCED POINT CLOUD INSIDE THE RADIUS
[vertexIndices,~] = find(ind);
end


function r = csc(a)
%CSC          Taylor cosecans  csc(a)
%
%Thanks to George Corliss for providing the Taylor expansion
%

% written  06/03/09     S.M. Rump
% modified 08/26/12     S.M. Rump  global variables removed
%

  e = 1e-30;
  if 1+e==1-e                   % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  K = getappdata(0,'INTLAB_TAYLOR_ORDER');

  ct = a.t;
  st = a.t;
  r = a;
  N = size(a.t,2);
  st(1,:) = sin(a.t(1,:));
  ct(1,:) = cos(a.t(1,:));
  r.t(1,:) = 1 ./ sin(a.t(1,:));
  st1 = -st(1,:);
  for j=2:K+1
    at_ = a.t(2:j,:);           % some 3 % faster
    st(j,:) = sum( repmat((1:j-1)',1,N).*ct(j-1:-1:1,:).*at_ , 1 ) ./ (j-1);
    if j~=K+1
      ct(j,:) = - sum( repmat((1:j-1)',1,N).*st(j-1:-1:1,:).*at_ , 1 ) ./ (j-1);
    end
    r.t(j,:) = sum( r.t(1:j-1,:).*st(j:-1:2,:) , 1 ) ./ st1;
  end

  if rndold
    setround(rndold)
  end


function [sol, infos] = gsp_ml_rls_tv(G, xl, y, k, tau,lambda, A, At, param)
%GSP_ML_RLS_TV Manifold Learning regularized least square with TV regularization
%   Usage: sol = gsp_ml_rls_tv(G, xl, y, k, tau,lambda);
%          sol = gsp_ml_rls_tv(G, xl, y, k, tau, lambda, A, At);
%          sol = gsp_ml_rls_tv(G, xl, y, k, tau, lambda, A, At, param);
%          [sol, infos] = gsp_ml_rls_tv(...)
%
%   Input parameters:
%       G       : Graph
%       xl      : labeled points
%       y       : labels
%       k       : kernel
%       tau     : regularization parameters
%       lambda  : regularization parameters
%       A       : Operator
%       At      : Adoint operator
%       param   : Optional parameters
%   Output parameters:
%       sol     : solution of the problem (kernel coefficients)
%       infos   : convergence info
%
%   *param* is a structure of optional argument given to the solver
%   gradient_descent. Please see the function gradient descent for more
%   information.
%
%   In *param*, you also have to set an upperbound for the operator A as
%   param.nu!
%
%   This function solves the following problem:
%
%   ..  argmin_alpha  || A (K alpha) - y ||_2^2
%                       + tau *alpha^T K alpha
%                       + lambda || L K alpha ||_TVG
%
%   If tau is set to zero, then the following problem is solved
%
%   ..  argmin_alpha alpha^T K alpha
%                      + lambda || L K alpha ||_TVG
%                      s. t.  A (K alpha) = y
%

% Author: Nathanael Perraudin
% Date  : 8 decembre 2014

if nargin<7
    A = @(x) x;
end

if nargin<8
    At = A;
end

if nargin<9
    param = struct;
end

if ~isfield(param, 'tol'), param.tol = 1e-6; end
if ~isfield(param, 'nu'), param.nu = 1; end
if ~isfield(param, 'verbose'), param.verbose = 1; end

N = size(xl,2);

% Evaluate the kernel on the data points
K = gsp_rkhs_evaluate(k,xl);
nu = norm(K);

alpha_in = zeros(N,size(y,2));

if ~isfield(G,'D');
    G = gsp_adj2vec(G);
end
if ~isfield(G,'lmax');
    G = gsp_estimate_lmax(G);
end

paramtv.verbose = param.verbose - 1;
ftv.eval = @(x) sum(lambda*gsp_norm_tv(G,K*x));
ftv.prox = @(x,T) gsp_prox_tv(x,lambda*T,G,paramtv);

if tau >0

    fp.eval = @(x) tau * sum(gsp_norm_tik(K,x));
    fp.grad = @(x) 2*tau*K*x;

    ffid.eval = @(x) norm(A(K*x)-y,'fro')^2;
    ffid.grad = @(x) 2*K'*At(A(K*x)-y);

    ftot.grad = @(x) ffid.grad(x) + fp.grad(x);
    ftot.eval = @(x) ffid.eval(x) + fp.eval(x);

    param.gamma = 0.5/(tau*nu+nu^2*param.nu^2+lambda*nu*G.lmax);

    [sol,infos] = forward_backward(alpha_in, ftv, ftot, param);
else

    fp.eval = @(x)  x'*K*x;
    fp.grad = @(x) 2*K*x;

    paramproj.A = @(x) A(K*x);
    paramproj.At = @(x) K'*At(x);
    paramproj.nu = nu^2*param.nu^2;
    paramproj.tight = 0;
    paramproj.verbose = param.verbose-1;
    paramproj.y = y;
    paramproj.maxit = 50;
    ffid.eval = @(x) eps;
    ffid.prox = @(x,T) proj_b2(x,T,paramproj);

    param.gamma = 0.5/(nu+lambda*nu*G.lmax);

    [sol,infos] = generalized_forward_backward(alpha_in, {ffid,ftv}, fp, param);

end

end


function RDot = rotationDerivativeFromAngVel(omega,R)

    % ROTATIONDERIVATIVEFROMANGVEL computes the derivative of a rotation matrix
    %                              given the angular velocity. It makes use of
    %                              the following convention: if the rotation of
    %                              a body b is expressed in the WORLD FRAME w
    %                              (i.e., R = w_R_b), then this function is
    %                              expecting the angular velocity of the body
    %                              to be expressed in the WORLD FRAME, i.e.
    %                              omega = w_omega.
    %
    % USAGE: please note that this function has been designed for being inserted
    %        in a Simulink model.
    %
    % FORMAT: RDot = rotationDerivativeFromAngVel(omega,R)
    %
    % INPUT:  - omega = [3 * 1] angular velocity
    %         - R = [3 * 3] rotation matrix
    %
    % OUTPUT: - RDot = [3 * 3] rotation matrix derivative
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    % Rotation matrix derivative (with correction to keep the integration
    % inside the space of rotation matrices). Omega is assumed to be w.r.t.
    % the inertial frame, i.e. w_omega.
    kCorr   = 1;
    RDot    = wbc.skew(omega)*R +kCorr*(eye(3)-R*transpose(R))*R;
end


% simple wrapper for softmax function
function [y] = softmax(x)

logZ = logsum(x, 2);
y = exp(bsxfun(@minus, x, logZ));


function [] = ControlPanel()

% initialize control variables
Lx = 200*10^-3; % billet size
Ly = 200*10^-3;
dx = 01*10^-3;
dy = 01*10^-3;
dt = 0.001;

% Temperature definitions
initial_Temperature = 510+273;
left_Temperature = 65+273;
upper_Temperature = 65+273;
right_Temperature = 65+273;
bottom_Temperature = 65+273;
BC_temperature = left_Temperature;
new_Temperature = BC_temperature;
reference_Temperature = 200+273;

% Secondary variables
boolean = 0;
timeRequired = 0;
time = 26;

% Material properties
alpha = 6.584*10^-5; % Steel
rho = 2700;
Cp = 900;

courant_Number = alpha*dt/(dx^2);

% Define computational domain (Geometry)

% storage parameters
x_intervals = Lx/dx + 1;
y_intervals = Ly/dy + 1;

% Matrices
T_old =  zeros(x_intervals,y_intervals); % 2-D storage for previous time step
T_new = zeros(x_intervals,y_intervals); % 2-D storage for current time step
%T = zeros((time/dt),x_intervals,y_intervals); % Mega Temperature matrix to stroe in 2-D for each time step, hence 3-D

% Initialize domain
T_old = InitializeSolution(T_old,initial_Temperature,left_Temperature,upper_Temperature,right_Temperature,bottom_Temperature,x_intervals,y_intervals);
T_boundary = zeros(1,time/dt);

% formulation : EXPLICIT scheme
for time_index = 1:1:(time/dt)

    T_new = ExplicitScheme(T_old,alpha,dx,dy,dt,x_intervals,y_intervals);
    % Introduce the water bath dynaic BC factor
    new_Temperature = CalculatedBathAdjustedBoundaryTemperature(T_new,T_old,x_intervals,y_intervals,dt,rho,Cp,new_Temperature);
    boolean = ExitConditionEvaluation(T_new,reference_Temperature,x_intervals,y_intervals);

    if(boolean == 1 && timeRequired == 0)
        timeRequired = time_index;
    end

    T_boundary(time_index) = new_Temperature;

    T_old = ReInitializeSolution(T_new,new_Temperature,x_intervals,y_intervals);

end

T_plot = T_new';
T_plotting = CalculatePlottingMatrix(T_plot,x_intervals,y_intervals); % Plotting matrix

%Contour plot on the 2-D spatial domain
T_constant = initial_Temperature:.001:max([initial_Temperature,left_Temperature,upper_Temperature,right_Temperature,bottom_Temperature]);
[C,h] = contour(T_plotting,T_constant);

end


% Local Regression and Likelihood, Figure 7.3.
%
% Censored local regression.
%
% Author: Catherine Loader
%
% NEEDS: Kaplan-Meier based estimate. Identify curves and cens/uncens. data

load heart;
fit = locfit(age,log(0.5+surv));
figure('Name','fig7_3: censored local regression Kaplan-Meier based;' );
lfplot(fit);
xlabel('Age at Transplant (years)');
ylabel('0.5+Survival Time (Days)');

fit = lf_censor(age,log(0.5+surv),cens);
hold on;
lfplot(fit,'nodata');
hold off;


% THE DEMO PRESENTS AN ERROR PROPAGATION USING A MONTE-CARLO METHOD
% The error in the positioning of the joints is propagated to an error
% in the position of the end effector

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

close all

M=1500; %number of particles

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

%standard deviation AT EACH JOINT
sigmaq=0.017;%rad

%find errors around this pose
q=[pi/2 -pi/2 0 0 0 0]';

puntos=[];
for i=1:M,
    qi = q + [normrnd(0, sigmaq, robot.DOF, 1)];
    T=directkinematic(robot, qi);
    puntos=[puntos; T(1,4) T(2,4) T(3,4)];
end

adjust_view(robot)
drawrobot3d(robot,q), hold on
plot3(puntos(:,1),puntos(:,2), puntos(:,3),'r.')


function DEM_demo_OU
% DEM demo for linear deconvolution:  This demo considers the deconvolution
% of one of the simplest dynamical process; a random walk or Ornstein-
% Uhlenbeck process.  It shows how DEM can infer on the causes as stochastic
% innovations (c.f., Bayesian filtering) by exploiting temporal
% correlations.  Strictly speaking this is not a Ornstein-Uhlenbeck process
% because the innovations are themselves correlated and would normally be a
% Wiener process

% get a simple convolution model
%==========================================================================
M       = spm_DEM_M('OU');

% and generate data
%==========================================================================
N       = 64;                                 % length of data sequence
DEM     = spm_DEM_generate(M,N,{},{[] 8});

% invert model
%==========================================================================
DEM     = spm_DEM(DEM);

% overlay true values
%--------------------------------------------------------------------------
spm_DEM_qU(DEM.qU,DEM.pU)


function [A,h] = build_A_h_3d(dphi,cert,phiGrad,transformatioinModel)
% BUILD_A_H_LINEAR3D Builds the equation system A*p = h
%
% INPUT ARGUMENTS
% dphi                  - Phase-difference
% cert                  - Certainty
% phiGrad               - Phase gradient
% transformationModel   - Transformation model (translation or affine)
%
% OPTIONAL INPUT ARGUMENTS
% N/A
%
% OUTPUT ARGUMENTS
% A 					- A matrix
% h 					- h vector
%
% See "Phase-Based Multidimensional Volume Registration" by Hemmendorf et al
% or "PHASE BASED VOLUME REGISTRATION USING CUDA" by Eklund et al for a
% detailed description of how the equation system is set up. Note that here
% we use a differenct S(x)p then the one used in the papers. This is done
% in order to be consistent with functions build_G_h_linear2d and
% build_G_h_linear3d.

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

sz = size(dphi(:,:,:,1));

dphiX = vec(dphi(:,:,:,1));
dphiY = vec(dphi(:,:,:,2));
dphiZ = vec(dphi(:,:,:,3));
certX = vec(cert(:,:,:,1));
certY = vec(cert(:,:,:,2));
certZ = vec(cert(:,:,:,3));
phiGradX = vec(phiGrad(:,:,:,1));
phiGradY = vec(phiGrad(:,:,:,2));
phiGradZ = vec(phiGrad(:,:,:,3));

switch transformatioinModel
    case 'translation'
        A = [   sum(certX.*phiGradX.^2),    0,                          0; ...
                0,                          sum(certY.*phiGradY.^2),    0; ...
                0,                          0,                          sum(certZ.*phiGradZ.^2)];
        h = [   sum(certX.*dphiX.*phiGradX); ...
                sum(certY.*dphiY.*phiGradY); ...
                sum(certZ.*dphiZ.*phiGradZ)];
    case {'rigid','affine'}
        [x,y,z] = meshgrid(1:sz(2),1:sz(1),1:sz(3));
        x = x - sz(2)/2 - 0.5;
        y = y - sz(1)/2 - 0.5;
        z = z - sz(3)/2 - 0.5;
        x = x(:);
        y = y(:);
        z = z(:);

        A = [   sum(certX.*phiGradX.^2.*x.^2),  sum(certX.*phiGradX.^2.*x.*y),  sum(certX.*phiGradX.^2.*x.*z)       0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2.*x), 0,                          0; ...
                0,                              sum(certX.*phiGradX.^2.*y.^2),  sum(certX.*phiGradX.^2.*y.*z)       0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2.*y), 0,                          0; ...
                0,                              0,                              sum(certX.*phiGradX.^2.*z.^2)       0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2.*z), 0,                          0; ...
                0,                              0,                              0,                                  sum(certY.*phiGradY.^2.*x.^2),      sum(certY.*phiGradY.^2.*x.*y),  sum(certY.*phiGradY.^2.*x.*z),  0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2.*x), 0; ...
                0,                              0,                              0,                                  0,                                  sum(certY.*phiGradY.^2.*y.^2),  sum(certY.*phiGradY.^2.*y.*z),  0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2.*y), 0; ...
                0,                              0,                              0,                                  0,                                  0,                              sum(certY.*phiGradY.^2.*z.^2),  0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2.*z), 0; ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              sum(certZ.*phiGradZ.^2.*x.^2),  sum(certZ.*phiGradZ.^2.*x.*y),  sum(certZ.*phiGradZ.^2.*x.*z),  0,                          0,                          sum(certZ.*phiGradZ.^2.*x); ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              sum(certZ.*phiGradZ.^2.*y.^2),  sum(certZ.*phiGradZ.^2.*y.*z),  0,                          0,                          sum(certZ.*phiGradZ.^2.*y); ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              sum(certZ.*phiGradZ.^2.*z.^2),  0,                          0,                          sum(certZ.*phiGradZ.^2.*z); ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              0,                              sum(certX.*phiGradX.^2),    0,                          0; ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              0,                              0,                          sum(certY.*phiGradY.^2),    0; ...
                0,                              0,                              0,                                  0,                                  0,                              0,                              0,                              0,                              0,                              0,                          0,                          sum(certZ.*phiGradZ.^2)];

        h = [   sum(certX.*dphiX.*phiGradX.*x); ...
                sum(certX.*dphiX.*phiGradX.*y); ...
                sum(certX.*dphiX.*phiGradX.*z); ...
                sum(certY.*dphiY.*phiGradY.*x); ...
                sum(certY.*dphiY.*phiGradY.*y); ...
                sum(certY.*dphiY.*phiGradY.*z); ...
                sum(certZ.*dphiZ.*phiGradZ.*x); ...
                sum(certZ.*dphiZ.*phiGradZ.*y); ...
                sum(certZ.*dphiZ.*phiGradZ.*z); ...
                sum(certX.*dphiX.*phiGradX); ...
                sum(certY.*dphiY.*phiGradY); ...
                sum(certZ.*dphiZ.*phiGradZ)];

        A = A + transpose(A) - diag(diag(A));
end


function [data,units] = compute_max_absdwing_length(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  data{i} = max(abs(diff(trx(fly).wing_lengthl_mm,1,2)),abs(diff(trx(fly).wing_lengthr_mm,1,2))) ./ trx(fly).dt;

end
units = parseunits('mm/s');


function [xs,ys] = draw_Gaussian_1D(Mu,Var)
%DRAW_1D_GAUSSIAN Draws a Gaussian function onto axis.
%
%
%   input -----------------------------------------------------------------
%
%       o haxes, axis handle
%
%       o Mu & Sigma of Gaussian

Mu = Mu(:);

D = size(Mu,1);

if D == 1

    Max = Mu + 4.0*sqrt(Var);
    Min = Mu - 4.0*sqrt(Var);
    xs=linspace(Min,Max, 400);
    ys = normpdf(xs,Mu,sqrt(Var));
else

    disp('Not implemented yet');

end

end


function [h]=rayleigh(fd,t)

    N=40;
    wm=2*pi*fd;
    N0=N/4;
    Tc=zeros(1,length(t));
    Ts=zeros(1,length(t));
    P_nor=sqrt(1/N0);
    theta=2*pi*rand(1,1)-pi;

    for ii=1:N0

            alfa(ii)=(2*pi*ii-pi+theta)/N;
            fi_tc=2*pi*rand(1,1)-pi;
            fi_ts=2*pi*rand(1,1)-pi;
            Tc=Tc+cos(cos(alfa(ii))*wm*t+fi_tc);
            Ts=Ts+cos(sin(alfa(ii))*wm*t+fi_ts);
    end;

   h=P_nor*(Tc+j*Ts );


function [num, den, gamma] = designIIR(phi, D, m0, h, M)

% designIIR: design filters F_i(z) (as in Section III.B.)
%
% Usage:    [num, den, gamma] = designIIR(phi, D, m0, h, M)
%
% INPUTS:
%   phi: input rational transfer functions
%   D:   vector of fractional delays
%   m0:  system delay tolerance
%   h:   fast sampling interval
%   M:   superresolution factor (integer)
%
% OUTPUTS:
%   gamma:  the H infinity norm of the induced error system K
%   num: numerator vectors
%   den: denominator vectors
%
% filter F_i(z) will be an IIR filter with num{i} is the coefficients of
% the numerator and den{i} is the coefficients of the denominator.
%
% See also: getF, demo

% Get the ingeter and residual of the delays
m = floor(D(:)/h);
d = D(:) - m * h;

% Get the digital system as in Prop. 1 and 2
Ad = getAd(phi, h, d);
Bd = getBd(phi, h, d);
Cd = getCd(phi, h, d);
Dd = zeros(size(Cd,1), size(Bd,2));

% The Integer Delay Operator
sysd = IntDelayOp([m0; m]);

% Get the digital system as in Prop. 3 by taking into account the integer
% delay operators
sys = sminreal( sysd * ss(Ad, Bd, Cd, Dd, -1) );    % sminreal to reduce the system's dimension on the fly
% sys = sysd * ss(Ad, Bd, Cd, Dd, -1);

% Get the system P (see Fig. 6)
[Ap, Bp, Cp, Dp, p, q] = getP(sys, M);

% Convert P into analog, since hinfsyn works in analog domain
[Ac, Bc, Cc, Dc] = Digital2Analog(Ap, Bp, Cp, Dp);

% Design in continuous time
P = pck(Ac, Bc, Cc, Dc);

ubd = 1;
lbd = 0;
inc = .0001;

[F, g, gamma] = hinfsyn(P,p,q,lbd,ubd,inc);

% Get the synthesis system F
[Af, Bf, Cf, Df] = unpck(F);

% Convert F back to digital domain
[Af, Bf, Cf, Df] = Analog2Digital(Af, Bf, Cf, Df);

% Convert to filter coefficients
[num, den] = getF(Af, Bf, Cf, Df, M);


function P = compute_image_pyramid2(img, sig, nL, ratio)
%%  COMPUTE_IMAGE_PYRAMID computes nL level image pyramid of the input image IMG using filter F
% downsample each level directly from the original images
%
%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2007-10-10 $
%   $Revision $
%
% Copyright 2007-2010, Brown University, Providence, RI. USA
%
%                          All Rights Reserved
%
% All commercial use of this software, whether direct or indirect, is
% strictly prohibited including, without limitation, incorporation into in
% a commercial product, use in a commercial service, or production of other
% artifacts for commercial purposes.
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for research purposes is hereby granted without fee,
% provided that the above copyright notice appears in all copies and that
% both that copyright notice and this permission notice appear in
% supporting documentation, and that the name of the author and Brown
% University not be used in advertising or publicity pertaining to
% distribution of the software without specific, written prior permission.
%
% For commercial uses contact the Technology Venture Office of Brown University
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO
% THIS SOFTWARE, INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND
% FITNESS FOR ANY PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR
% BROWN UNIVERSITY BE LIABLE FOR ANY SPECIAL, INDIRECT OR CONSEQUENTIAL
% DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR
% PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS
% ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF
% THIS SOFTWARE.

P   = cell(nL,1);
tmp = img;
P{1}= tmp;

% Get version information (from http://www.mathworks.com/matlabcentral/fileexchange/17285)
v = sscanf (version, '%d.%d.%d') ; v = 10.^(0:-1:-(length(v)-1)) * v ;

for m = 2:nL

    % Gaussian filtering
    s   = sig / ratio^(m-2);
    f   = fspecial('gaussian', 2*round(1.5*s) +1, s);
    tmp = imfilter(img, f, 'corr', 'symmetric', 'same');

    sz  = round([size(img,1) size(img,2)]*ratio^(m-1));

    %tmp = imresize(img, sz, 'bilinear', 'Antialiasing', true);   % better than 'nearest'
    % IMRESIZE changes default algorithm since version 7.4 (R2007a)
    if v > 7.3
        tmp = imresize(tmp, sz, 'bilinear', 'Antialiasing', false);   % better than 'nearest'
    else
        tmp = imresize(tmp, sz, 'bilinear', 0); % Disable antialiasing, old version for cluster
    end;

    P{m} = tmp;
end;


%%
% Test for discrete 2D sampling.

N = 512;

rep = '../results/sampling/';
if not(exist(rep))
    mkdir(rep);
end

name = 'ring';
name = 'peaks';

% helpers
rho = .8;
t = linspace(0,1,256);
cm = @(c0,c1)(t').^(rho)*c0(:)' + (1-(t').^(rho))*c1(:)';

gaussian = @(m,x,y) exp( -(x-y).^2 );

t = (0:N-1)'/N;
[Y,X] = meshgrid(t,t);
switch name
    case 'peaks'
        a = peaks(N);
        a = abs(a);
        col = [0 0 1];
    case 'ring'
        r = 1*sqrt((X-.5).^2+(Y-.5).^2);
        a = abs(sin(r))^2./(r+eps);
        a = cos(10*r).^2.* exp(-14*r.^2.2);
        a = min(a,.65);
%        surf(a); shading interp;
        col = [0 0 1];
end
a = a/sum(a(:));

% discrete samples
K = 1000;

for K=[100 500 1000 4000 10000]
    I = randsample(N^2,K,true,a(:));
    x = X(I); y = Y(I);

    clf; hold on;
    %imagesc(t,t,a);
    %colormap(cm(col,[1 1 1]));
    ms = 15;
    plot(x,y, '.', 'MarkerSize', ms, 'color', col);
    axis equal;
    axis([0 1 0 1]);  box on;
    set(gca, 'XTick', [], 'YTick', [], 'box', 'on');
    saveas(gcf, [rep name '-disc-' num2str(K) '.eps'], 'epsc');
end

col_cont = [1/2 0 1/2];
clf; hold on;
imagesc(t,t,a);
colormap(cm(col_cont,[1 1 1]));
q = 8;
contour(t,t,a, linspace(0,max(a(:)),q), 'color', col_cont);
caxis([0 max(a(:))]);
% axis off;
set(gca, 'XTick', [], 'YTick', [], 'box', 'on');
axis equal;
saveas(gcf, [rep name '-dens.png'], 'png');

%% pixelize
col_euler = [1 0 0];
b = a/max(a(:));
for n = [8 16 32 64]
P = N/n;
A = zeros(N,N,3);
for i=1:n
    for j=1:n
        seli = (i-1)*P+1:i*P;
        selj = (j-1)*P+1:j*P;
        u = mean(mean(b(seli,selj)));
        for s=1:3
            A(seli,selj,s) = col_euler(s)*u + (1-u);
        end
    end
end
A = A/max(A(:));
imwrite(A, [rep name '-pix-n' num2str(n) '.png'], 'png');
end


%KNNR Trainable Nearest Neighbor Regression
%
%    Y = KNNR(X,K)
%    Y = X*KNNR([],K)
%    Y = X*KNNR(K)
%
% INPUT
%   X    Regression dataset, used for training
%   K    number of neighbors (default K=3)
%
% OUTPUT
%   Y    k-nearest neighbor regression
%
% DESCRIPTION
% Define a k-Nearest neighbor regression on dataset X.
%
% SEE ALSO (PRTools Guide)
% LINEARR, TESTR, PLOTR

% Copyright: D.M.J. Tax, D.M.J.Tax@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function y = knnr(varargin)

  mapname = 'KNN regression';
  argin = shiftargin(varargin,'scalar');
  argin = setdefaults(argin,[],3);

  if mapping_task(argin,'definition')

    y = define_mapping(argin,'untrained',mapname);

	elseif mapping_task(argin,'training')			% Train a mapping.

		[x,k] = deal(argin{:});
    [n,d] = size(x);
    W.x = +x;
    W.y = gettargets(x);
    W.k = k;
    y = prmapping(mfilename,'trained',W,1,d,1);
    y = setname(y,'k-nearest neighbor regression');

  else                                      % Evaluation

    [x,v] = deal(argin{1:2});
    w = getdata(v);
    [n,d] = size(x);
    D = distm(+x,w.x);
    [sD,I] = sort(D,2);
    if n==1
      out = mean(w.y(I(:,1:w.k)));
    else
      out = mean(w.y(I(:,1:w.k)),2);
    end
    y = setdat(x,out);

  end


function value = r4_besk1 ( x )

%*****************************************************************************80
%
%% R4_BESK1 evaluates the Bessel function K of order 1 of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the Bessel function K of order 1 of X.
%
  persistent bk1cs
  persistent ntk1
  persistent xmax
  persistent xmin
  persistent xsml

  if ( isempty ( ntk1 ) )

    bk1cs = [ ...
       0.0253002273389477705, ...
      -0.353155960776544876, ...
      -0.122611180822657148, ...
      -0.0069757238596398643, ...
      -0.0001730288957513052, ...
      -0.0000024334061415659, ...
      -0.0000000221338763073, ...
      -0.0000000001411488392, ...
      -0.0000000000006666901, ...
      -0.0000000000000024274, ...
      -0.0000000000000000070 ]';

    ntk1 = r4_inits ( bk1cs, 11, 0.1 * r4_mach ( 3 ) );
    xmin = exp ( max ( log ( r4_mach ( 1 ) ), ...
      - log ( r4_mach ( 2 ) ) ) + 0.01 );
    xsml = sqrt ( 4.0 * r4_mach ( 3 ) );
    xmax = - log ( r4_mach ( 1 ) );
    xmax = xmax - 0.5 * xmax * log ( xmax ) ...
      / ( xmax + 0.5 ) - 0.01;

  end

  if ( x <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BESK1 = Fatal error!\n' );
    fprintf ( 1, '  X <= 0.\n' );
    error ( 'R4_BESK1 = Fatal error!' )
  elseif ( x <= xsml )
    y = 0.0;
    value = log ( 0.5 * x ) * r4_besi1 ( x ) + ( 0.75 ...
      + r4_csevl ( 0.5 * y - 1.0, bk1cs, ntk1 ) ) / x;
  elseif ( x <= 2.0 )
    y = x * x;
    value = log ( 0.5 * x ) * r4_besi1 ( x ) + ( 0.75 ...
      + r4_csevl ( 0.5 * y - 1.0, bk1cs, ntk1 ) ) / x;
  elseif ( x <= xmax )
    value = exp ( - x ) * r4_besk1e ( x );
  else
    value = 0.0;
  end

  return
end


function r = mescatter(f,E)
  N = size(f,1);
  A = 2*(N+E);

  r = zeros(A,size(f,2));

  ridx = -N-E:-N-1;  fidx = N-E:N-1;
  r(mod(ridx,A)+1,:) = -f(mod(fidx,N)+1,:);
  ridx = -N+1:-1;  fidx = N-1:-1:1;
  r(mod(ridx,A)+1,:) = f(mod(fidx,N)+1,:);
  ridx = 0;  fidx = 0;
  r(mod(ridx,A)+1,:) = f(mod(fidx,N)+1,:) * sqrt(2);
  ridx = 1:N-1;  fidx = 1:N-1;
  r(mod(ridx,A)+1,:) = f(mod(fidx,N)+1,:);
  ridx = N+1:N+E-1;  fidx = N-1:-1:N-E+1;
  r(mod(ridx,A)+1,:) = -f(mod(fidx,N)+1,:);
  ridx = 0;


%SF_HEX_Q4 Triquartic conforming shape function for hexahedrons (Q4).
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_HEX_Q4( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates conforming triquartic Q4 shape functions on hexahedrons with values defined
%   in the nodes, edges, faces, and cell center. XI is [-1..1]^3 reference coordinates.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar:  3             Number of space dimensions
%       n_vert      scalar:  8             Number of vertices per cell
%       i_dof       scalar: 1-n_ldof       Local basis function to evaluate
%       xi          [n_sdim]               Local coordinates of evaluation point
%       aInvJac     [n,n_sdim*n_sdim]      Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [4]                    Number of local degrees of freedom on
%                                          vertices, edges, faces, and cell interiors
%       xLDof       [n_sdim,n_ldof]        Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SFLAG4, SF_HEX_Q1

% Copyright 2013-2022 Precise Simulation, Ltd.


function y = MVL(Ai, Bi, Aj, Bj, t)

% MVL: compute the integral Mij(t) of equation (51)
%
% Usage:    y = MVL(Ai, Bi, Aj, Bj, t)
%
% INPUTS:
%       Ai, Aj, Bi, Bj: matrices of the operators Phi_i(s) and Phi_j(s)
%       t: upper limit of the integral
%
% OUTPUT:
%       y = MVL(t) = int_0^t expm(tau*Ai)*Bi*Bj'*expm(tau*Aj')dtau
%
% SEE ALSO: reference [19]

F = expm([-Ai                                   Bi*Bj';
          zeros( size(Aj',1), size(Ai,2) )      Aj'] * t );

y = expm(Ai*t) * F( 1:size(Ai,1), (1+size(Ai,2)):end );


function [ primal_obj ] = compute_primal(X, Y, W, Omega, lambda)
% Inputs
%   Xtrain: input training data (m-length cell)
%   Ytrain: output training data (m-length cell)
%   W: current models (d x m)
%   Omega: precision matrix (m x m)
%   lambda: regularization parameter
% Output
%   primal objective

% compute primal
total_loss = 0;
for t=1:length(X)
    preds = Y{t}.*(X{t}*W(:, t));
    total_loss = total_loss + mean(max(0.0, 1.0 - preds));
end
primal_obj = total_loss + lambda / 2 * trace(W * Omega * W');

end


function D = driving_function_mono_localwfs_sbl_pw(x0,nk,f,conf)
%DRIVING_FUNCTION_MONO_LOCALWFS_SBL_PW driving signal for a plane wave using
%local WFS with spatial bandwidth limitation
%
%   Usage: D = driving_function_mono_localwfs_sbl_pw(x0,nk,f,conf)
%
%   Input parameters:
%       x0          - position and direction of the secondary source / m [nx7]
%       nk          - propagation direction of plane wave / m [1x3]
%       f           - frequency of the monochromatic source / Hz
%       conf        - configuration struct (see SFS_config)
%
%   Output parameters:
%       D           - driving function [nx1]
%
%   See also: sound_field_mono_localwfs_sbl, driving_function_mono_localwfs_sbl
%
%   References:
%       Winter, Hahn, Spors (2017), "Time-Domain Realisation of Model-Based
%       Rendering for 2.5D Local Wave Field Synthesis Using Spatial
%       Bandwidth-Limitation", 25th European Signal Processing Conference
%       (EUSIPCO), pp. 718-722, https://bit.ly/2yMjlOw

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 4;
nargmax = 4;
narginchk(nargmin,nargmax);

%% ===== Configuration ========================================================
N0 = size(x0,1);
xref = conf.xref;
% Maximum order of circular basis expansion of sound field
if isempty(conf.localwfs_sbl.order)
    Nce = nfchoa_order(N0,conf);
else
    Nce = conf.localwfs_sbl.order;
end
% Resolution of plane wave decomposition
if isempty(conf.localwfs_sbl.Npw)
    Npw = 2*ceil(2*pi*0.9*f/conf.c*conf.secondary_sources.size/2);
else
    Npw = conf.localwfs_sbl.Npw;
end

%% ===== Computation ==========================================================
% Circular expansion coefficients, Winter et al. (2017), eq. (12)
Pm = circexp_mono_pw(nk,Nce,f,xref,conf);
% Modal window
wm = modal_weighting(Nce,conf);
Pm = bsxfun(@times,[wm(end:-1:2),wm],Pm);
% Plane wave decomposition, inverse FT of Winter et al. (2017), eq. (10)
Ppwd = pwd_mono_circexp(Pm,Npw);
% Driving signal, Winter et al. (2017), eq. (8)
D = driving_function_mono_wfs_pwd(x0,Ppwd,f,xref,conf);


function [Y1 h11]=derotated_dtcwt(I1,n,biot,Qshift)
%     X -> 2D real matrix/Image
%
%     nlevels -> No. of levels of wavelet decomposition
%
%     biort ->  'antonini'   => Antonini 9,7 tap filters.
%               'legall'     => LeGall 5,3 tap filters.
%               'near_sym_a' => Near-Symmetric 5,7 tap filters.
%               'near_sym_b' => Near-Symmetric 13,19 tap filters.
%
%     qshift -> 'qshift_06' => Quarter Sample Shift Orthogonal (Q-Shift) 10,10 tap filters,
%                              (only 6,6 non-zero taps).
%               'qshift_a' =>  Q-shift 10,10 tap filters,
%                              (with 10,10 non-zero taps, unlike qshift_06).
%               'qshift_b' => Q-Shift 14,14 tap filters.
%               'qshift_c' => Q-Shift 16,16 tap filters.
%               'qshift_d' => Q-Shift 18,18 tap filters.
%
%
%     Y1     -> The real lowpass image from the final level
%     h11     -> A cell array containing the 6 complex highpass subimages
%     for each level.
%clear all;
%clc;
[Y1,h1] = dtwavexfm2(I1,n,biot,Qshift);
%[Y2,h2] = dtwavexfm2(I2,n,biot,Qshift);

h11{n}=h1{n};
for k=n:-1:2
    for m=1:6
        xp=imresize(h1{k}(:,:,m),2);%
        argxp=angle(xp);
        argx=angle(h1{k-1}(:,:,m));
        argx=argx-2.*argxp;
        absx=abs(h1{k-1}(:,:,m));
        xa=absx.*cos(argx);
        xb=absx.*sin(argx);
        h11{k-1}(:,:,m)=complex(xa,xb);
    end
end
%figure;
%cimage5(h11{1}(:,:,4));
%figure;
%cimage5(h1{1}(:,:,4));
%figure;
%cimage5(h11{2}(:,:,4));
%figure;
%cimage5(h1{2}(:,:,4));


% Script to reproduce the experiments leading to the results provided in the
% Table 2 of the paper "Deep Scattering Spectrum" by J. Andén and S. Mallat.

% M=2 scattering, frequency scattering, multiple Q1, multiple T

run_name = 'DSS_Table2_GTZAN_m2_freq_multQ1_multT';

src = gtzan_src('/path/to/gtzan');

N = 5*2^17;

filt1_opt.filter_type = {'gabor_1d','morlet_1d'};
filt1_opt.Q = [8 1];
filt1_opt.J = T_to_J(8192,filt1_opt);

sc1_opt.M = 2;

ffilt1_opt.filter_type = 'morlet_1d';
ffilt1_opt.J = 7;

fsc1_opt.M = 1;

Wop1 = wavelet_factory_1d(N, filt1_opt, sc1_opt);
fWop1 = wavelet_factory_1d(128, ffilt1_opt, fsc1_opt);

scatt_fun1 = @(x)(log_scat(renorm_scat(scat(x,Wop1))));
fscatt_fun1 = @(x)(func_output(@scat_freq,2,scatt_fun1(x),fWop1));
feature_fun1 = @(x)(format_scat(fscatt_fun1(x)));

filt2_opt = filt1_opt;
filt2_opt.Q = [1 1];
filt2_opt.J = T_to_J(8192,filt2_opt);

sc2_opt = sc1_opt;

ffilt2_opt = ffilt1_opt;
ffilt2_opt.J = 5;

fsc2_opt = fsc1_opt;

Wop2 = wavelet_factory_1d(N, filt2_opt, sc2_opt);
fWop2 = wavelet_factory_1d(32, ffilt2_opt, fsc2_opt);

scatt_fun2 = @(x)(log_scat(renorm_scat(scat(x,Wop2))));
fscatt_fun2 = @(x)(func_output(@scat_freq,2,scatt_fun2(x),fWop2));
feature_fun2 = @(x)(format_scat(fscatt_fun2(x)));

filt3_opt = filt1_opt;
filt3_opt.J = T_to_J(2*8192,filt3_opt);

sc3_opt = sc1_opt;

Wop3 = wavelet_factory_1d(N, filt3_opt, sc3_opt);

scatt_fun3 = @(x)(log_scat(renorm_scat(scat(x,Wop3))));
fscatt_fun3 = @(x)(func_output(@scat_freq,2,scatt_fun3(x),fWop1));
feature_fun3 = @(x)(format_scat(fscatt_fun3(x)));

filt4_opt = filt2_opt;
filt4_opt.J = T_to_J(2*8192,filt4_opt);

sc4_opt = sc2_opt;

Wop4 = wavelet_factory_1d(N, filt4_opt, sc4_opt);

scatt_fun4 = @(x)(log_scat(renorm_scat(scat(x,Wop4))));
fscatt_fun4 = @(x)(func_output(@scat_freq,2,scatt_fun4(x),fWop2));
feature_fun4 = @(x)(format_scat(fscatt_fun4(x)));

filt5_opt = filt1_opt;
filt5_opt.J = T_to_J(4*8192,filt5_opt);

sc5_opt = sc1_opt;

Wop5 = wavelet_factory_1d(N, filt5_opt, sc5_opt);

scatt_fun5 = @(x)(log_scat(renorm_scat(scat(x,Wop5))));
fscatt_fun5 = @(x)(func_output(@scat_freq,2,scatt_fun5(x),fWop1));
feature_fun5 = @(x)(format_scat(fscatt_fun5(x)));

filt6_opt = filt2_opt;
filt6_opt.J = T_to_J(4*8192,filt6_opt);

sc6_opt = sc2_opt;

Wop6 = wavelet_factory_1d(N, filt6_opt, sc6_opt);

scatt_fun6 = @(x)(log_scat(renorm_scat(scat(x,Wop6))));
fscatt_fun6 = @(x)(func_output(@scat_freq,2,scatt_fun6(x),fWop2));
feature_fun6 = @(x)(format_scat(fscatt_fun6(x)));

features = {feature_fun1, feature_fun2, feature_fun3, ...
	feature_fun4, feature_fun5, feature_fun6};

for k = 1:length(features)
    fprintf('testing feature #%d...',k);
    tic;
    sz = size(features{k}(randn(N,1)));
    aa = toc;
    fprintf('OK (%.2fs) (size [%d,%d])\n',aa,sz(1),sz(2));
end

db = prepare_database(src,features);
db.features = single(db.features);
db = svm_calc_kernel(db,'gaussian','square',1:2:size(db.features,2));

rs = RandStream.create('mt19937ar','Seed',floor(pi*1e9));
RandStream.setGlobalStream(rs);
[train_set{1}, test_set{1}] = create_partition([src.objects.class], 0.9);
for k = 2:10
	[train_set{k}, test_set{k}] = ...
		next_fold([src.objects.class], train_set{k-1}, test_set{k-1});
end

optt.kernel_type = 'gaussian';
optt.C = 2.^[0:4:8];
optt.gamma = 2.^[-16:4:-8];
optt.search_depth = 3;
optt.full_test_kernel = 0;

for k = 1:10
	[dev_err_grid,C_grid,gamma_grid] = ...
		svm_adaptive_param_search(db,train_set{k},[],optt);

	[dev_err(k),ind] = min(mean(dev_err_grid{end},2));
	C(k) = C_grid{end}(ind);
	gamma(k) = gamma_grid{end}(ind);

	optt1 = optt;
	optt1.C = C(k);
	optt1.gamma = gamma(k);

	model = svm_train(db,train_set{k},optt1);
	labels(:,k) = svm_test(db,model,test_set{k});
	err(k) = classif_err(labels(:,k),test_set{k},db.src);

	fprintf('dev err = %f, test err = %f\n',dev_err(k),err(k));

	save([run_name '.mat'],'labels','dev_err','err','C','gamma');
end


%  Figure 10.42      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
%
% fig10_42.m is a script to generate Figure 10.42, the root locus for
% altitude control with inner-loop stabilization and altitude feedback
% alone

clf;
ftq = [-0.0064    0.0263         0  -32.2000         0;
   -0.0941   -0.6240  761.1400 -196.2000         0;
   -0.0002   -0.0015   -4.4120  -12.4800         0;
         0         0    1.0000         0         0;
         0   -1.0000         0  830.0000         0];

g =[0;
  -32.7000;
   -2.0800;
         0;
         0];

h=[0 0 0 0 1];

j=0;

rlocus(ftq,g,-h,j);
v=[-12 12 -4 4];
axis(v);
grid;
title('Fig. 10.42  Rootlocus for altitude feedback with inner-loop stab.')


function y = square( x )

%SQUARE    Square.
%   SQUARE(X) is an array of the same size as X, whose elements are the
%   squares of the elements of X.
%
%   Disciplined convex programming information:
%       If X is real, then SQUARE(X) is convex and nonmonotonic in X. If X
%       is complex, then SQUARE(X) is neither convex nor concave. Thus when
%       when use in CVX expressions, X must be real and affine.

narginchk(1,1);
y = x .* x;

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


%DEMO_REGRESSION_SPARSE2  Regression demo comparing different sparse
%                         approximations with optimization of inducing
%                         variables
%
%  Description
%    A regression problem with one input variable and one output
%    variable with Gaussian noise. The output is assumed to be
%    realization of additive functions and Gaussian noise.
%
%    For standard full GP demonstration, see for example
%    DEMO_REGRESSION1, and for detailed discussion, Rasmussen and
%    Williams (2006). For more sparse demonstrations including use
%    of compact support covariance functions see DEMO_REGRESSION2,
%    DEMO_MODELASSESMENT2, and DEMO_SPARSEAPPROX.
%
%    In this demo, sparse approximations for the full GP model are
%    compared. We use
%      - FIC, fully independent conditional
%      - DTC, deterministic training conditional
%      - VAR, variational approach
%    For illustration purposes the hyperparameters from the full GP
%    are used for the sparse models and only the inducing variables
%    are optimised.
%
%    For technical details, see Quinonero-Candela and Rasmussen
%    (2005) for the FIC and DTC models and Titsias (2009) for the
%    VAR model.
%
%    We use a simple one dimensional data set to present the three
%    methods.
%
%  See also DEMO_REGRESSION1, DEMO_REGRESSION2, DEMO_REGRESSION_SPARSE1
%
%
%  References:
%
%    Quinonero-Candela, J. and Rasmussen, C. E. (2005). A Unifying
%    View of Sparse Approximate Gaussian Process Regression. Journal
%    of Machine Learning Research.
%
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.
%
%    Titsias, M. K. (2009). Variational Model Selection for Sparse
%    Gaussian Process Regression. Technical Report, University of
%    Manchester.

% Copyright (c) 2010 Heikki Peura, Aki Vehtari

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

% Set randomstream for reproducing same results
prevstream=setrandstream();

% Start by creating 1D data
xx=linspace(1,10,901);

% Choose a subset of data so that the data are less dense in the right end.
% xt are the inputs and yt are the outputs, xstar are the values we want to
% predict.
x1=logspace(0,1,100);
x1=round(x1*100)-99;
x=xx(x1)';
y=2*sin(4*x)+0.2*randn(size(x));
xt=[1:0.01:14]';
[n,nin] = size(x);

fprintf('Full GP\n')
% Initialize full GP with a squared exponential component and set
% priors for their parameters.
pl = prior_t('s2', 1);
pm = prior_logunif();
pn = prior_logunif();

gpcfse = gpcf_sexp('lengthScale',0.5,'magnSigma2',1, 'lengthScale_prior', pl, 'magnSigma2_prior', pm);
lik = lik_gaussian('sigma2', 0.1, 'sigma2_prior', pn);

gp = gp_set('lik', lik, 'cf', gpcfse, 'jitterSigma2', 1e-6);

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

[Eft_full, Varft_full] = gp_pred(gp, x, y, xt);
Varft_full = Varft_full + gp.lik.sigma2;

figure
% Blue crosses are the initial inducing input locations, red ones are
% the optimised ones. Black circles represent the distance to the next
% optimized location, with a dashed trendline.');

subplot(2,2,1);hold on;
plot(xt,Eft_full,'k', 'LineWidth', 2)
plot(xt,Eft_full-2.*sqrt(Varft_full),'--','Color',[0 0.5 0])
plot(xt,Eft_full+2.*sqrt(Varft_full),'--','Color',[0 0.5 0])
plot(x,y,'.', 'MarkerSize',7)
ylim([-3 3])
title('FULL GP')

fprintf('FIC GP\n')
% Run FIC approximation for the same data: choose the inducing
% inputs Xu, then proceed with the inference with the optimized
% parameters from the full GP: here, we optimize only the locations
% of the inducing inputs for the FIC model.
Xu=round(10+90*rand(18,1))/10; % Random placement

% Change type to FIC, add inducing inputs, and optimize only inducing inputs
gp_fic = gp_set(gp, 'type','FIC','X_u',Xu,'infer_params','inducing');

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp_fic=gp_optim(gp_fic,x,y,'opt',opt);

[Eft_fic, Varft_fic] = gp_pred(gp_fic, x, y, xt);
Varft_fic = Varft_fic + gp_fic.lik.sigma2;

subplot(2,2,2);hold on;
h1=plot(xt,Eft_fic,'k', 'LineWidth', 2);
h2=plot(xt,Eft_fic-2.*sqrt(Varft_fic),'--','Color',[0 0.5 0]);
plot(xt,Eft_fic+2.*sqrt(Varft_fic),'--','Color',[0 0.5 0])
h3=plot(x,y,'.', 'MarkerSize',7);
h4=plot(Xu, -2.8, 'bx', 'MarkerSize', 5, 'LineWidth', 2);
h5=plot(gp_fic.X_u, -3, 'rx', 'MarkerSize', 5, 'LineWidth', 2);
legend([h1  h2 h3 h4(1) h5(1)],'Ef','95% CI','Data','Initial X_u','Optimized X_u')
% plot diff of sorted X_u and regress line for that
%XuSorted=sort(gp_fic.X_u);
%dXuSorted=diff(XuSorted);
%bb=regress(dXuSorted,[ones(size(dXuSorted)) XuSorted(1:end-1)]);
%plotbb=bb(1)+(min(XuSorted):0.1:max(XuSorted))*bb(2);
%plot(XuSorted(1:end-1),dXuSorted,'ko');
%plot(min(XuSorted):0.1:max(XuSorted),plotbb,'k--')
ylim([-3 3])
title('FIC')

fprintf('VAR GP\n')
% Run the VAR model similarly to the FIC model with the same
% starting inducing inputs. The difference in the optimized results
% is notable. The VAR model places the inducing inputs quite evenly
% (slightly increasing as the data becomes more sparse), with
% predictions closely matching the full GP model. The other two
% sparse approximations yield less reliable results.
gp_var = gp_set(gp,'type','VAR','X_u',Xu,'infer_params','inducing');

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp_var=gp_optim(gp_var,x,y,'opt',opt);

[Eft_var, Varft_var] = gp_pred(gp_var, x, y, xt);
Varft_var = Varft_var + gp_var.lik.sigma2;

subplot(2,2,4);hold on
plot(xt,Eft_var,'k', 'LineWidth', 2)
plot(xt,Eft_var-2.*sqrt(Varft_var),'--','Color',[0 0.5 0])
plot(xt,Eft_var+2.*sqrt(Varft_var),'--','Color',[0 0.5 0])
plot(x,y,'.', 'MarkerSize',7)
plot(gp_var.X_u, -3, 'rx', 'MarkerSize', 5, 'LineWidth', 2)
plot(Xu, -2.8, 'bx', 'MarkerSize', 5, 'LineWidth', 2)
% plot diff of sorted X_u and regress line for that
%XuSorted=sort(gp_var.X_u);
%dXuSorted=diff(XuSorted);
%bb=regress(dXuSorted,[ones(size(dXuSorted)) XuSorted(1:end-1)]);
%plotbb=bb(1)+(min(XuSorted):0.1:max(XuSorted))*bb(2);
%plot(XuSorted(1:end-1),dXuSorted,'ko');
%plot(min(XuSorted):0.1:max(XuSorted),plotbb,'k--')
ylim([-3 3])
title('VAR')

fprintf('DTC GP\n')
% Run the DTC model similarly to the FIC model with the same starting
% inducing inputs. The difference in the optimized results is notable.
gp_dtc = gp_set(gp,'type','DTC','X_u',Xu,'infer_params','inducing');

% Set the options for the optimization
opt=optimset('TolFun',1e-4,'TolX',1e-4);
% Optimize with the scaled conjugate gradient method
gp_dtc=gp_optim(gp_dtc,x,y,'opt',opt);

[Eft_dtc, Varft_dtc] = gp_pred(gp_dtc, x, y, xt);
Varft_dtc = Varft_dtc + gp_dtc.lik.sigma2;

subplot(2,2,3);hold on
plot(xt,Eft_dtc,'k', 'LineWidth', 2)
plot(xt,Eft_dtc-2.*sqrt(Varft_dtc),'--','Color',[0 0.5 0])
plot(xt,Eft_dtc+2.*sqrt(Varft_dtc),'--','Color',[0 0.5 0])
plot(x,y,'.', 'MarkerSize',7)
plot(gp_dtc.X_u, -3, 'rx', 'MarkerSize', 5, 'LineWidth', 2)
plot(Xu, -2.8, 'bx', 'MarkerSize', 5, 'LineWidth', 2)
% plot diff of sorted X_u and regress line for that
%XuSorted=sort(gp_dtc.X_u);
%dXuSorted=diff(XuSorted);
%bb=regress(dXuSorted,[ones(size(dXuSorted)) XuSorted(1:end-1)]);
%plotbb=bb(1)+(min(XuSorted):0.1:max(XuSorted))*bb(2);
%plot(XuSorted(1:end-1),dXuSorted,'ko');
%plot(min(XuSorted):0.1:max(XuSorted),plotbb,'k--')
ylim([-3 3])
title('DTC')

% Set back initial random stream
setrandstream([],prevstream);


function [ value_min, value_mean, value_max, value_var ] = ...
 tet_mesh_quality1 ( node_num, node_xyz, tetra_order, tetra_num, tetra_node )

%*****************************************************************************80
%
%% TET_MESH_QUALITY1 returns a tet mesh quality factor.
%
%  Discussion:
%
%    The tet mesh quality measure is the minimum of the
%    corresponding tetrahedron quality measure, over all tetrahedrons in the
%    tet mesh.
%
%    This routine is designed for an order-4 tet mesh.  Order 10 tet meshes
%    may be input, but the extra nodes are ignored.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 October 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, double NODE_XYZ(3,NODE_NUM), the nodes.
%
%    Input, integer TETRA_ORDER, the order of the tetrahedrons.
%
%    Input, integer TETRA_NUM, the number of tetrahedrons.
%
%    Input, integer TETRA_NODE(TETRA_ORDER,TETRA_NUM), the indices of the nodes
%    that make up the tetrahedrons.
%
%    Output, double VALUE_MIN, VALUE_MEAN, VALUE_MAX, VALUE_VAR,
%    the minimum, mean, maximum and variance of the quality measure.
%
  dim_num = 3;

  for tetra = 1 : tetra_num

    tetrahedron(1:dim_num,1:4) = node_xyz(1:dim_num,tetra_node(1:4,tetra));

    tetrahedron_quality(tetra) = tetrahedron_quality1_3d ( tetrahedron );

  end

  value_max  = max  ( tetrahedron_quality(1:tetra_num) );
  value_min  = min  ( tetrahedron_quality(1:tetra_num) );
  value_mean = mean ( tetrahedron_quality(1:tetra_num) );
  value_var  = var  ( tetrahedron_quality(1:tetra_num) );

  return
end


function [vertices, faces] = curveToMesh(curve, varargin)
%CURVETOMESH  Create a mesh surrounding a 3D curve
%
%   [V, F] = curveToMesh(CURVE)
%   Computes the vertices and the faces of the mesh surrounding the
%   specified 3D curve.
%
%   [V, F] = curveToMesh(CURVE, THICKNESS)
%   Specifies the thickness of the mesh (distance between mesh vertices and
%   curve vertices). Default is 0.5.
%
%   [V, F] = curveToMesh(CURVE, THICKNESS, NCORNERS)
%   Also specifies the number of mesh vertices around each curve vertex.
%   Default is 8.
%
%
%   Example
%     % Creates a tubular mesh around a trefoil knot curve
%     t = linspace(0, 2*pi, 200)';
%     x = sin(t) + 2 * sin(2 * t);
%     y = cos(t) - 2 * cos(2 * t);
%     z = -sin(3 * t);
%     curve = [x, y, z];
%     [v2, f2] = curveToMesh(curve, .5, 16);
%     figure;
%     drawMesh(v2, f2);
%     axis equal; view(3);
%     axis([-4 4 -4 4 -2 2]);
%
%   See also
%     meshes3d, torusMesh, surfToMesh

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2015-01-07,    using Matlab 8.4.0.150421 (R2014b)
% Copyright 2015 INRA - Cepia Software Platform.

radius = .1;
if nargin > 1
    radius = varargin{1};
end

nCorners = 8;
if nargin > 2
    nCorners = varargin{2};
end

nNodes = size(curve, 1);
nVerts = nNodes * nCorners;

vertices = zeros(nVerts, 3);

% create reference corners, that will be rotated and translated
t = linspace(0, 2*pi, nCorners + 1)';
t(end) = [];
baseCorners = radius * [cos(t) sin(t) zeros(size(t))];

for iNode = 1:nNodes
    % coordinate of current node
    node = curve(iNode, :);

    % compute local tangent vector
    iNext = mod(iNode, nNodes) + 1;
    tangentVector = normalizeVector3d(curve(iNext, :) - node);

    % convert to spherical coordinates
    [theta, phi, rho] = cart2sph2(tangentVector); %#ok<ASGLU>

    % apply transformation to place corners around current node
    rotY = createRotationOy(theta);
    rotZ = createRotationOz(phi);
    trans = createTranslation3d(node);
    transformMatrix = trans * rotZ * rotY;
    corners = transformPoint3d(baseCorners, transformMatrix);

    % concatenate with other corners
    vertices( (1:nCorners) + (iNode - 1) * nCorners, :) = corners;
end

% indices of vertices
inds = (1:nVerts)';
add1 = repmat([ones(nCorners-1, 1) ; 1-nCorners], nNodes, 1);

% generate faces
faces = [inds ...
    mod(inds + add1 - 1, nVerts) + 1 ...
    mod(inds + nCorners + add1 - 1, nVerts) + 1 ...
    mod(inds + nCorners - 1, nVerts) + 1];


% Copyright (C) 2012 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
%
% You are free to use this software for academic purposes if you cite our paper:
% Quan Wang, Kim L. Boyer,
% The active geometric shape model: A new robust deformable shape model and its applications,
% Computer Vision and Image Understanding, Volume 116, Issue 12, December 2012, Pages 1178-1194,
% ISSN 1077-3142, 10.1016/j.cviu.2012.08.004.
%
% For commercial use, please contact the authors.

function [xc yc a b phi]=fit_arb_ellipse_force(init,increment,threshold,bound,field_x,field_y,iter,show_fitness)

x0=init(1);
y0=init(2);
a0=init(3);
b0=init(4);
phi0=init(5);
d_xc=increment(1);
d_yc=increment(2);
d_a=increment(3);
d_b=increment(4);
d_phi=increment(5);
t_xc=threshold(1);
t_yc=threshold(2);
t_a=threshold(3);
t_b=threshold(4);
t_phi=threshold(5);
a_low=bound(1);
a_up=bound(2);
b_low=bound(3);
b_up=bound(4);

%%  Fit an arbitrary ellipse in the force filed. xc, yc, a, b and phi will be returned.
%   The parametric equations of the arbitrary ellipse:
%       x=xc+a*cos(theta)*cos(phi)-b*sin(theta)*sin(phi)
%       y=yc+a*cos(theta)*sin(phi)+b*sin(theta)*cos(phi)
%   x0, y0: initial position of center
%   a0, b0: initial semi-major and semi-minor axes
%   phi0: initial orientation
%   d_xc, d_yc: increment of xc and yc in each loop
%   d_a, d_b: increment of a and b in each loop
%   d_phi: increment of phi in each loop
%   t_xc, t_yc: threshold needed to update xc and yc
%   t_a, t_b: threshold needed to update a and b
%   t_phi: threshold needed to update phi
%   a_low, a_up: lower bound and upper bound of a
%   b_low, b_up: lower bound and upper bound of b
%   field_x: the x component of force field
%   field_y: the y component of force field
%   iter: number of iterations
%   show_fitness: a flag of whether to show the fitness function

% note: m rows and n columns, but x is column and y is row here
[m n]=size(field_x);

xc=x0;
yc=y0;
a=a0;
b=b0;
phi=phi0;

for it=1:iter
    [x,y,theta]=arb_ellipse_in_image(m,n,xc,yc,a,b,phi);
    N=max(size(x));

    %% torque along the ellpise about center
    torque_v=[0 0 0]; % the torque as a vector
    for i=1:max(size(theta))
        torque_v=torque_v+cross([x(i)-xc,y(i)-yc,0],...
            [field_x(y(i),x(i)),field_y(y(i),x(i)),0]);
    end
    torque=torque_v(3);
    torque=torque/N^2;
    if torque>t_phi
        phi=phi+d_phi;
    elseif torque<-t_phi
        phi=phi-d_phi;
    end

    %% F_around
    F_round=[0,0];
    for i=1:max(size(theta))
        F_round=F_round+[field_x(y(i),x(i)),field_y(y(i),x(i))];
    end
    F_round=F_round/max(size(theta));

    %% F_left, which is on the left quarter of the ellipse, and inward
    index = find( theta>pi*3/4 & theta<pi*5/4 );
    F_left=0;
    for i=index
        F_left=F_left+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[cos(phi),sin(phi)]);
    end
    F_left=F_left/max(size(index));

    %% F_right, which is on the right quarter of the ellipse, and inward
    index = find( theta<pi/4 | theta>pi*7/4 );
    F_right=0;
    for i=index
        F_right=F_right+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[-cos(phi),-sin(phi)]);
    end
    F_right=F_right/max(size(index));

    %% F_up, which is on the up quarter of the ellipse, and inward
    index = find( theta>pi/4 & theta<pi*3/4 );
    F_up=0;
    for i=index
        F_up=F_up+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[sin(phi),-cos(phi)]);
    end
    F_up=F_up/max(size(index));

    %% F_down, which is on the low quarter of the ellipse, and inward
    index = find( theta>pi*5/4 & theta<pi*7/4 );
    F_down=0;
    for i=index
        F_down=F_down+dot([field_x(y(i),x(i)),field_y(y(i),x(i))],[-sin(phi),cos(phi)]);
    end
    F_down=F_down/max(size(index));

    %% update xc and yc
    F_left_right=dot(F_round,[1,0]);
    if F_left_right>t_xc
        xc=xc+d_xc;
    elseif F_left_right<-t_xc
        xc=xc-d_xc;
    end

    F_down_up=dot(F_round,[0,1]);
    if F_down_up>t_yc
        yc=yc+d_yc;
    elseif F_down_up<-t_yc
        yc=yc-d_yc;
    end

    %% update xc and yc again according to diagonal force
    F_diag1=dot(F_round,[0.7071,0.7071]);
    if F_diag1>t_xc+t_yc
        xc=xc+d_xc;
        yc=yc+d_yc;
    elseif F_diag1<-t_xc-t_yc
        xc=xc-d_xc;
        yc=yc-d_yc;
    end

    F_diag2=dot(F_round,[-0.7071,0.7071]);
    if F_diag2>t_xc+t_yc
        xc=xc-d_xc;
        yc=yc+d_yc;
    elseif F_diag2<-t_xc-t_yc
        xc=xc+d_xc;
        yc=yc-d_yc;
    end

    %% update a and b

    if F_left+F_right>t_a
        a=a-d_a;
    elseif F_left+F_right<-t_a
        a=a+d_a;
    end

    if F_up+F_down>t_b
        b=b-d_b;
    elseif F_up+F_down<-t_b
        b=b+d_b;
    end

    if b>a
        temp=a;a=b;b=temp;
        phi=mod(phi+pi/2,pi);
    end

    %% restrict a and b using lower and upper bounds
    if a>a_up
        a=a_up;
    end
    if a<a_low
        a=a_low;
    end
    if b>b_up
        b=b_up;
    end
    if b<b_low
        b=b_low;
    end

    %% fitness function
    if show_fitness==1
        beta=0.9;
        fit0=0;
        fit1=0;
        fit2=0;
        [x,y,theta]=arb_ellipse_in_image(m,n,xc,yc,a,b,phi);
        [x1,y1,theta1]=arb_ellipse_in_image(m,n,xc,yc,a*beta,b*beta,phi);
        [x2,y2,theta2]=arb_ellipse_in_image(m,n,xc,yc,a/beta,b/beta,phi);
        for i=1:max(size(theta))
            fit0=fit0+norm([field_x(y(i),x(i)),field_y(y(i),x(i))]);
        end
        for i=1:max(size(theta1))
            fit1=fit1+norm([field_x(y1(i),x1(i)),field_y(y1(i),x1(i))]);
        end
        for i=1:max(size(theta2))
            fit2=fit2+norm([field_x(y2(i),x2(i)),field_y(y2(i),x2(i))]);
        end
        fit0=fit0/max(size(theta));
        fit1=fit1/max(size(theta1));
        fit2=fit2/max(size(theta2));
        fit_save(it)=fit0-fit1/2-fit2/2;
    end

end

if show_fitness==1
    figure;hold on;
    plot(1:iter,fit_save,'b','LineWidth',2);
    legend('fitness function');
    grid on;
    xlabel('iteration');
    ylabel('fitness function');
    title('fitness function in each iteration');
end


function x = normalize(x, mu, sigma)
    x=bsxfun(@minus,x,mu);
	x=bsxfun(@rdivide,x,sigma);
end


function centroids = kMeansInitCentroids(X, K)
%KMEANSINITCENTROIDS This function initializes K centroids that are to be
%used in K-Means on the dataset X
%   centroids = KMEANSINITCENTROIDS(X, K) returns K initial centroids to be
%   used with the K-Means on the dataset X
%

% You should return this values correctly
centroids = zeros(K, size(X, 2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should set centroids to randomly chosen examples from
%               the dataset X
%

randidx = randperm(size(X , 1));
centroids = X(randidx(1 : K) ,:);

% =============================================================

end


%% LIPSOL TESTING

%% LP1 [-31.4]
clc
%Objective & Constraints
f = -[6 5]';
A = ([1,4; 6,4; 2, -5]);
b = [16;28;6];
lb = [0;0];
ub = [10;10];
%Solve
[x,fval,ef,info] = opti_lipsol(f,A,b,[],[],lb,ub)

%% LP2
clc
%Objective & Constraints
f = -[1 2 3]';
A = [-1,1,1; 1,-3,1];
b = [20,30]';
Aeq = [1 1 1];
beq = 40;
lb = [0;0;0];
ub = [40;inf;inf];
%Solve
[x,fval,ef,info] = opti_lipsol(f,A,b,Aeq,beq,lb,ub)

%% LP3 (needs lower bounds...)
% clc
% %Objective & Constraints
% f = [8,1]';
% A = [-1,-2;1,-4;3,-1;1,5;-1,1;-1,0;0,-1];
% b = [-4,2,21,39,3,0,0]';
% %Solve
% [x,fval,ef,info] = opti_lipsol(f,A,b)

%% Raw LIPSOL Test
clc
%Problem
A = sparse([ 1     3     4     1     0
             5    -1     1     0    -1
             2     1    -1     0     0]);
b = [8;0;4];
c = [2;1;-2.5;0;0];
lb = [2;0;0;0;0];
ub = [1e32;1e32;9;1e32;1e32];
[xsol,fp,fd,info,msg,times] = lipsol(A,b,c,lb,ub)

%% MPS Files
clc
prob = coinRead('maros-r7.mps');
opts = optiset('solver','lipsol','display','iter','maxiter',1e4);
Opt = opti(prob,opts);
%Solve
[~,f,e,i] = solve(Opt)


function L_adapt = ReinhardBilateralFiltering(L, pAlpha, pPhi, pEpsilon)
%
%
%      L_adapt = ReinhardBilateralFiltering(L, pAlpha, pPhi, pEpsilon)
%
%
%       Input:
%           -L: input grayscale image
%           -pAlpha: value of exposure of the image
%           -pPhi: a parameter which controls the sharpening
%           -pEpsilon: smoothing threshold
%
%       Output:
%           -L_adapt: filtered image
%
%     Copyright (C) 2015  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('pAlpha', 'var'))
    pAlpha = ReinhardAlpha(L);
end

if(~exist('pPhi', 'var'))
    pPhi = 8;
end

if(~exist('pEpsilon', 'var'))
    pEpsilon = 0.05; %as in the original paper
end

sMax = 8;
tmp = ((2^pPhi) * pAlpha) / (sMax^2);
L_tmp = L ./ (L + tmp);
L_adapt = bilateralFilter(L_tmp, [], 0, 1.0, 1.6, pEpsilon / 2.0);
L_adapt = RemoveSpecials(L_adapt * tmp ./ (1 - L_adapt));

end


function [ t, rank ] = subset_lex_successor ( n, t, rank )

%*****************************************************************************80
%
%% SUBSET_LEX_SUCCESSOR computes the subset lexicographic successor.
%
%  Discussion:
%
%    In the original code, there is a last element with no successor.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer N, the number of elements in the master set.
%    N must be positive.
%
%    Input/output, integer T(N), describes a subset.  T(I) is 0 if
%    the I-th element of the master set is not in the subset, and is
%    1 if the I-th element is part of the subset.
%    On input, T describes a subset.
%    On output, T describes the next subset in the ordering.
%    If the input T was the last in the ordering, then the output T
%    will be the first.
%
%    Input/output, integer RANK, the rank.
%    If RANK = -1 on input, then the routine understands that this is
%    the first call, and that the user wishes the routine to supply
%    the first element in the ordering, which has RANK = 0.
%    In general, the input value of RANK is increased by 1 for output,
%    unless the very last element of the ordering was input, in which
%    case the output value of RANK is 0.
%

%
%  Return the first element.
%
  if ( rank == -1 )
    t(1:n) = 0;
    rank = 0;
    return
  end
%
%  Check.
%
  subset_check ( n, t );

  for i = n : -1 : 1

    if ( t(i) == 0 )
      t(i) = 1;
      rank = rank + 1;
      return
    else
      t(i) = 0;
    end

  end

  rank = 0;

  return
end


function z = quad_over_lin( varargin )

%QUAD_OVER_LIN Sum of squares over linear.
%   Z=QUAD_OVER_LIN(X,Y), where X is a vector and Y is a scalar, is equal to
%   SUM(ABS(X).^2)./Y if Y is positive, and +Inf otherwise. Y must be real.
%
%   If X is a matrix, QUAD_OVER_LIN(X,Y) is a row vector containing the values
%   of QUAD_OVER_LIN applied to each column. If X is an N-D array, the operation
%   is applied to the first non-singleton dimension of X.
%
%   QUAD_OVER_LIN(X,Y,DIM) takes the sum along the dimension DIM of X.
%   A special value of DIM == 0 is accepted here, which is automatically
%   replaced with DIM == NDIMS(X) + 1. This has the effect of eliminating
%   the sum; thus QUAD_OVER_LIN( X, Y, NDIMS(X) + 1 ) = ABS( X ).^2 ./ Y.
%
%   In all cases, Y must be compatible in the same sense as ./ with the squared
%   sum; that is, Y must be a scalar or the same size as SUM(ABS(X).^2,DIM).
%
%   Disciplined convex programming information:
%       QUAD_OVER_LIN is convex, nonmontonic in X, and nonincreasing in Y.
%       Thus when used with CVX expressions, X must be convex (or affine)
%       and Y must be concave (or affine).

%
% Check arguments
%

persistent P
if isempty( P ),
    P.map = cvx_remap( ...
        { { 'any' }, { 'nonpositive' } }, ...
        { { 'constant' }, { 'positive' } }, ...
        { { 'l_convex' },  { 'l_concave' } }, ...
        { { 'l_concave' }, { 'l_convex' } }, ...
        { { 'r_affine', 'p_convex', 'n_concave' }, { 'positive' } }, ...
        { { 'r_affine', 'p_convex', 'n_concave' }, { 'concave' } }, ...
        { { 'affine', 'p_convex', 'n_concave' }, { 'positive' } }, ...
        { { 'affine', 'p_convex', 'n_concave' }, { 'concave' } }, ...
        [ 0, 1, 2, 2, 3, 4, 5, 6 ] );
    P.funcs = { @qol_cnst, @qol_log, @qol_sqr, @qol_lin, @qol_sqa, @qol_lin, @qol_cpx };
    P.constant = 1;
    P.name = 'quad_over_lin';
end
[ sx, x, y, dim ] = cvx_get_dimension( varargin, 3, 'zero', true );
if sx(dim) > 1, x = norms( x, 2, dim ); end
z = cvx_binary_op( P, x, y );

function z = qol_cnst( x, y )
z = x .^ 2 / y;

function z = qol_log( x, y )
z = exp( 2 * log( x ) - log( y ) );

function z = qol_sqr( x, y )
z = square( x ) ./ y;

function z = qol_lin( x, y ) %#ok
sz = max( size(x), size(y) );
cvx_begin
    epigraph variable z( sz ) nonnegative_
    { linearize(x), linearize(y), 0.5 * z } == rotated_lorentz( sz, 0 ); %#ok
cvx_end

function z = qol_sqa( x, y )
z = square( abs( x ) ) ./ y;

function z = qol_cpx( x, y ) %#ok
sz = max( size(x), size(y) );
cvx_begin
    epigraph variable z( sz ) nonnegative_
    { linearize(x), linearize(y), 0.5 * z } == rotated_complex_lorentz( sz, 0 ); %#ok
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [comb] = mne_combine_xyz(vec)
%
% function [comb] = mne_combine_xyz(vec)
%
% Compute the three Cartesian components of a vector together
%
%
% vec         - Input row or column vector [ x1 y1 z1 ... x_n y_n z_n ]
% comb        - Output vector [x1^2+y1^2+z1^2 ... x_n^2+y_n^2+z_n^2 ]
%

%
%
%   Author : Matti Hamalainen, MGH Martinos Center
%   License : BSD 3-clause
%
%   Revision 1.1  2006/05/05 03:50:40  msh
%   Added routines to compute L2-norm inverse solutions.
%   Added mne_write_inverse_sol_stc to write them in stc files
%   Several bug fixes in other files
%
%

me = 'MNE:mne_combine_xyz';
if nargin ~= 1
    error(me,'Wrong number of arguments');
end
if size(vec,1) > size(vec,2)
    vec = vec';
end
if size(vec,1) ~= 1 || mod(size(vec,2),3) ~= 0
    error(me,'Input must be a row or a column vector with 3N components');
end

s = mne_block_diag(vec,3);
comb = full(diag(s*s'));
if size(vec,1) > size(vec,2)
    comb = comb';
end

return;


%PLOT_ELLIPSE Draw an ellipse or ellipsoid
%
% plot_ellipse(E, OPTIONS) draws an ellipse or ellipsoid defined by X'EX =
% 0 on the current plot, centred at the origin.  E (2x2) for an ellipse and
% E (2x3) for an ellipsoid.
%
% plot_ellipse(E, C, OPTIONS) as above but centred at C=[X,Y].  If
% C=[X,Y,Z] the ellipse is parallel to the XY plane but at height Z.
%
% H = plot_ellipse(...) as above but return graphic handle.
%
% Options::
% 'confidence',C   confidence interval, range 0 to 1
% 'alter',H        alter existing ellipses with handle H
% 'npoints',N      use N points to define the ellipse (default 40)
% 'edgecolor'      color of the ellipse boundary edge, MATLAB color spec
% 'fillcolor'      the color of the ellipses's interior, MATLAB color spec
% 'alpha'          transparency of the fillcolored ellipse: 0=transparent, 1=solid
% 'shadow'         show shadows on the 3 walls of the plot box
%
% - For an unfilled ellipse:
%   - any standard MATLAB LineStyle such as 'r' or 'b---'.
%   - any MATLAB LineProperty options can be given such as 'LineWidth', 2.
% - For a filled ellipse any MATLAB PatchProperty options can be given.
%
% Example::
%
%          H = plot_ellipse(diag([1 2]), [3 4]', 'r'); % draw red ellipse
%          plot_ellipse(diag([1 2]), [5 6]', 'alter', H); % move the ellipse
%          plot_ellipse(diag([1 2]), [5 6]', 'alter', H, 'LineColor', 'k'); % change color
%
%          plot_ellipse(COVAR, 'confidence', 0.95); % draw 95% confidence ellipse
%
% Notes::
% - The 'alter' option can be used to create a smooth animation.
% - If E (2x2) draw an ellipse, else if E (3x3) draw an ellipsoid.
% - The ellipse is added to the current plot irrespective of hold status.
% - Shadow option only valid for ellipsoids.
% - If a confidence interval is given then E is interpretted as a covariance
%   matrix and the ellipse size is computed using an inverse chi-squared function.
%   This requires CHI2INV in the Statistics and Machine Learning Toolbox or
%   CHI2INV_RTB from the Robotics Toolbox for MATLAB.
%
% See also PLOT_ELLIPSE_INV, PLOT_CIRCLE, PLOT_BOX, PLOT_POLY, CH2INV.

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

function handles = plot_ellipse(E, varargin)

    assert(size(E,1) == size(E,2), 'ellipse is defined by a square matrix');
    assert( size(E,1) == 2 || size(E,1) == 3, 'can only plot ellipsoid for 2 or 3 dimenions');

    opt.fillcolor = 'none';
    opt.alpha = 1;
    opt.edgecolor = 'k';
    opt.alter = [];
    opt.npoints = 40;
    opt.shadow = false;
    opt.confidence = [];

    [opt,arglist,ls] = tb_optparse(opt, varargin);

    % process some arguments

    if ~isempty(ls)
        opt.edgecolor = ls{1};
    end

    % process the probability
    if isempty(opt.confidence)
        s = 1;
    else
        if exist('chi2inv') == 2
            s = sqrt(chi2inv(opt.confidence, 2));
        elseif exist('chi2inv_rtb') == 2
            s = sqrt(chi2inv_rtb(opt.confidence, 2));
        else
            error('SMTB:missingfunc', 'Requires Stats Toolbox or RTB to be installed');
        end
    end

    if length(arglist) > 0 && isnumeric(arglist{1})
        % ellipse centre is provided
        centre = arglist{1};
        arglist = arglist(2:end);
    else
        % default to origin
        centre = zeros(1, size(E,1));
    end

    % check the ellipse to be altered
    if ~isempty(opt.alter) & ~ishandle(opt.alter)
        error('SMTB:plot_ellipse:badarg', 'argument to alter must be a valid graphic object handle');
    end

    holdon = ishold();
    hold on

    if size(E,1) == 3
        %% plot an ellipsoid

        % define mesh points on the surface of a unit sphere
        [Xs,Ys,Zs] = sphere();
        ps = [Xs(:) Ys(:) Zs(:)]';

        % warp it into the ellipsoid
        pe = sqrtm(E) * ps;

        % offset it to optional non-zero centre point
        if nargin > 1
            pe = bsxfun(@plus, centre(:), pe);
        end

        % put back to mesh format
        Xe = reshape(pe(1,:), size(Xs));
        Ye = reshape(pe(2,:), size(Ys));
        Ze = reshape(pe(3,:), size(Zs));

        if isempty(opt.alter)
              % plot it
%             Ce = ones(size(Xe));
%             Ce = cat(3, Ce*0.8, Ce*0.4, Ce*0.4);
            h = mesh(Xe, Ye, Ze, 'FaceColor', opt.fillcolor, ...
                        'FaceAlpha', opt.alpha, 'EdgeColor', opt.edgecolor, arglist{:});
        else
            % update an existing plot
            set(opt.alter, 'xdata', Xe, 'ydata', Ye, 'zdata', Ze,  ...
                        arglist{:});
        end

        % draw the shadow
        if opt.shadow
            I = ones(size(Xe));
            a = [xlim ylim zlim];
            mesh(a(1)*I, Ye, Ze, 'FaceColor', 0.7*[1 1 1], 'EdgeColor', 'none', 'FaceAlpha', 0.5);
            mesh(Xe, a(3)*I, Ze, 'FaceColor', 0.7*[1 1 1], 'EdgeColor', 'none', 'FaceAlpha', 0.5);
            mesh(Xe, Ye, a(5)*I, 'FaceColor', 0.7*[1 1 1], 'EdgeColor', 'none', 'FaceAlpha', 0.5);
        end

    else
        %% plot an ellipse

        [V,D] = eig(E);

        % define points on a unit circle
        th = linspace(0, 2*pi, opt.npoints);
        pc = [cos(th);sin(th)];

        % warp it into the ellipse
        pe = sqrtm(E)*pc * s;

        % offset it to optional non-zero centre point
        centre = centre(:);
        if nargin > 1
            pe = bsxfun(@plus, centre(1:2), pe);
        end
        x = pe(1,:); y = pe(2,:);

%         if length(centre) > 2
%             % plot 3D data
%             z = ones(size(x))*centre(3);
%             if isempty(opt.alter)
%                 h = plot3(x', y', z', varargin{:});
%             else
%                 set(opt.alter, 'xdata', x, 'ydata', y, 'zdata', z, arglist{:});
%             end

            % plot 2D data

            if length(centre) > 2
                % plot 3D data
                z = ones(size(x))*centre(3);
            else
                z = zeros(size(x));
            end

            if strcmpi(opt.fillcolor, 'none')
                % outline only, draw a line

                if isempty(ls)
                    if ~isempty(opt.edgecolor)
                        arglist = ['Color', opt.edgecolor, arglist];
                    end
                else
                    arglist = [ls arglist];
                end

                if isempty(opt.alter)
                    h = plot3(x', y', z', arglist{:});
                else
                    set(opt.alter, 'xdata', x, 'ydata', y);
                end
            else
                % fillcolored, use a patch

                if ~isempty(opt.edgecolor)
                    arglist = ['EdgeColor', opt.edgecolor, arglist];
                end

                arglist = [ls, 'FaceAlpha', opt.alpha, arglist];

                if isempty(opt.alter)
                    h = patch(x', y', z', opt.fillcolor, arglist{:});
                else
                    set(opt.alter, 'xdata', x, 'ydata', y);
                end

            end
        end

  if ~holdon
      hold off
  end

    if nargout > 0
        handles = h;
    end
end