classdef lasso
%function [Problem] = lasso(A, b, lambda, sub_mode, smooth_lambda)
% This file defines the lasso (least absolute shrinkage and selection operator) problem for L1 norm.
%
% Inputs:
%       A               dictionary matrix of size dxn.
%       b               observation vector of size dx1.
%       lambda          l1-regularized parameter.
%       sub_mode        {'prox_reg', 'subgrad_reg', 'smooth', 'const'}
%       smooth_lambda
% Output:
%       Problem     problem instance.
%
%
% The problem of interest is defined as
%
%       (1) sub_mode = 'prox_reg' for L1 regularizer solved by proximal methods
%
%           min f(w) = 1/2 * || A * w - b ||_2^2 + lambda * || w ||_1
%
%       (2) sub_mode = 'subgrad_reg' for L1 regularizer solved by subgrad methods
%
%           min f(w) = 1/2 * || A * w - b ||_2^2 + lambda * || w ||_1
%
%       (3) sub_mode = 'smooth' for smoothing
%
%           min f(w) = 1/2 * || A * w - b ||_2^2 + smooth_lambda * || w ||_1.
%
%       (43) sub_mode = 'const' for L1 constraint
%
%           min f(w) = 1/2 * || A * w - b ||_2^2,      s.t. || w ||_1 < r
%
%
% "w" is the model parameter of size d vector.
%
%
% This file is part of GDLibrary and SGDLibrary.
%
% Created by H.Kasai on Apr. 17, 2017
% Modified by H.Kasai on Mar. 25, 2018
% Modified by H.Kasai on Mar. 27, 2020
% Modified by H.Kasai on Mar. 30, 2020

    properties
        name;
        dim;
        samples;
        sub_mode;
        lambda;
        d;
        n;
        A;
        b;
        AtA;
        Atb;
        L;
        prox_flag;
        sign_flag;
        idx;
        smooth_mu;      % for smoothing
        smooth_type;    % for smoothing
    end

    methods
        function obj = lasso(A, b, lambda, sub_mode, varargin)

            obj.A = A;
            obj.b = b;

            if strcmp(sub_mode, 'prox_reg')
                obj.prox_flag = true;
            elseif strcmp(sub_mode, 'subgrad_reg')
                obj.prox_flag = false;
            elseif strcmp(sub_mode, 'const')
                obj.prox_flag = true;
            elseif strcmp(sub_mode, 'smooth')
                obj.prox_flag = false;
            else
                error('Not correctly specified for sub_mode');
            end

            obj.sub_mode = sub_mode;
            obj.lambda = lambda;

            if nargin < 6
                obj.smooth_type = 'type1';
            else
                obj.smooth_type = varargin{2};
            end

            if nargin < 5
                obj.smooth_mu = 0.1;
            else
                obj.smooth_mu = varargin{1};
            end

            obj.d = size(obj.A, 2);
            obj.n = size(obj.A, 2);

            obj.name = 'lasso';
            obj.dim = obj.d;
            obj.samples = obj.n;

            obj.AtA = obj.A'*obj.A;
            obj.Atb = obj.A'*obj.b;
            %L = max(eig(AtA));
            fprintf('Calculated Lipschitz constant (L), i.e., max(eig(AtA)), .... ')
            obj.L = eigs(obj.AtA, 1);
            fprintf('is L = %f.\n', obj.L);
        end

        function v = prox(obj, w, t)
            if strcmp(obj.sub_mode, 'prox_reg')
                v = soft_thresh(w, t * obj.lambda);
            elseif strcmp(obj.sub_mode, 'const')
                %v = sign(w) .* proj_simplex(abs(w), obj.lambda, 'ineq');
                v = proj_l1_ball(w, obj.lambda);
            else
                error('Not correctly specified for sub_mode');
            end
        end

        function f = cost(obj, w)
            reg = obj.reg(w);
            f = 1/2 * sum((obj.A * w - obj.b).^2) + obj.lambda * reg;
        end

        % calculate l1 norm
        function r = reg(obj, w)
            r = norm(w,1);
        end

        function r = residual(obj, w)
            r = - obj.A * w + obj.b;
        end

        function f = cost_batch(obj, w, indices)
            error('Not implemted yet.');
        end

        function g = full_grad(obj, w)
            %g = obj.A' * (obj.A * w - obj.obj.b);
            g = obj.AtA * w - obj.Atb;
        end

        function g = grad(obj, w, indices)
            A_partial = obj.A(:,indices);
            g = A_partial' * (A_partial * w - obj.b);
        end

        % for Subgradient descent
        function subg = full_subgrad(obj, w)
            subg = obj.lambda * sign(w);
        end

        % for Smoothing
        function smooth_g = full_smooth_grad(obj, w)
            smooth_g = zeros(obj.n, 1);

            if strcmp(obj.smooth_type, 'type1') % f_mu(w) = w^2/2mu for abs(w)<= mu, abs(w)-mu/2 for abs(w)> mu.
                for i = 1 : obj.n
                    if abs(w(i)) <= obj.smooth_mu
                        smooth_g(i) = w(i) / obj.smooth_mu;
                    elseif w(i) > obj.smooth_mu
                        smooth_g(i) =1;
                    else
                        smooth_g(i) =-1;
                    end
                end
            elseif strcmp(obj.smooth_type, 'type2') % f_mu(w) = (w^2+mu^2)^(1/2)
                for i = 1 : obj.n
                    smooth_g(i) = w(i)/sqrt(w(i)^2+obj.smooth_mu^2);
                end
            elseif strcmp(obj.smooth_type, 'type3') % f_mu(w) = mu log((exp(-w/mu) + exp(w/mu))/2)
                for i = 1 : obj.n
                    p = exp(-w(i)/obj.smooth_mu);
                    q = exp(w(i)/obj.smooth_mu);
                    smooth_g(i) = (q-p)/(p+q);
                end
                %smooth_g = smooth_g;
            end

            smooth_g = obj.lambda * smooth_g;
        end

        % for Frank-Wolfe, a.k.a. Condtional Gradient
        function [s, idx, sign_flag] = LMO(obj, grad)
            [val, idx] = max( abs(grad) );
            obj.sign_flag = -1 * sign(grad(idx));
            obj.idx = idx;

            s = zeros(size(grad));
            s(idx) = obj.sign_flag * obj.lambda;

            sign_flag = obj.sign_flag;
        end

        function h = hess(obj, w, indices)
            error('Not implemted yet.');
        end

        function h = full_hess(obj, w)
            h = obj.AtA;
        end

        function hv = hess_vec(obj, w, v, indices)
            error('Not implemted yet.');
        end

        function step = exact_line_search(obj, w, dir, idx, sign_flag)
            As = sign_flag * obj.lambda * obj.A(:,idx); % = the i-th column of the dictionary matrix A
            As_minus_Ax = As - obj.A*w;
            step = max(0, min(1, As_minus_Ax' * (obj.b-obj.A*w) / (As_minus_Ax' * As_minus_Ax)));
        end

        function w_opt = calc_solution(obj, method, options_in)

            if nargin < 1
                method = 'ista';
            end

            options.max_epoch = options_in.max_epoch;
            options.w_init = options_in.w_init;
            options.verbose = options_in.verbose;
            options.tol_optgap = 1.0e-24;
            options.tol_gnorm = 1.0e-16;
            options.step_alg = 'backtracking';

            if ~options.verbose
                fprintf('Calclation of solution started ... ');
            end

            if strcmp(method, 'sd')
                [w_opt,~] = sd(obj, options);
            elseif strcmp(method, 'cg')
                [w_opt,~] = ncg(obj, options);
            elseif strcmp(method, 'newton')
                options.sub_mode = 'INEXACT';
                options.step_alg = 'non-backtracking';
                [w_opt,~] = newton(obj, options);
            elseif strcmp(method, 'ista')
                options.step_alg = 'fix';
                options.step_init = 1/obj.L;
                options.sub_mode  = 'FISTA';
                [w_opt,~] = ag(obj, options);
            else
                options.step_alg = 'backtracking';
                options.mem_size = 5;
                [w_opt,~] = lbfgs(obj, options);
            end

            if ~options.verbose
                fprintf('done\n');
            end
        end
    end
end


%RODRIGUES  Converts a rotation matrix to a rotation vector or vice versa
%
%     dst = cv.Rodrigues(src)
%     [dst,jacobian] = cv.Rodrigues(src)
%
% ## Input
% * __src__ Input rotation vector (3x1 or 1x3) or rotation matrix (3x3). Both
%   single and double-precision floating-point types are supported.
%
% ## Output
% * __dst__ Output rotation matrix (3x3) or rotation vector (3x1 or 1x3),
%   respectively. Same data type as `src`.
% * __jacobian__ Optional output Jacobian matrix, 3x9 or 9x3, which is a
%   matrix of partial derivatives of the output array components with respect
%   to the input array components. Same data type as `src`.
%
% The function transforms a rotation matrix in the following way:
%
%     theta <- norm(r)
%     r <- r/theta
%     R = cos(theta) * I + (1 - cos(theta)) * r * r^T + sin(theta) * A
%     A = [0, -rz, ry; rz, 0, -rx; -ry, rx, 0]
%
% Inverse transformation can be also done easily, since
%
%     sin(theta) * A = (R - R^T) / 2
%
% A rotation vector is a convenient and most compact representation of a
% rotation matrix (since any rotation matrix has just 3 degrees of
% freedom). The representation is used in the global 3D geometry
% optimization procedures like cv.calibrateCamera, cv.stereoCalibrate, or
% cv.solvePnP.
%
% See also: cv.calibrateCamera, cv.stereoCalibrate, cv.solvePnP,
%  rotationMatrixToVector, rotationVectorToMatrix
%


function r8mat_is_solution_test ( )

%*****************************************************************************80
%
%% R8MAT_IS_SOLUTION_TEST tests R8MAT_IS_SOLUTION.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 March 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8MAT_IS_SOLUTION_TEST:\n' );
  fprintf ( 1, '  R8MAT_IS_SOLUTION tests whether X is the solution of\n' );
  fprintf ( 1, '  A*X=B by computing the Frobenius norm of the residual.\n' );
%
%  Get random shapes.
%
  i4_lo = 1;
  i4_hi = 10;
  seed = 123456789;
  [ m, seed ] = i4_uniform_ab ( i4_lo, i4_hi, seed );
  [ n, seed ] = i4_uniform_ab ( i4_lo, i4_hi, seed );
  [ k, seed ] = i4_uniform_ab ( i4_lo, i4_hi, seed );
%
%  Get a random A.
%
  r8_lo = -5.0;
  r8_hi = +5.0;
  [ a, seed ] = r8mat_uniform_ab ( m, n, r8_lo, r8_hi, seed );
%
%  Get a random X.
%
  r8_lo = -5.0;
  r8_hi = +5.0;
  [ x, seed ] = r8mat_uniform_ab ( n, k, r8_lo, r8_hi, seed );
%
%  Compute B = A * X.
%
  b = a * x;
%
%  Compute || A*X-B||
%
  enorm = r8mat_is_solution ( m, n, k, a, x, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  A is %d by %d\n', m, n );
  fprintf ( 1, '  X is %d by %d\n', n, k );
  fprintf ( 1, '  B is %d by %d\n', m, k );
  fprintf ( 1, '  Frobenius error in A*X-B is %g\n', enorm );

  return
end


function out = matrix(L, varargin)
%   MATRIX(L, DIM) returns an DIM by (DIM+D) discretization of the linear
%   chebop L, where D is the differential order of L. If L is not linear
%   then an error is thrown.
%
%   If L has assigned boundary conditions then these are included in the
%   discretization, increasing the number of rows. If the domain of L has
%   breakpoints then DIM must be a vector specifying the discretization size
%   on each interval. In this case, appropriate continuity constraints are
%   included in the discretization, further increasing the number of rows
%   (by D times the number of breakpoints).
%
%   MATRIX(L, DIM, PREFS) allows additional preferences to be passed
%   via the CHEBOPPREF PREFS.
%
%   MATRIX(L, DIM, 'oldschool') forces the returned differentiation
%   matrices to be square rather than rectangular. See LINOP/MATRIX and
%   LINOP/FEVAL for further details.
%
%   MATRIX(L, DIM, 'oldschool', PREFS) allows, again, additional
%   preferences to be passed via the CHEBOPPREF PREFS.
%
%   For mathematical details of these matrices see Aurentz and Trefethen,
%   "Block operators and spectral discretizations," SIAM Review, 2017.
%   For the 'oldschool' discretizations, see Trefethen, Spectral Methods
%   in MATLAB, SIAM, 2000.
%
% See also FEVAL, LINOP/MATRIX, LINOP/FEVAL.

% Get the preferences if given:
isPrefGiven = 0;
for j = 1:nargin-1
    item = varargin{j};
    if ( isa(item,'cheboppref') )
        prefs = item;
        isPrefGiven = 1;
    end
end

% Otherwise, use default prefs
if ( ~isPrefGiven )
    prefs = cheboppref();
end

% Linearize:
[L_linop, ~, fail] = linop(L);
if ( fail )
    error('CHEBFUN:CHEBOP:matrix:nonlinear',...
        'Matrix expansion is only allowed for linear CHEBOP objects.')
end

% Determine the discretization:
prefs = determineDiscretization(L, L_linop, prefs);

% Derive continuity conditions in the case of breakpoints:
L_linop = deriveContinuity(L_linop);

% Call LINOP/FEVAL or LINOP/MATRIX:
if ( (numel(varargin) > 1) && strcmpi(varargin{2}, 'oldschool') )
    warnState = warning('off', 'CHEBFUN:LINOP:feval:deprecated');
    out = feval(L_linop, varargin{:});
    warning(warnState);
else
    % Add the preferences in vargarin to pass them to LINOP/MATRIX:
    if ( isPrefGiven )
        % If a CHEBOPPREF was passed to the method, it will have been at the
        % last position of varargin, indexed at nargin-1. Overwrite it with the
        % current PREFS.DISCRETIZATION, as the discretization might have changed
        % in the periodic case:
        varargin{nargin-1} = prefs;
    else
        % Otherwise, add the PREFS.DISCRETIZATION to VARARGIN, so that it can be
        % passed to the call to LINOP/MATRIX below.
        varargin{nargin} = prefs;
    end
    out = matrix(L_linop, varargin{:});
end

end


%calling function to search the optimal number of clusters
close all
clear all
path(path,'..\..\FUZZCLUST')

%the data
load motorcycle.txt
data.X=motorcycle(:,[1 2]);
[N,n]=size(data.X);
%data normalizaiton
data = clust_normalize(data,'range');

%parameters
ncmax=14; %maximal number of cluster
param.m=2;
param.e=1e-3;
%
ment=[];
figure(1)
for cln=2:ncmax
param.c=cln;
    param.ro = ones(1,param.c);
    result=GKclust(data,param);
    clf
    plot(data.X(:,1),data.X(:,2),'b.',result.cluster.v(:,1),result.cluster.v(:,2),'r*');
    hold on
    new.X=data.X;
    clusteval(new,result,param)
    %validation
    result=modvalidity(result,data,param);
    ment{cln}=result.validity;

end

PC=[];CE=[];SC=[];S=[];XB=[];DI=[];ADI=[];

    for i=2:ncmax
       PC=[PC ment{i}.PC];
       CE=[CE ment{i}.CE];
       SC=[SC ment{i}.SC];
       S=[S ment{i}.S];
       XB=[XB ment{i}.XB];
       DI=[DI ment{i}.DI];
       ADI=[ADI ment{i}.ADI];
   end
    figure(2)
    clf
    subplot(2,1,1), plot([2:ncmax],PC)
    title('Partition Coefficient (PC)')
    subplot(2,1,2), plot([2:ncmax],CE,'r')
    title('Classification Entropy (CE)')
    figure(3)
    subplot(3,1,1), plot([2:ncmax],SC,'g')
    title('Partition Index (SC)')
    subplot(3,1,2), plot([2:ncmax],S,'m')
    title('Separation Index (S)')
    subplot(3,1,3), plot([2:ncmax],XB)
    title('Xie and Beni Index (XB)')
    figure(4)
    subplot(2,1,1), plot([2:ncmax],DI)
    title('Dunn Index (DI)')
    subplot(2,1,2), plot([2:ncmax],ADI)
    title('Alternative Dunn Index (ADI)')


%%*********************************************************************
%% norm_min: matrix 2-norm minimization problem
%%
%% (primal) minimize || B0 + x1*B1 + ... + xm*Bm ||_2
%%
%% (dual)   maximize    - Tr B0*Q
%%          subject to  Tr Bk*Q = 0, k=1,...,m
%%                      sum of singular values of Q is less than one.
%%
%% The matrices Bk are of size p x q.
%%
%% The problem is equivalent to the following SDP:
%%
%%    minimize    t
%%                         [0,   Bk]
%%    s.t.  sum_{k=1}^m xk*[       ]
%%                         [Bk^*, 0]
%%                               [0,     i*Bk]          [0,   B0]
%%         + sum_{k=m+1}^{2m} xk*[           ] -t*I <= -[       ]
%%                               [(i*Bk)^*, 0]          [B0^*, 0]
%%
%% Adapted from Vandenberghe and Boyd, SIAM Review 96.
%% see also igmres.m
%%--------------------------------------------------------------------
%%
%%  [blk,Avec,C,b,X0,y0,Z0,objval,x] = norm_min(B,feas,solve);
%%
%%  Input:   B = cell array such that B{k} = B_k.
%%           feas  = 1 if want feasible starting point
%%                 = 0 if otherwise.
%%           solve = 0 just to initialize
%%                 = 1 if want to solve the problem.
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%*********************************************************************

   function [blk,Avec,C,b,X0,y0,Z0,objval,x] = norm_min(B,feas,solve);

   if (nargin < 3); solve = 0; end;
   if (nargin < 2); feas = 0; end;
   if isempty(feas); feas = 0; end
%%
%% dimensions
%%
   m = size(B,2)-1;
   [p,q] = size(B{1}); isrealB = 1;
   for k = 1:m;
       [p1,q1] = size(B{k});
       if any([p q]-[p1 q1]);
          error('size of B_k is not the same for all k');
       end
       isrealB = min(isrealB, isreal(B{k}));
   end
   cmp = 1-isrealB;
%%
%% A{k} = [0 Bk; Bk' 0],  i=1,...,m+1;  A{m+1} = I
%%
   if (~cmp)
       n = p+q;
       blk{1,1} = 's'; blk{1,2} = n;
       b = [zeros(m,1); -1];
       C = -[zeros(p), B{1}; B{1}', zeros(q)];
       A = cell(1,m+1);
       for k = 1:m
           Bk = B{k+1};
           A{k} = [zeros(p), Bk; Bk', zeros(q)];
       end
       A{m+1} = -speye(n,n);
       Avec = svec(blk,A,ones(size(blk,1),1));
   elseif (cmp)
       n = p+q;
       blktmp{1,1} = 's'; blktmp{1,2} = n;
       b = [zeros(2*m,1); -1];
       Ctmp{1} = -[zeros(p),  B{1}; B{1}', zeros(q)];
       A = cell(1,2*m+1); ii = sqrt(-1);
       for k = 1:m
           Bk = B{k+1};
           A{k}   = [zeros(p), Bk; Bk', zeros(q)];
           A{k+m} = [zeros(p), ii*Bk; -ii*Bk', zeros(q)];
       end
       A{2*m+1} = -speye(n,n);
       [blk,Avec,C,b] = convertcmpsdp(blktmp,A,Ctmp,b);
   end
%%
%%
   if (feas == 1);
      if (~cmp)
         X0{1} = eye(n)/n;
      else
         X0{1} = eye(2*n)/(2*n);
      end
      y0 = 1.1*ops(C,'norm')*[zeros(length(b)-1,1); 1];
      Z0 = ops(C,'-',Atyfun(blk,Avec,[],[],y0));
   elseif (feas == 0)
      [X0,y0,Z0] = infeaspt(blk,Avec,C,b);
   end
   if (solve)
      [obj,X,y,Z] = sqlp(blk,Avec,C,b,[],X0,y0,Z0);
      objval = -mean(obj);
      if ~cmp; x = y(1:m);
      else;    x = y(1:m) +sqrt(-1)*y(m+1:2*m);
      end;
   else
      objval = []; x = [];
   end
%%*********************************************************************


%GRADIENTDERICHE  Applies Deriche filter to an image
%
%     dst = cv.GradientDeriche(op, dir)
%     dst = cv.GradientDeriche(op, dir, 'OptionName',optionValue, ...)
%
% ## Input
% * __op__ Source 8-bit or 16-bit image, 1-channel or 3-channel image.
% * __dir__ Filter direction. One of:
%   * __X__
%   * __Y__
%
% ## Output
% * __dst__ result `single` image with same number of channel than `op`.
%
% ## Options
% * __AlphaDerive__ double see paper. default 1.0
% * __AlphaMean__ double see paper. default 1.0
%
% For more details about this implementation, please see [deriche1987].
%
% ## References
% [deriche1987]:
% > Rachid Deriche. "Using Canny's criteria to derive a recursively
% > implemented optimal edge detector". International Journal of Computer
% > Vision, Volume 1 Issue 2, pages 167-187, 1987.
% > http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.476.5736&rep=rep1&type=pdf
%
% See also: imgradientxy
%


function asa007_test02 ( )

%*****************************************************************************80
%
%% TEST02 demonstrates the use of SYMINV.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2008
%
%  Author:
%
%    John Burkardt
%
  n_max = 15;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02:\n' );
  fprintf ( 1, '  SYMINV computes the inverse of a positive\n' );
  fprintf ( 1, '  definite symmetric matrix.\n' );
  fprintf ( 1, '  A compressed storage format is used.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Here we look at the Hilbert matrix\n' );
  fprintf ( 1, '  A(I,J) = 1 / ( I + J - 1 )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We expect errors to grow quickly with N.\n' );

  for n = 1 : n_max
%
%  Set A to the Hilbert matrix.
%
    k = 0;
    for i = 1 : n
      for j = 1 : i
        k = k + 1;
        a(k) = 1.0 / ( i + j - 1 );
      end
    end

    [ c, nullty, ifault ] = syminv ( a, n );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Matrix order N = %d\n', n );
    fprintf ( 1, '  Maxtrix nullity NULLTY = %d\n', nullty );

    k = 0;
    for j = 1 : n
      for i = 1 : j - 1
        k = k + 1;
        cfull(i,j) = c(k);
        cfull(j,i) = c(k);
      end
      k = k + 1;
      cfull(j,j) = c(k);
    end

    k = 0;
    for j = 1 : n
      for i = 1 : j - 1
        k = k + 1;
        afull(i,j) = a(k);
        afull(j,i) = a(k);
      end
      k = k + 1;
      afull(j,j) = a(k);
    end
%
%  Compute C * A - I.
%
    b(1:n,1:n) = cfull(1:n,1:n) * afull(1:n,1:n) - eye(n);

    diff = sqrt ( sum ( sum ( b(1:n,1:n).^2 ) ) );

    fprintf ( 1,  '  RMS ( C * A - I ) = %e\n', diff );
  end

  return
end


function test_approx_test03 ( )

%*****************************************************************************80
%
%% TEST_APPROX_TEST03 uses polynomial interpolation on data vector problems.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 August 2011
%
%  Author:
%
%    John Burkardt
%
  max_tab = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST_APPROX_TEST03\n' );
  fprintf ( 1, '  Polynomial interpolation to a vector of data.\n' );

  prob_num = p00_prob_num ( );

  for prob = 1 : prob_num

    title = p00_title ( prob );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Problem  %d\n', prob );
    fprintf ( 1, '  %s\n', title )

    data_num = p00_data_num ( prob );

    fprintf ( 1, '  DATA_NUM = %d\n', data_num )

    if ( max_tab < data_num )
      fprintf ( 1, '\n' );
      fprintf ( 1, '  This problem has too much data for polynomial interpolation.\n' );
      continue
    end

    [ xdata, ydata ] = p00_dat ( prob, data_num );

    ntab = data_num;

    fprintf ( 1, '\n' );
    fprintf ( 1, '  Interpolating polynomial order = %d\n', ntab );
    fprintf ( 1, '\n' );
%
%  Construct the interpolating polynomial via finite differences.
%
    diftab = data_to_dif ( ntab, xdata, ydata );
%
%  Print out the approximation, including midpoints of the intervals.
%
    for i = 1 : ntab

      if ( i < ntab )
        jhi = 2;
      else
        jhi = 1;
      end

      for j = 1 : jhi

        if ( i < ntab )
          x = ( ( jhi - j + 1 ) * xdata(i)     ...
              + (       j - 1 ) * xdata(i+1) ) ...
              / ( jhi         );
        else
          x = xdata(ntab);
        end

        if ( j == 1 )
          mark = '*';
        else
          mark = ' ';
        end

        yapprox = dif_val ( ntab, xdata, diftab, x );

        fprintf ( 1, '  %c  %14g  %14g\n', mark, x, yapprox );

      end

    end

  end

  return
end


function f=evalContFrac(bFunc,aFunc,tinyVal,maxIter)
%%EVALCONTFRAC Evaluate a continued fraction of the form
%              f=b(1)+a(1)/(b(2)+a(2)/(b(3)+a(3)/(b(4)+a(4)/...
%              Given either matrices for a finite number of a and b terms
%              of function for the a and b values such that the required
%              number of terms can be requested.
%
%INPUTS: bFunc This can either be a function such that bFunc(i) provides
%              b(i) in the continued fraction, or this can be a maxNX1 or
%              1XmaxN vector of explicit b values.
%        aFunc This has to be of the same type as bFunc, either a function
%              handle or a vector to provide the a values. If a vector,
%              then it is 1X(maxN-1) or (maxN-1)X1 as it takes one fewer
%              call to the a terms than the b terms to evaluate the
%              continued fraction.
%      tinyVal An optional parameter that is less than typical values of
%              eps(b), but is not zero. This parameter is used to mitigate
%              divide-by zero errors. The default if this parameter is
%              omitted or an empty matrix is passed if 2e-100.
%      maxIter An optional parameter considering the maximum number of
%              terms to evaluate in the continued fraction. The default
%              value if omitted is 1000. This parameter is only used if
%              aFunc and bFunc are functions, not arrays. If they are
%              arrays, then the maximum number of terms used is determined
%              by the array length.
%
%OUTPUTS: f The value of the continued fraction. The smallest possible
%           value that this can take if b(0)=0 and all a and b are
%           non-negative is eps, due to how the function is initialized to
%           be numerically robust.
%
%The algorithm is the modified Lentz's method. A good description of the
%method, demonstrating its origin is given in Chapter II and Appendix C of
%[1]. The origin of the algorithm is [2]. However, the algorithm is harder
%to understand outside of the context of Bessel functions when consulting
%[2].
%
%An example of a continued fraction is an approximation to pi:
% bFunc=@(i)((i==1)*0+(i~=1)*(2*(i-1)-1));
% aFunc=@(i)(4*(i==1)+(i-1)^2*(i~=1));
% piApprox=evalContFrac(bFunc,aFunc)
%
%REFERENCES:
%[1] C.-Y. Shih, "An investigation of a stable algorithm for the evaluation
%    of fractional order Bessel functions," Master's thesis, Emporia
%    State University, Emporia, KS, Nov. 1993. [Online]. Available:
%    https://esirc.emporia.edu/bitstream/handle/123456789/1773/Shih%201993.pdf
%[2] W. J. Lentz, "Generating Bessel functions in Mie scattering
%    calculations using continued fractions," Applied optics, vol. 15, no.
%    3, pp. 668-671, Mar. 1976.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The floating point precision.
epsVal=eps();

if(nargin<3)
    tinyVal=2^(-100);
end

if(isa(bFunc,'double'))
    maxN=length(bFunc);
    maxIter=maxN-1;
elseif(nargin<4)
    maxIter=1000;
end

b0=bFunc(1);
if(b0==0)
    f=tinyVal;
else
    f=b0;
end

CPrev=f;
DPrev=0;

j=1;
while(j<maxIter)
    bj=bFunc(j+1);
    aj=aFunc(j);

    DCur=bj+aj*DPrev;
    if(DCur==0)
        DCur=tinyVal;
    end
    DCur=1/DCur;
    DPrev=DCur;

    CCur=bj+aj/CPrev;
    if(CCur==0)
        CCur=tinyVal;
    end
    CPrev=CCur;

    Delta=CCur*DCur;
    f=f*Delta;

    if(all(abs(Delta-1)<epsVal))
        break;
    end
    j=j+1;
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 a = wathen ( nx, ny, n )

%*****************************************************************************80
%
%% WATHEN returns the WATHEN matrix.
%
%  Discussion:
%
%    The Wathen matrix is a finite element matrix which is sparse.
%
%    The entries of the matrix depend in part on a physical quantity
%    related to density.  That density is here assigned random values between
%    0 and 100.
%
%    The matrix order N is determined by the input quantities NX and NY,
%    which would usually be the number of elements in the X and Y directions.
%    The value of N is
%
%      N = 3*NX*NY + 2*NX + 2*NY + 1,
%
%    and sufficient storage in A must have been set aside to hold
%    the matrix.
%
%    A is the consistent mass matrix for a regular NX by NY grid
%    of 8 node serendipity elements.
%
%    The local element numbering is
%
%      3--2--1
%      |     |
%      4     8
%      |     |
%      5--6--7
%
%    Here is an illustration for NX = 3, NY = 2:
%
%     23-24-25-26-27-28-29
%      |     |     |     |
%     19    20    21    22
%      |     |     |     |
%     12-13-14-15-16-17-18
%      |     |     |     |
%      8     9    10    11
%      |     |     |     |
%      1--2--3--4--5--6--7
%
%    For this example, the total number of nodes is, as expected,
%
%      N = 3 * 3 * 2 + 2 * 2 + 2 * 3 + 1 = 29
%
%  Properties:
%
%    A is symmetric positive definite for any positive values of the
%    density RHO(X,Y), which is here given the value 1.
%
%    The problem could be reprogrammed so that RHO is nonconstant,
%    but positive.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Nicholas Higham,
%    Algorithm 694: A Collection of Test Matrices in MATLAB,
%    ACM Transactions on Mathematical Software,
%    Volume 17, Number 3, September 1991, pages 289-305.
%
%    Andrew Wathen,
%    Realistic eigenvalue bounds for the Galerkin mass matrix,
%    IMA Journal of Numerical Analysis,
%    Volume 7, Number 4, October 1987, pages 449-457.
%
%  Parameters:
%
%    Input, integer NX, NY, values which determine the size of A.
%
%    Input, integer N, the order of N, as determined by NX and NY.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  em =  [ ...
     6.0, -6.0,  2.0, -8.0,  3.0, -8.0,  2.0, -6.0; ...
    -6.0, 32.0, -6.0, 20.0, -8.0, 16.0, -8.0, 20.0; ...
     2.0, -6.0,  6.0, -6.0,  2.0, -8.0,  3.0, -8.0; ...
    -8.0, 20.0, -6.0, 32.0, -6.0, 20.0, -8.0, 16.0; ...
     3.0, -8.0,  2.0, -6.0,  6.0, -6.0,  2.0, -8.0; ...
    -8.0, 16.0, -8.0, 20.0, -6.0, 32.0, -6.0, 20.0; ...
     2.0, -8.0,  3.0, -8.0,  2.0, -6.0,  6.0, -6.0; ...
    -6.0, 20.0, -8.0, 16.0, -8.0, 20.0, -6.0, 32.0 ];

  for j = 1 : ny

    for i = 1 : nx
%
%  For the element (I,J), determine the indices of the 8 nodes.
%
      node(1) = 3 * j * nx + 2 * j + 2 * i + 1;
      node(2) = node(1) - 1;
      node(3) = node(1) - 2;

      node(4) = ( 3 * j - 1 ) * nx + 2 * j + i - 1;
      node(8) = node(4) + 1;

      node(5) = ( 3 * j - 3 ) * nx + 2 * j + 2 * i - 3;
      node(6) = node(5) + 1;
      node(7) = node(5) + 2;

      rho = 0.5;
%     [ rho, seed ] = r8_uniform_01 ( seed );
      rho = 100.0 * rho;

      for krow = 1 : 8
        for kcol = 1 : 8

          a(node(krow),node(kcol)) = a(node(krow),node(kcol)) ...
            + rho * em(krow,kcol);

        end
      end

    end
  end

  return
end


% MAKE_B creates a weighted symmetric filter matrix encoding the
% neighborhood for isotropic label diffusion.
%
% Usage:
%
% B = make_B(1,4);
% Returns a 3 x 3 symmetric filter matrix encoding a 4-neighborhood
% corresponding to 4 equally spaced points on a 1 point radius of the
% center point.
%
% B = make_B(2,16);
% Returns a 5 x 5 symmetric filter matrix encoding 16 equally spaced points
% on a 2 point radius of the center point.
%
% Inputs:
%
% radius: distance from the center point used to determine filter matrix.
% num_points: number of neighbors used in the filter matrix.
%
% Output:
%
% B: filter matrix (approximately) encoding num_points equally spaced points
% on the speccified radius from the center point.
%
% Copyright (c) Roman Garnett, 2012--2014.

function B = make_B(radius, num_points)

  factor = 100;

  small_length = 2 * radius + 1;
  big_length = factor * small_length;

  big_B = zeros(big_length);

  offset = big_length / 2;
  side = factor / 2;

  for i = 1:num_points
    angle = (i - 1) / num_points * 2 * pi;

    x = round(offset + factor * radius * cos(angle));
    y = round(offset + factor * radius * sin(angle));

    x_lower = max(x - side, 1);
    x_upper = min(x + side, big_length);
    y_lower = max(y - side, 1);
    y_upper = min(y + side, big_length);

    big_B(x_lower:x_upper, y_lower:y_upper) = ...
        big_B(x_lower:x_upper, y_lower:y_upper) + 1;
  end

  B = zeros(small_length, small_length);

  for i = 1:small_length
    for j = 1:small_length
      x_range = (1 + (i - 1) * factor):(i * factor);
      y_range = (1 + (j - 1) * factor):(j * factor);

      B(i, j) = sum(sum(big_B(x_range, y_range)));
    end
  end

  B = B + rot90(B, 1) + rot90(B, 2) + rot90(B, 3);
  B = B / sum(B(:));

  B = round(B * 100) / 100;
end


function plotGamma(as,bs)
% as = [1 1.5 2];
% b = 1;
% bs = b*ones(1,length(as));
% figure;
[styles, colors, symbols] = plotColors;
legendStr = cell(1, length(as));
%styles = {'k:',  'b--', 'r-'};
% textstr={'Iter.=20', 'Iter.=22', 'Iter.=25', 'Iter.=33'};
for i=1:length(as)
    a = as(i); b = bs(i);
    xs = linspace(max(0, a/b - 5* sqrt(a/(b^2))),a/b + 5*sqrt(a/(b^2)), 50);
    model.a = a;
    model.b = b;
    ps = exp(gammaLogprob(model, xs));
    style = [styles{i}, colors(i), symbols(i)];
    plot(xs , ps, style, 'linewidth', 1.5, 'markersize', 1.2);
%     plot(xs , ps, styles{i}, 'color', colors(i), 'linewidth', 1.5);
%     text(a/b,exp(gammaLogprob(model, a/b)), sprintf('a=%2.1f,b=%2.1f', a, b));
    hold on
    legendStr{i} = sprintf('a=%2.1f,\nb=%2.1f', a, b);
    %     axis tight;
end
% legend(legendStr, 'fontsize', 10);
% title('Gamma distributions');
hold off;
end

function [styles, colors, symbols, str] =  plotColors()
% Use plot(x,y,str{i}) to print in i'th style
% We use colors and linestyles, not markers

% This file is from pmtk3.googlecode.com

colors =  ['b' 'r' 'k' 'g' 'c' 'y' 'm' ...
    'r' 'b' 'k' 'g' 'c' 'y' 'm'];
symbols = ['o' 'x' '*' '>' '<' '^' 'v' ...
    '+' 'p' 'h' 's' 'd' 'o' 'x'];
styles = {'-', ':', '-.', '--', '-', ':', '-.', '--', ...
    '-', ':', '-.', '--', '-', ':', '-.', '--'};

for i=1:length(colors)
    %str{i} = sprintf('-%s%s', colors(i), symbols(i));
    str{i} = sprintf('%s%s', colors(i), styles{i});
end

end

function logp = gammaLogprob(arg1, arg2, arg3)
% logp(i) = log p(X(i) | a, b)
% logp = gammaLogprob(model, X); OR logp = gammaLogprob(a, b, X);
% a is the shape,
% b is the rate, i.e. 1/scale

% This file is from pmtk3.googlecode.com

if isstruct(arg1)
    model = arg1;
    X = arg2;
    a = model.a;
    b = model.b;
else
    a = arg1;
    b = arg2;
    X = arg3;
end

logZ = gammaln(a) - a.*log(b);
logp = (a-1).*log(X) - b.*X - logZ;
logp = logp(:);

end


% ======================================================== %
% Files of the Matlab programs included in the book:       %
% Xin-She Yang, Nature-Inspired Metaheuristic Algorithms,  %
% Second Edition, Luniver Press, (2010).   www.luniver.com %
% ======================================================== %

% -------------------------------------------------------- %
% Bat-inspired algorithm for continuous optimization (demo)%
% Programmed by Xin-She Yang @Cambridge University 2010    %
% -------------------------------------------------------- %
% Usage: bat_algorithm([20 1000 0.5 0.5]);                 %

% -------------------------------------------------------------------
% This is a simple demo version only implemented the basic          %
% idea of the bat algorithm without fine-tuning the parameters,     %
% Then, though this demo works very well, it is expected that       %
% this demo is much less efficient than the work reported in        %
% the following papers:                                             %
% (Citation details):                                               %
% 1) Yang X.-S., A new metaheuristic bat-inspired algorithm,        %
%    in: Nature Inspired Cooperative Strategies for Optimization    %
%    (NISCO 2010) (Eds. J. R. Gonzalez et al.), Studies in          %
%    Computational Intelligence, Springer, vol. 284, 65-74 (2010).  %
% 2) Yang X.-S., Nature-Inspired Metaheuristic Algorithms,          %
%    Second Edition, Luniver Presss, Frome, UK. (2010).             %
% 3) Yang X.-S. and Gandomi A. H., Bat algorithm: A novel           %
%    approach for global engineering optimization,                  %
%    Engineering Computations, Vol. 29, No. 5, pp. 464-483 (2012).  %
% -------------------------------------------------------------------

% Main programs starts here
function [best,fmin,N_iter]=bat_algorithm(para)
% Display help
 help bat_algorithm.m

% Default parameters
if nargin<1,  para=[20 1000 0.5 0.5];  end
n=para(1);      % Population size, typically 10 to 40
N_gen=para(2);  % Number of generations
A=para(3);      % Loudness  (constant or decreasing)
r=para(4);      % Pulse rate (constant or decreasing)
% This frequency range determines the scalings
% You should change these values if necessary
Qmin=0;         % Frequency minimum
Qmax=2;         % Frequency maximum
% Iteration parameters
N_iter=0;       % Total number of function evaluations
% Dimension of the search variables
d=10;           % Number of dimensions
% Lower limit/bounds/ a vector
Lb=-2*ones(1,d);
% Upper limit/bounds/ a vector
Ub=2*ones(1,d);
% Initializing arrays
Q=zeros(n,1);   % Frequency
v=zeros(n,d);   % Velocities
% Initialize the population/solutions
for i=1:n,
  Sol(i,:)=Lb+(Ub-Lb).*rand(1,d);
  Fitness(i)=Fun(Sol(i,:));
end
% Find the initial best solution
[fmin,I]=min(Fitness);
best=Sol(I,:);

% ======================================================  %
% Note: As this is a demo, here we did not implement the  %
% reduction of loudness and increase of emission rates.   %
% Interested readers can do some parametric studies       %
% and also implementation various changes of A and r etc  %
% ======================================================  %

% Start the iterations -- Bat Algorithm (essential part)  %
for t=1:N_gen,
% Loop over all bats/solutions
        for i=1:n,
          Q(i)=Qmin+(Qmin-Qmax)*rand;
          v(i,:)=v(i,:)+(Sol(i,:)-best)*Q(i);
          S(i,:)=Sol(i,:)+v(i,:);
          % Apply simple bounds/limits
          Sol(i,:)=simplebounds(Sol(i,:),Lb,Ub);
          % Pulse rate
          if rand>r
          % The factor 0.001 limits the step sizes of random walks
              S(i,:)=best+0.001*randn(1,d);
          end

     % Evaluate new solutions
           Fnew=Fun(S(i,:));
     % Update if the solution improves, or not too loud
           if (Fnew<=Fitness(i)) & (rand<A) ,
                Sol(i,:)=S(i,:);
                Fitness(i)=Fnew;
           end

          % Update the current best solution
          if Fnew<=fmin,
                best=S(i,:);
                fmin=Fnew;
          end
        end
        N_iter=N_iter+n;
end
% Output/display
disp(['Number of evaluations: ',num2str(N_iter)]);
disp(['Best =',num2str(best),' fmin=',num2str(fmin)]);

% Application of simple limits/bounds
function s=simplebounds(s,Lb,Ub)
  % Apply the lower bound vector
  ns_tmp=s;
  I=ns_tmp<Lb;
  ns_tmp(I)=Lb(I);

  % Apply the upper bound vector
  J=ns_tmp>Ub;
  ns_tmp(J)=Ub(J);
  % Update this new move
  s=ns_tmp;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Objective function: your own objective function can be written here
% Note: When you use your own function, please remember to
%       change limits/bounds Lb and Ub (see lines 52 to 55)
%       and the number of dimension d (see line 51).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function z=Fun(u)
% Sphere function with fmin=0 at (0,0,...,0)
z=sum(u.^2);
%%%%% ============ end ====================================


%{
Solves a simple quadratic problem
(with optimal solution known in closed-form)
with all the various solvers

the quadratic problem is:

min_x  c'x + x'Px/2, P > 0, hence the optimal solution is x = -inv(Q)*c

On a simple quadratic example like this, when there is no projection,
all the solver behave similarly. So this script is more useful for debugging
to make sure that all solvers are working

%}

%% Make a problem
N   = 100;
randn('state',2334);
P   = randn(N); P = P*P' + .1*eye(N); % ensure P is symmetric positive definite
c   = randn(N,1);

x_ref   = -P\c;

%% Solve with TFOCS
f   = smooth_quad( P, c );
affine    = [];
projector = [];
x0        = zeros(N,1);
opts      = [];
opts.printEvery     = 500;
opts.tol  = 1e-8;
opts.printStopCrit  = true;
opts.restart    = 350;  % use this since the problem is strongly convex
opts.errFcn     = @(f,x) norm( x-x_ref)/norm(x_ref);
opts.maxIts     = 750;

% The 'N83' and 'AT' methods can take advantage when you know
%   the exact value of the strong convexity parameter
%   (and the Lipschitz constant).  In this case, do not
%   use restart. But we won't test this right now,
%   since it is unusual that you have information
%   about the strong convexity constant.
% opts.restart    = Inf;
% opts.mu         = min(eig(P)); % strong convexity parameter. N83 can make use of this
% opts.Lexact     = max(eig(P));

solverList = { 'GRA', 'AT', 'LLM', 'N07', 'N83', 'TS' };
% solverList = { 'GRA', 'AT', 'N83', 'TS' };
% solverList = { 'LLM','N07'}; % these take two steps per iteration

figure(1); clf;

for k = 1:length( solverList )
    solver  = solverList{k};
    opts.alg    = solver;
    fprintf('\nSolver: %s\n\n', solver );
    [x,out,optsOut] = tfocs( f, affine, projector, x0, opts );
    semilogy( out.err );
    hold all
end
legend( solverList );

%% close all figures
close all

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


%cells(.5*n-1,.5*n-1)=1;
%cells(.5*n-2,.5*n-2)=1;
%cells(.5*n-3,.5*n-3)=1;
cells = (rand(n,n))<.5 ;
%how long for each case to stability or simple oscillators
%build an image and display it
imh = image(cat(3,cells,z,z));
set(imh, 'erasemode', 'none')
axis equal
axis tight
%index definition for cell update
x = 2:n-1;
y = 2:n-1;
%Main event loop
stop= 0; %wait for a quit button push
run = 0; %wait for a draw
freeze = 0; %wait for a freeze
while (stop==0)

 if (run==1)
 %nearest neighbor sum
 sum(x,y) = cells(x,y-1) + cells(x,y+1) + ...
 cells(x-1, y) + cells(x+1,y) + ...
 cells(x-1,y-1) + cells(x-1,y+1) + ...
 cells(3:n,y-1) + cells(x+1,y+1);
 % The CA rule
 cells = (sum==3) | (sum==2 & cells);
 %draw the new image
 set(imh, 'cdata', cat(3,cells,z,z) )
 %update the step number diaplay
 stepnumber = 1 + str2num(get(number,'string'));
 set(number,'string',num2str(stepnumber))
 end

 if (freeze==1)
 run = 0;
 freeze = 0;
 end
 drawnow %need this in the loop for controls to work
end
cells = (rand(n,n))<.5 ;
%how long for each case to stability or simple oscillators
%build an image and display it
imh = image(cat(3,cells,z,z));
set(imh, 'erasemode', 'none')
axis equal
axis tight
%index definition for cell update
x = 2:n-1;
y = 2:n-1;
%Main event loop
stop= 0; %wait for a quit button push
run = 0; %wait for a draw
freeze = 0; %wait for a freeze
while (stop==0)

 if (run==1)
 %nearest neighbor sum
 sum(x,y) = cells(x,y-1) + cells(x,y+1) + ...
 cells(x-1, y) + cells(x+1,y) + ...
 cells(x-1,y-1) + cells(x-1,y+1) + ...
 cells(3:n,y-1) + cells(x+1,y+1)+...
 cells(x,y);
 % The CA rule
 cells = ~((sum<4) | (sum==5));
 %draw the new image
 set(imh, 'cdata', cat(3,cells,z,z) )
 %update the step number diaplay
 stepnumber = 1 + str2num(get(number,'string'));
 set(number,'string',num2str(stepnumber))
 end

 if (freeze==1)
 run = 0;
 freeze = 0;
 end

 drawnow %need this in the loop for controls to work

end


function [bc] = PoissonIPDGbc3D(ubc)

% function [bc] = PoissonIPDGbc3D(ubc)
% Purpose: Set up the discrete Poisson matrix directly
%          using IP. The operator is set up in the weak form

Globals3D;

% face mass matrix 1
Fm = Fmask(:,1); faceR = r(Fm); faceS = s(Fm);
V2D = Vandermonde2D(N, faceR, faceS);
massEdge(Fm,Fm,1) = inv(V2D*V2D');

% face mass matrix 2
Fm = Fmask(:,2); faceR = r(Fm); faceT = t(Fm);
V2D = Vandermonde2D(N, faceR, faceT);
massEdge(Fm,Fm,2) = inv(V2D*V2D');

% face mass matrix 3
Fm = Fmask(:,3); faceS = s(Fm); faceT = t(Fm);
V2D = Vandermonde2D(N, faceS, faceT);
massEdge(Fm,Fm,3) = inv(V2D*V2D');

% face mass matrix 4
Fm = Fmask(:,4); faceS = s(Fm); faceT = t(Fm);
V2D = Vandermonde2D(N, faceS, faceT);
massEdge(Fm,Fm,4) = inv(V2D*V2D');

% build DG right hand side
bc = zeros(Np, K);

for k1=1:K
  if(~mod(k1,1000)) k1, end;

  for f1=1:Nfaces

    if(EToE(k1,f1)==k1)

      Fm1 = Fmask(:,f1);
      fidM  = (k1-1)*Nfp*Nfaces + (f1-1)*Nfp + (1:Nfp)';

      id = 1+(f1-1)*Nfp + (k1-1)*Nfp*Nfaces;
      lnx = nx(id);  lny = ny(id);  lnz = nz(id);
      lsJ = sJ(id); hinv = Fscale(id);

      Dx = rx(1,k1)*Dr + sx(1,k1)*Ds + tx(1,k1)*Dt;
      Dy = ry(1,k1)*Dr + sy(1,k1)*Ds + ty(1,k1)*Dt;
      Dz = rz(1,k1)*Dr + sz(1,k1)*Ds + tz(1,k1)*Dt;
      Dn1 = lnx*Dx + lny*Dy +lnz*Dz;

      mmE = lsJ*massEdge(:,:,f1);

      gtau = 2*(N+1)*(N+1)*hinv; % set penalty scaling

      bc(:,k1) = bc(:,k1) + (gtau*mmE(:,Fm1) - Dn1'*mmE(:,Fm1))*ubc(fidM);

    end
  end
end
return


function [m,Lambda,Cm] = spm_nwrnd (M,N)
% Generate N samples from Normal-Wishart density
% FORMAT [m,Lambda,Cm] = spm_nwrnd (M,N)
%
% Parameters M
%           .a,.B,.beta,.m
% N         number of samples
%
% m         Means
% Lambda    precisions
% Cm        covariances
%
% See J. Bernardo and A. Smith (2000)
% Bayesian Theory, Wiley (page 435)
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_nwrnd.m 6548 2015-09-11 12:39:47Z will $

%iB=inv(M.B);
for n=1:N,
    % Sample precisions
    %L=wishrnd(iB,M.a);
    L=spm_wishrnd(M.B,M.a);
    C=pinv(L);

    if nargout>1
        Lambda(:,:,n)=L;
        Cm(:,:,n)=C;
    end

    % Sample means
    %C=pinv(M.beta*L);
    %m(:,n)=spm_normrnd(M.m,C,1);

    m(:,n)=spm_normrnd(M.m,C/M.beta,1);
end

if N==1
    Lambda=squeeze(Lambda);
    Cm=squeeze(Cm);
end


function D=dist_mat(P1, P2)

n1 = zeros(1,size(P1,1));
n2 = zeros(1,size(P2,1));
for i=1:size(P1,1)
    n1(i) = norm(P1(i,:));
end
for i=1:size(P2,1)
    n2(i) = norm(P2(i,:));
end
n1 = n1.^2;
n2 = n2.^2;

D = -2*P1*P2';
N = repmat(n1',[1,size(P2,1)]) + repmat(n2,[size(P1,1),1]);
D = D+N;

D = sqrt(D);


function [y,zf,u,p]=randfilt(pb,pa,ny,zi)
%RANDFILT Generate filtered gaussian noise without initial transient
%
%  Inputs: pb(1,:)  Numerator polynomial of discrete time filter
%          pa(1,:)  Denominator polynomial of discrete time filter
%          ny       Number of output samples required
%          zi       Optional initial filter state
%
% Outputs: y(ny,1)  Filtered Gaussian noise
%          zf       final filter state (can be used to extend the noise sequence)
%          u        The state covariance matrix, <zf*zf'>, is u*u'
%          p        Is the expected value of y(i)^2
%
% zf and zi are the output and optional input state as defined in filter()
% If zi is not specified, random numbers with the correct covariance will be used.
% output u*u' is the state covariance matrix for filter(). Output p is the
% mean power of the output signal y.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: randfilt.m 5885 2015-03-18 09:35:01Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 2 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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% first normalize the denominator polynomial if necessary

if pa(1)~=1
    pb=pb/pa(1); pa=pa/pa(1);
end

% check to see if we must generate zi

if nargin<4 | nargout>2
    lb=length(pb);
    la=length(pa);

    k=max(la,lb)-1;
    l=la-1;
    ii=k+1-l:k;

    % form controllability matrix

    q=zeros(k,k);
    [z,q(:,1)]=filter(pb,pa,1);
    for i=2:k [z,q(:,i)]=filter(pb,pa,0,q(:,i-1)); end

    % we generate m through the step-down procedure
    s=randn(k,1);
    if l
        m=zeros(l,l);
        g=pa;
        for i=1:l
            g=(g(1)*g(1:end-1)-g(end)*g(end:-1:2))/sqrt((g(1)-g(end))*(g(1)+g(end)));
            m(i,i:l)=g;
        end
        s(ii)=triu(toeplitz(pa(1:l)))*(m\s(ii));
        if nargout>2
            u=q;
            u(:,ii)=q(:,ii)*triu(toeplitz(pa(1:l)))/m;
        end
    else
        if nargout>2
            u=q;
        end
    end
    if nargin < 4
        if k
            zi=q*s;
        else
            zi=[];
        end
    end
end
if nargout>2
    if ~numel(u)
        p=pb(1).^2;
    else
        p=u(1,:)*u(1,:)'+pb(1).^2;
    end
end
if nargin>2 && ny>0
    [y,zf]=filter(pb,pa,randn(ny,1),zi);
else
    zf=zi;
    y=[];
end


function [Pts, varargout] = Glo2Loc( pcData, varargin )
[ Dim, DataLen] = size(pcData);
if Dim ~= 2 && Dim ~= 3
    error('the input pcData must be 2 * N or 3 * N format!');
end
if (nargin - 1) == 2
    R = varargin{1};
    T = varargin{2};
end
if (nargin - 1) == 1
    Pose = varargin{1};
    [R T] = ExtractRT(Pose);
end
Pts = R * ( bsxfun( @minus, pcData, T ) );
if (nargout-1) == 1
    varargout{1} = [inv(R) -R*T];
end
if (nargout-1) == 2
    varargout{1} = inv(R);
    varargout{2} = -R*T;
end
end


function [ d2q ] = gen_DirectDynamics(q,Pcii,Icii,mcii,dq,taw)
%% This function is used to calculate the direct dynamics of the KUKA iiwa 7  R 800
% This function proposes that the robot is mounted with the base in the
% horizontal poistion.

% Arreguments:
%--------------------
% q: is 1x7 vector, joint nagles vector of the manipulator
% dq: is 1x7 vector, joint angular velocity vector
% taw: 1x7 vector, the torques on the joints.
% Pcii: is 3X7 matrix while each column represents the local coordinates
% of the center of mass of each link.
% Icii: is (3x3x7) matrix, each 3x3 matrix of which represnets the
% associated link inertial tensor represented in its local inertial frame
% mcii: is (1x7) vector, each element of which specifies a mass of one of
% the links

% Return value:
%--------------------
% d2q: is 7x1 vector, joint angular acceleration vector

% Copyright: Mohammad SAFEEA, 9th-April-2018

[M]=gen_MassMatrix(q,Pcii,Icii,mcii);
[B]=gen_CoriolisMatrix(q,Pcii,Icii,mcii,dq);
[ G ] = gen_GravityVector(q,Pcii,mcii);
% convert angular velocity/acceleration into column vectors
dq=columnVec(dq);
taw=columnVec(taw);
d2q=M\(taw-B*dq-G);
end

function y=columnVec(x)
if(size(x,2)==1)
    y=x;
else
    y=x';
end
end


function sc = census(I,J)

% CENSUS  Census Correlation coefficient
%   CENSUS(I,J) computes the census score of matrices I and J:
%
%     CENSUS = 1/N * ~xor( I > Io , J > Jo )
%
%   where N  = prod(size(I))
%         Io = I(central pixel)
%         Jo = J(central pixel)
%   and   size(I) = size(J)
%
%   See also PATCHCORR, ZNCC, SSD

% (c) 2005 Joan Sola

if size(I) ~= size(J)
    error ('Matrices must be the same size.')
else
    s = size(I); % patch size
    if any(iseven(s))
        error('Matrix sizes must be odd.')
    else
        c = (s+1)/2; % patch center
        i = I(c(1),c(2)); % central pixel
        j = J(c(1),c(2)); % central pixel

        V = I > i;
        W = J > j;

        SC = ~xor(V,W);
        sc = sum(sum(SC))/numel(I);
    end
end
% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function [logp, yhat, res] = tapas_rs_surprise(r, infStates, ptrans)
% Calculates the log-probability of response speed y (in units of ms^-1) according to the surprise
% model introduced in:
%
% Vossel, S.*, Mathys, C.*, Daunizeau, J., Bauer, M., Driver, J., Friston, K. J., and Stephan, K. E.
% (2013). Spatial Attention, Precision, and Bayesian Inference: A Study of Saccadic Response Speed.
% Cerebral Cortex.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2013 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Transform zetas to their native space
ze1v = exp(ptrans(1));
ze1i = exp(ptrans(2));
ze2  = exp(ptrans(3));
ze3  = exp(ptrans(4));

% Initialize returned log-probabilities, predictions,
% and residuals as NaNs so that NaN is returned for all
% irregualar trials
n = size(infStates,1);
logp = NaN(n,1);
yhat = NaN(n,1);
res  = NaN(n,1);

% Weed irregular trials out from inferred states, responses, and inputs
mu1hat = infStates(:,1,1);
mu1hat(r.irr) = [];

y = r.y(:,1);
y(r.irr) = [];

u = r.u(:,1);
u(r.irr) = [];

% Calculate alpha (i.e., attention)
alpha = 1./(1-log2(mu1hat));

% Calculate predicted response speed
rs = u.*(ze1v + ze2*alpha) + (1-u).*(ze1i + ze2*(1-alpha));

% Calculate log-probabilities for non-irregular trials
% Note: 8*atan(1) == 2*pi (this is used to guard against
% errors resulting from having used pi as a variable).
reg = ~ismember(1:n,r.irr)
logp(reg) = -1/2.*log(8*atan(1).*ze3) -(y-rs).^2./(2.*ze3);
yhat(reg) = rs;
res(reg) = y-rs;

return;


function plotquadtfdist(p, varargin);
%PLOTQUADTFDIST Plot quadratic time-frequency distribution
%   Usage: plotquadtfdist(p);
%
%   'plotquadtfdist(p)' plots the quadratic time-frequency distribution
%   on the time-frequency plane. The quadratic time-frequency distribution
%   should be a square matrix.
%
%   PLOTQUADTFDIST takes the following additional arguments:
%
%     'dynrange',r
%              Limit the dynamical range to r by using a colormap in
%              the interval [chigh-r,chigh], where chigh is the highest
%              value in the plot. The default value of [] means to not
%              limit the dynamical range.
%
%     'db'     Apply 20*log_{10} to the coefficients. This makes
%              it possible to see very weak phenomena, but it might show
%              too much noise. A logarithmic scale is more adapted to
%              perception of sound. This is the default.
%
%     'dbsq'   Apply 10*log_{10} to the coefficients. Same as the
%              'db' option, but assume that the input is already squared.
%
%     'lin'    Show the coefficients on a linear scale. This will
%              display the raw input without any modifications. Only works for
%              real-valued input.
%
%     'linsq'  Show the square of the coefficients on a linear scale.
%
%     'linabs'  Show the absolute value of the coefficients on a linear scale.
%
%     'tc'     Time centring. Move the beginning of the signal to the
%              middle of the plot.
%
%     'clim',clim   Use a colormap ranging from clim(1) to clim(2). These
%                   values are passed to imagesc. See the help on imagesc.
%
%     'image'       Use imagesc to display the plot. This is the default.
%
%     'contour'     Do a contour plot.
%
%     'surf'        Do a surf plot.
%
%     'colorbar'    Display the colorbar. This is the default.
%
%     'nocolorbar'  Do not display the colorbar.
%
%     'display'     Display the figure. This is the default.
%

% AUTHOR: Jordy van Velthoven

complainif_notenoughargs(nargin, 1, 'PLOTQUADTFDIST');

[N1, N2] = size(p);

if N1==N2
 yr = [0, 1-2/N1];
else
 error('%s: The input should be a square matrix.', upper(mfilename));
end;

tfplot(p, 1, yr, varargin{:});


function w = hermite_cubic_spline_quad_rule ( nn, xn )

%*****************************************************************************80
%
%% HERMITE_CUBIC_SPLINE_QUAD_RULE: Hermite cubic spline quadrature rule.
%
%  Discussion:
%
%    The integral is taken over the definition interval [X(1),X(NN)].
%
%    Note that if the intervals are equal in size, then the derivative
%    information in DN has no effect on the integral value,
%    except for the first and last entries.
%
%    The quadrature rule is
%
%      Integral ( XN(1) <= X <= XN(NN) ) F(X) dX is approximately
%
%      Sum ( 1 <= I <= NN ) W(1,I) * F(X(I)) + W(2,I) * F'(X(I))
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2011
%
%  Author:
%
%    John Burkardt.
%
%  Reference:
%
%    Fred Fritsch, Ralph Carlson,
%    Monotone Piecewise Cubic Interpolation,
%    SIAM Journal on Numerical Analysis,
%    Volume 17, Number 2, April 1980, pages 238-246.
%
%  Parameters:
%
%    Input, integer NN, the number of data points.
%
%    Input, real XN(NN), the coordinates of the data points.
%    The entries in XN must be in strictly ascending order.
%
%    Output, real W(2,NN), the quadrature weights for F(1:NN)
%    and DN(1:NN).
%
  w = zeros ( 2, nn );

  w(1,1)      = 0.5 * ( xn(1,2)    - xn(1,1)      );
  w(1,2:nn-1) = 0.5 * ( xn(1,3:nn) - xn(1,1:nn-2) );
  w(1,nn)     = 0.5 * ( xn(1,nn)   - xn(1,nn-1)   );

  w(2,1)      =   ( xn(1,2) - xn(1,1) ).^2 / 12.0;
  w(2,2:nn-1) =   ( xn(1,3:nn) - xn(1,1:nn-2) ) ...
                .* ( xn(1,3:nn) - 2.0 * xn(1,2:nn-1) + xn(1,1:nn-2) ) / 12.0;
  w(2,nn)     = - ( xn(1,nn-1) - xn(1,nn) ).^2 / 12.0;

  return
end


% Driver script for solving the 1D Burgers equations
Globals1D;

% Orer of polymomials used for approximation
N = 6;

% Generate simple mesh
xL = -1.0; xR = 1.0;
[Nv, VX, K, EToV] = MeshGen1D(xL,xR,10);

% Initialize solver and construct grid and metric
StartUp1D;

% Set initial conditions
epsilon = 0.1;
%u = -tanh((x+0.5)/(2*epsilon)) + 1.0;
%u=2*sin(x);
xmid=(x(end)+x(1))/2; x1=x<xmid; x2=x>=xmid; u=1*x1+0.1*x2;

% Solve Problem
FinalTime = 0.15;
[u] = Burgers1D(u,epsilon,xL,xR,FinalTime);


%==========================================================================
%                         copywrite 2011
%           iterative-Maximum ratio combining (i-MRC) detector
%     for spatial Modulation (SM) MIMO transmission technique.
%
% This Matlab code computes the BER of the SM for the case where the
% channel coefficients are modeled as uncorrelated  Rayleigh fading channel
% M is the modulation order of M-QAM schemes.
% Mt = number of transmit antennas.
% Mr = number of receive antennas.
%                This code is written by Rajab Legnain
%==========================================================================
function simulation_of_SM_iMRC()
clear all;   clc;
mode = 1;                    % mode = 1 the channel is     normalized,
                             % mode = 2 the channel is not normalized
global M Mt hmodem bit_T
M = 16;
Mt = 4;
Mr = 4;
bit_SMsym = log2(M*Mt);      % number of bit per spatial modulation sysmbol
Nbits = bit_SMsym*1e4;       % Number of bits to be simulated.

hmodem =   modem.qammod('M',M,  'SymbolOrder', 'Gray','InputType', 'bit');
hdemodem = modem.qamdemod('M', M,'SymbolOrder','Gray','OutputType','bit');
Eac = (mean(hmodem.Constellation .* conj(hmodem.Constellation)));
SNR = 0 : 2 :24;             % signal-to-noise ratio in dB
No= (Eac)*10.^(-SNR/10);     % noise variance

L_SNR=length(SNR);
ber= zeros (L_SNR,1);
bit_R=zeros(Nbits, 1);

%%
bit_T = randi([0 1],Nbits,1);
[mod_T ante]= SpatialMod();    % Spatial Modulation
mod_T = mod_T.';

for ii=1:L_SNR
    for j = 1 : size(mod_T ,2)
        channel = sqrt(.5)*( randn(Mr,Mt,1) + 1i*randn(Mr,Mt,1));
        noise = sqrt(.5)*(randn(Mr , 1) + 1i*randn(Mr , 1))* sqrt(No(ii));
        p = diag(channel'*channel);
        switch mode
            case 1
               y = channel(:,ante(j))*(mod_T(ante(j) ,j)./sqrt(p(ante(j)))) + noise;
               z = (channel'*y)./p.^.5;
            case 2
               y = channel(:,ante(j)) * mod_T(ante(j) ,j) + noise ;
               z = (channel'*y)./p;
        end
        [~,ant_est] = max(abs(z));
        bi_ant = de2bi(ant_est - 1 ,  log2(Mt)  ,'left-msb');
        bi_mod = demodulate(hdemodem, z(ant_est,1) );
        bit_R( (j-1)*bit_SMsym+1 : (j-1)*bit_SMsym+bit_SMsym,1) = [bi_ant.' ; bi_mod];

    end
    [~,ber(ii,1)] = biterr(bit_T(:,1),bit_R(:));
end

semilogy(SNR,ber(:,1),'color',[0,0.75,0.75],'linestyle','-','LineWidth',1);
grid on;
xlabel('$$SNR$$','Interpreter','latex')
ylabel('BER','Interpreter','latex')
title('Bit Error Rate of SM using i-MRC detector')
legend([num2str(M),'-QAM'] ,  'Location','NorthEast')
end

function [signal ant_no]= SpatialMod()
global M Mt hmodem bit_T
Nt    = log2(Mt);
Nobit = log2(M);
x = reshape(bit_T,Nobit+Nt,[]);

ant_no =  bi2de([x(1:Nt,:)].' , 'left-msb') + 1;
digMod = modulate(hmodem,x(Nt+1:end,:));

signal = zeros(Mt , length(digMod));

for i = 1 : length(digMod)
    signal( ant_no(i) , i) = digMod(i);
end

signal = signal.';
end


function prob_test030 ( )

%*****************************************************************************80
%
%% TEST030 tests CHI_CDF, CHI_CDF_INV, CHI_PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST030\n' );
  fprintf ( 1, '  For the Chi PDF:\n' );
  fprintf ( 1, '  CHI_CDF evaluates the CDF.\n' );
  fprintf ( 1, '  CHI_CDF_INV inverts the CDF.\n' );
  fprintf ( 1, '  CHI_PDF evaluates the PDF.\n' );

  a = 1.0;
  b = 2.0;
  c = 3.0;

  check = chi_check ( a, b, c );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST030 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A = %14f\n', a );
  fprintf ( 1, '  PDF parameter B = %14f\n', b );
  fprintf ( 1, '  PDF parameter C = %14f\n', c );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '       X            PDF           CDF            CDF_INV\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10

    [ x, seed  ] = chi_sample ( a, b, c, seed );

    pdf = chi_pdf ( x, a, b, c );

    cdf = chi_cdf ( x, a, b, c );

    x2 = chi_cdf_inv ( cdf, a, b, c );

    fprintf ( 1, ' %14f  %14f  %14f  %14f\n', x, pdf, cdf, x2 );

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) Parabolic. with friction.
%
% Help: Function prototype
% [y, t] = runge_kutta(f, y0, [t0 tfinal], timestep)
% where f is the function being integrated as dy/dt = f(t, y).
% y0 are the initial conditions
% t0: initial time
% tfinal: final time.
% h: time step for the calculations.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function parabolic_with_friction()
global g k m
close all;
g = 9.81; %m/s^ 2. gravity at equator
% datos de ajustados a 1000 m de https://en.wikipedia.org/wiki/Ballistic_table#/media/File:Ballistic_table_for_7.62x51_mm_NATO_(mil_and_moa).png
% 7.62x51 mm NATO ammunition
theta0 = 0; %initial angle
k = 0.0000005; % N*s/m friction coefficient
m=9.7/1000; %mass of the bullet kg
v=860; % m/s initial speed

theta0 = 0; %initial angle
k = 0.0000005; % N*s/m friction coefficient
m=0.47; %mass of the bullet kg
v=609; % m/s initial speed
 % 914

t0 = 0;
tfinal = 2.2;
x0 = 0;
y0 = 0;
vx0 = v*cos(theta0);
vy0 = v*sin(theta0);

[t, x] = runge_kutta(@parabolic_friction_square, [x0 y0 vx0 vy0]', [t0 tfinal], 0.01);
x = x(:, 1:length(t));
plot(t, x(1,:), 'r'), hold
plot(t, x(2,:), 'g')
plot(t, x(3,:), 'b')
plot(t, x(4,:), 'c')
legend('Position X (m) RK4', 'Position Y (m) RK4', 'Speed X (m/s) RK4', 'Speed Y (m/s) RK4')

figure,
plot(x(1,:), x(2,:), 'r')
xlabel('X Position (m)')
ylabel('Y Position (m)')

x(2,end)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a constant acceleration on x axis on a ramp.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = parabolic_friction_square(t, x)
global k g m
% We must return the solution of
% [dx1/dt; dx2/dt; dx3/dt dx4/dt]
vx = x(3);
vy = x(4);

%forces in both axes
sq = sqrt(vx^2+vy^2);
xd(1) = vx;
xd(2) = vy;
xd(3) = -(k/m)*vx*sq;
xd(4) = -(k/m)*vy*sq - g;
xd = xd(:);


function colorposneg
% colorposneg
% Use colormap which shows negative values green & blue, positive values yellow & red, zero white
caxis('auto');
C = caxis;
C2 = max(C(2),0);
C1 = min(C(1),0);
R = max(-C1,C2);
caxis([-R,R])
q = (C2-C1)/(2*R);
colormap(posneg(round(100/q)))

%%%%%%%%
function J = posneg(m)
%     colormap similar to jet, but with white instead of green in the center
if nargin < 1
   m = size(get(gcf,'colormap'),1);
end
x = linspace(-2.5,2.5,m)';        % consider x in [-2.5,2.5]
f = @(x) max(0,min(1,2-abs(x)));  % function for green
J = [f(x-1),f(x),f(x+1)];


function err = getL2error3NE1(node,elem,exactE,Eh,markedElem)
%% GETL2ERROR3NE1 L2 norm of the linear Nedelec edge element.
%
%  err = getL2error3NE(node,elem,exactE,Eh,markedElem);
%
% Example
%
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     maxIt = 4;
%     L2Err = zeros(maxIt,1);
%     N = zeros(maxIt,1);
%     for k = 1:maxIt
%         [node,elem] = uniformbisect3(node,elem);
%         [elem2dof,edge] = dof3edge(elem);
%         pde = Maxwelldata2;
%         uI = edgeinterpolate1(pde.exactu,node,edge);
%         L2Err(k) = getL2error3NE1(node,elem,pde.exactu,uI);
%         N(k) = length(uI);
%     end
%     r = showrate(N,L2Err,1,'b-+');
%     legend('||u-u_I||',['N^{' num2str(r) '}'],'LOCATION','Best');
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Sort elem to ascend ordering
elem = sort(elem,2);

%% Construct Data Structure
elem2dof = dof3edge(elem);
NT = size(elem,1); Ndof = max(elem2dof(:)); %N = size(node,1);
[Dlambda,volume] = gradbasis3(node,elem);
locEdge = [1 2; 1 3; 1 4; 2 3; 2 4; 3 4];

%% Compute square of the L2 error element-wise
[lambda,w] = quadpts3(3); % quadrature order is 3
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    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),:);
    Ep = exactE(pxy);
    Ehp = zeros(NT,3);
    for k = 1:6 % for each basis
        i = locEdge(k,1); j = locEdge(k,2);
        % phi_k = lambda_iDlambda_j - lambda_jDlambda_i;
        Ehp = Ehp + repmat(Eh(elem2dof(:,k)),1,3).*...
                   (lambda(p,i)*Dlambda(:,:,j)-lambda(p,j)*Dlambda(:,:,i));
        % psi_k = lambda_iDlambda_j + lambda_jDlambda_i;
        Ehp = Ehp + repmat(Eh(elem2dof(:,k)+Ndof),1,3).*...
                   (lambda(p,i)*Dlambda(:,:,j)+lambda(p,j)*Dlambda(:,:,i));
    end
    err = err + w(p)*sum((Ep - Ehp).^2,2);
end
err = err.*volume;
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % L2 err on some marked region
end
err = sqrt(sum(err));


function [M,indi,indj] = randblock(M,S)
% RANDBLOCK - randomize blocks of a matrix
%    R = RANDBLOCK(M,S) randomizes the matrix M by dividing M into
%    non-overlapping blocks of the size specified by S, and shuffling these
%    blocks. M can be a N-D matrix.
%    The number of elements in S should match the number of dimensions of
%    M, or S can be a scalar specifying a S-by-S-by-S-by ... block size.
%    S should contain positive integers. TThe size of M in any dimension
%    should be an integer number of times the specified size of the block in
%    that dimension (e.g., if size(M,1) equals 6, S(dim) can be 1,2,3, or 6).
%
%    [R,I,J] = RANDBLOCK(...) also returns indices I and J, so that R equals A(I)
%    and R(J) equals A.
%
%    M can be a numerical or cell array.
%
%    Examples:
%      % Shuffle blocks of 3 elements of a 15-element vector
%        M = 1:15 ;
%        randblock(M,3)
%
%      % Scramble a 2D matrix
%        M = reshape(1:24,6,[]) ;
%        randblock(M,[3 2])   % randomize the position of the four 3-by-2 blocks
%        randblock(M,2)       % randomize the position of the six 2-by-2 blocks
%
%      % Scramble a 3D volume
%        M = reshape(1:64,[4 8 2]) ;
%        randblock(M,[2 4 2]) % randomize the position of the four
%                             % 2-by-4-by-2 sub-volumes
%
%      % Scramble a cell matrix
%        M = {'1','a','bb','1c' ; '2a',[3 4 5],'2c','2dd'} ;
%        randblock(M,[2 2])   % randomize the position of the four 2-by-2 blocks
%
%      % Scramble a RGB image Z, and retrieve the original using the
%      % indices
%        Z = peaks(200) ; Z = cat(3,Z,flipud(Z), -(circshift(Z.',[100,100]))) ;
%        Z = (Z - min(Z(:))) ; Z = Z./ max(Z(:)) ;
%        [Z2,I,J] = randblock(Z,[25,25,size(Z,3)]) ; % Scramble 25-by-25 blocks
%        subplot(2,2,1) ; image(Z) ; title('Original image') ;
%        subplot(2,2,2) ; image(Z2) ; title('25x25 scrambled image') ;
%        subplot(2,2,3) ; image(Z2(J)) ; title('Z2(J) = original image') ;
%        subplot(2,2,4) ; image(Z(I)) ; title('Z(I) = scrambled image') ;
%        set (gcf,'Name','[Z2,I,J] = randblock(Z,[25,25,size(Z,3)])') ;
%
%  See also RAND, RANDPERM, MAT2CELL
%       and RANDSWAP, SHAKE on the File Exchange

% for Matlab R13 and up (works on R2010b)
% version 2.3 (mar 2011)
% (c) Jos van der Geest
% email: jos@jasen.nl

% History:
% Created: nov 2007
% 2.0 (dec 2007) implemented cell matrix capabilities
% 2.1 (dec 2007) added several examples for the File Exchange
% 2.2 (dec 2007) modified image example, remove minor m-lint messages
% 2.3 (mar 2011) fixed a small error in the examples; tested in 2010b

% check if the size of the blocks is specified for each dimension of M
NdimM = ndims(M) ;
sizM = size(M) ;  % size of the matrix
nS = numel(S) ;

if nS==1,
    S = repmat(S,1,NdimM) ; % scalar expansion
elseif nS ~= NdimM,
    error('Number of elements of S should equal the number of dimensions of M.') ;
end

% S should contain positive integers only
if ~isnumeric(S) || any(S ~= fix(S)) || any(S<1),
    error('S should be a numeric array with positive integer values.') ;
end

% vectors are 2D and S can be a scalar in this case:
% the singleton dimension of M should be taken care of.
if NdimM==2 && any(sizM==1) && nS==1 && all(S~=1),
    S(sizM==1) = 1 ;
end

if all(S==1),
    % Each element of M is a single block. We can just randomize the linear
    % indices.
    indj = randperm(numel(M)) ;
else

    nb = sizM ./ S ;  % how many blocks in each dimension, this should be an integer
    B = cell(NdimM,1) ;
    % for each dimension of M:
    for i=1:NdimM,
        % check if the number of blocks is an integer
        if nb(i) ~= fix(nb(i)),
            error(['Size mismatch: the size of the matrix M (= %d) in dimension %d\n' ...
                    'is not an integer number of times the specified blocksize (= %d).'],...
                sizM(i),i,S(i)) ;
        else
            % expand the size in that dimension. B is used for mat2cell
            B{i} = repmat(S(i),1,nb(i)) ;
        end
    end

    % M can be a numerical or cell array. By randomizing the (linear)
    % indices, we do not have to worry about the actual contents of M.
    indj = reshape(1:numel(M),sizM) ;
    % convert to a cell array. Each cell contains a block.
    C = mat2cell(indj,B{:}) ;
    % randomize the order of the cells
    C(randperm(numel(C))) = C ;
    % convert back from cell array
    indj = cell2mat(C) ;
end

% use these randomized block indices to shuffle M
M(indj) = M ;

if nargout>1,
    indi(indj) = 1:numel(indj) ;
    indi = reshape(indi,sizM) ;
    indj = reshape(indj,sizM) ;
end


function cheby_t_poly_test ( )

%*****************************************************************************80
%
%% CHEBY_T_POLY_TEST tests CHEBY_T_POLY.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 March 2012
%
%  Author:
%
%    John Burkardt
%
  n_max = 12;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CHEBY_T_POLY_TEST:\n' );
  fprintf ( 1, '  CHEBY_T_POLY evaluates the Chebyshev T polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N      X        Exact F       T(N)(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, x, fx ] = cheby_t_poly_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = cheby_t_poly ( 1, n, x );

    fprintf ( 1, '  %6d  %8f  %12f  %12f\n', n, x, fx, fx2(n+1) );

  end

  return
end


% function x = cwt_iw(Wx, type, opt)
%
% The inverse wavelet transform of signal Wx
%
% Implements Eq. (4.67) of [1].
%
% 1. Mallat, S., Wavelet Tour of Signal Processing 3rd ed.
%
% Inputs:
%  Wx: wavelet transform of a signal, see help cwt_fw
%  type: wavelet used to take the wavelet transform,
%        see help cwt_fw and help wfiltfn
%  opt: options structure used for forward wavelet transform.
%
% Output:
%  x: the signal, as reconstructed from Wx
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function x = cwt_iw(Wx, type, opt)
    if nargin<3, opt = struct(); end

    [na, n] = size(Wx);

    [N,n1,n2] = p2up(n);
    Wxp = zeros(na, N);

    % TODO - do we want to pad the wavelet representation here?  Or not
    % cut off Wx in cwt_fw and then use that to reconstruct?
    Wxp(:, n1+1:n1+n) = Wx;
    Wx = Wxp; clear Wxp;

    % Following the same value in cwt_fw
    noct = log2(N)-1;
    nv = na/noct;
    as = 2^(1/nv) .^ (1:1:na);

    assert(mod(noct,1) == 0);
    assert(nv>0 && mod(nv,1)==0); % integer

    % Find the admissibility coefficient Cpsi
    Cpsi = cwt_adm(type, opt);

    x = zeros(1, N);
    for ai=1:na
        a = as(ai);
        Wxa = Wx(ai, :);

        psih = wfilth(type, N, a, opt);

        % Convolution theorem here
        Wxah = fft(Wxa);
        xah = Wxah .* psih;
        xa = ifftshift(ifft(xah));

        x = x + xa/a;
    end

     % Take real part and normalize by log_e(a)/Cpsi
     x = log(2^(1/nv))/Cpsi * real(x);

     % Keep the unpadded part
     x = x(n1+1: n1+n);

end % cwt_iw


function fd1d_burgers_lax ( )

%*****************************************************************************80
%
%% FD1D_BURGERS_LAX solves the nonviscous Burgers equation using Lax-Wendroff.
%
%  Discussion:
%
%    The non-viscous time-dependent Burgers equation is:
%
%      du/dt + u du/dx = 0
%
%    which can be written in conservative form as
%
%      du/dt + 1/2 d/dx ( u^2 ) = 0
%
%    or
%
%      du/dt + dF/dx = 0
%
%    For the Burgers equation, we define
%
%      F(x,t) = 1/2 u^2,
%      A(x,t) = dF/dx = u
%
%    and then the Lax-Wendroff method approximates the solution
%    using the iteration:
%
%      u(x,t+dt) = u(t) - dt dF/dx + 1/2 dt^2 d/dx A dF/dx
%
%    which can be written:
%
%      u(x,t+dt) = u(x,t) - dt ( F(x+dx,t) - F(x-dx,t) ) / ( 2 * dx )
%        + 1/2 dt^2/dx^2 ( A(x+dx/2,t) * ( F(x+dx,t) - F(x,t) )
%                        - A(x-dx/2,t) * ( F(x,t) - F(x-dx,t) )
%
%    where we approximate:
%
%      A(x+dx/2,t) = 1/2 ( u(x+dx,t) + u(x,t) )
%      A(x-dx/2,t) = 1/2 ( u(x,t) + u(x-dx,t) )
%
%    There is a stability condition that applies here, which requires that
%
%      dt * max ( abs ( u ) ) / dx <= 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    None
%
  timestamp ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_BURGERS_LAX:\n' );
  fprintf ( 1, '  MATLAB version\n' );
  fprintf ( 1, '  Solve the non-viscous time-dependent Burgers equation,\n' );
  fprintf ( 1, '  using the Lax-Wendroff method.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Equation to be solved:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    du/dt + u * du/dx = 0\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  for x in [ a, b ], for t in [t_init, t_last]\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  with initial conditions:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    u(x,o) = u_init\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  and boundary conditions:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '    u(a,t) = u_a(t), u(b,t) = u_b(t)\n' );
%
%  Set and report the problem parameters.
%
  n = 41;
  a = -1.0;
  b = +1.0;
  dx = ( b - a ) / ( n - 1 );
  step_num = 80;
  t_init = 0.0;
  t_last = 1.0;
  dt = ( t_last - t_init ) / step_num;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  %f <= X <= %f\n', a, b );
  fprintf ( 1, '  Number of nodes = %d\n', n );
  fprintf ( 1, '  DX = %f\n', dx );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %f <= t <= %f\n', t_init, t_last );
  fprintf ( 1, '  Number of time steps = %d\n', step_num );
  fprintf ( 1, '  DT = %f\n', dt );

  x = r8vec_even ( n, a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  X:\n' );
  fprintf ( 1, '\n' );
  for ilo = 1 : 5 : n
    ihi = min ( ilo + 4, n );
    for i = ilo : ihi
      fprintf ( 1, '  %16f', x(i,1) );
    end
    fprintf ( 1, '\n' );
  end
%
%  Set the initial condition,
%  and apply boundary conditions to first and last entries.
%
  step = 0;
  t = t_init;
  un(1:n,1) = u_init ( n, x, t );
  un(1,1) = u_a ( x(1,1), t );
  un(n,1) = u_b ( x(n,1), t );

  stability = ( dt / dx ) * max ( abs ( un(1:n,1) ) );
  report ( step, step_num, n, x, t, un, stability );

  if ( 1 )
    plot ( x, un );
    title ( sprintf ( 'Step %d, Time %f', step, t ) );
    pause
  end
%
%  March in time.
%
  c1 = - ( 0.5 * dt   / dx );
  c2 = - ( 0.5 * dt^2 / dx^2 );

  for step = 1 : step_num

    t = ( ( step_num - step ) * t_init   ...
        + (            step ) * t_last ) ...
        / ( step_num        );

    uo(1:n,1) = un(1:n,1);

    un(2:n-1,1) = uo(2:n-1,1) ...
      -       ( dt    / dx  ) * ( uo(3:n,1).^2 - uo(1:n-2,1).^2 ) ...
      + 0.5 * ( dt^2 / dx^2 ) * (  0.5 * ( uo(3:n,1)   + uo(2:n-1,1) ) ...
                                   .* ( uo(3:n,1).^2   - uo(2:n-1,1).^2 ) ...
                                 - 0.5 * ( uo(2:n-1,1) + uo(1:n-2,1) ) ...
                                   .* ( uo(2:n-1,1).^2 - uo(1:n-2,1).^2 ) );

    un(1,1) = u_a ( x(1,1), t );
    un(n,1) = u_b ( x(n,1), t );

    stability = ( dt / dx ) * max ( abs ( un(1:n,1) ) );
    report ( step, step_num, n, x, t, un, stability );

    if ( 1 )
      plot ( x, un );
      title ( sprintf ( 'Step %d, Time %f', step, t ) );
      pause
    end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FD1D_BURGERS_LAX:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function a = r8vec_even ( n, alo, ahi )

%*****************************************************************************80
%
%% R8VEC_EVEN returns N real values, evenly spaced between ALO and AHI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of values.
%
%    Input, real ALO, AHI, the low and high values.
%
%    Output, real A(N), N evenly spaced values.
%    Normally, A(1) = ALO and A(N) = AHI.
%    However, if N = 1, then A(1) = 0.5*(ALO+AHI).
%
  if ( n == 1 )

    a(1,1) = 0.5 * ( alo + ahi );

  else

    a(1:n,1) = ( (n-1:-1:0) * alo + (0:n-1) * ahi ) / ( n - 1 );

  end

  return
end
function report ( step, step_num, n, x, t, u, stability )

%*****************************************************************************80
%
%% REPORT prints or plots or saves the data at the current time step.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer STEP, the index of the current step,
%    between 0 and STEP_NUM.
%
%    Input, integer STEP_NUM, the number of steps to take.
%
%    Input, integer N, the number of nodes.
%
%    Input, real X(N), the coordinates of the nodes.
%
%    Input, real T, the current time.
%
%    Input, real U(N), the initial values U(X,T).
%
%    Input, real STABILITY, the stability factor, which should be
%    no greater than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  STEP = %d\n', step );
  fprintf ( 1, '  TIME = %f\n', t );
  fprintf ( 1, '  STABILTY = %f\n', stability )
  fprintf ( 1, '\n' );
  for ilo = 1 : 5 : n
    ihi = min ( ilo + 4, n );
    for i = ilo : ihi
      fprintf ( 1, '  %14g', u(i) );
    end
    fprintf ( 1, '\n' );
  end

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end
function ua = u_a ( x, t )

%*****************************************************************************80
%
%% U_A sets the boundary condition for U at A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, T, the position and time.
%
%    Output, real UA, the prescribed value of U(X,T).
%
  ua = + 0.5;

  return
end
function ub = u_b ( x, t )

%*****************************************************************************80
%
%% U_B sets the boundary condition for U at B.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, T, the position and time.
%
%    Output, real UB, the prescribed value of U(X,T).
%
  ub = - 0.5;

  return
end
function u = u_init ( n, x, t )

%*****************************************************************************80
%
%% U_INIT sets the initial condition for U.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 August 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of nodes.
%
%    Input, real X(N), the coordinates of the nodes.
%
%    Input, real T, the current time.
%
%    Output, real U(N), the initial values U(X,T).
%
  ua = u_a ( x(1,1), t );
  ub = u_b ( x(n,1), t );

  q = 2.0 * ( ua - ub ) / pi;
  r = ( ua + ub ) / 2.0;
%
%  S can be varied.  It is the slope of the initial condition at the midpoint.
%
  s = 1.0;
  u(1:n,1) = ( 2 * x(1:n,1) - x(n,1) - x(1,1) ) ...
           / (                x(n,1) - x(1,1) );

  u(1:n,1) = - q * atan ( s * u(1:n,1) ) + r;

  return
end


function [Y, spectrum] = slgembed(G, Gc, d, fm, varargin)
%SLGEMBED Solves the general graph-based embedding
%
% $ Syntax $
%   - Y = slgembed(G, Gc, d, fm, ...)
%   - [Y, spectrum] = slgembed(G, Gc, d, fm, ...)
%
% $ Arguments $
%   - G:        The graph to be optimized
%   - Gc:       The constraint graph
%   - d:        The dimension of the embedding
%   - fm:       The type of formulation
%   - Y:        The embedding sample coordinates
%
% $ Description $
%   - Y = slgembed(G, Gc, d, fm, ...) solves the general graph-based
%     embedding of dimension d. In mathematics, it is to solve the
%     following optimization problem:
%           min/max y^T M y,  s.t. y^T C y = I
%     Based on different fm, the formulations of M and C are different:
%     fm is a cell array of two char string elements:
%           fm = {fg, fc}
%     fg indicates the formulation of M, fc indicates the formulation of
%     the constraint matrix C.
%       fg has the following different values:
%           - 'minW':   minimization using M = W, (W is the adjmat of G)
%           - 'maxW':   maximization using M = W
%           - 'minL':   minimization using M = L = D - W
%           - 'maxL':   maximization using M = L = D - W
%       fc has the following different values:
%           - 'I':      use C = I, that is y^T * y = I
%           - 'D':      use C = D, that is y^T * D * y = I (based on G)
%           - 'WC':      use C = W, adjacency matrix (based on Gc)
%           - 'LC':      use C = L = D - W, (based on Gc)
%     For example, if you specify fm = {'maxW', 'D'}, then the function
%     will solve the following optimization problem:
%           maximize y^T W y,   s.t. y^T * D * y = I
%     You can also specify the following properties:
%       - 'inv':        The parameters do to eigenvalue inverse
%                       {method, ...}
%                       (refer to slinvevals for details)
%                       This parameter take effects only when fc = 'WC" or
%                       fc = 'LC'.
%       - 'skip':       How many eigen-components to skip. default = 0.
%                       (In some algorithms, it is necessary to skip
%                        the first or first several eigen-components).
%
%   - [Y, spectrum] = slgembed(G, Gc, d, fm, ...) additionally return
%     the spectrum of the embedding, the eigenvalues of the whitened
%     C^(-1/2) M C^(-1/2).
%
% $ Remarks $
%   - The fc can be 'WC' or 'LC' only when Gc is full matrix, in current
%     version of implementation.
%
% $ History $
%   - Created by Dahua Lin, on Sep 12, 2006
%

%% parse and verify input arguments

if nargin < 4
    raise_lackinput('slgembed', 4);
end

gi = slgraphinfo(G, {'square'});
n = gi.n;
W = sladjmat(G, 'valtype', 'numeric', 'sparse', issparse(G));

[fg, fc] = deal(fm{:});

Wc = [];
if strcmp(fc, 'WC') || strcmp(fc, 'LC')
    if ~isempty(Gc)
        if ~isnumeric(Gc) || issparse(Gc) || ~isequal(size(Gc), [n n])
            error('sltoolbox:invalidarg', ...
                'In current implementation, Gc must be an n x n full numeric matrix for fc = WC or GC');
        end
        Wc = Gc;
    else
        error('sltoolbox:invalidarg', ...
            'When fc is WC or LC, Wc should be specified');
    end
end

opts.inv = {};
opts.skip = 0;
opts = slparseprops(opts, varargin{:});

if d + opts.skip > n
    error('sltoolbox:invalidarg', ...
        'The embedding dimension d plus the skip dimension should not exceed n');
end

%% main skeleton

W = W + W';
if ~isempty(Wc)
    Wc = Wc + Wc';
end

% parse formulation and decide scheme

switch fg
    case 'minW'
        Mfunc = @make_Wmat;
        optype = 'max';
    case 'maxW'
        Mfunc = @make_Wmat;
        optype = 'max';
    case 'minL'
        Mfunc = @make_Lmat;
        optype = 'min';
    case 'maxL'
        Mfunc = @make_Lmat;
        optype = 'max';
    otherwise
        error('sltoolbox:invalidarg', 'Invalid fg type: %s', fg);
end

switch fc
    case 'I'
        whfunc = [];
    case 'D'
        whfunc = @(M) whM_by_D(M, W);
        nyfunc = @(Y, T) nY_by_S(Y, T);
    case 'WC'
        whfunc = @(M) whM_by_W(M, Wc, opts.inv);
        nyfunc = @(Y, T) nY_by_T(Y, T);
    case 'LC'
        whfunc = @(M) whM_by_L(M, Wc, opts.inv);
        nyfunc = @(Y, T) nY_by_T(Y, T);
    otherwise
        error('sltoolbox:invalidarg', 'Invalid fc type: %s', fc);
end

% make M
M = Mfunc(W);

% whiten M
if ~isempty(whfunc)
    MTcell = whfunc(M);
    M = MTcell{1};
    T = MTcell{2};
    clear MTcell;
end

% optimize to solve embedding
[Y, spectrum] = optim_embed(M, optype, d, opts.skip);

% normalize the embedding to satisfy the constraint
if ~isempty(whfunc)
    Y = nyfunc(Y, T);
end
Y = Y';

%% Core routines

function M = make_Wmat(W)

M = W;

function vD = make_Dvec(W)

vD = sum(W, 1)';
if issparse(vD)
    vD = full(vD);
end

function L = make_Lmat(W)

vD = make_Dvec(W);
n = size(W, 1);
if issparse(W)
    D = sparse((1:n)', (1:n)', vD, n, n, n);
    L = D - W;
else
    L = -W;
    dinds = (1:n)'*(n+1)-n;
    L(dinds) = L(dinds) + vD;
end

function MTcell = whM_by_D(M, W)

vD = make_Dvec(W);
vD(vD < eps) = eps;
cv = 1 ./ sqrt(vD);

n = size(M, 1);
if issparse(M)
    Mw = M;
    for i = 1 : n
        Mw(:,i) = Mw(:,i) * cv(i);
    end
    for i = 1 : n
        Mw(i,:) = Mw(i,:) * cv(i);
    end
else
    Mw = slmulrowcols(M, cv', cv);
end

MTcell = {Mw, cv};

function MTcell = whM_by_W(M, W, invparams)

T = slwhiten_from_cov(W, invparams{:});
Mw = T' * M * T;
MTcell = {Mw, T};

function MTcell = whM_by_L(M, W, invparams)

L = make_Lmat(W);
MTcell = whM_by_W(M, L, invparams);

function [Y, spectrum] = optim_embed(M, optype, d, dskip)

switch optype
    case 'min'
        ord = 'ascend';
    case 'max'
        ord = 'descend';
end

d0 = d + dskip;
[spectrum, Y] = slsymeig(M, d0, ord);
if dskip > 0
    spectrum = spectrum(dskip+1:d0);
    Y = Y(:, dskip+1:d0);
end

function Y = nY_by_S(Y, T)

Y = slmulvec(Y, T, 1);

function Y = nY_by_T(Y, T)

Y = T * Y;


function b = isposdef(a)
% ISPOSDEF   Test for positive definite matrix.
%    ISPOSDEF(A) returns 1 if A is positive definite, 0 otherwise.
%    Using chol is much more efficient than computing eigenvectors.

% Written by Tom Minka

[R,p] = chol(a);
b = (p == 0);


function popselected = select(pop, fitness, nselected, method)
% SELECT
% popselected = SELECT(pop, fitness, nselected, method) select the fittest
% individuals to survive to the next generation.
%

popSize = size(pop,1);

switch method

    case 'roulette'
        p=fitness/sum(fitness); % probabilities of select
        cump=cumsum(p);         % cumulative sum of probabilities
        I = interp1([0 cump],1:(popSize+1),rand(1,nselected),'linear');
        % random numbers from 1:nselected according probabilities
        I = floor(I);

    case 'competition'
        % randomly generated two sets of population
        i1 = ceil( popSize*rand(1,nselected) );
        i2 = ceil( popSize*rand(1,nselected) );
        % compare the fitness and select the fitter
        I = i1.*( fitness(i1)>=fitness(i2) ) + ...
            i2.*( fitness(i1)< fitness(i2) );

end

popselected=pop(I,:);


function [cols] = cubicle2col(vol,scale)
% convert a 3D volume to columns, i.e, each column is a small cube in the
% 3D volume. (An extension of im2col).
% vol: 3D volume.
% scale: the size of the small cube. The size of the volume should be
% divisible by scale.

if ndims(vol) < 3
    error('input data should be a volume');
end

a_vol = vol;
square_size = size(a_vol);
block_size = square_size(1:3) ./ scale;
assert(all(block_size == floor(block_size)));

cols = zeros([scale^2, numel(vol)./scale^3], class(vol));
for c = 1 : scale ^ 3
    x_off = mod(c-1 , scale); y_off = mod(floor((c-1) / scale), scale); z_off = floor((c-1) / scale^2 );
    per_channel = vol((x_off + 1) : scale : end, (y_off + 1) : scale : end , (z_off + 1) : scale : end, :);
    cols(c, :) = per_channel(:);
end


function Q = S_object(prediction,GT)
% S_object Computes the object similarity between foreground maps and ground
% truth(as proposed in "Structure-measure:A new way to evaluate foreground
% maps" [Deng-Ping Fan et. al - ICCV 2017])
% Usage:
%   Q = S_object(prediction,GT)
% Input:
%   prediction - Binary/Non binary foreground map with values in the range
%                [0 1]. Type: double.
%   GT - Binary ground truth. Type: logical.
% Output:
%   Q - The object similarity score
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% compute the similarity of the foreground in the object level
prediction_fg = prediction;
prediction_fg(~GT)=0;
O_FG = Object(prediction_fg,GT);

% compute the similarity of the background
prediction_bg = 1.0 - prediction;
prediction_bg(GT) = 0;
O_BG = Object(prediction_bg,~GT);

% combine the foreground measure and background measure together
u = mean2(GT);
Q = u * O_FG + (1 - u) * O_BG;

end

function score = Object(prediction,GT)

% check the input
if isempty(prediction)
    score = 0;
    return;
end
if isinteger(prediction)
    prediction = double(prediction);
end
if (~isa( prediction, 'double' ))
    error('prediction should be of type: double');
end
if ((max(prediction(:))>1) || min(prediction(:))<0)
    error('prediction should be in the range of [0 1]');
end
if(~islogical(GT))
    error('GT should be of type: logical');
end

% compute the mean of the foreground or background in prediction
x = mean2(prediction(GT));

% compute the standard deviations of the foreground or background in prediction
sigma_x = std(prediction(GT));

score = 2.0 * x./(x^2 + 1.0 + sigma_x + eps);
end