function pdf_discrete_test ( )

%*****************************************************************************80
%
%% PDF_DISCRETE_TEST tests PDF_DISCRETE_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PDF_DISCRETE_TEST\n' );
  fprintf ( 1, '  PDF_DISCRETE_VALUE evaluates the PDF associated with a\n' );
  fprintf ( 1, '  discrete histogram.\n' );
%
%  Set up the discrete histogram from sample data.
%
  s_num = 6;
  s_min = 0.0;
  s_max = 10.0;
  s = [ 0.0, 2.0, 2.0, 4.0, 5.0, 8.0 ];
  fprintf ( 1, '\n' );
  fprintf ( 1, '  S_MIN = %g\n', s_min );
  fprintf ( 1, '  S_MAX = %g\n', s_max );
  r8vec_print ( s_num, s, '  Sample data:' );

  [ x_num, x, y ] = setup_discrete ( s_num, s, s_min, s_max );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Discrete histogram data:\n' );
  fprintf ( 1, '\n' );
  for i = 1 : x_num
    fprintf ( 1, '  %10.6g  %10.6g\n', x(i), y(i) );
  end
%
%  Evaluate the discrete PDF.
%
  v_num = 21;
  v = linspace ( s_min, s_max, v_num );
  pdf = pdf_discrete_value ( x_num, x, y, v_num, v );
  r8vec2_print ( v_num, v, pdf, '  Discrete PDF table:' );
%
%  Plot the discrete PDF.
%
  plot ( v, pdf, 'bo-', 'Linewidth', 3 )
  grid on
  xlabel ( '<--- X --->' )
  ylabel ( '<--- PDF(X) --->' )
  title ( 'Discrete PDF function' )

  filename = 'pdf_discrete_test.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Discrete PDF plotted as "%s".\n', filename );

  return
end


function geometry_test035 ( )

%*****************************************************************************80
%
%% TEST035 tests LINE_IMP_POINT_DIST_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;
  ntest = 3;

  atest = [ 2.0, 2.0, 2.0 ];
  btest = [ 5.0, 5.0, 5.0 ];
  ctest = [ 3.0, 3.0, 3.0 ];
  ptest = [ ...
    0.0, 6.0; ...
    0.0, 5.0; ...
    0.0, 4.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST035\n' );
  fprintf ( 1, '  LINE_IMP_POINT_DIST_2D finds the distance from\n' );
  fprintf ( 1, '    a point X Y to a line A * X + B * Y + C = 0.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   X       Y       A       B       C       DIST\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    a = atest(i);
    b = btest(i);
    c = ctest(i);
    p(1:dim_num) = ptest(1:dim_num,i);

    dist = line_imp_point_dist_2d ( a, b, c, p );

    fprintf ( 1, '  %8f  %8f  %8f  %8f  %8f  %8f\n', ...
      p(1:dim_num), a, b, c, dist );

  end

  return
end


%BIPOLAR returns an M-by-3 matrix containing a blue-red colormap, in
%	in which red stands for positive, blue stands for negative,
%	and white stands for 0.
%
%  Usage: cmap = bipolar(M, lo, hi, contrast);  or  cmap = bipolar;
%
%  cmap:  output M-by-3 matrix for BIPOLAR colormap.
%  M:	  number of shades in the colormap. By default, it is the
%	  same length as the current colormap.
%  lo:	  the lowest value to represent.
%  hi:	  the highest value to represent.
%
%  Inspired from the LORETA PASCAL program:
%	http://www.unizh.ch/keyinst/NewLORETA
%
%  jimmy@rotman-baycrest.on.ca
%
%----------------------------------------------------------------
function cmap = bipolar(M, lo, hi, contrast)

   if ~exist('contrast','var')
      contrast = 128;
   end

   if ~exist('lo','var')
      lo = -1;
   end

   if ~exist('hi','var')
      hi = 1;
   end

   if ~exist('M','var')
      cmap = colormap;
      M = size(cmap,1);
   end

   steepness = 10 ^ (1 - (contrast-1)/127);
   pos_infs = 1e-99;
   neg_infs = -1e-99;

   doubleredc = [];
   doublebluec = [];

   if lo >= 0		% all positive

      if lo == 0
         lo = pos_infs;
      end

      for i=linspace(hi/M, hi, M)
         t = exp(log(i/hi)*steepness);
         doubleredc = [doubleredc; [(1-t)+t,(1-t)+0,(1-t)+0]];
      end

      cmap = doubleredc;

   elseif hi <= 0	% all negative

      if hi == 0
         hi = neg_infs;
      end

      for i=linspace(abs(lo)/M, abs(lo), M)
         t = exp(log(i/abs(lo))*steepness);
         doublebluec = [doublebluec; [(1-t)+0,(1-t)+0,(1-t)+t]];
      end

      cmap = flipud(doublebluec);

   else

      if hi > abs(lo)
         maxc = hi;
      else
         maxc = abs(lo);
      end

      for i=linspace(maxc/M, hi, round(M*hi/(hi-lo)))
         t = exp(log(i/maxc)*steepness);
         doubleredc = [doubleredc; [(1-t)+t,(1-t)+0,(1-t)+0]];
      end

      for i=linspace(maxc/M, abs(lo), round(M*abs(lo)/(hi-lo)))
         t = exp(log(i/maxc)*steepness);
         doublebluec = [doublebluec; [(1-t)+0,(1-t)+0,(1-t)+t]];
      end

      cmap = [flipud(doublebluec); doubleredc];

   end

   return;					% bipolar


function d = distance(u, v)
% Euclidean distance between u and v
%
% :Usage:
% ::
%
%     d = distance(u, v)
%
% :Inputs:
%
%   **u** and **v:**
%        should be column vectors or matrices in k-dim space, where k is
%        the number of columns of u and v.
%
%        if u is a single row, replicates u to size(v,1)

if size(u,1) < size(v,1)
    u = repmat(u,size(v,1),1);
    if size(u,1) < size(v,1), error('Inputs must have same number of rows, or one row for 1st input!'),end
end

delt = u - v;
d = (sum(delt .^ 2,2)) .^.5;

return


function geometry_test179 ( )

%*****************************************************************************80
%
%% TEST179 tests SOCCER_SIZE_3D and SOCCER_SHAPE_3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
  dim_num = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST179\n' );
  fprintf ( 1, '  For the truncated icosahedron, or soccer ball,\n' );
  fprintf ( 1, '  SOCCER_SIZE_3D returns dimension information;\n' );
  fprintf ( 1, '  SOCCER_SHAPE_3D returns face and order information.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  We will use this information to compute the\n' );
  fprintf ( 1, '  areas and centers of each face.\n' );

  [ point_num, edge_num, face_num, face_order_max ] = soccer_size_3d ( );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of points =   %d\n', point_num );
  fprintf ( 1, '  Number of edges =    %d\n', edge_num );
  fprintf ( 1, '  Number of faces =    %d\n', face_num );
  fprintf ( 1, '  Maximum face order = %d\n', face_order_max );

  [ point_coord, face_order, face_point ] = soccer_shape_3d ( ...
    point_num, face_num, face_order_max );
%
%  Compute the area of each face.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Face  Order  Area\n' );
  fprintf ( 1, '\n' );

  for i = 1 : face_num

    for j = 1 : face_order(i)
      k = face_point(j,i);
      v(1:dim_num,j) = point_coord(1:dim_num,k);
    end

    [ area, normal ] = polygon_area_3d ( face_order(i), v );

    fprintf ( 1, '  %6d  %5d  %8f\n', i, face_order(i), area );

  end
%
%  Find the center of each face.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Face  Center\n' );
  fprintf ( 1, '\n' );

  for i = 1 : face_num

    vave(1:dim_num) = 0.0;

    for j = 1 : face_order(i)
      k = face_point(j,i);
      vave(1:dim_num) = vave(1:dim_num) + point_coord(1:dim_num,k)';
    end

    vave(1:dim_num) = vave(1:dim_num) / face_order(i);

    fprintf ( 1, '  %6d  %10f  %10f  %10f\n', i, vave(1:dim_num) );

  end

  return
end


%RPY2R Roll-pitch-yaw angles to SO(3) rotation matrix
%
% R = RPY2R(ROLL, PITCH, YAW, OPTIONS) is an SO(3) orthonornal rotation
% matrix (3x3) equivalent to the specified roll, pitch, yaw angles angles.
% These correspond to rotations about the Z, Y, X axes respectively. If
% ROLL, PITCH, YAW are column vectors (Nx1) then they are assumed to
% represent a trajectory and R is a three-dimensional matrix (3x3xN), where
% the last index corresponds to rows of ROLL, PITCH, YAW.
%
% R = RPY2R(RPY, OPTIONS) as above but the roll, pitch, yaw angles are
% taken from the vector (1x3) RPY=[ROLL,PITCH,YAW]. If RPY is a matrix
% (Nx3) then R is a three-dimensional matrix (3x3xN), where the last index
% corresponds to rows of RPY which are assumed to be [ROLL,PITCH,YAW].
%
% Options::
%  'deg'      Compute angles in degrees (radians default)
%  'xyz'      Rotations about X, Y, Z axes (for a robot gripper)
%  'zyx'      Rotations about Z, Y, X axes (for a mobile robot, default)
%  'yxz'      Rotations about Y, X, Z axes (for a camera)
%  'arm'      Rotations about X, Y, Z axes (for a robot arm)
%  'vehicle'  Rotations about Z, Y, X axes (for a mobile robot)
%  'camera'   Rotations about Y, X, Z axes (for a camera)
%
% Note::
% - Toolbox rel 8-9 has XYZ angle sequence as default.
% - ZYX order is appropriate for vehicles with direction of travel in the X
%   direction.  XYZ order is appropriate if direction of travel is in the Z
%   direction.
% - 'arm', 'vehicle', 'camera' are synonyms for 'xyz', 'zyx' and 'yxz'
%   respectively.
%
% See also TR2RPY, EUL2TR.

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

function R = rpy2r(roll, varargin)
    opt.order = {'zyx', 'xyz', 'yxz', 'arm', 'vehicle', 'camera'};
    opt.deg = false;
    [opt,args] = tb_optparse(opt, varargin);

    % unpack the arguments
    if numcols(roll) == 3
		pitch = roll(:,2);
		yaw = roll(:,3);
		roll = roll(:,1);
	elseif nargin >= 3
        pitch = args{1};
        yaw = args{2};
    else
        error('SMTB:rpy2r:badarg', 'bad arguments')
    end

    % optionally convert from degrees
    if opt.deg
        d2r = pi/180.0;
        roll = roll * d2r;
        pitch = pitch * d2r;
        yaw = yaw * d2r;
    end

    switch opt.order
        case {'xyz', 'arm'}
            % XYZ order
            if numrows(roll) == 1
                R = rotx(yaw) * roty(pitch) * rotz(roll);
            else
                R = zeros(3,3,numrows(roll));
                for i=1:numrows(roll)
                    R(:,:,i) = rotx(yaw(i)) * roty(pitch(i)) * rotz(roll(i));
                end
            end
        case {'zyx', 'vehicle'}
            % ZYX order
            if numrows(roll) == 1
                R = rotz(yaw) * roty(pitch) * rotx(roll);
            else
                R = zeros(3,3,numrows(roll));
                for i=1:numrows(roll)
                    R(:,:,i) = rotz(yaw(i)) * roty(pitch(i)) * rotx(roll(i));
                end
            end

        case {'yxz', 'camera'}
            % YXZ order
            if numrows(roll) == 1
                R = roty(yaw) * rotx(pitch) * rotz(roll);
            else
                R = zeros(3,3,numrows(roll));
                for i=1:numrows(roll)
                    R(:,:,i) = roty(yaw(i)) * rotx(pitch(i)) * rotz(roll(i));
                end
            end
    end
end


%------------------------------- getting ready ----------------------------
clc
clear all
close all
%--------------------------------------------------------------------------

global A E
tol  = 0.001;
max  = 20;
init = 0;
xn   = init*ones(2,78);          % estimation for initial trejctory for states
pn   = init*ones(2,78);          % estimation for initial trejctory for co-states
check = [0.01:0.01:0.78];

for loop = 1:max
    xh1 = [0 0 1 0];             % initial conditions for homogeneous solution1
    xh2 = [0 0 0 1];             % initial conditions for homogeneous solution2
    xp0 = [0.05 0 0 0];          % initial conditions for particular  solution0
    a = 0;
    b = 0.01;
    for I=1:78
        t       = [xn(:,I)' pn(:,I)'];
        pointer = 100*loop+I
        clc
        A  = calculate_matrix(t);
        x1 = [-2*(t(1)+0.25)+(t(2)+0.5)*exp(25*t(1)/(t(1)+2))-(t(1)+0.25)^2*t(3)/0.2    0.5-t(2)-(t(2)+0.5)*exp(25*t(1)/(t(1)+2))];
        x2 = [-2*t(1)+2*t(3)-t(3)*(t(2)+0.5)*(50/(t(1)+2)^2)*exp(25*t(1)/(t(1)+2))+t(3)^2*(t(1)+0.25)/0.2+t(4)*(t(2)+0.5)*(50/(t(1)+2)^2)*exp(25*t(1)/(t(1)+2)) -2*t(2)-t(3)*exp(25*t(1)/(t(1)+2))+t(4)*(1+exp(25*t(1)/(t(1)+2)))];
        e1 = [x1 x2]';
        e2 = A*t';
        E  = e1 - e2;

        clear t1 c
        x0  = [xp0 xh1 xh2]';
        [t1,c]  = ode45('stirred_model', [a b], x0);

        L       = length(t1);
        X1p(I)  = c(L,1);
        X2p(I)  = c(L,2);
        P1p(I)  = c(L,3);
        P2p(I)  = c(L,4);
        xp0     = [X1p(I) X2p(I) P1p(I) P2p(I)];

        X11h(I) = c(L,5);
        X12h(I) = c(L,6);
        P11h(I) = c(L,7);
        P12h(I) = c(L,8);
        xh1     = [X11h(I) X12h(I) P11h(I) P12h(I)];

        X21h(I) = c(L,9 );
        X22h(I) = c(L,10);
        P21h(I) = c(L,11);
        P22h(I) = c(L,12);
        xh2     = [X21h(I) X22h(I) P21h(I) P22h(I)];

        % simulation time for next interval of integrtion
        a = b;
        b = b + 0.01;

    end

    % calculting constant to satisfy boudary condition
    B1  = [0 0]'-[P1p(78) P2p(78)]';
    ph1 = [P11h(78) P12h(78)]';
    ph2 = [P21h(78) P22h(78)]';
    A1  = [ph1 ph2];
    const = A1\B1;

    % calculation of states and costates
    state   = [X1p;X2p] + [const(1)*X11h;const(1)*X12h]+[const(2)*X21h;const(2)*X22h];
    costate = [P1p;P2p] + [const(1)*P11h;const(1)*P12h]+[const(2)*P21h;const(2)*P22h];
    test = terminate(state,xn,costate,pn);
    fprintf('Terminate Check = %f\n',test);
    const
    pause
    clc
    if test < tol
        break
    end

    xn = state;
    pn = costate;

end

plot(check,state,'-');
title('Optimal Trejctory');
grid on
%--------------------------------------------------------------------------
for j = 1:78
   u(j)    = 0.5*costate(1,j)*(state(1,j)+0.25);
   intg(j) = state(1,j)^2+state(2,j)^2+0.1*u(j)^2;
end
figure
plot(check,u);
title('U^*');
grid on
J = integrate(check,intg);
fprintf('Optimal Cost = %f\n',J);


function [f, lineSegs, theta] = fov(A, pref)
%FOV   Field of values (numerical range) of matrix A.
%   F = FOV(A), where A is a square matrix, returns a CHEBFUN F with domain [0
%   2*pi]. The image F([0 pi]) is a curve describing the extreme points of the
%   boundary of the field of values A, a convex region in the complex plane.
%
%   For a generic matrix, the boundary of the field of values is smooth and all
%   boundary points are extreme points. If A is normal, the field of values is
%   the convex hull of the eigenvalues of A, so the extreme points consist only
%   of the eigenvalues and hence F has one constant piece for each eigenvalue,
%   so it is not continuous on [0, 2*pi].
%
%   The numerical abscissa of A is equal to max(real(F)), though this is much
%   better computed as max(real(eig(A + A')))/2.
%
%   The algorithm used is that of C. R. Johnson, Numerical determination of the
%   field of values of a general complex matrix, SIAM J. Numer. Anal. 15 (1978),
%   595-602.
%
%   F = FOV(A, PREF) allows the preferences in the CHEBFUNPREF structure PREF to
%   be used in constructing F. Note that PREF.splitting will always be set to
%   TRUE by FOV and the domain will always be [0, 2*pi].
%
%   [F, LINESEGS, THETA] = FOV(A) also returns a quasimatrix LINESEGS whose
%   columns are CHEBFUN objects defining the line segments connecting up the
%   extreme points and a vector THETA specifying the values in [0, 2*pi] where
%   the discontinuities in F occur.
%
% Example 1 (smooth boundary)
%   A = randn(5);
%   F = fov(A);
%   hold off, plot(F, '-b')
%   e = eig(A);
%   hold on, plot(e, '*k'), hold off, axis equal
%
% Example 2 (boundary has a corner)
%   A = [0 1 0 ; 0 0 0 ; 0 0 1];
%   [F, lineSegs] = fov(A);
%   plot(F, '-b'), hold on
%   plot(lineSegs, '-r')
%   e = eig(A);
%   plot(complex(e), '*k'), hold off, axis equal

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

if ( nargin < 2 )
    % Obtain preferences:
    pref = chebfunpref();
end

% Set splitting to 'on' and the domain to [0, 2*pi].
pref.splitting = true;
pref.domain = [0, 2*pi];

% Construct a CHEBFUN of the FOV curve, and try to merge any unnecessary
% breakpoints out of the result:
f = chebfun(@(theta) fovCurve(theta, A), pref);
f = merge(f);

if ( nargout == 1 )
    return
end

%% LINESEGS - implemented by Michael Overton
% Look for possible discontinuities at any break points. The boundary of the
% field of values may have corners, but it is continuous: hence the need
% sometimes for linear interpolation connecting up the sets of extreme points.

ends = f.domain;      % Break points are between 0 and 2*pi.
delta = 1e-14;        % Must be bigger than machine precision but not much
                      % bigger: if it is too small spurious line segments will
                      % have tiny length, which hardly matters.
if ( any(diff(ends) < delta) )
    delta = min(diff(ends))/3;
end
m = length(ends) - 1; % Number of pieces

% Get the values to the left and right of break points. The first break point is
% zero, which wraps around to 2*pi and needs special treatment. Exclude the
% right end point 2*pi.
lVal = feval(f, ends(1:end-1), 'left');  % Values to left of break points.
lVal(1) = feval(f, ends(end),  'left');  % Value to left of 2*pi.
rVal = feval(f, ends(1:end-1), 'right'); % Values to right of break points.

% Determine which points correspond to discontinuities:
tol = 10*delta*max(abs([lVal, rVal]), [], 2);
discont = abs(lVal - rVal) > tol;

% Define additional CHEBFUNs for the line segments joining discontinuities.
% (First idea was to do this by inserting tiny intervals with steep slopes into
% the CHEBFUN f, but this leads to loss of accuracy.) Each column of lineSegs
% represents a line in the complex plane connecting the points lVal(j) and
% rVal(j) to each other, along with the corresponding theta value in [0,2*pi].
theta = ends(discont);
lineSegs = chebfun([lVal(discont) ; rVal(discont)], [-1 1], 'tech', @chebtech2);

end

function z = fovCurve(theta, A)
z = NaN(size(theta));
for j = 1:length(theta)
    r = exp(1i*theta(j));
    B = r*A;
    H = (B + B')/2;
    [X, D] = eig(H);
    [ignored, k] = max(diag(D));
    v = X(:,k);
    z(j) = v'*A*v/(v'*v);
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);
%

initial_theta = zeros(n + 1, 1);
options = optimset('GradObj', 'on', 'MaxIter', 50);

for c=1:num_labels

    y1 = (y==c);
    [theta] = ...
	fmincg(@(t)(lrCostFunction(t, X, y1, lambda)), initial_theta, options);

    all_theta(c,:) = theta;

end;

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

end


function bezedit(alpha, haxes, color, varargin)
    % BEZEDIT Draw and edit Bezier polynomial by dragging control points
    %
    %   BEZEDIT(ALPHA) draws the graph of a 1D Bezier polynomial or
    %   arbitrary degree. Draggable control points are also shown. As the
    %   control points are moved, the graph is updated.
    %
    %   Written by Brian G. Buss

    if nargin<2
        figure;
        haxes = gca;
        set(gca, 'XLim', [-0.2 1.2]);
    end
    if nargin<3
        color = 'b';
    end

    s = 0:0.01:1;
    line(s, bezier_vec(alpha, s), 'LineStyle', ':', 'Color', color, 'Parent', haxes);
    hline=line(s, bezier_vec(alpha, s), 'LineStyle', '-', 'Color', color, 'Parent', haxes, varargin{:});

    for k=1:length(alpha)
        hpoints(k)=impoint(haxes, [(k-1)/5 alpha(k)]);
        setPositionConstraintFcn(hpoints(k), makeConstrainToRectFcn('impoint', [(k-1)/5 (k-1)/5], [-inf inf]));
        addNewPositionCallback(hpoints(k), @(pos)update(haxes));
        setColor(hpoints(k), color);

        hcmenu = get(hpoints(k), 'uicontextmenu');
        exportmenu = uimenu(hcmenu, 'Label', 'Export', 'Callback', @(a,b)export(haxes));
        set(hpoints(k), 'uicontextmenu', hcmenu);
    end

    setappdata(haxes, 'alpha', alpha);
    setappdata(haxes, 'impoints', hpoints);
    setappdata(haxes, 'hline', hline);
end

function update(haxes)
    hpoints = getappdata(haxes, 'impoints');
    hline = getappdata(haxes, 'hline');

    alpha = zeros(1, length(hpoints));
    for k=1:length(hpoints)
        pos = getPosition(hpoints(k));
        alpha(k) = pos(2);
    end

    s = 0:0.01:1;
    set(hline, 'YData', bezier_vec(alpha, s));
end

function export(haxes)
    hpoints = getappdata(haxes, 'impoints');
    hline = getappdata(haxes, 'hline');

    alpha = zeros(1, length(hpoints));
    for k=1:length(hpoints)
        pos = getPosition(hpoints(k));
        alpha(k) = pos(2);
    end

    name=inputdlg('Enter variable name:', 'Save Bezier parameters to base workspace', 1);
    while (~isvarname(name{1}) && ~isempty(name))
        name=inputdlg(sprintf('''%s'' is not a valid variable name.\nEnter variable name:', name{1}), 'Save Bezier parameters to workspace', 1);
    end
    if isvarname(name{1})
        assignin('base',name{1},alpha);
    end
end

function y = bezier_vec(alpha, s)
    y = zeros(size(alpha,1), size(s,2));
    for k=1:length(s)
        y(:,k) = bezier(alpha, s(k));
    end
end


function [Q fcnEvals iter] = adaptiveSimpson(fcn, a, b, varargin)
% adaptiveSimpson - Numerically evaluate integral, adaptive Simpson quadrature.
%
% function [Q fcnEvals iter] = adaptiveSimpson(fcn, a, b, varargin)
%
% (c) Matthias Conrad and Nils Papenberg (2007-08-03)
%
% Authors:
%   Matthias Conrad (e-mail: conrad@tiaco.de)
%   Nils Papenberg  (e-mail: papenber@math.uni-luebeck.de)
%
% Version:
%		Release date: 2008-08-12   Version: 1.2
%   MATLAB Version 7.5.0.338 (R2007b)
%
% Description:
%   The adaptive Simpson algorithm programmed in an iterative not recursive
%   manner
%
% Input arguments:
%   fcn             - function to be integrated
%   a               - first point of interval
%   b               - final point of interval
%   #varargin       - further options of algorithm
%     tol           - tolerance accuracy of quadrature [ 1e-6 ]
%     parts         - initial number of partitions [ 2 ]
%     maxFcnEvals   - maximal number of function evaluations allowed [ 20000 ]
%     maxParts      - maximal number of partitions allowed [ 8000 ]
%
% Output arguments:
%   Q               - numerical integral of function fcn on [a,b]
%   fcnEvals        - number of function evaluations
%   iter            - number of iterations
%
% Details:
%   This function behavior is similar to of Matlab integrated function "quadv".
%
%   Example:
%     Q = adaptiveSimpson(@(x) [-cos(50*x); sin(x)], 0, pi, 'tol', 1e-6)
%
% References:
%   [1] Gander, W. & Gautschi, W. Adaptive Quadrature - Revisited
%       Eidgenoessische technische Hochschule Zuerich, 2000.

% check scalar limits of interval
if ~isscalar(a) || ~isscalar(b)
  error('Matlab:adaptiveSimpson:Limits',...
    'The limits of integration must be scalars.');
end

% default values
tol = 1e-6; parts = 2; maxFcnEvals = 20000; maxParts = 8000;

% rewrite default options if needed
for j = 1 : length(varargin) / 2
  eval([varargin{2 * j - 1},'=varargin{',int2str(2 * j),'};']);
end

% initial values, termination constant (incl. quadr factor 15), parts of interval and integral value
tol = 15 * tol; m = parts;  parts = 4 * parts + 1; Q = 0;
iter = 0; fcnEvals = 0; maxResolution = 0; minH = eps(b - a) / 1024;
poleWarning = 0;

% check if interval has infinite boundaries, in case substitute function
if ~isfinite(a) || ~isfinite(b)
  warning('Matlab:adaptiveSimpson:infiniteInterval',...
    'The integral has an infinite interval; proceed with a substitution of function on finite interval.')
  if ~isfinite(a) && isfinite(b)
    [Q fcnEvals iter] = adaptiveSimpson(fcn, 0, b, varargin);
    fcn = @(t) infiniteLeft(t, fcn);
    a = 0; b = 1;
  elseif isfinite(a) && ~isfinite(b)
    [Q fcnEvals iter] = adaptiveSimpson(fcn, a, 0, varargin);
    fcn = @(t) infiniteRight(t, fcn);
    a = 0; b = 1;
  else
    fcn = @(t) infiniteBoth(t, fcn);
    a = - pi / 2; b = pi / 2;
  end
end

% choice of initial evaluation points
t = [a + (b - a) / m * (kron(ones(1,m), [0, 0.27158, 0.72842]) + kron(0: m - 1, [1 1 1])), b];
H = diff(t);
t = [t(1:end-1); t(1:end-1) + H/4; t(1:end-1) + H/2; t(1:end-1) + 3 * H/4;];
t = [t(:);b]';

% initialize equidistant mesh and dimension of fcn
y = fcn(t); fcnEvals = fcnEvals + length(y); n = size(y,1);

% avoid infinities at start point of interval
if any(~isfinite(y(:,1)))
  y(:,1) = fcn(a + eps(superiorfloat(a,b)) * (b - a));
  fcnEvals = fcnEvals + 1;
  poleWarning = 1;
end

% avoid infinities at end point of interval
if any(~isfinite(y(:, end)))
  y(:, end) = fcn(b - eps(superiorfloat(a,b)) * (b - a));
  fcnEvals = fcnEvals + 1;
  poleWarning = 1;
end

% poles at initial points
if ~isempty(find(~isfinite(max(abs(y))))), poleWarning = 1; end

% initialize interval boundaries and values
A = t(1:4:end-1); yA = y(:, 1:4:end-1);
B = t(5:4:end);   yB = y(:, 5:4:end);
C = t(3:4:end-1); yC = y(:, 3:4:end-1);
D = t(2:4:end-1); yD = y(:, 2:4:end-1);
E = t(4:4:end-1); yE = y(:, 4:4:end-1);

% adaptive Simpson iteration
while 1

  % number of iteration
  iter = iter + 1;

  % Simpson formulas (on rough and fine grid)
  Q1 = kron(H, ones(n,1)) / 6 .* (yA + 4 * yC + yB);
  Q2 = kron(H, ones(n,1)) / 12 .* (yA + 4 * yD + 2 * yC + 4 * yE + yB);

  % difference of Simpson formulas
  diffQ = Q2 - Q1; diffQ(find(isnan(diffQ))) = 0;

  % intervals which do not fulfill termination criterion
  idx = find(max(abs(diffQ), [], 1) > tol);

  % intervals fulfill termination criterion
  idxQ = setdiff(1:length(A), idx);

  % check stop criterions
  STOP1 = isempty(idx); % check regular termination
  STOP2 = fcnEvals > maxFcnEvals;  % check maximal function evaluations
  STOP3 = 2 * length(idx) > maxParts; % check maximal partition

  % regular termination
  if STOP1
    Q = Q + sum(Q2 + diffQ / 15, 2);
    break
  end

  % check if maximal resolution reached
  idxH = find(abs(H) < minH);
  if ~isempty(idxH)
    Q = Q + sum(Q2(idxH) + diffQ(idxH) / 15, 2);
    idx = setdiff(idx, idxH);
    idxQ = setdiff(idxQ, idxH);
    maxResolution = 1;
    % termination criterion
    if isempty(idx), break, end
  end

  % maximal function evaluations reached
  if STOP2
    warning('Matlab:adaptiveSimpson:MaxEvaluations',...
      'The maximal number of function evaluations reached; singularity likely.')
    Q = Q + sum(Q2 + diffQ / 15, 2);
    break
  end

  % maximal partition reached
  if STOP3
    warning('Matlab:adaptiveSimpson:parts',...
      'The maximal number of parts reached.')
    Q = Q + sum(Q2 + diffQ / 15, 2);
    break
  end

  % update quadrature value
  Q = Q + sum(Q2(:,idxQ) + diffQ(:,idxQ) / 15, 2);

  % update A, B, and C
  t = zeros(1, 2*length(idx));
  t(1:2:end) = A(idx); t(2:2:end) = C(idx); A = t;
  t(1:2:end) = C(idx); t(2:2:end) = B(idx); B = t;
  t(1:2:end) = D(idx); t(2:2:end) = E(idx); C = t;

  % update yA, yB, and yC
  y = zeros(n, 2*length(idx));
  y(:,1:2:end) = yA(:,idx); y(:,2:2:end) = yC(:,idx); yA = y;
  y(:,1:2:end) = yC(:,idx); y(:,2:2:end) = yB(:,idx); yB = y;
  y(:,1:2:end) = yD(:,idx); y(:,2:2:end) = yE(:,idx); yC = y;

  % update interval length
  H = B - A;

  % update D and E by interval bisection
  D = (A + H / 4); E = (A + 3 * H / 4);

  % calculate yD and yE
  y = fcn([D,E]); fcnEvals = fcnEvals + 4 * length(idx);

  % poles at new points
  if ~isempty(find(~isfinite(max(abs(y))))), poleWarning = 1; end

  % assign new values ob yD and yE
  yD = y(:,1:end/2); yE = y(:,end/2+1: end);

end

% display warnings
if any(~isfinite(Q))
  warning('Matlab:adaptiveSimpson:Infinite',...
    'The Quadrature of the function reached infinity or is Not-a-Number.')
end
if maxResolution
  warning('Matlab:adaptiveSimpson:MaxResolution',...
    'The maximal resolution of partial interval reached; singularity likely.')
end
if poleWarning
  warning('Matlab:adaptiveSimpson:PoleDetection',...
    'A detection of a pole; singularity likely.')
end

return

% substitute function interval [-inf, 0] on [0, 1]
function f = infiniteLeft(t, fcn)
f = fcn(log(t));
f = f ./ kron(ones(size(f,1),1), t);
return

% substitute function interval [0, inf] on [0, 1]
function f = infiniteRight(t, fcn)
f = fcn(-log(t));
f = f ./ kron(ones(size(f,1),1), t);
return

% substitute function interval [-inf, inf] on [-pi / 2, pi / 2]
function f = infiniteBoth(t, fcn)
f = fcn(tan(t));
f = f ./ kron(ones(size(f,1),1), cos(t).^2);
return


function [  ] = bz_piTickLabel(axis,discretization)
%bz_piTickLabel(axis) changes tick labels to 0 pi, 2pi, etc.
%
%%
if ~exist('discretization','var')
    discretization = 1;
end

switch axis
    case 'x'
        limits = xlim(gca)./pi;
    case 'y'
        limits = ylim(gca)./pi;
end
whichtick = [axis,'tick'];
whichticklabel = [axis,'ticklabel'];

ticks = round(limits(1):discretization:limits(2),1);
for ll = 1:length(ticks)
    labels{ll} = [num2str(ticks(ll)),'\pi'];
end

set(gca,whichtick,ticks.*pi)
set(gca,whichticklabel,labels)

end


function [imgw, imgwr, map] = tpswarp(img, outDim, Zp, Zs, interp)
%
% Description: Thin-Plane spline warping of the input image (img) to
% output image (imgw). The warping is defined by a set of reference points
% (Zp=[Xp Yp]) on the [img] image and a set of corresponding points (Zs)
% on the (imgw) image. The warping will translate Zp in img to Zs in imgw.
%
% Input:
% img - input image
% outDim - Output canvas ([W H])
% Zp - landmark in img
% Zs - landmark in canvas
% interp.method - interpolation mode('nearest', 'invdist', 'none')
% interp.radius - Max radius for nearest neighbor interpolation or
%                 Radius of inverse weighted interpolation
% interp.power - power for inverse weighted interpolation
%
% Output:
% imgw - warped image with no holes
% imgwr - warped image with holes
% map - Map of the canvas with 0 indicating holes and 1 indicating pixel
%
% Reference:
% F.L. Bookstein, "Principal Warps: Thin-Plate splines and the
% decomposition of deformations", IEEE Transaction on Pattern Analysis and
% Machine Intelligence, Vol. 11, No. 6, June 1989
%
% Author: Fitzgerald J Archibald
% Date: 07-Apr-09

%% Initialization
NPs = size(Zp,1); % number of landmark points

imgH = size(img,1); % height
imgW = size(img,2); % width

outH = outDim(2);
outW = outDim(1);

% landmark in input
Xp = Zp(:,1)';
Yp = Zp(:,2)';

% landmark in output (homologous)
Xs = Zs(:,1)';
Ys = Zs(:,2)';

%% Algebra of Thin-plate splines

% Compute thin-plate spline mapping [W|a1 ax ay] using landmarks
[wL]=computeWl(Xp, Yp, NPs);
wY = [Xs(:) Ys(:); zeros(3,2)]; % Y = ( V| 0 0 0)'   where V = [G] where G is landmark homologous (nx2) ; Y is col vector of length (n+3)
wW = inv(wL)*wY; % (W|a1 ax ay)' = inv(L)*Y

% Thin-plate spline mapping (Map all points in the plane)
% f(x,y) = a1 + ax * x + ay * y + SUM(wi * U(|Pi-(x,y)|)) for i = 1 to n
[Xw, Yw]=tpsMap(wW, imgH, imgW, Xp, Yp, NPs);

%% Warping

% input grid for warping
[X Y] = meshgrid(1:imgH,1:imgW); % HxW

% Nearest neighbor or inverse distance weighted based interpolation
[imgw,imgwr,map] = interp2d(X(:), Y(:), img, Xw, Yw, outH, outW, interp);

return

%% [L] = [[K P];[P' 0]]
% np - number of landmark points
% (xp, yp) - coordinate of landmark points
function [wL]=computeWl(xp, yp, np)

rXp = repmat(xp(:),1,np); % 1xNp to NpxNp
rYp = repmat(yp(:),1,np); % 1xNp to NpxNp

wR = sqrt((rXp-rXp').^2 + (rYp-rYp').^2); % compute r(i,j)

wK = radialBasis(wR); % compute [K] with elements U(r)=r^2 * log (r^2)
wP = [ones(np,1) xp(:) yp(:)]; % [P] = [1 xp' yp'] where (xp',yp') are n landmark points (nx2)
wL = [wK wP;wP' zeros(3,3)]; % [L] = [[K P];[P' 0]]

return

%% Mapping: f(x,y) = a1 + ax * x + ay * y + SUM(wi * U(|Pi-(x,y)|)) for i = 1 to n
% np - number of landmark points
% (xp, yp) - coordinate of landmark points
function [Xw, Yw]=tpsMap(wW, imgH, imgW, xp, yp, np)

[X Y] = meshgrid(1:imgH,1:imgW); % HxW
X=X(:)'; % convert to 1D array by reading columnwise (NWs=H*W)
Y=Y(:)'; % convert to 1D array (NWs)
NWs = length(X); % total number of points in the plane

% all points in plane
rX = repmat(X,np,1); % Np x NWs
rY = repmat(Y,np,1); % Np x NWs

% landmark points
rxp = repmat(xp(:),1,NWs); % 1xNp to Np x NWs
ryp = repmat(yp(:),1,NWs); % 1xNp to Np x NWs

% Mapping Algebra
wR = sqrt((rxp-rX).^2 + (ryp-rY).^2); % distance measure r(i,j)=|Pi-(x,y)|

wK = radialBasis(wR); % compute [K] with elements U(r)=r^2 * log (r^2)
wP = [ones(NWs,1) X(:) Y(:)]'; % [P] = [1 x' y'] where (x',y') are n landmark points (nx2)
wL = [wK;wP]'; % [L] = [[K P];[P' 0]]

Xw  = wL*wW(:,1); % [Pw] = [L]*[W]
Yw  = wL*wW(:,2); % [Pw] = [L]*[W]

return

%% k=(r^2) * log(r^2)
function [ko]=radialBasis(ri)

r1i = ri;
r1i(find(ri==0))=realmin; % Avoid log(0)=inf
ko = 2*(ri.^2).*log(r1i);

return


function prob_test011 ( )

%*****************************************************************************80
%
%% TEST011 tests BETA, GAMMA.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST011\n' );
  fprintf ( 1, '  BETA evaluates the Beta function;\n' );
  fprintf ( 1, '  GAMMA evaluates the Gamma function.\n' );

  a = 2.2;
  b = 3.7;

  beta1 = beta ( a, b );
  beta2 = gamma ( a ) * gamma ( b ) / gamma ( a + b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Argument A =                   %14f\n', a );
  fprintf ( 1, '  Argument B =                   %14f\n', b );
  fprintf ( 1, '  Beta(A,B) =                    %14f\n', beta1 );
  fprintf ( 1, '  (Expected value = 0.0454 )\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Gamma(A)*Gamma(B)/Gamma(A+B) = %14f\n', beta2 );

  return
end


function jac = p30_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P30_JAC evaluates the jacobian for problem p30.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac = zeros ( neqn, neqn );

  csum = 0.0;
  for i = 1 : 19
    csum = csum + i.^(4.0/3.0);
  end

  pprime = 0.0;
  for i = 1 : 19
    pprime = pprime + ( 4.0 / 3.0 ) * r8_cube_root ( t - i );
  end

  jac(1,1) = pprime / csum;

  return
end


function [H] = rotate(f)

% ROTATE returns the homogenous coordinate transformation matrix
% corresponding to a rotation around the x, y and z-axis. The direction of
% the rotation is according to the right-hand rule.
%
% Use as
%   [H] = rotate(R)
% where
%   R       [rx, ry, rz] in degrees
%   H       corresponding homogenous transformation matrix
%
% Note that the order in which the rotations are performs matters. The
% rotation is first done around the z-axis, then the y-axis and finally the
% x-axis.

% Copyright (C) 2000-2005, Robert Oostenveld
%
% 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 should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

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

if numel(f)~=3
  ft_error('incorrect input vector');
end

% convert degrees to radians
f = f*pi/180;

% get the individual angles (in radians)
rx = f(1);
ry = f(2);
rz = f(3);

% precompute the sin/cos values of the angles
cX = cos(rx);
cY = cos(ry);
cZ = cos(rz);
sX = sin(rx);
sY = sin(ry);
sZ = sin(rz);

% according to Roger Woods' http://bishopw.loni.ucla.edu/AIR5/homogenous.html
% it should be this, but I cannot reproduce his rotation matrix
% H = eye(4,4);
% H(1,1) = cZ*cY + sZ*sX*sY;
% H(1,2) = sZ*cY - cZ*sX*sY;
% H(1,3) =            cX*sY;
% H(2,1) = -sZ*cX;
% H(2,2) =  cZ*cX;
% H(2,3) =     sX;
% H(3,1) =  sZ*sX*cY - cZ*sY;
% H(3,2) = -cZ*sX*cY - sZ*sY;
% H(3,3) =             cX*cY;

% instead, the following rotation matrix does work according my
% expectations. It rotates according to the right hand rule and first
% rotates around z, then y and then x axis
H = [
           cZ*cY,          -sZ*cY,              sY,               0
  cZ*sY*sX+sZ*cX, -sZ*sY*sX+cZ*cX,          -cY*sX,               0
 -cZ*sY*cX+sZ*sX,  sZ*sY*cX+cZ*sX,           cY*cX,               0
               0,               0,               0,               1
];

if 0
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% The following code can be used to construct the combined rotation matrix
% for either xyz or zyx ordering (using the MATLAB symbolic math toolbox)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  syms sX sY sZ cX cY cZ
  % this is for only rotating around x
  Rx = [
    1   0    0   0
    0   cX  -sX  0
    0   sX   cX  0
    0   0    0   1
    ];
  % this is for only rotating around y
  Ry = [
    cY  0   sY  0
    0   1   0   0
    -sY  0   cY  0
    0   0   0   1
    ];
  % this is for only rotating around z
  Rz = [
    cZ -sZ  0   0
    sZ  cZ  0   0
    0   0   1   0
    0   0   0   1
    ];
  % combine them
  Rzyx = Rz * Ry * Rx  % rotate around x, y, then z
  Rxyz = Rx * Ry * Rz  % rotate around z, y, then x
end


clear all;
addpath('../lib');

%% lets plot 3 cycles of 50Hz AC voltage
f = 50;
Vm = 10;
phi = 0;

% generate the signal
t = [0:0.0001:3/f];
th = 2*pi*f*t;
v = Vm*sin(th+phi);

%% plot now
plt = Plot(t*1E3, v);

plt.XLabel = 'Time, t (ms)'; % xlabel
plt.YLabel = 'Voltage, V (V)'; %ylabel
plt.YTick = [-10, 0, 10]; %[tick1, tick2, .. ]
plt.YLim = [-11, 11]; % [min, max]
plt.XMinorTick = 'off'; % 'on' or 'off'
plt.Colors = {[1, 0, 0], [0, 0, 1]}; %[red, green, blue]

plt.FontName = 'Times';

% Save? comment the following line if you do not want to save
plt.export('plotVoltSomeOther.tiff');


function [rhsQ] = CurvedEulerRHS2D(Q, time, SolutionBC, fluxtype)

% function [rhsQ] = CurvedEulerRHS2D(Q, time, SolutionBC, fluxtype)
% Purpose: compute right hand side residual for the compressible Euler
%          gas dynamics equations

Globals2D;

% 1.1 Interpolate solution to cubature nodes
cQ = zeros(cub.Ncub, K, 4);
for n=1:4, cQ(:,:,n) = cub.V*Q(:,:,n); end;

% 1.2 Evaluate flux function at cubature nodes
gamma = 1.4;
[F,G,rho,u,v,p] = EulerFluxes2D(cQ, gamma);

% 1.3 Compute volume terms (dphidx, F) + (dphidy, G)
rhsQ = zeros(Np, K, 4);
for n=1:4
  ddr = (cub.Dr')*(cub.W.*(cub.rx.*F(:,:,n) + cub.ry.*G(:,:,n)));
  dds = (cub.Ds')*(cub.W.*(cub.sx.*F(:,:,n) + cub.sy.*G(:,:,n)));
  rhsQ(:,:,n) = ddr + dds;
end

% 2.1 SURFACE TERMS (using Gauss nodes on element faces)
nx = gauss.nx; ny = gauss.ny;
mapW = gauss.mapW; mapI = gauss.mapI; mapO = gauss.mapO; mapB = gauss.mapB; mapC = gauss.mapC;

% 2.2 Interpolate solution to Gauss surface nodes
gQM = zeros(gauss.NGauss*Nfaces, K, 4); gQP = zeros(gauss.NGauss*Nfaces, K, 4);
for n=1:4
  gQ = gauss.interp*Q(:,:,n);
  gQM(:,:,n) = gQ(gauss.mapM);  gQP(:,:,n) = gQ(gauss.mapP);
end

% 2.3 Apply boundary conditions to '+' traces
if(~isempty(SolutionBC))
  gQP = feval(SolutionBC, gauss.x, gauss.y, gauss.nx, gauss.ny, gauss.mapI, ...
                 gauss.mapO, gauss.mapW, gauss.mapC, gQP, time);
end

% 2.4 Evaluate surface flux functions with stabilization
switch fluxtype
  case {'LF'}
    [flux] = EulerLF2D (nx, ny, gQM, gQP, gamma);
  case {'Roe'}
    [flux] = EulerRoe2D(nx, ny, gQM, gQP, gamma);
  case {'HLL'}
    [flux] = EulerHLL2D(nx, ny, gQM, gQP, gamma);
  case {'HLLC'}
    [flux] = EulerHLLC2D(nx, ny, gQM, gQP, gamma);
end

% 2.5 Compute surface integral terms
for n=1:4
  rhsQ(:,:,n) = rhsQ(:,:,n) - gauss.interp'*(gauss.W.*flux(:,:,n));
end

% 3.1 Multiply by inverse mass matrix
for n=1:4
  % 3.1.a Multiply straight sided elements by inverse mass matrix
  rhsQ(:,straight, n) = V*V'*(rhsQ(:,straight, n)./J(:,straight));

  % 3.1.b Multiply curvilinear elements by custom inverse mass matrices
  Ncurved = length(curved);
  for m=1:Ncurved
    k = curved(m);
    mmCHOL = cub.mmCHOL(:,:,k);
    rhsQ(:,k,n) = mmCHOL\(mmCHOL'\rhsQ(:,k,n));
  end
end
return;


function [A, AH, normA] = MakeTransforms(type, N, params)

% [A, AH] = MakeTransforms(type, N, params)
% with A AH = I
%
% INPUT
%    N : signal length
%    type : type of transform can be 'DFT' or 'STFT'
%    params : parameters for transform
%
%    For DFT, params = [Nfft]
%       Nfft = number of DFT coefficients, Nfft >= N
%       Nfft/N is over-sampling factor
%
%   For STFT, params = [R, Nfft]
%       R = length of frame
%       Nfft = number of DFT coefficients, Nfft >= R
%
%    For DCT, params = [Ndct]
%       Ndct = number of DCT coefficients, Ndct >= N
%       Ndct/N is over-sampling factor
%
%   For STDCT, params = [R, Ndct]
%       R = length of frame
%       Ndct = number of DCT coefficients, Ndct >= R
%
% OUTPUT
%    A, AH : function handles for transform
%       AH is the conjugate transpose of A
%       Note: input to AH must be a row vector of length N

% DFT : Discrete Fourier transform with zero-padding
% STFT : Short-time Fourier transform
% DCT : Discrete cosine transform with zero-padding
% STDCT : Short-time DCT

switch type
    case '2D-CDWT'
        [Faf, Fsf] = FSfarras;
        [af, sf] = dualfilt1;
        AH = @(x) cplxdual2D(x,params(1),Faf,af);
        A = @(x) icplxdual2D(x,params(1),Fsf,sf);

        normA = 1;

    case 'DWT'
        [h0, h1, g0, g1] = daubf(params(2));    % Get filters
        AH = @(x) udwt(x, params(1), h0, h1);
        A = @(x) iudwt(x, params(1), g0, g1);
        normA = 1;
    case '2D-DWT'
        [Faf, Fsf] = FSfarras;
        [af, sf] = dualfilt1;
        AH = @(x) dualtree2D(x,params(1), Faf, af);
        A = @(x) idualtree2D(x,params(1), Fsf, sf);

        normA = 1;
    case 'DFT'

        % over-sampled DFT (oversampling factor = Nfft/N)
        Nfft = params(1);
        truncate = @(x, N) x(1:N);
        AH = @(x) fft(x, Nfft)/sqrt(Nfft);
        A = @(X) truncate(ifft(X), N)*sqrt(Nfft);

        normA = sqrt(N/Nfft);

    case 'STFT'

        R = params(1);
        M = params(2);
        K = params(3);
        Nfft = params(4);
        AH = @(x) pSTFT2(x, R, M, K, Nfft);
        A = @(x) ipSTFT2(x,R,M,K,N);

        normA = sqrt(R/(M*Nfft));

    case 'DCT'

        % over-sampled DFT (oversampling factor = Nfft/N)
        Ndct = params(1);
        truncate = @(x, N) x(1:N);
        AH = @(x) dct(x, Ndct);
        A = @(X) truncate(idct(X), N);

        normA = sqrt(N/Ndct);

    case 'STDCT'

        R = params(1);
        M = params(2);
        K = params(3);
        Ndct = params(4);
        AH = @(x) stdct(x, R,M,K, Ndct);
        A = @(x) istdct(x, R,M,K, N);

        normA = sqrt(R/(2*Ndct));

end

% x = randn(1, N);         % assuming row vector ! ! !
%
% % Verify perfect reconstruction property of transform 1 (A*AH = I)
% c = AH(x);
% err = x - A(c)';
% re = max(abs(err(:)));          % should be zero
% fprintf('Reconstruction error = %f\n', re)
%
% % Verify Parseval energy identity
% E = sum(abs(x(:)).^2);
% E1 = sum(abs(c(:)).^2);         % should be unity
% fprintf('Energy ratio = %f\n', E/E1)


function [power] = VBA_power(type, alpha, Estat, dof)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [power] = VBA_power(type, alpha, Estat, dof)
% Statistical power of a t- or F-test
%
% IN:
%   - type: type of test. Can be set to 't' or 'F'.
%   - alpha: statistical significance level
%   - Estat: expected effect size (z-score for a t-test, F value for an
%            F-test)
%   - dof: degrees of freedom
%
% OUT:
%   - power: ensuing statistical power
%
% /////////////////////////////////////////////////////////////////////////

%  ensure numerical stability
dof(isinf(dof)) = 1e8;
dof(dof==0) = 1e-8;

switch type
    case 't'
        tcritic = VBA_spm_invTcdf (1 - alpha, dof);
        power = 1 - VBA_spm_Tcdf (tcritic - Estat, dof);
    case 'F'
        Fcritic = VBA_spm_invFcdf (1 - alpha, dof(1), dof(2));
        power = 1-VBA_ncfcdf (Fcritic, dof(1), dof(2), Estat);
    otherwise
        disp('*** VBA_power: ''type'' argument can only be ''t'' or ''F''')
        power = [];
        return
end


function lorenza
% Lorenz convection model

sigma = 16; rho = 45.92; beta = 4;  % parameters
N = 1000;         % no. of time steps
span = 0.05;      % inner iteration span
AbsTol = 1.e-5;   % absolute tolerance for ODE solver
RelTol = 1.e-5;   % relative tolerance for ODE solver

H = figure; set(H,'DefaultLineLineWidth',1.0);
options = odeset('RelTol',RelTol,'AbsTol',ones(1,3)*AbsTol);
u0 = [1;1;1];
for i = 1:N
    [t,u] = ode45(@lornz,[0 span],u0,odeset,beta,rho,sigma);
    hold on;
    plot(u(:,1),u(:,2),'r');
    u0 = u(end,:);
end

title('Attractor of Lorenz System');
xlabel('Component 1'); ylabel('Component 2');
axis off; hold off;

function dydt = lornz(t,y,beta,rho,sigma)
dydt = [sigma*(y(2)-y(1)); rho*y(1)-y(2)-y(1)*y(3); y(1)*y(2)-beta*y(3)];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% intlinear.m
%
%[A,B] = intlinear(x,y)
%
% Does de linear regression (least mean squares) for given (x,y).
% Returns A and B, with meaning y = A + B*x.

function    [A,B] = intlinear(x,y)

mx = mean(x);        my = mean(y);
mx2 = mean(x.^2);    my2 = mean(y.^2);
mxy = mean(x.*y);
A = (mx2*my-mx*mxy)/(mx2-mx^2);
B = (mxy - (mx*my))/(mx2-mx^2);


function cond = condition_sample1 ( n, a, m )

%*****************************************************************************80
%
%% CONDITION_SAMPLE1 estimates the L1 condition number of a matrix.
%
%  Discussion:
%
%    A naive sampling method is used.
%
%    Only "forward" sampling is used, that is, we only look at results
%    of the form y=A*x.
%
%    Presumably, solving systems A*y=x would give us a better idea of
%    the inverse matrix.
%
%    Moreover, a power sequence y1 = A*x, y2 = A*y1, ... and the same for
%    the inverse might work better too.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 October 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the dimension of the matrix.
%
%    Input, real A(N,N), the matrix.
%
%    Input, integer M, the number of samples to use.
%
%    Output, real COND, an estimate of the L1 condition number.
%
  a_norm = 0.0;
  ainv_norm = 0.0;
  seed = 123456789;

  for i = 1 : m

    [ x, seed ] = r8vec_uniform_unit ( n, seed );
    x_norm = norm ( x, 1 );
    ax = a * x;
    ax_norm = norm ( ax, 1 );

    if ( ax_norm == 0.0 )
      cond = 0.0;
      return
    end

    a_norm    = max ( a_norm,    ax_norm / x_norm  );
    ainv_norm = max ( ainv_norm, x_norm  / ax_norm );

  end

  cond = a_norm * ainv_norm;

  return
end


function pass = test_hermpoly( prefs )
% Test the HERMPOLY command.

if ( nargin < 1 )
    prefs = chebfunpref();
end
tol = 1e6*prefs.techPrefs.chebfuneps;

% Physicists' Hermite polynomials
h0 = hermpoly(0);
err = norm( h0 - chebfun(@(x) 1+0*x, [-inf,inf] ) );
pass(1) = err < tol;

h1 = hermpoly(1);
err = norm( h1 - chebfun(@(x) 2*x, [-inf,inf] ) );
pass(2) = err < tol;

h2 = hermpoly(2);
err = norm( h2 - chebfun(@(x) 4*x.^2-2, [-inf,inf] ) );
pass(3) = err < tol;

h3 = hermpoly(3);
x = linspace(-1,1);
err = norm( feval(h3,x) - 8*x.^3+12*x );
pass(4) = err < tol;

h4 = hermpoly(4);
x = linspace(-1,1);
err = norm( feval(h4,x) - (16*x.^4-48*x.^2+12) );
pass(5) = err < tol;

h5 = hermpoly(5);
x = linspace(-1,1);
err = norm( feval(h5,x) - (32*x.^5-160*x.^3+120*x) );
pass(6) = err < 10*tol;

h6 = hermpoly(6);
x = linspace(-1,1);
err = norm( feval(h6,x) - (64*x.^6-480*x.^4+720*x.^2-120) );
pass(7) = err < 100*tol;

% probabilists' Hermite polynomials:
h0 = hermpoly(0, 'prob');
err = norm( h0 - chebfun(@(x) 1+0*x, [-inf,inf] ) );
pass(8) = err < tol;

h1 = hermpoly(1, 'prob');
err = norm( h1 - chebfun(@(x) x, [-inf,inf] ) );
pass(9) = err < tol;

h2 = hermpoly(2, 'prob');
g2 = chebfun(@(x) x.^2-1, [-inf,inf] );
err = norm( h2 - chebfun(@(x) x.^2-1, [-inf,inf] ) );
pass(10) = err < tol;

h3 = hermpoly(3, 'prob');
x = linspace(-1,1);
err = norm( feval(h3,x) - (x.^3-3*x) );
pass(11) = err < tol;

h4 = hermpoly(4, 'prob');
x = linspace(-1,1);
err = norm( feval(h4,x) - (x.^4-6*x.^2+3) );
pass(12) = err < tol;

h5 = hermpoly(5, 'prob');
x = linspace(-1,1);
err = norm( feval(h5,x) - (x.^5-10*x.^3+15*x) );
pass(13) = err < tol;

h6 = hermpoly(6, 'prob');
x = linspace(-1,1);
norm( feval(h6,x) - (x.^6-15*x.^4+45*x.^2-15) );
pass(14) = err < 10*tol;


function b = r8pbu_ml ( n, mu, a_lu, x )

%*****************************************************************************80
%
%% R8PBU_ML multiplies a vector times a matrix that was factored by R8PBU_FA.
%
%  Discussion:
%
%    The R8PBU storage format is for a symmetric positive definite band matrix.
%
%    To save storage, only the diagonal and upper triangle of A is stored,
%    in a compact diagonal format that preserves columns.
%
%    The diagonal is stored in row MU+1 of the array.
%    The first superdiagonal in row MU, columns 2 through N.
%    The second superdiagonal in row MU-1, columns 3 through N.
%    The MU-th superdiagonal in row 1, columns MU+1 through N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer MU, the number of superdiagonals of the matrix.
%    MU must be at least 0 and no more than N-1.
%
%    Input, real A_LU(MU+1,N), the matrix, as factored by R8PBU_FA.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product A * x.
%
  b(1:n) = x(1:n);
%
%  Multiply U * X = Y.
%
  for k = 1 : n

    ilo = max ( 1, k - mu );
    for i = ilo : k - 1
      b(i) = b(i) + a_lu(mu+1+i-k,k) * b(k);
    end

    b(k) = a_lu(mu+1,k) * b(k);

  end
%
%  Multiply L * Y = B.
%
  for k = n : -1 : 1

    jhi = min ( k + mu, n );
    for j = k + 1 : jhi
      b(j) = b(j) + a_lu(mu+1+k-j,j) * b(k);
    end

    b(k) = a_lu(mu+1,k) * b(k);

  end

  return
end


% This function compute the instantaneous frequency from phase spectrogram
% Input:
%   phase: D x T matrix of phase spectrogram, where D is FFT-bin number and
%       T is number of frames.
% Output:
%   instantaneous frequency of the size size as phase.
% The implementation is based on equation (3) of
%   Krawczyk, Martin, and Timo Gerkmann. "STFT Phase Reconstruction in
%   Voiced Speech for an Improved Single-Channel Speech Enhancement." (2014).
%
% Author: Xiong Xiao, Nanyang Tech. Univ., Singapore.
% Created: 06 Jan 2015
% Last modified: 06 Jan 2015
%
function [instan_freq] = comp_instan_freq(phase)

phase_delta = phase(:,2:end) - phase(:,1:end-1);
phase_delta = [phase(:,1) phase_delta];

[dim, nFr] = size(phase_delta);

phase_delta_vec = reshape(phase_delta, dim*nFr, 1);
phase_delta_vec = get_principal_value(phase_delta_vec);
instan_freq = reshape(phase_delta_vec, dim, nFr);

if 0
    subplot(3,1,1); imagesc(phase); title('Phase'); colorbar;
    subplot(3,1,2); imagesc(phase_delta); title('Phase delta');colorbar;
    subplot(3,1,3); imagesc(instan_freq); title('Phase delta principal value - Instantaneous frequency');colorbar;
end

end

function phase_delta_vec = get_principal_value(phase_delta_vec)
idx = find(phase_delta_vec>pi);
phase_delta_vec(idx) = phase_delta_vec(idx) - 2*pi;

idx = find(phase_delta_vec<-pi);
phase_delta_vec(idx) = phase_delta_vec(idx) + 2*pi;

end


% GAUSSIAN_DLOGPDF_COV - The gradient of the log pdf of a multivariate
%                        normal distribution with respect to the covariance
%                        matrix.
%
% Computes the gradient of the logarithm of the multivariate normal
% probability density function N(Y|MU,COV), where Y is the random variable,
% MU is the mean and COV is the covariance. The gradient is evaluated with
% respect to the covariance matrix COV and returned in matrix form.
%
%   d(LOG(N(Y|MU,COV))) / dCOV
%
% Usage:
%
%   DX = GAUSSIAN_DLOGPDF_COV(INVCOV_Y, INVCOV_MU, INVCOV)
%
% where
%
% INVCOV_Y   : INV(COV)*Y
% INVCOV_MU  : INV(COV)*MU
% INVCOV     : INV(COV)
%
% Letting the user to compute the terms allows greater efficiency and
% flexibility.

% Last modified 2010-11-19
% Copyright (c) Jaakko Luttinen

function dx = gaussian_dlogpdf_cov(invCov_y, invCov_mu, invCov)

% $$$ if nargin == 3
% $$$   y = varargin{1};
% $$$   mu = varargin{2};
% $$$   L = varargin{3};
% $$$   invCov_y = linsolve_chol(L, y, 'lower');
% $$$   invCov_mu = linsolve_chol(L, mu, 'lower');
% $$$   invCov = linsolve_chol(L, eye(size(L)), 'lower');
% $$$ end

dx = - 0.5 * invCov ...
     + 0.5 * (invCov_y * invCov_y'  - ...
              invCov_y * invCov_mu' - ...
              invCov_mu * invCov_y' + ...
              invCov_mu * invCov_mu');


function [ x, w ] = bashforth_set ( n )

%*****************************************************************************80
%
%% BASHFORTH_SET sets abscissas and weights for Adams-Bashforth quadrature.
%
%  Discussion:
%
%    Adams-Bashforth quadrature formulas are normally used in solving
%    ordinary differential equations, and are not really suitable for
%    general quadrature computations.  However, an Adams-Bashforth formula
%    is equivalent to approximating the integral of F(Y(X)) between X(M)
%    and X(M+1), using an explicit formula that relies only on known values
%    of F(Y(X)) at X(M-N+1) through X(M).  For this reason, the formulas
%    have been included here.
%
%    Suppose the unknown function is denoted by Y(X), with derivative
%    F(Y(X)), and that approximate values of the function are known at a
%    series of X values, which we write as X(1), X(2), ..., X(M).  We write
%    the value Y(X(1)) as Y(1) and so on.
%
%    Then the solution of the ODE Y'=F(X,Y) at the next point X(M+1) is
%    computed by:
%
%      Y(M+1) = Y(M) + Integral ( X(M) < X < X(M+1) ) F(Y(X)) dX
%             = Y(M) + H * Sum ( 1 <= I <= N ) W(I) * F(Y(M+1-I)) approximately.
%
%    In the documentation that follows, we replace F(Y(X)) by F(X).
%
%    The integral:
%
%      Integral ( 0 <= X <= 1 ) F(X) dX.
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( 1 - I ),
%
%    The Adams-Bashforth formulas require equally spaced data.
%
%    Here is how the formula is applied in the case with non-unit spacing:
%
%      Integral ( A <= X <= A+H ) F(X) dX =
%      H * Sum ( 1 <= I <= N ) W(I) * F ( A - (I-1)*H ),
%      approximately.
%
%    The reference lists the second coefficient of the order 8 Adams-Bashforth
%    formula as
%      w(2) =  -1162169.0 / 120960.0
%    but this should be
%      w(2) =  -1152169.0 / 120960.0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 April 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964.
%
%    Jean Lapidus, John Seinfeld,
%    Numerical Solution of Ordinary Differential Equations,
%    Academic Press, 1971.
%
%    Daniel Zwillinger, editor,
%    Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996.
%
%  Parameters:
%
%    Input, integer N, the order.
%    N should be between 1 and 10, or 12, 14, 16, 18 or 20.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%    W(1) is the weight at X = 0, W(2) the weight at X = -1,
%    and so on.  The weights are rational, and should sum to 1.  Some
%    weights may be negative.
%
  x = zeros ( n, 1 );
  w = zeros ( n, 1 );

  if ( n == 1 )

    w(1) = 1.0;

  elseif ( n == 2 )

    d = 2.0;

    w(1) =   3.0 / d;
    w(2) = - 1.0 / d;

  elseif ( n == 3 )

    d = 12.0;

    w(1) =   23.0 / d;
    w(2) = - 16.0 / d;
    w(3) =    5.0 / d;

  elseif ( n == 4 )

    d = 24.0;

    w(1) =   55.0 / d;
    w(2) = - 59.0 / d;
    w(3) =   37.0 / d;
    w(4) =  - 9.0 / d;

  elseif ( n == 5 )

    d = 720.0;

    w(1) =   1901.0 / d;
    w(2) = - 2774.0 / d;
    w(3) =   2616.0 / d;
    w(4) = - 1274.0 / d;
    w(5) =    251.0 / d;

  elseif ( n == 6 )

    d = 1440.0;

    w(1) =   4277.0 / d;
    w(2) = - 7923.0 / d;
    w(3) =   9982.0 / d;
    w(4) = - 7298.0 / d;
    w(5) =   2877.0 / d;
    w(6) =  - 475.0 / d;

  elseif ( n == 7 )

    d = 60480.0;

    w(1) =    198721.0 / d;
    w(2) =  - 447288.0 / d;
    w(3) =    705549.0 / d;
    w(4) =  - 688256.0 / d;
    w(5) =    407139.0 / d;
    w(6) =  - 134472.0 / d;
    w(7) =     19087.0 / d;

  elseif ( n == 8 )

    d = 120960.0;

    w(1) =     434241.0 / d;
    w(2) =  - 1152169.0 / d;
    w(3) =    2183877.0 / d;
    w(4) =  - 2664477.0 / d;
    w(5) =    2102243.0 / d;
    w(6) =  - 1041723.0 / d;
    w(7) =     295767.0 / d;
    w(8) =    - 36799.0 / d;

  elseif ( n == 9 )

    d = 3628800.0;

    w(1) =   14097247.0 / d;
    w(2) =  -43125206.0 / d;
    w(3) =   95476786.0 / d;
    w(4) = -139855262.0 / d;
    w(5) =  137968480.0 / d;
    w(6) =  -91172642.0 / d;
    w(7) =   38833486.0 / d;
    w(8) =   -9664106.0 / d;
    w(9) =    1070017.0 / d;

  elseif ( n == 10 )

    d = 7257600.0;

    w( 1) =   30277247.0 / d;
    w( 2) = -104995189.0 / d;
    w( 3) =  265932680.0 / d;
    w( 4) = -454661776.0 / d;
    w( 5) =  538363838.0 / d;
    w( 6) = -444772162.0 / d;
    w( 7) =  252618224.0 / d;
    w( 8) =  -94307320.0 / d;
    w( 9) =   20884811.0 / d;
    w(10) =   -2082753.0 / d;

  elseif ( n == 12 )

    d = 958003200.0;

    w( 1) =    4527766399.0 / d;
    w( 2) =  -19433810163.0 / d;
    w( 3) =   61633227185.0 / d;
    w( 4) = -135579356757.0 / d;
    w( 5) =  214139355366.0 / d;
    w( 6) = -247741639374.0 / d;
    w( 7) =  211103573298.0 / d;
    w( 8) = -131365867290.0 / d;
    w( 9) =   58189107627.0 / d;
    w(10) =  -17410248271.0 / d;
    w(11) =    3158642445.0 / d;
    w(12) =    -262747265.0 / d;

  elseif ( n == 14 )

    d = 5230697472000.0;

    w( 1) =    27511554976875.0 / d;
    w( 2) =  -140970750679621.0 / d;
    w( 3) =   537247052515662.0 / d;
    w( 4) = -1445313351681906.0 / d;
    w( 5) =  2854429571790805.0 / d;
    w( 6) = -4246767353305755.0 / d;
    w( 7) =  4825671323488452.0 / d;
    w( 8) = -4204551925534524.0 / d;
    w( 9) =  2793869602879077.0 / d;
    w(10) = -1393306307155755.0 / d;
    w(11) =   505586141196430.0 / d;
    w(12) =  -126174972681906.0 / d;
    w(13) =    19382853593787.0 / d;
    w(14) =    -1382741929621.0 / d;

  elseif ( n == 16 )

    d = 62768369664000.0;

    w( 1) =     362555126427073.0 / d;
    w( 2) =   -2161567671248849.0 / d;
    w( 3) =    9622096909515337.0 / d;
    w( 4) =  -30607373860520569.0 / d;
    w( 5) =   72558117072259733.0 / d;
    w( 6) = -131963191940828581.0 / d;
    w( 7) =  187463140112902893.0 / d;
    w( 8) = -210020588912321949.0 / d;
    w( 9) =  186087544263596643.0 / d;
    w(10) = -129930094104237331.0 / d;
    w(11) =   70724351582843483.0 / d;
    w(12) =  -29417910911251819.0 / d;
    w(13) =    9038571752734087.0 / d;
    w(14) =   -1934443196892599.0 / d;
    w(15) =     257650275915823.0 / d;
    w(16) =     -16088129229375.0 / d;

  elseif ( n == 18 )

    d = 64023737057280000.0;

    w( 1) =     401972381695456831.0 / d;
    w( 2) =   -2735437642844079789.0 / d;
    w( 3) =   13930159965811142228.0 / d;
    w( 4) =  -51150187791975812900.0 / d;
    w( 5) =  141500575026572531760.0 / d;
    w( 6) = -304188128232928718008.0 / d;
    w( 7) =  518600355541383671092.0 / d;
    w( 8) = -710171024091234303204.0 / d;
    w( 9) =  786600875277595877750.0 / d;
    w(10) = -706174326992944287370.0 / d;
    w(11) =  512538584122114046748.0 / d;
    w(12) = -298477260353977522892.0 / d;
    w(13) =  137563142659866897224.0 / d;
    w(14) =  -49070094880794267600.0 / d;
    w(15) =   13071639236569712860.0 / d;
    w(16) =   -2448689255584545196.0 / d;
    w(17) =     287848942064256339.0 / d;
    w(18) =     -15980174332775873.0 / d;

  elseif ( n == 20 )

    d = 102181884343418880000.0;

    w( 1) =      691668239157222107697.0 / d;
    w( 2) =    -5292843584961252933125.0 / d;
    w( 3) =    30349492858024727686755.0 / d;
    w( 4) =  -126346544855927856134295.0 / d;
    w( 5) =   399537307669842150996468.0 / d;
    w( 6) =  -991168450545135070835076.0 / d;
    w( 7) =  1971629028083798845750380.0 / d;
    w( 8) = -3191065388846318679544380.0 / d;
    w( 9) =  4241614331208149947151790.0 / d;
    w(10) = -4654326468801478894406214.0 / d;
    w(11) =  4222756879776354065593786.0 / d;
    w(12) = -3161821089800186539248210.0 / d;
    w(13) =  1943018818982002395655620.0 / d;
    w(14) =  -970350191086531368649620.0 / d;
    w(15) =   387739787034699092364924.0 / d;
    w(16) =  -121059601023985433003532.0 / d;
    w(17) =    28462032496476316665705.0 / d;
    w(18) =    -4740335757093710713245.0 / d;
    w(19) =      498669220956647866875.0 / d;
    w(20) =      -24919383499187492303.0 / d;

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'BASHFORTH_SET - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of N = %d\n', n );
    fprintf ( 1, '  Legal values are 1 through 10,\n' );
    fprintf ( 1, '  or 12, 14, 16, 18 or 20.\n' );
    error ( 'BASHFORTH_SET - Fatal error!' );

  end

  for i = 1 : n
    x(i) = 1 - i;
  end

  return
end


function r=rot4y(theta)
    % r = rot4y(theta)
    %
    % roty produces a 4x4 rotation matrix representing
    % a rotation by theta radians about the y axis.
    %
    %	Argument definitions:
    %
    %	theta = rotation angle in radians
    c = cos(theta);
    s = sin(theta);
    r = [c  0  s 0;
        0  1  0 0;
        -s  0  c 0;
        0  0  0 1];


function [ftAllNew,transMdl] = ftTrans_itl(ft,maSrc,target,maLabeled,param)
%Wrapper of the Information-Theoretical Learning (ITL) algorithm
%
%	A feaure-level transfer learning (domain adaptation) algorithm which
% learns a domain-invariant subspace. Application scope:
%	+ two discrete domains (source and target)
%	+ labeled source domain
%	+ unlabeled target domain
%	+ label type: classification
%
% ftAll:	All samples in all domains. n-by-m matrix, n is the number of
%	samples, m is the dimension of features.
% maSrc:	n-by-1 logical vector, maSrc(i)=true if i is from the source
%	domain, 0 if target domain.
% target:	The class labels of the source domain, nsrc-by-1 matrix, nsrc
%	is the number of source	samples. target(i)=j if the i'th sample is from
%	the j'th class.
% maLabeled:	Mask for the labeled samples. n-by-1 matrix, must be the
% same with maSrc.
%
% param: Struct of hyper-parameters, please see the first cell of this
%	program ("default parameters") for details. You can set parameter p to
%	x by setting param.p = x. For parameters that are not set, default
%	values will be used.
%
% ftAllNew:	All samples in the learned subspace.
% transMdl:	A struct containing the model, transMdl.W is the projection
%	matrix.
%
%	The ftProc_pca_tr function is a part of the PRTools toolbox.
%	Notice from the author of the paper: for practice use, you need to
% properly preprocess the data and tune lambda and d.
%
% ref: Y. Shi and F. Sha, "Information-theoretical learning of discriminative
% clusters for unsupervised domain adaptation," in ICML, 2012.
%
% Copyright 2016 Ke YAN, Tsinghua Univ. http://yanke23.com , xjed09@gmail.com

addpath infometric_0.1

%% default parameters
pcaCoef = 0; % see ftProc_pca_tr
lambda = 1; % regularization parameter

defParam

%% sort samples
if any(maSrc~=maLabeled)
	error('maLabeled must be the same with maSrc')
end

ftSrc = ft(maSrc,:);
ftTar = ft(~maSrc,:);

%% compute
[~,pcaModelS] = ftProc_pca_tr(ftSrc,[],struct('pcaCoef',pcaCoef));
[~,pcaModelT] = ftProc_pca_tr(ftTar,[],struct('pcaCoef',pcaCoef));
d = min(size(pcaModelS.W_prj,2),size(pcaModelT.W_prj,2));
W_prjT = pcaModelT.W_prj(:,1:d);

L = infometric(W_prjT, ftSrc, target, ftTar, lambda);

%% project the samples
ftAllNew = zeros(size(ft,1),d);
ftAllNew(maSrc,:) = ftSrc*L;
ftAllNew(~maSrc,:) = ftTar*L;
transMdl.L = L;

end


function v = lpp_value ( m, n, o, x )

%*****************************************************************************80
%
%% LPP_VALUE evaluates a Legendre Product Polynomial at several points X.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of evaluation points.
%
%    Input, integer O(M,1), the degree of the polynomial factors.
%    0 <= O(*).
%
%    Input, real X(M,N), the evaluation points.
%
%    Output, real VALUE(N,1), the value of the Legendre Product Polynomial
%    of degree O at the points X.
%
  v = ones ( n, 1 );

  for i = 1 : m

    vi = lp_value ( n, o(i), x(i,1:n) );

    v(1:n) = v(1:n) .* vi(1:n);

  end

  return
end


function pdf = burr_pdf ( x, a, b, c, d )

%*****************************************************************************80
%
%% BURR_PDF evaluates the Burr PDF.
%
%  Discussion:
%
%    PDF(X)(A,B,C,D) = ( C * D / B ) * ( ( X - A ) / B )**( - C - 1 )
%      * ( 1 + ( ( X - A ) / B )**( - C ) )**( - D - 1 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    M E Johnson,
%    Multivariate Statistical Simulation,
%    Wiley, New York, 1987.
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    A <= X
%
%    Input, real A, B, C, D, the parameters of the PDF.
%    0 < B,
%    0 < C.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x <= a )
    pdf = 0.0;
  else

    y = ( x - a ) / b;

    pdf = ( c * d / b ) * y^( - c - 1.0 ) * ( 1.0 + y^( -c ) )^( - d - 1.0 );

  end

  return
end


function C1 = C1f(epsi)
%C1F  Evaluate C_{1,k}
%
%   C1 = C1F(EPSI) evaluates C_{1,l} using Eq. (18).  EPSI is a K x 1
%   array and C1 is a K x 6 array.

  nC1 = 6;
  C1 = zeros(length(epsi), nC1);
  eps2 = epsi.^2;
  d = epsi;
  C1(:,1) = d.*((6-eps2).*eps2-16)/32;
  d = d.*epsi;
  C1(:,2) = d.*((64-9*eps2).*eps2-128)/2048;
  d = d.*epsi;
  C1(:,3) = d.*(9*eps2-16)/768;
  d = d.*epsi;
  C1(:,4) = d.*(3*eps2-5)/512;
  d = d.*epsi;
  C1(:,5) = -7*d/1280;
  d = d.*epsi;
  C1(:,6) = -7*d/2048;
end


clear all
close all
%Setting variables to parameters
img=imread('synthetic1.png');
if length(size(img))==3             %generalized to rgb images
	img=rgb2gray(img);
end
img= double(img);

p=0;
phi=1;  % for 1.5 octave bandwidth
K = sqrt(2*log(2))*((2^phi+1)/(2^phi-1)); % for phi denoting spatial freq bandwidth
%{ admissible phi for simple and complex cells found 2 b in [0.5 2.5] }

w0_deg=[8 16 32 64 128];
%w0_deg=[8 16 32];
theta_deg=[0 22.5 45 67.5 90 112.5 135 157.5];
%theta_deg=[0 22.5];

w0=[8 16 32 64 128].*(pi/180);
%w0=[8 16 32].*(pi/180);
theta=[0 22.5 45 67.5 90 112.5 135 157.5].*(pi/180);
%theta=[0 22.5].*(pi/180);

len_w0=length(w0);
len_theta=length(theta);

k=1;
for i=1:1:len_w0
	for j=1:1:len_theta
		grid_size=2^(i-1)
		[gabor_even, gabor_odd]=gabor_filter_new(img,K,w0(i),theta(j),p,grid_size);

		sze = size(gabor_even,3);
		%             gabor_even_store(:,:,k:1:k+sze-1)=gabor_even;
		%             gabor_odd_store(:,:,k:1:k+sze-1)=gabor_odd;

		str_title=strcat('angle:',num2str(theta_deg(j)),' freq:', num2str(w0_deg(i)));

		for n=1:1:sze
			img_filtd_even(:,:,1)=normalize(conv2(gabor_even(:,:,n),img,'same'));
			img_filtd_odd(:,:,1)= normalize(conv2(gabor_odd(:,:,n),img,'same'));
			img_filtd_even_coeff(1)=sum(sum(img_filtd_even(:,:,1).^2));
			img_filtd_odd_coeff(1)=sum(sum(img_filtd_odd(:,:,1).^2));
			img_filtd_coeff(k+n-1)=sqrt(img_filtd_even_coeff(1) + img_filtd_odd_coeff(1));
		end
		%             clear img_filtd_even;
		%             clear img_filtd_odd;
		%             clear img_filtd_even_coeff;
		%             clear img_filtd_odd_coeff;
		%             figure;
		%             subplot(1,2,1)
		%             imshow(gabor_even(:,:,n),[])
		%             title(str_title);
		%             subplot(1,2,2)
		%             imshow(img_filtd_even(:,:,k+n-1),[])
		%             title('even filtered img')
		%
		%
		%             figure;
		%             subplot(1,2,1)
		%             imshow(gabor_odd(:,:,n),[])
		%             title(str_title);
		%             subplot(1,2,2)
		%             imshow(img_filtd_odd(:,:,k+n-1),[])
		%             title('odd filtered img')
		k=k+sze;
	end
end

figure;
plot(0:1:(k-2),img_filtd_coeff,'ro')
hold on
plot(0:1:(k-2),img_filtd_coeff,'g')
xlabel('w0:theta'); ylabel('image contrast energy');grid on;
title('Image Contrast Energy computed Quadrature pair-wise')

% contrast energy of filtered image computed quadrature pair-wise for
% every orientation and freq.


function w = MakeSineWindow(boxlen,sigma)
% MakeSineWindow -- Make a nice window out of the IteratedSine function.
%  Inputs
%    boxlen   abscissa values for window evaluation -boxlen <= t < boxlen
%    sigma    scale of the window, controls the size of the
%             effective support
%  Outputs
%    w        values of the window
%  See Also
%    IteratedSine
%
% By Emmanuel Candes, 2003-2004

  ix = ((-boxlen:(boxlen -1)) + .5)./boxlen;
  w = IteratedSineWindow(ix./sigma);


function w = cplxdual3D(x, J, Faf, af)

% 3D Complex Dual-Tree Discrete Wavelet Transform
%
% USAGE:
%   w = cplxdual3D(x, J, Faf, af)
% INPUT:
%   x - 3D array
%   J - number of stages
%   Faf - first stage filters
%   af - filters for remaining stages
% OUPUT:
%   w{j}{m}{n}{p}{d} - wavelet coefficients
%       j = 1..J, m = 1..2, n = 1..2, p = 1..2, d = 1..7
%   w{J+1}{m}{n}{d} - lowpass coefficients
%       m = 1..2, n = 1..2, p = 1..2, d = 1..7
% EXAMPLE:
%   x = rand(64,64,64);
%   J = 3;
%   [Faf, Fsf] = FSfarras;
%   [af, sf] = dualfilt1;
%   w = cplxdual3D(x, J, Faf, af);
%   y = icplxdual3D(w, J, Fsf, sf);
%   err = x - y;
%   max(max(max(abs(err))))
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

% normalization
x = x/sqrt(8);

for m = 1:2
    for n = 1:2
        for p = 1:2
            [lo w{1}{m}{n}{p}] = afb3D(x, Faf{m}, Faf{n}, Faf{p});
            for j = 2:J
                [lo w{j}{m}{n}{p}] = afb3D(lo, af{m}, af{n}, af{p});
            end
            w{J+1}{m}{n}{p} = lo;
        end
    end
end

for j = 1:J
    for m = 1:7
        [w{j}{1}{1}{1}{m} w{j}{2}{2}{1}{m} w{j}{2}{1}{2}{m} w{j}{1}{2}{2}{m}] = ...
            pm4(w{j}{1}{1}{1}{m}, w{j}{2}{2}{1}{m}, w{j}{2}{1}{2}{m}, w{j}{1}{2}{2}{m});
         [w{j}{2}{2}{2}{m} w{j}{1}{1}{2}{m} w{j}{1}{2}{1}{m} w{j}{2}{1}{1}{m}] = ...
            pm4(w{j}{2}{2}{2}{m}, w{j}{1}{1}{2}{m}, w{j}{1}{2}{1}{m}, w{j}{2}{1}{1}{m});
    end
end


% SENSE_recon_demo.m
% - demonstrates use of SENSE_recon.m, which implements the SENSE
% reconstruction
% - figure shows how Tikhonov regularization mitigates the increase in error
% as noise variance increases
% - applies complex AWGN to kspace
%
% 2012-06-08, Mai Le

% number of coils
nc = 4;
% factor of undersampling
np = 2;
% dimension to undersample
reduced_dim = 1;
regularizer1 = 'none';
regularizer2 = 'tikhonov';
figs = 0;

image = double(imread('./data/mri/brainweb_t1.jpg','jpg'));

dims = size(image);
% introduce planar phase
[xx,yy] = ndgrid(-dims(1)/2:dims(1)/2-1,-dims(2)/2:dims(2)/2-1);
ph_max = pi/5;
ph = ph_max*xx/(dims(1)/2)+ph_max*yy/(dims(2)/2);
image = image.*exp(1i*ph);

% generate sensitivity maps
smap = mri_sensemap_sim('nx',dims(1),'ny',dims(2),'ncoil',nc);

im_rep = repmat(image,[1 1 nc]);

% apply sensitivity maps
mapped_im = im_rep.*smap;

% throw away k-space data
im_fft = [];
reduced_fft = [];
for ii = 1:nc
    im_fft(:,:,ii) = fft2(mapped_im(:,:,ii));
    if (reduced_dim == 1)
        reduced_fft(:,:,ii) = im_fft(1:np:end,:,ii);
    else
        reduced_fft(:,:,ii) = im_fft(:,1:np:end,ii);
    end
end
%% perform SENSE reconstruction for varying levels of noise
sigmas = 0:25:150;
for ii = 1:length(sigmas)
    % introduce complex AWGN
    sigma = sigmas(ii);
    noisy_reduced_fft = reduced_fft + sigma*randn(size(reduced_fft)) + i*sigma*randn(size(reduced_fft));

    % SENSE reconstruction
    recon_im1(:,:,ii) = SENSE_recon(smap, noisy_reduced_fft, reduced_dim, np, regularizer1, figs);
    recon_im2(:,:,ii) = SENSE_recon(smap, noisy_reduced_fft, reduced_dim, np, regularizer2, figs);

    error1(ii) = sqrt((sum(sum((real(image)-real(recon_im1(:,:,ii))).^2))+sum(sum((imag(image)-imag(recon_im1(:,:,ii))).^2)))/prod(dims));
    error2(ii) = sqrt((sum(sum((real(image)-real(recon_im2(:,:,ii))).^2))+sum(sum((imag(image)-imag(recon_im2(:,:,ii))).^2)))/prod(dims)) ;
end
%% plot error as a function of noise variance
figure; plot(sigmas.^2, error1,'k');
hold on; plot(sigmas.^2, error2, 'r'); legend('no regularization', 'tikhonov');
xlabel('variance of complex AWGN in kspace');
ylabel('RMS error in reconstructed images');
title('effect of Tikhonov regularization on noisy SENSE reconstruction')
%% plot reconstructed image at sigma(ii)
ii = 4; % change as desired
figure; subplot(331); imshow(real(image),[]); colorbar;
title('real of original image');
subplot(332); imshow(imag(image),[]); colorbar;
title(['imag of original image, noise variance: ' num2str(sigmas(ii).^2)]);
subplot(334); imshow(real(recon_im1(:,:,ii)),[]); colorbar;
title('real of recon image, no reg');
subplot(335); imshow(imag(recon_im1(:,:,ii)),[]); colorbar;
title('imag of recon image, no reg');
subplot(336); imshow(abs(real(image)-real(recon_im1(:,:,ii)))+abs(imag(image)-imag(recon_im1(:,:,ii))),[]); colorbar;
title('abs(differences), no reg');
subplot(337); imshow(real(recon_im2(:,:,ii)),[]); colorbar;
title('real of recon image, Tikhonov');
subplot(338); imshow(imag(recon_im2(:,:,ii)),[]); colorbar;
title('imag of recon image, Tikhonov');
subplot(339); imshow(abs(real(image)-real(recon_im2(:,:,ii)))+abs(imag(image)-imag(recon_im2(:,:,ii))),[]); colorbar;
title('abs(differences), Tikhonov');


function [ table ] = integerparttable(nfinal )
%Calculates a table of integer partions using numbers l.t. or e.t. k
%(or equivilently using k nonnegative integers which can be zero)
%Just calls with the largest value which has to recurrsively call all other
%possible values.
%
%this table states with first row "ways to partition 0" and first column
%"using only numbers <=0", this gives the one in the top left, these two
%rows are essentially a definition limit to finish the reccurance relation.
%Row k+1 column j+1 contains the ways to partition k
% using integers <= j (or equivilently j integers >= 0)
%
%to obtain the partitions of nfinal using numbers g.e. 2 and l.e. k do
%  [1;table(2:nfinal,k+1)-table(1:nfinal-1,k+1)]
%
%use of 32 bit integers means this is only good up to 2^32-1= 4294967295,
%for larger values change to 64 bit or dp
% Scales as ~ exp(pi*sqrt(2*n/3))/(4*n*sqrt(3)) for n>>1 and k>n

%Author: David Holdaway
%Last update: 27/04/2012

intpart(1,nfinal,1,1); %clears table

for n = 2:nfinal
    intpart(uint32(n),uint32(n));
end
table = intpart(1,nfinal,2); %gets table

for n = 1:nfinal
 table(n+1,n+1:nfinal+1) =  table(n+1,n+1);
end
end

function p = intpart(k,n,gettable,cleartable) %#ok<INUSD>
persistent table
if nargin == 4
    table = uint32(zeros(n+1)); %clears
%     table(1,:) = uint32(ones(1,1+n)); %n=0
%     table(2,2:n+1) = uint32(ones(1,n)); %n=1 k>1
%     table(:,2) = uint32(ones(n+1,1)); %k=0
    table(1,:) = uint32(1); %n=0
    table(2,2:n+1) = uint32(1); %n=1 k>1
    table(:,2) = uint32(1); %k=0
    p = 0; return
end
if nargin == 3
    p = table; return
end
k = uint32(k);
n = uint32(n);
if n < 0
    p = uint32(0); return
end
if k < 0
    p = uint32(0); return
end
if table(n+1,k+1) ~= 0
    p = table(n+1,k+1);  return
end

if n==0
    p = uint32(0); return
end

    table(n+1,k+1) = intpart(k,n-k) + intpart(k-1,n);
    p = table(n+1,k+1);
    for lp = n+2:length(table)
    table(n+1,lp) = table(n+1,n+1);
    end

end


function u = SB_ATV(g,mu)
% Split Bregman Anisotropic Total Variation Denoising
%
%   u = arg min_u 1/2||u-g||_2^2 + mu*ATV(u)
%
%   g : noisy image
%   mu: regularisation parameter
%   u : denoised image
%
% Refs:
%  *Goldstein and Osher, The split Bregman method for L1 regularized problems
%   SIAM Journal on Imaging Sciences 2(2) 2009
%  *Micchelli et al, Proximity algorithms for image models: denoising
%   Inverse Problems 27(4) 2011
%
% Benjamin Trémoulhéac
% University College London
% b.tremoulheac@cs.ucl.ac.uk
% April 2012
g = g(:);
n = length(g);
[B Bt BtB] = DiffOper(sqrt(n));
b = zeros(2*n,1);
d = b;
u = g;
err = 1;k = 1;
tol = 1e-3;
lambda = 1;
while err > tol
    fprintf('it. %g ',k);
    up = u;
    [u,~] = cgs(speye(n)+BtB, g-lambda*Bt*(b-d),1e-5,100);
    Bub = B*u+b;
    d = max(abs(Bub)-mu/lambda,0).*sign(Bub);
    b = Bub-d;
    err = norm(up-u)/norm(u);
    fprintf('err=%g \n',err);
    k = k+1;
end
fprintf('Stopped because norm(up-u)/norm(u) <= tol=%.1e\n',tol);
end

function [B Bt BtB] = DiffOper(N)
D = spdiags([-ones(N,1) ones(N,1)], [0 1], N,N+1);
D(:,1) = [];
D(1,1) = 0;
B = [ kron(speye(N),D) ; kron(D,speye(N)) ];
Bt = B';
BtB = Bt*B;
end


function bg = ball_grid ( n, r, c, ng )

%*****************************************************************************80
%
%% BALL_GRID computes grid points inside a ball.
%
%  Discussion:
%
%    The grid is defined by specifying the radius and center of the ball,
%    and the number of subintervals N into which the horizontal radius
%    should be divided.  Thus, a value of N = 2 will result in 5 points
%    along that horizontal line.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of subintervals.
%
%    Input, real R, the radius of the ball.
%
%    Input, real C(3), the coordinates of the center of the ball.
%
%    Input, integer NG, the number of grid points, as determined by
%    BALL_GRID_COUNT.
%
%    Output, real BG(3,NG), the grid points inside the ball.
%
  bg = zeros ( 3, ng );

  p = 0;

  for i = 0 : n
    x = c(1) + r * 2 * i / ( 2 * n + 1 );

    for j = 0 : n
      y = c(2) + r * 2 * j / ( 2 * n + 1 );

      for k = 0 : n

        z = c(3) + r * 2 * k / ( 2 * n + 1 );

        if ( r * r < ( x - c(1) ).^2 ...
                   + ( y - c(2) ).^2 ...
                   + ( z - c(3) ).^2 )
          break
        end
        p = p + 1;
        bg(1:3,p) = [ x, y, z ]';
        if ( 0 < i )
          p = p + 1;
          bg(1:3,p) = [ 2 * c(1) - x, y, z ]';
        end
        if ( 0 < j )
          p = p + 1;
          bg(1:3,p) = [ x, 2 * c(2) - y, z ]';
        end
        if ( 0 < k )
          p = p + 1;
          bg(1:3,p) = [ x, y, 2 * c(3) - z ]';
        end
        if ( 0 < i && 0 < j )
          p = p + 1;
          bg(1:3,p) = [ 2 * c(1) - x, 2 * c(2) - y, z ]';
        end
        if ( 0 < i && 0 < k )
          p = p + 1;
          bg(1:3,p) = [ 2 * c(1) - x, y, 2 * c(3) - z ]';
        end
        if ( 0 < j && 0 < k )
          p = p + 1;
          bg(1:3,p) = [ x, 2 * c(2) - y, 2 * c(3) - z ]';
        end
        if ( 0 < i && 0 < j && 0 < k )
          p = p + 1;
          bg(1:3,p) = [ 2 * c(1) - x, 2 * c(2) - y, 2 * c(3) - z ]';
        end
      end
    end
  end

  return
end


function dif = r8mat_diff_frobenius ( m, n, a, b )

%*****************************************************************************80
%
%% R8MAT_DIFF_FROBENIUS: Frobenius norm of the difference of two R8MAT's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, real A(M,N), B(M,N), the matrices for which we
%    are to compute the Frobenius norm of the difference.
%
%    Output, real DIF, the Frobenius norm of A-B.
%
  dif = sqrt ( sum ( sum ( ( a(1:m,1:n) - b(1:m,1:n) ).^2 ) ) );

  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% EUROPEAN OPTION PRICER (RUN SCRIPT)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Descritpion: Script to Price European options in Regime Switching Diffusion Models
%              using the PROJ method
% Author:      Justin Kirkby
% References:  (1) Efficient Option Pricing By Frame Duality with The Fast
%              Fourier Transform, SIAM J. Financial Math., 2015
%              (2) A unified approach to Bermudan and Barrier options under stochastic
%               volatility models with jumps. J. Economic Dynamics and Control, 2017
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[folder, name, ext] = fileparts(which( mfilename('fullpath')));
cd(folder);
addpath('../')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 1) CHOOSE CONTRACT/GENERAL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
call = 1;    %For call use 1 (else, its a put)
S_0  = 100;  %Initial price
W    = 100;  %Strike            %NOTE: no error handling in place for extreme values of W (increase grid if strike falls outside)
r    = .05;  %Interest rate
q    = .00;  %dividend yield
T    = 1;    %Time (in years)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 2) CHOOSE MODEL PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Transition Matrix (dictates how the regimes transition)
Q = [-1 0.5 0.5;
    0.5 -1 0.5;
    0.5 0.5 -1];

sigma_vec = [0.15  0.25  0.35]; % Volatility in each state

initial_state = 1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  Step 3) CHOOSE PROJ PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
order = 3;  %Choose spline order from { 0,1,2,3} => {Haar, Linear, Quadratic, Cubic}, Haar is least accurate
logN  = 8;   %Uses N = 2^logN  gridpoint
L1 = 8;  % determines grid witdth (usually set L1 = 8 to 15 for Levy)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% PRICE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alpha = L1*sqrt(T)*max(sigma_vec);   % Choose grid width based on the largest volatility
N = 2^logN;    % grid roughly centered on [- alph, alph]

tic
price = PROJ_RegimeSwitching_European(order, N, alpha, r, q, T, S_0, W, call, Q, sigma_vec, initial_state);
toc

fprintf('%.8f \n', price)


%Program for SVD-based Image Quality Measure

%Program Description
% This program outputs a graphical & numerical image quality measure
% based on Singular Value Decomposition.
% For details on the implementation, please refer
% Aleksandr Shnayderman, Alexander Gusev, and Ahmet M. Eskicioglu,
% "An SVD-Based Grayscale Image Quality Measure for Local and Global Assessment",
% IEEE TRANSACTIONS ON IMAGE PROCESSING, VOL. 15, NO. 2, FEBRUARY 2006.
%
%Parameters
% refImg        -   Input Reference Gray Image
% distImg       -   Input Distorted Gray Image
% blkSize       -   Window size for block processing
% graMeasure    -   Graphical Image quality measure
% scaMeasure    -   Numerical Image quality measure
%
%Author : Athi Narayanan S
%Student, M.E, EST,
%K.S.R College of Engineering
%Erode, Tamil Nadu, India.
%s_athi1983@yahoo.co.in
%http://sites.google.com/site/athisnarayanan/

function [graMeasure, scaMeasure] = SVDQualityMeasure(refImg, distImg, blkSize)

k = size(refImg, 1);

blkx = blkSize;
blky = blkSize;

blockwise1 = MatDec(refImg,blkx);
blockwise2 = MatDec(distImg,blkx);
[blkx blky imgx imgy] = size(blockwise1);
graMeasure = zeros(imgx,imgy);
blockwise1 = double(blockwise1);
blockwise2 = double(blockwise2);

for i=1:imgx
    for j=1:imgy
        temp_in_image = blockwise1(:,:,i,j);temp_in_image=temp_in_image(:);
        original_img = reshape(temp_in_image,blkx,blky);
        temp_dist_image = blockwise2(:,:,i,j);temp_dist_image=temp_dist_image(:);
        distorted_img = reshape(temp_dist_image,blkx,blky);
        graMeasure(i,j) = sqrt(sum((svd(original_img)-svd(distorted_img)).^2));
    end
end

graMeasure = round((graMeasure/max(max(graMeasure)))*255);
graMeasure = uint8(graMeasure);

scaMeasure = sum(sum(abs(graMeasure-median(median(graMeasure)))))/((k/blkx).^2);


% Fig. 6.26   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

num=1;
den=conv([1 0],[1 2 1]);
w=logspace(-2,3,100);
[re,im]=nyquist(num,den,w);
plot(re,im,re,-im);
axis([-2 2 -2 2]);
axis('square')
xlabel('Re(G(s))');
ylabel('Im(G(s))');
title('Fig. 6.26 Nyquist plot');
nicegrid;
hold on
ii=[11 21 47 61];
plot(re(ii),im(ii),'*');
plot(-.5,0,'*')
hold off


function krn = spm_smoothkern(fwhm,x,t)
% Generate a Gaussian smoothing kernel
% FORMAT krn = smoothing_kernel(fwhm,x,t)
%     fwhm - full width at half maximum
%     x    - position
%     t    - either 0 (nearest neighbour) or 1 (linear).
%            if only two arguments are passed, then a value
%            of one is assumed.
%     krn  - value of kernel at position x
%
% For smoothing images, one should really convolve a Gaussian
% with a sinc function.  For smoothing histograms, the
% kernel should be a Gaussian convolved with the histogram
% basis function used. This function returns a Gaussian
% convolved with a triangular (1st degree B-spline) basis
% function (by default). A Gaussian convolved with a hat
% function (0th degree B-spline) can also be returned.
%_______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id$

if nargin<3, t = 1; end;

% Variance from FWHM
s = (fwhm/sqrt(8*log(2)))^2+eps;

% The simple way to do it. Not good for small FWHM
% krn = (1/sqrt(2*pi*s))*exp(-(x.^2)/(2*s));

if t==0
    % Gaussian convolved with 0th degree B-spline
    % int(exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= -0.5..0.5)
    w1  = 1/sqrt(2*s);
    krn = 0.5*(erf(w1*(x+0.5))-erf(w1*(x-0.5)));
    krn(krn<0) = 0;

elseif t==1
    % Gaussian convolved with 1st degree B-spline
    %  int((1-t)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t= 0..1)
    % +int((t+1)*exp(-((x+t))^2/(2*s))/sqrt(2*pi*s),t=-1..0)
    w1  =  0.5*sqrt(2/s);
    w2  = -0.5/s;
    w3  = sqrt(s/2/pi);
    krn = 0.5*(erf(w1*(x+1)).*(x+1) + erf(w1*(x-1)).*(x-1) - 2*erf(w1*x   ).* x)...
          +w3*(exp(w2*(x+1).^2)     + exp(w2*(x-1).^2)     - 2*exp(w2*x.^2));
    krn(krn<0) = 0;

else
    error('Only defined for nearest neighbour and linear interpolation.');
    % If anyone knows a nice formula for a sinc function convolved with a
    % a Gaussian, then that could be quite useful.
end;
return;


function [ mX ] = ProjectSymmetricMatrixSet( mX )
% ----------------------------------------------------------------------------------------------- %
% [ mX ] = ProjectSymmetricMatrixSet( mX )
%   Projecting the input matrix into the Convex Set of Symmetric Matrices.
% Input:
%   - mX            -   Input Matrix.
%                       Structure: Matrix.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% Output:
%   - mX            -   Output Matrix.
%                       Symmteirc Matrix which is the orthogonal projection
%                       of the input matrix.
%                       Structure: Matrix.
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
% References
%   1.  h
% Remarks:
%   1.  a
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     15/08/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

FALSE   = 0;
TRUE    = 1;

OFF     = 0;
ON      = 1;

mX = (mX.' + mX) / 2;

end


% DEMO_fitSpline.m
%
% Fit a spline to data, which can then be evaluated by ppval

tSpan = [0,10];
nData = 100;
nKnot = 10;

tData = linspace(tSpan(1),tSpan(2),nData);
xData = sin(tData);
yData = cos(tData);

tKnot = linspace(tSpan(1),tSpan(2),nKnot);

pp = fitSpline(tData, [xData;yData], tKnot);

zSpline = ppval(pp,tData);

figure(15); clf;

subplot(2,1,1); hold on;
plot(tData,xData,'ko')
plot(tData,zSpline(1,:),'b-')
legend('data','spline');

subplot(2,1,2); hold on;
plot(tData,yData,'ko')
plot(tData,zSpline(2,:),'b-')
legend('data','spline');


function [A,Q] = lti_disc(F,L,Q,dt)
% LTI_DISC Equivalent discrete-time solution of an LTI SDE
%
% Syntax:
%   [A,Q] = lti_disc(F,L,Qc,dt)
%
% In:
%   F  - NxN Feedback matrix
%   L  - NxL Noise effect matrix        (optional, default identity)
%   Qc - LxL Diagonal Spectral Density  (optional, default zeros)
%   dt - Time step                      (optional, default 1)
%
% Out:
%   A - Transition matrix
%   Q - Discrete process covariance matrix
%
% Description:
%   Equivalent discrete-time solution of an LTI SDE of form
%
%     dx/dt = F x + L w,
%
%   where w(t) is a white-noise process with spectral density Qc.
%   Results in the following model:
%
%     x[k] = A x[k-1] + q, q ~ N(0,Q).
%
%   Can be used for integrating the model exactly over time steps,
%   which are multiples of dt.
%
% Copyright:
%   2019 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

  % Check number of arguments and defaults
  if nargin < 1
    error('Too few arguments');
  end
  if nargin < 2
    L = [];
  end
  if nargin < 3
    Q = [];
  end
  if nargin < 4
    dt = [];
  end
  if isempty(L)
    L = eye(size(F,1));
  end
  if isempty(Q)
    Q = zeros(size(F,1),size(F,1));
  end
  if isempty(dt)
    dt = 1;
  end

  % Closed-form integration of the transition matrix
  A = expm(F*dt);

  % Closed-form integration of the covariance matrix
  n   = size(F,1);
  Phi = [F L*Q*L'; zeros(n,n) -F'];
  AB  = expm(Phi*dt)*[zeros(n,n);eye(n)];
  Q   = AB(1:n,:)*A'; % A' = inv(AB((n+1):(2*n),:));


clc;
close all;
clearvars;
rng('default');
n = 10;
% Let's get a symmetric positive definite matrix
A = gallery('gcdmat', n);
% Compute its Cholesky decomposition
L = chol(A, 'lower')
b = randn(n, 1);
% solve the problem
x = L \b
x = spx.la.tris.forward_row(L, b)


%% Gauss Laguerre Quadrature
% solution for class exercise:

clear
clc
close all

%% Aproximate solution
% Tau(m) is a function of m.

m=5.5   % Exponent value of f(x).
n=9;    % number of points used to compute approximate solution.

[x w]=GaussLaguerre(n,0); % built in function to generate weight and points

% Initalizing row vectors:
l=length(x);
f=zeros(1,l);
t=zeros(1,l);

for i=1:l
    f(i)=x(i)^(m-1);
    t(i)=w(i)*f(i);
end

Gamma=sum(t)

%% Exact Solution
% Gamma=(m-1)!
% Carefull! m can only be an integer number!

Gamma2=factorial(floor(m)-1)
% I'm using a round to the floor in case m is a rational number.

%% Exact solution of the Gamma function
% this time using the Matlab's gamma function.

Gamma3=gamma(m)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Solve the following exercises:
%   A) Cart and pendulum.
%              Simulate a cart and pendulum system. Then, try to control it
%              using a P controller.
%
% 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 cart_and_pendulum_controlled()
close all;

%uncomment to execute each of the exercises
exerciseA()%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulate a cart and pendulum
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function exerciseA()
global L
L = 0.8; % m length of the pendulum
t0 = 0;
tfinal = 10;
[t, x] = runge_kutta(@fcart_and_pendulum, [0 0 0.01 0.01]', [t0 tfinal], 0.01);
x = x(:, 1:length(t));

% animate the system
close all
figure
for i=1:length(t)
    %cart position
    posx_cart = x(1,i);
    posy_cart = 0;

    theta = x(3,i);
    %pendulum position
    posx_pend = L*sin(theta) + posx_cart;
    posy_pend = L*cos(theta);
    plot(posx_cart, posy_cart, 'rs', 'MarkerSize', 30), hold on
    plot(posx_pend, posy_pend, 'bo', 'MarkerSize', 12)
    xlim([-5 5])
    ylim([-0.9 0.9])
    line([posx_cart posx_pend], [posy_cart posy_pend])
    pause(0.05);
    hold off
end
% plot results
figure
plot(t, x(1,:), 'r'), hold
plot(t, x(2,:), 'g')
plot(t, x(3,:), 'b')
plot(t, x(4,:), 'k')
legend('Position x (m)', 'Speed xd (m/s)', '\theta (rad)', '\thetad (rad/s)')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Helper function to solve a constant acceleration on x axis on a ramp.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xd = fcart_and_pendulum(t, x)
% We must return the solution of
% [dx1/dt; dx2/dt]
global L
M = 5; % kg mass of the cart
m = 5; % kg mass of the pendulum (centered at the tip)
g = 9.81; %m/s^ 2

% Computing a simple P controller
ref = 0; % theta reference
theta = x(3);
error = ref-theta;
k = 1000;
% the control action!
F = k*error;
% if F > 20
%     F=20;
% elseif F<-20
%     F=-20
% end

% compute xdd and thetadd to simulate the system
num = F + m*sin(x(3))*(g*cos(x(3)) - L*x(4)^2);
den = M + m*(1-cos(x(3))^2);
xdd = num/den;

num = F + g*(M+m)*tan(x(3))-m*L*x(4)^2*sin(x(3));
den = L*((M+m)/cos(x(3)) - m*cos(x(3)));
thetadd = num/den;

xd(1) = x(2);
xd(2) = xdd;
xd(3) = x(4);
xd(4) = thetadd;
xd = xd(:);


function [out, bestL] = Peubseparate(k, d, PeCh, e)
%Excess-distortion probability for the transmission of a Gaussian source
%over a Gaussian channel as achieved by a separated scheme - optimized with respect to the number of
%messages allowable between the channel encoder and the channel decoder (2^L)

%PeCh - array of channel error probabilities
%k - source blocklength
%d - target distortion
%e - excess distortion probability

%outputs:
%out - the excess distortion probability
%bestL - the optimal length of messages exchanged between the encoder and
%the decoder

%
%   Created in 2012 by Victoria Kostina (vkostina@caltech.edu)
%

Kbig = 20;
tol = 1e-3;

out = Inf;
bestL = NaN;
for L = find(PeCh <= e)
    cur = PeubSource(L, k, d) + PeCh(L);
    if out > cur
        out = cur;
        bestL = L;
    end
end
%fprintf('ASeparate: k = %i, Peub = %f\n', k, out);

    function out = PeubSource(L, k, d)
        %achievability via sphere covering

        r0 = sqrt( max( 0, 1 - d) );
        a = r0 - sqrt(d);
        b = r0 + sqrt(d);

        out = chi2cdf((a^2 + tol)*k, k) + 1 - chi2cdf((b^2 - tol)*k, k)...  %nontypical source realization
            + quad(@Pes, a^2 + tol, b^2- tol);                              %source error

        function out = logPdball(x)
            %x - distance squared from the origin (x*k central chi square k)
            cosa = (r0^2 + x - d)./(2*r0.*sqrt(x));
            sina = (1 - cosa.^2).^(1/2);
            out = loggamma(k/2+1, 'LB') - loggamma((k-1)/2 +1, 'UB')- .5*log(pi)-log(k) + (k-1)*log(sina);
        end

        function out = Pes(x)
            %source error conditioned on the distance squared from the origin (x*k central chi square k)
            if k <  Kbig
                out = (1 - exp(logPdball(x))).^exp(L);
            else
                out = exp( - exp( logPdball(x) + L));
            end
            out = out.*chi2pdf(k*x, k).*k;
        end
    end
end