function [lin_fun, pp] = RAWCRF(image_raw, image_jpg, N, threshold_outliers)
%
%       [lin_fun, pp] = RAWCRF(image_raw, image_jpg, N, threshold_outliers)
%
%
%
%        Input:
%           -image_raw:
%           -image_raw:
%           -N:
%           -threshold_outliers:
%
%        Output:
%           -lin_fun:
%           -pp:
%

if(isempty(image_raw))
    error('RAWCRF: a stack cannot be empty!');
end

if(isempty(image_jpg))
    error('RAWCRF: a stack_exposure cannot be empty!');
end

if(~exist('threshold', 'var'))
    threshold_outliers = 0.05;
end

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

threshold_outliers_inv = 1.0 - threshold_outliers;

col = size(image_raw, 3);

lin_fun = zeros(256, col);

err = -1;
N_max = 6;
thr = [threshold_outliers, threshold_outliers_inv];

if(N < 0)
    for N_tmp=1:N_max
        pp = RAWCRFn(image_raw, image_jpg, N_tmp, thr);

        imgOut = RemoveCRF(image_jpg, 'poly', pp);

        tmp_err = abs(imgOut - image_raw).^2;
        tmp_err = mean(tmp_err(:));

        if(err < 0)
            err = tmp_err;
            N = N_tmp;
        else
            if(tmp_err < err)
                err = tmp_err;
                N = N_tmp;
            end
        end

    end
else
    pp = RAWCRFn(image_raw, image_jpg, N, thr);
end

x_val = (0:255) / 255;

for i=1:col
    lin_fun(:, i) = polyval(pp(:, i), x_val);
end

end

function pp = RAWCRFn(image_raw, image_jpg, N, thr)
    col = size(image_raw, 3);

    pp = zeros(N + 1, col);

    bFlag = 0;

    %bDebug = 1;

    for i=1:col
        slice_raw = image_raw(:,:,i);
        slice_jpg = image_jpg(:,:,i);

        indx = find(slice_jpg > thr(1) & slice_jpg < thr(2));

        if(~isempty(indx))
            x = slice_jpg(indx);
            y = slice_raw(indx);

%
%             if(bDebug)
%                 figure(i);
%                 c = zeros(256,256);
%                 for k =1:length(x)
%                     tx = ClampImg(round(x(k) * 255) + 1, 1, 256);
%                     ty = ClampImg(round(y(k) * 255) + 1, 1, 256);
%                     c(256 - ty + 1, tx) = c(256 - ty + 1, tx) + 1;
%                 end
%                 imshow(c);
%             end

            pp(:, i) = polyfit(x, y, N);
        else
            bFlag = 1;
            disp('RAWCRF: no enough data for estimating the CRF');
        end
    end

    if(bFlag)
        pp = zeros(N + 1, col);
    end
end


function legendre_polynomial_test08 ( )

%*****************************************************************************80
%
%% LEGENDRE_POLYNOMIAL_TEST08 tests PMN_POLYNOMIAL_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 March 2012
%
%  Author:
%
%    John Burkardt
%
  mm = 1;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LEGENDRE_POLYNOMIAL_TEST08:\n' );
  fprintf ( 1, '  PMN_POLYNOMIAL_VALUES stores values of\n' );
  fprintf ( 1, '  the Legendre polynomial Pmn(n,m,x).\n' );
  fprintf ( 1, '  PMN_POLYNOMIAL_VALUE evaluates the polynomial.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '                                Tabulated                 Computed\n' );
  fprintf ( 1, '     N     M        X           Pmn(N,M,X)                Pmn(N,M,X)             Error\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, m, x, fx1 ] = pmn_polynomial_values ( n_data );

    if ( n_data == 0 )
      break
    end

    v = pmn_polynomial_value ( mm, n, m, x );
    fx2 = v(1,n+1);

    e = fx1 - fx2;

    fprintf ( 1, '  %4d  %4d  %12g  %24g  %24g  %8g\n', n, m, x, fx1, fx2, e );

  end

  return
end


function procNoiseParam=processNoiseSuggest(algorithm,maxVal,T,sigmaw2,manDur)
%%PROCESSNOISESUGGEST Use one of a number methods for choosing the scaling
%             parameter for the process noise covariance in a number of
%             continuous-time and discrete-time dynamic models. This only
%             provides the scaling parameter for one dimension of motion.
%             In a multidimensional system, if the measurement noise
%             variance varies between dimensions (such as tracking in range
%             and angle with a range-angle state), then one might use
%             different values in different dimensions. The method only
%             considers scalar measurements in the respective dimensions
%             and is thus only good for a rough approximation.
%
%INPUTS: algorithm A string specifying the algorithm and model used to
%           estimate a good process noise parameter. Possible values are:
%                  'PolyKal-ROT' Use a rule-of-thumb method for determining
%                                the q0 term for the QPolyKal function, for
%                                discrete-time models, and the DPoly
%                                function, for continuous-time models. This
%                                covers the continuous white noise
%                                acceleration (CWNA) model (order=1), the
%                                discretized continuous white noise
%                                acceleration (DCWNA) model (order=1), the
%                                continuous Wiener process acceleration
%                                (CWPA) model  (order=2), and the
%                                discretized continuous Wiener process
%                                acceleration (DCWPA) model (order=2),
%                                among others.
%           'PolyKalDirectDisc-ROT' Use a rule-of-thumb method for
%                                determining the sigmaV2 parameter for the
%                                QPolyKalDirectDisc function. This covers
%                                the discrete white noise acceleration
%                                (DWNA) model (order=1), among others.
%           'PolyKalDirectAlt-ROT' Use a rule-of-thumb method for
%                                determining the sigmaV2 parameter for the
%                                QPolyKalDirectAlt function. This covers
%                                the discrete Wiener process acceleration
%                                (DWPA) model (order=2), among others.
%                 'CWNA-OptMMSE' Use the method of Blair for determining
%                                the asymptotically optimal value of q0 in
%                                terms of MSE for the CWNA and DCWNA
%                                dynamic models for a maneuver having a
%                                fixed maximum acceleration.
%                 'DWNA-OptMMSE' Use the method of Blair for determining
%                                the asymptotically optimal value of
%                                sigmaV2 for the DWNA model for a maneuver
%                                having a fixed maximum acceleration.
%               'CWNA-ConstMeas' Use the method of Blair for determining
%                                the asymptotically optimal value of q0
%                                for the CWNA and DCWNA models such that
%                                the MSE of a Kalman filter under a maximum
%                                acceleration maneuver is not worse than
%                                the measurement accuracy.
%               'DWNA-ConstMeas' Use the method of Blair for determining
%                                the asymptotically optimal value of
%                                sigmaV2 for the DWNA model such that
%                                the MSE of a Kalman filter under a maximum
%                                acceleration maneuver is not worse than
%                                the measurement accuracy.
%    maxVal All of the methods require a maximum bound related to how the
%           target can maneuver. For algorithms CWNA-OptMMSE,
%           DWNA-OptMMSE, CWNA-ConstMeas, and DWNA-ConstMeas, maxVal is
%           the maximum acceleration of the target. For PolyKal-ROT and
%           PolyKalDirectDisc-ROT, and PolyKalDirectAlt-ROT maxVal is the
%           maximum value of a moment one order higher than the maximum
%           order of the dynamic model. For example, if order=1, then
%           maxVal is a maximum acceleration.  If order=2, then maxVal is
%           a maximum jerk.
%         T For all of the algorithms except PolyKalDirectDisc-ROT, this
%           parameter is required and is the typical time between
%           measurements of the target.
%   sigmaw2 For algorithms CWNA-OptMMSE, DWNA-OptMMSE, CWNA-ConstMeas, and
%           DWNA-ConstMeas, this parameter is required and is the variance
%           of the 1D measurement, which should be the lowest order moment
%           of the state. In other words, position.
%    manDur For algorithms CWNA-OptMMSE, DWNA-OptMMSE, CWNA-ConstMeas, and
%           DWNA-ConstMeas, this parameter is required and is the
%           maximum number of samples of duration T that a maximum
%           acceleration maneuver is expected to take. This can take
%           values, 3, 4, 5, 6 and infinity. The default if omitted or an
%           empty matrix is passed is Inf.
%
%OUTPUTS: procNoiseParam The value of q0 or sigmaV2 for the specified
%                        dynamic model, chosen according to the selected
%                        ad-hoc parameter.
%
%The output of this function goes into functions like QPolyKal. It is meant
%that one computes the process noise parameter once (for QPolyKal, this
%would correspond to the q0 parameter), and then one calls the other
%functions with different samples prediction intervals. This function
%should not be called to change the process noise parameter as the sampling
%period changes.
%
%All of the rule-of-thumb methods for choosing the process noise values are
%from Chapter 6.2 and 6.3 of [1] and modified slightly, as described in the
%comments for the implementation below.
%
%The solutions for the ConstMeas and MMSE methods are from [2], which is an
%extension of the work in [3] and [4].
%
%The rule-of thumb parameters might be suitable for other dynamic models.
%For example, when using the Singer model, given with aGaussMarkov and
%DPoly in continuous-time and with FGaussMarkov and QGaussMarkov in
%discrete-time, there does not appear to be a clear way to set the scaling
%value for the process noise in the literature. However, if tau=infinity in
%the model, then in continuous time, it reduces to the CWPA model. Thus,
%methods for choosing the process noise parameter in the CWPA and the DCWPA
%models might be good starting points for setting the process noise
%parameter in the continuous and discrete Singer models.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%[2] W. D. Blair, "Design of nearly constant velocity filters for radar
%    tracking of maneuvering targets," in Proceedings of the IEEE Radar
%    Conference, Atlanta, GA, 7-11 May 2012, pp. 1008-1013.
%[3] W. D. Blair, "Design of nearly constant velocity track filter for
%    brief maneuvers," in Proceedings of the 14th International Conference
%    on Information Fusion, Chicago, IL, 5-8 Jul. 2011.
%[4] W. D. Blair, "Design of nearly constant velocity track filters for
%    tracking maneuvering targets," in Proceedings of the 11th
%    International Conference on Information Fusion, Cologne, Germany, 30
%    Jun. - 3 Jul. 2008.
%
%April 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(manDur))
    manDur=Inf;
end

switch(algorithm)
    case 'PolyKal-ROT'
        %The rule of thumb is generalized from the rules of thumb in
        %Chapters 6.2.2 and 6.2.3 of Bar-Shalom's book, when considering
        %the CWNA, DCWNA, CWPA and DCWPA. For the CWNA and DCWNA models, it
        %is suggested that sqrt(q0*T) be on the order of the changes in
        %velocity over a sampling interval. In other words, it is the
        %average acceleration. It might be more useful to express things in
        %terms of a maximum acceleration, though. In Chapter 6.3.2 for the
        %DWNA model, it is suggested that  0.5*aMax<=sigmaV<=aMax. Using a
        %similar logic, one might try  0.5*aMax<=sqrt(q0*T)<=aMax. Using
        %the same logic for the CWPA and DCWPA models of Chapter 6.2.3, one
        %might choose q0 such that  0.5*jerkMax<=sqrt(q0*T)<=jerkMax, where
        %jerkMax is the maximum possible jerk (derivative of acceleration).
        %This function generalizes the rule  to any order and chooses the
        %point midway in that specified range.

        procNoiseParam=(0.75*maxVal)^2/T;
        return;
    case 'PolyKalDirectDisc-ROT'
        %The rule-of-thumb is generalized from that given in Chapter 6.3.2
        %of Bar-Shalom's book. In the book, it is suggested that
        %0.5*aMax<=sigmaV<=aMax for the DWNA model (order=1). This function
        %generalizes it to any order and chooses the point midway in that
        %specified range.

        procNoiseParam=(0.75*maxVal)^2;
        return;
    case 'PolyKalDirectAlt-ROT'
        %The rule-of-thumb is generalized from that given in Chapter 6.3.3
        %of Bar-Shalom's book. In the book, it is suggested that
        %0.5*deltaAMax<=sigmaV<=deltaAMax for the DWPA model (order=2),
        %where deltaAMax is the maximum change in acceleration over an
        %interval of duration T. This can be related to a maximum jerk as
        %0.5*jerkMax*T<=sigmaV<=jerkMax*T. This function generalizes it to
        %any order and chooses the point midway in that range.

        procNoiseParam=(0.5*maxVal*T)^2;
        return;
    case 'CWNA-OptMMSE'
        optType=0;
        isDiscrete=false;
    case 'DWNA-OptMMSE'
        optType=0;
        isDiscrete=true;
    case 'CWNA-ConstMeas'
        optType=1;
        isDiscrete=false;
    case 'DWNA-ConstMeas'
        optType=1;
        isDiscrete=true;
    otherwise
        error('Unknown algorithm chosen')
end

%Equation 3, the deterministic maneuvering index.
GammaD=maxVal*T^2/sqrt(sigmaw2);

if(0.01>GammaD||GammaD>10)
   warning('The deterministic maneuvering index is outside of the tabulated range of values. The process noise parameter might be inaccurate.')
end
%Blair's papers do not say the base, but if one uses base 10, one gets the
%results in the plots. If one uses base e, the results do not match the
%plots in the paper.
logVal=log10(GammaD);

%optType=0 means MMSE (so use max)
%optType=1 means limit the maximum error to the measurement noise (so use
%          min)
switch(manDur)
    case Inf
        if(optType)
            curRow=2;
        else
            curRow=1;
        end
    case 3
        if(optType)
            curRow=4;
        else
            curRow=3;
        end
    case 4
        if(optType)
            curRow=6;
        else
            curRow=5;
        end
    case 5
    %If one wants a five-sample maneuver, then we will average the values
    %from the length four and the length-six maneuvers.
        if(optType)
            curRow=[6;8];
        else
            curRow=[5;7];
        end
    case 6
        if(optType)
            curRow=8;
        else
            curRow=7;
        end
    otherwise
        error('An untabulated value for the maximum maneuver duration has been given')
end

%Table I in Blair's paper
k1CoeffTable=[1.677, -0.726,  0.230, -0.012, 0.005,  0.000;%k^{max}_{\inf}
              0.872, -0.102, -0.019,  0.010, 0.006,  0.001;%k^{min}_{\inf}
              1.539, -0.189, -0.651,  0.187, 0.284,  0.059;%k^{max}_{3}
              0.707,  0.346, -0.318, -0.097, 0.088,  0.029;%k^{min}_{3}
              1.630, -0.473, -0.403,  0.272, 0.173,  0.023;%k^{max}_{4}
              0.803,  0.197, -0.385, -0.009, 0.138,  0.035;%k^{min}_{4}
              1.671, -0.697,  0.025,  0.250,-0.0324,-0.028;%k^{max}_{6}
              0.869,  0.008, -0.320,  0.088, 0.123,  0.023];%k^{min}_{6}

a0=k1CoeffTable(curRow,1);
a1=k1CoeffTable(curRow,2);
a2=k1CoeffTable(curRow,3);
a3=k1CoeffTable(curRow,4);
a4=k1CoeffTable(curRow,5);
a5=k1CoeffTable(curRow,6);

%Equations 6 and 7. The mean command is for averaging if one wanted a
%length 5 maneuver. Note that equation 6 has a repeated cubed term. That
%appears to be a mistake.
k1=mean(a0+logVal*(a1+logVal*(a2+logVal*(a3+logVal*(a4+a5*logVal)))));

if(isDiscrete)%Discrete-time
    procNoiseParam=(k1*maxVal)^2;
else%Continuous-time
    procNoiseParam=T*(k1*maxVal)^2;
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 [secUT] = convertYearDayHrMnSec2Sec(year, day, hour, min, sec)
%convertYearDayHrMnSec2Sec Summary of this function goes here
%   Detailed explanation goes here

    [secInMin, secInHr, secInDay, secInYear] = getSecondsInVariousTimeUnits();

    secUT = (year-1)*secInYear;
    secUT = secUT + (day-1)*secInDay;
    secUT = secUT + hour*secInHr;
    secUT = secUT + min*secInMin;
    secUT = secUT + sec;
end


%MIRT3D_MEXINTERP  Fast 3D linear interpolation
%
% Output_image = mirt3D_mexinterp(Input_image, XI,YI,ZI) interpolates the 3D image 'Input_image' at
%    the points with coordinates X,Y,Z. Input_image is assumed to  be defined at a regular grid 1:N, 1:M, 1:K,
%    where [M,N,K]=size(Input_images). Points outside the boudary return NaNs.
%    This is equivalent (but much faster) to Matlab's:
%    Output_image = interp3(Input_image,XI,YI,ZI,'linear',NaN);
%
% Output_images = mirt3D_mexinterp(Input_images, XI,YI,ZI). Input_images can be a stack of many 3D images (4D).
%   The function interpolates each of the 3D images at X,Y,Z coordinates and return a stack of corresponding
%   interpolated images. This is equivalent to Matlab's
%
%   Input_images(:,:,:,1)=Input_image1;
%   Input_images(:,:,:,2)=Input_image2;
%   Input_images(:,:,:,3)=Input_image3;
%   Input_images(:,:,:,4)=Input_image4;
%
%   Output_images(:,:,:,1) = interp3(Input_image1,XI,YI,ZI,'linear',NaN);
%   Output_images(:,:,:,2) = interp3(Input_image2,XI,YI,ZI,'linear',NaN);
%   Output_images(:,:,:,3) = interp3(Input_image3,XI,YI,ZI,'linear',NaN);
%   Output_images(:,:,:,4) = interp3(Input_image4,XI,YI,ZI,'linear',NaN);
%
%  This is especially usefull fpr vector valued 3D images, RGB images, to interpolate the whole 3D video at the same coordinates
%  or to interpolate image and its gradients at the same time (in image registration).
%  The speed gain is also from the precomputation of nearest points for interpolation, which are the same for all images in a stack.
%
%  Andriy Myronenko, Feb 2008, email: myron@csee.ogi.edu,
%  homepage: http://www.bme.ogi.edu/~myron/

% The function below compiles the mirt3D_mexinterp.cpp file if you haven't done it yet.
% It will be executed only once at the very first run.
function Output_images = mirt3D_mexinterp(Input_images, XI,YI,ZI)

pathtofile=which('mirt3D_mexinterp.cpp');
pathstr = fileparts(pathtofile);
mex(pathtofile,'-outdir',pathstr);

Output_images = mirt3D_mexinterp(Input_images, XI,YI,ZI);

end


function optimo = LocalSearch(Problem,pos,w)

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

    MaxIter = 5;
    Tol     = 1e-3;
    step    = 1;
    k       = 1;
    error   = 10;
    while error>Tol && k<MaxIter
        grad(1,:)    = FiniteDifference(pos,w,Problem);
        offspringdec = pos.dec - step*grad(1,:);
        offspringdec = min(max(offspringdec,Problem.lower),Problem.upper);
        offspring    = Problem.Evaluation(offspringdec);
        grad(2,:)    = FiniteDifference(offspring,w,Problem);
        step         = abs((offspring.dec-pos.dec)*(grad(2,:)-grad(1,:))')/norm(grad(2,:)-grad(1,:))^2;
        error = norm(offspring.dec-pos.dec);
        pos   = offspring;
        k     = k + 1;
    end
    optimo = pos;
end

function df = FiniteDifference(X,W,Problem)
    if any(X.con>0)
        df = Problem.CalConGrad(X.dec)';
        df = sum(df,2);
    else
        df = Problem.CalObjGrad(X.dec)';
        df = df*W';
    end
end


function bcdof = BoundaryCondition(typeBC,coordinates)

%--------------------------------------------------------------------------
%   Purpose:
%           To determine the boundary conditions degree of freedom
%   Synopsis:
%           bcdof = BoundaryCondition(typeBC,coordinates)
%
%   Variable Description:
%           bcdof - boundary condition degree of freedom
%                   dof's are (UZ,RX,RY)
%           typeBC - string which gives the state of boundary condition
%           coordinates - geometric coordinates of nodes
%
%--------------------------------------------------------------------------

L1 = find(coordinates(:,2)==min(coordinates(:,2))) ; % at y = 0 (along X-axes)
L2 = find(coordinates(:,1)==max(coordinates(:,1))) ; % at x = a (along Y-axes)
L3 = find(coordinates(:,2)==max(coordinates(:,2))) ; % at y = b (along X-axes)
L4 = find(coordinates(:,1)==min(coordinates(:,1))) ; % at x = 0 (along Y-axes)
n = length(L1) ;

switch typeBC
    case 'ss-ss-ss-ss'
        disp('plate is simply supported at all the edges')
        dofL1 = zeros(1,2*n) ;
        dofL2 = zeros(1,2*n) ;
        dofL3 = zeros(1,2*n) ;
        dofL4 = zeros(1,2*n) ;
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL1(i1) = 3*L1(i);
            dofL1(i2) = 3*L1(i)-2 ;
            dofL3(i1) = 3*L3(i) ;
            dofL3(i2) = 3*L3(i)-2 ;
        end
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL2(i1) = 3*L2(i)-1 ;
            dofL2(i2) = 3*L2(i)-2 ;
            dofL4(i1) = 3*L4(i)-1 ;
            dofL4(i2) = 3*L4(i)-2 ;
        end
        L1UL3 = union(dofL1,dofL3) ;
        L2UL4 = union(dofL2,dofL4) ;
        bcdof = union(L1UL3,L2UL4) ;

    case 'c-c-c-c'
        disp('plate is clamped at all the edges')
        dofL1 = zeros(1,2*n) ;
        dofL2 = zeros(1,2*n) ;
        dofL3 = zeros(1,2*n) ;
        dofL4 = zeros(1,2*n) ;
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL1(i1) = 3*L1(i)-1;
            dofL1(i2) = 3*L1(i)-2 ;
            dofL3(i1) = 3*L3(i)-1 ;
            dofL3(i2) = 3*L3(i)-2 ;
        end
        for i = 1:n
            i1 = 2*(i-1)+2 ;
            i2 = i1-1 ;
            dofL2(i1) = 3*L2(i) ;
            dofL2(i2) = 3*L2(i)-2 ;
            dofL4(i1) = 3*L4(i) ;
            dofL4(i2) = 3*L4(i)-2 ;
        end
        L1UL3 = union(dofL1,dofL3) ;
        L2UL4 = union(dofL2,dofL4) ;
        bcdof = union(L1UL3,L2UL4) ;
end


function [y,deriv] = dotprod_of_functions(w,f,g)
% This is an MV2DF (see MV2DF_API_DEFINITION.readme) which
% represents the new function,
%
%    g(w) = f(w)'g(w)
%
% where f(w) and g(w) are column vectors of the same size.
%
% Here f,g are function handles to MV2DF's.

if nargin==0
    test_this();
    return;
end

function A = extractA(w)
    A = w(1:length(w)/2);
    A = A(:).';
end

function B = extractB(w)
    B = w(1+length(w)/2:end);
    B = B(:)    ;
end

if isempty(w)

    s = stack(w,f,g);
    y = mm_special(s,@(w)extractA(w),@(w)extractB(w));
    return;
end

if isa(w,'function_handle')
    f = dotprod_of_functions([],f,g);
    y = compose_mv(f,w,[]);
    return;
end

f = dotprod_of_functions([],f,g);
if nargout==1
    y = f(w);
else
    [y,deriv] = f(w);
end

end

function test_this()

m = 5;
w = randn(2*m,1);
f = subvec([],2*m,1,m);
g = subvec([],2*m,m+1,m);

h = dotprod_of_functions([],f,g);
test_MV2DF(h,w);
end


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

clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Taille de la boite
edg = 1;

% Precision ou nombre de points de quadrature polynomiale
tol = 1e-3;

% Regular cube
n       = 10;
x       = 0:1/(n-1):1;
[x,y,z] = meshgrid(x,x,x);

% Recepteurs
Nx     = 1000;
% X      = edg*rand(Nx,3);
X      = edg*[x(:),y(:),z(:)];

% Emmeteurs
Ny     = 1000;
% Y      = edg*rand(Ny,3);
Y      = edg*[x(:),y(:),z(:)];
Y(:,1) = 2*edg + Y(:,1);

% Potentiel aleatoire aux emmeteurs
V  = (-1-1i) + (2+2i)*rand(Ny,1);

% Representation graphique
figure(1)
plot3(X(:,1),X(:,2),X(:,3),'*b',Y(:,1),Y(:,2),Y(:,3),'r*')
grid on
axis equal

% Noyau de green Helmholtz
k     = 5;
rxy   = @(X,Y) sqrt( (X(:,1)-Y(:,1)).^2 + (X(:,2)-Y(:,2)).^2 + (X(:,3)-Y(:,3)).^2 );
green = @(X,Y) exp(1i*k*rxy(X,Y))./rxy(X,Y);

% Gridding
[I,J] = ndgrid(1:Nx,1:Ny);

% Noyau de green
Gxy = green(X(I,:),Y(J,:));
Gxy = reshape(Gxy,Nx,Ny);
% Gxy = [Gxy zeros(Nx,Ny) ; ...
%     zeros(Nx,Ny) Gxy];
% Gxy = [Gxy zeros(Nx,2*Ny) ; ...
%     zeros(Nx,Ny) Gxy zeros(Nx,Ny) ; ...
%     zeros(Nx,2*Ny) Gxy];
% figure
% imagesc(Gxy)

% Calcul du rang
tic
rk = rank(Gxy,tol);
toc
rk

% Compression ACA, pivotage total
tic
[A,B] = hmxACA(Gxy,tol);
toc
size(A)
norm(A*B-Gxy,'fro')./norm(Gxy,'fro')

% Compression ACA, pivotage partiel
tic
[A,B] = hmxACA(X,Y,@(X,Y) green(X,Y),tol);
toc
size(A)
norm(A*B-Gxy,'fro')./norm(Gxy,'fro')

% % Recompression ACA, pivotage total
% tic
% [Ap,Bp] = hmxACA(A,B,tol);
% toc
% size(Ap)
% norm(Ap*Bp-Gxy,'fro')./norm(Gxy,'fro')

% Recompression RSVD
tic
[Ap,Bp,flag] = hmxRSVD(A,B,tol);
toc
size(Ap)
norm(Ap*Bp-Gxy,'fro')./norm(Gxy,'fro')

% Recompression QRSVD
tic
[Ap,Bp] = hmxQRSVD(A,B,tol);
toc
size(Ap)
norm(Ap*Bp-Gxy,'fro')./norm(Gxy,'fro')

disp('~~> Michto gypsilab !')


%% Set Parameters
% Preferences
global ggamma rrho
ggamma = 2;        % coefficient of relative risk aversion
rrho = 0.01;       % rate of time preference

% Production function
global ddelta aalpha
ddelta = .025;     % capital depreciation
aalpha = 1 / 3;    % capital share

% Aggregate shock
global ssigmaTFP rrhoTFP
ssigmaTFP = .007;  % standard deviation of TFP shock
rrhoTFP = .95;     % quarterly autocorrelation of TFP shock

% Idiosyncratic shocks
global z
zz1 = 0;           % unemployed
zz2 = 1;           % employed
z = [zz1,zz2];

% Transition probabilities
global lla
llambda1 = 1 / 2;  % expected duration of unemployment is 2 quarters
llambda2 = (llambda1 / (zz2 * .93 - zz1))*(zz2 - zz2 * .93); % unemployment rate 7%
lla = [llambda1,llambda2];

% Tax system
global mmu ttau
mmu = .15;        % UI replacement rate 15%
ttau = (mmu / zz2) * (lla(2) / lla(1));	     % labor income tax

% Labor supply
global zAvg
zAvg = (lla(1) * z(2) + lla(2) * z(1)) / (lla(1) + lla(2));

%% Approximation Parameters
% Wealth grid
global I amin amax a da aa aaa
I = 100;           % can't be too fine for some reason
amin = 0;
amax = 100;
a = linspace(amin,amax,I)';
da = (amax - amin) / (I - 1);
aa = [a,a];
aaa = reshape(aa,2*I,1);

% Labor productivity
global zz zzz
zz = ones(I,1) * z;
zzz = reshape(zz,2*I,1);

% Idiosyncratic shocks for income
global Aswitch
Aswitch = [-speye(I) * lla(1),speye(I) * lla(1);speye(I) * lla(2),-speye(I) * lla(2)];

% Steady state computations
global rmin rmax r0 maxit crit Delta Ir crit_S
rmin = .0001;       % lower bound for steady state interest rate
rmax = rrho;        % upper bound for steady state interest rate
r0 = .005;          % initial guess for steady state interest rate
maxit = 100;        % maximum iterations on steady state HJB
crit = 1e-6;        % error criterion for steady state value function convergence
Delta = 1e4;        % update size for implicit scheme on steady state HJB
Ir = 100;           % maximum iterations on steady state interest rate
crit_S = 1e-5;      % error criterion for steady state interest rate

% Number of variables in the system
global nVars nEErrors n_v n_g n_p
n_v = 2 * I;
n_g = 2 * I-1 + 1;
n_p = 6;
nVars = n_v + n_g + n_p;
nEErrors = 2 * I;

% Spline parameters
global n_splined
n_knots = 12;
c_power = 7;
x = a';
n_post = 2;	        % This is from two income states
n_prior = 1;
n_splined = n_prior*n_knots*n_post;


classdef CBFDT < handle
    properties
        systemParam
        optType
        optParam
        xcurr
        timecurr
        solvertime
        xdim
        udim
        tlog = []
        xlog = []
        ulog = []
        omegalog = []
    end

    methods
        function self = CBFDT(system_param, x0, t0)
            self.systemParam = system_param;
            self.xcurr = x0;
            self.timecurr = t0;
            self.xdim = size(self.systemParam.A, 1);
            self.udim = size(self.systemParam.B, 2);
        end

        function setOpt(self, opt_type, opt_param)
            self.optType = opt_type;
            self.optParam = opt_param;
        end

        function sim(self, total_time)
            xk = self.xcurr;
            while self.timecurr <= total_time
                [feas, xopt, uopt, Jopt] = self.solve();
                if feas ~= 1
                    return;
                end
                uk = uopt(:,1);
                xk = self.systemParam.A * xk + self.systemParam.B * uk;
                self.xcurr = xk;
                self.timecurr = self.timecurr + self.systemParam.timestep;
                self.tlog = [self.tlog, self.timecurr];
                self.xlog = [self.xlog, xk];
                self.ulog = [self.ulog, uk];
            end
        end

        function [feas, xopt, uopt, Jopt] = solve(self)
            if strcmp(self.optType,'dclfdcbf')
                [feas, xopt, uopt, Jopt] = solveDCLFDCBF(self);
            elseif strcmp(self.optType,'nmpcdclfdcbf')
                [feas, xopt, uopt, Jopt] = solveNMPCDCLFDCBF(self);
            elseif strcmp(self.optType,'mpcdcbf')
                [feas, xopt, uopt, Jopt] = solveMPCDCBF(self);
            elseif strcmp(self.optType,'mpcgcbf')
                [feas, xopt, uopt, Jopt] = solveMPCGCBF(self);
            elseif strcmp(self.optType,'nmpcdcbf')
                [feas, xopt, uopt, Jopt] = solveNMPCDCBF(self);
            else
                disp('optimal control policy undefined');
            end
        end

        function [feas, xopt, uopt, Jopt] = solveDCLFDCBF(self)
            x = sdpvar(self.xdim, 2);
            u = sdpvar(self.udim, 1);
            s = sdpvar(1,1);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % dynamics
            constraints = [constraints; x(:,2) == self.systemParam.A * x(:,1) + self.systemParam.B * u(:,1)];
            % DCLF constraint
            v = x(:,1)' * self.optParam.P * x(:,1);
            vnext = x(:,2)' * self.optParam.P * x(:,2);
            constraints = [constraints; vnext - v + self.optParam.alpha * v <= s];
            % DCBF constraint
            r = 1;
            b = x(:,1)' * x(:,1) - r^2;
            bnext = x(:,2)' * x(:,2) - r^2;
            constraints = [constraints; bnext - b + self.optParam.gamma * b >= 0];
            % input constraint
            constraints = [constraints; self.systemParam.ul <= u <= self.systemParam.uu];
            % cost
            cost = cost + u' * self.optParam.uWeight * u + s' * self.optParam.sWeight * s;
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveNMPCDCLFDCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            s = sdpvar(1,self.optParam.horizonCLF);
            omega = sdpvar(1,self.optParam.horizonCBF);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % CLF constraints
            for i = 1:self.optParam.horizonCLF
                v = x(:,1)' * self.optParam.P * x(:,1);
                vnext = x(:,2)' * self.optParam.P * x(:,2);
                constraints = [constraints; vnext - v + self.optParam.alpha * v <= s(i)];
                cost = cost + s(i)' * self.optParam.sWeight * s(i);
            end
            % CBF constraints
            for i = 1:self.optParam.horizonCBF
                r = 1;
                b = x(:,1)' * x(:,1) - r^2;
                bnext = x(:,2)' * x(:,2) - r^2;
                constraints = [constraints; bnext >= omega(i) * (1 - self.optParam.gamma) * b];
                constraints = [constraints; omega(i) >= 0];
                assign(omega(i), 1);
                cost = cost + self.optParam.omegaWeight * (omega(i) - 1)^2;
            end
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveMPCDCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % CBF constraints
            for i = 1:self.optParam.horizon
                r = 0;
                b = r - x(1,i);
                bnext = r - x(1,i+1);
                constraints = [constraints; bnext - b + self.optParam.gamma * b >= 0];
            end
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveMPCGCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % GCBF constraints
            num_HO = 3;
            r = 0;
            b = r - x(1,1);
            bnext = r - x(1,1+num_HO);
            constraints = [constraints; bnext >= (1 - self.optParam.gamma)^num_HO * b];
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end

        function [feas, xopt, uopt, Jopt] = solveNMPCDCBF(self)
            x = sdpvar(self.xdim, self.optParam.horizon + 1);
            u = sdpvar(self.udim, self.optParam.horizon);
            omega = sdpvar(1,self.optParam.horizonCBF);
            cost = 0;
            constraints = [];
            % initial condition
            constraints = [constraints; x(:,1) == self.xcurr];
            % MPC cost and constraints
            for i = 1:self.optParam.horizon
                constraints = [constraints;
                    self.systemParam.ul <= u(:,i) <= self.systemParam.uu;
                    x(:,i+1) == self.systemParam.A * x(:,i) + self.systemParam.B * u(:,i)];
                cost = cost + x(:,i)'*self.optParam.xWeight*x(:,i);
                cost = cost + u(:,i)'*self.optParam.uWeight*u(:,i);
            end
            cost = cost + x(:,self.optParam.horizon)' * self.optParam.P * x(:,self.optParam.horizon);
            % CBF constraints
            for i = 1:self.optParam.horizonCBF
                r = 0;
                b = r - x(1,i);
                bnext = r - x(1,i+1);
                constraints = [constraints; bnext >= omega(i) * (1 - self.optParam.gamma) * b];
                constraints = [constraints; omega(i) >= 0];
                assign(omega(i), 1);
                cost = cost + self.optParam.omegaWeight * (omega(i) - 1)^2;
            end
            opt_settings = sdpsettings('solver','ipopt','verbose',0);
            diagnostics = optimize(constraints, cost, opt_settings);
            if diagnostics.problem == 0
                feas = true;
                xopt = value(x);
                uopt = value(u);
                Jopt = value(cost);
            else
                feas = false;
                xopt= [];
                uopt = [];
                Jopt = value(cost);
            end
            self.solvertime = diagnostics.solvertime;
            fprintf('solver time: %f\n', diagnostics.solvertime);
        end
    end
end


function ar=lpcim2ar(im)
%LPCIM2AR Convert impulse response to AR coefs AR=(IM)

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpcim2ar.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p1]=size(im);
ar=zeros(nf,p1);
wz=[1 zeros(1,p1-1)];
for k=1:nf
  ar(k,:)=wz/toeplitz(wz,im(k,:)/im(k,1));
end


function [report_data] = report_convergence(report_data, state)
% Report convergence of the algorithm

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if ~isfield(report_data, 'report_interval')
  report_data.report_interval = 5;
  dss_message(state, 2, sprintf('Setting default report interval to %d (report_data.report_interval)\n', report_data.report_interval));
end

% deflation
if state.iteration==1
  % -- first iteration
  if state.algorithm=='defl'
    % deflation
    report_data.change(state.component,1)=0;
    report_data.deltaw_old=state.w;
  else
    % symmetric
    report_data.change = zeros(size(state.W, 1),1);
    report_data.dW_old = zeros(size(state.W));
    report_data.W_old = state.W;
  end
else
  % -- iterations 2->
  if (mod(state.iteration, report_data.report_interval)==0)
    if state.algorithm=='defl'
      % deflation
      change = acos(state.w' * state.w_old/norm(state.w)/norm(state.w_old)) / pi * 180;
    else
      % symmetric
      change = abs(angle(state.W, state.W_old)) / pi * 180;
    end
    message(state,1,sprintf('Change (angles): %d\n', change));
  end
end

% -----------------------------------
function rad = angle(A, B)

sum_cross = sum(A .* B, 2);
sum_A = sum(A .* A, 2);
sum_B = sum(B .* B, 2);
rad = acos(sum_cross ./ sum_A.^(-1/2) ./ sum_B.^(-1/2));


function D = agreement_weighted(CI,Wts)
%WEIGHTED_AGREEMENT     weights agreement matrix
%
%   D = AGREEMENT_WEIGHTED(CI,WTS) is identical to AGREEMENT, with the
%   exception that each partitions contribution is weighted according to
%   the corresponding scalar value stored in the vector WTS. As an example,
%   suppose CI contained partitions obtained using some heuristic for
%   maximizing modularity. A possible choice for WTS might be the Q metric
%   (Newman's modularity score). Such a choice would add more weight to
%   higher modularity partitions.
%
%   NOTE: Unlike AGREEMENT, this script does not have the input argument
%   BUFFSZ.
%
%   Inputs:     CI,     set of partitions
%               WTS,    relative weight of importance of each paritition
%
%   Outputs:    D,      weighted agreement matrix
%
%   Richard Betzel, Indiana University, 2013

Wts = Wts./sum(Wts);
[N,M] = size(CI);
D = zeros(N);
for i = 1:M
    d = dummyvar(CI(:,i));
    D = D + (d*d')*Wts(i);
end


%
% state = [x, y, yaw, delta]
% input = [v_des, delta_des]
% ref = [x_ref, y_ref, yaw_ref, v_ref]
%
%
%

clear variables;
close all;

set(0, 'defaultAxesFontSize', 12);
set(0, 'defaultTextFontSize', 20);
set(0, 'DefaultAxesLineWidth', 1.0, 'DefaultLineLineWidth', 1.0);

addpath ../path_design

control_mode_option = ["pure_pursuit", "pid", "mpc", "mpc_no_constraints"];
control_mode = control_mode_option(3);

save_video = 0; %1:save, 0:no

%% preliminaries
rad2deg = 180 / pi;
deg2rad = pi / 180;
kmh2ms = 1000 / 3600;

simulation_time = 35;
simulation_rk4_time_step = 0.002; % simulation time step

vel_ref = 30 * kmh2ms;

% for dynamics model
param.tau = 0.27; % steering dynamics: 1d-approximated time constant
param.wheelbase = 2.69;
param.steer_lim = 30 * deg2rad;
param.vel_max = 10;
param.vel_min = -5;

param.input_delay = 0.24; % [s]
param.control_dt = 0.03; % [s]
param.measurement_noise_stddev = [0.1, 0.1, 1.0*deg2rad, 0.5*deg2rad]; % measurement noise
% param.measurement_noise_stddev = [0,0,0,0]; % measurement noise
param.steering_steady_state_error_deg = 1;

% for pure pursuit only
param.pure_pursuit_lookahead = 8.0; % [m]

% for mpc only
param.mpc_dt = 0.1;
param.mpc_n = 30;
param.mpc_constraint_steering_deg = 30;
param.mpc_constraint_steer_rate_deg = 280;
param.mpc_model_dim = 3;
param.mpc_Q = diag([1,2]);
param.mpc_R = 0.5;
param.mpc_delay_comp_step = round(param.input_delay / param.control_dt);
% param.mpc_delay_comp_step = 0.0;

% use the input ahead of the delay time
param.mpc_sensor_delay = param.input_delay;

% for mpc2
param.mpc2_dt = 0.2;
param.mpc2_n = 10;
param.mpc2_steering_lim_deg = 40;
param.mpc2_model_dim = 4;
param.mpc2_Q = diag([1,1,0]);
param.mpc2_R = 0.05;

%% simulation parameters

% initial position (x, y, yaw, delta)
x0 = [0, 0.5, 0, 0];

ts = 0;
dt = simulation_rk4_time_step;
tf = simulation_time;
t = ts:dt:tf;

%% reference trajectory design

path_design; % using spline
load path; % x, y, yaw

ref = zeros(length(path), 6);
IDX_X = 1;
IDX_Y = 2;
IDX_XY = 1:2;
IDX_XYYAW = 1:3;
IDX_YAW = 3;
IDX_VEL = 4;
IDX_CURVATURE = 5;
IDX_TIME = 6;

IDX_STEER = 4;

path_size_scale = 15;
path(:,IDX_XY) = path(:,IDX_XY) * path_size_scale;
ref(:,IDX_XYYAW) = path(:,IDX_XYYAW);

ref(:,IDX_VEL) = ones(length(path),1)*vel_ref;

% insert curvature into path
for i = 2:length(ref)-1
    p1_ = ref(i-1,IDX_XY);
    p2_ = ref(i, IDX_XY);
    p3_ = ref(i+1, IDX_XY);
    A_ = ((p2_(1)-p1_(1))*(p3_(2)-p1_(2)) - (p2_(2)-p1_(2))*(p3_(1)-p1_(1))) / 2;
    ref(i, IDX_CURVATURE) = 4 * A_ / (norm(p1_-p2_) * norm(p2_-p3_) * norm(p3_-p1_));
end

% insert relative time into path
for i = 2:length(ref)
    v_ = ref(i,IDX_VEL);
    d_ = norm(ref(i,IDX_XY)-ref(i-1,IDX_XY));
    dt_ = d_ / v_;
    ref(i, IDX_TIME) = ref(i-1, IDX_TIME) + dt_;
end

%% simulation

if control_mode == "pure_pursuit"
    [X, U, debug] = simulate_rk4(@kinematics_model, @pure_pursuit, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "pid"
   [X, U, debug] = simulate_rk4(@kinematics_model, @pid_controller, x0, ref, ts, dt, tf, param);
   lat_error_vec = debug(:,end);
elseif control_mode == "mpc"
    param.mpc_solve_without_constraint = false;
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "mpc_no_constraints"
    param.mpc_solve_without_constraint = true;
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
elseif control_mode == "mpc2"
    [X, U, debug] = simulate_rk4(@kinematics_model, @model_predictive_controller2, x0, ref, ts, dt, tf, param);
    lat_error_vec = debug(:,end);
end
fprintf("lattitude error: mean square = %f, max = %f", norm(lat_error_vec)/simulation_time, max(lat_error_vec));

%% movie plot

sp_num = 18;
subpl1 = 'subplot(sp_num,sp_num, sp_num+1:sp_num*12);';
subpl2 = 'subplot(sp_num,sp_num, sp_num*13+1:sp_num*15);';
subpl3 = 'subplot(sp_num,sp_num, sp_num*16+1:sp_num*18);';

% tire2steer = 12.5;

fig_trajectory_result = figure(1);
set(fig_trajectory_result, 'Position', [716 735 1026 1146]);
eval(subpl1);
plot(ref(:,1), ref(:,2),'k-.'); hold on; grid on;
% plot(X(:,1), X(:,2));
% legend('ref','tracked');
xlabel('x [m]'); ylabel('y [m]');
% img_orig = imread('handle.jpg');
% img = imrotate(img_orig, 10);
% handle_plt = image([150, 170],[110, 90], img);
% handle_plt_point = [160, 100, 0];

eval(subpl2);
plot(t, lat_error_vec, 'b'); grid on; hold on;
xlabel('t [s]'); ylabel('latitude error [m]');
ulim = ceil(2*max(lat_error_vec))/2;
dlim = floor(2*min(lat_error_vec))/2;
ylim([dlim, ulim]);

eval(subpl3);
p1 = plot(t, X(:,IDX_STEER)*rad2deg, 'b'); grid on; hold on;
p2 = plot(t, U(:,2)*rad2deg, 'Color', [0.7 0. 1]); hold on;
legend([p1,p2], {'measured','command'})
xlabel('t [s]'); ylabel('steering angle [deg]');
ulim = round(2*max(X(:,IDX_STEER)*rad2deg))/2;
dlim = round(2*min(X(:,IDX_STEER)*rad2deg))/2;
ylim([dlim, ulim]);

z_axis = [0 0 1];
setpoint = []; rear_tire = []; front_tire = []; body = []; tracked = [];
setpoint_ideal = []; error_point = []; steer_point = []; time_bar_laterror = []; time_bar_steer = [];
L = param.wheelbase;
rear_length = 1;
front_length = 1;
side_width = 0.9;
fig_draw_i = 1:round(1/dt/20):length(t);

% for movie
clear frame_vec;
frame_vec(length(fig_draw_i)) = struct('cdata', [], 'colormap',[]);

j = 1;
fig_trajectory_result; hold on;
for i = fig_draw_i
    eval(subpl1);
    disp(t(i))
    rear_x = X(i,1);
    rear_y = X(i,2);
    yaw = X(i,3);
    delta = X(i,IDX_STEER);
    front_x = rear_x + L;
    front_y = rear_y;
    delete([setpoint, rear_tire, front_tire, body, tracked, setpoint_ideal, error_point, steer_point, time_bar_laterror, time_bar_steer]);
    tracked = plot(X(1:i,1), X(1:i,2),'r');
    title_draw = "t = "+num2str(t(i),'%5.1f') + "[s], steer = " + num2str(delta*rad2deg,'%+3.1f') + "[deg], v = " + ...
        num2str(vel_ref*3600/1000,'%3.1f') + "[km/h], lat error = "+num2str(lat_error_vec(i),'%+2.2f') + "[m]";
    title_draw = [title_draw; "Simulation: solver = rk4, sensor-delay = "  + num2str(param.input_delay*1000, '%d') + "[ms], control freq=" + ...
        num2str(1/param.control_dt, '%d') + "[hz]"];
    title_draw = [title_draw; "noise-sigma = " + num2str(param.measurement_noise_stddev(1),'%2.2f')+"(pos), "+ ...
        num2str(param.measurement_noise_stddev(3),'%2.2f')+"(yaw), "+num2str(param.measurement_noise_stddev(4),'%2.2f')+"(steer)"];
    if control_mode == "mpc" || control_mode == "mpc_no_constraints"
        title_draw = [title_draw; "MPC: dt = " + num2str(param.mpc_dt, '%3.3f') + "[s], horizon step = " + num2str(param.mpc_n, '%d')];
%         pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));
%         pred_states = reshape(pred_states, param.mpc_n, length(pred_states)/param.mpc_n);
%         setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error
        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(2+4+1));
        pred_error = reshape(pred_error, param.mpc_n, length(pred_error)/param.mpc_n);
        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error
    elseif control_mode == "mpc2"
        title_draw = [title_draw; "MPC2: dt = " + num2str(param.mpc_dt, '%3.3f') + "[s], horizon step = " + num2str(param.mpc_n, '%d')];
        pred_states = debug(i, param.mpc_n+1:param.mpc_n*(4+1));
        pred_states = transpose(reshape(pred_states, length(pred_states)/param.mpc_n, param.mpc_n));
        setpoint = plot(pred_states(:,1), pred_states(:,2), 'bo'); % include linealize error
        pred_error = debug(i, param.mpc_n*(4+1)+1:param.mpc_n*(4+4+1));
        pred_error = transpose(reshape(pred_error, length(pred_error)/param.mpc_n, param.mpc_n));
        setpoint_ideal = plot(pred_error(:,1), pred_error(:,2), 'mx'); % without linealize error
    elseif control_mode == "pure_pursuit"
        title_draw = [title_draw; "pure-pursuit: lookahead dist="+num2str(param.pure_pursuit_lookahead, '%1.1f')+"[m]"];
        sp = debug(i,:);
        setpoint = plot(sp(1), sp(2), 'ro');
    elseif control_mode == "pid"
        title_draw = [title_draw; "PID: kp = " + num2str(0.3, '%3.3f') + ", ki = " + num2str(0, '%3.3f') + ", kd = " + num2str(1.5, '%3.3f')];
        sp = debug(i,:);
        setpoint = plot(sp(1), sp(2), 'ro');
    end
    rear_tire = plot([rear_x-0.3, rear_x+0.3],[rear_y, rear_y], 'k', 'LineWidth', 2.0);
    front_tire = plot([front_x-0.3, front_x+0.3],[front_y, front_y], 'k', 'LineWidth', 2.0);
    body = plot([rear_x-rear_length, front_x+front_length, front_x+front_length, rear_x-rear_length, rear_x-rear_length], ...
        [rear_y-side_width, front_y-side_width, front_y+side_width, rear_y+side_width, rear_y-side_width],'k');
    rear_origin = [rear_x, rear_y, 0];
    front_origin = [rear_x + L*cos(yaw), rear_y + L*sin(yaw), 0];
    rotate(body, z_axis, yaw * rad2deg, rear_origin);
    rotate(rear_tire, z_axis, yaw * rad2deg, rear_origin);
    rotate(front_tire, z_axis, yaw * rad2deg, rear_origin);
    rotate(front_tire, z_axis, delta * rad2deg, front_origin);
%     rotate(handle_plt, [0,0,1], delta*tire2steer*rad2deg, handle_plt_point)
    title(title_draw);
    xlim([0 120]);

    % lat error
    eval(subpl2);
    error_point = plot(t(i), lat_error_vec(i), 'ko');
    time_bar_laterror = plot([t(i), t(i)], [100, -100], 'k');

    % steering
    eval(subpl3);
    steer_point = plot(t(i), X(i, IDX_STEER)*rad2deg, 'ko');
    time_bar_steer = plot([t(i), t(i)], [100, -100], 'k');
    legend([p1,p2], {'measured','command'})
    ylim([-40 40]);

    drawnow;
    frame_vec(j) = getframe(fig_trajectory_result);

    j = j + 1;
end

% for video save
if (save_video == 1)
    cd ./movie
    frame_vec(1) = [];
    vidObj = VideoWriter('result.avi');
    vidObj.FrameRate = 25;
    open(vidObj);
    writeVideo(vidObj, frame_vec);
    close(vidObj);
    cd ../
end

% plot_pid;


function [accuracy,threshold] = Sys_accuracy(match, nonmatch)
    [TAR,FAR,nonmatch_score] = rocplot(match, nonmatch);
    %[length(TAR),length(FAR),length(match),length(nonmatch_score),length(nonmatch)]
    accuracies=(TAR*length(match)+(1-FAR)*length(nonmatch))/(length(nonmatch)+length(match));
    [~,idx]=sort(abs(TAR-(1-FAR)),1,'ascend');

    [accuracy,a_idx]=max(accuracies(idx(1:100)));
    threshold=nonmatch_score(idx(a_idx(1)));


function volume = cube01_volume ( )

%*****************************************************************************80
%
%% CUBE01_VOLUME returns the volume of the unit cube in 3D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VOLUME, the volume.
%
  volume = 1.0;

  return
end


% [PYR, INDICES] = buildLpyr(IM, HEIGHT, FILT1, FILT2, EDGES)
%
% Construct a Laplacian pyramid on matrix (or vector) IM.
%
% HEIGHT (optional) specifies the number of pyramid levels to build. Default
% is 1+maxPyrHt(size(IM),size(FILT));
%
% FILT1 (optional) can be a string naming a standard filter (see
% namedFilter), or a vector which will be used for (separable)
% convolution.  Default = 'binom5'.  FILT2 specifies the "expansion"
% filter (default = filt1).  EDGES specifies edge-handling, and
% defaults to 'reflect1' (see corrDn).
%
% PYR is a vector containing the N pyramid subbands, ordered from fine
% to coarse.  INDICES is an Nx2 matrix containing the sizes of
% each subband.  This is compatible with the MatLab Wavelet toolbox.

% Eero Simoncelli, 6/96.

function [pyr,pind] = buildLpyr(im, ht, filt1, filt2, edges)

if (nargin < 1)
  error('First argument (IM) is required');
end

im_sz = size(im);

%------------------------------------------------------------
%% OPTIONAL ARGS:

if (exist('filt1') ~= 1)
  filt1 = 'binom5';
end

if isstr(filt1)
  filt1 = namedFilter(filt1);
end

if ( (size(filt1,1) > 1) & (size(filt1,2) > 1) )
  error('FILT1 should be a 1D filter (i.e., a vector)');
else
  filt1 = filt1(:);
end

if (exist('filt2') ~= 1)
  filt2 = filt1;
end

if isstr(filt2)
  filt2 = namedFilter(filt2);
end

if ( (size(filt2,1) > 1) & (size(filt2,2) > 1) )
  error('FILT2 should be a 1D filter (i.e., a vector)');
else
  filt2 = filt2(:);
end

max_ht = 1 + maxPyrHt(im_sz, max(size(filt1,1), size(filt2,1)));
if (exist('ht') ~= 1)
  ht = max_ht;
else
  if (ht > max_ht)
    error(sprintf('Cannot build pyramid higher than %d levels.',max_ht));
  end
end

if (exist('edges') ~= 1)
  edges= 'reflect1';
end

%------------------------------------------------------------

if (ht <= 1)

  pyr = im(:);
  pind = im_sz;

else

  if (im_sz(2) == 1)
    lo2 = corrDn(im, filt1, edges, [2 1], [1 1]);
  elseif (im_sz(1) == 1)
    lo2 = corrDn(im, filt1', edges, [1 2], [1 1]);
  else
    lo = corrDn(im, filt1', edges, [1 2], [1 1]);
    int_sz = size(lo);
    lo2 = corrDn(lo, filt1, edges, [2 1], [1 1]);
  end

  [npyr,nind] = buildLpyr(lo2, ht-1, filt1, filt2, edges);

  if (im_sz(1) == 1)
    hi2 = upConv(lo2, filt2', edges, [1 2], [1 1], im_sz);
  elseif (im_sz(2) == 1)
    hi2 = upConv(lo2, filt2, edges, [2 1], [1 1], im_sz);
  else
    hi = upConv(lo2, filt2, edges, [2 1], [1 1], int_sz);
    hi2 = upConv(hi, filt2', edges, [1 2], [1 1], im_sz);
  end

  hi2 = im - hi2;

  pyr = [hi2(:); npyr];
  pind = [im_sz; nind];

end


%  Computes the hessian by finite differences
%
%  ::
%
%     H = finite_differences(Objective,params)
%     H = finite_differences(Objective,params,varargin)
%
%  Args:
%
%     - **Objective** [char|function handle]: function to differentiate
%     - **params** [vector]: point at which the differentiation is taken
%     - **varargin** : optional/further arguments of the objective function
%
%  Returns:
%     :
%
%     - **H** [matrix]: Hessian matrix
%
%  See also:
%
%     - utils.hessian.outer_product
%
%


function [ component_num, c ] = i4block_components ( l, m, n, a )

%*****************************************************************************80
%
%% I4BLOCK_COMPONENTS assigns contiguous nonzero pixels to a common component.
%
%  Discussion:
%
%    On input, the A array contains values of 0 or 1.
%
%    The 0 pixels are to be ignored.  The 1 pixels are to be grouped
%    into connected components.
%
%    The pixel A(I,J,K) is "connected" to the pixels:
%
%      A(I-1,J,  K  ),  A(I+1,J,  K  ),
%      A(I,  J-1,K  ),  A(I,  J+1,K  ),
%      A(I,  J,  K-1),  A(I,  J,  K+1),
%
%    so most pixels have 6 neighbors.
%
%    On output, COMPONENT_NUM reports the number of components of nonzero
%    data, and the array C contains the component assignment for
%    each nonzero pixel, and is 0 for zero pixels.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer L, M, N, the order of the array.
%
%    Input, integer A(L,M,N), the pixel array.
%
%    Output, integer COMPONENT_NUM, the number of components
%    of nonzero data.
%
%    Output, integer C(L,M,N), the component array.
%

%
%  Initialization.
%
  c = zeros ( l, m, n );
  component_num = 0;
%
%  P is simply used to store the component labels.  The dimension used
%  here is, of course, usually an absurd overestimate.
%
  p = 1 : l * m * n;
%
%  "Read" the array one pixel at a time.  If a (nonzero) pixel's north or
%  west neighbor already has a label, the current pixel inherits it.
%  In case the labels disagree, we need to adjust the P array so we can
%  later deal with the fact that the two labels need to be merged.
%
  for i = 1 : l

    for j = 1 : m

      for k = 1 : n

        if ( i == 1 )
          north = 0;
        else
          north = c(i-1,j,k);
        end

        if ( j == 1 )
          west = 0;
        else
          west = c(i,j-1,k);
        end

        if ( k == 1 )
          up = 0;
        else
          up = c(i,j,k-1);
        end

        if ( a(i,j,k) ~= 0 )
%
%  New component?
%
          if ( north == 0 && west == 0 && up == 0 )
            component_num = component_num + 1;
            c(i,j,k) = component_num;
%
%  One predecessor is labeled.
%
          elseif ( north ~= 0 && west == 0 && up == 0 )
            c(i,j,k) = north;
          elseif ( north == 0 && west ~= 0 && up == 0 )
            c(i,j,k) = west;
          elseif ( north == 0 && west == 0 && up ~= 0 )
            c(i,j,k) = up;
%
%  Two predecessors are labeled.
%
          elseif ( north == 0 && west ~= 0 && up ~= 0 )
            c(i,j,k) = min ( west, up );
            c1 = min ( p(west), p(up) );
            p(west) = c1;
            p(up) = c1;
          elseif ( north ~= 0 && west == 0 && up ~= 0 )
            c(i,j,k) = min ( north, up );
            c1 = min ( p(north), p(up) );
            p(north) = c1;
            p(up) = c1;
          elseif ( north ~= 0 && west ~= 0 && up == 0 )
            c(i,j,k) = min ( north, west );
            c1 = min ( p(north), p(west) );
            p(north) = c1;
            p(west) = c1;
%
%  Three predecessors are labeled.
%
          elseif ( north ~= 0 && west ~= 0 && up ~= 0 )
            c(i,j,k) = min ( north, min ( west, up ) );
            c1 = min ( p(north), min ( p(west), p(up) ) );
            p(north) = c1;
            p(west) = c1;
            p(up) = c1;
          end

        end

      end

    end

  end
%
%  When a component has multiple labels, have the higher labels
%  point to the lowest one.
%
  for component = component_num : -1 : 1
    b = component;
    while ( p(b) ~= b )
      b = p(b);
    end
    p(component) = b;
  end
%
%  Locate the minimum label for each component.
%  Assign these mininum labels new consecutive indices.
%
  q = zeros ( 1, component_num );
  i = 0;
  for component = 1 : component_num
    if ( p(component) == component )
      i = i + 1;
      q(component) = i;
    end
  end

  component_num = i;
%
%  Replace the labels by consecutive labels.
%
  i = find ( c ~= 0 );
  c(i) = q ( p ( c(i) ) );

  return
end


function deg=degree(p,y,e)
%DEGREE Polynomial degree
%
% DEG = DEGREE(p,x,e)
%
% p : SDPVAR object.
% x : Degree w.r.t linear SDPVAR objects, can be [].

% e : If e=1, returns degree of each element in p
%
% Examples
% x1 = sdpvar(1,1);x2 = sdpvar(1,1);
% p = [x1;x1*x2+x2^2];
%
% degree(p) returns 2
%
% degree(p,x1) returns 1
%
% degree(p,[x1 x2]) returns [1 2]
%
% degree(p,[x1 x2],1) returns [1 0;1 2]
%
% degree(p,[],1) returns [1;3]

if isa(p,'double')
    if nargin==1
        deg = 0;
    else
        deg = zeros(1,length(y));
    end
    return
end

if nargin<2
    y = recover(depends(p));
end

if nargin<3 | (nargin==3 & e==0)
    exponent_p = exponents(p,y);
    switch nargin
        case 1
            deg = full(max(sum(exponent_p,2)));
        case {2,3}
            deg = full(max(exponent_p,[],1));
        otherwise
            error('Too many arguments. Wadda ya mean?')
    end
else
    p = p(:);
    if isempty(y)
        yy = recover(depends(p));
    else
        yy = y;
    end

    for i = 1:length(p)
        z.type = '()';
        z.subs{1} = i;
        exponent_p = exponents(subsref(p,z),yy);
        switch nargin
            case 1
                deg(i,:) = full(max(sum(exponent_p,2)));
            case {2,3}
                deg(i,:) = full(max(exponent_p,[],1));
            otherwise
               error('Too many arguments. Wadda ya mean?')
        end
    end
    if isempty(y)
        deg = sum(deg,2);
    end
end


function [tf, tf_lambda, tf_U] = isequal(A,B)
%ISEQUAL True if each component of two ktensor's is numerically equal.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

tf = false;
tf_lambda = false;
tf_U = false;

if ~isa(B,'ktensor')
    return;
end

tf_lambda = isequal(A.lambda, B.lambda);
if ncomponents(A) == ncomponents(B)
    tf_U = cellfun(@isequal, A.u, B.u);
end
tf = tf_lambda & all(tf_U);


function v = newtonmonoms(p)
% NEWTONMONOMS Computes all monoms inside half Newton polytope
%
% V = NEWTONMONOMS(P)
%
% Input
%  P : Scalar SDPVAR object
%
% Output
%  V : Vector with SDPVAR objects
%
% Example:
%
% sdpvar x y
% sdisplay(newtonmonoms(1+x^4*y^2+x^2*y^4))
%
% See also NEWTONREDUCE, CONSISTENT, CONGRUENCEBLOCKS

if isa(p,'double')
    v = 1;
else
    x = recover(depends(p));
    v = monolist(x,degree(p)/2);
    v = newtonreduce(v,p);
end


function [newton_m2,N_unique,newton_m2_unique] = monomialproducts(N,n);
%MONOMIALPRODUCTS  Internal function used for monomial reduction

% Exponents in squared monomials

N_unique = [];
for i = 1:size(N,1)
    newton_m2{i} = [];
    n = size(N{i},1);
    for j = 1:n
        newton_m2{i} = [newton_m2{i};[(1:n)' repmat(j,n,1) N{i}(1:n,:)+repmat(N{i}(j,:),n,1)]];
    end
    % Whoops, double copies of diagonal (we want double copies of non-diagonals though)
    if isempty(newton_m2{i})
        newton_m2_unique{i} = [];
    else
        [dummy,j,dummy2] = uniquesafe(newton_m2{i}(:,1:2),'rows');
        newton_m2{i} = newton_m2{i}(j,:);
        % Extract unique monomial products
        [dummy,j,dummy2] = uniquesafe(newton_m2{i}(:,3:end),'rows');
        newton_m2_unique{i} = newton_m2{i}(j,:);
    end
    N_unique = [N_unique;newton_m2_unique{i}];
end
if ~isempty(N_unique)
    [dummy,j,dummy2] = uniquesafe(N_unique(:,3:end),'rows');
    N_unique = N_unique(j,:);
end


function vscl = vscale(f)
%VSCALE   Vertical scale of a SEPARABLEAPPROX.
%
% VSCL = VSCALE(F) returns the vertial scale of a SEPARABLEAPPROX as determined
% by evaluating on a coarse tensor-product grid.

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

% TODO: Should this also be taking the maximum along the edges when we are
% evaluating at 1st kind grids.

% If f is an empty SEPARABLEAPPROX, VSCL = 0:
if ( isempty( f ) )
    vscl = 0;
    return
end

% Get the degree of the SEPARABLEAPPROX:
[m, n] = length(f);

% If F is of low degree, then oversample:
m = min(max(m, 9),2000);
n = min(max(n, 9),2000); % cannot afford to go over 2000x2000.

% Calculate values on a tensor grid:
vals = sample(f, m, n);

% Take the absolute maximum:
vscl = max(abs(vals(:)));

end


function [ y, m, d ] = day_carry_roman ( y, m, d )

%*****************************************************************************80
%
%% DAY_CARRY_ROMAN carries days to months in a Roman date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, the YMD date.
%    On output, D is between 1 and the number of days in M.
%
  days = month_length_roman ( y, m );

  while ( days < d )

    d = d - days;
    m = m + 1;
    days = month_length_roman ( y, m );
%
%  Make sure the month isn't too big.
%
    [ y, m ] = month_carry_roman ( y, m );

  end

  return
end


load 'sdae_mnist_vis.mat';

colors = colormap;

figure;
hold on;
for c = 1:10
    x = H(X_labels == (c-1), 1);
    y = H(X_labels == (c-1), 2);
    rndidx = randperm(length(x));
    x = x(rndidx(1:500));
    y = y(rndidx(1:500));

    plot(x, y, 'x', 'Color', colors(ceil(c/10 * 64), :));
end
hold off;


%  Figure 7.23      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
% script to generate Fig. 7.23
% fig7_23.m
% Nyquist for LQR
clf;
f=[0 1;0 0];
g=[0;1];
h=[1 0];
j=0;
r=1;
rho=1;
q=rho*h'*h;
[k,s]=lqr(f,g,q,r);
sys=ss(f,g,k,0);
% circle of radius one
plot(-1+sin(0:.1:2*pi),cos(0:.1:2*pi),'r--');
axis equal;
text(-1,0,'x');
hold on;
w=logspace(0,2);
nyquist(sys,w);
title('Fig. 7.23: Nyquist diagram');


function [Va, Mask] = reslice_vol(Vo, M, interp)
% function [Va, Mask] = reslice_vol(Vo, M, interp)

% Ripped out of SPM 8 and modified (2010, S.Klanke)

dim = size(Vo);
wrap = [1;1;0];
d = [interp*[1;1;1] wrap];

[x1,x2] = ndgrid(1:dim(1), 1:dim(2));
C = spm_bsplinc(Vo, d);
Va = zeros(dim);
Mask = zeros(dim);

for x3 = 1:dim(3)
	[msk_x3,y1,y2,y3] = getmask(M,x1,x2,x3,dim,wrap);
	Mask(:,:,x3) = msk_x3;
    Va(:,:,x3) = spm_bsplins(C, y1,y2,y3, d);
end
return;

function [Mask,y1,y2,y3] = getmask(M,x1,x2,x3,dim,wrp)
tiny = 5e-2; % From spm_vol_utils.c
y1   = M(1,1)*x1+M(1,2)*x2+(M(1,3)*x3+M(1,4));
y2   = M(2,1)*x1+M(2,2)*x2+(M(2,3)*x3+M(2,4));
y3   = M(3,1)*x1+M(3,2)*x2+(M(3,3)*x3+M(3,4));
Mask = true(size(y1));
if ~wrp(1), Mask = Mask & (y1 >= (1-tiny) & y1 <= (dim(1)+tiny)); end
if ~wrp(2), Mask = Mask & (y2 >= (1-tiny) & y2 <= (dim(2)+tiny)); end
if ~wrp(3), Mask = Mask & (y3 >= (1-tiny) & y3 <= (dim(3)+tiny)); end
return;


function [x_mu, y_mu sig] = localizew(w,peval,localMaxRadius)
% [x_mu, y_mu sig] = localizew(w,peval,localMaxRadius)
% Localilzes w (#pixels x #components) from NMF model (V=WH)
% peval : parameters (needed peval.nx, peval.ny)
% localMaxDiameter : diameter in pixels to which confine a local maximum search. If set to 0 (default) no confinemend is done.
if ~exist('localMaxRadius', 'var')
    localMaxRadius = 0; % Finding global max.
end
K=size(w,2);
wr=reshape(w,peval.nx, peval.ny,K); %K can be different from peval.ncomp if background is takes as one component...
[x,y] = meshgrid(1:peval.ny, 1:peval.nx); % nx and ny has to be swapped as in the image the first coordiante is number of rows
if localMaxRadius
    fprintf('Looking for local maximum %g only.\n', localMaxRadius);
end

x_mu=inf(1,K);
y_mu=inf(1,K);
sig=inf(1,K);
differ=inf(1,K);
for ii=1:K %background is not localized...
    [x_mu(ii), y_mu(ii), sig(ii), differ(ii)] = fitgauss2d(wr(:,:,ii));
    if localMaxRadius>0
        confinementMaskTmp=(x-x_mu(ii)).^2+(y-y_mu(ii)).^2<=localMaxRadius^2;
        [ymtmp,xmtmp]=find(wr(:,:,ii)==max(max(wr(:,:,ii).*confinementMaskTmp)),1);
        if isempty(xmtmp)&&isempty(ymtmp)
            xmtmp=10*peval.nx;
            ymtmp=xmtmp;
            fprintf('Problem with maximum localisation.\n');
        end
        confinementMask=(x-xmtmp).^2+(y-ymtmp).^2<=localMaxRadius^2; % mask centered on the local maximum
        [x_mu(ii), y_mu(ii), sig(ii), differ(ii)] = fitgauss2d(wr(:,:,ii).*confinementMask);
    end

end

if sum(isinf([x_mu, y_mu,sig,differ]))
    warning('Problem with localisation...')
end


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

clear all;
%close all;
clf

num=10;
den=[1 1 0];
w=logspace(-2,1,100);
[m,p]=bode(num,den,w);
wa=[.01 1.2];
ma=[1000 8.3333];
loglog(w,m,w,10*ones(1,100),'--',wa,ma);
xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.13 Determination of Kv from the Bode plot');
bodegrid;


function [strshocks_record]=strshocks(beta_gibbs,D_record,Y,X,n,k,It,Bu,favar)

% first create the call storing the results
strshocks_record=cell(n,1);
Bgibbs=reshape(beta_gibbs,k,n,It-Bu);
Dgibbs=reshape(D_record,n,n,It-Bu);

% recall X and Y from the sampling process in this case
if favar.FAVAR==1
    if isfield(favar,'bvarXY')==1
        bvarXY=1;
        Xgibbs=reshape(favar.X_gibbs,size(X,1),size(X,2),It-Bu);
        Ygibbs=reshape(favar.Y_gibbs,size(Y,1),size(Y,2),It-Bu);
    else
        bvarXY=0;
    end
else
    bvarXY=0;
end

% then loop over iterations
for ii=1:It-Bu

    % recover the VAR coefficients, reshaped for convenience
    B=squeeze(Bgibbs(:,:,ii));

    if bvarXY==1
        X=squeeze(Xgibbs(:,:,ii));
        Y=squeeze(Ygibbs(:,:,ii));
    end

    % obtain the residuals from (XXX)
    EPS=Y-X*B;

    % then recover the structural marix D
    D=squeeze(Dgibbs(:,:,ii));

    % obtain the structural disturbances from (XXX)
    ETA=D\EPS';

    % save in struct_shocks_record
    for jj=1:n
        strshocks_record{jj,1}(ii,:)=ETA(jj,:);
    end

end


function [ber,rate] = Mrate_method()
% traditional SVD algorithm for rate maximization

global  H Ns V_ropt W_ropt;
[U,~,V] = svd(H);
V_ropt = V(:,1:Ns);
%power constraint
V_ropt = V_ropt / norm(V_ropt,'fro');
W_ropt = U(:,1:Ns);

ber = get_ber(V_ropt,W_ropt);
rate = get_rate(V_ropt,W_ropt);


function Fs = fu2F7(u)

Z = lin_fm(u);

NullSp   = null(Z);
if size(NullSp,2) > 2
   Fs = [];
   return; %degenerated sample
end

F1    = reshape(NullSp(:,1),3,3);
F2    = reshape(NullSp(:,2),3,3);
p = fslcm(F1,F2);
aroots = rroots(p);

%xr = o_fslcm(F1,F2)

%aroots == xr

for i = 1:length(aroots)
   l  = aroots(i);
   Ft    = F1 * l + F2 * (1-l);
%   Ft = Ft /norm(Ft,2);
   Fs(:,:,i) = Ft;
end


clc
clear all
addpath(genpath('./'));

tol = 5e-6; % optimality tolerance for stopping_type 1
DB=20; % SNR of the video
frame_array=[35,100,125]; % frames that will be shown at the end

seed = 602;
randn('state',seed); rand('state',seed);
if exist('Hall_airport_1000_1496_497_144_176_gray.mat')==0
    error('Video file cannot be found. Please download it from: http://www2.ie.psu.edu/aybat/Hall_airport_1000_1496_497_144_176_gray.mat')
else
    load Hall_airport_1000_1496_497_144_176_gray.mat;
end
D = images(:,1:201);
n1=144*176; n2=201;
stdev = norm(D,'fro')/(sqrt(144*176*201)*10^(DB/20));
D = D+stdev*randn(144*176,201);
[X,S]=nsa(D,stdev,tol,1);
figure
plot_data(frame_array,D,X,S,144,176)


function cache = score_init_cache(N,L)
% SCORE_INIT_CACHE generate an empty cache for local computation in structure learning
% cache = score_init_cache(number_of_nodes,cache_size)
%
% For 2 nodes with cache of size 5 :
%
% cache =
%   Nw  b        0      0      0 --> Nw=number of writings in cache (+1) and b==1 iff the cache is full
%   0   0        1   -239.12   1 --> 1st familly in the cache (node 1 without parents) calculate with bic
%   0   0        2   -318.98   1
%   1   0        2   -189.23   2 --> 3rd familly in the cache (node 2 with 1 as parent) calculate with bayesian
%   0   1        1   -251.09   1
%   0   0        0      0      0 --> empty entry
%   |   |        |      |      |
%   |   |        |      |      |___> scoring function : 1 for 'bic', 2 for 'bayesian', ...
%   |   |        |      |__________> local score of the familly
%   |   |        |_________________> son node of the familly
%   |   |__________________________> ==1 iff node 2 is parent of son node
%   |______________________________> ==1 iff node 1 is parent of son node
%
%
% V1.1 : 6 may 2003 (O. Francois - francois.olivier.c.h@gmail.com, Ph. Leray - philippe.leray@univ-nantes.fr)
%
%

cache=zeros(L+1,N+3);
cache(1,1)=2;

% using a sparse matrix does not improve performances


%%%% EVALUATES CONSTRIANT ERROR

% Course: Robotic Manipulation and Mobility
% Advisor: Dr. V. Krovi
%
% Homework Number: MIDTERM
%
% Names: Sourish Chakravarty
% 	Hrishi Lalit Shah

function [Cerr]= CONSTRAINT_ERROR_1(th1,x2,y2,th2)

global l1 lc1 l2 lc2

Cerr=[];

for i=1:length(th1)

    c1=cos(th1(i));
    c2=cos(th2(i));
    s1=sin(th1(i));
    s2=sin(th2(i));

    C= [x2(i)-l1*c1-lc2*c2;
        y2(i)-l1*s1-lc2*s2];
    Cerr=[Cerr;abs(C')];
end

return


function [qualMeas]=Measure_Quality(res_prev,res,QualMeasOpts)
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% This file is part of the TIGRE Toolbox
%
% Copyright (c) 2015, University of Bath and
%                     CERN-European Organization for Nuclear Research
%                     All rights reserved.
%
% License:            Open Source under BSD.
%                     See the full license at
%                     https://github.com/CERN/TIGRE/blob/master/LICENSE
%
% Contact:            tigre.toolbox@gmail.com
% Codes:              https://github.com/CERN/TIGRE/
% Coded by:           Manasavee Lohvithee
%--------------------------------------------------------------------------

%for loop over parameters to get the name
for ii=1:length(QualMeasOpts)
    opt=QualMeasOpts{ii};

    switch opt
        case 'RMSE'
         q=RMSE(res_prev,res);

        case 'CC'
         q=CC(res_prev,res);

        case 'MSSIM'
         q=MSSIM(res_prev,res);

        case 'UQI'
         q=UQI(res_prev,res);
        case 'error_norm'
         q=im3Dnorm(res_prev-res,'L2');

    end

    qualMeas(ii)=q;
end

end


function W = upgrading_merge(W, miu)
N = size(W, 2);%N is not a constant if merge is needed
if N <= miu
    return
else
%         sum_before_merge = 0;
%         for i = 1 : N/2
%           sum_before_merge = sum_before_merge + norm(W(:, i) - W(end:-1:1, N - i + 1));
%         end
%         sum_before_merge
%
%         if sum_before_merge > 0.01
%             sum(W(1, :))
%              sum(W(2, :))
%              W
%         end
%
%         if sum_before_merge ~= 0
%             sum_before_merge
%             W
%             sum(W(1, :))
%             sum(W(2, :))
%             LLR = log(W(1,:)) - log(W(2,:))
%             [~, orderd] = sort(LLR, 'descend')
%         end

    epsilon = 1e-3;
    while(N > miu)

        W_first_half = W(:, 1 : N/2);
        LR = W_first_half(1, :)./W_first_half(2, :);
        numerical_warning = 0;

        for i = 1 : N/2 - 1
            ratio = LR(i)/LR(i + 1);
            if ratio < 1 + epsilon
                numerical_warning = 1;
                break;
            end
        end

        if numerical_warning == 1

            min_deltaI = realmax;
            min_index = 0;
            for i = 1 : N/2 - 1
                a2 = W(1, i);
                b2 = W(2, i);
                a1 = W(1,i + 1);
                b1 = W(2, i + 1);
                deltaI = delta_capacity_lemma9(a1, a2, b1, b2);
                if deltaI < min_deltaI %find minimum delta I
                    min_deltaI = deltaI;
                    min_index = i;
                end
            end

            if min_index == 0
                for k = 1 : N/2
                    if sum(W(:, k)) < 1e-20
                        min_index = k;
                        W(:, min_index) = [];
                        W(:, N - min_index) = [];
                        N = size(W, 2);
                        break;
                    end
                end
                continue;
            end

            a2 = W(1, min_index);
            b2 = W(2, min_index);
            a1 = W(1, min_index + 1);
            b1 = W(2, min_index + 1);

            if a2/b2 < inf
                lambda2 = a2/b2;
                alpha2 = lambda2 * (a1 + b1)/(lambda2 + 1);
                beta2 = (a1 + b1)/(lambda2 + 1);
            else
                alpha2 = a1 + b1;
                beta2 = 0;
            end

            W(1, min_index) =  a2 + alpha2;
            W(2, min_index) =  b2 + beta2;
            W(1, N - min_index + 1) =  b2 + beta2;
            W(2, N - min_index + 1) =  a2 + alpha2;

            W(:, min_index + 1) = [];
            W(:, N - min_index - 1) = [];
            N = size(W, 2);

        else

            min_deltaI = realmax;
            min_index = 0;

            for i = 1 : N/2 - 2
                a3 = W(1, i);
                b3 = W(2, i);
                a2 = W(1,i + 1);
                b2 = W(2, i + 1);
                a1 = W(1,i + 2);
                b1 = W(2, i + 2);

                deltaI = delta_capacity_lemma11(a1, a2, a3, b1, b2, b3);

                if deltaI < min_deltaI %find minimum delta I
                    min_deltaI = deltaI;
                    min_index = i;
                end
            end

            if min_index == 0
                for k = 1 : N/2
                    if sum(W(:, k)) < 1e-20
                        min_index = k;
                        W(:, min_index) = [];
                        W(:, N - min_index) = [];
                        N = size(W, 2);
                        break;
                    end
                end
                continue;
            end

            a3 = W(1, min_index);
            b3 = W(2, min_index);
            a2 = W(1, min_index + 1);
            b2 = W(2, min_index + 1);
            a1 = W(1, min_index + 2);
            b1 = W(2, min_index + 2);

            lambda1 = a1/b1;
            if a3/b3 < inf
                lambda3 = a3/b3;
                alpha1 = lambda1 * (lambda3 * b2 - a2) / (lambda3 - lambda1);
                beta1 = (lambda3 * b2 - a2) / (lambda3 - lambda1);
                alpha3 = lambda3 * (a2 - lambda1 * b2) / (lambda3 - lambda1);
                beta3 = (a2 - lambda1 * b2) / (lambda3 - lambda1);
            else
                alpha1 = lambda1 * b2;
                beta1 = b2;
                alpha3 = a2 - lambda1 * b2;
                beta3 = 0;
            end

            W(1, min_index) =  a3 + alpha3;
            W(2, min_index) =  b3 + beta3;

            W(1, min_index + 1) =  a1 + alpha1;
            W(2, min_index + 1) =  b1 + beta1;

            W(1, N - min_index + 1) =  b3 + beta3;
            W(2, N - min_index + 1) =  a3 + alpha3;

            W(1, N - min_index) =  b1 + beta1;
            W(2, N - min_index) =  a1 + alpha1;

            W(:, min_index + 2) = [];
            W(:, N - min_index - 2) = [];
            N = size(W, 2);

%             if N == miu
%                 W
%             end
        end
    end

%         sum_after_merge = 0;
%         for i = 1 : N/2
%             sum_after_merge = sum_after_merge + norm(W(:, i) - W(end:-1:1, N - i + 1));
%         end
%         sum_after_merge
% W
end
end


function im_o   =  Add_noise( im, v )

seed  =  0;
randn( 'state', seed );
noise     =  randn( size(im) );
% noise     =  noise/sqrt(mean2(noise.^2));
im_o      =  double(im) + v*noise;


nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard deviation of computing time; third row: infeasible rate
impcdata=zeros(3,6);
for ii = 1:6
i=ii*4;
filenm1 = ['timecom' num2str(i) '.mat'];
filenm2 = ['feasibility' num2str(i) '.mat'];
load(filenm1);
load(filenm2);
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
nmpcdata(1,ii)=mu1;
impcdata(1,ii)=mu2;
nmpcdata(2,ii)=sigma1;
impcdata(2,ii)=sigma2;
nmpcdata(3,ii)=nmpcinf;
impcdata(3,ii)=impcinf;
end


% MAIN.m  --  Particle Swarm Optimization
%
% This script is used to run particle swarm optimization on several test
% functions.
%

quadraticBowl = @(x)( sum(x.^2,1) );
quadraticBowlSkew2d = @(x)( 0.3*x(1,:).^2 + 4.2*x(2,:).^2 );
noisyBowl = @(x)( sum(x.^2 + 0.1*randn(size(x)),1) );

problem.options.populationCount = 15;   % Number of particles in the search
problem.options.w = 0.3;  % Particle damping coefficient
problem.options.pg = 0.7;   % Global search coefficient
problem.options.pl = 0.5;   % Local search coefficient
problem.options.maxIter = 20;  % Number of iterations

problem.xLow = -ones(2,1);
problem.xUpp = ones(2,1);

problem.objFun = quadraticBowl;
% problem.objFun = quadraticBowlSkew2d;
% problem.objFun = noisyBowl;

soln = particleSwarmOptimization(problem);

%%%% Plotting:
figure(1);
for i=1:problem.options.maxIter
    plotIterData(soln,i);   %Only works for 2D objective function
    pause(0.5);
end


% RSADJUST - adjust l-values (Ramberg-Schmeiser distribution)
%                with respect to signal mean and variance
%
% Usage: p = rsadjust(l3, l4, m, var, skew)
%
% Input:
%   l3   - value lambda3 for Ramberg-Schmeiser distribution
%   l4   - value lambda4 for Ramberg-Schmeiser distribution
%   m    - mean of the signal distribution
%   var  - variance of the signal distribution
%   skew - skewness of the signal distribution (only the sign of
%          this parameter is used).
%
% Output:
%   l1  - value lambda3 for Ramberg-Schmeiser distribution
%   l2  - value lambda4 for Ramberg-Schmeiser distribution
%   l3  - value lambda3 for Ramberg-Schmeiser distribution (copy
%         from input)
%   l4  - value lambda4 for Ramberg-Schmeiser distribution (copy
%         from input)
%
% Author: Arnaud Delorme, SCCN, 2003
%
% See also: RSFIT, RSGET
%
% Reference: Ramberg, J.S., Tadikamalla, P.R., Dudewicz E.J., Mykkytka, E.F.
%            A probability distribution and its uses in fitting data.
%            Technimetrics, 1979, 21: 201-214.

% Copyright (C) 2003 Arnaud Delorme, SCCN, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% 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.

function [l1,l2,l3,l4] = rsadjust( l3, l4, mu, sigma2, m3);

    % swap l3 and l4 for negative skewness
    % ------------------------------------
    if m3 < 0
        ltmp = l4;
        l4   = l3;
        l3   = ltmp;
    end

    A = 1/(1 + l3) - 1/(1 + l4);
    B = 1/(1 + 2*l3) + 1/(1 + 2*l4) - 2*beta(1+l3, 1+l4);
    C = 1/(1 + 3*l3) - 1/(1 + 3*l4) ...
             - 3*beta(1+2*l3, 1+l4) + 3*beta(1+l3, 1+2*l4);

    % compute l2 (and its sign)
    % ------------------------
    l2 = sqrt( (B-A^2)/sigma2 );
    if m3 == 0, m3 = -0.000000000000001; end
    if (m3*(C - 2*A*B + 2*A^3)) < 0, l2 = -l2; end
    %l22 = ((C - 2*A*B + 2*A^3)/m3)^(1/3) % also equal to l2

    % compute l1
    % ----------
    l1 = mu - A/l2;

    return;

    % fitting table 1 of
    % ---------------
    [l1 l2]  = pdffitsolv2(-.0187,-.0388, 0, 1, 1)
    [l1 l2]  = pdffitsolv2(-.1359,-.1359, 0, 1, 0)

    % sign problem
    [l1 l2]  = pdffitsolv2(1.4501,1.4501, 0, 1)

    % numerical problem for l1
    [l1 l2]  = pdffitsolv2(-.00000407,-.001076, 0, 1, 2)
    [l1 l2]  = pdffitsolv2(0,-.000580, 0, 1, 2)


function VectorFitting_Sparam ()
clear; close all; FONTSIZE=20;

file='../DC_ringmod_type1_R=10,gap=180,Lc=0,wg=500,lambda=1550,mesh=2,angle=30.mat';
load (file)

c=3e8; wavelength=c./f*1e6;
fSCALING = 1e14; f=f/fSCALING; s=1i*2*pi*f;  Np=length(f);
hwait = waitbar(0,'Please wait...');

% average the S parameters for the symmetric parameters that were simulated twice,
% by considering amplitude and phase separately
S1221 = (abs(S12)+abs(S21))/2 .* exp ( 1i * (unwrap(angle(S12)) + unwrap(angle(S21)) ) /2 );
S12=S1221; S21 = S1221;
S4132 = (abs(S41)+abs(S32))/2 .* exp ( 1i * (unwrap(angle(S41)) + unwrap(angle(S32)) ) /2 );
S41=S4132; S32 = S4132;
S13=S31; S23=S41; S33=S11; S43=S21; S14=S32; S24=S42; S34=S12; S44=S22;

for i=1:Np
  Sparam = [ [ S11(i),S12(i),S13(i),S14(i)];
    [ S21(i),S22(i),S23(i),S24(i)];
    [ S31(i),S32(i),S33(i),S34(i)];
    [ S41(i),S42(i),S43(i),S44(i)] ] ;
  Test1(i) = norm(Sparam);
  Sparam_w (:,:,i)=Sparam;
end

% Check if S-Parameters are already passive; if not, perform Vector Fit,
% otherwise, export.
if ~isempty(find(Test1>1))

  % rational fit, sweep number of parameters, N:
  optsN=20:1:100;
  opts.poletype='lincmplx'; opts.parametertype='S'; opts.stable=0; opts.Niter_out=10;
  Npassive=[]; rms3passive=[];
  for i=1:length(optsN)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    opts.N=optsN(i);    % Vector Fit:
    [SER,rmserr,Hfit,opts2]=VFdriver(Sparam_w,s,[],opts);

    %%%%%%%%%%%%%%%%%%%%%%  Enforce Passivity:
    [SER,H_passive,opts3,wintervals]=RPdriver(SER,s,opts);
    % Note: added output parameter wintervals to RPdriver.

    % Find rms error:
    tell=0; Nc=length(SER.D);
    for col=1:Nc
      for row=col:Nc % makes assumption that S = S'
        tell=tell+1; % make a single vector:
        Sparam10(tell,:)= squeeze(Sparam_w(row,col,:)).';
        fit10(tell,:)= squeeze(Hfit(row,col,:)).';
        fitP(tell,:)= squeeze(H_passive(row,col,:)).';
      end
    end
    rms2(i) = sqrt(sum(sum(abs((Sparam10-fit10).^2))))/sqrt(4*Np);
    rms3(i) = sqrt(sum(sum(abs((Sparam10-fitP).^2))))/sqrt(4*Np);

    % Determine if the Passivity Enforcement was successful.
    if (rms3(i) < 1) && isempty(wintervals)
      Npassive=[Npassive optsN(i)]; rms3passive = [rms3passive rms3(i)];
    end

    waitbar (i/length(optsN), hwait);
    if (rms3(i) < 1e-4) && isempty(wintervals) ; break; end
  end
  close (hwait);

  % Plot error versus number of fitting parameters:
  figure;
  semilogy(optsN(1:i),rms2,'ro-', 'LineWidth',2, 'MarkerSize',7); hold all;
  labels={}; labels{end+1} = 'Vector Fit, rms';
  plot (Npassive, rms3passive, 'kx', 'MarkerSize',14, 'LineWidth',3);
  labels{end+1} = 'Passivity-Enforced Fit, rms';
  xlabel('Fitting order, N');   ylabel('rms error');
  for ii=1:length(labels); labels{ii}=[labels{ii} ' ' char(31) ]; end;
  legend (labels,'Location','NorthEast');
  printfig(file,'convergence');

  % Plot residue values:
  figure;
  residues=sort(squeeze(prod(prod(abs(SER.R),1),2).^(1/4)), 1, 'descend');
  semilogy(residues,'-s', 'LineWidth',3, 'MarkerSize',8);
  xlabel('Residue index'); ylabel('Residue magnitude');
  printfig(file,'residues');

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Plot S-Parameters + fit functions:
  figure;
  Nc=length(SER.D);
  for row=1:Nc
    for col= row:Nc
      dum1=squeeze(Sparam_w(row,col,:));
      dum2=squeeze(H_passive(row,col,:));
      h1=semilogy(wavelength,abs(dum1),'b','LineWidth',3); hold on
      h2=semilogy(wavelength,abs(dum2),'r-.','LineWidth',4);
      h3=semilogy(wavelength,abs(dum2-dum1),'g--','LineWidth',3);
    end
  end
  hold off
  xlabel('Wavelength'); ylabel('Amplitude [S]');
  axis tight; Yl =ylim; ylim ([Yl(1)*10 Yl(2)*2]);
  labels={}; labels{end+1} = 'FDTD S-Parameters';
  labels{end+1} = 'Passivity-enforced Fit'; labels{end+1}='Deviation';
  for i=1:length(labels); labels{i}=[labels{i} ' ' char(31)]; end;
  legend (labels,'Location','Best');
  printfig(file,'VF2a');

  figure;
  Nc=length(SER.D);
  for row= 1:Nc  %1
    for col= row:Nc  %3
      dum1=squeeze(Sparam_w(row,col,:));
      dum2=squeeze(H_passive(row,col,:));
      h1=semilogy(wavelength,unwrap(angle(dum1)),'b','LineWidth',3); hold on
      h2=plot(wavelength,unwrap(angle(dum2)),'r-.','LineWidth',4);
      h3=plot(wavelength,abs(unwrap(angle(dum2))-unwrap(angle(dum1))),'g--','LineWidth',3);
    end
  end
  hold off
  xlabel('Wavelength'); ylabel('Phase [S]');
  axis tight; Yl =ylim; ylim ([Yl(1)*10 Yl(2)*2]);
  labels={}; labels{end+1} = 'FDTD S-Parameters';
  labels{end+1} = 'Passivity-enforced Fit'; labels{end+1}='Deviation';
  for i=1:length(labels); labels{i}=[labels{i} ' ' char(31)]; end;
  legend (labels,'Location','Best');
  printfig(file,'VF2p');

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Passivity test results:
  for i=1:length(f)
    Test0(i) = norm(Sparam_w(:,:,i));
    Test1(i) = norm(Hfit(:,:,i));
    Test2(i) = norm(H_passive(:,:,i));
  end
  figure;
  plot (wavelength,Test0,'LineWidth',2); hold all;
  plot (wavelength,Test1,'--','LineWidth',2)
  plot (wavelength,Test2,'LineWidth',4)
  plot(wavelength,ones(length(f),1),'--','LineWidth',3);
  labels={}; labels{1} = 'FDTD S-Parameters'; labels{2} = 'Rational Fit'; labels{3} = 'Passivity-enforced Fit'; labels{4}='Passivity limit';
  for i=1:length(labels); labels{i}=[labels{i} ' ' char(31)]; end; legend (labels,'Location','Best');
  axis tight
  xlabel ('Wavelength [\mum]');
  ylabel ('S-Parameter Passivity Test');
  printfig(file,'passivitytest3');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% export S parameters to INTERCONNECT
fid = fopen([file '.sparam'],'w');  Nc=4;
for row= 1:Nc
  for col= 1:Nc
    fprintf(fid,'%s\n',[ '(''port ' num2str(col) ''',''TE'',1,''port ' num2str(row) ''',1,''transmission'')' ]  );
    fprintf(fid,'%s\n',[ '(' num2str(Np) ',3)' ]  );
    dum2=squeeze(H_passive(row,col,:));
    mag = abs(dum2);
    phase = unwrap (angle(dum2)); % figure; plot (wavelength, phase);
    for i=1:Np
      fprintf(fid,'%g	%g	%g\n', f(i)*1e14, mag(i), phase(i));
    end
  end
end
fclose(fid);

function printfig (file, b)
global PRINT_titles;
PRINT_titles=0;
FONTSIZE=20;
set(get(gca,'xlabel'),'FontSize',FONTSIZE);
set(get(gca,'ylabel'),'FontSize',FONTSIZE);
set(get(gca,'title'),'FontSize',FONTSIZE-5);
set(gca,'FontSize',FONTSIZE-2);
if PRINT_titles==0
  delete(get(gca,'title'))
end
%a=strfind(file,'.'); file(a)=',';
pdf = [file(1:end-4) '_' b '.pdf'];
print ('-dpdf','-r300', pdf);
system([ 'pdfcrop ' pdf ' ' pdf ' &' ]);
% system(['acroread ' pdf '.pdf &']);


report_this_filefun(mfilename('fullpath'));

m = [];
m =0:0.1:max(newt2.Magnitude);
m2 =min(newt2.Magnitude):0.1:max(newt2.Magnitude);
m3 = 10.^(-(m2-min(newt2.Magnitude)));
m3 = m3*newt2.Count;

k = 0:0.1:min(newt2.Magnitude)-0.1;
m4 = k*8+newt2.Count;
m = [m4 m3];
clf
%plot(0:0.1:max(newt2.Magnitude),log10(m))
%grid
%hold on
newcat = newt2;

lepo = length(m) -1;
mm = [newt2.Count m(1:lepo) ];
bval = mm-m;
bvalfl = bval(length(bval):-1:1);
maxmag = max(newcat.Magnitude);
mima = min(newcat.Magnitude);
if mima > 0 ; mima = 0 ; end

% number of mag units
nmagu = (maxmag*10)+1;

bvalsum = zeros(1,nmagu);
bvalsum3 = zeros(1,nmagu);

bvalsum = m;
bvalsum3 = m(length(m):-1:1);
xt3 = (maxmag:-0.1:mima);

backg_be = log10(bvalsum);
backg_ab = log10(bvalsum3);
orient tall
rect = [0.2,  0.3, 0.70, 0.6];           % plot Freq-Mag curves
axes('position',rect);

semilogy(xt3,bvalsum3,'-.m')
hold on
semilogy(xt3,bvalsum3,'om')
difb = [0 diff(bvalsum3) ];
semilogy(xt3,difb,'xg')
semilogy(xt3,difb,'g')
grid

% Marks the point of maximum curvature
%
i = find(difb == max(difb));
te = semilogy(xt3(i),difb(i),'xk');
set(te,'LineWidth',2,'MarkerSize',ms10)
te = semilogy(xt3(i),bvalsum3(i),'xk');
set(te,'LineWidth',2,'MarkerSize',ms10)

% Estimate the b-value
%
i2 = round(i/3);
te = semilogy(xt3(i2),difb(i2),'xk');
set(te,'LineWidth',2,'MarkerSize',ms10)
te = semilogy(xt3(i2),bvalsum3(i2),'xk');
set(te,'LineWidth',2,'MarkerSize',ms10)

xlabel('Magnitude','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
ylabel('Cumulative Number','FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
set(gca,'Color',[1 1 0.6])
set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.m,'FontWeight','bold',...
    'FontWeight','bold','LineWidth',1.5,...
    'Box','on')

par2 = 0.1 * max(bvalsum3);
par3 = 0.12 * max(bvalsum3);
M1b = [];
M1b = [xt3(i) bvalsum3(i)];
tt3=num2str(fix(100*M1b(1))/100);
text( M1b(1),M1b(2),['|: M1=',tt3] )

M2b = [];
M2b =  [xt3(i2) bvalsum3(i2)];
tt4=num2str(fix(100*M2b(1))/100);
text( M2b(1),M2b(2),['|: M2=',tt4] )

ll = xt3 > M1b(1) & xt3 < M2b(1);
x = xt3(ll);

l = newcat.Magnitude > M1b(1) & newcat.Magnitude < M2b(1);
me = 0.4343/(sum(bval.*xt3)/(sum(bval))-M1b(1));
mer = 1.96*me/(sqrt(length(newcat(l,6))));

so = log10(bval(10*M1b(1)+2)) - log10(bval(10*M2b(1)));
me= so/( M2b(1)-0.2- M1b(1));

pause(0.1)

y = backg_ab(ll);
[p,s] = polyfit(x,y,1);                   % fit a line to background
f = polyval(p,x);
f = 10.^f;
hold on
ttm= semilogy(x,f,'b');                         % plot linear fit to backg
set(ttm,'LineWidth',2)
r = corrcoef(x,y);
r = r(1,2);
std_backg = std(y - polyval(p,x));      % standard deviation of fit

p=-p(1,1);
p=fix(100*p)/100;
std_backg=fix(100*std_backg)/100;
tt2=num2str(std_backg);
tt1=num2str(p);

n = length(x)+3;
l = b(:,6) > M1b(1) & b(:,6) <= M2b(1);
les = (mean(b(l,6)) - (M1b(1)+0.05))/0.1;

mysofu = @(x)sofu(x,n,les);
so = fzero(mysofu,1.0);
me2 = log(so)/(-2.3026*0.1)

rect=[0 0 1 1];
h2=axes('position',rect);
set(h2,'visible','off');

txt1=text(.16, .18,['B-Value(L2): ',tt1,'  B(eff): ',num2str(me), '  B(mean2)= ',num2str(me2)]);
set(txt1,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
txt1=text(.16, .1,['Standard Deviation: ',tt2]);
set(txt1,'FontWeight','bold','FontSize',ZmapGlobal.Data.fontsz.m)
set(gcf,'visible','on');
zmap_message_center.set_info('  ','Done')
done