function [xyZ,sigma,gammaVal]=ellips2PolarStereo(latLon,lambda0,k0,xyPole,a,f)
%%ELLIPS2POLARSTEREO Convert a point on the reference ellipsoid in
%            ellipsoidal (geodetic) coordinates gives as a latitude and a
%            longitude into a point on the polar stereographic projection.
%            Stereographic projections tend to be used near the poles. The
%            Uniform Polar Stereographic (UPS) coordinate system, which is
%            a specific realization of the polar stereographic coordinate
%            system, is typically only used at latitudes >=84 degrees and
%            those < -80 degrees.
%
%INPUTS: latLon A 2XN set of N [latitude;longitude] pairs given in radians
%               to be converted into polar stereographic coordinates.
%       lambda0 The location in raidns of the central meridian. If this
%               parameter is omitted or an empty matrix is passed, the
%               default of 0 is used, which is the value used in the UPS
%               system.
%            k0 The unitless central scale factor. If this parameter is
%               omitted or an empty matrix is passed, then the default of
%               0.994 is used, which is the value used in the UPS.
%        xyPole A 2X1 vector holding the false Easting and False
%               Northing of the origin. This is typically given in meters.
%               If this parameter is omitted or an empty matrix is passed,
%               then the default of [2000000;2000000] is used, which is the
%               value in meters used for the UPS system.
%             a The semi-major axis of the reference ellipsoid. If this
%               argument is omitted, the value in
%               Constants.WGS84SemiMajorAxis is used.
%             f The flattening factor of the reference ellipsoid. If this
%               argument is omitted, the value in Constants.WGS84Flattening
%               is used.
%
%OUTPUTS: xyZ A 3XN set of points in polar stereographic coordinates
%             consisting of [Easting;Northing;Z], where Z=sign(latitude),
%             so 1 for the northern hemisphere and -1 for the southern
%             hemisphere and 0 on the equator. Easting and Northing will be
%             in meters if a and xyPole are in meters.
%       sigma The NX1 set of point scales. This indicates how the map
%             projections enlarges or reduces small distances in this type
%             of map projection.
%    gammaVal The NX1 set of convergence of meridians in radians. This
%             gives the angles of intersection between the meridian and
%             vertical lines on the map projection that are given by
%             x=constant value. This value
%
%If lambda0, k0, xyPole, a and f are all omitted, then the conversion is
%for the UPS on the WGS-84 reference ellipsoid.
%
%Chapter 21 of [1] discusses the polar stereographic projection. The
%implementation is taken from Sections 8 and 9 of [1]. Section 10 of 2
%provides the values for the UPS.
%
%REFERENCES:
%[1] J. P. Snyder, "Map projections- a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%[2] Office of Geomatics, "National geospatial-intelligence agency
%    standardization document: Implementation practice: The universal
%    grids and the transverse mercator and polar stereographic map
%    projections," National Geospatial-Intelligence Agency, Tech. Rep.
%    NGA.SIG.0012 2.0.0 UTMUPS, 25 Mar. 2014. [Online]. Available:
%    http://earth-info.nga.mil/GandG/publications/NGA_SIG_0012_2_0_0_UTMUPS/NGA.SIG.0012_2.0.0_UTMUPS.pdf
%
%July 2017 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(lambda0))
   lambda0=0;%Central meridian location for UPS.
end

if(nargin<3||isempty(k0))
   k0=0.994;%Central scale factor for UPS
end

if(nargin<4||isempty(xyPole))
   xyPole=[2000000;2000000];%Easting and Northing of the pole for UPS.
end

if(nargin<5||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<6||isempty(f))
    f=Constants.WGS84Flattening;
end

%The first numerical eccentricity of the ellipsoid.
e=sqrt(2*f-f^2);

N=size(latLon,2);

xyZ=zeros(3,N);
sigma=zeros(N,1);
gammaVal=zeros(N,1);
for curPoint=1:N
    phi=latLon(1,curPoint);
    lambda=latLon(2,curPoint);
    lambda=lambda-lambda0;

    Z=sign(phi);
    if(Z==-1)
        phi=-phi;
    end

    sinPhi=sin(phi);
    sinLambda=sin(lambda);
    cosLambda=cos(lambda);

    [sinChi,cosChi]=ellipsLat2SinCosConformLat(phi,f);

    k90=sqrt(1-e^2)*exp(e*atanh(e));
    denom=k90*(1+sinChi);
    x=2*k0*a*sinLambda*cosChi/denom;
    y=-2*k0*a*cosLambda*cosChi/denom;
    sigma(curPoint)=k0*2*sqrt(1-e^2*sinPhi^2)*exp(e*atanh(e*sinPhi))/(k90*(1+sinPhi));
    gammaVal(curPoint)=lambda;

    if(Z==-1)
        y=-y;
        gammaVal(curPoint)=-gammaVal(curPoint);
    end

    xyZ(:,curPoint)=[x;y;Z]+[xyPole;0];
end
end

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


% This function implements the relaxed sparsification descibed in Section 3.4

function softSegments = sparsifySegments(softSegments, Laplacian, imageGrad)

    sigmaS = 1; % sparsity
    sigmaF = 1; % fidelity
    delta = 100; % constraint
    [h, w, compCnt] = size(softSegments);
    N = h * w * compCnt;

    if ~exist('imageGrad', 'var') || isempty(imageGrad)
        % If image gradient is not provided, set the param to the default 0.9
        spPow = 0.90;
    else
        % Compute the per-pixel sparsity parameter from the gradient
        imageGrad(imageGrad > 0.1) = 0.1;
        imageGrad = imageGrad + 0.9;
        spPow = repmat(imageGrad(:), [compCnt, 1]);
    end

    % Iter count for pcg and main optimization
    itersBetweenUpdate = 100;
    highLevelIters = 20;

    % Get rid of very low/high alpha values and normalize
    softSegments(softSegments < 0.1) = 0;
    softSegments(softSegments > 0.9) = 1;
    softSegments = softSegments ./ repmat(sum(softSegments, 3), [1 1 size(softSegments, 3)]);

    % Construct the linear system
    lap = Laplacian;
    for i = 2 : compCnt
        Laplacian = blkdiag(Laplacian, lap);
    end

    % The alpha constraint
    C = repmat(speye(h*w), [1 compCnt]);
    C = C' * C;
    Laplacian = Laplacian + delta * C;

    % The sparsification optimization
    softSegments = softSegments(:);
    compInit = softSegments; % needed for fidelity energy
    for iter = 1 : highLevelIters
        if rem(iter, 5) == 0
            disp(['               Iteration ' int2str(iter) ' of ' int2str(highLevelIters)]);
        end
        [u, v] = getUandV(softSegments, spPow); % The sparsity energy
        A = Laplacian + sigmaS * (spdiags(u, 0, N, N) + spdiags(v, 0, N, N)) + sigmaF * speye(N);
        b = sigmaS * v + sigmaF * compInit + delta;
        [softSegments, ~] = pcg(A, b, [], itersBetweenUpdate, [], [], softSegments);
    end

    % One final iter for good times (everything but sparsity)
    A = Laplacian + sigmaF * speye(N);
    b = sigmaF * softSegments + delta;
    softSegments = pcg(A, b, [], 10 * itersBetweenUpdate, [], [], softSegments);

    % Ta-dah
    softSegments = reshape(softSegments, [h w compCnt]);
end

function [u, v] = getUandV(comp, spPow)
    % Sparsity terms in the energy
    eps = 1e-2;
    u = max(abs(comp(:)), eps) .^ (spPow - 2);
    v = max(abs(1 - comp(:)), eps) .^ (spPow - 2);
end


%kvpml 'Estimate Fractal Dimension of Image Based on P(m,L) (K1)'
% This MatLab function was automatically generated by a converter (KhorosToMatLab) from the Khoros vpml.pane file
%
% Parameters:
% InputFile: i 'Input Image ', required: 'input image'
% Integer: l 'Lower window size', default: 3: 'select the initial size of the sliding window'
% Integer: u 'Upper window size', default: 5: 'select the final size of the sliding window'
% Integer: s 'Window step size ', default: 2: 'select the window step size or interval'
% Integer: q 'Range of moments ', default: 3: 'select the range of moments (-r/2 <= 0 >= r/2)'
% OutputFile: o1 'Output Image ', required: 'resulting multiband fractal dimension output image'
% OutputFile: o2 'Output FD Image ', required: 'output image specifying fractal dimension of each class'
% OutputFile: f1 'Output ASCII File', optional: 'output file for P(m,L) statistics'
%
% Example: [o1, o2, f1] = kvpml(i, {'i','';'l',3;'u',5;'s',2;'q',3;'o1','';'o2','';'f1',''})
%
% Khoros helpfile follows below:
%
%  PROGRAM
% vpml - Estimate Fractal Dimension of Image Based on P(m,L)  (K1)
%
%  DESCRIPTION
% .I vpml
% estimates the fractal dimension of an image based on the probability
% that there are m pixels within a window of size L centered on a pixel
% from a particular class.  For a selected range of window sizes (L),
% the window is centered on the first occurrence of the pixel belonging
% to a particular class.  The number of pixels within a window of size L,
% belonging to a specific class are counted (including the center pixel
% of the window), and a "histogram" is formed as the window is moved over
% the image.  This "histogram" represents the total number of occurrences,
% m, of a class of pixels in a window of size L.  From this, a normalized
% histogram is formed, which yields an estimate of the probability density
% function, P(m,L), for each window size L.
%
% All selected moments (q) of the P(m,L) distributions for each of the
% desired window sizes are determined, and a linear regression or "best fit
% line" is found for the moment generating function, log(M(L))^1/q, versus
% the log of the window size, log(L).  The slope of the "best fit line"
% provides an estimate for the fractal dimension, D.
%
% The center pixel of the largest window, Lmax, is replaced with the computed
% fractal dimension based on the probability density function, P(m,L).
% The largest window size, Lmax, determines the resulting size of the fractal
% dimension image.  This results in a border of size (Lmax / 2) around the
% image, which is set to zero.
%
% The input arguments are described as follows:
%
%  "-i" 15
% specifies the input image, which must be of data type BYTE or INTEGER.
% The input image must be a single band image.
%
%  "-o1" 15
% specifies the output image, which will be a multiband image representing
% the fractal dimension of the input image for each of the specified
% moments.  The resulting output image will be of data storage type
% FLOAT, and will have a border of pixels of value zero.  The size of
% the border will be determined by the size of the largest window
% specified.  This can be determined from the following formulation,
% (Lmax -1) / 2 = border size.  Where Lmax is the size of the largest
% window used.  For example, if the largest window size is 9, then the
% border size will be (9 - 1) / 2 = 4 pixels.
%
%  "-o2" 15
% specifies fractal dimension image, representing the fractal dimension
% vectors for each class and moment.  This image will always have a row
% size of 1 and a column size determined by the number of classes in the
% input image.  The number of data bands will be equal to the number of
% moments used in the estimation of the fractal dimension.
%
%  "-f1" 15
% uses an ASCII file as output for the image information specifying the
% size of the input image, number of classes in the input image, and the
% range of moments used in the estimation of the fractal dimension.  Also
% included is a listing of the fractal dimensions for each class and
% moment.
%
%  "-l" 15
% specifies the initial or lower size of the sliding window.  This MUST be
% an odd number resulting in a window with a center pixel.  This means that
% the minimum size of the sliding window is 3 x 3.  The default value is
% 3, resulting in an initial window size of 3 x 3.
%
%  "-u" 15
% specifies the final or upper size of the sliding window.  This MUST also
% be an odd number resulting in a window with a center pixel.  The size of
% the upper window MUST be at least one step greater than the lower window
% size.  That is, if the lower window is 3 x 3, then the minimum size of
% the upper window must be 5 x 5, or one step greater than the lower window.
% This is necessary, since there must be at least two points for a best
% fit line to be formed determining the slope and ultimately the fractal
% dimension.
%
%  "-s" 15
% specifies the step size or interval used when specifying a range of
% window sizes.  The default value is two, corresponding to the next odd
% window size with a center pixel.  This may be helpful when a wide range
% of window sizes is required, as it will reduce the number of points
% generated for the best fit line and hence the number of calculations.
%
%  "-q" 15
% specifies the range of moments to base the P(m,L) fractal dimension
% calculations on.  This must be an odd number, corresponding to the
% number of moments centered on zero.  For example, a value of 5 would
% result in the computation of the fractal dimension for the range of
% moments, -2, -1, 0, 1, 2.  The default value is 3, resulting in the
% range of -1, 0, 1.
%
% The input image MUST be of data storage type BYTE or INTEGER.
%
%
%
%  EXAMPLES
%
% vpml -i in_img -o1 out_img1 -o2 out_img2 -f1 file1 -u 11 -s 4 -q 5
% this will estimate the fractal dimension of the in_img using a
% range of sliding window sizes from 3 x 3 (default lower value of 3)
% to an upper size of 11 x 11, with a step size of 4.  This means that
% windows of size 3 x 3, 7 x 7, and 11 x 11 will be used in the fractal
% dimension calculation.  A value of 5 was selected for the number of
% moments to be calculated, meaning that the fractal dimension will be
% computed for the range of moments, -2, -1, 0, 1, 2.  The resulting
% output image, out_img1 will consist of a five-band FLOAT image with
% each band representing the fractal dimension for a particular moment, q.
% The out_img2 will consist of a single row, five-band image specifying
% the fractal dimension for each class of the input image and moment, q.
% The ASCII file1 will provide the user with information on the size of
% the input image, number of classes, and the resulting fractal dimensions
% for each class and moment.
%
%  "SEE ALSO"
% vfractal(1)
%
%  RESTRICTIONS
%
% The input image MUST be of data storage type BYTE or INTEGER.
% The output images are of data storage type FLOAT.
%
%  REFERENCES
% A reference for the pml algorithm is p. 67 of The Science of Fract.
% Images edited by Hienz-Otto Peitgen and Dietmar Saupe.
%
%  COPYRIGHT
% Copyright (C) 1993 - 1997, Khoral Research, Inc. ("KRI")  All rights reserved.
%

function varargout = kvpml(varargin)
if nargin ==0
  Inputs={};arglist={'',''};
elseif nargin ==1
  Inputs=varargin{1};arglist={'',''};
elseif nargin ==2
  Inputs=varargin{1}; arglist=varargin{2};
else error('Usage: [out1,..] = kvpml(Inputs,arglist).');
end
if size(arglist,2)~=2
  error('arglist must be of form {''ParameterTag1'',value1;''ParameterTag2'',value2}')
 end
narglist={'i', '__input';'l', 3;'u', 5;'s', 2;'q', 3;'o1', '__output';'o2', '__output';'f1', '__output'};
maxval={0,10000,10000,10000,1000,0,0,1};
minval={0,3,5,2,3,0,0,1};
istoggle=[0,1,1,1,1,0,0,1];
was_set=istoggle * 0;
paramtype={'InputFile','Integer','Integer','Integer','Integer','OutputFile','OutputFile','OutputFile'};
% identify the input arrays and assign them to the arguments as stated by the user
if ~iscell(Inputs)
Inputs = {Inputs};
end
NumReqOutputs=2; nextinput=1; nextoutput=1;
  for ii=1:size(arglist,1)
  wasmatched=0;
  for jj=1:size(narglist,1)
   if strcmp(arglist{ii,1},narglist{jj,1})  % a given argument was matched to the possible arguments
     wasmatched = 1;
     was_set(jj) = 1;
     if strcmp(narglist{jj,2}, '__input')
      if (nextinput > length(Inputs))
        error(['Input ' narglist{jj,1} ' has no corresponding input!']);
      end
      narglist{jj,2} = 'OK_in';
      nextinput = nextinput + 1;
     elseif strcmp(narglist{jj,2}, '__output')
      if (nextoutput > nargout)
        error(['Output nr. ' narglist{jj,1} ' is not present in the assignment list of outputs !']);
      end
      if (isempty(arglist{ii,2}))
        narglist{jj,2} = 'OK_out';
      else
        narglist{jj,2} = arglist{ii,2};
      end

      nextoutput = nextoutput + 1;
      if (minval{jj} == 0)
         NumReqOutputs = NumReqOutputs - 1;
      end
     elseif isstr(arglist{ii,2})
      narglist{jj,2} = arglist{ii,2};
     else
        if strcmp(paramtype{jj}, 'Integer') & (round(arglist{ii,2}) ~= arglist{ii,2})
            error(['Argument ' arglist{ii,1} ' is of integer type but non-integer number ' arglist{ii,2} ' was supplied']);
        end
        if (minval{jj} ~= 0 | maxval{jj} ~= 0)
          if (minval{jj} == 1 & maxval{jj} == 1 & arglist{ii,2} < 0)
            error(['Argument ' arglist{ii,1} ' must be bigger or equal to zero!']);
          elseif (minval{jj} == -1 & maxval{jj} == -1 & arglist{ii,2} > 0)
            error(['Argument ' arglist{ii,1} ' must be smaller or equal to zero!']);
          elseif (minval{jj} == 2 & maxval{jj} == 2 & arglist{ii,2} <= 0)
            error(['Argument ' arglist{ii,1} ' must be bigger than zero!']);
          elseif (minval{jj} == -2 & maxval{jj} == -2 & arglist{ii,2} >= 0)
            error(['Argument ' arglist{ii,1} ' must be smaller than zero!']);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} < minval{jj})
            error(['Argument ' arglist{ii,1} ' must be bigger than ' num2str(minval{jj})]);
          elseif (minval{jj} ~= maxval{jj} & arglist{ii,2} > maxval{jj})
            error(['Argument ' arglist{ii,1} ' must be smaller than ' num2str(maxval{jj})]);
          end
        end
     end
     if ~strcmp(narglist{jj,2},'OK_out') &  ~strcmp(narglist{jj,2},'OK_in')
       narglist{jj,2} = arglist{ii,2};
     end
   end
   end
   if (wasmatched == 0 & ~strcmp(arglist{ii,1},''))
        error(['Argument ' arglist{ii,1} ' is not a valid argument for this function']);
   end
end
% match the remaining inputs/outputs to the unused arguments and test for missing required inputs
 for jj=1:size(narglist,1)
     if  strcmp(paramtype{jj}, 'Toggle')
        if (narglist{jj,2} ==0)
          narglist{jj,1} = '';
        end;
        narglist{jj,2} = '';
     end;
     if  ~strcmp(narglist{jj,2},'__input') && ~strcmp(narglist{jj,2},'__output') && istoggle(jj) && ~ was_set(jj)
          narglist{jj,1} = '';
          narglist{jj,2} = '';
     end;
     if strcmp(narglist{jj,2}, '__input')
      if (minval{jj} == 0)  % meaning this input is required
        if (nextinput > size(Inputs))
           error(['Required input ' narglist{jj,1} ' has no corresponding input in the list!']);
        else
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        end
      else  % this is an optional input
        if (nextinput <= length(Inputs))
          narglist{jj,2} = 'OK_in';
          nextinput = nextinput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end;
     else
     if strcmp(narglist{jj,2}, '__output')
      if (minval{jj} == 0) % this is a required output
        if (nextoutput > nargout & nargout > 1)
           error(['Required output ' narglist{jj,1} ' is not stated in the assignment list!']);
        else
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
          NumReqOutputs = NumReqOutputs-1;
        end
      else % this is an optional output
        if (nargout - nextoutput >= NumReqOutputs)
          narglist{jj,2} = 'OK_out';
          nextoutput = nextoutput + 1;
        else
          narglist{jj,1} = '';
          narglist{jj,2} = '';
        end;
      end
     end
  end
end
if nargout
   varargout = cell(1,nargout);
else
  varargout = cell(1,1);
end
global KhorosRoot
if exist('KhorosRoot') && ~isempty(KhorosRoot)
w=['"' KhorosRoot];
else
if ispc
  w='"C:\Program Files\dip\khorosBin\';
else
[s,w] = system('which cantata');
w=['"' w(1:end-8)];
end
end
[varargout{:}]=callKhoros([w 'vpml"  '],Inputs,narglist);


% This script shows how the finger region and edges can be detected.

img = im2double(imread('finger.png')); % Read image
img = imresize(img, 0.5);              % Downscale image

mask_height=4; % Height of the mask
mask_width=20; % Width of the mask
[fvr, edges] = lee_region(img,mask_height,mask_width);

% Create a nice image for showing the edges
edge_img = zeros(size(img));
edge_img(edges(1,:) + size(img,1)*[0:size(img,2)-1]) = 1;
edge_img(edges(2,:) + size(img,1)*[0:size(img,2)-1]) = 1;

rgb = zeros([size(img) 3]);
rgb(:,:,1) = (img - img.*edge_img) + edge_img;
rgb(:,:,2) = (img - img.*edge_img);
rgb(:,:,3) = (img - img.*edge_img);

% Show the original, detected region and edges in one figure
figure;
subplot(3,1,1)
  imshow(img,[])
  title('Original image')
subplot(3,1,2)
  imshow(fvr)
  title('Finger region')
 subplot(3,1,3)
  imshow(rgb)
  title('Finger edges')


function pic_serial ( )

%*****************************************************************************80
%
%% PIC_SERIAL applies the Particle in Cell method to an electrostatic problem.
%
%  Discussion:
%
%    Simple Electrostatic Particle In Cell (PIC) code in MATLAB
%    Flow of solar wind around a charged plate
%
%    For more, visit http://www.particleincell.com/2010/es-pic-method/
%    and http://www.particleincell.com/2011/particle-in-cell-example/
%
%  Modified:
%
%    27 July 2013
%
%  Parameters:
%
%    Local, real A(NX*NY,NX*NY), the finite difference stencil matrix.
%
%    Local, real ACC, the acceleration experienced by a particle.
%
%    Local, real AMU, the atomic mass unit.
%
%    Local, integer BOX(2,2), the indices of the lower left and upper right
%    corners of the internal obstruction.
%
%    Local, real CHG(NX,NY), the charge distribution.
%
%    Local, real DEBYE, the Debye length.
%
%    Local, real DEN(NX,NY), the charge density.
%
%    Local, real DH, the "diameter" of a single cell.
%
%    Local, real DT, the time step.
%
%    Local, real E(2), the electric field experienced by a particle.
%
%    Local, real EFX(NX,NY), the X component of the electric field.
%
%    Local, real EFY(NX,NY), the Y component of the electric field.
%
%    Local, real EPS0, the permittivity of free space.
%
%    Local, real F, the Lorentz force experienced by a particle.
%
%    Local, real FLUX, the flux of entering particles.
%
%    Local, real K, the Boltzmann constant.
%    Oddly enough, this value seems never to be used.
%
%    Local, real M, the ion mass for molecular oxygen.
%
%    Local, real MP_Q, the macro-particle charge.
%
%    Local, integer N0, the average particle density per cubic meter.
%
%    Local, integer NN, the total number of nodes.
%
%    Local, integer NP, the number of particles.
%
%    Local, integer NP_INSERT, the number of (computational) particles inserted
%    per time step.
%
%    Local, real NPT, the number of real particles created per time step.
%
%    Local, integer NX, the number of nodes in the X direction.
%
%    Local, integer NY, the number of nodes in the Y direction.
%
%    Local, integer PART_MAX, the maximum number of particles.
%    Here, this is set to 20,000.
%
%    Local, real PART_V(NP,2), the X and Y velocities for each particle.
%
%    Local, real PART_X(NP,2), the X and Y coordinates for each particle.
%
%    Local, real PHI(NX,NY), the potential.
%
%    Local, real PHI_P, the wall potential.
%
%    Local, real PHI0, the reference potential.
%
%    Local, real QE, the elementary charge.
%
%    Local, integer SEED, a seed for the random number generator.
%
%    Local, real SPWT, the specific weight, real particles per macroparticle.
%
%    Local, integer STEP, the current time step.
%
%    Local, integer STEP_NUM, the number of time steps.
%
%    Local, real TE, the electron temperature in eV.
%
%    Local, real TI, the ion velocity in eV.
%
%    Local, real V_DRIFT, the ion injection velocity, 7 km/s.
%
%    Local, real VTH, the thermal velocity.
%
  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PIC_SERIAL\n' );
  fprintf ( 1, '  MATLAB version.\n' );
  fprintf ( 1, '  Apply the Particle in Cell (PIC) method to\n' );
  fprintf ( 1, '  an electrostatic problem that models the flow of\n' )
  fprintf ( 1, '  the "solar wind" around an electrically charged plate.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  It is worth considering how to turn this serial program\n' );
  fprintf ( 1, '  into one that uses parallel programming techniques.\n' );
%
%  Initialize the random number generator.
%
  seed = 123456789;
% rng ( seed );
%
%  Set physical constants.
%
  eps0 = 8.854e-12;
  qe = 1.602e-19;
  k = 1.381e-23;
  amu = 1.661e-27;
  m = 32.0 * amu;
%
%  Set problem parameters.
%
  n0 = 1.0e12;
  phi0 = 0.0;
  te = 1.0;
  ti = 0.1;
  v_drift = 7000.0;
  phi_p = -5.0;
%
%  Calculate plasma parameters.
%
  debye = sqrt ( eps0 * te / ( n0 * qe ) );
  vth = sqrt ( 2.0 * qe * ti / m );
%
%  Set the simulation domain.
%
  nx = 16;
  ny = 10;
  nn = nx * ny;
  step_num = 200;
  dh = debye;
  np_insert = ( ny - 1 ) * 15;
%
%  Compute other quantities.
%
  dt = 0.1 * dh / v_drift;
  width = ( nx - 1 ) * dh;
  height = ( ny - 1 ) * dh;
%
%  Indices of the corners of the obstruction.
%
  ox1 = floor ( nx / 3 );
  ox2 = floor ( nx / 3 ) + 2;
  oy1 = 1;
  oy2 = floor ( ny / 2 );
%
%  Indices of the corners of the obstruction, in an array.
%
  box(1,1) = floor ( nx / 3 );
  box(1,2) = floor ( nx / 3 ) + 2;
  box(2,1) = 1;
  box(2,2) = floor ( ny / 2 );
%
%  Calculate the specific weight.
%
  flux = n0 * v_drift * height;
  npt = flux * dt;
  spwt = npt / np_insert;
  mp_q = 1.0;
  part_max = 20000;
%
%  Allocate particle arrays.
%
  part_x = zeros(part_max,2);
  part_v = zeros(part_max,2);
%
%  Set the finite difference stencil matrix.
%
  A = stencil ( nx, ny, dh, box );

  phi = ones ( nx, ny ) * phi0;

  np = 0;
%
%   Time loop
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '    Time  Particles\n' );
  fprintf ( 1, '\n' );

  for step = 1 : step_num
%
%  1. CALCULATE THE CHARGE DENSITY.
%
    chg = zeros(nx,ny);

    for p = 1 : np

      fi = 1.0 + part_x(p,1) / dh;
      i = floor ( fi );
      hx = fi - i;

      fj = 1.0 + part_x(p,2) / dh;
      j = floor ( fj );
      hy = fj - j;

      chg(i,  j)   = chg(i,  j)   + ( 1.0 - hx ) * ( 1.0 - hy );
      chg(i+1,j)   = chg(i+1,j)   +         hx   * ( 1.0 - hy );
      chg(i,  j+1) = chg(i,  j+1) + ( 1.0 - hx ) *         hy;
      chg(i+1,j+1) = chg(i+1,j+1) +         hx   *         hy;

    end
%
%  Calculate the density.
%  Along the boundaries, the calculated density must be doubled.
%  Add a density floor for plotting, and to help the solver.
%
    den(1:nx,1:ny) = spwt * mp_q * chg(1:nx,1:ny) / ( dh * dh );

    den(1,:)  = 2.0 * den(1,:);
    den(nx,:) = 2.0 * den(nx,:);
    den(:,1)  = 2.0 * den(:,1);
    den(:,ny) = 2.0 * den(:,ny);

    den = den + 10000.0;
%
%  2. CALCULATE THE POTENTIAL.
%
    phi = eval_2dpot_gs ( nx, ny, phi, A, den, n0, phi0, te, phi_p, box, ...
      eps0, qe );
%
%  3. CALCULATE THE ELECTRIC FIELD.
%  Use central differences at internal nodes,
%  forward or backward differences at boundaries.
%
    efx = zeros(nx,ny);
    efy = zeros(nx,ny);

    efx(2:nx-1,:) = ( phi(1:nx-2,:) - phi(3:nx,:) ) / ( 2.0 * dh );
    efy(:,2:ny-1) = ( phi(:,1:ny-2) - phi(:,3:ny) ) / ( 2.0 * dh );

    efx(1,:)  = ( phi(1,:)    - phi(2,:) )  / dh;
    efx(nx,:) = ( phi(nx-1,:) - phi(nx,:) ) / dh;
    efy(:,1)  = ( phi(:,1)    - phi(:,2) )  / dh;
    efy(:,ny) = ( phi(:,ny-1) - phi(:,ny) ) / dh;
%
%  4. GENERATE NEW PARTICLES
%
    if ( part_max - np <= np_insert  )
%     np_insert = part_max - np;
    end
%
%  The new particles have coordinates randomly chosen within the first X layer,
%  and any Y layer.
%
    part_x(np+1:np+np_insert,1) = rand ( np_insert, 1 ) * dh;
    part_x(np+1:np+np_insert,2) = rand ( np_insert, 1 ) * height;
%
%  Sample Maxwellian in x and y, add drift velocity in x.
%
    part_v(np+1:np+np_insert,1) = v_drift ...
      + ( - 1.5 + rand(np_insert,1) + rand(np_insert,1) ...
      + rand(np_insert,1) ) * vth;

    part_v(np+1:np+np_insert,2) = 0.5 ...
      * ( - 1.5 + rand(np_insert,1) + rand(np_insert,1) ...
      + rand(np_insert,1) ) * vth;

    np = np + np_insert;
%
%  5. MOVE ALL THE PARTICLES
%
    p = 1;

    while ( p <= np )

      fi = 1.0 + part_x(p) / dh;
      i = floor ( fi );
      hx = fi - i;

      fj = 1.0 + part_x(p,2) / dh;
      j = floor ( fj );
      hy = fj - j;

      e = [ 0.0, 0.0 ];
      e =     [ efx(i,j),     efy(i,j)     ] * ( 1.0 - hx ) * ( 1.0 - hy );
      e = e + [ efx(i+1,j),   efy(i+1,j)   ] *         hx   * ( 1.0 - hy );
      e = e + [ efx(i,j+1),   efy(i+1,j)   ] * ( 1.0 - hx ) *         hy;
      e = e + [ efx(i+1,j+1), efy(i+1,j+1) ] *         hx   *         hy;
%
%  Compute the Lorentz force and the corresponding acceleration.
%  Then update the particle velocity and position.
%
      f = qe * e;
      acc = f / m;
      part_v(p,:) = part_v(p,:) + acc * dt;
      part_x(p,:) = part_x(p,:) + part_v(p,:) * dt;
%
%  The bottom boundary is reflective.
%
      if ( part_x(p,2) < 0 )
        part_x(p,2) = - part_x(p,2);
        part_v(p,2) = - part_v(p,2);
      end
%
%  Is the particle inside the plate?
%
      in_box = ( box(1,1) <= i && i < box(1,2) && ...
                 box(2,1) <= j && j < box(2,2) );
%
%  Particle is absorbed if it passes left, right or top boundaries,
%  or is inside the plate.
%  Kill the particle by replacing it with the last particle.
%
      if ( part_x(p,1) < 0.0 || width <= part_x(p,1) || height <= part_x(p,2) || in_box )
        part_x(p,:) = part_x(np,:);
        part_v(p,:) = part_v(np,:);
        np = np - 1;
        p = p - 1;
      end

      p = p + 1;

    end
%
%  6. PLOT RESULTS
%
    if ( mod ( step, 25 ) == 0 || step == step_num )

      figure ( 1 );
      clf
      hold on
      contourf ( den' );
      colorbar;
      patch ( [ ox1 ox2 ox2 ox1 ], [ oy1, oy1, oy2, oy2 ], 'w' )
      title ( sprintf ( 'Density on step %i', step ), 'Fontsize', 16 );
      hold off;

      figure ( 2 );
      clf
      hold on
      contourf ( phi' );
      patch ( [ ox1 ox2 ox2 ox1 ], [ oy1, oy1, oy2, oy2 ], 'w' )
      title ( sprintf ( 'Potential on step %i', step ), 'Fontsize', 16 );
      colorbar;
      hold off

      drawnow;

    end

    fprintf ( 1, '  %6i  %9i\n', step, np );

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

  return
end
function phi = eval_2dpot_gs ( nx, ny, phi, A, den, n0, phi0, te, phi_p, ...
  box, eps0, qe )

%*****************************************************************************80
%
%% EVAL_2DPOT_GS computes the 2D potential using Gauss-Seidel iteration.
%
%  Discussion:
%
%    For more information, refer to
%      http://www.particleincell.com/2010/es-pic-method/
%    and
%      http://www.particleincell.com/2011/particle-in-cell-example/
%
%  Modified:
%
%    05 July 2013
%
%  Parameters:
%
%    Input, integer NX, NY, the number of nodes in the X and Y directions.
%
%    Input, real PHI(NX,NY), the current potential function.
%
%    Input, real A(NX*NY,NX*NY), the finite difference stencil matrix.
%
%    Input, real DEN(NX,NY), the charge density.
%
%    Input, real PHI0, the reference potential.
%
%    Input, real TE, the electron temperature in eV.
%
%    Input, real EPS0, the permittivity of free space.
%
%    Input, real QE, the elementary charge.
%
%    Output, real PHI(NX,NY), the updated potential function.
%
  nn = nx * ny;
%
%  Solver tolerance.
%
  tol = 0.1;
%
%  Convert the density and potential into column vectors.
%
  den = reshape ( den, numel(den), 1 );
  phi = reshape ( phi, numel(phi), 1 );
%
%  Carry out the Gauss-Seidel iteration.
%
  for it = 1 : 2000
%
%  Recalculate the right hand side, adding Boltzmann term for the electrons.
%
    b = den - n0 * exp ( ( phi - phi0 ) / te );
    b = - b * qe / eps0;
%
%  Set the boundaries.
%  Zero electric field on y = 0, y = L, x = L.
%  Fixed potential on X = 0.
%
    b(1:nx) = 0.0;
    b(nn-nx+1:nn) = 0.0;
    b(nx:nx:nn) = 0.0;
    b(1:nx:nn) = phi0;
%
%  Set the potential on the fixed nodes.
%
    for j = box(2,1) : box(2,2)
      b([box(1,1):box(1,2)]+(j-1)*nx) = ones ( box(1,2)-box(1,1)+1, 1 ) * phi_p;
    end
%
%  Apply the Gauss-Seidel update to the current solution estimate.
%
    for i = 1 : nn
      phi(i) = ( b(i) - A(i,1:i-1)  * phi(1:i-1) ...
                      - A(i,i+1:nn) * phi(i+1:nn) ) / A(i,i);
    end
%
%  Compute the residual.
%
    if ( mod ( it, 10 ) == 0 )
      res = norm ( b - A * phi );
      if ( res <= tol )
        phi = reshape ( phi, nx, ny );
        return;
      end
    end

  end
%
%  Check if the solver converged.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EVAL_2DPOT_GS - Warning\n' );
  fprintf ( 1, '  The Gauss-Seidel iteration did not converge.\n' );
  fprintf ( 1, '  Residual norm = %g\n', res );

  phi = reshape ( phi, nx, ny );

  return
end
function A = stencil ( nx, ny, dh, box )

%*****************************************************************************80
%
%% STENCIL sets up the finite difference stencil matrix.
%
%  Modified:
%
%    05 July 2013
%
%  Parameters:
%
%    Input, integer NX, NY, the number of nodes in the X and Y directions.
%
%    Local, real DH, the "diameter" of a single cell.
%
%    Input, integer BOX(2,2), contains the coordinates of the lower left
%    and upper right corners of the box.
%
%    Output, real A(NX*NY,NX*NY), the finite difference stencil matrix.
%
  A = zeros ( nx*ny, nx*ny );
%
%  For internal nodes, here are the node numberings for the
%  north, west, central, east, and south locations:
%
%             u-nx
%      u-1    u      u+1
%             u+nx
%
  for j = 2 : ny - 1
    for i = 2 : nx - 1

      u = ( j - 1 ) * nx + i;

      A(u,u)    = -4.0 / ( dh * dh );
      A(u,u-1)  =  1.0 / ( dh * dh );
      A(u,u+1)  =  1.0 / ( dh * dh );
      A(u,u-nx) =  1.0 / ( dh * dh );
      A(u,u+nx) =  1.0 / ( dh * dh );

    end
  end
%
%  Neumann boundary on y=0
%
  j = 1;
  for i = 1 : nx
    u = ( j - 1 ) * nx + i;
    A(u,u)    = -1.0 / dh;
    A(u,u+nx) =  1.0 / dh;
  end
%
%  Neumann boundary on y=height
%
  j = ny;
  for i = 1 : nx
    u = ( j - 1 ) * nx + i;
    A(u,u-nx) =  1.0 / dh;
    A(u,u)    = -1.0 / dh;
  end
%
%  Neumann boundary on x=width
%
  i = nx;
  for j = 1 : ny
    u = ( j - 1 ) * nx + i;
    A(u,:) = zeros ( 1, nx * ny );
    A(u,u-1) =  1.0 / dh;
    A(u,u)   = -1.0 / dh;
  end
%
%  Dirichlet boundary on x=0
%
  i = 1;
  for j = 1 : ny
    u = ( j - 1 ) * nx + i;
    A(u,:) = zeros ( 1, nx * ny );
    A(u,u) = 1.0;
  end
%
%  Dirichlet boundary on nodes corresponding to the plate
%
  for j = box(2,1) : box(2,2)
    for i = box(1,1) : box(1,2)
      u = ( j - 1 ) * nx + i;
      A(u,:) = zeros ( 1, nx * ny );
      A(u,u) = 1.0;
    end
  end

  return
end
function timestamp ( )

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

  return
end


function R = sw_ht_converse(a, n_vals, eps)
% Compute the converse sum rate (at the symmetrical rate point) from Han's point-to-point converse
% for the joint source distribution: [a 1/3*(1-a); 1/3*(1-a) 1/3*(1-a)].

% Input:
% a: parameter to define the joint source distribution, should be in range [0.25, 1)
% n_vals: a vector containing the blocklengths
% eps: target error probability
% Output:
% R: lower bounds of the rates for the blocklengths specified in n_vals
% Other:
% One can also choose to return
% R_sw_ht_hi: upper bounds of the rates for the blocklengths specified in n_vals
% R_sw_ht_lo: lower bounds of the rates for the blocklengths specified in n_vals

    % Compute the optimal log_c coefficients from binary hypothesis testing,
    % which serve as the start point for computing the parameters for
    % composite hypothesis testing.
    [~, log_c_binary] = p2p_ht_converse(a, n_vals, eps);

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

    R_sw_ht_hi = zeros(n_vals,1);
    R_sw_ht_lo = zeros(n_vals,1);

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

        % Initial points
        x = log_c_binary(n) - 0.1;
        y = log_c_binary(n) - 0.0001;

        [beta1, beta2] = my_bisect(n,eps,x,log_Pr,log_Pm,log_b);
        bx = [beta1 beta2];
        diffx = beta1 - beta2 > 0;
        [beta1, beta2] = my_bisect(n,eps,y,log_Pr,log_Pm,log_b);
        by = [beta1 beta2];
        diffy = beta1 - beta2 > 0;

        % For catching special cases: boolean 1 -> zero exists, 0 -> no zero
        disp(strcat('n =',32,num2str(n),',',32,num2str(diffx ~= diffy)));

        if diffx ~= diffy
            % Do bisection search if zero exists
            iter = 0;
            while abs(x-y) > 0.0001 && iter < 20
                c = (x+y)/2;
                [beta1, beta2] = my_bisect(n,eps,c,log_Pr,log_Pm,log_b);
                if (beta1 - beta2 > 0) == diffx
                   x = c;
                   bx = [beta1 beta2];
                else
                   y = c;
                   by = [beta1 beta2];
                end
                iter = iter + 1;
            end
            [M,I] = min([max(bx) max(by)]);  % Minimum upper bound
            R_sw_ht_hi(ni) = M;
            lo = [min(bx) min(by)];
            R_sw_ht_lo(ni) = lo(I);          % A lower bound
        else
            % Scan a range of possible parameters for the best bound
            if (n < 100)
                c_val = log_c_binary(n)-0.1:0.0001:log_c_binary(n)-0.0001;
            else
                c_val = log_c_binary(n)-0.005:0.0001:log_c_binary(n)-0.0001;
            end
            beta1_val = zeros(length(c_val),1);
            beta2_val = zeros(length(c_val),1);
            for ci = 1:length(c_val)
                c = c_val(ci);
                [beta1, beta2] = my_bisect(n,eps,c,log_Pr,log_Pm,log_b);
                beta1_val(ci) = beta1;
                beta2_val(ci) = beta2;
            end
            R_sw_ht_hi(ni) = min(max(beta1_val,beta2_val));  % Minimum upper bound
            R_sw_ht_lo(ni) = max(min(beta1_val,beta2_val));  % Maximum lower bound
        end
    end
    R = R_sw_ht_lo;
end


function s=AAFT(x,c)
%Syntax: s=AAFT(x,c)
%___________________
%
% Makes c Amplitude Adjusted Fourier Transformed (AAFT) surrogates of a time
% series x.
%
% s is the AAFT time series.
% x is the original time series.
% c is the number of surrogates.
%
%
% References:
%
% Theiler J, Galdrikian B, Longtin A, Eubank S, Farmer D J (1992): Using
% Surrogate Data to Detect Nonlinearity in Time Series. In Nonlinear Modeling
% and Forecasting, eds. Casdagli M & Eubank S. 163-188. Addison-Wesley
%
% Theiler J, Eubank S,Galdrikian B, Longtin A,  Farmer D J (1992): Testing
% for nonlinearity in time series: the method of surrogate data. Physica D
% 58: 77-94
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 12 Apr 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
end

if nargin<2 | isempty(c)==1
   c=1;
else
   % c must be scalar
   if sum(size(c))>2
      error('c must be scalar.');
   end
   % c must be greater or equal than 1
   if c<1
      error('c must be greater or equal than 1.');
   end
end

for i=1:c
    % Initialize
    y=x;
    % Make n normal random devaiates
    normal=sort(randn(size(y)));
    % Sort y and extract the ranks
    [y,T]=sort(y);
    [T,r]=sort(T);
    % Assign the ranks of y to the normal deviates and apply the phase
    %  randomization
    normal=phaseran(normal(r));
    % Extract the ranks of the phase randomized normal deviates
    [normal,T]=sort(normal);
    [T,r]=sort(T);
    % Assign the ranks of the phase randomized normal deviates to y and
    %  obtain the AAFT surrogates
    s(:,i)=y(r);
end


function [ xy, w ] = triangle_nco_rule ( rule, order_num )

%*****************************************************************************80
%
%% TRIANGLE_NCO_RULE returns the points and weights of an NCO rule.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Silvester,
%    Symmetric Quadrature Formulae for Simplexes,
%    Mathematics of Computation,
%    Volume 24, Number 109, January 1970, pages 95-100.
%
%  Parameters:
%
%    Input, integer RULE, the index of the rule.
%
%    Input, integer ORDER_NUM, the order (number of points) of the rule.
%
%    Output, real XY(2,ORDER_NUM), the points of the rule.
%
%    Output, real W(ORDER_NUM), the weights of the rule.
%

%
%  Get the suborder information.
%
  suborder_num = triangle_nco_suborder_num ( rule );

  suborder = triangle_nco_suborder ( rule, suborder_num );

  [ suborder_xyz, suborder_w ] = triangle_nco_subrule ( rule, suborder_num );
%
%  Expand the suborder information to a full order rule.
%
  o = 0;

  for s = 1 : suborder_num

    if ( suborder(s) == 1 )

      o = o + 1;
      xy(1:2,o) = suborder_xyz(1:2,s);
      w(o) = suborder_w(s);

    elseif ( suborder(s) == 3 )

      for k = 1 : 3
        o = o + 1;
        xy(1,o) = suborder_xyz ( i4_wrap(k,  1,3), s );
        xy(2,o) = suborder_xyz ( i4_wrap(k+1,1,3), s );
        w(o) = suborder_w(s);
      end

    elseif ( suborder(s) == 6 )

      for k = 1 : 3
        o = o + 1;
        xy(1,o) = suborder_xyz ( i4_wrap(k,  1,3), s );
        xy(2,o) = suborder_xyz ( i4_wrap(k+1,1,3), s );
        w(o) = suborder_w(s);
      end

      for k = 1 : 3
        o = o + 1;
        xy(1,o) = suborder_xyz ( i4_wrap(k+1,1,3), s );
        xy(2,o) = suborder_xyz ( i4_wrap(k,  1,3), s );
        w(o) = suborder_w(s);
      end

    else

      fprintf ( 1, '\n' );
      fprintf ( 1, 'TRIANGLE_NCO_RULE - Fatal error!\n' );
      fprintf ( 1, '  Illegal SUBORDER(%d) = %d\n', s, suborder(s) );
      error ( 'TRIANGLE_NCO_RULE - Fatal error!' );

    end

  end

  return
end


% @author: Maziar Raissi

function plot_surface_griddata(X_star, u_star, xlab, ylab, tit)

x_l = min(X_star(:,1));
x_r = max(X_star(:,1));

y_l = min(X_star(:,2));
y_r = max(X_star(:,2));

nn = 100;
x = linspace(x_l, x_r, nn)';
y = linspace(y_l, y_r, nn)';
[Xplot, Yplot] = meshgrid(x,y);

Zplot = griddata(X_star(:,1),X_star(:,2),u_star,Xplot,Yplot,'cubic');

surface(Xplot, Yplot, Zplot);
xlabel(xlab);
ylabel(ylab);
title(tit);

axis tight
colormap jet
shading interp
colorbar
set(gca,'FontSize',14);
set(gcf, 'Color', 'w');


function [V_RF, V_U, iter] = mo_algorithm(V_RF, Vn, H1)

global manifold;
[Nt, Nrf] = size(V_RF);
problem.M = manifold;

problem.cost = @(x)MMSE_cost(x,H1,Vn);
problem.egrad = @(x)MMSE_egrad(x,H1,Vn);

[x,iter] = conjugategradient(problem,V_RF(:));

V_RF = reshape(x,Nt,Nrf);
V_U = inv(V_RF'*H1 * H1'* V_RF+ 1 * Vn *(V_RF)'*V_RF)*V_RF'*H1;


function out = logdet(x)
a=chol(x);
out=sum(log(diag(a))*2);

end


syms x
b=limit((sqrt(1+x^2)-1)/(1-cos(x)))


function value = imDynamicRange(img, bRobust, type)
%
%
%        value = imDynamicRange(img, bRobust, type)
%
%
%        Input:
%           -img: the input image
%           -bRobust: if bRobust > 0 --> robust statistics for min and max
%                     luminance values. bRboust becomes the percentile!
%           -type: 'Classic', 'Michelson', and 'Weber'
%
%        Output:
%           -value(1): dynamic range of img
%           -value(2): dynamic range of img in f-stops (if 'Classic')
%           -value(3): dynamic range of img in log10 space space (if
%           'Classic')
%
%     Copyright (C) 2011-20  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

if(~exist('bRobust', 'var'))
    bRobust = 0;
end

if(~exist('type', 'var'))
    type = 'Classic';
end

if(bRobust >= 0.5)
   bRobust = 0.01;
end

L = lum(img);

if(bRobust > 0.0)
    minL = MaxQuart(L, bRobust);
    maxL = MaxQuart(L, 1 - bRobust);
else
    minL = min(L(:));
    maxL = max(L(:));
end

if(minL < 1e-6)
    warning('minL is less than 1e-6 cd/m^2');
end

if(minL <= 0.0)
    warning('minL is 0.0 is set to the first value greater than zero.');

    minL = min(min(L(L > 0)));
end

switch type

    case 'Classic'
        value(1) = maxL / minL;
        value(2) = log2(value(1));
        value(3) = log10(value(1));

    case 'Michelson'
        value = (maxL - minL) / (maxL + minL);

    case 'Weber'
        value = (maxL - minL) / minL;
end

end


% Test if the A B Matrices are valid

% Coefficients
mdlCoeff

A = [ 0 1 0 0; [-ks -bs ks bs]/mb ; ...
      0 0 0 1; [ks bs -ks-kt -bs]/mw];
B = [0; 10000/mb ; 0;  -10000/mw];

susp_sys_linear(rand(4,1),rand, A, B);

P_init = lyap(A',eye(4));
K1 = 1/r*B'*P_init;

[P,~,K] = care(A,B,eye(4),1)

% Optimal values
%
% P =
%
%     1.8639    0.0703   -1.5871    0.0123
%     0.0703    0.0638   -0.9956    0.0069
%    -1.5871   -0.9956   40.3489   -0.0763
%     0.0123    0.0069   -0.0763    0.0063
%
%
% K =
%
%     0.2868    0.9727  -20.4658   -0.8259


function [Mc, Mc95, Mc90] = calc_McBest(magnitudes, binInterval)
    % CALC_MCBEST calculate best Magnitude of Completion
    % [fMc, fMc95, fMc90] = calc_McBest(magnitudes, binInterval)
    % each row of magitudes gets its own calculation

    % report_this_filefun();

    % Magnitude increment
    if ~exist('binInterval','var')
        binInterval = 0.1;
    end
    magnitudes = sort(magnitudes);

    half_bin   = binInterval / 2;
    % First estimation of magnitude of completeness (maximum curvature)
    McStarts = maxCurvature(magnitudes, [-2 : binInterval : 6] , half_bin); % vectorized

    % cheat to allow this to handle multiple rows of magnitudes (independently)
    for jj = 1:size(magnitudes,2)
        valid = ~isnan(magnitudes(:,jj));
        [Mc(jj), Mc95(jj), Mc90(jj)] = calc_McBest_unvectorized(McStarts(jj), magnitudes(valid,jj), binInterval, half_bin);
    end
end

function  [fMc, fMc95, fMc90] = calc_McBest_unvectorized(McStart, magnitudes, binInterval, half_fBin)
    magCenters  = (McStart - 0.9) : binInterval : (McStart + 1.5);
    eachedge    = [magCenters - half_fBin , magCenters(end) + half_fBin];

    magnitudes = flipud(magnitudes); % from biggest to smallest
    nGtEdge = sum(magnitudes > eachedge((end-1) : -1 : 1)); % magnitudes(nGtEdge) gives last event above threshhold
    nGtEdge = fliplr(nGtEdge);
    too_few = nGtEdge < 25;

    results=nan(numel(magCenters),1);
    for idx = 1:numel(magCenters)
        if too_few(idx)
            continue
        end
        hypotheticalMc  = magCenters(idx);
        results(idx) = doCalculation(magnitudes(1:nGtEdge(idx)), binInterval, hypotheticalMc);
    end

    % Evaluation of results

    % Is fMc90 available
    nSel = find(results < 10, 1, 'first' );
    if isempty(nSel)
        fMc90 = NaN;
    else
        fMc90 = magCenters(nSel);
    end

    % Is fMc95 available
    nSel = find(results < 5, 1 );
    if isempty(nSel)
        fMc95 = NaN;
    else
        fMc95 = magCenters(nSel);
    end

    % take results from bins. (I tested against discretize, and this was faster -CGR)

    j =  find(results < 10 , 1, 'first');
    if isempty(j)
        j =  find(results < 15 , 1, 'first' );
    end
    if isempty(j)
        j =  find(results < 20 , 1, 'first' );
    end
    if isempty(j)
        j =  find(results < 25 , 1, 'first' );
    end
    fMc = magCenters(j);
    if isempty(fMc)
        fMc = NaN;
    end
end

function [result] = doCalculation(theseMags, binInterval, hypotheticalMc)
        fBValue   = calc_bmemag(theseMags, binInterval);
        half_fBin = binInterval ./ 2;

        % log10(N)=A-B*M
        vMag    = hypotheticalMc:binInterval:15; % Ending magnitude must be sufficiently high (???what is "sufficently high")
        vNumber = 10.^(log10(numel(theseMags)) - fBValue*(vMag - hypotheticalMc));
        vNumber = round(vNumber);

        % PM=vMag(1:ct);
        PMedges     = [vMag-half_fBin , vMag(end)+half_fBin];
        [bval, ~]   = histcounts(theseMags, PMedges);
        b3          = cumsum(bval,'reverse');    % N for M >= (counted backwards)
        result      = sum(abs(b3 - vNumber)) / sum(b3)*100; %res2
end

function Mc = maxCurvature(m, centers, halfBinwidth)
    % MAXCURVATURE First estimation of magnitude of completeness (maximum curvature)
    % Mc = MAXCURVATURE(m, min_max, binwidth)
    %
    % vectorized
    edges           = [centers - halfBinwidth , centers(end) + halfBinwidth];
    [vEvents, ~]    = histc(m, edges, 1);
    [~,idx] = max(flipud(vEvents));
    nSel = size(vEvents,1) - idx + 1;
    % nSel    = find(vEvents == max(vEvents), 1, 'last' );
    Mc      = centers(nSel);
end


function ye = fL(y, D, hom)
% Neumann condition at outflow
z =  2^(-2.5)*(y*y*0.25/D - 1)* exp(-y*y/(8*D));
z = z + 2^(-2.5)*((2-y)*(2-y)*0.25/D - 1)*exp(-(2-y)*(2-y)/(8*D));
ye = hom*z;


% test code for tt_qlaplacex_dd()
%
% September 22, 2010
% Vladimir Kazeev
% vladimir.kazeev@gmail.com
% INM RAS
% Moscow, Russia
%
% Look for details in the Preprint No. 75, 2010 of
% Max-Planck Institute for Mathematics in the Sciences
% Vladimir A. Kazeev and Boris N. Khoromskij
% On explicit QTT representation of Laplace operator and its inverse
% http://www.mis.mpg.de/publications/preprints/2010/prepr2010-75.html

% d is the only parameter
d=[3,4,5];
%

D=size(d,2);
tt=tt_qlaplacex_dd(d);
tt=tt_mat_to_vec(tt);

full=nd_to_full(tt);

disp('inv computation... ');
Li=cell(D,1);
for k=1 : D
	L=2*eye(2^d(k));
	for i=1 : 2^d(k)-1
		L(i,i+1)=-1;
		L(i+1,i)=-1;
	end
	Li{k}=inv(L);
end
disp('OK');

disp('kron computation... ');
Z=Li{1};
for k=2 : D
	Z=kron(Z,Li{k});
end
disp('OK');

err=norm(full-Z,'fro');
fprintf('fro err = %e\n', err);


function [pcaDims, lambda] = GridSearch(trainX, trainY, testX, testY)
%% [pcaDims, lambda] = GridSearch(trainX, trainY, testX, testY)
%
% This function demostrates how to use the development set to learn the
% optimal parameters.
%
% Inputs:
%   trainX, trainY: training data and class labels.
%   testX, testY: test data and class labels.
% Outputs:
%   pcaDims: the optimal PCA dimensions.
%   lambda: the optimal ridge regression parameter.
%
% Version: 1.0
% Date: 2014-07-22
%
% Author: Shengcai Liao
% Institute: National Laboratory of Pattern Recognition,
%   Institute of Automation, Chinese Academy of Sciences
% Email: scliao@nlpr.ia.ac.cn

candiPcaDims = 100:100:500; % candidate parameter values for the pca dimensions.
candiLambda = 10.^(-4:0); % candidate parameter values for the ridge regression.
veriFarPoints = [0, kron(10.^(-8:-1), 1:9), 1]; % FAR points for verification performance evaluation

fprintf('Learn parameters on the development set...\n');

%% Learn a PCA subspace.
W = PCA(trainX);

%% Transform both the training and test data into the learned PCA subspace.
trainX = trainX * W;
testX = testX * W;

%% Select classes which have at least two images.
hst = hist(trainY, 1 : max(trainY));
classIndex = find(hst >= 2);
[sampleIndex, trainY] = ismember(trainY, classIndex); % class labels are continuously relabeled starting from 1
trainY = trainY(sampleIndex);
trainX = trainX(sampleIndex,:);
numTrainSamples = length(trainY);

%% Construct the discriminant response matrix
Y = zeros(numTrainSamples, length(classIndex));
Y( sub2ind(size(Y), 1 : numTrainSamples, trainY') ) = 1;

%% Pre-compute the working variables for the ridge regression.
R = trainX' * trainX;
Z = trainX' * Y;

numPara1 = length(candiPcaDims);
numPara2 = length(candiLambda);
auc = zeros(numPara1, numPara2);

%% Learn the optimal parameters by grid search
for i = 1 : numPara1
    d = candiPcaDims(i);

    for j = 1 : numPara2
        % Learn a linear subspace by ridge regression. You can replace this with
        % your own supervised learning algorithm here.
        W = (R(1:d, 1:d) + candiLambda(j) * numTrainSamples * eye(d)) \ Z(1:d,:);

        % Transform the test data into the learned subspace.
        X = testX(:, 1:d) * W;

        % Normlize each row to unit length. If you do not have this function,
        % do it manually.
        X = normr(X);

        % Compute the cosine similarity score between the test samples.
        score = X * X';

        % Evaluate the verification performance.
        VR = EvalROC(score, testY, [], veriFarPoints);

        % Average the verification rates as the AUC performance
        auc(i,j) = mean(VR);
    end
end

%% Plot the AUC performance w.r.t. different PCA dimensions.
figure; semilogx(candiPcaDims, auc, 'LineWidth', 2);
grid on;
xlabel('PCA Dimension');
ylabel('AUC');
title('AUC performance w.r.t. different PCA dimensions');
labels = cellstr( [repmat('lambda = ', [length(candiLambda),1]), num2str(candiLambda')] );
legend(labels, 'Location', 'NorthWest');
set(gca, 'XTick', candiPcaDims);
set(gca, 'XTickLabel', cellstr(num2str(candiPcaDims')));
drawnow;

%% Get the best parameters.
[bestAuc, index] = max(auc(:));
[r,c] = ind2sub(size(auc), index);
pcaDims = candiPcaDims(r);
lambda = candiLambda(c);
fprintf('The best AUC: %g. The best PCA dimensions: %d. The best lambda: %g.\n\n', bestAuc, pcaDims, lambda);


function varargout = normalization(PopObj)

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

global normalization_str
if strcmp(normalization_str, 'normalize')
    [N, ~] = size(PopObj);
    a = intercepts(PopObj);
    PopObj = (PopObj - repmat(min(PopObj, [], 1), size(PopObj, 1), 1)) ./ repmat(a,N,1);
end
varargout{1} = PopObj;
if nargout == 2
    varargout{2} = a;
end
end

function a = intercepts(PopObj)
[N, M] = size(PopObj);
%% Find the extreme points
[~, Choosed(1:M)] = min(PopObj, [], 1);
L2NormABO = zeros(N, M);
for i = 1 : M
    L2NormABO(:, i) = sum(PopObj(:, [1: i - 1, i + 1: M]) .^ 2, 2);
end
[~, Choosed(M + 1: 2 * M)] = min(L2NormABO, [], 1);
[~, Extreme] = max(PopObj(Choosed, :), [], 1);
Extreme = unique(Choosed(Extreme));
%% Calculate the intercepts
if length(Extreme) < M
    a = max(PopObj, [], 1);
else
    lastwarn('');
    Hyperplane = mldivide(PopObj(Extreme,:), ones(M, 1));
    [~, msgid] = lastwarn();
    if strcmp(msgid, 'MATLAB:nearlySingularMatrix')
        % error('error in normalize');
    end
    a = 1 ./ Hyperplane';
end
end


%% 3D Orientation Visualizations
%

%% Euler angle space
%
% By default the function <orientation.plot.html plot>
% plots orientations in the three dimensional Bunge Euler angle space

cs = crystalSymmetry('cubic')

ori = orientation.rand(100,cs);
plot(ori)

%%
% Note that the orientations are automatically projected into the
% fundamental region. In the case of cubic symmetry this means that the
% Euler angles $\Phi$ and $\phi_2$ are restricted to 90 degrees. If the
% orientations should be plotted at their specified Euler angles the option
% |'ignoreFundamentalRegion'| has to be used.

plot(ori,'ignoreFundamentalRegion')

%% Axis angle space
%
% Alternatively, orientations can be plotted in the three dimensional axis
% angle space.

plot(ori,'AxisAngle','markerEdgeColor',[0 0 0.8],'markerSize',8)

%%
% The orientations are automatically projected into its fundamental region.
% Again, this can be switched off with the option
% |'ignoreFundamentalRegion'|.

plot(ori,'axisAngle','ignoreFundamentalRegion','markerEdgeColor',[0 0 0.8],'markerSize',8)

% visualize the fundamental region
hold on
oR = fundamentalRegion(ori.CS,ori.SS)
plot(oR,'color',[1 0.5 0.5]),
hold off


function varargout = drawEllipse(varargin)
%DRAWELLIPSE Draw an ellipse on the current axis.
%
%   drawEllipse(ELLI);
%   Draws the ellipse ELLI in the form [XC YC RA RB THETA], with center
%   (XC, YC), with main axis of half-length RA and RB, and orientation
%   THETA in degrees counted counter-clockwise.
%
%   drawEllipse(XC, YC, RA, RB);
%   drawEllipse(XC, YC, RA, RB, THETA);
%   Specifies ellipse parameters as separate arguments (old syntax).
%
%   drawEllipse(..., NAME, VALUE);
%   Specifies drawing style of ellipse, see the help of plot function.
%
%   H = drawEllipse(...);
%   Also returns handles to the created line objects.
%
%   -> Parameters can also be arrays. In this case, all arrays are supposed
%   to have the same size.
%
%   Example:
%   % Draw an ellipse centered in [50 50], with semi major axis length of
%   % 40, semi minor axis length of 20, and rotated by 30 degrees.
%     figure(1); clf; hold on;
%     drawEllipse([50 50 40 20 30]);
%     axis equal; axis([0 100 10 90])
%
%   % add another ellipse with different orientation and style
%     drawEllipse([50 50 40 20 -10], 'LineWidth', 2, 'Color', 'g');
%
%
%   See also
%     ellipses2d, drawCircle, drawEllipseArc, drawEllipseAxes
%     fitEllipse, ellipseToPolygon, ellipsePoint, transformEllipse
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-12-11
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

%% Extract input arguments

% extract handle of axis to draw on
if isAxisHandle(varargin{1})
    ax = varargin{1};
    varargin(1) = [];
else
    ax = gca;
end

% extract dawing style strings
styles = {};
for i = 1:length(varargin)
    if ischar(varargin{i})
        styles = varargin(i:end);
        varargin(i:end) = [];
        break;
    end
end

% extract ellipse parameters
if length(varargin) == 1
    % ellipse is given in a single array
    ellipse = varargin{1};
    x0 = ellipse(:, 1);
    y0 = ellipse(:, 2);
    a  = ellipse(:, 3);
    b  = ellipse(:, 4);
    if length(ellipse) > 4
        theta = ellipse(:, 5);
    else
        theta = zeros(size(x0));
    end

elseif length(varargin) >= 4
    % ellipse parameters given as separate arrays
    x0 = varargin{1};
    y0 = varargin{2};
    a  = varargin{3};
    b  = varargin{4};
    if length(varargin) > 4
        theta = varargin{5};
    else
        theta = zeros(size(x0));
    end

else
    error('drawEllipse: incorrect input arguments');
end

%% Process drawing of a set of ellipses

% angular positions of vertices
t = linspace(0, 2*pi, 145);

% compute position of points to draw each ellipse
h = zeros(length(x0), 1);
for i = 1:length(x0)
    % pre-compute rotation angles (given in degrees)
    cot = cosd(theta(i));
    sit = sind(theta(i));

    % compute position of points used to draw current ellipse
    xt = x0(i) + a(i) * cos(t) * cot - b(i) * sin(t) * sit;
    yt = y0(i) + a(i) * cos(t) * sit + b(i) * sin(t) * cot;

    % stores handle to graphic object
    h(i) = plot(ax, xt, yt, styles{:});
end

% return handles if required
if nargout > 0
    varargout = {h};
end


function [out] = evap_18(p1,p2,p3,S,Ep)
%evap_18

% Copyright (C) 2019, 2021 Wouter J.M. Knoben, Luca Trotter
% This file is part of the Modular Assessment of Rainfall-Runoff Models
% Toolbox (MARRMoT).
% MARRMoT is a free software (GNU GPL v3) and distributed WITHOUT ANY
% WARRANTY. See <https://www.gnu.org/licenses/> for details.

% Flux function
% ------------------
% Description:  Exponentially declining evaporation from deficit store
% Constraints:  -
% @(Inputs):    p1   - linear scaling parameter [-]
%               p2   - linear scaling parameter [-]
%               p3   - storage scaling parameter [mm]
%               S    - current storage [mm]
%               Ep   - potential evapotranspiration rate [mm/d]

out = p1.*exp(-1.*p2.*S./p3).*Ep;

end


function A = nnls_spatial(Y, A, C, active_pixel, maxN)
%% run HALS by fixating all spatial components
% input:
%   Y:  d*T, fluorescence data
%   A:  d*K, spatial components
%   C:  K*T, temporal components
%   active_pixel: d*T binary matrix, indicating the nonzero elements of A
%   maxN: scalar, maximum number of neurons overlapping at one pixel
% output:
%   A: d*K, updated spatial components

% Author: Pengcheng Zhou, Carnegie Mellon University, adapted from Johannes

%% options for HALS
d = size(Y, 1);
K = size(C, 1);
if nargin<5;    maxN = 5;    end;    %maximum iteration number
if nargin<4;    active_pixel=true(d, K);
elseif isempty(active_pixel)
    active_pixel = true(d, K);
else
    active_pixel = logical(active_pixel);
end;     %determine nonzero pixels

%% initialization
Ymean = mean(Y,2);
Y = bsxfun(@minus, Y, Ymean);
C = bsxfun(@minus, C, mean(C,2));
CC = C*C';
YC = C*Y';
ind_fit = find(sum(active_pixel,2)>1e-9);
A = zeros(size(A));

%% updating
for m=1:length(ind_fit)
    ind = active_pixel(ind_fit(m), :);
    A(ind_fit(m), ind) = nnls(CC(ind, ind), YC(ind, ind_fit(m)), [], 1e-4, maxN);
end

function s = nnls(A, b, s, tol, maxIter)
%% fast algorithm for solving nonnegativity constrained least squared
% problem minize norm(y-K*s, 2), s.t. s>=0.

%% inputs:
%   A: n x p matrix, K'*K
%   b: n x 1 vector, K'*y
%   s: p x 1 vector, warm started s
%   tol: scalar, smallest nonzero values
%   maxIter: scalar, maximum nonzero values

%% outputs:
%   s: p x 1 vector, solution

%% Authors: Pengcheng Zhou, Carnegie Mellon University, 2016
% ported from the Python implementation from Johannes Friedrich

%% References
% Friedrich J et.al., NIPS 2016, Fast Active Set Method for Online Spike Inference from Calcium Imaging
% Bro R & Jong S, Journal of Chemometrics 1997, A FAST NON-NEGATIVITY-CONSTRAINED LEAST SQUARES ALGORITHM

%% input arguments
p = size(A,2);      % dimension of s
if ~exist('s', 'var') || isempty(s)
    s = zeros(p, 1);
end
if ~exist('tol', 'var') || isempty(tol)
    tol = 1e-9;
end
if ~exist('maxIter', 'var') || isempty(maxIter)
    maxIter = p;
end
if sum(s>0)>maxIter
    s = zeros(p,1);
end
for miter=1:maxIter
    l = b - A*s;            % negative gradient
    Pset = (s>0);       % passive set

    if max(l) < tol         % no more passive set
        break;
    end

    [~, temp] = max(l);         % choose the one with the largest gradient
    Pset(temp) = true;         % add it to the passive set
    if sum(Pset)>maxIter
        break;
    end
    % correct nonnegativity violations
    while any(Pset)
        % run unconstrained least squares for variables in passive sets
        try
            mu = A(Pset, Pset) \ b(Pset);
        catch
            mu = (A(Pset, Pset) + tol*eye(sum(Pset))) \ b(Pset);
        end

        if all(mu>tol)
            break;
        end

        temp = s(Pset) ./ (s(Pset)-mu);
        temp(mu>tol) = [];
        a = min(temp);
        s(Pset) = s(Pset) + a*(mu-s(Pset));
        Pset(s<tol) = false;
    end

    s(Pset) = mu;
end


% Fig. 9.17   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%script for Figure 5.59
sysG=tf([1],[1 0.2 1 0]);
sysH=tf(123*[1 .18 .81],[1 20 100]);
sysL=sysG*sysH;
rlocus(sysL)
title('Figure 9.17  Root locus for the system of Figure 9.18')
axis([-3 1 -1.5 1.5])
hold on
sysCL=feedback(sysG,sysH);
r=eig(sysCL);
plot(r,'*')
z=0:.1:.9;
wn= .5:.5:3;
sgrid(z, wn)
hold off


%ex2 simulates the example of Section 3.4.3
%	_______________________________________________________________________
%  /Program:  ex2.m								\
% / Description:  This program runs the simulation of the Rhors example     \
%|  	found in sections 3.4.3 of the text.  The program allows the user	 |
%|  	to choose between the three algorithms defined in the text and 		 |
%|	between the stable or unstable examples.  The program prompts the    |
%|	prompts the user to enter values for the variables that are adjusted |
%|	within the text.  This program calls and requires the fex2.m file.	 |
% \
%  \_______________________________________________________________________/

%Copyright

% Programmed Summer 1996 by Kenyon Thayer, RPI, Troy, NY

global am bm cm ap bp cp Af Bf Cf Df T Tbar G Qp Qf algor a b u

clear yp ym kpe kpf
temp=exist('Af');
if temp==1
	Af_old=Af;
	Bf_old=Bf;
	Cf_old=Cf;
	Df_old=Df;
	G_old=G;
	Qp_old=Qp;
	Qf_old=Qf;
else
	Af_old=0;
	Bf_old=-10;
	Cf_old=1;
	Df_old=0;
	G_old=-5.714;
	Qp_old=57.14;
	Qf_old=20;
end;

am=-3;bm=1;cm=3;

disp('   **This program simulates the text book example 3.4.3**');

algor=input('Which algorithm do you wish to model? 1,2,3: ');
ch2=input('Is this the stable example? Y/N: ','s');

if ch2=='n' | ch2=='N'
	%example 2 (unstable)
	ap=[-7 -92 100;1 0 0;0 1 0];bp=[1;0;0];cp=[0 0 200];
else
	%example 1 (rohrs)
	ap=[-31 -259 -229;1 0 0;0 1 0];bp=[1;0;0];cp=[0 0 458];
end;
% Initial Conditions
% ************************************************************
xp0=[0 0 0];xm0=0;yf0=0;ki0=[0 0 0 0];x0=[xp0,xm0,yf0,ki0]';
t0=0;tfinal=20;tol=1.e-6;
Af=input(sprintf('Enter a value for Af (default is %5.3f):  ',...
	Af_old));
if isempty(Af)
	Af=Af_old;
end
Bf=input(sprintf('Enter a value for Bf (default is %5.3f):  ',...
	Bf_old));
if isempty(Bf)
	Bf=Bf_old;
end
Cf=input(sprintf('Enter a value for Cf (default is %5.3f):  ',...
	Cf_old));
if isempty(Cf)
	Cf=Cf_old;
end
Df=input(sprintf('Enter a value for Df (default is %5.3f):  ',...
	Df_old));
if isempty(Df)
	Df=Df_old;
end
G=input(sprintf('Enter a value for G (default is %5.3f):  ',...
	G_old));
if isempty(G)
	G=G_old;
end
Qp=input(sprintf('Enter a value for Qp (default is %5.3f):  ',...
	Qp_old));
if isempty(Qp)
	Qp=Qp_old;
end
Qf=input(sprintf('Enter a value for Qf (default is %5.3f):  ',...
	Qf_old));
if isempty(Qf)
	Qf=Qf_old;
end

T=1;Tbar=1;

[tout,xout]=ode45('fex2',t0,tfinal,x0,tol,0);

for i=1:length(tout)
	yp(i)=cp*xout(i,[1:3])';
	ym(i)=cm*xout(i,4) ;
	yf=Cf*xout(i,5);
	eyp(i)=ym(i)-yp(i);
	xm=xout(i,4);
	um=0.3;
	if tout(i)>10
		um=-0.3;
	end;
	[ng,ng]=size(G);
	rvec=[eyp(i);-yf;xm;um];
	v1=Qp*eyp(i)-Qf*yf;
	up=inv(eye(ng)-G*rvec'*Tbar*rvec)*(xout(i,[6:9])+v1*rvec'*Tbar)*rvec;
	v=v1+G*up;
	kp=v*rvec'*Tbar;
	kpe(i)=kp(1,1);
	kpf(i)=kp(1,2);
end;

z=max(length(tout),length(ym(1,:)));
if z>length(tout)
	for i=length(tout):length(ym(1,:))
		tout(i,:)=tout((length(tout)),:);
	end;
end;
figure(1)
plot(tout,yp,tout,ym,'--')
axis([0 20 -.5 .5])
text(.41, 1.05, 'Ym', 'color', [1 0 1], 'FontSize', 12, 'Units', 'normal');
text(.51, 1.05, 'Yp', 'color', [1 1 0], 'FontSize', 12, 'Units', 'normal');


% Test file read
% File:  c:\M-files\shortcut_updates\readtxt.m
% 2/11/07
%
clc;clear;
[freq,mag,ph]=textread('c:\Spiceapps\datfiles\Elliptf7.txt','%f %f %f');
%
h=plot(log10(freq),20*log10(mag),'k');
set(h,'LineWidth',2);
hold on
% Plot -210 dB asymptote
h=plot(log10(freq),-210*log10(freq/1000),'k--');
hold off
axis([2 4 -80 40]);
set(h,'LineWidth',2);
grid on
xlabel('Log Freq(Hz)');
ylabel('dBV');
title('7th Order Elliptical LPF');
legend('Output','-210 dB Asymptote',0);
figure
%
h=plot(log10(freq),ph,'k');
grid on
xlabel('Log Freq(Hz)');
ylabel('Deg');
title('Elliptical LPF Phase');
axis([2 4 -200 200]);
set(h,'LineWidth',2);


%sculling (essentially sculling+rotation) correction for each minor
%interval
%gyro & acc must be increment type (3,length) outputs. alg is the desired
%sculling algorithm
function [vinc, ainc, corr]=sculling_minor(gyro, acc, alg)
inlen=size(acc,2);
switch (alg)
    case (0)
        %ignagni(1998):algorithm 1 -> (1996):algo2
        outlen=floor((inlen-3)/2)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=3:2:inlen
            vinc(:,ind)=sum(acc(:,i-1:i),2);
            ainc(:,ind)=sum(gyro(:,i-1:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((-1/30)*gyro(:,i-2)+(11/15)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((-1/30)*acc(:,i-2)+(11/15)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case (1)
        %ignagni(1998):algorithm 2 -> (1996):algo3
        outlen=floor((inlen-3)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=3:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((9/20)*gyro(:,i-2)+(27/20)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((9/20)*acc(:,i-2)+(27/20)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(2)
        %ignagni(1998):algorithm 3 -> (1996):algo5
        outlen=floor((inlen-4)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=4:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((3/280)*gyro(:,i-3)+(57/140)*gyro(:,i-2)+(393/280)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((3/280)*acc(:,i-3)+(57/140)*acc(:,i-2)+(393/280)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(3)
        %ignagni(1998):algorithm 4 -> (1996):algo6
        outlen=floor((inlen-5)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=5:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((-1/420)*gyro(:,i-4)+(1/40)*gyro(:,i-3)+(157/420)*gyro(:,i-2)+(1207/840)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((-1/420)*acc(:,i-4)+(1/40)*acc(:,i-3)+(157/420)*acc(:,i-2)+(1207/840)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(4)
        %ignagni(1998):algorithm 5 -> (1996):algo7
        outlen=floor((inlen-4)/4)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=4:4:inlen
            vinc(:,ind)=sum(acc(:,i-3:i),2);
            ainc(:,ind)=sum(gyro(:,i-3:i),2);

            rot=0.5*cross(ainc(:,ind), vinc(:,ind));
            vr_a=cross(((54/105)*gyro(:,i-3)+(92/105)*gyro(:,i-2)+(214/105)*gyro(:,i-1)), acc(:,i));
            vr_b=cross(((54/105)*acc(:,i-3)+(92/105)*acc(:,i-2)+(214/105)*acc(:,i-1)), gyro(:,i));
            corr(:,ind)=rot+vr_a+vr_b;
            ind=ind+1;
        end
    case(5) %%quadratic fit to both gyro and accel outputs (similar to the coning algorithm in ignagni(1990):algoD
        outlen=floor((inlen-3)/3)+1;
        vinc=zeros(3,outlen);
        ainc=zeros(3,outlen);
        corr=zeros(3,outlen);

        ind=1;
        for i=3:3:inlen
            vinc(:,ind)=sum(acc(:,i-2:i),2);
            ainc(:,ind)=sum(gyro(:,i-2:i),2);

            %compute the polynomial coefficients
            b=(11/6)*gyro(:,i-2)+(-7/6)*gyro(:,i-1)+(1/3)*gyro(:,i);
            c=(-2)*gyro(:,i-2)+(3)*gyro(:,i-1)+(-1)*gyro(:,i);
            d=(1/2)*gyro(:,i-2)+(-1)*gyro(:,i-1)+(1/2)*gyro(:,i);

            e=(11/6)*acc(:,i-2)+(-7/6)*acc(:,i-1)+(1/3)*acc(:,i);
            f=(-2)*acc(:,i-2)+(3)*acc(:,i-1)+(-1)*acc(:,i);
            g=(1/2)*acc(:,i-2)+(-1)*acc(:,i-1)+(1/2)*acc(:,i);

            %Compute sculling(+rotation)
            corr(:,ind)=(3^2/2)*cross(b,e)+(3^3/3)*(cross(b,f)+cross(c,e)/2)+(3^4/4)*(cross(b,g)+cross(c,f)/2+cross(d,e)/3)+...
                (3^5/5)*(cross(c,g)/2+cross(d,f)/3)+(3^6/6)*cross(d,g)/3;

            ind=ind+1;
        end

    otherwise
        disp('undefined algorithm');
end


function res=SODW(x,a,b,w);
% function res=SODW(x,a,b,w);
%
% Computes and sums over all outer products of the columns in x and
% weights them according to w.
%
% equivalent to:
%
% res=zeros(size(x,1));
% for i=1:n
%   res=res+w(i).*x(:,a(i))*x(:,b(i))';
% end;
%
%
% copyright 2005 by Kilian Q. Weinberger
% University of Pennsylvania
% kilianw@seas.upenn.edu
% ********************************************

if(min(w)<0) error('Weights must be non-negative in matlab version!\nPlease call "mex SODW.cpp" in the mexfunctions directory.\n');end;
[D,N]=size(x);
B=round(2500/D^2*1000000);
res=zeros(D^2,1);
sw=sqrt(w);
for i=1:B:length(a)
  BB=min(B,length(a)-i);
  Xa=mulh(x(:,a(i:i+BB)),sw(i:i+BB));
  Xb=mulh(x(:,b(i:i+BB)),sw(i:i+BB));
  XaXb=Xa*Xb';
  res=res+vec(Xa*Xa'+Xb*Xb'-XaXb-XaXb');

  if(i>1)   fprintf('.');end;
end;


function indx = r8vec2_sort_heap_index_a ( n, x, y )

%*****************************************************************************80
%
%% R8VEC2_SORT_HEAP_INDEX_A does an indexed heap ascending sort of an R8VEC2.
%
%  Discussion:
%
%    An R8VEC2 is two R8VEC's.
%
%    An R8VEC is a vector of R8 values.
%
%    The sorting is not actually carried out.  Rather an index array is
%    created which defines the sorting.  This array may be used to sort
%    or index the array, or to sort or index related arrays keyed on the
%    original array.
%
%    ( X(I), Y(I) ) < ( X(J), Y(J) ) if:
%
%    * X(I) < X(J), or
%
%    * X(I) = X(J), and Y(I) < Y(J).
%
%    Once the index array is computed, the sorting can be carried out
%    "implicitly:
%
%      ( X(INDX(1:N)), Y(INDX(1:N) ), is sorted,
%
%    or explicitly, by the call
%
%      call dvec_permute ( n, x, indx )
%      call dvec_permute ( n, y, indx )
%
%    after which ( X(1:N), Y(1:N) ), is sorted.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the array.
%
%    Input, real X(N),Y(N), pairs of X, Y coordinates of points.
%
%    Output, integer INDX(N), the sort index.  The
%    I-th element of the sorted array has coordinates ( X(INDX(I)), Y(INDX(I) ).
%
  if ( n < 1 )
    indx = [];
    return
  end

  indx(1:n) = 1 : n;

  if ( n == 1 )
    return
  end

  l = floor ( n / 2 ) + 1;
  ir = n;

  while ( 1 )

    if ( 1 < l )

      l = l - 1;
      indxt = indx(l);
      xval = x(indxt);
      yval = y(indxt);

    else

      indxt = indx(ir);
      xval = x(indxt);
      yval = y(indxt);
      indx(ir) = indx(1);
      ir = ir - 1;

      if ( ir == 1 )
        indx(1) = indxt;
        break
      end

    end

    i = l;
    j = l + l;

    while ( j <= ir )

      if ( j < ir )

        if ( x(indx(j)) < x(indx(j+1)) || ...
          ( x(indx(j)) == x(indx(j+1)) && y(indx(j)) < y(indx(j+1)) ) )
          j = j + 1;
        end

      end

      if ( xval < x(indx(j)) || ...
          ( xval == x(indx(j)) && yval < y(indx(j)) ) )
        indx(i) = indx(j);
        i = j;
        j = j + j;
      else
        j = ir + 1;
      end

    end

    indx(i) = indxt;

  end

  return
end


% SCRIPT TEST FOR THE KINEMATIC PROBLEM FOR SERIAL ROBOTS

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

close all

fprintf('\nTHE DEMO PRESENTS THE DIRECT AND INVERSE KINEMATIC PROBLEM')

%there are eight possible solutions for the inverse kinematic problem for most of these robots
n_solutions = 8;

%Try different configurations beware that, depending on the robot's topology
%not all the eight possible solutions will be feasible for an antropomorphic 6R robot.
%q=[0.5 -0.4 -0.2 0.1 0.1 0.1]

q=[0.2 -0.2 0.3 0.1 0 0.1]

%load robot parameters. You can try different robots%
robot=load_robot('ABB', 'IRB140'); n_solutions = 8;
%robot=load_robot('KUKA', 'KR60_3'); n_solutions = 8;
%robot=load_robot('ABB', 'IRB120'); n_solutions = 8;
%robot=load_robot('MOTOMAN', 'MH12'); n_solutions = 8;

%adjust 3D view as desired
adjust_view(robot)

%there are just 2 solutions for these robots and 4 DOF
%q = [pi/2 0.2 0.8 pi/4]
%robot=load_robot('kuka', 'KR5_scara_R350_Z200'); n_solutions = 2;
%robot=load_robot('example', 'scara'); n_solutions = 2;
%robot=load_robot('example', '2dofplanar'); n_solutions = 2;

%draw the robot
drawrobot3d(robot, q)

%Now compute direct kinematics for this position q
T = directkinematic(robot, q)

%Set to zero if you want to see the robot transparent
robot.graphical.draw_transparent=0;

%Set to one if you want to see the DH axes
%robot.graphical.draw_axes=1;

%Call the inversekinematic for this robot. All the possible solutions are
%stored at qinv. At least, one of the possible solutions should match q
qinv = inversekinematic(robot, T)

fprintf('\nNOW WE CAN REPRESENT THE DIFFERENT SOLUTIONS TO ACHIEVE THE SAME POSITION AND ORIENTATION\n')
fprintf('\nNote that some solutions may not be feasible since some joints may be out of range.\n')
correct=zeros(1,n_solutions);
%check that all of them are possible solutions!
for i=1:size(qinv,2),

    Ti = directkinematic(robot, qinv(:,i)) %Ti is constant for the different solutions

    % Note that all the solutions may not be feasible. Some of the joints may
    % be out of range. You can test this situation with test_joints
    test_joints(robot, qinv(:,i));

    %now draw the robot to see the solution
    drawrobot3d(robot, qinv(:,i))

    pause(1);

    k=sum(sum((T-Ti).^2));
    if k < 0.01 % a simple threshold to find differences in the solution
        correct(1,i)= 1;
    else
        correct(1,i)= 0; %uncorrect solution
        fprintf('\nERROR: One of the solutions seems to be uncorrect. Sum of errors: %f', i, k);
    end
end

fprintf('\n************** RESULTS **************')

%Display a message if any of the solutions is not correct
if sum(correct)==n_solutions
    fprintf('\nTEST 1--> OK: Every solution in qinv yields the same position/orientation T');
else
    fprintf('\nTEST 1--> ERROR: One or more of the solutions seem to be uncorrect.');
end

%Now, test if any of the solutions in qinv matches q
%find the solution that matches the initial q
%delta is just a squared sum of errors at each of the columns of the matrix
%which store the different solutions of qinv
delta=(repmat(q',[1 n_solutions])-qinv).^2;
i=find(sum(delta,1) < 0.01);
if ~isempty(i)
    fprintf('\nTEST 2--> OK!: Found a matching solution for the initial q.\n');
    solution=qinv(:,i)
else
    error_test2=1
    fprintf('\nTEST 2--> ERROR: Did not find a matching solution for the initial q.');
end

fprintf('\n************** ****** **************\n')


function normals = vertexNormal(vertices, faces)
%VERTEXNORMAL Compute normals to a mesh vertices.
%
%   N = vertexNormal(V, F)
%   Computes vertex normals of the mesh given by vertices V and F.
%   V is a vertex array with 3 columns, F is either a NF-by-3 or NF-by-4
%   index array, or a cell array with NF elements.
%
%   Example
%     % Draw the vertex normals of a sphere
%     s = [10 20 30 40];
%     [v f] = sphereMesh(s);
%     drawMesh(v, f);
%     view(3);axis equal; light; lighting gouraud;
%     normals = vertexNormal(v, f);
%     drawVector3d(v, normals);
%
%   See also
%     meshes3d, meshFaceNormals, triangulateFaces

% ------
% Author: David Legland
% E-mail: david.legland@grignon.inra.fr
% Created: 2011-12-19, using Matlab 7.9.0.529 (R2009b)
% Copyright 2011-2022 INRA - Cepia Software Platform

nv = size(vertices, 1);
nf = size(faces, 1);

% unit normals to the faces
faceNormals = normalizeVector3d(meshFaceNormals(vertices, faces));

% compute normal of each vertex: sum of normals to each face
normals = zeros(nv, 3);
if isnumeric(faces)
    for i = 1:nf
        face = faces(i, :);
        for j = 1:length(face)
            v = face(j);
            normals(v, :) = normals(v,:) + faceNormals(i,:);
        end
    end
else
    for i = 1:nf
        face = faces{i};
        for j = 1:length(face)
            v = face(j);
            normals(v, :) = normals(v,:) + faceNormals(i,:);
        end
    end
end

% normalize vertex normals to unit vectors
normals = normalizeVector3d(normals);


% Constructing a random graph based on a given degree sequence.
% Idea source: Molloy M. & Reed, B. (1995) Random Structures and Algorithms 6, 161-179
%
% INPUTs: a graphic sequence of numbers, 1xn
% OUTPUTs: adjacency matrix of resulting graph, nxn
%
% Note: The simple version of this algorithm gets stuck about half
%       of the time, so in this implementation the last problematic
%       edge is rewired.
%
% Other routines used: adj2edgeL.m, rewireThisEdge.m, edgeL2adj.m
% GB: last updated, Oct 25 2012

function adj= randomGraphFromDegreeSequence(Nseq)

stubs=Nseq;                % assign degrees to stubs
adj = zeros(length(Nseq)); % initialize adjacency matrix

old_sum = 0;
cnt=0;

while sum(stubs)>0   % while no more stubs are left to connect

  if cnt>5                       % if rewiring did not work 5 times

    el = adj2edgeL(adj);
    ind = find(stubs>0);

    if length(ind) == 1; elr = rewireThisEdge([el; ind(1) ind(1) 1],ind(1),ind(1));  end
    if length(ind) == 2;  elr = rewireThisEdge([el; ind(1) ind(2) 1; ind(2) ind(1) 1],ind(1),ind(2)); end

    if length(ind)>2 || isempty(elr)           % restart algorithm
      printf('randomGraphFromDegreeSequence(): restarting ...\n')
      stubs = Nseq;
      adj = zeros(length(Nseq));
      old_sum = 0;
      cnt=0;

    else
      adj = edgeL2adj(elr);  % return matrix with last edge rewired
      return

    end

  end

  new_sum = sum(stubs);

  if old_sum==new_sum; cnt = cnt+1; end       % no new nodes have been connected, counter+1
  if old_sum~=new_sum; cnt=0; end             % new connections, restart count

  [~,n1] = max(stubs);                % pick the node with highest number of remaining stubs

  old_sum = sum(stubs);

  ind = find(stubs>0);
  n2 = ind(randi(length(ind)));

  if n1==n2; continue; end            % no self-loops

  if adj(n1,n2)>0; continue; end      % no double edges
  adj(n1,n2)=1; adj(n2,n1)=1;
  stubs(n1) = stubs(n1) - 1;
  stubs(n2) = stubs(n2) - 1;

end


%% ------------------------solveAmplitudeFlow.m----------------------------

% Solver for Amplitude Flow as given in Algorithm 1 of the Truncated
% Amplitude Flow (TAF) paper minus the truncations.  The idea was to test the
% amplitude based objective function without removing any outliers. The
% advantage is that we can easily call FASTA. Refer to the userguide for a
% detailed usage of the package.

%  See the script 'testAmplitudeFlow.m' for an example of proper usage of
%  this function.

% PAPER TITLE:
%              Solving Systems of Random Quadratic Equations via Truncated
%              Amplitude Flow.

% ARXIV LINK:
%              https://arxiv.org/pdf/1605.08285.pdf

% INPUTS:
%         A:   Function handle/numerical matrix for data matrix A. The rows
%              of this matrix are the measurement vectors that produce
%              amplitude measurements '\psi'.
%         At:  Function handle/numerical matrix for A transpose.
%         b0:  Observed data vector consisting of amplitude measurements
%              generated from b0 = |A*x|. We assign it to 'psi' to be
%              consistent with the notation in the paper.
%         x0:  The initial vector to be used by any solver.
%        opts: struct consists of the options for the algorithm. For
%              details,see header in solvePhaseRetrieval.m or the User
%              Guide.

% OUPTUT :
%         sol: n x 1 vector. It is the estimated signal.
%        outs: A struct consists of the convergence info. For details,
%              see header in solvePhaseRetrieval.m or the User Guide.

% Note:        When a function handle is used, the value of 'n' (the length
%              of the unknown signal) and 'At' (a function handle for the
%              adjoint of 'A') must be supplied. When 'A' is numeric, the
%              values of 'At' and 'n' are ignored and inferred from the
%              arguments

% DESCRIPTION:
%             The wirtinger flow algorithm uses the squared magnitude
%             objective. This generates a slightly different gradient then
%             the amplitude based objective which is not squared. The TAF
%             paper proposes an amplitude based objective optimization
%             using truncation. This code implements a plain vanilla
%             gradient descent without truncation.

% METHOD:
%         1.) Our implementation uses FASTA, A Forward Backward Splitting
%             Package. FASTA can solve gradient descent schemes provided
%             the objective function expression, its gradient expression,
%             the non smooth term and its proximal operator
%

%         2.) Set the objectve f = @(z) 1/2 * norm(abs(z) - b0)^2.
%             The mask contains the truncated vectors that need to be
%             included.

%         3.) The gradient grad = @(z) (z - b0 .* sign(z)). There is no
%             non smooth term hence g = 0. Conseqently prox = @(z) z
%
%         4.) Send all the above parameters to FASTA which then spits out
%             the estimated signal.

% PhasePack by Rohan Chandra, Ziyuan Zhong, Justin Hontz, Val McCulloch,
% Christoph Studer, & Tom Goldstein
% Copyright (c) University of Maryland, 2017

%% -----------------------------START-----------------------------------

function [sol, outs] = solveAmplitudeFlow(A, At, b0, x0, opts)
%     addpath('solvers/linesearch');

    innerOpts = struct;
    innerOpts.maxIters = opts.maxIters;
    innerOpts.maxTime = opts.maxTime;
    innerOpts.tol = opts.tol;
    innerOpts.verbose = opts.verbose;
    innerOpts.recordTimes = opts.recordTimes;
    innerOpts.recordResiduals = opts.recordResiduals;
    innerOpts.recordMeasurementErrors = opts.recordMeasurementErrors;
    innerOpts.recordReconErrors = opts.recordReconErrors;
    innerOpts.xt = opts.xt;

    innerOpts.searchMethod = opts.searchMethod;
    innerOpts.betaChoice = opts.betaChoice;

    [sol, outs] = gradientDescentSolver(A, At, x0, b0, @updateObjective, innerOpts);

    function [f, gradf] = updateObjective(~, ~)
        f = @(z) 1/2 * norm(abs(z) - b0)^2;
        gradf = @(z) (z - b0 .* sign(z));
    end
end


%APPCR1 Character recognition.

% Mark Beale, 12-15-93
% Copyright 1992-2005 The MathWorks, Inc.
% $Revision: 1.15.2.1 $  $Date: 2005/11/15 01:14:56 $

clf;
figure(gcf)

echo on

%    NEWFF   - Inititializes feed-forward networks.
%    TRAINGDX - Trains a feed-forward network with faster backpropagation.
%    SIM   - Simulates feed-forward networks.

%    CHARACTER RECOGNITION:

%    Using the above functions a feed-forward network is trained
%    to recognize character bit maps, in the presence of noise.

pause % Strike any key to continue...

%    DEFINING THE MODEL PROBLEM
%    ==========================

%    The script file PRPROB defines a matrix ALPHABET
%    which contains the bit maps of the 26 letters of the
%    alphabet.

%    This file also defines target vectors TARGETS for
%    each letter.  Each target vector has 26 elements with
%    all zeros, except for a single 1.  A has a 1 in the
%    first element, B in the second, etc.

[alphabet,targets] = prprob;
[R,Q] = size(alphabet);
[S2,Q] = size(targets);

pause % Strike any key to define the network...

%    DEFINING THE NETWORK
%    ====================

%    The character recognition network will have 25 TANSIG
%    neurons in its hidden layer.

S1 = 10;
net = newff(minmax(alphabet),[S1 S2],{'logsig' 'logsig'},'traingdx');
net.LW{2,1} = net.LW{2,1}*0.01;
net.b{2} = net.b{2}*0.01;

pause % Strike any key to train the network...

%    TRAINING THE NETWORK WITHOUT NOISE
%    ==================================

net.performFcn = 'sse';        % Sum-Squared Error performance function
net.trainParam.goal = 0.1;     % Sum-squared error goal.
net.trainParam.show = 20;      % Frequency of progress displays (in epochs).
net.trainParam.epochs = 5000;  % Maximum number of epochs to train.
net.trainParam.mc = 0.95;      % Momentum constant.

%    Training begins...please wait...

P = alphabet;
T = targets;

[net,tr] = train(net,P,T);

%    ...and finally finishes.

pause % Strike any key to train the network with noise...

%    TRAINING THE NETWORK WITH NOISE
%    ===============================

%    A copy of the network will now be made.  This copy will
%    be trained with noisy examples of letters of the alphabet.

netn = net;

netn.trainParam.goal = 0.6;    % Mean-squared error goal.
netn.trainParam.epochs = 300;  % Maximum number of epochs to train.

%    The network will be trained on 10 sets of noisy data.

pause % Strike any key to begin training...

%    Training begins...please wait...

T = [targets targets targets targets];
for pass = 1:10
  fprintf('Pass = %.0f\n',pass);
  P = [alphabet, alphabet, ...
      (alphabet + randn(R,Q)*0.1), ...
      (alphabet + randn(R,Q)*0.2)];

  [netn,tr] = train(netn,P,T);
  echo off
end
echo on

%    ...and finally finishes.

pause % Strike any key to finish training the network...

%    TRAINING THE SECOND NETWORK WITHOUT NOISE
%    =========================================

%    The second network is now retrained without noise to
%    insure that it correctly categorizes non-noizy letters.

netn.trainParam.goal = 0.1;    % Mean-squared error goal.
netn.trainParam.epochs = 500;  % Maximum number of epochs to train.
net.trainParam.show = 5;       % Frequency of progress displays (in epochs).

%    Training begins...please wait...

P = alphabet;
T = targets;

[netn,tr] = train(netn,P,T);

%    ...and finally finishes.

pause % Strike any key to test the networks...

%    TRAINING THE NETWORK
%    ====================

% SET TESTING PARAMETERS
noise_range = 0:.05:.5;
max_test = 100;
network1 = [];
network2 = [];
T = targets;

% PERFORM THE TEST
for noiselevel = noise_range
  fprintf('Testing networks with noise level of %.2f.\n',noiselevel);
  errors1 = 0;
  errors2 = 0;

  for i=1:max_test
    P = alphabet + randn(35,26)*noiselevel;

    % TEST NETWORK 1
    A = sim(net,P);
    AA = compet(A);
    errors1 = errors1 + sum(sum(abs(AA-T)))/2;

    % TEST NETWORK 2
    An = sim(netn,P);
    AAn = compet(An);
    errors2 = errors2 + sum(sum(abs(AAn-T)))/2;
    echo off
  end

  % AVERAGE ERRORS FOR 100 SETS OF 26 TARGET VECTORS.
  network1 = [network1 errors1/26/100];
  network2 = [network2 errors2/26/100];
end
echo on

pause % Strike any key to display the test results...

%    DISPLAY RESULTS
%    ===============

%    Here is a plot showing the percentage of errors for
%    the two networks for varying levels of noise.

clf
plot(noise_range,network1*100,'--',noise_range,network2*100);
title('Percentage of Recognition Errors');
xlabel('Noise Level');
ylabel('Network 1 - -   Network 2 ---');

%    Network 1, trained without noise, has more errors due
%    to noise than does Network 2, which was trained with noise.

echo off
disp('End of APPCR1')


function [ a, info ] = spofa ( a, lda, n )

%*****************************************************************************80
%
%% SPOFA factors a real symmetric positive definite matrix.
%
%  Discussion:
%
%    SPOFA is usually called by SPOCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the symmetric matrix to be  factored.  Only the
%    diagonal and upper triangle are used.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(LDA,N), an upper triangular matrix R so that A = R'*R
%    where R' is the transpose.  The strict lower triangle is unaltered.
%    If INFO /= 0, the factorization is not complete.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
  for j = 1 : n

    s = 0.0;

    for k = 1 : j-1
      t = a(k,j) - sdot ( k-1, a(1:k-1,k), 1, a(1:k-1,j), 1 );
      t = t / a(k,k);
      a(k,j) = t;
      s = s + t * t;
    end

    s = a(j,j) - s;

    if ( s <= 0.0 )
      info = j;
      return
    end

    a(j,j) = sqrt ( s );

  end

  info = 0;

  return
end