% test for TV projection on the TV ball

path(path, 'toolbox/');

clear options;
options.bound = 'sym';

rep = 'results/tv-projection/';
if not(exist(rep))
    mkdir(rep);
end
repiter = [rep 'iter/'];
if not(exist(repiter))
    mkdir(repiter);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% geometrical image
n = 256;
J = 4;
sigma = 100;
options.bound = 'per';
A = ones(n);
for j=0:J-1
    B = A;
    for k=1:2^j
        I = find(B==k);
        U = perform_blurring(randn(n),sigma,options);
        s = median(U(I));
        I1 = find( (B==k) & (U>s) );
        I2 = find( (B==k) & (U<=s) );
        A(I1) = 2*k-1;
        A(I2) = 2*k;
    end
    clf; imageplot(A);
    imwrite(rescale(A), [rep 'geometrical-' num2str(j) '.png'], 'png');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1D test
name = 'random';
n = 1024*4;
f = randn(n,1);
tv = compute_total_variation(f, options);

tv_divide = [2 4  8 16 32];
m = length(tv_divide);
clf;
imageplot(f, 'Original', m+1,1,1);
for i=1:m
    tau = tv/tv_divide(i);
    [f1,err_tv,err_l2] = perform_tv_projection(f,tau,options);
    tv1 = compute_total_variation(f1, options);
    imageplot(f1, ['TV/' num2str(tv_divide(i))], m+1,1,i+1);
end
saveas(gcf, [rep name '-1d-tv-projection.png'], 'png');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% load image
name = 'boat';
name = 'noise';
n = 200;
M = rescale( crop( load_image(name), n ) );

sigma = 0; % smoothing
if sigma>0
    M = perform_blurring(M,sigma);
end

% reduce TV norm by projection
tv = compute_total_variation(M, options);
options.niter = 2000;

tv_divide = [2 4 8 16 32 64 128];
m = length(tv_divide);
a = 2; b = ceil((m+1)/2);
options.x = [];
clf;
if strcmp(name, 'noise')
    MM = clamp( 0.45*(M-mean(M(:)))/std(M(:)), -1,1);
else
    MM = M;
end
warning off;
imwrite( rescale(MM), [repiter name '-2d-tv-projection-sig' num2str(sigma) '-01.png'] );
warning on;
imageplot(M, 'Original', a,b,1);
for i=1:m
    tau = tv/tv_divide(i);
    [M1,err_tv,err_l2] = perform_tv_projection(M,tau,options);
    tv1 = compute_total_variation(M1, options);
    disp( ['Final TV error: ' num2str( (tv1-tau)/tv1 ) '.'] );
    options.x = M1;
    if strcmp(name, 'noise')
        MM = clamp( 0.45*(M1-mean(M1(:)))/std(M1(:)), -1,1);
    else
        MM = M1;
    end
    imageplot(MM, ['TV/' num2str(tv_divide(i))], a,b,i+1);
    warning off;
    imwrite( rescale(MM), [repiter name '-2d-tv-projection-sig' num2str(sigma) '-' num2string_fixeddigit(tv_divide(i),2) '.png'] );
    warning on;
end
saveas(gcf, [rep name '-2d-tv-projection-sig' num2str(sigma) '.png'], 'png');


function interior_point = point_inside_polygon(V)
  % POINT_INSIDE_POLYGON  return some point inside of the given polygon
  %
  % interior_point = point_inside_polygon(V)
  %
  % Input
  %  V n by 2 vertex array
  % Output
  %  interior_point  a point inside V
  %
  % Copyright 2011, Alec Jacobson (jacobson@inf.ethz.ch)

  assert(size(V,1) >= 3);
  % start with a point above the polygon in the middle
  p = 1.0 + [mean(V(:,1)) max(V(:,2))];

  % first hits are just mid points on segments
  first_hits = ...
    [V(:,1) + V([end, 1:(end-1)],1) , ...
    V(:,2) + V([end, 1:(end-1)],2)]/2;

  distance_to_p = ...
    sum((repmat(p,size(first_hits,1),1) - first_hits).^2,2);

  first_hit = first_hits(distance_to_p == min(distance_to_p),:);
  first_hit = first_hit(1,:);

  x1 = p(1);
  y1 = p(2);
  x2 = first_hit(1);
  y2 = first_hit(2);
  x3 = V(1:end,1);
  y3 = V(1:end,2);
  x4 = V([end, 1:(end-1)],1);
  y4 = V([end, 1:(end-1)],2);
  % Find intersections along this ray with each line segment's line
  % from http://en.wikipedia.org/wiki/Line-line_intersection
  second_hits = ...
      [((x1.*y2-y1.*x2).*(x3-x4) - (x1-x2).*(x3.*y4-y3.*x4))./...
      ((x1-x2).*(y3-y4) - (y1 - y2).*(x3-x4)), ...
      ((x1.*y2-y1.*x2).*(y3-y4) - (y1-y2).*(x3.*y4-y3.*x4))./...
      ((x1-x2).*(y3-y4) - (y1 - y2).*(x3-x4))];

  % Check if intersection is really on line segment, handling vertical and
  % horizontal lines
  valid_second_hits = ...
    second_hits( ...
      ((second_hits(:,1) >= x3 & second_hits(:,1) <= x4 )| ...
      (second_hits(:,1) <= x3 & second_hits(:,1) >= x4 ) | ...
      (x3 == x4) ...
      ) & ...
      ((second_hits(:,2) >= y3 & second_hits(:,2) <= y4 )| ...
      (second_hits(:,2) <= y3 & second_hits(:,2) >= y4 ) | ...
      (y3 == y4) ...
      ) & ~((x3 == x4)&(y3 == y4))...
    ,:);

  distance_to_first_hit = ...
    sum((repmat(first_hit,size(valid_second_hits,1),1) - ...
      valid_second_hits).^2,2);

  EPSILON = 10.0^-15;
  valid_second_hits = ...
    valid_second_hits(distance_to_first_hit>EPSILON,:);
  distance_to_first_hit = ...
    distance_to_first_hit(distance_to_first_hit>EPSILON,:);

  second_hit = valid_second_hits(...
    distance_to_first_hit == min(distance_to_first_hit),:);
  % shouldn't have to do this:
  second_hit = second_hit(1,:);

  interior_point = (first_hit + second_hit)/2;

end


function d = grPropagateDistance(v, e, v0, l)
%GRPROPAGATEDISTANCE Propagates distances from a vertex to other vertices
%
%   DISTS = grPropagateDistance(V, E, V0, L)
%   V0 is index of initial vertex
%   E is array of source and target vertices
%   L is the vector of length of each edge. If not specified, length 1 is
%       assumed for all edges.
%   The result DISTS is a column array with as many rows as the number of
%   vertices, containing the geodesic distance of each vertex to the vertex
%   of index V0.
%
%   Example
%     nodes = [20 20;20 50;20 80;50 50;80 20;80 50;80 80];
%     edges = [1 2;2 3;2 4;4 6;5 6;6 7];
%     figure; drawGraph(nodes, edges);
%     axis([0 100 0 100]); axis equal; hold on
%     DISTS = grPropagateDistance(nodes, edges, 2)
%     DISTS =
%          1
%          0
%          1
%          1
%          3
%          2
%          3
%     drawNodeLabels(nodes+1, DISTS);
%
%   See Also
%   graphRadius, graphCenter, graphDiameter, graphPeripheralVertices
%   grVertexEccentricity
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-09-07,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% initialize empty array for result
Nv  = length(v);
d   = ones(Nv, 1)*inf;
d(v0) = 0;

% ensure there is a valid length array
if nargin < 4
    l = ones(size(e,1), 1);
end

% iterate from germ vertex until there are no more vertices to process
verticesToProcess = v0;
while ~isempty(verticesToProcess)
    % init new iteration
    newVerticesToProcess = [];

    % iterate over vertices that need to be updated
    for i = 1:length(verticesToProcess)
        vertex = verticesToProcess(i);

        % iterate over neighbor edges of current vertex
        vertexEdges = grAdjacentEdges(e, vertex);
        for j = 1:length(vertexEdges)
            iEdge = vertexEdges(j);

            % compute distance between current vertex and its neighbor
            vertex2 = grOppositeNode(e(iEdge,:), vertex);
            newDist = d(vertex) + l(iEdge);

            % update geodesic distance of neighbor node if needed
            if newDist < d(vertex2)
                d(vertex2) = newDist;
                newVerticesToProcess = [newVerticesToProcess ; vertex2]; %#ok<AGROW>
            end
        end
    end

    % update set of vertices for next tieration
    verticesToProcess = newVerticesToProcess;
end


function [kHz] = radps2kHz(radps)
% Convert frequency from radians per second to kilohertz.
% Chad A. Greene 2012
kHz = radps/(2*pi)/1000;


function [u,Du,eqn,info] = Poisson3T1(node,elem,pde,bdFlag, option)
%% POISSON3T1 Poisson equation: trilinear element.
%
%   u = Poisson3T1(node,elem,pde,bdFlag) produces the trilinear 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
%
% Author: Huayi Wei < huayiwei1984@gmail.com>.
%
% See also: PoissonQ1
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('option','var'), option = []; end
[N,Dim] = size(node);
[NT,NV] = size(elem);
Ndof = N;

tic;

%% Stiffness matrix and mass matrix in each direction
hx = node(elem(:,2),1) - node(elem(:,1),1);
hy = node(elem(:,4),2) - node(elem(:,1),2);
hz = node(elem(:,5),3) - node(elem(:,1),3);
volume = abs(hx.*hy.*hz);
Ax = 1./hx*[1 -1];
Mx = hx*[1/3 1/6];
Ay = 1./hy*[1 -1];
My = hy*[1/3 1/6];
Az = 1./hz*[1 -1];
Mz = hz*[1/3 1/6];

%% Assemble stiffness matrix
% generate sparse pattern
ii = zeros(36*NT,1); jj = zeros(36*NT,1);
index = 0;
for i = 1:NV
    for j = i:NV
        ii(index+1:index+NT) = double(elem(:,i));
        jj(index+1:index+NT) = double(elem(:,j));
        index = index + NT;
    end
end
% index map
itoixiyiz = [[1 2 2 1 1 2 2 1]' [1 1 2 2 1 1 2 2]' [1 1 1 1 2 2 2 2]'];

if ~isfield(pde,'d'), pde.d = []; end
sA = zeros(36*NT,1);
index = 0;
for i = 1:NV
    for j = i:NV
        ix = itoixiyiz(i,1); jx = itoixiyiz(j,1);
        iy = itoixiyiz(i,2); jy = itoixiyiz(j,2);
        iz = itoixiyiz(i,3); jz = itoixiyiz(j,3);
        Aij = Ax(:,2-(ix==jx)).*My(:,2-(iy==jy)).*Mz(:,2-(iz==jz)) ...
            + Mx(:,2-(ix==jx)).*Ay(:,2-(iy==jy)).*Mz(:,2-(iz==jz)) ...
            + Mx(:,2-(ix==jx)).*My(:,2-(iy==jy)).*Az(:,2-(iz==jz));
        if ~isempty(pde.d) && isnumeric(pde.d) % d is piecewise constant
            Aij = pde.d.*Aij;
        end
        sA(index+1:index+NT) = Aij;
        index = index + NT;
    end
end
% assemble the matrix
diagIdx = (ii == jj);   upperIdx = ~diagIdx;
A = sparse(ii(diagIdx),jj(diagIdx),sA(diagIdx),Ndof,Ndof);
AU = sparse(ii(upperIdx),jj(upperIdx),sA(upperIdx),Ndof,Ndof);
A = A + AU + AU';
clear Aij ii jj sA

%% Assemble the right hand side
b = zeros(N,1);
if ~isfield(option,'fquadorder')
    option.fquadorder = 3;   % default order
end
if ~isfield(pde,'f') || (isreal(pde.f) && (pde.f==0))
    pde.f = [];
end

if ~isempty(pde.f)
    [GaussPt, weight] = quadptscube(option.fquadorder);
    lambdax = GaussPt(:,1);
    lambday = GaussPt(:,2);
    lambdaz = GaussPt(:,3);
    nQuad = size(GaussPt,1);
    bt = zeros(NT,NV);
    for p = 1:nQuad
		% quadrature points in the x-y-z coordinate
        phi2d = kron([lambday(p) 1-lambday(p)],[lambdax(p); 1-lambdax(p)]);
        phi = transpose(kron([lambdaz(p) 1-lambdaz(p)],phi2d([1 2 4 3])));
        J = volume;
%         [phi, tempvar, J] = hexbasis(node,elem, pts(p,:));
        pxyz = zeros(NT, Dim);
        for k = 1:Dim
            xk = node(:,k);
            pxyz(:,k) = xk(elem)*phi(:);
        end
		fp = pde.f(pxyz);
        bt = bt + (weight(p)*fp.*J)*phi';
    end
    b = accumarray(elem(:),bt(:),[Ndof 1]);
end
clear pxyz bt

%% Set up boundary conditions
if ~exist('bdFlag','var'), bdFlag = []; end
[AD,b,u,freeNode,isPureNeumann] = getbd3(b);

%% Record assembling time
assembleTime = toc;
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(freeNode), return; end
% Set up solver type
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(freeNode) = AD(freeNode,freeNode)\b(freeNode);
        residual = norm(b - AD*u);
        info = struct('solverTime',toc,'itStep',0,'err',residual,'flag',2,'stopErr',residual);
    case 'none'
        info = struct('solverTime',[],'itStep',0,'err',[],'flag',3,'stopErr',[]);
    case 'mg'
%         option.x0 = u;
        option.solver = 'CG';
%         [u,info] = mg(AD,b,elem,option);
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option);
    case 'amg'
        option.solver = 'CG';
        [u(freeNode),info] = amg(AD(freeNode,freeNode),b(freeNode),option);
end
% post-process for pure Neumann problem
if isPureNeumann
    patchArea = accumarray(elem(:),[J;J;J;J;J;J;J;J]/8, [N 1]); % TODO: Here is J?
    uc = sum(u.*patchArea)/sum(J);
    u = u - uc;   % int u = 0
end

%% Output information
eqn = struct('A',AD,'b',b,'freeNode',freeNode);
info.assembleTime = assembleTime;

%% Compute Du
Du = [];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% subfunctions getbd3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [AD,b,u,freeNode,isPureNeumann] = getbd3(b)
    %% Set up of boundary conditions.
    %
    % 1) Modify the matrix for Dirichlet boundary nodes, which are not degree
    % of freedom. Values at these nodes are evaluatation of pde.g_D. The
    % original stiffness matrix A is turn into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    %
    % 2) Modify the right hand side b. The Neumann boundary integral is added
    % to b. For Dirichlet boundary ndoes, b(fixedNode) is the evaluation of
    % pde.g_D.
    %
    % Special attentation should be given for the pure Neumann boundary
    % condition. To enforce the compatible condition, the vector b should have
    % mean value zero. To avoid a singular matrix, the 1st node is chosen as
    % fixedNode.
    %
    % The order of assigning Neumann and Dirichlet boundary condition is
    % important to get the right setting at the intersection nodes of Dirichlet
    % and Neumann boundary edges.
    %
    % Reference: Long Chen. Finite Element Methods and its Programming. Lecture
    % Notes.

    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 = [];
    isRobin = (bdFlag(:) == 3);
    if any(isRobin)
        allFace = [elem(:,[1 4 3 2]); elem(:,[1 2 6 5]); elem(:,[5 6 7 8]);...
                   elem(:,[8 7 3 4]); elem(:,[4 1 5 8]); elem(:,[2 3 7 6])];
        Robin = allFace(isRobin,:);
    end
    if ~isempty(Robin) && ~isempty(pde.g_R) && ~(isnumeric(pde.g_R) && (pde.g_R == 0))
        %% To do: this is for tet mesh. Modify for hex mesh later.
        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)));
        if ~isfield(option,'gRquadorder')
            option.gRquadorder = 3;   % default order exact for linear gR
        end
        [lambdagR,weightgR] = quadpts(option.gRquadorder);
        nQuadgR = size(lambdagR,1);
        bdphi = lambdagR;
        NR = size(Robin,1);
        ss = zeros(NR,3,3);
        % int g_R phi_i phi_j
        for pp = 1:nQuadgR
            ppxyz = lambdagR(pp,1)*node(Robin(:,1),:) ...
                  + lambdagR(pp,2)*node(Robin(:,2),:) ...
                  + lambdagR(pp,3)*node(Robin(:,3),:);
            gRp = pde.g_R(ppxyz);
            for iR = 1:3
                for jR = iR:3
                    ss(:,iR,jR) = ss(:,iR,jR) + ...
                    weightgR(pp)*gRp*bdphi(pp,iR).*bdphi(pp,jR);
                end
            end
        end
        ss(:) = ss(:).*repmat(area,9,1);
        index = 0;
        for iR = 1:3
            for jR = 1:3
                iiR(index+1:index+NR) = double(Robin(:,iR));
                jjR(index+1:index+NR) = double(Robin(:,jR));
                if jR>=iR
                    ssR(index+1:index+NR) = ss(:,iR,jR);
                else
                    ssR(index+1:index+NR) = ss(:,jR,iR);
                end
                index = index + NR;
            end
        end
        A = A + sparse(iiR,jjR,ssR,Ndof,Ndof);
    end

    % Find Dirichlet boundary nodes: fixedNode
    fixedNode = []; freeNode = [];
    if ~isempty(bdFlag) % bdFlag specifies different bd conditions
        [fixedNode,bdFace,isBdNode] = findboundary3(elem,bdFlag);
        freeNode = find(~isBdNode);
    end
    if isempty(bdFlag) && ~isempty(pde.g_D) && isempty(pde.g_N) && isempty(pde.g_R)
        % no bdFlag, only pde.g_D is given
        [fixedNode,bdFace,isBdNode] = findboundary3(elem);
        freeNode = find(~isBdNode);
    end
    isPureNeumann = false;
    if isempty(fixedNode) && isempty(Robin) % pure Neumann boundary condition
        % pde.g_N could be empty which is homogenous Neumann boundary condition
        isPureNeumann = true;
        fixedNode = 1;
        freeNode = 2:Ndof;    % eliminate the kernel by enforcing u(1) = 0;
    end
    % Modify the matrix
    % Build Dirichlet boundary condition into the matrix AD by enforcing
    % AD(fixedNode,fixedNode)=I, AD(fixedNode,freeNode)=0, AD(freeNode,fixedNode)=0.
    bdidx = zeros(Ndof,1);
    bdidx(fixedNode) = 1;
    Tbd = spdiags(bdidx,0,Ndof,Ndof);
    T = spdiags(1-bdidx,0,Ndof,Ndof);
    AD = T*A*T + Tbd;

    %% Part 2: Find boundary faces and modify the load b
    % Find boundary faces: Neumann
    Neumann = [];
    if ~isempty(bdFlag)  % bdFlag specifies different bd conditions
        Neumann = bdFace;
    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
        [tempvar,Neumann] = findboundary3(elem); %#ok<ASGLU>
    end

    % Neumann boundary condition
    if ~isempty(Neumann) && ~isempty(pde.g_N) && ~(isnumeric(pde.g_N) && (pde.g_N == 0))
        v12 = node(Neumann(:,2),:)-node(Neumann(:,1),:);
        v14 = node(Neumann(:,4),:)-node(Neumann(:,1),:);
        area = sqrt(abs(sum(mycross(v12,v14,2).^2,2)));
        % three middle points rule
        if ~isfield(option,'gNquadorder')
            option.gNquadorder = 3;   % default order exact for linear gN
        end
        [GaussPt,weightgN] = quadptsquad(option.gNquadorder);
        xxi = GaussPt(:,1);
        eta = GaussPt(:,2);
        nQuadgN = size(GaussPt,1);
        % barycentrical coordinate for four basis
        lambdagN = zeros(nQuadgN,4);
        lambdagN(:,1) = (1 - xxi).*(1 - eta);
        lambdagN(:,2) = xxi.*(1 - eta);
        lambdagN(:,3) = xxi.*eta;
        lambdagN(:,4) = (1 - xxi).*eta;
        ge = zeros(size(Neumann,1),4);
        for pp = 1:nQuadgN
            % quadrature points in the x-y coordinate
            ppxyz = node(Neumann(:,1),:)*lambdagN(pp,1) + node(Neumann(:,2),:)*lambdagN(pp,2)...
                  + node(Neumann(:,3),:)*lambdagN(pp,3) + node(Neumann(:,4),:)*lambdagN(pp,4);
            gNp = pde.g_N(ppxyz);
            for iN = 1:4
                ge(:,iN) = ge(:,iN) + weightgN(pp)*lambdagN(pp,iN)*gNp;
            end
        end
        ge = ge.*repmat(area,1,4);
        b = b + accumarray(Neumann(:),ge(:),[Ndof,1]);
    end
    % The case with non-empty Neumann edges but g_N=0 or g_N=[] corresponds to
    % the zero flux boundary condition on Neumann edges and no modification of
    % A,u,b is needed.

    % Dirichlet boundary condition
    if ~isPureNeumann && ~isempty(fixedNode) && ...
       ~isempty(pde.g_D) && ~(isnumeric(pde.g_D) && all(pde.g_D == 0))    % nonzero g_D
        if isnumeric(pde.g_D)
            u(fixedNode) = pde.g_D(fixedNode);
        else
            u(fixedNode) = pde.g_D(node(fixedNode,:));
        end
        b = b - A*u;
    end
    if ~isPureNeumann % non-empty Dirichlet boundary condition
        b(fixedNode) = u(fixedNode);
    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
    end % end of getbd3
end % end of Poisson3Q1


function [C,S]=fitSpherHarmonics(M,points,vals,a,c)
%%FITSPHERHARMONICS Given a number of field values at a diversity
%               of points in spherical coordinates (either range, azimuth
%               and elevation, or just azimuth and elevation), fit fully
%               normalized spherical harmonic coefficients up to a
%               particular degree to the data. The data being fitted can be
%               real or complex. One can fit multiple sets of data at once
%               as long as they share the same set of points.
%
%INPUTS: M The maximum degree of the coefficients to use.
%   points The 2XN or 3XN set of points in spherical coordinates in the
%          form [r;azimuth;elevation] or just [azimuth;elevation]. Azimuth
%          is measured counterclockwise from the x-axis in the x-y plane.
%          Elevation is measured up from the x-y plane (towards the
%          z-axis). Angles must be given in radians. Note the system to be
%          observable, N>=(M+1)^2.
%     vals The NXnumSets sets of real or complex values at the given
%          points.
%        a The real or complex numerator in the (a/r)^n term in the
%          spherical harmonic sum. If this parameter is omitted or an empty
%          matrix is passed, then a=1 is used.
%        c The real or complex constant value by which the spherical
%          harmonic series is multiplied. If this parameter is omitted or
%          an empty matrix is passed, then c=1 is used.
%
%OUTPUTS: C A numCoeffsXnumSets collections of arrays, one for each set of
%           values passed, holding the coefficient terms that are
%           multiplied by cosines in the spherical harmonic expansion. If
%           given to a CountingClusterSet class, C(n+1,m+1) is the
%           coefficient of degree n and order m. This and S can be given to
%           the function spherHarmonicEval.
%         S A numCoeffsXnumSets collection of arrays, one for each set of
%           values passed, holding the coefficient terms that are
%           multiplied by sines in the spherical harmonic expansion. S has
%           the same structure as C.
%
%The spherical harmonic series being fit is assumed to be of the form
%V=(c/r)*sum_{n=0}^M\sum_{m=0}^n(a/r)^n*(C(n+1,m+1)*cos(m*lambda)+S(n+1,m+1)*sin(m*lambda))*\bar{P}_{nm}(sin(theta))
%where \bar{P}_{nm} is a fully-normalized associated Legendre polynomial.
%This function uses the LegendreCos function to evaluate the Legendre
%polynomials and it them constructs a linear system to solve for the values
%of C and S. Unobservable values of S are where m=0, so S(n+1,0+1)=0 and
%those components are not part of the estimation problem.
%
%Real spherical harmonic models are often used for gravitational, magnetic
%field and some terrain modeling, as discussed in part of [1], whereas
%complex spherical harmonic models are often used for electromagnetic
%modeling.
%
%The complex spherical harmonic model that is usually used in
%electromagnetics is
%V=(c/r)*sum_{n=0}^M\sum_{m=-n}^n(a/r)^n*A(n+1,m+1)*\bar{P}_{nm}(sin(theta))*exp(1j*m*lambda)
%Initially, it just looks like one has combined the S and C values into one
%complex coefficient A and then multiplied the sin term by 1j. However, if
%that were all that had been done, one wouldn't be able to match many
%models, because if A(n+1,m+1)=AR+1j*AI, expanding the term
%A(n+1,m+1)*exp(1j*m*lambda), one gets
%aR*cos(m*lambda)-aI*sin(m*lambda)+1j*(aI*cos(m*lambda)+aR*sin(m*lambda))
%Where one can see that it is not possible to independently set the real
%and the imaginary terms. However, note that the formulation of the inner
%sum used for complex values starts at -m and not at 0. This extension of
%the sum allows one to uniquely form real and complex terms. Though such a
%symmetry does not occur at m=0, that does not actually matter, since the
%sine terms are zero, so one can still independently set the real and the
%imaginary coefficients parts. Indeed, it can be shown that the resulting
%expression allows one to fit the real and complex parts independently if a
%and c are real.
%
%EXAMPLE:
%We take the coefficients for the World Magnetic Model, evaluate the
%magnetic field values at a number of points around the globe, use this
%function to reconstruct the spherical harmonic coefficients from the field
%values and compare the computed coefficients with the coefficients used to
%generate the field values.
% [C,S,a,c]=getWMMCoeffs();
% M=(1/2)*(sqrt(1+8*length(C))-1);
%
% %Evaluate the model on a grid.
% numPoints=21;
% totalGridPoints=numPoints*numPoints;
% deltaLat=180/numPoints;
% lat=-90+(0:(numPoints-1))*deltaLat;
% lat=lat*(pi/180);%Convert latitude to radians.
%
% deltaLon=360/numPoints;
% lon=-180+(0:(numPoints-1))*deltaLon;
% lon=lon*(pi/180);%Convert longitude to radians.
% [latGrid,lonGrid]=ndgrid(lat,lon);
% latLonEllipse=[latGrid(:)';lonGrid(:)'];
% %Convert from ellipsoidal latitudes to spherical latitudes (This assumes
% %that the points are on the surface of the reference ellipsoid).
% points=ellips2Sphere([latLonEllipse;zeros(1,totalGridPoints)]);
%
% %Evaluate the magnetic field values at the given points.
% V=spherHarmonicEval(C,S,points,a,c);
%
% %Reconstruct the coefficients from the magnetic field values.
% [CFit,SFit]=fitSpherHarmonics(M,points,V,a,c);
%
% %Check how good the fit is to the original set of coefficients.
% sel1=C(:)~=0;
% max(abs(CFit(sel1)-C(sel1))./C(sel1))
% sel1=S(:)~=0;
% max(abs(SFit(sel1)-S(sel1))./S(sel1))
%In both instances, the maximum relative error should have been less than
%8e-10. The fact that there is any error stems from finite precision
%effects.
%
%REFERENCES:
%[1] D. F. Crouse, "An overview of major terrestrial, celestial, and
%    temporal coordinate systems for target tracking," Naval Research
%    Laboratory, Washington, DC, Tech. Rep. NRL/FR/5344-16-10,279, 10 Aug.
%    2016.
%
%June 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(c))
    c=1;
end

if(nargin<4||isempty(a))
    a=1;
end

pointDims=size(points,1);

numEq=size(points,2);

%The total number of associated Legendre polynomials.
numPBar=(M+1)*(M+2)/2;

%The M+1 being subtracted is the number of S_{n,0} values,
%because these can just be set to zero as they are never used; they are
%always multiplied by sin(0).
numCCoeffs=numPBar;
numSCoeffs=numPBar-(M+1);

%The total number of coefficients if all values for C and S must be
%computed.
coeffsTotal=numCCoeffs+numSCoeffs;

if(coeffsTotal>numEq)
   warning('The value of M is too large for the number of values provided. The exact solution is unobservable.')
end

%The columns of Y corresponds to the C coefficients followed by the S
%coefficients.
Y=zeros(numEq,coeffsTotal);
for curEq=1:numEq
    curPoint=points(:,curEq);

    if(pointDims==3)
        r=curPoint(1);
        Az=curPoint(2);
        coLat=pi/2-curPoint(3);
    else
        r=1;
        Az=curPoint(1);
        coLat=pi/2-curPoint(2);
    end
    %We now have r, azimuth, and colatitude.

    %Get all of the associated Legendre polynomials.
    PBarVals=LegendreCos(coLat,M);
    PBarVector=PBarVals(:);

    outerCoeff=c/r;
    innerCoeff=1;
    for n=0:M
        for m=0:n
            %The index of PBarVals(n+1,m+1) in the vector PBarVector.
            idx=n*(n+1)/2+m+1;

            coeffVal=outerCoeff*innerCoeff*PBarVector(idx);

            Y(curEq,idx)=coeffVal*cos(m*Az);
            if(m>0)
                %The -(n+1) deals with the fact that there are no values of
                %S for m=0.
                Y(curEq,numCCoeffs+idx-(n+1))=coeffVal*sin(m*Az);
            end
        end
        innerCoeff=innerCoeff*(a/r);
    end
end

%If one were to use linsolve(Y,vals(:)), many systems would complain about
%Y having poor conditioning.
coeffs=lsqminnorm(Y,vals);

C=coeffs(1:numPBar,:);

%Fill in S, noting that the unused parts are left set to zero.
numSets=size(vals,2);
SData=zeros(numPBar,numSets);
curSIdx=numCCoeffs+1;
for n=1:M
    for m=1:n
        idx=n*(n+1)/2+m+1;
        SData(idx,:)=coeffs(curSIdx,:);
        curSIdx=curSIdx+1;
    end
end
S=SData;

end

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


function zCD=spherITRS2SpherCD(zSpher,param1,param2,S11)
%%SPHERITRS2SPHERCD Convert points given in spherical coordinates in the
%              International Terrestrial Reference System (ITRS), a
%              standard Earth-Cenetred-Earth-fixed (ECEF) coordinate
%              system, into centered dipole (CD) coordinates expressed in
%              spherical form, the form most commonly used in papers. CD
%              coordinates is a type of coordinate system where the z-axis
%              is aligned with the magnetic dipole of the Earth (under a
%              dipole model). This coordinate system is useful when
%              studying the ionosphere.
%
%INPUTS: zSpher One or more points given in terms of range, azimuth
%               (geocentric longitude) and elevation (geocentric latitude),
%               with the angles in radians, or in terms of just azimuth
%               and elevation if a conversion of directions is desired in
%               the ITRS. To convert N points, zSpher is a 3XN matrix with
%               each column having the format [range;azimuth; elevation] or
%               it is a 2XN matrix with each column having format [azimuth;
%               elevation]. Azimuth is measured counterclockwise from the
%               x-axis in the x-y plane. Elevation is measured up from the
%               x-y plane (towards the z-axis).
%param1,param2,S11 These optional parameters specify the magnetic field
%               parameters from which the direction of the CD pole is
%               derived. These aprameters are impied set base don how the
%               function is called:
%               1) spherITRS2SpherCD(zSpher)
%                  If all of the other parameters are omitted, then the CD
%                  pole parameters for the latest epoch of the
%                  International Geomagnetic Reference Field (IGRF) are
%                  used; the Schmidt semi-normalized coefficients are
%                  obtained using the function getIGRFCoeffs.
%               2) spherITRS2SpherCD(zSpher, year)
%                  The parameters of the IGRF for the specified epoch year
%                  are used. The year is in the Gregorian calendar and is
%                  specified as noted in the comments to the getIGRFCoeffs
%                  function.
%               3) spherITRS2SpherCD(zSpher, year, model). This is the same
%                  as spher2CenteredDipole(zSpher, year), except model can
%                  be 'IGRF' or 'WMM' to specify which magnetic field model
%                  to use. 'WMM' refers to the World magnetic model using
%                  the function getWMMCoeffs. Note than an empty year
%                  matrix can be passed to get the latest epoch year of
%                  either model.
%               4) spherITRS2SpherCD(zSpher,C10,C11,S11). In this instance,
%                  the first few Schmidt semi-normalized coefficients for a
%                  magnetic field model are given. These are the only three
%                  coefficients used to define the orientation of the CD
%                  pole. If using the getWMMCoeffs or getIGRFCoeffs
%                  function to get the coordinates, then from the outputs
%                  of those functions (specifying in the second input that
%                  they should not be fully normalized), the outputs C and
%                  S of the functions are used here as
%                  C10=C(1+1,0+1), C11=C(1+1,1+1), S11=C(1+1,1+1).
%
%OUTPUTS: zCD The points in zSpher converted into centered dipole
%             coordinates under the selected model. If zSpher is 3XN, then
%             zCD consists of range as well as azimuth and elevation. If
%             zSpher was 2XN, then zCD just consists of azimuth and
%             elevation. All angles are in radians.
%
%The definition of centered dipole (CD) coordinates is taken from [1].
%However, as opposed to using the spherical trignonometric equations in the
%paper, the rotation is performed in Cartesian coordinates so as to avoid
%singularities.
%
%REFERENCES:
%[1] A. C. Fraser-Smith, "Centered and eccentric geomagnetic dipoles and
%    their poles 1600-1985," Reviews of Geophysics, vol. 25, no. 1, pp.
%    1-16, Feb. 1987.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(zSpher,1);

%%Determine the centered dipole location from a magnetic field model.
if(nargin==1||nargin==2||nargin==3)
    if(nargin==1||isempty(param1))
    %If only position components were given, then just use the values for
    %the reference epoch of the IGRF. The unnormalized coefficients are
    %desired.
        [C,S]=getIGRFCoeffs([],false);
    elseif(nargin==2)
    %If a reference year is given, then use that in the IGRF.
        [C,S]=getIGRFCoeffs(param1,false);
    elseif(nargin==3)%If one wishes to select the magnetic field model.
        switch(param2)
            case 'IGRF'
                if(isempty(param1))
                    [C,S]=getIGRFCoeffs([],false);
                else%If the year is given
                    [C,S]=getIGRFCoeffs(param1,false);
                end
            case 'WMM'
                if(isempty(param1))
                    [C,S]=getWMMCoeffs([],false);
                else%If the year is given
                    [C,S]=getWMMCoeffs(param1,false);
                end
            otherwise
                error('Unknown magnetic field model selected')
        end
    end

    g10=C(2);
    g11=C(3);
    h11=S(3);
elseif(nargin==4)
    %If the required coefficients are directly provided.
    g10=param1;
    g11=param2;
    h11=S11;
end

%Perform the rotation in Cartesian coordinates (avoiding the singularities
%of Equation 3 of [1])
xRot=ITRS2CartCD(spher2Cart(zSpher),g10,g11,h11);

%Convert the points back to spherical coordinates to return.
zCD=Cart2Sphere(xRot);

if(numDim<3)
%If range was not provided, discard the unit ranges that arose using the
%spher2Cart conversion.
    zCD=zCD(2:3,:);
end

end

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


function t = tvec_even_bracket3 ( nt, theta1, theta2 )

%*****************************************************************************80
%
%% TVEC_EVEN_BRACKET3 computes an evenly spaced set of angles between THETA1 and THETA2.
%
%  Example:
%
%    NT = 3
%    THETA1 = 30
%    THETA2 = 90
%
%    T = ( 40, 60, 80 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NT, the number of values to compute.
%
%    Input, real THETA1, THETA2, the limiting angles.
%
%    Output, real TVEC(NT), the evenly spaced angles.
%
  for i = 1 : nt
    t(i) = ( ( 2 * nt - 2 * i + 1 ) * theta1   ...
           + (          2 * i - 1 ) * theta2 ) ...
           / ( 2 * nt             );
  end

  return
end


%Computes the position difference in meters between two coordinates defined
%with Llh.
%Inputs:
%ref=[Lat, lon, height]' of reference.
%height=[h1;h2;...;hn] where n is the number of coordinate
%hpos must be oneof the following: (each position must be on a new row)
%hpos=3rd row of Cen=[-cos(Lat)cos(lon) -cos(Lat)sin(lon) -sin(Lat)];
%or
%hpos=[Lat lon]

%Output: Metric difference between corrdinates defined in the nav frame of
%reference.

function metricdif=posdiff(hpos, height, ref)
E_SQR=0.00669437999014;
SM_AXIS=6378137;

%Convert ref into ecef and compute Cen(ref)
sL=sin(ref(1));
cL=cos(ref(1));
sl=sin(ref(2));
cl=cos(ref(2));
h=ref(3);

Re=SM_AXIS/(sqrt(1.0-E_SQR*sL*sL));
refECEF=[(Re+h)*cL*cl (Re+h)*cL*sl (Re*(1-E_SQR)+h)*sL];
CenRef=[-sL*cl -sL*sl cL;-sl cl 0;-cL*cl -cL*sl -sL];

%%%%Convert hpos to ecef
%Be sure that hpos and height is in correct order
if (size(height,2)>size(height,1))
    height=height';
    hpos=hpos';
end

%Compute the normal to the elipsoid in ECEF frame
if size(hpos,2)==2 %hpos is defined as lat/lon. Convert it to 3rd row of Cen
    normal=[-cos(hpos(:,1)).*cos(hpos(:,2)) -cos(hpos(:,1)).*sin(hpos(:,2)) -sin(hpos(:,1))];
elseif (size(hpos,2)==3) %hpos is defined
    normal=hpos;
end
Re=SM_AXIS./(1.0-E_SQR*normal(:,3).^2).^0.5;

%Compute ecef coordinates
posECEF=-[(Re+height).*normal(:,1) (Re+height).*normal(:,2) (Re*(1-E_SQR)+height).*normal(:,3)];

%Difference
metricdif=(posECEF-ones(size(posECEF,1),1)*refECEF)*CenRef';


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% this function solves the eigenvalue problem omega=f(epsi,kx,ky) for
%%% in-plane propagation (i.e. z=0) in a 2D-PhC; the problem can be
%%% separated in two orthogonal polarizations, E-pol & H-pol
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [omegaE, omegaH]=eigsEH(kGx,kGy,epsi,N,bands)
eta=inv(epsi);
A=inv(eta*(kGx^2+kGy^2)); %%% matrix for E-pol
B=inv(kGx*eta*kGx+kGy*eta*kGy); %%% matrix for H-pol
%%% options used in "eigs" calculations - tolerance 1e-12, intermediate results of iterations will
%%% not be displayed
opts.tol=1e-12; opts.disp=0;
%%% calculate the largest eigs, which become the smallest after inversion
De=eigs(A,bands,'lr',opts);
Dh=eigs(B,bands,'lr',opts);
%%% eigenvalues "omega" are put in a column vector and sorted in ascending order
omegaE=sqrt(sort(1./De));
omegaH=sqrt(sort(1./Dh));


% ONE DIMENSIONAL EFG PROGRAM
% SET UP NODAL COORDINATES ALONG BAR, DETERMINE NUMBER OF CELLS
x = [0.0:.1:1.0];
nnodes = length(x);
ncells = nnodes-1;
nosDeDirichlet = 1;

% SET PARAMETERS FOR WEIGHT FUNCTION, MATERIAL PROPERITES
dmax = 2.0;
E=1.0; area=1.0;

% DETERMINE DMI FOR EACH NODE
dm = dmax*(x(2)-x(1))*ones(1,nnodes);

%SET UP GAUSS POINTS, WEIGHTS, AND JACOBIAN FOR EACH CELL
gg = zeros(1,ncells);
jac = (x(2)-x(1))/2;
weight = 2;
gg = -.05:.1:0.95; gg(1) = 0.0;

% INITIALIZE MATRICES
k = zeros(nnodes);
f = zeros(nnodes,1);
GG = zeros(nnodes,nosDeDirichlet);

% LOOP OVER GAUSS POINTS
for j = 1:length(gg)
   xg = gg(j);

% DETERMINE DISTANCE BETWEEN NODES AND GAUSS POINT
dif = xg*ones(1,nnodes)-x;

% SET UP WEIGHTS W AND DW FOR EACH NODE
clear w dw
for i=1:nnodes
drdx = sign(dif(i))/dm(i);
r = abs(dif(i))/dm(i);
if r<=0.5
  w(i) = (2/3) - 4*r*r + 4*r^3;
  dw(i) = (-8*r + 12*r^2)*drdx;
elseif r<=1.0
  w(i) = (4/3)-4*r+4*r*r -(4/3)*r^3;
  dw(i) = (-4 + 8*r-4*r^2)*drdx;
elseif r>1.0
w(i) = 0.0;
dw(i) = 0.0;
end
end

%SET UP SHAPE FUNCTIONS AND DERIVATIVES
won = ones(1,nnodes);
p = [won;x];
B = p.*[w;w];
pp = zeros(2);
A = zeros(2);
dA = zeros(2);
for i=1:nnodes
   pp = p(1:2,i)*p(1:2,i)';
   A = A+w(1,i)*pp;
   dA = dA+dw(1,i)*pp;
end
Ainv = inv(A);
pg = [1 xg];
phi(j,:) = pg*Ainv*B;
db = p.*[dw;dw];
da = -Ainv*(dA*Ainv);
dphi(j,:) = [0 1]*Ainv*B+pg*(da*B+Ainv*db);

%ASSEMBLE DISCRETE EQUATIONS
if j == 1
   GG(1:3,1) = -phi(1:3)';
end
%else
    k = k+(weight*E*area*jac)*(dphi(j,:)'*dphi(j,:));
    fbody = area*xg;
    f = f+(weight*fbody*jac)*phi(j,:)';
%end
end

% ENFORCE BOUNDARY CONDITIONS USING LAGRANGE MULTIPLIERS
q = [0];
m = [k GG;GG' zeros(1)];

% SOLVE FOR NODAL PARAMETERS
d = m\[f' q]';
u = d(1:nnodes);

result = phi*u;
for i=1:length(x)
    exact(i) =  0.5*(x(i)) - x(i)^3/6.0;
end
%%
plot(x,u);
title('valores u');
figure();
plot(x,result);
title('valores u*phi');
figure();
plot(x,exact);
title('valores exatos');


function [DI, k] = detectChange(obj, t1, t2)
% A naive implementation of the Multivariate Alteration Detection Algorithm
% Partially refer to http://people.compute.dtu.dk/alan/software.html
% and https://github.com/mortcanty/earthengine
[rows, cols, chns1] = size(t1);
chns2 = size(t2, 3);
chns = min(chns1, chns2);
x1 = reshape(double(t1), cols*rows, chns1);
x1 = x1 - mean(x1, 1);  % Subtract mean
x2 = reshape(double(t2), cols*rows, chns2);
x2 = x2 - mean(x2, 1);
sigma = cov([x1 x2]);
sigma11 = sigma(1:chns1, 1:chns1);
sigma12 = sigma(1:chns1, chns1+1:end);
sigma21 = sigma(chns1+1:end, 1:chns1);
sigma22 = sigma(chns1+1:end, chns1+1:end);

[a, e1] = eig(sigma12 / sigma22 * sigma21, sigma11);
e1 = sqrt(diag(e1));
[b, e2] = eig(sigma21 / sigma11 * sigma12, sigma22);
e2 = sqrt(diag(e2));
[~, idx1] = sort(e1, 'descend');
a = a(:,idx1);
a = a(:,chns:-1:1); % My choice is simply to drop the part of (q-p)
[~, idx2] = sort(e2, 'descend');
b = b(:,idx2);
b = b(:,chns:-1:1);

% Normalize a for unit dispersion
% Ensure that a'*s11*a=I to meet the constraints
vars1 = (a'*sigma11*a);
a = a ./ sqrt(diag(vars1))';
% Similiar operations on b
vars2 = (b'*sigma22*b);
b = b ./ sqrt(diag(vars2))';

% Ensure sum of positive correlations between x1 and x1*a is positive
% I have no idea what this is used for???
invStd1 = diag(1./std(x1));
sgn = diag(sign(sum(invStd1*sigma11*a)));
a = a*sgn;

% Assure positive correlation between pair of canonical variates
b = b .* diag(sign(a'*sigma12*b))';

mad = x1*a - x2*b;

% Normalize mad
% This is no regular operation, yet appears to improve the result
mad = (mad - mean(mad)) ./ (std(mad)+eps);

chi2 = sum(mad.^2, 2);
prob = chi2cdf(chi2, chns);
DI = reshape(abs(mad), rows, cols, chns);

% Select bands
k = min(chns, obj.nMADUsed);
DI = DI(:,:,1:k);
end


function [ A, T, error, alignedShape ] = AlignShapesWithScale( alignFrom, alignTo )
%ALIGNSHAPESWITHSCALE Summary of this function goes here
%   Detailed explanation goes here

    numPoints = size(alignFrom,1);

    meanFrom = mean(alignFrom);
    meanTo = mean(alignTo);

    alignFromMeanNormed = bsxfun(@minus, alignFrom, meanFrom);
    alignToMeanNormed = bsxfun(@minus, alignTo, meanTo);

    % scale now
    sFrom = sqrt(sum(alignFromMeanNormed(:).^2)/numPoints);
    sTo = sqrt(sum(alignToMeanNormed(:).^2)/numPoints);

    s = sTo / sFrom;

    alignFromMeanNormed = alignFromMeanNormed/sFrom;
    alignToMeanNormed = alignToMeanNormed/sTo;

    [R, t] = AlignShapesKabsch(alignFromMeanNormed, alignToMeanNormed);

    A = s * R;
    aligned = (A * alignFrom')';
    T = mean(alignTo - aligned);
    alignedShape = bsxfun(@plus, aligned, T);
    error = sqrt(mean(sum((alignedShape - alignTo).^2,2)));

end


function can = mpot_to_cpot(mom)
% MPOT_TO_CPOT Convert a moment potential to canonical form.
% mom = mpot_to_cpot(can)

[g, h, K] = moment_to_canonical(mom.logp, mom.mu, mom.Sigma);
can = cpot(mom.domain, mom.sizes, g, h, K);

%%%%%%%%%%%

function [g, h, K] = moment_to_canonical(logp, mu, Sigma)
% MOMENT_TO_CANONICAL Convert moment characteristics to canonical form.
% [g, h, K] = moment_to_canonical(logp, mu, Sigma)

K = inv(Sigma);
h = K*mu;
n = length(K);
if isempty(mu)
  g = logp + 0.5*(log(det(K)) - n*log(2*pi));
else
  g = logp + 0.5*(log(det(K)) - n*log(2*pi) - mu'*K*mu);
end


function pixeliZedScratchPad = plotCoordinatesToImagePixels(x_coordinates, y_coordinates, imageSize)
%PLOTCOORDINATESTOIMAGEPIXELS

numberOfPoints = length(x_coordinates);
% make into an image by pixelization the coordinates 1st
x_min = min(x_coordinates);
x_max = max(x_coordinates);
y_min = min(y_coordinates);
y_max = max(y_coordinates);

% [use linspace]
% numberOfGratings = 200;
numberOfGratings = imageSize;
unpixelizedRange_x = linspace(x_min, x_max, numberOfGratings);
unpixelizedRange_y = linspace(y_min, y_max, numberOfGratings);

pixelized_x_range = 1:numberOfGratings;
pixelized_y_range = 1:numberOfGratings;

pixeliZedScratchPad = zeros(numberOfGratings, numberOfGratings);

% pixelized the delineating cooridinates
for i = 1:numberOfPoints
        distances = abs(x_coordinates(i) - unpixelizedRange_x); % find closest distance
        min_distanceIndex = find(min(distances) == distances);
        min_distanceIndexFor_x(i) = min_distanceIndex(1,1); % if there is more than 1, choose 1

        distances = abs(y_coordinates(i) - unpixelizedRange_y); % find closest distance
        min_distanceIndex = find(min(distances) == distances);
        min_distanceIndexFor_y(i) = min_distanceIndex(1,1); % if there is more than 1, choose 1
end

% % pixelized the centroid coordinates
% distances = abs(centreOfShape(1,1) - unpixelizedRange_x);
% min_distanceIndex = find(min(distances) == distances);
% min_distanceIndexFor_Centroid_x0 = min_distanceIndex(1,1); % if there is more than 1, choose 1
%
% distances = abs(centreOfShape(1,2) - unpixelizedRange_y);
% min_distanceIndex = find(min(distances) == distances);
% min_distanceIndexFor_Centroid_y0 = min_distanceIndex(1,1); % if there is more than 1, choose 1

% % pixelized the centre coordinates
% distances = abs(0 - unpixelizedRange_x);
% min_distanceIndex = find(min(distances) == distances);
% min_distanceIndexFor_x0 = min_distanceIndex(1,1); % if there is more than 1, choose 1
%
% distances = abs(0 - unpixelizedRange_y);
% min_distanceIndex = find(min(distances) == distances);
% min_distanceIndexFor_y0 = min_distanceIndex(1,1); % if there is more than 1, choose 1

% connect the dots [in the case that the points are insufficient]
%{
increasedNumberOfPoints = 800;  % [TO DO]: automate this param
methodsOfInterpolation = {'spline', 'pchip', 'linear'};
methodOfInterpolationChosen = char(methodsOfInterpolation(1));
pt = interparc(increasedNumberOfPoints,min_distanceIndexFor_x, ...
    min_distanceIndexFor_y, methodOfInterpolationChosen);
min_distanceIndexFor_x = round(pt(:,1));
min_distanceIndexFor_y = round(pt(:,2));
%}
increasedNumberOfPoints = numberOfPoints; % in the case that the points are sufficient

% mark the delineating pixels
for i = 1:increasedNumberOfPoints
    pixeliZedScratchPad(min_distanceIndexFor_x(i), min_distanceIndexFor_y(i)) = 1;
end

% pixeliZedScratchPad(min_distanceIndexFor_Centroid_x0, min_distanceIndexFor_Centroid_y0) = 1; % mark the centroid pixel
% pixeliZedScratchPad(min_distanceIndexFor_x0, min_distanceIndexFor_y0) = 1; % mark the centre pixel relative to the plot

% invert it
pixeliZedScratchPad = ~pixeliZedScratchPad;

% plot right side up
pixeliZedScratchPad = pixeliZedScratchPad';
pixeliZedScratchPad = flipud(pixeliZedScratchPad);

end


% Author: Ricardo Baptista and Matthias Poloczek
% Date:   June 2018
%
% See LICENSE.md for copyright information
%

function ising_mom = ising_model_moments(Q)

n_vars = size(Q,1);

% Generate all binary vectors
bin_vals = dec2bin(0:2^n_vars-1)-'0';
bin_vals(bin_vals == 0) = -1;
n_vectors = size(bin_vals,1);

% Compute values of PDF
pdf_vals = zeros(n_vectors,1);
for i=1:n_vectors
	pdf_vals(i) = exp(bin_vals(i,:)*Q*bin_vals(i,:)');
end

% Compute normalizing constant
norm_const = sum(pdf_vals);

% Generate matrix to store moments
ising_mom = zeros(n_vars,n_vars);

% Compute second moment for each pair of values
for i=1:n_vars
	for j=1:n_vars
		bin_pair = bin_vals(:,i).*bin_vals(:,j);
		ising_mom(i,j) = sum(bin_pair.*pdf_vals)/norm_const;
	end
end


function [P1,P2]=mg_ellblock(nelx,nely,x,y)
%mg_ellblock   prolongation for part of L-shaped and step domains
%   [P1,P2] = mg_ellblock(nelx,nely,x,y);
%   input
%          nelx    number of elements in x-direction
%          nely    number of elements in y-direction
%          x       x coordinate vector for coarse grid
%          y       y coordinate vector for coarse grid
%   output
%          P1      component of prolongation operator for upper left of
%                  step [-1,h]x[0,1]
%          P2      component of prolngation operator for border between
%                  upper left of step and right part of step
%
%   IFISS function: AR; 19 November, 2001.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
ecx=nelx/2; ecy=nely/2;
tx = x(2:nelx+1) - x(1:nelx);
bx = zeros(nelx,1);
bx(1:2:nelx-1) = x(3:2:nelx+1) - x(1:2:nelx-1);
bx(2:2:nelx)   = bx(1:2:nelx-1);
dx = tx./bx;
%
ty = y(2:nely+1) - y(1:nely);
by = zeros(nely,1);
by(1:2:nely-1) = y(3:2:nely+1) - y(1:2:nely-1);
by(2:2:nely)   = by(1:2:nely-1);
dy = ty./by;
%
p = spalloc(2*ecx+1,ecx+1,3*ecx+1);
for j=2:ecx
   p(2*j-2,j) = dx(2*j-3);
   p(2*j-1,j) = 1.0;
   p(2*j  ,j) = dx(2*j);
end
j=1;
   p(2*j-1,1) = 1.0;
   p(2*j  ,1) = dx(2*j);
j=ecx+1;
   p(2*j-2,j) = dx(2*j-3);
   p(2*j-1,j) = 1.0;
%
% main block (last row/column omitted)
P1=spalloc( (2*ecy+1)*(2*ecx), (ecy+1)*(ecx), (3*ecy+1)*(3*ecx+1) );
%
sp=p(1:end-1,1:end-1);
for j=2:ecy,
    cols = (j-1)*(ecx)+[1:ecx];
    P1((2*j-3)*(2*ecx)+[1:2*ecx],cols) = dy(2*j-3)*sp;
    P1((2*j-2)*(2*ecx)+[1:2*ecx],cols) = sp;
    P1((2*j-1)*(2*ecx)+[1:2*ecx],cols) = dy(2*j)*sp;
end
j=1;
    cols = (j-1)*(ecx)+[1:ecx];
    P1((2*j-2)*(2*ecx)+[1:2*ecx],cols) = sp;
    P1((2*j-1)*(2*ecx)+[1:2*ecx],cols) = dy(2*j)*sp;
j=ecy+1;
    cols = (j-1)*(ecx)+[1:ecx];
    P1((2*j-3)*(2*ecx)+[1:2*ecx],cols) = dy(2*j-3)*sp;
    P1((2*j-2)*(2*ecx)+[1:2*ecx],cols) = sp;
%
% coupling with large block
P2=spalloc((2*ecy+1)*(2*ecx),(ecy+1)*(ecx+1),(3*ecy+1)*(3*ecx+1) );
entry=p(2*ecx,ecx+1);
for j=2:ecy,
    cols = (j-1)*(ecx+1)+1;
    P2((2*j-3)*(2*ecx)+[2*ecx],cols) = dy(2*j-3)*entry;
    P2((2*j-2)*(2*ecx)+[2*ecx],cols) = entry;
    P2((2*j-1)*(2*ecx)+[2*ecx],cols) = dy(2*j)*entry;
end
j=1;
    cols = (j-1)*(ecx+1)+1;
    P2((2*j-2)*(2*ecx)+[2*ecx],cols) = entry;
    P2((2*j-1)*(2*ecx)+[2*ecx],cols) = dy(2*j)*entry;
j=ecy+1;
    cols = (j-1)*(ecx+1)+1;
    P2((2*j-3)*(2*ecx)+[2*ecx],cols) = dy(2*j-3)*entry;
    P2((2*j-2)*(2*ecx)+[2*ecx],cols) = entry;
return


%                           buildTestProblem.m
%
% This function creates and outputs random generated data and measurements
% according to user's choice. It is invoked in test*.m in
% order to build a test problem.
%
% Inputs:
%   m(integer): number of measurements.
%   n(integer): length of the unknown signal.
%   isComplex(boolean, default=true): whether the signal and measurement
%     matrix is complex. isNonNegativeOnly(boolean, default=false): whether
%     the signal is real and non-negative.
%   dataType(string, default='gaussian'): it currently supports
%     ['gaussian', 'fourier'].
%
% Outputs:
%   A: m x n measurement matrix/function handle.
%   xt: n x 1 vector, true signal.
%   b0: m x 1 vector, measurements.
%   At: A n x m matrix/function handle that is the transpose of A.
%
%
% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

function [A, xt, b0, At] = buildTestProblem(m, n, isComplex, isNonNegativeOnly, dataType)
if ~exist('isComplex','var') | isempty(isComplex)
    isComplex = true;
end
if ~exist('isNonNegativeOnly','var') | isempty(isNonNegativeOnly)
    isNonNegativeOnly = false;
end
if ~exist('dataType','var')
    dataType = 'Gaussian';
end

switch lower(dataType)
    case 'gaussian'
        A = (mvnrnd(zeros(1, n), eye(n)/2, m) + isComplex * 1i * ...
            mvnrnd(zeros(1, n), eye(n)/2, m));
        At = A';
        xt = (mvnrnd(zeros(1, n), eye(n)/2) + isComplex * 1i * ...
            mvnrnd(zeros(1, n), eye(n)/2))';
        b0 = abs(A*xt);
    case 'fourier'
        %  Define the Fourier measurement operator.
        %  The operator 'A' maps an n-vector into an m-vector, then
        %  computes the fft on that m-vector to produce m measurements.

        % rips first 'length' entries from a vector
        rip = @(x,length) x(1:length);
        A = @(x) fft([x;zeros(m-n,1)]);
        At = @(x) rip(m*ifft(x),n);     % transpose of FM
        xt = (mvnrnd(zeros(1, n), eye(n)/2) + isComplex * 1i * ...
            mvnrnd(zeros(1, n), eye(n)/2))';
        b0 = abs(A(xt)); % Compute the phaseless measurements

    otherwise
        error('invalid dataType: %s',dataType);
end
end


function TFM = createRotationAboutPoint3d(ROT, point)
%CREATEROTATIONABOUTPOINT3D Rotate about a point using a rotation matrix.
%
%   TFM = createRotationAboutPoint3d(ROT, POINT) Returns the transformation
%   matrix corresponding to a translation(-POINT), rotation with ROT and
%   translation(POINT). Ignores a possible translation in ROT(1:3,4).
%
%   See also
%   transforms3d, transformPoint3d, createRotationOx, createRotationOy,
%   createRotationOz, createRotation3dLineAngle, createRotationVector3d,
%   createRotationVectorPoint3d

% ------
% Author: oqilipo
% E-mail: N/A
% Created: 2021-01-31
% Copyright 2021-2022

% Extract only the rotation
ROT = [ROT(1:3,1:3), [0 0 0]'; [0 0 0 1]];

TFM = createTranslation3d(point) * ROT * createTranslation3d(-point);

end


% This is a demonstration of Radon inversion from few angles using the Potts model.
% Tested on Mac OS 10.9, Matlab 2015a,
% If everything worked correctly a result as
% in results/plFigPottsRec7Angles.fig should appear
% Warning: This experiment can take more than one hour

% load Shepp-Logan
img = phantom(256);

% Radon transform with 7 equidistant projection angles
nAngles = 7;
theta = (1:nAngles)/nAngles * 180;
A = radonop(theta, size(img,1));
data = A * img;

% raw filtered backprojection and a regularized one
uFbp = iradon(data, theta, 'Linear', 'Ram-Lak', 1, size(img, 1));
uFbpReg = iradon(data, theta, 'Linear', 'Hamming', 0.3, size(img, 1));

% joint reconstruction and segmentation using the Potts model
gamma = 0.04;
uPotts = minL2iPotts2DADMM(data, gamma, A, 'verbose', 1, 'groundTruth', img, 'isotropic', 1 );

% show results
subplot(2,3,1)
imshow(img)
title('Original')
subplot(2,3,2)
imagesc(data)
title('Data (Sinogram)')
subplot(2,3,4)
imshow(uFbp)
title('FBP result')
subplot(2,3,5)
imshow(uFbpReg)
title('FBP with Hamming window')
subplot(2,3,6)
imshow(uPotts)
title('Potts result')


function gbasis=groebner(polyset,ord,varnames,tol)
% GROEBNER - calculate the reduced Groebner basis of a set of polynomials
%  usage: gbasis = groebner(polyset,ord)
%         gbasis = groebner(polyset,ord,varnames)
%         gbasis = groebner(polyset,ord,varnames,tol)
%
% INPUTS: polyset, ord, varnames, tol
%  polyset is a cell array of multivariate polynomial coefficients
%    the cell elements can be all strings or all arrays.
%    If the polynomials are specified with arrays, each row of the array
%    represents a single term.  The first element is the coefficient and
%    the others are the degrees of each unknown in the term.
%    e.g. [-1,2,1;1,0,1] represents -x1^2*x2+x2
%    If the polynomials are specified as strings, they must use variable
%    names 'x1','x2',... (unless varnames is provided) and be valid inputs
%    for str2poly.m
%  ord is a string specifying the monomial ordering:
%    'lex': lexicographical, order by highest power of most significant
%      indeterminate, e.g. x1>x2^2, x1*x2>x1.
%    'grlex': graded lex, order by total degree then lex,
%      e.g. x1^2*x2 > x1*x2^2 > x1^2
%    'grevlex': graded reverse lex, order by total degree then by lowest
%      power of least significant indeterminate, e.g. x1*x2^2 > x1*x2*x3
%    (for all cases have x1>x2>...>xn)
%    note: revlex is not a well-ordering because 1>x1>x1^2>...
%  varnames (optional) is a cell array of variable names if polyset is a
%    cell array of strings and the variable names are not 'x1', 'x2', etc.
%    If specified, there must always be at least 2 variables.
%  tol (optional) is the zero tolerance for coefficients, otherwise
%    catastrophic cancellation may occur.  Default value is 0
%
% OUTPUTS: gbasis
%  gbasis is the reduced Groebner basis of the set of polynomials, in the
%    same format (strings or matrices) as polyset{1}.
%
% ALGORITHM:
%  A modified Buchberger's algorithm is used.  In Buchberger's algorithm,
%  at each step calculate Sij = (Lij/ai)*Pi - (Lij/aj)*Pj, for all pairs of
%  polynomials Pi, Pj, where ai and aj are the leading terms of Pi and Pj,
%  and Lij is the least common multiple of ai and aj; then reduce Sij with
%  respect to the polynomials and add it to the set (note that Sij always
%  reduces to zero if ai and aj have no variables in common).  The
%  algorithm terminates when no new polynomials can be added to the set.
%  This function reduces the polynomials at each step in order to find the
%  reduced Groebner basis.
%
% EXAMPLE: simplify equations x^2+2xy^2=0, xy+2y^3=1
%  A1=[1,2,0;2,1,2]; % 1*x^2*y^0 + 2*x^1*y^2
%  A2=[-1,0,0;1,1,1;2,0,3]; % -1*x^0*y^0 + 1*x^1*y^1 + 2*x^0*y^3
%  y=groebner({A1,A2},'lex');
%  poly2str(y) % returns {'x2^3-0.5', 'x1'}
%
% EXAMPLE: same, but with equations specified as strings:
%  groebner({'x^2+2*x*y^2','x*y+2*y^3-1'},'lex',{'x','y'})
%  % returns {'y^3-0.5','x'}
%
% EXAMPLE: show that a set of equations is inconsistent
%  groebner({'x + y', 'x^2 - 1', 'y^2 - 2*x'}, 'lex', {'x', 'y'})
%  % returns {'1'}
%
%
% KNOWN ISSUES
%
%  0. GENERAL: while a significant effort has been made to ensure that the
%    program works as described, unidentified bugs may remain and the results
%    should be regarded with caution.  In particular, the suite of tests
%    performed was by no means comprehensive.
%
%  1. ACCURACY: the algorithm by default works in floating-point which
%    ultimately cannot perform exact arithmetic, so unexpected silent errors may
%    occur even when the coefficients are specified as integers.  A workaround
%    is to use other data types but this has its own issues (see below).
%
%  1a. EXAMPLE (roundoff - linearly independent polynomials)
%    eps % returns 2.2204e-16 (32-bit PC windows machine epsilon)
%    groebner({'x1+x2','x1+1.000000000000001*x2'},'lex') % ok, returns {'x2','x1'}
%    groebner({'x1+x2','x1+1.0000000000000001*x2'},'lex') % not ok, returns {'x2+x1'}
%
%  1b. EXAMPLE (roundoff - linearly dependent polynomials)
%    2/sqrt(2)-sqrt(2) % expect 0, returns -2.2204e-016
%    poly2str(groebner({[1,1,0;sqrt(2),0,1],[1/sqrt(2),1,0;1,0,1]},'lex')) % ok, returns {'1.41421*x2+x1'}
%    poly2str(groebner({[1,1,0;sqrt(2),0,1],[sqrt(2),1,0;2,0,1]},'lex')) % not ok, returns {'x2','x1'}
%    poly2str(groebner({[1,1,0;sqrt(2),0,1],[sqrt(2),1,0;2,0,1]},'lex',{},1e-14)) % ok, returns {'1.41421*x2+x1'}
%
%  1c. EXAMPLE (courtesy of Christophe Lauwerys - catastrophic cancellation & tolerance bug)
%    groebner({'t^3+x+y','t^2+0.5*x^2-x-z^2','t^2+y-z^2'},'lex',{'t','x','y','z'}) % ans{1} contains Inf
%
%  1d. EXAMPLE (from Mathematica documentation - catastrophic cancellation because of large intermediate coefficients)
%    % expect 3 polynomials returns, one of degree 8 in z; instead can get '1'
%    groebner({'x^2 + y^2 + z^2 - 1', 'x*y - z + 2', 'z^2 - 2*x + 3*y'}, 'lex', {'x', 'y','z'})
%
%  2. NON-DOUBLE INPUTS: use of non-double coefficients is not supported.  While
%    it may be tempting to introduce symbolic coefficients, it is not clear how
%    they should be consistently treated - for example 'A*x' could reduce to '0'
%    or 'x' or both, depending upon the assumptions on the coefficient A.
%
%  3. EFFICIENCY: The reduction algorithms aren't terribly efficient.  One of
%    the major bottlenecks at present is the calculation of the leading term,
%    which currently involves a call to sortrows each time.  It could be more
%    efficient to maintain an ordered list or even a priority queue (e.g.
%    implemented as a heap).  Also lattice reduction algorithms such as LLL
%    might speed things up.
%
%
% SEE ALSO:
%  poly2str, str2poly, polynsolve

% Author: Ben Petschel 19/6/2009
%
% Change history:
%  19/6/2009 - first release (ord='grlex' and ord='grevlex' not
%    implemented yet)
%  22/6/2009 - implemented ord='grlex' and ord='grevlex'
%    reduction algorithm now reduces lower-order terms as well
%  23/6/2009 - removed ord='revlex' because it is not a well-ordering
%  17/7/2009 - fixed bug in handling polynomials with >=3 variables
%  20/3/2010 - changed polynomial representation from multidimensional
%    arrays to 2d arrays (more memory efficient with large number of vars)
%  11/10/2010 - bugfix for handling error tolerance

if (numel(polyset)>0) && ischar(polyset{1}),
  charout = true;
  if nargin<3,
    polyset = str2poly(polyset);
  else
    polyset = str2poly(polyset,varnames);
  end;
else
  charout = false;
end;

if nargin<4,
  tol = 0;
end;

% determine the number of degrees
nd=max(cellfun(@(x)size(x,2)-1,polyset)); % error if = 0
if nd == 0
  error('number of variables must be >= 1');
end;

gbasis = fullreduce(polyset,ord,tol,nd);
oldgbasis = {};

while ~isequal(oldgbasis,gbasis),
  oldgbasis = gbasis;
  Qset = SPoly(gbasis,ord,tol,nd);
  gbasis = fullreduce([gbasis,Qset],ord,tol,nd);
end;

if charout,
  if nargin<3,
    gbasis = poly2str(gbasis);
  else
    gbasis = poly2str(gbasis,varnames);
  end;
end;

end % main function groebner(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Qset = SPoly(Pset,ord,tol,nd)
% calculates all reduced S-polynomials of Pset (Pset must be reduced)
%   Sij = (Lij/ai)*Pi - (Lij/aj)*Pj,
% for all pairs of polynomials Pi, Pj, where ai and aj are the leading
% terms of Pi and Pj, and Lij is the least common multiple of ai and aj
% then reduces Sij wrt Pset

Qset = {};
for i=1:numel(Pset)-1,
  for j=i+1:numel(Pset),
    [c1,d1] = leadterm(Pset{i},ord,tol,nd);
    [c2,d2] = leadterm(Pset{j},ord,tol,nd);
    if any(and(d1>0,d2>0)),
      % leading terms have a variable in common, so S-poly is nontrivial
      L = max(d1,d2); % LCM of leading terms
      S = addpolys(multiplyterm(Pset{i},L-d1),multconst(multiplyterm(Pset{j},L-d2),-1));
      S = reduceset(S,Pset,ord,tol,nd);
      %if any(S(:)~=0),
      if abs(leadterm(S,ord,tol,nd))>tol
        Qset{end+1}=S; % add S to the list
      end;
    end;
  end;
end;

end % helper function SPoly(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Qset = fullreduce(Pset,ord,tol,nd)
% reduces all polynomials wrt each other
Qset = Pset;
oldQset = {};

while ~isequal(oldQset,Qset),
  oldQset = Qset;
  for i=1:numel(Qset)-1,
    for j=i+1:numel(Qset),
      Qset{i}=reduce(Qset{i},Qset{j},ord,tol,nd);
      Qset{j}=reduce(Qset{j},Qset{i},ord,tol,nd);
    end;
  end;
end;

% keep only the nonzero results (to within zero tolerance tol)
Qset = Qset(cellfun(@(x)abs(leadterm(x,ord,tol,nd))>tol,Qset));

if numel(Qset)==1,
  % special case, a single equation can slip through without being reduced
  c = leadterm(Qset{1},ord,tol,nd);
  % coefficients occupy first column of the array
  Qset{1} = multconst(Qset{1},1/c); % know c~=0 because have already removed zero eqns
end;

end % helper function fullreduce(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Q = reduceset(P,Pset,ord,tol,nd)
% reduces P wrt all polynomials in Pset
Q = P;
oldQ = [];

while ~isequal(oldQ,Q),
  oldQ = Q;
  for i=1:numel(Pset),
    Q=reduce(Q,Pset{i},ord,tol,nd);
  end;
end;

end % helper function reduceset(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Q1=reduce(P1,P2,ord,tol,nd)
% reduces a polynomial P1 by subtracting multiples of P2 so that leading
% term coefficient is 1 and leading term of P2 does not divide that of P1

Q2=P2;
[c2,d2] = leadterm(Q2,ord,tol,nd);
if abs(c2)<=tol,
  % ignore terms less than tol
  keepgoing = false;
  Q1 = P1;
else
  keepgoing = true;
  Q2 = multconst(Q2,1/c2);
  Q1 = 0; % will successively add terms to Q
  % remove the leading terms, in order to reduce wrt lower terms
  remain = P1;
end;

while keepgoing,
  [c1,d1] = leadterm(remain,ord,tol,nd);
  if abs(c1)<=tol,
    % ignore terms less than tol
    keepgoing = false;
  else
    %reduce remain by Q2, if possible, or remove leading term
    if d1>=d2,
      % can reduce Q1 by Q2, so subtract a multiple of Q2 from Q1
      remain = addpolys(remain,multconst(multiplyterm(Q2,d1-d2),-c1));
    else
      % cannot reduce any further wrt leading term, but try lower terms
      lead = multiplyterm(c1,d1);
      remain = addpolys(remain,multconst(lead,-1));
      Q1 = addpolys(Q1,lead); % add irreducible terms to Q1
    end;
  end;
end;

% reduce leading coefficient, if possible
c1 = leadterm(Q1,ord,tol,nd);
if abs(c1)>tol,
  Q1 = multconst(Q1,1/c1);
end;

end % helper function reduce(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Q=multconst(P,c)
% returns c*P where c is a constant

Q = P;
Q(:,1)=Q(:,1)*c;

end % helper function multconst(...)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Q=addpolys(P1,P2)
% adds two polynomials

s1 = size(P1,2);
s2 = size(P2,2);

if s1>s2
  Q = [P1;P2,zeros(size(P2,1),s1-s2)];
else
  Q = [P1,zeros(size(P1,1),s2-s1);P2];
end;

Q = sortrows(Q,2:size(Q,2));
i=1;
while i<size(Q,1)
  % merge rows representing terms with the same exponents
  if Q(i,1)==0,
    Q(i,:)=[];
  elseif all(Q(i,2:end)==Q(i+1,2:end))
    Q(i,1)=Q(i,1)+Q(i+1,1);
    Q(i+1,:)=[];
    if Q(i,1)==0
      % remove terms that add to zero
      Q(i,:)=[];
    end;
  else
    i=i+1;
  end;
end;

end % helper function addpolys

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Q=multiplyterm(P,d)
% multiplies a polynomial by a term (no coefficient)

% padding a matrix at the end is same as multiplying by x
d=d(:).';
sP = size(P,2);
sd = size(d,2)+1; % d doesn't include coefficient
if sP<sd,
  Q=[P,zeros(size(P,1),sd-sP)];
else
  Q=P;
  d=[(d(:)).',zeros(1,sP-sd)];
end;
Q=Q+repmat([0,d],size(Q,1),1);

end % helper function multiplyterm

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [coeff,deg]=leadterm(P,ord,tol,nd)
% returns the leading term of P
% deg is a row vector of length size(P,2)-1
% coeff is the leading coefficient, deg(i) is the degree of xi
% ignores terms with coefficients <= tol
% nd is the minimum number of degrees (in order to handle cases where deg=0)

d=size(P,2);

if all(abs(P(:,1))<=tol),
  % P=0
  coeff=0;
  deg=zeros(1,d-1);
else
  switch ord
    case 'lex'
      P=sortrows(P,2:d);

    case 'revlex'
      error('revlex is not a well-ordering');

    case 'grlex'
      P = [P,sum(P(:,2:d),2)];
      P = sortrows(P,[d+1,2:d]);
      P = P(:,1:d);

    case 'grevlex'
      P = [P,sum(P(:,2:d),2)];
      P = sortrows(P,[d+1,-(d:-1:2)]);
      P = P(:,1:d);

    otherwise
      error('ordering %s not recognized',ord);

  end; % switch ord

  ind = find(abs(P(:,1))>tol,1,'last');
  if isempty(ind),
    coeff = 0;
    deg = zeros(1,d-1);
  else
    coeff = P(ind,1);
    deg = P(ind,2:end);
  end;

end; % if all(P(:)==0)

if nd>d-1,
  deg = [deg,zeros(1,nd-d+1)];
end;

end % helper function leadterm(...)


function [C, sigma] = dataset3Params(X, y, Xval, yval)
%EX6PARAMS returns your choice of C and sigma for Part 3 of the exercise
%where you select the optimal (C, sigma) learning parameters to use for SVM
%with RBF kernel
%   [C, sigma] = EX6PARAMS(X, y, Xval, yval) returns your choice of C and
%   sigma. You should complete this function to return the optimal C and
%   sigma based on a cross-validation set.
%

% You need to return the following variables correctly.
C = 1;
sigma = 0.3;

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return the optimal C and sigma
%               learning parameters found using the cross validation set.
%               You can use svmPredict to predict the labels on the cross
%               validation set. For example,
%                   predictions = svmPredict(model, Xval);
%               will return the predictions on the cross validation set.
%
%  Note: You can compute the prediction error using
%        mean(double(predictions ~= yval))
%

possibles = [0.01 0.03 0.1 0.3 1 3 10 30];
error = realmax();
for i=1:size(possibles, 2)
	for j=1:size(possibles, 2)
		C_pos = possibles(i);
		sigma_pos = possibles(j);

		model = svmTrain(X, y, C_pos, @(x1, x2) gaussianKernel(x1, x2, sigma_pos));
		predictions = svmPredict(model, Xval);
		new_error = mean(double(predictions ~= yval));

		if new_error < error
			error = new_error;
			C = C_pos;
			sigma = sigma_pos;
		end
	end
end

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

end


function heat_oned_test01 ( )

%*****************************************************************************80
%
%% HEAT_ONED_TEST01 runs Jeff Borggaard's test case.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 December 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HEAT_ONED_TEST01:\n' );
  fprintf ( 1, '  Run Jeff Borggaard''s test case.\n' );
%
%  Set up the problem.
%
  mu = 0.1;
  h = 100.0;
  rho = 40.0;
  degree = 2;
  xa = 0.0;
  xb = 1.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Viscosity parameter MU = %g\n', mu );
  fprintf ( 1, '  Heat transfer coefficient H = %g\n', h );
  fprintf ( 1, '  Mesh density RHO = %g\n', rho );
  fprintf ( 1, '  Requesting finite elements of degree %d\n', degree );
  fprintf ( 1, '  Spatial interval is [%g,%g].\n', xa, xb );

  [ A, B, M, n_nodes, x ] = heat_oned_setup ( mu, h, rho, degree, xa, xb );
%
%  Compute the solution over time.
%
  t_initial = 0.0;
  t_step    = 0.001;
  t_save    = 0.01;
  t_final   = 5.0;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial time =   %g\n', t_initial );
  fprintf ( 1, '  Final time =     %g\n', t_final );
  fprintf ( 1, '  Time step =      %g\n', t_step );
  fprintf ( 1, '  Data save step = %g\n', t_save );

  w_save = heat_oned ( A, B, M, n_nodes, x, t_initial, t_final, t_step, t_save );
%
%  Plot the results.
%
  t_plot = [ t_initial : t_save : t_final ];

  figure ( 1 )
  mesh ( t_plot, x, w_save )
  xlabel ( '<---Time--->' )
  ylabel ( '<---X--->' )
  zlabel ( '<--U(X,T)-->' )
  title ( 'U(X,T) as computed by HEAT\_ONED' )

  filename = 'heat_oned_test01.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Plot saved as "%s".\n', filename );

  return
end


function stroud_test37 ( )

%*****************************************************************************80
%
%% TEST37 tests SQUARE_UNIT_SET, SQUARE_UNIT_SUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2009
%
%  Author:
%
%    John Burkardt
%
  global FUNC_2D_INDEX;

  max_rule = 6;

  num = function_2d_num ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST37\n' );
  fprintf ( 1, '  SQUARE_UNIT_SET sets up quadrature on the unit square;\n' );
  fprintf ( 1, '  SQUARE_UNIT_SUM carries it out on the unit square.\n' );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : max_rule

    ihi = min ( ilo + 4, max_rule );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Rule:    ' );
    for rule = ilo : ihi
      fprintf ( 1, '  %6d     ', rule );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '  Function\n' );
    fprintf ( 1, '\n' );

    for i = 1 : num

      FUNC_2D_INDEX = i;

      for rule = ilo : ihi

        order = square_unit_size ( rule );

        [ xtab, ytab, weight ] = square_unit_set ( rule, order );

        result(rule) = square_unit_sum ( 'function_2d', order, xtab, ytab, weight );

      end

      fname = function_2d_name ( i );

      fprintf ( 1, '  %s', fname );
      for rule = ilo : ihi
        fprintf ( 1, '  %11f', result(rule) );
      end
      fprintf ( 1, '\n' );

    end

  end

  return
end


function spiral_test01 ( )

%*****************************************************************************80
%
%% SPIRAL_TEST01 generates a field and estimates its range.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'SPIRAL_TEST01:\n' );
  fprintf ( 1, '  Sample a spiral velocity field and estimate\n' );
  fprintf ( 1, '  the range of the solution values.\n' );

  n = 1000;

  x_lo = 0.0;
  x_hi = 1.0;
  seed = 123456789;
  [ x, seed ] = r8vec_uniform_ab ( n, x_lo, x_hi, seed );
  y_lo = 0.0;
  y_hi = 1.0;
  [ y, seed ] = r8vec_uniform_ab ( n, y_lo, y_hi, seed );

  c = 1.00;

  [ u, v ] = uv_spiral ( n, x, y, c );

  fprintf ( 1, '\n' );
  fprintf ( 1, '           Minimum       Maximum\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  U:  %14.6g  %14.6g\n', min ( u ), max ( u ) );
  fprintf ( 1, '  V:  %14.6g  %14.6g\n', min ( v ), max ( v ) );

  return
end


function [ n_data, x, fx ] = erf_values ( n_data )

%*****************************************************************************80
%
%% ERF_VALUES returns some values of the ERF or "error" function.
%
%  Discussion:
%
%    The error function is defined by:
%
%      ERF(X) = ( 2 / sqrt ( PI ) * integral ( 0 <= T <= X ) exp ( - T^2 ) dT
%
%    In Mathematica, the function can be evaluated by:
%
%      Erf[x]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real X, the argument of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 21;

  fx_vec = [ ...
     0.0000000000000000E+00, ...
     0.1124629160182849E+00, ...
     0.2227025892104785E+00, ...
     0.3286267594591274E+00, ...
     0.4283923550466685E+00, ...
     0.5204998778130465E+00, ...
     0.6038560908479259E+00, ...
     0.6778011938374185E+00, ...
     0.7421009647076605E+00, ...
     0.7969082124228321E+00, ...
     0.8427007929497149E+00, ...
     0.8802050695740817E+00, ...
     0.9103139782296354E+00, ...
     0.9340079449406524E+00, ...
     0.9522851197626488E+00, ...
     0.9661051464753107E+00, ...
     0.9763483833446440E+00, ...
     0.9837904585907746E+00, ...
     0.9890905016357307E+00, ...
     0.9927904292352575E+00, ...
     0.9953222650189527E+00 ];

  x_vec = [ ...
     0.0E+00, ...
     0.1E+00, ...
     0.2E+00, ...
     0.3E+00, ...
     0.4E+00, ...
     0.5E+00, ...
     0.6E+00, ...
     0.7E+00, ...
     0.8E+00, ...
     0.9E+00, ...
     1.0E+00, ...
     1.1E+00, ...
     1.2E+00, ...
     1.3E+00, ...
     1.4E+00, ...
     1.5E+00, ...
     1.6E+00, ...
     1.7E+00, ...
     1.8E+00, ...
     1.9E+00, ...
     2.0E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    x = 0.0;
    fx = 0.0;
  else
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function [kx, ky] = gradientKernels(sigma)
% Create kernels for computing gradient within 2D images.
%
%   KX = gradientKernels(SIGMA)
%   [KX, KY] = gradientKernels(SIGMA)
%   Generates the gradient kernel(s) used for computing gradients within 2D
%   images.
%   This is a low-level function that is called by the functions that
%   compute gradients.
%
%
%   Example
%   [kx, ky] = gradientKernels(2);
%
%   See also
%     imGradient, imGradientFilter, orientedGaussianKernel
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% INRAE - BIA Research Unit - BIBS Platform (Nantes)
% Created: 2022-06-01,    using Matlab 9.9.0.1570001 (R2020b) Update 4
% Copyright 2022 INRAE.

if nargin == 0 || sigma == 0
    % Default case: normalised sobel matrix
    kx = fspecial('sobel')' / 8;

else
    % compute size according to sigma
    Nx = ceil((3*sigma));
    lx = -Nx:Nx;
    ky = exp(-((lx / sigma) .^2) * .5);
    kx = -(lx / sigma) .* ky;
    kx = ky' * kx;
    kx = kx / sum(kx(kx > 0));
end

% optional kernel for gradient in Y-direction
if nargout > 1
    ky = kx';
end