classdef DOC8 < PROBLEM
% <multi> <real> <constrained>
% Benchmark MOP with constraints in both decision and objective spaces

%------------------------------- Reference --------------------------------
% Z. Liu and Y. Wang, Handling constrained multiobjective optimization
% problems with constraints in both the decision and objective spaces. IEEE
% Transactions on Evolutionary Computation, 2019, 23(5): 870-884.
%------------------------------- 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)
            obj.M = 3;
            obj.D = 10;
            obj.lower    = [0 0 500 1000 5000 100 100 100 100 100];
            obj.upper    = [1 1 1000 2000 6000 500 500 500 500 500];
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values and constraint violations
        function Population = Evaluation(obj,varargin)
            X = varargin{1};
            X = max(min(X,repmat(obj.upper,size(X,1),1)),repmat(obj.lower,size(X,1),1));
            g_temp = X(:, 3) + X(:, 4) + X(:, 5);
            g = g_temp-7049.2480205286 +1;
            PopObj(:,1) = (X(:,1).*X(:,2)).*g;
            PopObj(:,2) = (X(:,1).*(1 - X(:,2))).*g;
            PopObj(:,3) = (1-X(:,1)).*g;
            % Constraints in objective space
            c(:,1) = max( - (PopObj(:,3) - 0.4).*(PopObj(:,3) - 0.6), 0);
            % Constraints in decision space
            c(:,2) = -1 + 0.0025 * (X(:, 6) + X(:, 8));
            c(:,3) = -1 + 0.0025 * (X(:, 7) + X(:, 9) - X(:, 6));
            c(:,4) = -1 + 0.01 * (X(:, 10) - X(:, 7));
            c(:,5) = -X(:, 3).* X(:, 8) + 833.33252 * X(:, 6) + 100 * X(:, 3) - 83333.333;
            c(:,6) = -X(:, 4).* X(:, 9) + 1250 * X(:, 7) + X(:, 4).* X(:, 6) - 1250 * X(:, 6);
            c(:,7) = -X(:, 5).* X(:, 10) + 1250000 + X(:, 5).* X(:, 7) - 2500 * X(:, 7);
            Population  = SOLUTION(X,PopObj,c,varargin{2:end});
            obj.FE      = obj.FE + length(Population);
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
             R = UniformPoint(N,3);
             R(0.4<R(:,3)&R(:,3)<0.6,:) = [];
        end
        %% Generate the feasible region
        function R = GetPF(obj)
            a = linspace(0,1,20)';
            x = a*a';
            y = a*(1-a');
            z = (1-a)*ones(size(a'));
            z(0.4<z&z<0.6) = nan;
            R = {x,y,z};
        end
    end
end


function data2num_finviz(finviz_dir)
clearvars -except finviz_dir
addpath('functions');

% Finviz data is a cell matrix (n_tickers x n_vars)
finviz_filename=strcat(finviz_dir,'/finviz.csv');
[finviz_data,stop]=grab_all_finviz_data(finviz_filename);

FINHEAD = set_finviz_labels(); % global index for each column in finviz.csv
%FINHEAD_names = fieldnames(FINHEAD); % cell of field name strings

% Numbers file
% FHEAD is the local index we decide to output (FHEAD_names is its
% fieldnames)
ntickers=size(finviz_data,1);
if(1)

  data = [];
  i=1;

  MarketCap_vec = convert2numvec( finviz_data(:,FINHEAD.MarketCap) ); data = [data; MarketCap_vec']; FHEAD=struct('MarketCap',{i}); i=i+1;
  PE_vec = convert2numvec( finviz_data(:,FINHEAD.PE) ); data = [data; PE_vec']; [FHEAD(:).PE]=i; i=i+1;
  PEForward_vec = convert2numvec( finviz_data(:,FINHEAD.PEForward) ); data = [data; PEForward_vec']; [FHEAD(:).PEForward]=i; i=i+1;
  PEG_vec = convert2numvec( finviz_data(:,FINHEAD.PEG) ); data = [data; PEG_vec']; [FHEAD(:).PEG]=i; i=i+1;

  PS_vec = convert2numvec( finviz_data(:,FINHEAD.PS) ); data = [data; PS_vec']; [FHEAD(:).PS]=i; i=i+1;
  PB_vec = convert2numvec( finviz_data(:,FINHEAD.PB) ); data = [data; PB_vec']; [FHEAD(:).PB]=i; i=i+1;
  PCash_vec = convert2numvec( finviz_data(:,FINHEAD.PCash) ); data = [data; PCash_vec']; [FHEAD(:).PCash]=i; i=i+1;
  PFreeCash_vec = convert2numvec( finviz_data(:,FINHEAD.PFreeCash) ); data = [data; PFreeCash_vec']; [FHEAD(:).PFreeCash]=i; i=i+1;
  DividendYield_vec = convert2numvec( finviz_data(:,FINHEAD.DividendYield) ); data = [data; DividendYield_vec']; [FHEAD(:).DividendYield]=i; i=i+1;
  PayoutRatio_vec = convert2numvec( finviz_data(:,FINHEAD.PayoutRatio) ); data = [data; PayoutRatio_vec']; [FHEAD(:).PayoutRatio]=i; i=i+1;

  EPS_vec = convert2numvec( finviz_data(:,FINHEAD.EPS) ); data = [data; EPS_vec']; [FHEAD(:).EPS]=i; i=i+1;
  EPSGrowthThisYear_vec = convert2numvec( finviz_data(:,FINHEAD.EPSGrowthThisYear) ); data = [data; EPSGrowthThisYear_vec']; [FHEAD(:).EPSGrowthThisYear]=i; i=i+1;
  EPSGrowthNextYear_vec = convert2numvec( finviz_data(:,FINHEAD.EPSGrowthNextYear) ); data = [data; EPSGrowthNextYear_vec']; [FHEAD(:).EPSGrowthNextYear]=i; i=i+1;
  EPSGrowthPast5Years_vec = convert2numvec( finviz_data(:,FINHEAD.EPSGrowthPast5Years) ); data = [data; EPSGrowthPast5Years_vec']; [FHEAD(:).EPSGrowthPast5Years]=i; i=i+1;
  EPSGrowthNext5Years_vec = convert2numvec( finviz_data(:,FINHEAD.EPSGrowthNext5Years) ); data = [data; EPSGrowthNext5Years_vec']; [FHEAD(:).EPSGrowthNext5Years]=i; i=i+1;

  SalesGrowth5Years_vec = convert2numvec( finviz_data(:,FINHEAD.SalesGrowth5Years) ); data = [data; SalesGrowth5Years_vec']; [FHEAD(:).SalesGrowth5Years]=i; i=i+1;
  EPSGrowthLastQuarter_vec = convert2numvec( finviz_data(:,FINHEAD.EPSGrowthLastQuarter) ); data = [data; EPSGrowthLastQuarter_vec']; [FHEAD(:).EPSGrowthLastQuarter]=i; i=i+1;
  SalesGrowthLastQuarter_vec = convert2numvec( finviz_data(:,FINHEAD.SalesGrowthLastQuarter) ); data = [data; SalesGrowthLastQuarter_vec']; [FHEAD(:).SalesGrowthLastQuarter]=i; i=i+1;
  SharesOutstanding_vec = convert2numvec( finviz_data(:,FINHEAD.SharesOutstanding) ); data = [data; SharesOutstanding_vec']; [FHEAD(:).SharesOutstanding]=i; i=i+1;

  Float_vec = convert2numvec( finviz_data(:,FINHEAD.Float) ); data = [data; Float_vec']; [FHEAD(:).Float]=i; i=i+1;
  InsiderOwnership_vec  = convert2numvec( finviz_data(:,FINHEAD.InsiderOwnership) ); data = [data; InsiderOwnership_vec']; [FHEAD(:).InsiderOwnership]=i; i=i+1;
  InsiderTransactions_vec  = convert2numvec( finviz_data(:,FINHEAD.InsiderTransactions) ); data = [data; InsiderTransactions_vec']; [FHEAD(:).InsiderTransactions]=i; i=i+1;
  InstitutionalOwnership_vec = convert2numvec( finviz_data(:,FINHEAD.InstitutionalOwnership) ); data = [data; InstitutionalOwnership_vec']; [FHEAD(:).InstitutionalOwnership]=i; i=i+1;
  InstitutionalTransactions_vec = convert2numvec( finviz_data(:,FINHEAD.InstitutionalTransactions) ); data = [data; InstitutionalTransactions_vec']; [FHEAD(:).InstitutionalTransactions]=i; i=i+1;

  FloatShort_vec  = convert2numvec( finviz_data(:,FINHEAD.FloatShort) ); data = [data; FloatShort_vec']; [FHEAD(:).FloatShort]=i; i=i+1;
  ShortRatio_vec = convert2numvec( finviz_data(:,FINHEAD.ShortRatio) ); data = [data; ShortRatio_vec']; [FHEAD(:).ShortRatio]=i; i=i+1;

  ReturnOnAssets_vec = convert2numvec( finviz_data(:,FINHEAD.ReturnOnAssets) ); data = [data; ReturnOnAssets_vec']; [FHEAD(:).ReturnOnAssets]=i; i=i+1;
  ReturnOnEquity_vec = convert2numvec( finviz_data(:,FINHEAD.ReturnOnEquity) ); data = [data; ReturnOnEquity_vec']; [FHEAD(:).ReturnOnEquity]=i; i=i+1;
  ReturnOnInvestment_vec = convert2numvec( finviz_data(:,FINHEAD.ReturnOnInvestment) ); data = [data; ReturnOnInvestment_vec']; [FHEAD(:).ReturnOnInvestment]=i; i=i+1;
  CurrentRatio_vec = convert2numvec( finviz_data(:,FINHEAD.CurrentRatio) ); data = [data; CurrentRatio_vec']; [FHEAD(:).CurrentRatio]=i; i=i+1;
  QuickRatio_vec = convert2numvec( finviz_data(:,FINHEAD.QuickRatio) ); data = [data; QuickRatio_vec']; [FHEAD(:).QuickRatio]=i; i=i+1;
  LTDebtEquity_vec = convert2numvec( finviz_data(:,FINHEAD.LTDebtEquity) ); data = [data; LTDebtEquity_vec']; [FHEAD(:).LTDebtEquity]=i; i=i+1;
  TotalDebtEquity_vec = convert2numvec( finviz_data(:,FINHEAD.TotalDebtEquity) ); data = [data; TotalDebtEquity_vec']; [FHEAD(:).TotalDebtEquity]=i; i=i+1;

  GrossMargin_vec  = convert2numvec( finviz_data(:,FINHEAD.GrossMargin) ); data = [data; GrossMargin_vec']; [FHEAD(:).GrossMargin]=i; i=i+1;
  OperatingMargin_vec  = convert2numvec( finviz_data(:,FINHEAD.OperatingMargin) ); data = [data; OperatingMargin_vec']; [FHEAD(:).OperatingMargin]=i; i=i+1;
  ProfitMargin_vec  = convert2numvec( finviz_data(:,FINHEAD.ProfitMargin) ); data = [data; ProfitMargin_vec']; [FHEAD(:).ProfitMargin]=i; i=i+1;

  PerformanceWeek_vec  = convert2numvec( finviz_data(:,FINHEAD.PerformanceWeek) ); data = [data; PerformanceWeek_vec']; [FHEAD(:).PerformanceWeek]=i; i=i+1;
  PerformanceMonth_vec  = convert2numvec( finviz_data(:,FINHEAD.PerformanceMonth) ); data = [data; PerformanceMonth_vec']; [FHEAD(:).PerformanceMonth]=i; i=i+1;
  PerformanceQuarter_vec  = convert2numvec( finviz_data(:,FINHEAD.PerformanceQuarter) ); data = [data; PerformanceQuarter_vec']; [FHEAD(:).PerformanceQuarter]=i; i=i+1;
  PerformanceHalfYear_vec  = convert2numvec( finviz_data(:,FINHEAD.PerformanceHalfYear) ); data = [data; PerformanceHalfYear_vec']; [FHEAD(:).PerformanceHalfYear]=i; i=i+1;
  PerformanceYear_vec  = convert2numvec( finviz_data(:,FINHEAD.PerformanceYear) ); data = [data; PerformanceYear_vec']; [FHEAD(:).PerformanceYear]=i; i=i+1;
  PerformanceYTD_vec  = convert2numvec( finviz_data(:,FINHEAD.PerformanceYTD) ); data = [data; PerformanceYTD_vec']; [FHEAD(:).PerformanceYTD]=i; i=i+1;

  Beta_vec = convert2numvec( finviz_data(:,FINHEAD.Beta) ); data = [data; Beta_vec']; [FHEAD(:).Beta]=i; i=i+1;
  AverageTrueRange_vec = convert2numvec( finviz_data(:,FINHEAD.AverageTrueRange) ); data = [data; AverageTrueRange_vec']; [FHEAD(:).AverageTrueRange]=i; i=i+1;
  VolatilityWeek_vec = convert2numvec( finviz_data(:,FINHEAD.VolatilityWeek) ); data = [data; VolatilityWeek_vec']; [FHEAD(:).VolatilityWeek]=i; i=i+1;
  VolatilityMonth_vec = convert2numvec( finviz_data(:,FINHEAD.VolatilityMonth) ); data = [data; VolatilityMonth_vec']; [FHEAD(:).VolatilityMonth]=i; i=i+1;

  SMA20day_vec  = convert2numvec( finviz_data(:,FINHEAD.SMA20day) ); data = [data; SMA20day_vec']; [FHEAD(:).SMA20day]=i; i=i+1;
  SMA50day_vec  = convert2numvec( finviz_data(:,FINHEAD.SMA50day) ); data = [data; SMA50day_vec']; [FHEAD(:).SMA50day]=i; i=i+1;
  SMA200day_vec  = convert2numvec( finviz_data(:,FINHEAD.SMA200day) ); data = [data; SMA200day_vec']; [FHEAD(:).SMA200day]=i; i=i+1;

  High50day_vec  = convert2numvec( finviz_data(:,FINHEAD.High50day) ); data = [data; High50day_vec']; [FHEAD(:).High50day]=i; i=i+1;
  Low50day_vec  = convert2numvec( finviz_data(:,FINHEAD.Low50day) ); data = [data; Low50day_vec']; [FHEAD(:).Low50day]=i; i=i+1;
  High52week_vec  = convert2numvec( finviz_data(:,FINHEAD.High52week) ); data = [data; High52week_vec']; [FHEAD(:).High52week]=i; i=i+1;
  Low52week_vec  = convert2numvec( finviz_data(:,FINHEAD.Low52week) ); data = [data; Low52week_vec']; [FHEAD(:).Low52week]=i; i=i+1;

  RSI14_vec = convert2numvec( finviz_data(:,FINHEAD.RSI14) ); data = [data; RSI14_vec']; [FHEAD(:).RSI14]=i; i=i+1;
  ChangeFromOpen_vec = convert2numvec( finviz_data(:,FINHEAD.ChangeFromOpen) ); data = [data; ChangeFromOpen_vec']; [FHEAD(:).ChangeFromOpen]=i; i=i+1;
  Gap_vec = convert2numvec( finviz_data(:,FINHEAD.Gap) ); data = [data; Gap_vec']; [FHEAD(:).Gap]=i; i=i+1;
  AnalystRecommendation_vec = convert2numvec( finviz_data(:,FINHEAD.AnalystRecommendation) ); data = [data; AnalystRecommendation_vec']; [FHEAD(:).AnalystRecommendation]=i; i=i+1;
  AverageVolume_vec = convert2numvec( finviz_data(:,FINHEAD.AverageVolume) ); data = [data; AverageVolume_vec']; [FHEAD(:).AverageVolume]=i; i=i+1;
  RelativeVolume_vec = convert2numvec( finviz_data(:,FINHEAD.RelativeVolume) ); data = [data; RelativeVolume_vec']; [FHEAD(:).RelativeVolume]=i; i=i+1;
  Price_vec = convert2numvec( finviz_data(:,FINHEAD.Price) ); data = [data; Price_vec']; [FHEAD(:).Price]=i; i=i+1;
  ChangePercentage_vec = convert2numvec( finviz_data(:,FINHEAD.ChangePercentage) ); data = [data; ChangePercentage_vec']; [FHEAD(:).ChangePercentage]=i; i=i+1;
  Volume_vec = convert2numvec( finviz_data(:,FINHEAD.Volume) ); data = [data; Volume_vec']; [FHEAD(:).Volume]=i; i=i+1;
  %TargetPrice_vec = convert2numvec( finviz_data(:,FINHEAD.TargetPrice) ); data = [data; TargetPrice_vec']; [FHEAD(:).TargetPrice]=i; i=i+1;

  FHEAD_names = fieldnames(FHEAD); % cell of field name strings

  % Save name cells
  name_cell = cell(ntickers,5);
  for i=1:ntickers
    cell_print = finviz_data(i,FINHEAD.Ticker);
    name_cell{i,1} = cell_print{1};
    cell_print = finviz_data(i,FINHEAD.Company);
    name_cell{i,2} = cell_print{1};
    cell_print = finviz_data(i,FINHEAD.Sector);
    name_cell{i,3} = cell_print{1};
    cell_print = finviz_data(i,FINHEAD.Industry);
    name_cell{i,4} = cell_print{1};
    cell_print = finviz_data(i,FINHEAD.Country);
    name_cell{i,5} = cell_print{1};
  end

  % Save all data into data2.mat
  output_matfile = strcat(finviz_dir,'/finviz.mat');
  save(output_matfile,'FHEAD','FHEAD_names','name_cell','data');

end

rmpath('functions');
end

function [numvec]=convert2numvec(cellvec)
fprintf('Converting 2 numvec...\n')
n = length(cellvec);
numvec=zeros(n,1);
for i=1:n
  cellvec_i = cellvec(i);
  numstring_i = cellvec_i{1};
  if(strcmp(numstring_i,'')==1)
    numvec(i) = NaN;
  elseif(strcmp(numstring_i(end),'%')==1)
    numstring_i = numstring_i(1:end-1);
    numvec(i) = str2num(numstring_i);
  else
    numvec(i) = str2num(numstring_i);
  end
end
end

function [FINHEAD]=set_finviz_labels()
FINHEAD = struct('Ticker',{1});
[FINHEAD(:).Company]=2;
[FINHEAD(:).Sector]=3;
[FINHEAD(:).Industry]=4;
[FINHEAD(:).Country]=5;

[FINHEAD(:).MarketCap]=6;
[FINHEAD(:).PE]=7;
[FINHEAD(:).PEForward]=8;
[FINHEAD(:).PEG]=9;

[FINHEAD(:).PS]=10;
[FINHEAD(:).PB]=11;
[FINHEAD(:).PCash]=12;
[FINHEAD(:).PFreeCash]=13;
[FINHEAD(:).DividendYield]=14;
[FINHEAD(:).PayoutRatio]=15;

[FINHEAD(:).EPS]=16;%
[FINHEAD(:).EPSGrowthThisYear]=17;%
[FINHEAD(:).EPSGrowthNextYear]=18;%
[FINHEAD(:).EPSGrowthPast5Years]=19;%
[FINHEAD(:).EPSGrowthNext5Years]=20;%

[FINHEAD(:).SalesGrowth5Years]=21; %
[FINHEAD(:).EPSGrowthLastQuarter]=22;%
[FINHEAD(:).SalesGrowthLastQuarter]=23;%
[FINHEAD(:).SharesOutstanding]=24;%

[FINHEAD(:).Float]=25;%
[FINHEAD(:).InsiderOwnership]=26; %
[FINHEAD(:).InsiderTransactions]=27; %
[FINHEAD(:).InstitutionalOwnership]=28; %
[FINHEAD(:).InstitutionalTransactions]=29; %

[FINHEAD(:).FloatShort]=30; %
[FINHEAD(:).ShortRatio]=31; %

[FINHEAD(:).ReturnOnAssets]=32;
[FINHEAD(:).ReturnOnEquity]=33;
[FINHEAD(:).ReturnOnInvestment]=34;
[FINHEAD(:).CurrentRatio]=35;
[FINHEAD(:).QuickRatio]=36;
[FINHEAD(:).LTDebtEquity]=37;
[FINHEAD(:).TotalDebtEquity]=38;

[FINHEAD(:).GrossMargin]=39; %
[FINHEAD(:).OperatingMargin]=40; %
[FINHEAD(:).ProfitMargin]=41; %

[FINHEAD(:).PerformanceWeek]=42; %
[FINHEAD(:).PerformanceMonth]=43; %
[FINHEAD(:).PerformanceQuarter]=44;%
[FINHEAD(:).PerformanceHalfYear]=45; %
[FINHEAD(:).PerformanceYear]=46; %
[FINHEAD(:).PerformanceYTD]=47; %

[FINHEAD(:).Beta]=48;
[FINHEAD(:).AverageTrueRange]=49;
[FINHEAD(:).VolatilityWeek]=50;
[FINHEAD(:).VolatilityMonth]=51;

[FINHEAD(:).SMA20day]=52; %
[FINHEAD(:).SMA50day]=53; %
[FINHEAD(:).SMA200day]=54; %

[FINHEAD(:).High50day]=55; %
[FINHEAD(:).Low50day]=56; %
[FINHEAD(:).High52week]=57; %
[FINHEAD(:).Low52week]=58;%

[FINHEAD(:).RSI14]=59; %
[FINHEAD(:).ChangeFromOpen]=60; %
[FINHEAD(:).Gap]=61; %
[FINHEAD(:).AnalystRecommendation]=62; %
[FINHEAD(:).AverageVolume]=63; %
[FINHEAD(:).RelativeVolume]=64; %
[FINHEAD(:).Price]=65; %
[FINHEAD(:).ChangePercentage]=66; %
[FINHEAD(:).Volume]=67; %
[FINHEAD(:).EarningsDate]=68;
[FINHEAD(:).TargetPrice]=69;
[FINHEAD(:).IPODate]=70;
end


function M = obliquecomplexfactory(n, m, transposed)
% Returns a manifold struct defining complex matrices w/ unit-norm columns.
%
% function M = obliquecomplexfactory(n, m)
% function M = obliquecomplexfactory(n, m, transposed)
%
% Oblique manifold: deals with complex matrices of size n x m such that
% each column has unit 2-norm, i.e., is a point on the unit sphere in C^n.
% The geometry is a product geometry of m unit spheres in C^n. For the
% metric, C^n is treated as R^(2n), so that the real part and imaginary
% parts are treated separately as 2n real coordinates. As such, the complex
% oblique manifold is a Riemannian submanifold of (R^2)^(n x m), with the
% usual metric <u, v> = real(u'*v).
%
% If transposed is set to true (it is false by default), then the matrices
% are transposed: a point Y on the manifold is a matrix of size m x n and
% each row has unit 2-norm. It is the same geometry, just a different
% representation.
%
% In transposed form, a point Y is such that Y*Y' is a Hermitian, positive
% semidefinite matrix of size m and of rank at most n, such that all the
% diagonal entries are equal to 1.
%
% Note: obliquecomplexfactory(1, n, true) is equivalent to (but potentially
% slower than) complexcirclefactory(n).
%
% See also: spherecomplexfactory complexcirclefactory obliquefactory

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, Sep. 3, 2014.
% Contributors:
% Change log:
%
%   Oct. 21, 2016 (NB)
%       Formatted for inclusion in Manopt release.
%
%   July 20, 2017 (NB)
%       Distance function is now accurate for close-by points. See notes
%       inside the spherefactory file for details. Also improves distances
%       computation as part of the log function.

    if ~exist('transposed', 'var') || isempty(transposed)
        transposed = false;
    end

    if transposed
        trnsp = @(X) X.';
    else
        trnsp = @(X) X;
    end

    M.name = @() sprintf('Complex oblique manifold COB(%d, %d)', n, m);

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

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

    M.norm = @(x, d) norm(d(:));

    M.dist = @(x, y) norm(real(2*asin(.5*sqrt(sum(trnsp(abs(x - y).^2), 1)))));

    M.typicaldist = @() pi*sqrt(m);

    M.proj = @(X, U) trnsp(projection(trnsp(X), trnsp(U)));

    M.tangent = M.proj;

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

    M.ehess2rhess = @ehess2rhess;
    function rhess = ehess2rhess(X, egrad, ehess, U)
        X = trnsp(X);
        egrad = trnsp(egrad);
        ehess = trnsp(ehess);
        U = trnsp(U);

        PXehess = projection(X, ehess);
        inners = sum(real(conj(X).*egrad), 1);
        rhess = PXehess - bsxfun(@times, U, inners);

        rhess = trnsp(rhess);
    end

    M.exp = @exponential;
    % Exponential on the complex oblique manifold
    function y = exponential(x, d, t)
        x = trnsp(x);
        d = trnsp(d);

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

        nrm_td = sqrt(sum(real(td).^2 + imag(td).^2, 1));

        y = bsxfun(@times, x, cos(nrm_td)) + ...
            bsxfun(@times, td, sinxoverx(nrm_td));

        y = trnsp(y);
    end

    M.log = @logarithm;
    function v = logarithm(x1, x2)
        x1 = trnsp(x1);
        x2 = trnsp(x2);

        v = projection(x1, x2 - x1);
        dists = real(2*asin(.5*sqrt(sum(trnsp(abs(x2 - x1).^2), 1))));
        norms = sqrt(sum(real(v).^2 + imag(v).^2, 1));
        factors = dists./norms;
        % For very close points, dists is almost equal to norms, but
        % because they are both almost zero, the division above can return
        % NaN's. To avoid that, we force those ratios to 1.
        factors(dists <= 1e-10) = 1;
        v = bsxfun(@times, v, factors);

        v = trnsp(v);
    end

    M.retr = @retraction;
    % Retraction on the oblique manifold
    function y = retraction(x, d, t)
        x = trnsp(x);
        d = trnsp(d);

        if nargin < 3
            td = d;
        else
            td = t*d;
        end

        y = normalize_columns(x + td);

        y = trnsp(y);
    end

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

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

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

    M.lincomb = @matrixlincomb;

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

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

    M.pairmean = @pairmean;
    function y = pairmean(x1, x2)
        y = trnsp(x1+x2);
        y = normalize_columns(y);
        y = trnsp(y);
    end

    % vec returns a vector representation of an input tangent vector which
    % is represented as a matrix. mat returns the original matrix
    % representation of the input vector representation of a tangent
    % vector. vec and mat are thus inverse of each other. They are
    % furthermore isometries between a subspace of R^2nm and the tangent
    % space at x.
    vect = @(X) X(:);
    M.vec = @(x, u_mat) [vect(real(trnsp(u_mat))) ; ...
                         vect(imag(trnsp(u_mat)))];
    M.mat = @(x, u_vec)    trnsp(reshape(u_vec(1:(n*m)),     [n, m])) + ...
                        1i*trnsp(reshape(u_vec((n*m+1):end), [n, m]));
    M.vecmatareisometries = @() true;

end

% Given a matrix X, returns the same matrix but with each column scaled so
% that they have unit 2-norm.
function X = normalize_columns(X)
    norms = sqrt(sum(real(X).^2 + imag(X).^2, 1));
    X = bsxfun(@times, X, 1./norms);
end

% Orthogonal projection of the ambient vector H onto the tangent space at X
function PXH = projection(X, H)

    % Compute the inner product between each vector H(:, i) with its root
    % point X(:, i), that is, real(X(:, i)' * H(:, i)).
    % Returns a row vector.
    inners = real(sum(conj(X).*H, 1));

    % Subtract from H the components of the H(:, i)'s that are parallel to
    % the root points X(:, i).
    PXH = H - bsxfun(@times, X, inners);

end

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

    x = normalize_columns(randn(n, m) + 1i*randn(n, m));

end

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

    d = randn(n, m) + 1i*randn(n, m);
    d = projection(x, d);
    d = d / norm(d(:));

end


% cmbglobl.m : cmb globals : physical constants that are used through all files
% are given a value here
%
% D Vangheluwe 28 jan 2005, see cmbaccur.m anc cmbacc1.m as the main files

global  GL_cmb_c GL_cmb_h0  GL_cmb_t0  GL_cmb_T0  GL_cmb_rv  GL_cmb_fv  GL_cmb_kg1  GL_cmb_ka1 ...
  GL_cmb_yp  GL_cmb_ncr  GL_cmb_cr  GL_cmb_pcm  GL_cmb_dha;

% velocity of light in m/s
GL_cmb_c = 2.998e8;
% the Hubble constant at present in (h Mpc^-1), see my notes p71
GL_cmb_h0 = 1e5/GL_cmb_c;
% the present temperature of the CMB in degr Kelvin and in eV
GL_cmb_t0 = 2.725;
GL_cmb_T0 = GL_cmb_t0/11605;
% ratio of radiation density/critical density, see Dodelson (2.87)
%fv = 0.405;
GL_cmb_rv = (21/8) * (4/11)^(4/3);
GL_cmb_fv = GL_cmb_rv/(1 + GL_cmb_rv);
GL_cmb_kg1 = 2.47e-5;
GL_cmb_ka1 = GL_cmb_kg1/(1 - GL_cmb_fv);
%ka1 = 4.15e-5;
% the primordial helium mass fraction
GL_cmb_yp = 0.24;
% critical density in m^-3
GL_cmb_ncr = 11.23;
%ncr = 10.8;
% Compton cross section in m^2, see Dodelson p 72
GL_cmb_cr = 0.665e-28;
% conversion of Parsec to meters
GL_cmb_pcm = 3.0856e22;
% amplitude of the primordial density perturbation at the horizon, see Dodelson (8.76)
GL_cmb_dha = 4.47e-5;


%% Load data
clear all; close all; clc;

nst_on = load('msckf_NST_on.mat');
nst_off = load('msckf_NST_off.mat');

kStart = 500; kEnd = 1000;

%% Compute RMSE
nst_off_trans_rmse = sqrt(mean(nst_off.msckf_trans_err.^2,1));
nst_off_rot_rmse = sqrt(mean(nst_off.msckf_rot_err.^2,1));

nst_on_trans_rmse = sqrt(mean(nst_on.msckf_trans_err.^2,1));
nst_on_rot_rmse = sqrt(mean(nst_on.msckf_rot_err.^2,1));

%% Plot stuff
figure(1); clf;
fontSize = 14;
lineWidth = 2;
pos = [200,200,640,400];
xLim = [kStart, kEnd];
k = kStart:kEnd;
xticks = linspace(kStart,kEnd,11);

transLim = [0 0.6];
rotLim = [0 0.25];

set(gcf,'Position',pos);

% Translational RMSE
subplot(2,1,1);
plot(k,nst_off_trans_rmse, '-r','LineWidth',lineWidth); hold on;
plot(k,nst_on_trans_rmse, '-b','LineWidth',lineWidth);
xlim(xLim);
set(gca,'XTick',xticks);
% ylim(transLim)
legend('Noise Correlated to State', 'Noise De-correlated from State', ...
    'Location','northwest');
ylabel('Trans. RMSE (m)')
title('Effect of Nullspace Projection');
set(gca,'FontSize',fontSize);
grid minor;
box on;

% Rotational RMSE
subplot(2,1,2);
plot(k,nst_off_rot_rmse,'-r','LineWidth',lineWidth); hold on;
plot(k,nst_on_rot_rmse,'-b','LineWidth',lineWidth);
xlim(xLim);
set(gca,'XTick',xticks);
% ylim(rotLim)
legend('Noise Correlated to State', 'Noise De-correlated from State', ...
    'Location','northwest');
ylabel('Rot. RMSE (Axis-Angle)')
xlabel('Timestep')
set(gca,'FontSize',fontSize)
grid minor;
box on;

%% Export figure
export_fig(gcf, 'NST_RMSE.pdf', '-transparent');


% Fast convolution, replacement for both conv2 and convn.
%
% See conv2 or convn for more information on convolution in general.
%
% This works as a replacement for both conv2 and convn.  Basically,
% performs convolution in either the frequency or spatial domain, depending
% on which it thinks will be faster (see below). In general, if A is much
% bigger then B then spatial convolution will be faster, but if B is of
% similar size to A and both are fairly big (such as in the case of
% correlation), convolution as multiplication in the frequency domain will
% tend to be faster.
%
% The shape flag can take on 1 additional value which is 'smooth'.  This
% flag is intended for use with smoothing kernels.  The returned matrix C
% is the same size as A with boundary effects handled in a special manner.
% That is instead of A being zero padded before being convolved with B;
% near the boundaries a cropped version of the matrix B is used, and the
% results is scaled by the fraction of the weight found in  the cropped
% version of B.  In this case each dimension of B must be odd, and all
% elements of B must be positive.  There are other restrictions on when
% this flag can be used, and in general it is only useful for smoothing
% kernels.  For 2D filtering it does not have much overhead, for 3D it has
% more and for higher dimensions much much more.
%
% For optimal performance some timing constants must be set to choose
% between doing convolution in the spatial and frequency domains, for more
% info see timeConv below.
%
% USAGE
%  C = convnFast( A, B, [shape] )
%
% INPUTS
%  A       - d dimensional input matrix
%  B       - d dimensional matrix to convolve with A
%  shape   - ['full'] 'valid', 'full', 'same', or 'smooth'
%
% OUTPUTS
%  C       - result of convolution
%
% EXAMPLE
%
% See also CONV2, CONVN

% Piotr's Image&Video Toolbox      Version 2.0
% Written and maintained by Piotr Dollar    pdollar-at-cs.ucsd.edu
% Please email me if you find bugs, or have suggestions or questions!

function C = convnFast( A, B, shape )

if( nargin<3 || isempty(shape)); shape='full'; end
if(isempty(strmatch(shape, char({'same', 'valid', 'full', 'smooth'}))))
  error( 'convnFast: unknown shape flag' ); end

shapeorig = shape;
smoothFlag = (strcmp(shape,'smooth'));
if( smoothFlag ); shape = 'same'; end;

% get dimensions of A and B
ndA = ndims(A);  ndB = ndims(B); nd = max(ndA,ndB);
sizA = size(A); sizB = size(B);
if (ndA>ndB); sizB = [sizB ones(1,ndA-ndB)]; end
if (ndA<ndB); sizA = [sizA ones(1,ndB-ndA)]; end

% ERROR CHECK if smoothflag
if( smoothFlag )
  if( ~all( mod(sizB,2)==1 ) )
    error('If flag==''smooth'' then must have odd sized mask');
  end;
  if( ~all( B>0 ) )
    error('If flag==''smooth'' then mask must have >0 values.');
  end;
  if( any( (sizB-1)/2>sizA ) )
    error('B is more then twice as big as A, cannot use flag==''smooth''');
  end;
end

% OPTIMIZATION for 3D conv when B is actually 2D - calls (spatial) conv2
% repeatedly on 2D slices of A.  Note that may need to rearange A and B
% first and use recursion. The benefits carry over to convnBound
% (which is faster for 2D arrays).
if( ndA==3 && ndB==3 && (sizB(1)==1 || sizB(2)==1) )
  if (sizB(1)==1)
    A = permute( A, [2 3 1]);  B = permute( B, [2 3 1]);
    C = convnFast( A, B, shapeorig );
    C = permute( C, [3 1 2] );
  elseif (sizB(2)==1)
    A = permute( A, [3 1 2]);  B = permute( B, [3 1 2]);
    C = convnFast( A, B, shapeorig );
    C = permute( C, [2 3 1] );
  end
  return;
elseif( ndA==3 && ndB==2 )
  C1 = conv2( A(:,:,1), B, shape );
  C = zeros( [size(C1), sizA(3)] ); C(:,:,1) = C1;
  for i=2:sizA(3); C(:,:,i) = conv2( A(:,:,i), B, shape ); end
  if (smoothFlag)
    for i=1:sizA(3)
      C(:,:,i) = convnBound(A(:,:,i),B,C(:,:,i),sizA(1:2),sizB(1:2));
    end
  end
  return;
end

% get predicted time of convolution in frequency and spatial domain
% constants taken from timeConv
sizfft = 2.^ceil(real(log2(sizA+sizB-1))); psizfft=prod(sizfft);
frequenPt = 3 * 1e-7 * psizfft * log(psizfft);
if (nd==2)
  spatialPt = 5e-9 * sizA(1) * sizA(2) * sizB(1) * sizB(2);
else
  spatialPt = 5e-8 * prod(sizA) * prod(sizB);
end

% perform convolution
if ( spatialPt < frequenPt )
  if (nd==2)
    C = conv2( A, B, shape );
  else
    C = convn( A, B, shape );
  end
else
  C = convnFreq( A, B, sizA, sizB, shape );
end;

% now correct boundary effects (if shape=='smooth')
if( ~smoothFlag ); return; end;
C = convnBound( A, B, C, sizA, sizB );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate boundary values for C in spatial domain
function C = convnBound( A, B, C, sizA, sizB )
nd = length(sizA);
radii = (sizB-1)/2;

% flip B appropriately (conv flips B)
for d=1:nd; B = flipdim(B,d); end

% get location that need to be updated
% this is the LEAST efficient part (fixing is annoying though)
inds = {':'}; inds = inds(:,ones(1,nd));
Dind = zeros( sizA );
for d=1:nd
  inds1 = inds; inds1{ d } = 1:radii(d);
  inds2 = inds; inds2{ d } = sizA(d)-radii(d)+1:sizA(d);
  Dind(inds1{:}) = 1;  Dind(inds2{:}) = 1;
end
Dind = find( Dind );
Dndx = ind2sub2( sizA, Dind );
nlocs = length(Dind);

% get cuboid dimensions for all the boundary regions
sizeArep = repmat( sizA, [nlocs,1] );
radiiRep = repmat( radii, [nlocs,1] );
Astarts = max(1,Dndx-radiiRep);
Aends = min( sizeArep, Dndx+radiiRep);
Bstarts = Astarts + (1-Dndx+radiiRep);
Bends = Bstarts + (Aends-Astarts);

% now update these locations
vs = zeros( 1, nlocs );
if( nd==2 )
  for i=1:nlocs % accelerated for 2D arrays [fast]
    Apart = A( Astarts(i,1):Aends(i,1), Astarts(i,2):Aends(i,2) );
    Bpart = B( Bstarts(i,1):Bends(i,1), Bstarts(i,2):Bends(i,2) );
    v = (Apart.*Bpart); vs(i) = sum(v(:)) ./ sum(Bpart(:));
  end
elseif( nd==3 ) % accelerated for 3D arrays
  for i=1:nlocs
    Apart = A( Astarts(i,1):Aends(i,1), Astarts(i,2):Aends(i,2), ...
      Astarts(i,3):Aends(i,3) );
    Bpart = B( Bstarts(i,1):Bends(i,1), Bstarts(i,2):Bends(i,2), ...
      Bstarts(i,3):Bends(i,3) );
    za = sum(sum(sum(Apart.*Bpart))); zb=sum(sum(sum(Bpart)));
    vs(1,i) = za./zb;
  end
else % general case [slow]
  extract=cell(1,nd);
  for i=1:nlocs
    for d=1:nd; extract{d} = Astarts(i,d):Aends(i,d); end
    Apart = A( extract{:} );
    for d=1:nd; extract{d} = Bstarts(i,d):Bends(i,d); end
    Bpart = B( extract{:} );
    v = (Apart.*Bpart); vs(i) = sum(v(:)) ./ sum(Bpart(:));
  end
end
C( Dind ) = vs * sum(B(:));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Convolution as multiplication in the frequency domain
function C = convnFreq( A, B, sizA, sizB, shape )
siz = sizA + sizB - 1;

% calculate correlation in frequency domain
Fa = fftn(A,siz);
Fb = fftn(B,siz);
C = ifftn(Fa .* Fb);

% make sure output is real if inputs were both real
if(isreal(A) && isreal(B)); C = real(C); end

% crop to size
if(strcmp(shape,'valid'))
  C = arrayToDims( C, max(0,sizA-sizB+1 ) );
elseif(strcmp(shape,'same'))
  C = arrayToDims( C, sizA );
elseif(~strcmp(shape,'full'))
  error('unknown shape');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function used to calculate constants for prediction of convolution in the
% frequency and spatial domains.  Method taken from normxcorr2.m
% May need to reset K's if placing this on a new machine, however, their
% ratio should be about the same..
function K = timeConv() %#ok<DEFNU>
mintime = 4;

switch 3
  case 1  % conv2  [[empirically K = 5e-9]]
    % convolution time = K*prod(size(a))*prod(size(b))
    siza = 30;  sizb = 200;
    a = ones(siza);  b = ones(sizb);
    t1 = cputime;  t2 = t1; k = 0;
    while (t2-t1)<mintime;
      disc = conv2(a,b); k = k + 1; t2 = cputime; %#ok<NASGU>
    end
    K = (t2-t1)/k/siza^2/sizb^2;

  case 2  % convn  [[empirically K = 5e-8]]
    % convolution time = K*prod(size(a))*prod(size(b))
    siza = [10 10 10];  sizb = [30 30 10];
    a = ones(siza);  b = ones(sizb);
    t1 = cputime;  t2 = t1;  k = 0;
    while (t2-t1)<mintime;
      disc = convn(a,b); k = k + 1; t2 = cputime; %#ok<NASGU>
    end
    K = (t2-t1)/k/prod(siza)/prod(sizb);

  case 3 % fft (one dimensional) [[empirically K = 1e-7]]
    % fft time = K * n log(n)  [if n is power of 2]
    % Works fastest for powers of 2.  (so always zero pad until have
    % size of power of 2?).  2 dimensional fft has to apply single
    % dimensional fft to each column, and then signle dimensional fft
    % to each resulting row.  time = K * (mn)log(mn).  Likewise for
    % highter dimensions.  convnFreq requires 3 such ffts.
    n = 2^nextpow2(2^15);
    vec = complex(rand(n,1),rand(n,1));
    t1 = cputime;  t2 = t1;  k = 0;
    while (t2-t1) < mintime;
      disc = fft(vec); k = k + 1; t2 = cputime; %#ok<NASGU>
    end
    K = (t2-t1) / k / n / log(n);
end


function hbg = bgCircleGraph(bg,NodeSizes,varargin)
% (optional) NodeSizes should be an array specifying the radius of each node
% as a fraction (0 to 1) of the radius of the circlegraph

if nargin<2
    NodeSizes = [];
elseif numel(NodeSizes)==1
    NodeSizes = NodeSizes*ones(1,length(bg.Nodes));
end

% I create a random graph with 28 nodes:
% bg = biograph(28);

% Display it in the GUI and get a handle
% back to work with it (this will use by default
% the hierarchical layout):
hbg = view(bg);
% Get an idea of what are the actual extents of
% the page in the GUI by looking at all the current
% node positions:
page_Size = max(cell2mat(arrayfun(@(x) get(x,'Position'),...
            get(hbg,'Nodes'),'Uniform',false)));
% Place the nodes in a circular layout, in my graph
% page_size was around [1000,1000], so I will select a
% center at [500 500] and a radius of 300, this will keep
% the layout approximately in the same scale and I will
% not have to manually change the size of the nodes or
% the fonts:
radius = 0.6*(min(page_Size)/2);
center = [page_Size(1)/2 page_Size(2)/2];
numNodes = length(hbg.Nodes);
for i = 1:numNodes
set(hbg.Nodes(i),'Position',...
   [center(1)+radius.*sin((i*2*pi/numNodes)),...
    center(2)+radius.*cos((i*2*pi/numNodes))],'shape','circle')

    if ~isempty(NodeSizes)
        sz = 2*radius*NodeSizes(i);
        set(hbg.Nodes(i),'Size',[sz sz]);
    end
end

% Now I can use dolayout:
set(bg,'NodeAutoSize','off','EdgeType','curved',varargin{:});
dolayout(hbg,'pathsOnly',true)


function test_suite = test_medianLine
%TESTMEDIANLINE  One-line description here, please.
%   output = testMedianLine(input)
%
%   Example
%   testMedianLine
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2009-04-22,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.
% Licensed under the terms of the LGPL, see the file "license.txt"

test_suite = functiontests(localfunctions);

function testTwoPoints(testCase) %#ok<*DEFNU>
% test with 2 points

p1 = [0 0];
p2 = [10 0];
exp = [5 0 0 10];
line = medianLine(p1, p2);
testCase.assertEqual(exp, line, 'AbsTol', .01);

function testEdge(testCase)
% test with an edge as input

p1 = [0 0];
p2 = [10 0];
exp = [5 0 0 10];
line = medianLine([p1 p2]);
testCase.assertEqual(exp, line, 'AbsTol', .01);

function testTwoPointArrays(testCase) %#ok<*DEFNU>
% test with 2 points

p1 = [0 0; 10 10];
p2 = [10 0;10 20];

exp = [5 0 0 10; 10 15 -10 0];

line = medianLine(p1, p2);
testCase.assertEqual(exp, line, 'AbsTol', .01);


function sR=icassoCluster(sR,varargin)
%function sR=icassoCluster(sR,['identifier1',value1,'identifier2',value2,...])
%
%PURPOSE
%
%To cluster the ICA estimates and to compute a relative clustering validity
%index (R-index)
%
%EXAMPLE OF BASIC USAGE
%
% sR=icassoCluster(sR);
%
%where sR is an Icasso result structure. This applies hierarchical
%clustering using group-average linkage agglomeration strategy and
%stores the results back into workspace variable sR.
%
%INPUT
%
% sR (struct) Icasso result data structure
%
%Optional input arguments are given as argument identifier - value
%pairs: 'identifier1', value1, 'identifier2', value2,...
%(case insensitive)
%
% 'simfcn' (string) 'abscorr' (default) | (matrix)
%   Indicates how to compute similarities S between estimates i,j
%   'abscorr' S(i,j) is the absolute value of the linear
%     correlation coefficient
%   (matrix) explicitly given similarity matrix S
%     (elements should be 0...1)
% 'strategy' (string) 'AL' (default) | 'SL' | 'CL'
%  Sets the clustering strategy:
%  'AL' hierarchical group average linkage
%  'SL' hierarchical single linkage (nearest neighbor)
%  'CL' hierarchical complete linkage (furthest neighbor)
% 'L' (integer) | (string) 'rdim' (default)
%   computes a relative clustering validity index for 2...L
%   clusters. Default string 'rdim' sets L same as the (reduced)
%   data dimension
%'s2d' (string) (default is 'sim2dis')
%  the name of function that is used to make the transformation
%  from similarities between IC components to dissimilarities
%  D. Default 'sim2dis' makes simply D=1-S;
%
%OUTPUT
%
% sR (struct) updated Icasso result data structure
%
%The function updates the fields sR.cluster.* only.
%
%DETAILS
%1. The function computes similarities between the
%estimates. See Note 1
%2. stores the similarity matrix S into field sR.cluster.similarity
%and the method into field sR.cluster.simfcn
%2. transforms the similarities into dissimilarities (distances)
%D. See Note 2.
%3. applies the selected clustering strategy on dissimilarities. D
%The results is a partition matrix P (of size MxM). See explanation
%in function hcluster The function stores P in field
%sR.cluster.partition (Stores also other outputs of function
%hcluster into sR.cluster.dendrogram).
%4. computes a relative clustering validity index (see function
%rindex) for dissimilarities D and partitions P from 2 to L
%clusters. If not explicitly given (or string 'rdim' is given, L
%will be the (reduced data) dimension. Stores the result in field
%sR.cluster.index.R.
%
%NOTE 1
%
%By default, the similarities between estimates are computed as
%linear correlation coefficients (using the demixing matrix rows and
%dewhitening matrix; see function corrw). You can also explicitly
%give any MxM similarity matrix as input. The similarities
%should be between 0...1. (M is the total number of estimates.)
%
%NOTE 2
%The clustering and clustering validity indices are computed for
%dissimilarities. Therefore, the similarities must be transformed
%into dissimilarities. By default, icassoCluster uses subfunction
%sim2dis to do this; sim2dis performs simply D=1-S. If you wish to
%make the transformation otherwise, you can set a different
%function by using input identifier - valuepair
%'s2d','myfunctionname'. icassoCluster then computes
%dissimilarities by calling
%   D=feval('myfunctionname',sR.cluster.similarity);
%A possible function is sqrtsim2dis
%
%SEE ALSO
% hcluster
% som_linkage
% sqrtsim2dis
% rindex
% icassoDendrogram

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

% ver 1.21 070305 johan

% Init some variables

% total number of estimates
M=icassoGet(sR,'M');

%reduced data dimension
rdim=icassoGet(sR,'rdim');

% Set default parameters
default={'simfcn','abscorr','s2d','sim2dis','strategy','AL','L','rdim'};

%% Check optional arguments and add defaults
clusterparameters=processvarargin(varargin,default);
num_of_args=length(clusterparameters);

%% check arguments
for i=1:2:num_of_args;
  switch lower(clusterparameters{i})

   case 'simfcn'
    simfcn=clusterparameters{i+1};

    % Explicit similarity matrix?
    if isnumeric(simfcn),
      if size(simfcn,1)==M & size(simfcn,2)==M,
        sR.cluster.similarity=simfcn;
        sR.cluster.simfcn='<similarities given explicitly>';
      else
        error('Explicitly given similarity matrix has wrong size!');
      end
    else
      % should be a string
      switch lower(simfcn)
       case 'abscorr'
        % ok
        sR.cluster.simfcn=lower(simfcn);
       otherwise
        error('''simfcn'' must be string ''abscorr'' or an MxM similarity matrix');
      end
    end
   case 's2d'
    s2dfcn=lower(clusterparameters{i+1});
    if ~ischar(s2dfcn),
      error('''s2d'' must be a string (name of a function)');
    end
    sR.cluster.s2d=s2dfcn;
   case 'l'
    L=clusterparameters{i+1};
    if isnumeric(L),
      % The user has specified max number for clusters

      % Check L
      if fix(L)~=L,
        error('''L'' must be an integer.');
      elseif L<2,
        error('''L'' must be at least 2.');
      elseif L>M,
        error('''L'' cannot be more than the number of estimates.');
      end
    else
      if ~strcmp(lower(L),'rdim'),
        error('''L'' expects an integer value or ''rdim''.');
      end
      % set (reduced) data dimension
      L=icassoGet(sR,'rdim');
    end

    if L>100,
      warning(['R-index requested for more that 100 clusters: this can' ...
               ' be heavy...']);
    end

   case 'strategy'
    strategy=clusterparameters{i+1};
    if ~ischar(strategy),
      error('''strategy'' must be a string');
    end

    % we are case insensitive
    strategy=upper(strategy);
    sR.cluster.strategy=strategy;

    switch sR.cluster.strategy
     case {'AL','CL','SL'}
      ; % hierarchical clustering
     otherwise
      error(['Strategy ' strategy ' not implemented.']);
    end
   otherwise
    error(['Indentifier ' clusterparameters{i} ' not recognized.']);
  end
end

%%%% Compute similarities %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

switch lower(sR.cluster.simfcn)
 case '<similarities given explicitly>'
  % already handled
 case 'abscorr'
  sR.cluster.similarity=abs(corrw(icassoGet(sR,'W'),icassoGet(sR,'dewhitemat')));
  %just to make sure
  sR.cluster.similarity(sR.cluster.similarity>1)=1;
  sR.cluster.similarity(sR.cluster.similarity<0)=0;
end

%%%%% Convert to dissimilarities using .s2d

D=feval(sR.cluster.s2d, sR.cluster.similarity);

%%%% Make partition %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[sR.cluster.partition,sR.cluster.dendrogram.Z,sR.cluster.dendrogram.order]=...
    hcluster(D,sR.cluster.strategy);

%%%%% Compute cluster validity %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% init R
sR.cluster.index.R=ones(M,1)*NaN;
% compute

sR.cluster.index.R(1:L,1)=rindex(D,sR.cluster.partition(1:L,:));


% ------------------------------------------------------------------------
function stats = vid_image_stats(imdb_video, perc_training, base_path)
%VID_IMAGE_STATS
%	Compute basic colour stats for a random PERC_TRAINING of the dataset
%	Used for data augmentation during training.
% 	e.g. vid_image_stats(imdb_video, 0.1, /path/to/curated/ILSVRC15/')
% ------------------------------------------------------------------------
    % collect different stats for z and x crops (x contains more padding)
    z_sz = 127;
    x_sz = 255;
    samples_per_video = 16;
    imout_z = zeros(z_sz, z_sz, 3, samples_per_video, 'single');
    imout_x = zeros(x_sz, x_sz, 3, samples_per_video, 'single');
    crops_z_string = cell(1, samples_per_video);
    crops_x_string = cell(1, samples_per_video);
    n_video = numel(imdb_video.id);
    n_video_train = round(perc_training * n_video);
    avg_z = cell(1,n_video_train);
    rgbm1_z = cell(1,n_video_train);
    rgbm2_z = cell(1,n_video_train);
    n_z = samples_per_video*z_sz*z_sz;
    avg_x = cell(1,n_video_train);
    rgbm1_x = cell(1,n_video_train);
    rgbm2_x = cell(1,n_video_train);
    n_x = samples_per_video*x_sz*x_sz;
    for v=1:n_video_train
        n_obj = numel(imdb_video.objects{v});
        rand_objs = datasample(1:n_obj, samples_per_video);
        for o=1:samples_per_video
            crops_z_string{o} = [base_path strrep(imdb_video.objects{v}{rand_objs(o)}.frame_path, '.JPEG','') '.' num2str(imdb_video.objects{v}{rand_objs(o)}.track_id, '%02d') '.crop.z.jpg'];
            crops_x_string{o} = [base_path strrep(imdb_video.objects{v}{rand_objs(o)}.frame_path, '.JPEG','') '.' num2str(imdb_video.objects{v}{rand_objs(o)}.track_id, '%02d') '.crop.x.jpg'];
        end
        files = [crops_z_string crops_x_string];
        imgs = vl_imreadjpeg(files, 'numThreads', 12);
        crops_z = imgs(1:samples_per_video);
        crops_x = imgs(samples_per_video+1 : end);
        for o=1:samples_per_video
           imout_z(:,:,:,o) = crops_z{o};
           imout_x(:,:,:,o) = crops_x{o};
        end

        Z = reshape(permute(imout_z,[3 1 2 4]),3,[]);
        avg_z{end+1} = mean(imout_z, 4);
        rgbm1_z{end+1} = sum(Z,2)/n_z;
        rgbm2_z{end+1} = Z*Z'/n_z;

        X = reshape(permute(imout_x,[3 1 2 4]),3,[]);
        avg_x{end+1} = mean(imout_x, 4);
        rgbm1_x{end+1} = sum(X,2)/n_x;
        rgbm2_x{end+1} = X*X'/n_x;
        fprintf('Processed video %d/%d\n', v, n_video_train);
    end

    stats = struct();
    stats.z = struct();
    stats.x = struct();

    stats.z.averageImage = mean(cat(4,avg_z{:}),4);
    rgbm1_z = mean(cat(2,rgbm1_z{:}),2);
    stats.z.rgbm1 = rgbm1_z;
    rgbm2_z = mean(cat(3,rgbm2_z{:}),3);
    stats.z.rgbm2 = rgbm2_z;
    stats.z.rgbMean = rgbm1_z;
    stats.z.rgbCovariance = rgbm2_z - rgbm1_z*rgbm1_z';

    stats.x.averageImage = mean(cat(4,avg_x{:}),4);
    rgbm1_x = mean(cat(2,rgbm1_x{:}),2);
    stats.x.rgbm1 = rgbm1_x;
    rgbm2_x = mean(cat(3,rgbm2_x{:}),3);
    stats.z.rgbm2 = rgbm2_z;
    stats.x.rgbMean = rgbm1_x;
    stats.x.rgbCovariance = rgbm2_x - rgbm1_x*rgbm1_x';
end


function [F,sE,sC] = VBA_spm_log_evidence(varargin)
% Return the log-evidence of a reduced model (under Laplace approximation)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC,rE,rC)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC,priorfun,varargin)
% FORMAT [F,sE,sC] = spm_log_evidence(qE,qC,pE,pC)
%
% qE,qC    - posterior expectation and covariance of full model
% pE,pC    - prior expectation and covariance of full model
% rE,rC    - prior expectation and covariance of reduced model
% or
% priorfun - inline function that returns prior moments
%            {rE rC} = priorfun(varargin{:})
%
% or (if omitted) rE = 0 and rC = 0;
%
% F        - reduced log-evidence: ln p(y|reduced model) - ln p(y|full model)
% [sE,sC]  - posterior expectation and covariance of reduced model
%
%--------------------------------------------------------------------------
% This routine assumes the reduced model is nested within a full model and
% that the posteriors (and priors) are Gaussian. Nested here means that the
% prior precision of the reduced model, minus the prior precision of the
% full model is positive definite. We additionally assume that the prior
% means are unchanged. The two input argument formats are for use with
% spm_argmax.
%__________________________________________________________________________
% Copyright (C) 2005 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_log_evidence.m 4281 2011-03-31 19:49:57Z karl $

% Compute reduced log-evidence
%==========================================================================

% check to see if priors are specified by a function
%--------------------------------------------------------------------------
qE = varargin{1};
qC = varargin{2};
pE = varargin{3};
pC = varargin{4};
try
    priors = varargin{5}(varargin{6:end});
    rE     = priors{1};
    rC     = priors{2};
catch
    try
        rE = varargin{5};
        rC = varargin{6};
    catch
        n  = size(qC,1);
        rE = sparse(n,1);
        rC = sparse(n,n);
    end
end

% reduced subspace
%--------------------------------------------------------------------------
qE  = VBA_spm_vec(qE);
pE  = VBA_spm_vec(pE);
rE  = VBA_spm_vec(rE);

if nargout < 2
    dE  = pE - rE;
    dC  = pC - rC;
    k   = find(dE | any(dC,2));
    if ~isempty(k)
        qE  = qE(k);
        pE  = pE(k);
        rE  = rE(k);
        qC  = qC(k,k);
        pC  = pC(k,k);
        rC  = rC(k,k);
    else

        % the reduced and full models are the same
        %------------------------------------------------------------------
        F   = 0;
        return
    end
end

% fix tolerance for matrix inversions
%--------------------------------------------------------------------------
TOL   = exp(-16);

% remove fixed parameters under full model
%--------------------------------------------------------------------------
i     = find(diag(pC));

% preliminaries
%--------------------------------------------------------------------------
qP    = VBA_spm_inv(qC(i,i),TOL);
pP    = VBA_spm_inv(pC(i,i),TOL);
rP    = VBA_spm_inv(rC(i,i),TOL);
sP    = qP + rP - pP;
sC    = VBA_spm_inv(sP,TOL);
sE    = qP*qE(i) + rP*rE(i) - pP*pE(i);

% log-evidence
%--------------------------------------------------------------------------
F     = VBA_spm_logdet(rP*qP*sC*pC(i,i)) ...
      - (qE(i)'*qP*qE(i) + rE(i)'*rP*rE(i) - pE(i)'*pP*pE(i) - sE'*sC*sE);
F     = F/2;

% restore full conditional density
%--------------------------------------------------------------------------
if nargout > 1
    rE(i)   = sC*sE;
    rC(i,i) = sC;
    sE      = VBA_spm_unvec(rE,varargin{1});
    sC      = rC;
end


function [Wout,Phiout,Phioutbis]=dualwavelets(W,Phi,LP)
	%for jr=1:Jres
	%filters.psi=W;
	%filters.phi=Phi;
	%LP=littlewood_paley_ISCV(filters);
	J=size(W,2);
	L=size(W{end},2);
	eps=1e-12;
	tight=0;

	Wout = W;
	Phiout = Phi;

	for j=1:J
		if ~isempty(W{j})
			for l=1:L
				if tight
					% if frame is tight, LP = 1 for all omega, so we only need to conjugate
					Wout{j}{l}=conj(W{j}{l});
				else
					% if frame is not tight, we need to divide by LP so that
					% sum W_j*Wd_j = sum W_j*conj(W_j)/LP = LP/LP = 1
					% but this can be dangerous is LP becomes very small, so only
					% do this on the support of W_j
					supp=(abs(W{j}{l})>eps);
					Wout{j}{l} = supp.*(conj(W{j}{l})./(LP));
				end
			end
		end
	end
	eps = 2e-2;
	supp=(abs(Phi)>eps);
	Phiout=supp.*((Phi)./(LP));
	if tight
		Phiout=Phi;
		Phioutbis=supp.*min(8,1./Phi);
		Ic=find(supp==0);
		Phioutbis(Ic)=0;
	end
end


function linplus_test445 ( )

%*****************************************************************************80
%
%% TEST445 tests R8PBL_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 9;
  mu = 3;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST445\n' );
  fprintf ( 1, '  R8PBL_INDICATOR sets up a R8PBL indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
  fprintf ( 1, '  Bandwidth MU = %d\n', mu );

  a = r8pbl_indicator ( n, mu );

  r8pbl_print ( n, mu, a, '  The R8PBL indicator matrix:' );

  return
end


function H = mrdivide(F, G)
%/   SPHEREFUNV right divide.
%   F/G divides each component of the SPHEREFUNV F by the DOUBLE or
%   SPHEREFUN G.
%
%   Only allowed to divide by a DOUBLE or a SPHEREFUN.
%
% See also SPHEREFUNV/MLDIVIDE.

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

if ( isempty(F) || isempty(G) )
   H = spherefunv;
   return
end

if ( ~isa(G,'double') && ~isa(G,'chebfun2') )
    error('SPHEREFUN:SPHEREFUNV:mrdivide:nonScalar', ...
        'Division must be scalar valued.');
end

% componentwise divide.
if ( isa(G,'double') )
    H = F;
    for j = 1:3
        H.components{j} = mrdivide(F.components{j}, G);
    end
else
    H = F;
    for j = 1:3
        H.components{j} = rdivide(F.components{j}, G);
    end
end

end


function [OffDec,OffVel] = LCSA_CoefficientSMPSOOperator(Particle,Pbest,Gbest,xlower,xupper)
% -----------------------------------------------------------------------
%  Copyright (C) 2020 Heiner Zille
%
%  This work is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0
%  International License. (CC BY-NC-SA 4.0). To view a copy of this license,
%  visit http://creativecommons.org/licenses/by-nc-sa/4.0/ or see the
%  pdf-file "License-CC-BY-NC-SA-4.0.pdf" that came with this code.
%
%  You are free to:
%  * Share ? copy and redistribute the material in any medium or format
%  * Adapt ? remix, transform, and build upon the material
%  Under the following terms:
%  * Attribution ? You must give appropriate credit, provide a link to the
%     license, and indicate if changes were made. You may do so in any reasonable
%     manner, but not in any way that suggests the licensor endorses you or your use.
%  * NonCommercial ? You may not use the material for commercial purposes.
%  * ShareAlike ? If you remix, transform, or build upon the material, you must
%    distribute your contributions under the same license as the original.
%  * No additional restrictions ? You may not apply legal terms or technological
%    measures that legally restrict others from doing anything the license permits.
%
%  Author of this Code:
%   Heiner Zille <heiner.zille@ovgu.de> or <heiner.zille@gmail.com>
%
%  This code is based on the following publications:
%
%  1) Heiner Zille
%     "Large-scale Multi-objective Optimisation: New Approaches and a Classification of the State-of-the-Art"
%     PhD Thesis, Otto von Guericke University Magdeburg, 2019
%     http://dx.doi.org/10.25673/32063
%
%  2) Heiner Zille and Sanaz Mostaghim
%     "Linear Search Mechanism for Multi- and Many-Objective Optimisation"
%     10th International Conference on Evolutionary Multi-Criterion Optimization (EMO 2019),
%        Lecture Notes in Computer Science, vol 11411.
%        Deb K. et al. (eds), Springer, Cham, East Lansing, Michigan, USA, March 2019
%     https://doi.org/10.1007/978-3-030-12598-1_32.
%
%  This file is intended to work with the PlatEMO framework version 2.5.
%  Date of publication of this code: 06.04.2020
%  Last Update of this code: 06.04.2020
%  A newer version of this algorithm may be available. Please contact the author
%  or see http://www.ci.ovgu.de/Research/Codes.html.
%
% The files may have been modified in Feb 2021 by the authors of the Platemo framework to work with the Platemo 3.0 release.
% -----------------------------------------------------------------------
% This file is derived from its original version containied in the PlatEMO
% framework.
% -----------------------------------------------------------------------

    %% Parameter setting
    [ParticleDec,ParticleVel] = unpackDecAndVel(Particle.adds);
    [PbestDec,~] = unpackDecAndVel(Pbest.adds);
    [GbestDec,~] = unpackDecAndVel(Gbest.adds);
    [N,D]        = size(ParticleDec);

    %% Particle swarm optimization
    W  = repmat(unifrnd(0.1,0.5,N,1),1,D);
    r1 = repmat(rand(N,1),1,D);
    r2 = repmat(rand(N,1),1,D);
    C1 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    C2 = repmat(unifrnd(1.5,2.5,N,1),1,D);
    OffVel = W.*ParticleVel + C1.*r1.*(PbestDec-ParticleDec) + C2.*r2.*(GbestDec-ParticleDec);
    phi    = max(4,C1+C2);
    OffVel = OffVel.*2./abs(2-phi-sqrt(phi.^2-4*phi));
    delta  = repmat((xupper-xlower)/2,N,1);
    OffVel = max(min(OffVel,delta),-delta);
    OffDec = ParticleDec + OffVel;

    %% Deterministic back
    Lower  = repmat(xlower,N,1);
    Upper  = repmat(xupper,N,1);
    repair = OffDec < Lower | OffDec > Upper;
    OffVel(repair) = 0.001*OffVel(repair);
    OffDec = max(min(OffDec,Upper),Lower);

    %% Polynomial mutation
    disM  = 20;
    Site1 = repmat(rand(N,1)<0.15,1,D);
    Site2 = rand(N,D) < 1/D;
    mu    = rand(N,D);
    temp  = Site1 & Site2 & mu<=0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp  = Site1 & Site2 & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
end

function [positions, velocities] = unpackDecAndVel(pack)
   noOfSolutions = size(pack,1);
   P = arrayfun(@(K) pack(K).xDecs, 1:noOfSolutions, 'UniformOutput',0);
   positions = cell2mat(transpose(P));
   V = arrayfun(@(K) pack(K).xDecs, 1:noOfSolutions, 'UniformOutput',0);
   velocities = cell2mat(transpose(V));
end


function  h11=derotated_dtcwt(n,Yh)
%FUNCTION to perform a derotated DT_CWT

% n  -> No. of levels of wavelet decomposition
% Yh -> A cell array containing the 6 complex highpass subimages for
%  each level.
% h11 ->detotated Yh
h11{n}=Yh{n};
for k=n:-1:2
    for m=1:6
        xp=imresize(Yh{k}(:,:,m),2);%
        argxp=angle(xp);
        argx=angle(Yh{k-1}(:,:,m));
        argx=argx-2.*argxp;
        absx=abs(Yh{k-1}(:,:,m));
        xa=absx.*cos(argx);
        xb=absx.*sin(argx);
        h11{k-1}(:,:,m)=complex(xa,xb);
    end
end
%figure;
%cimage5(h11{1}(:,:,4));
%figure;
%cimage5(h1{1}(:,:,4));
%figure;
%cimage5(h11{2}(:,:,4));
%figure;
%cimage5(h1{2}(:,:,4));


%  plots prior and, if information is available, posterior densities
%
%  ::
%
%    [h,legend_]=prior_posterior(ss,varargin)
%
%  Args:
%
%     - **ss** [struct]: structure containing the relevant information to plot
%       and more specifically
%
%       - **x_kdens** [vector]: x-axis values for the posterior density
%       - **f_kdens** [vector]: y-axis values for the posterior density
%       - **x_prior** [vector]: x-axis values for the prior density
%       - **f_prior** [vector]: y-axis values for the prior density
%       - **mean_sim** [scalar]: mean of the posterior simulation
%       - **post_mode** [scalar]: value at the posterior maximization mode
%       - **post_mode_sim** [scalar]: value at the posterior simulation mode
%       - **tex_name** [char]: name of the parameter
%
%     - **varargin** [pairwise arguments]: standard plotting arguments for
%       matlab
%
%  Returns:
%     :
%
%     - **h** [handle]: handle for the plot
%     - **legend_** [cellstr]: names of the lines in the plot
%     - **tex_name** [char]: name of the parameter
%
%  Note:
%
%     - Only the prior density is plotted if no posterior information is
%       available.
%
%


function [yhat prdt_per_tree] = classRFPredict( x, rf )
    [nsample ndim] = size(x);
    ntree = rf.ntree;
    prdt_per_tree = zeros(nsample, ntree);

    ntree = rf.ntree;

    for ix = 1 : nsample
        for n = 1 : ntree
            k = 1;
            while rf.nodestatus{n}(k) ~= -1
                if x(ix, rf.bestvar{n}(k)) <= rf.bestsplit{n}(k)
                    k = rf.treemap{n}(k, 1);
                else
                    k = rf.treemap{n}(k, 2);
                end
            end
            prdt_per_tree(ix, n) = rf.nodeclass{n}(k);
            % fprintf( 'ix: %d, n: %d\n', ix, n );
        end
    end

    yhat = zeros(nsample, 1);
    for ix = 1 : nsample
        % for n = 1 : ntree
            % when there are equal votes for the classes, it favors the
            % first class
            yhat(ix) = mode( prdt_per_tree(ix, :) );
        % end
    end

    for ix = 1 : length(rf.new_labels)
        yhat( yhat == rf.new_labels(ix) ) = rf.orig_labels(ix);
    end
end


%% DEMO_additional_colormaps
% Below is a demonstration for:
%
% * Additional colormaps available in GIBBON

%%
clear; close all; clc;

%%
%Plot settings
fontSize=15;

% Create example data for visualizations
n=250;
s=1;
[X,Y]=ndgrid(linspace(-3*s,3*s,n));
Z=exp( -0.5.*((X./s).^2+(Y./s).^2));
Z=Z./max(Z(:));
Z(X<0)=-Z(X<0);
colorLim=[-1 1];
colormapset={'gjet','wjet','graygjet','grayjet',...
    'fire','ice','fireice','warmcold','iwarmcold','blood','bloodbone',...
    'che','wcbp','viridis','magma','inferno','turbo',...
    'plasma','gviridis','igviridis','kvw','cividis','spectral','ukr','flare'};

%% The gibbon color maps

%%
% Visualizing all additional color maps

n1=floor(sqrt(numel(colormapset)));
n2=ceil(numel(colormapset)/n1);

cFigure;

for q=1:1:numel(colormapset)

    h=subplot(n1,n2,q); hold on;
    title(colormapset{q},'FontSize',fontSize);
    imagesc(Z);
    colormap(h,colormapset{q}); colorbar;
    axis tight; axis equal; axis xy; box on;
    axis off;
    set(h,'FontSize',fontSize);
    clim(colorLim);
end
drawnow;

%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function pcircles(fhdl, center, radius, varargin)
%
% PCIRCLE(fhdl, center, radius, varargin)
% A simple function for drawing a circle on the selected figure using
% polylines.
%
% INPUT:
% fhdl:         figure handler
% center:       center of the circle in [x0 y0] format
% radius:       radius of the circle (in pixels)
% varargin:     colors, plot symbols and line types

format = '-w';  % default
if nargin == 4
    format = varargin{1};
end

x0 = center(1);
y0 = center(2);
nseg = ceil(2*pi*radius);
theta = 0 : (2 * pi / nseg) : (2 * pi);
x = radius * cos(theta) + x0;
y = radius * sin(theta) + y0;

figure(fhdl), hold;
plot(x0, y0, '.r', 'LineWidth',5);
plot(x, y, format);


function Z = or(X,Y)
%OR Logical OR (|) for tensors.
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

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

Z = tenfun(@or,X,Y);


function y = linearize( x )

% LINEARIZE    Linearize.
%    For real affine X, Y = X.
%    For convex X, Y is linear, and satisfies Y >= X.
%    For concave X, Y is linear, and satisfies Y <= X.
% This is used primarily within CVX functions to efficiently implement
% certain monotonic functions.

persistent P
if isempty( P ),
    P.map = cvx_remap( { 'r_affine' }, ...
        { 'convex' }, { 'concave' }, [2,3,4] );
    P.funcs = { [], @lin_affn, @lin_cnvx, @lin_cncv };
end
y = cvx_unary_op( P, x );

function y = lin_affn( x )
y = x;

function y = lin_cnvx( x ) %#ok
cvx_begin set
    variable y(size(x))
    x <= y; %#ok
cvx_end

function y = lin_cncv( x ) %#ok
cvx_begin set
    variable y(size(x))
    x >= y; %#ok
cvx_end

% Copyright 2005-2014 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function [loss, fchg, tchg, dloss_dV, dchg_dVm] = get_losses(baseMVA, bus, branch)
%GET_LOSSES   Returns series losses (and reactive injections) per branch.
%
%   LOSS = GET_LOSSES(RESULTS)
%   LOSS = GET_LOSSES(BASEMVA, BUS, BRANCH)
%
%   [LOSS, CHG] = GET_LOSSES(RESULTS)
%   [LOSS, FCHG, TCHG] = GET_LOSSES(RESULTS)
%   [LOSS, FCHG, TCHG, DLOSS_DV] = GET_LOSSES(RESULTS)
%   [LOSS, FCHG, TCHG, DLOSS_DV, DCHG_DVM] = GET_LOSSES(RESULTS)
%
%   Computes branch series losses, and optionally reactive injections from
%   line charging, as functions of bus voltages and branch parameters, using the
%   following formulae:
%
%       loss = abs( Vf / tau - Vt ) ^ 2 / (Rs - j Xs)
%       fchg = abs( Vf / tau ) ^ 2 * Bc / 2
%       tchg = abs( Vt ) ^ 2 * Bc / 2
%
%   Optionally, computes the partial derivatives of the line losses with
%   respect to voltage angles and magnitudes.
%
%   Input:
%       RESULTS - a MATPOWER case struct with bus voltages corresponding to
%                 a valid power flow solution.
%                 (Can optionally be specified as individual fields BASEMVA,
%                  BUS, and BRANCH.)
%
%   Output(s):
%       LOSS - complex NL x 1 vector of losses (in MW), where NL is the number
%              of branches in the system, representing only the losses in the
%              series impedance element of the PI model for each branch.
%       CHG -  NL x 1 vector of total reactive injection for each line
%              (in MVAr), representing the line charging injections of both
%              of the shunt elements of PI model for each branch.
%       FCHG - Same as CHG, but for the element at the "from" end of the
%              branch only.
%       TCHG - Same as CHG, but for the element at the "to" end of the branch.
%       DLOSS_DV - Struct with partial derivatives of LOSS with respect to bus
%              voltages, with fields:
%           .a  - Partial with respect to bus voltage angles.
%           .m  - Partial with respect to bus voltage magnitudes.
%       DCHG_DVM - Struct with partial derivatives of FCHG and TCHG with
%              respect to bus voltage magnitudes, with fields:
%           .f  - Partial of FCHG with respect to bus voltage magnitudes.
%           .t  - Partial of TCHG with respect to bus voltage magnitudes.
%
%   Example:
%       results = runpf(mycase);
%       [loss, chg] = get_losses(results);
%       total_system_real_losses = sum(real(loss));
%       total_system_reac_losses = sum(imag(loss)) - sum(chg);
%
%       [loss, fchg, tchg, dloss_dV] = get_losses(results);

%   MATPOWER
%   Copyright (c) 1996-2017, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

%% default arguments
if isstruct(baseMVA)
    mpc = baseMVA;
    [baseMVA, bus, branch] = deal(mpc.baseMVA, mpc.bus, mpc.branch);
end

%% define named indices into bus, gen, branch matrices
[PQ, PV, REF, NONE, BUS_I, BUS_TYPE, PD, QD, GS, BS, BUS_AREA, VM, ...
    VA, BASE_KV, ZONE, VMAX, VMIN, LAM_P, LAM_Q, MU_VMAX, MU_VMIN] = idx_bus;
[F_BUS, T_BUS, BR_R, BR_X, BR_B, RATE_A, RATE_B, RATE_C, ...
    TAP, SHIFT, BR_STATUS, PF, QF, PT, QT, MU_SF, MU_ST, ...
    ANGMIN, ANGMAX, MU_ANGMIN, MU_ANGMAX] = idx_brch;

%% create map of external bus numbers to bus indices
i2e = bus(:, BUS_I);
e2i = sparse(max(i2e), 1);
e2i(i2e) = (1:size(bus, 1))';
out = find(branch(:, BR_STATUS) == 0);          %% out-of-service branches

%% sizes of things
nb = size(bus, 1);      %% number of buses
nl = size(branch, 1);   %% number of branches

%% construct complex bus voltage vector
V = bus(:, VM) .* exp(1j * pi/180 * bus(:, VA));

%% parameters
Cf = sparse(1:nl, e2i(branch(:, F_BUS)), branch(:, BR_STATUS), nl, nb);
Ct = sparse(1:nl, e2i(branch(:, T_BUS)), branch(:, BR_STATUS), nl, nb);
tap = ones(nl, 1);                              %% default tap ratio = 1 for lines
xfmr = find(branch(:, TAP));                    %% indices of transformers
tap(xfmr) = branch(xfmr, TAP);                  %% include transformer tap ratios
tap = tap .* exp(1j*pi/180 * branch(:, SHIFT)); %% add phase shifters
A = spdiags(1 ./ tap, 0, nl, nl) * Cf - Ct;
Ysc = 1 ./ (branch(:, BR_R) - 1j * branch(:, BR_X));
Vdrop = A * V;      %% vector of voltage drop across series impedance element
loss = baseMVA * Ysc .* Vdrop .* conj(Vdrop);
% loss = baseMVA * abs(V(e2i(branch(:, F_BUS))) ./ tap - V(e2i(branch(:, T_BUS)))) .^ 2 ./ ...
%             (branch(:, BR_R) - 1j * branch(:, BR_X));
% loss(out) = 0;

if nargout > 1
    Vf = Cf * V;
    Vt = Ct * V;
    fchg = real(baseMVA / 2 * branch(:, BR_B) .* Vf .* conj(Vf) ./ (tap .* conj(tap)));
    tchg = real(baseMVA / 2 * branch(:, BR_B) .* Vt .* conj(Vt));
%     fchg = abs(V(e2i(branch(:, F_BUS))) ./ tap) .^ 2 .* branch(:, BR_B) * baseMVA / 2;
%     tchg = abs(V(e2i(branch(:, T_BUS)))       ) .^ 2 .* branch(:, BR_B) * baseMVA / 2;
    fchg(out) = 0;
    tchg(out) = 0;

    if nargout == 2
        fchg = fchg + tchg;
    end
end

if nargout > 3
    B = spdiags(A * V, 0, nl, nl) * conj(A) * spdiags(conj(V), 0, nb, nb);
    dYsc = spdiags(Ysc, 0, nl, nl);
    dloss_dV = struct(...
        'a', -1j * baseMVA * dYsc * (B - conj(B)), ...
        'm',       baseMVA * dYsc * (B + conj(B)) * spdiags(1 ./ abs(V), 0, nb, nb) ...
    );
    if nargout > 4
        Bc = spdiags(branch(:, BR_B), 0, nl, nl);
        tt = spdiags(1 ./ (tap .* conj(tap)), 0, nl, nl);
        dchg_dVm = struct(...
            'f', baseMVA * Bc * tt * spdiags(Cf * bus(:, VM), 0, nl, nl) * Cf, ...
            't', baseMVA * Bc      * spdiags(Ct * bus(:, VM), 0, nl, nl) * Ct ...
        );
    end
end


function i4vec_sorted_unique_test (  )

%*****************************************************************************80
%
%% I4VEC_SORTED_UNIQUE_TEST tests I4VEC_SORTED_UNIQUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  n = 20;
  b = 0;
  c = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'I4VEC_SORTED_UNIQUE_TEST\n' );
  fprintf ( 1, '  I4VEC_SORTED_UNIQUE finds unique entries in a sorted array.\n' );

  seed = 123456789;

  [ a, seed ] = i4vec_uniform_ab ( n, b, c, seed );

  a = i4vec_sort_heap_a ( n, a );

  i4vec_print ( n, a, '  Input vector:' );

  [ unique_num, a_unique ] = i4vec_sorted_unique ( n, a );

  i4vec_print ( unique_num, a_unique, '  Unique entries:' );

  return
end


function ap_multi = compute_AP_multiCam(good_image, junk_image, index, queryCam, testCam)
good_cam = testCam(good_image);
good_cam_uni = unique(good_cam);
ap_multi = zeros(1, 6);

% on the same camera
good_cam_now = queryCam;
ngood = length(junk_image);
junk_image_now = good_image;
good_image_now = junk_image;
old_recall = 0;
old_precision = 1.0;
ap = 0;
intersect_size = 0;
j = 0;
good_now = 0;
for n = 1:length(index)
    flag = 0;
    if ~isempty(find(good_image_now == index(n), 1))
        flag = 1; % good image
        good_now = good_now+1;
    end
    if ~isempty(find(junk_image_now == index(n), 1))
        continue; % junk image
    end

    if flag == 1%good
        intersect_size = intersect_size + 1;
    end
    if ngood == 0
        ap_multi(good_cam_now) = 0;
        break;
    end
    recall = intersect_size/ngood;
    precision = intersect_size/(j + 1);
    ap = ap + (recall - old_recall)*((old_precision+precision)/2);
    old_recall = recall;
    old_precision = precision;
    j = j+1;

    if good_now == ngood
        ap_multi(good_cam_now) = ap;
        break;
    end
end

for k = 1:length(good_cam_uni)
    good_cam_now = good_cam_uni(k);
    ngood = length(find(good_cam == good_cam_now));
    pos_junk = find(good_cam ~= good_cam_now);
    junk_image_now = [junk_image good_image(pos_junk)];
    pos_good = find(good_cam == good_cam_now);
    good_image_now = good_image(pos_good);
    old_recall = 0;
    old_precision = 1.0;
    ap = 0;
    intersect_size = 0;
    j = 0;
    good_now = 0;
    for n = 1:length(index)
        flag = 0;
        if ~isempty(find(good_image_now == index(n), 1))
            flag = 1; % good image
            good_now = good_now+1;
        end
        if ~isempty(find(junk_image_now == index(n), 1))
            continue; % junk image
        end

        if flag == 1%good
            intersect_size = intersect_size + 1;
        end
        recall = intersect_size/ngood;
        precision = intersect_size/(j + 1);
        ap = ap + (recall - old_recall)*((old_precision+precision)/2);
        old_recall = recall;
        old_precision = precision;
        j = j+1;

        if good_now == ngood
            ap_multi(good_cam_now) = ap;
            break;
        end
    end
end

end


function [ ave_recall, recall,  thresholds] = compute_ave_recall_of_bbox( bbox_pred, bbox_gt, thresholds )

if ~exist('thresholds','var') || isempty(thresholds)
    thresholds = 0.5:0.05:0.95;
end

if iscell(bbox_pred)
    assert(iscell(bbox_gt));
    assert(length(bbox_pred) == length(bbox_gt));
    num_imgs = length(bbox_pred);
    overlap  = cell(num_imgs,1);
    for i = 1:num_imgs
        bbox_pred_this = bbox_pred{i};
        bbox_gt_this   = bbox_gt{i};
        num_bbox_gt    = size(bbox_gt_this,1);
        num_bbox_pred  = size(bbox_pred_this,1);
        overlap{i}      = zeros(num_bbox_gt,1,'single');

        if num_bbox_gt && num_bbox_pred
            [overlap{i}, ~] = closest_candidates(bbox_gt_this(:,1:4), bbox_pred_this(:,1:4));
            overlap{i} = single(overlap{i});
%             overlap{i} = max(boxoverlap(bbox_pred_this(:,1:4), bbox_gt_this(:,1:4)),[],1)';
        end
    end
    overlap = cell2mat(overlap);
else
    assert(size(bbox_pred,1) == size(bbox_gt,1));
    assert(size(bbox_pred,2) == size(bbox_gt,2));
    assert(size(bbox_pred,2) == 4);
    overlap = boxoverlap(bbox_pred, bbox_gt, true);
    assert(size(bbox_pred,1) == size(overlap,1));
    assert(size(overlap,2)==1);
end

[thresholds, recall, ave_recall] = compute_average_recall(overlap);

end

function [overlap, recall, AR] = compute_average_recall(unsorted_overlaps)
all_overlaps = sort(unsorted_overlaps(:)', 'ascend');
num_pos = numel(all_overlaps);
dx = 0.001;

overlap = 0:dx:1;
overlap(end) = 1;
recall = zeros(length(overlap), 1);
for i = 1:length(overlap)
recall(i) = sum(all_overlaps >= overlap(i)) / (num_pos+eps);
end

good_recall = recall(overlap >= 0.5);
AR = 2 * dx * trapz(good_recall);

if num_pos == 0
    AR = 0;
end
end

function [best_overlap,best_boxes] = closest_candidates(gt_boxes, candidates)
% do a matching between gt_boxes and candidates

  num_gt_boxes = size(gt_boxes, 1);
  num_candidates = size(candidates, 1);

%   iou_matrix = zeros(num_gt_boxes, num_candidates);
  iou_matrix = boxoverlap(candidates, gt_boxes)';
%   for i = 1:num_gt_boxes
%     iou = boxoverlap(gt_boxes(i,:), candidates);
%     iou_matrix(i,:) = iou';
%   end

  best_overlap = zeros(num_gt_boxes, 1);
  best_boxes = -ones(num_gt_boxes, 4);

  [best_overlap,best_boxes] = greedy_matching(iou_matrix, gt_boxes, candidates);
end

function [best_overlap,best_boxes] = greedy_matching(iou_matrix, gt_boxes, candidates)
  [n, m] = size(iou_matrix);
  assert(n == size(gt_boxes, 1));
  assert(m == size(candidates, 1));
  if n > m
    gt_matching = greedy_matching_rowwise(iou_matrix');
    candidate_matching = (1:m)';
  else
    gt_matching = (1:n)';
    candidate_matching = greedy_matching_rowwise(iou_matrix);
  end

  best_overlap = zeros(n, 1);
  best_boxes = zeros(n, 4);
  for pair_idx = 1:numel(gt_matching)
    gt_idx = gt_matching(pair_idx);
    candidate_idx = candidate_matching(pair_idx);

    best_overlap(gt_idx) = iou_matrix(gt_idx, candidate_idx);
    best_boxes(gt_idx,:) = candidates(candidate_idx, :);
  end
end

function [matching, objective] = greedy_matching_rowwise(iou_matrix)
  assert(size(iou_matrix, 1) <= size(iou_matrix, 2));
  n = size(iou_matrix, 1);
  matching = zeros(n, 1);
  objective = 0;
  for i = 1:n
    % find max element int matrix
    [max_per_row, max_col_per_row] = max(iou_matrix, [], 2);
    [max_iou,row] = max(max_per_row);
    if max_iou == -inf
      break
    end

    objective = objective + max_iou;
    col = max_col_per_row(row);
    matching(row) = col;
    iou_matrix(row,:) = -inf;
    iou_matrix(:,col) = -inf;
  end
end


%% Demo for semi-supervised learning
% Copyright: Andreas Damianou 2015

%% id of differerent random trials
allTrials = 1:2;
saveDir = './results/semiSupervised_master/';

%%
for trial = allTrials
    % For each trial, try different number of observed outputs
    for NtrObs = [10:10:160]
        keep('saveDir','allTrials','trial','NtrObs','res');
        % Different trials should have different seeds
        curSeed = trial + 1000;
        randn('seed', curSeed);
        rand('seed', curSeed);

        dataSetName = 'oil';
        experimentNo = curSeed;
        % This is for when we bootstrap: we try to populate the output
        % labelled space as follows: if Y_i is in the labelled set,
        % (i.e. labels_i exists), then we find this point's embeding, q(x_i) ~
        % N(mu_i, S_i), from which we sample samplesPerObserved points
        % x_samp_i,k, k=1,..., samplesPerObserved and form the pairs
        % {x_samp_i,k, labels_i}, for all k. Here we try two different
        % numbers for samplesPerObserved (a small and a big number).
        samplesPerObserved3 = 6;
        samplesPerObserved4 = 50;

        % Number of iterations for initialization of the model (helps
        % avoiding early local minima)
        initVardistIters = 500;
        % Number of optimization iterations
        itNo = [1200 1000];

        % Load data
        [Y, lbls] = lvmLoadData(dataSetName);
        labels = transformLabels(lbls)';

        % Use Nts test points randomly from the data set. Use the rest
        % NtrObs as training points.
        Nts = 700;
        perm = randperm(size(Y,1));
        ts = perm(1:Nts); t = Nts;
        trObs = perm(t+1:t+NtrObs); t = t+NtrObs;
        trMis = perm(t+1:end);

        %%%%% ATTENTION!!!! For compatibility with the rest of the code,
        %%%%% the notation here differs than from the paper:
        %%%%% Y here is Z in paper
        %%%%% labels here is Y in paper
        % SO:
        % YtrObs: Training points in the labelled set (observed and we also not know their corresponding labelsObs
        % YtrMis: Training points in the unlabelled set (observed, but we do not know their labels labelsMis (we have the below but not show them to the model))
        % Yts:    Test points (we'll infer their corresponding labels labelsTs at test time)
        YtrObs = Y(trObs,:); lblsObs = lbls(trObs, :); labelsObs = labels(trObs,:);
        YtrMis = Y(trMis,:); lblsMis = lbls(trMis, :); labelsMis = labels(trMis,:);
        Yts = Y(ts,:); lblsTs = lbls(ts, :);  labelsTs = labels(ts,:);

        % Options for bgplvm
        options = vargplvmOptions('dtcvar');
        options.kern = 'rbfardjit';
        options.numActive = min(25, size(YtrObs,1)); % number of ind. points
        options.latentDim = 8; % number of latent dimensions
        options.optimiser = 'scg2';
        options.initSNR = 100; % Initial signal to noise ratio

        %----- BGPLVM embed only from the labelled data (no semi-supervised learning here)
        % X is the embedding of only labelled data
        [X, sigma2, W, ~, modelInitVardist] = vargplvmEmbed(YtrObs, options.latentDim, options, initVardistIters, 0 );
        modelInitVardist.globalOpt.experimentNo = experimentNo;
        modelInitVardist.throwSNRError = false;
        model = vargplvmOptimiseModel(modelInitVardist, 1,1, {[], itNo}, 1);
        SNR = vargplvmShowSNR(model,false);
        [x_star_all, varx_star_all, mini] = vargplvmPredictLatent(model, Yts, [], false, 1000, false);

        %----- BGPLVM embed from the labelled and unlabelled data (semi-supervised learning)
        % X2 is a riccher embedding because it is learned from more data
        [X2, sigma22, W2, ~, modelInitVardist2] = vargplvmEmbed([YtrObs; YtrMis], options.latentDim, options, initVardistIters, 0 );
        modelInitVardist2.globalOpt.experimentNo = experimentNo;
        modelInitVardist2.throwSNRError = false;
        model2 = vargplvmOptimiseModel(modelInitVardist2, 1,1, {[], itNo}, 1);
        SNR2 = vargplvmShowSNR(model2,false);
        [x_star_all2, varx_star_all2, mini2] = vargplvmPredictLatent(model2, Yts, [], false, 1000, false);

        %----  X3 is an even richer embedding, because it is a superset of X2:
        % it starts as X2, but for every point in X2 which has a known
        % label we sample extra points and assign that label. So, X3 is a
        % larger set.
        % First copy X3 = X2 (later we'll augment it)
        X3 = model2.X(1:length(trObs),:);
        % We'll need the variances too in order to sample
        varX3 = model2.vardist.covars(1:length(trObs),:);
        % Copy the labels
        labels3 = labelsObs;
        % Xnew, labelsNew is initialized in size
        Xnew = nan(size(X3,1)*samplesPerObserved3, size(X3,2));
        labelsNew = nan(size(Xnew,1), size(labelsObs,2));
        k=1;
        % Now for each labelled point in the embedding we sample more
        % points
        for n=1:size(X3,1) % At this point X3 == X2
            for kk = 1:samplesPerObserved3
                Xnew(k,:) = X3(n,:) + randn(size(X3(n,:))).*sqrt(varX3(n,:));
                labelsNew(k,:) = labels3(n,:);
                k = k + 1;
            end
        end
        X3 = [X3; Xnew];
        labels3 = [labels3; labelsNew];
        clear 'Xnew' 'labelsNew';

        %---------- As above but obtain many more samples ----------------

        %samplesPerObserved4 = min(samplesPerObserved4, floor(10000/NtrObs));
        X4 = model2.X(1:length(trObs),:);
        varX4 = model2.vardist.covars(1:length(trObs),:);
        labels4 = labelsObs;
        Xnew = nan(size(X4,1)*samplesPerObserved4, size(X4,2));
        labelsNew = nan(size(Xnew,1), size(labelsObs,2));
        k=1;
        for n=1:size(X4,1)
            for kk = 1:samplesPerObserved4
                Xnew(k,:) = X4(n,:) + randn(size(X4(n,:))).*sqrt(varX4(n,:));
                labelsNew(k,:) = labels4(n,:);
                k = k + 1;
            end
        end
        X4 = [X4; Xnew];
        labels4 = [labels4; labelsNew];
        clear 'Xnew' 'labelsNew';

        %% %%%%%%%%%%%%%%%%%%% CLASSIFICATION STEP: %%%%%%%%%%%%%%%%
        % Now we have all 4 embeddings / models. Recap:
        % - Model 1: Embedding only from the labelled data
        % - model 2: Embedding from the labelled and unlabelled data
        % - model 3: As above but bootstrap
        % - model 4: As above but bootstrap more points
        %
        % Given these embeddings, we'll train classifiers from the embedded
        % point to the label space.
        %
        %--------- Train classifiers from X to Z
        %
        %---- Model 1
        % Training of logistic regression classifier (replace with any
        % classifier you want). Do this for each of the embeddings.
        LogRegError = util_fitLogReg(model.X, labelsObs, x_star_all, lblsTs);
        LogRegError2 = util_fitLogReg(model2.X(1:length(trObs),:), labelsObs, x_star_all2, lblsTs);
        LogRegError3 = util_fitLogReg(X3, labels3, x_star_all2, lblsTs);
        LogRegError4 = util_fitLogReg(X4, labels4, x_star_all2, lblsTs);
        % We can also classify not from the embeddings to the labels, but
        % directly from the points. However, when the dimensionality of
        % these points is too big, the following method will automatially
        % do a PCA embedding. Notice that the directly classifying from the
        % points might be better than going through an embedding, if tjhe
        % points' dimensionality is small or the data is relatively
        % simple-structured.
        LogRegErrorOutputs = util_fitLogReg(YtrObs, labelsObs, Yts, lblsTs);

        %% -------------- Save results in a structure 'res'
        tmp=['tr' num2str(trial) '_NtrObs' num2str(NtrObs)];
        res.(tmp).seed = curSeed;
        res.(tmp).LogRegError = LogRegError;
        res.(tmp).LogRegError2 = LogRegError2;
        res.(tmp).LogRegError3 = LogRegError3;
        res.(tmp).LogRegError4 = LogRegError4;
        res.(tmp).LogRegErrorOutputs = LogRegErrorOutputs;
        res.(tmp).samplesPerObserved3 = samplesPerObserved3;
        res.(tmp).samplesPerObserved4 = samplesPerObserved4;
        res.(tmp).trObs = trObs;
        res.(tmp).ts = ts;
        res.(tmp).trMis = trMis;
        res.(tmp).W = W;
        res.(tmp).W2 = W2;
        res.(tmp).SNR = SNR;
        res.(tmp).SNR2 = SNR2;
        diary(['LOG_demOilSemiSup4.txt']);
        res.(tmp)
        diary off
        save([saveDir 'demOilSemiSup4.mat'], 'res');
    end
end

%% ------------ PLOTS ------------------
close all
cd(saveDir);
ll = [];
lw = 1.7;
ms = 12;
fs = 40;
errorBars = true;
stdMult = 0.8; % Smaller error bar to be in 0

resAll = [];
doneTrials = [];
try
    load(['demOilSemiSup4.mat']);
    ff = fieldnames(res);
    for jj=1:length(ff)
        if isfield(resAll, ff{jj})
            warning('field already exists')
        end
        resAll.(ff{jj}) = res.(ff{jj});
        try
            doneTrials(end+1) = str2num(ff{jj}(3:find(ff{1}=='_')-1));
        catch ee
            error('ll')
        end
    end
catch e
    warning(['Did not find demOilSemiSup4.mat'])
end
doneTrials = unique(doneTrials);

%%% ATTENTION: Reject trials with low SNR (or repeat them with more
%%% initiVardistIters).
% If embedding results in low SNR (i.e. close to 1), then it has to be rejected. There's no
% automatic check for this now, so you can use the next array manually.
excludeTrials = [];
allTrials = setdiff(doneTrials, excludeTrials);
allNtrObs = [10:10:160];
logRegErrorMatrix = NaN(length(allTrials), length(allNtrObs));
logRegErrorSemiSupMatrix = logRegErrorMatrix;
logRegErrorSemiSupMatrix3 = logRegErrorMatrix;
logRegErrorSemiSupMatrix4 = logRegErrorMatrix;
weightKeptMatrix = logRegErrorMatrix;
weigthKeptSemiSupMatrix = logRegErrorMatrix;
SNRMatrix = logRegErrorMatrix;
SNRSemiSupMatrix = logRegErrorMatrix;

thresh = 0.0002;
%

for i=1:length(allTrials)
    for j=1:length(allNtrObs)
        trial = allTrials(i);
        NtrObs = allNtrObs(j);
        try
            logRegErrorMatrix(i, j) = resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).LogRegError;
            logRegErrorSemiSupMatrix(i, j) = resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).LogRegError2;
            logRegErrorSemiSupMatrix3(i, j) = resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).LogRegError3;
            logRegErrorSemiSupMatrix4(i, j) = resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).LogRegError4;
            weightKeptMatrix(i, j) = sum(resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).W > thresh);
            weigthKeptSemiSupMatrix(i, j) = sum(resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).W2 > thresh);
            SNRMatrix(i, j) = resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).SNR;
            SNRSemiSupMatrix(i, j) = resAll.(['tr' num2str(trial) '_NtrObs' num2str(NtrObs)]).SNR2;
        catch e
            warning(['Could not find tr' num2str(trial) '_NtrObs' num2str(NtrObs)])
        end
    end
end

inds = 1:size(logRegErrorMatrix,2);

meanlogRegErrorMatrix = nanmean(logRegErrorMatrix(:,inds));
meanlogRegErrorSemiSupMatrix = nanmean(logRegErrorSemiSupMatrix(:,inds));
meanlogRegErrorSemiSupMatrix3 = nanmean(logRegErrorSemiSupMatrix3(:,inds));
meanlogRegErrorSemiSupMatrix4 = nanmean(logRegErrorSemiSupMatrix4(:,inds));
if errorBars
    ll(end+1)=errorbar(allNtrObs(inds), meanlogRegErrorMatrix, stdMult*nanstd(logRegErrorMatrix(:,inds)), 'x-', 'LineWidth', lw, 'MarkerSize', ms); hold on;
    ll(end+1)=errorbar(allNtrObs(inds), meanlogRegErrorSemiSupMatrix, stdMult*nanstd(logRegErrorSemiSupMatrix(:,inds)), 'ro-', 'LineWidth', lw, 'MarkerSize', ms);
    ll(end+1)=errorbar(allNtrObs(inds), meanlogRegErrorSemiSupMatrix3, stdMult*nanstd(logRegErrorSemiSupMatrix3(:,inds)), 'g+-', 'LineWidth', lw, 'MarkerSize', ms);
    ll(end+1)=errorbar(allNtrObs(inds), meanlogRegErrorSemiSupMatrix4, stdMult*nanstd(logRegErrorSemiSupMatrix4(:,inds)), 'ks-', 'LineWidth', lw, 'MarkerSize', ms);
    xlabel('# Observed')
    ylabel('# Errors')
    legend('Without Z^u','With Z^u', 'With Samples', 'With More Samples');
    xlim([17,163])
    pp = ylim;
    ylim([-20 pp(2)]);
else
    plot(allNtrObs(inds), meanlogRegErrorMatrix, 'x-'); hold on; title('# Errors')
    plot(allNtrObs(inds), meanlogRegErrorSemiSupMatrix, 'ro-');
    plot(allNtrObs(inds), meanlogRegErrorSemiSupMatrix3, 'g+-');  legend('GP','Semi-supervised GP', 'SS-GP with Samples');
    plot(allNtrObs(inds), meanlogRegErrorSemiSupMatrix4, 'ks-');  legend('GP','Semi-supervised GP', 'SS-GP with More Samples');
end

% figure
% plot(allNtrObs(inds), nanmean(weightKeptMatrix(:,inds)), 'x-'); hold on; title(['Weights kept (threshold=' num2str(thresh) ')'])
% plot(allNtrObs(inds), nanmean(weigthKeptSemiSupMatrix(:,inds)), 'ro-');  legend('GP','Semi-supervised GP');
%
% figure
% plot(allNtrObs(inds), nanmean(SNRMatrix(:,inds)), 'x-'); hold on; title('SNR')
% plot(allNtrObs(inds), nanmean(SNRSemiSupMatrix(:,inds)), 'ro-');  legend('GP','Semi-supervised GP');
%


function mask=getMask(l,h,w,batchS,depth,posTempX,posTempY)
mask=abs(posTempX-repmat(l.mu_x,[h,w,1]));
mask=mask+abs(posTempY-repmat(l.mu_y,[h,w,1]));
mask=max(1-mask.*repmat(reshape(l.weights{3},[1,1,depth]),[h,w,1,batchS]),-1);
mask(:,:,l.filter~=1,:)=1;
end


function F = minus (F,w)
%MINUS downdate a dense Cholesky factorization
%
% Example
%   % F becomes the Cholesky factorization of A-w*w'
%   F = factorize (A) ;
%   w = rand (size (A,1),1) ;
%   F = F - w ;
%   x = F\b ;               % computes x = (A-w*w')\b
%
% See also factorize, cholupdate.

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

if (F.kind ~= 6)
    error ('Only dense Cholesky factorization downdate supported.') ;
end

F.R = cholupdate (F.R, w, '-') ;
F.A = F.A - w*w' ;


function g = findBestGammaAsCRF(img0, img1, exposure0, exposure1)
%
%       g = findBestGammaAsCRF(img0, img1, exposure0, exposure1)
%
%       This function computes the best gamma paramters for RGB that approximate
%       the camera response function.
%
%        Input:
%           -img0: an SDR image
%           -img1: an SDR image
%           -exposure0: the shutter speed of img0
%           -exposure1: the shutter speed of img1
%
%        Output:
%           -lin_fun: the inverse CRF
%           -max_lin_fun: maximum value of the inverse CRF
%
%     Copyright (C) 2014-15  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

delta_exposure = exposure1 / exposure0;
mask = ones(size(img0));
mask(img0>0.95) = 0.0;
mask(img1>0.95) = 0.0;
mask(img0<0.05) = 0.0;
mask(img1<0.05) = 0.0;

    function err = residualFunction(p)
        err = 0.0;
        for i=1:3
            gt = p(i);
            img0_lin(:,:,i) = (img0(:,:,i).^gt);
            img0_re(:,:,i) = img0_lin(:,:,i) * delta_exposure;
            img0_re(:,:,i) = img0_re(:,:,i).^(1.0/gt);

            delta = abs(img0_re(:,:,i) - img1(:,:,i)) .* mask(:,:,i);
            err = err + mean(delta(:));
        end
    end

    opts = optimset('Display', 'iter', 'TolFun', 1e-12, 'TolX', 1e-12, 'MaxIter', 1000);
    gi = 2.2 * ones(1,3);
    g = fminsearch(@residualFunction, gi, opts);
    for i=1:3
        img0_lin(:,:,i) = (img0(:,:,i).^g(i));
        img0_re(:,:,i) = img0_lin(:,:,i) * delta_exposure;
        img0_re(:,:,i) = img0_re(:,:,i).^(1.0/g(i));
    end

    imshow([img0, img0_re, img1]);

end