function [azStart,dist,azEnd,latLonWaypoints]=indirectGreatCircleProb(latLonStart,latLonEnd,r,N,algorithm,waypointType,noJumps)
%%INDIRECTGREATCIRCLEPROB Solve the indirect great circle problem. That is,
%                      given two points on a spherical Earth, find the
%                      initial bearing and distance one must travel to
%                      take the shortest (geodesic) path between the
%                      points. Additionaly, this function can compute
%                      waypoints on the path. This type of navigation is
%                      sometimes referred to as "great circle sailing."
%                      When used for navigation, it is less accurate than
%                      an ellipsoidal Earth approximation.
%
%INPUTS: latLonStart The 2XnumPts set of numPts initial points given in
%               latitude and longitude in radians in the format
%               [latitude;longitude] (on a reference sphere, latitude is
%               spherical elevation; longitude is spherical azimuth). If
%               all these points are the same but latLonEnd varies, then a
%               single 2X1 vector can be passed. Extra rows, if passed,
%               will be ignored.
%     latLonEnd The 2XnumPts final points given in latitude and longitude
%               in adians in the format [latitude;longitude]. If all these
%               points are the same but latLonStart varies, then a single
%               2X1 vector can be passed.
%             r The assumed radius of the spherical Earth model. If omitted
%               or an empty matrix is passed, the default of
%               r=osculatingSpher4LatLon(latLonStart) is used.
%             N The number of waypoints, besides the initial and final
%               points on the trajectory, to produce. If omitted or an
%               empty matrix is passed, then no waypoints are generated and
%               latLonWaypoints is an empty matrix.
%     algorithm An optional parameter selecting the algorithm to use.
%               Possible values are:
%               0 Use the algorithm of [2], which avoids issues with
%                 singularities at the poles.
%               1 (The default if omitted or an empty matrix is passed and
%                 waypointType=1) Use the COFI algorithm of [1]. Latitudes
%                 within 2^24*eps(pi/2) of +/-pi/2 (the North and South
%                 poles) will be clipped to that bound. This avoids a
%                 singularity at the poles.
%               2 (The default if omitted or an empty matrix is passed and
%                 waypointType=0) Obtain angles using Equation 5-4b of [3],
%                 which is stable at the poles. Obtaincthe distance using a
%                 cross product formula, which is also used in algorithm 1,
%                 as discussed below.
%    waypointType An optional parameter indicating how the waypoints will
%                 be spaced. Possible values are:
%                 0 (The default if omitted or an empty matrix is passed)
%                   Space the N waypoints between the start and end
%                   uniformly in distance.
%                 1 Space the N waypoints between the start and end
%                   uniformly in longitude. This option is only available
%                   for algorithm 0. If a trajectory is meridional, then
%                   only the starting and stopping points will be returned.
%
%OUTPUTS: azStart The 1XnumPts scalar forward azimuths at the starting
%                 point in radians East of true North on the reference
%                 sphere.
%            dist The 1XnumPts distances on the sphere-approximated Earth
%                 between the starting and stopping points.
%           azEnd The 1XnumPts forward azimuth at the ending point in
%                 radians East of true North on the reference sphere.
% latLonWaypoints A 2X(N+2) set of waypoints on the trajectory.
%                 latLonWaypoints(:,i) is the ith points as
%                 [latitude;longitude] in radians. The first point is
%                 latLonStart and the final points is latLonEnd, though the
%                 points are made to avoid jumps of 2*pi in longitude,
%                 which can mean that the last point in this can be off
%                 from the provided last point by a factor of 2*pi. The
%                 other points are equally spaced along the trajectory. If
%                 the trajectory is meridional and waypointType=1, then
%                 only the starting and ending points will be returned.
%
%Great circles are geodesics on a sphere. A geodesic path between two
%points on a sphere is much simpler to determine as compared to an
%ellipsoid (which is what one gets using the indirectGeodeticProb
%function). For the spherical model, the latitude is taken as the elevation
%and the longitude as the azimuth on the sphere (we assume coordinate
%system 0 in the spher2Cart function). The latitudes and longitudes
%returned as waypoints by this function can be used to piece together an
%_approximate_ geodesic trajectory on the reference ellipsoid. One can
%obtain headings and distances between the reference points using rhumb
%lines.
%
%The azimuthal values returned when one of the points is at the pole
%depends on the longitude value given for the points. The getENUAxes
%function can return East-North-Up axes anywhere on the globe (for a
%sphere, the ellipsoidal flattening is 0). When given a pole, the result is
%the equivalent one would get by taking a point with the same latitude and
%with the latitude magnitude a tiny epsilon less than pi/2.
%
%The implementation of the COFI algorithm of [1] has been modified so that
%the angular difference D is obtained using the angBetweenVecs given F and
%T rather than the technique derived from the dot product relation starting
%in Equation 13 in [1]. This is because the cross product relation used in
%angBetweenVecs is more accurate. This is also used for the distance in
%algorithm 2. Minor changes had to be made to algorithm 1 to deal with
%trajectories going West, since the derivation in [1] is for East-bound
%trajectories.
%
%EXAMPLE 1:
%This is example 2 from [1]. It is a trajectory that crosses the
%international date and and goes from the Northern hemisphere to the
%southern hemisphere. We also compute the reverse path and show that the
%start and end azimuth angles produced in each direction are consistent
%with each other (when the same Earth radius is used eahc time). We then
%plot the trajectory on an image of the spherical Earth. For better
%plotting, the radius of the Earth has been normalized to 1.
% N=500;
% latStart=degMinSec2Rad(37,47.5);
% lonStart=degMinSec2Rad(-122,-27.8);
% latEnd=degMinSec2Rad(-33,-51.7);
% lonEnd=degMinSec2Rad(151,12.7);
%
% latLonStart=[latStart;lonStart];
% latLonEnd=[latEnd;lonEnd];
%
% [azStartFwd,distFwd,azEndFwd,latLonWayPoints]=indirectGreatCircleProb(latLonStart,latLonEnd,1,N);
% [azStartRev,distRev,azEndRev,latLonWayPointsRev]=indirectGreatCircleProb(latLonEnd,latLonStart,1,N);
%
% %All four of these should be approximately zero if the algorithm works
% %forwards and backwards:
% distFwd-distRev
% wrapRange(azStartFwd-wrapRange(azEndRev+pi,-pi,pi),-pi,pi)
% wrapRange(azStartRev-wrapRange(azEndFwd+pi,-pi,pi),-pi,pi)
% max(max(abs(wrapRange(latLonWayPoints-fliplr(latLonWayPointsRev),-pi,pi))))
%
% xStartCart=ellips2Cart([latLonStart;0],1,0);
% xEndCart=ellips2Cart([latLonEnd;0],1,0);
% %Give the points a nonzero elevation so the lines are easier to see.
% pathPoints=ellips2Cart([latLonWayPoints;0.02*ones(1,N+2)],1,0);
%
% figure(1)
% clf
% hold on
% plotMapOnEllipsoid([],1,0);
% scatter3(xStartCart(1),xStartCart(2),xStartCart(3),100,'filled')
% scatter3(xEndCart(1),xEndCart(2),xEndCart(3),100,'filled')
% plot3(pathPoints(1,:),pathPoints(2,:),pathPoints(3,:),'-r','linewidth',4)
% view([-75.368365161957513,3.494544111878512])
%
%EXAMPLE 2:
%This is the same as example 1, except one point is given at the North
%pole.
% N=500;
% latStart=pi/2;%North pole.
% lonStart=degMinSec2Rad(-122,-27.8);
% latEnd=degMinSec2Rad(-33,-51.7);
% lonEnd=degMinSec2Rad(151,12.7);
%
% latLonStart=[latStart;lonStart];
% latLonEnd=[latEnd;lonEnd];
%
% [azStartFwd,distFwd,azEndFwd,latLonWayPoints]=indirectGreatCircleProb(latLonStart,latLonEnd,1,N);
% [azStartRev,distRev,azEndRev,latLonWayPointsRev]=indirectGreatCircleProb(latLonEnd,latLonStart,1,N);
%
% %All four of these should be approximately zero if the algorithm works
% %forwards and backwards:
% distFwd-distRev
% wrapRange(azStartFwd-wrapRange(azEndRev+pi,-pi,pi),-pi,pi)
% wrapRange(azStartRev-wrapRange(azEndFwd+pi,-pi,pi),-pi,pi)
% max(max(abs(wrapRange(latLonWayPoints-fliplr(latLonWayPointsRev),-pi,pi))))
%
% xStartCart=ellips2Cart([latLonStart;0],1,0);
% xEndCart=ellips2Cart([latLonEnd;0],1,0);
% pathPoints=ellips2Cart([latLonWayPoints;0.02*ones(1,N+2)],1,0);
%
% figure(1)
% clf
% hold on
% plotMapOnEllipsoid([],1,0);
% scatter3(xStartCart(1),xStartCart(2),xStartCart(3),100,'filled')
% scatter3(xEndCart(1),xEndCart(2),xEndCart(3),100,'filled')
% plot3(pathPoints(1,:),pathPoints(2,:),pathPoints(3,:),'-r','linewidth',4)
%
%REFERENCES:
%[1] C.-L. Chen, P.-F. Liu, and W.-T. Gong, "A simple approach to great
%    circle sailing: The COFI method," The Journal of Navigation, vol. 67,
%    no. 3, pp. 403-418, May 2014.
%[2] D. F. Crouse, "Singularity-free great-circle sailing," Naval Research
%    Laboratory, Washington, DC, Tech. Rep. NRL/5340/MR-2021/4, 26
%    Jul. 2021.
%[3] J. P. Snyder, "Map projections-a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<7||isempty(noJumps))
    noJumps=true;
end

if(nargin<6||isempty(waypointType))
    waypointType=0;
end

if(nargin<5||isempty(algorithm))
    if(waypointType==1)
        algorithm=1;
    else
        algorithm=2;
    end
end

if(nargin<4)
    N=[];%Generate no waypoints.
end

if(nargin<3||isempty(r))
    r=osculatingSpher4LatLon(latLonStart);
end

if(nargout<3)
    N=[];%Do not generate waypoints.
end

numPts1=size(latLonStart,2);
numPts2=size(latLonEnd,2);
numPts=max(numPts1,numPts2);

if(numPts1>numPts2)
    latLonEnd=repmat(latLonEnd,[1,numPts]);
elseif(numPts2>numPts1)
    latLonStart=repmat(latLonStart,[1,numPts]);
end

azStart=zeros(1,numPts);
dist=zeros(1,numPts);
azEnd=zeros(1,numPts);
if(~isempty(N))
    latLonWaypoints=zeros(2,N+2,numPts);
else
    latLonWaypoints=[];
end

switch(algorithm)
    case 0
        if(waypointType~=0)
            error('This algorithm only supports waypointType=0.');
        end

        for k=1:numPts
            [azStart(k),dist(k),azEnd(k),latLonWaypointsCur]=indirectGreatCircleProbCrouse(latLonStart(1:2,k),latLonEnd(1:2,k),r,N);
            if(~isempty(N))
                latLonWaypoints(:,:,k)=latLonWaypointsCur;
            end
        end
    case 1
        for k=1:numPts
            [azStart(k),dist(k),azEnd(k),latLonWaypointsCur]=indirectGreatCircleProbChen(latLonStart(1:2,k),latLonEnd(1:2,k),r,N,waypointType);
            if(~isempty(N))
                latLonWaypoints(:,:,k)=latLonWaypointsCur;
            end
        end
    case 2
        for k=1:numPts
            [azStart(k),dist(k),azEnd(k),latLonWaypointsCur]=indirectGreatCircleProbSnyder(latLonStart(1:2,k),latLonEnd(1:2,k),r,N);
            if(~isempty(N))
                latLonWaypoints(:,:,k)=latLonWaypointsCur;
            end
        end
    otherwise
        error('Unknown algorithm chosen.')
end

if(~isempty(N)&&noJumps&&N>0)
    cumOffset=0;
    for k=1:numPts
        for j=2:(N+2)
            diffVal=latLonWaypoints(2,j)+cumOffset-latLonWaypoints(2,j-1);
            if(diffVal>pi)
                cumOffset=cumOffset-2*pi;
            elseif(diffVal<-pi)
                cumOffset=cumOffset+2*pi;
            end
            latLonWaypoints(2,j)=latLonWaypoints(2,j)+cumOffset;
        end
    end
end
end

function [azStart,dist,azEnd,latLonWayPoints]=indirectGreatCircleProbChen(latLonStart,latLonEnd,r,N,waypointType)
%%INDIRECTGREATCIRCLEPROBCHEN Solve the indirect great circle problem using
%                   the algorithm of [1], except the angular difference D
%                   is obtained using the angBetweenVecs given F and T.
%
%Starting latitude values within 2^24*eps(pi/2); of the pole will be
%clipped to be 2^24*eps(pi/2); away from the pole. This reduces problems
%with singularities at the poles at the cost of a loss of accuracy compared
%to elsewhere on the globe.
%
%REFERENCES:
%[1] C.-L. Chen, P.-F. Liu, and W.-T. Gong, "A simple approach to great
%    circle sailing: The COFI method," The Journal of Navigation, vol. 67,
%    no. 3, pp. 403-418, May 2014.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Latitude at the start.
LF=latLonStart(1);
%Deal with locations very close to the North or South pole by offsetting
%them just enough to avoid finite precision issues.
epsVal=2^24*eps(pi/2);
if(abs(LF-pi/2)<epsVal)
    LF=pi/2-epsVal;
elseif(abs(LF+pi/2)<epsVal)
    LF=pi/2+epsVal;
end

%Latitude at the destination.
LT=latLonEnd(1);

%Difference in longitude. If DLo is positive, then the shortest path is
%going East. If it is negative, then the shortest path is going west.
DLo=wrapRange(latLonEnd(2)-latLonStart(2),-pi,pi);

goingWest=(DLo<0);

sinLF=sin(LF);
cosLF=cos(LF);
sinLT=sin(LT);
cosLT=cos(LT);

lonStart=latLonStart(2);
cosLS=cos(lonStart);
sinLS=sin(lonStart);

lonEnd=latLonEnd(2);
cosLE=cos(lonEnd);
sinLE=sin(lonEnd);

%The Cartesian starting point (unit sphere).
F=[cosLS*cosLF;
   sinLS*cosLF;
   sinLF];

%The Cartesian ending point (unit sphere).
T=[cosLE*cosLT;
   sinLE*cosLT;
   sinLT];

%This is more accurate than using the cosine formula that is implicit in
%Equations 13 and 15 in [1].
D=angBetweenVecs(F,T);
if(D==0)
   %Special case: The starting and ending points are the same.
    azStart=NaN;%Undefined direction.
    dist=0;
    azEnd=NaN;%Undefined direction.
    latLonWayPoints=repmat(latLonStart,[1,N+2]);
    return
end

cosD=cos(D);
sinD=sin(D);

%From Equation 17 in [1].
cosC=(sinLT-sinLF*cosD)/(cosLF*sinD);

%Deal with finite precision issues.
cosC=min(1,max(-1,cosC));
C=acos(cosC);

%The sign adjustment for going west is not mentioned in [1].
%Alternatively, one could get sinC=cosLT*sin(DLo)/sinD; and then use the
%atan2 function.
if(goingWest)
    azStart=-C;
else
    azStart=C;
end
dist=D*r;

%%%%%%%%%%%%%
%%%%The Azimuth at the End
%%%%%%%%%%%%%

%To get the azimuth at the end, use Equation 17 in [1] switching the
%beginning and the end; this flips the sign of D, so the sign of sinD
%flips. Then, flip the direction of the result 180 degrees if we are going
%West.
cosCEnd=-(sinLF-sinLT*cosD)/(cosLT*sinD);
%Deal with finite precision issues.
cosCEnd=max(-1,min(1,cosCEnd));
azEnd=acos(cosCEnd);
if(goingWest)
    azEnd=-azEnd;
end

%%%%%%%%%%%%%
%%%%Waypoints
%%%%%%%%%%%%%
if(~isempty(N))
    %If waypoints are desired.
    latLonWayPoints=zeros(2,N+2);
    latLonWayPoints(:,1)=latLonStart;
    latLonWayPoints(:,N+2)=latLonEnd;
    if(waypointType==0)
        %Waypoints uniformly spaced in distance.
        step=D/(N+1);
        DFX=step:step:(step*N);

        cosDFX=cos(DFX);
        sinDFX=sin(DFX);

        %Equation 19 in [1].
        sinLX=sinLF.*cosDFX+cosLF.*sinDFX.*cosC;
        cosLX=sqrt(1-sinLX.^2);%Equivalent to cosLX=cos(asin(sinLX));

        %Equation 21 in [1].
        cosDLoFX=(cosDFX-sinLF.*sinLX)./(cosLF.*cosLX);

        %Deal with finite precision issues.
        cosDLoFX=max(-1,min(1,cosDLoFX));
        if(goingWest)
            lonPoints=wrapRange(latLonStart(2)-acos(cosDLoFX),-pi,pi);
        else
            lonPoints=wrapRange(latLonStart(2)+acos(cosDLoFX),-pi,pi);
        end
        latLonWayPoints(:,2:(N+1))=[asin(sinLX);lonPoints];
    else
        %Waypoints uniformly spaced in longitude.
        DLonFT=wrapRange(lonEnd-lonStart,-pi,pi);

        if(DLonFT==0)
            %Meridonal trajectory.
            latLonWayPoints=[latLonStart,latLonEnd];
            return
        end

        deltaSign=sign(DLonFT);

        step=DLonFT/(N+1);
        DLoFX=abs(step:step:(step*N));
        cosDLoFX=cos(DLoFX);
        sinDLoFX=sin(DLoFX);

        sinC=sin(C);

        %Equation 38 in [1].
        tanLX=(cosC*sinDLoFX+sinLF*sinC*cosDLoFX)/(cosLF*sinC);

        latLonWayPoints(:,2:(N+1))=[atan(tanLX);wrapRange(lonStart+deltaSign*DLoFX,-pi,pi)];
    end
else
    latLonWayPoints=[];
end
end

function [azStart,dist,azEnd,latLonWayPoints]=indirectGreatCircleProbCrouse(latLonStart,latLonEnd,r,N)
%%INDIRECTGREATCIRCLEPROBCROUSE Solve the indirect great circle problem
%               using the algorithm of [1].
%
%REFERENCES:
%[1] D. F. Crouse, "Singularity-free great-circle sailing," Naval Research
%    Laboratory, Washington, DC, Tech. Rep. NRL/5340/MR-2021/4, 26
%    Jul. 2021.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Convert the latitude-longitude values to unit vectors.
u1=spher2Cart([latLonStart(2);latLonStart(1)]);
u2=spher2Cart([latLonEnd(2);latLonEnd(1)]);

%Rotate the vectors both onto the equator.
[vec1Rot,vec2Rot,R]=rot2Vecs2CommonPlane(u1,u2);

%The geodesic goes along the equator in the rotated coordinate system in
%the tangent plane. We need to find the shortest direction around the
%equator. To do this, we shall get the azimuth angles.
az1=atan2(vec1Rot(2),vec1Rot(1));
az2=atan2(vec2Rot(2),vec2Rot(1));
DLo=wrapRange(az2-az1,-pi,pi);
goingWest=(DLo<0);

%The distance traveled between the points.
dist=abs(DLo)*r;

%Unit vectors in the traveling directions are thus either the positive or
%the negative of the East unit direction vector at the starting and ending
%points.
uENU1=getENUAxes([0;az1],false,1,0);
uENU2=getENUAxes([0;az2],false,1,0);

if(goingWest)
    uDir1=-uENU1(:,1);
    uDir2=-uENU2(:,1);
else
    uDir1=uENU1(:,1);
    uDir2=uENU2(:,1);
end

%Now, we shall undo the rotations to get the unit direction vectors in the
%original Cartesian coordinate system.
uDir1=R'*uDir1;
uDir2=R'*uDir2;

%Given the unit vectors in the original Cartesian coordinate system, we
%need to get the azimuth angle. The azimuth angle is in terms of degrees
%East of North, so we need to project into the East-North plane.
%Get the new set of ENU basis vectors in the  original coordinate system
uENU1=getENUAxes(latLonStart,false,1,0);
uENU2=getENUAxes(latLonEnd,false,1,0);

EProj=dot(uDir1,uENU1(:,1));
NProj=dot(uDir1,uENU1(:,2));
azStart=atan2(EProj,NProj);

EProj=dot(uDir2,uENU2(:,1));
NProj=dot(uDir2,uENU2(:,2));
azEnd=atan2(EProj,NProj);

if(~isempty(N))%If waypoints are desired.
    distPts=linspace(0,dist,N+2);
    latLonWayPoints=directGreatCircleProb(latLonStart,azStart,distPts,r);
    %This ensures that the longitudes match even if the starting and/or
    %stopping points are at the poles.
    latLonWayPoints(:,1)=latLonStart;
    latLonWayPoints(:,N+2)=latLonEnd;
else
    latLonWayPoints=[];
end
end

function [azStart,dist,azEnd,latLonWayPoints]=indirectGreatCircleProbSnyder(latLonStart,latLonEnd,r,N)
%%INDIRECTGREATCIRCLEPRBSNYDER This function calls greatCircleDistance with
%   algorithm 0 selected, which should be the most stable solution. Then,
%   it calls greatCircleAzimuth with algorithm 2 selected, which is the
%   very simple formula in Equation 5-4b in [1].
%
%REFERENCES:
%[1] J. P. Snyder, "Map projections-a working manual," U.S. Geological
%    Survey, Tech. Rep. 1395, 1987.
%
%December 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Latitude at the start.
phi1=latLonStart(1);
%Latitude at the destination.
phi2=latLonEnd(1);

lambda1=latLonStart(2);
lambda2=latLonEnd(2);
cosPhi1=cos(phi1);
sinPhi1=sin(phi1);
cosPhi2=cos(phi2);
sinPhi2=sin(phi2);

cosLambda1=cos(lambda1);
sinLambda1=sin(lambda1);
cosLambda2=cos(lambda2);
sinLambda2=sin(lambda2);
deltaLambda=lambda2-lambda1;
cosDeltaLambda=cos(deltaLambda);
sinDeltaLambda=sin(deltaLambda);

%The Cartesian starting point (unit sphere).
F=[cosLambda1*cosPhi1;
   sinLambda1*cosPhi1;
   sinPhi1];
%The Cartesian ending point (unit sphere).
T=[cosLambda2*cosPhi2;
   sinLambda2*cosPhi2;
   sinPhi2];
dist=r*angBetweenVecs(F,T);

azStart=atan2(cosPhi2*sinDeltaLambda,cosPhi1*sinPhi2-sinPhi1*cosPhi2*cosDeltaLambda);
azEnd=atan2(-cosPhi1*sinDeltaLambda,cosPhi2*sinPhi1-sinPhi2*cosPhi1*cosDeltaLambda);
azEnd=wrapRange(azEnd+pi,-pi,pi);

if(~isempty(N))%If waypoints are desired.
    distPts=linspace(0,dist,N+2);
    latLonWayPoints=directGreatCircleProb(latLonStart,azStart,distPts,r);
    %This ensures that the longitudes match even if the starting and/or
    %stopping points are at the poles.
    latLonWayPoints(:,1)=latLonStart;
    latLonWayPoints(:,N+2)=latLonEnd;
else
    latLonWayPoints=[];
end
end

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


function y=bcrecur(a, n)
%
% Computation of the fractional difference coefficients
% by the recurrence relation
%           bc(j)=(1-(a+1)/j)*bc(j-1),
%  a - order of the fractional difference
%  n - required number of coefficients
%
%  Computation of bcrecur(k,k) takes (3*k+1) flops.
%
%  Copyright (C) Igor Podlubny
%  15 Nov 1994
%
%  See also:
%  [1] Podlubny, I.: Fractional Differential Equations.
%      Academic Press, San Diego, 1999, 368 pages, ISBN 0125588402.

y=cumprod([1, 1 - ((a+1) ./ (1:n))]);


function subpak_test08 ( )

%*****************************************************************************80
%
%% TEST08 tests FAC_DIV, FAC_GCD, FAC_LCM, FAC_MUL, FAC_TO_I4, I4_TO_FAC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2009
%
%  Author:
%
%    John Burkardt
%
  prime_num = 5;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST08\n' );
  fprintf ( 1, '  For products of prime factors:\n' );
  fprintf ( 1, '  FAC_DIV computes a quotient;\n' );
  fprintf ( 1, '  FAC_MUL multiplies;\n' );
  fprintf ( 1, '  FAC_LCM computes the LCM;\n' );
  fprintf ( 1, '  FAC_GCD computes the GCD;\n' );
  fprintf ( 1, '  I4_TO_FAC converts an integer;\n' );
  fprintf ( 1, '  FAC_TO_I4 converts to an integer.\n' );
  fprintf ( 1, '  FAC_TO_RAT converts to a ratio.\n' );

  i1 = 720;
  i2 = 42;

  npower1 = i4_to_fac ( i1, prime_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Representation of I1 = %d\n', i1 );
  fprintf ( 1, '\n' );

  fac_print ( prime_num, npower1 );

  npower2 = i4_to_fac ( i2, prime_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Representation of I2 = %d\n', i2 );
  fprintf ( 1, '\n' );

  fac_print ( prime_num, npower2 );

  npower3 = fac_lcm ( prime_num, npower1, npower2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  LCM of I1, I2:\n' );
  fprintf ( 1, '\n' );

  fac_print ( prime_num, npower3 );

  npower3 = fac_gcd ( prime_num, npower1, npower2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  GCD of I1, I2:\n' );
  fprintf ( 1, '\n' );

  fac_print ( prime_num, npower3 );

  npower3 = fac_mul ( prime_num, npower1, npower2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Product of I1, I2:\n' );
  fprintf ( 1, '\n' );

  fac_print ( prime_num, npower3 );

  npower3 = fac_div ( prime_num, npower2, npower1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quotient of I2 / I1:\n' );
  fprintf ( 1, '\n' );

  fac_print ( prime_num, npower3 );

  [ top, bot ] = fac_to_rat ( prime_num, npower3 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quotient as a rational: %d / %d\n', top, bot );

  return
end


 function x = ir_idct2(y)
%function x = ir_idct2(y)
%|
%| Compute 2D inverse DCT of many images
%| (matlab idct2 can handle only one image)
%|
%| in
%|	y [M N (many)]	2D (M by N) DCT coefficients for many images
%|
%| out
%|	x [M N (many)]	corresponding synthesized 2D images
%|
%| 2016 Anish Lahiri
%| 2016-08-024 modified by JF to include test and comments

if nargin < 1, ir_usage, end
if streq(y, 'test'), ir_idct2_test, return, end

[nx, ny, np] = size(y);
x = reshape(y, nx, []); % [M *]
x = idct(x); % [M *] 1D along 1st dim
x = permute(reshape(x, nx, ny, np), [2,1,3]); % [M N np]
x = reshape(x, ny, []); % [N *]
x = idct(x); % [N *] 1D along 2nd dim
x = permute(reshape(x, ny, nx, np), [2 1 3]); % [M N np]
x = reshape(x, size(y));

function ir_idct2_test
m = 8;
n = 10;
nrep = 100;
rng(0);
y = rand(m, n, nrep);
x1 = ir_idct2(y);
x2 = zeros(size(x1));
for ii=1:nrep
	x2(:,:,ii) = idct2(y(:,:,ii));
end
equivs(x1, x2)

y1 = ir_dct2(x1);
equivs(y, y1)

if im % show all the 2D basis functions
	n = 8;
	y = eye(n^2);
	y2 = reshape(y, n, n, []);
	x2 = ir_idct2(y2);
	im plc 1 2
	im(1, x2, '2D: correct')

	x1 = idct(y);
	x12 = reshape(x1, n, n, []);
	im(2, x12, '1D: incorrect')
end


function M = spherefactory(n, m, gpuflag)
% Returns a manifold struct to optimize over unit-norm vectors or matrices.
%
% function M = spherefactory(n)
% function M = spherefactory(n, m)
% function M = spherefactory(n, m, gpuflag)
%
% Manifold of n-by-m real matrices of unit Frobenius norm.
% By default, m = 1, which corresponds to the unit sphere in R^n. The
% metric is such that the sphere is a Riemannian submanifold of the space
% of nxm matrices with the usual trace inner product, i.e., the usual
% metric.
%
% Set gpuflag = true to have points, tangent vectors and ambient vectors
% stored on the GPU. If so, computations can be done on the GPU directly.
%
% See also: obliquefactory spherecomplexfactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Dec. 30, 2012.
% Contributors:
% Change log:
%
%   Oct. 8, 2016 (NB)
%       Code for exponential was simplified to only treat the zero vector
%       as a particular case.
%
%   Oct. 22, 2016 (NB)
%       Distance function dist now significantly more accurate for points
%       within 1e-7 and less from each other.
%
%   July 20, 2017 (NB)
%       Following conversations with Bruno Iannazzo and P.-A. Absil,
%       the distance function is now even more accurate.
%
%   Sep. 7, 2017 (NB)
%       New isometric vector transport available in M.isotransp,
%       contributed by Changshuo Liu.
%
%   April 17, 2018 (NB)
%       ehess2rhess: Used to compute projection of ehess, then subtract a
%       multiple of u (which is assumed tangent.) Now, similarly to what
%       happens in stiefelfactory, we first subtract the multiple of u from
%       ehess, then we project. Mathematically, these operations are the
%       same. Numerically, the former version used to be better because tCG
%       in trustregions had some drift near fine convergence. Now that the
%       drift in tCG has been fixed, it is reasonable to apply the
%       projection last, to ensure best tangency of the output.
%
%   July 18, 2018 (NB)
%       Added the inverse retraction (M.invretr) for the sphere.
%
%   Aug. 3, 2018 (NB)
%       Added GPU support: just set gpuflag = true.
%
%   Jan. 8, 2021 (NB)
%       Added tangent2ambient/tangent2ambient_is_identity pair.

    if ~exist('m', 'var') || isempty(m)
        m = 1;
    end
    if ~exist('gpuflag', 'var') || isempty(gpuflag)
        gpuflag = false;
    end

    % If gpuflag is active, new arrays (e.g., via rand, randn, zeros, ones)
    % are created directly on the GPU; otherwise, they are created in the
    % usual way (in double precision).
    if gpuflag
        array_type = 'gpuArray';
    else
        array_type = 'double';
    end

    if m == 1
        M.name = @() sprintf('Sphere S^%d', n-1);
    else
        M.name = @() sprintf('Unit F-norm %dx%d matrices', n, m);
    end

    M.dim = @() n*m-1;

    M.inner = @(x, d1, d2) d1(:)'*d2(:);

    M.norm = @(x, d) norm(d, 'fro');

    M.dist = @dist;
    function d = dist(x, y)

        % The following code is mathematically equivalent to the
        % computation d = acos(x(:)'*y(:)) but is much more accurate when
        % x and y are close.

        chordal_distance = norm(x - y, 'fro');
        d = real(2*asin(.5*chordal_distance));

        % Note: for x and y almost antipodal, the accuracy is good but not
        % as good as possible. One way to improve it is by using the
        % following branching:
        % % if chordal_distance > 1.9
        % %     d = pi - dist(x, -y);
        % % end
        % It is rarely necessary to compute the distance between
        % almost-antipodal points with full accuracy in Manopt, hence we
        % favor a simpler code.

    end

    M.typicaldist = @() pi;

    M.proj = @(x, d) d - x*(x(:)'*d(:));

    M.tangent = M.proj;

    M.tangent2ambient_is_identity = true;
    M.tangent2ambient = @(X, U) U;

    % For Riemannian submanifolds, converting a Euclidean gradient into a
    % Riemannian gradient amounts to an orthogonal projection.
    M.egrad2rgrad = M.proj;

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(x, egrad, ehess, u)
        rhess = M.proj(x, ehess - (x(:)'*egrad(:))*u);
    end

    M.exp = @exponential;

    M.retr = @retraction;
    M.invretr = @inverse_retraction;

    M.log = @logarithm;
    function v = logarithm(x1, x2)
        v = M.proj(x1, x2 - x1);
        di = M.dist(x1, x2);
        % If the two points are "far apart", correct the norm.
        if di > 1e-6
            nv = norm(v, 'fro');
            v = v * (di / nv);
        end
    end

    M.hash = @(x) ['z' hashmd5(x(:))];

    M.rand = @() random(n, m, array_type);

    M.randvec = @(x) randomvec(n, m, x, array_type);

    M.zerovec = @(x) zeros(n, m, array_type);

    M.lincomb = @matrixlincomb;

    M.transp = @(x1, x2, d) M.proj(x2, d);

    % Isometric vector transport of d from the tangent space at x1 to x2.
    % This is actually a parallel vector transport, see Ch. 5 in
    % http://epubs.siam.org/doi/pdf/10.1137/16M1069298
    % "A Riemannian Gradient Sampling Algorithm for Nonsmooth Optimization
    %  on Manifolds", by Hosseini and Uschmajew, SIOPT 2017
    M.isotransp = @(x1, x2, d) isometricTransp(x1, x2, d);
    function Td = isometricTransp(x1, x2, d)
        v = logarithm(x1, x2);
        dist_x1x2 = norm(v, 'fro');
        if dist_x1x2 > 0
            u = v / dist_x1x2;
            utd = u(:)'*d(:);
            Td = d + (cos(dist_x1x2)-1)*utd*u ...
                    -  sin(dist_x1x2)  *utd*x1;
        else
            % x1 == x2, so the transport is identity
            Td = d;
        end
    end

    M.pairmean = @pairmean;
    function y = pairmean(x1, x2)
        y = x1+x2;
        y = y / norm(y, 'fro');
    end

    M.vec = @(x, u_mat) u_mat(:);
    M.mat = @(x, u_vec) reshape(u_vec, [n, m]);
    M.vecmatareisometries = @() true;

    % Automatically convert a number of tools to support GPU.
    if gpuflag
        M = factorygpuhelper(M);
    end

end

% Exponential on the sphere
function y = exponential(x, d, t)

    if nargin == 2
        % t = 1
        td = d;
    else
        td = t*d;
    end

    nrm_td = norm(td, 'fro');
    y = x*cos(nrm_td) + td*sinxoverx(nrm_td);

end

% Retraction on the sphere
function y = retraction(x, d, t)

    if nargin == 2
        % t = 1;
        td = d;
    else
        td = t*d;
    end

    y = x + td;
    y = y / norm(y, 'fro');

end

% Given x and y two points on the manifold, if there exists a tangent
% vector d at x such that Retr_x(d) = y, this function returns d.
function d = inverse_retraction(x, y)

    % Since
    %   x + d = y*||x + d||
    % and x'd = 0, multiply the above by x' on the left:
    %   1 + 0 = x'y * ||x + d||
    % Then solve for d:

    d = y/(x(:)'*y(:)) - x;

end

% Uniform random sampling on the sphere.
function x = random(n, m, array_type)

    x = randn(n, m, array_type);
    x = x / norm(x, 'fro');

end

% Random normalized tangent vector at x.
function d = randomvec(n, m, x, array_type)

    d = randn(n, m, array_type);
    d = d - x*(x(:)'*d(:));
    d = d / norm(d, 'fro');

end


function value = p10_f ( dim_num, point_num, x )

%*****************************************************************************80
%
%% P10_F evaluates the integrand for problem 10.
%
%  Dimension:
%
%    DIM_NUM arbitrary.
%
%  Region:
%
%    0 <= X(1:DIM_NUM) <= 1
%
%  Integrand:
%
%    sum ( abs ( x(1:dim_num) - 0.5 ) )
%
%  Exact Integral:
%
%    DIM_NUM / 4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%    Thomas Patterson,
%    [Integral #4],
%    On the Construction of a Practical Ermakov-Zolotukhin
%    Multiple Integrator,
%    in Numerical Integration: Recent Developments, Software
%    and Applications,
%    edited by Patrick Keast, Graeme Fairweather,
%    D. Reidel, 1987, pages 269-290,
%    LC: QA299.3.N38.
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the dimension of the argument.
%
%    Input, integer POINT_NUM, the number of points.
%
%    Input, real X(DIM_NUM,POINT_NUM), the evaluation points.
%
%    Output, real VALUE(POINT_NUM), the integrand values.
%
  value(1:point_num) = 0.0;

  for point = 1 : point_num
    value(point) = sum ( abs ( x(1:dim_num,point) - 0.5 ) );
  end

  p10_i4 ( 'I', '#', point_num );

  return
end


%GSP_DEMO_GRAPH_TV Reconstruction of missing sample on a graph using TV
%
%   In this demo, we try to reconstruct missing sample of a piece-wise
%   smooth signal on a graph. To do so, we will minimize the well-known TV
%   norm defined on the graph.
%
%   For this example, you need the unlocbox. You can download it here:
%   http://unlocbox.sourceforge.net/download
%
%   We express the recovery problem as a convex optimization problem of the
%   following form:
%
%   ..   argmin   ||grad(x)||_1   s. t. ||Mx-b||_2 < epsilon
%
%   .. math:: arg \min_x  \|\nabla(x)\|_1 \text{ s. t. } \|Mx-b\|_2 \leq \epsilon
%
%   Where b represents the known measurements, M is an operator
%   representing the mask and $\epsilon$ is the radius of the l2 ball.
%
%   We set
%
%   * $f_1(x)=||\nabla x ||_1$
%     We define the prox of $f_1$ as:
%
%     .. prox_{f1,gamma} (z) = argmin_{x} 1/2 ||x-z||_2^2  +  gamma  ||grad(z)||_1
%
%     .. math:: prox_{f1,\gamma} (z) = arg \min_{x} \frac{1}{2} \|x-z\|_2^2 +  \gamma \| \nabla z \|_1
%
%   * $f_2$ is the indicator function of the set S define by $||Mx-b||_2 < \epsilon$
%     We define the prox of $f_2$ as
%
%     .. prox_{f2,gamma} (z) = argmin_{x} 1/2 ||x-z||_2^2  +  gamma i_S( x ),
%
%     .. math:: prox_{f2,\gamma} (z) = arg \min_{x} \frac{1}{2} \|x-z\|_2^2   + i_S(x) ,
%
%     with $i_S(x)$ is zero if x is in the set S and infinity otherwise.
%     This previous problem has an identical solution as:
%
%     .. argmin_{z} ||x - z||_2^2   s.t.  ||b - M z||_2 < epsilon
%
%     .. math:: arg \min_{z} \|x - z\|_2^2   \hspace{1cm} such \hspace{0.25cm} that \hspace{1cm} \|Mz-b\|_2 \leq \epsilon
%
%     It is simply a projection on the B2-ball.
%
%   Results
%   -------
%
%   .. figure::
%
%      Original signal on graph
%
%      This figure shows the original signal on graph.
%
%   .. figure::
%
%      Depleted signal on graph
%
%      This figure shows the signal on graph after the application of the
%      mask and addition of noise. Half of the vertices are set to 0.
%
%   .. figure::
%
%      Reconstructed signal on graph usign TV
%
%      This figure shows the reconstructed signal thanks to the algorithm.
%
%   Comparison with Tikhonov regularization
%   ---------------------------------------
%
%   We can also use the Tikhonov regularizer that will promote smoothness.
%   In this case, we solve:
%
%   ..   argmin   ||grad(x)||_2^2   s. t. ||Mx-b||_2 < epsilon
%
%   .. math:: arg \min_x \tau \|\nabla(x)\|_2^2 \text{ s. t. } \|Mx-b\|_2 \leq \epsilon
%
%   The result is presented in the following figure:
%
%   .. figure::
%
%      Reconstructed signal on graph using Tikhonov
%
%      This figure shows the reconstructed signal thanks to the algorithm.
%

% Author: Nathanael Perraudin
% Date: 4th March 2014

%% Initialisation
clear;
close all;
% Load toolbox
init_unlocbox();

%% Important parameters
% size of the graph for the demo
N = 250;
% probability of having a label on a vertex.
p = 0.1;
verbose = 1;    % verbosity level
sigma = 0.0;

%% Create a graph
%  paramgraph.distribute = 1;
% G = gsp_sensor(N, paramgraph);
G = gsp_sensor(N);
G = gsp_community(N);

% for a path with irregular weights and distances
% G = gsp_path(N);
% G = gsp_update_coordinates(G, G.coords + [rand(G.N, 1), zeros(G.N, 1)]);
% G = gsp_update_weights(G, G.W .* exp(-gsp_distanz(G.coords').^2));

G = gsp_adj2vec(G);
G = gsp_estimate_lmax(G);
G = gsp_compute_fourier_basis(G);

% x_0 = (1 + sign(G.U(:,4))) / 2;
x_0 = round((1 + sign(G.U(:,2))) * 2+(1 + sign(G.U(:,3))) +(1 + sign(G.U(:,4))) / 2);
% x_0 = round(linspace(0, 5, N))';
x_0 = G.info.node_com;

%% Set the labels
% create the mask
% ind_obs = boolean(full(sparse(1:4:G.N, 1, 1, G.N, 1)));
ind_obs = rand(G.N, 1) < p;
ind_unobs = not(ind_obs);

%applying the Mask to the data
x_obs = ind_obs.*(x_0+sigma*randn(G.N,1));

%% Method 1: Graph TV
% setting different parameter for the simulation
param_solver.verbose = verbose;  % display parameter
param_solver.tol = 1e-5;
param_solver.maxit = 4000;

tic;
sol_tv = gsp_regression_tv(G, ind_obs, x_obs, 0, param_solver);
toc

%% Method 2: Tikhonov
tic;
G2 = G;
G2.L = G.L^3;
%sol_tik = gsp_regression_tik(G, ind_obs, x_obs, 0, param_solver);
sol_tik = gsp_regression_tik(G2, ind_obs, x_obs, 0, param_solver);
toc

%% Method 3: My idea: inpaint using graph TV on one variable that is l-2
%% away from another one that is smooth
%%
% x, y are primal variables: z is the dual
%      min_x,y,z ||grad(x_all)||_1 + a/2||x-y||^2 + b/2 ||grad(y_all)||^2
%
% grad(x_all) = grad([x;x_obs]) = A*x + A_obs * x_obs = A*x - b
%
% only rows of gradient corresponding to unknown values used:
%      min_x,y,z ||Ax - b||_1  + a/2||x-y||^2 + b/2 ||Ay - b||^2
% Get rid of operator A by using dual variable:
%      min_x,y,z ||z - b||_1   + a/2||x-y||^2 + b/2 ||Ay - b||^2
% Think of all primal variables as one vector w = [x;y] containing ONLY
% the elements on the unlabeled nodes

% choose parameters for solving the problem:
alpha = .1;
beta = 1;

% compute the Diff matrix of the graph
G = gsp_adj2vec(G);

% operators based on the Diff
A_obs = G.Diff(:, ind_obs);
A_unobs = G.Diff(:, not(ind_obs));
n_unobs = nnz(not(ind_obs));
zeros_x = zeros(n_unobs, 1);
zeros_y = zeros_x;

% graph TV term is the l-1 on the dual:
b = - A_obs * x_obs(ind_obs);
f1_params.y = b;
f1.eval = @(z) norm(z - b, 1);
f1.prox = @(z, gamma) prox_l1(z, gamma, f1_params);
f1.L = @(w) A_unobs * w(1:n_unobs);
f1.Lt = @(z) [A_unobs' * z; zeros_y];
f1.norm_L = normest(A_unobs);
%f1.norm_L = sqrt(G.lmax);

% use prox for the second function
% w = [x;y], w0 = [x0;y0]
% prox_g(x) = ((1+g)x + gy)/(1+2g)
f2.prox = @(w, gamma) [ ((1+alpha*gamma) * w(1:n_unobs) + ...
    alpha*gamma     * w(n_unobs+1:end))/(1+2*alpha*gamma);...
    (alpha*gamma     * w(1:n_unobs) + ...
    (1+alpha*gamma) * w(n_unobs+1:end))/(1+2*alpha*gamma)];
f2.eval = @(w) alpha/2 * norm(w(1:n_unobs) - w(n_unobs+1:end))^2;

% use gradient for the last function
AtA = A_unobs' * A_unobs;
Atb = A_unobs' * b;
f3.grad = @(w) [zeros_x; beta * (AtA*w(n_unobs+1:end) - Atb)];
f3.eval = @(w) beta/2 * norm(A_unobs * w(n_unobs+1:end) - b)^2;
f3.beta = beta * normest(AtA);

%% solve the problem
param_solver.verbose = 1;
param_solver.algo = 'FBF_PRIMAL_DUAL';
param_solver.normalized_timestep = 0.99;
tic;
[sol_mine, info_sol] = solvep([sol_tv(not(ind_obs));  sol_tik(not(ind_obs))], {f1,f2,f3}, param_solver);
toc

sol_mine_x = x_obs;
sol_mine_x(ind_unobs) = sol_mine(1:n_unobs);
sol_mine_y = x_obs;
sol_mine_y(ind_unobs) = sol_mine(n_unobs+1:end);

%% Method 4: L-1 of Lx ||Lx||_1
tic;
param_solver = rmfield(param_solver, 'algo');
sol_Lx_l1 = gsp_regression_Lx_l1(G, ind_obs, x_obs, 0, param_solver);
toc

%% Compute the errors
ans_tv = round(sol_tv);
ans_tik = round(sol_tik);
ans_mine = round(sol_mine_x);
ans_Lxl1 = round(sol_Lx_l1);

err_tv      = nnz(ans_tv ~= x_0) / G.N
err_tik     = nnz(ans_tik ~= x_0) / G.N
err_mine    = nnz(ans_mine ~= x_0) / G.N
err_Lxl1    = nnz(ans_Lxl1 ~= x_0) / G.N

%% Print the result
paramplot.show_edges = 1;
val_lims = lin_map([-.05, 1.05], [min([x_0; sol_tv; sol_tik]), max([x_0; sol_tv; sol_tik])], [0, 1]);

%% Plot the original graph
if not(strcmp(G.type, 'path'))
    figure(1)
    paramplot.vertex_highlight = ind_obs;
    gsp_plot_signal(G, x_0, paramplot)
    caxis(val_lims);
    title('Original signal: highlighted known values')

    % % Let show depleted graph
    % figure(2)
    % gsp_plot_signal(G,depleted_graph_value,paramplot)
    % caxis([-1 1])
    % title('Measurement')
    % Let show the reconstructed graph
    figure(3)
    paramplot.vertex_highlight = (ans_tv ~= x_0);
    gsp_plot_signal(G, ans_tv, paramplot)
    caxis(val_lims)
    title('TV solution: highlighted mistakes')

    % Let show the reconstructed graph
    figure(4)
    paramplot.vertex_highlight = (ans_tik ~= x_0);
    gsp_plot_signal(G, ans_tik, paramplot)
    caxis(val_lims)
    title('Tikhonov solution: highlighted mistakes')

    % Let show the reconstructed graph
    figure(5)
    paramplot.vertex_highlight = (ans_mine ~= x_0);
    gsp_plot_signal(G, ans_mine, paramplot)
    caxis(val_lims)
    title('My solution: highlighted mistakes')

    % Let show the reconstructed graph
    figure(6)
    paramplot.vertex_highlight = (ans_Lxl1 ~= x_0);
    gsp_plot_signal(G, ans_Lxl1, paramplot)
    caxis(val_lims)
    title('||Lx||_1 solution: highlighted mistakes')

    %%
else strcmp(G.type, 'path')
    figure;
    plot(G.coords(:, 1), sol_tv)
    hold on;
    plot(G.coords(:, 1), sol_tik)
    plot(G.coords(:, 1), x_0);
    plot(G.coords(:, 1), sol_Lx_l1)
    plot(G.coords(ind_obs, 1), x_0(ind_obs), 'o');
    legend('TV', 'Tikhonov', '||Lx||_1', 'true');
end

figure; plot(sol_mine_x)
hold on; plot(sol_mine_y)


function p = normal2(mu, Sigma, dom)
%NORMAL2   Bivariate normal distribution.
%   NORMAL2(MU, SIGMA, DOMAIN) returns the joint probability density for two
%   variables whose means are in the vector MU and whose covariance is the 2x2
%   positive definite matrix SIGMA. The result is a chebfun2 defined on the
%   (finite) domain DOMAIN.
%
%   If DOMAIN is not supplied, the domain is taken to be large enough to make
%   the density essentially zero at the boundary.
%
%   Example:
%      p = cheb.normal2([1,0], [1 1;1 5]);
%      surf(p)

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

if ( any(eig(Sigma) < 0) || norm(Sigma - Sigma') > 0 )
    error('CHEB:NORMAL2:covariance:nonSymPosDef', ...
        'Covariance matrix must be symmetric positive definite.')
end

if nargin < 3
    sig = svd(Sigma);
    dom = [ mu(1)+4*[-sig(1) sig(1)], mu(2)+4*[-sig(1) sig(1)] ];
end

x = chebfun2(@(x,y) x,dom);
y = chebfun2(@(x,y) y,dom);
invSig = inv(Sigma);
x = x - mu(1);
y = y - mu(2);
z = invSig(1,1)*x.^2 + (invSig(1,2)+invSig(2,1))*x.*y + invSig(2,2)*y.^2;
const = 2*pi*sqrt(det(Sigma));

p = exp(-z/2)/const;

end


function [tscr] = triscr2(pp,tt)
%TRISCR2 calc. area-len. ratios for triangles in a 2-simplex
%triangulation in the two-dimensional plane.
%   [SCR2] = TRISCR2(VERT,TRIA) returns the area-len. ratios
%   where SCR2 is a T-by-1 vector, VERT is a V-by-2 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 triangle.
%
%   See also TRIAREA, TRIANG2, TRIBAL2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 17/01/2017

%--------------------------- compute signed area-len. ratios
    scal = 4.0 * sqrt(3.0) / 3.0;

    area = triarea(pp,tt) ;             % also error checks!

    lrms = sum((pp(tt(:,2),:)        ...
              - pp(tt(:,1),:)).^2,2) ...
         + sum((pp(tt(:,3),:)        ...
              - pp(tt(:,2),:)).^2,2) ...
         + sum((pp(tt(:,3),:)        ...
              - pp(tt(:,1),:)).^2,2) ;

    lrms =(lrms / 3.0) .^ 1.00 ;

    tscr = scal * area ./ lrms ;

end


function L = gaussianNoiseLogLikelihood(noise, mu, varsigma, y)

% GAUSSIANNOISELOGLIKELIHOOD Log likelihood of the data under the GAUSSIAN noise model.
% FORMAT
% DESC returns the log likelihood of a data set under the  Gaussian noise model.
% ARG noise : the noise structure for which the log likelihood is required.
% ARG mu : input mean locations for the log likelihood.
% ARG varSigma : input variance locations for the log likelihood.
% ARG y : target locations for the log likelihood.
%
% SEEALSO : gaussianNoiseParamInit, gaussianNoiseLikelihood, noiseLogLikelihood
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

N = size(mu, 1);
D = size(mu, 2);
varsigma = varsigma + noise.sigma2;
for i = 1:D
  mu(:, i) = mu(:, i) + noise.bias(i);
end
arg = (y - mu);
arg = arg.*arg./varsigma;

L = - 0.5*sum(sum(log(varsigma))) ...
    - 0.5*sum(sum(arg)) ...
    - 0.5*N*D*log(2*pi);


function [sigma,shrinkage]=cov1para(x,shrink)

% function sigma=cov1para(x)
% x (t*n): t iid observations on n random variables
% sigma (n*n): invertible covariance matrix estimator
%
% Shrinks towards one-parameter matrix:
%    all variances are the same
%    all covariances are zero
% if shrink is specified, then this value is used for shrinkage

% This version: 04/2014

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file is released under the BSD 2-clause license.

% Copyright (c) 2014, Olivier Ledoit and Michael Wolf
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright
% notice, this list of conditions and the following disclaimer in the
% documentation and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
% IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
% PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% de-mean returns
[t,n]=size(x);
meanx=mean(x);
x=x-meanx(ones(t,1),:);
%x = bsxfun(@minus,x,meanx);

% compute sample covariance matrix
sample=(1/t).*(x'*x);

% compute prior
%meanvar=mean(diag(sample));
meanvar = trace(sample)/n;
prior=meanvar*eye(n);

%if (nargin < 2 | shrink == -1) % compute shrinkage parameters

  % what we call p
  y=x.^2;
  phiMat=y'*y/t-sample.^2;
  phi=sum(sum(phiMat));

  % what we call r is not needed for this shrinkage target

  % what we call c
  %gamma=norm(sample-prior,'fro')^2;
  gamma = sample-prior;
  gamma = sum(gamma(:).^2);

  % compute shrinkage constant
  kappa=phi/gamma;
  shrinkage=max(0,min(1,kappa/t));

%else % use specified number
%  shrinkage=shrink;
%end

% compute shrinkage estimator
sigma=shrinkage*prior+(1-shrinkage)*sample;


function [A,b,x] = spikes(n,t_max)
%SPIKES Test problem with a "spiky" solution.
%
% [A,b,x] = spikes(n,t_max)
%
% Artificially generated discrete ill-posed problem.
%
% The solution x consists of a unit step at t = .5, and a pulse train
% of spikes of decrasing magnitude at t = .5, 1.5, 2.5, ...
%
% The parameter t_max is optional; its default value is 5.
% It controls the length of the pulse train.

% Per Christian Hansen, IMM, 04/21/97.

% Initialization.
if (nargin == 1), t_max = 5; end
del = t_max/n;

% Compute the matrix A.
[t,sigma] = meshgrid(del:del:t_max,del:del:t_max);
A = sigma./(2*sqrt(pi*t.^3)).*exp(-(sigma.^2)./(4*t));

% Compute the right-hand side b and the solution x.
if (nargout > 1)
  heights = 2*ones(t_max,1); heights(1) = 25;
  heights(2) = 9; heights(3) = 5; heights(4) = 4; heights(5) = 3;
  x = zeros(n,1); n_h = 1;
  peak = 0.5/t_max; peak_dist = 1/t_max;
  if (peak < 1)
    n_peak = round(peak*n); x(n_peak) = heights(n_h);
    x(n_peak+1:n) = ones(n-n_peak,1);
    peak = peak + peak_dist; n_h = n_h + 1;
  end
  while (peak < 1)
    x(round(peak*n)) = heights(n_h);
    peak = peak + peak_dist; n_h = n_h + 1;
  end
  b = A*x;
end


function a = covar2 ( m, n, b, a )

%*****************************************************************************80
%
%% COVAR2 returns the covariance matrix for a rectangular matrix.
%
%  Discussion:
%
%    M should be at least N, and B has maximal rank N.
%    The covariance matrix is defined as
%
%      A = inverse ( B' * B ).
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the order of B.
%    N <= M is required.
%
%    Input, real B(M,N), the matrix whose covariance matrix
%    is desired.
%
%    Output, real A(N,N), the covariance matrix of B.
%
%    Output, integer IERROR, error flag.
%    IERROR = 0 for no error,
%    IERROR is nonzero to mean that A is singular.
%
  if ( m < n )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVAR2 - Fatal error!\n' );
    fprintf ( 1, '  M < N\n' );o
    error ( 'COVAR2 - Fatal error!' );
  end
%
%  Compute B' * B.
%
  btb(1:n,1:n) = b(1:m,1:n)' * b(1:m,1:n);
%
%  Factor the matrix.
%
  [ btb, pivot, error ] = r8mat_gefa ( btb, n );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'COVAR2 - Fatal error!\n' );
    fprintf ( 1, '  The matrix B'' * B does not have full rank.\n' );
    fprintf ( 1, '  This means that B does not have rank N.\n' );
    error ( 'COVAR2 - Fatal error!' );
  end
%
%  Solve for A by solving the equations ( B' * B ) * A = I
%  one column at a time.
%
  a = identity ( n, n );

  job = 0;

  for j = 1 : n
    a(1:n,j) = r8mat_gesl ( btb, n, pivot, a(1:n,j), job );
  end

  return
end


function [deltaPoseNoisy] = poseSubNoisy(p_j , p_i, sigmaT, sigmaR, isUniform)
% pose_sub = (p_j , p_i) = inv(p_i) * p_j + noise
% returns the planar roto-translation that tranform pj in pi
% plus noise (sigmaT is the std of the noise on the cartesian components,
% sigmaR is the std of the noise added to the angle before wrapping)

if nargin < 5
   isUniform = 0; % Gaussian noise by default
end

if nargin == 2
  sigmaT = 0;
  sigmaR = 0;
end

p_j = p_j(:);
p_i = p_i(:);

rho_i = p_i(1:2);
rho_j = p_j(1:2);
th_i = p_i(3);
th_j = p_j(3);

R = rot2D(th_i);

delta_th = wrapToPi(th_j - th_i);

if(abs(delta_th) > pi)
    disp('Error in pose_sub')
    disp(delta_th)
end

deltaPose = [ R' * (rho_j   -  rho_i)
    delta_th  ];

if isUniform == 0 % Gaussian noise
    noise = [sigmaT*randn(2,1) ;  sigmaR*randn];
else % uniform noise in the interval [-sigma, + sigma]
    noise = [sigmaT*2*(rand(2,1)-0.5*ones(2,1)) ; sigmaR*2*(rand-0.5)];
end

deltaPoseNoisy = deltaPose + noise;
deltaPoseNoisy(3) = wrapToPi(deltaPoseNoisy(3)); % wrap angle


function [A,C] = fd_bilinear_coefficients(mn,mx,side,V)
  % FD_BILINEAR_COEFFICIENTS  Given a grid from minimum corner mn to maximum
  % corner mx, with `side` nodes on each side, compute a matrix `A` so that
  % `V=A*C` if `C` are the node centers.
  %
  % [A,C] = fd_bilinear_coefficients(mn,mx,side,V)
  %
  % Inputs:
  %   mn  3d position of minimum corner
  %   mx  3d position of maximum corner
  %   side  number of nodes in each dimension [x y]
  %   V  #V by 2 list of query points
  % Outputs:
  %   A  x*y by #V matrix
  %   C  x*y by 3 list of node center positions
  %

  [X,Y] = meshgrid( ...
    linspace(mn(1),mx(1),side(1)), ...
    linspace(mn(2),mx(2),side(2)));
  C = [X(:) Y(:)];
  r = (mx-mn)./(side-1);

  I = floor(bsxfun(@times,bsxfun(@rdivide,bsxfun(@minus,V,mn),mx-mn),side-1));
  A = sparse(size(V,1),prod(side));
  for x = 1:2
    for y = 1:2
      Ixy = sub2ind(side([2 1]),I(:,2)+x,I(:,1)+y);
      Sxy = C(Ixy,:);
      Axy = prod(bsxfun(@minus,r,abs(V-Sxy)),2)/prod(r);
      A = A + sparse((1:size(V,1))',Ixy,Axy,size(V,1),size(C,1));
    end
  end

end


function p = predictOneVsAll(all_theta, X)
%PREDICT Predict the label for a trained one-vs-all classifier. The labels
%are in the range 1..K, where K = size(all_theta, 1).
%  p = PREDICTONEVSALL(all_theta, X) will return a vector of predictions
%  for each example in the matrix X. Note that X contains the examples in
%  rows. all_theta is a matrix where the i-th row is a trained logistic
%  regression theta vector for the i-th class. You should set p to a vector
%  of values from 1..K (e.g., p = [1; 3; 1; 2] predicts classes 1, 3, 1, 2
%  for 4 examples)

m = size(X, 1);
num_labels = size(all_theta, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

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

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters (one-vs-all).
%               You should set p to a vector of predictions (from 1 to
%               num_labels).
%
% Hint: This code can be done all vectorized using the max function.
%       In particular, the max function can also return the index of the
%       max element, for more information see 'help max'. If your examples
%       are in rows, then, you can use max(A, [], 2) to obtain the max
%       for each row.
%

[c,p] = max(sigmoid(X*all_theta'),[],2);

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

end


function [x,minCost]=convexQuadProgIntegerNoConst2D(Q,b,nonNeg)
%%CONVEXQUADPROGINTEGERNOCONST2D Perform quadratic programming on a 2D
%       convex problem with no constraints except that the solutions must
%       be integers that are either unconstrained or are non-negative.
%       This functions solves the optimization problem
%         minimize_x x'*Q*x+2*b'*x
%         subject to x is a 2X1 set of integers (optionally non-negative
%                    integers).
%
%INPUTS: Q An 2X2 real, positive definite symmetric matrix.
%        b An 2X1 real vector.
%   nonNeg This is true if all the x elements should be non-negative and
%          false otherwise. The default if omitted or an empty matrix is
%          passed is false.
%
%OUTPUTS: x The 2X1 optimal solution (or an empty matrix if the optimal
%           cost is infinite).
%   minCost The cost of the optimal constrained solution. If the cost isn't
%           finite, then this is an empty matrix.
%
%One could implement the branch-and bound algorithm for large scale
%problems as in [1]. However, in the 2D case, there are so few branches, it
%isn't worth it. Instead, we simply branch as in [1] (with a
%simple modification for non-negative constraints) and we traverse all 8
%hypotheses and then choose the lowest cost one. In [1], when branching,
%one creates smaller Q and b submatrices to optimize over rather than
%redoing the entire problem with an added eualaity constraint. In the 2D
%case, fixing an index means that the subproblem is just optimizing a scalar
%quadratic equation. Thus, we don't need to explicitely form submatrices;
%we just directly evaluate the scalar solution to the quadratic.
%
%EXAMPLE 1:
%Here is a simple example. We get the unconstrained solution. Then, we get
%the integer constrained solution and see that the cost is higher. Finally,
%we get the integer constrained solution with non-negativity constraints.
%That is NOT just clipping the integer constrained solution to 0. To show
%that, we compute the cost of the clipped solution and we see that it is
%much larger than the solution returned by the function.
% Q=[32,  20;
%    20, 22];
% b=[-178;-1200];
% [xOpt,minCost]=convexQuadProgEqConst(Q,b)
% nonNeg=false;
% [xOptInt,minCostInt]=convexQuadProgIntegerNoConst2D(Q,b,nonNeg)
% nonNeg=true;
% [xNonNegInt,minCostNonNegInt]=convexQuadProgIntegerNoConst2D(Q,b,nonNeg)
% %We show that the zero-clipped solution is much higher than the one with
% %non-negativity constraints.
% xTest=max(xOptInt,0);
% testCost=xTest'*Q*xTest+2*b'*xTest
%
%EXAMPLE 2:
%One might be tempted the think that one can always obtain the globally
%optimal solution by solving the unconstrained problem and then just
%rouning the result or by just trying the 4 points obtained by taking the
%floor and ceiling of each element of the unconstrained solution. However,
%that is not the case. In this example, we show the unconstrained solution
%and then the optimal integer constrained differ by more than 1 index.
% Q=[25, 0.3;
%   0.3, 0.01];
% b=[-156.9;
%    -3.31];
% xUnconst=convexQuadProgEqConst(Q,b)
% [x,minCost]=convexQuadProgIntegerNoConst2D(Q,b)
% %Now, we show that the cost of just rouning up or down the non-integer part
% %of xUnconst does not produce a globally optimal solution. Only the first
% %element of xUnConst is not an integer, so we only need to consider points
% %rounding that one up or down (as opposed to 4 points rounding each
% %element).
% xJustRoundUp=ceil(xUnconst);
% costJustRoundUp=xJustRoundUp'*Q*xJustRoundUp+2*b'*xJustRoundUp
% xJustRoundDown=floor(xUnconst);
% costJustRoundDown=xJustRoundDown'*Q*xJustRoundDown+2*b'*xJustRoundDown
%
%REFERENCES:
%[1] F. Koerner, "An efficient branch and bound algorithm to solve the
%    quadratic integer programming problem," Computing, vol. 30, pp. 253-
%    260, Sep. 1983.
%
%October 2022 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(nonNeg))
    nonNeg=false;
end

xOpt=convexQuadProgEqConst(Q,b);

%Impose the non-negativity constraint. When branching on each value, if the
%non-negativity constraint was enforce,d both branches will be the same.
%However, we aren't going to bother checking to see whether a branch should
%be skipped, because there are so few branches.
if(nonNeg)
    xOpt=max(xOpt,0);
end

x=[];
minCost=Inf;

%Branch on the first index.
for x1=[floor(xOpt(1)),ceil(xOpt(1))]
    %The part of the cost function that depends only on x1 and not x2
    %(which is fixed).
    constCostTerm=2*b(1)*x1+Q(1,1)*x1^2;
    %The optimal solution for x2 given x1 and no constraints.
    x2Node=-(b(2)+Q(1,2)*x1)/(Q(2,2));
    if(nonNeg)
        %Apply a non-negativity constraint.
        x2Node=max(x2Node,0);
    end

    %Now, branch on the second index. With it fixed, we just evaluate the
    %cost (There is nothing else to optimize).
    for x2=[floor(x2Node),ceil(x2Node)]
        costCur=2*b(2)*x2+2*Q(1,2)*x1*x2+Q(2,2)*x2^2+constCostTerm;
        if(costCur<minCost)
            minCost=costCur;
            x=[x1;x2];
        end
    end
end

%Now branch on the second index.
for x2=[floor(xOpt(2)),ceil(xOpt(2))]
    %The part of the cost function that depends only on x2 and not x1
    %(which is fixed).
    constCostTerm=2*b(2)*x2+Q(2,2)*x2^2;
    %The optimal solution for x1 given x2 and no constraints.
    x1Node=-(b(1)+Q(1,2)*x2)/(Q(1,1));

    if(nonNeg)
        %Apply a non-negativity constraint.
        x1Node=max(x1Node,0);
    end

    %Now, branch on the first index.
    for x1=[floor(x1Node),ceil(x1Node)]
        costCur=2*b(1)*x1+Q(1,1)*x1^2+2*Q(1,2)*x1*x2+constCostTerm;
        if(costCur<minCost)
            minCost=costCur;
            x=[x1;x2];
        end
    end
end
end

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


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%
for i=1:size(X,1)
    tempMin=inf;
    for j=1:K
        temp =sum((centroids(j,:)-X(i,:)).^2);
        if temp<tempMin
           idx(i)=j;
           tempMin = temp;
        end
    end

end

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

end


%% newton raphson example
% Find the Darcy friction factor for pipe flow using the Colebrook
% equation.
fprintf('\n**************************************************\n')
fprintf('NON-LINEAR SYSTEM OF EQUATIONS - PIPE FLOW EXAMPLE\n')
fprintf('**************************************************\n')
%% References:
% * http://en.wikipedia.org/wiki/Darcy_friction_factor_formulae
% * http://en.wikipedia.org/wiki/Darcy%E2%80%93Weisbach_equation
% * http://en.wikipedia.org/wiki/Moody_chart
% * http://www.mathworks.com/matlabcentral/fileexchange/35710-iapwsif97-functional-form-with-no-slip
%% inputs:
p = 0.68; % [MPa] water pressure (100 psi)
dp = -0.068*1e6; % [Pa] pipe pressure drop (10 psi)
T = 323; % [K] water temperature
D = 0.10; % [m] pipe hydraulic diameter
L = 100; % [m] pipe length
roughness = 0.00015; % [m] cast iron pipe roughness
rho = 1./IAPWS_IF97('v_pT',p,T); % [kg/m^3] water density (988.1 kg/m^3)
mu = IAPWS_IF97('mu_pT',p,T); % [Pa*s] water viscosity (5.4790e-04 Pa*s)
Re = @(u) rho*u*D/mu; % Reynolds number
%% governing equations
% Use Colebrook and Darcy-Weisbach equation to solve for pipe flow.

% friction factor (Colebrook eqn.)
residual_friction = @(u, f) 1/sqrt(f) + 2*log10(roughness/3.7/D + 2.51/Re(u)/sqrt(f));
% pressure drop (Darcy-Weisbach eqn.)
residual_pressdrop = @(u, f) rho*u^2*f*L/2/D + dp;
% residuals
fun = @(x) [residual_friction(x(1),x(2)), residual_pressdrop(x(1),x(2))];
%% solve
x0 = [1,0.01]; % initial guess
fprintf('\ninitial guess: u = %g[m/s], f = %g\n',x0) % display initial guess
options = optimset('TolX',1e-12); % set TolX
[x, resnorm, f, exitflag, output, jacob] = newtonraphson(fun, x0, options);
fprintf('\nexitflag: %d, %s\n',exitflag, output.message) % display output message
%% results
fprintf('\nOutputs:\n')
properties = {'Pressure','Pressure-drop','Temperature','Diameter','Length', ...
    'roughness','density','viscosity','Reynolds-number','speed','friction'};
units = {'[Pa]','[Pa]','[C]','[cm]','[m]','[mm]','[kg/m^3]','[Pa*s]','[]','[m/s]','[]'};
values = {p*1e6,dp,T-273.15,D*100,L,roughness*1000,rho,mu,Re(x(1)),x(1),x(2)};
fprintf('%15s %10s %10s\n','Property','Unit','Value')
results = [properties; units; values];
fprintf('%15s %10s %10.4g\n',results{:})
%% comparison
% solve using Haaland
Ntest = 10;
u0 = linspace(x(1)*0.1, x(1)*10, Ntest); % [m/s]
Re0 = Re(u0);
f0 = (1./(-1.8*log10((roughness/D/3.7)^1.11 + 6.9./Re0))).^2;
u0 = sqrt(-dp/rho./(f0*L/2/D));
% plot
plot(u0, f0, '-', u0, x(2)*ones(1,Ntest), '--', x(1)*ones(1,Ntest), f0, '--')
grid
title('Pipe flow solution using Haaland equation.')
xlabel('water speed, u [m/s]'),ylabel('friction factor, f')
legend('f_{Haaland}',['f = ',num2str(x(2))], ['u = ',num2str(x(1)),' [m/s]'])
%% LSQ Curve Fitting
fprintf('\n**********************************************\n')
fprintf('LEAST SQUARES CURVE FITTING WITH NEWTONRAPHSON\n')
fprintf('**********************************************\n')
% independent variables
[x,y] = meshgrid(0:10,0:10);
% bivariate distribution
bivar = @(x1,x2,sig,u1,u2)1/2/pi/sig^2*exp(-1/2*(((x1-u1).^2+(x2-u2).^2)/sig));
sigma = 3; ux = 4; uy = 5; % std dev, x & y means
z = bivar(x,y,sigma,ux,uy); % dist
% plot
figure,contour(x,y,z),hold('all')
title('lsq curve-fitting bivariate pdf with newtonraphson')
xlabel('x-coord'),ylabel('y-coord') % axes titles
grid,colorbar % show colorbar and grid
z_meas = z + (2*rand(11)-1)/1e4; % measured data
% fitting function
lsqfitfun = @(c)z_meas-bivar(x,y,c(1),c(2),c(3));
% fit coefficients to fun
c0 = [1,2,3]; % initial guess
fprintf('\ninitial guess: sigma = %g, ux = %g, uy = %g\n',c0) % display initial guess
options = optimset('TolX',1e-12); % set TolX
[c, ~, ~, exitflag, output] = newtonraphson(lsqfitfun, c0, options);
fprintf('\nexitflag: %d, %s\n',exitflag, output.message) % display output message
fprintf('\ncurve-fit coefficients: sigma=%g, ux=%g, uy=%g\n',c)
lines = plot(repmat(c(2),1,11),0:10,'--',0:10,repmat(c(3),1,11),'--', ...
    c(2),c(3),'o');
set(lines,'LineWidth',2);
legend('bivariate distribution','u_x','u_y','center')


function [ grid_level, grid_point ] = cc_levels_constrained ( dim_num, ...
  q_max, alpha, level_min, level_max, grid_num, point_num )

%*****************************************************************************80
%
%% CC_LEVELS_CONSTRAINED: CC grids with constrained levels.
%
%  Discussion:
%
%    The constraint on the levels of the 1D Clenshaw Curtis rule in
%    spatial dimension I is:
%
%      LEVEL_MIN(I) <= LEVEL(I) <= LEVEL_MAX(I)
%
%    The constraint on the collection of levels making up a rule is:
%
%      Sum ( 1 <= I <= DIM_NUM ) ALPHA(I) * LEVEL(I) <= Q_MAX.
%
%    The relationship of level to order is roughly
%
%      ORDER = 2**LEVEL+1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 November 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, real Q_MAX, the maximum values of
%    Q, the sum of the weighted orders in each spatial coordinate.
%
%    Input, real ALPHA(DIM_NUM), the weight factors for
%    the orders in each spatial dimension.
%
%    Input, integer LEVEL_MIN(DIM_NUM), LEVEL_MAX(DIM_NUM), the minimum
%    and maximum values of the level of the 1D Clenshaw Curtis rule
%    in each spatial dimension.
%
%    Input, integer GRID_NUM, the number of Clenshaw Curtis
%    grids in the constraint set.
%
%    Input, integer POINT_NUM, the total number of points in the grids.
%
%    Output, integer GRID_LEVEL(DIM_NUM,GRID_NUM), contains, for each
%    grid, the level of the Clenshaw-Curtis rule in each dimension.
%
%    Output, real GRID_POINT(DIM_NUM,POINT_NUM), contains
%    a list of all the abscissas of all the rules, listed one grid at
%    a time.  If a point occurs in several grids, it will be listed
%    several times.
%
  point_num = 0;
  grid_num = 0;

  level_1d = [];
  more = 0;

  while ( 1 )

    [ level_1d, more ] = vector_constrained_next4 ( dim_num, alpha, level_min, ...
      level_max, level_1d, q_max, more );

    if ( ~more )
      break
    end

    order_1d = cc_level_to_order ( dim_num, level_1d );

    order_nd = prod ( order_1d(1:dim_num) );

    grid_point(1:dim_num,point_num+1:point_num+order_nd) = cc_grid ( dim_num, ...
      order_1d, order_nd );

    point_num = point_num + order_nd;

    grid_num = grid_num + 1;
    grid_level(1:dim_num,grid_num) = level_1d(1:dim_num);

  end

  return
end


function gauss_seidel_test02 ( )

%*****************************************************************************80
%
%% GAUSS_SEIDEL_TEST02 tests GAUSS_SEIDEL2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 November 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'GAUSS_SEIDEL_TEST02:\n' );

  it_num = 400;
  n = 20;

  x_exact = ( 1 : n )';

  l = sparse ( 2:n,   1:n-1, -1.0, n, n );
  d = sparse ( 1:n,   1:n,    2.0, n, n );
  u = sparse ( 1:n-1, 2:n,   -1.0, n, n );

  a = l + d + u;

  b = a * x_exact;

  x = zeros ( n, 1 );
  x_plot(1:n,1) = x;

  step = 1 : it_num + 1;
  e = nan ( it_num+1, 1 );
  xm = nan ( it_num+1, 1 );

  e(1,1) = ( norm ( a * x - b ) ).^2;

  for it = 1 : it_num

    x_new = gauss_seidel2 ( n, l, d, u, b, x );

    e(it+1,1) = ( norm ( a * x_new - b ) ).^2;
    x_plot(1:n,it+1) = x_new;
%
%  Display the error.
%
    figure ( 1 )
    plot ( step, log ( e ), 'm-*' )
    title ( 'Log (Error^2)' )
    xlabel ( 'Step' )
    ylabel ( 'Error' )
    grid
%
%  Display the motion.
%
    xm(it,1) = sum ( ( x_new(:) - x(:) ).^2 ) / n;

    figure ( 2 )
    plot ( step, log ( xm ), 'm-*' )
    title ( 'Log (Average generator motion)' )
    xlabel ( 'Step' )
    ylabel ( 'Energy' )
    grid
%
%  Update the solution
%
    x = x_new;

  end
%
%  Plot the evolution of the locations of the generators.
%
  figure ( 3 )

  y = ( 0 : it_num );
  for k = 1 : n
    plot ( x_plot(k,1:it_num+1), y )
    hold on;
  end
  grid on
  hold off;

  title ( 'Generator evolution.' );
  xlabel ( 'Generator positions' );
  ylabel ( 'Iterations' );

  return
end


function [ vX ] = SolveProxTvAdmm( vY, mD, paramLambda, numIterations )
% ----------------------------------------------------------------------------------------------- %
%[ vX ] = SolveProxTvAdmm( vY, mD, paramLambda, numIterations )
% Solves the Prox of the Total Variation (TV) Norm using Alternating
% Direction Method of Multipliers (ADMM) Method.
% Basically solves the problem given by:
% $$ \arg \min_{ x \in \mathbb{R}^{n} } \frac{1}{2} {\left\| x - y \right|}_{2}^{2} + \lambda {\left\| D x \right\|}_{1} $$
% Input:
%   - vX                -   input Vector.
%                           Initialization of the iterative process.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - mA                -   Input Matirx.
%                           The model matrix.
%                           Structure: Matrix (m X n).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - vB                -   input Vector.
%                           The model known data.
%                           Structure: Vector (m X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - vX                -   Output Vector.
%                           Structure: Vector (n X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
% References
%   1.  Wikipedia ADMM - https://en.wikipedia.org/wiki/Augmented_Lagrangian_method#Alternating_direction_method_of_multipliers.
% Remarks:
%   1.  Using vanilla ADMM with no optimization of the parameter or
%       smoothing.
%   2.  Matrix Factorization caching according to "Matrix Inversion Lemma"
%       (See S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein -
%       Distributed Optimization and Statistical Learning via the
%       Alternating Direction Method of Multipliers Page 28). Basically:
%       (mA.' * mA + paramRho * I)^(-1) = (1 / paramRho) + (1 / (paramRho * paramRho)) * mA.' * (I + (1 /
%       paramRho) * mA * mA.')^(-1) * mA
% Known Issues:
%   1.  A
% TODO:
%   1.  Pre calculate decomposition of the Linear System.
% Release Notes:
%   -   1.0.000     27/11/2019  Royi Avital
%       *   First realease version.
% ----------------------------------------------------------------------------------------------- %

paramRho = 5;

mI = eye(size(vY, 1));
mC = decomposition(mI + paramRho * (mD.' * mD), 'chol');

vX = vY;
vZ = ProxL1(mD * vX, paramLambda / paramRho);
vU = mD * vX - vZ;

for ii = 2:numIterations

    vX = mC \ (vY + (paramRho * mD.' * (vZ - vU)));
    vZ = ProxL1(mD * vX + vU, paramLambda / paramRho);
    vU = vU + mD * vX - vZ;

end

end

function [ vX ] = ProxL1( vX, lambdaFactor )

% Soft Thresholding
vX = max(vX - lambdaFactor, 0) + min(vX + lambdaFactor, 0);

end


% Butterworth IIR Filter
srate=1000; % sample rate
npts=2000; %npts in signal
Nyquist=srate/2; %Nyquist frequency
lpf=300; %low-pass frequency
order=5; %filter order
t=[0:npts-1]/srate; %time scale for plot
x=(rand(npts,1)*2)-1; % raw data from -1 to +1

[b,a]=butter(order,lpf/Nyquist); %create filter coefficients

filtered_data=filter(b,a,x); % filter using 'b' and 'a' coefficients

% Calculate FFT
fftpts=npts; % number points in FFT
hpts=fftpts/2; % half number of FFT points
binwidth=srate/fftpts;
f=[0:binwidth:srate-binwidth];
x_fft=abs(fft(x))/hpts; %scaled FFT of original signal
filtered_fft=abs(fft(filtered_data))/hpts; %scaled FFT of filtered signal

subplot(2,2,1)
plot(t,x);
title('Raw Time Series');
subplot(2,2,3)
plot(t,filtered_data);
title('Filtered Time Series');
xlabel('Time (s)');
subplot(2,2,2)
plot(f(1:hpts),x_fft(1:hpts));
title('Raw FFT');
subplot(2,2,4)
plot(f(1:hpts),filtered_fft(1:hpts));
title('Filtered FFT');
xlabel('Frequency (Hz)');


function [tfr,t,f] = tfrwv(x,t,N,trace);
%TFRWV	Wigner-Ville time-frequency distribution.
%	[TFR,T,F]=TFRWV(X,T,N,TRACE) computes the Wigner-Ville distribution
%	of a discrete-time signal X,
%	or the cross Wigner-Ville representation between two signals.
%
%	X     : signal if auto-WV, or [X1,X2] if cross-WV.
%	T     : time instant(s)          (default : 1:length(X)).
%	N     : number of frequency bins (default : length(X)).
%	TRACE : if nonzero, the progression of the algorithm is shown
%	                                 (default : 0).
%	TFR   : time-frequency representation. When called without
%	        output arguments, TFRWV runs TFRQVIEW.
%	F     : vector of normalized frequencies.
%
%	Example :
%	 sig=fmlin(128,0.1,0.4); tfrwv(sig);
%
%	See also all the time-frequency representations listed in
%	the file CONTENTS (TFR*)

%	F. Auger, May-August 1994, July 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  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., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin == 0),
 error('At least one parameter required');
end;
[xrow,xcol] = size(x);

if (nargin == 1),
 t=1:xrow; N=xrow ; trace=0;
elseif (nargin == 2),
 N=xrow ; trace=0;
elseif (nargin == 3),
 trace = 0;
end;

if (N<0),
 error('N must be greater than zero');
end;

[trow,tcol] = size(t);
if (xcol==0)|(xcol>2),
 error('X must have one or two columns');
elseif (trow~=1),
 error('T must only have one row');
elseif (2^nextpow2(N)~=N),
 fprintf('For a faster computation, N should be a power of two\n');
end;

tfr= zeros (N,tcol);
if trace, disp('Wigner-Ville distribution'); end;
for icol=1:tcol,
 ti= t(icol); taumax=min([ti-1,xrow-ti,round(N/2)-1]);
 tau=-taumax:taumax; indices= rem(N+tau,N)+1;
 tfr(indices,icol) = x(ti+tau,1) .* conj(x(ti-tau,xcol));
 tau=round(N/2);
 if (ti<=xrow-tau)&(ti>=tau+1),
  tfr(tau+1,icol) = 0.5 * (x(ti+tau,1) * conj(x(ti-tau,xcol))  + ...
                           x(ti-tau,1) * conj(x(ti+tau,xcol))) ;
 end;
 if trace, disprog(icol,tcol,10); end;
end;
tfr= fft(tfr);
if (xcol==1), tfr=real(tfr); end ;

if (nargout==0),
 tfrqview(tfr,x,t,'tfrwv');
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function F = Gaus_filter(I,sigma)

R = ceil(3*sigma);
for i = -R:R,
    for j = -R:R,
        M(i+ R+1,j+R+1) = exp(-(i*i+j*j)/2/sigma/sigma)/(2*pi*sigma*sigma);
    end
end
M = M/sum(sum(M));  % normalize
F = xconv2(I,M);


%% inpainting RobustPCA example: moon picture corrupted with some text
addpath('../');

% read image and add the mask
img = double(imread('moon.tif'))/255;
img = img(141:140+256, 51:50+256);
msk = zeros(size(img));
msk(65:192,65:192) = imresize(imread('text.png'), 0.5);
img_corrupted = img;
img_corrupted(msk > 0) = nan;

fprintf(1, '%d corrupted entries\n', nnz(isnan(img_corrupted)));

% create a matrix X from overlapping patches
ws = 16; % window size
no_patches = size(img, 1) / ws;
X = zeros(no_patches^2, ws^2);
k = 1;
for i = (1:no_patches*2-1)
    for j = (1:no_patches*2-1)
        r1 = 1+(i-1)*ws/2:(i+1)*ws/2;
        r2 = 1+(j-1)*ws/2:(j+1)*ws/2;
        patch = img_corrupted(r1, r2);
        X(k,:) = patch(:);
        k = k + 1;
    end
end

% apply Robust PCA
lambda = 0.02; % close to the default one, but works better
tic
[L, S] = RobustPCA(X, lambda, 1.0, 1e-5);
toc

% reconstruct the image from the overlapping patches in matrix L
img_reconstructed = zeros(size(img));
img_noise = zeros(size(img));
k = 1;
for i = (1:no_patches*2-1)
    for j = (1:no_patches*2-1)
        % average patches to get the image back from L and S
        % todo: in the borders less than 4 patches are averaged
        patch = reshape(L(k,:), ws, ws);
        r1 = 1+(i-1)*ws/2:(i+1)*ws/2;
        r2 = 1+(j-1)*ws/2:(j+1)*ws/2;
        img_reconstructed(r1, r2) = img_reconstructed(r1, r2) + 0.25*patch;
        patch = reshape(S(k,:), ws, ws);
        img_noise(r1, r2) = img_noise(r1, r2) + 0.25*patch;
        k = k + 1;
    end
end
img_final = img_reconstructed;
img_final(~isnan(img_corrupted)) = img_corrupted(~isnan(img_corrupted));

% show the results
figure;
subplot(2,2,1), imshow(img_corrupted), title('Corrupted image')
subplot(2,2,2), imshow(img_final), title('Recovered image')
subplot(2,2,3), imshow(img_reconstructed), title('Recovered low-rank')
subplot(2,2,4), imshow(img_noise), title('Recovered sparse')

fprintf(1, 'ws=%d\tlambda=%f\trank(L)=%d\tcard(S)=%d\terr=%f\n', ...
       ws, lambda, rank(L), nnz(S), norm(img - img_final, 'fro'));


function yval = r8poly_der_val ( n, poly_cof, xval )

%*****************************************************************************80
%
%% R8POLY_DER_VAL evaluates the derivative of a polynomial in standard form.
%
%  Discussion:
%
%    A polynomial in standard form, with coefficients POLY_COF(*),
%    may be written:
%
%      P(X) = POLY_COF(1)
%           + POLY_COF(2) * X
%           ...
%           + POLY_COF(N) * X**(N-1)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the polynomial.
%
%    Input, real POLY_COF(1:N), the polynomial coefficients.
%    POLY_COF(1) is the constant term, and POLY_COF(N) is the coefficient of
%    X**(N-1).
%
%    Input, real XVAL, a value where the derivative of the
%    polynomial is to be evaluated.
%
%    Output, real YVAL, the value of the derivative of the
%    polynomial at XVAL.
%
  yval = ( n - 1 ) * poly_cof(n);

  for i = n-1 : -1 : 2
    yval = yval * xval + ( i - 1 ) * poly_cof(i);
  end

  return
end


function pdf = truncated_normal_a_pdf ( x, mu, sigma, a )

%*****************************************************************************80
%
%% TRUNCATED_NORMAL_A_PDF evaluates the lower truncated Normal PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 August 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%
%    Input, real MU, S, the mean and standard deviation of the
%    parent Normal distribution.
%
%    Input, real A, the lower truncation limit.
%
%    Output, real PDF, the value of the PDF.
%
  alpha = ( a - mu ) / sigma;
  xi = ( x - mu ) / sigma;

  alpha_cdf = normal_01_cdf ( alpha );
  xi_pdf = normal_01_pdf ( xi );

  pdf = xi_pdf / ( 1.0 - alpha_cdf ) / sigma;

  return
end


function [c,s,r] = Givens(x,y)
%
%  computes c,s,r such that
%
%  [c  s] [x] = [r]
%  [-s c] [y]   [0]
%
%  with c*c + s*s = 1;
%
   if (y == 0),
      c = 1; s = 0; r = x;
   else
      if (abs(x) >= abs(y))
         t = y/x; r = sqrt(1 + t*t);
         c = 1/r;
         s = t*c;
         r = x*r;
      else
         t = x/y; r = sqrt(1 + t*t);
         s = 1/r;
         c = t*s;
         r = y*r;
      end
   end


function a = mirt_dctn(a)
%MIRT_DCTN N-D (multidimensional) discrete cosine transform.
%   B = MIRT_DCTN(A) returns the discrete cosine transform of A.
%   The array B is the same size as A and contains the
%   discrete cosine transform coefficients.
%
%   This function works much faster than Matlab standard
%   dct (for 1D case) or dct2 (for 2D case), and also allows the ND input.
%   The function takes advantage of 1) FFT 2) fast permutation through indicies
%   3) persistent precomputation (coefficients and indicies are precomputed during
%   the first run).
%
%   This transform can be inverted using MIRT_IDCTN.
%   Author: Andriy Myronenko, see www.bme.ogi.edu/~myron
%   revised on May 06, 2010
%
%   See also MIRT_IDCT.

persistent siz ww ind isreala;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check input
if (nargin == 0) || isempty(a) ,
    error('Insufficient input');
end

isreala=isreal(a);
ndim=ndims(a);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check for the row vector
transpose=0;
if (ndim==2) && (size(a,1)==1)
    transpose=1; a=a';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Check if the variable size has changed and we need to
%%% precompute weights and indicies

precompute=0;
if  ~exist('siz','var'),
    precompute=1;
elseif abs(numel(siz)-ndims(a))>0
    precompute=1;
elseif sum(abs(siz-size(a)),2)>0,
    precompute=1;
elseif isreala~=isreal(a),
    precompute=1;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Precompute weights and indicies
if precompute,
    siz=size(a);
    ndim=ndims(a);

    for i=1:ndim,
        n=siz(i);

        ww{i} = 2*exp(((-1i*pi)/(2*n))*(0:n-1)')/sqrt(2*n);
        ww{i}(1) = ww{i}(1) / sqrt(2);
        ind{i}=bsxfun(@plus, [(1:2:n) fliplr(2:2:n)]', 0:n:n*(prod(siz)/n-1));
        if (siz(i)==1), break; end;
    end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Actual multidimensional DCT. Handle 1D and 2D cases
%%% separately, because .' is much faster than shiftdim.

% check for 1D or 2D cases
if ndim==2
    if (min(siz)==1),
        a=dct(a,ww{1},ind{1});if transpose, a=a'; end;  % 1D case
    else a = dct(dct(a,ww{1},ind{1}).',ww{2},ind{2}).'; % 2D case
    end
else
    % ND case (3D and higher)
    for i=1:ndim
        a=reshape(dct(reshape(a,siz(1),[]),ww{i},ind{i}), siz); % run dct along vectors (1st dimension)
        siz=[siz(2:end) siz(1)];                   % circular shift of size array by 1
        a=shiftdim(a,1);                           % circular shift dimensions by 1
    end
end

function a=dct(a,ww,ind)
%DCT  Discrete cosine transform 1D (operates along first dimension)

isreala=isreal(a);
k=1; if ~isreala, ia = imag(a); a = real(a); k=2; end; % check if complex

% k=1 (if 'a' is real) and 2 (if 'a' is complex)
for i=1:k
     a=a(ind);     % rearange
     a = fft(a);  % ifft
     a = real(bsxfun(@times,  ww, a)); % multiply weights

    % check if the data is not real
    if ~isreala,
        if i==1, ra = a; a = ia; clear ia;  % proceed to imaginary part
        else a = complex(ra,a); end;        % finalize the output
    end

end


function [ vWm, vWc, scalingFactor ] = CalcSigmaPointsWeights( paramAlpha, paramBeta, paramKappa, stateOrder )
% ----------------------------------------------------------------------------------------------- %
% [ vWm, vWc, scalingFactor ] = CalcSigmaPointsWeights( paramAlpha, paramBeta, paramKappa, stateOrder )
%   Calculates the weights for the mean, covariance and scaling factor of
%   the Unscented Transform.
% Input:
%   - paramAlpha    -   Parameter Alpha.
%                       Influences how far the Sigma Points are form the
%                       mean.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: (0, 1].
%   - paramBeta     -   Parameter Beta.
%                       For Gaussian PDF the optimal value is 2.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: NA.
%   - paramKappa    -   Parameter Kappa.
%                       Influences how far the Sigma Points are form the
%                       mean.
%                       Type: 'Single' / 'Double'.
%                       Range: [0, inf).
%   - stateOrder    -   State Vector  Dimension.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
% Output:
%   - vWm           -   Mean Weights Vector.
%                       Weights used for calculation of the mean of the
%                       transformed data.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - vWc           -   Covariance Weights Vector.
%                       Weights used for calculation of the covariance of
%                       the transformed data.
%                       Structure: Vector (Column).
%                       Type: 'Single' / 'Double'.
%                       Range: (-inf, inf).
%   - scalingFctr   -   Scaling Factor.
%                       Parameter of the Generalized Unscented Transform
%                       which controls
%                       Equals to 'size(hF(mX(:, 1), 1)'.
%                       Structure: Scalar.
%                       Type: 'Single' / 'Double'.
%                       Range: {1, 2, ...}.
% References
%   1.  Unscented Transform (Wikipedia) - https://en.wikipedia.org/wiki/Unscented_transform.
%   2.  Lecture 5: Unscented Kalman Filter and General Gaussian Filtering (Simo Sarkka).
%   3.  Robot Mapping - Unscented Kalman Filter (Cyrill Stachniss).
% Remarks:
%   1.  This method of parameters is often called Generalized / Scaled
%       Unscented Kalman Filter.
% TODO:
%   1.  U.
% Release Notes:
%   -   1.0.000     24/08/2018  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

numSigmaPoints  = (2 * stateOrder) + 1;

vWm = zeros(numSigmaPoints, 1);
vWc = zeros(numSigmaPoints, 1);

paramLambda = ((paramAlpha * paramAlpha) * (stateOrder + paramKappa)) - stateOrder;

vWm(1)      = paramLambda / (stateOrder + paramLambda);
vWm(2:end)  = 1 / (2 * (stateOrder + paramLambda));
vWc(1)      = vWm(1) + (1 - (paramAlpha * paramAlpha) + paramBeta);
vWc(2:end)  = vWm(2:end);

scalingFactor = sqrt(stateOrder + paramLambda);

end


function distance_map_in_meters = distance_in_meters_cityscapes(...
    depth_map_in_meters, camera_parameters_file)
%DISTANCE_IN_METERS_CITYSCAPES  Compute air thickness, i.e. distance of depicted
%object from camera center, expressed in meters, as a dense map with same
%resolution as the image, using a dense depth map and intrinsic camera
%parameters as inputs.

% Retrieve relevant intrinsic camera parameters: focal length and optical
% center, both expressed in pixel coordinates.
[~, f_x, c_x, c_y] = camera_parameters_cityscapes(camera_parameters_file);

% Compute medium (i.e. air) thickness in meters from depth. The derivation of
% the formula in the final lines of code is based on similar triangles.

[height, width] = size(depth_map_in_meters);
[X, Y] = meshgrid(1:width, 1:height);

distance_map_in_meters = depth_map_in_meters .*...
    sqrt((f_x ^ 2 + (X - c_x) .^ 2 + (Y - c_y) .^ 2) / f_x ^ 2);

end


function a = combin ( alpha, beta, n )

%*****************************************************************************80
%
%% COMBIN returns the combinatorial matrix.
%
%  Formula:
%
%    If ( I = J ) then
%      A(I,J) = ALPHA + BETA
%    else
%      A(I,J) = BETA
%
%  Example:
%
%    N = 5, ALPHA = 2, BETA = 3
%
%    5 3 3 3 3
%    3 5 3 3 3
%    3 3 5 3 3
%    3 3 3 5 3
%    3 3 3 3 5
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is persymmetric: A(I,J) = A(N+1-J,N+1-I).
%
%    det ( A ) = ALPHA**(N-1) * ( ALPHA + N * BETA ).
%
%    LAMBDA(1:N-1) = ALPHA,
%    LAMBDA(N) = ALPHA + N * BETA.
%
%    The eigenvector associated with LAMBDA(N) is (1,1,1,...,1)/sqrt(N).
%
%    The other N-1 eigenvectors are simply any (orthonormal) basis
%    for the space perpendicular to (1,1,1,...,1).
%
%    A is nonsingular if ALPHA /= 0D+00 and ALPHA + N * BETA /= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Robert Gregory, David Karney,
%    Example 3.25,
%    A Collection of Matrices for Testing Computational Algorithms,
%    Wiley, New York, 1969, page 53,
%    LC: QA263.G68.
%
%    Donald Knuth,
%    The Art of Computer Programming,
%    Volume 1, Fundamental Algorithms, Second Edition,
%    Addison-Wesley, Reading, Massachusetts, 1973, page 36.
%
%  Parameters:
%
%    Input, real ALPHA, BETA, scalars that define A.
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a(1:n,1:n) = beta;

  for i = 1 :n
    a(i,i) = a(i:i) + alpha;
  end

  return
end


function [J grad] = nnCostFunction(nn_params, ...
                                   input_layer_size, ...
                                   hidden_layer_size, ...
                                   num_labels, ...
                                   X, y, lambda)
%NNCOSTFUNCTION Implements the neural network cost function for a two layer
%neural network which performs classification
%   [J grad] = NNCOSTFUNCTON(nn_params, hidden_layer_size, num_labels, ...
%   X, y, lambda) computes the cost and gradient of the neural network. The
%   parameters for the neural network are "unrolled" into the vector
%   nn_params and need to be converted back into the weight matrices.
%
%   The returned parameter grad should be a "unrolled" vector of the
%   partial derivatives of the neural network.
%

% Reshape nn_params back into the parameters Theta1 and Theta2, the weight matrices
% for our 2 layer neural network
Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
                 hidden_layer_size, (input_layer_size + 1));

Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
                 num_labels, (hidden_layer_size + 1));

% Setup some useful variables

m = size(X, 1);
X = [ones(size(X,1),1), X];

% You need to return the following variables correctly
J = 0;
Theta1_grad = zeros(size(Theta1));
Theta2_grad = zeros(size(Theta2));

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the code by working through the
%               following parts.
%
% Part 1: Feedforward the neural network and return the cost in the
%         variable J. After implementing Part 1, you can verify that your
%         cost function computation is correct by verifying the cost
%         computed in ex4.m
%
% Part 2: Implement the backpropagation algorithm to compute the gradients
%         Theta1_grad and Theta2_grad. You should return the partial derivatives of
%         the cost function with respect to Theta1 and Theta2 in Theta1_grad and
%         Theta2_grad, respectively. After implementing Part 2, you can check
%         that your implementation is correct by running checkNNGradients
%
%         Note: The vector y passed into the function is a vector of labels
%               containing values from 1..K. You need to map this vector into a
%               binary vector of 1's and 0's to be used with the neural network
%               cost function.
%
%         Hint: We recommend implementing backpropagation using a for-loop
%               over the training examples if you are implementing it for the
%               first time.
%
% Part 3: Implement regularization with the cost function and gradients.
%
%         Hint: You can implement this around the code for
%               backpropagation. That is, you can compute the gradients for
%               the regularization separately and then add them to Theta1_grad
%               and Theta2_grad from Part 2.
%

Z2 = X*Theta1';
A2 = sigmoid(Z2);

A2 = [ones(size(A2,1),1), A2];
Z3 = A2*Theta2';

A3 = sigmoid(Z3);

% Y = zeros(m, max(y(:)));
% Y(y,:)

Y = dummyvar(y);

J = sum(sum(-1.*Y.*log(A3)-(1-Y).*log(1-A3),2))/m;

reg = (sum(sum(Theta1(:,2:end).^2)) + sum(sum(Theta2(:,2:end).^2)))*lambda/(2*m);
J = J+reg;

delta3 = A3 - Y;

Theta2_grad = Theta2_grad + (delta3'*A2)/m;

delta2 = delta3*Theta2(:,2:end).*sigmoidGradient(Z2);

Theta1_grad = Theta1_grad+(delta2'*X)/m;

Theta1_grad(:,2:end) = Theta1_grad(:,2:end) + lambda*Theta1(:,2:end)/m;
Theta2_grad(:,2:end) = Theta2_grad(:,2:end) + lambda*Theta2(:,2:end)/m;
% -------------------------------------------------------------

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

% Unroll gradients
grad = [Theta1_grad(:) ; Theta2_grad(:)];

end


function variance = nakagami_variance ( a, b, c )

%*****************************************************************************80
%
%% NAKAGAMI_VARIANCE returns the variance of the Nakagami PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, the parameters of the PDF.
%    0.0 < B
%    0.0 < C
%
%    Output, real VARIANCE, the variance of the PDF.
%
  t1 = gamma ( c + 0.5 );
  t2 = gamma ( c );

  variance = b * b * ( 1.0 - t1 * t1 / ( c * t2 * t2 ) );

  return
end


function z = spadaptdemo
% SPADAPTDEMO   Simple example of dimension-adaptive interpolation
%    A 2D-example for dimension-adaptive multi-linear sparse grid
%    interpolation using the Chebyshev grid and vectorized processing
%    of the model function.
%
%    See also SPINTERP, SPVALS.

% Author : Andreas Klimke, Universitaet Stuttgart
% Version: 1.1
% Date   : September 29, 2003

% ------------------------------------------------------------
% Sparse Grid Interpolation Toolbox
% Copyright (c) 2006 W. Andreas Klimke, Universitaet Stuttgart
% Copyright (c) 2007-2008 W. A. Klimke. All Rights Reserved.
% See LICENSE.txt for license.
% email: klimkeas@ians.uni-stuttgart.de
% web  : http://www.ians.uni-stuttgart.de/spinterp
% ------------------------------------------------------------

% The Branin function is used here
f = inline(['(5/pi*x-5.1/(4*pi^2)*x.^2+y-6).^2 + 10*(1-1/(8*pi))*' ...
	    ' cos(x)+10']);

% Define objective box
range = [-5 10; 0 15];

% Define problem dimension
d = 2;

% Create full grid for plotting
gs = 33;
[X,Y] = meshgrid(linspace(range(1,1),range(1,2),gs),...
								 linspace(range(2,1),range(2,2),gs));

% Set options: Switch vectorized processing on.
options = spset('Vectorized', 'on','RelTol', 1e-2, ...
								'GridType', 'Chebyshev', ...
								'DimensionAdaptive', 'on', ...
								'DimadaptDegree', 1, ...
                'MinPoints', 10);

% Compute sparse grid weights over range
z = spvals(f, d, range, options);

% Compute inpterpolated values at full grid
ip = spinterp(z, X, Y);

% Plot original function, interpolation, and error
subplot(2,2,1);
mesh(X,Y,f(X,Y));
title('original');
axis tight;

subplot(2,2,2);
mesh(X,Y,ip);
title('interpolated');
axis tight;

subplot(2,2,3);
mesh(X,Y,abs(f(X,Y)-ip));
title('absolute error');
axis tight;

subplot(2,2,4);
plotindices(z);
title('resulting index sets');