function [ y, m, d, f ] = jed_to_ymdf_julian2 ( jed )

%*****************************************************************************80
%
%% JED_TO_YMDF_JULIAN2 converts a JED to a Julian YMDF date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 March 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, M, D, real F, the YMDF date.
%

%
%  Check the input.
%
  ierror = jed_check ( jed );

  if ( ierror ~= 0 )
    y = -1;
    m = -1;
    d = -1;
    f = -1.0;
    return
  end

  j = floor ( jed + 0.5 );
  f = ( jed + 0.5 ) - j;

  jd = floor ( ( j + 1524 - 122.1 ) / 365.25 );
  je = floor ( 365.25 * jd );
  jg = floor ( ( j + 1524 - je ) / 30.6001 );
%
%  Now compute D, M and Y.
%
  d = j + 1524 - je - floor ( 30.6001 * jg );

  if ( jg <= 13 )
    m = jg - 1;
  else
    m = jg - 13;
  end

  if ( 2 < m )
    y = jd - 4716;
  else
    y = jd - 4715;
  end
%
%  Any year before 1 AD must be moved one year further back, since
%  this calendar does not include a year 0.
%
  y = y_astronomical_to_common ( y );

  return
end


% Simple Example #3 -- products of gaussian mixtures
%
%
fprintf('KDE Example #3 : Product sampling methods (single, anecdotal run)\n');
rand('state',0);
randn('state',0);

p = kde([.1,.45,.55,.8],.05);  % create a mixture of 4 gaussians for testing
q = kde([.25,.35,.7,.75],.05);  % and another
p = ksize(resample(p,100),'rot');
q = ksize(resample(q,150),'rot'); fprintf('.');
pd = hist(p,100,[1],[0,1]);
qd = hist(q,100,[1],[0,1]);

%pEx = productExact(p,{p,q},{},{}); fprintf('.');
pEx = pd.*qd; pEx = 100*pEx./sum(pEx);
dummy = kde(rand(1,200),1);  % placeholder for productApprox call
pSm = productApprox(dummy,{p,q},{},{},'exact'); fprintf('.');
pEp = productApprox(dummy,{p,q},{},{},'epsilon',1e-3); fprintf('.');
pGi = productApprox(dummy,{p,q},{},{},'gibbs1',100); fprintf('.');
pGM = productApprox(dummy,{p,q},{},{},'gibbsMS1',5); fprintf('.\n');

figure(1); hold off;
plot(linspace(0,1,100),pEx,'r-');
hold on;
plot(pSm,'k-');
plot(pEp,'g-');
plot(pGi,'c-');
plot(pGM,'b-');

legend('Discretized','Exact Sampling','Epsilon-Exact','Gibbs1','GibbsMS1');


function [ff,f]=lpccc2ff(cc,np,nc,c0)
%LPCCC2FF Convert complex cepstrum to complex spectrum FF=(CC,NP,NC)
%
%  Inputs: cc(nf,n)     Complex ceptral coefficients excluding c(0), one frame per row
%          np           Size of output spectrum is np+1 [n]
%                       Alternatively, a vector of output frequencies in the range 0 to 0.5
%          nc           Number of cepstral coefficients to use [np or, if np is a vector, n]
%                       Set nc=-1 to use n coefficients
%          c0(nf,1)     Cepstral coefficient c(0) [0]
%
% Outputs: ff(nf,np+2)  Complex spectrum from DC to Nyquist
%          f(1,np+2)    Normalized frequencies (0 to 0.5)
%
% The "complex cepstral coefficients", cc(n), are the inverse discrete-time Fourier transform
% of the log of the complex-valued spectrum. The cc(n) are real-valued and, for n<0, cc(n)=0.
% The "real cepstral coeffcients", rc(n), are the inverse discrete-time Fourier transform
% of the log of the magnitude spectrum; rc(0)=cc(0) and rc(n)=0.5*cc(n) for n~=0.
% For highest speed, choose np+1 to be a power of 2.

%      Copyright (C) Mike Brookes 2014
%      Version: $Id: lpccc2ff.m 5025 2014-08-22 17:07:24Z 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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[nf,mc]=size(cc);
if nargin<2 || ~numel(np)
    if nargout
        np=mc;
    else
        np=128;
    end
end
if nargin>=3 && numel(nc)==1 && nc==-1 nc=mc; end
if nargin<4 || ~numel(c0) c0=zeros(nf,1); end
if numel(np)>1 || np(1)<1
    if nargin<3 || ~numel(nc) nc=mc; end
    f=np(:)';
    if nc==mc
        ff=exp([c0 cc]*exp(-2i*pi*(0:mc)'*f));
    else
        ff=exp([c0 lpccc2cc(cc,nc)]*exp(-2i*pi*(0:nc)'*f));
    end
else
    if nargin<3 || ~numel(nc) nc=np; end
    if nc==mc
        ff=exp(rfft([c0 cc].',2*np).');
    else
        ff=exp(rfft([c0 lpccc2cc(cc,nc)].',2*np).');
    end
    f=linspace(0,0.5,np+1);
end
if ~nargout
    subplot(2,1,2);
    plot(f,unwrap(angle(ff.')));
    xlabel('Normalized frequency f/f_s');
    ylabel('Phase (rad)');
    subplot(2,1,1);
    plot(f,db(abs(ff.')));
    xlabel('Normalized frequency f/f_s');
    ylabel('Gain (dB)');
end


function [tcorr,rho,X_ECEF] = get_rho(tR_RAW,pseudorange,Eph,X_receiver)
%GET_RHO  Calculation of distance in ECEF system between
%	      satellite and receiver at time tR_RAW given the
%	      ephemeris Eph.

%Kai Borre 04-01-96
%Copyright (c) by Kai Borre
%$Revision: 1.0 $  $Date: 1997/09/23  $

% Initial assigment of constants
vlight = 299792458;	         % vacuum speed of light in m/s
Omegae = 7.292115147e-5;	 % rotation rate of the earth in rad/s

%signal sended time=signal received time-signal transmission time
tx_RAW = tR_RAW-pseudorange/vlight;

%time correction acording to system time
toc = Eph(21);
      dt = check_t(tx_RAW-toc);
      tcorr = (Eph(2)*dt + Eph(20))*dt + Eph(19);
      tx_GPS = tx_RAW-tcorr;
      dt = check_t(tx_GPS-toc);
      tcorr = (Eph(2)*dt + Eph(20))*dt + Eph(19);
      tx_GPS = tx_RAW-tcorr;

% satellite position
X = satpos(tx_GPS, Eph);

%geometric range
rho = norm(X-X_receiver(1:3));

%satellite coordinates into inertial coordinate system
omegatau = Omegae*rho/vlight;
R3 = [ cos(omegatau) sin(omegatau) 0;
      -sin(omegatau) cos(omegatau) 0;
              0	       0	     1];
X_ECEF = R3*X;

%geometric range acording to inertial coordinate system
rho = norm(X_ECEF-X_receiver(1:3));
%%%%%%%%% end get_rho.m %%%%%%%%%


function agm_pi_test ( )

%*****************************************************************************80
%
%% AGM_PI_TEST tests the AGM_PI function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2012
%
%  Author:
%
%    John Burkardt
%
  clear all

  fprintf ( 1, '\n' );
  fprintf ( 1, 'AGM_PI_TEST\n' );
  fprintf ( 1, '  AGM_PI computes the value of pi using the \n' );
  fprintf ( 1, '  arithmetic-geometric-mean approach.\n' );
%
%  Get pi to 10, 20, 40, 80 decimal digits:
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Request PI to 10, 20, 40, 80 digits.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Digits   PI:\n' );
  fprintf ( 1, '\n' );

  d = 10;

  for i = 1 : 4

    p = agm_pi ( d );
    s = [ '  ', num2str(d,'%6d'), '  ', p ];
%
%  Trying to format output involving a symbolic variable seems to
%  be rather unsatisfactory.  FPRINTF won't work.  DISP can be used,
%  but you have to concatenate items first, and the concatenation operator
%  goes nuts when you have unexpected spaces in a phrase like "num2str ( d, '%6d' )"
%  and then DISP automatically prints the item PLUS an apparently unavoidable blank line
%  while suppressing all but a single blank when you inserted blanks so stuff
%  would line up, AND you also get the extraneous square brackets in your output.
%
    disp ( s )
    d = d * 2;

  end
%
%  Timings.
%  Apparently, D = 1 is illegal input to AGM_PI!
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  How long does it take to compute PI to D digits?\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '          Digits    Seconds\n' );
  fprintf ( 1, '\n' );
  d = 2;
  for i = 1 : 20
    tic
    p = agm_pi ( d );
    t = toc;
    fprintf ( 1, '  %2d  %10d  %14g\n', i, d, t );
    d = d * 2;
  end

  return
end


function [ val ] = check_dominance(vec1, vec2)
%   This procedure checks the dominance of two vectors,
%   where the vectors are in the form [obj,cv]
%   The non-domnination check will work like this --
%       1   if vec1 dominates vec2
%      -1   if vec2 dominates vec1
%       0   if both vec1 and vec2 are non-dominated
%
%   Please note: Vectorization on this function may not be too
%   efficient as it operates on only two vectors, and couple of
%   variables.

% split the obj and cv values
obj1 = vec1(1:end-1);
cv1 = vec1(end);
obj2 = vec2(1:end-1);
cv2 = vec2(end);

if(cv1 < 0 && cv2 < 0)
    if(cv1 > cv2)
        val = 1 ;
        return ;
    elseif(cv1 < cv2)
        val = -1 ;
        return ;
    else
        val = 0;
        return ;
    end
else
    if(cv1 < 0 && cv2 == 0)
        val = -1 ;
        return ;
    elseif(cv1 == 0 && cv2 < 0)
        val = 1  ;
        return ;
    else
        if(any(obj1 < obj2) && ~(any(obj1 > obj2)))
            val = 1 ;
        elseif(~(any(obj1 < obj2)) && any(obj1 > obj2))
            val = -1 ;
        else
            val = 0 ;
            return ;
        end
    end
end
% end of check_dominance
end


%
%   simple example to show the usage of l1_ls
%

% problem data
A  = [1    0    0   0.5;...\
      0    1  0.2   0.3;...\
      0  0.1    1   0.2];
x0 = [1 0 1 0]';    % original signal
y  = A*x0;          % measurements with no noise
lambda = 0.01;      % regularization parameter
rel_tol = 0.01;     % relative target duality gap

[x,status]=l1_ls(A,y,lambda,rel_tol);


function f=involute(f,dim);
%INVOLUTE  Involution
%   Usage: finv=involute(f);
%          finv=involute(f,dim);
%
%   `involute(f)` will return the involution of *f*.
%
%   `involute(f,dim)` will return the involution of *f* along dimension *dim*.
%   This can for instance be used to calculate the 2D involution::
%
%     f=involute(f,1);
%     f=involute(f,2);
%
%   The involution *finv* of *f* is given by::
%
%     finv(l+1)=conj(f(mod(-l,L)+1));
%
%   for $l=0,\ldots,L-1$.
%
%   The relation between conjugation, Fourier transformation and involution
%   is expressed by::
%
%     conj(dft(f)) == dft(involute(f))
%
%   for all signals *f*. The inverse discrete Fourier transform can be
%   expressed by::
%
%     idft(f) == conj(involute(dft(f)));
%
%   See also:  dft, pconv

% Assert correct input.

%   AUTHOR : Peter L. Søndergaard
%   TESTING: TEST_INVOLUTE
%   REFERENCE: OK

complainif_argnonotinrange(nargin,1,2,mfilename);

if nargin==1
  dim=[];
end;

L=[];
[f,L,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,L,dim,'INVOLUTE');

% This is where the calculation is performed.
% The reshape(...,size(f) ensures that f will keep its
% original shape if it is multidimensional.
f=reshape(conj([f(1,:); ...
	  flipud(f(2:L,:))]),size(f));

f=assert_sigreshape_post(f,dim,permutedsize,order);


function indx = whichInterval(dom, x, direction)
%WHICHINTERVAL   Determine which interval a point lies in.
%   INDX = WHICHINTERVAL(DOM, X) returns a matrix of size(X) whos j,k entry is a
%   positive integer denoting which subinterval of the domain DOM (which should
%   be a sorted real-valued vector) the real part of the point X(j,k) is
%   positioned. INDX(j,k) = -/+INF if X(j,k) < DOM(1) or X(j,k) > DOM(end),
%   respectively.
%
%   WHICHINTERVAL(DOM, X, DIRECTION) determines which interval those points in X
%   which lie on the breakpoints of DOM should be counted in. DIRECTION = -1
%   (the default) places such points in the interval to the left, whilst
%   DIRECTION = 1 places them to the right.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

numInts = numel(dom) - 1;
xReal = real(x);
indx = NaN(size(x));

% Points to the left of the domain:
indx(xReal < dom(1)) = -inf;

% Deal with the points that lie on breakpoints:
if ( nargin > 2 && direction > 0 )
    mygt = @(x, y) x >= y;
    mylt = @(x, y) x < y;
else
    mygt = @(x, y) x > y;
    mylt = @(x, y) x <= y;
end

indx(xReal == dom(1)) = 1;
% Points within the domain:
for j = 1:numInts
    indx( mygt(xReal, dom(j)) & mylt(xReal, dom(j+1)) ) = j;
end
indx(xReal == dom(end)) = numInts;

% Points to the right of the domain:
indx(xReal > dom(end)) = inf;

end


function inside = quad_contains_point_2d ( q, p )

%*****************************************************************************80
%
%% QUAD_CONTAINS_POINT_2D finds if a point is inside a convex quadrilateral in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real Q(2,4), the vertices of the quadrilateral.
%
%    Input, real P(2,1), the point to be checked.
%
%    Output, logical INSIDE, is TRUE if the point is in the quadrilateral.
%

%
%  This will only handle convex quadrilaterals.
%
  inside = 0;

  angle_1 = angle_rad_2d ( q(1:2,1), q(1:2,2), q(1:2,3) );
  angle_2 = angle_rad_2d ( q(1:2,1), q(1:2,2), p(1:2,1) );

  if ( angle_1 < angle_2 )
    return
  end

  angle_1 = angle_rad_2d ( q(1:2,2), q(1:2,3), q(1:2,4) );
  angle_2 = angle_rad_2d ( q(1:2,2), q(1:2,3), p(1:2,1) );

  if ( angle_1 < angle_2 )
    return
  end

  angle_1 = angle_rad_2d ( q(1:2,3), q(1:2,4), q(1:2,1) );
  angle_2 = angle_rad_2d ( q(1:2,3), q(1:2,4), p(1:2,1) );

  if ( angle_1 < angle_2 )
    return
  end

  angle_1 = angle_rad_2d ( q(1:2,4), q(1:2,1), q(1:2,2) );
  angle_2 = angle_rad_2d ( q(1:2,4), q(1:2,1), p(1:2,1) );

  if ( angle_1 < angle_2 )
    return
  end

  inside = 1;

  return
end


function [ n_data, a, b, fx ] = binomial_values ( n_data )

%*****************************************************************************80
%
%% BINOMIAL_VALUES returns some values of the binomial coefficients.
%
%  Discussion:
%
%    The formula for the binomial coefficient is
%
%      C(N,K) = N! / ( K! * (N-K)! )
%
%    In Mathematica, the function can be evaluated by:
%
%      Binomial[n,k]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz and Irene Stegun,
%    Handbook of Mathematical Functions,
%    US Department of Commerce, 1964.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, integer A, B, the arguments of the function.
%
%    Output, integer FX, the value of the function.
%
  n_max = 20;

  a_vec = [ ...
     1,  6,  6,  6, 15, ...
    15, 15, 15, 15, 15, ...
    15, 25, 25, 25, 25, ...
    25, 25, 25, 25, 25  ];

  b_vec = [ ...
     0,  1,  3,  5,  1, ...
     3,  5,  7,  9, 11, ...
    13,  1,  3,  5,  7, ...
     9, 11, 13, 15, 17 ];

  fx_vec = [ ...
           1, ...
           6, ...
          20, ...
           6, ...
          15, ...
         455, ...
        3003, ...
        6435, ...
        5005, ...
        1365, ...
         105, ...
          25, ...
        2300, ...
       53130, ...
      480700, ...
     2042975, ...
     4457400, ...
     5200300, ...
     3268760, ...
     1081575 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0;
    b = 0;
    fx = 0;
  else
    a = a_vec(n_data);
    b = b_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function fx = annulus ( n, x )

%*****************************************************************************80
%
%% ANNULUS evaluates the annulus function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N,2), the point coordinates.
%
%    Output, real FX(N), the function values.
%
  fx = zeros ( n, 1 );

  c1 = 0.25 * ones ( n, 2 );
  r1 = 0.5 * ones ( n, 1 );

  c2 = 0.0 * ones ( n, 2 );
  r2 = 1.0 * ones ( n, 1 );

  a = ( sqrt ( sum ( ( x - c1 ) .* ( x - c1 ), 2 ) ) ) - r1;
  b = r2 - ( sqrt ( sum ( ( x - c2 ) .* ( x - c2 ), 2 ) ) );

  fx = a .* b;

  return
end


function s = resampres(p,m,n);
%RESAMPRES Residual resampling
%
%   Description:
%   S = RESAMPRES(P) returns a new set of indices according to
%   the probabilities P. P is array of probabilities, which are
%   not necessarily normalized, though they must be non-negative,
%   and not all zero. The size of S is the size of P.
%
%   S = RESAMPRES(P,M,N) return M by N matrix.
%
%   S = RESAMPRES(P,M) returns M by M matrix.
%
%   Note that stratified and deterministic resampling have smaller
%   variance.
%
%   Residual resampling retains k_j=floor(n*p_j) copies of index j,
%   where n is length of P and p is normalized probabilities.
%   Additionally n_r=n-k_1-...-k_n indices are sampled with
%   probabilities proportional to n*p_j-k_j. See, e.g., Liu, J. S.,
%   Monte Carlo Strategies in Scientific Computing, Springer, 2001,
%   p. 72.
%
%   See also RESAMPSIM, RESAMPSTR, RESAMPDET
%
% Copyright (c) 2003-2004 Aki Vehtari

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

if nargin<2
    [m,n] = size(p);
elseif nargin==2
    n = m;
end
nin=m.*n;
p=p(:);
pn=p./sum(p).*nin;
fpn=floor(pn);
s=zeros(m,n);
k=0;
for i=1:numel(pn)
  if pn(i)>=1
    a=fpn(i);
    pn(i)=pn(i)-a;
    s(k+[1:a])=i;
    k=k+a;
  end
end
pc=cumsum(pn./(nin-k));
pc(end)=1;
s((k+1):nin)=binsgeq(pc,rand(1,nin-k));


function [x, counter] = conjugate_gradient(K, P, tol)
    % 共轭梯度法对方程进行求解

    % 默认精度
    if nargin == 2
        tol = 1e-5;
    end

    [m, n] = size(K);
    [p, q] = size(P);

    % 初始化
    x = sparse(zeros(m,1));

    % 根据P初始化 r0 and d
    r0 = P;
    d  = P;
    % 计数器
    counter = 0;

    % 如果成立，直接返回
    if sqrt(r0'*r0) < tol;
        return;
    end

    v  = K * d;
    % 计算步长
    w  = r0' * r0 / (d' * v);
    % 更新 x
    x  = x + w * d;
    % 计算剩余的
    r1 = r0 - w * v;
    counter = counter + 1;

    while sqrt(r1'*r1) > tol
        P  = r1' * r1 / (r0' * r0);
        % 更新搜索方向
        d  = r1 + P * d;
        r0 = r1;
        v  = K * d;
        % 计算步长
        w  = r0' * r0 / (d' * v);
        x  = x + w * d;
        r1 = r0 - w * v;
        counter = counter + 1;
    end


function [xMid,fMid,exitCode]=bisectionRootFind(f,xSpan,AbsTol,maxIter)
%%BISECTIONROOTFIND Given two points that are of different signs
%          surrounding the root of a scalar funding, find the root of the
%          function via a bisection search.
%
%INPUTS: f The handle to the function whose root is desired. The function
%          is called as f(x), where x is a scalar value.
%    xSpan A 2X1 or 1X2 vector such that
%          sign(f(xSpan(1)))~=sign(f(xSpan(2)))
%   AbsTol The absolute tolerance on the value of f for converence. The
%          default if omitted or an empty matrix is passed is eps().
%  maxIter The maximum number of bisections to perform. The default if
%          omitted or an empty matrix is passed is 100.
%
%OUTPUTS: xMid The point found that is the value of the approximate zero.
%         fMid The value of the function at xMid.
%     exitCode A value indicating how the function terminated. Possible
%              values are:
%              0 Either AbsTol was satisfied or the bounds on the value
%                xMid went below finite precision constraints.
%              1 The maximum number of iterations occurred.
%
%The bisection algorithm is straightforward: Assuming that only a single
%root is surrounded by xSpan, one evaluates the point at the middle of the
%interval. One then discards the point in xSpan having the same sign as the
%point in the middle of the interval. That gives a new interval and the
%process continues until convergence. The method is discussed in [1].
%
%EXAMPLE:
% f=@(x)(x.^2-4);
% xSpan=[0;5];
% [xMid,fMid,exitCode]=bisectionRootFind(f,xSpan)
%In this example convergence is to the exact root, xMid=2.
%
%REFERENCES:
%[1] Weisstein, Eric W. "Bisection." From MathWorld--A Wolfram Web
%    Resource. http://mathworld.wolfram.com/Bisection.html
%
%July 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(maxIter))
    maxIter=100;
end

if(nargin<3||isempty(AbsTol))
    AbsTol=eps();
end

xLeft=xSpan(1);
xRight=xSpan(2);

fLeft=f(xLeft);
fRight=f(xRight);

if(sign(fLeft)==sign(fRight))
    error('bisectionRootFind:NoChange','There is no sign change in the initial set. ');
end

xMid=xRight;
fMid=fRight;

exitCode=1;
for curIter=1:maxIter
    xMid=(xLeft+xRight)/2;
    fMid=f(xMid);

    if(abs(fMid)<AbsTol||xMid==xRight||xMid==xLeft)
        exitCode=0;
        break;
    end

    if(sign(fLeft)~=sign(fMid))
        xRight=xMid;
    else
        fLeft=fMid;
        xLeft=xMid;
    end
end
end

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


function ned = ecef2ned(ecef, llh_org)
% ecef2ned: converts from ECEF coordinates to NED coordinates.
%
% INPUTS
%   ecef: Nx3 ECEF coordinates [X Y Z] (m, m, m).
%   llh_org: 1x3 system origin [lat, lon, h] (rad, rad, m).
%
% OUTPUTS
%   ned: Nx3 NED coordinates [X Y Z] (m, m, m).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%	R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics}, vol. 17,
% issue 2, pp. 110-120, 2015. Inverse process of Eq. 15.
%
% Version: 003
% Date:    2019/01/16
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

lat = llh_org(1);
lon = llh_org(2);

ecef_org = llh2ecef(llh_org)';

slat = sin(lat);
clat = cos(lat);
slon = sin(lon);
clon = cos(lon);

R = [  -slat*clon  -slat*slon   clat; ...
       -slon          clon         0; ...
       -clat*clon  -clat*slon  -slat];

[MAX, N] = size(ecef);
ned = zeros(MAX, N);

for i=1:MAX

    ned_t = R * (ecef(i, :)' - ecef_org) ;
    ned (i, :) = ned_t';
end


function [km3] = gal2km3(gal)
% Convert volume from US liquid gallons to cubic kilometers.
% Chad Greene 2012
km3 = gal*3.785411784e-12;


function varargout = entropy(varargin)
%ENTROPY
%
% y = ENTROPY(x)
%
% Computes/declares concave entropy -sum(x.*log(x))
%
% Implemented as evalutation based nonlinear operator. Hence, the concavity
% of this function is exploited to perform convexity analysis and rigorous
% modelling.
%
% See also CROSSENTROPY, KULLBACKLEIBLER.

switch class(varargin{1})

    case 'double'
        x = varargin{1};
        % Safe version with defined negative values (helps fmincon when
        % outside feasible region)
        if any(x<=0)
            z = abs(x);z(z==0)=1;
            y = z.*log(z);
            y(x==0) = 0;
            y(x<0) = 3*(x(x<0)-1).^2-3;
            varargout{1} = -sum(y);
        else
            varargout{1} = -sum(x.*log(x));
        end

    case {'sdpvar','ndsdpvar'}

        varargin{1} = reshape(varargin{1},[],1);
        varargout{1} = yalmip('define',mfilename,varargin{1});

    case 'char'

        X = varargin{3};
        F = (X >= 0);

        operator = struct('convexity','concave','monotonicity','none','definiteness','none','model','callback');
        operator.range = [-inf exp(-1)*length(X)];
        operator.domain = [0 inf];
        operator.bounds = @bounds;
        operator.convexhull = @convexhull;
        operator.derivative = @derivative;
        varargout{1} = F;
        varargout{2} = operator;
        varargout{3} = X;

    otherwise
        error('SDPVAR/ENTROPY called with CHAR argument?');
end

function df = derivative(x)
x(x<=0)=eps;
df = (-1-log(x));

function [L, U] = bounds(xL,xU)

t = find(xL==0);
u = find(xL<0);
xL(t)=1;
LU = [-xL.*log(xL) -xU.*log(xU)];
LU(t,1)=0;
xL(t) = 0;
L = min(LU,[],2);
U = max(LU,[],2);
U((xL < exp(-1)) & (xU > exp(-1))) = exp(-1);
L = sum(L);
U = sum(U);

function [Ax, Ay, b] = convexhull(xL,xU)

if length(xL)==1
    xM = (xU+xL)/2;
    f1 = entropy(xL);
    f2 = entropy(xM);
    f3 = entropy(xU);
    df1 = derivative(xL);
    df2 = derivative(xM);
    df3 = derivative(xU);
    [Ax,Ay,b] = convexhullConcave(xL,xM,xU,f1,f2,f3,df1,df2,df3);

elseif length(xL)==2
    x1 = [xL(1);xL(2)];
    x2 = [xU(1);xL(2)];
    x3 = [xL(1);xU(2)];
    x4 = [xU(1);xU(2)];
    x5 = (xL+xU)/2;

    f1 = entropy(x1);
    f2 = entropy(x2);
    f3 = entropy(x3);
    f4 = entropy(x4);
    f5 = entropy(x5);

    df1 = derivative(x1);
    df2 = derivative(x2);
    df3 = derivative(x3);
    df4 = derivative(x4);
    df5 = derivative(x5);

    [Ax,Ay,b] = convexhullConcave2D(x1,f1,df1,x2,f2,df2,x3,f3,df3,x4,f4,df4,x5,f5,df5);
else
    Ax = [];
    Ay = [];
    b = [];
end


function [ out ] = MSE( pic1,pic2 )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
e=0;
[m n]=size(pic1);

for i=1:m
    for j=1:n
        e=e+double((pic1(i,j)-pic2(i,j))^2);
    end
end
out=e/(m*n);
end


function [a,t]=rotqr2ax(q)
%ROTQR2AX converts a real quaternion to the corresponding rotation axis and angle
% Inputs:
%
%     Q(4,1)   real-valued quaternion (with magnitude = 1)
%
% Outputs:
%
%     A(3,1)   Unit vector in the direction of the rotation axis.
%     T        Rotation angle in radians (in range 0 to 2pi)
%
% In the quaternion representation of a rotation, and q(1) = cos(t/2)
% where t is the angle of rotation in the range 0 to 2pi
% and q(2:4)/sin(t/2) is a unit vector lying along the axis of rotation
% a positive rotation about [0 0 1] takes the X axis towards the Y axis.
%
%      Copyright (C) Mike Brookes 2007
%      Version: $Id: rotqr2ax.m,v 1.2 2007/11/21 12:42:36 dmb Exp $
%
%   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.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
a=q(2:4);
m=sqrt(a'*a);
t=2*atan2(m,q(1));      % avoids problems if unnormalized
if m>0
    a=a/m;
else
    a=[0 0 1]';
end


function pass = test_std(pref)
% Test the chebfun3/max command.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e3*pref.cheb3Prefs.chebfun3eps;

% Note that std(chebfun(@(x) x.^2)) = sqrt(4/45).

f1 = chebfun3(@(x,y,z) x.^2 + y.*z); % f1 has x-std equal to sqrt(4/45).
f2 = chebfun3(@(x,y,z) y.^2 + x.*z); % f2 has y-std equal to sqrt(4/45).
f3 = chebfun3(@(x,y,z) z.^2 + x.*y); % f3 has z-std equal to sqrt(4/45).

exact = chebfun2(@(x,y) sqrt(4/45)); % x-std of y.*z equals 0.

h1 = std(f1);
h2 = std(f1,[],1);
h3 = std(f2,[],2);
h4 = std(f3,[],3);

pass(1) = norm(h1 - exact) < tol;
pass(2) = norm(h2 - exact) < tol;
pass(3) = norm(h3 - exact) < tol;
pass(4) = norm(h4 - exact) < tol;

end


function r = invgamrand(a, b, varargin)
%INVGAMRAND Random matrices from inverse gamma distribution
%
%   R = INVGAMRAND(A,B) returns a matrix of random numbers chosen
%   from the inverse gamma distribution with parameters A and B.
%   The size of R is the common size of A and B if both are matrices.
%   If either parameter is a scalar, the size of R is the size of the other
%   parameter. Alternatively, R = GAMRND(A,B,M,N) returns an M by N matrix.
%
%   Note: Parameterization as in (Neal, 1996).
%      A is mean of the distribution
%      B is degrees of freedom
%
%	See also GAMRAND

% Copyright (c) 1999 Aki Vehtari

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

error('No mex-file for this archtitecture. See Matlab help and convert.m in ./linuxCsource or ./winCsource for help.')


function [data_out] = rsencodecod(data_in,rate_id,TxRx)
%%  Description: The Reed-Solomon encoder is realized according to the standard. %

if rate_id==0
    if TxRx==10
        data_out=[data_in;0;0;0;0;0;0;0;0]; %RS encoder is by passed for BPSK1/2
    elseif TxRx==01
        data_out=[data_in(1:end-8)];     %remove added zero bit
    end
else
switch (rate_id)
    case 1
         n = 32;
         k = 24;
    case 2
        n = 40;
        k = 36;
    case 3
        n = 64;
        k = 48;
    case 4
        n = 80;
        k = 72;
    case 5
        n = 108;
        k = 96;
    case 6
        n = 120;
        k = 108;
 end
m = 8;   % Number of bits per symbol
d=n-k;
if TxRx==10
    %  To realize the Reed-Solomon code, the information is needed in decimal.
    data = reshape(data_in,8,length(data_in)/8);
    data = bi2de(data.','left-msb');
    % one bytes are needed with a stuffed zero at the end of the vector:
      data=[data.' 0];
   % The Galois vector is generated, the generating polynomial of the
   % code. Then the symbols are encoded with Reed-Solomon.
     msg = gf([data],m);   % used polynomial to generate gf field array is same as describe in ieee standrds
     codeRS = rsenc(msg,n,k);  % here we used primitive polynomial as in bydefault in matlab
     out = codeRS.x ;           % convert codeRS a gf object to uni8 array
     data_out=double(out);     % convert to double array
     data_out=[data_out(end-d+1:end) data_out(1:end-d)];  % extra bits should be send befor the orignal bits
   % decimal to binary conversion for continue
   data_out=de2bi(data_out,'left-msb');
   data_out=reshape(data_out.',length(data_out)*8,1);

elseif TxRx==01
    % RS DECODER
    % binary to decimal conversion
    data = reshape(data_in,8,length(data_in)/8);
    data = bi2de(data.','left-msb');
    data=[data.'];
    data=[data(d+1:end) data(1:d)]; % putting the extra bit to last of the array
    %decoding
    msg = gf([data],m);
    decodeRS = rsdec(msg,n,k);
    out = decodeRS(1:end-1)';
    out= out.x;
    out = double (out);
    % The binary data to continue working:
    out = de2bi (out,'left-msb');
    out = reshape (out.', 1, length(out)*8);
   data_out=out';
end
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logisitc regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell use
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

% Set Initial theta
initial_theta = zeros(n + 1, 1);

% Set options for fminunc
options = optimset('GradObj', 'on', 'MaxIter', 50);
for c = 1:num_labels
  [theta] = fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), initial_theta, options);
  all_theta(c, :) = theta';
end

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

end


function bessel_i0_values_test ( )

%*****************************************************************************80
%
%% BESSEL_I0_VALUES_TEST demonstrates the use of BESSEL_I0_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BESSEL_I0_VALUES_TEST:\n' );
  fprintf ( 1, '  BESSEL_I0_VALUES stores values of \n' );
  fprintf ( 1, '  the Bessel I0 function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X            I0(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = bessel_i0_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function epitrochoid(r1,r2,len)
%animation of EPITROCHOID
%epitrochoid(r1,r2,len);
%r1=radius of fixed circle on which smaller circle rolls
%r2=radius of rolling circle
%len=line connected to centre of rolling circle
%example: epitrochoid(1,0.25,0.5);
%         epitrochoid(1,0.25,1);
%this code also trace epicycloid
%when r2=len then it becomes epicycloid
%example for epicycloid: epitrochoid(1,0.25,0.25);
%always keep r1>r2
if nargin==0
    r1=1;          %radius of fixed circle on which smaller circle rolls
    r2=0.25;        %radius of rolling circle
    len=0.5;       %line connected to centre of rolling circle
end
n=0;
dtheta=pi/20;  %increase in angle per iteration
roughx=[];     %variable to trace trochoid
roughy=[];     %variable to trace trochoid
upto=2*pi*r1/r2+dtheta; %rotate upto
t=linspace(0,2*pi,75);

[x,y]=pol2cart(t,r2);
h1=patch(x,y,'r');%draw rolling circle

[phil,rl]=cart2pol([-len 0],[0 0]);
h2=line(0,0);%draw line connected to centre of rolling circle

[x_fix,y_fix]=pol2cart(t,r1);
line(x_fix,y_fix);%draw fixed circle,only once because it is fixed

axis equal
axis(r1*[-2 2 -2 2]) %fix axis

while n<upto && ishandle(h1)
     [xl,yl]=pol2cart(phil-n,rl); %rotate line
     [x2,y2]=transform2d(xl,yl,0,-(r1+r2),-n*r2/r1,0,r1+r2);
     set(h2,'xdata',x2,'ydata',y2);%draw line

     [x,y]=pol2cart(t,r2); %rotate circle
     [x3,y3]=transform2d(x,y,0,-(r1+r2),-n*r2/r1,0,r1+r2);
     set(h1,'xdata',x3,'ydata',y3);%draw rolling circle

     line([roughx x2(1)],[roughy y2(1)]);%trace trochoid
     roughx=x2(1);
     roughy=y2(1);

     pause(0.025)
     n=n+dtheta;
end
 %function end


function h = p02_h ( n, x )

%*****************************************************************************80
%
%% P02_H evaluates the Hessian for problem 2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  for i = 1 : 13

    c = - i / 10.0;

    fi =   x(3) * exp ( c * x(1) )  - x(4) * exp ( c * x(2) ) ...
          + x(6) * exp ( c * x(5) ) -        exp ( c ) ...
          + 5.0 * exp ( 10.0 * c ) -  3.0 * exp ( 4.0 * c );

    dfdx1 =     c     * x(3) * exp ( c * x(1) );
    d2fdx11 =   c * c * x(3) * exp ( c * x(1) );
    d2fdx13 =   c     *        exp ( c * x(1) );
    dfdx2 =   - c     * x(4) * exp ( c * x(2) );
    d2fdx22 = - c * c * x(4) * exp ( c * x(2) );
    d2fdx24 = - c     *        exp ( c * x(2) );
    dfdx3 =                    exp ( c * x(1) );
    d2fdx31 =   c     *        exp ( c * x(1) );
    dfdx4 =   -                exp ( c * x(2) );
    d2fdx42 = - c     *        exp ( c * x(2) );
    dfdx5 =     c     * x(6) * exp ( c * x(5) );
    d2fdx55 =   c * c * x(6) * exp ( c * x(5) );
    d2fdx56 =   c     *        exp ( c * x(5) );
    dfdx6 =                    exp ( c * x(5) );
    d2fdx65 =   c     *        exp ( c * x(5) );

    h(1,1) = h(1,1) + 2.0 * dfdx1 * dfdx1 + 2.0 * fi * d2fdx11;
    h(1,2) = h(1,2) + 2.0 * dfdx2 * dfdx1;
    h(1,3) = h(1,3) + 2.0 * dfdx3 * dfdx1 + 2.0 * fi * d2fdx13;
    h(1,4) = h(1,4) + 2.0 * dfdx4 * dfdx1;
    h(1,5) = h(1,5) + 2.0 * dfdx5 * dfdx1;
    h(1,6) = h(1,6) + 2.0 * dfdx6 * dfdx1;

    h(2,1) = h(2,1) + 2.0 * dfdx1 * dfdx2;
    h(2,2) = h(2,2) + 2.0 * dfdx2 * dfdx2 + 2.0 * fi * d2fdx22;
    h(2,3) = h(2,3) + 2.0 * dfdx3 * dfdx2;
    h(2,4) = h(2,4) + 2.0 * dfdx4 * dfdx2 + 2.0 * fi * d2fdx24;
    h(2,5) = h(2,5) + 2.0 * dfdx5 * dfdx2;
    h(2,6) = h(2,6) + 2.0 * dfdx6 * dfdx2;

    h(3,1) = h(3,1) + 2.0 * dfdx1 * dfdx3 + 2.0 * fi * d2fdx31;
    h(3,2) = h(3,2) + 2.0 * dfdx2 * dfdx3;
    h(3,3) = h(3,3) + 2.0 * dfdx3 * dfdx3;
    h(3,4) = h(3,4) + 2.0 * dfdx4 * dfdx3;
    h(3,5) = h(3,5) + 2.0 * dfdx5 * dfdx3;
    h(3,6) = h(3,6) + 2.0 * dfdx6 * dfdx3;

    h(4,1) = h(4,1) + 2.0 * dfdx1 * dfdx4;
    h(4,2) = h(4,2) + 2.0 * dfdx2 * dfdx4 + 2.0 * fi * d2fdx42;
    h(4,3) = h(4,3) + 2.0 * dfdx3 * dfdx4;
    h(4,4) = h(4,4) + 2.0 * dfdx4 * dfdx4;
    h(4,5) = h(4,5) + 2.0 * dfdx5 * dfdx4;
    h(4,6) = h(4,6) + 2.0 * dfdx6 * dfdx4;

    h(5,1) = h(5,1) + 2.0 * dfdx1 * dfdx5;
    h(5,2) = h(5,2) + 2.0 * dfdx2 * dfdx5;
    h(5,3) = h(5,3) + 2.0 * dfdx3 * dfdx5;
    h(5,4) = h(5,4) + 2.0 * dfdx4 * dfdx5;
    h(5,5) = h(5,5) + 2.0 * dfdx5 * dfdx5 + 2.0 * fi * d2fdx55;
    h(5,6) = h(5,6) + 2.0 * dfdx6 * dfdx5 + 2.0 * fi * d2fdx56;

    h(6,1) = h(6,1) + 2.0 * dfdx1 * dfdx6;
    h(6,2) = h(6,2) + 2.0 * dfdx2 * dfdx6;
    h(6,3) = h(6,3) + 2.0 * dfdx3 * dfdx6;
    h(6,4) = h(6,4) + 2.0 * dfdx4 * dfdx6;
    h(6,5) = h(6,5) + 2.0 * dfdx5 * dfdx6 + 2.0 * fi * d2fdx65;
    h(6,6) = h(6,6) + 2.0 * dfdx6 * dfdx6;

  end

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  [T]= direct_kinematics_3RRR_numerical(ROBOT, THETA, threshold, max_iter)
%
%  Compute the direct kinematic problem in position for a 3RRR arm.
%  Returns the matrix T with position/orientation of the end effector.
%
%   T=[cos(Phi) -sin(Phi) 0 xA;
%      sin(Phi) cos(Phi) 0  yA;
%        0         0     1   0;
%        0          0    0   1];
%
%   The found solution of joint coordinates is drawn.
%
%  Input parameters:
%  ROBOT: robot structure (parallel).
%  THETA: The input vector of active joint coordinates THETA = [theta1
%           theta2 theta3];
%  Threshold: Target sum of squares. When the sum of squares of the
%           function values is below this value the method stops (default 0.01).
%  Max_iter: Maximum number of iteration steps in the algorithm (default 50).
%
%  WARNING: The geometric parameters have been fixed in this function, they
%  should be extracted from the robot structure.
%
%
% THE FUNCTION SHOWS HOW TO SOLVE THE DIRECT KINEMATICS OF THE 3RRR USING A
% NUMERICAL METHOD. THIS METHOD MAY BE APPLIED TO ANY PARALELL ROBOT FOR WHICH YOU
% MAY NOT FIND A CLOSED SOLUTION. OF COURSE, BE AWARE THAT A CLOSED
% SOLUTION EXISTS FOR THE 3RRR PARALLEL ROBOT, HOWEVER, FOR SIMPLICITY, WE
% APPLY THE METHOD TO A WELL-KNOWN, SIMPLE ROBOT.
%
% The solution uses a vanilla Newton-Raphson method, also known as a
% Levenberg-Marquardt method with Lambda=1.
%
%   Please, for more information see:
%   http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
%
% Example:
%   >>robot=load_robot('example','3RRR');
%   >>T=directkinematics_3RRR_numerical(robot, [pi/2 pi/4 -pi/2])
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   20/01/2014
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function T=direct_kinematics_3RRR_numerical(robot, theta, randomize, threshold, max_iter)

global xQ yQ xR yR b1 b2 b3 a1 a2 a3 h
close all
L=3; % basic mechanism length
%position of the fixed points xQ, yQ, xR, yR
xQ=L; yQ=0; xR=L/2; yR=L;

% Geometry
b1=1; b2=1; b3=1; a1=1; a2=1;a3=1; % m
%end effector is a triangle of side h
h=0.5; %m

%check convergence parameters
if ~exist('threshold','var')
    threshold=0.01;
end

if ~exist('max_iter','var')
    max_iter=50;
end

%randomize solutions
if ~exist('randomize','var')
    randomize=0;
end

%input parameters for the direct kinematics
th1=theta(1);%1.9948;
th2=theta(2);%3.2869;
th3=theta(3);%-1.3998;

theta = [th1 th2 th3];

%initial beta, different solutions may be found, depending on the
%starting values for the parameters beta
% beta = [xA yA Phi phi1 phi2 phi3]
beta=[0.0 0.0 -pi/4 0 0 0]';

%randomize solution
if randomize
    beta = beta + randn(1,6)';
end

%the vector y is the vector of values that the Gamma equations should have
%in this case, they should be all null
% Gamma=[Gamma1, Gamma2, Gamma3..., Gamma6];
y=[0 0 0 0 0 0]';

fs=[];

for i=1:max_iter,
    f=compute_gamma_values(beta,theta);
    J=compute_jacobian_beta(beta,theta);

    delta=inv(J'*J)*J'*(y-f);

    %update beta
    beta = beta + delta;

    sumofsqes=(y-f)'*(y-f);
    fs=[fs sumofsqes];

    if sumofsqes < threshold
        fprintf('\ndirect_kinematics_3RRR::Solution found in %d iterations',i);
        fprintf('\nPlease note that only ONE possible solution is returned');
        break;
    end
end

%check whether the algorithm converged in less than max_iter steps
if i==max_iter
  fprintf('ERROR:: direct_kinematics_3RRR::Could not converge in %d iterations',i);
end

fprintf('FINAL VALUE of Gamma function');
f

fprintf('FINAL VALUE of sumof squares');
sumofsqes

%build solution from the beta parameters.
T=eye(4);
T(1,4)=beta(1);
T(2,4)=beta(2);
T(1,1)=cos(beta(3));
T(2,1)=sin(beta(3));
T(1,2)=-sin(beta(3));
T(2,2)=cos(beta(3));

%draw the given solution
%q=[th1 phi1 th2 phi2 th3 phi3]
q=[th1 beta(4) th2 beta(5) th3 beta(6)];
drawrobot3d(robot, q), pause(2);

figure, plot(fs), title('Sum of squares vs. iteration step')

%compute the gamma functions, each function fi corresponds to a loop closing
%equation in the 3RRR mechanism, given the current estimates of beta
%the theta (theta1, theta2, theta3) may be considered here as constants,
%since they are known values for the direct kinematic solution
function f=compute_gamma_values(beta, theta)

global xQ yQ xR yR b1 b2 b3 a1 a2 a3 h

%just assign the values to some variables, so that the equations are easier
%to read
xA=beta(1);
yA=beta(2);
Phi=beta(3);
phi1=beta(4);
phi2=beta(5);
phi3=beta(6);

%known values of the direct kinematic problems
th1=theta(1);
th2=theta(2);
th3=theta(3);

%Loop closing equations for the 3RRR mechanism
f1=eval('xA-a1*cos(th1)-b1*cos(th1+phi1)');
f2=eval('yA-a1*sin(th1)-b1*sin(th1+phi1)');
f3=eval('xA-xQ-a2*cos(th2)-b2*cos(th2+phi2)+h*cos(Phi)');
f4=eval('yA-yQ-a2*sin(th2)-b2*sin(th2+phi2)+h*sin(Phi)');
f5=eval('xA-xR-a3*cos(th3)-b3*cos(th3+phi3)+h*cos(Phi+pi/3)');
f6=eval('yA-yR-a3*sin(th3)-b3*sin(th3+phi3)+h*sin(Phi+pi/3)');

%form vector with values,
%note that the f functions are the loop closing equations that should be
%equal to zero
f=[f1 f2 f3 f4 f5 f6]';

%Compute Jacobian with respect to beta parameters
%xA yA Phi, ph1, ph2, ph3
function J=compute_jacobian_beta(beta, theta)

global xQ yQ xR yR b1 b2 b3 a1 a2 a3 h

%just assign the values to some variables, so that the equations are easier
%to read
%xA=beta(1);
%yA=beta(2);
Phi=beta(3);
ph1=beta(4);
ph2=beta(5);
ph3=beta(6);

%known values of the direct kinematic problems
th1=theta(1);
th2=theta(2);
th3=theta(3);

%Compute Jacobian
%Gamma1 with respect to xA yA Phi, ph1, ph2, ph3
J11=1;
J12=0;
J13=0;
J14=b1*sin(th1+ph1);
J15=0;
J16=0;

%Gamma2 with respect to xA yA Phi, ph1, ph2, ph3
J21=0;
J22=1;
J23=0;
J24=-b1*cos(th1+ph1);
J25=0;
J26=0;

%Gamma3 with respect to xA yA Phi, ph1, ph2, ph3
J31=1;
J32=0;
J33=-h*sin(Phi);
J34=0;
J35=b2*sin(th2+ph2);
J36=0;

%Gamma4 with respect to xA yA Phi, ph1, ph2, ph3
J41=0;
J42=1;
J43=h*cos(Phi);
J44=0;
J45=-b2*cos(th2+ph2);
J46=0;

%Gamma5 with respect to xA yA Phi, ph1, ph2, ph3
J51=1;
J52=0;
J53=-h*sin(Phi+pi/3);
J54=0;
J55=0;
J56=b3*sin(th3+ph3);

%Gamma6 with respect to xA yA Phi, ph1, ph2, ph3
J61=0;
J62=1;
J63=h*cos(Phi+pi/3);
J64=0;
J65=0;
J66=-b3*cos(th3+ph3);

J=[J11 J12 J13 J14 J15 J16;
   J21 J22 J23 J24 J25 J26;
   J31 J32 J33 J34 J35 J36;
   J41 J42 J43 J44 J45 J46;
   J51 J52 J53 J54 J55 J56;
   J61 J62 J63 J64 J65 J66];


function z = entropy(x)
% Compute entropy z=H(x) of a discrete variable x.
% Input:
%   x: a integer vectors
% Output:
%   z: entropy z=H(x)
% Written by Mo Chen (sth4nth@gmail.com).
n = numel(x);
[~,~,x] = unique(x);
Px = accumarray(x, 1)/n;
Hx = -dot(Px,log2(Px));
z = max(0,Hx);


% Basic demo for fitting mixture of Gaussians
% This demo first simulates a noisy MoG density profile, whose mode
% position and width are then estimated using VBA.

close all
clear all
clc

%---- simulate noisy data
% Choose basic settings for simulations
sigma = 1e-1;            % precision
phi = [5;-1;1;3];         % observation parameters
g_fname = @g_Gaussian; % observation function
inG.grid = -10:0.1:10;  % grid on which the gbf is evaluated
inG.input = zeros(length(inG.grid),1);
inG.input(50) = 1;
% Build simulated observations
[gx] = feval(g_fname,[],phi,[],inG);
y = gx + sqrt(sigma.^-1)*randn(size(gx));
% display time series of hidden states and observations
figure,
plot(y','ro')
hold on;
plot(gx')

%---- Invert model on simulated data
% Build priors structure
priors.muPhi = zeros(4,1); % prior mean on observation params
priors.SigmaPhi = 1e0*eye(4); % prior covariance on observation params
priors.a_sigma = 1; % Jeffrey's prior
priors.b_sigma = 1; % Jeffrey's prior
options.priors = priors; % include priors in options structure
options.inG = inG; % input structure (grid)
dim.n_phi = 4; % nb of observation parameters
dim.n_theta = 0; % nb of evolution parameters
dim.n = 0;  % nb of hidden states
% Call inversion routine
[posterior,out] = VBA_NLStateSpaceModel(y,[],[],g_fname,dim,options);

%---- Display results
displayResults(posterior,out,y,[],[],[],phi,[],sigma);


function [x, c, funVal, ValueL]=mcLogisticC(A, y, z, opts)
%
%%
% Function mcLeastR:
%      Logistic Loss for Multi-class (task) Learning
%             with the (group) L2/L1-norm Constraint
%
%% Problem
%
%  min  - sum_{il} weight_{il} log( p_{il} )
%  s.t.  sum_j ||x^j||_q <=z
%
%  p_{il}= 1 / (1+ exp(-y_i (x_i' * a_i + c_l) ) ) denotes the probability
%  weight_{il} is the weight for the i-th sample in the l-th classifier
%                is a m x k matrix
%  c_l is the intercept for the l-th classfier, and is a 1xk vector
%  x_i denotes the i-th column of x
%  x^j denotes the j-th row of x
%  a_i' denotes the i-th row of A
%
%  y_i (either 1 or -1) is the response
%
%  In this implementation, we assume weight_{il}=1/(mk)
%
%  When y are of binary values (1 and -1), this problem is the
%  multi-class classificaiton problem, which learns the projection
%  matrix x, by sharing information across k classification tasks (with the
%  Lq/L1-norm Regularization)
%
%  For the case that the multi tasks have separate data
%  matrix, please refer to the functions:
%           mtLeastR and mtLogisticR.
%
%% Input parameters:
%
%  A-         Matrix of size m x n
%                A can be a dense matrix
%                         a sparse matrix
%                         or a DCT matrix
%  y -        Response vector (of size m x k)
%  z -        L_1/L_q norm regularization parameter (z >=0)
%  opts-      Optional inputs (default value: opts=[])
%
%% Output parameters:
%  x-         The obtained weight of size n x k
%  c-         The obtained intercept if size 1 x k
%  funVal-    Function value during iterations
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified on February 19, 2010.
%
%% Related papers
%
% [1]  Jun Liu, Shuiwang Ji, and Jieping Ye, Multi-Task Feature Learning
%      Via Efficient L2,1-Norm Minimization, UAI, 2009
%
% [2]  Jun Liu, Lei Yuan, Songcan Chen and Jieping Ye, Multi-Task Feature Learning
%      Via Efficient L2,1-Norm Minimization, Technical Report ASU, 2009.
%
%% Related functions:
%
%  sll_opts, initFactor, pathSolutionLeast
%  mcLeastR, mcLeastC, mcLogisticR, ep21d
%
%%

%% Verify and initialize the parameters
%%
if (nargin <3)
    error('\n Inputs: A, y and z should be specified!\n');
end

[m,n]=size(A);

k=size(y,2);

if (size(y,1) ~=m)
    error('\n Check the length of y!\n');
end

if (z<=0)
    error('\n z should be positive!\n');
end

opts=sll_opts(opts); % run sll_opts to set default values (flags)

%% Detailed initialization
%% Normalization

% Please refer to sll_opts for the definitions of mu, nu and nFlag
%
% If .nFlag =1, the input matrix A is normalized to
%                     A= ( A- repmat(mu, m,1) ) * diag(nu)^{-1}
%
% If .nFlag =2, the input matrix A is normalized to
%                     A= diag(nu)^{-1} * ( A- repmat(mu, m,1) )
%
% Such normalization is done implicitly
%     This implicit normalization is suggested for the sparse matrix
%                                    but not for the dense matrix
%

if (opts.nFlag~=0)
    if (isfield(opts,'mu'))
        mu=opts.mu;
        if(size(mu,2)~=n)
            error('\n Check the input .mu');
        end
    else
        mu=mean(A,1);
    end

    if (opts.nFlag==1)
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=n)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,1)/m).^(0.5); nu=nu';
        end
    else % .nFlag=2
        if (isfield(opts,'nu'))
            nu=opts.nu;
            if(size(nu,1)~=m)
                error('\n Check the input .nu!');
            end
        else
            nu=(sum(A.^2,2)/n).^(0.5);
        end
    end

    ind_zero=find(abs(nu)<= 1e-10);    nu(ind_zero)=1;
    % If some values in nu is typically small, it might be that,
    % the entries in a given row or column in A are all close to zero.
    % For numerical stability, we set the corresponding value to 1.
end

if (~issparse(A)) && (opts.nFlag~=0)
    fprintf('\n -----------------------------------------------------');
    fprintf('\n The data is not sparse or not stored in sparse format');
    fprintf('\n The code still works.');
    fprintf('\n But we suggest you to normalize the data directly,');
    fprintf('\n for achieving better efficiency.');
    fprintf('\n -----------------------------------------------------');
end

%% Group Property

% Initialize q
if (~isfield(opts,'q'))
    q=2; opts.q=2;
else
    q=opts.q;
    if (q~=2)
        error('\n The current program only supports q=2');
    end
end

%% Starting point initialization

% % % compute AT y
% % if (opts.nFlag==0)
% %     ATy =A'* y;
% % elseif (opts.nFlag==1)
% %     ATy= A'* y -  mu' * sum(y, 1);    ATy=ATy./repmat(nu, 1, k);
% % else
% %     invNu=y./repmat(nu, 1, k);        ATy=A'*invNu-mu' * sum(invNu, 1);
% % end

p_flag=(y==1);                  % the indices of the postive samples
m1=sum(p_flag,1);               % the total number of the positive samples
m2=m-m1;                        % the total number of the negative samples
% m1 and m2 are 1 x k vectors

% initialize a starting point
if opts.init==2
    x=zeros(n,k); c=zeros(1,k);
else
    if isfield(opts,'x0')
        x=opts.x0;
        if ( size(x,1)~=n && size(x,2)~=k )
            error('\n Check the input .x0');
        end
    else
        x=zeros(n,k);
    end

    if isfield(opts,'c0')
        c=opts.c0;

        if ( length(c)~=k )
            error('\n Check the input .c0');
        end
    else
        c=log(m1./m2);
    end
end

% compute Ax= A * x
if (opts.nFlag==0)
    Ax=A* x;
elseif (opts.nFlag==1)
    invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
    Ax=A*invNu -repmat(mu_invNu, m, 1);
else
    Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
end

%% The main program

%% The Armijo Goldstein line search schemes

if (opts.lFlag==0)

    lambda0=0;
    % a guess of the root

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/(m*k); % the intial guess of the Lipschitz continuous gradient

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);
    cp=c;         ccp=zeros(1,k);

    alphap=0; alpha=1;

    for iterStep=1:opts.maxIter
        % --------------------------- step 1 ---------------------------
        % compute search point s based on xp and x (with beta)
        beta=(alphap-1)/alpha;    s=x + beta* xxp;  sc=c + beta* ccp;

        % --------------------------- step 2 ---------------------------
        % line search for L and compute the new approximate solution x

        % compute the gradient (g) at s
        As=Ax + beta* (Ax-Axp);

        % aa= - diag(y) * (A * s + sc)
        aa=- y.*(As+ repmat(sc,m,1));

        % fun_s is the logistic loss at the search point
        bb=max(aa,0);
        fun_s= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

        % compute prob=[p_1;p_2;...;p_m]
        prob=1./( 1+ exp(aa) );

        % b= - diag(y) * (1 - prob)
        b= -y.*(1-prob) / (m * k);

        gc=sum(b); % the gradient of c

        % compute g= AT b, the gradient of x
        if (opts.nFlag==0)
            g=A'*b;
        elseif (opts.nFlag==1)
            g= A'* b -  mu' * sum(b, 1);  g=g./repmat(nu, 1, k);
        else
            invNu=b./repmat(nu, 1, k);        g=A'*invNu-mu' * sum(invNu, 1);
        end

        % copy x and Ax to xp and Axp
        xp=x;    Axp=Ax;
        cp=c;

        while (1)
            % let s walk in a step in the antigradient of s to get v
            % and then do the Lq/L1-norm regularized projection
            v=s-g/L; c= sc- gc/L;

            % Lq/L1-norm regularized projection
            [x, lambda, zf_step]=ep21d(v, n, k, z, lambda0);
            lambda0=lambda;

            v=x-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute Ax= A * x
            if (opts.nFlag==0)
                Ax=A* x;
            elseif (opts.nFlag==1)
                invNu=x./repmat(nu, 1, k);  mu_invNu=mu * invNu;
                Ax=A*invNu -repmat(mu_invNu, m, 1);
            else
                Ax=A*x-repmat(mu*x, m, 1);     Ax=Ax./repmat(nu, 1, k);
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Ax+ repmat(c,m,1));

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

            r_sum=(norm(v,'fro')^2 + norm(c-sc,2)^2) / 2;
            l_sum=fun_x - fun_s - sum(sum(v.* g)) - (c-sc)* gc';

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + <v, g> + <c ,gc>
            %                           + L/2 * (<v,v> + <c-sc,c-sc> )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        % --------------------------- step 3 ---------------------------
        % update alpha and alphap, and check whether converge
        alphap=alpha; alpha= (1+ sqrt(4*alpha*alpha +1))/2;

        ValueL(iterStep)=L;
        % store values for L

        xxp=x-xp;  ccp=c-cp;

        funVal(iterStep)=fun_x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end

%% adpative line search

if (opts.lFlag==1)

    lambda0=0;
    % a guess of the root

    bFlag=0; % this flag tests whether the gradient step only changes a little

    L=1/(m*k); % the intial guess of the Lipschitz continuous gradient

    gamma=1;
    % we shall set the value of gamma = L,
    % and L is appropriate for the starting point

    % assign xp with x, and Axp with Ax
    xp=x; Axp=Ax; xxp=zeros(n,k);
    cp=c;         ccp=zeros(1,k);

    for iterStep=1:opts.maxIter
        while (1)
            if (iterStep~=1)
                alpha= (-gamma+ sqrt(gamma*gamma + 4* L * gamma)) / (2*L);
                beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);
                % beta is the coefficient for generating search point s

                s=x + beta* xxp; sc=c + beta* ccp;
                As=Ax + beta* (Ax-Axp);
            else
                alpha= (-1+ sqrt(5)) / 2;  beta=0;
                s=x; sc= c;
                As=Ax;
            end

            % aa= - diag(y) * (A * s + sc)
            aa=- y.*(As+ repmat(sc,m,1));

            % fun_s is the logistic loss at the search point
            bb=max(aa,0);
            fun_s= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

            % compute prob=[p_1;p_2;...;p_m]
            prob=1./( 1+ exp(aa) );

            % b= - diag(y) * (1 - prob)
            b= -y.*(1-prob) / (m * k);

            gc=sum(b); % the gradient of c

            % compute g= AT b, the gradient of x
            if (opts.nFlag==0)
                g=A'*b;
            elseif (opts.nFlag==1)
                g= A'* b -  mu' * sum(b, 1);  g=g./repmat(nu, 1, k);
            else
                invNu=b./repmat(nu, 1, k);        g=A'*invNu-mu' * sum(invNu, 1);
            end

            % let s walk in a step in the antigradient of s to get v
            % and then do the Lq/L1-norm regularized projection
            v=s-g/L; cnew= sc- gc/L;

            % projection
            [xnew, lambda, zf_step]=ep21d(v, n, k, z, lambda0);
            lambda0=lambda;

            v=xnew-s;  % the difference between the new approximate solution x
            % and the search point s

            % compute A * xnew
            if (opts.nFlag==0)
                Axnew=A* xnew;
            elseif (opts.nFlag==1)
                invNu=xnew./nu; mu_invNu=mu * invNu;
                Axnew=A*invNu -repmat(mu_invNu, m, 1);
            else
                Axnew=A*xnew-repmat(mu*xnew, m, 1);     Axnew=Axnew./nu;
            end

            % aa= - diag(y) * (A * x + c)
            aa=- y.*(Axnew+ repmat(cnew,m,1));

            % fun_x is the logistic loss at the new approximate solution
            bb=max(aa,0);
            fun_x= sum(sum ( log( exp(-bb) +  exp(aa-bb) ) + bb ) ) / (m * k);

            r_sum=(norm(v,'fro')^2 + norm(cnew-sc,2)^2) / 2;
            l_sum=fun_x - fun_s - sum(sum(v.* g)) - (cnew-sc)* gc';

            if (r_sum <=1e-20)
                bFlag=1; % this shows that, the gradient step makes little improvement
                break;
            end

            % the condition is fun_x <= fun_s + <v, g> + <c ,gc>
            %                           + L/2 * (<v,v> + <c-sc,c-sc> )
            if(l_sum <= r_sum * L)
                break;
            else
                L=max(2*L, l_sum/r_sum);
                % fprintf('\n L=%5.6f',L);
            end
        end

        gamma=L* alpha* alpha;    alphap=alpha;
        % update gamma, and alphap

        ValueL(iterStep)=L;
        % store values for L

        tao=L * r_sum / l_sum;
        if (tao >=5)
            L=L*0.8;
        end
        % decrease the value of L

        xp=x;     Axp=Ax;
        x=xnew;   Ax=Axnew;
        cp=c; c=cnew;
        % update x and Ax with xnew and Axnew

        xxp=x-xp;  ccp=c-cp;

        funVal(iterStep)=fun_x;

        if (bFlag)
            % fprintf('\n The program terminates as the gradient step changes the solution very small.');
            break;
        end

        switch(opts.tFlag)
            case 0
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <= opts.tol)
                        break;
                    end
                end
            case 1
                if iterStep>=2
                    if (abs( funVal(iterStep) - funVal(iterStep-1) ) <=...
                            opts.tol* funVal(iterStep-1))
                        break;
                    end
                end
            case 2
                if ( funVal(iterStep)<= opts.tol)
                    break;
                end
            case 3
                norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol)
                    break;
                end
            case 4
                norm_xp=norm(xp,'fro');    norm_xxp=norm(xxp,'fro');
                if ( norm_xxp <=opts.tol * max(norm_xp,1))
                    break;
                end
            case 5
                if iterStep>=opts.maxIter
                    break;
                end
        end
    end
end


function A = ambiguityfunction(f,g)
%AMBIGUITYFUNCTION Ambiguity function
%   Usage: A = ambiguityfunction(f);
%          A = ambiguityfunction(f,g);
%
%   Input parameters:
%         f,g    : Input vector(s).
%
%   Output parameters:
%         A      : ambiguity function
%
%   `ambiguityfunction(f)` computes the (symmetric) ambiguity function of f.
%   The ambiguity function is computed as the two-dimensional Fourier transform
%   of the Wigner-Ville distribution |wignervilledist|.
%
%   **WARNING**: The quadratic time-frequency distributions are highly
%   redundant. For an input vector of length L, the quadratic time-frequency
%   distribution will be a $L \times L$ matrix.

% AUTHOR: Jordy van Velthoven
% TESTING: TEST_AMBIGUITYFUNCTION
% REFERENCE: REF_AMBIGUITYFUNCTION

upfname = upper(mfilename);
complainif_notenoughargs(nargin, 1, upfname);
complainif_toomanyargs(nargin, 2, upfname);

[f,~,W]=comp_sigreshape_pre(f,upfname);

if W>1
   error('%s: Only one-dimensional vectors can be processed.',upfname);
end

if (nargin == 1)
  if isreal(f)
    z1 = comp_fftanalytic(f);
  else
    z1 = f;
  end
  z2 = z1;

elseif (nargin == 2)
  [g,~,W]=comp_sigreshape_pre(g,upfname);
  if W>1
     error('%s: Only one-dimensional vectors can be processed.',upfname);
  end

  if ~all(size(f)==size(g))
  	error('%s: f and g must have the same length.', upfname);
  end;

  if xor(isreal(f), isreal(g))
      error('%s: One input is real, the other one must be real too. ',...
            upfname);
  end

  if isreal(f) || isreal(g)
    z1 = comp_fftanalytic(f);
    z2 = comp_fftanalytic(g);
  else
    z1 = f;
    z2 = g;
  end;
end

R = comp_instcorrmat(z1, z2);

A = fftshift(fft2(fft(R)));


function [ h, m, s ] = frac_to_hms ( f )

%*****************************************************************************80
%
%% FRAC_TO_HMS converts a fractional day into hours, minutes, seconds.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 April 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real F, a day fraction between 0.0 and 1.0.
%
%    Output, integer H, M, S, the equivalent hours, minutes
%    and seconds.
%
  f2 = f;

  f2 = 24.0 * f2;
  h = floor ( f2 );
  f2 = f2 - h;

  f2 = 60.0 * f2;
  m = floor ( f2 );
  f2 = f2 - m;

  f2 = 60.0 * f2;
  s = floor ( f2 );
  f2 = f2 - s;

  return
end


function [B,L,var,fac,E] = FA(X,s)
% FA Factor analysis (principal factoring technique)
%
% The reason to favour this method opposite to maximum likelihood
% (Statistics toolbox) is no distributional assumptions. In comparison to
% SPSS program, this provides the same results. We expect identical
% numerical solution, especially advantage of singular value decomposition.
% Initial estimates of communalities are set as squared multiple
% correlations. In iteration process (convergence criterium 0.001, limit
% for iteration 75), communalities are set to one if Heywood case occurs.
% The loadings are rotated (normalized varimax rotation, convergence
% criterium 0.00001, iteration limit 35). Estimate of rotated factors is
% based on regression approach.
%
% [B,L,var,fac,E] = FA(X,s)
%
% Inputs:
%   X is the data matrix (columns as variables, X must have more
%     than one row and more than one column).
%
%   s is the number of extracted factors (if this parameter is
%     not included, number is chosen by principal component
%     criterion with eigenvalues greater than or equal to one).
%
% Results:
%   B is the matrix of factor loadings (unrotated), last column
%     of matrix B indicates an extraction estimate of
%     communalities.
%
%   L is the varimax rotated loadings matrix.
%
%   var describes the variability proportions, last item is
%     the cumulative variance proportion.
%
%   fac is the matrix of factors.
%
%   E is the matrix of specific variances.
%
% Example: From the data containing temperature, relative humidity, wind
% speed, radiation, NO/NO2 ratio and ozone gained in boundary layer
%  [B,L,var] = FA(X,3)
%   The number of factors extracted: 3
%   Factorial number of iterations: 14
%   Rotational number of iterations: 5
%
%     B =
%        -0.962    0.145    0.088    0.954
%         0.964   -0.065   -0.230    0.986
%        -0.551    0.189    0.061    0.343
%        -0.557    0.422   -0.444    0.685
%         0.718    0.649    0.238    0.994
%        -0.975   -0.077    0.081    0.963
%
%     L =
%        -0.850   -0.308   -0.370
%         0.913    0.322    0.220
%        -0.516   -0.087   -0.262
%        -0.291   -0.083   -0.771
%         0.297    0.948    0.087
%        -0.807   -0.495   -0.258
%
%     var =
%         0.441    0.226    0.154    0.821
%
% The biggest values of rotated loadings points dependence of tempetature,
% relative humidity, wind speed and ozone in the first factor and NO/NO2
% ratio with ozone in another.
%
% Communalities show the biggest relation of NO/NO2 ratio to the others. Be
% careful, variances need not be in decreasing order.
%
% For citation ensue following format: Malec L., Skacel F., Trujillo-Ortiz
% A.(2007). FA: Factor analysis by principal factoring. A MATLAB file.
% [WWW document]. URL http://www.mathworks.com/matlabcentral/fileexchange/
% loadFile.do?objectId=14115
%
% Golub G. H., Van Loan C. F.: Matrix Computations. The Johns Hopkins
% University Press, Baltimore 1996.
% Harman H. H.: Modern Factor Analysis. The University of Chicago Press,
% Chicago 1976.
% Krzanowski W. J.: Principles of Multivariate Analysis. Oxford University
% Press, Oxford 2003.
%
% Copyright. February 26, 2007.

[m,n] = size(X);

if nargin==2 && n<s
  error 'The number of factors requested(s) is too large'
end

% Variables standardization and evaluation of correlation matrix.
e = ones(m,1);
X = (X - e * mean(X)) ./ (e * std(X));
R = cov(X);

% Maximization of variance of original variables.
a = svd(R,0);

% Number of factors according roots of principal components.
if nargin<2
   f = a>=1;
   s = sum(f);
end

fprintf ('The number of factors extracted: %.i\n', s);

% Communality estimation by coefficients of multiple correlation.
c = ones(n,1) - 1 ./ diag(R \ diag(ones(n,1)));
g = [];

% Iteration cycle which maximizes factors correlations.
for k = 1:75
    [U,D,V] = svd(R - diag(diag(R)) + diag(c),0);
    N = V * sqrt(D(:,1:s));
    p = c;
    c = sum(N.^2,2);
    g = [g find(c>1)']; % Heywood case in communality estimates.
    c(g) = 1;
    if max(abs(c - p))<0.001
       break
    end
end

fprintf ('Factorial number of iterations: %.i\n', k);

% Evaluation of factor loadings and communalities estimations.
B = [N c];

% Normalization of factor loadings.
h = sqrt(c);
N = N ./ repmat(h,1,s);

% Initial choice of eigenvalues and loadings labelling.
L = N;
z = 0;

% Iteration cycle maximizing variance of individual columns.
for l = 1:35
    [A,S,M] = svd(N' * (n * L.^3 - L * diag(sum(L.^2))),0);
    L = N * A * M';
    b = z;
    z = sum(diag(S));
    if abs(z - b)<0.00001
       break
    end
end

fprintf ('Rotational number of iterations: %.i\n', l);

% Unnormalization of factor loadings.
L = L .* repmat(h,1,s);

% Factors computation by regression and variance proportions.
t = sum(L.^2) / n;
var = [t sum(t)];
fac = X * (R \ diag(ones(n,1))) * L;

% Evaluation of given factor model variance specific matrix.
r = diag(R) - sum(L.^2,2);
E = R - L * L' - diag(r);


function x = log_series_cdf_inv ( cdf, a )

%*****************************************************************************80
%
%% LOG_SERIES_CDF_INV inverts the Logarithmic Series CDF.
%
%  Discussion:
%
%    Simple summation is used.  The only protection against an
%    infinite loop caused by roundoff is that X cannot be larger
%    than 1000.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 December 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, the parameter of the PDF.
%    0.0 < A < 1.0.
%
%    Output, real X, the argument of the CDF for which
%    CDF(X-1) <= CDF <= CDF(X).
%
  xmax = 1000;

  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LOG_SERIES_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'LOG_SERIES_CDF_INV - Fatal error!' );
  end

  cdf2 = 0.0;
  x = 1;

  while ( cdf2 < cdf & x < xmax )

    if ( x == 1 )
      pdf = - a / log ( 1.0 - a );
    else
      pdf = ( x - 1 ) * a * pdf / x;
    end

    cdf2 = cdf2 + pdf;

    x = x + 1;

  end

  return
end


%PCA Principal component analysis (PCA or MCA on overall covariance matrix)
%
%   [W,FRAC] = PCA(A,N)
%   [W,N]    = PCA(A,FRAC)
%
% INPUT
%   A           Dataset
%   N  or FRAC  Number of dimensions (>= 1) or fraction of variance (< 1)
%               to retain; if > 0, perform PCA; otherwise MCA. Default: N = inf.
%
% OUTPUT
%   W           Affine PCA mapping
%   FRAC or N   Fraction of variance or number of dimensions retained.
%
% DESCRIPTION
% This routine performs a principal component analysis (PCA) or minor
% component analysis (MCA) on the overall covariance matrix (weighted
% by the class prior probabilities). It finds a rotation of the dataset A to
% an N-dimensional linear subspace such that at least (for PCA) or at most
% (for MCA) a fraction FRAC of the total variance is preserved.
%
% PCA is applied when N (or FRAC) >= 0; MCA when N (or FRAC) < 0. If N is
% given (abs(N) >= 1), FRAC is optimised. If FRAC is given (abs(FRAC) < 1),
% N is optimised.
%
% Objects in a new dataset B can be mapped by B*W, W*B or by A*PCA([],N)*B.
% Default (N = inf): the features are decorrelated and ordered, but no
% feature reduction is performed.
%
% ALTERNATIVE
%
%   V = PCA(A,0)
%
% Returns the cumulative fraction of the explained variance. V(N) is the
% cumulative fraction of the explained variance by using N eigenvectors.
%
% Use KLM for a principal component analysis on the mean class covariance.
% Use FISHERM for optimizing the linear class separability (LDA).
%
% SEE ALSO
% MAPPINGS, DATASETS, PCLDC, KLLDC, KLM, FISHERM

% Copyright: R.P.W. Duin, duin@ph.tn.tudelft.nl
% Faculty of Applied Sciences, Delft University of Technology
% P.O. Box 5046, 2600 GA Delft, The Netherlands

% $Id: pca.m,v 1.2 2006/03/08 22:06:58 duin Exp $

function [w,truefrac] = pca (varargin)

	prtrace(mfilename);

	[w,truefrac] = pcaklm(mfilename,varargin{:});

	return


function G = curl(F)
%CURL  Numerical surface curl.
%   G = CURL(F) returns the numerical surface curl of the SPHEREFUNV F.
%   This only makes mathematical sense for a tangential vector field.
%
% See also SPHEREFUNV/DIVERGENCE, SPHEREFUN/GRADIENT, SPHEREFUNV/VORTICITY.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Empty check.
if ( isempty(F) )
    G = spherefunv;
    return
end

Fc = F.components;

% The following derivatives are all tangential derivatives.

% First component: d/dy(fz) - d/dz(fy)
gx = diff(Fc{3}, 2) - diff(Fc{2}, 3);

% Second component: d/dz(fx) - d/dx(fz)
gy = diff(Fc{1}, 3) - diff(Fc{3}, 1);

% Third component: d/dx(fy) - d/dy(fx)
gz = diff(Fc{2}, 1) - diff(Fc{1}, 2);

G = spherefunv(gx,gy,gz);

end


function r8bto_print ( m, l, a, title )

%*****************************************************************************80
%
%% R8BTO_PRINT prints a R8BTO matrix.
%
%  Discussion:
%
%    The R8BTO storage format is for a block Toeplitz matrix. The matrix
%    can be regarded as an L by L array of blocks, each of size M by M.
%    The full matrix has order N = M * L.  The L by L matrix is Toeplitz,
%    that is, along its diagonal, the blocks repeat.
%
%    Storage for the matrix consists of the L blocks of the first row,
%    followed by the L-1 blocks of the first column (skipping the first row).
%    These items are stored in the natural way in an (M,M,2*L-1) array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the order of the blocks of the matrix A.
%
%    Input, integer L, the number of blocks in a row or column of A.
%
%    Input, real A(M,M,2*L-1), the R8BTO matrix.
%
%    Input, string TITLE, a title to be printed.
%
  r8bto_print_some ( m, l, a, 1, 1, m*l, m*l, title );

  return
end


function [tx, ty] = orientedBoxToPolygon(obox)
%ORIENTEDBOXTOPOLYGON Convert an oriented box to a polygon (set of vertices)
%
%   POLY = orientedBoxToPolygon(OBOX);
%   Converts the oriented box OBOX given either as [XC YC W H] or as
%   [XC YC W H THETA] into a 4-by-2 array of double, containing coordinates
%   of box vertices.
%   XC and YC are center of the box. W and H are the width and the height
%   (dimension in the main directions), and THETA is the orientation, in
%   degrees between 0 and 360.
%
%   Example
%     OBOX = [20 10  40 20 0];
%     RECT = orientedBoxToPolygon(OBOX)
%     RECT =
%         -20 -10
%          20 -10
%          20  10
%         -20  10
%
%
%   See also:
%   polygons2d, orientedBox, drawOrientedBox, rectToPolygon
%

%   ---------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% INRA - TPV URPOI - BIA IMASTE
% created the 06/04/2005.
%

%   HISTORY
%   2011-10-09 rewrite from rectAsPolygon to orientedBoxToPolygon
%   2016: Simplify by JuanPi Carbajal

% extract box parameters
theta = 0;
x = obox(1);
y = obox(2);
w = obox(3) / 2;  % easier to compute with w and h divided by 2
h = obox(4) / 2;
if length(obox) > 4
    theta = obox(5);
end

v = [cosd(theta); sind(theta)];
M = bsxfun (@times, [-1 1; 1 1; 1 -1; -1 -1], [w h]);
tx  = x + M * v;
ty  = y + M(4:-1:1,[2 1]) * v;

if nargout <= 1
  tx = [tx ty];
end


function varargout = sltensor_svd(T, n)
%SLTENSOR_SVD Performs a Higher-Order SVD on a Tensor
%
% $ Syntax $
%   - [S, U1, U2, ...] = sltensor_svd(T)
%   - [S, U1, U2, ...] = sltensor_svd(T, n)
%   - [S, Us] = sltensor_svd(...)
%
% $ Description $
%   - [S, U1, U2, ...] = sltensor_svd(T) Performs a Higher Order Singular Value
%   Decomposition to a Tensor T. In the output arguments, S is the core
%   tensor, U1, U2, ... are the singular vector matrices of mode 1, 2,...
%
%   - [S, U1, U2, ...] = sltensor_svd(T, n) Here n specifies the order of the
%   tensor T, n should be not less than ndims(T). If n > ndims(T) is just
%   regarded as a tensor of dimensions d1xd2x...x1.
%
%   - [S, Us] = sltensor_svd(...) where all mode matrices are returned to Us,
%   which is an 1 x n cell array, with each cell containing a mode matrix.
%
% $ History $
%   - Created by Dahua Lin on Dec 17th, 2005
%

%% parse and verify the arguments
slchknargs(nargin, 1);
n0 = ndims(T);
if nargin == 1
    n = n0;
else
    if n < n0;
        error('sltoolbox:invalidarg', ...
            'The order %d is too small', n);
    end
end
if nargout > 2 && nargout ~= n+1
    error('sltoolbox:invalidnargout', ...
        'The number of outputs is not valid');
end

%% compute

Us = cell(n, 1);
S = T;
for i = 1 : n0
    M = sltensor_unfold(T, i);
    [d1, d2] = size(M);
    if d1 < d2
        [V, D, U] = svd(M', 0);
    else
        [U, D, V] = svd(M, 0);
    end
    slignorevars(D, V);
    clear D V;

    Us{i} = U;
    S = sltensor_multiply(S, U', i);
end
if n > n0
    for i = n0+1 : n
        Us{i} = 1;
    end
end

%% output
varargout{1} = S;
if nargout == 2
    varargout{2} = Us;
else
    varargout(2:n+1) = Us;
end


% SCIP  Solve a LP/MILP/QP/MIQP/QCQP/MIQCQP/NLP/MINLP using SCIP
%
% SCIP uses the Solving Constraint Integer Programs library, available
% from: http://scip.zib.de/scip.shtml
%
%   [x,fval,exitflag,stats] = scip(H, f, A, rl, ru, lb, ub, xtype, sos, qc, nl, opts)
%
%   Input arguments*:
%       H - quadratic objective matrix (sparse, optional [NOT TRIL / TRIU])
%       f - linear objective vector
%       A - linear constraint matrix (sparse)
%       rl - linear constraint lhs
%       ru - linear constraint rhs
%       lb - decision variable lower bounds
%       ub - decision variable upper bounds
%       xtype - string of variable integrality ('c' continuous, 'i' integer, 'b' binary)
%       sos - SOS structure with fields type, index and weight (see below)
%       qc - Quadratic Constraints structure with fields Q, l, qrl and qru (see below)
%       nl - Nonlinear Objective and Constraints structure (see below)
%       opts - solver options (see below)
%
%   Return arguments:
%       x - solution vector
%       fval - objective value at the solution
%       exitflag - exit status (see below)
%       stats - statistics structure
%
%   Option Fields (all optional - also see scipset):
%       tolrfun - LP primal convergence tolerance
%       maxiter - maximum LP solver iterations
%       maxnodes - maximum nodes to explore
%       maxtime - maximum execution time [s]
%       display - solver display level [0-5]
%       objbias - constant objective bias term
%
%   Return Status:
%       0 - Unknown
%       1 - User Interrupted
%       2 - Node Limit Reached
%       3 - Total Node Limit Reached
%       4 - Stall Node Limit Reached
%       5 - Time Limit Reached
%       6 - Memory Limit Reached
%       7 - Gap Limit Reached
%       8 - Solution Limit Reached
%       9 - Solution Improvement Limit Reached
%      10 - Restart Limit Reached
%      11 - Problem Solved to Optimality
%      12 - Problem is Infeasible
%      13 - Problem is Unbounded
%      14 - Problem is Either Infeasible or Unbounded
%
%   Special Ordered Sets (SOS):
%       type    - A string containing '1' or '2' for each SOS1 or SOS2 constraint.
%       index   - A double array of the indices of the variables in the SOS,
%                 group multiple SOS index arrays in a cell array.
%       weight  - A double array of the weights of each of the variable
%                 above, indicating variables next to each other. Group
%                 multiple SOS via cell arrays as above.
%
%   Quadratic Constraints (QC) [qrl <= x'Qx + l'x <= qru]:
%       Q       - A sparse double matrix of the quadratic terms for the
%                 constraint, group multiple quadratic constraints via a
%                 cell array of matrices. [NOT TRIL / TRIU]
%       l       - A column vector of the linear terms for the constraint,
%                 group multiple quadratic constraints in a matrix, each
%                 column representing each constraint.
%       qrl     - A scalar representing the quadratic constraint lower
%                 bound. Group multiple quadratic constraints in a
%                 column vector, each row representing each constraint.
%       qru     - A scalar representing the quadratic constraint upper
%                 bound. Group multiple quadratic constraints in a
%                 column vector, each row representing each constraint.
%
%   Nonlinear Objective & Constraints (min f(x) and cl <= c(x) <= cu)
%       See opti_scipnl for usage. Contains multiple fields of instructions
%       lists which the MEX interface parses into SCIP expressions.
%
%   * From OPTI v1.78 scip can also solve AMPL .nl models. Simply pass the FULL path
%	  of the AMPL .nl file to scip as the first argument, and options (if requred) as
%	  the second argument. This interface will also write an AMPL solution file to the
%	  same directory as the input file. Remember SCIP does not solve models with
%	  trigonometric functions.
%
%   Copyright (C) 2012-2013 Jonathan Currie (IPL)


clear, clc;

% This is an example for running the function fusedLeastR
%
%% Problem
%
%  min  1/2 || A x - y||^2 + rho * ||x||_1 +
%       opts.fusedPenalty * sum_i |x_i-x_{i+1}|
%
% For detailed description of the function, please refer to the Manual.
%
%% Related papers
%
% [1]  Jun Liu, Lei Yuan, and Jieping Ye, An Efficient Algorithm for
%      a Class of Fused Lasso Problems, KDD, 2010.
%
%% ------------   History --------------------
%
% First version on 14 November 2009.
%
% For any problem, please contact Jun Liu (j.liu@asu.edu)

cd ..
cd ..

root=cd;
addpath(genpath([root '/SLEP']));
                   % add the functions in the folder SLEP to the path

% change to the original folder
cd Examples/fusedLasso;

m=100;  n=1000;    % The data matrix is of size m x n

% for reproducibility
randNum=1;

% ---------------------- Generate random data ----------------------
randn('state',(randNum-1)*3+1);
A=randn(m,n);       % the data matrix

randn('state',(randNum-1)*3+2);
xOrin=randn(n,1);

randn('state',(randNum-1)*3+3);
noise=randn(m,1);
y=A*xOrin +...
    noise*0.01;     % the response

rho=0.001;          % the regularization parameter
                    % it is a ratio between (0,1), if .rFlag=1

%----------------------- Set optional items ------------------------
opts=[];

% Starting point
opts.init=2;        % starting from a zero point

% termination criterion
opts.tFlag=5;       % run .maxIter iterations
opts.maxIter=500;   % maximum number of iterations

% normalization
opts.nFlag=0;       % without normalization

% regularization
opts.rFlag=1;       % the input parameter 'rho' is a ratio in (0, 1)
%opts.rsL2=0.01;    % the squared two norm term

% fused penalty
opts.fusedPenalty=0.01;

% line search
opts.lFlag=0;

%----------------------- Run the code LeastR -----------------------
tic;
[x1, funVal1, ValueL1]= fusedLeastR(A, y, rho, opts);
toc;

figure;
plot(funVal1,'-.b');
xlabel('Iteration (i)');
ylabel('The objective function value');