function [model] = USRtrain(fea, options)
% USRtrain: Training Unsupervised Spectral Regression Model
%
%       [model] = USRtrain(fea, options)
%
%             Input:
%
%               fea     - data matrix. Each row is a data point.
%           options     - Struct value in Matlab. The fields in options
%                         that can be set:
%
%                      W       -  Affinity matrix. You can either call
%                                 "constructW" to construct the W, or
%                                 construct it by yourself.
%                                 If W is not provided, USRtrain will
%                                 build a k-NN graph with Heat kernel
%                                 weight, where k is a prameter.
%
%                      k       -  The parameter for k-NN graph (Default is 5)
%                                 If W is provided, this k will be ignored.
%
%                  ReducedDim  -  The number of reduced dimensions.
%                                 Default ReducedDim = 30.
%
%                  ReguType    -  'Ridge': L2-norm regularizer (default)
%                                 'Lasso': L1-norm regularizer
%                  ReguAlpha   -  regularization paramter for L2-norm regularizer
%                                 Default 0.1
%                  ReguGamma   -  regularization paramter for L1-norm regularizer
%                                 Default 0.05
%                  LASSOway    -  'LARs': use LARs to solve the LASSO
%                                 problem. You need to specify the
%                                 cardinality requirement in LassoCardi.
%                                 'SLEP': use SLEP to solve the LASSO
%                                 problem. Please see http://www.public.asu.edu/~jye02/Software/SLEP/
%                                  for details on SLEP. (The Default)
%
%                 bCenter = 0 | 1  whether to center the data. (In some
%                                  cases, e.g., text categorization, The
%                                  data is very spase, centering the data
%                                  will destroy the sparsity and consume
%                                  too much memory. In this case, bCenter
%                                  should be set to 0)
%                                   Default: 1
%
%
%             Output:
%               model   -  used for USRtest.m
%
%
%    Examples:
%
%
%
% See also SR, SR_caller
%
%Reference:
%
%   [1] Deng Cai, Xiaofei He, Wei Vivian Zhang, Jiawei Han, "Regularized
%   Locality Preserving Indexing via Spectral Regression", Proc. 2007 ACM
%   Int. Conf. on Information and Knowledge Management (CIKM'07), Lisboa,
%   Portugal, Nov. 2007.
%
%   [2] Deng Cai, "Spectral Regression: A Regression Framework for
%   Efficient Regularized Subspace Learning", PhD Thesis, Department of
%   Computer Science, UIUC, 2009.
%
%   version 3.0 --Jan/2012
%   version 2.0 --December/2011
%   version 1.0 --May/2006
%
%   Written by Deng Cai (dengcai AT gmail.com)
%

if ~exist('options','var')
    options = [];
end

bCenter = 1;
if isfield(options,'bCenter')
    bCenter = options.bCenter;
end

k = 5;
if isfield(options,'k')
    k = options.k;
end

ReducedDim = 30;
if isfield(options,'ReducedDim')
    ReducedDim = options.ReducedDim;
end

if ~isfield(options,'ReguType')
    options.ReguType = 'Ridge';
end

LARs = false;
switch lower(options.ReguType)
    case {lower('Ridge')}
        if ~isfield(options,'ReguAlpha')
            options.ReguAlpha = 0.1;
        end
    case {lower('Lasso')}
        if isfield(options,'ReguAlpha') && options.ReguAlpha > 0
            options.RidgeAlpha = options.ReguAlpha;
            options.ReguType = 'RidgeLasso';
        end
        if isfield(options,'ReguGamma')
            options.ReguAlpha = options.ReguGamma;
        else
            options.ReguAlpha = 0.05;
        end
        if ~isfield(options,'LASSOway')
            options.LASSOway = 'SLEP';
        end

        if strcmpi(options.LASSOway,'LARs')
            LARs = true;
            if ~isfield(options,'LassoCardi')
                options.LassoCardi = 10:10:50;
            end
        end
    otherwise
        error('ReguType does not exist!');
end

nSmp=size(fea,1);
% Graph construction
if isfield(options,'W')
    W = options.W;
else
    Woptions.k = k;
    if nSmp > 3000
        tmpD = EuDist2(fea(randsample(nSmp,3000),:));
    else
        tmpD = EuDist2(fea);
    end
    Woptions.t = mean(mean(tmpD));
    W = constructW(fea,Woptions);
end

% Respnse generation
Y = Eigenmap(W,ReducedDim);

% Projection learning
if bCenter
    sampleMean = mean(fea);
    fea = (fea - repmat(sampleMean,nSmp,1));
end

[model.projection, LassoCardi] = SR(options, Y, fea);

model.LARs = LARs;
model.LassoCardi = LassoCardi;

model.TYPE = 'USR';
model.options = options;


function y = aprod( mode, m, n, x, iw, rw )
% This is the simplest example for testing  LSQR.
% :math:`A = rw`.
% If `mode = 1`, `aprod` computes :math:`y = A x`.
% Ff `mode = 2`, `aprod` computes :math:`y = A^T x`.
% for some matrix  `A`.

if mode == 1,
   y = rw*x;
else
   y = rw'*x;
end


function [ offspring ] = DE_transfer(Problem, Population1, Population2, popsize)

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

% This function is written by Kangjia Qiao

    Fm  = [0.6,0.8,1.0];
    CRm = [0.1,0.2,1.0];

    index = randi([1,length(Fm)],popsize,1);
    F     = Fm(index);
    F     = F';
    index = randi([1,length(CRm)],popsize,1);
    CR    = CRm(index);
    CR    = CR';

    index =randi(Problem.N, popsize,1);

    permutation = randperm(Problem.N);

    array = permutation(1:popsize);

    pop1 = Population1(array).decs;
    pop2 = Population2.decs;

    vi = pop2(index,:);

    mask  = rand(popsize, Problem.D) > CR(:, ones(1, Problem.D)); % mask is used to indicate which elements of ui comes from the parent
    rows  = (1 : popsize)'; cols = floor(rand(popsize, 1) * Problem.D)+1; % choose one position where the element of ui doesn't come from the parent
    jrand = sub2ind([popsize Problem.D], rows, cols); mask(jrand) = false;
    u     = vi; u(mask) = pop1(mask);

    offspring = Problem.Evaluation(u);
end


function res = vl_ffdnet_matlab(net, input)

%% If you did not install the matconvnet package, you can use this for testing.

global sigmas;
n = numel(net.layers);
res = struct('x', cell(1,n+1));
res(1).x = input;

for i = 1 : n
    l = net.layers{i};
    switch l.type

        case 'conv'
            disp(['Processing ... ',int2str(i),'/',int2str(n)]);
            for noutmaps = 1 : size(l.weights{1},4)
                z = zeros(size(res(i).x,1),size(res(i).x,2),'single');
                for ninmaps = 1 : size(res(i).x,3)
                    z = z + convn(res(i).x(:,:,ninmaps), rot90(l.weights{1}(:,:,ninmaps,noutmaps),2),'same'); % 180 degree rotation for kernel
                end
                res(i+1).x(:,:,noutmaps) = z + l.weights{2}(noutmaps);
            end

        case 'relu'
            res(i+1).x = max(res(i).x,0);

        case 'concat'
            if size(sigmas,1)~=size(res(i).x,1)
                sigmaMap   = bsxfun(@times,ones(size(res(i).x,1),size(res(i).x,2),1,size(res(i).x,4),'single'),permute(sigmas,[3 4 1 2]));
                res(i+1).x = cat(3,res(i).x,sigmaMap);
            else
                res(i+1).x = cat(3,res(i).x,sigmaMap);
            end

        case 'SubP'
            res(i+1).x = vl_nnSubP(res(i).x, [],'scale',l.scale);

    end
    res(i).x = [];
end

end


function X = mat(x,n)
% Y = MAT(x,n)   or   Y = MAT(x)     (the 2nd argument is optional)
%   Given a vector of length n^2, this produces the n x n matrix
%   Y such that x = vec(Y).  In other words, x contains the columns of the
%   matrix Y, stacked below each other.
%
% See also vec.

% This file is part of SeDuMi 1.1 by Imre Polik and Oleksandr Romanko
% Copyright (C) 2005 McMaster University, Hamilton, CANADA  (since 1.1)
%
% Copyright (C) 2001 Jos F. Sturm (up to 1.05R5)
%   Dept. Econometrics & O.R., Tilburg University, the Netherlands.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% Affiliation SeDuMi 1.03 and 1.04Beta (2000):
%   Dept. Quantitative Economics, Maastricht University, the Netherlands.
%
% Affiliations up to SeDuMi 1.02 (AUG1998):
%   CRL, McMaster University, Canada.
%   Supported by the Netherlands Organization for Scientific Research (NWO).
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc.,  51 Franklin Street, Fifth Floor, Boston, MA
% 02110-1301, USA
%

if nargin < 2
    n = floor(sqrt(length(x)));
    if (n*n) ~= length(x)
        error('Argument X has to be a square matrix')
    end
end
X = reshape(x,n,n);


function [train,B, A] = generate_data(n, d, num_atoms, active_size)
B = cell(num_atoms,1);
for i=1:num_atoms
    x=randn(d)-rand(d); b=x'*x/d+eye(d)*1e-2; B{i}=b/trace(b);
end
train = cell(n,1); A = zeros(num_atoms, n);
for t=1:n
    p=rand(active_size,1);
    idx = randperm(num_atoms, active_size);
    A(idx, t) = p;

    X = zeros(d);
    for s=1:active_size
        X = X + p(s)*B{idx(s)};
    end
    train{t} = X;
end
end


function c = tapas_ehgf_binary_pu_config
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Contains the configuration for the Hierarchical Gaussian Filter (HGF)
% for binary inputs in the *presence* of perceptual uncertainty.
%
% The HGF is the model introduced in
%
% Mathys C, Daunizeau J, Friston, KJ, and Stephan KE. (2011). A Bayesian foundation
% for individual learning under uncertainty. Frontiers in Human Neuroscience, 5:39.
%
% The binary HGF model has since been augmented with a positive factor kappa1 which
% scales the second level with respect to the first, i.e., the relation between the
% first and second level is
%
% p(x1=1|x2) = s(kappa1*x2), where s(.) is the logistic sigmoid.
%
% By default, kappa1 is fixed to 1, leading exactly to the model introduced in
% Mathys et al. (2011).
%
% This file refers to BINARY inputs (Eqs 1-3 in Mathys et al., (2011));
% for continuous inputs, refer to tapas_hgf_config.
%
% This file refers to UNCERTAIN inputs (Eqs 45-47 in Mathys et al., (2011));
% for inputs without uncertainty, refer to tapas_hgf_binary_config.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% The HGF configuration consists of the priors of parameters and initial values. All priors are
% Gaussian in the space where the quantity they refer to is estimated. They are specified by their
% sufficient statistics: mean and variance (NOT standard deviation).
%
% Quantities are estimated in their native space if they are unbounded (e.g., the omegas). They are
% estimated in log-space if they have a natural lower bound at zero (e.g., the sigmas).
%
% Parameters can be fixed (i.e., set to a fixed value) by setting the variance of their prior to
% zero. Aside from being useful for model comparison, the need for this arises whenever the scale
% and origin at the j-th level are arbitrary. This is the case if the observation model does not
% contain the representations mu_j and sigma_j. A choice of scale and origin is then implied by
% fixing the initial value mu_j_0 of mu_j and either kappa_j-1 or omega_j-1.
%
% Fitted trajectories can be plotted by using the command
%
% >> tapas_hgf_binary_plotTraj(est)
%
% where est is the stucture returned by tapas_fitModel. This structure contains the estimated
% perceptual parameters in est.p_prc and the estimated trajectories of the agent's
% representations (cf. Mathys et al., 2011). Their meanings are:
%
%         est.p_prc.mu_0       row vector of initial values of mu (in ascending order of levels)
%         est.p_prc.sa_0       row vector of initial values of sigma (in ascending order of levels)
%         est.p_prc.rho        row vector of rhos (representing drift; in ascending order of levels)
%         est.p_prc.ka         row vector of kappas (in ascending order of levels)
%         est.p_prc.om         row vector of omegas (in ascending order of levels)
%         est.p_prc.al         scalar alpha (perceptual uncertainty)
%         est.p_prc.eta0       scalar eta0 (mean of first input category)
%         est.p_prc.eta1       scalar eta1 (mean of second input category)
%
% Note that the first entry in all of the row vectors will be NaN because, at the first level,
% these parameters are either determined by the second level (mu_0 and sa_0) or undefined (rho,
% kappa, and omega).
%
%         est.traj.mu          mu (rows: trials, columns: levels)
%         est.traj.sa          sigma (rows: trials, columns: levels)
%         est.traj.muhat       prediction of mu (rows: trials, columns: levels)
%         est.traj.sahat       precisions of predictions (rows: trials, columns: levels)
%         est.traj.v           inferred variance of random walk (rows: trials, columns: levels)
%         est.traj.w           weighting factors (rows: trials, columns: levels)
%         est.traj.da          volatility prediction errors  (rows: trials, columns: levels)
%         est.traj.ud          updates with respect to prediction  (rows: trials, columns: levels)
%         est.traj.psi         precision weights on prediction errors  (rows: trials, columns: levels)
%         est.traj.epsi        precision-weighted prediction errors  (rows: trials, columns: levels)
%         est.traj.wt          full weights on prediction errors (at the first level,
%                                  this is the learning rate) (rows: trials, columns: levels)
%
% Note that in the absence of sensory uncertainty (which is the assumption here), the first
% column of mu, corresponding to the first level, will be equal to the inputs. Likewise, the
% first column of sa will be 0 always.
%
% Tips:
% - When analyzing a new dataset, take your inputs u and use
%
%   >> est = tapas_fitModel([], u, 'tapas_hgf_binary_pu_config', 'tapas_bayes_optimal_binary_config');
%
%   to determine the Bayes optimal perceptual parameters (given your current priors as defined in
%   this file here, so choose them wide and loose to let the inputs influence the result). You can
%   then use the optimal parameters as your new prior means for the perceptual parameters.
%
% - When analyzing a new dataset, take your inputs u and use
%
% - If you get an error saying that the prior means are in a region where model assumptions are
%   violated, lower the prior means of the omegas, starting with the highest level and proceeding
%   downwards.
%
% - Alternatives are lowering the prior means of the kappas, if they are not fixed, or adjusting
%   the values of the kappas or omegas, if any of them are fixed.
%
% - If the log-model evidence cannot be calculated because the Hessian poses problems, look at
%   est.optim.H and fix the parameters that lead to NaNs.
%
% - Your guide to all these adjustments is the log-model evidence (LME). Whenever the LME increases
%   by at least 3 across datasets, the adjustment was a good idea and can be justified by just this:
%   the LME increased, so you had a better model.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2017 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Config structure
c = struct;

% Model name
c.model = 'ehgf_binary_pu';

% Number of levels (minimum: 3)
c.n_levels = 3;

% Input intervals
% If input intervals are irregular, the last column of the input
% matrix u has to contain the interval between inputs k-1 and k
% in the k-th row, and this flag has to be set to true
c.irregular_intervals = false;

% Sufficient statistics of Gaussian parameter priors

% Initial mus and sigmas
% Format: row vectors of length n_levels
% For all but the first two levels, this is usually best
% kept fixed to 1 (determines origin on x_i-scale). The
% first level is NaN because it is determined by the second,
% and the second implies neutrality between outcomes when it
% is centered at 0.
c.mu_0mu = [NaN, 0, 1];
c.mu_0sa = [NaN, 0, 0];

c.logsa_0mu = [NaN,   log(0.1), log(1)];
c.logsa_0sa = [NaN,          0,      0];

% Rhos
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
% Fix this to zero to turn off drift.
c.rhomu = [NaN, 0, 0];
c.rhosa = [NaN, 0, 0];

% Kappas
% Format: row vector of length n_levels-1.
% Fixing log(kappa1) to log(1) leads to the original HGF model.
% Higher log(kappas) should be fixed (preferably to log(1)) if the
% observation model does not use mu_i+1 (kappa then determines the
% scaling of x_i+1).
c.logkamu = [log(1), log(1)];
c.logkasa = [     0,      0];

% Omegas
% Format: row vector of length n_levels.
% Undefined (therefore NaN) at the first level.
c.ommu = [NaN,  -3,  -6];
c.omsa = [NaN, 4^2, 4^2];

% Alpha
% Format: scalar.
c.logalmu = log(0.5);
c.logalsa = 1;

% Eta0
% Format: scalar.
c.eta0mu = 0;
c.eta0sa = 0;

% Eta1
% Format: scalar.
c.eta1mu = 1;
c.eta1sa = 0;

% Gather prior settings in vectors
c.priormus = [
    c.mu_0mu,...
    c.logsa_0mu,...
    c.rhomu,...
    c.logkamu,...
    c.ommu,...
    c.logalmu,...
    c.eta0mu,...
    c.eta1mu,...
         ];

c.priorsas = [
    c.mu_0sa,...
    c.logsa_0sa,...
    c.rhosa,...
    c.logkasa,...
    c.omsa,...
    c.logalsa,...
    c.eta0sa,...
    c.eta1sa,...
         ];

% Check whether we have the right number of priors
expectedLength = 3*c.n_levels+2*(c.n_levels-1)+4;
if length([c.priormus, c.priorsas]) ~= 2*expectedLength;
    error('tapas:hgf:PriorDefNotMatchingLevels', 'Prior definition does not match number of levels.')
end

% Model function handle
c.prc_fun = @tapas_ehgf_binary_pu;

% Handle to function that transforms perceptual parameters to their native space
% from the space they are estimated in
c.transp_prc_fun = @tapas_ehgf_binary_pu_transp;

return;


function labels = grLabel(nodes, edges)
%GRLABEL Associate a label to each connected component of the graph
%
%   LABELS = grLabel(NODES, EDGES)
%   Returns an array with as many rows as the array NODES, containing index
%   number of each connected component of the graph. If the graph is
%   totally connected, returns an array of 1.
%
%   Example
%       nodes = rand(6, 2);
%       edges = [1 2;1 3;4 6];
%       labels = grLabel(nodes, edges);
%   labels =
%       1
%       1
%       1
%       2
%       3
%       2
%
%   See also
%   getNeighborNodes
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2007-08-14,    using Matlab 7.4.0.287 (R2007a)
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

% init
Nn = size(nodes, 1);
labels = (1:Nn)';

% iteration until stability
modif = true;
while modif
    modif = false;

    % compute the minimum label in the neighborhood of each node
    for i = 1:Nn
        neigh = grAdjacentNodes(edges, i);
        neighLabels = labels([i;neigh]);

        % check for a modification
        if length(unique(neighLabels)) > 1
            modif = true;
        end

        % put new labels
        labels(ismember(labels, neighLabels)) = min(neighLabels);
    end
end

% renumbering to have fewer labels
labels2 = unique(labels);
for i = 1:length(labels2)
    labels(labels == labels2(i)) = i;
end


% This is an example of using this software to train a binary
% decision tree classifier, and use this classifier to classify
% testing data.

% Copyright (C) 2012 Quan Wang <wangq10@rpi.edu>,
% Signal Analysis and Machine Perception Laboratory,
% Department of Electrical, Computer, and Systems Engineering,
% Rensselaer Polytechnic Institute, Troy, NY 12180, USA
%
% You are free to use this software for academic purposes if you cite our paper:
% Q. Wang, Y. Ou, A.A. Julius, K.L. Boyer, M.J. Kim,
% Tracking tetrahymena pyriformis cells using decision trees,
% in: 2012 International Conference on Pattern Recognition, Tsukuba Science City, Japan.
%
% For commercial use, please contact the authors.

clear;clc;close all;

%% settings
Depth=5; % maximal depth of decision tree
Splits=100; % number of candidate thresholds at each node
MinNode=10; % minimal size of a non-leaf node

%% training
load TrainingData.mat;

tic;
T=create01Tree(X,Y,Depth,Splits,MinNode);
t1=toc;

clear X Y;

%% testing
load TestingData.mat;

tic;
y=[];
for i=1:size(X,1)
    x=X(i,:);
    y(i,1)=decide01Tree(x,T);
end
t2=toc;

%% evaluation
errorRate=sum(abs(y-Y))/max(size(Y));
fprintf('Error rate = %.4f\n',errorRate);
fprintf('Training time = %.4f seconds\n',t1);
fprintf('Testing time = %.4f seconds\n',t2);


function [ cx, cy ] = cswap ( n, cx, incx, cy, incy )

%*****************************************************************************80
%
%% CSWAP interchanges two complex vectors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 April 2006
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, complex CX(*), one of the vectors to swap.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Input, complex CY(*), one of the vectors to swap.
%
%    Input, integer INCY, the increment between successive elements of CY.
%
%    Output, complex CX(*), the input vector CX, with some elements swapped.
%
%    Input, integer INCX, the increment between successive entries of CX.
%
%    Output, complex CY(*), the input vector CY, with some elements swapped.
%
  temp                    = cx(1:incx:1+(n-1)*incx);
  cx(1:incx:1+(n-1)*incx) = cy(1:incy:1+(n-1)*incy);
  cy(1:incy:1+(n-1)*incy) = temp;

  return
end


classdef trigspec < coeffsDiscretization
%TRIGSPEC    Fourier spectral method in coefficient space.
%   TRIGSPEC is an implementation of OPDISCRETIZATION that implements a
%   Fourier spectral method in coefficient space.
%
% See also TRIGCOLLOC.
%
% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS PROPERTIES:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties ( Access = public )
        coeffs        % Coefficients of the operator.
        outputSpace   % The range of the operator.
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% CLASS CONSTRUCTOR:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = false )

        function disc = trigspec(varargin)
            disc = disc@coeffsDiscretization(varargin{:});
            % No dimension adjustment are required for TRIGSPEC.
            disc.dimAdjust = 0;
            disc.projOrder = 0;
        end

    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% STATIC METHODS:
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods ( Access = public, Static = true )

        function tech = returnTech()
            %RETURNTECH    Return the appropriate tech to use for TRIGSPEC.
            tech = @trigtech;
        end

        % Differentiation matrices for TRIGSPEC.
        D = diffmat(N, m, flag)

        % Multiplication matrices for TRIGSPEC.
        D = multmat(N, f)

    end

end


% KM_DEMO_AKCCA Demonstration of Alternating Kernel Canonical Correlation
% Analysis algorithm for blind equalization of single-input multiple-output
% Wiener systems.
%
% Author: Steven Van Vaerenbergh (steven *at* gtas.dicom.unican.es), 2011.
%
% The algorithm in this file is based on the following publication:
% S. Van Vaerenbergh, J. Via and I. Santamaria, "Blind Identification of
% SIMO Wiener Systems based on Kernel Canonical Correlation Analysis",
% accepted for publication in IEEE Transactions on Signal Processing, 2013.
%
% This file is part of the Kernel Methods Toolbox for MATLAB.
% https://github.com/steven2358/kmbox

close all
clear
rs = 1; % seed for random generator
rng('default')
rng(rs)

fprintf('\nAlternating kernel CCA for blind equalization of ')
fprintf('SIMO Wiener systems.\n');

%% SETUP PARAMETERS
pars_setup.model_num = 30244; % source model, see generate_data.m
pars_setup.model_L = 5; % channel length
pars_setup.data_type = 'gaussian'; % bits, gaussian
pars_setup.data_N = 256; % number of data points
pars_setup.data_SNR = 20; % signal-to-noise ratio
pars_setup.zf_k = 15; % number of observations to estimate zero-forcing equalizers
pars_setup.p = 4; % number of branches in SIMO Wiener system
pars_setup.verbose = false;

%% ALT-KCCA PARAMETERS
pars = pars_setup; % copy some setup parameters
pars.it_max = 100; % maximum number of iterations
pars.it_stop = 1E-10; % stop iteration if change in cost is smaller than this
pars.kernel.type = 'gauss'; % kernel type
pars.kernel.par = @(x) km_silverman(x); % kernel parameter, either a scalar or a function to determine it
pars.m = 1E-8; % number of KPCA autovectors / precision of ICD, or fraction of discarded signal energy
pars.reg = 1E-5; % regularization
pars.decomp = 'ICD'; % ICD or KPCA
pars.identical_nonlin = 0;	% boolean indicating common nonlinearity for each channel

%% PROGRAM
tic

N = pars_setup.data_N; p = pars_setup.p; SNR = pars_setup.data_SNR;
% GENERATE DATA
switch pars_setup.data_type,
    case 'gaussian'
        s = randn(N,1); s = s-mean(s); % source signal
    case 'bits'
        s = 2*round(rand(N,1))-1; % source signal
end
y = cell(p,1); x = cell(p,1); sigpow = 0;
f = @(x) tanh(0.8*x)+0.1*x; % nonlinearity
B1 = [0.6172   -0.8601    2.1383    0.4269   -1.3153
    0.6247    0.1532    0.9686   -0.5820   -0.4584
    0.3373   -0.1888   -1.4263    0.8060   -0.1740
    -0.0349   -0.6264   -0.2486    1.1975    1.2195
    -3.2957    0.9985   -0.3768    0.6139   -1.2011]/1.5; % linear filter
B = cell(p,1);
for i=1:p, % simulate Wiener system
    B{i} = B1(:,i);
    y{i} = filter(B{i},1,s);
    x{i} = f(y{i});
    sigpow = sigpow + x{i}'*x{i}/p/N;
end
noisepow = 10^(-SNR/10)*sigpow;
for i=1:p, x{i} = x{i} + sqrt(noisepow)*randn(N,1); end % add noise
data.s = s; data.B = B; data.y = y; data.x = x;

%% AKCCA algorithm
[vars,eval] = km_akcca(pars,data);

toc
%% OUTPUT

% calculate MSE or BER
switch pars_setup.data_type
    case 'gaussian'
        % scale signals and compare
        s1 = s(vars.ind_s); s2 = eval.s_est;
        s1_norm = s1/norm(s1); sc = s1_norm'*s2/(s2'*s2);
        err_vec = s1_norm - sc*s2;

        MSE = err_vec'*err_vec;
        sc = sc*norm(s1);

        fprintf('MSE = %.2f dB\n',10*log10(MSE));
    case 'bits'
        sc = 1;
        fprintf('BER = %.4f\n',eval.finalresult);
end

N = pars_setup.data_N;

% draw estimates of nonlinearities
figure
for i=1:p
    subplot(1,p,i); hold all
    xi = vars.x{i};
    y_est = vars.y_est{i};
    sci = y{i}'*y_est/(y_est'*y_est);

    plot(xi,data.y{i},'.b')
    plot(xi,sci*y_est,'.r');
    legend(sprintf('x%d vs true, unknown y%d',i),sprintf('x%d vs estimated y%d',i))
    xlabel('x')
end

% draw estimates of linear filters
figure
for i=1:p
    subplot(1,p,i); hold all
    stem(B{i},'ob','LineWidth',2,'MarkerSize',8);
    stem(vars.h{i}*norm(B{i})/norm(vars.h{i}),'xr','LineWidth',2,...
        'LineStyle','none','MarkerSize',10);

    xlabel('l');ylabel(sprintf('h_%d',i));
    legend({'real channel','estimated channel'})
end

% draw equalization result
figure; hold all
plot(s(vars.ind_s))
plot(sc*eval.s_est);
legend('source signal','recovered signal')


function pred_label=SpatialJSRC(feat, train_data, train_label, sp_label, block_size, param)
% Classification of superpixels with joint sparse representation
% Input:
%    feat: extracted img features, nr*nc*nd where nd is the number of feature channel
%    dict: the dictionary
%    dict_label: the label of dictionary atoms
%    sp_label: the label of superpixels, nr*nc
%    block_size: the size for block-wise processing
%    param: parameter for sparse representation
%            param.L (optional, maximum number of elements in each decomposition,
%               min(m,p) by default)
%            param.eps (optional, threshold on the squared l2-norm of the residual,
%               0 by default
%            param.lambda (optional, penalty parameter, 0 by default
%            param.numThreads (optional, number of threads for exploiting
%            multi-core / multi-cpus. By default, it takes the value -1,
%            which automatically selects all the available CPUs/cores).
%   2016-10-20, jlfeng

[nr,nc,~]=size(feat);
sp_label=reshape(sp_label,[nr*nc 1]);
[group_label,sortidx]=sort(sp_label);
feat_sort=VectorIndexing3D(feat,sortidx);
idx_group_start=zeros(length(unique(group_label)),1);
idx_group_start(2:end)=int32(find(diff(group_label)~=0));

num_sp=length(idx_group_start);
num_block=ceil(num_sp/block_size);
disp('Block-wise classification strategy is used. ');
disp(['Number of Blocks: ', num2str(num_block)]);
pause(0.05)
pred_label_sort=zeros(nr*nc,1);
for nn=1:num_block
    disp(['Processing block ',num2str(nn)]);tic
    idxstart=(nn-1)*block_size;
    idxend=min(nn*block_size-1,num_sp-1);
    idx_block=(group_label>=idxstart)&(group_label<=idxend);
    test_data=feat_sort(:,idx_block);
    idx_group_start_block=idx_group_start(idxstart+1:idxend+1);
    idx_group_start_block=idx_group_start_block-idx_group_start_block(1);
    pred_label_block=JSRClassifier(train_data,train_label,test_data,idx_group_start_block,param);
    pred_label_sort(idx_block)=pred_label_block;
end
pred_label=zeros(nr*nc,1);
pred_label(sortidx)=pred_label_sort;
pred_label=reshape(pred_label,[Nx Ny]);


function SO3F = rotate_outer(SO3F,rot,varargin)
% rotate a function on SO(3)
%
% Syntax
%   SO3F = SO3F.rotate_outer(rot)
%
% Input
%  SO3F - @SO3FunHarmonic
%  rot  - @rotation
%
% Output
%  SO3F - @SO3FunHarmonic
%

L = SO3F.bandwidth;
D = WignerD(rot,'bandwidth',L);

for l = 0:L
  SO3F.fhat(deg2dim(l)+1:deg2dim(l+1)) = ...
    reshape(SO3F.fhat(deg2dim(l)+1:deg2dim(l+1)),2*l+1,2*l+1) * ...
    reshape(D(deg2dim(l)+1:deg2dim(l+1)),2*l+1,2*l+1).' ;
end

end


function [y,L] = midwt(x,h,L);
%    [x,L] = midwt(y,h,L);
%
%    Function computes the inverse discrete wavelet transform x for a 1D or
%    2D input signal y using the scaling filter h.
%
%    Input:
%	y : finite length 1D or 2D input signal (implicitly periodized)
%           (see function mdwt to find the structure of y)
%       h : scaling filter
%       L : number of levels. In the case of a 1D signal, length(x) must be
%           divisible by 2^L; in the case of a 2D signal, the row and the
%           column dimension must be divisible by 2^L.  If no argument is
%           specified, a full inverse DWT is returned for maximal possible
%           L.
%
%    Output:
%       x : periodic reconstructed signal
%       L : number of decomposition levels
%
%    1D Example:
%       xin = makesig('LinChirp',8);
%       h = daubcqf(4,'min');
%       L = 1;
%       [y,L] = mdwt(xin,h,L);
%       [x,L] = midwt(y,h,L)
%
%    1D Example's  output:
%
%       x = 0.0491 0.1951 0.4276 0.7071 0.9415 0.9808 0.6716 0.0000
%       L = 1
%
%    See also: mdwt, mrdwt, mirdwt
%

%
%
%File Name: midwt.m
%Last Modification Date: 08/07/95	15:13:52
%Current Version: midwt.m	2.4
%File Creation Date: Wed Oct 19 10:51:58 1994
%Author: Markus Lang  <lang@jazz.rice.edu>
%
%Copyright (c) 2000 RICE UNIVERSITY. All rights reserved.
%Created by Markus Lang, Department of ECE, Rice University.
%
%This software is distributed and licensed to you on a non-exclusive
%basis, free-of-charge. Redistribution and use in source and binary forms,
%with or without modification, are permitted provided that the following
%conditions are met:
%
%1. Redistribution of source code must retain the above copyright notice,
%   this list of conditions and the following disclaimer.
%2. Redistribution in binary form must reproduce the above copyright notice,
%   this list of conditions and the following disclaimer in the
%   documentation and/or other materials provided with the distribution.
%3. All advertising materials mentioning features or use of this software
%   must display the following acknowledgment: This product includes
%   software developed by Rice University, Houston, Texas and its contributors.
%4. Neither the name of the University nor the names of its contributors
%   may be used to endorse or promote products derived from this software
%   without specific prior written permission.
%
%THIS SOFTWARE IS PROVIDED BY WILLIAM MARSH RICE UNIVERSITY, HOUSTON, TEXAS,
%AND CONTRIBUTORS AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
%BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
%FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL RICE UNIVERSITY
%OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
%EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
%PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
%OR BUSINESS INTERRUPTIONS) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
%WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
%OTHERWISE), PRODUCT LIABILITY, OR OTHERWISE ARISING IN ANY WAY OUT OF THE
%USE OF THIS SOFTWARE,  EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
%
%For information on commercial licenses, contact Rice University's Office of
%Technology Transfer at techtran@rice.edu or (713) 348-6173
%
%Change History:
%
%Modification #1
%Mon Aug  7 11:52:33 CDT 1995
%Rebecca Hindman <hindman@ece.rice.edu>
%Added L to function line so that it can be displayed as an output
%
%Thu Mar  2 13:07:11 CDT 2000
%Ramesh Neelamani<neelsh@ece.rice.edu>
%Revamped the help file
%


function score =dmapMetricRel(dmap1,gtdmap,mask)

    dmap1(isinf(dmap1)) = nan;
    gtdmap(isinf(gtdmap)) = nan;
    mask = logical(mask);
    x = dmap1(mask);
    y = gtdmap(mask);
    nanMask = ~isnan(y); % Keep points present in gt depth
    x = x(nanMask); y = y(nanMask);
    x(isnan(x)) = median(x(~isnan(x)));
    b = median(x-y);
    score = abs(x-y-b)./y;
    score = mean(score);
end


function [ x, y ] = p06_dat ( data_num )

%*****************************************************************************80
%
%% P06_DAT returns the data vector for problem 6.
%
%  Discussion:
%
%    The X data is equally spaced.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer DATA_NUM, the number of data points.
%
%    Output, real X(DATA_NUM,1), the abscissa data.
%
%    Output, real Y(DATA_NUM,1), the ordinate data.
%
  x = zeros ( data_num, 1 );
  y = zeros ( data_num, 1 );

  num_int = 5;

  n = 1;
  x(n,1) = 0.0;
  y(n,1) = 0.0;

  for i = 1 : num_int

    for j = 1 : i
      n = n + 1;
      x(n,1) = ( i - 1 ) + 0.5 * j /i;
      y(n,1) = j / i;
    end

    for j = 1 : i
      n = n + 1;
      x(n,1) = i - 1 + 0.5 + 0.5 * j / i;
      y(n,1) = 1.0 - j / i;
    end

  end

  return
end


function [gps_week, gps_sow, gps_dow] = date2gps(date)

% SYNTAX:
%   [gps_week, gps_sow, gps_dow] = date2gps(date);
%
% INPUT:
%   date = date [year, month, day, hour, min, sec]
%
% OUTPUT:
%   gps_week = GPS week
%   gps_sow  = GPS seconds of week
%   gps_dow  = GPS day of week
%
% DESCRIPTION:
%   Conversion from calendar date to GPS time.

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   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/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

gps_start_datenum = 723186; %This is datenum([1980,1,6,0,0,0])

%number of days since the beginning of GPS time
%deltat   = (datenum([date(:,1), date(:,2), date(:,3)]) - gps_start_datenum);
% hack: datenummmx is faster cause it does not check argins
deltat   = (datenummx(date(:,1:3)) - gps_start_datenum);

gps_week = floor(deltat/7);            %GPS week
gps_dow  = floor(deltat - gps_week*7); %GPS day of week
gps_sow  = (deltat - gps_week*7)*86400;
gps_sow = gps_sow + date(:,4)*3600 + date(:,5)*60 + date(:,6); %GPS seconds of week

% %alternative way, using the Julian day
% jd = date2jd(date);
% [gps_week, gps_dow, gps_sow] = jd2gps(jd);


% GENERATE_RANDOM_OPTIONS Create random options according to some
% delimiters
%
% Usage
%	options = GENERATE_RANDOM_OPTIONS(white_list,type,values)
%
% Input
%    white_list (cell): the white-list corresponding to the name of the
%    field of the options
%    type (cell of string): the type of the variable that one should
%    expect. 'c' continuous, 'i' integer, 'd' discrete
%    values (cell of cell): the corresponding values to a given type.
%
% Output
%    filt_for_disp (numerical): displayed (real) image.
%
% Description
%    For continous and integer, one excepts some interval given by
%    {{s},{e}} where s is the first extremity of the interval and e the
%    end.
%    When the value is discrete, a value of the corresponding cell of
%    values is chosen.
%
% Example: If white_list = {'filter_type', 'precision', 'Q', 'J', 'L',
%   'sigma_phi','sigma_psi','xi_psi', 'slant_psi'};
%   type = {'d', 'i', 'i', 'i', 'i', 'c', 'c', 'c', 'c'}
%   values = { {'morlet','gabor'}, {0,1}, {1,10}, {1,10},
%   {1,10}, {0,50}, {0,30}, {0,30}, {0,10} };
%
%   then GENERATE_RANDOM_OPTIONS(white_list,type,values) is for instance
%
%     filter_type: 'morlet'
%       precision: 0
%               Q: 3
%               J: 8
%               L: 9
%       sigma_phi: 25.7629
%       sigma_psi: 26.1973
%          xi_psi: 1.8308
%       slant_psi: 8.2580
%
% See also
%   CHECK_OPTIONS_WHITE_LIST

function options=generate_random_options(white_list,type,values)

assert(length(white_list)==length(type));
assert(length(white_list)==length(values));

options=struct;

for i=1:length(white_list)
   switch type{i}
       case 'c'  % continuous
           r=values{i}{1}+rand*(values{i}{2}-values{i}{1});
           options=setfield(options,white_list{i},r);
       case 'i'  % integer
           r=floor(values{i}{1}+rand*(1+values{i}{2}-values{i}{1}-1));
           options=setfield(options,white_list{i},r);
       case 'd'  % discrete
           s=ceil(rand*(length(values{i})));
           options=setfield(options,white_list{i},values{i}{s});
   end
end
end


b = ones(size(A,1),1);
tol = 1e-6; maxit = 100;
% tic; L1 = chol(A,'lower'); toc;
% fprintf('\n Incomplete chol decomposition');
% tic;
% L1 = ichol(A);
% toc;
% fprintf('\n ichol as Preconditioner');
% tic;
% [x1,fl1,rr1,it1,rv1] = pcg(A,b,tol,maxit,@(x)icholpre(x,A,L1,L1'));
% toc;
% fprintf('#dof: %8.0u,  iter: %2.0u\n',size(A,1), it1)
% semilogy(0:it1,rv1./norm(b),'r.');
%
fprintf('\n Approximate chol decomposition');
tic;
[L2,p,Ac] = achol(A);
toc;
fprintf('\n Achol as Preconditioner');
tic;
Ap = A(p,p);
[x2,fl2,rr2,it2,rv2] = pcg(A,b,tol,maxit,@(r)acholpre(r,Ap,L2,L2',p,Ac));
toc;
fprintf('#dof: %8.0u,  iter: %2.0u\n',size(A,1), it2)
semilogy(0:it2,rv2./norm(b),'b.');
hold on


function [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3)
% vgg_PX_from_6pts_3img  Computes camera matrices and world points
% from 6 points across 3 images.
%
%   [P,X] = vgg_PX_from_6pts_3img(x), where
%      x ... double(3,6,3) or cell{3} of double(3,6), 6 homogeneous points in 3 images
%      P ... double(3,4,3), P(:,:,k) is k-th camera matrix
%      X ... double(4,6), homogeneous world points
%   There are 0 to 3 solutions for (P,X). Solutions are pruned by requirement that
%   scalars s in all projective equations s*x==P*X are positive.
%   In case of multiple solutions, P and X have one dimension
%   more such that P(:,:,:,n) and X(:,:,n) is the n-th solution.
%
%   Also the form [P,X] = vgg_PX_from_6pts_3img(x1,x2,x3) is accepted.

% Algorithm in Hartley-Zisserman, Alg 19.1 page 493 in 1st edition,
%                                 Alg 20.1 page 511 in 2nd edition
% Coded by werner@robots.ox.ac.uk, Nov 2002.

if nargin==3
  x = cat(3,x1,x2,x3);
else
  if iscell(x1)
    x = cat(3,x1{:});
  else
    x = x1;
  end
end
if any(size(x)~=[3 6 3])
  error('Wrong size of input points.');
end

for k = 1:3
  % Find homographies H_k mapping first 4 pts in each image to standard projective basis.
  % Now, x(:,1:4,k) = H(:,:,k)*[eye(3) [1;1;1]].
  H(:,:,k) = H_from_4x(x(:,3:6,k));

  % Form transformed points xs(:,1:2,k)
  xs(:,:,k) = inv(H(:,:,k))*x(:,:,k);
end

% Compute dual fundamental matrix
Fd = Fdual_from_x(xs(:,1:2,:));

% Retrieve (non-dual) cameras P and world points X from (each solution for) dual fund. matrix.
P = [];
X = [];
for i = 1:size(Fd,3)

  % Compute canonical Xi and Pi
  Xi = X_from_Fdual(Fd(:,:,i));
  Pi = P_from_Xx_canonical(Xi,xs);

  % return from canonical to original image bases
  for k = 1:3
    Pi(:,:,k) = H(:,:,k)*Pi(:,:,k);
  end

  % Compute signs of P and X, if possible. If impossible, Pi==Xi==[].
  [Pi,Xi] = vgg_signsPX_from_x(Pi,Xi,x);
  if isempty(Pi), continue, end
  %for k = 1:3, Pi(:,:,k)*Xi ./ x(:,:,k), end  % test code

  P = cat(4,P,Pi);
  X = cat(3,X,Xi);

end

return

%%%%%%%%%%%%%%%% auxiliary functions

% Solve for dual fundamental matrix.
function F = Fdual_from_x(x)

% Linear step. After that,
% for k=1:3 and i=1:2, it is x(:,1,k)'*F_i*x(:,2,k)==0.
A = [];
for k = 1:3
  x1 = x(1,1,k);  y1 = x(2,1,k);  z1 = x(3,1,k);
  x2 = x(1,2,k);  y2 = x(2,2,k);  z2 = x(3,2,k);
  A = [A; [x1*y2-z1*y2,...
           x1*z2-z1*y2,...
           y1*x2-z1*y2,...
           y1*z2-z1*y2,...
           z1*x2-z1*y2] ];
end
[u,s,v] = svd(A,0);
v1 = v(:,end-1);
v2 = v(:,end);
FF{1} = [0 v1(1:2)'; v1(3) 0 v1(4); v1(5) -sum(v1) 0];
FF{2} = [0 v2(1:2)'; v2(3) 0 v2(4); v2(5) -sum(v2) 0];

% Non-linear step. Find linear combination of F_i having zero determinant.
% Dual fund. matrix is now  F = a*FF{1} + (1-a)*FF{2}, for each element of a.
a = vgg_singF_from_FF(FF);
for i = 1:length(a)
  F(:,:,i) = a(i)*FF{1} + (1-a(i))*FF{2};
end
return

% Given dual fundamental matrix Fd, computes cameras P and world points X such that
% for each k, P(:,:,k)*X ~ x(:,:,k).
function X = X_from_Fdual(Fd)

% Retrieve second dual camera matrix Pd2.
% It is done by considering Fdual in form
%[0 b*(d-c) -c*(d-b)
% -a*(d-c) 0 c*(d-a)
% a*(d-b) -b*(d-a) 0].

Fd_aux = reshape( Fd([4 7 1 2 1 8 1 3 6]), [3 3] );
[u,s,v] = svd(Fd_aux,0);
ABC = v(:,3); % It is a:b:c = A:B:C
[u,s,v] = svd(Fd',0);
KLM = v(:,3); % It is K:L:M = ((d-a):(d-b):(d-c)

G = [0 -ABC(3) ABC(2) 0
     ABC(3) 0 -ABC(1) 0
     -ABC(2) ABC(1) 0 0
     KLM(2) -KLM(1) 0 KLM(1)-KLM(2)
     0 KLM(3) -KLM(2) KLM(2)-KLM(3)
     -KLM(3) 0 KLM(1) KLM(3)-KLM(1) ];
[u,s,v] = svd(G,0);
abcd = v(:,end);

% The test code:
%Pd1 = [eye(3) [1;1;1]];
%Pd2 = [diag(abcd(1:3)) [1;1;1]*abcd(4)];
%vgg_F_from_P(Pd1,Pd2) ./ Fd

% Retrieve 6 world points X(:,1:6)
X = [ abcd [1;1;1;1] eye(4) ];

return

% Computes camera P from world points X and image points x, everything in canonical form.
function P = P_from_Xx_canonical(X,x)
X = X(:,1);
for k = 1:3
  A = [contreps(x(:,1,k))*[X(1) 0 0 X(4)
                           0 X(2) 0 X(4)
                           0 0 X(3) X(4)]
       contreps(x(:,2,k))*[eye(3) [1;1;1]]];
  [u,s,v] = svd(A,0);
  a = v(:,end);
  P(:,:,k) = [a(1) 0 0 a(4)
              0 a(2) 0 a(4)
              0 0 a(3) a(4)];
end
return

% A bit faster vgg_contreps.
function X = contreps(x)
X = [0 x(3) -x(2)
     -x(3) 0 x(1)
     x(2) -x(1) 0];
return

% H_from_4x  Having four point matches, the homography relating them is given by
% H = H_from_4x(x2)*inv(H_from_4x(x1)).
function H = H_from_4x(x)
H = x(:,1:3) * diag(inv(x(:,1:3))*x(:,4));
return

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Test code:
P = randn(3,4,3); X = randn(4,6);
for k = 1:3, x(:,:,k) = P(:,:,k)*X; end
[Pn,Xn] = vgg_PX_from_6pts_3img(x);

% check image points x predicted up to scale
for k=1:3, Pn(:,:,k)*Xn(:,:) ./ x(:,:,k), end


%  switching_divided_difference_filter - filter for nonlinear regime-swiching models
%
%  ::
%
%
%    [loglik,Incr,retcode,Filters]=switching_divided_difference_filter(...
%     syst,y,U,z,options)
%
%  Args:
%
%     - **syst** [struct]: structure containing:
%
%           - **PAI00** [vector]: initial probability distributions of regimes
%
%           - **a** [cell]: initial conditions in each regime
%
%           - **Qfunc** [function handle]: transition matrix generator
%
%           - **ff** [function handle]: ft=ff(rt,xt,et), where rt is the
%           regime, xt is the vector of state variables and et the vector of
%           shocks
%
%           - **P** [cell]: initial covariance matrix of the states in each
%           regime
%
%           - **H** [cell]: Measurement error covariance matrices in each regime
%
%           - **SIGeta** [cell]: Covariance matrix of structural shocks.
%
%     - **y** [matrix]: ny x T matrix of data
%
%     - **U** [[]|matrix]: ndx x T matrix of exogenous data
%
%     - **z** [function handle|logical|vector]: linear connection of the
%     observables to the state.
%
%     - **include_in_likelihood** [logical]: selector of increments to include
%     in the likelihood calculation
%
%     - **options** [struct]: structure with various options
%
%  Returns:
%     :
%
%     - **loglik** [scalar]: log likelihood
%
%     - **Incr** [vector]: increments of elements going into the likelihood
%
%     - **retcode** [{0}|integer]: flag for problems.
%
%     - **Filters** [struct]: Filtered, updated and smoothed variables
%
%  Note:
%
%  Example:
%
%     See also:
%


function [b,R,se]=drxlr_wfit(y,x,w,p)
%DREX subfunction
%Written by Issam El Naqa 2003-2005
%Extracted for generalized use 2005, AJH
%
% Copyright 2010, Joseph O. Deasy, on behalf of the DREES development team.
%
% This file is part of the Dose Response Explorer System (DREES).
%
% DREES development has been led by:  Issam El Naqa, Aditya Apte, Gita Suneja, and Joseph O. Deasy.
%
% DREES has been financially supported by the US National Institutes of Health under multiple grants.
%
% DREES is distributed under the terms of the Lesser GNU Public License.
%
%     This version of DREES 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.
%
% DREES 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 DREES.  If not, see <http://www.gnu.org/licenses/>.

sw = sqrt(w);
[r c] = size(x);
yw = y .* sw;
xw = x .* sw(:,ones(1,c));
[Q,R]=qr(xw,0);
b = R\(Q'*yw);
RI = R\eye(p);
C = RI * RI';
se = sqrt(max(eps,diag(C)));
return


function [nrm] = lognrm(tt)
%Frobenius norm of the TT-tensor
%   [NRM]=LOGNRM(TT) Computes log10 of the Frobenius norm of a tensor
%
%
% TT-Toolbox 2.2, 2009-2012
%
%This is TT Toolbox, written by Ivan Oseledets et al.
%Institute of Numerical Mathematics, Moscow, Russia
%webpage: http://spring.inm.ras.ru/osel
%
%For all questions, bugs and suggestions please mail
%ivan.oseledets@gmail.com
%---------------------------

d=tt.d;
n=tt.n;
r=tt.r;
pos=tt.ps;
cr=tt.core;
pos1=1;
nrm=zeros(d,1);
core0=cr(1:r(1)*n(1)*r(2));
%Orthogonalization from left-to-tight
for i=1:d-1
   core0=reshape(core0,[r(i)*n(i),r(i+1)]);
   [core0,ru]=qr(core0,0); nrm(i)=norm(ru,'fro');
   nrm(i)=max(nrm(i),1e-308);
   ru=ru./nrm(i);
   core1=cr(pos(i+1):pos(i+2)-1);
   core1=reshape(core1,[r(i+1),n(i+1)*r(i+2)]);
   core1=ru*core1;
   r(i+1)=size(core0,2);
   cr(pos1:pos1-1+r(i)*n(i)*r(i+1))=core0(:);
   cr(pos1+r(i)*n(i)*r(i+1):pos1+r(i)*n(i)*r(i+1)+r(i+1)*n(i+1)*r(i+2)-1)=core1(:);
   core0=core1;
   pos1=pos1+r(i)*n(i)*r(i+1);
end
pos1=pos1+r(d)*n(d)*r(d+1)-1;
nrm(d)=norm(core0(:));
nrm=sum(log10(nrm));
return
end


function [g1, g2] = rbfinfwhiteXwhiteKernGradient(rbfKern, whiteKern, t1, varargin)

% RBFINFWHITEXWHITEKERNGRADIENT Compute gradient between the RBF-WHITE kernel
% (with integration limits between minus infinity and infinity) and the
% WHITE kernel.
% FORMAT
% DESC computes the gradient of an objective function with respect to cross
% kernel terms between RBF-WHITE and WHITE kernels for the multiple output
% kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE
% kernel.
% ARG whiteKern : the kernel structure associated with the WHITE
% kernel.
% ARG t1 : inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of RBF-WHITE kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of WHITE kernel.
%
% FORMAT
% DESC computes the gradient of an objective function with respect
% to cross kernel terms between RBF-WHITE and WHITE kernels for
% the multiple output kernel.
% ARG rbfKern : the kernel structure associated with the RBF-WHITE
% kernel.
% ARG whiteKern : the kernel structure associated with the WHITE
% kernel.
% ARG t1 : row inputs for which kernel is to be computed.
% ARG t2 : column inputs for which kernel is to be computed.
% ARG covGrad : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of objective function with respect to kernel
% parameters of RBF-WHITE kernel.
% RETURN g2 : gradient of objective function with respect to kernel
% parameters of WHITE kernel.
%
% SEEALSO : multiKernParamInit, multiKernCompute, rbfinfwhiteKernParamInit,
% whiteKernParamInit
%
% COPYRIGHT : David Luengo, 2009

% KERN

if nargin < 5
    t2 = t1;
else
    t2 = varargin{1};
end
covGrad = varargin{end};

if size(t1, 2) > 1 | size(t2, 2) > 1
  error('Input can only have one column');
end
if rbfKern.variance ~= whiteKern.variance
  error('Kernels cannot be cross combined if they have different variances.')
end

g1 = zeros(1, 2);
g2 = 0; % The only parameter of the WHITE kernel (its variance) is already
        % accounted for in g1

% Parameters required for further computations
variance = rbfKern.variance;
invWidth = rbfKern.inverseWidth;

T1 = repmat(t1, 1, size(t2, 1));
T2 = repmat(t2.', size(t1, 1), 1);
deltaT = T1 - T2;

K = exp(-0.5*invWidth*(deltaT.^2));

% Gradient w.r.t. the inverse width
g1(1) = (0.5*variance/sqrt(2*pi)) ...
    * sum(sum((1/sqrt(invWidth)-sqrt(invWidth)*(deltaT.^2)) .* K .* covGrad));

% Gradient w.r.t. sigma_r^2
g1(2) = sqrt(invWidth/(2*pi)) * sum(sum(K .* covGrad));


%% plot array map & compute eastings and northings
if make_figures
    disp('Plotting array map')
    close all
    cols = 'rwbggg';
    for c=1:length(lat)
        chan = get(w(c),'channel');;
        plot(easting(c),northing(c),'o','MarkerFaceColor',cols(c),'MarkerSize',10)
        hold on
        quiver(easting(c),northing(c),-easting(c)/100,-northing(c)/100,0); % /100 just gives arrow length
        text(easting(c)+1,northing(c),chan(1:3));
    end
    grid on
    quiver(440,1325,wind_speed*sin(deg2rad(wind_direction)), wind_speed*cos(deg2rad(wind_direction)) ,0,'k');
    text(440,1325,'wind')
    hold off
    title('Beach House array position relative to SLC40');
    xlabel('metres east');
    ylabel('metres north');
    axis equal;
    outfile = sprintf('%s/arraymap.png',figureOutDirectory);
    feval('print', '-dpng', outfile);
    close
end


function [footPrint, nPix]= get_footprint(xs, Ly, Lx, ops)

[Mmax, imax] = max(xs, [], 2);

yp = rem(imax-1, Ly) + 1;
xp = ceil(imax/Ly);

Nk = size(xs,1);
footPrint = zeros(Nk, 1);
nPix = zeros(Nk, 1);

d = ops.diameter;
XS = repmat([-d*2:d*2], 4*d+1, 1);
YS = XS';

ds = (XS.^2 + YS.^2).^.5;

for j = 1:Nk
    yp1 = yp(j) + YS;
    xp1 = xp(j) + XS;

    badi = (yp1<1) | (yp1>Ly) | (xp1<1) | (xp1>Lx);
    yp1 = yp1(~badi);
    xp1 = xp1(~badi);
    allds = ds(~badi);

    ind = xs(j, yp1 + (xp1-1) * Ly) > Mmax(j)/2;

    footPrint(j) = mean(allds(ind));
    nPix(j) = numel(allds);
end

end


function [tinyImage, colorHist, param] = LMcolor(D, HOMEIMAGES, param)
%
% [tinyImage, colorHist, param] = LMcolor(D, HOMEIMAGES, param);
%
% param.tinySize
% param.colorHist.imagesize
% param.colotHist.
%
% Use transformations from "color indexing", Swain & Ballard, IJCV 91
%
%   wb = (r+g+b)/3
%   rg = r - g
%   by = b-r/2-g/2

if nargin<3
    % Default parameters
    param.tinySize = [16 16];
    param.colorHist.imagesize = [256 256];
    param.colorHist.nbins = [8 16 16];
    param.colorHist.margins.wb = [0 255];
    param.colorHist.margins.rg = [-255 255];
    param.colorHist.margins.by = [-255 255];
end

% Precompute filter transfert functions (only need to do this once, unless
% image size is changes):
Nfeatures = prod(param.colorHist.nbins);

if isstruct(D)
    % [gist, param] = LMcolor(D, HOMEIMAGES, param);
    Nscenes = length(D);
    typeD = 1;
end
if iscell(D)
    % [gist, param] = LMcolor(filename, HOMEIMAGES, param);
    Nscenes = length(D);
    typeD = 2;
end
if isnumeric(D)
    % [gist, param] = LMcolor(img, HOMEIMAGES, param);
    Nscenes = size(D,4);
    typeD = 3;
end

% Loop: Compute gist features for all scenes
tinyImage = zeros([param.tinySize 3 Nscenes], 'single');
colorHist = zeros([Nscenes Nfeatures], 'single');
for n = 1:Nscenes
    disp([n Nscenes])

    % load image
    try
        switch typeD
            case 1
                img = LMimread(D, n, HOMEIMAGES);
            case 2
                img = imread(fullfile(HOMEIMAGES, D{n}));
            case 3
                img = D(:,:,:,n);
        end
    catch
        disp(D(n).annotation.folder)
        disp(D(n).annotation.filename)
        rethrow(lasterror)
    end

    % resize and crop image to make it square
    tiny = imresizecrop(img, param.tinySize+2, 'bilinear');
    tinyImage(:,:,:,n) = tiny(2:end-1, 2:end-1,:);

    subplot(211)
    imagesc(img); axis('off'); axis('equal')
    subplot(212)
    imagesc(tiny); axis('on'); axis('equal')

    % resize and crop image to make it square
    img = single(imresizecrop(img, param.imagesize, 'bilinear'));

    %   wb = (r+g+b)/3
    %   rg = r - g
    %   by = b-r/2-g/2
    wb = mean(img,3);
    rg = img(:,:,1)-img(:,:,2);
    by = img(:,:,3)-img(:,:,1)/2-img(:,:,2)/2;

    wb = (wb(:)-param.colorHist.margins.wb(1))/(param.colorHist.margins.wb(2)-param.colorHist.margins.wb(1));
    rg = (rg(:)-param.colorHist.margins.rg(1))/(param.colorHist.margins.rg(2)-param.colorHist.margins.rg(1));
    by = (by(:)-param.colorHist.margins.by(1))/(param.colorHist.margins.by(2)-param.colorHist.margins.by(1));

    wb = fix(wb*param.colorHist.nbins(1));
    rg = fix(rg*param.colorHist.nbins(2));
    by = fix(by*param.colorHist.nbins(3));

    wb = min(max(0,wb), param.colorHist.nbins(1)-1);
    rg = min(max(0,rg), param.colorHist.nbins(2)-1);
    by = min(max(0,by), param.colorHist.nbins(3)-1);

    h = by + rg*param.colorHist.nbins(3) + wb*param.colorHist.nbins(3)*param.colorHist.nbins(2);
    H = hist(h, [0:Nfeatures-1]);

    % store
    colorHist(n,:) = H;
    drawnow
end


function planC = calculateGRE(baseScanNum,movScanNum,planC)
% function planC = calculateGRE(baseScanNum,movScanNum,planC)
%
% APA, 03/21/2017

if ~exist('planC','var')
    global planC
end
indexS = planC{end};

% Absolute Difference between two scans
siz = size(planC{indexS.scan}(baseScanNum).scanArray);
baseMask3M = logical(maskByThresh3D(planC{indexS.scan}(baseScanNum).scanArray));
movMask3M = logical(maskByThresh3D(planC{indexS.scan}(movScanNum).scanArray));
sA1 = zeros(siz,'single');
meanSa1 = mean(single(planC{indexS.scan}(baseScanNum).scanArray(baseMask3M)));
sdSa1 = std(single(planC{indexS.scan}(baseScanNum).scanArray(baseMask3M)));
sA1(baseMask3M) = (single(planC{indexS.scan}(baseScanNum).scanArray(baseMask3M)) - meanSa1)/sdSa1;
sA2 = zeros(siz,'single');
meanSa2 = mean(single(planC{indexS.scan}(movScanNum).scanArray(movMask3M)));
sdSa2 = std(single(planC{indexS.scan}(movScanNum).scanArray(movMask3M)));
sA2(movMask3M) = (single(planC{indexS.scan}(movScanNum).scanArray(movMask3M)) - meanSa2)/sdSa2;
diff3M = abs(sA1 - sA2);

% Window size
slcWindow = 5;
rowWindow = 5;
colWindow = 5;

% Number of levels for histogram
numLevels = 16;
% Create initial imM with dimension of slcWindow
imM = [];
for slc = 1:slcWindow
    imTmpM = im2col(diff3M(:,:,slc),[rowWindow colWindow],'sliding');
    imM = [imM;imTmpM];
end

numNeighbors = rowWindow*colWindow;
numSlcs = size(diff3M,3);
numRows = size(diff3M,1);
numCols = size(diff3M,2);
entropy3M = zeros(siz,'single');
mean3M = zeros(siz,'single');
var3M = zeros(siz,'single');
for slc = 1:numSlcs
    disp(['-------------------', num2str(slc)])
    if slc > floor(slcWindow/2) && slc <= (numSlcs-floor(slcWindow/2))
        imM(1:numNeighbors,:) = [];
        imTmpM = im2col(diff3M(:,:,slc),[rowWindow colWindow],'sliding');
        imM = [imM;imTmpM];
    end
    countsM = hist(imM,numLevels);
    countsM = countsM/numNeighbors/colWindow;
    entrpy2M = col2im(-sum(countsM.*log2(countsM+eps)),[rowWindow colWindow],[numRows numCols],'sliding');
    mean2M = col2im(mean(imM),[rowWindow colWindow],[numRows numCols],'sliding');
    var2M = col2im(var(imM),[rowWindow colWindow],[numRows numCols],'sliding');
    for i = 1:floor(slcWindow/2)
        entrpy2M = [entrpy2M(:,1), entrpy2M, entrpy2M(:,end)];
        entrpy2M = [entrpy2M(1,:); entrpy2M; entrpy2M(end,:)];
        mean2M = [mean2M(:,1), mean2M, mean2M(:,end)];
        mean2M = [mean2M(1,:); mean2M; mean2M(end,:)];
        var2M = [var2M(:,1), var2M, var2M(:,end)];
        var2M = [var2M(1,:); var2M; var2M(end,:)];
    end
    entropy3M(:,:,slc) = entrpy2M;
    mean3M(:,:,slc) = mean2M;
    var3M(:,:,slc) = var2M;
end

gre3M = entropy3M/median(entropy3M(baseMask3M)) + mean3M/median(mean3M(baseMask3M)) + var3M/median(var3M(baseMask3M));
gre3M = gre3M / 3;

%
% showIMDose(diff3M,'Diff',1);
% showIMDose(entropy3M,'Entropy',1);
% showIMDose(mean3M,'Mean',1);
% showIMDose(var3M,'Variance',1);
% showIMDose(gre3M,'GRE',1);

register = 'UniformCT';  %Currently only option supported.  Dose has the same shape as the uniformized CT scan.
doseError = [];
doseEdition = 'Generalized Registration Error';
overWrite = 'no';  %Overwrite the last CERR dose?
if ~exist('assocScanNum','var')
    assocScanNum = 1;
end
fractionGroupID = 'GRE';
assocScanUID = planC{indexS.scan}(assocScanNum).scanUID;
description = '';
planC = dose2CERR(gre3M,doseError,fractionGroupID,doseEdition,description,register,[],overWrite,assocScanUID,planC);


function IOU = IOU_with_GT(estimate, ground_truth)
    % strip off NaNs used to maintain all groundtruth in same matrix
    ground_truth = ground_truth(~isnan(ground_truth));
    % axis-aligned rectangle format
    if numel(ground_truth)==4
        if sum(estimate)==0
            IOU = 0;
        else
            IOU = bboxOverlapRatio(estimate, ground_truth);
        end
    else
        % rotated bounding box format (VOT)
        warning off;
        e_x = [estimate(1) estimate(1)+estimate(3) estimate(1)+estimate(3) estimate(1)];
        e_y = [estimate(2) estimate(2) estimate(2)+estimate(4) estimate(2)+estimate(4)];
        gt_x = ground_truth(1:2:7);
        gt_y = ground_truth(2:2:8);
        [intersection_x, intersection_y] = polybool('intersection',e_x,e_y,gt_x,gt_y);
        [union_x, union_y] = polybool('union',e_x,e_y,gt_x,gt_y);

        interseaction_area = polyarea(intersection_x, intersection_y);
        union_area = polyarea(union_x, union_y);

        IOU = interseaction_area / union_area;
        if isnan(IOU)
            IOU = 0;
        end
    end
end


function res = reduce(im,bfilt)
%
% function res = reduce(im,bfilt)
%
% Separable convolution and subsampling by a factor of two
% im: input image
% bfilt: convolution kernel (vector).
%       Default is: [.0625 .25 .375 .25 .0625]'
% res: result image
%
% res is 1/2 the size of im.  Fills NaNs for invalid pixels near
% edges.

if ~exist('bfilt')
  bfilt=[.0625 .25 .375 .25 .0625]';
end

% Use standard Matlab convolution routines instead for ease of
% distribution, and to set edge values to NaNs
bsize = floor(length(bfilt)/2);
tmp1 = conv2sep(im,bfilt,bfilt,'valid');
tmp2 = NaN*ones(size(im));
tmp2(1+bsize:size(im,1)-bsize,1+bsize:size(im,2)-bsize)=tmp1;
res = tmp2(1:2:size(im,1),1:2:size(im,2));

return;

%%%%%%%%%
% Debug %
%%%%%%%%%

foo=pgmRead('einstein.pgm');
bar=reduce(foo);
bar=replaceValue(bar,NaN,0);
displayImage(bar);

in=ones(7,7)
res=reduce(in)


function [f, fval] = test(varargin)

% Images
if nargin == 0
    A = imread('pout.tif');
    B = imread('cameraman.tif');
elseif nargin == 2
    A = varargin{1};
    B = varargin{2};
    if ischar(A)
        A = imread(A);
    end;
    if ischar(B)
        B = imread(B);
    end;
end;

% Histograms
nbins = 10;
[ca ha] = imhist(A, nbins);
[cb hb] = imhist(B, nbins);

% Features
f1 = ha;
f2 = hb;

% Weights
w1 = ca / sum(ca);
w2 = cb / sum(cb);

% Earth Mover's Distance
[f, fval] = emd(f1, f2, w1, w2, @gdf);

% Results
wtext = sprintf('fval = %f', fval);
figure('Name', wtext);
subplot(121);imshow(A);title('first image');
subplot(122);imshow(B);title('second image');

end


%DEMO_PASSGP Demonstration of PASS-GP method for GP classification
%
%  Description
%    Here we demonstrate PASS-GP method for Gaussian Processes
%    classification. Data used is 2-dimensional toy data with Gaussian
%    bumbs defining classes. We demonstrate with both fixed and not fixed
%    sizes of active set for PASS-GP.
%
%    PASS-GP uses a predictive active set selection method by Henao &
%    Winther (2012) to select a subset of training data to be used for
%    inference in classification problems.
%
%  Reference:
%    Ricardo Henao & Ole Winther (2012). Predictive active set selection
%    methods for Gaussian processes. Neurocomputing 80 (2012), 10-18.
%
%  See also PASSGP
%
% Copyright (c) 2013 Ville Tolvanen

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

% Generate toy data
prevstream=setrandstream(0);
[x1,x2]=meshgrid(-5:0.1:5,-5:0.1:5);
x=[x1(:) x2(:)]; x=x(randperm(size(x,1),3000),:);
y=2.*mnorm_pdf(x, [0 0], [0.5 0;0 0.5]) + mnorm_pdf(x, [3 3], [0.5 0;0 0.5]) + mnorm_pdf(x, [-3 -3], [0.5 0;0 0.5]);
y=y+mnorm_pdf(x, [3 -3], [0.5 0;0 0.5])+mnorm_pdf(x, [-3 3], [0.5 0;0 0.5]);
y=y+0.03.*randn(size(y,1),1);
y(y>0.15)=1; y(y<=0.15)=-1;

[xt1, xt2]=meshgrid(-5:0.23:5,-5:0.23:5);
xt=[xt1(:) xt2(:)];
yt=ones(size(xt,1),1);

[n, nin] = size(x);

% Define covariance and likelihood functions and create the model
gpcf = gpcf_sexp();
lik=lik_probit();
gp=gp_set('lik', lik, 'cf', gpcf, 'jitterSigma2', 1e-6);

opt=optimset('TolX',1e-3,'TolFun',1e-3,'display','on');
w0=gp_pak(gp);

% fPASS-GP with fixed size of 800 points in active set and data divided to
% 10 subsets with 4 sweeps over data
start=tic;[gp, indA]=passgp(gp, x, y, 'opt', opt, 'npass', 4, 'ninit', 800, 'nsub', 10, 'display', 'on', 'fixed', 'on', 'pexc', 0.1, 'optimn', 2);time=toc(start);
tt=time;
[Eft, Varft, lpyt, Eyt, Varyt]=gp_pred(gp, x(indA,:), y(indA,:), xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (fpass-gp)')
param=gp_pak(gp);

% PASS-GP with inclusion threshold 0.65, deletion threshold 0.99, intial
% size of 400 points in active set and 3 sweeps over data.
gp=gp_unpak(gp,w0);
start=tic;[gp, indA2]=passgp(gp, x, y, 'opt', opt, 'pinc', 0.65, 'pdel', 0.99, 'npass', 3, 'ninit', 400, 'nsub', 10, 'display', 'on', 'optimn', 2);time=toc(start);
tt2=time;
[Eft2, Varft2, lpyt2, Eyt2, Varyt2]=gp_pred(gp, x(indA2,:), y(indA2,:), xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt2),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (pass-gp)')
param2=gp_pak(gp);

% Full Gaussian process for comparison
gp=gp_unpak(gp,w0);
opt.Display='iter';
start=tic;gp=gp_optim(gp,x,y,'opt',opt);tt3=toc;
[Eft3, Varft3, lpyt3, Eyt3, Varyt3]=gp_pred(gp, x, y, xt, 'yt', yt);
figure, [cc,hh]=contour(reshape(xt(:,1),size(xt1,1), size(xt1,1)), reshape(xt(:,2),size(xt1,1), size(xt1,1)), reshape(exp(lpyt3),size(xt1,1), size(xt1,1)), [0.1 0.9]);
clabel(cc,hh); title('Pr(y==1) (full gp)')

% Display some statistics
mlpd_fpassgp=mean(mean(lpyt,2))
time_fpassgp=mean(tt)
mlpd_passgp=mean(mean(lpyt2,2))
time_passgp=mean(tt2)
mlpd_full=mean(lpyt3)
time_full=tt3

% Plot data and active sets for both methods
figure(4), subplot(1,2,1),  plot(x(y==1,1),x(y==1,2),'or',x(y==-1,1),x(y==-1,2),'ob');
hold all; plot(x(indA,1), x(indA,2), '.k'); title('Data and active set (fpass-gp)')
legend('y=1', 'y=-1', 'Active set for fpass-gp');
subplot(1,2,2),  plot(x(y==1,1),x(y==1,2),'or',x(y==-1,1),x(y==-1,2),'ob');
hold all; plot(x(indA2,1), x(indA2,2), '.k'); title('Data and active set (pass-gp)')
legend('y=1', 'y=-1', 'Active set for pass-gp');
setrandstream(prevstream);


function [  ] = DP_save_rows( cutCostsFile, cut_num,  search_k, saveFileAddress )
%DP_SAVE_ROWS Summary of this function goes here
%   Detailed explanation goes here

    file_t = fopen(cutCostsFile);
    cut_costs = fscanf(file_t, '%d');
    fclose(file_t);

    % get the initial averaging-cut

    cut_length = length(cut_costs) / cut_num;

    for i = 1:cut_num-1
       initial_cuts(i,:) = floor(i*cut_length);
    end

    % start Dynamic Programming
    % find the best/min value in the domain near cut_points as initial
    % values

    best_values = [];
    best_cuts = [];
    search_domain_start = floor(cut_length / search_k);
    search_domain_finish = floor(cut_length / search_k);

    sum_best_value = 0;

    for i = 1:length(initial_cuts)

        start = initial_cuts(i,:)-search_domain_start;
        finish = initial_cuts(i,:)+search_domain_finish;

        if start <= 0
            start = 1;
            search_domain_start = initial_cuts(i,:) - start;
        end

        if finish > length(cut_costs)-1
            finish  = length(cut_costs)-1;
            search_domain_finish = finish - initial_cuts(i,:);
        end

        domain = cut_costs(start:finish,:);

        % observe the minimum
%         [min_value, min_cut] = min(domain);
        % obserbe the maximum
        [min_value, min_cut] = max(domain);

        best_values(i,:) = min_value;

        if min_cut < initial_cuts(i,:)
            best_cuts(i,:) = initial_cuts(i,:) + (min_cut - search_domain_start - 1);
        else
            best_cuts(i,:) = initial_cuts(i,:) + (min_cut - search_domain_finish - 1);
        end

        if i > 1 && best_cuts(i,:) == best_cuts(i-1,:)
            best_cuts(i,:) = best_cuts(i,:) + 1;
            best_values(i,:) = domain(min_cut + 1);
        end

        sum_best_value = sum_best_value + best_values(i,:);

    end

    dlmwrite(saveFileAddress, best_cuts);
    sum_best_value

end


function polygon_properties_test07 ( )

%*****************************************************************************80
%
%% POLYGON_PROPERTIES_TEST07 tests POLYGON_EXPAND;
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 May 2014
%
%  Author:
%
%    John Burkardt
%
  n = 4;
  v = [ ...
    1.0, 1.0; ...
    5.0, 1.0; ...
    2.0, 4.0; ...
    1.0, 3.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYGON_PROPERTIES_TEST07\n' );
  fprintf ( 1, '  For a polygon:\n' );
  fprintf ( 1, '  POLYGON_EXPAND "expands" it by an amount H.\n' );

  h = 0.5;

  r8mat_transpose_print ( 2, n, v, '  The polygon vertices:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The expansion amount H = %g\n', h );

  w = polygon_expand ( n, v, h );

  r8mat_transpose_print ( 2, n, w, '  The expanded polygon:' );

  return
end


function a=update_a(dvec,n,alpha)
% a=update_a(dvec,n, alpha)
% Computes update for a_ki (KxI) in variational approximation (Buntine & Jakulin
% DCA 2006)
% dvec: data - each column is one image (J x I)
% n: (J x K x I)
% alpha, beta: parameters of the Gamma distribution for latent h (1xK each)
% J=#pixels
% K=#components
% I=#images.

[j,k,i]=size(n);

dvec_reshaped = reshape(dvec,j,1,i); % (Jx1xI)
atmp = sum(bsxfun(@times,dvec_reshaped,n),1); % (1xKxI)
a=bsxfun(@plus, squeeze(atmp), alpha'); % (KxI)


% op_CSIRemoveLipids.m
%
% Removes lipids from CSI data using L2 regularization. This minimizes the
% equation norm(x - x_0, 2) + beta * norm(W'x, 2)
%
% INPUT:
% MRSIStruct        = MRSI structure used in FID-A
% lipidComponents   = number of lipid spectra in the lipid basis
% lineWidthRange    = range of linewidth used for building lipid basis
% ppmRange          = ppm range used in building lipid basis
% beta              = regularization term
% plotBasis         = plot basis spectra
%
%
% OUTPUT:
% MRSIStruct        = MRSI structure with lipids removed

function [MRSIStruct] = op_CSIRemoveLipids(MRSIStruct, basisArguments, plottingArguments)
    arguments
        MRSIStruct (1, 1) struct
        basisArguments.lipidComponenets (1, 1) double = 1000
        basisArguments.lineWidthRange (1, 2) double = [1 80]
        basisArguments.lipidPPMRange (1, 2) double = [0.3 1.9000]
        basisArguments.beta (1, 1) double = 1e-4
        plottingArguments.plotBasis (1, 1) logical = false
    end
    % extract arguments from name value pairs
    lipidComponents = basisArguments.lipidComponenets;
    lineWidthRange = basisArguments.lineWidthRange;
    lipidPPMRange = basisArguments.lipidPPMRange;
    beta = basisArguments.beta;

    spectraSize = getSizeFromDimensions(MRSIStruct, {'t'});

    % calculate lipid basis used for L2 regularization
    lipidBasis = createLipipBasis(MRSIStruct, lipidComponents, lineWidthRange, lipidPPMRange);
    if(plottingArguments.plotBasis)
        figure
        plot(MRSIStruct.ppm, flip(real(lipidBasis),1));
    end
    % calculate solution from the basis
    L2Solution = inv(eye(spectraSize) + beta * (lipidBasis * lipidBasis'));

    [MRSIStruct, prevPermute, prevShape] = reshapeDimensions(MRSIStruct, {'t'});
    data = getData(MRSIStruct);

    data = L2Solution * data;
    MRSIStruct = setData(MRSIStruct, data);
    MRSIStruct = reshapeBack(MRSIStruct, prevPermute, prevShape);
end

% calculate lipid basis
function lipidBasis = createLipipBasis(MRSIStruct, lipidComponents, lineWidthRange, lipidPPMRange)
    spectralWidth = getSpectralWidth(MRSIStruct);

    spectralPoints = getSizeFromDimensions(MRSIStruct, {'t'});
    fidBasis = zeros(spectralPoints, lipidComponents);

    lipidStructure = load('Lip.mat', 'sysLip');
    lipidStructure = lipidStructure.sysLip;

    for iSpectra = 1:lipidComponents

        fidBasis(:, iSpectra) = getRandomLipidFids(spectralPoints, spectralWidth, ...
                                                  lineWidthRange, lipidPPMRange, ...
                                                  lipidStructure);
    end
    lipidBasis = fftshift(fft(fidBasis, [], 1), 1);
end

% calculate the single lipid spectra for the basis
function lipidFids = getRandomLipidFids(spectralPoints, spectralWidth, lineWidth, ...
                                        lipidPPMRange, lipidSystem)
    [randomLineWidth, randomPPM] = getRandomLineWidthandPPM(lineWidth, lipidPPMRange);
    lipidSystem.shifts = randomPPM;

    simulatedSignal = sim_onepulse(spectralPoints, spectralWidth, 3, randomLineWidth, lipidSystem);
    simulatedSignal = op_complexConj(simulatedSignal);
    simulatedSignal = addRandomPhase(simulatedSignal);
    %simulatedSignal = scaleSpectra(simulatedSignal, randomPPM, lipidPPMRange);
    lipidFids = simulatedSignal.fids;
end

% pick a random number from lower bounds and upper bounds
function randomNumber = randomNumberInRange(lowerBounds, upperBounds)
    difference = upperBounds - lowerBounds;
    randomNumber = lowerBounds + difference * rand(1);
end

function simulatedSignal = addRandomPhase(simulatedSignal)
    sepctralPhase = randomNumberInRange(-180, 180);
    simulatedSignal = op_addphase(simulatedSignal, sepctralPhase, 0, 4.65, 1);
end

% scale spectra based on a normal distribution. Signal near the center of lipid
% range will be scaled high and signal near the edges scaled lower.
function simulatedSignal = scaleSpectra(simulatedSignal, lipidPPM, lipidRange)
    normalProbability = normpdf(lipidPPM, mean(lipidRange), diff(lipidRange)/4);
    simulatedSignal = op_ampScale(simulatedSignal, normalProbability);
    simulatedSignal = op_ampScale(simulatedSignal, 10);

end

function [randomLineWidth, randomPPM] = getRandomLineWidthandPPM(lineWidth, lipidPPMRange)
    randomLineWidth = randomNumberInRange(lineWidth(1), lineWidth(2));
    randomPPM = randomNumberInRange(lipidPPMRange(1), lipidPPMRange(2));
end


function pcut = addSinCosCuts(p)

pcut = p;
if ~isempty(p.evalMap)
    sin_ = [];
    cos_ = [];
    for i = 1:length(p.evalMap)
        if isequal(p.evalMap{i}.fcn,'sin')
            sin_ = [sin_;p.evalMap{i}.variableIndex p.evalMap{i}.computes];
        elseif isequal(p.evalMap{i}.fcn,'cos')
            cos_ = [cos_;p.evalMap{i}.variableIndex p.evalMap{i}.computes];
        end
    end
    if ~isempty(sin_) && ~isempty(cos_) && ~isempty(intersect(sin_(:,1),cos_(:,1)))
        for i = 1:size(sin_,1)
            j = find(sin_(i,1)==cos_(:,1));
            if ~isempty(j)
                k1 = sin_(i,2);
                k2 = cos_(j,2);
                k3 = sin_(i,1);
                % x_k1 and x_k2 rpresent sin and cos of x_k3

                % Construct the SOCP model for 1 >= x_k1^2 + x_k2^2
                % i.e. norm([x_k1;x_k2]) <= 1
                % i.e. [1;x_k1;x_k2] in socp cone
                if 0
                    F_structemp = spalloc(3,length(p.c)+1,5);
                    F_structemp(:,1) = [1;0;0];
                    F_structemp(2,1+k1) = 1;
                    F_structemp(3,1+k2) = 1;
                    K.f = 0;
                    K.l = 0;
                    K.s = 0;
                    K.e = 0;
                    K.q = 3;
                    localModel1 = createNumericalModel(F_structemp,K);
                    pcut = mergeNumericalModels(pcut,localModel1);
                end

                % We also add simple linear cuts, in case the lower bound
                % solver doesn't support socps. FIX ME: Decide globally,
                % but make sure it is consistent with stand-alone envelope
                % y <= sqrt(2)-x, y>=-sqrt(2)+x,
                % y <= sqrt(2)+x, y>=-sqrt(2)-x
                % sqrt(2)-x-y, sqrt(2)+x-y, sqrt(2)-x+y, sqrt(2)+x+y
                F_structemp = spalloc(4,length(p.c)+1,5);
                F_structemp(:,1) = sqrt(2)*[1;1;1;1];
                F_structemp(1,1+k1) = -1;
                F_structemp(1,1+k2) = -1;
                F_structemp(2,1+k1) = 1;
                F_structemp(2,1+k2) = -1;
                F_structemp(3,1+k1) = -1;
                F_structemp(3,1+k2) = 1;
                F_structemp(4,1+k1) = 1;
                F_structemp(4,1+k2) = 1;
                K.f = 0;
                K.l = 4;
                K.s = 0;
                K.e = 0;
                K.q = 0;
                localModel2 = createNumericalModel(F_structemp,K);
                pcut = mergeNumericalModels(pcut,localModel2);
            end
        end
    end
end


% Find out how big the cliques are in an HHMM as a function of depth
% (This is how we get the complexity bound of O(D K^{1.5D}).)

if 0
Qsize = [];
Fsize = [];
Nclqs = [];
end

ds = 1:15;

for d = ds
  allQ = 1;
  [intra, inter, Qnodes, Fnodes, Onode] = mk_hhmm_topo(d, allQ);

  N = length(intra);
  ns = 2*ones(1,N);

  bnet = mk_dbn(intra, inter, ns);
  for i=1:N
    bnet.CPD{i} = tabular_CPD(bnet, i);
  end

  if 0
    T = 5;
    dag = unroll_dbn_topology(intra, inter, T);
    engine = jtree_unrolled_dbn_inf_engine(bnet, T, 'constrained', 1);
    S = struct(engine);
    S1 = struct(S.sub_engine);
  end

  engine = jtree_dbn_inf_engine(bnet);
  S = struct(engine);
  J = S.jtree_struct;

  ss = 2*d+1;
  Qnodes2 = Qnodes + ss;
  QQnodes = [Qnodes Qnodes2];

  % find out how many Q nodes in each clique, and how many F nodes
  C = length(J.cliques);
  Nclqs(d) = 0;
  for c=1:C
    Qsize(c,d) = length(myintersect(J.cliques{c}, QQnodes));
    Fsize(c,d) = length(myintersect(J.cliques{c}, Fnodes));
    if length(J.cliques{c}) > 1 % exclude observed leaves
      Nclqs(d) = Nclqs(d) + 1;
    end
  end
  %pred_max_Qsize(d) = ceil(d+(d+1)/2);
  pred_max_Qsize(d) = ceil(1.5*d);

  fprintf('d=%d\n', d);
  %fprintf('D=%d, max F = %d. max Q = %d, pred max Q = %d\n', ...
	%  D, max(Fsize), max(Qsize), ceil(D+(D+1)/2));

  %histc(Qsize,1:max(Qsize)) % how many of each size?
end % next d

Q = 2;
pred_mass = ds.*(Q.^ds) + Q.^(ceil(1.5 * ds))
pred_mass2 = Q.^(ceil(1.5 * ds))

for d=ds
  mass(d) = 0;
  for c=1:C
    mass(d) = mass(d) + Q^Qsize(c,d);
  end
end

if 0
%plot(ds, max(Qsize), 'o-',  ds, pred_max_Qsize, '*--');
%plot(ds, max(Qsize), 'o-',  ds, 1.5*ds, '*--');
%plot(ds, mass, 'o-',  ds, pred_mass, '*--');
D = 15;
%plot(ds(1:D), mass(1:D), 'bo-',  ds(1:D), pred_mass(1:D), 'g*--', ds(1:D), pred_mass2(1:D), 'k+-.');
plot(ds(1:D), log(mass(1:D)), 'bo-',  ds(1:D), log(pred_mass(1:D)), 'g*--', ds(1:D), log(pred_mass2(1:D)), 'k+-.');

grid on
xlabel('depth of hierarchy')
title('max num Q nodes in any clique vs. depth')
legend('actual', 'predicted')

%previewfig(gcf, 'width', 3, 'height', 1.5, 'color', 'bw');
%exportfig(gcf, '/home/cs/murphyk/WP/ConferencePapers/HHMM/clqsize2.eps', ...
%          'width', 3, 'height', 1.5, 'color', 'bw');

end

if 0
for d=ds
  effnumclqs(d) = length(find(Qsize(:,d)>0));
end
ds = 1:10;
Qs = 2:10;
maxC = size(Qsize, 1);
cost = [];
cost_bound = [];
for qi=1:length(Qs)
  Q = Qs(qi);
  for d=ds
    cost(d,qi) = 0;
    for c=1:maxC
      if length(Qsize(c,d) > 0) % this clique contains Q nodes
	cost(d,qi) = cost(d,qi) + Q^Qsize(c,d)*2^Fsize(c,d);
      end
    end
    %cost_bound(d,qi) = effnumclqs(d) * 8 * Q^(max(Qsize(:,d)));
    cost_bound(d,qi) = (effnumclqs(d)*8) + Q^(max(Qsize(:,d)));
  end
end

qi=2; plot(ds, cost(:,qi), 'o-',  ds, cost_bound(:,qi), '*--');
end

if 0
% convert numbers in cliques into names
for d=1:D
  Fdecode(Fnodes(d)) = d;
end
for c=8:15
  clqs = J.cliques{c};
  fprintf('clique %d: ', c);
  for k=clqs
    if myismember(k, Qnodes)
      fprintf('Q%d ', k)
    elseif myismember(k, Fnodes)
      fprintf('F%d ', Fdecode(k))
    elseif isequal(k, Onode)
      fprintf('O ')
    elseif myismember(k, Qnodes2)
      fprintf('Q%d* ', k-ss)
    else
      error(['unrecognized node ' k])
    end
  end
  fprintf('\n');
end
end


% Prabhakar. S
% AE 110 Lab Assignment
% Satellite Tracking Program
% Satellite:  NOAA 12
% Program Name elaz : Elevation-Azimuth being called by the program sts

% Orbital Elements ready for degree to radians conversion!
% Two Line Elements and the Epoch Data are obtained from the GUI Interface

load variables.mat; % Loading the variables from the GUI Interface sts
I2 = yoe;		% Year of epoch
J2 = moe;			% Month of epoch
%h2 = hoe;       % Hour of epoch
%m2 = mioe;       % Minute of epoch
%K2 = doe + hoe/24 + mioe/(24*60);		% Day of epoch
K2=doe;
zero=0; % For printing a single digit month or day preceeded by a zero.
%ut=.08235372;
%frac=.08235372; % fraction of the day from the TLE

% This calculation is to find the Julian Date,
% JD Fortran code written by Fliegel and Van Flandern [1968]
% Handout by Dr. P

JD = (367*I2 - (fix(7*(I2 + fix((J2 + 9)/12))/4)) + fix((275*J2)/9) + K2 + 1721013.5 + frac);

%JD = (K2 - 32075 + round(1461 * (I2 + 4800 + (J2 -14)/12)/4 ...
%+ 367* (J2 - 2 - (J2 - 14)/12*12)/12 - 3*((I2 + 4900 + (J2 - 14)/12)/100)/4)) ;
% fprintf('\n Two Line Elements Data - 28 Dec 2000, Time 11:30:00\n');
% fprintf(' The Julian Date is %f',JD);

% Propagation begins
I = yoe;								% Year
J = moe;								% Month
%h = hoe;   				   		% Hour
%m = mioe;     				 		% Minute
%K = doe + hoe/24 + mioe/(24*60);	% Day
K=doe;

JD0 = (367*I - (fix(7*(I + fix((J + 9)/12))/4)) + fix((275*J)/9) + K + 1721013.5 + frac);

%JD0 = (K - 32075 + round(1461 * (I + 4800 + (J -14)/12)/4 ...
%+ 367 * (J - 2 - (J - 14)/12*12)/12 - 3*((I + 4900 + (J - 14)/12)/100)/4)) ;

I3 = yoe;							% Year
J3 = moe;							% Month
%h3 = hoe;       					% Hour
%m3 = mioe;       				% Minute
%K3 = (doe+pro) + h3/24 + m3/(24*60);	% Day
K3 = (doe+pro);

JD1 = (367*I3 - (fix(7*(I3 + fix((J3 + 9)/12))/4)) + fix((275*J3)/9) + K3 + 1721013.5 + frac);

%JD1 = K3 - 32075 + round(1461 * (I3 + 4800 + (J3 -14)/12)/4 ...
%  + 367 * (J3 - 2 - (J3 - 14)/12*12)/12 - 3*((I3 + 4900 + (J3 - 14)/12)/100)/4);
% Propagation ends here.

% Propagation step Size
step = 5/(24*60); % right now for every 5 minutes
j=1;

fprintf('\n\t\t\t SATELLITE    TRACKING    SYSTEM  \n');
fprintf(' ---------------------------------------------------------------------------');
fprintf('\n				             G r e g o r i a n   D a t e \n');
fprintf(' ---------------------------------------------------------------------------');
fprintf('\n  Julian Date      Azimuth     Elevation  Month/Day/Year Hour:Minute:Seconds\n');
fprintf(' ---------------------------------------------------------------------------');

while JD0<JD1;

% Declaring variables and initializing values

degtorad = pi/180;		% Radians conversion factor
i = incl*degtorad;		% Inclination
CapOmega = raan*degtorad;		% Right Ascension of the Ascending Node - denoted by symbol Capital Omega
ecc = e;					% Eccentricity
SmallOmega = aop*degtorad;						% Argument of Periapsis - denoted by  small Omega
Mo = ma * degtorad;		% Initial Mean anomaly
n = revs;	   		% Mean Motion (revs/day)
GP = 3.986e5;				% Gravitational parameter - denoted by nu
a = 24000;					% Semi-major Axis
% a = (GP/n^2)^(1/3);
% fprintf('Semi-major axis is computed to be :\n',a);

Latitude = 0 * degtorad;
Longitude = 280 * degtorad;
L = ((90*degtorad) - Latitude);

We = 6.300388097;		% Angular velocity of planet Earth
perturb = 98.9246096453622 * degtorad; % Perturbation due to the Oblateness
tq = 2448621.5; % JD for
Qg = perturb + We * (JD0 - tq);
Q = Qg + Longitude;

tp = JD - Mo/n;
M = n * (JD0 - tp);

E = M;				          		% Use M for the first value of E
	f=M-E+ecc*sin(E);			 		% Kepler's Equation
	while abs(f)>0.00000001; 		% Loop for Kepler's equation begins
		f=M-E+ecc*sin(E);			 	%
		fd=-1+ecc*cos(E);			 	%
		E=E-f/fd;						%
	end;							 		% loop for Kepler's eqn ends here!

% Computing true anomaly and radius

TA=2*atan((((1+ecc)/(1-ecc))^0.5)*tan(E/2));
    if TA<0;
    	  TA=2*pi+TA;
    end;

r=a*(1-ecc*cos(E)); % radius

% Coordinate transformations

adot = (-(3/2) * n * 1.0827e-3 * (6378.14/a)^2 * cos(i))/(1-ecc^2)^2;
A = CapOmega + adot * (JD0 - JD);
SmallOmega = SmallOmega + adot * (JD0 - JD);

Rseu = [0; 0; 6378.14];
Txyz = [ cos(A) -sin(A) 0; sin(A) cos(A) 0; 0 0 1 ] * [ 1 0 0; 0 cos(i) -sin(i); 0 sin(i) cos(i)] * ...
   [ cos(SmallOmega) -sin(SmallOmega) 0; sin(SmallOmega) cos(SmallOmega) 0; 0 0 1 ];
Tseu = [ cos(L) 0 -sin(L); 0 1 0; sin(L) 0 cos(L)] * [ cos(Q) sin(Q) 0; -sin(Q) cos(Q) 0; 0 0 1];
ruvw = [ (r * cos(TA)); (r * sin(TA)); 0 ];
Pseu = Tseu*(Txyz*ruvw) - Rseu;
P = (Pseu(1)^2 + Pseu(2)^2 + Pseu(3)^2)^0.5;

El = (asin ( Pseu(3)/P ))/degtorad;
Az = (atan2 ( Pseu(2), -Pseu(1)))/degtorad;
	if Az < 0;
   	Az = 360 + Az;
   end;

%Loop begins here  %if El>0;

array(j,1) = JD0;
array(j,3) = Az;
array(j,4) = El;
%array(j,5) = cat(year,month,day);
j = j+1;
% loop ends here

JD0 = JD0 + step;
			% Gregorian Date from Julian Date
			% This calculation is valid for any Julian Day Number including negative JDN and
			% produces a Gregorian date (or possibly a proleptic Gregorian date)

			Z = floor(JD0 - 1721118.5); % JD0 is the Julian Date in propagation
			R = (JD0 - 1721118.5 - Z);  % R is the fractional part of JD0
			G = (Z - .25);
			A = (floor(G / 36524.25));  % Calculate the value of A which is the number of full centuries
			B = (A - (A / 4)); % The value of B is this number of days minus a constant
			year = (floor((B+G) / 365.25)); % Calculate the value of Y, the year in a calendar whose years start on March 1
			C = (B + Z - floor(365.25 * year));  % Day count
         month = (fix((5 * C + 456) / 153));  % Month
         UT = (C - fix((153 * month - 457) / 5) + R); % Calculation for UTC
         day = floor(UT);  % Gregorian Day

						if month > 12;
		      			year = year + 1 ;
		      			month = month - 12;
                  end;
                  UT = UT - floor(UT);
         UT = UT*24;
         hr = floor(UT);   %  hour
         UT = UT-floor(UT);
	      UT =UT* 60;
	      min = floor(UT);  % minute
   	   UT =UT- floor(UT);
      	UT =UT* 60;
         secs = round(UT); % seconds
         if secs==60;
            min=min+1;
            secs=0;
         end;
         % Gregorian Date conversion algorithm ends here

         % computations for printintg two digits in case year,month,day,min and secs is < 10
         % (i.e) to print third month, 3 as 03 and day 5 as 05 etc..
         ZERO=int2str(zero);
			MIN=int2str(min); % converting minute into a string
			if min <10;
		  			 MinNew=strcat(ZERO,MIN);
                  min=MinNew;
               else;
                  min=MIN;
               end;

         	DAY=int2str(day); % converting DAY into a string
				 if day <10;
		  				 DayNew=strcat(ZERO,DAY);
                   day=DayNew;
                else;
                  day=DAY;
         	 end;
          HR=int2str(hr); % converting Hour into a string
					if hr <10;
		  				 HrNew=strcat(ZERO,HR);
                   hr=HrNew;
               else;
                 	 hr=HR;
		          end;

                MONTH=int2str(month); % converting month into a string
						if month <10;
			  				 MonthNew=strcat(ZERO,MONTH);
   	                month=MonthNew;
            	   else
         	         month=MONTH;
                  end;

                 SECS=int2str(secs); % converting seconds into a string
						if secs <10;
			  				 SecsNew=strcat(ZERO,SECS);
   	                secs=SecsNew;
            	   else
         	         secs=SECS;
			         end;

         % computations for printing two digits ends here!

         if El>0;
            fprintf('\n %f | %f | %f |  %s/%s/%4d  |  %s:%s:%s  |',JD0, Az, El, month,day,year, hr,min,secs);
            %plot(El,Az);
         end;
end;
fprintf('\n ---------------------------------------------------------------------------\n');
fprintf('\n\t\t\t       End of Output\!!! \n\n\n');


function [x] = tt_meshgrid(varargin)
%Analogue of the meshgrid function for the TT-format
%   X = TT_MESHGRID(A,B,C,...) Computes the meshgrid based on "1d"
%       representations
%   X = TT_MESHGRID(A) Computes the meshgrid based on the cell array A of
%       the representations
%   X = TT_MESHGRID(T,D) Computes the d-dimensional meshgrid, using T as a
%       one-dimensional grid
if ( numel(varargin) == 2 )
    if ( ismatrix(varargin{2}) )
        d = varargin{2};
        t = varargin{1};
        z = cell(d,1);
        for i = 1:d
            z{i} = t;
        end
    else
        z = varargin{1};
    end
else
    z = varargin{1};
end
if ~iscell(z)
    z = varargin;
end
d = numel(z);
x = cell(d,1);
e = cell(d,1);
for i = 1:d
    e{i} = tt_ones(size(z{i}));
end
for i = 1:d
    v = z{i};
    for j = i-1:-1:1
        v = kron(e{j},v);
    end
    for j = i+1:d
        v = kron(v,e{j});
    end
    x{i} = v;
end
end


%% MTEX check calcEBSD and calcODF
% check for the dependency between the number of sample
% orientations and the error between the estimated and the
% true ODF

for i = 1:5

  ebsd = discreteSample(SantaFe,10^i);

  odf = calcODF(ebsd);

  e(i) = calcError(odf,SantaFe,'resolution',2.5*degree);

end

plot(e)


function g = gaussianPriorGradient(prior, x)

% GAUSSIANPRIORGRADIENT Gradient wrt x of the log Gaussian prior.

% PRIOR

% Compute gradient of prior
g = -prior.precision*x;


 function[M1,M2,M3] = integrate_vf(k,w,f,v)
      M1   = (k*sum(v .* w .* f));    % Number density
      M2   = (k*sum((v .^2 ) .* w .* f));   % Macrospic moment in x
      M3   = (k*sum(1/2*( v .*abs(v).^2 ).* w .* f)); % Energy Density
 end


%  Figure 10.27      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
%  fig10_27.m is a script to generate Fig. 10.27, the
%  step response of the collocated
%  design for the satellite with PD compensation
clf;
m=[1, .1]; k0=[0, .091] ; d0=[0, .0036]; k1=[0, .4];
[f,g,h,j]=twomass(m,k0,d0); [f1,g,h,j] = twomass(m,k1,d0);
h1=[0, 0, 1, 0];

nc1=0.25*[2, 1];
dc1=[1/40, 1];

[ac,bc,cc,dc]=tf2ss(nc1, dc1);
[aol,bol,col,dol]= series(ac, bc,cc,dc,f,g,h1,j);
[acl]=aol-bol*col;
ccl2=[0*cc h]
[aol1,bol1,col1,dol1] = series(ac,bc,cc,dc,f1,g,h1,j);
acl1=aol1-bol1*col1;
t=0:.3:50;
sys1=ss(acl,bol,col,dol);
step(sys1,t);
hold on;
grid;
sys2=ss(acl1,bol1,col1,dol1);
step(sys2,t);
title('Closed-loop step response for D_5(s)G_{co}(s).')


function tone=note(keynum, dur)

fs=11025;
tt = 0:(1/fs):dur;

%  This generates white noise for whatever duration specified..  good snare
%  sound with the right envelope, but matlab developed timing problems with this.  I'll probably
%  try to get these running sometime after lab
 if keynum == 2
     tone=rand(1,length(tt));
     return;
 end

%generates rests
if keynum == 0
%for kk = 1:length(tt)
    tone([1:length(tt)]) = 0;
    %end
return;
end

%adding these octaves rounded out the sound.
freq=440*2^((keynum-49)/12);
freq3=freq*3;
freq5=freq*5;
freq9=freq*9;
freq7=freq*7;
tone1 = .75*sin(2*pi*freq*tt);
 tone3 = .65*sin(2*pi*freq3*tt);
 tone5=.5*sin(2*pi*freq5*tt);
 tone9 = .222*sin(2*pi*freq9*tt);
 tone7 = .12*sin(2*pi*freq7*tt);
 tone12= 1*sin(2*pi*freq*12*tt);

tone=tone1+tone3+tone5+tone7+tone9;%+tone12;


function calpak_test336 ( )

%*****************************************************************************80
%
%% CALPAK_TEST336 tests MONTH_LENGTH_EG_CIVIL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 March 2013
%
%  Author:
%
%    John Burkardt
%
  n_test = 2

  fprintf ( 1, '\n' );
  fprintf ( 1, 'CALPAK_TEST336\n' );
  fprintf ( 1, '  For the Egyptian Civil calendar,\n' );
  fprintf ( 1, '  MONTH_LENGTH_EG_CIVIL returns month lengths.\n' );

  y_test(1) = 3;
  y_test(2) = 4;

  for i_test = 1 : n_test

    y = y_test(i_test);
    sy = y_to_s_eg_civil ( y );
    months = year_length_months_eg_civil ( y );
    days = year_length_eg_civil ( y );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %d\n', y );
    fprintf ( 1, '  %s\n', sy );
    fprintf ( 1, '  Year length in months = %d\n', months );
    fprintf ( 1, '  Year length in days = %d\n', days );
    fprintf ( 1, '\n' );

    for m = 1 : months
      month_name = month_to_month_name_eg_civil ( m );
      fprintf ( 1, '  %10s  %2d\n', month_name, month_length_eg_civil ( y, m ) );
    end

  end

  return
end


function [ a, info ] = dpofa ( a, lda, n )

%*****************************************************************************80
%
%% DPOFA factors a real symmetric positive definite matrix.
%
%  Discussion:
%
%    DPOFA is usually called by DPOCO, but it can be called
%    directly with a saving in time if RCOND is not needed.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the symmetric matrix to be  factored.  Only the
%    diagonal and upper triangle are used.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(LDA,N), an upper triangular matrix R so that A = R'*R
%    where R' is the transpose.  The strict lower triangle is unaltered.
%    If INFO /= 0, the factorization is not complete.
%
%    Output, integer INFO, error flag.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
  for j = 1 : n

    s = 0.0;

    for k = 1 : j-1
      t = a(k,j) - ddot ( k-1, a(1:k-1,k), 1, a(1:k-1,j), 1 );
      t = t / a(k,k);
      a(k,j) = t;
      s = s + t * t;
    end

    s = a(j,j) - s;

    if ( s <= 0.0 )
      info = j;
      return
    end

    a(j,j) = sqrt ( s );

  end

  info = 0;

  return
end


function hand_plot ( )

%*****************************************************************************80
%
%% HAND_PLOT plots the hand data.
%
%  Discussion:
%
%     This program assumes that the file 'HAND_NODES.TXT' is available.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 April 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Cleve Moler,
%    Numerical Computing with MATLAB,
%    SIAM, 2004,
%    ISBN13: 978-0-898716-60-3,
%    LC: QA297.M625.
%

%
%  Read the data.
%
  xy = load ( 'hand_nodes.txt' );
%
%  Make XY an array of column vectors.
%
  xy = xy';
%
%  Repeat the first column at the end so the polygon closes.
%
  xy = [ xy, [ xy(:,1) ] ];
%
%  Clear the graphics frame.
%
  clf

  plot ( xy(1,:), xy(2,:), 'Color', 'r', 'LineWidth', 2 );
  hold on;
  plot ( xy(1,:), xy(2,:), 'b.', 'MarkerSize', 15 );
  axis equal
  grid on
  title ( 'Hand data and straight line interpolant' )

  hold off

  return
end


function [d pred] = dag_sp(A,u,varargin)
% DAG_SP Compute the weighted single source shortest path problem.
%
% The DAG shortest path algorithm for the single source shortest path
% problem only works on directed acyclic-graphs (DAGs).
%
% If the graph is not a DAG, the results are undefined.  In the future, the
% function may throw an error if the graph is not a DAG.
%
% See the shortest_paths function for calling information.  This function
% just calls shortest_paths(...,struct('algname','dag'));
%
% This algorithm works on weighted directed acyclic graphs.
% The runtime is O(V+E)
%
% ... = clustering_coefficients(A,...) takes a set of
% key-value pairs or an options structure.  See set_matlab_bgl_options
% for the standard options.
%    There are no additional options for this function.
%
% Example:
%    load graphs/kt-3-7.mat
%    dag_sp(A,1)
%
% See also SHORTEST_PATHS

% David Gleich
% Copyright, Stanford University, 2006-2008

%% History
%  2006-04-23: Initial version
%  2008-10-07: Changed options parsing
%%

algname = 'dag';
if ~isempty(varargin),
    options = merge_options(struct(),varargin{:});
    options.algname= algname;
else options = struct('algname',algname);
end

[d pred] = shortest_paths(A,u,options);


function c=ref_gdgt(f,g,a,M,c_t,c_f,c_w)
%REF_GDGT  Reference generalized DGT
%   Usage:  c=ref_dgtiv(f,g,a,M,c_t,c_f,c_w);
%
%   Linear algebra version of the algorithm. Create big matrix
%   containing all the basis functions and multiply with the transpose.

L=size(f,1);

N=L/a;

F=zeros(L,M*N);

l=(0:L-1).';

for n=0:N-1
  for m=0:M-1
    F(:,M*n+m+1)=exp(2*pi*i*(m+c_f)*(l+c_t)/M).*circshift(g,n*a+c_w);
  end;
end;

c=F'*f;