function test_polygon_recovery(record)
import iris.inflate_region;

if nargin < 1
  record = false;
end

r = 1;

lb = [-r;-r];
ub = [r;r];

n_obs = 7;
th = linspace(0,2*pi,n_obs+1);
% th = [0,pi/4,pi/2,pi,5*pi/4,3*pi/2,2*pi];

% th = cumsum(rand(6, 1) * 3*pi./2);
% th = th * (2*pi / th(end));

obstacles = zeros(2, 2, length(th)-1);
for j = 1:length(th)-1
  obstacles(:,:,j) = [[r*cos(th(j));r*sin(th(j))],[r*cos(th(j+1));r*sin(th(j+1))]];
end

A_bounds = [-1,0;0,-1;1,0;0,1];
b_bounds = [-lb; ub];

start = 0.5 .* (rand(2,1) .* (ub - lb) + lb);
% start = [0;0];
% start = [0;0.98];

% profile on
[A,b,C,d,results] = inflate_region(obstacles, A_bounds, b_bounds, start);
% profile viewer
iris.drawing.animate_results(results, record);

end


function obj = GEVD_method(obj)

%the proposed GEVD-HBF scheme

global H Vn  Nrf Nt Nr W_mopt;
t1 = clock;
i = 0;   %itertion index

%just to achieve W_RF*W_D = W_mopt
W_equal = W_mopt;
w = trace (W_equal' * W_equal);
%random initialization
V_RF = exp( 1i*unifrnd(0,2*pi,Nt,Nrf));
W_RF = exp( 1i*unifrnd(0,2*pi,Nr,Nrf));
%iteration trigger, the normal initialization just for pass into functions
trigger = 1;
m_MSE_new = 100;

%limit the iterations number by i<10
while (trigger > 1e-5 && i<10)

    % precoding
    H1 = H' * W_equal;
    [V_RF, V_U] = gevd_algorithm(V_RF, w, H1);
    V_equal = V_RF *V_U;
    v = trace (V_equal * V_equal');   %beta^(-2)

    %combining
    H2 = H * V_equal;
    [W_RF, W_B] = gevd_algorithm(W_RF, v, H2);
    W_equal = W_RF * W_B;
    w = trace (W_equal' * W_equal);
    %modified MSE
    H_equal = W_equal'*H2;

    m_MSE_old = m_MSE_new;
    m_MSE_new = trace(H_equal * H_equal' - H_equal - H_equal') + Vn * v * w;
    trigger = m_MSE_old - m_MSE_new;

    i = i + 1;
end

V_B = V_U / sqrt(v);

t2 = clock;
runtime = etime(t2,t1);
obj.runtime = obj.runtime + runtime;
obj.V_B = V_B;
obj.W_B = W_B;
obj.V_RF = V_RF;
obj.W_RF = W_RF;
obj = get_metric(obj);


classdef TestingOrthogonalCorrectors < handle

    properties (Access = public)

    end

    properties (Access = private)

    end

    properties (Access = private)
       mesh
       orientation
       interpolator
       orthogonalCorrector
    end

    methods (Access = public)

        function obj = TestingOrthogonalCorrectors()
          %  obj.createToyMesh();
          %  obj.createToyOrientation();
            obj.createBenchmarkMesh();
            obj.createBenchmarkOrientation();
            obj.plotOrientationVector();
            obj.createInterpolator();
            obj.createOrthogonalCorrector();
        end

    end

    methods (Access = private)

        function createToyMesh(obj)
            x = [66.90 128.89 115.25 76.73 26.84 157.24 ...
                168.58 141.74 98.65 45.74 3.78 2.65 31.37 ...
                83.53 137.21 174.63 147.03 99.03 38.55];
            y = [89.20 89.58 120.20 130.40 119.44 113.39 ...
                153.08 154.59 160.26 164.80 152.32 220.74 ...
                207.13 202.977 190.88 192.77 239.64 241.90 242.28];
            s.coord(:,1) = x;
            s.coord(:,2) = y;
            s.connec = delaunay(s.coord);
            m = Mesh(s);
            m.plot();
            obj.mesh = m;
        end

        function createToyOrientation(obj)
            alpha = pi/180*[190 140 300 150 185 ...
                            120 75  45  150 130 ...
                            160+180 0   190  5  -5   ...
                            170 0   190 190];
            a(:,1) = cos(alpha);
            a(:,2) = sin(alpha);
            obj.orientation = a;
        end

       function createBenchmarkMesh(obj)
           h = 0.03;
           xmin = 0.50;
           xmax = 2.0;
           ymin = 0.25;
           ymax = 1.75;
           xv = xmin:h:xmax;
           yv = ymin:h:ymax;
           [X,Y] = meshgrid(xv,yv);
           s.coord(:,1) = X(:);
           s.coord(:,2) = Y(:);
           s.connec = delaunay(s.coord);
            m = Mesh(s);
            obj.mesh = m;
        end

        function createBenchmarkOrientation(obj)
            s1 = 0.32;
            s2 = -0.8;
            x1 = obj.mesh.coord(:,1);
            x2 = obj.mesh.coord(:,2);
            v(:,1) = cos(pi*(x1 + s1*x2));
            v(:,2) = cos(pi*(x2 + s2*x1));
            beta = atan2(v(:,2),v(:,1));
            alpha = beta/2;
            obj.orientation(:,1) = cos(alpha);
            obj.orientation(:,2) = sin(alpha);
        end

        function plotOrientationVector(obj)
            %figure()
            a = obj.orientation;
            x = obj.mesh.coord(:,1);
            y = obj.mesh.coord(:,2);
            ax = a(:,1);
            ay = a(:,2);
            quiver(x,y,ax,ay);
        end

        function createInterpolator(obj)
            s.meshCont    = obj.mesh;
            s.meshDisc    = obj.mesh.createDiscontinousMesh;
            s.orientation = obj.orientation;
            s = SymmetricContMapCondition(s);
            sC = s.computeCondition();
            obj.interpolator = sC;
        end

        function createOrthogonalCorrector(obj)
            s.mesh            = obj.mesh;
            s.interpolator    = obj.interpolator;
            s.correctorValue  = obj.computeCorrector();
            o = OrthogonalCorrectorComputer(s);
            oC = o.compute();
            o.plot();
            obj.orthogonalCorrector = oC;
        end

        function cV = computeCorrector(obj)
            s.mesh               = obj.mesh;
            s.orientation        = obj.orientation;
            s.singularityCoord   = obj.computeSingularities();
            c = CorrectorComputer(s);
            cV = c.compute();
            c.plot()
        end

        function sCoord = computeSingularities(obj)
            s.mesh        = obj.mesh;
            s.orientation = obj.orientation;
            sF = SingularitiesFinder(s);
            isS = sF.computeSingularElements();
            sF.plot();
            coordB = obj.mesh.computeBaricenter();
            coordB = transpose(coordB);
            sCoord =  coordB(isS,:);
            sCoord = sCoord(1,:);
        end

    end

end


%glinspace - Gamma-corrected linearly spaced vector.
%
%  USAGE
%
%    x = glinspace(d1,d2,gamma,n)
%
%    d1             start value
%    d2             stop value
%    gamma          gamma value
%    n              number of values
%
%  SEE ALSO
%
%    See also glinspace.
%

% Copyright (C) 2013 by Michaël Zugaro
%
% 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.

function x = glinspace(d1,d2,gamma,n)

% Check number of parameters
if nargin < 3,
  error('Incorrect number of parameters (type ''help glinspace'' for details).');
end

% Check parameters
if ~isdscalar(d1),
  error('Incorrect start value (type ''help glinspace'' for details).');
end
if ~isdscalar(d2),
  error('Incorrect stop value (type ''help glinspace'' for details).');
end
if ~isdscalar(gamma) || gamma <= 0,
  error('Incorrect start value (type ''help glinspace'' for details).');
end
if nargin < 3,
	n = 100;
end
if ~isiscalar(n),
  error('Incorrect number of values (type ''help glinspace'' for details).');
end

if d1 > d2
	gamma = 1/gamma;
end

x = linspace(0,1,n) .^ (1/gamma);
x = x *(d2-d1) + d1;


function lbls = transformLabels(labels)

% TRANSFORMLABELS A small utility which transform the label vector: if labels are in 1-of-K encoding it transforms it into a vector of
% integers and vice versa.
% SEEALSO: utils_transformMultiLabel
%
% COPYRIGHT: Andreas C. Damianou, 2012

% VARGPLVM

if size(labels,1) == 1 || size(labels,2) ==1
    N = max(size(labels,1), size(labels,2));
    % The labels are not 1-K-encoding
    uniqueLabels = unique(labels);
    numClasses = length(uniqueLabels);
    lbls = zeros(N, numClasses);
    for c = 1:numClasses
        lbls(labels == uniqueLabels(c),c) = 1;
    end
else
    multilabel = false;
    N = size(labels,1);
    % This is in case we have e.g. 1 -1 -1 instead of 1 0 0
    labels(labels==-1)=0;
   % numClasses = size(labels,2);
    lbls = zeros(1,N);
    for n=1:N
        indx = find(labels(n,:));
        if length(indx) == 1
            lbls(1,n) = indx;
        else
            % WARNING!!!! When point n belongs to more than one classes,
            % this function will RANDOMLY select one!!!! This should be
            % fixed in future releases!
            permIndx = randperm(length(indx));
            lbls(1,n) = permIndx(1);
            multilabel = true;
        end
    end
    if multilabel
        warning('This is a multilabel dataset, transformLabels just kept one label per point randomly!')
    end
end


function H = recnotch(notch,mode,M,N,W,S)
%RECNOTCH Generates axes notch filter transfer functions.
%   H = RECNOTCH(NOTCH,MODE,M,N,W,S) generates an M-by-N notch filter
%   transfer function consisting of symmetric pairs of rectangles of
%   width W placed on the vertical and/or horizontal axes of the
%   (centered) frequency rectangle. W must be an odd integer
%   to preserve the symmetry of the filtered Fourier transform.
%
%   NOTCH can be:
%
%      'reject'    Notchreject filter transfer function.
%
%      'pass'      Notchpass filter transfer function.
%
%   MODE can be:
%
%      'vertical'     Filtering on vertical axis only.
%
%      'horizontal'   Filtering on horizontal axis only.
%
%      'both'         Filtering on both axes.
%
%   If MODE is 'vertical', then vertical rectangles start at +/- S
%   pixels from the center of the frequency rectangle along the vertical
%   axis and extend to both ends of that axis. Similarly, if MODE is
%   'horizontal', then horizontal rectangles start at +/- S pixels from
%   the center and extend to both ends of that axis. If MODE is 'both',
%   then rectangles are placed in both directions starting at +/- S.
%   Alternatively, if MODE is 'both', then S can be a two-element
%   vector, [SV SH], specifying the starting rectangle locations in both
%   directions.
%
%   H = RECNOTCH(NOTCH,MODE,M,N) uses W = 3 and S = 1.
%
%   H is of floating point class double. It is returned uncentered for
%   consistency with the filtering function dftfilt. To view H as an
%   image or mesh plot, it should be centered using Hc = fftshift(H).
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Defaults.
if nargin < 6
   S = 1;
end
if nargin < 5
   W = 3;
end
if ~isodd(W)
   error('W must be odd.')
end

% AV and AH are rectangle amplitude values for the vertical and
% horizontal rectangles: 0 for notchreject and 1 for notchpass. The
% transfer functions are computed initially as reject transfer functions
% and then changed to pass if so specified in NOTCH.
if isequal(mode,'vertical')
   % Reject only along vertical axis.
   AV = 0;
   AH = 1;
   SV = S;
   SH = 1;
elseif isequal(mode,'horizontal')
   % Reject only along horizontal axis.
   AV = 1;
   AH = 0;
   SH = S;
   SV = 1;
elseif isequal(mode,'both')
   % Reject along both axes.
   AV = 0;
   AH = 0;
   if numel(S) == 1
      SV = S;
      SH = S;
   else
      SV = S(1);
      SH = S(2);
   end
else
   error('Unknown mode')
end

% Begin computation. The transfer function is generated as a 'reject'
% function. At the end, it is changed to a 'pass' function if it is so
% specified in parameter NOTCH.
H = rectangleReject(M,N,W,SV,SH,AV,AH);

% Finished computing the rectangle notch transfer function. Format the
% output so it is returned uncentered for use in function dftfilt, and
% change H to a 'pass' function if so specified.
H = ifftshift(H);
if isequal(notch,'pass')
   H = 1 - H;
end

%----------------------------------------------------------------------%
function H = rectangleReject(M,N,W,SV,SH,AV,AH)
% Preliminaries.
H = ones(M,N);
% Center of frequency rectangle.
UC = floor(M/2) + 1;
VC = floor(N/2) + 1;
% Width limits.
WL = (W - 1)/2;
% Compute rectangle notches with respect to center.
% Left, horizontal rectangle.
H(UC-WL:UC+WL, 1:VC-SH) = AH;
% Right, horizontal rectangle.
H(UC-WL:UC+WL, VC+SH:N) = AH;
% Top vertical rectangle.
H(1:UC-SV, VC-WL:VC+WL) = AV;
% Bottom vertical rectangle.
H(UC+SV:M, VC-WL:VC+WL) = AV;


function [x,state] = struct_rational(z,task,t)
%STRUCT_RATIONAL Matrix with columns as rational functions.
%   [x,state] = struct_rational(z,[],t) computes a matrix x in which the
%   jth column is equal to the rational function
%
%      polyval(z{1}(j,:),s)./polyval([z{2}(j,:) 1],s)
%
%   evaluated at the points s, defined as
%
%      (t-0.5*(min(t)+max(t)))/(0.5*(max(t)-min(t))).
%
%   The degree of the numerator and denomator are equal to size(z{1},2)-1
%   and size(z{2},2), respectively. The structure state stores information
%   which is reused in computing the right and left Jacobian-vector
%   products.
%
%   struct_rational(z,task,t) computes the right or left Jacobian-vector
%   product of this transformation, depending on the structure task. Use
%   the structure state and add the field 'r' of the same shape as z or the
%   field 'l' of the same shape as x to obtain the structure task for
%   computing the right and left Jacobian-vector products
%
%      (dF(:)/dz(:).')*task.r(:) and
%      (dF(:)/dz(:).')'*task.l(:) + conj((dF(:)/dconj(z(:)).')'*task.l(:)),
%
%   respectively. Here, F(z) represents this transormation, (:) signifies
%   vectorization and the derivative w.r.t. z (conj(z)) is a partial
%   derivative which treats conj(z) (z) as constant. The output has the
%   same shape as x or z for the right and left Jacobian-vector products,
%   respectively.
%
%   See also struct_poly, struct_rbf.

%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
%            Marc Van Barel (Marc.VanBarel@cs.kuleuven.be)
%            Lieven De Lathauwer (Lieven.DeLathauwer@kuleuven-kulak.be)
%
%   References:
%   [1] L. Sorber, M. Van Barel, L. De Lathauwer, "Structured data fusion,"
%       ESAT-SISTA Internal Report 13-177, KU Leuven, 2013.

if nargin < 2, task = []; end
if nargin < 3
    error('struct_rational:t','Please supply evaluation points.');
end

% Center and scale evaluations points to fit in [-1,1].
mx = max(t);
mn = min(t);
t = (t-0.5*(mn+mx))/(0.5*(mx-mn));

if isempty(task) || (isempty(task.l) && isempty(task.r))
    x = polyval(z{1},t(:));
    if ~isempty(z{2})
        state.denom = polyval([z{2} ones(size(z{2},1),1)],t(:));
        x = x./state.denom;
        state.deriv = -x./state.denom;
    else
        state = [];
    end
elseif ~isempty(task.r)
    x = polyval(task.r{1},t(:));
    if ~isempty(z{2})
        x = x./task.denom+ ...
            task.deriv.*polyval([task.r{2} zeros(size(z{2},1),1)],t(:));
    end
    state = [];
elseif ~isempty(task.l)
    x = {zeros(size(z{1})),zeros(size(z{2}))};
    tmp = ones(length(t),1);
    if isempty(z{2})
        for i = size(z{1},2):-1:1
            x{1}(:,i) = sum(bsxfun(@times,conj(tmp),task.l),1);
            tmp = tmp.*t(:);
        end
    else
        for i = size(z{1},2):-1:1
            x{1}(:,i) = sum(conj(bsxfun(@rdivide,tmp,task.denom)).* ...
                task.l,1);
            tmp = tmp.*t(:);
        end
        tmp = t(:);
        for i = size(z{2},2):-1:1
            x{2}(:,i) = sum(conj(bsxfun(@times,tmp,task.deriv)).*task.l,1);
            tmp = tmp.*t(:);
        end
    end
    state = [];
end

end

function y = polyval(p,t)
    y = p(:,ones(length(t),1)).';
    for i = 2:size(p,2)
        y = bsxfun(@plus,bsxfun(@times,y,t),p(:,i).');
    end
end


%  Figure 3.7      Feedback Control of Dynamic Systems, 6e
%                        Franklin, Powell, Emami
% script to generate Fig. 3.7
% fig3_07.m
% Satellite Double-Pulse Response Example 3.19
clf;
dI=1/5000;
numG=dI;
denG=[1 0 0];
sys=tf(numG,denG);
t=0:0.01:10;
% pulse input
u2=[zeros(1,500), 25*ones(1,10), zeros(1,100), -25*ones(1,10), zeros(1,381)];
[y2]=lsim(sys,u2,t);
figure();
plot(t,u2);
axis([0, 10, -26, 26]);
xlabel('Time (sec)');
ylabel('Thrust Fc');
title('Fig. 3.7(a): Thrust input')
% grid
nicegrid
pause;
% conversion to degrees
ff=180/pi;
y2=ff*y2;
figure();
plot(t,y2);
xlabel('Time (sec)');
ylabel('\theta (deg)');
title('Fig. 3.7(b): Satellite attitude')
% grid
nicegrid


function arg = calc_acosInput(a, b)
% calculate normalized dot product and enforce result to be within limits
% [-1, 1] to avoid acos function throwing an error caused by numerical
% inaccuracy

    expr = dot(a,b)/(norm(a)*norm(b));

    % fix to avoid error due to numerical inaccuracy in combination with
    % acos calculation
    arg = max(min(expr, 1.0), -1.0);

end


% FDR - compute false detection rate mask
%
% Usage:
%   >> [p_fdr, p_masked] = fdr( pvals, alpha);
%
% Inputs:
%   pvals   - vector or array of p-values
%   alpha   - threshold value (non-corrected). If no alpha is given
%             each p-value is used as its own alpha and FDR corrected
%             array is returned.
%   fdrtype - ['parametric'|'nonParametric'] FDR type. Default is
%             'parametric'.
%
% Outputs:
%   p_fdr    - pvalue used for threshold (based on independence
%              or positive dependence of measurements)
%   p_masked - p-value thresholded. Same size as pvals. Can only be 0 or 1.
%
% Author: Arnaud Delorme, SCCN, 2008-
%         Based on a function by Tom Nichols
%
% Reference: Bejamini & Yekutieli (2001) The Annals of Statistics

% Copyright (C) 2002 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This file is part of EEGLAB, see http://www.eeglab.org
% for the documentation and details.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:
%
% 1. Redistributions of source code must retain the above copyright notice,
% this list of conditions and the following disclaimer.
%
% 2. Redistributions in binary form must reproduce the above copyright notice,
% this list of conditions and the following disclaimer in the documentation
% and/or other materials provided with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
% THE POSSIBILITY OF SUCH DAMAGE.

function [pID, p_masked] = fdr(pvals, q, fdrType);

if nargin < 3, fdrType = 'parametric'; end
if isempty(pvals), pID = []; return; end
p = sort(pvals(:));
V = length(p);
I = (1:V)';

cVID = 1;
cVN = sum(1./(1:V));

if nargin < 2
    pID = ones(size(pvals));
    thresholds = exp(linspace(log(0.1),log(0.000001), 1000));
    for index = 1:length(thresholds)
        [tmp p_masked] = fdr(pvals, thresholds(index));
        pID(p_masked) = thresholds(index);
    end
else
    if strcmpi(fdrType, 'parametric')
        pID = p(max(find(p<=I/V*q/cVID))); % standard FDR
    else
        pID = p(max(find(p<=I/V*q/cVN)));  % non-parametric FDR
    end
end
if isempty(pID), pID = 0; end

if nargout > 1
    p_masked = pvals<=pID;
end


function [kld, llh1, llh2] = GLG_kld( theta1, theta2, sz )

% Kullback-Leibler divergence between GLG models
% The KLD is computed by simulating observations from model 1 and
% evaluating the composite log-likelihood of these observations under
% model 1 and model 2.
%
% Syntax:
%   [kld, llh1, llh2] = GLG_kld( theta1, theta2, sz )
%
%
% Input:
%   theta1 : Parameters of model 1.
%
%   theta2 : Parameters of model 2.
%
%   sz     : Size of image simulated from model 1
%
%
% Output:
%   kld    : The Kullback-Leibler divergence from model 1 to model 2
%
%   llhX   : The likelihood of the simulation from model 1 under model X
%
%
% See also: GLG_SIMULATION

if ~isequal( size(theta1), size(theta2) )
    error('Models must be of the same size');
end

% Simulate from model1
tree = GLG_simulation( theta1, sz );

% Evaluate likelihood for each direction and level
llh1 = GLG_llh( theta1, tree );
llh2 = GLG_llh( theta2, tree );

% Sum composite likelihood and directions into KLD
kld = sum( llh1(:) - llh2(:) );

end


function N = unormal( dom )
%UNORMAL unit normal vector to the unit sphere.
%   N = UNORMAL( DOM ) returns a SPHEREFUNV representing the unit normal
%   vector to the unit sphere using the spherical coordinate domain
%   specified, i.e. DOM = [-pi -pi 0 pi], for co-latitude.
%
%   N = UNORMAL returns a SPHEREFUNV representing the unit normal
%   vector to the unit sphere, using the default spherical coordinates
%   domain, which is co-latitude.
%
%   See also SPHEREFUNV/TANGENTIAL, SPHEREFUNV/DOT, SPHEREFUNV/CURL

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

if ~exist( 'dom', 'var' ) || isempty( dom )
    dom = [-pi pi 0 pi];
end

if ( numel( dom ) ~= 4 )
    error('SPHEREFUN:SPHEREFUNV:normal:domain',...
        ['Domain for the unit vector is incorrect.  It should be a '...
        'double vector with 4 components.']);
end

% Normal vector to the unit sphere.
x = spherefun(@(x,y,z) x, dom);
y = spherefun(@(x,y,z) y, dom);
z = spherefun(@(x,y,z) z, dom);
N = spherefunv(x,y,z);

end


% This is like robot1, except we only use a Kalman filter.
% The goal is to study how the precision matrix changes.

seed = 1;
rand('state', seed);
randn('state', seed);

if 0
  T = 20;
  ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
		 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
else
  T = 60;
  ctrl_signal = repmat([1 0]', 1, T);
end

nlandmarks = 6;
if 0
  true_landmark_pos = [1 1;
		    4 1;
		    4 4;
		    1 4]';
else
  true_landmark_pos = 10*rand(2,nlandmarks);
end
if 0
figure(1); clf
hold on
for i=1:nlandmarks
  %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
  plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
end
hold off
end

init_robot_pos = [0 0]';

true_robot_pos = zeros(2, T);
true_data_assoc = zeros(1, T);
true_rel_dist = zeros(2, T);
for t=1:T
  if t>1
    true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
  else
    true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
  end
  nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
  %true_data_assoc(t) = nn;
  %true_data_assoc = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ...
  true_data_assoc  = sample_discrete(normalise(ones(1,nlandmarks)),1,T);
  true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
end

R = 1e-3*eye(2); % noise added to observation
Q = 1e-3*eye(2); % noise added to robot motion

% Create data set
obs_noise_seq = sample_gaussian([0 0]', R, T)';
obs_rel_pos = true_rel_dist + obs_noise_seq;
%obs_rel_pos = true_rel_dist;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Create params for inference

% X(t) = A X(t-1) + B U(t) + noise(Q)

% [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
% [L2]    [  1   ]    [L2]         [0]          [ 0  ]
% [R ]t   [     1]    [R ]t-1      [1]          [   Q]

% Y(t)|S(t)=s  = C(s) X(t) + noise(R)
% Yt|St=1 = [1 0 -1]  * [L1]  + R
%                       [L2]
%                       [R ]

% Create indices into block structure
bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
robot_block =  block(nlandmarks+1, bs);
for i=1:nlandmarks
  landmark_block(:,i) = block(i, bs)';
end
Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
Ysz = 2; % observe relative location
Usz = 2; % input is (dx, dy)

% create block-diagonal trans matrix for each switch
A = zeros(Xsz, Xsz);
for i=1:nlandmarks
  bi = landmark_block(:,i);
  A(bi, bi) = eye(2);
end
bi = robot_block;
A(bi, bi) = eye(2);
A = repmat(A, [1 1 nlandmarks]); % same for all switch values

% create block-diagonal system cov

Qbig = zeros(Xsz, Xsz);
bi = robot_block;
Qbig(bi,bi) = Q; % only add noise to robot motion
Qbig = repmat(Qbig, [1 1 nlandmarks]);

% create input matrix
B = zeros(Xsz, Usz);
B(robot_block,:) = eye(2); % only add input to robot position
B = repmat(B, [1 1 nlandmarks]);

% create observation matrix for each value of the switch node
% C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
% This computes L(i) - R
C = zeros(Ysz, Xsz, nlandmarks);
for i=1:nlandmarks
  C(:, landmark_block(:,i), i) = eye(2);
  C(:, robot_block, i) = -eye(2);
end

% create observation cov for each value of the switch node
Rbig = repmat(R, [1 1 nlandmarks]);

% initial conditions
init_x = zeros(Xsz, 1);
init_v = zeros(Xsz, Xsz);
bi = robot_block;
init_x(bi) = init_robot_pos;
%init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step
for i=1:nlandmarks
  bi = landmark_block(:,i);
  init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
  %init_x(bi) = true_landmark_pos(:,i);
  %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
end

%k = nlandmarks-1; % exact
k = 3;
ndx = {};
for t=1:T
  landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
  tmp = [landmark_block(:, landmarks) robot_block'];
  ndx{t} = tmp(:);
end

[xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
		       'ndx', ndx);

[xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);

if 0
est_robot_pos = x(robot_block, :);
est_robot_pos_cov = V(robot_block, robot_block, :);

for i=1:nlandmarks
  bi = landmark_block(:,i);
  est_landmark_pos(:,i) = x(bi, T);
  est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
end
end

nrows = 10;
stepsize = T/(2*nrows);
ts = 1:stepsize:T;

if 1 % plot

clim = [0 max(max(Va(:,:,end)))];

figure(2)
if 0
  imagesc(Ve(1:2:end,1:2:end, T))
  clim = get(gca,'clim');
else
  i = 1;
  for t=ts(:)'
    subplot(nrows,2,i)
    i = i + 1;
    imagesc(Ve(1:2:end,1:2:end, t))
    set(gca, 'clim', clim)
    colorbar
  end
end
suptitle('exact')

figure(3)
if 0
  imagesc(Va(1:2:end,1:2:end, T))
  set(gca,'clim', clim)
else
  i = 1;
  for t=ts(:)'
    subplot(nrows,2,i)
    i = i+1;
    imagesc(Va(1:2:end,1:2:end, t))
    set(gca, 'clim', clim)
    colorbar
  end
end
suptitle('approx')

figure(4)
i = 1;
for t=ts(:)'
  subplot(nrows,2,i)
  i = i+1;
  Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
  imagesc(Vd)
  set(gca, 'clim', clim)
  colorbar
end
suptitle('diff')

end % all plot

for t=1:T
  i = 1:2*nlandmarks;
  denom = Ve(i,i,t) + (Ve(i,i,t)==0);
  Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
  Verr(t) = max(Vd(:));
end
figure(6); plot(Verr)
title('max relative Verr')

for t=1:T
  %err(t)=rms(xa(:,t), xe(:,t));
  err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
end
figure(5);plot(err)
title('rms mean pos')


function [rbin]=rdfcalc(C,xlim,ylim)

%This file takes a file containing the co-ordinates of the centers as
%input, and finds the rdf

%xlim=512; % The maximum x co-ordinate of the image
%ylim=512; % The maximum y co-ordinate of the image

n=size(C,1);

rmin=1;
rmax=100;
dr=2;

rvecarray=rmin:dr:rmax;

m=size(rvecarray,2);

rbin=zeros(m,2);
rbin(:,1)=rvecarray;

for j=1:m

    rvec=rvecarray(j);

for i=1:n
    x1=C(i,1);
    y1=C(i,2);
    bini=0;

    for k=1:n
        rk= sqrt((C(k,1)-C(i,1))^2 +(C(k,2)-C(i,2))^2);
        if rk>= rvec && rk<=(rvec+dr)
            bini=bini+1;
        end
    end

    theta1=checkquad(x1,y1,rvec,1,xlim,ylim);
    theta2=checkquad(x1,y1,rvec,2,xlim,ylim);
    theta3=checkquad(x1,y1,rvec,3,xlim,ylim);
    theta4=checkquad(x1,y1,rvec,4,xlim,ylim);

    theta=theta1+theta2+theta3+theta4;

    area1=rvec*theta*dr;

    bini=bini/area1;
    rbin(j,2)=rbin(j,2)+bini;

end
end

% rbin=rbin/n; % This is to divide the result by the number of particles.
%
% numden=(n/(xlim*ylim)); %This is the number density
%
% rbin=rbin/numden;
%
% plot(rvecarray,rbin,'ro','MarkerSize',10)


function [UV,F, res, edge_norms] = ...
  create_regular_grid(xRes, yRes, xWrap, yWrap, near, far)
% Creates list of triangle vertex indices for a rectangular domain,
% optionally wrapping around in X/Y direction.
%
% Usage:
%   [UV,F,res,edge_norms] = create_regular_grid(xRes, yRes, xWrap, yWrap)
%
% Input:
%   xRes, yRes: number of points in X/Y direction
%   wrapX, wrapY: wrap around in X/Y direction
%   near, far: near and far should be fractions of one which control the
%              pinching of the domain at the center and sides
%
% Output:
%   F : mesh connectivity (triangles)
%   UV: UV coordinates in interval [0,1]x[0,1]
%   res: mesh resolution
%
% Example:
%  % Create and m by n cylinder
%  m = 10; n = 20;
%  [V,F] = create_regular_grid(m,n,1,0);
%  V = [sin(2*pi*V(:,1)) cos(2*pi*V(:,1)) (n-1)*2*pi/(m-1)*V(:,2)];
%  tsurf(F,V); axis equal;
%
%  % Quads:
%  Q = [F(1:2:end-1,[1 2]) F(2:2:end,[2 3])];
%
%  % Alternating diagonals:
%  Q(1:2:end,:) = Q(1:2:end,[2 3 4 1]);
%  F = [Q(:,[1 2 3]);Q(:,[1 3 4])];
%

if (nargin<2) yRes=xRes; end
if (nargin<3) xWrap=0; end
if (nargin<4) yWrap=0; end
if (nargin<5) overlap=0; end

%res = [yRes, xRes];
res_wrap = [yRes+yWrap, xRes+xWrap];

%xSpace = linspace(0,1,xRes+xWrap); if (xWrap) xSpace = xSpace(1:end-1); end
%ySpace = linspace(0,1,yRes+yWrap); if (yWrap) ySpace = ySpace(1:end-1); end
xSpace = linspace(0,1,xRes+xWrap);
ySpace = linspace(0,1,yRes+yWrap);

[X, Y] = meshgrid(xSpace, ySpace);
UV_wrap = [X(:), Y(:)];

% Must perform pinch before edge_norms are taken
if(exist('near') & exist('far'))
  if(near>0 & far>0)
  t = ( ...
      UV_wrap(:,1).*(UV_wrap(:,1)<0.5)+ ...
      (1-UV_wrap(:,1)).*(UV_wrap(:,1)>=0.5) ...
    )/0.5;
  t = 1-sin(t*pi/2+pi/2);
  UV_wrap(:,2) = ...
    far/2 + ...
    near*(UV_wrap(:,2)-0.5).*(1-t) + ...
    far*(UV_wrap(:,2)-0.5).*t;
  else
    %error('Pinch must be between 0 and 1');
  end
end

idx_wrap = reshape(1:prod(res_wrap), res_wrap);

v1_wrap = idx_wrap(1:end-1, 1:end-1); v1_wrap=v1_wrap(:)';
v2_wrap = idx_wrap(1:end-1, 2:end  ); v2_wrap=v2_wrap(:)';
v3_wrap = idx_wrap(2:end  , 1:end-1); v3_wrap=v3_wrap(:)';
v4_wrap = idx_wrap(2:end  , 2:end  ); v4_wrap=v4_wrap(:)';

F_wrap = [v1_wrap;v2_wrap;v3_wrap; v2_wrap;v4_wrap;v3_wrap];
F_wrap = reshape(F_wrap, [3, 2*length(v1_wrap)])';

% old way
% edges = [F_wrap(:,1) F_wrap(:,2); F_wrap(:,2) F_wrap(:,3); F_wrap(:,3) F_wrap(:,1)];
% edge_norms = sqrt(sum((UV_wrap(edges(:,1),:)-UV_wrap(edges(:,2),:)).^2,2));
% edge_norms = reshape(edge_norms,size(F_wrap,1),3);

% edges numbered same as opposite vertices
edge_norms = [ ...
  sqrt(sum((UV_wrap(F_wrap(:,2),:)-UV_wrap(F_wrap(:,3),:)).^2,2)) ...
  sqrt(sum((UV_wrap(F_wrap(:,3),:)-UV_wrap(F_wrap(:,1),:)).^2,2)) ...
  sqrt(sum((UV_wrap(F_wrap(:,1),:)-UV_wrap(F_wrap(:,2),:)).^2,2)) ...
  ];

% correct indices
res = [yRes,xRes];
idx = reshape(1:prod(res),res);
if (xWrap) idx = [idx, idx(:,1)]; end
if (yWrap) idx = [idx; idx(1,:)]; end
idx_flat = idx(:);

% this might not be neccessary, could just rebuild UV like before
UV = reshape(UV_wrap,[size(idx_wrap),2]);
UV = UV(1:end-yWrap,1:end-xWrap,:);
UV = reshape(UV,xRes*yRes,2);

F = [idx_flat(F_wrap(:,1)),idx_flat(F_wrap(:,2)),idx_flat(F_wrap(:,3))];


function err = getL2error3RT0(node,elem,sigma,sigmah,markedElem)
%% GETL2ERROR3RT0  L2 norm of RT0 element in 3D.
%
%  The input sigma can now just be function boundle. sigmah is the
%  flux through faces.
%  err = getL2error3RT0(node,elem,sigma,sigmah,markedElem)
%
% Example
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];  % nodes
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7]; % elements
%     elem = label3(node,elem);
%     [node,elem] = uniformbisect3(node,elem);
%     bdFace = setboundary3(node,elem,'Dirichlet','y~=-1','Neumann','y==-1');
%     maxIt = 3;
%     pde = mixBCdata3;
%     err = zeros(maxIt,1); N = zeros(maxIt,1);
%     for i =1:maxIt
%         [node,elem,~,bdFace] = uniformbisect3(node,elem,[],bdFace);
%         barycenter = 1/4.*(node(elem(:,1),:)+node(elem(:,2),:)+node(elem(:,3),:)+node(elem(:,4),:));
%         uexa = pde.exactu(barycenter);
%         [u,sigma] = Poisson3RT0(node,elem,pde,bdFace);
%         err(i) = getL2error3(node,elem,pde.exactu,u);
%         N(i) = length(u) + length(sigma) ;
%     end
%     r1 = showrate(N,err,2);
%     legend('||\sigma-\sigma_h||',['N^{' num2str(r1) '}'],...
%           'LOCATION','Best')
%
%% TODO: add jump coefficent and sigma is vector array.
%
% Created by Ming Wang at Jan 17, 2011, modified m-lint May 15, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Construct Data Structure
[elem2dof,dofSign] = dof3RT0(elem);
NT = size(elem,1);% Ndof = max(elem2dof(:)); %N = size(node,1);
[Dlambda,area] = gradbasis3(node,elem);
locFace = [2 3 4; 1 4 3; 1 2 4; 1 3 2];

%% Compute square of the L2 error element-wise
[lambda,w] = quadpts3(3); % quadrature order is 3
nQuad = size(lambda,1);
err = zeros(NT,1);
for p = 1:nQuad
    % quadrature points in the x-y-z coordinate
    pxy = lambda(p,1)*node(elem(:,1),:) ...
        + lambda(p,2)*node(elem(:,2),:) ...
        + lambda(p,3)*node(elem(:,3),:) ...
        + lambda(p,4)*node(elem(:,4),:);
    sigmap = sigma(pxy);
    sigmahp = zeros(NT,3);
    for l = 1:4 % for each basis
        i = locFace(l,1); j = locFace(l,2); k = locFace(l,3);
        % phi_k = lambda_iRot_j - lambda_jRot_i;
        sigmahp = sigmahp + repmat(double(dofSign(:,l)).*sigmah(elem2dof(:,l)),1,3)*2.*...
                   (lambda(p,i)*cross(Dlambda(:,:,j),Dlambda(:,:,k)) + ...
                    lambda(p,j)*cross(Dlambda(:,:,k),Dlambda(:,:,i)) + ...
                    lambda(p,k)*cross(Dlambda(:,:,i),Dlambda(:,:,j)));
    end
    err = err + w(p)*sum((sigmap - sigmahp).^2,2);
end
err = err.*area;
% modify the error
err(isnan(err)) = 0;
if (nargin == 5) && ~isempty(markedElem)
    err = err(markedElem); % L2 err on some marked region
end
err = sqrt(sum(err));


function [v,f] = max_kur(x,d0,maxit,show)

%
% [v,f] = max_kur(x,d,maxit,show)
%
% Compute direction maximizing the kurtosis of the projections
% Observations passed to the routine should be standardized
% Uses Newton's method and an augmented Lagrangian merit function
% To be used as a subroutine of kur_rce
%
% Inputs:  x, observations (by rows)
%          d, initial estimate of the direction (optional)
%          maxit, a limit to the number of Newton iterations
%          show, <>0 generates output for each iteration
% Outputs: v, max. kurtosis direction (local optimizer)
%          f, max. kurtosis value
%

% Daniel Pena/Francisco J Prieto 23/5/00

% Initialization

maxitdefault = 30;

if nargin < 4,
  show = 0;
end
if nargin < 3,
  maxit = maxitdefault;
end
if maxit <= 0,
  maxit = maxitdefault;
end
if nargin < 2,
  d0 = [];
end

%% Tolerances

maxitini = 1;
tol = 1.0e-4;
tol1 = 1.0e-7;
tol2 = 1.0e-2;
beta = 1.0e-4;
rho0 = 0.1;

[n,p] = size(x);

%% Initial estimate of the direction

if length(d0) == 0,

  uv = sum((x.*x)')';
  uw = 1../(eps + sqrt(uv));
  uu = x.*(uw*ones(1,p));

  Su = cov(uu);
  [V,D] = eig(Su);

  r = [];
  for i = 1:p,
    r = [ r val_kur(x,V(:,i)) ];
  end
  [v,ik] = max(r);
  a = V(:,ik(1));

  itini = 1;
  difa = 1;
  while (itini <= maxitini)&(difa > tol2),
    z = x*a;
    zaux = z.^2;
    xaux = x'.*(ones(p,1)*zaux');
    H = 12*xaux*x;
    [V,E] = eig(H);
    [vv,iv] = max(diag(E));
    aa = V(:,iv(1));
    difa = norm(a - aa);
    a = aa;
    itini = itini + 1;
  end
else
  a = d0/norm(d0);
end

%% Values at iteration 0 for the optimization algorithm

z = x*a;
sk = sum(z.^4);
lam = 2*sk;
f = sk;
g = (4*(z.^3)'*x)';
zaux = z.^2;
xaux = x'.*(ones(p,1)*zaux');
H = 12*xaux*x;

al = 0;
it = 0;
diff = 1;
rho = rho0;
clkr = 0;

c = 0;

% Newton method starting from the initial direction

if show,
  disp(' It.      Obj.F.       | g |           c         alpha      rho');
end

while 1,

%% Check termination conditions

  gl = g - 2*lam*a;

  A = 2*a';
  [Q,W] = qr(a);
  Z = Q(:,(2:p));

  if show,
    aa = sprintf('%3.0f  %12.5e %13.4e',it,f,norm(gl));
    bb = sprintf(' %13.4e %8.3f %11.2e',abs(a'*a-1),al,rho);
    disp([ aa bb ]);
  end

  crit = norm(gl) + abs(c);
  if (crit <= tol)|(it >= maxit),
    break
  end

%% Compute search direction

  Hl = H - 2*lam*eye(p,p);
  Hr = Z'*Hl*Z;
  [V,E] = eig(Hr);
  Es = min(-abs(E),-1.0e-4);
  Hs = V*Es*V';

  py = - c/(A*A');
  rhs = Z'*(g + H*A'*py);
  pz = - Hs\rhs;
  pp = Z*pz + py*A';

  dlam = (2*a)\(gl + H*pp);

%% Adjust penalty parameter

  f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
  crit1 = beta*norm(pp)^2;

  if f0d < crit1,
    rho1 = 2*(crit1 - f0d)/(eps + c^2);
    rho = max([2*rho1 1.5*rho rho0]);
    f = sk - lam*c - 0.5*rho*c^2;
    f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
    clkr = 0;
  elseif (f0d > 1000*crit1)&(rho > rho0),
    rho1 = 2*(crit1 - gl'*pp + dlam*c)/(eps + c^2);
    if (clkr == 4)&(rho > 2*rho1),
      rho = 0.5*rho;
      f = sk - lam*c - 0.5*rho*c^2;
      f0d = gl'*pp - 2*rho*c*a'*pp - dlam*c;
      clkr = 0;
    else
      clkr = clkr + 1;
    end
  end
  if (abs(f0d/(norm(g-2*rho*a*c)+norm(c))) < tol1),
    break
  end

%% Line search

  al = 1;
  itbl = 0;
  while itbl < 20,
    aa = a + al*pp;
    lama = lam + al*dlam;
    zz = x*aa;
    cc = aa'*aa - 1;
    sk = sum(zz.^4);
    ff = sk - lama*cc - 0.5*rho*cc^2;

    if ff > f + 0.0001*al*f0d,
      break
    end
    al = al/2;
    itbl = itbl + 1;
  end
  if itbl >= 20,
    if show,
      disp('Error in the line search');
    end
    break
  end

%% Update values for the next iteration

  a = aa;
  lam = lama;
  z = zz;

  nmd2 = a'*a;
  c = nmd2 - 1;
  f = sk - lam*c - 0.5*rho*c^2;
  g = (4*(z.^3)'*x)';
  zaux = z.^2;
  xaux = x'.*(ones(p,1)*zaux');
  H = 12*xaux*x;

  it = it + 1;

end

% Values to be returned

v = a/norm(a);
xa = x*v;
f = sum(xa.^4)/n;


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% Laplace Transform properties

% Time shifting

syms t s
t0=2;

Le=cos(t-t0)*heaviside(t-t0);
Left= laplace(Le)

X=laplace(cos(t),s);
Right=exp(-s*t0)*X


function newCoords = UpsampleCoords(coords, factor)

% newCoords = UpsampleCoords(coords, factor);
%
% Returns a coordinate array that oversamples the input coords by the
% specified factor.
%
% Ress, 2/04

% Determine a few index parameters
factor = ceil(factor);
i1 = factor - 1;
i0 = -i1;
stride = 1 / factor;
dims = size(coords);
nCoords = dims(2);

% Initialize the upsampled coordinates:
newCoords = zeros(3, nCoords*(2*factor-1)^3);
iC = 1;

for iz=i0:i1
  offset(3) = iz * stride;
  for iy=i0:i1
    offset(2) = iy * stride;
    for ix=i0:i1
      offset(1) = ix * stride;
      offsetCoords = coords;
      for ii=1:3, offsetCoords(ii, :) = offsetCoords(ii, :) + offset(ii); end
      newCoords(:, iC:(iC+nCoords-1)) = offsetCoords;
      iC = iC + nCoords;
    end
  end
end


function x=nsst_rec2(dst,shear_f,lpfilt)
% This function performs the inverse (local) nonsubsampled shearlet transform as given
% in G. Easley, D. Labate and W. Lim, "Sparse Directional Image Representations
% using the Discrete Shearlet Transform", Appl. Comput. Harmon. Anal. 25 pp.
% 25-46, (2008).
%
% Input:
%
% dst          - the nonsubsampled shearlet coefficients
%
% shear_f      - the cell array containing the shearing filters
%
% lpfilt       - the filter to be used for the Laplacian
%                Pyramid/ATrous decomposition using the codes
%                written by Arthur L. Cunha
%
% Output
%
% x         - the reconstructed image
%
% Code contributors: Glenn R. Easley, Demetrio Labate, and Wang-Q Lim.
% Copyright 2011 by Glenn R. Easley. All Rights Reserved.
%

level=length(dst)-1;
y{1}=dst{1};
for i=1:level,
      l=size(dst{i+1},3);
      for k=1:l,
          dst{i+1}(:,:,k)=conv2p(conj(shear_f{i}(:,:,k)),dst{i+1}(:,:,k));
      end
      y{i+1} = real(sum(dst{i+1},3));
end

x=real(atrousrec(y,lpfilt));


function varargout = vals2coeffs( U, varargin )
%VALS2COEFFS    Convert matrix of values to Chebyshev-Fourier coefficients.
%
% V = VALS2COEFFS( U ) converts a matrix U of values representing
% samples of a function from a tensor Chebyshev-Fourier grid
% to a matrix V of Chebyshev-Fourier coefficients for the corresponding
% interpolant.
%
% [U, S, V] = VALS2COEFFS( U, S, V ) the same as above but keeps
% everything in low rank form.
%
% See also COEFFS2VALS

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

if ( nargin == 1 )
    U = chebtech2.vals2coeffs( U );
    U = trigtech.vals2coeffs( U.' ).';
    varargout = {U};
elseif ( nargin == 3 )
    S = varargin{1};
    V = varargin{2};
    U = chebtech2.vals2coeffs( U );
    V = trigtech.vals2coeffs( V );
    varargout = { U S V };
else
    error('CHEBFUN:DISKFUN:vals2coeffs:inputs', ...
        'The number of input arguments should be one or two.');
end

end


% time varying Rayleigh flat fading channel with Clark's model
% using filter method
% Ns number of channel samples
% fDTs: fD*Ts doppler spread fD*Ts
% fdTs= .001 slow, .01 medioum, .03 fast

function out=flat_filter(Ns,fD,fDTs)

% parameters
Ts=fDTs/fD;
% filter window length at least three side lobs
Ng=ceil(2/fDTs);

% Complex Gaussian input
x=cxn(Ns,1);

% compute g(t) and g_hat(t)
t=(-Ng:Ng)*Ts;
g=besselj(1/4,2*pi*fD*abs(t))./((abs(t)).^(1/4));
g(Ng+1)=((pi*fD)^(1/4))/gamma(5/4);
%verify filter
%plot(abs(g));

% compute K such that ...
g_hat=g./sqrt(sum(g.^2));
% verify K
%sum_g_hat=sum(g_hat.^2);

% channel impulse response
cn=conv(x,g_hat)*Ts;

% reject transient
cn=cn(2*Ng+1:length(cn)-2*Ng);
out=cn/sqrt(var(cn));

end


function [bankAng,angOfAttack,angOfSideslip,totalAoA] = computeAeroAnglesFromFrameBodyAxes(rVectFrame, vVectFrame, bodyXFrame, bodyYFrame, bodyZFrame)
    %Source: http://www.dept.aoe.vt.edu/~cdhall/courses/aoe5204/AircraftMotion.pdf

    [R_wind_2_frame, ~, ~, ~] = computeWindFrame(rVectFrame, vVectFrame);
    R_body_2_frame = horzcat(bodyXFrame, bodyYFrame, bodyZFrame);

    angles = real(rotm2eulARH(R_wind_2_frame' * R_body_2_frame, 'zyx'));

    bankAng = angles(3);
	angOfAttack = angles(2);
	angOfSideslip = angles(1);

    [x,y,z] = sph2cart(angleNegPiToPi(angOfSideslip),angleNegPiToPi(angOfAttack),1);
    v1 = [1;0;0];
    v2 = [x;y;z];
    totalAoA = dang(v1,v2);
end


function H = get_shelve_lagrange(f,H,FlagSub,fSub,FlagAliasing,fAliasing,Bandwidth_in_Oct)
%GET_SHELVE_LAGRANGE Lagrange interpolation towards shelving filter
%
%   Usage: H = get_shelve_lagrange(f,H,FlagSub,fSub,FlagAliasing, ...
%                                  fAliasing,Bandwidth_in_Oct)
%
%   Input parameters:
%       f                -  frequency vector in Hz, typical 0 to half
%                           sampling frequency, equidistant sampling is
%                           assumed (i.e. DFT frequencies) but not required
%       H                -  complex spectrum (mag/phase) at specified frequencies
%                           with meaningful slope, typically used for +3dB/oct.
%       FlagSub          -  use low-shelf part, 0 or 1
%       fSub             -  cut-frequency in Hz for low-shelf,
%                           check e.g. fLow=100Hz in [Fig2, Sch13]
%       FlagAliasing     -  use high-sehlf part, 0 or 1
%       fAliasing        -  cut-frequency in Hz for high-shelf,
%                           check e.g. fAliasing=2kHz in [Fig2, Sch13]
%       Bandwidth_in_Oct -  interpolation bandwidth, i.e. shelf knee range
%                           in octaves, allowed: 0.5, 1, 2, 3, 4
%
%   Output parameters:
%       H                -  return complex spectrum at specified frequencies
%                           after low-/ and high-shelf interpolation              -
%
%   GET_SHELVE_LAGRANGE(f,H,FlagSub,fSub,FlagAliasing,fAliasing,Bandwidth_in_Oct)
%   does an Lagrange interplation to get a shelving filter. This function is used
%   in wfs_iir_prefilter(conf).
%
%   Note 1: there is a analytical expression for arbitrary Bandwidth_in_Oct
%   which is not implemented yet
%   Note 2: the Offset_dB specified for Bandwidth_in_Oct work only for the
%   3dB/oct. slope, other driving functions may have other slopes and
%   different interpolation offsets may be required
%
%   See also: wfs_iir_prefilter
%
%   References:
%       Schultz, Erbes, Spors, Weinzierl (2013) - "Derivation of IIR prefilters
%       for soundfield synthesis using linear secondary source distributions",
%       International Conference on Acoustics (AIA-DAGA), p.2372-2375,
%       http://pub.dega-akustik.de/AIA_DAGA_2013/data/articles/000604.pdf

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
% Revision: 07/02/2013 frank.schultz@uni-rostock.de initial development      *
%*****************************************************************************
    Hphase = unwrap(angle(H));  %save original phase
    H = 20*log10(abs(H));       %interpolation in dB

    if Bandwidth_in_Oct==4
        Offset_dB = 1.5053;
    elseif Bandwidth_in_Oct==3
        Offset_dB = 1.1286;
    elseif Bandwidth_in_Oct==2
        Offset_dB = 0.7528;
    elseif Bandwidth_in_Oct==1
        Offset_dB = 0.3766;
    elseif Bandwidth_in_Oct==0.5
        Offset_dB = 0.1857;
    else
        disp('error, Bandwidth_in_Oct must be 1,2,3 or 4')
    end

    if FlagSub
        %Sub-Bass limitation:
        tmp=(find(f<=fSub)); fSub_i = tmp(end);
        H_Sub = H*0+H(fSub_i);
        H_Sub(1) = H_Sub(2);
    end

    if FlagAliasing
        % Aliasing frequency limitation:
        tmp=(find(f<=fAliasing)); falias_i = tmp(end);
        H_Alias = H*0+H(falias_i);
        H_Alias(1) = H_Alias(2);
    end

    if FlagSub
        % Lagrange interpolation for SUB:
        fl = f(fSub_i)*2^(-Bandwidth_in_Oct/2);
        fh = f(fSub_i)*2^(+Bandwidth_in_Oct/2);
        tmp=(find(f<=fl)); fl_i = tmp(end);
        tmp=(find(f<=fh)); fh_i = tmp(end);

        P1x = log10(f(fl_i));
        P1y = H_Sub(fl_i);
        P2x = log10(f(fSub_i));
        P2y = H_Sub(fSub_i)+Offset_dB;
        P3x = log10(f(fh_i));
        P3y = H(fh_i);

        % Lagrange interpolation tmp variables:
        ipol1=P1y/((P1x-P2x)*(P1x-P3x));
        ipol2=P2y/((P2x-P1x)*(P2x-P3x));
        ipol3=P3y/((P3x-P1x)*(P3x-P2x));

        for fi=1:fl_i-1
        H(fi) = H_Sub(fi);
        end

        for fi=fl_i:fh_i
            H(fi)=          (ipol1*(log10(f(fi))-P2x)*(log10(f(fi))-P3x))+...
                            (ipol2*(log10(f(fi))-P1x)*(log10(f(fi))-P3x))+...
                            (ipol3*(log10(f(fi))-P1x)*(log10(f(fi))-P2x));
        end
    end

    if FlagAliasing
        % Lagrange interpolation for ALIASING:
        fl = f(falias_i)*2^(-Bandwidth_in_Oct/2);
        fh = f(falias_i)*2^(+Bandwidth_in_Oct/2);
        tmp=(find(f<=fl)); fl_i = tmp(end);
        tmp=(find(f<=fh)); fh_i = tmp(end);

        P1x = log10(f(fl_i));
        P1y = H(fl_i);
        P2x = log10(f(falias_i));
        P2y = H_Alias(falias_i)-Offset_dB;
        P3x = log10(f(fh_i));
        P3y = H_Alias(fh_i);

        % Lagrange interpolation tmp variables:
        ipol1=P1y/((P1x-P2x)*(P1x-P3x));
        ipol2=P2y/((P2x-P1x)*(P2x-P3x));
        ipol3=P3y/((P3x-P1x)*(P3x-P2x));

        for fi=fl_i:fh_i
            H(fi)=      (ipol1*(log10(f(fi))-P2x)*(log10(f(fi))-P3x))+...
                        (ipol2*(log10(f(fi))-P1x)*(log10(f(fi))-P3x))+...
                        (ipol3*(log10(f(fi))-P1x)*(log10(f(fi))-P2x));
        end

        for fi=fh_i+1:length(H)
            H(fi) = H_Alias(fi);
        end
    end

    %apply original phase and delog
    H = 10.^(H/20).*exp(1i*Hphase);
    H(1) = abs(H(2));
    %we ignore the special treatment for DC and fs/2 as would be usual for
    %DFT data, since we need a 'classical' frequency response
end


function pass = test_end(pref)

if ( nargin == 0 )
    pref = chebfunpref();
end

%% test end in infinite dimension:
% Scalar:
f = chebfun(@(x) sin(pi*x));
out = f(end);
pass(1) = isnumeric(out) && length(out) == 1 && abs(out) < eps;

% Array:
f = chebfun(@(x) [sin(pi*x), cos(pi*x), x]);
out = f(end);
pass(2) = isnumeric(out) && all(size(out) == [1, 3]) && ...
    norm(out - [0 -1 1], inf) < eps;

out = f(end,2);
pass(3) = isnumeric(out) && numel(out) == 1 && abs(out + 1) < eps;

% transpose
g = f.';
out = g(end);
pass(4) = isnumeric(out) && all(size(out) == [3, 1]) && ...
    norm(out - [0 -1 1].', inf) < eps;

%% test end in scalar dimension:
f = chebfun(@(x) [sin(pi*x), cos(pi*x), x]);
out = f(0,end);
pass(5) = isnumeric(out) && length(out) == 1&& abs(out) < eps;

g = f.';
out = f(0,end);
pass(6) = isnumeric(out) && length(out) == 1&& abs(out) < eps;

%% test use of ':'
f = chebfun(@(x) [sin(pi*x), cos(pi*x), x]);
out = f(end, :);
pass(7) = isnumeric(out) && all(size(out) == [1, 3]) && ...
    norm(out - [0 -1 1], inf) < eps;

out = f(:, end);
x = chebfun(@(x) x);
err = normest(out - x);
pass(8) = err < eps;

% Transpose:
g = f.';
out = g(end, :);
err = normest(out - x.');
pass(9) = err < eps;

out = g(:, end);
pass(10) = isnumeric(out) && all(size(out) == [3, 1]) && ...
    norm(out - [0 -1 1].', inf) < eps;

%% Test on singular function:

% Set a domain:
dom = [-2 7];

pow = -0.5;
op = @(x) (x - dom(2)).^pow.*sin(100*x);
f = chebfun(op, dom, 'exps', [0 pow], 'splitting', 'on');
out = f(end);
pass(11) = ( isnumeric(out) ) && all( size(out) == ones(1,2) );

%% Test for function defined on unbounded domain:

% Set a domain:
dom = [0 Inf];

op = @(x) 0.75+sin(10*x)./exp(x);
f = chebfun(op, dom, 'splitting', 'on');
out = f(end);
pass(12) = ( isnumeric(out) ) && all(size(out) == ones(1, 2) ) && ...
    ( abs(out - 0.75) < 1e2*eps );

end

function out = normest(f, dom)

% Generate a few random points to use as test values.
seedRNG(6178);
if ( nargin == 1 )
    x = 2 * rand(100, 1) - 1;
else
    x = sum(dom) * rand(10, 1) - dom(1);
end

out = norm(feval(f, x), inf);

end


% Gdsft_test.m
% Test the Gdsft object

% create Gdsft object
if ~isvar('A'), printm 'setup Gdsft_test'
	N = [32 30];
	M = 201;
	omega = linspace(0, 10*2*pi, M)'; % crude spiral:
	omega = pi*[cos(omega) sin(omega)].*omega(:,[1 1])/max(omega);
	pl = 330;
	if im, clf, subplot(pl+1), plot(omega(:,1), omega(:,2), '.'), end
	n_shift = N/2;

	mask = true(N);
	mask(1:5) = false; % stress

	if 1 % non-mex version
		A = Gdsft(omega, N, 'n_shift', n_shift, 'mask', mask, ...
			'use_mex', 0, 'class', 'fatrix2');
		fatrix2_tests(A, 'complex', 1)
		test_adjoint(A, 'complex', 1, 'big', 1);
	end

	classes = {'Fatrix', 'fatrix2'};
	for ic=1:numel(classes)
		cl = classes{ic};
		A = Gdsft(omega, N, 'n_shift', n_shift, 'nthread', 2, ...
			'mask', mask, 'class', cl);

		fatrix2_tests(A, 'complex', 1)
		test_adjoint(A, 'complex', 1, 'big', 1);
	end
end

% test data
if ~isvar('x'), printm 'setup data'
	x = zeros(N);
	x(5:25,10:25) = 1;
	x(15:20,15:20) = 2;
	x(15,5) = 2;
end

% compare forward
if 1
	yt = dtft(x, omega, 'n_shift', n_shift);
	yd = A * [x(mask) x(mask)]; % test with two
	yd = yd(:,1);
	equivs(yt, yd, 'thresh', 1.3e-6)
%	printf('forward max%%diff = %g', max_percent_diff(yt, yd))
end

% compare adjoint
if 1
	xt = mask .* dtft_adj(yt, omega, N, n_shift);
	xd = A' * [yt yt]; % test with two
	xd = xd(:,1);
	xd = embed(xd, mask);
	equivs(xt, xd)
%	printf('back max%%diff = %g', max_percent_diff(xt, xd))
end

if 1, printm 'test adjoint'
	As = Gdsft(omega, [7 8], 'n_shift', n_shift);
	test_adjoint(As, 'complex', 1);
end

if 1 % look at norm for small problem: sqrt(MN) is loose bound
	pr [norm(As) sqrt(prod(size(As))) sqrt(prod(As.idim))]
	Ad = Gdft('mask', true(As.idim));
	pr [norm(Ad) sqrt(prod(size(As))) sqrt(prod(Ad.idim))]
end


 function diff_im = anisodiff2D(im, num_iter, delta_t, kappa, option)
%ANISODIFF2D Conventional anisotropic diffusion
%   DIFF_IM = ANISODIFF2D(IM, NUM_ITER, DELTA_T, KAPPA, OPTION) perfoms
%   conventional anisotropic diffusion (Perona & Malik) upon a gray scale
%   image. A 2D network structure of 8 neighboring nodes is considered for
%   diffusion conduction.
%
%       ARGUMENT DESCRIPTION:
%               IM       - gray scale image (MxN).
%               NUM_ITER - number of iterations.
%               DELTA_T  - integration constant (0 <= delta_t <= 1/7).
%                          Usually, due to numerical stability this
%                          parameter is set to its maximum value.
%               KAPPA    - gradient modulus threshold that controls the conduction.
%               OPTION   - conduction coefficient functions proposed by Perona & Malik:
%                          1 - c(x,y,t) = exp(-(nablaI/kappa).^2),
%                              privileges high-contrast edges over low-contrast ones.
%                          2 - c(x,y,t) = 1./(1 + (nablaI/kappa).^2),
%                              privileges wide regions over smaller ones.
%
%       OUTPUT DESCRIPTION:
%                DIFF_IM - (diffused) image with the largest scale-space parameter.
%
%   Example
%   -------------
%   s = phantom(512) + randn(512);
%   num_iter = 15;
%   delta_t = 1/7;
%   kappa = 30;
%   option = 2;
%   ad = anisodiff2D(s,num_iter,delta_t,kappa,option);
%   figure, subplot 121, imshow(s,[]), subplot 122, imshow(ad,[])
%
% See also anisodiff1D, anisodiff3D.

% References:
%   P. Perona and J. Malik.
%   Scale-Space and Edge Detection Using Anisotropic Diffusion.
%   IEEE Transactions on Pattern Analysis and Machine Intelligence,
%   12(7):629-639, July 1990.
%
%   G. Grieg, O. Kubler, R. Kikinis, and F. A. Jolesz.
%   Nonlinear Anisotropic Filtering of MRI Data.
%   IEEE Transactions on Medical Imaging,
%   11(2):221-232, June 1992.
%
%   MATLAB implementation based on Peter Kovesi's anisodiff(.):
%   P. D. Kovesi. MATLAB and Octave Functions for Computer Vision and Image Processing.
%   School of Computer Science & Software Engineering,
%   The University of Western Australia. Available from:
%   <http://www.csse.uwa.edu.au/~pk/research/matlabfns/>.
%
% Credits:
% Daniel Simoes Lopes
% ICIST
% Instituto Superior Tecnico - Universidade Tecnica de Lisboa
% danlopes (at) civil ist utl pt
% http://www.civil.ist.utl.pt/~danlopes
%
% May 2007 original version.

% Convert input image to double.
im = double(im);

% PDE (partial differential equation) initial condition.
diff_im = im;

% Center pixel distances.
dx = 1;
dy = 1;
dd = sqrt(2);

% 2D convolution masks - finite differences.
hN = [0 1 0; 0 -1 0; 0 0 0];
hS = [0 0 0; 0 -1 0; 0 1 0];
hE = [0 0 0; 0 -1 1; 0 0 0];
hW = [0 0 0; 1 -1 0; 0 0 0];
hNE = [0 0 1; 0 -1 0; 0 0 0];
hSE = [0 0 0; 0 -1 0; 0 0 1];
hSW = [0 0 0; 0 -1 0; 1 0 0];
hNW = [1 0 0; 0 -1 0; 0 0 0];

% Anisotropic diffusion.
for t = 1:num_iter

        % Finite differences. [imfilter(.,.,'conv') can be replaced by conv2(.,.,'same')]
        nablaN = imfilter(diff_im,hN,'conv');
        nablaS = imfilter(diff_im,hS,'conv');
        nablaW = imfilter(diff_im,hW,'conv');
        nablaE = imfilter(diff_im,hE,'conv');
        nablaNE = imfilter(diff_im,hNE,'conv');
        nablaSE = imfilter(diff_im,hSE,'conv');
        nablaSW = imfilter(diff_im,hSW,'conv');
        nablaNW = imfilter(diff_im,hNW,'conv');

        % Diffusion function.
        if option == 1
            cN = exp(-(nablaN/kappa).^2);
            cS = exp(-(nablaS/kappa).^2);
            cW = exp(-(nablaW/kappa).^2);
            cE = exp(-(nablaE/kappa).^2);
            cNE = exp(-(nablaNE/kappa).^2);
            cSE = exp(-(nablaSE/kappa).^2);
            cSW = exp(-(nablaSW/kappa).^2);
            cNW = exp(-(nablaNW/kappa).^2);
        elseif option == 2
            cN = 1./(1 + (nablaN/kappa).^2);
            cS = 1./(1 + (nablaS/kappa).^2);
            cW = 1./(1 + (nablaW/kappa).^2);
            cE = 1./(1 + (nablaE/kappa).^2);
            cNE = 1./(1 + (nablaNE/kappa).^2);
            cSE = 1./(1 + (nablaSE/kappa).^2);
            cSW = 1./(1 + (nablaSW/kappa).^2);
            cNW = 1./(1 + (nablaNW/kappa).^2);
        end

        % Discrete PDE solution.
        diff_im = diff_im + ...
                  delta_t*(...
                  (1/(dy^2))*cN.*nablaN + (1/(dy^2))*cS.*nablaS + ...
                  (1/(dx^2))*cW.*nablaW + (1/(dx^2))*cE.*nablaE + ...
                  (1/(dd^2))*cNE.*nablaNE + (1/(dd^2))*cSE.*nablaSE + ...
                  (1/(dd^2))*cSW.*nablaSW + (1/(dd^2))*cNW.*nablaNW );

        % Iteration warning.
        %fprintf('\rIteration %d\n',t);
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Eugenio Alcala Baselga
% Date: 02/06/2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function dx = dynamic_complex_model(t, states, u)

% % %     % SEAT IRI Vehicle Parameters
% % %     m       = 1500 + 2*80 + 300; % mass, kg
% % %     Area    = 2.2;      % Superficie frontal, m^2
% % %     Cd      = 0.3;      % Cd aerodinamico
% % %     g       = 9.81;     % Gravity
% % %     ro      = 1.225;    % Densidad del aire en SI
% % %     a       = 4;
% % %     b       = 1.8;
% % %     I       = m*(a*a + b*b)/12;
% % %     FzF     = m/2 * 9.81;
% % %     FzR     = m/2 * 9.81;
% % %     muy     = 0.8;

    % CVC Vehicle Parameters
    m       = vehicle.m;
    I       = vehicle.I;
    a       = vehicle.a;
    b       = vehicle.b;
    ro      = vehicle.ro;
    Cd      = vehicle.Cd;
    Area    = vehicle.Area;
    g       = vehicle.g;
    muy     = vehicle.muy;
    FzF     = vehicle.FzF;
    FzR     = vehicle.FzR;

    mu_friction  = u(3);
    F_friction = mu_friction*m*g;  % Fuerzas que se oponen

    % States
    X       = states(1);
    Y       = states(2);
    PSI     = states(3);
    V       = states(4);        % Velocity [m/s]
    ALPHAT  = states(5);        % Slip angle [rad]
    OMEGA   = states(6);        % Yaw rate [rad/s]

    DELTA  = u(2);

    % Slip angles
    ALPHAF = atan2((V * sin(ALPHAT) + a * OMEGA), (V * cos(ALPHAT))) - DELTA;
    ALPHAR = atan2((V * sin(ALPHAT) - b * OMEGA), (V * cos(ALPHAT)));

    %%% Lateral forces (non-linear model)
    FyF = Fy_Tire_Model(ALPHAF, FzF, muy);
    FyR = Fy_Tire_Model(ALPHAR, FzR, muy);

%     Cf      = 15000;
%     FyF = Cf*(DELTA-ALPHAT-(a*OMEGA/V))
%     FyR = Cf*(-ALPHAT+(b*OMEGA/V))

    FxR = u(1);

    F_drag  = 0.5*ro*Cd*Area*V*V;

    % Equations of motion:
    dx(1,1) = V * cos(ALPHAT + PSI);    % X
    dx(2,1) = V * sin(ALPHAT + PSI);    % Y
    dx(3,1) = OMEGA;                     % dPSI
    dx(4,1) =  ( FxR * cos(ALPHAT) + FyF * sin(ALPHAT - DELTA) + FyR * sin(ALPHAT)...
                - F_drag - F_friction )/m ;                          %dV
    dx(5,1) = ( - FxR * sin(ALPHAT) +...
        FyF * cos(ALPHAT - DELTA) + FyR * cos(ALPHAT) - m * V * OMEGA) / (m * V);    %dALPHAT
    dx(6,1) = (FyF * a * cos(DELTA) - FyR * b) / I;          % Theta acceleration


clear
clc
PATH = rand(3,3);
tau_vec = rand(size(PATH,1)-1,1)*2+1;
t = rand(10,1)*sum(tau_vec);
%t = 0;
K = length(tau_vec);
% Mapping matrix from p to equality constraint, b
A = A_const_appA(tau_vec);
A_yaw = augmentA_yaw(tau_vec);
% Permutation matrix to rearrange b
C = permut_mat(K);
C_yaw = permutMat_yaw(tau_vec);
% Cost matrix
Q = augmentQ(tau_vec,4);
Q_yaw = augmentQ(tau_vec,2);
% C*A^(-T)*Q*A^(-1)*C^(T) = R
R = comp_R_appA(A,C,Q);
R_yaw = comp_R_appA(A_yaw,C_yaw,Q_yaw);
% Known derivatives including position.
bF = const_bf(PATH);
bFyaw = bF_yaw(0, tau_vec);
% Unknown b terms are optimized.
b_sorted = comp_b_sorted(R, bF);
b_srtdYaw = b_srtd_yaw(R_yaw, bFyaw);
% C*A*P = b_sorted
% P = [p0 p1 ... p8 p9| ... ] where P(t) = p9t^9 + p8t^8 + ... + p0
P = (C*A)\b_sorted;
states_matlab = [];
for i=1:length(t)
    des_s = desired_state(tau_vec, t(i), PATH, P);
    ParticleState = [des_s.pos des_s.vel des_s.acc des_s.jerk]';
    Snap = des_s.snap';
    states_matlab = [states_matlab; ParticleState;Snap];
end

dlmwrite('t_matlab.csv',t, 'precision', '%2.16f')
dlmwrite('path_matlab.csv',PATH, 'precision', '%2.16f')
dlmwrite('tau_vec_matlab.csv',tau_vec, 'precision', '%2.16f')
dlmwrite('states_matlab.csv',states_matlab, 'precision', '%2.16f')


function ROTZ = ROTZ(theta)

ROTZ = [cos(theta) -sin(theta) 0;
        sin(theta) cos(theta) 0;
        0 0 1];
end


function sig = MakeSignal(Name,n)
% MakeSignal -- Make artificial signal
%  Usage
%    sig = MakeSignal(Name,n)
%  Inputs
%    Name   string: 'HeaviSine', 'Bumps', 'Blocks',
%            'Doppler', 'Ramp', 'Cusp', 'Sing', 'HiSine',
%            'LoSine', 'LinChirp', 'TwoChirp', 'QuadChirp',
%            'MishMash', 'WernerSorrows' (Heisenberg),
%            'Leopold' (Kronecker), 'Piece-Regular' (Piece-Wise Smooth),
%	     'Riemann','HypChirps','LinChirps', 'Chirps', 'Gabor'
%	     'sineoneoverx','Cusp2','SmoothCusp','Gaussian'
%	     'Piece-Polynomial' (Piece-Wise 3rd degree polynomial)
%    n      desired signal length
%  Outputs
%    sig    1-d signal
%
%  References
%    Various articles of D.L. Donoho and I.M. Johnstone
%
   if nargin > 1,
	t = (1:n) ./n;
   end
	if strcmp(Name,'HeaviSine'),
	    sig = 4.*sin(4*pi.*t);
	    sig = sig - sign(t - .3) - sign(.72 - t);
	elseif strcmp(Name,'Bumps'),
	    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
	    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
	    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
	    sig = zeros(size(t));
	    for j =1:length(pos)
	       sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
	    end
	elseif strcmp(Name,'Blocks'),
	    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
	    hgt = [4 (-5) 3 (-4) 5 (-4.2) 2.1 4.3  (-3.1) 2.1 (-4.2)];
	    sig = zeros(size(t));
	    for j=1:length(pos)
	        sig = sig + (1 + sign(t-pos(j))).*(hgt(j)/2) ;
	    end
	elseif strcmp(Name,'Doppler'),
	    sig = sqrt(t.*(1-t)).*sin((2*pi*1.05) ./(t+.05));
	elseif strcmp(Name,'Ramp'),
	    sig = t - (t >= .37);
	elseif strcmp(Name,'Cusp'),
	    sig = sqrt(abs(t - .37));
	elseif strcmp(Name,'Sing'),
	    k = floor(n * .37);
	    sig = 1 ./abs(t - (k+.5)/n);
	elseif strcmp(Name,'HiSine'),
	    sig = sin( pi * (n * .6902) .* t);
	elseif strcmp(Name,'LoSine'),
	    sig = sin( pi * (n * .3333) .* t);
	elseif strcmp(Name,'LinChirp'),
	    sig = sin(pi .* t .* ((n .* .500) .* t));
	elseif strcmp(Name,'TwoChirp'),
	    sig = sin(pi .* t .* (n .* t)) + sin((pi/3) .* t .* (n .* t));
	elseif strcmp(Name,'QuadChirp'),
	    sig = sin( (pi/3) .* t .* (n .* t.^2));
	elseif strcmp(Name,'MishMash'),  % QuadChirp + LinChirp + HiSine
	    sig = sin( (pi/3) .* t .* (n .* t.^2)) ;
	    sig = sig +  sin( pi * (n * .6902) .* t);
	    sig = sig +  sin(pi .* t .* (n .* .125 .* t));
	elseif strcmp(Name,'WernerSorrows'),
	    sig = sin( pi .* t .* (n/2 .* t.^2)) ;
	    sig = sig +  sin( pi * (n * .6902) .* t);
	    sig = sig +  sin(pi .* t .* (n .* t));
	    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
	    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
	    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
	    for j =1:length(pos)
	       sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
	    end
	elseif strcmp(Name,'Leopold'),
	    sig = (t == floor(.37 * n)/n);  % Kronecker
	elseif strcmp(Name,'Riemann'),
		sqn = round(sqrt(n));
	    sig = t .* 0;  % Riemann's Non-differentiable Function
		sig((1:sqn).^2) = 1. ./ (1:sqn);
		sig = real(ifft(sig));
	elseif strcmp(Name,'HypChirps'), % Hyperbolic Chirps of Mallat's book
		alpha	= 15*n*pi/1024;
		beta    = 5*n*pi/1024;
		t  	= (1.001:1:n+.001)./n;
		f1      = zeros(1,n);
		f2      = zeros(1,n);
		f1  	= sin(alpha./(.8-t)).*(0.1<t).*(t<0.68);
		f2  	= sin(beta./(.8-t)).*(0.1<t).*(t<0.75);
		M  	= round(0.65*n);
		P 	= floor(M/4);
		enveloppe = ones(1,M); % the rising cutoff function
            enveloppe(1:P) = (1+sin(-pi/2+((1:P)-ones(1,P))./(P-1)*pi))/2;
            enveloppe(M-P+1:M) = reverse(enveloppe(1:P));
  		env 	= zeros(1,n);
  		env(ceil(n/10):M+ceil(n/10)-1) = enveloppe(1:M);
		sig     = (f1+f2).*env;
	elseif strcmp(Name,'LinChirps'), % Linear Chirps of Mallat's book
		b 	= 100*n*pi/1024;
		a 	= 250*n*pi/1024;
		t 	= (1:n)./n;
		A1 	= sqrt((t-1/n).*(1-t));
		sig	= A1.*(cos((a*(t).^2)) + cos((b*t+a*(t).^2)));
	elseif strcmp(Name,'Chirps'), % Mixture of Chirps of Mallat's book
		t 	= (1:n)./n.*10.*pi;
  		f1 	= cos(t.^2*n/1024);
		a 	= 30*n/1024;
  		t 	= (1:n)./n.*pi;
  		f2 	= cos(a.*(t.^3));
  		f2 	= reverse(f2);
		ix 	= (-n:n)./n.*20;
 		g 	= exp(-ix.^2*4*n/1024);
		i1 	= (n/2+1:n/2+n);
		i2 	= (n/8+1:n/8+n);
		j  	= (1:n)/n;
    		f3 	= g(i1).*cos(50.*pi.*j*n/1024);
		f4 	= g(i2).*cos(350.*pi.*j*n/1024);
		sig 	= f1+f2+f3+f4;
   	 enveloppe = ones(1,n); % the rising cutoff function
  	 enveloppe(1:n/8) = (1+sin(-pi/2+((1:n/8)-ones(1,n/8))./(n/8-1)*pi))/2;
  	 enveloppe(7*n/8+1:n) = reverse(enveloppe(1:n/8));
 		sig 	= sig.*enveloppe;
        elseif strcmp(Name,'Gabor'), % two modulated Gabor functions in
				     % Mallat's book
		N = 512;
   		t = (-N:N)*5/N;
        	j = (1:N)./N;
		g = exp(-t.^2*20);
		i1 = (2*N/4+1:2*N/4+N);
		i2 = (N/4+1:N/4+N);
		sig1 = 3*g(i1).*exp(i*N/16.*pi.*j);
		sig2 = 3*g(i2).*exp(i*N/4.*pi.*j);
   		sig = sig1+sig2;
	elseif strcmp(Name,'sineoneoverx'), % sin(1/x) in Mallat's book
		N = 1024;
		i1 = (-N+1:N);
		i1(N) = 1/100;
		i1 = i1./(N-1);
		sig = sin(1.5./(i1));
		sig = sig(513:1536);
	elseif strcmp(Name,'Cusp2'),
		N = 64;
		i1 = (1:N)./N;
		x = (1-sqrt(i1)) + i1/2 -.5;
		M = 8*N;
		sig = zeros(1,M);
		sig(M-1.5.*N+1:M-.5*N) = x;
		sig(M-2.5*N+2:M-1.5.*N+1) = reverse(x);
		sig(3*N+1:3*N + N) = .5*ones(1,N);
	elseif strcmp(Name,'SmoothCusp'),
		sig = MakeSignal('Cusp2');
		N = 64;
		M = 8*N;
		t = (1:M)/M;
		sigma = 0.01;
		g = exp(-.5.*(abs(t-.5)./sigma).^2)./sigma./sqrt(2*pi);
		g = fftshift(g);
		sig2 = iconv(g',sig)'/M;
   	elseif strcmp(Name,'Piece-Regular'),
		sig1=-15*MakeSignal('Bumps',n);
		t = (1:fix(n/12)) ./fix(n/12);
		sig2=-exp(4*t);
		t = (1:fix(n/7)) ./fix(n/7);
		sig5=exp(4*t)-exp(4);
		t = (1:fix(n/3)) ./fix(n/3);
		sigma=6/40;
		sig6=-70*exp(-((t-1/2).*(t-1/2))/(2*sigma^2));
		sig(1:fix(n/7))= sig6(1:fix(n/7));
		sig((fix(n/7)+1):fix(n/5))=0.5*sig6((fix(n/7)+1):fix(n/5));
		sig((fix(n/5)+1):fix(n/3))=sig6((fix(n/5)+1):fix(n/3));
		sig((fix(n/3)+1):fix(n/2))=sig1((fix(n/3)+1):fix(n/2));
		sig((fix(n/2)+1):(fix(n/2)+fix(n/12)))=sig2;
		sig((fix(n/2)+2*fix(n/12)):-1:(fix(n/2)+fix(n/12)+1))=sig2;
sig(fix(n/2)+2*fix(n/12)+fix(n/20)+1:(fix(n/2)+2*fix(n/12)+3*fix(n/20)))=...
-ones(1,fix(n/2)+2*fix(n/12)+3*fix(n/20)-fix(n/2)-2*fix(n/12)-fix(n/20))*25;
		k=fix(n/2)+2*fix(n/12)+3*fix(n/20);
		sig((k+1):(k+fix(n/7)))=sig5;
		diff=n-5*fix(n/5);
		sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
		% zero-mean
		bias=sum(sig)/n;
		sig=bias-sig;
   	elseif strcmp(Name,'Piece-Polynomial'),
		t = (1:fix(n/5)) ./fix(n/5);
		sig1=20*(t.^3+t.^2+4);
		sig3=40*(2.*t.^3+t) + 100;
		sig2=10.*t.^3 + 45;
		sig4=16*t.^2+8.*t+16;
		sig5=20*(t+4);
		sig6(1:fix(n/10))=ones(1,fix(n/10));
		sig6=sig6*20;
		sig(1:fix(n/5))=sig1;
		sig(2*fix(n/5):-1:(fix(n/5)+1))=sig2;
		sig((2*fix(n/5)+1):3*fix(n/5))=sig3;
		sig((3*fix(n/5)+1):4*fix(n/5))=sig4;
		sig((4*fix(n/5)+1):5*fix(n/5))=sig5(fix(n/5):-1:1);
		diff=n-5*fix(n/5);
		sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
		%sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=-ones(1,fix(n/10))*20;
		sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=ones(1,fix(n/10))*10;
		sig((n-fix(n/10)+1):(n+fix(n/20)-fix(n/10)))=ones(1,fix(n/20))*150;
		% zero-mean
		bias=sum(sig)/n;
		sig=sig-bias;
   	elseif strcmp(Name,'Gaussian'),
		sig=GWN(n,beta);
		g=zeros(1,n);
		lim=alpha*n;
		mult=pi/(2*alpha*n);
		g(1:lim)=(cos(mult*(1:lim))).^2;
		g((n/2+1):n)=g((n/2):-1:1);
		g = rnshift(g,n/2);
		g=g/norm(g);
		sig=iconv(g,sig);
       else
	    disp(sprintf('MakeSignal: I don*t recognize <<%s>>',Name))
	    disp('Allowable Names are:')
	       disp('HeaviSine'),
	       disp('Bumps'),
	       disp('Blocks'),
	       disp('Doppler'),
	       disp('Ramp'),
	       disp('Cusp'),
	       disp('Crease'),
	       disp('Sing'),
	       disp('HiSine'),
	       disp('LoSine'),
	       disp('LinChirp'),
	       disp('TwoChirp'),
	       disp('QuadChirp'),
	       disp('MishMash'),
	       disp('WernerSorrows'),
	       disp('Leopold'),
	       disp('Sing'),
	       disp('HiSine'),
	       disp('LoSine'),
	       disp('LinChirp'),
	       disp('TwoChirp'),
	       disp('QuadChirp'),
	       disp('MishMash'),
	       disp('WernerSorrows'),
	       disp('Leopold'),
	       disp('Riemann'),
	       disp('HypChirps'),
	       disp('LinChirps'),
	       disp('Chirps'),
	       disp('sineoneoverx'),
	       disp('Cusp2'),
	       disp('SmoothCusp'),
	       disp('Gabor'),
	       disp('Piece-Regular');
	       disp('Piece-Polynomial');
	       disp('Gaussian');
	end

%
% Originally made by David L. Donoho.
% Function has been enhanced.

%
%  Part of Wavelab Version 850
%  Built Tue Jan  3 13:20:39 EST 2006
%  This is Copyrighted Material
%  For Copying permissions see COPYING.m
%  Comments? e-mail wavelab@stat.stanford.edu


%bilinearFit
% Fits data to bilinear function
%
% Author: Jonathan Karr, jkarr@stanford.edu
% Affiliation: Covert Lab, Department of Bioengineering, Stanford University
% Last Updated: 1/11/2012
function [a, b, cutX, gof] = bilinearFit(x, y)

tf = ~isnan(x) & ~isnan(y);
x = x(tf);
y = y(tf);

[~, order] = sort(x);
x = x(order);
y = y(order);

a = NaN(numel(x), 2);
b = NaN(numel(x), 2);
sse = NaN(numel(x), 2);
rsquare = NaN(numel(x), 2);
adjrsquare = NaN(numel(x), 2);
for i = 2:numel(x)-2
    [fitobject1, gof1] = fit(x(1:i), y(1:i), 'poly1');
    [fitobject2, gof2] = fit(x(i+1:end), y(i+1:end), 'poly1');
    a(i, :) = [fitobject1.p2 fitobject2.p2];
    b(i, :) = [fitobject1.p1 fitobject2.p1];
    sse(i, :) = [gof1.sse gof2.sse];
    rsquare(i, :) = [gof1.rsquare gof2.rsquare];
    adjrsquare(i, :) = [gof1.adjrsquare gof2.adjrsquare];
end

[~, cutIdx] = nanmin(sum(sse, 2));
cutX = mean(x(cutIdx:cutIdx+1));

a = a(cutIdx, :);
b = b(cutIdx, :);
sse = sse(cutIdx, :);
rsquare = rsquare(cutIdx, :);
adjrsquare = adjrsquare(cutIdx, :);

sse0 = sum(sse);
rsquare0 = 1 - sse0 / sum((y-mean(y)).^2);
n = numel(x);
p = 3;
adjrsquare0 = 1 - (1 - rsquare0) * (n-1) / (n-p-1);

gof = struct(...
    'sse', [sse0 sse], ...
    'rsquare', [rsquare0 rsquare], ...
    'adjrsquare', [adjrsquare0 adjrsquare]);


function x=istft_multi_2(X,nsampl)

% ISTFT_MULTI Multichannel inverse short-time Fourier transform (ISTFT)
% using half-overlapping sine windows.
%
% x=istft_multi(X,nsampl)
%
% Inputs:
% X: nfram x nbin x nchan matrix containing STFT coefficients for nchan
% sources with nbin frequency bins and nfram time frames
% nsampl: number of samples to which the corresponding time-domain signals
% are to be truncated
%
% Output:
% x:  nsampl x nchan matrix containing the corresponding time-domain signals
% If x is a set of signals of length nsampl and X=stft_multi(x), then
% x=istft_multi(X,nsampl).

%%% Errors and warnings %%%
if nargin<2, error('Not enough input arguments.'); end
[nfram,nbin,nchan]=size(X);
if nbin==2*floor(nbin/2), error('The number of frequency bins must be odd.'); end
wlen=2*(nbin-1);

%%% Computing inverse STFT signal %%%
% Defining sine window
win=sin((.5:wlen-.5)/wlen*pi).';

%%% windowing method 1:
swin=ones((nfram+1)*wlen/2,1);
% for t=0:nfram-1,
%     swin(t*wlen/2+1:t*wlen/2+wlen)=swin(t*wlen/2+1:t*wlen/2+wlen)+win.^2;
% end
% swin=sqrt(swin);
%%%
swin(1:wlen/2,1)=win(1:wlen/2);
swin(nfram*wlen/2+1:end,1)=win(wlen/2+1:wlen);

x=zeros((nfram+1)*wlen/2,nchan);
for i=1:nchan,
    for t=0:nfram-1,
        % IFFT
        fframe=[X(t+1,:,i),conj(X(t+1,wlen/2:-1:2,i))];
        frame=real(ifft(fframe)).';
        %%% Overlap-add method 1
        x(t*wlen/2+1:t*wlen/2+wlen,i)=x(t*wlen/2+1:t*wlen/2+wlen,i)+frame.*win./swin(t*wlen/2+1:t*wlen/2+wlen);
%         %%% Overlap-add method 2
%         x(t*wlen/2+1:t*wlen/2+wlen,i)=x(t*wlen/2+1:t*wlen/2+wlen,i)+frame.*win;
    end
end

%%% just for method 2: to keep the energy before and after windowing the same.
% swin=zeros((nfram+1)*wlen/2,1);
% for t=0:nfram-1,
%     swin(t*wlen/2+1:t*wlen/2+wlen)=swin(t*wlen/2+1:t*wlen/2+wlen)+win.^2;
% end
% x=x./swin;

% % Truncation
x=x(1:nsampl,:);
return;


function linplus_test11 ( )

%*****************************************************************************80
%
%% TEST11 tests R83P_ML.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST11\n' );
  fprintf ( 1, '  R83P_ML computes A*x or A''*X\n' );
  fprintf ( 1, '    where A has been factored by R83P_FA.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );

  for job = 0 : 1
%
%  Set the matrix.
%
    [ a, seed ] = r83p_random ( n, seed );
%
%  Set the desired solution.
%
    x = r8vec_indicator ( n );
%
%  Compute the corresponding right hand side.
%
    if ( job == 0 )
      b = r83p_mxv ( n, a, x );
    else
      b = r83p_vxm ( n, a, x );
    end
%
%  Factor the matrix.
%
    [ a_lu, work2, work3, work4, info ] = r83p_fa ( n, a );

    if ( info ~= 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'TEST11 - Fatal error!\n' );
      fprintf ( 1, '  R83P_FA declares the matrix is singular!\n' );
      fprintf ( 1, '  The value of INFO is %d\n', info );
      return;
    end
%
%  Now multiply factored matrix times solution to get right hand side again.
%
    b2 = r83p_ml ( n, a_lu, x, job );

    if ( job == 0 )
      r8vec2_print_some ( n, b, b2, 10, '  A*x and PLU*x' );
    else
      r8vec2_print_some ( n, b, b2, 10, '  A''*x and (PLU)''*x' );
    end

  end

  return
end


function wavelenvsth
% _____________________________________________________
% This function gets the nomograph which shows  the relationship which exists
% between the wavelength, wave period and water depth using the dispersion
% equation.
%
% 0. Syntax:
% >> wavelenvsth
%
% 1. Inputs:
%     None.
%
% 2. Outputs:
%     Nomograph.
%
% 3. Example:
%  >> run(wavelenvsth)
%
%  4. Notes:
%     - It's only necessary to run the function.
%     - In the nomograph, it is activated the data cursor mode in order to
%       get the wavelength and waver depth in function of the water period.
%     - Wave depths in meters.
%     - Wave length in meters.
%
% 5. Referents:
%      Darlymple, R.G. and Dean R.A. (1999). Water Wave Mechanics
%              for Engineers and Scientist. Advanced Series on Ocean Engineering, Vol. 2
%              World Scientific. Singapure.
%      Le Mehuate, Bernard. (1976). An introduction to hidrodynamics and water waves.
%              Springer-Verlag. USA.
%
% Gabriel Ruiz.
% Jun-2006
% UNAM
%_______________________________________________________________________

%%%%%%%%%%  M A I N      F U N C T I O N   %%%%%%%%%%
% clear all; clc;
 h = 1:1:130;
    g = 9.81;
        T = 4.5;
            dh = h';
        m=1;
    fg =figure('Menubar', 'none', 'Name', 'Wavelength vs Period and Depth', 'NumberTitle', 'off',...
                    'Position' , [ 6 37 1011 697 ] , 'Color' , [ 0.87 0.87 0.87 ]);

    while T ~= 17

        for i=1:130
                    con = 1;
                        l(con) = 0;
                    l(con+1) = 1.56 * T ^ 2;

                    while abs( l(con+1) - l(con) ) > 0.0001,
                                l(con+2) = ( ( 9.81 * T ^ 2 ) / ( 2 * pi ) ) * tanh( ( 2 * pi * h(i) ) / l(con+1) );
                                con = con + 1;
                    end

                    L(i) =  l(con);
                    k = ( 2 * pi ) / L(i);
        end

        Ls{1,m} = L';
            hold on
                fgh =plot(dh,Ls{1,m});
                    xlabel('Water Depth (h), meters');
                        ylabel('Wave Length (L), meters');
                    xlim( [ 1 130 ] )
                set(fgh,'Color',rand(1,3));
            set(gca,'Box','on');
        drt = title('Wavelength vs Period and Depth', 'HorizontalAlignment' , 'center' , 'FontWeight', 'bold');
            set(gca, 'XGrid', 'off', 'XMinorTick', 'on' , 'YGrid' , 'off' , 'YMinorTick' , 'on', 'Fontsize', 8 );
                m = m+1;
                    T = 0.5+T;
    end

    T= 4.5:0.5:17;
        hj =length(T);
    textos2 = 'T = ';

    for i = 1 : hj
            stringer{i,1} = num2str(T(1,i));
            textos{i,1} = horzcat(textos2,stringer{i,1},' s');
    end
        asdk = legend(textos, 'Location', 'EastOutside');
            set(asdk, 'FontSize', 6);
                clc;
            datacursormode on;
        dcm_obj = datacursormode(fg);
       fundat = str2func('camdatos');
    set(dcm_obj, 'DisplayStyle', 'Window' , 'UpdateFcn' , fundat)
%%%%%%%%%%%%    E N D     M A I N     F U N C T I O N %%%%%%%%%%%%%

function [txtdcm,pos] = camdatos(empt,event_obj)    %% Subfunction 1
pos = get(event_obj,'Position');
txtdcm = {['Water Depth: ',num2str(pos(1))] , ['Wave Length: ',num2str(pos(2))]};

% End of Subfunction 1


function [Abar, Bbar, Cbar] = genCompMat(phi, gamma, lambda, Np, Nc)
% genCompMat Generate composite matrices for FTCOCP
%
% Author : Ajinkya Khade, askhade@ncsu.edu

Abar = cell(Np, 1);
Bbar = cell(Np, Nc);

b0 = zeros(size(gamma));

for i = 1:Np
    Abar{i} = phi^i;

    for j = 1:Nc
        if i >= j
            Bbar{i,j} = (phi^(i-j))*gamma;
        else
            Bbar{i,j} = b0;
        end
    end
end

Abar = cell2mat(Abar);
Bbar = cell2mat(Bbar);
Cbar = kron(eye(Np),lambda);

end


%[2016]-"Jaya: A simple and new optimization algorithm for solving
%constrained and unconstrained optimization problems"

% (9/12/2020)

function JA = jJayaAlgorithm(feat,label,opts)
% Parameters
lb    = 0;
ub    = 1;
thres = 0.5;

if isfield(opts,'N'), N = opts.N; end
if isfield(opts,'T'), max_Iter = opts.T; end
if isfield(opts,'thres'), thres = opts.thres; end

% Objective function
fun = @jFitnessFunction;
% Number of dimensions
dim = size(feat,2);
% Initial (26)
X   = zeros(N,dim);
for i = 1:N
	for d = 1:dim
    X(i,d) = lb + (ub - lb) * rand();
  end
end
% Fitness
fit  = zeros(1,N);
fitG = inf;
for i = 1:N
  fit(i) = fun(feat,label,(X(i,:) > thres),opts);
  % Best
  if fit(i) < fitG
    fitG = fit(i);
    Xgb  = X(i,:);
  end
end
% Pre
Xnew = zeros(N,dim);

curve = zeros(1,max_Iter);
curve(1) = fitG;
t = 2;
% Iteration
while t <= max_Iter
  % Identify best & worst in population
  [~, idxB] = min(fit);
  Xbest     = X(idxB,:);
  [~, idxW] = max(fit);
  Xworst    = X(idxW,:);
  % Start
  for i = 1:N
    for d = 1:dim
      % Random numbers
      r1 = rand();
      r2 = rand();
      % Update (1)
      Xnew(i,d) = X(i,d) + r1 * (Xbest(d) - abs(X(i,d))) - ...
        r2 * (Xworst(d) - abs(X(i,d)));
    end
    % Boundary
    XB = Xnew(i,:); XB(XB > ub) = ub; XB(XB < lb) = lb;
    Xnew(i,:) = XB;
  end
  % Fitness
  for i = 1:N
    Fnew = fun(feat,label,(Xnew(i,:) > thres),opts);
    % Greedy selection
    if Fnew < fit(i)
      fit(i) = Fnew;
      X(i,:) = Xnew(i,:);
    end
    % Best
    if fit(i) < fitG
      fitG = fit(i);
      Xgb  = X(i,:);
    end
  end
  % Save
  curve(t) = fitG;
  fprintf('\nIteration %d Best (JA)= %f',t,curve(t))
  t = t + 1;
end
% Select features based on selected index
Pos   = 1:dim;
Sf    = Pos((Xgb > thres) == 1);
sFeat = feat(:,Sf);
% Store results
JA.sf = Sf;
JA.ff = sFeat;
JA.nf = length(Sf);
JA.c  = curve;
JA.f  = feat;
JA.l  = label;
end


function [X, A, B, t, varargout] = VBMC(P, Y, options)
% Variational Bayesian Low Rank Matrix Completion
% Author (a.k.a. person to blame): S. Derin Babacan
%
% Last updated: January 31, 2012
% Usage:
% [X, A, B, t, varargout] = VSBL_MAT(P, Y, options)
% Solves for X = AB' in
% Y = P(X) + N = P(AB') + N  where X, A, B are low rank,
% N is dense Gaussian noise, and P is a binary sampling matrix.
%
% If you're using this code, please acknowledge
% Reference:
%    S. D. Babacan, M. Luessi, R. Molina, and A. K. Katsaggelos,
%    "Sparse Bayesian Methods for Low-Rank Matrix Estimation,"
%    IEEE Transactions on Signal Processing, 2012.
%
% This code is not optimized for speed. It implements the full variational
% Bayesian inference described in the paper, without any manipulation of
% the matrices. These are described in the paper above but not implemented
% in this version of the code. These manipulations might lead to significant
% decrease in running times depending on the size of matrix Y.
%
% -----------------------------------------------------------------------
%                                INPUTS
%   P               : binary (0-1) sampling matrix same size as Y
%   Y               : input matrix
%   options         : All options are *optional*. The default values are set
%                     automatically.
%
%   verbose       : output the progress? (0/1) default: 0.
%   init          : Initialization method.
%                   'rand': initialize A and B with random matrices
%                   'ml'  : Apply SVD to Y and initialize A and B using its factors.
%                   (default)
%   a_gamma0
%   b_gamma0      : hyperparameter values for gamma. default: 1e-6
%
%
%   a_beta0
%   b_beta0      : hyperparameter values for beta. default: 0
%
%   inf_flag      : Flag for inference of components of E. default: 1
%                    1: Standard
%                    2: fixed point MacKay
%   MAXITER       : max number of iterations. default: 100
%   UPDATE_BETA   : update noise variance? (0/1). default: 1
%   UPDATE_BETA_START : iteration number to start updating the noise variance
%   DIM_RED       : prune irrelevant dimensions during iterations? (0/1).  default: 1
%   DIMRED_THR    : threshold to remove columns from A and B. Used only if
%                   DIM_RED is 1.  default: 1e4
%   initial_rank  : The initial rank of A and B to start. Default is full
%                   rank, calculated from the SVD of Y. A smaller value is
%                   helpful if the data dimensions is large.
%   X_true,E_true : Original X and E matrices for simulations.
%                   If supplied and verbose=1,
%                   the errors are reported at each iteration
% -----------------------------------------------------------------------
%                                OUTPUTS
% X               : low rank component
% A, B            : low rank factors of X = AB'
% optional outputs (see the paper for the full definitions):
%   varargout{1} = gamma    --- hyperparameters for A and B
%   varargout{2} = beta     --- noise inverse variance
%   varargout{3} = it       --- number of iterations
%   varargout{4} = t        --- running time
%   varargout{5} = Sigma_A  --- covariance matrix of A
%   varargout{6} = Sigma_B  --- covariance matrix of B
% -----------------------------------------------------------------------
%
% Copyright (c): S. Derin Babacan, 2011
% 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 <http://www.gnu.org/licenses/> for more details.
%

%% Check input variables,
if ~exist('options','var')
    options = populate_vars([]);
else
    options = populate_vars(options);
end

verbose      = options.verbose;
a_gamma0     = options.a_gamma0;
b_gamma0     = options.b_gamma0;
a_beta0      = options.a_beta0;
b_beta0      = options.b_beta0;
MAXITER      = options.MAXITER;
inf_flag     = options.inf_flag;
UPDATE_BETA  = options.UPDATE_BETA;
DIMRED       = options.DIMRED;

if DIMRED,
    if isfield(options, 'DIMRED_THR')
        DIMRED_THR = options.DIMRED_THR;
    else
        DIMRED_THR = 1e4;
    end
end

UPDATE_BETA_START = options.UPDATE_BETA_START;

% For synthetic simulations
if isfield(options, 'X_true')
    X_true = options.X_true;
end

%% Initialization
[m n] = size(Y);
mn = m*n;
pmn = length(find(P == 1));
p = pmn/mn;

Y2sum = sum(Y(:).^2);
scale2 = Y2sum / (mn);
scale = sqrt(scale2);

% Initialize A and B
switch options.init,

    % Maximum likelihood
    case 'ml'
        [U, S, V] = svd(Y, 'econ');

        if strcmp(options.initial_rank, 'auto')
            r = min([m,n]);
        else
            r = options.initial_rank;
        end

        A = U(:,1:r)*(S(1:r,1:r)).^(0.5);
        B = (S(1:r,1:r)).^(0.5)*V(:,1:r)'; B = B';

        Sigma_A = repmat( scale*eye(r,r), [1 1 m] );
        Sigma_B = repmat( scale*eye(r,r), [1 1 n] );

        gammas = 1./diag(S.^2);
        gammas = gammas(1:r);
        gammas = ones(r,1)*scale;

        ssscale2 = 1;
        beta = ssscale2./scale2;

        if UPDATE_BETA == 0 & isfield(options, 'beta')
            beta = options.beta;
        end

    % Random initialization
    case 'rand'

        if strcmp(options.initial_rank, 'auto')
            r = min([m,n]);
        else
            r = options.initial_rank;
        end

        A = randn(m,r) * sqrt(scale);
        B = randn(n,r) * sqrt(scale);
        gammas = scale*ones(r,1);

        Sigma_A = repmat( scale*eye(r,r), [1 1 m] );
        Sigma_B = repmat( scale*eye(r,r), [1 1 n] );

        if UPDATE_BETA == 0 & isfield(options, 'beta')
            beta = options.beta;
        else
            beta = 1./scale2;
        end

end

X = A*B';

%% Iterations
tic
for it=1:MAXITER,

    old_X = X;

    Aw = diag(gammas);

    %% A step
    diagsA = 0;
    for i=1:m, %iterate over rows

        observed = find(P(i,:));
        Bi = B(observed,:);
        Sigma_A(:,:,i) = (beta*Bi'*Bi + beta*sum(Sigma_B(:,:,observed),3) + Aw)^(-1);
        A(i,:) = beta*Y(i,observed)*Bi*Sigma_A(:,:,i);
        diagsA = diagsA + diag( Sigma_A(:,:,i) );

    end

    %% B step
    diagsB = 0;
    for j=1:n, %Iterate over cols

        observed = find(P(:,j));
        Aj = A(observed,:);
        Sigma_B(:,:,j) = (beta*Aj'*Aj + beta*sum(Sigma_A(:,:,observed),3) + Aw)^(-1);
        B(j,:) = beta*Y(observed,j)'*Aj*Sigma_B(:,:,j);
        diagsB = diagsB + diag( Sigma_B(:,:,j) );

    end

    %% update X
    X = A*B';

    %% estimate gammas
    % Choose inference method (fixed point Mackay or standard)
    if inf_flag == 1, %Standard
        gammas = (m + n + a_gamma0)./( diag(B'*B) + diag(sum(Sigma_B,3)) + diag(A'*A)+ diag(sum(Sigma_A,3))+ b_gamma0);
    elseif inf_flag == 2, %Mackay
        gammas = (m + n + a_gamma0 - gammas.*( diag(sum(Sigma_A,3)) + diag(sum(Sigma_B,3)) ))./( diag(A'*A) + diag(B'*B)+b_gamma0);
    end

    %% estimate beta
    if  UPDATE_BETA & it>=UPDATE_BETA_START,

        err = sum(sum( abs(Y - P.*X).^2 ) );

        for l = 1: m
            observed = find(P(l, :));
            err = err + trace(B(observed,:)'*B(observed,:)*Sigma_A(:, :, l)) ...
                + trace(A(l, :)'*A(l, :) *sum(Sigma_B(:, :, observed), 3)) ...
                + trace( Sigma_A(:, :, l)*sum(Sigma_B(:, :, observed), 3));

        end

        beta = (pmn + a_beta0)/(err+b_beta0);
    end

    %% Prune irrelevant dimensions?
    if DIMRED,
        MAX_GAMMA = min(gammas) * DIMRED_THR;

        if sum(find(gammas > MAX_GAMMA)),

            indices = find(gammas <= MAX_GAMMA);

            A = A(:,indices);
            B = B(:,indices);
            gammas = gammas(indices);

            Sigma_A = Sigma_A(indices,indices,:);
            Sigma_B = Sigma_B(indices,indices,:);

            [m r] = size(A);
            [n r] = size(B);
        end

    end

    %% Check convergence and display progress
    conv = sum(sum( ( old_X - X ).^2 ) ) / mn;

    if verbose,

        if exist('X_true','var')
            % For synthetic simulations
            err = sum(sum( ( X - X_true ).^2 ) ) / sum(sum( ( X_true ).^2 ) );
            rms = sqrt( sum(sum( ( X - X_true ).^2 ) )/mn );
            relrec = norm(X_true-X,'fro')/norm(X_true,'fro');

            fprintf('it %d: Error = %g, beta = %g, conv = %g, r = %d\n', it, relrec, beta, conv, r);

        else

            fprintf('it %d: beta = %g, conv = %g, r = %d\n', it, beta, conv, r);

        end
    end

    % Check for convergence (do at least 5 iterations)
    if it > 5 & conv < options.thr
        break;
    end

end
t = toc;

varargout{1} = gammas;
varargout{2} = beta;
varargout{3} = it;
varargout{4} = t;
varargout{5} = Sigma_A;
varargout{6} = Sigma_B;

%% Function to populate options.
function [options] = populate_vars(options)

if isempty(options)
    options.init         = 'ml';
    options.verbose      = 1;
    options.a_gamma0     = 1e-6;
    options.b_gamma0     = 1e-6;
    options.a_beta0      = 0;
    options.b_beta0      = 0;
    options.MAXITER      = 100;
    options.DIMRED       = 1;
    options.UPDATE_BETA  = 1;
    options.mode         = 'VB';
    options.thr          = 1e-7;
    options.initial_rank = 'auto';
    options.inf_flag     = 1;
    options.UPDATE_BETA_START = 1;
else

    if ~isfield(options, 'init')
        options.init = 'ml';
    end

    if ~strcmp(options.init,'ml') | ~strcmp(options.init,'rand')
        options.init = 'ml';
    end

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

    if ~isfield(options, 'a_gamma0')
        options.a_gamma0   = 1e-6;
    end

    if ~isfield(options, 'b_gamma0')
        options.b_gamma0   = 1e-6;
    end

    if ~isfield(options, 'a_beta0')
        options.a_beta0   = 0;
    end

    if ~isfield(options, 'b_beta0')
        options.b_beta0   = 0;
    end

    if ~isfield(options, 'MAXITER')
        options.MAXITER   = 100;
    end

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

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

    if options.inf_flag ~= 1 & options.inf_flag ~= 2,
        options.inf_flag = 1; % Standard inference
    end

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

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

    if ~isfield(options, 'mode')
        options.mode = 'VB';
    end

    if ~isfield(options, 'thr')
        options.thr = 1e-7;
    end

    if ~isfield(options, 'initial_rank')
        options.initial_rank = 'auto';
    end

end


function Y = multiplyOneRegion( psfMatData, X );
%
%           Y = multiplyOneRegion( psfMatData, X );
%
%  This function computes the multiplication of a point spread
%  function matrix times an image vector; that is,
%
%       Y = psfMatrix * X
%
%  Here we assume the dimension of the PSF is essentially the same
%  as the dimension of the image.
%
%  Input:
%   psfMatData  -  complex array containing the matrix data, usually
%                  computed from onePsfMatrix.m
%            X  -  array containing the image to which the psf matrix
%                  is to be multiplied
%
%  Output:
%          Y  -  contains the result after multiplication.
%

%  J. Nagy  1/6/02

%
%  First we determine if any padding is needed.
%
Xpad = padarray(X, size(psfMatData) - size(X), 'post');

%
%  Now we perform the multiplications.
%  Note that this should work for 2D and 3D images.
%

Y = ifftn( psfMatData .* fftn( Xpad ) );

[nx, ny, nz] = size( X );
Y = real( Y(1:nx, 1:ny, 1:nz) );


function [w, g_sqr] = rmsprop(w, g_sqr, grad, opts, lr)
%RMSPROP
%   Example RMSProp solver, for use with CNN_TRAIN and CNN_TRAIN_DAG.
%
%   Set the initial learning rate for RMSProp in the options for
%   CNN_TRAIN and CNN_TRAIN_DAG. Note that a learning rate that works for
%   SGD may be inappropriate for RMSProp; the default is 0.001.
%
%   If called without any input argument, returns the default options
%   structure.
%
%   Solver options: (opts.train.solverOpts)
%
%   `epsilon`:: 1e-8
%      Small additive constant to regularize variance estimate.
%
%   `rho`:: 0.99
%      Moving average window for variance update, between 0 and 1 (larger
%      values result in slower/more stable updating).

% Copyright (C) 2016 Joao F. Henriques.
% All rights reserved.
%
% This file is part of the VLFeat library and is made available under
% the terms of the BSD license (see the COPYING file).

if nargin == 0 % Return the default solver options
  w = struct('epsilon', 1e-8, 'rho', 0.99);
  return;
end

if isempty(g_sqr)
  g_sqr = 0 ;
end

g_sqr = g_sqr * opts.rho + grad.^2 * (1 - opts.rho) ;

w = w - lr * grad ./ (sqrt(g_sqr) + opts.epsilon) ;