function test_example_simulateddata_beamformer

% MEM 4gb
% WALLTIME 00:10:00

%
%% Compute forward simulated data and apply a beamformer scan
%
% This example script shows you how to create some simulated channel-level MEG data with a single dipole at a specified location in the head. Subsequently it does a beamformer source reconstruction to localize that source.
%
% create an array with some magnetometers at 12cm distance from the origin
[X, Y, Z] = sphere(10);
pos = unique([X(:) Y(:) Z(:)], 'rows');
pos = pos(pos(:,3)>=0,:);
grad = [];
grad.coilpos = 12*pos;
grad.coilori = pos; % in the outward direction
% grad.tra = eye(length(pos)); % each coils contributes exactly to one channel
for i=1:length(pos)
  grad.label{i} = sprintf('chan%03d', i);
end

% create a spherical volume conductor with 10cm radius
vol.r = 10;
vol.o = [0 0 0];

% note that beamformer scanning will be done with a 1cm grid, so you should
% not put the dipole on a position that will not be covered by a grid
% location later
cfg = [];
cfg.headmodel = vol;
cfg.grad = grad;
cfg.dip.pos = [0 0 4];    % you can vary the location, here the dipole is along the z-axis
cfg.dip.mom = [1 0 0]';   % the dipole points along the x-axis
cfg.relnoise = 10;
cfg.ntrials = 20;
data = ft_dipolesimulation(cfg);

% compute the data covariance matrix, which will capture the activity of
% the simulated dipole
cfg = [];
cfg.covariance = 'yes';
timelock = ft_timelockanalysis(cfg, data);

% do the beamformer source reconstuction on a 1 cm grid
cfg = [];
cfg.headmodel = vol;
cfg.grad = grad;
cfg.resolution = 1;
cfg.method = 'lcmv';
cfg.lcmv.projectnoise = 'yes'; % needed for neural activity index
source = ft_sourceanalysis(cfg, timelock);

% compute the neural activity index, i.e. projected power divided by
% projected noise
cfg = [];
cfg.powmethod = 'none'; % keep the power as estimated from the data covariance, i.e. the induced power
source_nai = ft_sourcedescriptives(cfg, source);

cfg = [];
cfg.method = 'ortho';
cfg.funparameter = 'nai';
cfg.funcolorlim = [1.4 1.5];  % the voxel in the center of the volume conductor messes up the autoscaling
ft_sourceplot(cfg, source_nai);


function stimList = getRandom(stimList)
% Randomizes rows of a matrix, preserving dependencies between columns
%
% :Usage:
% ::
%
%     stimList = getRandom(stimList)
%
% :Input:
%   a col. vector or matrix of stimulus conditions,
%
%   e.g. [1 1 1 1 2 2 2 2 3 3 4 4]'
%
%output: a randomized permutation of this vector or matrix
% all columns are resorted with the same order
%
% ..
%    Tor Wager, created 2 / 01, last modified 2/25/04 to sort whole matrix
%    Modified 8/7/2015 by Tor to speed and simplify code
% ..

n = size(stimList, 1);

wh = randperm(n)';

stimList = stimList(wh, :);

% Old
%[n, k] = size(stimList);
% randvector = rand(n, 1);						% create random vector
% stimList(:, k + 1) = randvector;
% stimList = sortrows(stimList, k);				% sort the rows by random seed
% stimList = stimList(:, 1:end-1);

end % function


function varargout = mtimes(varargin)
%*	   Pointwise multiplication for CHEBFUN2 objects.
%
%   c*F or F*c multiplies a CHEBFUN2 F by a scalar c.
%
%   F*G computes the integral of F(s,y)G(x,s) over s, the continuous
%   analogue of matrix-matrix multiplication.
%
% See also TIMES.

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

[varargout{1:nargout}] = mtimes@separableApprox(varargin{:});

end


function [Population,Fitness,Next] = EnvironmentalSelection(Population,N,isOrigin)
% The environmental selection of SPEA2

%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Kangjia Qiao

    %% Calculate the fitness of each solution
    if isOrigin==1
        Fitness = CalFitness(Population.objs,Population.cons);
    else
        Fitness = CalFitness(Population.objs);
    end

    %% Environmental selection
    Next = Fitness < 1;
    if sum(Next) < N
        [~,Rank] = sort(Fitness);
        Next(Rank(1:N)) = true;
    elseif sum(Next) > N
        Del  = Truncation(Population(Next).objs,sum(Next)-N);
        Temp = find(Next);
        Next(Temp(Del)) = false;
    end

    % Population for next generation
    Population = Population(Next);
    Fitness    = Fitness(Next);
    % Sort the population
    [Fitness,rank] = sort(Fitness);
    Population = Population(rank);
end

function Del = Truncation(PopObj,K)
% Select part of the solutions by truncation

    %% Truncation
    Distance = pdist2(PopObj,PopObj);
    Distance(logical(eye(length(Distance)))) = inf;
    Del = false(1,size(PopObj,1));
    while sum(Del) < K
        Remain   = find(~Del);
        Temp     = sort(Distance(Remain,Remain),2);
        [~,Rank] = sortrows(Temp);
        Del(Remain(Rank(1))) = true;
    end
end


clc
clear all
tic
%***********************User inserted input for the problem***************

nosp = 5;             %reads the number of sales points considered
noft = 4;                %reads the number of factories
nopd = 3;         %number of planning period
trkcap = 70;                    %Capacity of the truck

%***********************inputs from excel file for the formulation ***************

demand = [35 35	60	80	100; 25	100	125	55	20; 150	120	100	60	110; 50	70	40	140	40 ; 90	110	90	50	30;0	110	90	40	50; 60	70	0	110	10; 140	70	90	100	30 ;10	130	130	40	140; 30	90	70	40	0; 70	100	100	50	90; 110	50	50	110	100 ]; %stores the demand in the time horizons for each sales point
lostdemandcost = [30	35	25	40	55] ;  %lost demand per unit for each sales point
transcost = [ 100	400	800	1100	1700 ; 1500	1800	1200	2200	1500 ; 1400	1000	400	1000	700 ; 800	700	900	700	1200] ; %xlsread('greedyinput','Sheet1','O3:S6');        %transportation cost matrix
setupcost = [ 4500	2000	2500	3000 ] ;  %setup costs for individual factories
factorycap = [300	500	250	150]; % xlsread('greedyinput','Sheet2','B4:E4');       %capacity of the factories
factholdcost = [5	2	4	3]; %xlsread('greedyinput','Sheet2','B5:E5');     %holding cost per unit in a factory
for i = 1:nopd
quantityshipped{i} = zeros(noft,nosp);
factoryresiduals{i} = zeros(1,noft);
factoryactivation{i} = zeros(1,noft);
salespointresiduals{i} = zeros(1,nosp);
end

%*****************************counter-check if the inputs are well read*********

disp(demand);
disp(lostdemandcost);
disp(transcost);
disp(setupcost);
disp(factorycap);
disp(factholdcost);
for i = 1:nopd
disp(quantityshipped{i});
end

%****************************starts the calculation of the greedy alg********

initlostsales = calclostsales(demand,lostdemandcost,nopd,nosp); %Calculates the lost demand with the initial assumption that all the demand is lost
disp(initlostsales);
cumsetupcost = 0;
cumtransportcost = 0;
cumholdingcost = 0;
saveddemand = 0;
salespoint{nosp}= [0,0];
for ft = 1:noft                                          %Initialize the factory activation status and residual for each time preiod (zero for the first one)
           factory{ft}(1,1) = 0;
           factory{ft}(1,2) = 0;
end
for time = 1:nopd                                               %iterates for each planning period beginning from the 1st

       for sp = 1:nosp
           salespoint{sp}(1,1) = 0;                             %the array stores the activation status and the demand of a sales point
            salespoint{sp}(1,2) =  demand(time,sp);             %this populates the sales point for a time horizon with demand and filling status
       end

       for ft = 1:noft                                          %Initialize the factory activation status and residual for each time preiod (zero for the first one)
           factory{ft}(1,1) = 0;

       end

   % loadfactory(capacity,prevresidual);

    for points = 1:nosp
        [selectedsp,maxlostsales] = choosesalespt(lostdemandcost,salespoint,nosp);             %Chooses the salespoint with the highest possible lost sales
        disp('----------------------------');
        fprintf('Selected Sales Point: %d\n', selectedsp);
        fprintf('Potential Lost Sales cost: %.2f\n', maxlostsales);

        salespoint{selectedsp}(1,1)=1;

            [costunit,selectedf] = choosefactory(factorycap,factholdcost,transcost,setupcost,factory,trkcap,salespoint,selectedsp); %chooses the preferable factory to deliver to the selected salespoint
            fprintf('selected factory: %d\n',selectedf);
            fprintf('Cost of the selected factory: %.2f\n', costunit);
            quantityshipped{time}(selectedf,selectedsp) = salespoint{selectedsp}(1,2);
            if (factory{selectedf}(1,2)>= salespoint{selectedsp}(1,2))
            factory{selectedf}(1,2) = factory{selectedf}(1,2) - salespoint{selectedsp}(1,2);

            % saveddemand = saveddemand + salespoint{selectedsp}(1,2)*lostdemandcost(selectedsp);                             % accumulates the reduction of the lost demand for each sent item to a destination sales point
            end
            if(factory{selectedf}(1,1)==0 && factory{selectedf}(1,2)<= salespoint{selectedsp}(1,2))
                factory{selectedf}(1,2) = factory{selectedf}(1,2)+factorycap(selectedf)-salespoint{selectedsp}(1,2);
                cumsetupcost = cumsetupcost + setupcost(selectedf);                                                         %accumulates the activation cost for each factory when there is an accompanying activation
               % cumtransportcost = cumtransportcost + ceil(salespoint{selectedsp}(1,2)/trkcap)*transcost(selectedf,selectedsp);
                factory{selectedf}(1,1)=1;
            end
            saveddemand = saveddemand + salespoint{selectedsp}(1,2)*lostdemandcost(selectedsp);
            cumtransportcost = cumtransportcost + ceil(salespoint{selectedsp}(1,2)/trkcap)*transcost(selectedf,selectedsp); %accumulates the transportaton cost for each iteration

            fprintf('Cumulative setupcost: %.2f\n',cumsetupcost);
            fprintf('Cumulative trasportation cost: %.2f\n',cumtransportcost);
    end
    currentholdingcost = 0;
    for i = 1:noft
        currentholdingcost= currentholdingcost + factory{i}(1,2)*factholdcost(i);                                         %the holding costs are accumulatedat each factory at the end of each planning period
        factoryresiduals{time}(i) = factory{i}(1,2);
        factoryactivation{time}(i) = factory{i}(1,1);
        factory{i}(1,1)= 0;
    end
    cumholdingcost = cumholdingcost + currentholdingcost;                                                                  % holding cost of the whole factory at the end of a planning period
    objectivesolution = initlostsales -saveddemand+ cumholdingcost+cumsetupcost+cumtransportcost
    end
%objectivesolution = initlostsales-saveddemand+cumsetupcost+cumtransportcost
%objectivesolution = initlostsales -saveddemand+ cumholdingcost+cumsetupcost+cumtransportcost
disp('====================shipped quantity matrix==================')
for i = 1:nopd
disp(quantityshipped{i});
end
disp('=====================factory residual matrix=================')
for i = 1:nopd
disp(factoryresiduals{i});
end
disp('=====================factory activation status===============')
for i = 1:nopd
disp(factoryactivation{i});
end
objectivesolution
% New_transportation_costs=0;
% F_holding_cost=0;
% Act_cost= 0;
% for t=1:nopd
%     for i=1:noft
%         F_holding_cost = F_holding_cost + factholdcost(i) *factoryresiduals{t}(1,i);
%         Act_cost = Act_cost + factoryactivation{t}(1,i)* setupcost(i);
%
%
%         for j =1:nosp
%     New_transportation_costs= ceil(quantityshipped{t}(i,j)/trkcap) * transcost(i,j)+ New_transportation_costs;
%         end
%     end
% end
% New_transportation_costs
% F_holding_cost
% Act_cost
% tot_cost = New_transportation_costs + F_holding_cost +Act_cost
% saveddemand
% initlostsales
% cumtransportcost
toc


classdef ThinPlate < Kernel
    % Object of type kernel but optimized for ThinPlate so that the computation of the
    % radial quadrature goes faster
    properties (Access = public)
        C = 1;  %such that G(r) = C*r^2*log(R*r)
        R = 1;
    end

    methods
        function[kernel] = ThinPlate(CC,RR)
            if nargin == 0
                CC = 1;
            end
            if nargin <= 1
                RR = 1;
            end
            kernel@Kernel(@(x)(CC*x.^2.*log(RR*x)),@(x)(2*CC*x.*log(RR*x) + CC*x))
            kernel.scalFunc = @(a,b,rho)(CC*thinPlateSP([a,b],rho,RR));
            kernel.normFunc = @(a,b)(abs(CC)*ThinPlate.norm_func(a,b,RR));
            kernel.gamma_est = @ThinPlate.gamma_est;
            kernel.C = CC;
            kernel.R = RR;
            kernel.singular = false;
        end
    end
    methods (Access = public)
        function[out] = dilatation(this,lambda)
            CC = this.C;
            RR = this.R;
            out = ThinPlate(lambda^2*CC,lambda*RR);
        end
        function[out] = mtimes(this,mu)
            if isa(this,'Kernel')
                assert(and(isa(mu,'double'),isscalar(mu)));
                out = ThinPlate(mu*this.C);
            else
                out = mtimes(mu,this);
            end
        end
        function[out] = radialQuadKernel(this,a,tol,varargin)
            Cmem = this.C;
            this = (1/Cmem)*this;
            rq = RadialQuadrature(a,this,tol/abs(Cmem),varargin{:});
            out = Cmem*rq;
        end
        function[onlineEBD,rq,loc] = offlineEBD(this,X,Y,a,tol)
            % Special way to handle the radial quadrature : we add a term
            % in r^2 to enforce multi-Dirichlet condition.
            rMax = rMaxCalc(X,Y);
            x = X/rMax;
            y = Y/rMax;
            k1 = ThinPlate(this.C*rMax^2,this.R*rMax) +...
                X2kKernel(1,-this.C*(1+log(rMax*this.R))*rMax^2); % adjust multi-Dirichlet condition.
            rq = RadialQuadrature(a,k1,tol);
            q2d = Quad2D(rq);
            loc = localCorrections(x,y,a,k1,rq,tol,false);
            onlineEBD = @(v)(q2d.conv(x,y,v) + loc*v + suppTerm(v));

            % We remove the added r^2 term by computing the convolution as
            % follows:
            function[res] = suppTerm(alpha)
                alphay = [alpha alpha].*y;
                x2 = x(:,1).^2 + x(:,2).^2;
                y2 = y(:,1).^2 + y(:,2).^2;
                alphay2 = alpha.*y2;
                res = this.C*rMax^2*(log(rMax*this.R)+1)*(...
                    x2*sum(alpha)...
                    - 2*x(:,1)*sum(alphay(:,1)) - 2*x(:,2)*sum(alphay(:,2))...
                    + sum(alphay2));
            end
        end
    end
    methods (Static, Access = protected)
        function[res] = norm_func(a,b,R)
            % Found with Maple
            if a==0
                res = sqrt(2*pi*(...
                    b ^ 4 * log(R * b) ^ 2 +...
                    b ^ 4 * log(R * b) / 0.2e1 + b ^ 4 / 0.8e1));

            else
                res = sqrt(2*pi*(...
                    -a ^ 4 * log(R * a) ^ 2 - a ^ 4 * log(R * a) / 0.2e1 ...
                    - a ^ 4 / 0.8e1 + b ^ 4 * log(R * b) ^ 2 +...
                    b ^ 4 * log(R * b) / 0.2e1 + b ^ 4 / 0.8e1));
            end
        end
        function[low,up] = gamma_est(~)
            % Helps the radial quadrature to guess the number of components
            up = 7;
            low= 0;
        end

    end

end


function S = globMatrixPGIFE3DMass(fun,mesh,femI,fem1,fem2)

%% USAGE: generate stiffness global matrix on a 3D mesh
%
% INPUTS:
% fun --- coefficient function
% mesh --- a struct data contains very rich mesh information.
% fem1 --- global DoF for test function space
% fem2 --- global DoF for trial function space
%
% OUTPUTS:
% [IN JN XN] --- triplets of the sparse matrix from regular elements.
% [II JI XI] --- triplets of the sparse matrix from interface elements.

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

%% 0. Initializaiton
if strcmp(fem1.type,'P1')||strcmp(fem1.type,'DGP1')||strcmp(fem1.type,'CR')
    feEvalBas1 = @evalP1Bas3D;
elseif strcmp(fem1.type,'P2')||strcmp(fem1.type,'DGP2')
    feEvalBas1 = @evalP2Bas3D;
end

if strcmp(fem2.type,'P1')||strcmp(fem2.type,'DGP1')||strcmp(fem2.type,'CR')
    feEvalBas2 = @evalP1Bas3D;
elseif strcmp(fem2.type,'P2')||strcmp(fem2.type,'DGP2')
    feEvalBas2 = @evalP2Bas3D;
end

%% 1. Matrix on noninterface elements
dof1 = fem1.ldof; dof2 = fem2.ldof; nloc = dof1*dof2;
ntID = find(mesh.tLoc > 0); ntN = length(ntID);
AN = fem1.area(ntID);
gxN = fem1.gx(ntID,:); gyN = fem1.gy(ntID,:); gzN = fem1.gz(ntID,:); gw = fem1.gw;
XN = zeros(nloc*ntN, 1);

coefN = feval(fun,gxN,gyN,gzN);
Ibas = cell(dof1,1);
Jbas = cell(dof2,1);
for i = 1:dof1
    Ibas{i} = feEvalBas1(fem1.bas(ntID,:,i), gxN, gyN, gzN, [0,0,0]);
end
for j = 1:dof2
    Jbas{j} = feEvalBas2(fem2.bas(ntID,:,j), gxN, gyN, gzN, [0,0,0]);
end

IN = reshape(repmat(fem1.t(ntID,:),4,1),nloc*ntN,1);
JN = repmat(reshape(fem2.t(ntID,:),dof2*ntN,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XN(ind+1:ind+ntN) = AN.*(sum(((Ibas{i}.*(coefN.*Jbas{j})).*gw'),2));
        ind = ind + ntN;
    end
end
ID = find(XN~=0);
SN = sparse(IN(ID),JN(ID),XN(ID),size(fem1.p,1),size(fem2.p,1));

%% 2. Matrix on interface elements
AI = femI.area; gw = femI.gw; gxI = femI.gx; gyI = femI.gy; gzI = femI.gz;
ntI = size(femI.t,1); % not number of interface element, but quadrature element
XI = zeros(nloc*ntI, 1);
tID = femI.QelemID;

coefI = feval(fun,gxI,gyI,gzI);
Ibas = cell(dof1,1);
Jbas = cell(dof2,1);
for i = 1:dof1
    Ibas{i} = feEvalBas1(fem1.bas(tID,:,i), gxI, gyI, gzI, [0,0,0]);
end
for j = 1:dof2
    Jbas{j} = feEvalBas2(femI.bas(:,:,j), gxI, gyI, gzI, [0,0,0]);
end

II = reshape(repmat(femI.t,4,1),nloc*ntI,1);
JI = repmat(reshape(femI.t,dof2*ntI,1),4,1);
ind = 0;
for i = 1:dof1
    for j = 1:dof2
        XI(ind+1:ind+ntI) = AI.*(sum(((Ibas{i}.*(coefI.*Jbas{j})).*gw'),2));
        ind = ind + ntI;
    end
end
SI = sparse(II,JI,XI,size(fem1.p,1),size(fem2.p,1));
S = SN + SI;


function range_profile = RCS_Range_Profile( complex_data, ...
    oversample_ratio, cal_sf, mask)
%RCS_RANGE_PROFILE Computes calibrated range profile
%
% USAGE:
%   [range_profile] = RCS_Compute(complex_data, oversample_ratio, cal_sf, mask)
%
% INPUTS:
%   complex_data     - required : Complex valued SAR dataset in the image
%                                 domain.  First dimension is azimuth,
%                                 second range.  Third dimension could be
%                                 for multi-channel (i.e. polarimetric)
%                                 data.  This code assumes the frequency
%                                 support is centered and constant across
%                                 the image.
%   oversample_ratio - optional : Overample or zeropad factor. (Required
%                                 for calibrated RCS.)
%   cal_sf           - optional : Calibration scale factor (linear).
%                                 Either a constant or an array the same
%                                 size as complex_data with per-pixel
%                                 values. (Required for calibrated RCS.)
%   mask             - optional : Binary image which is ones over the
%                                 region of interest. (Default is an image
%                                 of all ones.)
%
% OUTPUTS:
%   range_profile    - optional : Range profile.
%
% Author: Wade Schwartzkopf, NGA/IDT
%
% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////

% Apply shape mask to rectangular data
filtimg = complex_data.*repmat(mask,[1 1 size(complex_data,3)]);
if ~isscalar(cal_sf)
    % Multi-channel (polarimetric)
    cal_sf = repmat(cal_sf,[1 1 size(complex_data,3)]);
end

range_profile = (1/prod(oversample_ratio)) * sum(cal_sf .* abs(filtimg).^2,1);
if isvector(range_profile)
    range_profile = range_profile(:); % Assure in first dimension
else
    range_profile = squeeze(range_profile); % For multi-channel (polarimetric) data
end

end

% //////////////////////////////////////////
% /// CLASSIFICATION: UNCLASSIFIED       ///
% //////////////////////////////////////////


clear all; close all;
I=imread('snowflakes.png');
se=strel('disk', 5);
J=imopen(I, se);
figure;
subplot(121);  imshow(I);
subplot(122);  imshow(J, []);


classdef MaF1 < PROBLEM
% <multi/many> <real> <large/none>
% Inverted DTLZ1

%------------------------------- Reference --------------------------------
% R. Cheng, M. Li, Y. Tian, X. Zhang, S. Yang, Y. Jin, and X. Yao, A
% benchmark test suite for evolutionary many-objective optimization,
% Complex & Intelligent Systems, 2017, 3(1): 67-81.
%------------------------------- 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".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            if isempty(obj.M); obj.M = 3; end
            if isempty(obj.D); obj.D = obj.M + 9; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            g      = sum((PopDec(:,obj.M:end)-0.5).^2,2);
            PopObj = repmat(1+g,1,obj.M) - repmat(1+g,1,obj.M).*fliplr(cumprod([ones(size(g,1),1),PopDec(:,1:obj.M-1)],2)).*[ones(size(g,1),1),1-PopDec(:,obj.M-1:-1:1)];
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R = 1 - UniformPoint(N,obj.M);
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            if obj.M == 2
                R = obj.GetOptimum(100);
            elseif obj.M == 3
                a = linspace(0,1,10)';
                R = {1-a*a',1-a*(1-a'),1-(1-a)*ones(size(a'))};
            else
                R = [];
            end
        end
    end
end


function [posterior] = tapas_linear_estimate(y, x, model, inference, pars)
%% Estimate a linear model
%
% aponteeduardo@gmail.com
% copyright (C) 2016
%

n = 3;
if nargin < n
    model = struct();
end

n = n + 1;
if nargin < n
    inference = struct();
end

n = n + 1;
if nargin < n
    pars = struct();
end

[pars] = tapas_linear_pars(pars);
[data] = tapas_linear_data(y, x, pars);
[model] = tapas_linear_model(model, pars);
[inference] = tapas_linear_inference(inference, pars);

[posterior] = tapas_linear_estimate_interface(data, model, inference);

end


% Test file for @deltafun/zeroDeltaFun.m.

function pass = test_zeroDeltaFun(pref)

if (nargin < 1)
    pref = chebfunpref();
end
%%
dTol = pref.deltaPrefs.deltaTol;

d = deltafun.zeroDeltaFun();
pass(1) = iszero(d.funPart);
pass(2) = all( d.funPart.domain == [-1, 1] );
pass(3) = isempty(d.deltaMag);
pass(4) = isempty(d.deltaLoc);

d = deltafun.zeroDeltaFun([4, 5]);
pass(5) = iszero(d.funPart);
pass(6) = all( d.funPart.domain == [4, 5] );
pass(7) = isempty(d.deltaMag);
pass(8) = isempty(d.deltaLoc);

end


function [mssim, ssim_map] = ssim(img1, img2, K, window, L)

% ========================================================================
% SSIM Index with automatic downsampling, Version 1.0
% Copyright(c) 2009 Zhou Wang
% All Rights Reserved.
%
% ----------------------------------------------------------------------
% Permission to use, copy, or modify this software and its documentation
% for educational and research purposes only and without fee is hereby
% granted, provided that this copyright notice and the original authors'
% names appear on all copies and supporting documentation. This program
% shall not be used, rewritten, or adapted as the basis of a commercial
% software or hardware product without first obtaining permission of the
% authors. The authors make no representations about the suitability of
% this software for any purpose. It is provided "as is" without express
% or implied warranty.
%----------------------------------------------------------------------
%
% This is an implementation of the algorithm for calculating the
% Structural SIMilarity (SSIM) index between two images
%
% Please refer to the following paper and the website with suggested usage
%
% Z. Wang, A. C. Bovik, H. R. Sheikh, and E. P. Simoncelli, "Image
% quality assessment: From error visibility to structural similarity,"
% IEEE Transactios on Image Processing, vol. 13, no. 4, pp. 600-612,
% Apr. 2004.
%
% http://www.ece.uwaterloo.ca/~z70wang/research/ssim/
%
% Note: This program is different from ssim_index.m, where no automatic
% downsampling is performed. (downsampling was done in the above paper
% and was described as suggested usage in the above website.)
%
% Kindly report any suggestions or corrections to zhouwang@ieee.org
%
%----------------------------------------------------------------------
%
%Input : (1) img1: the first image being compared
%        (2) img2: the second image being compared
%        (3) K: constants in the SSIM index formula (see the above
%            reference). defualt value: K = [0.01 0.03]
%        (4) window: local window for statistics (see the above
%            reference). default widnow is Gaussian given by
%            window = fspecial('gaussian', 11, 1.5);
%        (5) L: dynamic range of the images. default: L = 255
%
%Output: (1) mssim: the mean SSIM index value between 2 images.
%            If one of the images being compared is regarded as
%            perfect quality, then mssim can be considered as the
%            quality measure of the other image.
%            If img1 = img2, then mssim = 1.
%        (2) ssim_map: the SSIM index map of the test image. The map
%            has a smaller size than the input images. The actual size
%            depends on the window size and the downsampling factor.
%
%Basic Usage:
%   Given 2 test images img1 and img2, whose dynamic range is 0-255
%
%   [mssim, ssim_map] = ssim(img1, img2);
%
%Advanced Usage:
%   User defined parameters. For example
%
%   K = [0.05 0.05];
%   window = ones(8);
%   L = 100;
%   [mssim, ssim_map] = ssim(img1, img2, K, window, L);
%
%Visualize the results:
%
%   mssim                        %Gives the mssim value
%   imshow(max(0, ssim_map).^4)  %Shows the SSIM index map
%========================================================================

if (nargin < 2 || nargin > 5)
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

if (size(img1) ~= size(img2))
   mssim = -Inf;
   ssim_map = -Inf;
   return;
end

[M N] = size(img1);

if (nargin == 2)
   if ((M < 11) || (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   %window = fspecial('gaussian', 11, 1.5);	%
   window = Gaussian_window(11,1.5);
   K(1) = 0.01;					% default settings
   K(2) = 0.03;					%
   L = 255;                                     %
end

if (nargin == 3)
   if ((M < 11) || (N < 11))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   %window = fspecial('gaussian', 11, 1.5);
   window = Gaussian_window(11,1.5);
   L = 255;
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 4)
   [H W] = size(window);
   if ((H*W) < 4 || (H > M) || (W > N))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   L = 255;
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

if (nargin == 5)
   [H W] = size(window);
   if ((H*W) < 4 || (H > M) || (W > N))
	   mssim = -Inf;
	   ssim_map = -Inf;
      return
   end
   if (length(K) == 2)
      if (K(1) < 0 || K(2) < 0)
		   mssim = -Inf;
   		ssim_map = -Inf;
	   	return;
      end
   else
	   mssim = -Inf;
   	ssim_map = -Inf;
	   return;
   end
end

img1 = double(img1);
img2 = double(img2);

% automatic downsampling
f = max(1,round(min(M,N)/256));
%downsampling by f
%use a simple low-pass filter
if(f>1)
    lpf = ones(f,f);
    lpf = lpf/sum(lpf(:));
    img1 = imfilter(img1,lpf,'symmetric','same');
    img2 = imfilter(img2,lpf,'symmetric','same');

    img1 = img1(1:f:end,1:f:end);
    img2 = img2(1:f:end,1:f:end);
end

C1 = (K(1)*L)^2;
C2 = (K(2)*L)^2;
window = window/sum(sum(window));

mu1   = filter2(window, img1, 'valid');
mu2   = filter2(window, img2, 'valid');
mu1_sq = mu1.*mu1;
mu2_sq = mu2.*mu2;
mu1_mu2 = mu1.*mu2;
sigma1_sq = filter2(window, img1.*img1, 'valid') - mu1_sq;
sigma2_sq = filter2(window, img2.*img2, 'valid') - mu2_sq;
sigma12 = filter2(window, img1.*img2, 'valid') - mu1_mu2;

if (C1 > 0 && C2 > 0)
   ssim_map = ((2*mu1_mu2 + C1).*(2*sigma12 + C2))./((mu1_sq + mu2_sq + C1).*(sigma1_sq + sigma2_sq + C2));
else
   numerator1 = 2*mu1_mu2 + C1;
   numerator2 = 2*sigma12 + C2;
	denominator1 = mu1_sq + mu2_sq + C1;
   denominator2 = sigma1_sq + sigma2_sq + C2;
   ssim_map = ones(size(mu1));
   index = (denominator1.*denominator2 > 0);
   ssim_map(index) = (numerator1(index).*numerator2(index))./(denominator1(index).*denominator2(index));
   index = (denominator1 ~= 0) & (denominator2 == 0);
   ssim_map(index) = numerator1(index)./denominator1(index);
end

mssim = mean(ssim_map(:));

return


function varargout = fillPolygon(varargin)
%FILLPOLYGON Fill a polygon specified by a list of points
%
%   fillPolygon(POLY);
%   Fills the interior of the polygon specified by POLY. The boundary of
%   the polygon is not drawn, see 'drawPolygon' to do it.
%   POLY is a single [N*2] array.
%   If POLY contains NaN-couples, each portion between the [NaN;NaN] will
%   be filled separately.
%
%   fillPolygon(PX, PY);
%   Specifies coordinates of the polygon in separate arrays.
%
%
%   H = fillPolygon(...);
%   Also returns a handle to the created patch
%
%
%   See also:
%     polygons2d, drawCurve, drawPolygon

%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 07/04/2005.
%

%   HISTORY
%   2008-05-07 add psb to specify drawing options
%   2008/10/15 add psb to draw polygons with holes

% check input
if isempty(varargin)
    error('need to specify a polygon');
end

% case of a set of polygons stored in a cell array
var = varargin{1};
if iscell(var)
    N = length(var);
    h = zeros(N, 1);
    for i = 1:N
        % check for empty polygons
        if ~isempty(var{i})
            h(i) = fillPolygon(var{i}, varargin{2:end});
        end
    end

    % setup output values
    if nargout > 0
        varargout{1} = h;
    end
    return;
end

% Extract coordinates of polygon vertices
if size(var, 2) > 1
    % first argument is a polygon array
    px = var(:, 1);
    py = var(:, 2);
    varargin(1) = [];
else
    % arguments 1 and 2 correspond to x and y coordinate respectively
    if length(varargin) < 2
        error('should specify either a N*2 array, or 2 N*1 vectors');
    end

    px = varargin{1};
    py = varargin{2};
    varargin(1:2) = [];
end

% Find position of breaks, and copies first point of each loop at the end
inds = find(isnan(px(:)));
i1 = [inds ; length(px)+1];
i0 = [1 ; inds+1];
px(i1, :) = px(i0, :);
py(i1, :) = py(i0, :);

% set default line format
if isempty(varargin)
    varargin = {'b'};
end

% fill the polygon with desired style
h = fill(px, py, varargin{:}, 'lineStyle', 'none');

% output
if nargout > 0
    varargout{1} = h;
end


%% Solves the Krusell and Smith (1998)
% Solving Krusell-Smith model using perturbation method involves simple 5
% step process given by
%
% # Solve for Steady State
% # Linearize Model Equations
% # Solve out Static Constraint (optional reduce model)
% # Solve Linear System
% # Compute Impulse Response Functions
% # Check for Internal Consistency (if model reduction is used)
% where each step just requires about one function call.
%
% In this example file, the entire problem will be solved total of 3 times.
% First, KS model will be solved without any model reduction. Second, KS
% model will be solved with model reduction, to compare how well the model
% reduction works. Lastly, different parts of observables will be
% introduced to compare how the model reduction works with different
% requirements. We have updates to Internal Consistency check coming in the
% future.
%
% Estimated Runtime: 1.5 seconds
%
% REFERENCES:
%
% * Ahn, SeHyoun, Greg Kaplan, Benjamin Moll, Thomas Winberry, and
% Christian Wolf. "When Inequality Matters for Macro and Macro Matters for
% Inequality."
% * Krusell, Per, and Anthony A. Smith, Jr. "Income and wealth heterogeneity in the macroeconomy." Journal of political Economy 106.5 (1998): 867-896.
%
% REQUIRES:
%
% * auto diff toolbox: <https://github.com/sehyoun/MATLABAutoDiff>
% * phact toolbox: <https://github.com/gregkaplan/phact>
% * compute_steady_state.m
% * equilibrium_conditions.m
% * plot_steady_state.m
% * plot_IRFs.m
% Relevant files except for auto diff toolbox, can be found in
%    /examples/KrusellSmith folder of phact toolbox provided on github
%
%%

%% Setup the toolbox
% Just need to include folders containing the files in the path.

addpath('/home/sehyoun/Dropbox/0Packages/AutoDiff');
addpath('/home/sehyoun/Dropbox/0Packages/PHACT');

%% Set options for this example run
% example_case : 1   run full and compare with only value function reduction
%                2   run full and compare with only state space reduction
%                3   run full and compare with both reductions
example_case = 3;
reduceDist_hor = 50;        % m of K_m(A,b)

% initialize shocks for simulation
T = 200;
N = 2000;
vAggregateShock = zeros(1,N);
vAggregateShock(1,1) = 1;
%vAggregateShock = randn(1,N);     % Uncomment for one realization instead
                                  % of IRFs

%% Step 0: Set Parameters
% The script sets up parameters relevant for the model
set_parameters;

%% Step 1: Solve for Steady State
% Non-stochastic steady state can be found using any methods. In
%    particular, example codes can be found at
%    <http://www.princeton.edu/~moll/HACTproject.htm>.

tStart = tic;
fprintf('Computing steady state...\n')
global IfSS IbSS I0SS varsSS A

[rSS,wSS,KSS,ASS,uSS,cSS,VSS,gSS,dVUSS,dVfSS,dVbSS,IfSS,IbSS,I0SS] = ...
    compute_steady_state();

fprintf('Time to compute steady state: %.3g seconds\n\n\n',toc(tStart));

% Store steady state values
varsSS = zeros(nVars,1);
varsSS(1:2*I,1) = reshape(VSS,2*I,1);
ggSS = reshape(gSS,2*I,1);
varsSS(2*I+1:4*I-1,1) = ggSS(1:2*I-1);
varsSS(4*I,1) = 0;
varsSS(4*I+1,1) = KSS;
varsSS(4*I+2,1) = rSS;
varsSS(4*I+3,1) = wSS;
varsSS(4*I+4,1) = (KSS ^ aalpha) * (zAvg ^ (1 - aalpha));
CSS = sum(cSS(:) .* gSS(:) * da);
varsSS(4*I+5,1) = CSS;
varsSS(4*I+6,1) = ddelta * KSS;

% plot steady state results
plot_steady_state;

%% Step 2: Linearize Model Equations
% For computing derivatives, the codes written for solving for the
%    steady-state can be used almost verbatim using automatic
%    differentiation toolbox as long as only the functions supported by
%    automatic differentation are used. For list of supported functions and
%    documentation of relevant syntax check
%    <https://github.com/sehyoun/MATLABAutoDiff>. Example usage/syntax of
%    automatic differentiation can be found at
%    <http://sehyoun.com/EXAMPLE_AutoDiff_Syntax.html>
fprintf('Taking derivatives of equilibrium conditions...\n')
t0 = tic;

% Prepare automatic differentiation
vars = zeros(2*nVars+nEErrors+1,1);
vars = myAD(vars);

% Evaluate derivatives
derivativesIntermediate = equilibrium_conditions(vars);

% Extract out derivative values
derivs = getderivs(derivativesIntermediate);

tDerivs = toc(t0);
fprintf('...Done!\n')
fprintf('Time to compute derivatives: %2.4f seconds\n\n\n',tDerivs)
if tDerivs > 1
    warning('If you do not compile mex files for automatics differentiation, matrix vector multiplication will be slow');
    disp('Press any key to continue...');
    pause();
end

% Unpackage derivatives
mVarsDerivs = derivs(:,1:nVars);
mVarsDotDerivs = derivs(:,nVars+1:2*nVars);
mEErrorsDerivs = derivs(:,2*nVars+1:2*nVars+nEErrors);
mShocksDerivs = derivs(:,2*nVars+nEErrors+1);

%% Step 3 (full): Solve out Static Constraints and/or Reduce Models
% For the first run, a unreduced model will be solved

% rename derivative matrix to match notation in paper
g0 = mVarsDotDerivs;
g1 = -mVarsDerivs;
c = sparse(nVars,1);
psi = -mShocksDerivs;
pi = -mEErrorsDerivs;

% Solve out static constratins
[~,inv_state_red,g0,g1,constant,pi,psi] = clean_G0_sparse(g0,g1,c,pi,psi);

%% Step 4: Solve Linear System
t0 = tic;
fprintf('Solving linear system...\n')

[G1,~,impact,eu] = schur_solver(g0,g1,c,psi,pi,1,1,1);

fprintf('...Done!\n')
fprintf('Existence and uniqueness? %2.0f and %2.0f\n',eu);
fprintf('Time to solve full linear system: %2.4f seconds\n\n\n',toc(t0))

%% Step 5: Simulate Impulse Response Functions
t0 = tic;

[simulated,~] = simulate(G1,impact,T,N,vAggregateShock,'implicit',inv_state_red);%,4*I:4*I+6);

fprintf('...Done!\n')
fprintf('Time to simulate model: %2.4f seconds\n\n\n',toc(t0))

big_simul = simulated + varsSS;
simulated = simulated(4*I:4*I+6,:);

varsSS_small = varsSS(4*I:4*I+6,1);
% Add state-states back in to get values in levels
vAggregateTFP			= simulated(1,:) + varsSS_small(1);
vAggregateOutput		= simulated(5,:) + varsSS_small(5);
vAggregateConsumption	= simulated(6,:) + varsSS_small(6);
vAggregateInvestment 	= simulated(7,:) + varsSS_small(7);

% Compute log differences for plotting
vAggregateTFP_full		= vAggregateTFP;
vAggregateOutput_full	= log(vAggregateOutput) - log(varsSS_small(5));
vAggregateConsumption_full	= log(vAggregateConsumption) - log(varsSS_small(6));
vAggregateInvestment_full	= log(vAggregateInvestment) - log(varsSS_small(7));

%% Step 3 (reduced): Solve Out Static Constraints and/or Reduce Models

% Set relevant parameters for example cases
if (example_case == 1)
    reduceV = 1;
    reduceDistribution = 0;
elseif (example_case == 2)
    reduceV = 0;
    reduceDistribution = 1;
else
    reduceV = 1;
    reduceDistribution = 1;
end

% rename derivatives to match notation in paper
g0 = mVarsDotDerivs;
g1 = -mVarsDerivs;
c = sparse(nVars,1);
psi = -mShocksDerivs;
pi = -mEErrorsDerivs;

t0 = tic;
fprintf('Model Reduction ...\n')

%%
% *State space reduction using Krylov subspace method*
if reduceDistribution == 1
    % State space reduction
    [state_red,inv_state_red,n_g_red] = krylov_reduction(g0,g1,n_v,n_g,reduceDist_hor);
    [g1,psi,pi,c,g0] = change_basis(state_red,inv_state_red,g1,psi,pi,c,g0);
else
    % Clean G0
    [state_red,inv_state_red,g0,g1,c,pi,psi] = clean_G0_sparse(g0,g1,c,pi,psi);
    n_g_red = n_g;
end

%%
% *Value function reduction using spline inspired bases*
if reduceV == 1
    % Create knot points for spline (the knot points are not uniformly spaced)
    knots = linspace(amin,amax,n_knots-1)';
    knots = (amax-amin)/(2^c_power-1)*((knots-amin)/(amax-amin)+1).^c_power+amin-(amax-amin)/(2^c_power-1);
    % Function calls to create basis reduction
    [from_spline, to_spline] = oneDquad_spline(x,knots);
    [from_spline, to_spline] = extend_to_nd(from_spline,to_spline,n_prior,n_post);
    n_splined = size(from_spline,2);
    [from_spline, to_spline] = projection_for_subset(from_spline,to_spline,0,n_g_red);

    % Reduce the decision vector
    [g1,psi,~,c,g0] = change_basis(to_spline,from_spline,g1,psi,pi,c,g0);
    pi = to_spline * pi * from_spline(1:n_v,1:n_splined);
elseif reduceV == 0
    from_spline = speye(n_g_red + n_v);
    to_spline = speye(n_g_red + n_v);
    n_splined = n_v;
end
fprintf('...Done!\n')
fprintf('Time to reduce dimensionality: %2.4f seconds\n\n\n',toc(t0))

%% Step 4: Solve Linear System
t0 = tic;
fprintf('Solving reduced linear system...\n')

[G1,~,impact,eu,F] = schur_solver(g0,g1,c,psi,pi,1,1,1);

fprintf('...Done!\n')
fprintf('Existence and uniqueness? %2.0f and %2.0f\n',eu);
fprintf('Time to solve linear system: %2.4f seconds\n\n\n',toc(t0))

%% Step 5: Simulate Impulse Response Functions
fprintf('Simulating Model...\n')
t0 = tic;

trans_mat = inv_state_red*from_spline;
[simulated,vTime] = simulate(G1,impact,T,N,vAggregateShock,'implicit',trans_mat,4*I:4*I+6);

fprintf('...Done!\n')
fprintf('Time to simulate model: %2.4f seconds\n\n\n',toc(t0))

% Add state-states back in to get values in levels
vAggregateTFP = simulated(1,:) + varsSS_small(1);
vAggregateOuput = simulated(5,:) + varsSS_small(5);
vAggregateConsumption = simulated(6,:) + varsSS_small(6);
vAggregateInvestment = simulated(7,:) + varsSS_small(7);

% Compute log differences for plotting
vAggregateTFP_reduced = vAggregateTFP;
vAggregateOutput_reduced = log(vAggregateOutput) - log(varsSS_small(5));
vAggregateConsumption_reduced = log(vAggregateConsumption) - log(varsSS_small(6));
vAggregateInvestment_reduced = log(vAggregateInvestment) - log(varsSS_small(7));

%% Step 6: Internal Consistency Check
% For large problem, we are forced to take explicit update, so it can take
% awhile to run, but the good thing is that this only needs to be run at
% the end. We are currently working on this, so speed for this part might
% improve in the future. (For small problems, the speed is not an issue). A
% different consistency check is in works, and will be updated in the
% future.

g1 = -mVarsDerivs;
psi = -mShocksDerivs;
from_red = inv_state_red * from_spline;
to_red = to_spline * state_red;
[epsilon] = internal_consistency_check(G1,impact,n_g_red,from_red,to_red,g1,psi,F,n_v,n_g,1000,varsSS,1,0);

%% (optional) Step 7: Plot relevant values
% Plot impulse response functions
plot_IRFs;

%% State Space Reduction with More Observables
% It is possible to include other variables into observables. In this part,
%    different parts of the distribution will be included in the model
%    reduction to show how the Krylov subspace based model reduction
%    behaves. To that regard, the reduced model is solved while including
%    different parts of distribution as part of the observable. In this
%    case, a very strong requirement of knowing the probability density
%    value at different grid points of the distribution g is taken. In the
%    first case, 80th through 89th grid points were included in the parts
%    that we consider to be observable. For the second example, 85th
%    through 94th grid points were included in observable.

% rename derivative matrix to match notation in paper
g0 = mVarsDotDerivs;
g1 = -mVarsDerivs;
c = sparse(nVars,1);
psi = -mShocksDerivs;
pi = -mEErrorsDerivs;

% Solve full model
[~,inv_state_red,g0_full,g1_full,c_full,pi_full,psi_full] = clean_G0_sparse(g0,g1,c,pi,psi);
[G1,~,impact,eu] = schur_solver(g0_full,g1_full,c_full,psi_full,pi_full,1,1,1);
[simulated,vTime] = simulate(G1,impact,T,N,vAggregateShock,'implicit',inv_state_red);
simulated_full = simulated + varsSS;

% Solve reduced without extra observables
[state_red,inv_state_red,n_g_red] = krylov_reduction(g0,g1,n_v,n_g,20);
[g1_red,psi_red,pi_red,c_red,g0_red] = change_basis(state_red,inv_state_red,g1,psi,pi,c,g0);
[G1,~,impact,eu] = schur_solver(g0_red,g1_red,c_red,psi_red,pi_red,1,1,1);
[simulated,vTime] = simulate(G1,impact,T,N,vAggregateShock,'implicit',inv_state_red);
simulated_red = simulated + varsSS;

%%
% Solve reduced with extra observables for 80th through 89th grid point

[state_red,inv_state_red,n_g_red] = krylov_reduction(g0,g1,n_v,n_g,20,spdiags(ones(n_g,1),79,10,n_g));
[g1_80,psi_80,pi_80,c_80,g0_80] = change_basis(state_red,inv_state_red,g1,psi,pi,c,g0);
[G1,~,impact,eu] = schur_solver(g0_80,g1_80,c_80,psi_80,pi_80,1,1,1);
[simulated,vTime] = simulate(G1,impact,T,N,vAggregateShock,'implicit',inv_state_red);
simulated_obs = simulated + varsSS;
diff_red = (abs(simulated_red(n_v+1:end,:) - simulated_full(n_v+1:end,:)))./simulated_full(n_v+1:end,:);
diff_obs80 = (abs(simulated_obs(n_v+1:end,:) - simulated_full(n_v+1:end,:)))./simulated_full(n_v+1:end,:);

figure;
plot(70:99,log(diff_red(70:99,11)));
hold on;
plot(70:99,log(diff_obs80(70:99,11)));
grid on
legend('Location','northwest');
legend('without','with grid 80~89 included');
title(['Error Comparison at time ',num2str(vTime(11))],'interpreter','latex','fontsize',14)
ylabel('$\log$(relative error)','interpreter','latex')
xlabel('Distribution Location','interpreter','latex')

%%
% Solve reduced with extra observables for 85th through 94th grid point

[state_red,inv_state_red,n_g_red] = krylov_reduction(g0,g1,n_v,n_g,20,spdiags(ones(n_g,1),84,10,n_g));
[g1,psi,pi,c,g0] = change_basis(state_red,inv_state_red,g1,psi,pi,c,g0);
[G1,~,impact,eu] = schur_solver(g0,g1,c,psi,pi,1,1,1);
[simulated,vTime] = simulate(G1,impact,T,N,vAggregateShock,'implicit',inv_state_red);
simulated_obs = simulated + varsSS;
diff_obs = (abs(simulated_obs(n_v+1:end,:) - simulated_full(n_v+1:end,:)))./simulated_full(n_v+1:end,:);

figure;
plot(70:99,log(diff_red(70:99,11)));
hold on;
plot(70:99,log(diff_obs(70:99,11)));
grid on
legend('Location','northwest');
legend('without','with grid 85~94 included');
title(['Error Comparison at time ',num2str(vTime(11))],'interpreter','latex','fontsize',14)
ylabel('$\log$(relative error)','interpreter','latex')
xlabel('Distribution Location','interpreter','latex')

%%
% Note how the errors decreases only for the parts that we required to be
% observable while the irrelevant parts were reduced away. This is the
% magic of Krylov subspace based reduction in action. Though we could solve
% the full model for the simple Krusell-Smith case, this ability to reduce
% away irrelevant part will be essential for bigger problems.
%

%%
% Lastly, some of the functions have lengthy parameters, you can always
% type "help function_name;" to see documentation. All functions contain the
% function call syntax as the last line, and thoese can be copied and
% pasted into the program file. For example, to use the
% krylov_reduction function, you can call
help krylov_reduction;

%%
% and just copy the lastline of the documentation string. Alternately, you
% can call "doc function_name" to read the doc string in the MATLAB help
% browser, and "edit function_name" to see the function codes.


clear;
disp('Program started');
% control mode (0: impedance control
%               1: admittance control(velocity command)
%               2: admittance control(position command))
control_mode = 0;

% reference trajectory mode (0: horizontal line
%                            1: circle)
trajectory_mode = 0;

% dyanmic params(the same as those used in vrep)
I1 = 1;
I2 = 1;
l1 = 0.3;
l2 = 0.3;
m1 = 0.5;
m2 = 0.5;
a1 = 0.15;
a2 = 0.15;
g = 9.81;
% impedance model params
kpx = 150;
kpy = 150;
kdx = 70;
kdy = 70;
mx = 1;
my = 1;

% for moving avarage force filter
windowSize = 5;
b = (1/windowSize)*ones(1,windowSize);
a = 1;
raw_externalforces = zeros(2,windowSize);
filtered_externalforce = zeros(2,1);

vrep=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
vrep.simxFinish(-1);      % just in case, close all opened connections
id=vrep.simxStart('127.0.0.1',19997,true,true,5000,5); % connect to vrep server
if (id>-1)
    disp('Connected to remote API server');
    % timestep
    dt = 0.01;
    vrep.simxSetFloatingParameter(id,vrep.sim_floatparam_simulation_time_step,dt,vrep.simx_opmode_oneshot_wait);
    % set sychronous mode
    vrep.simxSynchronous(id,true);
    % start the simulation:
    vrep.simxStartSimulation(id,vrep.simx_opmode_oneshot_wait);
    % trigger one step simulation and pause a short time to clear dirty
    % data in vrep buffer
    vrep.simxSynchronousTrigger(id);
%     pause(0.3);

    handles = two_link_workcell_init(vrep,id);
    armJoints = handles.armJoints;
    forcesensor = handles.forcesensor;
    x0 = [0;0;0;0];
    tau = [0;0];

    % set joint control mode in vrep
    if control_mode == 2
        for i=1:2
          vrep.simxSetObjectIntParameter(id,armJoints(i),2001,1,vrep.simx_opmode_oneshot_wait);
        end
    else
        for i=1:2
          vrep.simxSetObjectIntParameter(id,armJoints(i),2001,0,vrep.simx_opmode_oneshot_wait);
        end
    end
    % get init joint states
    for i=1:2
        [res,x0(i)] = vrep.simxGetJointPosition(id,armJoints(i),vrep.simx_opmode_buffer);vrchk(vrep, res, true);
        [res,x0(i+2)] = vrep.simxGetObjectFloatParameter(id,armJoints(i),2012,vrep.simx_opmode_buffer);vrchk(vrep, res, true);
        [res,tau(i)] = vrep.simxGetJointForce(id,armJoints(i),vrep.simx_opmode_buffer);vrchk(vrep, res, true);
    end
    % get external force in force sensor frame
    [res,state,fexternal,tauexternal] = vrep.simxReadForceSensor(id,forcesensor,vrep.simx_opmode_buffer);vrchk(vrep, res, true);
    % transform external force into the robot base frame(here the robot base frame coincides with the world frame in vrep)
    [res,eulerAngles]=vrep.simxGetObjectOrientation(id,forcesensor,-1,vrep.simx_opmode_buffer);vrchk(vrep, res, true);
    forcesensorrotation = rotx(double(eulerAngles(1)*180/pi))*roty(double(eulerAngles(2)*180/pi))*rotz(double(eulerAngles(3)*180/pi));
    fexternal_inertial = forcesensorrotation*[fexternal(1);fexternal(2);fexternal(3)];
    raw_externalforces(1,:) = fexternal_inertial(1)*ones(1,windowSize);
    raw_externalforces(2,:) = fexternal_inertial(3)*ones(1,windowSize);
    % record data
    recordData.x = [];
    recordData.tau = [];
    recordData.fmea = [];
    recordData.u = [];
    recordData.t = [];
    % max simulation duration
    maxsimtime = 20;
    % current time
    current_time = 0;
    % reference joint trajectory
    qref = x0(1:2);
    qdotref = [0;0];
    qdotdotref = [0;0];
    % reference ee trajectory
    eeposref = Direct_Kinematics(x0(1:2),[l1;l2]);
    eevref = [0;0];
    eearef = [0;0];
    % for impedance model
    xe_admittance = [0;0];
    xedot_admittance = [0;0];
    xedotdot_admittance = [0;0];
    q_admittance = [0;0];
    qdot_admittance = [0;0];
    qdotdot_admittance = [0;0];

    while vrep.simxGetConnectionId(id)~=-1
        % get robot joint states
        for i=1:2
            [res,x0(i)] = vrep.simxGetJointPosition(id,armJoints(i),vrep.simx_opmode_buffer);vrchk(vrep, res, true);
            [res,x0(i+2)] = vrep.simxGetObjectFloatParameter(id,armJoints(i),2012,vrep.simx_opmode_buffer);vrchk(vrep, res, true);
            [res,tau(i)] = vrep.simxGetJointForce(id,armJoints(i),vrep.simx_opmode_buffer);vrchk(vrep, res, true);
        end
        % get external force
        [res,state,fexternal,tauexternal] = vrep.simxReadForceSensor(id,forcesensor,vrep.simx_opmode_buffer);vrchk(vrep, res, true);

        % transfor external force form force sensor frame to robot base frame
        [res,eulerAngles]=vrep.simxGetObjectOrientation(id,forcesensor,-1,vrep.simx_opmode_buffer);vrchk(vrep, res, true);
        forcesensorrotation = rotx(double(eulerAngles(1)*180/pi))*roty(double(eulerAngles(2)*180/pi))*rotz(double(eulerAngles(3)*180/pi));
        fexternal_inertial = forcesensorrotation*[fexternal(1);fexternal(2);fexternal(3)];

        % shift raw external forces array
        for i=1:windowSize-1
            raw_externalforces(:,i) = raw_externalforces(:,i+1);
        end
        raw_externalforces(1,windowSize) = fexternal_inertial(1);
        raw_externalforces(2,windowSize) = fexternal_inertial(3);
        % force filtering
        filtered_externalforces_x = filter(b,a,raw_externalforces(1,:));
        filtered_externalforces_y = filter(b,a,raw_externalforces(2,:));
        filtered_externalforce = [filtered_externalforces_x(windowSize);filtered_externalforces_y(windowSize)];

        % feedbacked joint force in v-rep is the force sufferred by joint,
        % so change the sign to get the joint drive torque
        tau = -tau;

        if (current_time > maxsimtime)
            break;
        end

        % get reference trajectory point
        [eeposref,eevref,eearef] = ReferenceTrajectory(current_time,trajectory_mode);

       %% impedance control
        if control_mode == 0
            J =Geometric_Jacobian(x0(1:2),[l1;l2]);
            dJ_dt = Geometric_Jacobian_Derivative(x0,[l1;l2]);
            qdotdotref = J\(eearef - dJ_dt*x0(3:4));
            Mass = Mass_Matrix(x0(1:2),[I1 I2 l1 l2 m1 m2 a1 a2 g]');
            u = Impedance_Controller(x0,[l1;l2],[kpx;kpy;kdx;kdy],eeposref,eevref)...
                + Mass*qdotdotref...
                + Coriolis_Centrifugal_Torque(x0,[l1;l2;m1;m2;a1;a2;g])...
                + Gravity_Torque(x0(1:2),[l1;l2;m1;m2;a1;a2;g]);
            % limit joint tourque
            for i=1:2
                if u(i) > 25
                    u(i) = 25;
                elseif u(i) < -25
                    u(i) = -25;
                end
            end
            % send torque command to vrep
            for i=1:2
                if u(i) < 0
                    res = vrep.simxSetJointTargetVelocity(id,armJoints(i),-99999,vrep.simx_opmode_oneshot);vrchk(vrep, res, true);
                    res = vrep.simxSetJointForce(id,armJoints(i),-u(i),vrep.simx_opmode_oneshot);vrchk(vrep, res, true);
                else
                    res = vrep.simxSetJointTargetVelocity(id,armJoints(i),99999,vrep.simx_opmode_oneshot);vrchk(vrep, res, true);
                    res = vrep.simxSetJointForce(id,armJoints(i),u(i),vrep.simx_opmode_oneshot);vrchk(vrep, res, true);
                end
            end
       %% admittance control
        elseif control_mode == 1
            % --- velocity command --- (qdot_d + qdot_admittance)
            % compute qdotdot corresponds to impedance model
            qdotdot_admittance =  Admittance_Controller(x0,filtered_externalforce,[I1 I2 l1 l2 m1 m2 a1 a2 g]',[mx;my;kpx;kpy;kdx;kdy],eeposref,eevref);
            J =Geometric_Jacobian(x0(1:2),[l1;l2]);
            dJ_dt = Geometric_Jacobian_Derivative(x0,[l1;l2]);
            qdotdot_tracking =  J\(eearef - dJ_dt*x0(3:4));
            qdot_admittance = qdot_admittance + (qdotdot_admittance)*dt;
            qdotref  = J\eevref + qdot_admittance;
            u = qdotref;
            % send velocity command to vrep
            for i=1:2
                vrep.simxSetJointTargetVelocity(id,armJoints(i),qdotref(i),vrep.simx_opmode_oneshot);vrchk(vrep, res, true);
            end
        elseif control_mode == 2
            % --- position command ---  qd + q_admittance
            %compute qdotdot corresponds to impedance model
            qdotdot_admittance =  Admittance_Controller(x0,filtered_externalforce,[I1 I2 l1 l2 m1 m2 a1 a2 g]',[mx;my;kpx;kpy;kdx;kdy],eeposref-eevref*dt,eevref-eearef*dt);%-eevref*dt -eearef*dt
            J =Geometric_Jacobian(x0(1:2),[l1;l2]);
            dJ_dt = Geometric_Jacobian_Derivative(x0,[l1;l2]);
            qdotdot_tracking =  J\(eearef - dJ_dt*x0(3:4));
            q_admittance = q_admittance + qdot_admittance*dt +  1/2*qdotdot_admittance*dt^2;
            qdot_admittance = qdot_admittance + qdotdot_admittance*dt;
            qref = Inverse_Kinematics(x0(1:2),eeposref,[l1 l2]) + q_admittance;
            u = qref;
            % send position command to vrep
            for i=1:2
                vrep.simxSetJointTargetPosition(id,armJoints(i),u(i),vrep.simx_opmode_oneshot);vrchk(vrep, res, true);
            end
        end

        %% record data
        recordData.x = [recordData.x,x0];
        recordData.tau = [recordData.tau,tau];
        recordData.fmea = [recordData.fmea,raw_externalforces(:,end)];
        recordData.u = [recordData.u,u];
        recordData.t = [recordData.t,current_time];

        % update current_time
        current_time = current_time + dt;
        vrep.simxSynchronousTrigger(id);
    end
    % display result
    titles = {'q1','q2','dq1','dq2'};
    figure('name','state');
    for i=1:4
        subplot(2,2,i);
        plot(recordData.t,recordData.x(i,:));
        title(titles{i});
        xlabel('t/s');
    end

    titles = {'u1','u2'};
    figure('name','input');
    for i=1:2
        subplot(1,2,i);
        plot(recordData.t,recordData.u(i,:));
        title(titles{i});
        xlabel('t/s');
    end

    figure('name','external force');
    subplot(1,2,1);
    plot(recordData.t,recordData.fmea(1,:));
    title('fextx');
    xlabel('t/s');
    hold off

    subplot(1,2,2);
    plot(recordData.t,recordData.fmea(2,:));
    title('fexty');
    xlabel('t/s');
    hold off
else
    disp('Failed connecting to remote API server1');
end
vrep.simxStopSimulation(id,vrep.simx_opmode_oneshot_wait);
vrep.simxFinish(id);
vrep.delete();


%  myKnownRegimesFilter - filtering procedure for state-space models with
%  known switching dates
%
%  ::
%
%
%    [LogLik,Incr,retcode,Filters]=myKnownRegimesFilter(syst,y,U,z,options,regs)
%
%  Args:
%
%     - **syst** [struct]: structure provided by dsge.filter
%
%     - **y** [matrix]: matrix of data provided by dsge.filter
%
%     - **U** [matrix]: matrix of trends provided by dsge.filter
%
%     - **z** [matrix]: matrix of deterministic terms provided by dsge.filter
%
%     - **options** [struct]: options provided by dsge.filter
%
%     - **regs** [vector]: history of regimes, must be of the same length as
%        **y**
%
%  Returns:
%     :
%
%     - **LogLik** [numeric]: value of the log likelihood
%
%     - **Incr** [vector]: contributions to the likelihood in each period
%
%     - **retcode** [numeric]: flag equal to 0 if there is no problem
%
%     - **Filters** [struct]: structure containing all the filtering
%       information
%
%  Note:
%
%     - If the filter is run on a constant parameter model, then the last
%       input argument need not be specified.
%
%     - If the function is passed through a rise/dsge object, then it should
%       be called as
%       ff=filter(m,'kf_user_algo',{@myKnownRegimesFilter,regs}). That is,
%       RISE will provide all input arguments expect the last one that needs
%       to be provided by the user.
%
%     - It is the responsibility of the user to make sure that the solution
%       of the model is consistent with agents expecting no transition from
%       one regime to another. This occurs when the original model is
%       backward-looking and/or the transition matrix is diagonal.
%
%     See also: myConstantParamFilter
%


function [x,fval,exitflag,output,grad]=fminlbfgs(funfcn,x_init,optim)
%FMINLBFGS finds a local minimum of a function of several variables.
%   This optimizer is developed for image registration methods with large
%	amounts of unknown variables.
%
%   Optimization methods supported:
%	- Quasi Newton Broyden-Fletcher-Goldfarb-Shanno (BFGS)
%   - Limited memory BFGS (L-BFGS)
%   - Steepest Gradient Descent optimization.
%
%   [X,FVAL,EXITFLAG,OUTPUT,GRAD] = FMINLBFGS(FUN,X0,OPTIONS)
%
%   Inputs,
%		FUN: Function handle or string which is minimized, returning an
%				error value and optional the error gradient.
%		X0: Initial values of unknowns can be a scalar, vector or matrix
%	 (optional)
%		OPTIONS: Structure with optimizer options, made by a struct or
%				optimset. (optimset doesnot support all input options)
%
%   Outputs,
%		X : The found location (values) which minimize the function.
%		FVAL : The minimum found
%		EXITFLAG : Gives value, which explain why the minimizer stopt
%		OUTPUT : Structure with all important ouput values and parameters
%		GRAD : The gradient at this location
%
%   Extended description of input/ouput variables
%   OPTIONS,
%		OPTIONS.GoalsExactAchieve : If set to 0, a line search method is
%               used which uses a few function calls to do a good line
%               search. When set to 1 a normal line search method with Wolfe
%				conditions is used (default).
%		OPTIONS.GradConstr, Set this variable to true if gradient calls are
%				cpu-expensive (default). If false more gradient calls are
%				used and less function calls.
%	    OPTIONS.HessUpdate : If set to 'bfgs', Broyden�Fletcher�Goldfarb�Shanno
%				optimization is used (default), when the number of unknowns is
%				larger then 3000 the function will switch to Limited memory BFGS,
%				or if you set it to 'lbfgs'. When set to 'steepdesc', steepest
%				decent optimization is used.
%		OPTIONS.StoreN : Number of itterations used to approximate the Hessian,
%			 	in L-BFGS, 20 is default. A lower value may work better with
%				non smooth functions, because than the Hessian is only valid for
%				a specific position. A higher value is recommend with quadratic equations.
%		OPTIONS.GradObj : Set to 'on' if gradient available otherwise finited difference
%				is used.
%     	OPTIONS.Display : Level of display. 'off' displays no output; 'plot' displays
%				all linesearch results in figures. 'iter' displays output at  each
%               iteration; 'final' displays just the final output; 'notify'
%				displays output only if the function does not converge;
%	    OPTIONS.TolX : Termination tolerance on x, default 1e-6.
%	    OPTIONS.TolFun : Termination tolerance on the function value, default 1e-6.
%		OPTIONS.MaxIter : Maximum number of iterations allowed, default 400.
% 		OPTIONS.MaxFunEvals : Maximum number of function evaluations allowed,
%				default 100 times the amount of unknowns.
%		OPTIONS.DiffMaxChange : Maximum stepsize used for finite difference gradients.
%		OPTIONS.DiffMinChange : Minimum stepsize used for finite difference gradients.
%		OPTIONS.OutputFcn : User-defined function that an optimization function calls
%				at each iteration.
%		OPTIONS.rho : Wolfe condition on gradient (c1 on wikipedia), default 0.01.
%		OPTIONS.sigma : Wolfe condition on gradient (c2 on wikipedia), default 0.9.
%		OPTIONS.tau1 : Bracket expansion if stepsize becomes larger, default 3.
%		OPTIONS.tau2 : Left bracket reduction used in section phase,
%		default 0.1.
%		OPTIONS.tau3 : Right bracket reduction used in section phase, default 0.5.
%   FUN,
%		The speed of this optimizer can be improved by also providing
%   	the gradient at X. Write the FUN function as follows
%   	function [f,g]=FUN(X)
%       	f , value calculation at X;
%   	if ( nargout > 1 )
%       	g , gradient calculation at X;
%   	end
%	EXITFLAG,
%		Possible values of EXITFLAG, and the corresponding exit conditions
%		are
%  		1, 'Change in the objective function value was less than the specified tolerance TolFun.';
%  		2, 'Change in x was smaller than the specified tolerance TolX.';
%  		3, 'Magnitude of gradient smaller than the specified tolerance';
%  		4, 'Boundary fminimum reached.';
%  		0, 'Number of iterations exceeded options.MaxIter or number of function evaluations exceeded options.FunEvals.';
%  		-1, 'Algorithm was terminated by the output function.';
%  		-2, 'Line search cannot find an acceptable point along the current search';
%
%   Examples
%       options = optimset('GradObj','on');
%       X = fminlbfgs(@myfun,2,options)
%
%   	% where myfun is a MATLAB function such as:
%       function [f,g] = myfun(x)
%       f = sin(x) + 3;
%	    if ( nargout > 1 ), g = cos(x); end
%
%   See also OPTIMSET, FMINSEARCH, FMINBND, FMINCON, FMINUNC, @, INLINE.
%
%   Function is written by D.Kroon University of Twente (March 2009)

% Contributed to SOM Toolbox vs2, December 3rd, 2012 by Alexander Schulz
% See license file BSD_license.txt for license information.
% http://www.cis.hut.fi/projects/somtoolbox/

% Read Optimalisation Parameters
defaultopt = struct('Display','final','HessUpdate','bfgs','GoalsExactAchieve',1,'GradConstr',true,  ...
			'TolX',1e-6,'TolFun',1e-6,'GradObj','off','MaxIter',400,'MaxFunEvals',100*numel(x_init)-1,  ...
			'DiffMaxChange',1e-1,'DiffMinChange',1e-8,'OutputFcn',[], ...
			'rho',0.0100,'sigma',0.900,'tau1',3,'tau2', 0.1, 'tau3', 0.5,'StoreN',20);

if (~exist('optim','var'))
    optim=defaultopt;
else
    f = fieldnames(defaultopt);
    for i=1:length(f),
        if (~isfield(optim,f{i})||(isempty(optim.(f{i})))), optim.(f{i})=defaultopt.(f{i}); end
    end
end

% Initialize the data structure
data.fval=0;
data.gradient=0;
data.fOld=[];
data.xsizes=size(x_init);
data.numberOfVariables = numel(x_init);
data.xInitial = x_init(:);
data.alpha=1;
data.xOld=data.xInitial;
data.iteration=0;
data.funcCount=0;
data.gradCount=0;
data.exitflag=[];
data.nStored=0;
%%% data.timeTotal=tic;
data.timeExtern=0;
% Switch to L-BFGS in case of more than 3000 unknown variables
if(false && optim.HessUpdate(1)=='b')
    if(data.numberOfVariables<3000),
        optim.HessUpdate='bfgs';
    else
        optim.HessUpdate='lbfgs';
    end
end

if(optim.HessUpdate(1)=='l')
    data.deltaX=zeros(data.numberOfVariables,optim.StoreN);
    data.deltaG=zeros(data.numberOfVariables,optim.StoreN);
    data.saveD=zeros(data.numberOfVariables,optim.StoreN);
end

exitflag=[];

% Display column headers
if(strcmp(optim.Display,'iter'))
    disp('     Iteration  Func-count   Grad-count         f(x)         Step-size');
end

% Calculate the initial error and gradient
data.initialStepLength=1;
[data,fval,grad]=gradient_function(data.xInitial,funfcn, data, optim);
data.gradient=grad;
data.dir = -data.gradient;
data.gOld=grad;
data.fInitial = fval;
data.fPrimeInitial= data.gradient'*data.dir(:);

gNorm = norm(data.gradient,Inf);  % Norm of gradient
data.initialStepLength = min(1/gNorm,5);

% Show the current iteration
if(strcmp(optim.Display,'iter'))
        s=sprintf('     %5.0f       %5.0f       %5.0f       %13.6g    ',data.iteration,data.funcCount,data.gradCount,data.fInitial); disp(s);
end

% Hessian intialization
if(optim.HessUpdate(1)=='b')
	data.Hessian=eye(data.numberOfVariables);
end

% Call output function
if(call_output_function(data,optim,'init')), exitflag=-1; end

% Start Minimizing
while(true)
    % Update number of itterations
    data.iteration=data.iteration+1;

    % Set current lineSearch parameters
    data.TolFunLnS = eps(max(1,abs(data.fInitial )));
    data.fminimum = data.fInitial - 1e16*(1+abs(data.fInitial));

	% Make arrays to store linesearch results
    data.storefx=[]; data.storepx=[]; data.storex=[]; data.storegx=[];

    % If option display plot, than start new figure
    if(optim.Display(1)=='p'), figure, hold on; end

    % Find a good step size in the direction of the gradient: Linesearch
    if(optim.GoalsExactAchieve==1)
		data=linesearch(funfcn, data,optim);
    else
        data=linesearch_simple(funfcn, data, optim);
    end

	% Make linesearch plot
	if(optim.Display(1)=='p');
		plot(data.storex,data.storefx,'r*');
		plot(data.storex,data.storefx,'b');

		alpha_test= linspace(min(data.storex(:))/3, max(data.storex(:))*1.3, 10);
		falpha_test=zeros(1,length(alpha_test));
        for i=1:length(alpha_test)
			[data,falpha_test(i)]=gradient_function(data.xInitial(:)+alpha_test(i)*data.dir(:),funfcn, data, optim);
        end
		plot(alpha_test,falpha_test,'g');
        plot(data.alpha,data.f_alpha,'go','MarkerSize',8);
	end

    % Check if exitflag is set
    if(~isempty(data.exitflag)),
        exitflag=data.exitflag;
        data.xInitial=data.xOld;
        data.fInitial=data.fOld;
        data.gradient=data.gOld;
        break,
    end;

    % Update x with the alpha step
    data.xInitial = data.xInitial + data.alpha*data.dir;

    % Set the current error and gradient
    data.fInitial =  data.f_alpha;
	data.gradient = data.grad;

    % Set initial steplength to 1
    data.initialStepLength = 1;

    gNorm = norm(data.gradient,Inf);  % Norm of gradient

    % Set exit flags
    if(gNorm <optim.TolFun), exitflag=1; end
    if(max(abs(data.xOld-data.xInitial)) <optim.TolX), exitflag=2; end
    if(data.iteration>=optim.MaxIter), exitflag=0; end

    % Check if exitflag is set
    if(~isempty(exitflag)), break, end;

    % Update the inverse Hessian matrix
    if(optim.HessUpdate(1)~='s')
        % Do the Quasi-Neton Hessian update.
        data = updateQuasiNewtonMatrix_LBFGS(data,optim);
    else
        data.dir = -data.gradient;
    end

    % Derivative of direction
    data.fPrimeInitial= data.gradient'*data.dir(:);

    % Call output function
    if(call_output_function(data,optim,'iter')), exitflag=-1; end

    % Show the current iteration
    if(strcmp(optim.Display(1),'i')||strcmp(optim.Display(1),'p'))
        s=sprintf('     %5.0f       %5.0f       %5.0f       %13.6g   %13.6g',data.iteration,data.funcCount,data.gradCount,data.fInitial,data.alpha); disp(s);
    end

    % Keep the variables for next iteration
    data.fOld=data.fInitial;
    data.xOld=data.xInitial;
    data.gOld=data.gradient;
end
% Set output parameters
fval=data.fInitial;
grad=data.gradient;
x = data.xInitial;

% Reshape x to original shape
x=reshape(x,data.xsizes);

% Call output function
if(call_output_function(data,optim,'done')), exitflag=-1; end

% Make exist output structure
if(optim.HessUpdate(1)=='b'), output.algorithm='Broyden-Fletcher-Goldfarb-Shanno (BFGS)';
elseif(optim.HessUpdate(1)=='l'), output.algorithm='limited memory BFGS (L-BFGS)';
else output.algorithm='Steepest Gradient Descent';
end
output.message=getexitmessage(exitflag);
output.iteration = data.iteration;
output.funccount = data.funcCount;
output.fval = data.fInitial;
output.stepsize = data.alpha;
output.directionalderivative = data.fPrimeInitial;
output.gradient = reshape(data.gradient, data.xsizes);
output.searchdirection = data.dir;
%%% output.timeTotal=toc;
output.timeExtern=data.timeExtern;
%%%oupput.timeIntern=output.timeTotal-output.timeExtern;
% Display final results
if(~strcmp(optim.Display,'off'))
    disp('    Optimizer Results')
    disp(['        Algorithm Used: ' output.algorithm]);
    disp(['        Exit message : ' output.message]);
    disp(['        iterations : '  int2str(data.iteration)]);
    disp(['        Function Count : ' int2str(data.funcCount)]);
    disp(['        Minimum found : ' num2str(fval)]);
%%%    disp(['        Intern Time : ' num2str(oupput.timeIntern) ' seconds']);
%%%    disp(['        Total Time : ' num2str(output.timeTotal) ' seconds']);
end

function message=getexitmessage(exitflag)
    switch(exitflag)
        case 1, message='Change in the objective function value was less than the specified tolerance TolFun.';
        case 2, message='Change in x was smaller than the specified tolerance TolX.';
        case 3, message='Magnitude of gradient smaller than the specified tolerance';
        case 4, message='Boundary fminimum reached.';
        case 0, message='Number of iterations exceeded options.MaxIter or number of function evaluations exceeded options.FunEvals.';
        case -1, message='Algorithm was terminated by the output function.';
        case -2, message='Line search cannot find an acceptable point along the current search';
        otherwise, message='Undefined exit code';
    end

function stopt=call_output_function(data,optim,where)
stopt=false;
if(~isempty(optim.OutputFcn))
    output.iteration = data.iteration;
    output.funccount = data.funcCount;
    output.fval = data.fInitial;
    output.stepsize = data.alpha;
    output.directionalderivative = data.fPrimeInitial;
    output.gradient = reshape(data.gradient, data.xsizes);
    output.searchdirection = data.dir;
    stopt=feval(optim.OutputFcn,reshape(data.xInitial,data.xsizes),output,where);
end

function data=linesearch_simple(funfcn, data, optim)
% Find a bracket of acceptable points
data = bracketingPhase_simple(funfcn, data, optim);

if (data.bracket_exitflag  == 2)
  % BracketingPhase found a bracket containing acceptable points;
  % now find acceptable point within bracket
  data = sectioningPhase_simple(funfcn, data, optim);
  data.exitflag = data.section_exitflag;
else
  % Already acceptable point found or MaxFunEvals reached
  data.exitflag = data.bracket_exitflag;
end

function data = bracketingPhase_simple(funfcn, data,optim)
% Number of itterations
itw=0;

% Point with smaller value, initial
data.beta=0;
data.f_beta=data.fInitial;
data.fPrime_beta=data.fPrimeInitial;

% Initial step is equal to alpha of previous step.
alpha = data.initialStepLength;

% Going up hill
hill=false;

% Search for brackets
while(true)
    % Calculate the error registration gradient
    if(optim.GradConstr)
        [data,f_alpha]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
        fPrime_alpha=nan;
        grad=nan;
    else
        [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
        fPrime_alpha = grad'*data.dir(:);
    end

	% Store values linesearch
	data.storefx=[data.storefx f_alpha];
    data.storepx=[data.storepx fPrime_alpha];
	data.storex=[data.storex alpha];
	data.storegx=[data.storegx grad(:)];

    % Update step value
    if(data.f_beta<f_alpha),
        % Go to smaller stepsize
        alpha=alpha*optim.tau3;

        % Set hill variable
        hill=true;
    else
        % Save current minium point
        data.beta=alpha; data.f_beta=f_alpha; data.fPrime_beta=fPrime_alpha; data.grad=grad;
        if(~hill)
            alpha=alpha*optim.tau1;
        end
    end

    % Update number of loop iterations
    itw=itw+1;

    if(itw>(log(optim.TolFun)/log(optim.tau3))),
      % No new optium found, linesearch failed.
      data.bracket_exitflag=-2; break;
    end

    if(data.beta>0&&hill)
            % Get the brackets around minimum point
            % Pick bracket A from stored trials
            [t,i]=sort(data.storex,'ascend');
            storefx=data.storefx(i);storepx=data.storepx(i); storex=data.storex(i);
            [t,i]=find(storex>data.beta,1);
            if(isempty(i)), [t,i]=find(storex==data.beta,1); end
            alpha=storex(i); f_alpha=storefx(i); fPrime_alpha=storepx(i);

            % Pick bracket B from stored trials
            [t,i]=sort(data.storex,'descend');
            storefx=data.storefx(i);storepx=data.storepx(i); storex=data.storex(i);
            [t,i]=find(storex<data.beta,1);
            if(isempty(i)), [t,i]=find(storex==data.beta,1); end
            beta=storex(i); f_beta=storefx(i); fPrime_beta=storepx(i);

            % Calculate derivatives if not already calculated
            if(optim.GradConstr)
                gstep=data.initialStepLength/1e6;
                if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
                if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
                [data,f_alpha2]=gradient_function(data.xInitial(:)+(alpha+gstep)*data.dir(:),funfcn, data, optim);
                [data,f_beta2]=gradient_function(data.xInitial(:)+(beta+gstep)*data.dir(:),funfcn, data, optim);
                fPrime_alpha=(f_alpha2-f_alpha)/gstep;
                fPrime_beta=(f_beta2-f_beta)/gstep;
            end

            % Set the brackets A and B
            data.a=alpha; data.f_a=f_alpha; data.fPrime_a=fPrime_alpha;
            data.b=beta; data.f_b=f_beta; data.fPrime_b=fPrime_beta;

            % Finished bracketing phase
            data.bracket_exitflag  = 2; return
    end

	% Reached max function evaluations
	if(data.funcCount>=optim.MaxFunEvals), data.bracket_exitflag=0; return; end
end

function data = sectioningPhase_simple(funfcn, data, optim)
% Get the brackets
brcktEndpntA=data.a; brcktEndpntB=data.b;

% Calculate minimum between brackets
[alpha,f_alpha_estimated] = pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,data.a,data.b,data.f_a,data.fPrime_a,data.f_b,data.fPrime_b,optim);
if(isfield(data,'beta')&&(data.f_beta<f_alpha_estimated)), alpha=data.beta; end

[t,i]=find(data.storex==alpha,1);
if((~isempty(i))&&(~isnan(data.storegx(i))))
    f_alpha=data.storefx(i); grad=data.storegx(:,i);
else
    % Calculate the error and gradient for the next minimizer itteration
    [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
    if(isfield(data,'beta')&&(data.f_beta<f_alpha)),
        alpha=data.beta;
        if((~isempty(i))&&(~isnan(data.storegx(i))))
            f_alpha=data.storefx(i); grad=data.storegx(:,i);
        else
            [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
        end
    end
end

% Store values linesearch
data.storefx=[data.storefx f_alpha]; data.storex=[data.storex alpha];

fPrime_alpha = grad'*data.dir(:);
data.alpha=alpha;
data.fPrime_alpha= fPrime_alpha;
data.f_alpha= f_alpha;
data.grad=grad;

% Set the exit flag to succes
data.section_exitflag=[];

function data=linesearch(funfcn, data, optim)

% Find a bracket of acceptable points
data = bracketingPhase(funfcn, data,optim);

if (data.bracket_exitflag  == 2)
  % BracketingPhase found a bracket containing acceptable points;
  % now find acceptable point within bracket
  data = sectioningPhase(funfcn, data, optim);
  data.exitflag = data.section_exitflag;
else
  % Already acceptable point found or MaxFunEvals reached
  data.exitflag = data.bracket_exitflag;
end

function data = sectioningPhase(funfcn, data, optim)
%
% sectioningPhase finds an acceptable point alpha within a given bracket [a,b]
% containing acceptable points. Notice that funcCount counts the total number of
% function evaluations including those of the bracketing phase.

while(true)

    % Pick alpha in reduced bracket
    brcktEndpntA = data.a + min(optim.tau2,optim.sigma)*(data.b - data.a);
    brcktEndpntB = data.b - optim.tau3*(data.b - data.a);

    % Find global minimizer in bracket [brcktEndpntA,brcktEndpntB] of 3rd-degree
    % polynomial that interpolates f() and f'() at "a" and at "b".
    alpha = pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,data.a,data.b,data.f_a,data.fPrime_a,data.f_b,data.fPrime_b,optim);

    % No acceptable point could be found
    if (abs( (alpha - data.a)*data.fPrime_a ) <= data.TolFunLnS), data.section_exitflag = -2; return; end

    % Calculate value (and gradient if no extra time cost) of current alpha
    if(~optim.GradConstr)
        [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
        fPrime_alpha = grad'*data.dir(:);
    else
        gstep=data.initialStepLength/1e6;
        if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
        if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
        [data,f_alpha]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data,optim);
        [data,f_alpha2]=gradient_function(data.xInitial(:)+(alpha+gstep)*data.dir(:),funfcn, data, optim);
        fPrime_alpha=(f_alpha2-f_alpha)/gstep;
    end

	% Store values linesearch
	data.storefx=[data.storefx f_alpha]; data.storex=[data.storex alpha];

    % Store current bracket position of A
    aPrev = data.a;
    f_aPrev = data.f_a;
    fPrime_aPrev = data.fPrime_a;

    % Update the current brackets
    if ((f_alpha > data.fInitial + alpha*optim.rho*data.fPrimeInitial) || (f_alpha >= data.f_a))
        % Update bracket B to current alpha
        data.b = alpha; data.f_b = f_alpha; data.fPrime_b = fPrime_alpha;
    else
        % Wolfe conditions, if true then acceptable point found
        if (abs(fPrime_alpha) <= -optim.sigma*data.fPrimeInitial),
            if(optim.GradConstr)
                % Gradient was not yet calculated because of time costs
                [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
                fPrime_alpha = grad'*data.dir(:);
            end
            % Store the found alpha values
            data.alpha=alpha; data.fPrime_alpha= fPrime_alpha; data.f_alpha= f_alpha;
            data.grad=grad;
            data.section_exitflag = []; return,
        end

        % Update bracket A
        data.a = alpha; data.f_a = f_alpha;  data.fPrime_a = fPrime_alpha;

        if (data.b - data.a)*fPrime_alpha >= 0
            % B becomes old bracket A;
            data.b = aPrev; data.f_b = f_aPrev;  data.fPrime_b = fPrime_aPrev;
        end
    end

    % No acceptable point could be found
    if (abs(data.b-data.a) < eps), data.section_exitflag = -2; return, end

    % maxFunEvals reached
    if(data.funcCount >optim.MaxFunEvals), data.section_exitflag = -1; return, end
end

function data = bracketingPhase(funfcn, data, optim)
% bracketingPhase finds a bracket [a,b] that contains acceptable points; a bracket
% is the same as a closed interval, except that a > b is allowed.
%
% The outputs f_a and fPrime_a are the values of the function and the derivative
% evaluated at the bracket endpoint 'a'. Similar notation applies to the endpoint
% 'b'.

% Parameters of bracket A
data.a = [];
data.f_a = [];
data.fPrime_a = [];

% Parameters of bracket B
data.b = [];
data.f_b = [];
data.fPrime_b = [];

% First trial alpha is user-supplied
% f_alpha will contain f(alpha) for all trial points alpha
% fPrime_alpha will contain f'(alpha) for all trial points alpha
alpha = data.initialStepLength;
f_alpha = data.fInitial;
fPrime_alpha = data.fPrimeInitial;

% Set maximum value of alpha (determined by fminimum)
alphaMax = (data.fminimum - data.fInitial)/(optim.rho*data.fPrimeInitial);
alphaPrev = 0;

while(true)
  % Evaluate f(alpha) and f'(alpha)
  fPrev = f_alpha;
  fPrimePrev = fPrime_alpha;

  % Calculate value (and gradient if no extra time cost) of current alpha
  if(~optim.GradConstr)
      [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
      fPrime_alpha = grad'*data.dir(:);
  else
      gstep=data.initialStepLength/1e6;
      if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
      if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
      [data,f_alpha]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
      [data,f_alpha2]=gradient_function(data.xInitial(:)+(alpha+gstep)*data.dir(:),funfcn, data, optim);
      fPrime_alpha=(f_alpha2-f_alpha)/gstep;
  end

  % Store values linesearch
  data.storefx=[data.storefx f_alpha]; data.storex=[data.storex alpha];

  % Terminate if f < fminimum
  if (f_alpha <= data.fminimum), data.bracket_exitflag = 4; return; end

  % Bracket located - case 1 (Wolfe conditions)
  if (f_alpha > (data.fInitial + alpha*optim.rho*data.fPrimeInitial)) || (f_alpha >= fPrev)
    % Set the bracket values
    data.a = alphaPrev; data.f_a = fPrev;  data.fPrime_a = fPrimePrev;
    data.b = alpha; data.f_b = f_alpha;  data.fPrime_b = fPrime_alpha;
    % Finished bracketing phase
    data.bracket_exitflag  = 2; return
  end

  % Acceptable steplength found
  if (abs(fPrime_alpha) <= -optim.sigma*data.fPrimeInitial),
      if(optim.GradConstr)
          % Gradient was not yet calculated because of time costs
          [data,f_alpha, grad]=gradient_function(data.xInitial(:)+alpha*data.dir(:),funfcn, data, optim);
          fPrime_alpha = grad'*data.dir(:);
      end
      % Store the found alpha values
      data.alpha=alpha;
      data.fPrime_alpha= fPrime_alpha; data.f_alpha= f_alpha; data.grad=grad;
      % Finished bracketing phase, and no need to call sectioning phase
      data.bracket_exitflag = [];  return
  end

  % Bracket located - case 2
  if (fPrime_alpha >= 0)
    % Set the bracket values
    data.a = alpha; data.f_a = f_alpha;  data.fPrime_a = fPrime_alpha;
    data.b = alphaPrev; data.f_b = fPrev; data.fPrime_b = fPrimePrev;
    % Finished bracketing phase
    data.bracket_exitflag  = 2; return
  end

  % Update alpha
  if (2*alpha - alphaPrev < alphaMax )
      brcktEndpntA = 2*alpha-alphaPrev;
      brcktEndpntB = min(alphaMax,alpha+optim.tau1*(alpha-alphaPrev));
      % Find global minimizer in bracket [brcktEndpntA,brcktEndpntB] of 3rd-degree polynomial
      % that interpolates f() and f'() at alphaPrev and at alpha
      alphaNew = pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,alphaPrev,alpha,fPrev, ...
                                         fPrimePrev,f_alpha,fPrime_alpha,optim);
      alphaPrev = alpha;
      alpha = alphaNew;
  else
      alpha = alphaMax;
  end

  % maxFunEvals reached
  if(data.funcCount >optim.MaxFunEvals), data.bracket_exitflag = -1; return, end
end

function [alpha,f_alpha]= pickAlphaWithinInterval(brcktEndpntA,brcktEndpntB,alpha1,alpha2,f1,fPrime1,f2,fPrime2,optim)
% finds a global minimizer alpha within the bracket [brcktEndpntA,brcktEndpntB] of the cubic polynomial
% that interpolates f() and f'() at alpha1 and alpha2. Here f(alpha1) = f1, f'(alpha1) = fPrime1,
% f(alpha2) = f2, f'(alpha2) = fPrime2.

% determines the coefficients of the cubic polynomial with c(alpha1) = f1,
% c'(alpha1) = fPrime1, c(alpha2) = f2, c'(alpha2) = fPrime2.
coeff = [(fPrime1+fPrime2)*(alpha2-alpha1)-2*(f2-f1) ...
    3*(f2-f1)-(2*fPrime1+fPrime2)*(alpha2-alpha1) (alpha2-alpha1)*fPrime1 f1];

% Convert bounds to the z-space
lowerBound = (brcktEndpntA - alpha1)/(alpha2 - alpha1);
upperBound = (brcktEndpntB - alpha1)/(alpha2 - alpha1);

% Swap if lowerbound is higher than the upperbound
if (lowerBound  > upperBound), t=upperBound; upperBound=lowerBound; lowerBound=t; end

% Find minima and maxima from the roots of the derivative of the polynomial.
sPoints = roots([3*coeff(1) 2*coeff(2) coeff(3)]);

% Remove imaginaire and points outside range

sPoints(imag(sPoints)~=0)=[];
sPoints(sPoints<lowerBound)=[]; sPoints(sPoints>upperBound)=[];

% Make vector with all possible solutions
sPoints=[lowerBound sPoints(:)' upperBound];

% Select the global minimum point
[f_alpha,index]=min(polyval(coeff,sPoints)); z=sPoints(index);

% Add the offset and scale back from [0..1] to the alpha domain
alpha = alpha1 + z*(alpha2 - alpha1);

% Show polynomial search
if(optim.Display(1)=='p');
    vPoints=polyval(coeff,sPoints);
    plot(sPoints*(alpha2 - alpha1)+alpha1,vPoints,'co');
    plot([sPoints(1) sPoints(end)]*(alpha2 - alpha1)+alpha1,[vPoints(1) vPoints(end)],'c*');
    xPoints=linspace(lowerBound/3, upperBound*1.3, 50);
    vPoints=polyval(coeff,xPoints);
    plot(xPoints*(alpha2 - alpha1)+alpha1,vPoints,'c');
end

function [data,fval,grad]=gradient_function(x,funfcn, data, optim)
    % Call the error function for error (and gradient)
    if ( nargout <3 )
        %%% timem=tic;
        fval=funfcn(reshape(x,data.xsizes));
        %%% data.timeExtern=data.timeExtern+toc(timem);
        data.funcCount=data.funcCount+1;
    else
        if(strcmp(optim.GradObj,'on'))
            %%% timem=tic;
            [fval, grad]=feval(funfcn,reshape(x,data.xsizes));
            %%% data.timeExtern=data.timeExtern+toc(timem);
            data.funcCount=data.funcCount+1;
            data.gradCount=data.gradCount+1;
        else
            % Calculate gradient with forward difference if not provided by the function
            grad=zeros(length(x),1);
            fval=funfcn(reshape(x,data.xsizes));
            gstep=data.initialStepLength/1e6;
            if(gstep>optim.DiffMaxChange), gstep=optim.DiffMaxChange; end
            if(gstep<optim.DiffMinChange), gstep=optim.DiffMinChange; end
            for i=1:length(x),
                x_temp=x; x_temp(i)=x_temp(i)+gstep;
                %%% timem=tic;
                [fval_g]=feval(funfcn,reshape(x_temp,data.xsizes)); data.funcCount=data.funcCount+1;
                %%% data.timeExtern=data.timeExtern+toc(timem);
                grad(i)=(fval_g-fval)/gstep;
            end
        end
        grad=grad(:);
    end

function data = updateQuasiNewtonMatrix_LBFGS(data,optim)
% updates the quasi-Newton matrix that approximates the inverse to the Hessian.
% Two methods are support BFGS and L-BFGS, in L-BFGS the hessian is not
% constructed or stored.
% Calculate position, and gradient diference between the
% itterations
deltaX=data.alpha* data.dir;
deltaG=data.gradient-data.gOld;

if ((deltaX'*deltaG) >= sqrt(eps)*max( eps,norm(deltaX)*norm(deltaG) ))

    if(optim.HessUpdate(1)=='b')
        % Default BFGS as described by Nocedal
        p_k = 1 / (deltaG'*deltaX);
        Vk = eye(data.numberOfVariables) - p_k*deltaG*deltaX';
        % Set Hessian
        data.Hessian = Vk'*data.Hessian *Vk + p_k * deltaX*deltaX';
        % Set new Direction
        data.dir = -data.Hessian*data.gradient;
    else
        % L-BFGS with scaling as described by Nocedal

        % Update a list with the history of deltaX and deltaG
        data.deltaX(:,2:optim.StoreN)=data.deltaX(:,1:optim.StoreN-1); data.deltaX(:,1)=deltaX;
        data.deltaG(:,2:optim.StoreN)=data.deltaG(:,1:optim.StoreN-1); data.deltaG(:,1)=deltaG;

        data.nStored=data.nStored+1; if(data.nStored>optim.StoreN), data.nStored=optim.StoreN; end

        % Initialize variables
        a=zeros(1,data.nStored);
        p=zeros(1,data.nStored);

        q = data.gradient;
        for i=1:data.nStored
            p(i)= 1 / (data.deltaG(:,i)'*data.deltaX(:,i));
            a(i) = p(i)* data.deltaX(:,i)' * q;
            q = q - a(i) * data.deltaG(:,i);
        end
        % Scaling of initial Hessian (identity matrix)
        p_k = data.deltaG(:,1)'*data.deltaX(:,1) / sum(data.deltaG(:,1).^2);

        % Make r = - Hessian * gradient
        r = p_k * q;
        for i=data.nStored:-1:1,
            b = p(i) * data.deltaG(:,i)' * r;
            r = r + data.deltaX(:,i)*(a(i)-b);
        end

        % Set new direction
        data.dir = -r;
    end
end

function earlystoppedreturn = LVQ_progresser(variables, output, where)
%
% control actions during each iteration of the fminlbfgs optimization
%
% Conditions of GNU General Public License, version 2 and BSD License apply.
% See file 'license-gpl2.txt' and 'BSD_license.txt' enclosed in this package.
% Programs are not for use in critical applications!
% Kerstin Bunte based on the code from Marc Strickert
%
global earlystopped threshstop

if sum(where == 'iter') < 4
    earlystopped = false;
    earlystoppedreturn = earlystopped;
    return
end

earlystopped = exist ('stop','file') > 0;
earlystoppedreturn = earlystopped;

if earlystopped
    delete('stop');
    return
end

if isempty(threshstop)
    return
end

if threshstop > 0  % early stopping by simple thresholding: if too good: stop
    earlystopped = output.fval < threshstop;  % close to perfect
    earlystoppedreturn = earlystopped;
    if earlystopped
      return
    end
end

global useEarlyStopping

if isempty(useEarlyStopping)
    return
end

%%% early stopping

global training_data training_label prototypeLabel% n_vec bestvariables
nb_prototypes = numel(prototypeLabel);
model.w = variables(1:nb_prototypes,:);
model.c_w = prototypeLabel;
model.omega = variables(nb_prototypes+1:end,:);
estimatedLabels = GMLVQ_classify(training_data, model);
ref = mean( training_label ~= estimatedLabels );
% fprintf('error: %f',ref);
%   if isempty(n_vec) || n_vec < 1 % GRLVQ
%     lam = variables(1,:); % weights
%     protos = variables(2:end,:); % protos
%     [cls C] = applyprotoseuc(protos, protolabl, lam, datval, labval);
%   else % MRLVQ
%     lam = variables(1:n_vec,:).';
%     protos = variables((n_vec+1):end,:);
%     [cls C] = applyprotosmat(protos, protolabl, lam, datval, labval);
%   end
% ref = trace(C)/sum(C(:))-1; % negative classif error

persistent last penalty
% disp(last);
if isempty(last)
%     bestvariables = variables;
    penalty = -25;  % penalty counter, skip first 15 iterations (burn-in phase)
%     last = ref * ones(5,1);  % memory of 5 elements
    last = ref;
else
%     [srt idxsrt] = sort([last; ref],'descend');
%     fvalval = srt(1);
%     fpos = find(idxsrt == 6);  % index of added element
%     if fpos == 6  % last position?
      penalty = penalty + 1;
      if penalty > 10  % allow a maximum of 10 times to fail
        earlystopped = true;
        earlystoppedreturn = earlystopped;
        return
      end
%     else
% %       last = srt(1:5);
% %       if fpos == 1  % best position
% %         bestvariables = variables;
% %       end
%       if penalty < 0  % still in burn-in phase
%         penalty = penalty + 1;
%       end
%       if penalty > 0  % do not go below here
%         penalty = penalty - 1;
%       end
%     end
end
earlystoppedreturn = earlystopped;


%%*********************************************************************
%% gdcomp: Compute gd = 1/td in Equation (15) of the paper:
%%
%% R.M. Freund, F. Ordonez, and K.C. Toh,
%% Behavioral measures and their correlation with IPM iteration counts
%% on semi-definite programming problems,
%% Mathematical Programming, 109 (2007), pp. 445--475.
%%
%% [gd,info,yfeas,Zfeas,blk2,At2,C2,b2] = gdcomp(blk,At,C,b,OPTIONS,solveyes);
%%
%% yfeas,Zfeas: a dual feasible pair when gd is finite.
%%              That is, if
%%              Aty = Atyfun(blk,At,[],[],yfeas);
%%              Rd = ops(C,'-',ops(Zfeas,'+',Aty));
%%              then
%%              ops(Rd,'norm') should be small.
%%
%%*********************************************************************

  function [gd,info,yfeas,Zfeas,blk2,At2,C2,b2] = gdcomp(blk,At,C,b,OPTIONS,solveyes);

  if (nargin < 6); solveyes = 1; end
  if (nargin < 5)
     OPTIONS = sqlparameters;
     OPTIONS.vers   = 1;
     OPTIONS.gaptol = 1e-10;
     OPTIONS.printlevel = 3;
  end
  if isempty(OPTIONS); OPTIONS = sqlparameters; end
  if ~isfield(OPTIONS,'solver'); OPTIONS.solver = 'HSDsqlp'; end
  if ~isfield(OPTIONS,'printlevel'); OPTIONS.printlevel = 3; end
  if ~iscell(C); tmp = C; clear C; C{1} = tmp; end
%%
  m = length(b);
  blk2 = blk;
  At2 = cell(size(blk,1),1);
  C2 = cell(size(blk,1),1);
  EE = cell(size(blk,1),1);
%%
%%
%%
  dd = zeros(1,m);
  alp = 0;
  beta = 0;
  for p = 1:size(blk,1)
     pblk = blk(p,:);
     n = sum(pblk{2});
     if strcmp(pblk{1},'s')
        C2{p,1} = sparse(n,n);
     else
        C2{p,1} = zeros(n,1);
     end
     dd = dd + sqrt(sum(At{p}.*At{p}));
     beta = beta + norm(C{p},'fro');
     alp = alp + sqrt(n);
  end
  alp  = 1./max(1,alp);
  beta = 1./max(1,beta);
  dd   = 1./max(1,dd);
%%
%% New multipliers in dual problem:
%% [v; tt; theta].
%%
   D = spdiags(dd',0,m,m);

   ss = 0; cc = 0; aa = zeros(1,m);
   exist_ublk = 0;
   for p = 1:size(blk,1)
      pblk = blk(p,:);
      n = sum(pblk{2});
      if strcmp(pblk{1},'s')
         At2{p} = [At{p}*D, svec(pblk,alp*speye(n,n),1), -svec(pblk,beta*C{p},1)];
         ss = ss + n;
         cc = cc + trace(C{p});
         aa = aa + svec(pblk,speye(n),1)'*At{p};
         EE{p} = speye(n,n);
      elseif strcmp(pblk{1},'q')
         eq = zeros(n,1);
         idx1 = 1+[0,cumsum(pblk{2})];
         idx1 = idx1(1:length(idx1)-1);
         eq(idx1) = ones(length(idx1),1);
         At2{p} = [At{p}*D, 2*sparse(alp*eq), -sparse(beta*C{p})];
         ss = ss + 2*length(pblk{2});
         cc = cc + sum(C{p}(idx1));
         aa = aa + eq'*At{p};
         EE{p} = eq;
      elseif strcmp(pblk{1},'l')
         el = ones(n,1);
         At2{p} = [At{p}*D, sparse(alp*el), -sparse(beta*C{p})];
         ss = ss + n;
         cc = cc + el'*C{p};
         aa = aa + el'*At{p};
         EE{p} = el;
      elseif strcmp(pblk{1},'u')
         At2{p} = [At{p}*D, sparse(n,1), -sparse(beta*C{p})];
         exist_ublk = 1;
         EE{p} = sparse(n,1);
      end
   end
   aa = aa.*dd;
   cc = cc*beta;
%%
%% 4 additional inequality constraints in dual problem.
%%
   numblk = size(blk,1);
   blk2{numblk+1,1} = 'l'; blk2{numblk+1,2} = 4;
   C2{numblk+1,1}  = [1; 1; 0; 0];
   At2{numblk+1,1} = [-aa,         0,   cc;
		     zeros(1,m),   0,   beta;
		     zeros(1,m),  alp, -beta
                     zeros(1,m), -alp,  0];
   At2{numblk+1} = sparse(At2{numblk+1});
   b2 = [zeros(m,1); alp; 0];
%%
%% Solve SDP
%%
   gd = []; info = []; yfeas = []; Zfeas = [];
   if (solveyes)
      if strcmp(OPTIONS.solver,'sqlp')
         [X0,y0,Z0] = infeaspt(blk2,At2,C2,b2,2,100);
         [obj,X,y,Z,info] = sqlp(blk2,At2,C2,b2,OPTIONS,X0,y0,Z0);
      else
         [obj,X,y,Z,info] = HSDsqlp(blk2,At2,C2,b2,OPTIONS);
      end
      tt = alp*abs(y(m+1)); theta = beta*abs(y(m+2));
      yfeas = D*y(1:m)/theta;
      Zfeas = ops(ops(Z(1:numblk),'+',EE,tt),'/',theta);
      %%
      if (obj(2) > 0) | (abs(obj(2)) < 1e-8)
         gd = 1/abs(obj(2));
      elseif (obj(1) > 0)
         gd = 1/obj(1);
      else
         gd = 1/exp(mean(log(abs(obj))));
      end
      err = max(info.dimacs([1,3,6]));
      if (OPTIONS.printlevel)
         fprintf('\n ******** gd = %3.2e, err = %3.1e\n',gd,err);
         if (err > 1e-6);
            fprintf('\n----------------------------------------------------')
            fprintf('\n gd problem is not solved to sufficient accuracy');
            fprintf('\n----------------------------------------------------\n')
         end
      end
   end
%%*********************************************************************


function moebius_values_test ( )

%*****************************************************************************80
%
%% MOEBGIUS_VALUES_TEST demonstrates the use of MOEBIUS_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 November 2005
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MOEBIUS_VALUES_TEST:\n' );
  fprintf ( 1, '  MOEBIUS_VALUES returns values of \n' );
  fprintf ( 1, '  the Moebius function.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '     N         MU(N)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, n, fn ] = moebius_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %4d  %10d\n', n, fn );

  end

  return
end


clear all
close all

bnet = mk_asia_bnet();

N = length(bnet.dag);

base_proba = 0.3;
bnet_miss = gener_MCAR_net(bnet, base_proba);

m=500;
[data, comp_data, bnet_miss, taux, bnet_orig, notok] = gener_data_from_bnet_miss(bnet_miss, m, base_proba);

dag0 = zeros(N);
bnet0 = mk_bnet(dag0, bnet.node_sizes);
for node = 1:N
  bnet0.CPD{node} = tabular_CPD(bnet0, node, 'prior_type', 'dirichlet', 'dirichlet_weight', 0);
end

max_loop = 6;
[bnet0, cpdag, BIC_score, nloop] = learn_struct_ges_EM(bnet0, data, max_loop);

[XX, YY] = draw_graph(bnet.dag);
figure;
draw_graph(bnet0.dag, {'1','2','3','4','5','6','7','8'}, zeros(1,N), XX, YY);