function R = sw_rcu(a, n_vals, eps)
% Compute the achievable sum rate (at the symmetrical rate point) from the SW RCU bound
% for the joint source distribution: [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)].

% Input:
% a: parameter to define the joint source distribution, should be in range [0.25, 1)
% n_vals: a vector containing the blocklengths
% eps: target error probability
% Output:
% R: the achievable rates for the blocklengths specified in n_vals.

    p = [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)];
    H = sum(sum(p.*log(1./p)));

    % Compute log binomial coefficients
    log_b = binomial_coeff(max(n_vals));

    R = zeros(length(n_vals),1);

    for i = 1:length(n_vals)
        n = n_vals(i);
        m = 0:n;
        % log_Pr stores all the log joint probabilities in ascending order
        log_Pr = m*log(a)+(n-m)*log(1/3*(1-a));

        % Calculate and store the coefficients that will be used iteratively later
        A_m = {};
        A_kj = {};
        for m = 0:n
            A_tempt = log_b{n+1}+(n:-1:0)*log(3);
            A_m{m+1} = my_logsumexp(A_tempt(m+1:n+1));
            for k = 0:n-m
                A_kj{m+1}(k+1) = my_logsumexp(log_b{m+k+1}(m+1:m+k+1))+(n-m-k)*log(2);
            end
        end

        % [x, y]: initial range for the bisection algorithm
        % For n small or eps small, need to adjust the right boundary of this initial interval.
        x = H;
        y = H + 0.5;
        %%%%%%%%%% Use this part to check if the range is valid for bisection algorithm %%%%%%%%%%
        c = x;
        log_err_m = zeros(1,n+1);
        for m = 0:n
            log_err_k = zeros(1,n-m+1);
            for k = 0:n-m
                log_err_j = zeros(1,n-m-k+1);
                for j = 0:n-m-k
                    log_err_j(j+1) = log_b{n-m-k+1}(j+1) + ...
                        min(0, my_logsumexp([A_kj{m+1}(j+1)-n*c/2 A_kj{m+1}(k+1)-n*c/2 A_m{m+1}-n*c]));
                end
                log_err_k(k+1) = log_b{n-m+1}(k+1) + my_logsumexp(log_err_j);
            end
            log_err_m(m+1) = log_Pr(m+1) + log_b{n+1}(m+1) + my_logsumexp(log_err_k);
        end
        err = exp(my_logsumexp(log_err_m));
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        % Start of bisection iteration
        iter = 0;
        while abs(x - y) > 0.00001 && iter <= 30
            c = (x+y)/2;
            %%%%%%%%%%%%%%%% body of iteration %%%%%%%%%%%%%%%%
            log_err_m = zeros(1,n+1);
            for m = 0:n
                log_err_k = zeros(1,n-m+1);
                for k = 0:n-m
                    log_err_j = zeros(1,n-m-k+1);
                    for j = 0:n-m-k
                        log_err_j(j+1) = log_b{n-m-k+1}(j+1) + ...
                            min(0, my_logsumexp([A_kj{m+1}(j+1)-n*c/2 A_kj{m+1}(k+1)-n*c/2 A_m{m+1}-n*c]));
                    end
                    log_err_k(k+1) = log_b{n-m+1}(k+1) + my_logsumexp(log_err_j);
                end
                log_err_m(m+1) = log_Pr(m+1) + log_b{n+1}(m+1) + my_logsumexp(log_err_k);
            end
            err = exp(my_logsumexp(log_err_m));
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            if err - eps < 0
                y = c;
            else
                x = c;
            end
            iter = iter + 1;
        end
        R(i) = c;
    end
end


function y=gaussmix(m1,sigma1,m2,sigma2,lambda)

n=length(m1);

for i=1:n
    if rand>lambda
y(i)=random('normal',m1(i),sigma1);
    else
y(i)=random('normal',m2(i),sigma2);
    end
end


function x = RofCurve(X,Y)

% X=[10 0 -8];
% Y=[0 10 0];

% Defining the three nonlinear equations to be solved (equation of circle
% passing through the 3 points)
defn1=['[ ( x(1)-' num2str(X(1)) ' )^2+( x(2)-' num2str(Y(1)) ')^2-x(3)^2 ;' ];
defn2=[  '( x(1)-' num2str(X(2)) ' )^2+( x(2)-' num2str(Y(2)) ')^2-x(3)^2 ;' ];
defn3=[  '( x(1)-' num2str(X(3)) ' )^2+( x(2)-' num2str(Y(3)) ')^2-x(3)^2 ]' ];
inlinedef=[defn1 defn2 defn3];

%Defining the inline function that will compute  the error, which has to be
%minimized
myfun=inline(inlinedef,'x');

options = optimset('Display','off','TolX',1e-10);
x0=[0;0;0];
[x,fval] = fsolve(myfun,x0,options);
x=[x(1) x(2) abs(x(3))];


function [nodes, edges, faces] = boundedVoronoi2d(box, germs)
%BOUNDEDVORONOI2D Return a bounded voronoi diagram as a graph structure
%
%   [NODES, EDGES, FACES] = boundedVoronoi2d(BOX, GERMS)
%   GERMS an array of points with dimension 2
%   NODES, EDGES, FACES: usual graph representation, FACES as cell array
%
%   Example
%   [n, e, f] = boundedVoronoi2d([0 100 0 100], rand(100, 2)*100);
%   drawGraph(n, e);
%
%   See also
%     graphs, boundedCentroidalVoronoi2d, clipGraph, clipGraphPolygon

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2007-01-12
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% uniformize input for box.
box = box';
box = box(:);

% add points far enough
boxSizeX  = box(2) - box(1);
boxSizeY  = box(4) - box(3);
farPoints = [...
    box(2)+2*boxSizeX  box(4)+3*boxSizeY;...
    box(1)-3*boxSizeX  box(4)+2*boxSizeY;...
    box(1)-2*boxSizeX  box(3)-3*boxSizeY;...
    box(2)+3*boxSizeX  box(3)-2*boxSizeY;...
    ];

% extract voronoi vertices and face structure
[V, C] = voronoin([germs ; farPoints]);

% initialize graph structure, without edges and without faces
nodes = V(2:end, :);
edges = zeros(0, 2);
faces = {};

for i = 1:size(germs, 1)
    cell = C{i};
    cell = cell-1;
    edges = [edges; sort([cell' cell([2:end 1])'], 2)]; %#ok<AGROW>
    faces{length(faces)+1} = cell; %#ok<AGROW>
end

edges = unique(edges, 'rows');


function [xs,phid,wlsden,timeiter,niter] = wls_cd(A, W, yy, xhat, niter, tol)
%	weighted least squares minimized by coordinate descent algorithm
%	Input
%		A	[nn,np]		system matrix
%		W	[nn,nn]		inverse data covariance
%		yy	[nn,1]		noisy data
%		xhat	[np,1] or [nx,ny]	initial estimate
%		tol			stopping criterion for convergence
%		niter			max # of iterations
%
%	Output
%		xs	[np,niter+1]	estimates each iteration
%		phid	[niter+1,1]	objective decrease
%		wlsden	[np,1]		curvature of surrogate
%		timeiter [niter+1,1]	CPU time each iteration
%		niter			# total iterations
%
% Saowapak Sotthivirat
% 8/21/00

[nx,ny]=size(xhat);
np = ncol(A);

yy = yy(:);
xhat = xhat(:);
xs(:,1) = xhat;

% Precompute the curvature
wlsden = diag(A'*W*A);

wii = diag(W);
res = A * xhat - yy;
phi0 = res'*W*res/2;
ii = 1;
phid(ii) = 0;
t0 = cputime;

while phid(ii) < tol & ii <= niter

	for j=1:np
		xjold = xhat(j);
		xjnew = xjold;
		phijdot = sum(A(:,j).*wii.*res);
		diff = phijdot/wlsden(j);
		xjnew = xjnew - diff;
		xhat(j) = max(0,xjnew);
		res = res - A(:,j) * diff;
	end

	ii = ii+1;
	xs(:,ii) = xhat;
	phid(ii) = phi0 - res'*W*res/2;
	timeiter(ii) = cputime - t0;
end
niter = ii;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% getalp.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [alp,lba,uba,ier]=getalp(alpu,alpo,gTp,pTGp);
% get minimizer alp in [alpu,alpo] for a univariate quadratic
%	q(alp)=alp*gTp+0.5*alp^2*pTGp
% lba	lower bound active
% uba	upper bound active
%
% ier	 0 (finite minimizer)
%	 1 (unbounded minimum)
%
function [alp,lba,uba,ier]=getalp(alpu,alpo,gTp,pTGp);

lba=0;
uba=0;

% determine unboundedness
ier=0;
if alpu==-inf & ( pTGp<0 | (pTGp==0 & gTp>0) ),
  ier=1; lba=1;
end;
if alpo==inf & (pTGp<0 | (pTGp==0 & gTp<0) ),
  ier=1; uba=1;
end;
if ier, alp=NaN; return; end;

% determine activity
if pTGp==0 & gTp==0,
  alp=0;
elseif pTGp<=0,
  % concave case minimal at a bound
  if alpu==-inf,     lba=0;
  elseif alpo== inf, lba=1;
  else               lba = (2*gTp+(alpu+alpo)*pTGp>0);
  end;
  uba = ~lba;
else
  alp=-gTp/pTGp;          % unconstrained optimal step
  lba = (alp <= alpu);    % lower bound active
  uba = (alp >= alpo);    % upper bound active
end;

if lba, alp=alpu; end;
if uba, alp=alpo; end;

% print?
if abs(alp)==inf, gTp,pTGp,alpu,alpo,alp,lba,uba,ier, end;


function [tap] = hanning(n, str)

%HANNING   Hanning window.
%   HANNING(N) returns the N-point symmetric Hanning window in a column
%   vector.  Note that the first and last zero-weighted window samples
%   are not included.
%
%   HANNING(N,'symmetric') returns the same result as HANNING(N).
%
%   HANNING(N,'periodic') returns the N-point periodic Hanning window,
%   and includes the first zero-weighted window sample.
%
%   NOTE: Use the HANN function to get a Hanning window which has the
%          first and last zero-weighted samples.
%
%   See also BARTLETT, BLACKMAN, BOXCAR, CHEBWIN, HAMMING, HANN, KAISER
%   and TRIANG.
%
%   This is a drop-in replacement to bypass the signal processing toolbox

% Copyright (c) 2010, Jan-Mathijs Schoffelen, DCCN Nijmegen
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin==1
  str = 'symmetric';
end

switch str
case 'periodic'
   % Includes the first zero sample
   tap = [0; hanningX(n-1)];
case 'symmetric'
   % Does not include the first and last zero sample
   tap = hanningX(n);
end

function tap = hanningX(n)

% compute taper
N   = n+1;
tap = 0.5*(1-cos((2*pi*(1:n))./N))';

% make symmetric
halfn = floor(n/2);
tap( (n+1-halfn):n ) = flipud(tap(1:halfn));


% function F = det_F_algebraic(x1,x2,L_COST,NORMALIZE)
% Determines the F by iteratively minimizing the Algebraic error
% Algorithm 11.2 in Hartley & Zisserman, Multiple View Geometry in Computer Vision
% Inputs:
%           x           3xN coordinates of matched points in image 1(homogeneous)
%           xp          3xN coordinates of matched points in image 2(homogeneous)
%           L_COST      1x1 (optional) controls penalization scheme
%                           for the cost function: L_COST 1 leads to L_1 minimization of
%                           the norm of the algebraic error (the one mentioned in the book)
%           NORMALIZE   1x1 (optional) determines if the algorithm should use the normalized points
% Outputs:
%           F           3x3 the fundamental matrix
%
% Author: Omid Aghazadeh, KTH(Royal Institute of Technology), 2010/05/09
function F = det_F_algebraic(x1,x2,L_COST,NORMALIZE)
if sum(size(x1)~=size(x2)), error('size of correspondences do not match!'), end
if size(x1,1) ~= 3, error('invalid points'), end
if nargin<3; L_COST = 1; end;
if nargin<4; NORMALIZE = 1; end
global A F_I EPSILON_I L_e TOL_X TOL_FUN MAX_FUN_EVAL MAX_ITER;
if NORMALIZE
    nmat1 = get_normalization_matrix(x1); % isotropic normalization (translation/scaling)
    nmat2 = get_normalization_matrix(x2);
    x1n = nmat1*x1;
    x2n = nmat2*x2;
    x1n = x1n./repmat(x1n(3,:),3,1); x2n = x2n./repmat(x2n(3,:),3,1); % normalizing points so their last coordinate is 1
else
    nmat1 = eye(3); nmat2 = eye(3); x1n = x1; x2n = x2;
end
F_0 = det_F_normalized_8point(x1n,x2n);

F_0 = F_0/norm(F_0(:));
F_0T = reshape(F_0',9,1); % f corresponds to F_0 in row order
F_I = [F_0T];
A = [(repmat(x2n(1,:),3,1).*x1n)',(repmat(x2n(2,:),3,1).*x1n)',x1n']; % the data matrix

EPSILON_I = norm(A*F_0T)^L_COST; %initial cost

%% (i) finding an initial estimate of the epipole from an initial estimate of F
[e_0,eprime_0] = get_epipole(F_0);
it=0;
L_e = e_0;

e_i = lsqnonlin(@(e)costAlgebraic(A,L_COST,e),e_0,[],[],optimset('Display','off','TolX',TOL_X,'TolFun',TOL_FUN,'MaxFunEval',MAX_FUN_EVAL,'MaxIter',MAX_ITER,'Algorithm',{'levenberg-marquardt' 0.01}));

F = reshape(F_I,3,3)';
F = nmat2'*F*nmat1;
end

%% function x = const_min_subject_span_space(A,G,r)
% finds the vector x that maximizes ||A x|| subject to the constraint ||x||=1 and x=G x_hat
% where G has rank r
function x = const_min_subject_span_space(A,G,r)
[u_g,s_g,v_g] = svd(G);
u_prime = u_g(:,1:r);

[u_a,s_a,v_a] = svd(A*u_prime);
x_p = v_a(:,end);

x = u_prime * x_p;
end

%% epsilon_cost_i = costAlgebraic(A,L_COST,e_i)
% the cost function for LM algorithm. The goal is to minimize || epsilon_i || by varying e_i
function epsilon_cost_i = costAlgebraic(A,L_COST,e_i)
global F_I EPSILON_I L_e;
%% (ii) find f_i that minimizes ||A f_i|| subject to ||f_i|| = 1
e_cross = get_x_cross(e_i);
E = [e_cross zeros(3) zeros(3); zeros(3) e_cross zeros(3); zeros(3) zeros(3) e_cross];
f_i = const_min_subject_span_space(A,E,6);

%% (iii) compute epsilon_i = A f_i and correct its sign
pos_sign = double(sign(e_i'*L_e));
f_i = f_i * pos_sign;

epsilon_i = A*f_i;
epsilon_cost_i = (norm(epsilon_i))^L_COST;

if epsilon_cost_i < EPSILON_I
    EPSILON_I = epsilon_cost_i;
    F_I = f_i;
    L_e = e_i;
end
end


function [SPP] = point_to_hae_newton(R_TGT_COA, Rdot_TGT_COA, ARP_COA, VARP_COA, SCP, HAE0, delta_MAX, NLIM)
% POINT_TO_HAE_NEWTON transforms pixel row, col to a constant height
% above the ellipsoed via algorithm in SICD Image Projections.
%
% GPP = point_to_hae_newton(R_TGT_COA, Rdot_TGT_COA, ARP_COA, VARP_COA, SCP, HAE0)
%
% Inputs:
%    R_TGT_COA    - range to the ARP at COA
%    Rdot_TGT_COA - range rate relative to the ARP at COA
%    ARP_COA      - aperture reference position at tCOA
%    VARP_COA     - velocity at tCOA
%    SCP          - scene center point (ECF meters)
%    HAE0         - Surface height (m) above the WGS-84 reference ellipsoid
%                   for projection point SPP
%    delta_MAX    - Threshold for convergence of iterative projection
%                   sequence.
%    NLIM         - Maximum number of iterations allowed.
%
% Outputs:
%    SPP          - [3xN] Surface Projection Point position on the HAE0
%                   surface and along the R/Rdot contour
%
% Authors: Rocco Corsetti, NGA/IB
%          Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

if ~exist('delta_MAX','var')
    delta_MAX = 1e-3;
end
if ~exist('NLIM','var')
    NLIM = 10;
end
SPP = zeros(3,numel(R_TGT_COA));

%% 9. Precise R/Rdot To Constant HAE Surface Projection

% This way of looping through points really adds no efficiency in MATLAB.
% Really we shouled vectorize the matrix operations and not loop.  However,
% this is an easy way to make its behaviour match the other projection
% functions in the toolbox (point_to_ground_plane, point_to_hae).
for points_i = 1:numel(R_TGT_COA)

    % Rename variables from SICD projection document notation to "Spotlight
    % Synthetic Aperture Radar (SAR) Sensor Model" notation
    R = SCP(:);
    h = HAE0;

    Range_obs = R_TGT_COA(points_i);
    Doppler_obs = -Rdot_TGT_COA(points_i);

    R_ARP_curr = ARP_COA(:,(points_i));
    V_ARP_curr = VARP_COA(:,(points_i));

    R0 = R;
    a = 6378137.0; % WGS84 semimajor axis (m)
    b = 6356752.314245; % WGS84 semiminor axis (m)

    delta = Inf;
    iters = 0;
    while (max(abs(delta)) > delta_MAX) && (iters < NLIM)

        F1 = -9999*ones(3,3);
        F2 = -9999*ones(3,3);
        F3 = -9999*ones(3,3);

        for i = 1:3
            for j = 1:3
                switch j
                    case(1)
                        eps = -0.5;
                    case(2)
                        eps = 0;
                    case(3)
                        eps = +0.5;
                end

                R = R0;
                R(i) = R(i) + eps;
                X = R(1); Y = R(2); Z = R(3);

                r_est = R - R_ARP_curr;

                Range_est = norm(r_est);
                Doppler_est = dot(V_ARP_curr,r_est)/norm(r_est);

                F1(i,j) = Range_obs - Range_est;
                F2(i,j) = Doppler_obs - Doppler_est;
                F3(i,j) = ((X^2 + Y^2)/(a+h)^2) + ((Z^2)/(b+h)^2) - 1;
            end
        end

        dF1dX = (F1(1,3) - F1(1,1)) / (2*0.5);
        dF1dY = (F1(2,3) - F1(2,1)) / (2*0.5);
        dF1dZ = (F1(3,3) - F1(3,1)) / (2*0.5);

        dF2dX = (F2(1,3) - F2(1,1)) / (2*0.5);
        dF2dY = (F2(2,3) - F2(2,1)) / (2*0.5);
        dF2dZ = (F2(3,3) - F2(3,1)) / (2*0.5);

        dF3dX = (F3(1,3) - F3(1,1)) / (2*0.5);
        dF3dY = (F3(2,3) - F3(2,1)) / (2*0.5);
        dF3dZ = (F3(3,3) - F3(3,1)) / (2*0.5);

        f = -[F1(1,2)
            F2(1,2)
            F3(1,2)];

        B = [dF1dX dF1dY dF1dZ
            dF2dX dF2dY dF2dZ
            dF3dX dF3dY dF3dZ];

        delta = B\f;

        R0 = R0 + delta;

        iters = iters + 1;
    end

    SPP(:,points_i) = R0; % Convert back to notation from SICD projection document
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


% This is a demonstration of inpainting using the Potts model.
% Note that it is an intrinsic property of the Potts model to
% create two triple junctions instead of a four-junction in the center.

% load image
img = double(imread('colors.png'))/255;

% set inpainting mask
simg = sum(img, 3);
weights = simg ~= 0;

% add noise
rng(123) % set seed value of random number gen. for reproducibility
imgNoisy = img + 0.3 * randn(size(img)) .* cat(3, weights, weights, weights);

%% Potts restoration
gamma = 2;
clear opts;
opts.weights = weights;
opts.verbose = true;
opts.muStep = 1.1;
opts.muInit = 1e-4;
tic
u = minL2Potts2DADMM(imgNoisy, gamma, opts);
toc

%% Show result
subplot(1,2,1)
imshow(imgNoisy)
title('Noisy data (Black pixels are missing')
subplot(1,2,2)
imshow(u)
title('Potts inpainting')


function[C] = Cp(rho)
% C = Cp(rho)
% C_p(rho) = sqrt{\frac{1}{2\pi\int_{0}^1 r rho^2 J_1(rho r)^2 dr}}
% This constant is chosen so that e_p(r) = C_p(rho) J_0(rho_p r) has a unit
% H_0^1(B) norm. (that is \int_{B}|\nabla e_p|^2 = 1).

C = sqrt(1./(pi*rho.*(besselj(0, rho).^2.*rho+besselj(1, rho).^2.*rho-2*besselj(0, rho).*besselj(1, rho))));
C(rho==0) = 1;

end


function [w] = simple_svm(X, y, lambda, opts)
% Simple function for solving an SVM with SDCA
% Used for local & global baselines

% Inputs
% X: input training data
% y: output training data
% lambda: regularization parameter

% Output
% w: the learned model

%% initialize
[n, d] = size(X);
w = zeros(d, 1);
alpha = zeros(n, 1);
primal_old = 0;

for iter=1:opts.max_sdca_iters
    % update coordinates cyclically
    for i=1:n
        % get current variables
        alpha_old = alpha(i);
        curr_x = X(i, :);
        curr_y = y(i);

        % calculate update
        grad = lambda * n * (1.0 - (curr_y * curr_x * w)) / (curr_x * curr_x') + (alpha_old * curr_y);

        % apply update
        alpha(i) = curr_y * max(0, min(1.0, grad));
        w = w + ((alpha(i) - alpha_old) * curr_x' * (1.0 / (lambda * n)));
    end

    % break if less than tol
    preds = y .* (X * w);
    primal_new = mean(max(0.0, 1.0 - preds)) + (lambda / 2.0) * (w' * w);
    if(abs(primal_old - primal_new) < opts.tol)
        break;
    end
    primal_old = primal_new;
end

end


clear;  clc;
%P2.16
%y(n)-0.5y(n-1)+0.25y(n-2)=x(n)+2x(n-x)+x(n-3)
a=[1,-0.5,0.25];b=[1,2,1];
h=impseq(0,-10,100);n=[-10:100];
y=filter(b,a,h);
subplot(2,1,1);
stem(n,y,'k');title('Impulse Response');xlabel('n');ylabel('h(n)')
%We determine S|h(n)|
sum(abs(y))
magz = abs(roots(a))
%ans = 6.5714
%Which implies that the system is stable.

%If x(n)=[5+cos(0.2pn)+4sin(0.6pn)]u(n),
%the impulse response will show no stability
a=[1,-0.5,0.25];b=[1,2,1];
n=[-10:200];
x=(5+3*cos(0.2*pi*n)+4*sin(0.6*pi*n)).*stepseq(0 ,-10,200);
y=filter(b,a,x);
subplot(2,1,2);
stem(n,y,'k');title('Response to x(n)');xlabel('n');ylabel('y(n)')
sum(abs(y))
magz = abs(roots(a))
%ans =  5.3389e+003
% We observe that the response is not bounded, since the output is not close to zero as n approaches infinity.


function imgOut = RotateLLGUI(img)
%
%        imgOut = RotateLLGUI(img)
%
%        This tool rotates an environment map according to two picking
%        points.
%
%        Input:
%           -img: an environment map encoded as logitude-latitude
%
%        Output:
%           -imgOut: img rotated
%
%     Copyright (C) 2016 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(size(img, 2) > 2048)
   img = imresize(img, [2048, 1024], 'bilinear');
end

[r, c, col] = size(img);

figure(1);
imshow(img.^0.45);
hold on;

r_h = round(r / 2);
line([1, c], [r_h, r_h], 'Color', 'r', 'LineWidth', 4);

[x0, y0] = ginput(1);

plot(x0, y0, 'r+');

[x1, y1] = ginput(1);

plot(x1, y1, 'r+');
hold off;

%theta1 = ( (x1 - c/2) / c ) * pi;
%phi1   = ( (r/2 - y1) / r ) * pi * 0.5;

[theta1, phi1] = getThetaPhi(x1,y1,r,c);
vec1 = PolarVec3(theta1, phi1);

%thetar1 = ( (x0 - c/2) / c ) * pi;
%phir1   = ( (r/2 - y1) / r ) * pi * 0.5;
[thetar1, phir1] = getThetaPhi(x0,y1,r,c);
vecr1 = PolarVec3(thetar1, phir1);

M = getMatrixForVectorRotation(vec1, vecr1);

disp(M)
% disp(vec1);
% disp(vecr1);
% disp((M*vec1')');

D = LL2Direction(r, c);
D_rot = RotateMap(D, M');

[X1, Y1] = Direction2LL(D_rot, r, c);
[X, Y] = meshgrid(0:(c-1), 0:(r-1));
X1 = real(round(X1));
Y1 = real(round(Y1));
imgOut = zeros(size(img));
size(X)
size(X1)
for i=1:col
    imgOut(:,:,i) = interp2(X, Y, img(:,:,i), X1, Y1, 'spline');
end

hold on;
imshow(imgOut.^0.45);
plot([x0, x1], [y0, y1], 'r');
plot(x0, y0, 'go');
plot(x1, y1, 'go');
hold off;

end

function [theta, phi] = getThetaPhi(x,y,r,c)
    phi   =  pi * ((x / c) * 2 - 1) - pi * 0.5;
    theta =  pi * (y / r);
end

function M = getMatrixForVectorRotation(u, v)
    a = cross(u, v);
    len_a = norm(a);
    if(len_a > 0.0)
        a = a / len_a;

        alpha = acos(dot(u, v));
        c = cos(alpha);
        s = sin(alpha);

        ci = 1.0 - c;

        M = [a(1)^2*ci + c, a(1)*a(2)*ci - s * a(3), a(1)*a(3)*ci+s*a(2);...
             a(1)*a(2) * ci + s * a(3), a(2)^2 * ci + c, a(2)*a(3)*ci-s*a(1);...
             a(1)*a(3) * ci - s*a(2), a(1)*a(3)*ci+s*a(1), a(3)^2 * ci + c];
    else
        M = diag([1.0,1.0,1.0]);
    end
end

function D_rot = RotateMap(D, M)
    D_rot = zeros(size(D));
    D_rot(:,:,1) = D(:,:,1) * M(1,1) + D(:,:,2) * M(1,2) + D(:,:,3) * M(1,3);
    D_rot(:,:,2) = D(:,:,1) * M(2,1) + D(:,:,2) * M(2,2) + D(:,:,3) * M(2,3);
    D_rot(:,:,3) = D(:,:,1) * M(3,1) + D(:,:,2) * M(3,2) + D(:,:,3) * M(3,3);
end


function [M,delta_list,theta_list] = compute_edge_patches(w, options)

% [M,delta_list,theta_list] = compute_edge_patches(w, options);

options.null = 0;
if isfield(options, 'n_delta')
    n_delta = options.n_delta;
else
    n_delta = 11;
end
if isfield(options, 'n_theta')
    n_theta = options.n_theta;
else
    n_theta = 12;
end
if isfield(options, 'sigma')
    sigma = options.sigma;
else
    sigma = 0.1;
end
sigma = sigma*w;
if isfield(options, 'rescale')
    rescale = options.rescale;
else
    rescale = 1;
end
% the window to weight the distance
if isfield(options, 'bell')
    bell = options.bell;
else
    bell = 'constant';
end
if isfield(options, 'manifold_type')
    manifold_type = options.manifold_type;
else
    manifold_type = 'edges';
end

switch lower(bell)
    case 'constant'
        B = ones(w);
    case 'sine'
        x = linspace(0,1,w);
        x = (1-cos(2*pi*x))/2;
        B = x'*x;
    otherwise
        error('Unknown bell shape');
end

dmax = sqrt(2)*w/2+2*sigma;
eta = 2;
t = linspace(0,2*pi-2*pi/n_theta,n_theta); t = t(:);
d = linspace(-1,1,n_delta);
d = sign(d).*abs(d).^eta;
d = d(:)*dmax;

[theta_list,delta_list] = meshgrid( t, d );
theta_list = theta_list(:);
delta_list = delta_list(:);
p = length(delta_list);

x = linspace( -(w-1)/2,(w-1)/2, w );
[Y,X] = meshgrid(x,x);
M = zeros(w,w,p);
% compute images
for k=1:p
    t = theta_list(k);
    d = delta_list(k);
    y = cos(t)*X + sin(t)*Y - d;
    if strcmp(manifold_type, 'edges')
        A = tanh(y/sigma);
    else
        A = 2*( 1 - exp( -y.^2 / (2*sigma^2) ) ) - 1;
    end
    M(:,:,k) = A;
end

% normalize
if rescale
    B = repmat(B,[1,1,p]);
    s = sqrt( sum( sum(B .* (M.^2),1), 2) );
    M = M ./ repmat(s,[w,w,1]);
end


classdef UF2 < PROBLEM
% <multi> <real> <large/none>
% Unconstrained benchmark MOP

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, S. Zhao, P. N. Suganthan, W. Liu, and S. Tiwari,
% Multiobjective optimization test instances for the CEC 2009 special
% session and competition, School of CS & EE, University of Essex, Working
% Report CES-487, 2009.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = [0,zeros(1,obj.D-1)-1];
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            J1 = 3 : 2 : obj.D;
            J2 = 2 : 2 : obj.D;
            Y       = zeros(size(X));
            X1      = repmat(X(:,1),1,length(J1));
            Y(:,J1) = X(:,J1)-(0.3*X1.^2.*cos(24*pi*X1+4*repmat(J1,size(X,1),1)*pi/obj.D)+0.6*X1).*cos(6*pi*X1+repmat(J1,size(X,1),1)*pi/obj.D);
            X1      = repmat(X(:,1),1,length(J2));
            Y(:,J2) = X(:,J2)-(0.3*X1.^2.*cos(24*pi*X1+4*repmat(J2,size(X,1),1)*pi/obj.D)+0.6*X1).*sin(6*pi*X1+repmat(J2,size(X,1),1)*pi/obj.D);
            PopObj(:,1) = X(:,1)         + 2*mean(Y(:,J1).^2,2);
            PopObj(:,2) = 1-sqrt(X(:,1)) + 2*mean(Y(:,J2).^2,2);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - R(:,1).^0.5;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
        	R = obj.GetOptimum(100);
        end
    end
end


%CONVEXHULL  Finds the convex hull of a point set
%
%     hull = cv.convexHull(points)
%     hull = cv.convexHull(points, 'OptionName', optionValue, ...)
%
% ## Input
% * __points__ Input 2D point set, stored in numeric array
%   (Nx2/Nx1x2/1xNx2) or cell array of 2-element vectors (`{[x,y], ...}`).
%
% ## Output
% * __hull__ Output convex hull. It is either an integer vector of indices or
%   vector of points. In the first case, the hull elements are 0-based indices
%   of the convex hull points in the original array (since the set of convex
%   hull points is a subset of the original point set). In the second case,
%   hull elements are the convex hull points themselves. In case output is the
%   hull points, it has the same type as the input.
%
% ## Options
% * __ReturnPoints__ Operation flag. In case of a matrix, when the flag is
%   true, the function returns convex hull points (Mx2 matrix). Otherwise, it
%   returns indices of the convex hull points (vector of length M). In case
%   the input is a cell-array, when the flag is true, the function return
%   convex hull points (as a cell-array of points). Otherwise it returns
%   indices of points (vector of length M). default true
% * __Clockwise__ Orientation flag. If it is true, the output convex hull is
%   oriented clockwise. Otherwise, it is oriented counter-clockwise. The usual
%   screen coordinate system is assumed so that the origin is at the top-left
%   corner, x axis is oriented to the right, and y axis is oriented downwards.
%   default false
%
% The function cv.convexHull finds the convex hull of a 2D point set using the
% Sklansky's algorithm [Sklansky82] that has `O(N logN)` complexity in the
% current implementation. See the sample `convexhull_demo.m` that demonstrates
% the usage of the function.
%
% ## References
% [Sklansky82]:
% > Jack Sklansky. "Finding the convex hull of a simple polygon".
% > Pattern Recognition Letters, 1(2):79-83, 1982.
%
% See also: convhull, convhulln, bwconvhull, boundary
%


classdef CF7 < PROBLEM
% <multi> <real> <large/none> <constrained>
% Constrained benchmark MOP

%------------------------------- Reference --------------------------------
% Q. Zhang, A. Zhou, S. Zhao, P. N. Suganthan, W. Liu, and S. Tiwari,
% Multiobjective optimization test instances for the CEC 2009 special
% session and competition, School of CS & EE, University of Essex, Working
% Report CES-487, 2009.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = [0,zeros(1,obj.D-1)-2];
            obj.upper    = [1,zeros(1,obj.D-1)+2];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            D  = size(X,2);
            J1 = 3 : 2 : D;
            J2 = 2 : 2 : D;
            Y           = zeros(size(X));
            Y(:,J1)     = X(:,J1) - cos(6*pi*repmat(X(:,1),1,length(J1))+repmat(J1,size(X,1),1)*pi/D);
            Y(:,J2)     = X(:,J2) - sin(6*pi*repmat(X(:,1),1,length(J2))+repmat(J2,size(X,1),1)*pi/D);
            h           = 2*Y.^2 - cos(4*pi*Y) + 1;
            h(:,[2,4])  = Y(:,[2,4]).^2;
            PopObj(:,1) = X(:,1)        + sum(h(:,J1),2);
            PopObj(:,2) = (1-X(:,1)).^2 + sum(h(:,J2),2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,X)
            PopCon(:,1) = -X(:,2) + sin(6*pi*X(:,1)+2*pi/size(X,2)) + sign(0.5*(1-X(:,1))-(1-X(:,1)).^2).*sqrt(abs(0.5*(1-X(:,1))-(1-X(:,1)).^2));
            PopCon(:,2) = -X(:,4) + sin(6*pi*X(:,1)+4*pi/size(X,2)) + sign(0.25*sqrt(1-X(:,1))-0.5*(1-X(:,1))).*sqrt(abs(0.25*sqrt(1-X(:,1))-0.5*(1-X(:,1))));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = (1-R(:,1)).^2;
            temp1  = 0.5<R(:,1) & R(:,1)<=0.75;
            temp2  = 0.75<R(:,1);
            R(temp1,2) = 0.5*(1-R(temp1,1));
            R(temp2,2) = 0.25*sqrt(1-R(temp2,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


function result = sphere_05_nd ( func, n, xc, r )

%*****************************************************************************80
%
%% SPHERE_05_ND approximates surface integrals on a sphere in ND.
%
%  Integration region:
%
%    R1**2 <= sum ( X(1:N) - XC(1:N) )**2 <= R2**2.
%
%  Discussion:
%
%    A 2*N+2**N points 5-th degree formula is used, Stroud number UN:5-2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X), at the N dimensional point
%    X, of the form
%      function value = func ( n, x )
%
%    Input, integer N, the dimension of the space.
%
%    Input, real XC(N), the center of the sphere.
%
%    Input, real R, the radius of the sphere.
%
%    Output, real RESULT, the approximate integral of the function.
%
  x1 = 1.0;
  x2 = 1.0 / sqrt ( n );

  w1 = 1.0 / ( n * ( n + 2 ) );
  w2 = n / ( ( n + 2 ) * 2^n );

  x(1:n) = xc(1:n);

  quad = 0.0;

  for i = 1 : n
    x(i) = xc(i) + r * x1;
    quad = quad + w1 * feval ( func, n, x );
    x(i) = xc(i) - r * x1;
    quad = quad + w1 * feval ( func, n, x );
    x(i) = xc(i);
  end
%
%  For this computation, we need to keep R8VEC_MIRROR_NEXT happy
%  by storing in X the "perturbation" from XC, for which we
%  interested in computing all possible sign variations.
%
  x(1:n) = abs ( r ) * x2;

  while ( 1 )

    quad = quad + w2 * feval ( func, n, xc(1:n) + x(1:n) );

    [ x, done ] = r8vec_mirror_next ( n, x );

    if ( done )
      break
    end

  end

  volume = sphere_area_nd ( n, r );
  result = quad * volume;

  return
end


function [xf,yf,modebmp] = imagemode(x,y,mode,dx,dy);

% Produces a properly scaled color plot of a two-dimensional
% mode.  This routine is especially useful when x and y are
% non-uniformly spaced vectors.  In this case, the mode is
% interpolated over a uniformly-spaced grid before producing
% an image plot.  The output can be directly saved to a file
% using the imwrite() function.
%
% USAGE:
%
% [xf,yf,modebmp] = imagemode(x,y,mode);
% [xf,yf,modebmp] = imagemode(x,y,mode,dx,dy);
%
% INPUT:
%
% x,y - vectors describing horizontal and vertical grid points
% mode - the mode or field component to be plotted
% dx, dy (optional) - fine grid spacing at which to oversample
%   (interpolate) the mode.  If left unspecified, this routine
%   will use the smallest value of diff(x) and diff(y).
%
% OUTPUT:
%
% xf,yf - points at which the mode was interpolated
% modebmp - 8-bit unsigned integer array representing the mode
%    image

x = real(x);
y = real(y);

if (size(mode) == [length(x)-1,length(y)-1])
    x = (x(1:end-1) + x(2:end))/2;
    y = (y(1:end-1) + y(2:end))/2;
end

if (nargin == 3)
  [dx,ix] = min(diff(x));
  [dy,iy] = min(diff(y));
  xf = (min(x):dx:max(x))';
  yf = (min(y):dy:max(y));
  % line up with finest portion of grid
  delta = dx*(interp1(xf,(1:length(xf)),x(ix+1)) - ...
              round(interp1(xf,(1:length(xf)),x(ix+1))));
  xf = xf + delta;
  delta = dy*(interp1(yf,(1:length(yf)),y(iy+1)) - ...
              round(interp1(yf,(1:length(yf)),y(iy+1))));
  yf = yf + delta;
  % eliminate points outside of range
  kv = find((min(x) < xf) & (xf < max(x)));
  xf = xf(kv);
  kv = find((min(y) < yf) & (yf < max(y)));
  yf = yf(kv);
else
  xf = (min(x):dx:max(x))';
  yf = (min(y):dy:max(y));
end

cmax = size(colormap,1)-1;

modebmp = uint8(transpose(interp2(y,x, ...
                abs(cmax*mode),yf,xf)));
image(xf,yf,modebmp);
set(gca,'YDir','normal');
v = [min(xf),max(xf),min(yf),max(yf)];
axis(v);
set(gca,'PlotBoxAspectRatio',[v(2)-v(1) v(4)-v(3) 1]);


%% Quasi symmetry
%
%%
% By quasi crystals we mean materials with a non crystallographic symmetry
% group, e.g. including a pentagonal or decagonal symmetry axis. Such
% symmetry groups do not lead to perfect translational invariant lattices
% and hence are not crystals.
%
% Setting up a non standard symmetry group can be done by specifying all
% its symmetry elements. To illustrate this definition lets consider the
% symmetry group corresponding to the dodecaeder which is generated by a 5
% fold and a 3 fold symmetry axis.

rot2 = rotation.byAxisAngle(vector3d.Z,180*degree);
rot5 = rotation.byAxisAngle(vector3d.byPolar(31.7171*degree,0*degree),72*degree);
%rot3 = rotation.byAxisAngle(vector3d.byPolar(20.9054*degree,90*degree),120*degree);

cs = crystalSymmetry.byElements([rot5,rot2])

plot(cs,'symbolSize',0.5,'projection','eangle','grid','on')

%%
% Many of the MTEX methods do work also with quasi symmetries. E.g. we can
% identify the fundamental sector by

hold on
plot(cs.fundamentalSector,'color','red')
hold off

%%
% the fundamental region is orientation space, which is exactly a
% dodecaeder,

oR = cs.fundamentalRegion;
plot(oR)
axis off

%%
% or define an approbiate color key

ipfKey = ipfHSVKey(cs);

plot(ipfKey,'complete','upper','resolution',0.5*degree)
hold on
plot(cs,'SymbolSize',0.6,'linewidth',1)
hold off

%%
% We may also choose a different setup where the 5 fold axis is aligned
% parallel to z

rot5 = rotation.byAxisAngle(zvector,72*degree);
rot3 = rotation.byAxisAngle(vector3d('polar',37.377*degree,0) ,120*degree);

cs = crystalSymmetry.byElements([rot5,rot3])

plot(cs,'symbolSize',0.5,'projection','eangle','grid','on')

%%
% or include the inversion

cs = crystalSymmetry.byElements([rot5,rot3,rotation.inversion])

ipfKey = ipfHSVKey(cs);

plot(ipfKey,'complete','upper','resolution',0.5*degree)

hold on
plot(cs,'symbolSize',0.6,'linewidth',2)
hold off


function tetrahedron_keast_rule_test07 ( )

%*****************************************************************************80
%
%% TETRAHEDRON_KEAST_RULE_TEST07 tests KEAST_RULE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TETRAHEDRON_KEAST_RULE_TEST07\n' );
  fprintf ( 1, '  KEAST_RULE returns the points and weights\n' );
  fprintf ( 1, '  of a Keast rule for the tetrahedron.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  In this test, we simply print a rule.\n' );

  rule = 10;
  degree = keast_degree ( rule );
  order_num = keast_order_num ( rule );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rule index  = %d\n', rule );
  fprintf ( 1, '  Rule degree = %d\n', degree );
  fprintf ( 1, '  Rule order  = %d\n', order_num );

  [ xyz, w ] = keast_rule ( rule, order_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '         I      W           X           Y           Z\n' );
  fprintf ( 1, '\n' );
  for order = 1 : order_num
    fprintf ( 1, '  %8d  %10f  %10f  %10f  %10f\n', ...
      order, w(order), xyz(1:3,order) );
  end

  return
end


function a = c8mat_identity ( n )

%*****************************************************************************80
%
%% C8MAT_IDENTITY sets the square matrix A to the identity.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, complex A(N,N), the N by N identity matrix.
%
  a(1:n,1:n) = 0.0;

  for i = 1 : n
    a(i,i) = 1.0;
  end

  return
end


% Data File MAH3
% Triple Pendulum
  s = 3; % degree of freedom
%  L a g r a n g i a n   of the system
  L = [' l1^2/6*(m1 + 3*m2 + 3*m3)*qt1^2 + '        , ...
       ' l2^2/6*(m2 + 3*m3)*qt2^2 + '               , ...
       ' l3^2/6*m3*qt3^2 + '                        , ...
       ' 1/2*(m2+2*m3)*l1*l2*cos(q1-q2)*qt1*qt2 + ' , ...
       ' 1/2*m3*l1*l3*cos(q1-q3)*qt1*qt3 + '        , ...
       ' 1/2*m3*l2*l3*cos(q2-q3)*qt2*qt3 + '        , ...
       ' 9.81*l1*(m1/2 + m2 + m3)*cos(q1) + '          , ...
       ' 9.81*l2*(m2/2 + m3)*cos(q2) + '               , ...
       ' 9.81*l3*m3/2*cos(q3) '              ];
  QN{1} = '-k1*qt1'; % Generalized
  QN{2} = '-k2*qt2'; % non potential
  QN{3} = '-k3*qt3'; % forces
  qj0   = [0, 0, 0]; % Initial coordinates
  qtj0  = [5, 0, 0]; % Initial velocities
  Tend  = 20;        % Upper bound of integration
  eps   = 1e-10;     % Desirable accuracy
  np    = 9;         % Number of parameters
  P{1}  = 'm1';      % Masses of
  P{2}  = 'm2';      % the rods 1, 2, 3
  P{3}  = 'm3';
  P{4}  = 'l1';      % Lenghts of
  P{5}  = 'l2';      % the rods 1, 2, 3
  P{6}  = 'l3';
  P{7}  = 'k1';      % Coefficients of
  P{8}  = 'k2';      % damping
  P{9}  = 'k3';


function l=lowerbound(dvec, w, alpha, beta, a, b)
% l=lowerbound(dvec, w, alpha, beta, a, b) (1 x I)
% Computes lower bound for variational approximation (Buntine & Jakulin
% DCA 2006)
% dvec: data - each column is one image (JxI)
% w: basis - each column is one basis vector (component, psf); (JxK)
% alpha, beta: parameters of the Gamma distribution for latent h (1xK each)
% a, b: parameters of the approximative Gamma distribution (KxI each)
% J=#pixels
% K=#components
% I=#images.

% this is the same as in update_n ...
% expectation of <log(l_k)>:
e_l = psi(a) - log(b); % (K x I)

% n-update:
[ntmp, z]=update_ntmp(w, a, b); % z (Jx1xI) normalization constant from n update

t1=sum(e_l.*bsxfun(@minus, alpha', a),1);   %(1xI)
t2=sum(dvec.*log(squeeze(z)),1);            %(1xI)

% approximation of the log-gamma function
t31=gammalogapprox(a)-a.*log(b);            % (KxI)
t32=gammalogapprox(alpha)-alpha.*log(beta); % (1xK)
t3=sum(bsxfun(@minus, t31, t32'),1);        % (1xI)
% This t4 is not really necessary as it is only data dependent:
t4=sum(factorialapprox(dvec),1);            % (1xI)
l=t1+t2+t3-t4; %(1xI)


function bf10 = corrbf(r,n)
%
% bf10 = corrbf(r,n)
%
% Calculates JZS Bayes factor for correlation r and sample size n.
% This quantifies the evidence in favour of the alternative hypothesis.
% See Wetzels & Wagemakers, 2012, Psychon Bull Rev for details.
%

% Function to be integrated - updated after personal communication from EJW (old one didn't work with large N)
F = @(g,r,n) exp(((n-2)./2).*log(1+g)+(-(n-1)./2).*log(1+(1-r.^2).*g)+(-3./2).*log(g)+-n./(2.*g));

% Bayes factor calculation
bf10 = sqrt((n/2)) / gamma(1/2) * integral(@(g) F(g,r,n),0,Inf);


function [K] = kv1v1(x, xp, hyp, i)

logsigmau = hyp(1);
logthetau = hyp(2);
logsigmav = hyp(3);
logthetav = hyp(4);

n_x = size(x,1);
n_xp = size(xp,1);

x = repmat(x,1,n_xp);
xp = repmat(xp',n_x,1);

switch i

case 0

K=exp(1).^(logsigmav+(-1/2).*exp(1).^((-1).*logthetav).*(x+(-1).*xp).^2); ...

case 1 % logsigmau

K=0;

case 2 % logthetau

K=0;

case 3 % logsigmav

K=exp(1).^(logsigmav+(-1/2).*exp(1).^((-1).*logthetav).*(x+(-1).*xp).^2); ...

case 4 % logthetav

K=(1/2).*exp(1).^(logsigmav+(-1).*logthetav+(-1/2).*exp(1).^((-1).* ...
  logthetav).*(x+(-1).*xp).^2).*(x+(-1).*xp).^2;

otherwise

        K = zeros(n_x, n_xp);
end

if K == 0

    K = zeros(n_x, n_xp);

end

end


function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha)

%project_points2.m
%
%[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)
%
%Projects a 3D structure onto the image plane.
%
%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
%       f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)
%       c: principal point location in pixel units (2x1 vector)
%       k: Distortion coefficients (radial and tangential) (4x1 vector)
%       alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)
%
%OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)
%        dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)
%        dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)
%        dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)
%        dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)
%        dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)
%
%Definitions:
%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
%The coordinate vector of P in the camera reference frame is: Xc = R*X + T
%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
%call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);
%The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.
%call r^2 = a^2 + b^2.
%The distorted point coordinates are: xd = [xx;yy] where:
%
%xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);
%yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6)      +      kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;
%
%The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion
%
%Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:
%
%xxp = f(1)*(xx + alpha*yy) + c(1)
%yyp = f(2)*yy + c(2)
%
%
%NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices
%
%
%Important function called within that program:
%
%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
%
%rigid_motion.m: Computes the rigid motion transformation of a given structure

if nargin < 7,
   alpha = 0;
   if nargin < 6,
      k = zeros(5,1);
      if nargin < 5,
         c = zeros(2,1);
         if nargin < 4,
            f = ones(2,1);
            if nargin < 3,
               T = zeros(3,1);
               if nargin < 2,
                  om = zeros(3,1);
                  if nargin < 1,
                     error('Need at least a 3D structure to project (in project_points.m)');
                     return;
                  end;
               end;
            end;
         end;
      end;
   end;
end;

[m,n] = size(X);

[Y,dYdom,dYdT] = rigid_motion(X,om,T);

inv_Z = 1./Y(3,:);

x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;

bb = (-x(1,:) .* inv_Z)'*ones(1,3);
cc = (-x(2,:) .* inv_Z)'*ones(1,3);

dxdom = zeros(2*n,3);
dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);
dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);

dxdT = zeros(2*n,3);
dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);
dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);

% Add distortion:

r2 = x(1,:).^2 + x(2,:).^2;

dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);
dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);

r4 = r2.^2;

dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;
dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;

r6 = r2.^3;

dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom;
dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT;

% Radial distortion:

cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;

dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom;
dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT;
dcdistdk = [ r2' r4' zeros(n,2) r6'];

xd1 = x .* (ones(2,1)*cdist);

dxd1dom = zeros(2*n,3);
dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;
dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;
coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));
dxd1dom = dxd1dom + coeff.* dxdom;

dxd1dT = zeros(2*n,3);
dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;
dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;
dxd1dT = dxd1dT + coeff.* dxdT;

dxd1dk = zeros(2*n,5);
dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;
dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;

% tangential distortion:

a1 = 2.*x(1,:).*x(2,:);
a2 = r2 + 2*x(1,:).^2;
a3 = r2 + 2*x(2,:).^2;

delta_x = [k(3)*a1 + k(4)*a2 ;
   k(3) * a3 + k(4)*a1];

%ddelta_xdx = zeros(2*n,2*n);
aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);
bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);
cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);

ddelta_xdom = zeros(2*n,3);
ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);
ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);

ddelta_xdT = zeros(2*n,3);
ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);
ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);

ddelta_xdk = zeros(2*n,5);
ddelta_xdk(1:2:end,3) = a1';
ddelta_xdk(1:2:end,4) = a2';
ddelta_xdk(2:2:end,3) = a3';
ddelta_xdk(2:2:end,4) = a1';

xd2 = xd1 + delta_x;

dxd2dom = dxd1dom + ddelta_xdom ;
dxd2dT = dxd1dT + ddelta_xdT;
dxd2dk = dxd1dk + ddelta_xdk ;

% Add Skew:

xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];

% Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha

dxd3dom = zeros(2*n,3);
dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);
dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);
dxd3dT = zeros(2*n,3);
dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);
dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);
dxd3dk = zeros(2*n,5);
dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);
dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);
dxd3dalpha = zeros(2*n,1);
dxd3dalpha(1:2:2*n,:) = xd2(2,:)';

% Pixel coordinates:
if length(f)>1,
    xp = xd3 .* (f * ones(1,n))  +  c*ones(1,n);
    coeff = reshape(f*ones(1,n),2*n,1);
    dxpdom = (coeff*ones(1,3)) .* dxd3dom;
    dxpdT = (coeff*ones(1,3)) .* dxd3dT;
    dxpdk = (coeff*ones(1,5)) .* dxd3dk;
    dxpdalpha = (coeff) .* dxd3dalpha;
    dxpdf = zeros(2*n,2);
    dxpdf(1:2:end,1) = xd3(1,:)';
    dxpdf(2:2:end,2) = xd3(2,:)';
else
    xp = f * xd3 + c*ones(1,n);
    dxpdom = f  * dxd3dom;
    dxpdT = f * dxd3dT;
    dxpdk = f  * dxd3dk;
    dxpdalpha = f .* dxd3dalpha;
    dxpdf = xd3(:);
end;

dxpdc = zeros(2*n,2);
dxpdc(1:2:end,1) = ones(n,1);
dxpdc(2:2:end,2) = ones(n,1);

return;

% Test of the Jacobians:

n = 10;

X = 10*randn(3,n);
om = randn(3,1);
T = [10*randn(2,1);40];
f = 1000*rand(2,1);
c = 1000*randn(2,1);
k = 0.5*randn(5,1);
alpha = 0.01*randn(1,1);

[x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);

% Test on om: OK

dom = 0.000000001 * norm(om)*randn(3,1);
om2 = om + dom;

[x2] = project_points2(X,om2,T,f,c,k,alpha);

x_pred = x + reshape(dxdom * dom,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on T: OK!!

dT = 0.0001 * norm(T)*randn(3,1);
T2 = T + dT;

[x2] = project_points2(X,om,T2,f,c,k,alpha);

x_pred = x + reshape(dxdT * dT,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on f: OK!!

df = 0.001 * norm(f)*randn(2,1);
f2 = f + df;

[x2] = project_points2(X,om,T,f2,c,k,alpha);

x_pred = x + reshape(dxdf * df,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on c: OK!!

dc = 0.01 * norm(c)*randn(2,1);
c2 = c + dc;

[x2] = project_points2(X,om,T,f,c2,k,alpha);

x_pred = x + reshape(dxdc * dc,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on k: OK!!

dk = 0.001 * norm(k)*randn(5,1);
k2 = k + dk;

[x2] = project_points2(X,om,T,f,c,k2,alpha);

x_pred = x + reshape(dxdk * dk,2,n);

norm(x2-x)/norm(x2 - x_pred)

% Test on alpha: OK!!

dalpha = 0.001 * norm(k)*randn(1,1);
alpha2 = alpha + dalpha;

[x2] = project_points2(X,om,T,f,c,k,alpha2);

x_pred = x + reshape(dxdalpha * dalpha,2,n);

norm(x2-x)/norm(x2 - x_pred)


function [fx] = f_FitzHughNagumo_calcium(Xt,Theta,ut,inF)

% FitzHugh-Nagumo membrane potential evolution function (+convolution)
% function [fx] = FitzHughNagumo(Xt,Theta,ut,inF)
% IN:
%   - Xt: system's states, ie:
%       Xt(1): calcium imaging prediction
%       Xt(2): membrane depolarization (mV)
%       Xt(3): proxy for ion channel opening probabilities
%   - Theta: evolution parameters (see bellow)
%   - ut: input current
%   - inF: [optional]
% OUT:
%   - fx: the evolution function evaluated at Xt

deltat = inF.delta_t;

try
    a = inF.a.^-1;
catch
    a = 1;
end
a = a.*exp(Theta(1));
K1 = (1/3)*exp(Theta(2));
K2 = 0.08*exp(Theta(3));
K3 = 0.7*exp(Theta(4));
K4 = 0.8*exp(Theta(5));
tmp = [ -a*Xt(1)+ exp(Xt(2))
        Xt(2) - K1*Xt(2).^3 - Xt(3) + ut
        K2*(Xt(2) - K3 - K4*Xt(3))];

fx = Xt + deltat.*tmp;

% A = [ -a  exp(Xt(2))
%        0  -b          ];
% dF_dX = eye(2) + deltat.*A';
%
% dF_dTheta = [ -deltat.*Xt(1)*a  0
%               0                 -deltat.*Xt(2)*b];


function [soln,eqn,info] = Poisson3WG(node,elem,bdFlag,pde,option,varargin)
%% POISSON3WG Poisson equation: P1 linear element in 3-D.
%
%   u = POISSON3WG(node,elem,bdFlag,pde) produces the P0-P0 Weak Galerkin
%   finite element approximation of the Poisson equation
%
%       -div(d*grad(u))=f  in \Omega, with
%       Dirichlet boundary condition u=g_D on \Gamma_D,
%       Neumann boundary condition   d*grad(u)*n=g_N on \Gamma_N,
%       Robin boundary condition     g_R*u + d*grad(u)*n=g_N on \Gamma _R
%
%   The mesh is given by node and elem and the boundary face is given by
%   bdFlag. See meshdoc, bddoc for details. The data is given by the
%   structure pde which contains function handles f, g_D, g_N, g_R, or d.
%   For general elliptic equations with convection and reaction
%   coefficients, see ellipticpde.
%
%   The function Poisson assembes the matrix equation AD*u = b and solves
%   it by the direct solver (small size <= 2e3) or the multigrid solver
%   (large size > 2e3). The Dirichlet boundary condition is built into the
%   matrix AD and the Neumann boundary condition is build into b.
%
%   The diffusion coefficient d is a scalar function or a column array with
%   the same length as the elem array.
%
%   u = Poisson3WG(node,elem,bdFlag,pde,option) specifies the options.
%     - option.solver == 'direct': the built in direct solver \ (mldivide)
%     - option.solver == 'mg':     multigrid-type solvers mg is used.
%     - option.solver == 'notsolve': the solution u = u_D.
%   The default setting is to use the direct solver for small size problems
%   and multigrid solvers for large size problems. For more options on the
%   multigrid solver mg, type help mg.
%
%   When only one type of boundary condition is imposed, the input argument
%   bdFlag can be skipped. The boundary condition is implicitly given in
%   the pde structure by specifying g_D or g_N only. See examples below.
%
%   [u,A] = Poisson3WG(node,elem,bdFlag,pde,HB) returns also the
%   non-modified stiffness matrix A, which is semi-definite. The kernel of
%   A consists of constant vectors. The matrix A can be used to evulate the
%   bilinear form A(u,v) = u'*A*v, especially the enery norm of a finite
%   element function u by sqrt(u'*A*u).
%
%   [u,A,eqn] = Poisson3WG(node,elem,bdFlag,pde,HB) returns also the equation
%   structure eqn, which includes:
%     - eqn.AD:  modified stiffness matrix AD;
%     - eqn.b:   the right hand side.
%   The solution u = AD\b. The output eqn can be used to test other solvers.
%
%   [u,A,eqn,info] = Poisson3WG(node,elem,bdFlag,pde) returns also the
%   information on the assembeling and solver, which includes:
%     - info.assembleTime: time to assemble the matrix equation
%     - info.solverTime:   time to solve the matrix equation
%     - info.itStep:       number of iteration steps for the mg solver
%     - info.error:        l2 norm of the residual b - A*u
%     - info.flag:         flag for the mg solver.
%       flag = 0: converge within max iteration
%       flag = 1: iterated maxIt times but did not converge
%       flag = 2: direct solver
%       flag = 3: no solve
%
%   Example
%     clear all
%     node = [0,0; 1,0; 1,1; 0,1];
%     elem = [2,3,1; 4,1,3];
%     for k = 1:4
%       [node,elem] = uniformbisect(node,elem);
%     end
%     % Homogenous Dirichlet boundary condition
%     pde.f = inline('ones(size(p,1),1)','p');
%     pde.g_D = 0;
%     u = Poisson3WG(node,elem,pde);
%     figure(1);
%     showresult(node,elem,u);
%     % Non-homogenous Dirichlet boundary condition
%     pde.f = inline('-4*ones(size(p,1),1)','p');
%     pde.g_D = inline('p(:,1).^2 + p(:,2).^2','p');
%     u = Poisson3WG(node,elem,pde);
%     figure(2);
%     showresult(node,elem,u);
%     % Homogenous Neumann boundary condition
%     clear pde
%     pde.f = inline('pi^2*cos(pi*p(:,1)).*cos(pi*p(:,2))','p');
%     u = Poisson3WG(node,elem,pde);
%     figure(3);
%     showresult(node,elem,u);
%
%   Example
%     cubePoisson;

%   Reference: Programming of Weak Galerkin Method. Long Chen
%   https://www.math.uci.edu/~chenlong/ifemdoc/fem/WGprogramming.pdf
%
%   See also Poisson3, Poisson3WGfemrate, Lshape, crack, mg
%
%   Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('bdFlag','var'), bdFlag = []; end
if ~exist('option','var'), option = []; end
NT = size(elem,1);

%% Diffusion coefficient
t = cputime;  % record assembling time
if ~isfield(pde,'d'), pde.d = []; end
if ~isfield(option,'dquadorder'), option.dquadorder = 1; end
if ~isempty(pde.d) && isnumeric(pde.d)
   K = pde.d;                   % d is an array
end
if ~isempty(pde.d) && ~isnumeric(pde.d)
    [lambda,weight] = quadpts3(option.dquadorder);
    nQuad = size(lambda,1);
    K = zeros(NT,1);
    for p = 1:nQuad
		pxy = lambda(p,1)*node(elem(:,1),:) ...
			+ lambda(p,2)*node(elem(:,2),:) ...
			+ lambda(p,3)*node(elem(:,3),:) ...
            + lambda(p,4)*node(elem(:,4),:);
        K = K + weight(p)*pde.d(pxy);           % d is a function
   end
end

%% Compute geometric quantities and gradient of local basis
[elem2face,face] = dof3face(elem);
NF = size(face,1);
Ndof = NT + NF;
elem2dof = NT + elem2face;

%% Assemble stiffness matrix
A = sparse(Ndof,Ndof);
% compute ct2 = 1/mean(||x-xc||^2)
center = (node(elem(:,1),:) + node(elem(:,2),:) + ...
          node(elem(:,3),:) + node(elem(:,4),:))/4;
[lambda,weight] = quadpts3(2);
nQuad = size(lambda,1);
ct2 = zeros(NT,1);
for p = 1:nQuad
    pxyz = lambda(p,1)*node(elem(:,1),:) ...
         + lambda(p,2)*node(elem(:,2),:) ...
         + lambda(p,3)*node(elem(:,3),:) ...
         + lambda(p,4)*node(elem(:,4),:);
    ct2 = ct2 + weight(p)*sum((pxyz-center).^2,2);
end
ct2 = 1./ct2;
[Dphi,volume] = gradbasis3(node,elem);
clear center pxyz

% Mbb: face - face
for i = 1:4
    for j = i:4
        % local to global index map
		ii = double(elem2dof(:,i));
		jj = double(elem2dof(:,j));
        % compuation of local stiffness matrix.
        Aij = 9*dot(Dphi(:,:,i),Dphi(:,:,j),2).*volume + 9/16*ct2.*volume;
        if ~isempty(pde.d)
            Aij = K.*Aij;
        end
        if (j==i)
            A = A + sparse(ii,jj,Aij,Ndof,Ndof);
        else
            A = A + sparse([ii,jj],[jj,ii],[Aij; Aij],Ndof,Ndof);
        end
    end
end

% Mob: interior - face
Aij = -9/4*ct2.*volume;
if ~isempty(pde.d)
    Aij = K.*Aij;
end
Mob = sparse([(1:NT)', (1:NT)', (1:NT)', (1:NT)'], ...
             double(elem2dof(:)), [Aij, Aij, Aij, Aij], Ndof, Ndof);
A = A + Mob + Mob';

% Moo: diagonal of interor
Aij = 9*ct2.*volume;
if ~isempty(pde.d)
    Aij = K.*Aij;
end
A =  A + sparse(1:NT, 1:NT, Aij, Ndof, Ndof);

clear K Aij

%% Assemble right hand side
b = zeros(Ndof,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 2;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end
if ~isempty(pde.f)
    [lambda,weight] = quadpts3(option.fquadorder);
	nQuad = size(lambda,1);
    bt = zeros(NT,1);
    for p = 1:nQuad
		% quadrature points in the x-y-z coordinate
		pxyz = lambda(p,1)*node(elem(:,1),:) ...
			 + lambda(p,2)*node(elem(:,2),:) ...
			 + lambda(p,3)*node(elem(:,3),:) ...
             + lambda(p,4)*node(elem(:,4),:);
		fp = pde.f(pxyz);
        bt = bt + weight(p)*fp;
    end
    bt = bt.*volume;
    b(1:NT) = bt;
end
clear pxyz bt

%% Set up boundary conditions
[AD,b,u,freeDof,isPureNeumann] = getbdWG3(A,b);

%% Record assembling time
assembleTime = cputime - t;
if ~isfield(option,'printlevel'), option.printlevel = 1; end
if option.printlevel >= 2
    fprintf('Time to assemble matrix equation %4.2g s\n',assembleTime);
end

%% Solve the system of linear equations
if isempty(freeDof), return; end
% Set up solver
if isempty(option) || ~isfield(option,'solver')    % no option.solver
    if Ndof <= 2e3  % Direct solver for small size systems
        option.solver = 'direct';
    else            % MGCG  solver for large size systems
        option.solver = 'mg';
    end
end
solver = option.solver;
% solve
switch solver
    case 'direct'
        tic;
        u(freeDof) = AD(freeDof,freeDof)\b(freeDof);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'error',[],'flag',3);
    case 'mg'
        option.solver = 'CG';
        if nargin>=6
            HB = varargin{1};
        else
            HB = [];
        end
        if isfield(option,'reducesystem') && (option.reducesystem == 0)
            % original system
            option.x0 = u;
            option.freeDof = freeDof;
            [u,info] = mg(AD,b,elem,option,face,HB);
        else  % reduced system
            option.x0 = u(NT+1:end);
            option.freeDof = freeDof(NT+1:end)-NT;
            % eleminate elementwise dof
            Aoinv = spdiags(1./diag(AD(1:NT,1:NT)),0,NT,NT);
            Aob = AD(1:NT,NT+1:end);
            Abo = Aob';
            Abb = AD(NT+1:end,NT+1:end);
            Abbm = Abb - Abo*Aoinv*Aob;
            bm = -Abo*Aoinv*b(1:NT) + b(NT+1:end);
            [ub,info] = mg(Abbm,bm,elem,option,face,HB);
            u(1:NT) = Aoinv*(b(1:NT) - Aob*ub);
            u(NT+1:end) = ub;
        end
    case 'amg'
        option.solver = 'CG';
        [u(freeDof),info] = amg(AD(freeDof,freeDof),b(freeDof),option);
end
% post-process for pure Neumann problem
if isPureNeumann
    uc = sum(u(1:NT).*volume);
    u = u - uc;   % normalization for pure Neumann problem
end

%% Compute Du
dudx = u(elem2dof(:,1)).*Dphi(:,1,1)+u(elem2dof(:,2)).*Dphi(:,1,2) ...
     + u(elem2dof(:,3)).*Dphi(:,1,3)+u(elem2dof(:,4)).*Dphi(:,1,4);
dudy = u(elem2dof(:,1)).*Dphi(:,2,1)+u(elem2dof(:,2)).*Dphi(:,2,2) ...
     + u(elem2dof(:,3)).*Dphi(:,2,3)+u(elem2dof(:,4)).*Dphi(:,2,4);
dudz = u(elem2dof(:,1)).*Dphi(:,3,1)+u(elem2dof(:,2)).*Dphi(:,3,2) ...
     + u(elem2dof(:,3)).*Dphi(:,3,3)+u(elem2dof(:,4)).*Dphi(:,3,4);
Du = -3*[dudx, dudy, dudz];

%% Output information
if nargout == 1
    soln = u;
else
    soln = struct('u',u,'Du',Du);
    eqn = struct('A',AD,'b',b,'face',face,'freeDof',freeDof,'Lap',A);
    info.assembleTime = assembleTime;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [AD,b,u,freeDof,isPureNeumann]= getbdWG3(A,b)
    %% GETBDCR Boundary conditions for Poisson equation: Crouzeix-Raviart element.

    u = zeros(Ndof,1);
    %% Initial check
    if ~isfield(pde,'g_D'), pde.g_D = []; end
    if ~isfield(pde,'g_N'), pde.g_N = []; end
    if ~isfield(pde,'g_R'), pde.g_R = []; end

    %% Part 1: Modify the matrix for Dirichlet and Robin condition
    % Robin boundary condition
    Robin = [];
    idxR = (bdFlag(:) == 3);      % index of Robin faces in bdFlag
    if any(idxR)
        isRobin = false(Ndof,1);
        isRobin(elem2face(idxR)) = true;
        Robin = face(isRobin,:);  % Robin faces
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        v12 = node(Robin(:,2),:)-node(Robin(:,1),:);
        v13 = node(Robin(:,3),:)-node(Robin(:,1),:);
        area = 0.5*sqrt(abs(sum(mycross(v12,v13,2).^2,2)));
        center = (node(Robin(:,1),:) + node(Robin(:,2),:)+node(Robin(:,3),:))/3;
        ii = NT + find(isRobin);
        ss = pde.g_R(center).*area; % exact for linear g_R
        A = A + sparse(ii,ii,ss,Ndof,Ndof);
    end

    % Dirichlet boundary nodes: fixedFace
    fixedFace = []; freeFace= [];
    if ~isempty(bdFlag) % find boundary faces
        idxD = (bdFlag(:)==1); % all Dirichlet faces in bdFlag
        isFixedFace = false(NF,1);
        isFixedFace(elem2face(idxD)) = true;
        fixedFace = find(isFixedFace);
        freeFace = find(~isFixedFace);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given in the input
        s = accumarray(elem2face(:), 1, [NF 1]);
        fixedFace = find(s == 1);
        freeFace = find(s == 2);
    end
    isPureNeumann = false;
    if isempty(fixedFace) && isempty(Robin)  % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedFace = 1;
        freeFace = (2:NF)';    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedFace,fixedFace)=I, AD(fixedFace,freeFace)=0, AD(freeFace,fixedFace)=0.
    if ~isempty(fixedFace)
        bdidx = zeros(Ndof,1);
        bdidx(NT + fixedFace) = 1;
        Tbd = spdiags(bdidx,0,Ndof,Ndof);
        T = spdiags(1-bdidx,0,Ndof,Ndof);
        AD = T*A*T + Tbd;
    else
        AD = A;
    end

    %% Part 2: Find boundary faces and modify the right hand side b
    % Find boundary faces: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        idxN = (bdFlag(:) == 2);      % all Neumann faces in bdFlag
        isNeumann = elem2face(idxN | idxR); % index of Neumann faces
        % since boundary integral is also needed for Robin edges
        Neumann = face(isNeumann,:);      % Neumann faces
    end
    if isempty(bdFlag) && (~isempty(pde.g_N) || ~isempty(pde.g_R))
        % no bdFlag, only pde.g_N or pde.g_R is given in the input
        s = accumarray(elem2face(:), 1, [NF 1]);
        Neumann = face(s == 1,:);
    end

    % Neumann boundary condition
    if ~isempty(Neumann) && ~isempty(pde.g_N) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 3;   % default order exact for linear gN
        end
        [lambdagN,weightgN] = quadpts(option.gNquadorder);
        nQuadgN = size(lambdagN,1);
        gf = zeros(size(Neumann,1),1);
        v12 = node(Neumann(:,2),:)-node(Neumann(:,1),:);
        v13 = node(Neumann(:,3),:)-node(Neumann(:,1),:);
        area = 0.5*sqrt(abs(sum(mycross(v12,v13,2).^2,2)));
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxy = lambdagN(pp,1)*node(Neumann(:,1),:) ...
                 + lambdagN(pp,2)*node(Neumann(:,2),:)...
                 + lambdagN(pp,3)*node(Neumann(:,3),:);
            gNp = pde.g_N(ppxy);
            gf = gf+ weightgN(pp)*gNp;
        end
        gf = gf.*area;
        b(NT+isNeumann) = b(NT+isNeumann) + gf;
    end
    % The case with non-empty Neumann faces but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann faces and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if ~isPureNeumann && ~isempty(fixedFace) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && all(pde.g_D == 0))    % nonzero g_D
        if isnumeric(pde.g_D)
            u(fixedFace) = pde.g_D(fixedFace);
        else % pde.g_D is a function handle
            center = (node(face(fixedFace,1),:)+node(face(fixedFace,2),:)+node(face(fixedFace,3),:))/3;
            u(NT+fixedFace) = pde.g_D(center);
        end
        b = b - A*u;
        b(NT+fixedFace) = u(NT+fixedFace);
    end
    % The case with non-empty Dirichlet nodes but g_D=0 or g_D=[] corresponds
    % to the zero Dirichlet boundary condition and no modification of u,b is
    % needed.

    % Pure Neumann boundary condition
    if isPureNeumann
        b = b - mean(b); % compatilbe condition: sum(b) = 0
        b(1) = 0;
    end

    freeDof = [(1:NT)'; NT+freeFace];
    end % end of getbd3WG
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function varargout = grad(varargin)
% VL_GRAD Compute the gradient of an image
%   [IX,IY] = VL_GRAD(I) returns the gradient components IX,IY of the
%   2-D discrete function I. I must be a two-dimensional
%   matrix. VL_GRAD() computes the gradient by using finite
%   differences; specifically, it uses central differences for all but
%   the boundary pixels, for which it uses forward/backward
%   differences as appropriate.
%
%   Remark::
%     VL_GRAD() is similar to the MATLAB built-in GRADIENT() function,
%     excepts that it supports different gradient approximations.
%
%   VL_GRAD() accepts the following options:
%
%   Type:: central
%     Specify which type of finite differences to use for all but the
%     boundary samples. TYPE can be one of 'central', 'forward', or
%     'backward'.
%
%   See also: GRADIENT(), VL_HELP().
[varargout{1:nargout}] = vl_grad(varargin{:});


function f = p15_f ( n, x )

%*****************************************************************************80
%
%% P15_F evaluates the objective function for problem 15.
%
%  Discussion:
%
%    The Hessian matrix is doubly singular at the minimizer,
%    suggesting that most optimization routines will experience
%    a severe slowdown in convergence.
%
%    The problem is usually only defined for N being a multiple of 4.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2000
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Richard Brent,
%    Algorithms for Minimization with Derivatives,
%    Dover, 2002,
%    ISBN: 0-486-41998-3,
%    LC: QA402.5.B74.
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the argument of the objective function.
%
%    Output, real F, the value of the objective function.
%
  f = 0.0;

  for j = 1 : 4 : n

    if ( j + 1 <= n )
      xjp1 = x(j+1);
    else
      xjp1 = 0.0;
    end

    if ( j + 2 <= n )
      xjp2 = x(j+2);
    else
      xjp2 = 0.0;
    end

    if ( j + 3 <= n )
      xjp3 = x(j+3);
    else
      xjp3 = 0.0;
    end

    f1 = x(j) + 10.0 * xjp1;

    if ( j + 1 <= n )
      f2 = xjp2 - xjp3;
    else
      f2 = 0.0;
    end

    if ( j + 2 <= n )
      f3 = xjp1 - 2.0 * xjp2;
    else
      f3 = 0.0;
    end

    if ( j + 3 <= n )
      f4 = x(j) - xjp3;
    else
      f4 = 0.0;
    end

    f = f +        f1 * f1 ...
          +  5.0 * f2 * f2 ...
          +        f3 * f3 * f3 * f3 ...
          + 10.0 * f4 * f4 * f4 * f4;

  end

  return
end


function [Z,H,T,perm] = dendrogram_subtreepixels(data, method, p , x_mu, y_mu, savethis, nameappend)
% [Z,H,T,perm] = dendrogram_subtreepixels(data, method, p , x_mu, y_mu, savethis, nameappend)

% dveccr= reshape(dpixc,15^2, 1000);
if ~exist('savethis', 'var')
    savethis = 0;
end
if ~exist('nameappend', 'var')
    nameappend=[];
else
    nameappend=['_' nameappend];
end

sized = size(data);
dveccr= reshape(data,sized(1)*sized(2), sized(3));
ccd = (corrcoef(dveccr'));
e=eye(size(ccd));
ccdflip=e-(ccd-e);
ccds=squareform(1-ccdflip);

Z = linkage(ccds,method);
[H,T,perm] = dendrogram(Z,0);
co= get(gca, 'colororder');
set(H,'color','k')
ylim([0 max(Z(:,3))])

zvalues = input('Z values from dendrogram[val1*10^4 val2*10^4 ...]\n');
clear('cl')
for ii=1:length (zvalues)
    cl(ii).ixZ = find(round(10^4*Z(:,3))==zvalues(ii));
end

imz2 = zeros([size(dveccr,1) 1]);
[H,T,perm] = dendrogram(Z,0);
co= get(gca, 'colororder');
set(H,'color','k')
ylim([0 max(Z(:,3))])

lcl=length(cl);
lco=length(co);
rat = lcl/lco;

if rat > 1 %more clusters than colors
    co = repmat(co, ceil(rat), 1);
end

for ii=1:lcl
    [cl(ii).ixZ_vec, cl(ii).endleaves_vec] = recursivesubtree(Z, cl(ii).ixZ, [], []);
    set(H(cl(ii).ixZ_vec), 'color', co(ii,:))
    imz2(cl(ii).endleaves_vec, ii)=1;
end
set(H,'linewidth',2)

if savethis
    name=['dendrogram-' method nameappend];
    fprintf('saving the dendrogram figure:'' %s ''\n',name)
    SaveImageFULL(name)
    save ([name '_zvalues'], 'zvalues')
end

imzRGB = reshape(imz2*co(1:lcl,:),sized(1), sized(2), 3);
dipshow(joinchannels('RGB', imzRGB));
hold on
if exist ('p', 'var')
    if ~isempty(p)
%         scatter(p.x_vec-0.5, p.y_vec-0.5,200,'xw')
%         scatter(p.x_vec, p.y_vec,'w')
        scatter(p.x_vec, p.y_vec,[],[.9 .9 .9])
    end
end
if and(exist ('x_mu', 'var'), exist ('y_mu', 'var'))
%     scatter(x_mu-1, y_mu-1,[],'xw')
    scatter(x_mu-1, y_mu-1,[],[.9 .9 .9],'x')
end
if savethis
    name=['clustfig-' method nameappend];
    fprintf('saving the clustered figure:'' %s ''\n',name)
    SaveImageFULL(name)
end


function y = tmat_mxp ( a, x )

%*****************************************************************************80
%
%% TMAT_MXP multiplies a geometric transformation matrix times a point.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Foley, van Dam, Feiner, Hughes,
%    Computer Graphics, Principles and Practice,
%    Addison Wesley, Second Edition, 1990.
%
%  Parameters:
%
%    Input, real A(4,4), the geometric transformation matrix.
%
%    Input, real X(3), the point to be multiplied.  The fourth
%    component of X is implicitly assigned the value of 1.
%
%    Output, real Y(3), the result of A*X.  The product is
%    accumulated in a temporary vector, and then assigned to the result.
%    Therefore, it is legal for X and Y to share memory.
%
  y(1:3) = a(1:3,4) + a(1:3,1:3) * x(1:3);

  return
end


%PREX_COST PRTools example on cost matrices and rejection
%
% Prtools example code to show the use of cost matrices and how
% to introduce a reject class.

help prex_cost
randstate = randreset;
echo on
                % Generate a three class problem
  randn('state',1);
  rand('state',1);
  n = 30;
  class_labels = char('apple','pear','banana');
  a = [gendatb([n,n]);  gendatgauss(n,[-2 6])];
  laba = genlab([n n n],class_labels);
  a = setlabels(a,laba);
                % Compute a simple ldc
  w = ldc(a);
                % Scatterplot and classifier
  figure;
  gridsize(30);
  scatterd(a,'legend');
  plotc(w);

                % Define a classifier with a new cost matrix,
					 % which puts a high cost on misclassifying
					 % pears to apples
  cost = [0.0  1.0  1.0;
          9.0  0.0  1.0;
		    1.0  1.0  0.0];
  wc = w*classc*costm([],cost,class_labels);
  plotc(wc,'b');

          % Define a classifier with a cost matrix where
					 % an outlier class is introduced. For this an
					 % extra column in the cost matrix has to be defined.
					 % Furthermore, the class labels have to be supplied
					 % to give the new class a name.
  cost = [0.0  1.0  1.0  0.2;
          9.0  0.0  1.0  0.2;
          1.0  1.0  0.0  0.2];
  class_labels = char('apple','pear','banana','reject');
  wr = w*classc*costm([],cost,class_labels);
  plotc(wr,'--')

echo off
randreset(randstate);
disp(' ')
disp('   The black decision boundary shows the standard ldc classifier');
disp('   for this data. When the misclassification cost of a pear to an');
disp('   apple is increased, we obtain the blue classifier. When on top');
disp('   of that a rejection class is introduced, we get the blue dashed');
disp('   classifier. In that case, all objects between the dashed lines');
disp('   are rejected.');
fprintf('\n');
fprintf('  Cost of basic classifier  =  %4.2f\n',...
             a*w*testcost([],cost,class_labels));
fprintf('  Cost of cost classifier   =  %4.2f\n',...
             a*wc*testcost([],cost,class_labels));
fprintf('  Cost of reject classifier =  %4.2f\n',...
             a*wr*testcost([],cost,class_labels));


function L = evaluate_log_posterior(this, uv)
%EVALUATE_LOG_POSTERIOR computes the log-posterior (negative energy) of the
%   flow fields UV
%   Actually only proportional to the log posterior since the variance of neither the
%   spatial nor the data terms is considered
%
% This is a member function of the class 'classic_nl_optical_flow'.
%
% Authors: Deqing Sun, Department of Computer Science, Brown University
% Contact: dqsun@cs.brown.edu
% $Date: $
% $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.

% Spatial term
S = this.spatial_filters;
p = 0;

for i = 1:length(S)

    u_ = conv2(uv(:,:,1), S{i}, 'valid');
    v_ = conv2(uv(:,:,2), S{i}, 'valid');

    if isa(this.rho_spatial_u{i}, 'robust_function')

        p = p - sum(evaluate(this.rho_spatial_u{i}, u_(:)))...
            - sum(evaluate(this.rho_spatial_v{i}, v_(:)));

    elseif isa(this.rho_spatial_u{i}, 'gsm_density')

        p   = p + sum(evaluate_log(this.rho_spatial_u{i}, u_(:)'))...
                    + sum(evaluate_log(this.rho_spatial_v{i}, v_(:)'));

    else
        error('evaluate_log_posterior: unknown rho function!');
    end;
end;

% likelihood
It  = partial_deriv(this.images, uv, this.interpolation_method, this.deriv_filter);

if isa(this.rho_data, 'robust_function')

    l   = -sum(evaluate(this.rho_data, It(:)));

elseif isa(this.rho_data, 'gsm_density')

    l   = sum(evaluate_log(this.rho_data, It(:)'));

else
    error('evaluate_log_posterior: unknown rho function!');
end;

L = this.lambda*p + l;

if this.display
    fprintf('spatial\t%3.2e\tdata\t%3.2e\n', this.lambda*p, l);
end;


function [month, day, year] = gdate (jdate)

% convert Julian date to Gregorian (calendar) date

% input

%  jdate = julian day

% output

%  month = calendar month [1 - 12]
%  day   = calendar day [1 - 31]
%  year  = calendar year [yyyy]

%  note: day may include fractional part

% Orbital Mechanics with MATLAB

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

jd = jdate;

z = fix(jd + .5);
fday = jd + .5 - z;

if (fday < 0)
   fday = fday + 1;
   z = z - 1;
end

if (z < 2299161)
   a = z;
else
   alpha = floor((z - 1867216.25) / 36524.25);
   a = z + 1 + alpha - floor(alpha / 4);
end

b = a + 1524;
c = fix((b - 122.1) / 365.25);
d = fix(365.25 * c);
e = fix((b - d) / 30.6001);
day = b - d - fix(30.6001 * e) + fday;

if (e < 14)
   month = e - 1;
else
   month = e - 13;
end

if (month > 2)
   year = c - 4716;
else
   year = c - 4715;
end


function [acc,estimated]=sim_NN(W1,W2,b1,b2,X,t)
[L,Col]=size(X);
error=0;
estimated=zeros(1,Col);
for k=1:Col
    estimated(k)=W2*logsig(W1*X(:,k)+b1)+b2;
    estimated_aprox=sign(estimated(k));
    if not(estimated_aprox==t(k))
        error=error+1;
    end
end
acc=(Col-error)/Col;


function value = r8_chi_pdf ( df, rval )

%*****************************************************************************80
%
%% R8_CHI_PDF evaluates the PDF of a chi-squared distribution.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2013
%
%  Author:
%
%    Original FORTRAN90 version by Guannan Zhang.
%    MATLAB version by John Burkardt.
%
%  Parameters:
%
%    Input, real DF, the degrees of freedom.
%    0.0 < DF.
%
%    Input, real RVAL, the point where the PDF is evaluated.
%
%    Output, real VALUE, the value of the PDF at RVAL.
%
  if ( df <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8_CHI_PDF - Fatal error!\n' );
    fprintf ( 1, '  Degrees of freedom must be positive.\n' );
    error ( 'R8_CHI_PDF - Fatal error!' );
  end if

  if ( rval <= 0.0 )

    value = 0.0;

  else

    temp2 = df * 0.5;

    temp1 = ( temp2 - 1.0 ) * log ( rval ) - 0.5 * rval ...
      - temp2 * log ( 2.0 ) - r8_gamma_log ( temp2 );

    value = exp ( temp1 );

  end

  return
end


function [C, details] = spr_cvx(Y, options)
%%% validate arguments
if nargin < 2
    options = struct;
end
% verbosity
verbose = 0;
if isfield(options, 'verbose')
    verbose = options.verbose;
end
% noise factor
noise_factor = 0.01;
if isfield(options, 'noise_factor')
    noise_factor = options.noise_factor;
end
% affine
affine = false;
if isfield(options, 'affine')
    affine = options.affine;
end
% size of data-set
[M, S] = size(Y);
% iterate over signals
all_cols = 1:S;
% The representation matrix
C = zeros(S);
details = struct;
for s=all_cols
    if verbose > 0
        fprintf('.');
        if mod(s, 50) == 0
            fprintf('\n');
        end
    end
    % s-th data vector
    y = Y(:, s);
    % the signal dictionary to be used for reconstruction
    cols = all_cols ~= s; % S - 1 columns
    % dimensions are D x (S - 1)
    A = Y(:, cols);
    % threshold for the noise norm
    noise_norm_threshold = noise_factor * norm(y);
    if affine
        cvx_begin quiet;
            cvx_precision high;
            variable rep(S-1,1);
            minimize( norm(rep,1) );
            subject to
                norm(A * rep  - y) <= noise_norm_threshold;
                sum(rep) == 1;
        cvx_end;
    else
        cvx_begin quiet;
            cvx_precision high;
            variable rep(S-1,1);
            minimize( norm(rep,1) );
            subject to
                norm(A * rep  - y) <= noise_norm_threshold;
        cvx_end;
    end
    % Prepare the l1 solver
    % solver = spx.pursuit.single.BasisPursuit(A, x);
    % solver.Quiet = true;
    % lambda = 2.5;
    % Run the solver to obtain sparse representation
    % rep = solver.solve_l1_noise();
    % The obtained representation is in R^{S - 1} dimensions

    % Put back the representation
    C(cols, s) = rep;
end
if verbose > 0
    fprintf('\n');
end

end


%DEMOLGD1 Demonstrate simple MLP optimisation with on-line gradient descent
%
%	Description
%	The problem consists of one input variable X and one target variable
%	T with data generated by sampling X at equal intervals and then
%	generating target data by computing SIN(2*PI*X) and adding Gaussian
%	noise. A 2-layer network with linear outputs is trained by minimizing
%	a  sum-of-squares error function using on-line gradient descent.
%
%	See also
%	DEMMLP1, OLGD
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Generate the matrix of inputs x and targets t.

ndata = 20;			% Number of data points.
noise = 0.2;			% Standard deviation of noise distribution.
x = [0:1/(ndata - 1):1]';
randn('state', 42);
rand('state', 42);
t = sin(2*pi*x) + noise*randn(ndata, 1);

clc
disp('This demonstration illustrates the use of the on-line gradient')
disp('descent algorithm to train a Multi-Layer Perceptron network for')
disp('regression problems.  It is intended to illustrate the drawbacks')
disp('of this algorithm compared to more powerful non-linear optimisation')
disp('algorithms, such as conjugate gradients.')
disp(' ')
disp('First we generate the data from a noisy sine function and construct')
disp('the network.')
disp(' ')
disp('Press any key to continue.')
pause
% Set up network parameters.
nin = 1;			% Number of inputs.
nhidden = 3;			% Number of hidden units.
nout = 1;			% Number of outputs.
alpha = 0.01;			% Coefficient of weight-decay prior.

% Create and initialize network weight vector.
net = mlp(nin, nhidden, nout, 'linear');
% Initialise weights reasonably close to 0
net = mlpinit(net, 10);

% Set up vector of options for the optimiser.
options = foptions;
options(1) = 1;			% This provides display of error values.
options(14) = 20;		% Number of training cycles.
options(18) = 0.1;		% Learning rate
%options(17) = 0.4;		% Momentum
options(17) = 0.4;		% Momentum
options(5) = 1; 		% Do randomise pattern order
clc
disp('Then we set the options for the training algorithm.')
disp(['In the first phase of training, which lasts for ',...
    num2str(options(14)), ' cycles,'])
disp(['the learning rate is ', num2str(options(18)), ...
    ' and the momentum is ', num2str(options(17)), '.'])
disp('The error values are displayed at the end of each pass through the')
disp('entire pattern set.')
disp(' ')
disp('Press any key to continue.')
pause

% Train using online gradient descent
[net, options] = olgd(net, options, x, t);

% Now allow learning rate to decay and remove momentum
options(2) = 0;
options(3) = 0;
options(17) = 0.4;	% Turn off momentum
options(5) = 1;		% Randomise pattern order
options(6) = 1;		% Set learning rate decay on
options(14) = 200;
options(18) = 0.1;	% Initial learning rate

disp(['In the second phase of training, which lasts for up to ',...
    num2str(options(14)), ' cycles,'])
disp(['the learning rate starts at ', num2str(options(18)), ...
    ', decaying at 1/t and the momentum is ', num2str(options(17)), '.'])
disp(' ')
disp('Press any key to continue.')
pause
[net, options] = olgd(net, options, x, t);

clc
disp('Now we plot the data, underlying function, and network outputs')
disp('on a single graph to compare the results.')
disp(' ')
disp('Press any key to continue.')
pause

% Plot the data, the original function, and the trained network function.
plotvals = [0:0.01:1]';
y = mlpfwd(net, plotvals);
fh1 = figure;
plot(x, t, 'ob')
hold on
axis([0 1 -1.5 1.5])
fplot('sin(2*pi*x)', [0 1], '--g')
plot(plotvals, y, '-r')
legend('data', 'function', 'network');
hold off

disp('Note the very poor fit to the data: this should be compared with')
disp('the results obtained in demmlp1.')
disp(' ')
disp('Press any key to exit.')
pause
close(fh1);
clear all;


% Forward and Inverse Fast Wavelet Transforms
%
% Usage:
%   output = wlet( signal, [output_type [, num_coeff [, direction]]])
%
%
%   Parameters:
%
%      output_type is one of:
%
%         1  Output is one dimensional, and represents the raw result
%            from the fast wavelet transform algorithm (wavelet coefficients).
%            This is the default.
%
%
%         2  Output is two dimensional. This output format is convenient for
%  	     displaying transformed data as a time-scale matrix (scalogram).
%
%      num_coeff
%
%         gives the number of filter coeffients used in approximating
%         the wavelet.  Possible values are 4, 12 or 20, and the default is 20.
%
%      direction
%
%         1  Transform is forward, from time domain to time-scale
%	     domain. (default)
%
%        -1  Transform is reverse, from time-scale domain to time domain.
%
%
% Two dimensional output can only be used with the forward transform, and
% the input to both forward and reverse transforms must be one-dimensional.
%
% TFSAP 7.0
% Copyright Prof. B. Boashash
% Qatar University, Doha
% email: tfsap.research@gmail.com


% 2012-fall by Greg Handy, based on cbct_back.m
% 2013-04-04 refined by Rebecca Malinas
% 2013-04-07 refined by Jeff Fessler

function img = cbct_back_mat_par(interpRect,rectGrid,proj, ns, nt, na, ...
	ds, dt, offset_s, offset_t, offset_source, ...
	dsd, dso, dfs, orbit, orbit_start, ...
	source_zs, ...
	mask, nz, dx, dy, dz, offset_xyz, ia_skip, scale_dang, extrapolate_t)

if any(source_zs ~= 0)
	warn('helix not yet tested')
end

[nx ny] = size(mask);
betas = deg2rad(orbit_start + orbit * [0:na-1] / na); % [na] source angles

% precompute as much as possible
wx = (nx-1)/2;
wy = (ny-1)/2;
wz = (nz-1)/2;
[xc yc] = ndgrid(([0:nx-1] - wx) * dx, ([0:ny-1] - wy) * dy);
zc = ([0:nz-1] - wz) * dz;

clear wx wy wz rr smax rmax

xc = xc(mask); % [np] pixels within mask
yc = yc(mask);

ws = (ns+1)/2 + offset_s; % trick: +1 because matlab starts from 1
wt = (nt+1)/2 + offset_t; % offset_t should be zero for cone-par and tent geometry

% loop over slices
img = zeros([size(mask) nz]);
sdim = [ns+1 nt]; % trick: extra zeros saves indexing in loop
proj1 = zeros(sdim);
ticker reset
for iz=1:nz

	ia_min = 1;
	ia_max = na;

	% loop over each projection angle
	img2 = 0;
	for ia=ia_min:ia_skip:ia_max
		ticker(mfilename, [iz ia], [nz na])
		beta = betas(ia);

		x_beta = +xc * cos(beta) + yc * sin(beta);
		y_betas = (-xc * sin(beta) + yc * cos(beta));

		if interpRect
			mag = sqrt(dso^2-(x_beta).^2) ./ (sqrt(dso^2-(x_beta).^2)+y_betas); %T-FDK?
		else
			mag = dsd ./ (sqrt(dso^2-(x_beta).^2)+y_betas); %P-FDK?
		end

		sprime = x_beta;

		tprime = mag .* (zc(iz)-source_zs(ia));

			bs = sprime / ds + ws;

		if interpRect
			newdt = rectGrid(2)-rectGrid(1);
			bt = tprime / newdt + wt;
		else
			bt = tprime / dt + wt;
		end

		bt = max(bt, 1);
		bt = min(bt, nt);

		% bi-linear interpolation:
		is = floor(bs); % left bin
		it = floor(bt);

		itbad = (it==nt);
		it(itbad)=nt-1;

		isbad = (bs < 1) | (bs > ns);
		is(isbad) = ns;

		wr = bs - is;	% left weight
		wr(isbad)=1;

		wl = 1 - wr;	% right weight
		wu = bt - it;	% upper weight
		wd = 1 - wu;	% lower weight

%		ibad = (is < 0) | (is > ns) | (it < 0) | (it > nt);
%		is(ibad) = ns+1; % trick! point at harmless zeros
%		it(ibad) = nt+1;

 %		proj1(1+[1:ns],1+[1:nt]) = proj(:,:,ia); % trick: left side

%		p1 =	wl .* proj1(sub2ind(sdim, is+1,it+1)) + ...
%			wr .* proj1(sub2ind(sdim, is+2,it+1));
%		p2 =	wl .* proj1(sub2ind(sdim, is+1,it+2)) + ...
%			wr .* proj1(sub2ind(sdim, is+2,it+2));

									proj1(1:ns,1:nt) = proj(:,:,ia);%now only have extra row of zeros at ns+1 % trick: left side

		p1 =	wl .* proj1(sub2ind(sdim, is, it)) + ...
			wr .* proj1(sub2ind(sdim, is+1,it));

		p2 =	wl .* proj1(sub2ind(sdim, is, it+1)) + ...
			wr .* proj1(sub2ind(sdim, is+1,it+1));
		p0 = wd .* p1 + wu .* p2; % vertical interpolation

		% no backprojection weighting needed
		img2 = img2 + p0;
	end % ia

	img(:,:,iz) = embed(img2, mask);
end % iz

if scale_dang % final "\der angle" scale:
	img = (0.5 * deg2rad(abs(orbit)) / (na/ia_skip)) * img;
end

end % cbct_back_mat_par()


function [label, energy] = knKmeansPred(model, Xt)
% Prediction for kernel kmeans clusterng
% Input:
%   model: trained model structure
%   Xt: d x n testing data
% Ouput:
%   label: 1 x n predict label
%   engery: optimization target value
% Written by Mo Chen (sth4nth@gmail.com).
X = model.X;
t = model.label;
kn = model.kn;

n = size(X,2);
k = max(t);
E = sparse(t,1:n,1,k,n,n);
E = E./sum(E,2);
Z = E*kn(X,Xt)-dot(E*kn(X,X),E,2)/2;
[val, label] = max(Z,[],1);
energy = sum(kn(Xt))-2*sum(val);


function [J] = kcal2J(kcal)
% Convert energy or work from kilocalories to joules.
% Chad A. Greene 2012
J = kcal*4186.8;


function A = warmUpExercise()
%WARMUPEXERCISE Example function in octave
%   A = WARMUPEXERCISE() is an example function that returns the 5x5 identity matrix

A = [];
% ============= YOUR CODE HERE ==============
% Instructions: Return the 5x5 identity matrix
%               In octave, we return values by defining which variables
%               represent the return values (at the top of the file)
%               and then set them accordingly.

A = eye(5)    % 5x5 identity matrix

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

end


function [ vX, mX ] = SolveLsL1ComplexAdmm( mA, vB, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX, mX ] = SolveLsL1ComplexSubGrad( mA, vB, lambdaFctr, numIterations )
% Solves the 0.5 * || A x - b ||_2 + \lambda || x ||_1 problem using ADMM
% Method. The model allows A, b and x to be Complex.
% Input:
%   - mA                -   Model Matrix.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - vB                -   Input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double' (Complex).
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia ADMM Method - https://en.wikipedia.org/wiki/Augmented_Lagrangian_method#Alternating_direction_method_of_multipliers.
% Remarks:
%   1.  This implementation assumes the Rho Factor is 1.
% Known Issues:
%   1.  A
% TODO:
%   1.  B
% Release Notes:
%   -   1.0.000     07/11/2016
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

numRows = size(mA, 1);
numCols = size(mA, 2);

vX  = pinv(mA) * vB; %<! Dealing with "Fat Matrix"
vU = zeros([numCols, 1]);
vV = zeros([numCols, 1]);

mAA = mA' * mA;
vAb = mA' * vB;
mI = eye(numCols);

mAAI = mAA + mI;

mL = chol(mAAI, 'lower');
mU = mL';

sL.LT = true();
sU.UT = true();

mX = zeros([numCols, numIterations]);
mX(:, 1) = vX;

for ii = 2:numIterations
    % vX = mAAI \ (vAb + vV - vU);
    vX = linsolve(mU, linsolve(mL, vAb + vV - vU, sL), sU); %<! https://www.mathworks.com/help/matlab/ref/linsolve.html
    vV = ProxL1(vX + vU, paramLambda);
    vU = vU + vX - vV;

    mX(:, ii) = vX;
end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding - Complex Domain -> Keep Phase, Soft Threshold the
% Modulus

% vXAbs   = abs(vX);
% vXPhase = angle(vX);
%
% vX = max(vXAbs - lambdaFactor, 0) .* exp(1i * vXPhase);

vXAbs = abs(vX);

vX              = (vX ./ vXAbs) .* max((vXAbs - lambdaFactor), 0);
vX(vXAbs == 0)  = 0;

end


function [CT] = calculate_CT(Seconds, Throttle, RPM, b, h, gramsMeas, varargin)
% This function calculates the parameter for a zero intercept linear fit
% between RPM^2 and thrust of the form:
%   Thrust = (RPM^2)*CT
% Output is given in the form of returned CT value and plots.
%
% Argument definitions:
%   b = length of test rig from pin joint to scale contact (same units as h)
%   h = length of test rig from pin joint to center motor axis (same units as b)
%   gramsMeas = user measured values direct from scale, MUST BE IN GRAMS!
%   These values must correspond to ALL unique throttle values, even those
%   outside of "LowCutoff" and "HighCutoff."
% (Note: values of gramsMeas corresponding to throttle setting outside
% "LowCutoff" and "HighCutoff" are not used in calculation, therefore if
% measurements are only available for the range of "LowCutoff" to
% "HighCutoff", the vector can be padded with zeros in the appropriate
% locations.
%
% See also calculate_CR_B, calculate_CQ.

% Assign variable length input arguments
if ((nargin-5)==0) % if no input arguments are given for cutoff range:
LowCutoff = min(Throttle); % Use full range of input throttle data
HighCutoff = max(Throttle);
else
LowCutoff = varargin{1}; % Ignore data below this throttle value
HighCutoff = varargin{2}; % Ignore data above this throttle value
end

% Find unique Throttle Setting values (Not counting 0)
ThrottleU = unique(Throttle(Throttle~=0));

% Check length of inputs match
lengthGM = length(gramsMeas);
lengthTU = length(ThrottleU);
if lengthGM ~= lengthTU
    msgbox({['Length of measured grams values vector (' num2str(lengthGM) ')'];
        [' and number of unique Throttle values (' num2str(lengthTU) ')']; ...
       ' do not match! Check input data!'},'Argument Error','warn');
    return
end

% Find unique/filtered Throttle Setting values between low/high cutoff limits
ThrottleUF = ThrottleU(ThrottleU>=LowCutoff & ThrottleU<=HighCutoff);

% Find coresponding gramsMeasurments
gramsMeasU = gramsMeas(ThrottleU>=LowCutoff & ThrottleU<=HighCutoff); % Unique scale values

% Calculate arms ratio
armRatio = b/h;
g = 9.81; % Gravity (m/s^2)

% Calculate thrust generated by motors
newtonsThrust = g*armRatio*gramsMeasU/1000; % Vector of thrusts in Newtons

% Find an average RPM value for each unique/filtered Throttle Setting
RPMAverages100 = zeros(length(ThrottleUF),1); % preallocate
for i = 1:length(ThrottleUF) % For all of the unique and filtered throttle settings:
    RPMUF = RPM(Throttle==ThrottleUF(i));  % get RPM values cooresponding to unique/filtered Throttle value
    if length(RPMUF)>=120 % check to make sure there are enough data points for filtering
        RPMfilt100 = RPMUF(100:end); % RPMUF minus the first 100 data points (to remove transient RPM fluctuation as steady state value is reached)
    else RPMfilt100 = RPMUF((fix(length(RPMUF)/2)):end); % If there's a limited number of points, just take the second half of the values as steady state
    end
    RPMAverages100(i,1) = mean(RPMfilt100); % Take the average RPM value from these filtered data points
end

RPMsq = RPMAverages100.^2;
CT = RPMsq\newtonsThrust; % This is a technique for forcing a zero intercept using matrix multiplication techniques
CTp = [CT; 0];

% Plot the linear relationship and display calculated CT value
figure
plot(RPMsq,newtonsThrust,'*',RPMsq,polyval(CTp,RPMsq))
xlabel('RPM^2 (rev^2/min^2)','FontSize',12)
ylabel('Thrust (N)','FontSize',12)
title({['Linear Fit of Thrust vs. RPM^2']; ...
    [' '];['CT: ' num2str(CT) ' (N/RPM^2)']},'FontSize',14)
xlims = get(gca,'xlim');
ylims = get(gca,'ylim');
grid on

% Plot Residuals for assesing general quality of fit
predictedThrust = polyval(CTp, RPMsq); % predict thrust for each throttle setting of interest
residuals = newtonsThrust - predictedThrust; % find absolute error between prediction and actual
figure
plot(predictedThrust,residuals,'r*',[min(predictedThrust) max(predictedThrust)],[0 0])
title('CT Residuals Plot','FontSize',14)
xlabel('Predicted Thrust (N)','FontSize',12)
ylabel('Prediction Error (N)','FontSize',12)
grid on
axis tight
end

github-repos/MATLAB/dch33-Quad-Sim/Quad-Sim-961bc69d4939c8d0661eeb9820de668994262f65/Quadcopter Dynamic Modeling and Simulation/Data Acquisition & Analysis/MATLAB Data Analysis/calculate_CT.m