classdef QuadrilaterSubTriangulator < handle

    properties (Access = public)

    end

    properties (Access = private)
        subTriangulation
    end

    properties (Access = private)
        localIncidenceMatrix
        coord
        connec
        xLocalVertex
    end

    methods (Access = public)

        function obj = QuadrilaterSubTriangulator(cParams)
            obj.init(cParams)
        end

        function [connR,xCoordsIsoBoundaryR] = compute(obj)

            for is = 1:2
                subTriang = obj.subTriangulation(:,:,is);
                connecL             = obj.computeSubtriangulation(subTriang);
                conne               = obj.divideInSubTriangle(obj.connec,connecL);

                xB = permute(obj.xLocalVertex,[3 2 1]);
                xCoordsIsoBoundaryS = obj.divideInSubTriangle(xB,connecL);

                xCoordsIsoBoundaryS = permute(xCoordsIsoBoundaryS,[4 1 2 3]);

                conn1(:,:,is) = conne(:,:,1);
                conn2(:,:,is) = conne(:,:,2);
                %connRi(:,:,is) = cat(1,conn1,conn2);

                nodesSubCases(:,:,is,:) = permute(conne,[3 2 1]);

                xCoordsIsoBoundary1(:,:,:,is) = xCoordsIsoBoundaryS(:,:,:,1);
                xCoordsIsoBoundary2(:,:,:,is) = xCoordsIsoBoundaryS(:,:,:,2);

                %  nodesSubCases(:,:,1,:) =
            end

            s.nodesSubCases = nodesSubCases;
            s.coord = obj.coord;
            b = BestSubCellCaseSelector(s);
            imax = b.compute();

            conR1 = zeros(size(conn1(:,:,1)));
            conR2 = zeros(size(conn1(:,:,1)));
            xCoordsIsoBoundaryR1 = zeros(size(xCoordsIsoBoundary1(:,:,:,1)));
            xCoordsIsoBoundaryR2 = zeros(size(xCoordsIsoBoundary1(:,:,:,1)));
            for is = 1:2
                isIndex = imax == is;
                con1 = conn1(:,:,is);
                con2 = conn2(:,:,is);

                conR1(isIndex,:) = con1(isIndex,:);
                conR2(isIndex,:) = con2(isIndex,:);

                xR1 = xCoordsIsoBoundary1(:,:,:,is);
                xR2 = xCoordsIsoBoundary2(:,:,:,is);
                xCoordsIsoBoundaryR1(:,isIndex,:) = xR1(:,isIndex,:);
                xCoordsIsoBoundaryR2(:,isIndex,:) = xR2(:,isIndex,:);

            end
            %   connR = cat(1,conR1,conR2);
            connR = cat(1,conne(:,:,1),conne(:,:,2));

            xCoordsIsoBoundaryRN = cat(2,xCoordsIsoBoundaryR1,xCoordsIsoBoundaryR2);
            xCoordsIsoBoundaryR = permute(xCoordsIsoBoundaryRN,[1 3 2]);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.localIncidenceMatrix = cParams.localIncidenceMatrix;
            obj.coord                = cParams.coord;
            obj.connec               = cParams.connec;
            obj.xLocalVertex         = cParams.xLocalVertex;
            obj.subTriangulation(:,:,1) = [1 2 3; 3 4 1];
            obj.subTriangulation(:,:,2) = [2 3 4; 4 1 2];
        end

        function [xCoordsIsoBoundaryR] = divideInSubTriangle(obj,xB,connecL)

     %       connB = obj.localToGlobal(connecL,connec);

            nElem = size(connecL,1);
            xCoordsIsoBoundaryR = zeros(nElem,3,2,size(xB,3));
            for idim = 1:size(xB,3)
                xBi = (xB(:,:,idim));
                xC = obj.localToGlobal(connecL,xBi);
                xCoordsIsoBoundaryR(:,:,:,idim) = xC;
            end
        end

        function [connecG] = localToGlobal(obj,connecL,dataToTriang)

            for itriang = 1:2
                for i = 1:3
                    nodei1 = connecL(:,i,itriang);
                    index1 = sub2ind(size(dataToTriang),[1:size(connecL,1)]',nodei1);
                    connecG(:,i,itriang) = dataToTriang(index1);
                end
            end

        end

        function nodes = obtainOrderedNodes(obj)
            incMatrix = obj.localIncidenceMatrix;
            nElem = size(incMatrix,1);
            incMatrix = permute(incMatrix,[1 3 2]);
            edge1 = incMatrix(:,:,1);
            edge2 = incMatrix(:,:,2);
            edge3 = incMatrix(:,:,3);
            edge4 = incMatrix(:,:,4);
            areIntersected(:,1) = obj.areEdgesIntersected(edge1,edge2);
            areIntersected(:,2) = obj.areEdgesIntersected(edge1,edge3);
            areIntersected(:,3) = obj.areEdgesIntersected(edge1,edge4);

            integerCases = obj.computeIntegerCases(areIntersected);

            nodes = zeros(nElem,4);
            cases = [3 5 6];
            nodesCases = [1 2 4 3; 1 2 3 4; 1 3 2 4];
            for icase = 1:3
                isCase = integerCases == cases(icase);
                for inode = 1:4
                    nodes(isCase,inode) = nodesCases(icase,inode);
                end
            end

        end

        function connec = computeSubtriangulation(obj,subTriang)
            nodes = obj.obtainOrderedNodes();
            connec = zeros(size(nodes,1),3,2);
            for inode = 1:3
                for itriangle = 1:2
                    connec(:,inode,itriangle) = nodes(:,subTriang(itriangle,inode));
                end
            end
        end

    end

    methods (Access = private, Static)

        function [areInt] = areEdgesIntersected(edge1,edge2)
            isNode11 = edge1(:,1) == edge2(:,1);
            isNode12 = edge1(:,1) == edge2(:,2);
            isNode21 = edge1(:,2) == edge2(:,1);
            isNode22 = edge1(:,2) == edge2(:,2);
            areInt = any([isNode11 isNode12 isNode21 isNode22],2);
        end

        function integerCases = computeIntegerCases(isNeigbour)
            nodes = isNeigbour;
            nnode = size(nodes,2);
            nodePos = (1:nnode) - 1;
            pow2vector = 2.^(nodePos);
            d = pow2vector*nodes';
            integerCases = d;
        end

    end

end


function [ know, x ] = p22_sol ( n )

%*****************************************************************************80
%
%% P22_SOL returns the solution for problem 22.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 December 2000
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  know = 1;

  x = zeros ( n, 1 );

  return
end


filename='Cantilever_triangle_fine';
ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'compliance'};
weights = 1;
constraint = {'volume'};
optimizer = 'MMA';
incrementFactor = 1;
designVariable = 'Density';
filterType = 'P1';
constraint_case = 'INEQUALITY';
line_search_initiator = 'STANDARD';

nsteps = 15;
Vfrac_final = 0.4;
Perimeter_target = 1;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function [pvec, pstruct] = tapas_condhalluc_obs2_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

pvec(1)    = exp(ptrans(1));         % be
pstruct.be = pvec(1);

pvec(2)    = exp(ptrans(2));         % nu
pstruct.nu = pvec(2);

return;


% This is selgp3dB_x

figure;

plot(Da(:,1),-Da(:,7),'.k','MarkerSize',1);
xlabel('Distance in [km]')
ylabel('Depth in [km]')
axis image
set(gca,'visible','on','FontSize',ZmapGlobal.Data.fontsz.s,...
    'LineWidth',1.0,'TickDir','out','Ticklength',[0.02 0.02],...
    'Box','on')

ax = findobj('Tag','main_map_ax');
[x,y, mouse_points_overlay] = select_polygon(ax);

zmap_message_center.set_info('Message',' Thank you .... ')

plos2 = plot(x,y,'b-','era','xor', 'Color', 'r');        % plot outline
sum3 = 0.;
pause(0.3)

%create a rectangular grid
xvect=[min(x):dx:max(x)];
yvect=[min(y):dy:max(y)];
zvect=[z2:dz:z1];

gx = xvect;gy = yvect;
tmpgri=zeros((length(xvect)*length(yvect)),2);
tmpgri2=zeros((length(xvect)*length(zvect)),2);

n=0;

for i=1:length(xvect)
    for j=1:length(yvect)
        n=n+1;
        tmpgri(n,:)=[xvect(i) yvect(j)];
    end
end

%extract all gridpoints in chosen polygon
XI=tmpgri(:,1);
YI=tmpgri(:,2);

    ll = polygon_filter(x,y, XI, YI, 'inside');
%grid points in polygon
newgri=tmpgri(ll,:);

newgri=tmpgri(ll,:);

n2 = repmat(newgri,length(zvect),1);
k = repmat(zvect',length(newgri),1);
t3 = [n2(:,1)  k n2(:,2) ];

%n3 = repmat(tmpgri2,length(yvect),1);
%k = repmat((1:1:length(yvect)),length(tmpgri),1);
%k = reshape(k,length(k)*length(yvect),1);
%t4 = [t3 n3 k];

%l = t4(:,4) == 1;
%t5 = t4(l,:);

% Plot all grid points
plot(newgri(:,1),newgri(:,2),'+k','era','back')

figure
plot3(Da(:,1),Da(:,2),-Da(:,7),'.k','MarkerSize',1)
hold on
box on ; %axis image

plot3(t3(:,1),t3(:,2),t3(:,3),'+')

if length(xvect) < 2  ||  length(yvect) < 2
    errordlg('Selection too small! (not a matrix)');
    return
end

itotal = length(newgri(:,1));
if length(gx) < 4  ||  length(gy) < 4
    errordlg('Selection too small! ');
    return
end


%% COLLATZ_POOL uses the MATLABPOOL command to run the COLLATZ code.
%
%  Discussion:
%
%    Output printed by the function appears directly on the screen.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 March 2010
%
%  Author:
%
%    John Burkardt
%
  matlabpool open local 4

  n = 10000000;

  tic
  j_max = collatz_fun ( n );
  toc

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Searched integers 1 through %d\n', n );
  fprintf ( 1, '  Longest Collatz sequence had length %d\n', j_max );

  matlabpool close


%--- help for ts/describe ---
%
%  Print the description of the time series object
%
%  Args:
%
%     db (ts object): time series to describe
%
%  Note:
%
%     The function prints
%
%        - mean
%        - standard deviation
%        - min
%        - 25th percentile
%        - 50th percentile
%        - 75th percentile
%        - max
%        - variable names
%        - variable descriptions
%
%


%% Basic Usage
% This document introduces the basic usage of MaxwellFDFD with example codes.

%% Example Code
% We start with a simple example code that solves a 2D problem, which is to
% examine the transmission of a plane wave through a narrow slit:
[E, H] = maxwell_run(...
	'OSC', 1e-9, 1550, ...
	'DOM', {'vacuum', 'none', 1.0}, [-1100 1100; -1100 2600; 0 10], 10, BC.p, [100 100 0],...
	'OBJ', {'CRC/Ag', 'k'}, ...
		Box([-1100 -80; 0 1000; 0 10]), ...
		Box([80 1100; 0 1000; 0 10]), ...
	'SRCJ', PlaneSrc(Axis.y, -500, Axis.x));

%%%
% The above is actually one line of code; the ellipsis |...| at the end of each
% line is MATLAB's way of indicating the continuation of a line.  Upon
% execution, this code calculates the solution _E_- and _H_-fields of the
% problem.  The solutions are stored in the variables |E| and |H|, which are the
% outputs of <../comp/maxwell_run.html |maxwell_run|>, MaxwellFDFD's core
% function, that takes a long list of arguments describing the problem.

%% Visualizing the Solution
% Before discussing the details of the arguments of |maxwell_run|, let's first
% visualize the solution to understand the problem we are solving better. This
% can be done by another simple line of code:
vis2d(E{Axis.x}, Axis.z, 5);

%%%
% which produces a figure that looks like:
%
% <<../img/basic_01.png>>
%
% Note that the PML regions are excluded from the plot by default.

%% Visualizing of Objects and Sources
% The above field plot would have been much more informative if it was overlaid
% with the objects and source placed in the simulation domain.  The arrays of
% the objects and sources can be obtained from |maxwell_run| by changing the
% output arguments as:
[E, H, obj_array, src_array] = maxwell_run({ARGUMENTS});

%%%
% These objects and sources (actually only one source for the present problem)
% are drawn on top of the field plot when they are supplied to |vis2d| as extra
% input arguments as
vis2d(E{Axis.x}, Axis.z, 5, obj_array, src_array);

%%%
% The modified code will produce an updated figure that looks like:
%
% <<../img/basic_02.png>>
%
% The black lines indicate the boundaies of the metal pieces, and green line
% indicates the location of the source plane.

%% Input Arguments of |maxwell_run|
% |maxwell_run| takes many input arguments that are grouped into several
% *parameter groups*.  Each parameter group is specified by |'NAME'| (such as
% |'OSC'|, |'DOM'|, |'OBJ'|, |'SRCJ'|) that is followed by the arguments in the
% parameter group.  Below, the meanings of the arguments used in the above
% example code are explained for each parameter group.
%
% *'OSC'* specifies the oscillation parameter group, which describes the
% wavelength of the source.
%
% * |1e-9|: the unit of wavelength is 1 nm (= 1e-9 m).
% * |1550|: the wavelength is 1550 nm.
%
% The unit of wavelength specified in this parameter group serves as the unit of
% all the length arguments of |maxwell_run|.
%
% *'DOM'* specifies the domain parameter group, which describes the simulation
% domain of the problem.
%
% * |{'vacuum', 'none', 1.0}|: the background material that fills the simulation
% domain is named |'vacuum'| (just a user-defined name), will be visualized with
% color |'none'| (i.e., the material will not be shown in the figure generated
% by |vis2d|), and its dielectric constant is 1.0.
% * |[-1100 1100; -1100 2600; 0 10]|: the simulation domain lies between -1100
% and 1100 nm in the _x_-direction, -1100 and 2600 nm in the _y_-direction, 0
% and 10 nm in the _z_-direction.
% * |10|: the coarsest grid size used to descritize the simulation domain is 10
% nm.
% * |BC.p|: periodic boundary conditions are used in all the _x_-, _y_-,
% _z_-directions.
% * |[100 100 0]|: the thicknesses of PML are 100 nm at the _x_- and _y_-normal
% boundaries, and no PML is used at the _z_-normal boundaries.  The simulation
% domain except PML lies between -1000 and 1000 nm in the _x_-direction, -1000
% and 2500 nm in the _y_-direction, 0 and 10 nm in the _z_-direction.
%
% *'OBJ'* specifies the object parameter group, which describes the objects
% placed in the simulation domain.
%
% * |{'CRC/Ag', 'k'}|: the material used to create objects has
% frequency-dependent dielectric constants defined in the file |Ag.mat| under
% the subdirectory |dielconst/CRC/| under the main MaxwellFDFD directory.  (In
% fact, the dielectric constants are taken from the silver data in the CRC
% handbook.)  When the objects made of this material are plotted, they will be
% colored black (whose MATLAB color code is |'k'|).
% * |Box([-1100 -80; 0 1000; 0 10])|: place a box made of the material.  The
% box lies between -1100 and -80 nm in the _x_-direction, 0 and 1000 nm in
% the _y_-direction, 0 and 10 nm in the _z_-direction.
% * |Box([80 1100; 0 1000; 0 10])|: place another box made of the material.
%
% *'SRCJ'* specifies the _J_ source parameter group, which describes the
% electric current sources placed in the simulation domain.
%
% * |PlaneSrc(Axis.y, -500, Axis.x))|: at _y_ = -500 nm, place a plane of
% electric dipoles oscillating in the _x_-direction.
%
% Note that the order of appearance of the parameter groups in |maxwell_run| is
% interchangeable; for example, you can put the source parameter group before
% the object parameter group, and vice versa.

%% Inspecting the Design before Solution
% The above example is simple, so it does not hurt to solve the problem without
% making sure that the system you described with the arguments of |maxwell_run|
% is indeed what you intended to examine.  However, for larger and more complex
% problems, the solution process can be time-consuming.  Therefore, in general
% it is a good practice to inspect the design before solving the problem.
%
% You can command |maxwell_run| to visualize the simulation domain without
% solving the problem by appending a logical argument (|true| or |false|) as:
[E, H, obj_array, src_array] = maxwell_run({PARAMETER GROUPS}, true);

%%%
% The above code does not return solution _E_- and _H_-fields in the output
% arguments |E| and |H|, i.e., |E| and |H| are empty, but it produces the
% following figure:
%
% <<../img/basic_03.png>>
%
% The above figure visualizes the objects and source in the simulation domain,
% including the PML regions.

%% Complete Code
% Below is the complete code that has everything discussed in this document.
% Note that |vis2d| works only if the last argument of |maxwell_run| is flipped
% to |false| to calculate the solution.
[E, H, obj_array, src_array] = maxwell_run(...
	'OSC', 1e-9, 1550, ...
	'DOM', {'vacuum', 'none', 1.0}, [-1100 1100; -1100 2600; 0 10], 10, BC.p, [100 100 0],...
	'OBJ', {'CRC/Ag', 'k'}, ...
		Box([-1100 -80; 0 1000; 0 10]), ...
		Box([80 1100; 0 1000; 0 10]), ...
	'SRCJ', PlaneSrc(Axis.y, -500, Axis.x), ...
	false);  % true to inspect arguments without solving equation

vis2d(E{Axis.x}, Axis.z, 5, obj_array, src_array);

%%% See Also
% <intermediate.html |Intermediate Usage|>, <../comp/maxwell_run.html |maxwell_run|>


classdef YawTermCondition < AbstractEventTerminationCondition
    %YawTermCondition Summary of this class goes here
    %   Detailed explanation goes here

    properties
        steeringModel(1,1) AbstractSteeringModel = RollPitchYawPolySteeringModel.getDefaultSteeringModel();
        targetYawAngle(1,1) double = 0;
        bodyInfo KSPTOT_BodyInfo
    end

    methods
        function obj = YawTermCondition(targetYawAngle)
            obj.targetYawAngle = targetYawAngle;
        end

        function evtTermCondFcnHndl = getEventTermCondFuncHandle(obj)
            evtTermCondFcnHndl = @(t,y) obj.eventTermCond(t,y, obj.targetYawAngle, obj.steeringModel, obj.bodyInfo);
        end

        function initTermCondition(obj, initialStateLogEntry)
            obj.steeringModel = initialStateLogEntry.steeringModel;
            obj.bodyInfo = initialStateLogEntry.centralBody;
        end

        function name = getName(obj)
            name = sprintf('Yaw Angle (%.3f deg)', rad2deg(obj.targetYawAngle));
        end

        function tf = shouldBeReinitOnRestart(obj)
            tf = false;
        end

        function params = getTermCondUiStruct(obj)
            params = struct();

            params.paramName = 'Target Yaw Angle';
            params.paramUnit = 'deg';
            params.useParam = 'on';
            params.useStages = 'off';
            params.useTanks = 'off';
            params.useEngines = 'off';
            params.useStopwatches = 'off';

            params.value = rad2deg(obj.targetYawAngle);
            params.refStage = LaunchVehicleStage.empty(1,0);
            params.refTank = LaunchVehicleEngine.empty(1,0);
            params.refEngine = LaunchVehicleEngine.empty(1,0);
            params.refStopwatch = LaunchVehicleStopwatch.empty(1,0);
        end

        function optVar = getNewOptVar(obj)
            optVar = YawAngleTermCondOptimVar(obj);
        end

        function optVar = getExistingOptVar(obj)
            optVar = obj.optVar;
        end

        function tf = usesStage(obj, stage)
            tf = false;
        end

        function tf = usesEngine(obj, engine)
            tf = false;
        end

        function tf = usesTank(obj, tank)
            tf = false;
        end

        function tf = usesEngineToTankConn(obj, engineToTank)
            tf = false;
        end

        function tf = usesStopwatch(obj, stopwatch)
            tf = false;
        end
    end

    methods(Static)
        function termCond = getTermCondForParams(paramValue, stage, tank, engine, stopwatch)
            termCond = YawTermCondition((paramValue));
        end
    end

    methods(Static, Access=private)
        function [value,isterminal,direction] = eventTermCond(t,y, targetYawAngle, steeringModel, bodyInfo)
            ut = t;
            rVect = y(1:3);
            vVect = y(4:6);

            dcm = steeringModel.getBody2InertialDcmAtTime(ut, rVect, vVect, bodyInfo);
            [~,~,yawAngle] = computeEulerAnglesFromInertialBodyAxes(ut, rVect, vVect, bodyInfo, dcm(:,1), dcm(:,2), dcm(:,3));

            yawAngle = AngleZero2Pi(yawAngle);

            value = yawAngle - targetYawAngle;
            isterminal = 1;
            direction = 0;
        end
    end
end


function [net1, avgImg] = initVGG16Net( vgg_16_path )
%VGG16
opts.gpus=1;
cold=true;
%prepareGPUs(opts,cold);

net2=load(vgg_16_path);
net2.layers(31:end) = [];

avgImg=net2.meta.normalization.averageImage;

net1=dagnn.DagNN();

net1.addLayer('conv1_1', dagnn.Conv('size', [3,3,3,64],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'input', 'conv_11', {'conv11_f', 'conv11_b'});
net1.addLayer('relu1_1', dagnn.ReLU(),'conv_11','relu_11');

f = net1.getParamIndex('conv11_f') ;
net1.params(f).value=net2.layers{1}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv11_b') ;
net1.params(f).value=net2.layers{1}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv1_2', dagnn.Conv('size', [3,3,64,64],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_11', 'conv_12', {'conv12_f', 'conv12_b'});
net1.addLayer('relu1_2', dagnn.ReLU(),'conv_12','relu_12');
net1.addLayer('pool1', dagnn.Pooling('poolSize', [2,2], 'pad',...
    [0,1,0,1], 'stride', [2,2]),'relu_12','pool_1');

f = net1.getParamIndex('conv12_f') ;
net1.params(f).value=net2.layers{3}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv12_b') ;
net1.params(f).value=net2.layers{3}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

%-------------------------covn 2--------------------
net1.addLayer('conv2_1', dagnn.Conv('size', [3,3,64,128],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_1', 'conv_21', {'conv21_f', 'conv21_b'});
net1.addLayer('relu2_1', dagnn.ReLU(),'conv_21','relu_21');

f = net1.getParamIndex('conv21_f') ;
net1.params(f).value=net2.layers{6}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv21_b') ;
net1.params(f).value=net2.layers{6}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv2_2', dagnn.Conv('size', [3,3,128,128],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_21', 'conv_22', {'conv22_f', 'conv22_b'});
net1.addLayer('relu2_2', dagnn.ReLU(),'conv_22','relu_22');
net1.addLayer('pool2', dagnn.Pooling('poolSize', [2,2], 'pad',...
    [0,1,0,1], 'stride', [2,2]),'relu_22','pool_2');

f = net1.getParamIndex('conv22_f') ;
net1.params(f).value=net2.layers{8}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv22_b') ;
net1.params(f).value=net2.layers{8}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

%-------------------------covn 3--------------------
net1.addLayer('conv3_1', dagnn.Conv('size', [3,3,128,256],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'pool_2', 'conv_31', {'conv31_f', 'conv31_b'});
net1.addLayer('relu3_1', dagnn.ReLU(),'conv_31','relu_31');

f = net1.getParamIndex('conv31_f') ;
net1.params(f).value=net2.layers{11}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv31_b') ;
net1.params(f).value=net2.layers{11}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv3_2', dagnn.Conv('size', [3,3,256,256],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_31', 'conv_32', {'conv32_f', 'conv32_b'});
net1.addLayer('relu3_2', dagnn.ReLU(),'conv_32','relu_32');

f = net1.getParamIndex('conv32_f') ;
net1.params(f).value=net2.layers{13}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv32_b') ;
net1.params(f).value=net2.layers{13}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv3_3', dagnn.Conv('size', [3,3,256,256],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_32', 'conv_33', {'conv33_f', 'conv33_b'});
net1.addLayer('relu3_3', dagnn.ReLU(),'conv_33','relu_33');

f = net1.getParamIndex('conv33_f') ;
net1.params(f).value=net2.layers{15}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv33_b') ;
net1.params(f).value=net2.layers{15}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

%-------------------------covn 4--------------------
net1.addLayer('conv4_1', dagnn.Conv('size', [3,3,256,512],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_33', 'conv_41', {'conv41_f', 'conv41_b'});
net1.addLayer('relu4_1', dagnn.ReLU(),'conv_41','relu_41');

f = net1.getParamIndex('conv41_f') ;
net1.params(f).value=net2.layers{18}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv41_b') ;
net1.params(f).value=net2.layers{18}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv4_2', dagnn.Conv('size', [3,3,512,512],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_41', 'conv_42', {'conv42_f', 'conv42_b'});
net1.addLayer('relu4_2', dagnn.ReLU(),'conv_42','relu_42');

f = net1.getParamIndex('conv42_f') ;
net1.params(f).value=net2.layers{20}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv42_b') ;
net1.params(f).value=net2.layers{20}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

net1.addLayer('conv4_3', dagnn.Conv('size', [3,3,512,512],...
    'hasBias', true, 'pad',...
    [1,1,1,1], 'stride', [1,1]), 'relu_42', 'conv_43', {'conv43_f', 'conv43_b'});
net1.addLayer('relu4_3', dagnn.ReLU(),'conv_43','relu_43');

f = net1.getParamIndex('conv43_f') ;
net1.params(f).value=net2.layers{22}.weights{1};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1e3;

f = net1.getParamIndex('conv43_b') ;
net1.params(f).value=net2.layers{22}.weights{2};
net1.params(f).learningRate=0;
net1.params(f).weightDecay=1;

% net1.addLayer('pool2_show', dagnn.Crop('crop',[1,1]),...
%     {'pool_2','pool_2'}, 'pool2show');
% net1.addLayer('relu33_show', dagnn.Crop('crop',[1,1]),...
%     {'relu_33','relu_33'}, 'relu33_show');

net1.move('gpu');
clear net2;
end

% -------------------------------------------------------------------------
function clearMex()
% -------------------------------------------------------------------------
clear vl_tflow vl_imreadjpeg ;
end

% -------------------------------------------------------------------------
function prepareGPUs(opts, cold)
% -------------------------------------------------------------------------
numGpus = numel(opts.gpus) ;
if numGpus > 1
  % check parallel pool integrity as it could have timed out
  pool = gcp('nocreate') ;
  if ~isempty(pool) && pool.NumWorkers ~= numGpus
    delete(pool) ;
  end
  pool = gcp('nocreate') ;
  if isempty(pool)
    parpool('local', numGpus) ;
    cold = true ;
  end

end
if numGpus >= 1 && cold
  fprintf('%s: resetting GPU\n', mfilename)
  clearMex() ;
  if numGpus == 1
    gpuDevice(opts.gpus)
  else
    spmd
      clearMex() ;
      gpuDevice(opts.gpus(labindex))
    end
  end
end
end


%% Calculate errors between the corrected image and the ground truth image.
%
% Copyright (c) 2018-present, Mahmoud Afifi
% York University, Canada
% mafifi@eecs.yorku.ca | m.3afifi@gmail.com
%
% This source code is licensed under the license found in the
% LICENSE file in the root directory of this source tree.
% All rights reserved.
%
% Please cite the following work if this program is used:
% Mahmoud Afifi, Brian Price, Scott Cohen, and Michael S. Brown,
% "When color constancy goes wrong: Correcting improperly white-balanced
% images", CVPR 2019.
%
% Input:
%   -corrected: corrected image.
%   -gt: ground-truth image.
%   -color_chart_area: If there is a color chart in the image, that is
%   masked out from both images, this variable represents the number of
%   pixels of the color chart.
%   -opt: determines the required error metric(s) to be reported.
%         Options:
%           opt = 1 delta E 2000 (default).
%           opt = 2 delta E 2000 and mean squared error (MSE)
%           opt = 3 delta E 2000, MSE, and mean angular eror (MAE)
%           opt = 4 delta E 2000, MSE, MAE, and delta E 76
%
% Output:
%   -varargout: a cell contains error between corrected and gt images.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%

function varargout = evaluate_cc(corrected, gt, color_chart_area, opt)

if nargin == 3
    opt = 1;
end
switch opt
    case 1
        varargout{1} = calc_deltaE2000(corrected,gt,color_chart_area);
    case 2
        varargout{1} = calc_deltaE2000(corrected,gt,color_chart_area);
        varargout{2} =calc_mse(corrected,gt,color_chart_area);
    case 3
        varargout{1} = calc_deltaE2000(corrected,gt,color_chart_area);
        varargout{2} =calc_mse(corrected,gt,color_chart_area);
        varargout{3} =calc_mae(reshape(corrected,[],3),reshape(gt,[],3),...
            color_chart_area);
    case 4
        varargout{1} = calc_deltaE2000(corrected,gt,color_chart_area);
        varargout{2} =calc_mse(corrected,gt,color_chart_area);
        varargout{3} =calc_mae(reshape(corrected,[],3),reshape(gt,[],3),...
            color_chart_area);
        varargout{4} = calc_deltaE(corrected,gt,color_chart_area);
    otherwise
        error('Error in evaluate_cc function');
end
end


function DCM = spm_dcm_ind(DCM)
% Estimate parameters of a (bilinear) DCM of induced spectral responses
% FORMAT DCM = spm_dcm_ind(DCM)
%
% DCM
%    name: name string
%       M:  Forward model
%              M.dipfit - leadfield specification
%       xY: data   [1x1 struct]
%       xU: design [1x1 struct]
%
%   Sname: cell of source name strings
%       A: {[nr x nr double]  [nr x nr double]  [nr x nr double]}
%       B: {[nr x nr double], ...}   Connection constraints
%       C: [nr x 1 double]
%
%   options.Nmodes       - number of frequency modes
%   options.Tdcm         - [start end] time window in ms
%   options.D            - time bin decimation       (usually 1 or 2)
%   options.h            - number of DCT drift terms (usually 1 or 2)
%   options.type         - 'ECD' (1) or 'Imaging' (2) (see spm_erp_L)
%   options.onset        - stimulus onset (ms)
%   options.dur          - and dispersion (sd)
%______________________________________________________________________
% This routine inverts dynamic causal models (DCM) of induced or spectral
% responses as measured with the electroencephalogram (EEG) or the
% magnetoencephalogram (MEG). It models the time-varying power, over a
% range of frequencies, as the response of a distributed system of coupled
% electromagnetic sources to a spectral perturbation. The model parameters
% encode the frequency response to exogenous input and coupling among
% sources and different frequencies. Bayesian inversion of this model,
% given data enables inferences about the parameters of a particular model
% and allows one to compare different models, or hypotheses. One key aspect
% of the model is that it differentiates between linear and non-linear
% coupling; which correspond to within and between frequency coupling
% respectively.
%
% The number of nodes can be optimised using Bayesian model selection. The
% data are reduced to a fixed number of principal components that capture
% the greatest variation inspection responses never peristimulus time. The
% number of nodes specified by the user tries to reconstruct the response
% in the space of the principle components or eigenmodes using a reduced
% set of eigenmodes. The number of modes corresponding to data features can
% be changed (from Nf = 8) by editing spm_dcm_ind_data.m
%
% see also: spm_dcm_ind_data; spm_gen_ind; spm_fx_ind and spm_lx_ind
%
% See: Chen CC, Kiebel SJ, Friston KJ.
% Dynamic causal modelling of induced responses.
% Neuroimage. 2008 Jul 15;41(4):1293-312.
% ______________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_ind.m 7143 2017-07-29 18:50:38Z karl $

% check options
%==========================================================================
clear spm_erp_L
DCM.options.analysis  = 'IND';

% Filename and options
%--------------------------------------------------------------------------
try, DCM.name;                  catch, DCM.name           = 'DCM_IND'; end
try, DCM.options.Nmodes;        catch, DCM.options.Nmodes = 4;         end
try, onset = DCM.options.onset; catch, onset              = 80;        end
try, dur   = DCM.options.dur;   catch, dur                = 32;        end
try, DATA  = DCM.options.DATA;  catch, DATA               = 1;         end

% Data and spatial model
%==========================================================================
if DATA
    DCM      = spm_dcm_erp_dipfit(DCM, 1);
    if ~isfield(DCM.xY,'source')
        DCM  = spm_dcm_ind_data(DCM);
    end
end
xY     = DCM.xY;
xU     = DCM.xU;
xU.dt  = xY.dt;

% dimensions
%--------------------------------------------------------------------------
Nm     = DCM.options.Nmodes;          % number of frequency modes modelled
Nf     = size(xY.U,2);                % number of frequency modes explained
Nt     = length(xY.y);                % number of trials
Nr     = size(DCM.C,1);               % number of sources
nu     = size(DCM.C,2);               % number of neuronal inputs
Ns     = size(xY.y{1},1);             % number of samples
Nu     = size(xU.X,2);                % number of trial-specific effects
nx     = Nr*Nf;                       % number of states

% assume noise precision is the same over modes:
%--------------------------------------------------------------------------
xY.Q   = {spm_Q(1/2,Ns,1)};
xY.X0  = sparse(Ns,0);

% Inputs
%==========================================================================

% trial-specific effects
%--------------------------------------------------------------------------
try
    if length(DCM.B) ~= Nu;
        warndlg({'please ensure number of trial specific effects', ...
                 'encoded by DCM.xU.X & DCM.B are the same'})
    end
catch
    DCM.B = {};
end

% model specification and nonlinear system identification
%==========================================================================
M      = DCM.M;
try, M = rmfield(M,'g');  end
try, M = rmfield(M,'FS'); end

% prior moments
%--------------------------------------------------------------------------
[pE,gE,pC,gC] = spm_ind_priors(DCM.A,DCM.B,DCM.C,Nm,Nf);

% hyperpriors (assuming about 99% signal to noise)
%--------------------------------------------------------------------------
hE    = 8 - log(var(spm_vec(xY.y)));
hC    = exp(-4);

% likelihood model
%--------------------------------------------------------------------------
M.IS  = 'spm_gen_ind';
M.f   = 'spm_fx_ind';
M.G   = 'spm_lx_ind';
M.x   = sparse(nx,1);
M.pE  = pE;
M.pC  = pC;
M.gE  = gE;
M.gC  = gC;
M.hE  = hE;
M.hC  = hC;
M.m   = nu;
M.n   = nx;
M.l   = Nr*Nf;
M.ns  = Ns;
M.ons = onset - xY.pst(1);
M.dur = dur;

% EM: inversion
%--------------------------------------------------------------------------
[Qp,Qg,Cp,Cg,Ce,F] = spm_nlsi_N(M,xU,xY);

% Data ID
%==========================================================================
if isfield(M,'FS') && DATA
    try
        ID  = spm_data_id(feval(M.FS,xY.y,M));
    catch
        ID  = spm_data_id(feval(M.FS,xY.y));
    end
else
    ID  = spm_data_id(xY.y);
end

% Bayesian inference {threshold = prior} NB Prior on A,B  and C = exp(0) = 1
%==========================================================================
warning('off','SPM:negativeVariance');
dp  = spm_vec(Qp) - spm_vec(pE);
Pp  = spm_unvec(1 - spm_Ncdf(0,abs(dp),diag(Cp)),Qp);
warning('on','SPM:negativeVariance');

% neuronal and sensor responses (x and y)
%--------------------------------------------------------------------------
L   = feval(M.G, Qg,M);        % get gain matrix
x   = feval(M.IS,Qp,M,xU);     % prediction (source space)

% trial-specific responses (in mode, channel and source space)
%--------------------------------------------------------------------------
for i = 1:Nt

    s  = x{i};                 % prediction (source space)
    y  = s*L';                 % prediction (sensor space)
    r  = xY.y{i} - y;          % residuals  (sensor space)

    % parse frequency modes
    %----------------------------------------------------------------------
    for j = 1:Nm
        f      = (1:Nr) + (j - 1)*Nr;
        H{i,j} = y(:,f);
        R{i,j} = r(:,f);
        K{i,j} = s(:,f);
    end
end

% store estimates in DCM
%--------------------------------------------------------------------------
DCM.M  = M;                    % model specification
DCM.xY = xY;                   % data structure
DCM.xU = xU;                   % input structure
DCM.Ep = Qp;                   % conditional expectation f(x,u,p)
DCM.Cp = Cp;                   % conditional covariances G(g)
DCM.Eg = Qg;                   % conditional expectation
DCM.Cg = Cg;                   % conditional covariances
DCM.Pp = Pp;                   % conditional probability
DCM.H  = H;                    % conditional responses (y), channel space
DCM.K  = K;                    % conditional responses (x)
DCM.R  = R;                    % conditional residuals (y), channel space
DCM.Ce = Ce;                   % ReML error covariance
DCM.F  = F;                    % Laplace log evidence
DCM.ID = ID;                   % data ID
DCM.L  = [];

% and save
%--------------------------------------------------------------------------
save(DCM.name, 'DCM', spm_get_defaults('mat.format'));
assignin('base','DCM',DCM)


function [imageCha, ssim_map, metrics] = algo_FCSA_proxA_3D( obj,input )
% 3D variant of FCSA
% based on Huang et al. paper on FCSA
%
% input:
% obj           CS reconstruction object (holding all parameters)
% input         struct containing recon parameters and image
%
% output:
% imageCha      reconstructed channel individual image
% ssim_map      structural similarity map
% metrics       evaluation metrics
%
% (c) Marc Fischer, Thomas Kuestner, May 2015
% -------------------------------------------------------------------------

%%
timer_proxA = tic;

%% variables:
% internal flags
flag_wavetree = true;
flag_fast = true;
flag_extendImage = true;

% internal variables:
L = 1;
t_old = 1;
NLTV_struct.kernelratio = 3;
NLTV_struct.windowratio = 6;
NLTV_struct.nThreads = 1;  % mind this option if used with -singleCompThread on BWCluster
itrNLTV = obj.iNINNER - 5;
% chambolle tv:
parsin.MAXITER=100; parsin.tv='iso'; % 'iso' or 'l1'

% from obj:
% maxitr = obj.maxitr;
maxitr = obj.iNINNER;
% n1 = obj.measPara.dim(1);
% n2 = obj.measPara.dim(2);
n1 = input.n1;
n2 = input.n2;
nSlices = input.nSlices;
% nSlices = obj.measPara.dim(3);
nCha = obj.measPara.dim(5);
lambdaWave = obj.lambda;
lambdaTV = obj.lambdaTV;
lambdaGroup = obj.lambdaGroup;
NLTV_struct.filterstrength = obj.lambdaNLTV_h; % 0.03 % converted from NLTV h = 0.01 %old: used: 3e-10
lambdaNLTV = obj.lambdaNLTV;
lambdaNLTV_h = obj.lambdaNLTV_h;
regularizerWeights = obj.regularizerWeights;
flagTV = obj.flagTV;
flagTV_iso = obj.flagTV_iso;
flagWave = obj.flagWave;
flagGroup = obj.flagGroup;
flagNLTV = obj.flagNLTV;
flagSBNLTV = obj.flagSBNLTV;
flagRealAndImag = obj.flagRealAndImag;
waveletStages = obj.trafo.waveletStages;
maxWavecells = waveletStages*7+1;
waveletFilterName_l1 = obj.trafo.waveletFilterName_l1;
waveletFilterName_l12 = obj.trafo.waveletFilterName_l12;

% from input:
b=input.b;
mask = input.mask;
G_prox = input.G_prox;
Gt_prox = input.Gt_prox;
groupnorm_index = input.groupnorm_index;
waveS_l1 = input.waveS_l1;
waveS_l12 = input.waveS_l12;
waveS_l12proxA = input.waveS_l12proxA;
proxA_extend_y = waveS_l12proxA(waveletStages+2,1) - waveS_l12(waveletStages+2,1);
proxA_extend_x = waveS_l12proxA(waveletStages+2,2) - waveS_l12(waveletStages+2,2);
z_proxA = cell(1,nCha);

% im_ref = input.im_ref;
% im_ref_full = zeros(n1,n2,nSlices);
% for j = 1:nCha
%     im_ref_full = im_ref_full + abs(im_ref{1,j}).^2;
% end;
% im_ref_full = sqrt(im_ref_full);
clear input

% initialize cells/vectors
for j=1:nCha
    FTb{1,j} = iFFT3D(b{1,j});
    y{1,j} = real(FTb{1,j}); % needed for y_old
    y{1,j+nCha} = imag(FTb{1,j});
end;
z = FTb; % starting point

x_wave = cell(1,2*nCha);
x_helper = x_wave;
x_wave_helper = x_wave;
x_tv = x_wave;
x_nltv = x_wave;
for j=1:2*nCha
    x_nltv{1,j} = 0;
end;
x_g = x_wave;
x_g_helper = x_wave;
x_g_proxA = x_wave;
z_comp = x_wave;

%% MAD dependent lambdas:
flag_MAD = true;
if flag_MAD
    x_wavedec_2D = cell(1,nCha);
    x_wavedec_3D = cell(1,nCha);
    threshold_2D = zeros(nCha,1); % one threshold value is enough (only for testing purposes)
    threshold_3D = zeros(nCha,1); % one threshold value is enough (only for testing purposes)
    if flagRealAndImag
        for j=1:nCha
            for l = 1:nSlices
                x_wavedec_2D{1,j} = wavedec2(abs(z{1,j}),waveletStages,waveletFilterName_l12); % atm only based on l1-daubechie
                x_wave_fine_scale_2D = size(x_wavedec_2D{1,j},2) - (3*waveS_l12(waveletStages+1,1)*waveS_l12(waveletStages+1,2));
                threshold_2D(j) = mad(x_wavedec_2D{1,j}(x_wave_fine_scale_2D:end),1);
            end;
            x_wavedec_3D{1,j} = wavedec3(abs(z{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale_3D = zeros(0,0);
            for k = (7*(x_wavedec_3D{1,j}.level-1)+2):maxWavecells
                x_wave_fine_scale_3D = [x_wave_fine_scale_3D  x_wavedec_3D{1,j}.dec{k}];
            end;
            [size1, size2, size3] = size(x_wave_fine_scale_3D);
            threshold_3D(j) = mad(reshape(x_wave_fine_scale_3D,size1*size2*size3,1));
        end;
    else
        for j=1:nCha
            for l = 1:nSlices
                x_wavedec_2D{1,j} = wavedec2(real(z{1,j}),waveletStages,waveletFilterName_l12); % atm only based on l1-daubechie
                x_wavedec_2D{1,j+nCha} = wavedec2(imag(z{1,j}),waveletStages,waveletFilterName_l12); % atm only based on l1-daubechie
                x_wave_fine_scale_2D_real = size(x_wavedec_2D{1,j},2) - (3*waveS_l12(waveletStages+1,1)*waveS_l12(waveletStages+1,2));
                threshold_2D(j) = mad(x_wavedec_2D{1,j}(x_wave_fine_scale_2D_real:end),1);
                x_wave_fine_scale_2D_imag = size(x_wavedec_2D{1,j},2) - (3*waveS_l12(waveletStages+1,1)*waveS_l12(waveletStages+1,2));
                threshold_2D(j+nCha) = mad(x_wavedec_2D{1,j}(x_wave_fine_scale_2D_imag:end),1);
            end;
            x_wavedec_3D{1,j} = wavedec3(real(z{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wavedec_3D{1,j} = wavedec3(imag(z{1,j}),waveletStages,waveletFilterName_l1); % atm only based on l1-daubechie
            x_wave_fine_scale_3D_real = zeros(0,0);
            x_wave_fine_scale_3D_imag = zeros(0,0);
            for k = (7*(x_wavedec_3D{1,j}.level-1)+2):maxWavecells
                x_wave_fine_scale_3D_real = [x_wave_fine_scale_3D_real  x_wavedec_3D{1,j}.dec{k+2}];
                x_wave_fine_scale_3D_imag = [x_wave_fine_scale_3D_imag  x_wavedec_3D{1,j}.dec{k+2}];
            end;
            [size1, size2, size3] = size(x_wave_fine_scale_3D_real);
            threshold_3D(j) = mad(reshape(x_wave_fine_scale_3D_real,size1*size2*size3,1));
            threshold_3D(j+nCha) = mad(reshape(x_wave_fine_scale_3D_imag,size1*size2*size3,1));
        end;
    end;
    clear x_wavedec
else
    threshold_2D(1:nCha) = 1;
    threshold_3D(1:nCha) = 1;
end;

if flagRealAndImag
    threshold_group = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold_3D(j)/2 * 2/L;
        threshold_TV(j) = lambdaTV * threshold_2D(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold_2D(j) * 2/L;
        threshold_group = threshold_group + lambdaGroup * threshold_2D(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group = threshold_group /3;
else
    threshold_group_real = 0;
    threshold_group_imag = 0;
    for j = 1:nCha
        threshold_wave(j) = lambdaWave * threshold_3D(j) * 2/L;
        threshold_wave(j+nCha) = lambdaWave * threshold_3D(j+nCha) * 2/L;
        threshold_TV(j) = lambdaTV * threshold_2D(j) * 2/L;
        threshold_TV(j+nCha) = lambdaTV * threshold_2D(j+nCha) * 2/L;
        threshold_group_real = threshold_group_real + lambdaGroup * threshold_2D(j) * 2/L;
        threshold_group_imag = threshold_group_imag + lambdaGroup * threshold_2D(j) * 2/L;
        threshold_NLTV(j) = lambdaNLTV; % * threshold(j) * 2/L;
        threshold_NLTV(j+nCha) = lambdaNLTV;
        threshold_NLTV_h(j) = lambdaNLTV_h; %*threshold(j); % adjust carefully or NLTV won't find a solution. lambdaNLTV_h should be < 0.01
        threshold_NLTV_h(j+nCha) = lambdaNLTV_h;
    end;
    threshold_group_real = threshold_group_real /3;
    threshold_group_imag = threshold_group_imag /3;
end;

%% initialize metrics:
itr = 0;
metrics.xtime(itr+1)= 0;
% [metrics, ssim_map{1,1}] = get_metrics_itr( im_ref, im_ref_full, z, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma, nSlices );
ssim_map = [];

%% recon
dispProgress('Proximal Average', 0, maxitr);
for itr = 1:maxitr  % total iter counter

    t_new = (1+sqrt(1+4*t_old^2))/2;
    t_old = t_new;

    y_old = y;   % y_old = y for complex case

    %% landweber step
    for j = 1:nCha
        x_helper{1,j} = iFFT3D(FFT3D_mask(z{1,j},mask)) -FTb{1,j} ;
        z{1,j} = z{1,j} - x_helper{1,j}/L;
        z_comp{1,j} = real(z{1,j});
        z_comp{1,j+nCha} = imag(z{1,j});
    end;

    %% l1-Wavelet
    if flagWave
        for j = 1:2*nCha
            x_wave_helper{1,j} = wavedec3(z_comp{1,j},waveletStages,waveletFilterName_l1);
        end;
        if flagRealAndImag
            for j = 1:nCha
                for k = 1:maxWavecells
                    dec_cell{j}{k} = x_wave_helper{1,j}.dec{k,1};
                    dec_cell{j+nCha}{k} = x_wave_helper{1,j+nCha}.dec{k,1};
                    [c1, c2, c3] = size(dec_cell{j}{k});
                    [dec_cell{j}{k}, dec_cell{j+nCha}{k}] = groupthresh_2vec(reshape(dec_cell{j}{k},c1*c2*c3,1,1),reshape(dec_cell{j+nCha}{k},c1*c2*c3,1,1),threshold_wave(j));
                    x_wave_helper{1,j}.dec{k,1} = reshape(dec_cell{j}{k},c1,c2,c3);
                    x_wave_helper{1,j+nCha}.dec{k,1} = reshape(dec_cell{j+nCha}{k},c1,c2,c3);
                    dec_cell{j}{k} = [];
                    dec_cell{j+nCha}{k} = [];
                end;
            end;
        else
            for j = 1:2*nCha
                x_wave_helper{1,j} = softthresh_real(x_wave_helper{1,j},threshold_wave(j));
                for k = 1:maxWavecells
                    dec_cell{j}{k} = x_wave_helper{1,j}.dec{k,1};
                    [c1, c2, c3] = size(dec_cell{j}{k});
                    dec_cell{j}{k} = softthresh_real(reshape(dec_cell{j}{k},c1*c2*c3,1,1),threshold_wave(j));
                    x_wave_helper{1,j}.dec{k,1} = reshape(dec_cell{j}{k},c1,c2,c3);
                    dec_cell{j}{k} = [];
                end;
            end;
        end;
        for j = 1:2*nCha
            x_wave{1,j} = waverec3(x_wave_helper{1,j});
        end;
    end;

    %% TV
    if flagTV
        if ~flagTV_iso

            for j = 1:2*nCha
                for i = 1:nSlices
                    x_tv{1,j}(:,:,i) = MTV_2D(z_comp{1,j}(:,:,i),threshold_TV(j),n1,n2);
                end;
            end;
        else

            for j = 1:2*nCha
                for i = 1:nSlices
                    if (itr==1)
                        [x_tv{1,j}(:,:,i), P]=denoise_TV_One((z_comp{1,j}(:,:,i)), threshold_TV(j),-inf,inf,[],parsin);
                    else
                        [x_tv{1,j}(:,:,i), P]=denoise_TV_One((z_comp{1,j}(:,:,i)), threshold_TV(j),-inf,inf,P,parsin);
                    end;
                end;
            end;
        end;
    end;

    %% NLTV
    if flagNLTV
        if itr >= itrNLTV
            if flagSBNLTV
                for j = 1:2*nCha
                    for i = 1:nSlices
                        x_nltv{1,j}(:,:,i) = SB_NLTVfunc_slim_rescale(z_comp{1,j}(:,:,i),n1,n2, threshold_NLTV(j), threshold_NLTV_h(j) );
                    end;
                end;
            else
                for j = 1:2*nCha
                    for i = 1:nSlices
                        % if mod(itr,5) == 0 || itr == 1
                        x_nltv{1,j}(:,:,i) = NLMF(z_comp{1,j}(:,:,i),NLTV_struct);
                        x_nltv{1,j}(:,:,i) = (L.*z_comp{1,j}(:,:,i) + 2*threshold_NLTV(j)*x_nltv{1,j}(:,:,i))./(L+2*threshold_NLTV(j));
                        % end;
                    end;
                end;
            end;
        end;
    end;

    %% l12-Wavelet
    if flagGroup
        for j = 1:2*nCha
            for i = 1:nSlices
                if flag_extendImage
                    z_proxA{1,j}(:,:,i) = extend_image(z_comp{1,j}(:,:,i), waveS_l12proxA, waveletStages, proxA_extend_y, proxA_extend_x);
                    x_g_helper{i}{1,j} = wavedec2(z_proxA{1,j}(:,:,i),waveletStages,waveletFilterName_l12);
                else
                    x_g_helper{i}{1,j} = wavedec2(z_comp{1,j}(:,:,i),waveletStages,waveletFilterName_l12);
                end;

                if flag_wavetree
                    x_g_helper{i}{2,j} = (G_prox*x_g_helper{i}{1,j}')';
                else
                    x_g_helper{i}{2,j} = zeros(1,size(x_g_helper{i}{1,j}(:,:,i),2));
                end;
            end;
        end;
        for i = 1:nSlices
            if flagRealAndImag
                x_g_helper{i}(1,:) = softthresh_proxA_cha(x_g_helper{i}(:,:),threshold_group,2*nCha,waveS_l12proxA,groupnorm_index);
            else
                x_g_helper{i}(1,1:nCha) = softthresh_proxA_cha(x_g_helper{i}(:,1:nCha),threshold_group_real,nCha,waveS_l12proxA,groupnorm_index);
                x_g_helper{i}(1,nCha+1:2*nCha) = softthresh_proxA_cha(x_g_helper{i}(:,1:nCha),threshold_group_imag,nCha,waveS_l12proxA,groupnorm_index);
            end;
        end;
        for j = 1:2*nCha
            for i = 1:nSlices
                x_g_proxA{1,j}(:,:,i) = waverec2(x_g_helper{i}{1,j},waveS_l12proxA,waveletFilterName_l12);
                x_g{1,j}(:,:,i) =  x_g_proxA{1,j}(1:end-proxA_extend_y,1:end-proxA_extend_x,i);
            end;
        end;
    end;

    %% add prox(.)
    for j = 1:2*nCha
        y{1,j} = zeros(n1,n2,nSlices);
        if flagWave y{1,j} = y{1,j} + x_wave{1,j}.*regularizerWeights(1); end;
        if flagTV y{1,j} = y{1,j} + x_tv{1,j}.*regularizerWeights(2); end;
        if flagGroup y{1,j} = y{1,j} + x_g{1,j}.*regularizerWeights(3); end;
        if flagNLTV y{1,j} = y{1,j} + x_nltv{1,j}.*regularizerWeights(4); end;

        if itr < itrNLTV
            y{1,j} = y{1,j}/(flagTV.*regularizerWeights(2) + flagWave.*regularizerWeights(1) + flagGroup.*regularizerWeights(3));
        else
            y{1,j} = y{1,j}/(flagTV.*regularizerWeights(2) + flagNLTV.*regularizerWeights(4) + flagWave.*regularizerWeights(1) + flagGroup.*regularizerWeights(3));
        end;

        if flag_fast
            y{1,j}=y{1,j}+((t_old-1)/t_new).*(y{1,j}-y_old{1,j});
        end;
    end;

    %% metrics of current itr:
%     disp(itr);
    dispProgress('Proximal Average', itr/maxitr);

    metrics.xtime(itr+1)= toc(timer_proxA);
    for j = 1:nCha
        z{1,j} = y{1,j} + 1i*y{1,j+nCha};
    end;
%     [metrics, ssim_map{1,2}] = get_metrics_itr( im_ref, im_ref_full, z, itr, maxitr, nCha, n1, n2, metrics, obj.K_1, obj.K_2, obj.W_size, obj.W_sigma, nSlices );

end;
dispProgress('Proximal Average', 'Close');

imageCha = z;
for j = 1:nCha
    imageCha{1,j} = turn_image( imageCha{1,j} );
end;
% for j = 1:nCha+1
%     ssim_map{1,1}{1,j} = turn_image( ssim_map{1,1}{1,j} );
%     ssim_map{1,2}{1,j} = turn_image( ssim_map{1,2}{1,j} );
% end;

end


function [uOutput] = scecdcimp(nFunction, sFilename)

% Filter function switchyard
if nFunction == FilterOp.getDescription
    uOutput = 'USGS PDE Data Center Compressed Format (stringconvert)';
elseif nFunction == FilterOp.importCatalog
    % Read formated data
    mData = textread(sFilename, '%s', 'delimiter', '\n', 'whitespace', '');
    % Create empty catalog
    uOutput = zeros(length(mData), 9);
    % Loop thru all lines of catalog and convert them
    for i = 1:length(mData)
        if rem(i,100) == 0 ; disp([ num2str(i) ' of ' num2str(length(mData)) ' events processed ']); end
        try

            uOutput(i,1) = str2num(mData{i}(35:42));  % Longitude
            uOutput(i,2) = str2num(mData{i}(28:34));  % Latitude
            uOutput(i,3) = str2num(mData{i}(8:11));    % Year
            uOutput(i,4) = str2num(mData{i}(13:14));    % Month
            uOutput(i,5) = str2num(mData{i}(15:16));   % Day
            uOutput(i,6) = str2num(mData{i}(66:68));  % Magnitude
            uOutput(i,7) = str2num(mData{i}(43:47));  % Depth
            uOutput(i,8) = str2num(mData{i}(17:18));  % Hour
            uOutput(i,9) = str2num(mData{i}(19:20));  % Minute

             %Create decimal year
            % uOutput(i,3) = decyear([uOutput(i,3) uOutput(i,4) uOutput(i,5) uOutput(i,8) uOutput(i,9)]);
         catch
            msg.dbfprintf('Import: Problem in line %d of %s. Line ignored.\n',i, sFilename);
            uOutput(i,:)=nan;
        end
    end
    l = isnan(uOutput(:,1));
    uOutput(l,:) = [];
end


%% Multi-target planar tracking
%
% Example of using features2d framework for multiple planar targets tracking
% in a video using homography matching. This sample uses the |PlaneTracker|
% class.
%
% Sample video: <http://www.youtube.com/watch?v=pzVbhxx6aog>.
%
% Sources:
%
% * <https://github.com/opencv/opencv/blob/3.3.1/samples/python/plane_tracker.py>
%

% video file, and multiple targets to track [x,y,w,h]
vid = fullfile(mexopencv.root(), 'test', 'blais.mp4');
assert(exist(vid, 'file') == 2, 'Missing video file');
win = [
    136 0 366 433;   % book
    135 165 285 175  % face
];
N = size(win,1);

% open video feed, and get firs frame
cap = cv.VideoCapture(vid);
pause(1);
assert(cap.isOpened(), 'Failed to open video');
frame = cap.read();
assert(~isempty(frame), 'Failed to read frames');

% create and initialize tracker
tracker = PlaneTracker();
for i=1:N
    tracker.addTarget(frame, win(i,:));
end

% prepare plot
hImg = imshow(frame);
clr = lines(N);
for i=1:N
    % keypoints and bounding box for each tracked target
    hPts(i) = line(NaN, NaN, 'Color',clr(i,:), 'LineStyle','none', 'Marker','o');
    hLin(i) = line(NaN, NaN, 'Color',clr(i,:), 'LineWidth',3);
end

% main loop
while ishghandle(hImg)
    % read new frame
    frame = cap.read();
    if isempty(frame), break; end

    % track and update keypoints/boundaries of matched targets
    tracked = tracker.track(frame);
    for i=1:numel(tracked)
        tr = tracked(i);
        set(hPts(tr.index), 'XData',tr.pt1(:,1), 'YData',tr.pt1(:,2));
        set(hLin(tr.index), ...
            'XData',tr.quad([1:end 1],1), 'YData',tr.quad([1:end 1],2));
    end

    % update tracked objects which were not found in current frame
    idx = setdiff(1:N, [tracked.index]);
    if ~isempty(idx)
        set(hPts(idx), 'XData',NaN, 'YData',NaN);
        set(hLin(idx), 'XData',NaN, 'YData',NaN);
    end

    % display result
    set(hImg, 'CData',frame);
    drawnow;
end
cap.release();


function c = camSimpleGraphics(csize)

%CAMSIMPLEGRAPHICS Create a simple camera graphics 3D object.
%   CAMGRAPHICS creates a solid representing a camera. The objective is
%   heading towards the camera optical axis and is located at camera
%   position. Initial configuration is: position at origin and optical
%   axis aligned with world's Z axis.
%
%   This function creates an object much simpler (with much less vertices)
%   than CAMGRAPHICS, with the aim of accelerating rendering.
%
%   The result is a structure with fields:
%       .vert0  the vertices in camera frame.
%       .vert   the vertices in world frame.
%       .faces  the definition of faces.
%
%   Fields .vert and .faces are used to draw the object via the PATCH
%   command. Further object repositionning is accomplished with DRAWOBJECT.
%
%   CAMSIMPLEGRAPHICS(SIZE) allows for choosing the camera size. Default is 0.1.
%
%   See also CAMGRAPHICS, PATCH, SET, DRAWOBJECT.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargin == 0
    csize = 0.1;
end

h = .8; % body height
w = 1;  % body width
t = .3; % body thickness

% body vertices
rect      = [1 1;-1 1;-1 -1;1 -1]*diag([w h])/2;
body      = [rect,zeros(4,1);rect,-t*ones(4,1)];

% vertices in camera frame
vert0     = body;

% graphics structure - vertices and faces
c.vert0   = csize*vert0;
c.vert    = c.vert0;
c.faces   = [ ...
    01 02 03 04
    05 06 07 08
    01 02 06 05
    02 03 07 06
    03 04 08 07
    04 01 05 08];

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function X=pathSolutionLeast(A, y, z, opts)
%
%% Fuction pathSolution:
%      Solving the pathwise solutions
%
%% Input & Output parameters
%  See the description of the related functions
%
%% Copyright (C) 2009-2010 Jun Liu, and Jieping Ye
%
% You are suggested to first read the Manual.
%
% For any problem, please contact with Jun Liu via j.liu@asu.edu
%
% Last modified 2 August 2009.
%
% Related functions:
%  sll_opts, initFactor,
%  eppVector, eppMatrix, eplb,
%
%  LeastR, LeastC,
%  nnLeastR, nnLeastC
%  glLeastR,  mtLeastR, mcLeastR
%%

switch(opts.fName)
    case 'LeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=LeastR(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastR
            [x, funVal]=LeastR(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'LeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=LeastC(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastC
            [x, funVal]=LeastC(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'glLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=glLeastR(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function glLeastR
            [x, funVal]=glLeastR(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'mtLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        k=length(opts.ind)-1;        % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mtLeastR(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mtLeastR
            [x, funVal]=mtLeastR(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    case 'mcLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        k=size(y,2);                 % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mcLeastR(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=1;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mcLeastR
            [x, funVal]=mcLeastR(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    case 'nnLeastR'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(-z);   % sort z in a decresing order
        z_value=-z_value;            % z_value in a decreasing order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=nnLeastR(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastR
            [x, funVal]=nnLeastR(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'nnLeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        X=zeros(n,z_num);            % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=nnLeastC(A, y, z_value(1), opts);

        X(:,z_ind(1))=x;             % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function LeastC
            [x, funVal]=nnLeastC(A, y, z_value(i), opts);

            X(:,z_ind(i))=x;         % store the solution
        end

    case 'mtLeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        k=length(opts.ind)-1;        % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mtLeastC(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=0;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mtLeastC
            [x, funVal]=mtLeastC(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    case 'mcLeastC'
        z_num=length(z);             % the number of parameters
        [z_value, z_ind]=sort(z);    % sort z in an ascending order
        n=size(A,2);                 % the dimensionality of the data
        k=size(y,2);                 % the number of tasks
        X=zeros(n,k,z_num);          % set the size of output X

        % run the code to compute the first solution
        [x, funVal]=mcLeastC(A, y, z_value(1), opts);

        X(:,:,z_ind(1))=x;           % store the solution

        % set .init for warm start
        opts.init=1;                 % using .initFactor

        for i=2:z_num
            opts.x0=x;               % warm-start

            % run the function mcLeastC
            [x, funVal]=mcLeastC(A, y, z_value(i), opts);

            X(:,:, z_ind(i))=x;      % store the solution
        end

    otherwise
        fprintf('\n The function value specified in opts.fName is not supported!');
end


function subpak_test07 ( )

%*****************************************************************************80
%
%% TEST07 tests BMI_ENGLISH.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  test_num = 10;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST07\n' );
  fprintf ( 1, '  BMI_ENGLISH computes the Body Mass Index\n' );
  fprintf ( 1, '  given body measurements in English Units.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      Weight         Height            BMI\n' );
  fprintf ( 1, '       (LB)     (FT           IN)\n' );
  fprintf ( 1, '\n' );

  seed = 123456789;

  for test = 1 : test_num

    b = 100.0;
    c = 250.0;

    [ w, seed ] = r8_uniform ( b, c, seed );

    b = 4.0;
    c = 6.75;

    [ h, seed ] = r8_uniform ( b, c, seed );

    h_ft = round ( h );
    h_in = round ( 12.0 * ( h - h_ft ) );

    bmi = bmi_english ( w, h_ft, h_in );

    fprintf ( 1, '  %8f  %8f  %8f  %8f\n', w, h_ft, h_in, bmi );

  end

  return
end


%% This file is the main file of using the SINDy-PI method to
% infer the Yeast Glycolysis Model. We will figure out what is the minimum
% data length needed for SINDy-PI to accurately discover the six-th state
% of the Yeast Glycolysis Model. This file will be used to swipe through
% different data length.
%
% Date: 2019/06/13
% Coded By: K

%% Close all, clear all, clc
close all;clear all; clc;
set(0,'defaulttextInterpreter','latex')
addpath('Functions')
addpath('Datas')
%% Define some parameters
% Define whehter you have control, if you have it, please define it
n_control=0;u=0;

% Run the ODE files and gather the simulation data.
% (We use the same data for both the iSINDy method and SINDy-PI method for better comparision)
% (Baseline data for the data length comparison of state 1,2,3,4,5,6,7)
% The data is noise clean. It is generated using 900 different initial
% conditions with 51 points of each initial condition.
load('TrainingData.mat')

% Choose whether you want to display actual ODE or not
disp_actual_ode=1;

% If the ODEs you want to display is the actual underlyting dynamics of the
% system, please set actual as 1
actual=1;

% Define how many states we have in our example
n_state=7;

% Print the actual ODE we try to discover
digits(4)
Print_ODEs(@(t,y)YeastGlycolysis_ODE(t,y),n_state,n_control,disp_actual_ode,actual);

% Create symbolic states
dz=sym('dz',[n_state,1]);

% Now we first create the parameters of the function right hand side
Highest_Poly_Order_Guess=0;
Highest_Trig_Order_Guess=0;
Highest_U_Order_Guess=0;

% Then create the right hand side library parameters
Highest_Trig_Order=0;
Highest_U_Order=0;
Highest_dPoly_Order=1;

% Set the parameter normLib=1 to normalize the librry
normLib=1;

% Set how amny iterations you want for the sparse regression
N_iter=10;

% Set whether you want to display the ODE or not
disp=0;

% Set the library size
Highest_Poly_Order_Lib=[6;6;3;3;3;6;3];

% Determine how many percent of data you want.
percent_start=1;d_percent=0.005;percent_end=12;

% Determine which states you want to discover
Which_State=6;

% Determine the left hand side guess number
if Which_State==1 || Which_State==2 || Which_State==6
    LHS_Num=2;
else
    LHS_Num=1;
end

% Get the poly-order
Highest_Poly_Order=Highest_Poly_Order_Lib(Which_State);

% Determine which LHS you want to use, the first one or the second one
LHS_Pin=1;
%%
tic
% Create the library data using all the data points
[SINDy_Data_Full,SINDy_Struct]=SINDyLib(xt,dxt(:,Which_State),Which_State,u,Highest_Poly_Order,Highest_Trig_Order,Highest_U_Order,Highest_dPoly_Order);
fprintf('\n\t Original library creation finished, using %i seconds...\n',toc)

% Create left hand side guess
[LHS_Data_Full,LHS_Sym]=GuessLib(xt,dxt(:,Which_State),Which_State,u,Highest_Poly_Order_Guess,Highest_Trig_Order_Guess,Highest_U_Order_Guess);

tic
% Create the right hand side, exclude the guess from SINDy library
[RHS_Data_Full,RHS_Struct]=ExcludeGuess(SINDy_Data_Full,SINDy_Struct,LHS_Sym{LHS_Pin});
LHS_Data_Full_Dum=LHS_Data_Full(:,LHS_Pin);
fprintf('\t Library without LHS guess creation finished, using %i seconds...\n',toc)

% Determine sparsity parameter
lambda=0.1;

% Create the new directory to save the result
FolderName=strcat('Result_DL_SINDy_Data_Length_Compare_State_',num2str(Which_State),'_LHS_Guess_',num2str(LHS_Pin),'_SwipeLength_',num2str(lambda));
[fld_status, fld_msg, fld_msgID]=mkdir(FolderName);

% Set up dummy variables for parfor
LHS_Sym_Dum=LHS_Sym{LHS_Pin};
dz_dum=dz(Which_State);
%% determine the percentage you want to use
if LHS_Pin==1
    Percent_Iter=[0.30 0.31 0.32 0.33 0.34 0.345 0.35 0.355 0.36 0.365 0.37 0.38 0.39 0.40];
else
    Percent_Iter=[0.02 0.03 0.04 0.05 0.055 0.0575 0.06 0.0625 0.065 0.0675 0.07 0.0725 0.075 0.08 0.09 0.1 0.2];
end

for per=1:size(Percent_Iter,2)
    percent=Percent_Iter(per);

    % Start!
    for Total_Run=1:10
        fprintf('\n \n Get the result for the %i time...\n',Total_Run)

        % Print the state we are testing
        fprintf('\n \t Calculating the %i expression...\n',Which_State)

        % Print the left hand side that we are testing
        fprintf('\t Testing the left hand side as %s:\n',char(LHS_Sym_Dum))

        % Define the new data length
        new_length=round((percent)*length(xt));

        % Shuffel the original data
        Sequence=randperm(size(SINDy_Data_Full,1));
        Sequence_Trimed=Sequence(1:round((percent)*length(xt)));

        RHS_Data=RHS_Data_Full(Sequence_Trimed,:);
        LHS_Data=LHS_Data_Full_Dum(Sequence_Trimed,1);

        fprintf('\n \t Data preparation finished, using %i seconds...\n',toc)

        % We fix the lambda and change the data usage.
        tic
        fprintf('\n \t Testing the percentage as %i ...\n',(percent*100))

        % Perform the sparse regression problem
        Xi=sparsifyDynamics_simplified(RHS_Data,LHS_Data,lambda,N_iter,normLib);
        fprintf('\t Uses %i seconds...\n',toc)

        % Save the calculation result of current iteration
        fprintf('\n\t Saving the result...\n')
        tic
        cc=clock;
        ResultName=strcat(FolderName,'/DL_SINDY_Data_Length_',num2str(Total_Run),'__','LHS',num2str(LHS_Pin),'_',num2str(cc(3)),'_',num2str(cc(4)),'_',num2str(cc(5)),'_',num2str(round(cc(6))),'_P3','.mat');
        save(ResultName,'Xi','lambda','percent')
        fprintf('\n\t Saving finished! Using %i seconds...\n',toc)
    end
end


%helpme_it    iterative solvers interactive help
%   IFISS scriptfile: HCE; 23 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

fprintf(' \n');
fprintf('  To use a preconditioned Krylov subspace iterative method\n');
fprintf('  to solve the current discrete system, run the script file: \n');
fprintf('  it_solve\n');
fprintf(' \n');


% Copyright 2009 - 2010 The MathWorks, Inc.
function plot_trajectory(z,y)
%#eml
eml.extrinsic('title','xlabel','ylabel','plot','axis','pause');
title('Trajectory of object [blue] its Kalman estimate[green]');
xlabel('horizontal position');
ylabel('vertical position');
plot(z(1), z(2), 'bx-');
plot(y(1), y(2), 'go-');
axis([-1.1, 1.1, -1.1, 1.1]);
pause(0.02);
end


function Nstar = ctranspose(N)
%'   Compute the adjoint of a CHEBOP.
%   NSTAR = N' returns the adjoint of a CHEBOP that has either periodic or
%   endpoint boundary conditions. If N is nonlinear then N is first
%   linearized around U = 0 and NSTAR is the adjoint of the linearization.
%
%   N' is calculated by a call to ADJOINT(N).  To linearize about
%   a nonzero function U, use ADJOINT(N, U).
%
%  Example:
%   L = chebop(-1,1); L.op = @(x,u) diff(u,2) + diff(u,1) + u;
%   L.lbc = 0; L.rbc = 1, Ls = L'
%
% See also CHEBOP/ADJOINT.

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

% This is just a wrapper for adjoint().
Nstar = adjoint(N);

end


function r = roots( f, g, varargin )
%ROOTS   Zeros of a SEPARABLEAPPROX.
%   R = ROOTS(F) returns the zero contours of F as a quasimatrix of chebfuns.
%   Each column of R is one zero contour.  This command only finds contours when
%   there is a change of sign, and it can also group intersecting contours in a
%   non-optimal way.
%
%   For a faster plot to graphical accuracy use CONTOUR(F, [0 0]).
%
%   R = ROOTS(F, G) returns the isolated mutual roots of F and G.
%
%   R = ROOTS(F, G, METHOD) allows the underlying rootfinding algorithm to
%   be specified.  If METHOD = 'ms' or 'marchingsquares', the Marching
%   Squares algorithm is employed, which is fast but not very robust.
%   If METHOD = 'resultant', a hidden variable resultant method
%   based on Bezout resultants is employed, slower but more robust.
%   See the CHEBFUN2V/ROOTS documentation to see which algorithm is used
%   when no METHOD is passed.
%
% Example:
%   cheb.xy;
%   f = x.^2 + y.^2 - 1/4;
%   roots(x,f)                               % [0 -.5; 0 .5]
%   c = roots(f);
%   arclength = sum(abs(diff(c)))            % pi
%   area = abs(sum(real(c).*diff(imag(c))))  % pi/4
%
% See also CHEBFUN2V/ROOTS.

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

% Check for empty chebfun input:
if ( isempty( f ) )
    r = [];
    return
end

dom = f.domain;
scl = norm(dom,inf);

if ( nargin == 1 )  % Seek zero curves of scalar function

    if ( length( f ) == 1 )  % Special case: f has rank 1:
        cols = f.cols;
        rows = f.rows;
        yrts = 1i*(roots( cols )+realmin);
        xrts = roots( rows ) + realmin*1i; % Add complex to ensure it's not real
        r = chebfun;
        % Go though col(yrts) = 0, make into a horizontal line:
        dom = rows.domain;
        len = (dom(2)-dom(1))/2;
        for j = 1 : numel(yrts)
            f = chebfun(@(x) (len*(x+1)+dom(1)) + yrts(j));
            r = [r f];
        end
        % Go though row(xrts) = 0, make into a vertical line:
        dom = cols.domain;
        len = (dom(2)-dom(1))/2;
        for j = 1 : numel(xrts)
            f = chebfun(@(x) 1i*(len*(x+1)+dom(1)) + xrts(j));
            r = [r f];
        end

    elseif ( isreal( f ) )  % Main case: zero curves for real function
        % We seek accurate chebfun representations of each component
        % of the zero curve.  First we call Matlab's contourc function
        % to get initial data points, which typically will lie on the zero
        % curve to 5-6 digit accuracy and will be smoothly spaced along
        % it to 2-3 digit accuracy.  The choice n = 502 is used to get a
        % grid that does not involve the boundary of the domain.
        n = 502;
        x = linspace( dom(1), dom(2), n );
        x(1) = []; x(end) = [];
        y = linspace( dom(3), dom(4), n );
        y(1) = []; y(end) = [];
        [xx, yy] = meshgrid( x, y );
        vals = feval( f, xx, yy );
        C = contourc( x, y, vals, 0*[1 1] );
        [fx, fy] = grad(f);
        gradf = @(data) feval( fx, real(data), imag(data)) ...
                   + 1i*feval( fy, real(data), imag(data));
        fval = @(data) feval( f, real(data), imag(data));

        % The construction of chebfuns proceeds by improving the accuracy
        % of the curves component by component, using complex arithmetic
        % for convenience.
        j = 1; r = chebfun;
        while ( j < length(C) )
            k = j + C(2, j);
            D = C(:, j+1:k);
            data = ( D(1, :) + 1i*(D(2, :)+realmin) ).';
            ii = find(abs(diff(data)) < 1e-8*scl);
            data(ii) = [];                           % eliminate repetitions
            npts = length(data);
            err = 999; errnew = 1e-2;
            stepno = 0;
            curvenew = chebfun(data);
            while (errnew < err) && (stepno < 6)
                stepno = stepno+1;
                err = errnew;
                curve = curvenew;
                s = [0; cumsum(abs(diff(data)))];    % empirical arclength
                s = 2*s/s(end) - 1;                  % normalize to [-1,1]
                data = interp1(s, data, chebpts ...  % interpolate
                    (length(data)), 'spline');
                curvenew = chebfun(data);            % make chebfun
                data = curvenew(chebpts(npts));      % sample finely
                data = snap(data);                   % snap to boundary
                ff = fval(data);                     % function values
                g = gradf(data);                     % gradient values
                errnew = norm(ff,inf)/vscale(f);     % max rel error
                data = data - ff.*( g./abs(g).^2 );  % Newton step
                curvenew = chebfun(data);            % make chebfun
                curvenew = simplify(curvenew);       % simplify it
            end
            j = k + 1;
            r = [ r , curve ];
        end

    else  % Function is complex-valued: reduce to real case
        r = roots( [ real(f) ; imag(f) ] );
        if ( ~isempty( r ) )
            r = r(:, 1) + 1i*r(:, 2);
        end
    end

elseif ( isa(g, 'separableApprox') )  % seek zero points of vector function
    % TODO: This should not call chebfun2v directly.
    r = roots( chebfun2v( f, g ), varargin{:} );

end

function d = snap(d)   % adjust endpoints to snap to boundary of domain
    n = length(d);

    if abs(real(d(1))-dom(2)) < .02*scl                 % snap to right bndry
        dx0 = dom(2)-real(d(2)); dx = real(d(1)-d(2));
        d(1) = d(2) + (dx0/dx)*(d(1)-d(2));
    end
    if abs(real(d(n))-dom(2)) < .02*scl
        dx0 = dom(2)-real(d(n-1)); dx = real(d(n)-d(n-1));
        d(n) = d(n-1) + (dx0/dx)*(d(n)-d(n-1));
    end

    if abs(real(d(1))-dom(1)) < .02*scl                 % snap to left bndry
        dx0 = dom(1)-real(d(2)); dx = real(d(1)-d(2));
        d(1) = d(2) + (dx0/dx)*(d(1)-d(2));
    end
    if abs(real(d(n))-dom(1)) < .02*scl
        dx0 = dom(1)-real(d(n-1)); dx = real(d(n)-d(n-1));
        d(n) = d(n-1) + (dx0/dx)*(d(n)-d(n-1));
    end

    if abs(imag(d(1))-dom(4)) < .02*scl                 % snap to top bndry
        dy0 = dom(4)-imag(d(2)); dy = imag(d(1)-d(2));
        d(1) = d(2) + (dy0/dy)*(d(1)-d(2));
    end
    if abs(imag(d(n))-dom(4)) < .02*scl
        dy0 = dom(4)-imag(d(n-1)); dy = imag(d(n)-d(n-1));
        d(n) = d(n-1) + (dy0/dy)*(d(n)-d(n-1));
    end

    if abs(imag(d(1))-dom(3)) < .02*scl                 % snap to bottom bndry
        dy0 = dom(3)-imag(d(2)); dy = imag(d(1)-d(2));
        d(1) = d(2) + (dy0/dy)*(d(1)-d(2));
    end
    if abs(imag(d(n))-dom(3)) < .02*scl
        dy0 = dom(3)-imag(d(n-1)); dy = imag(d(n)-d(n-1));
        d(n) = d(n-1) + (dy0/dy)*(d(n)-d(n-1));
    end

end

end