% POLYGONS Manipulation of planar polygons and polylines
% Version 1.6 21-Mar-2011 .
%
%   The 'polygons' module contains functions operating on shapes composed
%   of a vertex list, like polygons or polylines.
%
%   We call 'polyline' the curve defined by a series of vertices.
%   A polyline can be either closed or open, depending on whether the last
%   vertex is connected to the first one or not. This can be given as an
%   option is some functions in the module.
%   A 'polygon' is the planar domain delimited by a closed polyline. We
%   sometimes want to consider 'complex polygons', whose boundary is
%   composed of several disjoint domains. The domain defined by a single
%   closed polyline is called 'simple polygon'.
%   We call 'curve' a polyline with many vertices, such that the polyline
%   can be considered as a discrete approximation of a "real" curve.
%
%   A simple polygon or polyline is represented by a N-by-2 array, each row
%   of the array representing the coordinates of a vertex.
%   Simple polygons are assumed to be closed, so there is no need to repeat
%   the first vertex at the end.
%   As both polygons and polylines can be represented by a list of vertex
%   coordinates, some functions also consider the vertex list itself. Such
%   functions are prefixed by 'pointSet'. Also, many functions prefixed by
%   'polygon' or 'polyline' works also on the other type of shape.
%
%   For multiple-connected polygons, the different connected boundaries are
%   separated by a row [NaN NaN].
%
%   For some functions, the orientation of the polygon can be relevant: CCW
%   stands for 'Conter-Clockwise' (positive orientation), CW stands for
%   'Clockwise'.
%
%   Polylines are parametrized in the following way:
%   * the i-th vertex is located at position i-1
%   * points of the i-th edge have positions ranging linearly from i-1 to i
%   The parametrization domain for an open polyline is from 0 to Nv-1, and
%   from 0 to Nv for a closed polyline (positions 0 and Nv correspond to
%   the same point).
%
%   Example:
%   % Simple polygon:
%   P1 = [1 1;2 1;2 2;1 2];
%   drawPolygon(P1);
%   axis([0 5 0 5]);
%   % Multiple polygon:
%   P2 = [10 10;40 10;40 40;10 40;NaN NaN;20 20;20 30;30 30;30 20];
%   figure;drawPolygon(P2); axis([0 50 0 50]);
%
%
% Polylines
%   polylinePoint             - Extract a point from a polyline
%   polylineLength            - Return length of a polyline given as a list of points
%   polylineCentroid          - Compute centroid of a curve defined by a series of points
%   polylineSubcurve          - Extract a portion of a polyline
%   resamplePolyline          - Distribute N points equally spaced on a polyline
%   resamplePolylineByLength  - Resample a polyline with a fixed sampling step
%   reversePolyline           - Reverse a polyline, by iterating vertices from the end
%   isPointOnPolyline         - Test if a point belongs to a polyline
%   projPointOnPolyline       - Compute position of a point projected on a polyline
%   distancePointPolyline     - Compute shortest distance between a point and a polyline
%   distancePolylines         - Compute the shortest distance between 2 polylines
%   intersectLinePolyline     - Intersection points between a line and a polyline
%   intersectPolylines        - Find the common points between 2 polylines
%   polylineSelfIntersections - Find self-intersection points of a polyline
%   simplifyPolyline          - Douglas-Peucker simplification of a polyline
%   smoothPolyline            - Smooth a polyline using local averaging
%   removeMultipleVertices    - Remove multiple vertices of a polygon or polyline
%
% Polygon basic manipulation
%   polygonPoint              - Extract a point from a polygon
%   polygonSubcurve           - Extract a portion of a polygon
%   polygonEdges              - Return the edges of a simple or multiple polygon
%   reversePolygon            - Reverse a polygon, by iterating vertices from the end
%   smoothPolygon             - Smooth a polygon using local averaging
%   simplifyPolygon           - Douglas-Peucker simplification of a polygon
%   projPointOnPolygon        - Compute position of a point projected on a polygon
%   splitPolygons             - Convert a NaN separated polygon list to a cell array of polygons
%   polygonLoops              - Divide a possibly self-intersecting polygon into a set of simple loops
%
% Polygon clipping and intersections
%   intersectLinePolygon      - Intersection points between a line and a polygon
%   intersectRayPolygon       - Intersection points between a ray and a polygon
%   intersectEdgePolygon      - Intersection point of an edge with a polygon
%   polygonSelfIntersections  - Find self-intersection points of a polygon
%   clipPolygon               - Clip a polygon with a rectangular box
%   clipPolygonHP             - Clip a polygon with a Half-plane defined by a directed line
%
% Point Sets
%   pointSetsAverage          - Compute the average of several point sets
%   minimumCaliperDiameter    - Minimum caliper diameter of a set of points
%   findPoint                 - Find index of a point in an set from its coordinates
%   convexHull                - Convex hull of a set of points
%   randomPointInPolygon      - Generate random point(s) in a polygon
%
% Measures on Polygons
%   isPointInPolygon          - Test if a point is located inside a polygon
%   polygonContains           - Test if a point is contained in a multiply connected polygon
%   polygonCentroid           - Compute the centroid (center of mass) of a polygon
%   polygonArea               - Compute the signed area of a polygon
%   polygonInertiaEllipse     - Compute ellipse with same inertia moments as polygon
%   polygonSecondAreaMoments  - Compute second-order area moments of a polygon
%   polygonLength             - Perimeter of a polygon
%   polygonNormalAngle        - Compute the normal angle at a vertex of the polygon
%   polygonBounds             - Compute the bounding box of a polygon
%   polygonOuterNormal        - Outer normal vector for a given vertex(ices)
%   distancePointPolygon      - Shortest distance between a point and a polygon
%   distancePolygons          - Compute the shortest distance between 2 polygons
%   distancePolygonsNoCross   - Compute the shortest distance between 2 polygons
%   polygonSignature          - Polar signature of a polygon (polar distance to origin)
%   signatureToPolygon        - Reconstruct a polygon from its polar signature
%
% More complex operations on polygons
%   resamplePolygon           - Distribute N points equally spaced on a polygon
%   resamplePolygonByLength   - Resample a polygon with a fixed sampling step
%   densifyPolygon            - Add several points on each edge of the polygon
%   expandPolygon             - Expand a polygon by a given (signed) distance
%   triangulatePolygon        - Compute a triangulation of the polygon
%   polygonSymmetryAxis       - Try to identify symmetry axis of polygon
%   medialAxisConvex          - Compute medial axis of a convex polygon
%
% Curves (polylines with lot of vertices)
%   parametrize               - Parametrization of a polyline, based on edges lengths
%   curvature                 - Estimate curvature of a polyline defined by points
%   cart2geod                 - Convert cartesian coordinates to geodesic coord.
%   geod2cart                 - Convert geodesic coordinates to cartesian coord.
%   curveMoment               - Compute inertia moment of a 2D curve
%   curveCMoment              - Compute centered inertia moment of a 2D curve
%   curveCSMoment             - Compute centered scaled moment of a 2D curve
%
% Functions from stochastic geometry
%   steinerPoint              - Compute steiner point (weighted centroid) of a polygon
%   steinerPolygon            - Create a Steiner polygon from a set of vectors
%   supportFunction           - Compute support function of a polygon
%   convexification           - Compute the convexification of a polygon
%
% Input, Output and conversions
%   polygonToRow              - Convert polygon coordinates to a row vector
%   rowToPolygon              - Create a polygon from a row vector
%   contourMatrixToPolylines  - Converts a contour matrix array into a polyline set
%   readPolygonSet            - Read a set of simple polygons stored in a file
%   writePolygonSet           - Write a set of simple polygons into a file
%
% Drawing functions
%   drawPolyline              - Draw a polyline specified by a list of points
%   drawPolygon               - Draw a polygon specified by a list of points
%   fillPolygon               - Fill a polygon specified by a list of points
%   drawVertices              - Draw the vertices of a polygon or polyline
%
%
%   Credits:
%   * function intersectPolylines uses the 'interX' contribution from "NS"
%       (file exchange 22441, called 'curve-intersections')
%
% -----
% Author: David Legland
% e-mail: david.legland@inra.fr
% created the  07/11/2005.
% Project homepage: http://github.com/mattools/matGeom
% http://www.pfl-cepia.inra.fr/index.php?page=geom2d
% Copyright INRA - Cepia Software Platform.

help('Contents');

%% Requires further development

%% Others...


%%**********************************************************
%% cheby0:
%%
%%    minimize || p(d) ||_infty
%%    p = polynomial of degree <= m such that p(0) = 1.
%%
%%    Here d = n-vector
%%----------------------------------------------------------
%% [blk,Avec,C,b,X0,y0,Z0,objval,p] = cheby0(d,m,solve);
%%
%% d = a vector.
%% m = degree of polynomial.
%% feas  = 1 if want feasible starting point
%%       = 0 if otherwise.
%% solve = 0 if just want initialization
%%       = 1 if want to solve the problem
%%*****************************************************************
%% SDPT3: version 4.0
%% Copyright (c) 1997 by
%% Kim-Chuan Toh, Michael J. Todd, Reha H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*****************************************************************

function [blk,Avec,C,b,X0,y0,Z0,objval,p] = cheby0(d,m,solve);

  if nargin <= 2; solve = 0; end;
  if (size(d,1) < size(d,2)); d = d.'; end;
  cmp = 1-isreal(d);

  tstart=cputime;
  n = length(d);
  e = ones(n,1);
  V(1:n,1) = e/norm(e); R(1,1) = 1/norm(e);
  for i =1:m
      v = d.*V(:,i);
      for j = 1:i                 %% Arnoldi iterations:
          H(j,i) = (V(:,j))'*v;   %% constructing upper-Hessenberg matrix.
          v = v - H(j,i)*V(:,j);  %% orthonormaliztion of Krylov basis.
      end;
      H(i+1,i) = norm(v);
      V(:,i+1) = v/H(i+1,i);
      R(1:i+1,i+1) = (1/H(i+1,i))*([0; R(1:i,i)] - [R(1:i,1:i)*H(1:i,i); 0]);
  end
  if (cmp)
     blk{1,1} = 'q'; blk{1,2} = 3*ones(1,n);
     C = zeros(3*n,1); C(2:3:3*n) = ones(n,1);
     b = [zeros(2*m,1); -1];
     Atmp = [];
     II = [0:3:3*n-3]'; ee = ones(n,1);
     for k=1:m
         dVk = d.*V(:,k);
         Atmp = [Atmp; [2+II, k*ee, real(dVk)]; [3+II, k*ee, imag(dVk)]];
         Atmp = [Atmp; [2+II, (m+k)*ee, -imag(dVk)]; [3+II, (m+k)*ee, real(dVk)]];
     end
     Atmp = [Atmp;  [1+II, (2*m+1)*ee, -ones(n,1)]];
  else
     blk{1,1} = 'l'; blk{1,2} = 2*ones(1,n);
     b = [zeros(m,1); -1];
     C = [ones(n,1); -ones(n,1)];
     Atmp = [];
     II = [1:n]'; ee = ones(n,1);
     for k=1:m
         dVk = d.*V(:,k);
         Atmp = [Atmp; [II, k*ee, dVk]; [n+II, k*ee, -dVk]];
     end
     Atmp = [Atmp; [II, (m+1)*ee, -ee]; [n+II, (m+1)*ee, -ee]];
  end
  Avec = spconvert(Atmp);
  [X0,y0,Z0] = infeaspt(blk,Avec,C,b);
%%
  if (solve)
     [obj,X,y,Z] = sqlp(blk,Avec,C,b,[],X0,y0,Z0);
     if (cmp)
        y  = y(1:m) + sqrt(-1)*y(m+1:2*m);
     else
        y = y(1:m);
     end
     x1 = R(1:m,1:m)*y(1:m);
     p = [-x1(m:-1:1); 1];
     objval = -mean(obj);
  else
     objval = []; p = [];
  end
%%**********************************************************


function [vdeg] = trideg2(pp,tt)
%TRIDEG2 calc. topological degree for vertices in a 2-simpl-
%ex triangulation.
%   [VDEG] = TRIDEG2(VERT,TRIA) returns the no. of triangles
%   incident to each vertex. VDEG is a V-by-1 array of vert-
%   ex degrees, VERT is a V-by-D array of XY coordinates,
%   and TRIA is a T-by-3 array of vertex indexing, where
%   each row defines a triangle, such that
%   VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and
%   VERT(TRIA(II,3),:) are the coordinates of the II-TH tri-
%   angle.
%
%   See also TRISCR2, TRIVOL2, TRIANG2, TRIBAL2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 10/07/2018

%---------------------------------------------- basic checks
    if (~isnumeric(pp) || ~isnumeric(tt) )
        error('trideg2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ndims(tt) ~= +2 )
        error('trideg2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(pp,2) < +2 || size(tt,2) < +3 )
        error('trideg2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nvrt = size(pp,1) ;
    ntri = size(tt,1) ;

%---------------------------------------------- basic checks
    if (min(min(tt(:,1:3))) < +1 || ...
            max(max(tt(:,1:3))) > nvrt )
        error('trideg2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%------------------------------------- compute vertex degree
    vdeg = sum(sparse( ...
        tt(:,1:3),repmat( ...
            (1:ntri)',1,3),+1,nvrt,ntri),2) ;

end


function [Y,W,SetupStruc] = Process_FastICA_HO_Sawada(s,Transfer,SetupStruc)
K = SetupStruc.FastICA_HO_Sawada.K;
hop = SetupStruc.FastICA_HO_Sawada.hop;
win = hanning(K,'periodic');
win = win/sqrt(sum(win(1:hop:K).^2));
SetupStruc.FastICA_HO_Sawada.win = win;  % Preserve 'win' in 'SetupStruc'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N = size(s,2);
for i = 1:N
    X(:,:,i) = fft(enframe(s(:,i),win,hop)');
end
frame_N = size(X,2);
K_m = K/2+1;
Num = size(Transfer,3);
Y = zeros((frame_N-1)*hop+K,Num);
Y_f = zeros(size(X,1),size(X,2),Num);
Y_P = zeros(frame_N,Num,K_m);
%%%%%%%%%%%%%%%%%%%%%%%%%% Obtain processing matrix 'W'
theta = 10^-6;
W = zeros(Num,N,K_m);
A = zeros(1001,K/2)-1; %%%% Show the decrease of non-linear correlation, ICA max iterations 1000
for i = 2:K_m
    X_f = permute(X(i,:,:),[3 2 1]);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%% PCA and ICA processing
    [E,D] = PCA(X_f,1,Num);
    V = sqrt(D)\E';
    X_f = V*X_f;
    %%%%%%%% FastICA based on higher-order statistics
    W_f = eye(Num);
    pObj = inf;
    for i_iteration = 1:200
        Obj = 0;
        for i_n = 1:Num
            W_i = W_f(i_n,:);
            y_ = W_i*X_f;
            y_2 = real(y_).^(2)+imag(y_).^(2);
            %%%%%%%%%%%%%%%%%%%%% There are three different contrast functions.
%             %%%  G1(y) = sqrt(a1+y), g1(y) = 1/(2*sqrt(a1+y))
%             g = 0.5*(0.1+y_2).^(-1/2);
%             g_ = -0.25*(0.1+y_2).^(-3/2);
%             Obj = Obj+sum(sqrt(0.1+y_2))/frame_N;
            %%%  G2(y) = log(a2+y),  g2(y) = 1/(a2+y)
            g = (0.1+y_2).^(-1);
            g_ = -(0.1+y_2).^(-2);
            Obj = Obj+sum(log(0.1+y_2))/frame_N;
%             %%%  G3(y) = 0.5*y^2,    g3(y) = y     (Kurtosis)
%             g = y_2;
%             g_ = ones(1,frame_N);
%             Obj = Obj+0.5*sum(y_2.^2)/frame_N;
            %%%%%%%%%%%%%%%%%%%%%
%             W_i = sum(X_f.*repmat(conj(y_).*g,[Num,1]),2)/frame_N-sum(g+y_2.*g_,2)/frame_N*W_i';
            W_i = (X_f*(conj(y_).*g).'-(sum(g,2)+y_2*g_.')*W_i')/frame_N;
            W_i = W_i/norm(W_i);
            W_f(i_n,:) = W_i';
        end
        W_f = (W_f*W_f')^(-1/2)*W_f;
        dObj = pObj-Obj;
        pObj = Obj;
        A(i_iteration,i-1) = Obj;
        if(abs(dObj)/abs(Obj)<theta)
            break;
        end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     [Y_,W_ICA,A] = FDICA(X_f,eye(Num),A,i);  %%% 'A', 'i' record the decrease for observation
    Y_ = W_f*X_f;
    W(:,:,i) = W_f*V;
    Y_P(:,:,i) = Y_.';
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Process the ambiguity of permutation and amplitude
P = Permu_Sawada(W,Y_P,SetupStruc,'all');  %%%% Options: 'DOA','cor', 'all'
for i = 2:K_m
    W(:,:,i) = P(:,:,i)*W(:,:,i);
    Y_ = permute(Y_P(:,:,i),[2 1 3]);
    Y_ = P(:,:,i)*Y_;
    Y_f(i,:,:) = Y_.';
    if(i~=K_m)
        Y_f(K+2-i,:,:) = Y_';
    end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Recover signals
if(K/hop==2)
    win = ones(K,1);
end
for i = 1:Num
    Y(:,i) = overlapadd(real(ifft(Y_f(:,:,i)))',win,hop);
end
return;


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

clear all;
%close all;
clf

num=1;
den=[1 0];
w=logspace(-2,3);
[m,p]=bode(num,den,w);;
m1=ones(50,1)./m;
den=[1 0 0];
[m2,p]=bode(num,den,w);
m2=ones(50,1)./m2;
loglog(w,m,w,m1,w,m2);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.6 Magnitude of (j\omega)^n');
bodegrid;


function n=random_n;
% random unit vector n

while 1
    % random from cube 3*3*3:
    rfc=3*rand(3,1)-1.5;
    rfcl=sqrt(rfc'*rfc); % length
    if (0.5<=rfcl)&&(rfcl<=1)
        break
    end
end

n=rfc/rfcl;


function b = r8cbb_to_r8ge ( n1, n2, ml, mu, a )

%*****************************************************************************80
%
%% R8CBB_TO_R8GE copies a R8CBB matrix to a R8GE matrix.
%
%  Discussion:
%
%    The R8CBB storage format is for a compressed border banded matrix.
%    Such a matrix has the logical form:
%
%      A1 | A2
%      ---+---
%      A3 | A4
%
%    with A1 a (usually large) N1 by N1 banded matrix, while A2, A3 and A4
%    are dense rectangular matrices of orders N1 by N2, N2 by N1, and N2 by N2,
%    respectively.
%
%    The R8CBB format is the same as the DBB format, except that the banded
%    matrix A1 is stored in compressed band form rather than standard
%    banded form.  In other words, we do not include the extra room
%    set aside for fill in during pivoting.
%
%    A should be defined as a vector.  The user must then store
%    the entries of the four blocks of the matrix into the vector A.
%    Each block is stored by columns.
%
%    A1, the banded portion of the matrix, is stored in
%    the first (ML+MU+1)*N1 entries of A, using the obvious variant
%    of the LINPACK general band format.
%
%    The following formulas should be used to determine how to store
%    the entry corresponding to row I and column J in the original matrix:
%
%    Entries of A1:
%
%      1 <= I <= N1, 1 <= J <= N1, (J-I) <= MU and (I-J) <= ML.
%
%      Store the I, J entry into location
%      (I-J+MU+1)+(J-1)*(ML+MU+1).
%
%    Entries of A2:
%
%      1 <= I <= N1, N1+1 <= J <= N1+N2.
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+(J-N1-1)*N1+I.
%
%    Entries of A3:
%
%      N1+1 <= I <= N1+N2, 1 <= J <= n1.
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%
%    Entries of A4:
%
%      N1+1 <= I <= N1+N2, N1+1 <= J <= N1+N2
%
%      Store the I, J entry into location
%      (ML+MU+1)*N1+N1*N2+(J-1)*N2+(I-N1).
%      (same formula used for A3).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N1, N2, the order of the banded and dense blocks.
%    N1 and N2 must be nonnegative, and at least one must be positive.
%
%    Input, integer ML, MU, the lower and upper bandwidths.
%    ML and MU must be nonnegative, and no greater than N1-1.
%
%    Input, real A((ML+MU+1)*N1+2*N1*N2+N2*N2), the R8CBB matrix.
%
%    Output, real B(N1+N2,N1+N2), the R8GE matrix.
%
  for i = 1 : n1
    for j = 1 : n1

      if ( mu+ml < (j-i) | ml < (i-j) )
        b(i,j) = 0.0;
      else
        ij = (i-j+mu+1)+(j-1)*(ml+mu+1);
        b(i,j) = a(ij);
      end

    end
  end

  for i = 1 : n1
    for j = n1+1 : n2
      ij = (ml+mu+1)*n1+(j-n1-1)*n1+i;
      b(i,j) = a(ij);
    end
  end

  for i = n1+1 : n2
    for j = 1 : n1+n2
      ij = (ml+mu+1)*n1+n2*n1+(j-1)*n2+(i-n1);
      b(i,j) = a(ij);
    end
  end

  return
end


function Y = var(w,flag)
   %VAR Variance of a waveform's data
   %  Y = var(W), where W is an N-dimensional waveform object returns the
   %  variance of the data within each waveform, normalized by N-1
   %
   %  Y = var(W,1) returns the variance within each waveform,
   %  normalized by N.
   %
   %  Y = var(W,weights), where weights is a vector the same length as the
   %  number of data samples, applies the weights to the variance.
   %
   % See the help for MATLAB's built in VAR for more details.
   %
   % Note: NAN values are completely ignored.
   %
   %  See also WAVEFORM/MEAN, WAVEFORM/MEDIAN, WAVEFORM/STD, NANVAR

   % AUTHOR: Celso Reyes, Geophysical Institute, Univ. of Alaska Fairbanks
   % $Date$
   % $Revision$

   Y = zeros(size(w));

   %if the statistics toolbox is installed, use the builtin nanvar function to
   %ignore NaN values during the variance calculation.
   if ~isempty(ver('stats'))
      if exist('flag','var')
         for n = 1:numel(Y)
            Y(n) = nanvar(w(n).data,flag);
         end
      else
         for n = 1:numel(Y)
            Y(n) = nanvar(w(n).data);
         end
      end
   else
      % the statistics toolbox is not installed, so any nan values will have
      % to be dealt with (ignored) manually.
      if exist('flag','var')
         for n = 1:numel(Y)
            d = w(n).data;
            %d = d(~isnan(d));
            Y(n) = var(d(~isnan(d)),flag);
         end
      else
         for n = 1:numel(Y)
            d = w(n).data;
            %d = d(~isnan(d));
            Y(n) = var(d(~isnan(d)));
         end
      end
   end
end


function bas = bas3DP1(p,t)

%% USAGE: coefficient of Lagrange P1 basis for tetrahedral mesh
%
% INPUTS:
% p --- np-by-3 vector
% t --- nt-by-4 vector
%
% OUTPUTS:
% bas --- nt-4-4 matrix stores the coefficient of basis value(s)
%         The k-i-j th entry denotes the j-th basis function on k-th cell
%         with the i-th coefficient where c_i (i=1,2,3,4) stands for
%                 c_1 + c_2*x + c_3*y + c_4*z

% Last Modified: 08/07/2020 by Xu Zhang

%%
x1 = p(t(:,1),1); y1 = p(t(:,1),2); z1 = p(t(:,1),3);
x2 = p(t(:,2),1); y2 = p(t(:,2),2); z2 = p(t(:,2),3);
x3 = p(t(:,3),1); y3 = p(t(:,3),2); z3 = p(t(:,3),3);
x4 = p(t(:,4),1); y4 = p(t(:,4),2); z4 = p(t(:,4),3);

A = -x2.*y3.*z1 + x2.*y4.*z1 + x1.*y3.*z2 - x1.*y4.*z2 + x2.*y1.*z3 ...
    -x1.*y2.*z3 + x1.*y4.*z3 - x2.*y4.*z3 + x4.*(-y2.*z1 + y3.*z1 + ...
    y1.*z2 - y3.*z2 - y1.*z3 + y2.*z3) + (-x2.*y1 + x1.*y2 - x1.*y3 + ...
    x2.*y3).*z4 + x3.*(-y4.*z1 - y1.*z2 + y4.*z2 + y2.*(z1 - z4) + y1.*z4);

nt = size(t,1);  bas = zeros(nt,4,4);
bas(:,1,1) = (-x4.*y3.*z2 + x3.*y4.*z2 + x4.*y2.*z3 - x2.*y4.*z3 - x3.*y2.*z4 + x2.*y3.*z4);
bas(:,2,1) =-(-y3.*z2 + y4.*z2 + y2.*z3 - y4.*z3 - y2.*z4 + y3.*z4);
bas(:,3,1) = (-x3.*z2 + x4.*z2 + x2.*z3 - x4.*z3 - x2.*z4 + x3.*z4);
bas(:,4,1) =-(-x3.*y2 + x4.*y2 + x2.*y3 - x4.*y3 - x2.*y4 + x3.*y4);
bas(:,1,2) =-(-x4.*y3.*z1 + x3.*y4.*z1 + x4.*y1.*z3 - x1.*y4.*z3 - x3.*y1.*z4 + x1.*y3.*z4);
bas(:,2,2) = (-y3.*z1 + y4.*z1 + y1.*z3 - y4.*z3 - y1.*z4 + y3.*z4);
bas(:,3,2) =-(-x3.*z1 + x4.*z1 + x1.*z3 - x4.*z3 - x1.*z4 + x3.*z4);
bas(:,4,2) = (-x3.*y1 + x4.*y1 + x1.*y3 - x4.*y3 - x1.*y4 + x3.*y4);
bas(:,1,3) = (-x4.*y2.*z1 + x2.*y4.*z1 + x4.*y1.*z2 - x1.*y4.*z2 - x2.*y1.*z4 + x1.*y2.*z4);
bas(:,2,3) =-(-y2.*z1 + y4.*z1 + y1.*z2 - y4.*z2 - y1.*z4 + y2.*z4);
bas(:,3,3) = (-x2.*z1 + x4.*z1 + x1.*z2 - x4.*z2 - x1.*z4 + x2.*z4);
bas(:,4,3) =-(-x2.*y1 + x4.*y1 + x1.*y2 - x4.*y2 - x1.*y4 + x2.*y4);
bas(:,1,4) =-(-x3.*y2.*z1 + x2.*y3.*z1 + x3.*y1.*z2 - x1.*y3.*z2 - x2.*y1.*z3 + x1.*y2.*z3);
bas(:,2,4) = (-y2.*z1 + y3.*z1 + y1.*z2 - y3.*z2 - y1.*z3 + y2.*z3);
bas(:,3,4) =-(-x2.*z1 + x3.*z1 + x1.*z2 - x3.*z2 - x1.*z3 + x2.*z3);
bas(:,4,4) = (-x2.*y1 + x3.*y1 + x1.*y2 - x3.*y2 - x1.*y3 + x2.*y3);
bas = bas./A;


function [Estimation_X] = EKF_propagate(Estimation_X, OdometryFromThis2Next, odom_sigma )

v = OdometryFromThis2Next(1:3);
w = OdometryFromThis2Next(4:6);

% update position and orientation
Estimation_X.position = Estimation_X.position+Estimation_X.orientation*v;
orientation=Estimation_X.orientation;
Estimation_X.orientation = Estimation_X.orientation*Exp(w);

NumberOfLandmarks = size(Estimation_X.landmarks, 2);
%Jrw = J_r(-w);
%ExpMinusM = Exp(-w);

%G1 = [-orientation*Jrw zeros(3,3);zeros(3,3)  -orientation];
%G1 = [-orientation zeros(3,3);zeros(3,3)  -orientation];
G1=[orientation zeros(3,3); -skew(orientation*v)   orientation];

G= [G1; zeros(3* NumberOfLandmarks ,6)];
odoCov=diag([w.^2;v.^2])*odom_sigma^2;
W = G*odoCov*G';

% compute matrix A_{n}
temp = repmat({ eye(3) }, NumberOfLandmarks+2,1 );
A = blkdiag(temp{:});
A(1:3,1:3) = eye(3);% ExpMinusM;
A(4:6,1:3) = -skew(orientation*v);

% final update the covariance
Estimation_X.cov = A*Estimation_X.cov*A'+W;

end


% undoradial    remove radial distortion
%
% [xl] = undoradial(x,K,kc)
%
% x ... 3xN coordinates of the distorted pixel points
% K ... 3x3 camera calibration matrix
% kc ... 4x1 vector of distortion parameters
%
% xl ... linearized pixel coordinates
%        these coordinates should obey the linear pinhole model
%
% It calls comp_distortion_oulu: undistort pixel coordinates.
% function taken from the CalTech camera calibration toolbox

function [xl] = undoradial(x_kk,K,kc)

cc(1) = K(1,3);
cc(2) = K(2,3);
fc(1) = K(1,1);
fc(2) = K(2,2);

% First: Subtract principal point, and divide by the focal length:
x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)];

if norm(kc) ~= 0,
	% Third: Compensate for lens distortion:
	xn = comp_distortion_oulu(x_distort,kc);
else
   xn = x_distort;
end;

% back to the linear pixel coordinates
xl = K*[xn;ones(size(xn(1,:)))];


function [u,p] = StokesIUzawa(u,p,f,g,A,B,auxMat,elem,smootherOpt)
%% STOKESIUzawa Inexact Uzawa smoother for the Stokes eqns.
%
%  In matrix form, we solve the Stokes equations
%
%               |A B'| |u| = |f|
%               |B 0 | |p| = |g|
%
%  Define
%            M = |I -S_u^-1 B'|   T = |S_u    0|
%                |     I      |       |B   -S_p|
%  The matrix form DGS update can be written as
%
%    |uk+1|   |uk|            |ru|     with ru = f-Auk -B'pk,
%    |    | = |  | + M*inv(T)*|  |
%    |pk+1|   |pk|            |rp|     and  rp = 0-Buk
%
%  Created by Ming Wang (with discussion of Long Chen) at Jan, 2012.
%  Revised at Sept 2012.
%
% Copyright (C)  Long Chen. See COPYRIGHT.txt for details.

%% Parameters
itStep = smootherOpt.itStep;
Su = auxMat.DA; % DA = 2*diag(A)
Bt = auxMat.Bt;
BinvDABt = auxMat.BinvDABt; % invDA = 1/(2*diag(A));
% mg options
mgoption.printlevel = 0;
mgoption.maxIt = 1;
mgoption.N0=50;
mgoption.solver = 'Vcycle';
mgoption.mu = 2;

%% DGS relaxation step
for k = 1: itStep
    % Step 1: relax Momentum eqns
    if isempty(u) && isempty(p)
        ru = f;
        u = zeros(size(f));
        p = zeros(size(g));
    else
        ru = f-Bt*p-A*u;
    end
    u = u + ru./Su;
    rp = g-B*u;
    rp = rp - mean(rp);
    % Step 2: relax transformed Continuity eqns
    dp = mg(BinvDABt,-rp,elem,mgoption);
    % Step 3: transform the correction back to the original variables.
    p = p + dp;
    u = u - (Bt*dp)./Su;
end
end


function rcov = cov_blockgeom(X,blocksize)
% like cov(), just robust (using the blockwise geometric median)
% The blocksize allows to reduce the memory requirements, at the cost of reduced robustness
% against outliers that occupy fewer samples than the blocksize.

% Copyright (C) Christian Kothe, SCCN, 2013, christian@sccn.ucsd.edu
%
% This program is free software; you can redistribute it and/or modify it under the terms of the GNU
% General Public License as published by the Free Software Foundation; either version 2 of the
% License, or (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
% even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License along with this program; if not,
% write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
% USA

if ~exist('blocksize','var')
    blocksize = 10; end

[n,m] = size(X);
X = bsxfun(@minus,X,median(X));
U = zeros(length(1:blocksize:n),m*m);
for k=1:blocksize
    range = min(n,k:blocksize:(n+k-1));
    U = U + reshape(bsxfun(@times,reshape(X(range,:),[],1,m),reshape(X(range,:),[],m,1)),size(U));
end
rcov = real(reshape(geometric_median(U/blocksize),m,m));


function geometry_test199 ( )

%*****************************************************************************80
%
%% TEST199 tests SHAPE_RAY_INT_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
  nside = 6;
  ntest = 4;

  center(1:2,1) = [ 3.0; 0.0 ];
  p1(1:2,1) = [ 5.0; 0.0 ];
  pa_test = [ ...
    3.0,  0.0; ...
    3.0,  0.0; ...
    3.0, -1.0; ...
    3.0, -1.0 ]';
  pb_test = [ ...
    4.0,  0.0; ...
    3.0,  1.0; ...
    3.0,  1.0; ...
    7.0,  5.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST199\n' );
  fprintf ( 1, '  For a shape in 2D,\n' );
  fprintf ( 1, '  SHAPE_RAY_INT_2D computes the intersection of\n' );
  fprintf ( 1, '    a shape and a ray whose origin is within\n' );
  fprintf ( 1, '    the shape.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sides:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %d\n', nside );

  r8vec_print ( 2, center, '  Hexagon center:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Hexagon vertex #1:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  %12f  %12f\n', p1(1:2,1) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     I       XA          YA          ' );
  fprintf ( 1, 'XB          YB          XI          YI\n' );
  fprintf ( 1, '\n' );

  for i = 1 : ntest

    pa(1:2,1) = pa_test(1:2,i);
    pb(1:2,1) = pb_test(1:2,i);

    pint = shape_ray_int_2d ( center, p1, nside, pa, pb );

    fprintf ( 1, '  %6d  %10f  %10f  %10f  %10f  %10f  %10f\n', ...
      i, pa(1:2,1), pb(1:2,1), pint(1:2,1) );

  end

  return
end


function [e, edata, eprior] = rbferr(net, x, t)
%RBFERR	Evaluate error function for RBF network.
%
%	Description
%	E = RBFERR(NET, X, T) takes a network data structure NET together
%	with a matrix X of input vectors and a matrix T of target vectors,
%	and evaluates the appropriate error function E depending on
%	NET.OUTFN.  Each row of X corresponds to one input vector and each
%	row of T contains the corresponding target vector.
%
%	[E, EDATA, EPRIOR] = RBFERR(NET, X, T) additionally returns the data
%	and prior components of the error, assuming a zero mean Gaussian
%	prior on the weights with inverse variance parameters ALPHA and BETA
%	taken from the network data structure NET.
%
%	See also
%	RBF, RBFFWD, RBFGRAD, RBFPAK, RBFTRAIN, RBFUNPAK
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Check arguments for consistency
switch net.outfn
case 'linear'
   errstring = consist(net, 'rbf', x, t);
case 'neuroscale'
   errstring = consist(net, 'rbf', x);
otherwise
   error(['Unknown output function ', net.outfn]);
end
if ~isempty(errstring);
  error(errstring);
end

switch net.outfn
case 'linear'
   y = rbffwd(net, x);
   edata = 0.5*sum(sum((y - t).^2));
case 'neuroscale'
   y = rbffwd(net, x);
   y_dist = sqrt(dist2(y, y));
   % Take t as target distance matrix
   edata = 0.5.*(sum(sum((t-y_dist).^2)));
otherwise
   error(['Unknown output function ', net.outfn]);
end

% Compute Bayesian regularised error
[e, edata, eprior] = errbayes(net, edata);


% test the sparse updates of augmented Lagrange.
function [] = testMatrixMultiply2()
clc;

data  = [2 3  4  5  6 ];
Known = [1 11 13 20 44];

m = 5; n = 10; r = 3;

[Ik, Jk] = ind2sub([m n],Known);
S_data   = sparse(Ik, Jk, data, m, n);
rho = 0.5;

% initially S_comp = empty. X0 = U0*V0 + S_comp
S_comp =  sparse(Ik, Jk, eps * ones(size(data)), m, n);

rng(100);
U0 = rand(m, r); U0s = U0;
V0 = rand(r, n); V0s = V0;

check_func = @(NameStr, X, Xs) fprintf('[%s] Related Error %.4g\n', NameStr, norm(X-Xs, 'fro')/norm(X, 'fro'));
%check_func = @(NameStr, X, Xs) fprintf('[%s] Related Error %.4g\n', NameStr, norm(X-Xs, 'fro'));

X0 = (U0 * V0 + S_comp);

disp('init lambda');
[X1  U1  V1]         = update_initLambda (X0, U0, V0, Known, data, rho);
[X1s U1s V1s S_comp] = update_initLambdaSparse (S_comp, U0s, V0s, Ik, Jk, data, rho, true);

check_func('U+', U1, U1s);
check_func('V+', V1, V1s);
check_func('X+', X1, X1s);
for i = 1: 2
    [X1  U1  V1]         = update_initLambda (X1, U1, V1, Known, data, rho);
    [X1s U1s V1s S_comp] = update_initLambdaSparse (S_comp, U1s, V1s, Ik, Jk, data, rho, false);
    check_func('U+', U1, U1s);
    check_func('V+', V1, V1s);
    check_func('X+', X1, X1s);
end

disp('given sparse lambda');
U0 = rand(m, r); U0s = U0;
V0 = rand(r, n); V0s = V0;
X0 = (U0 * V0 + S_comp);

Lambda = sparse(Ik, Jk, ones(size(data)), m, n);
[X1 U1 V1] = update_givenLambda (X0, U0, V0, Known, data, rho, Lambda);
[X1s U1s V1s S_comp] = update_givenLambdaSparse (S_comp, U0s, V0s, Ik, Jk, data, rho, Lambda, true);
check_func('U+', U1, U1s);
check_func('V+', V1, V1s);
check_func('X+', X1, X1s);
for i = 1: 2
    [X1  U1  V1]         = update_givenLambda (X1, U1, V1, Known, data, rho, Lambda);
    [X1s U1s V1s S_comp] = update_givenLambdaSparse (S_comp, U1s, V1s, Ik, Jk, data, rho, Lambda, false);
    check_func('U+', U1, U1s);
    check_func('V+', V1, V1s);
    check_func('X+', X1, X1s);
end

    function [X1 U1 V1] = update_initLambda (X0, U0, V0, Known, data, rho)
        Lambda0 = ones(m, n);
        % X, V => U
        T = X0 - Lambda0/rho;
        U1  = T * V0';
        % X, U => V
        V1  = U1' * T;
        % U, V => X
        T2 = Lambda0/rho + U1 * V1;
        T3 = (2 * data + rho * T2(Known))/(2+ rho);
        X1 = T2; X1(Known) = T3;
    end

    function [X1s U1s V1s S_comp] = update_initLambdaSparse (S_comp, U0, V0, Ik, Jk, data, rho, first_iter)
        % Lambda is given by Lambda = e * et
        e  = ones(m, 1);
        et = ones(1, n);
        if first_iter
            % X, V => U
            U1s =  U0 * (V0 * V0') + S_comp  * V0' -  e * (et * V0') /rho;
            % X, U => V
            V1s = (U1s' * U0) * V0 + U1s' * S_comp  - (U1s' * e) * et / rho;
            % U, V => X
            s_comp_val = 2/(2+rho) * ( data - ones(size(data))/ rho - sparse_inp(U1s', V1s, Ik, Jk));
        else
            % X, V => U
            U1s =  U0 * (V0 * V0') + S_comp  * V0';
            % X, U => V
            V1s = (U1s' * U0) * V0 + U1s' * S_comp;
            % U, V => X
            s_comp_val = 2/(2+rho) * ( data  - sparse_inp(U1s', V1s, Ik, Jk));
        end
        sparse_update(S_comp, s_comp_val);
        X1s = e * et/rho + U1s * V1s + S_comp;
    end

    function [X1 U1 V1] = update_givenLambda (X0, U0, V0, Known, data, rho, Lambda)
        % X, V => U
        T = X0 - Lambda/rho;
        U1  = T * V0';
        % X, U => V
        V1  = U1' * T;
        % U, V => X
        T2 = Lambda/rho + U1 * V1;
        T3 = (2 * data + rho * T2(Known))/(2+ rho);
        X1 = T2; X1(Known) = T3;
    end

    function [X1s U1s V1s S_comp] = update_givenLambdaSparse (S_comp, U0, V0, Ik, Jk, data, rho, LamSMat, first_iter)
        % Lambda is a sparse matrix here.
        if first_iter
            % X, V => U
            U1s =  U0 * (V0 * V0') + S_comp  * V0' -  (LamSMat * V0') /rho;
            % X, U => V
            V1s = (U1s' * U0) * V0 + U1s' * S_comp  - (U1s' * LamSMat) / rho;
            % U, V => X
            [~, ~, ll] = find(LamSMat);
            s_comp_val = 2/(2+rho) * ( data - ll'/ rho - sparse_inp(U1s', V1s, Ik, Jk));
        else
            % X, V => U
            U1s =  U0 * (V0 * V0') + S_comp  * V0';
            % X, U => V
            V1s = (U1s' * U0) * V0 + U1s' * S_comp;
            % U, V => X
            s_comp_val = 2/(2+rho) * ( data  - sparse_inp(U1s', V1s, Ik, Jk));
        end
        sparse_update(S_comp, s_comp_val);
        X1s = LamSMat/rho + U1s * V1s + S_comp;
    end
end


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

function y = svol_2(a, b, r, n, f, k, t)
% computes the SABR implied BS volatility due to Hagan's formula

	z = n./a.*(f*k).^((1-b)/2).*log(f./k);
	x = log((sqrt(1 - 2*r*z + z.^2) + z - r)/(1-r));
	Term1 = a ./ (f*k).^((1-b)/2) ./ (1 + (1-b)^2/24*log(f./k).^2 ...
        + (1-b)^4/1920*log(f./k).^4);
	Term2 = z ./ x;
	Term2(abs(x-z) < 1e-008) = 1;           % account for ATM
	Term3 = 1 + ((1-b)^2/24*a^2./(f*k).^(1-b) ...
        + r*b*n*a/4./(f*k).^((1-b)/2) + (2-3*r^2)/24*n.^2)*t;
	y = Term1.*Term2.*Term3;
end


% Dasslc test problem with classical pendulum
% requires files: pend.m

dae_index = 3;		% differential index of the DAE
g   = 9.8;			% gravity acceleration
L	 = 1.0;			% pendulum cord lenght
t0  = 0.0;        % initial value for independent variable
tf  = 10;         % final value for independent variable
y0  = [1 0 0 0 0]'; % initial state variables (overwritten by pend.dat)
rpar=[g L dae_index]; % optional arguments passed to residual and jacobian functions

index = [0 0 0 0 0	% index 0 formulation (with drift-off effect)
		   1 1 1 1 1	% index 1 formulation
			1 1 2 2 2	% index 2 formulation
         1 1 2 2 3];	% index 3 formulation

tspan=[t0:0.001:tf];
[t,y]=dasslc('pend',tspan,y0,rpar,[],[],index(dae_index+1,:),'pend.dat','jacpend');

plot(t,y);


function y=normminmax(x)
% bulld RBG image
xmin=min(x(:));
xmax=max(x(:));
y=(x-xmin)/(xmax -xmin);
return


function [varargout]=repop(varargin)
% Replicating arithemetical and logical operators.
%
% [Z]=repop(X,operator,Y [,options])
%
% Does element by element operations on X and Y where non-same sized
% dimensions are implicity wrapped round to match the size of the larger
% to give a result matrix Z with size max(size(X),size(Y));
%
% What this means is that if you give a [Nx1] and a [1xM] input you get
% a [NxM] output, or if you give it a [Nx1xQ] and a [NxMx1xP] you get
% [NxMxQxP] output etc..
% Note that the size of the input *completely and uniquely* determines the
% size of the output.
%
% In general this is at least 2x faster than the equivalent matlab code
% using repmats and has the advantage of requiring no additional memory.
%
% Example Usage:
%     X = randn(10000,10);                  % example signal with data in rows
%     stdX = repop(X,'-',mean(X,1));        % subtract the mean vector
%     stdX = repop(stdX,'/',std(stdX,0,1)); % divide by std-deviation
%
% Operator can be one of:
%
% Arthemetical -- returns a double matrix
%   '+','.+',plus   - Implicitly repmatted elementwise addition
%   '-','.-',minus  - Implicitly repmatted elementwise addition
%   '*','.*',times  - Implicitly repmatted elementwise multiplication
%   '^','.^',power  - Implicitly repmatted elementwise raise X to power Y
%   '\','.\',ldivide- Implicitly repmatted elementwise divide Y by X
%   '/','./',rdivide- Implicitly repmatted elementwise divide X by Y
%   'min'           - Implicitly repmatted elementwise min of X by Y
%   'max'           - Implicitly repmatted elementwise max of X by Y
%
% Relational -- returns a logical matrix
% N.B. for complex inputs the <,>,<=,>= operators are based upon abs(x)
% (not real(x) as in matlab)
%   '==',eq         - Implicitly repmatted elementwise equality
%   '~=',ne         - Implicitly repmatted elementwise dis-equality
%   '<' ,lt         - Implicitly repmatted elementwise less than
%   '>' ,gt         - Implicitly repmatted elementwise greater than
%   '<=',le         - Implicitly repmatted elementwise less than equal
%   '>=',ge         - Implicitly repmatted elementwise greater than equal
%
% N.B. the operator can go in any of the 3 argument positions, i.e.
% these are all valid: repop(X,'-',Y), repop(X,Y,'-'), repop('-',X,Y)
%
% The optional final argument is a string of single letter switches
% consisting off
%  'm'  -- allows replication of non-unit dimensions if the larger
%          dimensions size is an integer multiple of the smaller ones
%  'n'  -- allow replication of non-unit dimensions in *all* cases
%  'i'  -- perform "inplace" operation.  This means that we use a
%          *dangerous* matlab *hack* to perform the operation *without*
%          allocating new memory for the output, but by simply overwriting
%          the memory used for X in the input.  Thus the following code is
%          a memory (and time) efficient way to increment X.
%              X = repop(X,'+',1,'i');
%
%
% Class support of input P:
%     float: double, single
%
% SEE ALSO:   repop_testcases rplus rminus rtimes rpower rldivide rrdivide
%             req rne rlt rgt rle rge
%
% Copyright 2006-     by Jason D.R. Farquhar (jdrf@zepler.org)
% Permission is granted for anyone to copy, use, or modify this
% software and accompanying documents for any uncommercial
% purposes, provided this copyright notice is retained, and note is
% made of any changes that have been made. This software and
% documents are distributed without any warranty, express or
% implied
%
% Inspired by code from Douglas M. Schwarz and & Aki Vehtari.

% The rest of this code is a mex-hiding mechanism which compilies the mex if
% this runs and recursivly calls itself.
% Based upon code from: http://theoval.sys.uea.ac.uk/matlab
cwd  = pwd; % store the current working directory
name = mfilename('fullpath'); % get the directory where we are
% find out what directory it is defined in
name(name=='\')='/'; % deal with dos'isms
dir=name(1:max(find(name == '/')-1)); % dir is everything before final '/'
try % try changing to that directory
   cd(dir);
catch   % this should never happen, but just in case!
   cd(cwd);
   error(['unable to locate directory containing ''' name '.m''']);
end

try % try recompiling the MEX file
   fprintf(['Compiling ' mfilename ' for first use\n']);
   mex('ddrepop.c','dsrepop.c','sdrepop.c','ssrepop.c','repop_util.c','repop_mex.c','mxInfo.c','mxInfo_mex.c','-O','-output',mfilename);
   fprintf('done\n');
catch
   % this may well happen happen, get back to current working directory!
   cd(cwd);
   error('unable to compile MEX version of ''%s''%s\n%s%s', name, ...
         ', please make sure your', 'MEX compiler is set up correctly', ...
         ' (try ''mex -setup'').');
end

cd(cwd); % change back to the current working directory
rehash;  % refresh the function and file system caches

% recursively invoke MEX version using the same input and output arguments
[varargout{1:nargout}] = feval(mfilename, varargin{:});

% bye bye...

return;


function [S, neighbours] = findAcyclicNeighbours2(Y, k)

% FINDACYCLICNEIGHBOURS2 find the k nearest neighbours for each point in Y preventing cycles in the graph.
% FORMAT
% DESC returns the indices of the k nearest neighbours to each point in
% the given data matrix Y.
% ARG y : the data in which neighbours need to be found.
% ARG k : the number of neighbours that need to be found.
% RETURN ind : the indices of each points neighbours.
% RETURN D : the squared distance to each of the neighbours.
%
% COPYRIGHT : Neil D. Lawrence, 2010
%
% SEEALSO : lleOptimise, fmvuOptimise, isomapCreate

% MLTOOLS

  [neighboursInd, A] = findNeighbours(Y, k);
  N = size(Y, 1);
  W = spalloc(N, N, 2*size(Y, 1)*k);
  for i = 1:N
    for j = 1:k
      W(i, neighboursInd(i, j)) = -1;
      W(neighboursInd(i, j), i) = -1;
    end
  end
  L = W;
  jitter = 1e-6;
  L(1:N+1:end) = -sum(W)+jitter;
  %P = amd(L);
  %Y = Y(P, :);
  P = 1:size(L, 1);
  [UT, p, S] = chol(L, 'lower', 'vector');
  %UT = chol(L(P, P))';
  UT(1:N+1:end)=0;
  neighbours{1} = [];
  for i = 1:N-1
    neighbours{i} = find(UT(:, i));
  end
end


function [rho,u,p,fl,Ml_eq] = ModSBBGK()
%function [rho_our p_out t_out]=M_codeA(rho_in,p_in,t_in)
 %switch method
 %   case{1} % TVD 0(h^2)
        % Using discrete ordinate method (discrete and constant velocity
        % values in phase-space domain)
        a = v(:,1);
        % Load initial condition
%         if()
        f = f0;
%         end
% %            for tsteps = time
               f_eq = f_equilibrium_1d(rho_0,ux,v,t,RRR);
                % initialize variables
                 u_next = zeros(1,nx);
                 u_eq = zeros(1,nx);
                 u_l=zeros(1,nx);
                 u = zeros(1,nx);
                 for i = 1:nv
                      % load subcase
                      u_eq(:) = f_eq(i,:);
                      u(:) = f(i,:).*h;
                      u_l(:)=(1.-h).*f(i,:);
                      % Compute the smoothness factors, r(j), from data, u(j).
                       [r] = theta1d(u,a(i));
                        % Compute the Flux Limiter
                       [phi] = fluxlimiter1d(r,1); % using limiter = 1
                       % Compute TVD Fluxes
                       [F_left,F_right] = TVDflux1d(u,a(i),dtdx,phi);
                        [FL_left,FL_right] = TVDflux1d(u_l,a(i),dtdx,phi);
                       % Compute next time step
                        u_next= u - dtdx*(F_right - F_left).*h -dtdx*(FL_right - FL_left).*h + (dt/r_time).*h.*(u_eq-f(i,:));

                        % BC.4
                        u_next(1) = u_next(2);
                        u_next(nx) = u_next(nx-1);
                        % UPDATE info
                         u = u_next;

%                          for i=1:nx
%                              u
%                          end
                        % Going back to f
                         f(i,:) = u(:);
                 end
                   % Compute macroscopic moments

                   [n,j_x,E] = macromoments1d(k,w,f,v);

                   % UPDATE macroscopic properties
                     [ux,t,p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%                      [r,ux,t,p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%                         [p,yun] = macroproperties1d(n,j_x,E,nx,nv,theta);
%            end
%           case{2} % WENO k = 3 i.e. O(h^5)
%   otherwise
%          error('Order must be between 1 and 2');
% end


function x = typeIV(alpha)
%4 times of Bit-wise MAP
alpha_1 = alpha(1 : end/4);
% llr_1 = 2*atanh(prod(tanh(alpha_1/2)));
llr_1 = prod(sign(alpha_1))*min(abs(alpha_1));

alpha_2 = alpha(end/4 + 1 : end/2);
% llr_2 = 2*atanh(prod(tanh(alpha_2/2)));
llr_2 = prod(sign(alpha_2))*min(abs(alpha_2));

alpha_3 = alpha(end/2 + 1 : end*3/4);
% llr_3 = 2*atanh(prod(tanh(alpha_3/2)));
llr_3 = prod(sign(alpha_3))*min(abs(alpha_3));

alpha_4 = alpha(end*3/4 + 1 : end);
% llr_4 = 2*atanh(prod(tanh(alpha_4/2)));
llr_4 = prod(sign(alpha_4))*min(abs(alpha_4));

%Even Parity check
check_bit = (llr_1 + llr_2 + llr_3 + llr_4) < 0;
%Wagner Decoder
x1 = alpha_1 < 0;
if mod(sum(x1), 2) ~= check_bit
    x1(abs(alpha_1) == min(abs(alpha_1))) = mod(x1(abs(alpha_1) == min(abs(alpha_1))) + 1, 2);
end

x2 = alpha_2 < 0;
if mod(sum(x2), 2) ~= check_bit
    x2(abs(alpha_2) == min(abs(alpha_2))) = mod(x2(abs(alpha_2) == min(abs(alpha_2))) + 1, 2);
end

x3 = alpha_3 < 0;
if mod(sum(x3), 2) ~= check_bit
    x3(abs(alpha_3) == min(abs(alpha_3))) = mod(x3(abs(alpha_3) == min(abs(alpha_3))) + 1, 2);
end

x4 = alpha_4 < 0;
if mod(sum(x4), 2) ~= check_bit
    x4(abs(alpha_4) == min(abs(alpha_4))) = mod(x4(abs(alpha_4) == min(abs(alpha_4))) + 1, 2);
end

x_tmp = [x1; x2; x3; x4];

x = x_tmp(:);

end


function [newRO, newRR] = mstep_update_rotation(P, S_bar, V, E_z, E_zz, RO, c, Tr)
%[newRO, newRR] = mstep_update_rotation(P, S_bar, V, E_z, E_zz, RO, Tr)

% Linearizes the expression in Eq 24 using exponential maps and
% solves for an improved rotation

% update step
tw_step = 0.3;

[K, T] = size(E_z);
J = size(S_bar, 2);

Pc = P - Tr(:)*ones(1,J);

newRR = zeros(2*T,3);
newRO = RO;

for iter=1:1,
   for t = 1:T,
      A = zeros(3);
      B = zeros(2,3);

      zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*K+1:t*K,:)];

      for j=1:J,
         H_j = [S_bar(:,j) reshape(V(:,j), 3, K)];

         A = A + H_j*zz_hat_t*H_j';

         B = B + ([Pc(t,j); Pc(t+T,j)] * [1 E_z(:,t)'] * H_j');
      end

      oldRO_t = RO{t};

      %%%%%%%%%%%%%%%%%%%%%%%%%%% Changed code here %%%%%%%%%%%%%%%%%%%%%
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %C = oldRO_t*A;
      %D = B - oldRO_t(1:2,:)*A;

      C = c(t,1)^2*oldRO_t*A;
      D = c(t,1)*B - c(t,1)^2*oldRO_t(1:2,:)*A;
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

      % now we solve the system: [1 0 0; 0 1 0]*twist*C = D
      CC = [0   C(3,1) -C(2,1)
         -C(3,1) 0 C(1,1)
         0   C(3,2) -C(2,2)
         -C(3,2) 0 C(1,2)
         0   C(3,3) -C(2,3)
         -C(3,3) 0 C(1,3)];

      DD = D(:);

      % twist optimization
      twist_vect = tw_step*pinv(CC)*DD;

      twh = [0      -twist_vect(3) twist_vect(2)
         twist_vect(3)  0      -twist_vect(1)
         -twist_vect(2) twist_vect(1)  0     ];
      dR = expm(twh);
      newRO_t = dR*oldRO_t;

      newRO{t} = newRO_t;
      newRR(t,:) = newRO_t(1,:);
      newRR(t+T,:) = newRO_t(2,:);
   end

   RO = newRO;
end


%
%
%this script produces the figures of the paper scattering_fractal_analysis
%september 2013
%Joan Bruna
%
%

close all;
%clear all;
%startup;

clear filt_opt;
clear scat_opt;
%set the maximum analysis scale
filt_opt.J = 14;
filt_opt.filter_format='fourier';
%size of realizations
N=2^(4+filt_opt.J);

%choose the wavelet
filt_opt.filter_type='selesnick_1d';
%filt_opt.filter_type='morlet_1d';
scat_opt.M=2;

scat_opt.path_margin= Inf;
[Wop, filters]=wavelet_factory_1d(N,filt_opt,scat_opt);

R=4;
options.Wop=Wop;
options.J=filt_opt.J;

poisson=@(alpha) (cumsum(double(rand(N,1) < N^(-alpha))));
dpoisson=@(alpha) ((double(rand(N,1) < N^(-alpha))));

[S{1},T{1},Tu{1},ex{1}] = scat_renorm_1d_scatnet(dpoisson, 0.6, options, R);

figure
plot_full_transfer(log2(T{1}));

oldopts.J=14;
oldopts.filters=selesnick_bis([N 1], oldopts);
oldopts.fullscatt=1;
oldopts.oversampling=oldopts.J;
[S{2},T{2},Tu{2},ex{2}] = scatt_renorm_estimation(dpoisson, 0.6, oldopts, R);

figure
plot_full_transfer(log2(T{2}));


function sigma = optimize_wrt_sigma(I, I1, options)
% set the left hand side (warp the hyperspectral image)
options.isRigid = 0;
I1 = transform(I1,eye(3),[1,1],1e-3,size(I1,2),size(I1,1),options);
options.isRigid = 1;

% configure the right hand side (transform the color image)
sigma_step = mean(options.s);
step_size = sigma_step;

pa = ParameterAnalysis();
pa.StepSizeMode = 2;
pa.NeighborhoodMode = 3;
pa.Algorithm = 'BrutalForce';
pa.BrutalForceDepth = 5;
list_params = {'sigma'};
range = [1e-6,4*mean(options.s)]';
fcn_run = @(options1) calc_val(options1,I,I1,options);

options1 = [];
options1.sigma = options.sigma;

options1 = pa.autoParamSelection(fcn_run, options1, list_params, ...
    step_size, range);
sigma = options1.sigma;

function val = calc_val(options1,I,I1,options)
sigma = options1.sigma;
T = create_T(options.degree, options.t);
val = eval_obj_fun(I, I1, T, options.s, sigma, options);


%  Figure 10.32      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% Fig. 10.32 Script for computation of the root locus of yaw response,
% with no compensation, direct feedback

f =[-0.0558   -0.9968    0.0802    0.0415;
    0.5980   -0.1150   -0.0318         0 ;
   -3.0500    0.3880   -0.4650         0 ;
         0    0.0805    1.0000         0] ;
g =[0.0073;
   -0.4750;
    0.1530;
         0];
h = [0     1     0     0];
j =[0];
% the equations of the actuator:

na=[0 10];
da=[1 10];
[fa,ga,ha,ja]=tf2ss(na,da);

% the equations of the aircraft with actuator:

[fp,gp,hp,jp]=series(fa,ga,ha,ja,f,g,h,j);
% the uncompensated rootlocus
 hold off; clf
 rlocus(fp,-gp,hp,jp);
 axis([-2, 2, -1.5, 1.5]);
 grid;
 title('Fig. 10.32 Root locus of the aircraft with positive feedback')


function [AR, lambdamax]=sim_genRndVARcoeffs(varargin)
% Generate random coefficients for a VAR model of given order and with a
% specified degree of interaction sparsity.
% Coefficients are drawn from a normal distribution with specified standard
% deviation (sigma).
%
% The function will repeatedly generate random models until it obtains a
% stable VAR process (all eigenvalues of the system matrix are less than or
% equal to 1 in magnitude).
%
%
% See Also: sim_genTVARcoeffs()
%
% References:
% [1] This function is adapted from Stefan Haufe's gen_ar_sech.m
%
% Author: Tim Mullen 2011-2013, SCCN/INC, UCSD.
% Email:  tim@sccn.ucsd.edu

% This function is part of the Source Information Flow Toolbox (SIFT)
%
% 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, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

g = arg_define(varargin,...
        arg({'nchs','NumChans'},10,[],'Number of channels (variables)'), ...
        arg({'morder','ModelOrder'},10,[],'Model order'), ...
        arg({'sigma','Sigma'},0.2,[0 Inf],'Scale (std) of random VAR parameters. This determines the variance of the determinstic component of the generated process.'), ...
        arg({'maxinter','MaxNumInter'},0.5,[],'Maximum number of interactions. If >= 1, this is the expected number of interacting variables (chosen randomly). If < 1, this represents the proportion of interacting variable (out of nchs^2-nchs max possible interactions). This can also be a vector specifying the linear (column-major) index of each nonzero (non-diagonal) element of the interaction (connectivity) matrix'), ...
        arg({'probinter','PerLagInteractProb'},1,[0 1],'Percent of nonzero AR coefficients for a given interaction. This applies to auto-interactions as well as cross-interactions. For instance, a value of 1 will generate nonzero gaussian iid AR filter coefficients. A value of 0.5 will generate a mixed sparse AR filter with 50% zero coefficients and remaining nonzero iid gaussian coefficients'), ...
        arg({'maxtrys','MaxAttempts','maxattempts'},Inf,[],'Max number of retries for stable model gen.'), ...
        arg({'maxlambda','MaxLambdaMag'},1,[0 1],'Largest allowable eigenvalue (magnitude) of system matrix. This determines the stability of the process. The smaller this value the more stable the process. All eigenvalues must be <= 1 in magnitude for a stable process.') ...
        );

% RandStream.setDefaultStream(RandStream('mt19937ar','seed',sum(100*clock)));

if g.maxinter < 1
    g.maxinter = ceil(g.maxinter*(g.nchs^2-g.nchs));
end

% generate canonical interaction indices
if length(g.maxinter) == 1
    inddiag     = linspace(1, g.nchs^2, g.nchs);
    indndiag    = setdiff(1:g.nchs^2, inddiag);
    per         = randperm(length(indndiag));
    indndiag    = indndiag(per);
    indndiag    = indndiag(1:g.maxinter);
    ind         = [inddiag indndiag];
else
    inddiag     = linspace(1, g.nchs^2, g.nchs);
    ind         = unique([inddiag g.maxinter]);
end
ind = sort(ind);
% generate interaction indices for all model orders
ind = tensorsum(0:g.nchs^2:g.nchs^2*(g.morder-1),ind);

% generate random coefficients for interacting variables
lambdamax = Inf; ntrys = 0;
while lambdamax > g.maxlambda && ntrys < g.maxtrys
    AR      = zeros(g.nchs,g.nchs*g.morder);
    AR(ind) = double(rand(length(ind), 1) <= g.probinter).*randn(length(ind), 1)*g.sigma;

%     for k=1:g.morder
%       aloc = zeros(g.nchs);
%       aloc(ind) = double(rand(length(ind), 1) < g.probinter).*randn(length(ind), 1)*g.sigma;
%       AR(:,:,(k-1)*g.nchs+1:k*g.nchs) = aloc;
%     end

    % check model stability
    E         = eye(g.nchs*g.morder);
    AA        = [AR;E(1:end-g.nchs,:)];
    lambda    = eig(AA);
    lambdamax = max(abs(lambda));

    ntrys = ntrys + 1;
end
if ntrys>=g.maxtrys
    fprintf('Unable to find a stable model. Maximum number of attempts exceeded\n');
    AR = [];
    return;
end


%DBINV Convert from decibels.
%
% (c) 2008-2011 Daniel Halperin <dhalperi@cs.washington.edu>
%
function ret = dbinv(x)
    ret = 10.^(x/10);
end


function [I,src] = rayMeasure(ray,Xmes,rad,rMax)
%+========================================================================+
%|                                                                        |
%|           OPENRAY - LIBRARY FOR TRI-DIMENSIONAL RAY TRACING            |
%|           openRay is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : rayMeasure.m                                  |
%|    #    |   VERSION    : 0.41                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 01.04.2018                                    |
%| ( === ) |   SYNOPSIS   : Spherical measurement                         |
%|  `---'  |                                                              |
%+========================================================================+

% Initialization
I   = cell(1,length(ray.pos)-1);
src = cell(length(ray.pos)-1,1);
dst = zeros(length(ray),1);

% Loop
for i = 1:size(I,2)
    % Ray positions
    Pi   = ray.pos{i};
    Pip1 = ray.pos{i+1};

    % Ray direction and length
    U   = (Pip1 - Pi);
    lgt = sqrt(sum(U.^2,2));
    U   = U ./ (lgt * [1 1 1]);

    % Initial position -> micro
    PM = ones(size(Pi,1),1)*Xmes - Pi;

    % Measure triangle (pythagore)
    hyp2 = sum(PM.^2,2);
    adj  = sum(PM.*U,2);
    opp2 = hyp2 - adj.^2;

    % Measured ray
    I{i} = find( (hyp2 < lgt.^2) & (adj >= 0) & (opp2 <= rad^2) & (dst + sqrt(hyp2) < rMax) );

    % Sources (focusing)
    if ~isempty(I{i})
        src{i} = Pi(I{i},:) - (dst(I{i})*[1 1 1]) .* U(I{i},:);
    else
        src{i} = zeros(0,3);
    end

    % Total distance from initial position
    dst = dst + lgt;
end
end


function c = tan(a)
% TAN for adiff objects.

c = adiff( tan(a.x), rowmult(1./cos(a.x).^2, a.dx), a.root);


function combo_test27 ( )

%*****************************************************************************80
%
%% COMBO_TEST27 tests PERM_INV and PERM_MUL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2011
%
%  Author:
%
%    John Burkardt
%
  n = 4;

  fprintf ( 1, ' \n' );
  fprintf ( 1, 'COMBO_TEST27\n' );
  fprintf ( 1, '  Permutations of the integers:\n' );
  fprintf ( 1, ' \n' );
  fprintf ( 1, '  PERM_INV computes an inverse permutation,\n' );
  fprintf ( 1, '  PERM_MUL multiplies two permutations.\n' );
%
%  Enumerate.
%
  nperm = perm_enum ( n );
%
%  Unrank.
%
  rank = floor ( nperm / 2 );

  p = perm_lex_unrank ( rank, n );

  perm_print ( n, p, '  The permutation P is ' );
%
%  Invert.
%
  q = perm_inv ( n, p );

  perm_print ( n, q, '  The inverse permutation Q is ' );
%
%  Multiply.
%
  r = perm_mul ( n, p, q );

  perm_print ( n, r, '  The product R = P * Q is ' );

  return
end


function [S] = lukfPropagation(dt,chi,chiAnt,omega_b,a_b,S,omega,acc,Qc,g)
%Left-UKF on Lie Groups
q = length(S); % state size
S_aug = blkdiag(S,Qc);
N_aug = length(S_aug);

%scaled unsented transform
W0 = 1-N_aug/3;
Wj = (1-W0)/(2*N_aug);
gamma = sqrt(N_aug/(1-W0));

% Prediction
ichi = invSE3(chi);
omega = omega-omega_b; % unbiased inputs
acc = acc-a_b;
X = gamma*[zeros(N_aug,1) S_aug' -S_aug'];% sigma-points
X(10:15,:) = X(10:15,:)+X(q+7:q+12,:)*dt; % add bias noise
for j = 2:2*N_aug+1
    xi_j = X([1:9,16:q],j); %do not take bias
    w_j = X(q+1:N_aug,j);
    omega_bj = X(10:12,j);
    a_bj = X(13:15,j);
    chi_j = chiAnt*exp_multiSE3(xi_j);
    Rot = chi_j(1:3,1:3)*expSO3((omega+w_j(1:3)-omega_bj)*dt);
    v = chi_j(1:3,4)+(Rot*(acc+w_j(4:6)-a_bj)+g)*dt;
    x = chi_j(1:3,5)+v*dt;
    chi = state2chi(Rot,v,x,chi_j(1:3,6:end));
    Xi_j = ichi*chi; % can be more time efficient
    X([1:9,16:q],j) = log_multiSE3(Xi_j); %propagated sigma points
end
X = sqrt(Wj)*X;
[~,Rs] = qr(X(1:q,2:2*N_aug+1)');
S = Rs(1:q,1:q);
end


clear;
clc
close all;

addpath('../../library/nav_lib');

%% attidude

true_attitude = deg2rad([4 -6 30])';
roll_meas_err = deg2rad(0.1);
pitch_meas_err = deg2rad(-0.15);

%% Earth's Magnetic Field
flux = 45;
declination= deg2rad(10);
inclination = deg2rad(40);

delta_alpha_nE= deg2rad(0.3);

%% Other Souces of Magnetism
MAn = [2 -1.8 3]'; % local magnetic anomalies(N,E,D)
hard = [-1 -0.5 1]'; % hard-iron magnetism(body-frame axes)
soft = [0.012 -0.014 0.006; -0.008 0.007 0.017; 0.003 -0.019 -0.011]; % soft-iron magnetism, body-rame-axes

%%
Cn2b = ch_eul2m(true_attitude);

%% flux desity of Earth's magnetic filed
m_En = [cos(declination)*cos(inclination)  sin(declination)*cos(inclination) sin(inclination)]'*flux;

%% Total magnetic flux density
m_mb = hard + (eye(3) + soft)*Cn2b*(m_En + MAn);

%% Calculate Magnetic Heading Measurement
roll_meas = true_attitude(1) + roll_meas_err;
pitch_meas =  true_attitude(2) +pitch_meas_err;

yaw = atan2(-m_mb(2)*cos(roll_meas) + m_mb(3)*sin(roll_meas), m_mb(1)*cos(pitch_meas) + m_mb(2)*sin(pitch_meas)*sin(roll_meas) + m_mb(3)*cos(roll_meas)*sin(pitch_meas));

%% Calculate True Heading
data_base_magnetic_declination = declination + delta_alpha_nE

%% True heading
true_heading = yaw+ data_base_magnetic_declination;
%rad2deg(true_heading)

%% Heading error
error = true_heading - true_attitude(3);
rad2deg(error)


% distance to wall
function [data,units] = compute_distnose2wall_animaldir_rect(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);

  xnose = trx(fly).x_mm + 2*trx(fly).a_mm.*cos(trx(fly).theta_mm);
  ynose = trx(fly).y_mm + 2*trx(fly).a_mm.*sin(trx(fly).theta_mm);
  nose = [xnose;ynose];
  body = [trx(fly).x_mm;trx(fly).y_mm ];
  tl = [trx.landmark_params{n}.tl_x(fly),trx.landmark_params{n}.tl_y(fly)];
  bl = [trx.landmark_params{n}.bl_x(fly),trx.landmark_params{n}.bl_y(fly)];
  tr = [trx.landmark_params{n}.tr_x(fly),trx.landmark_params{n}.tr_y(fly)];
  br = [trx.landmark_params{n}.br_x(fly),trx.landmark_params{n}.br_y(fly)];

  dtop = getDist(nose,body,tl,tr);
  dleft = getDist(nose,body,bl,tl);
  dright = getDist(nose,body,tr,br);
  dbottom = getDist(nose,body,br,bl);

  data{i} = min([dtop;dleft; dright; dbottom],[],1);

end
units = parseunits('mm');

function d = getDist(a,b,u,v)

x1=a(1,:);
x2=b(1,:);
x3=u(1);
x4=v(1);
y1=a(2,:);
y2=b(2,:);
y3=u(2);
y4=v(2);

ua = ((x4-x3)*(y1-y3)-(y4-y3)*(x1-x3))./((y4-y3)*(x2-x1)-(x4-x3)*(y2-y1));
x = x1 + ua.*(x2 - x1);
y = y1 + ua.*(y2 - y1);

side = sign((x4-x3)*(y1-y3)-(y4-y3)*(x1-x3));
d = sqrt( (x-x1).^2 + (y-y1).^2);
d = side.*d;

% sign of ua tell us if the intersection point was away from the
% the body or not. ua>0 means that the intersection point was towards the
% tail rather than towards the head.
% side tells us whether the nose was beyond the wall or not.
% ua>0 & side>0 means that nose was inside the wall and the intersection
% point was behind the mice. In this case set the dist to 0.
d(ua>0 & side>0) = inf;


% Author Deepak.
% Just provide the path of the image to add the noise or to restore the
% image.
clc;
f = imread('Place the path of your image file');
figure
imshow(f),title('Original Image')
[M N] = size(f);
% Any type of noise can b added to the image provided. Just see the
% imnoise2 file to see the various noise effects. Here an example is shown
% where we are adding the pepper noise to the image.
r = imnoise2('salt & pepper',M,N,0,0.1);
figure
imshow(r),title('Noise to be added');
c = find(r == 1);
gp = f;
gp(c) = 255;
figure
imshow(gp),title('Image after adding the Noise');
% The image distorted by adding the noise can be restored by using the
% function imrest. Here an example is shown by using contraharmonic filter.
% you can use various type of other filter to restore the image. For more
% detail just see the funcion imrest.
fp = imrest(gp,'chmean',3,3,-5.5);
figure
imshow(fp),title('Image after restoration.')


% spherror() - chancenter() sub function to compute minimum distance
%               of Cartesian coordinates to a sphere
%
% Author: Scott Makeig, CNL / Salk Institute, 2000

% Copyright (C) Scott Makeig, CNL / Salk Institute, 2000
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function wobble = spherror(center,x,y,z)

% 03/14/02 corrected wobble calculation -lf

x = x - center(1);  % center the points at (0,0,0)
y = y - center(2);
z = z - center(3);
radius = (sqrt(x.^2+y.^2+z.^2)); % distances from the center
wobble = std(radius-mean(radius)); % test if xyz values are on a sphere


function [Knots_n] = linear_3d_grid_from_displ(T, Spacing, volsz)
    ksz = ceil(volsz ./ Spacing) + 1;
    Knots_n =...
        T([1, 1+Spacing(1):Spacing(1):(ksz(1)-1)*Spacing(1), volsz(1)], ...
          [1, 1+Spacing(2):Spacing(2):(ksz(2)-1)*Spacing(2), volsz(2)], ...
          [1, 1+Spacing(3):Spacing(3):(ksz(3)-1)*Spacing(3), volsz(3)], :);
end


function [f,g,H,T] = autoTensor(x,useComplex,funObj,varargin)
% [f,g,H,T] = autoTensor(x,useComplex,funObj,varargin)
% Numerically compute Tensor of 3rd-derivatives of objective function from Hessian values

p = length(x);

if useComplex % Use Complex Differentials
    mu = 1e-150;

    diff = zeros(p,p,p);
    for j = 1:p
        e_j = zeros(p,1);
        e_j(j) = 1;
        [f(j) g(:,j) diff(:,:,j)] = funObj(x + mu*i*e_j,varargin{:});
    end
    f = mean(real(f));
    g = mean(real(g),2);
    H = mean(real(diff),3);
    T = imag(diff)/mu;
else % Use finite differencing
    mu = 2*sqrt(1e-12)*(1+norm(x))/norm(p);

    [f,g,H] = funObj(x,varargin{:});
    diff = zeros(p,p,p);
    for j = 1:p
        e_j = zeros(p,1);
        e_j(j) = 1;
        [junk1 junk2 diff(:,:,j)] = funObj(x + mu*e_j,varargin{:});
    end
    T = (diff-repmat(H,[1 1 p]))/mu;
end


% RANSACFITLINE - fits line to 3D array of points using RANSAC
%
% Usage  [L, inliers] = ransacfitline(XYZ, t, feedback)
%
% This function uses the RANSAC algorithm to robustly fit a line
% to a set of 3D data points.
%
% Arguments:
%          XYZ - 3xNpts array of xyz coordinates to fit line to.
%          t   - The distance threshold between data point and the line
%                used to decide whether a point is an inlier or not.
%          feedback - Optional flag 0 or 1 to turn on RANSAC feedback
%                     information.
%
% Returns:.
%           V - Line obtained by a simple fitting on the points that
%               are considered inliers.  The line goes through the
%               calculated mean of the inlier points, and is parallel to
%               the principal eigenvector.  The line is scaled by the
%               square root of the largest eigenvalue.
%               This line is a n*2 matrix.  The first column is the
%               beginning point, the second column is the end point of the
%               line.
%           L - The two points in the data set that were found to
%               define a line having the most number of inliers.
%               The two columns of L defining the two points.
%           inliers - The indices of the points that were considered
%                     inliers to the fitted line.
%
% See also:  RANSAC, FITPLANE, RANSACFITPLANE

% Copyright (c) 2003-2006 Peter Kovesi and Felix Duvallet (CMU)
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% Aug  2006 - created ransacfitline from ransacfitplane
%             author: Felix Duvallet

function [V, L, inliers] = ransacfit3Dline(XYZ, t, feedback)

    if nargin == 2
	feedback = 0;
    end

    [rows, npts] = size(XYZ);

    if rows ~=3
        error('data is not 3D');
    end

    if npts < 2
        error('too few points to fit line');
    end

    s = 2;  % Minimum No of points needed to fit a line.

    fittingfn = @defineline;
    distfn    = @lineptdist;
    degenfn   = @isdegenerate;

    [L, inliers] = ransac(XYZ, fittingfn, distfn, degenfn, s, t, feedback);

    % Find the line going through the mean, parallel to the major
    % eigenvector
    V = fitline3d(XYZ(:, inliers));

%------------------------------------------------------------------------
% Function to define a line given 2 data points as required by
% RANSAC.

function L = defineline(X);
    L = X;

%------------------------------------------------------------------------
% Function to calculate distances between a line and an array of points.
% The line is defined by a 3x2 matrix, L.  The two columns of L defining
% two points that are the endpoints of the line.
%
% A line can be defined with two points as:
%        lambda*p1 + (1-lambda)*p2
% Then, the distance between the line and another point (p3) is:
%        norm( lambda*p1 + (1-lambda)*p2 - p3 )
% where
%                  (p2-p1).(p2-p3)
%        lambda =  ---------------
%                  (p1-p2).(p1-p2)
%
% lambda can be found by taking the derivative of:
%      (lambda*p1 + (1-lambda)*p2 - p3)*(lambda*p1 + (1-lambda)*p2 - p3)
% with respect to lambda and setting it equal to zero

function [inliers, L] = lineptdist(L, X, t)

    p1 = L(:,1);
    p2 = L(:,2);

    npts = length(X);
    d = zeros(npts, 1);

    for i = 1:npts
        p3 = X(:,i);

        lambda = dot((p2 - p1), (p2-p3)) / dot( (p1-p2), (p1-p2) );

        d(i) = norm(lambda*p1 + (1-lambda)*p2 - p3);
    end

    inliers = find(abs(d) < t);

%------------------------------------------------------------------------
% Function to determine whether a set of 2 points are in a degenerate
% configuration for fitting a line as required by RANSAC.
% In this case two points are degenerate if they are the same point
% or if they are exceedingly close together.

function r = isdegenerate(X)
    %find the norm of the difference of the two points
    % this will be 0 iff the two points are the same (the norm of their
    % difference is zero)
    r = norm(X(:,1) - X(:,2)) < eps;


function [ net,res,opts ] = qrnn_bp( net,res,opts )
%QRNN_BP Summary of this function goes here
%   Detailed explanation goes here

    n_frames=opts.parameters.n_frames;
    n_hidden_nodes=opts.parameters.n_hidden_nodes;
    batch_size=opts.parameters.batch_size;

    %1: calculate the gradients of the data fitting transform
    opts.dzdy=res.Fit(numel(net{end}.layers)+1).dzdx;
    [net{3},res.Fit,opts] = net_bp(net{3},res.Fit,opts);

    %2: BPTT: calculate the gradient wrt the hidden nodes
    res.Fit(1).dzdx=reshape(res.Fit(1).dzdx,n_hidden_nodes,batch_size,n_frames);
    for f=n_frames-1:-1:1
        res.Fit(1).dzdx(:,:,f)=res.Fit(1).dzdx(:,:,f)+(1-res.Gates(end).x(:,:,f+1)).*res.Fit(1).dzdx(:,:,f+1);
    end

    res.Gates(end).x=reshape(res.Gates(end).x,n_hidden_nodes,[]);
    res.Input(end).x=reshape(res.Input(end).x,n_hidden_nodes,[]);
    res.Fit(1).dzdx=reshape(res.Fit(1).dzdx,n_hidden_nodes,[]);
    res.Hidden_diff=reshape(res.Hidden_diff,n_hidden_nodes,[]);
    %3: calculate the gradients of the input transform
    opts.dzdy=res.Fit(1).dzdx.*res.Gates(end).x;
    [ net{1},res.Input,opts ] = net_bp( net{1},res.Input,opts );

    %4: calculate the gradients of the gates
    opts.dzdy=res.Fit(1).dzdx.*res.Hidden_diff;
    [ net{1},res.Gates,opts ] = net_bp( net{1},res.Gates,opts );

end


function W = tess_smooth_sources(Vertices, Faces, VertConn, FWHM, Method)
% TESS_SMOOTH_SOURCES: Gaussian smoothing matrix over a mesh.
%
% USAGE:  W = tess_smooth_sources(Vertices, Faces, VertConn=[], FWHM=0.010, Method='average')
%
% INPUT:
%    - Vertices : Vertices positions ([X(:) Y(:) Z(:)])
%    - Faces    : Triangles matrix
%    - VertConn : Vertices connectivity, logical sparse matrix [Nvert,Nvert]
%    - FWHM     : Full width at half maximum, in meters (default=0.010)
%    - Method   : {'euclidian', 'path', 'average', 'surface'}
% OUPUT:
%    - W: smoothing matrix (sparse)
%
% DESCRIPTION:
%    - The distance between two points is an average of:
%        - the direct euclidian between the two points and
%        - the number of edges between the two points * the average length of an edge
%    - Gaussian smoothing function on the euclidian distance:
%      f(r) = 1 / sqrt(2*pi*sigma^2) * exp(-(r.^2/(2*sigma^2)))
%    - Full Width at Half Maximum (FWHM) is related to sigma by:
%      FWHM = 2 * sqrt(2*log2(2)) * sigma

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2010-2013

% ===== PARSE INPUTS =====
if (nargin < 5) || isempty(Method)
    Method = 'average';
end
if (nargin < 4) || isempty(FWHM)
    FWHM = 0.010;
end
if (nargin < 3) || isempty(VertConn)
    VertConn = tess_vertconn(Vertices, Faces);
end
if ~islogical(VertConn)
    error('Invalid vertices connectivity matrix.');
end
nv = size(Vertices,1);

% ===== ANALYZE INPUT =====
% Calculate Gaussian kernel properties
Sigma = FWHM / (2 * sqrt(2*log2(2)));
% FWTM = 2 * sqrt(2*log2(10)) * Sigma;
% Get the average edge length
[vi,vj] = find(VertConn);
meanDist = mean(sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2));
% Guess the number of iterations
nIter = min(10, ceil(FWHM / meanDist));

% ===== COMPUTE DISTANCE =====
switch lower(Method)
    % === METHOD 1: USE EUCLIDIAN DISTANCE ===
    case 'euclidian'
        % Get the neighborhood around each vertex
        VertConn = mpower(VertConn, nIter);
        [vi,vj] = find(VertConn);
        % Use Euclidean distance
        d = sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2);
        Dist = sparse(vi, vj, d, nv, nv);

    % === METHOD 2: USE NUMBER OF CONNECTIONS =====
    % === METHOD 3: AVERAGE METHOD 1+2 ===
    case {'path', 'average'}
        % Initialize loop variables
        VertConnGrow = speye(nv);
        VertIter = sparse(nv,nv);
        vall = [];

        % Grow and keep track of the layers
        for iter = 1:nIter
            disp(sprintf('SMOOTH> Iteration %d/%d', iter, nIter));
            % Grow selection of vertices
            VertConnPrev = VertConnGrow;
            VertConnGrow = double(VertConnGrow * VertConn > 0);
            % Find all the new connections
            vind = find(VertConnGrow - VertConnPrev > 0);
            [vi,vj] = ind2sub([nv,nv], vind);
            VertIter = VertIter + iter * sparse(vi, vj, ones(size(vi)), nv, nv);
        end

        % Use distance in number of connections
        Dist = VertIter * meanDist;
        Dist(1:nv+1:nv*nv) = 0;

        % == AVERAGE WITH METHOD 1 ==
        if strcmpi(Method, 'average')
            % Calculate Euclidean distance
            [vi,vj] = find(VertConnGrow);
            d = sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2);
            % Average with results of method #2
            Dist = (0.5 .* Dist + 0.5 .* sparse(vi, vj, d, nv, nv));
        end

    % ===== METHOD 4: CALCULATE SURFACE DISTANCE =====
    % WARNING: NOT FINISHED!!!!
    case 'surface'
        % Initialize loop variables
        VertConnGrow = speye(nv);
        Dist = sparse([], [], [], nv, nv, 3*nnz(VertConn));
        vall = [];
        nIter = 2;
        % Grow until we reach an accepteable distance
        for iter = 1:nIter
            disp(sprintf('Iteration %d', iter));
            % Get neighbors
            VertConnGrow = VertConnGrow * VertConn;
            % Get all the existing edges in the surface
            vind = find(VertConnGrow);
            % Remove all the previously processed connections
            vind = setdiff(vind, vall);
            [vi,vj] = ind2sub([nv,nv], vind);
            % Remove diagonal
            iDel = (vi == vj);
            vi(iDel) = [];
            vj(iDel) = [];
            % Calculate the distance to the neighbor nodes
            if (iter == 1)
                % Calculate all the distances for all the pairs of edges
                d = sqrt((Vertices(vi,1) - Vertices(vj,1)).^2 + (Vertices(vi,2) - Vertices(vj,2)).^2 + (Vertices(vi,3) - Vertices(vj,3)).^2);
            else
                % Initialize d matrix
                d = zeros(size(vi));
                % Process each new connection separately
                for i = 1:length(vi)
                    % Find nodes that are connected to both nodes
                    iMid = find((Dist(vi(i),:) & VertConn(vj(i),:)) | (VertConn(vi(i),:) & Dist(vj(i),:)));
                    % Find nodes for which we know one connection at least
                    iMid0 = (Dist(vi(i),iMid) & Dist(vj(i),iMid));
                    iMid1 = (Dist(vi(i),iMid) & ~iMid0);
                    iMid2 = (Dist(vj(i),iMid) & ~iMid0);
                    % Compute distances
                    dMid = 0*iMid;
                    dMid(iMid0) = Dist(vi(i),iMid0) + Dist(vj(i),iMid0);
                    dMid(iMid1) = Dist(vi(i),iMid1) + sqrt((Vertices(vj(i),1) - Vertices(iMid1,1)).^2 + (Vertices(vj(i),2) - Vertices(iMid1,2)).^2 + (Vertices(vj(i),3) - Vertices(iMid1,3)).^2)';
                    dMid(iMid2) = Dist(vj(i),iMid2) + sqrt((Vertices(vi(i),1) - Vertices(iMid2,1)).^2 + (Vertices(vi(i),2) - Vertices(iMid2,2)).^2 + (Vertices(vi(i),3) - Vertices(iMid2,3)).^2)';
                    dMid(dMid == 0) = Inf;
                    % Find the shortest path
                    d(i) = min(dMid);
                    if isinf(d(i))
                        error('???');
                    end
                end
            end
            % Add to processed vertices
            vall = union(vind, vall);
            % Create a sparse distance matrix
            Dist = Dist + sparse(vi, vj, d, nv, nv);
        end
end

% ===== APPLY GAUSSIAN FUNCTION =====
% Gaussian function
fun = inline('1 / sqrt(2*pi*sigma2) * exp(-(x.^2/(2*sigma2)))', 'x', 'sigma2');
% Calculate interpolation as a function of distance
[vi,vj] = find(Dist>0);
vind = sub2ind([nv,nv], vi, vj);
w = fun(Dist(vind), Sigma^2);
% Build final symmetric matrix
%W = sparse([vi;vj], [vj;vi], [w;w], nv, nv);
W = sparse(vi, vj, w, nv, nv);
% Add the diagonal
W = W + fun(0,Sigma^2) * speye(nv);
% Normalize columns
W = bst_bsxfun(@rdivide, W, sum(W,1));
% Remove insignificant values
[vi,vj] = find(W>0.005);
vind = sub2ind([nv,nv], vi, vj);
W = sparse(vi, vj, W(vind), nv, nv);

% ===== FIX BAD TRIANGLES =====
% Only for methods including neighbor distance
if ismember(lower(Method), {'path', 'average'})
    % Configurations to detect:
    %    - One face divided in 3 with a point in the middle of the face
    %    - Square divided into 4 triangles with one point in the middle
    % Calculate face-vertex connectivity
    VertFacesConn = tess_faceconn(Faces);
    % Find vertices connected to three or four faces
    sumVert  = sum(VertConn,2);
    sumFaces = sum(VertFacesConn,2);
    % Three/Four vertices: average the values of their neighbors
    iVert = find(((sumVert == 3) & (sumFaces == 3)) | ((sumVert == 4) & (sumFaces == 4)));
    AvgConn = bst_bsxfun(@rdivide, W * VertConn(:,iVert), sumVert(iVert)');
    W(iVert,:) = AvgConn';
    W(:,iVert) = AvgConn;
end


%% mri_sense_demo1.m
%|
%| Example illustrating regularized iterative reconstruction for parallel MRI
%| (sensitivity encoding imaging or SENSE), from nonuniform k-space samples.
%| (This example does not include field inhomogeneity or relaxation.)
%|
%| Copyright 2006-4-18, Jeff Fessler, University of Michigan

%% make coil sensitivity maps
if ~isvar('smap'), printm 'sense maps'
	ig = image_geom('nx', 64, 'fov', 250); % 250 mm FOV
	ig.mask = ig.circ(ig.dx * (ig.nx/2-2), ig.dy * (ig.ny/2-1)) > 0;
	f.ncoil = 4;
	smap = mri_sensemap_sim('nx', ig.nx, 'ny', ig.ny, 'dx', ig.dx, ...
		'rcoil', 120, 'ncoil', f.ncoil, 'chat', 1);
prompt
end

%% true object (discrete because of discrete sense map)
if ~isvar('xtrue'), printm 'true object'
	xtrue = ellipse_im(ig, 'shepplogan-emis', 'oversample', 2);
	clim = [0 8];
	im plc 2 2
	im(1, ig.x, ig.y, xtrue, '\x true', clim), cbar
	im(4, ig.mask, 'mask')
prompt
end

%% trajectory and system matrix
if ~isvar('Am'), printm 'system matrix A'
	f.traj = 'spiral1';
	f.dens = 'voronoi';

	N = [ig.nx ig.ny];
	[kspace, omega, wi_traj] = mri_trajectory(f.traj, {}, ...
		N, ig.fov, {f.dens});

	% create Gnufft class object
	J = [6 6];
	nufft_args = {N, J, 2*N, N/2, 'table', 2^10, 'minmax:kb'};
	Am = Gmri(kspace, ig.mask, ...
		'fov', ig.fov, 'basis', {'rect'}, 'nufft', nufft_args);

	if im
		im subplot 2
		plot(omega(1:5:end,1), omega(1:5:end,2), '.')
		axis_pipi, axis square
		titlef('%s: %d', f.traj, size(omega,1))
	end
prompt
end

wi_basis = wi_traj ./ Am.arg.basis.transform;

%% SENSE system object
if ~isvar('Ab'), printm 'Ab object with sense maps within'
	for ic=1:f.ncoil
		tmp = smap(:,:,ic);
		tmp = Gdiag(tmp(ig.mask), 'mask', ig.mask);
		Ac{ic} = Am * tmp; % cascade
	end
	Ab = block_fatrix(Ac, 'type', 'col'); % [A1; A2; ... ]
end

%% noisy data
if ~isvar('yi'), printm 'data yi'
	ytrue = Ab * xtrue(ig.mask);

	% add noise
	rng(0)
	yi = ytrue + 0 * randn(size(ytrue));
	% todo: visualize data...
end

%% CP recon
if ~isvar('xcp'), printm 'conj. phase reconstruction'
	xcp = zeros(ig.nx, ig.ny, f.ncoil, 'single');
	y4 = reshape(yi, [], f.ncoil);
	for ic=1:f.ncoil
		tmp = Am' * (wi_basis .* y4(:,ic));
		xcp(:,:,ic) = ig.embed(tmp);
	end

	im(2, abs(xcp), 'Conj. Phase Recon for each coil'), cbar
prompt
end

%% SSoS
if ~isvar('xssos'), printm 'sqrt sum-of-squares reconstruction'
	xssos = sqrt(sum(abs(xcp).^2, 3));
	xssos = ir_wls_init_scale(1, xtrue, xssos); % cheat trick for scaling
	im(3, abs(xssos), 'sum-of-squares recon'), cbar
	xlabelf('NRMSE %.1f%%', 100*nrms(xssos(:), xtrue(:)))
prompt
end

%% regularizer
if ~isvar('R'), printm 'regularizer'
	f.beta = 2^11;
	R = Reg1(ig.mask, 'beta', f.beta, 'type_penal', 'mat'); % complex
	if 1
		psf = qpwls_psf(Ab, R, 1, ig.mask, 1, 'offset', [0 0]);
		im(4, psf)
	end
prompt
end

%% PCG
if ~isvar('xpcg'), printm 'PCG with quadratic penalty'
	f.niter = 10;
	xpcg = qpwls_pcg(xssos(ig.mask), Ab, 1, yi(:), 0, R.C, 1, f.niter);
	xpcg = ig.embed(xpcg(:,end)); % convert last vector to image for display

	im(4, abs(xpcg), '|\x| PCG quad', clim), cbar
	xlabelf('NRMSE %.1f%%', 100*nrms(xpcg(:), xtrue(:)))
end


function [IX,IX1,IX2] = FindLinearIdx(x,y,lon,lat)
% given points in vectors x,y (np x 1) find their linear indices IX (np x 1)
% from a matrix of x-locations X and y-locations Y of both size nx columns and ny rows.
% lon and lat must be matrices created by ndgrid.
% kjr, 20171210 chl, und.
% wjp, 20201120 making sure dx and dy can be different
%      20200325 implementing method for irregular grid, cleaning up
ny = size(lon,1);
nx = size(lon,2);
np = numel(x);

if nx == 1 && ny == 1
   IX = 1; IX1 = 1; IX2 = 1;
   return
end

% make sure entry points are column vectors
x = x(:);
y = y(:);

% Get the grid spacing in x and y directions
% (trying both directions so could be meshgrid or ndgrid format)
dx  = diff(lon(:,1));
dy  = diff(lat(1,:));
dx_cutoff = 0.1/111e3; % approx 10 cm
if (max(dx) - min(dx)) > dx_cutoff || (max(dy) - min(dy)) > dx_cutoff
    % % IRREGULAR GRID (SLOWER)

    % convert ndgrid to vector
    lonV = reshape(lon(:,1),1,[]);
    % repeat the vector along the length of x
    LON = repmat(lonV,np,1);
    % find the closest lon to each point of x
    [~,IX1] = min(abs(x - LON),[],2);

    % convert ndgrid to vector
    latV = lat(1,:);
    % repeat the vector along the length of y
    LAT = repmat(latV,np,1);
    % find the closest lon to each point of y
    [~,IX2] = min(abs(y - LAT),[],2);

else
    % % REGULAR GRID (FASTER)
    dx = dx(1); dy = dy(1);

    IX1 = (x-lon(1,1))/dx + 1;
    IX2 = (y-lat(1,1))/dy + 1;

    IX1 = round(IX1);
    IX2 = round(IX2);

    IX1 = max([IX1,ones(np,1)],[],2);
    IX1 = min([IX1,ny*ones(np,1)],[],2);

    IX2 = max([IX2,ones(np,1)],[],2);
    IX2 = min([IX2,nx*ones(np,1)],[],2);

end

IX = sub2ind([ny,nx],IX1,IX2);

end


function x = r8row_to_r8vec ( m, n, a )

%*****************************************************************************80
%
%% R8ROW_TO_R8VEC converts an R8ROW into an R8VEC.
%
%  Example:
%
%    M = 3, N = 4
%
%    A =
%      11 12 13 14
%      21 22 23 24
%      31 32 33 34
%
%    X = ( 11, 12, 13, 14, 21, 22, 23, 24, 31, 32, 33, 34 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns.
%
%    Input, real A(M,N), the R8ROW.
%
%    Output, real X(M*N), a vector containing the M rows of A.
%
  j = 1;
  for i = 1 : m
    x(j:j+n-1) = a(i,1:n);
    j = j + n;
  end

  return
end


function graph( plain, stego, bit, datasig, window, N, L )

bits = str2num(reshape(bit, N, 1))';

figure('Name','Data Bits','NumberTitle','off')
stairs(bits,'linewidth',1);
axis([0 length(bit) -0.2 1.2]);
grid minor;

figure('Name','Direct Sequence Spread Spectrum','NumberTitle','off')
subplot(2,1,1);
plot(datasig); axis([0 N*L -2 2]); grid minor;
title('Spread spectrum with data bits');

subplot(2,1,2);
plot(window); axis([0 N*L -2 2]); grid minor;
title('Hanning windowed data signal');

figure('Name','Audio Signals','NumberTitle','off')
subplot(2,1,1);
plot(plain(1:N*L,1)); axis([0 N*L -2 2]);
title('Plain signal');

subplot(2,1,2);
plot(stego(1:N*L,1)); axis([0 N*L -2 2]);
title('Stego signal');

t = floor(N/2);
figure('Name',['Segment: ' num2str(t)],'NumberTitle','off')
subplot(2,1,1);
plot(plain(t*L+1:(t+1)*L, 1)); axis([0 L -2 2]);
title('Plain signal');

subplot(2,1,2);
plot(stego(t*L+1:(t+1)*L, 1)); axis([0 L -2 2]);
title('Stego signal');

end


% @FACTORIZE: an object-oriented method for solving linear system, solving
% least-square problems, and for efficient computation of mathematical
% expressions that use inv(A).
%
%   disp      - displays the factorization F
%   double    - returns the factorization as a single matrix, A or inv(A)
%   end       - returns index of last item for use in subsref
%   factorize - an object-oriented method for solving linear systems
%   inverse   - "inverts" F by flagging it as the factorization of inv(A).
%   mldivide  - x = A\b using the factorization F = factorize(A)
%   mrdivide  - x = b/A using the factorization F = factorize(A)
%   mtimes    - A*b, inv(A)*b, b*A, or b*inv(A), without computing inv(A)
%   size      - returns the size of the matrix F.A in the factorization F
%   subsref   - A(i,j) or (i,j)th entry of inv(A) if F is inverted.
%   plus      - update a dense Cholesky factorization
%   minus     - downdate a dense Cholesky factorization
%
% Example
%
%   F = factorize(A) ;
%   x = F\b ;           % same as x=A\b

% Copyright 2009, Timothy A. Davis, University of Florida


% example: edge-preserving smoothing
% figure 1 in our paper

close all;

I = double(imread('.\img_smoothing\cat.bmp')) / 255;
p = I;
r = 4; % try r=2, 4, or 8
eps = 0.2^2; % try eps=0.1^2, 0.2^2, 0.4^2

q = guidedfilter(I, p, r, eps);

figure();
imshow([I, q], [0, 1]);


function [ diff_median_mask]= diff_median_blk( In, win, thres)

p = 2*win + 1;

median_val = medfilt2(In, [p p]);

diff_median_mat = abs( median_val - In);

diff_median_mask= diff_median_mat >= ( thres* max( diff_median_mat(:)));


function [out] = snowfall_1(In,T,p1,varargin)
%snowfall_1

% 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:  Snowfall based on temperature threshold
% Constraints:  -
% @(Inputs):    p1   - temperature threshold below which snowfall occurs [oC]
%               T    - current temperature [oC]
%               In   - incoming precipitation flux [mm/d]
%               varargin(1) - smoothing variable r (default 0.01)

if size(varargin,2) == 0
    out = In.*(smoothThreshold_temperature_logistic(T,p1));
elseif size(varargin,2) == 1
    out = In.*(smoothThreshold_temperature_logistic(T,p1,varargin(1)));
end

end