function allPosVar2D = rmPositionVariance2D(view, voxel, samp)
%
% rmPositionVariance2D - calculate the visual area size covered by the pRF
% centers of surrounding nodes, normalized by cortical area size (per 1 mm^2)
%
%  allPosVar2D = rmPositionVariance2D(view, voxel, samp)
%
% INPUT
%  view: VOLUME view should be provided
%  voxel: voxel size in mm (default = 1.5)
%  samp: sampling of neurons within a voxel (default=1)
%        1: uniform sampling, 2: gaussian sampling
% OUTPUT
%  allPosVar2D: 2D position variance (sigma)
%
% KA wrote it 08/10

if ieNotDefined('view')
    view = getCurView;
end
if ieNotDefined('voxel')
    voxel = 1.5;
end
if ieNotDefined('samp')
    samp = 1;
end

nodes   = double(view.nodes);
edges   = double(view.edges);
numNeighbors = double(view.nodes(4,:));
edgeOffsets  = double(view.nodes(5,:));

allPosVar2D=zeros(1,size(nodes,2));

X = view.rm.retinotopyModels{1}.x0;
Y = view.rm.retinotopyModels{1}.y0;

for ii=1:size(nodes,2)
    tri=[];
    neighbors_pRF_area = 0;
    neighbors_cort_area = 0;
    X_tmp = X(ii);
    Y_tmp = Y(ii);
    tmp=1;
    if nodes(6,ii)==1   % use the nodes in layer 1
        neighbors = edges(:, edgeOffsets(ii):edgeOffsets(ii)+numNeighbors(ii)-1);
        % calculate neighbors only in layer 1
        remove_nodes=find(nodes(6,neighbors)~=1);
        neighbors(remove_nodes)=[];

        if size(neighbors,2)>=3
            % Calculate the visual field size covered by pRF centers of neighboring
            % nodes
            for jj=1:size(neighbors,2) % remove neighbors having the same pRF center
                if sum(X_tmp==X(neighbors(jj))|Y_tmp==Y(neighbors(jj)))==0
                    X_tmp = [X_tmp X(neighbors(jj))];
                    Y_tmp = [Y_tmp Y(neighbors(jj))];
                end
            end
            if size(X_tmp,2)>=3 % remove the case where all points are on a single line
                for kk=3:size(X_tmp,2)
                    tmp=tmp & (X_tmp(kk)-X_tmp(2))/(X_tmp(1)-X_tmp(2))*(Y_tmp(1)-Y_tmp(2))+Y_tmp(2)==Y_tmp(kk);
                end
                if tmp~=1
                    % get triangles in visual field to calculate visual
                    % field coverage
                    tri_visual = delaunay(X_tmp,Y_tmp);
                    % calculate the sum of the area size of all triangles
                    for num=1:size(tri_visual,1)
                        a = norm([X_tmp(tri_visual(num,1))-X_tmp(tri_visual(num,2)) Y_tmp(tri_visual(num,1))-Y_tmp(tri_visual(num,2))]);
                        b = norm([X_tmp(tri_visual(num,2))-X_tmp(tri_visual(num,3)) Y_tmp(tri_visual(num,2))-Y_tmp(tri_visual(num,3))]);
                        c = norm([X_tmp(tri_visual(num,1))-X_tmp(tri_visual(num,3)) Y_tmp(tri_visual(num,1))-Y_tmp(tri_visual(num,3))]);
                        s = (a+b+c)/2;

                        neighbors_pRF_area = neighbors_pRF_area + sqrt(s.*(s-a).*(s-b).*(s-c));
                    end
                end
            end

            % Calculate the cortical area size covered by neighboring nodes

            % get triangles in cortex to calculate cortical area size
            for jj=1:size(neighbors,2)
                neighbors_tmp = edges(:,edgeOffsets(neighbors(jj)):edgeOffsets(neighbors(jj))+numNeighbors(neighbors(jj))-1);
                remove_nodes_tmp = find(nodes(6,neighbors_tmp)~=1);
                neighbors_tmp(remove_nodes_tmp)=[];
                for kk=1:size(neighbors_tmp,2)
                    if size(find(neighbors_tmp(kk)==neighbors),2)~=0
                        tmp = sort([ii neighbors(jj) neighbors_tmp(kk)]);
    %                     if size(find(sum((tri-ones(size(tri,1),1)*sort([ii neighbors(jj) neighbors_tmp(kk)]))')'==0),1)
                        if size(tri,1)==0 | (size(tri,1)>0 & ~(sum(tri(:,1)==tmp(1))*sum(tri(:,2)==tmp(2))*sum(tri(:,3)==tmp(3))))
                            tri=[tri;tmp];
                        end
                    end
                end
            end
            % calculate the sum of the cortical area size
            for num=1:size(tri,1)
                a = norm([nodes(1,tri(num,1))-nodes(1,tri(num,2)) nodes(2,tri(num,1))-nodes(2,tri(num,2)) nodes(3,tri(num,1))-nodes(3,tri(num,2))]);
                b = norm([nodes(1,tri(num,2))-nodes(1,tri(num,3)) nodes(2,tri(num,2))-nodes(2,tri(num,3)) nodes(3,tri(num,2))-nodes(3,tri(num,3))]);
                c = norm([nodes(1,tri(num,3))-nodes(1,tri(num,1)) nodes(2,tri(num,3))-nodes(2,tri(num,1)) nodes(3,tri(num,3))-nodes(3,tri(num,1))]);
                s = (a+b+c)/2;

                neighbors_cort_area = neighbors_cort_area + sqrt(s.*(s-a).*(s-b).*(s-c));
            end
        end
        allPosVar2D(ii) = neighbors_pRF_area/neighbors_cort_area;
    end
end

% multiply voxel size and transform into standard deviation
% uniform sampling in voxel
if samp==1
    allPosVar2D(ii) = sqrt(allPosVar2D(ii))*voxel/sqrt(3);
% gaussian sampling in voxel (diameter of the scatter area is assumed to be fwhm)
elseif dist==2
    allPosVar2D(ii) = sqrt(allPosVar2D(ii))*voxel/sqrt(2*log(2));
end

return;


function [dat_out,clindx,keepit] = iimg_cluster_prune(dat,datsig,volInfo)
% Prunes an image data vector (dat) assumed to contain suprathreshold
% contiguous clusters ("blobs") by saving only those blobs that have one or
% more significant (nonzero) elements in another image, datsig
%
% :Usage:
% ::
%
%     [dat_out,clindx,keepit] = iimg_cluster_prune(dat,datsig,volInfo)
%
% One intended use is to define an FWE-corrected significance map in
% datsig, and report blobs at some lower threshold (in dat) that have at
% least one corrected voxel.
%
% Try iimg_threshold to create dat vectors from Analyze images (e.g.,
% statistic images)
%
% ..
%    tor wager, july 06
% ..

% check data type and reduce to in-mask only if necessary
[dattype,dat] = iimg_check_indx(dat,volInfo,'masked');
[dattype2,datsig] = iimg_check_indx(datsig,volInfo,'masked');

% output in same format as dat input
switch dattype
    case 'full'
        dat_out = zeros(volInfo.nvox,1);          % in full image space
    case 'masked'
        dat_out = zeros(volInfo.n_inmask,1);      % in-mask space
    otherwise
        error('Internal error.  dattype should have been checked earlier.')
end

clindx = iimg_cluster_index(dat,volInfo.xyzlist');

n = max(clindx);
keepit = zeros(1,n);

for i = 1:n
    wh = clindx == i;
    keepit(i) = any(datsig(wh));

    if keepit(i)
        switch dattype
            case 'full'
                dat_out(volInfo.wh_inmask(wh)) = dat(wh);
            case 'masked'
                dat_out(wh) = dat(wh);
        end
    end
end

return


function value = r4_aint ( x )

%****************************************************************************80
%
%% R4_AINT truncates an R4 argument to an integer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2011
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the truncated version of X.
%
  if ( x < 0.0 )
    value = - floor ( abs ( x ) );
  else
    value =   floor ( abs ( x ) );
  end

  return
end


function value = r8_aint ( x )

%*****************************************************************************80
%
%% R8_AINT truncates an R8 argument to an integer.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    John Burkardt.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the truncated version of X.
%
  if ( x < 0.0 )
    value = - floor ( abs ( x ) );
  else
    value =   floor ( abs ( x ) );
  end

  return
end


function [F] = mci_nmm_r2p2_dfdp (x,u,P,M)
% State Jacobian for two region, two parameter NMM
% FORMAT [F] = mci_nmm_r2p2_dfdp (x,u,P,M)
%
% x         State
% u         Inputs
% P         Parameters
% M         Model structure
%
% F         F(i,j) = df(x)_i/dtheta_j
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny and Biswa Sengupta
% $Id: mci_nmm_r2p2_dfdx.m 6548 2015-09-11 12:39:47Z will $

% 18 state variables
F = zeros(18,18);

% f         Flow, dx/dt

curr_P=M.can_P; % Canonical parameter set

% 2 free parameters
curr_P.A{1}(2,1)=P(1); % Forward connection, w_21
curr_P.A{2}(1,2)=P(2); % Backward connection, w_12

f = mci_nmm_fx_delay(x,u,curr_P,M);


function makenoiseband(mrSESSION, fdrift, fresp, fcard)
% makenoiseband(mrSESSION, fdrift, fresp, fcard)
% PURPOSE: Provides input for mrVista function AddNoiseBand
% Works in Hz rather than cycles per scan : converts between the two
% using the correct TR taken from mrSESSION.functionals(1).framePeriod
% and nFrames
%
% Thomas Ferree @ UCSF
% Created 1/23/2005

% extract necessary parameters from mrSESSION
TR = mrSESSION.functionals(1).framePeriod;
nFrames = mrSESSION.functionals(1).nFrames;
df = 1/(TR*nFrames);

% compute discrete frequency indices from input

if length(fdrift) > 0
    drift1 = round(fdrift(1)/df) + 1;
    drift2 = round(fdrift(2)/df) + 1;
    driftband = [drift1:drift2];
    if drift1 < 1 | drift2 < 1 | drift1 > (fix(nFrames/2)+1) | drift2 > (fix(nFrames/2)+1)
        error('Drift band is out of range.');
    end
else
    driftband = [];
end

if length(fresp) > 0
    resp1 = round(fresp(1)/df) + 1;
    resp2 = round(fresp(2)/df) + 1;
    respband = [resp1:resp2];
    if resp1 < 1 | resp2 < 1 | resp1 > (fix(nFrames/2)+1) | resp2 > (fix(nFrames/2)+1)
        error('Respiration band is out of range.');
    end
else
    respband = [];
end

if length(fcard) > 0
    card1 = round(fcard(1)/df) + 1;
    card2 = round(fcard(2)/df) + 1;
    cardband = [card1:card2];
    if card1 < 1 | card2 < 1 | card1 > (fix(nFrames/2)+1) | card2 > (fix(nFrames/2)+1)
        error('Cardiac band is out of range.');
    end
else
    cardband = [];
end

% out of sequence errors

if length(fdrift) > 0 & length(fresp) > 0 & drift2 > resp1
    error('Definition of drift and respiration bands is mixed up.');
end

if length(fresp) > 0 & length(fcard) > 0 & resp2 > card1
    error('Definition of respiration and cardiac bands is mixed up.');
end

% compute noise band (frequencies to be kept)
allband = 1:nFrames/2+1;
dropband = union(union(driftband,respband),cardband);
noiseband = setdiff(allband,dropband);

% call mrVista function AddNoiseBand
AddNoiseBand(noiseband);


function linpack_s_test16 ( )

%*****************************************************************************80
%
%% TEST16 tests SPOCO.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  lda = n;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST16\n' );
  fprintf ( 1, '  For a positive definite symmetric matrix,\n' );
  fprintf ( 1, '  SPOCO estimates the reciprocal condition number.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Set the matrix A.
%
  a(1:n,1:n) = 0.0;

  for i = 1 : n
    a(i,i) = 2.0;
    if ( 1 < i )
      a(i,i-1) = -1.0;
    end
    if ( i < n )
      a(i,i+1) = -1.0;
    end
  end
%
%  Estimate the condition.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimate the condition.\n' );

  [ a, rcond, z, info ] = spoco ( a, lda, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Reciprocal condition  = %f\n', rcond );

  return
end


function diagX = cdiag(X)
% Extracts the diagonal elements of A.
%
% function diagX = cdiag(X)
%
% Returns the diagonal elements of A. The input A does not necessarily
% to be a square matrix. The function supports both numeric arrays and
% structs with fields real and imag. Note that diag currently does
% not support dlarrays and cdiag can be seen as a backup function.
%
% See also: manoptADhelp

% This file is part of Manopt: www.manopt.org.
% Original author: Xiaowen Jiang, Aug. 31, 2021.
% Contributors: Nicolas Boumal
% Change log:

    if iscstruct(X)
        assert(length(size(X.real)) == 2, 'Input should be a 2-D array')
        m = size(X.real,1);
        n = size(X.real,2);
        realX = X.real;
        imagX = X.imag;
        if n >= m
            diagX.real = realX(1:m+1:m^2);
            diagX.imag = imagx(1:m+1:m^2);
        else
            diagX.real = realX(1:m+1:m*n-m+n);
            diagX.imag = imagX(1:m+1:m*n-m+n);
        end

    elseif isnumeric(X)
        assert(length(size(X)) == 2, 'Input should be a 2-D array')
        m = size(X,1);
        n = size(X,2);
        if n >= m
            diagX = X(1:m+1:m^2);
        else
            diagX = X(1:m+1:m*n-m+n);
        end

    else
        ME = MException('cdiag:inputError', ...
                        'Input does not have the expected format.');
        throw(ME);
    end

end


function [dm] = m2dm(m)
% Convert length from meters to decimeters.
% Chad A. Greene 2012
dm = m*10;


function y=bvpme(t,x,flag,U)
y =[0.5*x(1)*(x(3)-x(1))/x(2);
    -0.5*(x(3)-x(1));
    (0.9-1000*(x(3)-x(5))-0.5*x(3)*(x(3)-x(1)))/x(4);
    0.5*(x(3)-x(1));
    -100*(x(5)-x(3));];
end


classdef prtClassMaryLogDisc < prtClass

    properties (SetAccess=private)
        name = 'bleh'  % Logistic Discriminant

        nameAbbreviation = 'meh'  % LogDisc

        isNativeMary = true;  % True
    end

    properties (SetAccess = protected)
        % w
        %   w is a DataSet.nDimensions + 1 x 1 vector of projection weights
        %   learned during LogDisc.train(DataSet)

        wMat = [];  % Regression weights

        % nIterations
        %   Number of iterations used in training.  This is set to a number
        %   between 1 and maxIter during training.

        nIterations = nan;  % The number of iterations used in training
        wChangeTolerance = 1e-2;
        converged = false;
    end

    properties
        % maxIter
        %   Maximum number of iterations to allow before exiting without
        %   convergence.

        maxIter = 500;  % Maxmimuum number of iterations
    end

    methods

        function self = prtClassMaryLogDisc(varargin)
            self = prtUtilAssignStringValuePairs(self,varargin{:});
        end

        function self = set.maxIter(self,val)
            if ~prtUtilIsPositiveScalarInteger(val)
                error('prt:prtClassLogisticDiscriminant:maxIter','maxIter must be a positive scalar integer');
            end
            self.maxIter = val;
        end
    end

    methods (Access=protected, Hidden = true)

        function self = trainAction(self,dataSet)
            %self = trainAction(self,dataSet)

            x = dataSet.getObservations;
            x = cat(2,ones(size(x,1),1),x); %DC component
            y = dataSet.getTargetsAsBinaryMatrix;

            nClasses = dataSet.nClasses;
            d = size(x,2);

            %random initialization; last set of weights is set to 0
            numWeights = d*(nClasses-1);
            weightMatrix = randn(nClasses-1,d);
            weightMatrix = cat(1,weightMatrix,zeros(1,size(weightMatrix,2)));
            weightMatrixOld = weightMatrix;

            %Can calculate B matrix outside loop; makes life fast
            xx = x'*x;
            B = kron(-1/2*(eye(nClasses-1)-ones(nClasses-1)/dataSet.nClasses),xx);

            self.converged = false;
            Binv = B^-1;
            for j = 1:self.maxIter
                psi = (weightMatrix*x')';
                py = bsxfun(@rdivide,exp(psi),sum(exp(psi),2));

                %label error
                yError = y-py;

                %Can we speed this up?  KRON is needlessly slow
                g = 0;
                for i = 1:size(yError,1)
                    g = g + kron(yError(i,:),x(i,:));
                end
                g = g(:);

                % Update weightMatrix in direction of gradient
                weightMatrix(1:end-1,:) = weightMatrix(1:end-1,:) - reshape((Binv*g(1:numWeights)),d,nClasses-1)';

                if norm(weightMatrix(:)-weightMatrixOld(:)) < self.wChangeTolerance
                    self.converged = true;
                    break;
                end
                weightMatrixOld = weightMatrix;

            end
            self.wMat = weightMatrix;
        end

        function ClassifierResults = runAction(self,dataSet)
            %ClassifierResults = runAction(self,DataSet)

            x = dataSet.getObservations;
            x = cat(2,ones(size(x,1),1),x);

            psi = (self.wMat*x')';
            y = bsxfun(@rdivide,exp(psi),sum(exp(psi),2));

            ClassifierResults = dataSet.setObservations(y);
        end
    end
end


function grad = B_log(future_layers, input,curr_layer)
future_grad = GetFutureGrad(future_layers, curr_layer);

grad = 1./(input+curr_layer.const).*future_grad;

end


function [L,U,p,q,inform] = luSOL(A)

%        [L,U,p,q,inform] = luSOL(A);
%
% This is an example script that uses lusolFactor
% to find LU factors of sparse matrix A
% using predefined options (hardwired here).

% In general, it is better to call lusolSet yourself
% and then reset a few options if necessary as shown.

% 28 Apr 2004: First version of luSOL based in splu.m.
%              Michael Saunders, SOL, Stanford University.

options = lusolSet;
options.Pivoting   = 'TRP';
options.FactorTol  = 4.0;

[L,U,p,q,options]  = lusolFactor(A,options);

inform = options.Inform;


function[varargout] = vsum(varargin)
%VSUM  Sum over non-NaN elements along a specified dimension.
%
%   Y=VSUM(X,DIM) takes the sum of all non-NaN elements of X along
%   dimension DIM.
%
%   [Y,NUM]=VSUM(X,DIM) also outputs the number of non-NaN data points NUM,
%   which has the same dimension as Y.
%
%   [Y1,Y2,...YN]=VSUM(X1,X2,...XN,DIM) also works.
%
%   VSUM(X1,X2,...XN,DIM);  with no output arguments overwrites the
%   original input variables.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2001--2016 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
  vsum_test,return
end

dim=varargin{end};

for i=1:length(varargin)-1
  [varargout{i},numi{i}]=vsum1(varargin{i},dim);
end

for i=length(varargin):nargout
  varargout{i}=numi{i-length(varargin)+1};
end

eval(to_overwrite(nargin-1))

function[y,num]=vsum1(x,dim)
ver=version;
%if str2num(ver(1)) >=9
try
    x=vswap(x,inf,nan);
    x=vswap(x,-inf,nan);
    x=vswap(x,inf+1i*inf,nan);

    y=sum(x,dim,'omitnan');
    if nargout==2
        num=sum(isfinite(x),dim,'omitnan');
    end
%else
catch
    y=sum(x,dim);
    if allall(isfinite(y))
        num=size(x,dim)+zeros(size(y));
    else
        nani=~isfinite(x);
        x(nani)=0;

        y=sum(x,dim);
        num=sum(~nani,dim);

        y(num==0)=nan;
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function[]=vsum_test
x1=[1 2 ; nan 4];
x2=[nan 6; nan 5];
ans1=[3 4]';
ans2=[6 5]';

vsum(x1,x2,2);
reporttest('VSUM output overwrite', aresame(x1,ans1) && aresame(x2,ans2))

x1=[1 2 ; nan 4];
ans1=[3 4]';
ans2=[2 1]';

[y1,y2]=vsum(x1,2);
reporttest('VSUM sum & num', aresame(y1,ans1) && aresame(y2,ans2))

x1=[1 2 ; 0 4];
ans1=[3 4]';
ans2=[2 2]';

[y1,y2]=vsum(x1,2);
reporttest('VSUM sum & num, no NaNs', aresame(y1,ans1) && aresame(y2,ans2))


% Test file for chebtech/conj.m

function pass = test_conj(pref)

if ( nargin < 1 )
    pref = chebtech.techPref();
end

for n = 1:2
    if (n == 1)
        testclass = chebtech1();
    else
        testclass = chebtech2();
    end

    % Test a scalar-valued function:
    f = testclass.make(@(x) cos(x) + 1i*sin(x), [], pref);
    g = testclass.make(@(x) cos(x) - 1i*sin(x), [], pref);
    h = conj(f);
    pass(n, 1) = norm(h.coeffs - g.coeffs, inf) < 10*vscale(h)*eps;

    % Test an array-valued function:
    f = testclass.make(@(x) [cos(x) + 1i*sin(x), -exp(1i*x)], [], pref);
    g = testclass.make(@(x) cos(x) - 1i*sin(x), [], pref);
    h = conj(f);
    pass(n, 2) = norm(h.coeffs - [g.coeffs, -(g.coeffs)], inf) < ...
        10*max(vscale(h)*eps);
end

end


function P=maxpooling_s2(A)

[h,w]=size(A);

if mod(h,2)==1
    A=[A;zeros(1,w)];
    h=h+1;
end
if mod(w,2)==1
    A=[A,zeros(h,1)];
    w=w+1;
end

A1=A(1:2:h-1,1:2:w-1);
A2=A(1:2:h-1,2:2:w);
A3=A(2:2:h,1:2:w-1);
A4=A(2:2:h,2:2:w);

P1=max(A1,A2);
P2=max(A3,A4);
P=max(P1,P2);


function CP = copula(gamma)

% copula - Compute copular associated to a joint distribution gamma.
%
% CP = copula(gamma)
%
%   Copyright (c) 2017 Gabriel Peyre

N = size(gamma,1);

mu = sum(gamma,2);
nu = sum(gamma,1)';

% cumulative
cmu = [0;cumsum(mu)];
cnu = [0;cumsum(nu)];
% inverse cumulatives
t = (0:N)'/N;
icmu = interp1(cmu, t, t, 'spline');
icnu = interp1(cnu, t, t, 'spline');
%
Cgamma = cumsum(cumsum(gamma,1),2);
s = linspace(0,1,N);

icmu = max(min(icmu,1),0);
icnu = max(min(icnu,1),0);

[S2,S1] = meshgrid(s,s);
[T2,T1] = meshgrid(icnu,icmu);
CP = interp2(S2,S1,Cgamma,T2,T1);

end


function imgRec = HDRJPEG2000Dec(name)
%
%
%       imgRec = HDRJPEG2000Dec(name)
%
%
%       Input:
%           -name: the prefix of the compressed HDR images using JPEG HDR
%
%       Output:
%           -imgRec: the reconstructed HDR image
%
%     Copyright (C) 2011-2015  Francesco Banterle
%
%     This program is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License
%     along with this program.  If not, see <http://www.gnu.org/licenses/>.
%

info = imfinfo(name); %read metadata
decoded = sscanf(cell2mat(info.Comments), '%g', 7);

nBit = decoded(end);
imgRec = double(imread(name)) / (2^nBit - 1);
for i=1:size(imgRec, 3)
    xMax = decoded((i - 1) * 2 + 1);
    xMin = decoded((i - 1) * 2 + 2);
    %range expansion
    imgRec(:,:,i) = exp(imgRec(:,:,i) * (xMax - xMin) + xMin) - 1e-6;
end

imgRec(imgRec < 0.0) = 0;

end


function fixed = p15_fixed_points ( m, fixed_num )

%*****************************************************************************80
%
%% P15_FIXED_POINTS returns the fixed points in problem 15.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per-Olof Persson and Gilbert Strang,
%    A Simple Mesh Generator in MATLAB,
%    SIAM Review,
%    Volume 46, Number 2, June 2004, pages 329-345.
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer FIXED_NUM, the number of fixed points.
%
%    Output, real FIXED(M,FIXED_NUM), the fixed points.
%
  fixed = [    ...
   -8.0, -1.0; ...
    2.0, -1.0; ...
    2.0,  0.0; ...
    8.0,  0.0; ...
    8.0,  1.0; ...
   -2.0,  1.0; ...
   -2.0,  0.0; ...
   -8.0,  0.0 ]';

  return
end


%This Matlab script can be used to reproduce Figures 4.5-6 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Number of BSs
L = 16;

%Number of UEs per BS
K = 10;

%Define the range of BS antennas
Mrange = 10:10:100;

%Extract maximum number of BS antennas
Mmax = max(Mrange);

%Define the range of pilot reuse factors
fRange = [1 2 4];

%Select the number of setups with random UE locations
nbrOfSetups = 100;

%Select the number of channel realizations per setup
nbrOfRealizations = 100;

%% Propagation parameters

%Communication bandwidth
B = 20e6;

%Total uplink transmit power per UE (mW)
p = 100;

%Noise figure at the BS (in dB)
noiseFigure = 7;

%Compute noise power
noiseVariancedBm = -174 + 10*log10(B) + noiseFigure;

%Select length of coherence block
tau_c = 200;

%Use the approximation of the Gaussian local scattering model
accuracy = 2;

%Angular standard deviation in the local scattering model (in degrees)
ASDdeg = 10;

%Prepare to save simulation results
sumSE_MR = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_ZF = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_RZF = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_SMMSE = zeros(length(Mrange),length(fRange),nbrOfSetups);
sumSE_MMMSE = zeros(length(Mrange),length(fRange),nbrOfSetups);

%% Go through all setups
for n = 1:nbrOfSetups

    %Output simulation progress
    disp([num2str(n) ' setups out of ' num2str(nbrOfSetups)]);

    %Compute channel statistics for one setup
    [R,channelGaindB] = functionExampleSetup(L,K,Mmax,accuracy,ASDdeg);

    %Compute the normalized average channel gain, where the normalization
    %is based on the noise power
    channelGainOverNoise = channelGaindB - noiseVariancedBm;

    %Go through all number of antennas
    for m = 1:length(Mrange)

        %Output simulation progress
        disp([num2str(m) ' antennas out of ' num2str(length(Mrange))]);

        %Go through all pilot reuse factors
        for s = 1:length(fRange)

            %Extract pilot reuse factor
            f = fRange(s);

            %Generate channel realizations with estimates and estimation
            %error correlation matrices
            [Hhat,C,tau_p,Rscaled] = functionChannelEstimates(R(1:Mrange(m),1:Mrange(m),:,:,:),channelGainOverNoise,nbrOfRealizations,Mrange(m),K,L,p,f);

            %Compute SEs using Theorem 4.1
            [SE_MR,SE_RZF,SE_MMMSE,SE_ZF,SE_SMMSE] = functionComputeSE_UL(Hhat,C,Rscaled,tau_c,tau_p,nbrOfRealizations,Mrange(m),K,L,p);

            %Save average sum SE per cell
            sumSE_MR(m,s,n) = mean(sum(SE_MR,1));
            sumSE_ZF(m,s,n) = mean(sum(SE_ZF,1));
            sumSE_SMMSE(m,s,n) = mean(sum(SE_SMMSE,1));
            sumSE_RZF(m,s,n) = mean(sum(SE_RZF,1));
            sumSE_MMMSE(m,s,n) = mean(sum(SE_MMMSE,1));

            %Delete large matrices
            clear Hhat C Rscaled;

        end

    end

    %Delete large matrices
    clear R;

end

%% Plot the simulation results
for s = 1:length(fRange)

    figure(s);
    hold on; box on;

    plot(Mrange,mean(sumSE_MMMSE(:,s,:),3),'rd-','LineWidth',1);
    plot(Mrange,mean(sumSE_SMMSE(:,s,:),3),'b:','LineWidth',1);
    plot(Mrange,mean(sumSE_RZF(:,s,:),3),'k-.','LineWidth',1);
    plot(Mrange,mean(sumSE_ZF(:,s,:),3),'r--','LineWidth',1);
    plot(Mrange,mean(sumSE_MR(:,s,:),3),'bs-','LineWidth',1);

    xlabel('Number of antennas (M)');
    ylabel('Average sum SE [bit/s/Hz/cell]');
    legend('M-MMSE','S-MMSE','RZF','ZF','MR','Location','NorthWest');
    ylim([0 60]);

end


function [ n_data, a, b, x, fx ] =  hyper_1f1_values ( n_data )

%*****************************************************************************80
%
%% HYPER_1F1_VALUES returns some values of the hypergeometric function 1F1.
%
%  Discussion:
%
%    In Mathematica, the function can be evaluated by:
%
%      fx = Hypergeometric1F1 [ a, b, x ]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Milton Abramowitz, Irene Stegun,
%    Handbook of Mathematical Functions,
%    National Bureau of Standards, 1964,
%    ISBN: 0-486-61272-4,
%    LC: QA47.A34.
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Cambridge University Press, 1999,
%    ISBN: 0-521-64314-7,
%    LC: QA76.95.W65.
%
%    Daniel Zwillinger, editor,
%    CRC Standard Mathematical Tables and Formulae,
%    30th Edition,
%    CRC Press, 1996,
%    ISBN: 0-8493-2479-3,
%    LC: QA47.M315.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0
%    before the first call.  On each call, the routine increments N_DATA by 1,
%    and returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real A, B, X, the parameters.
%
%    Output, real FX, the value of the function.
%
  n_max = 24;

  a_vec = [ ...
    -2.500, ...
    -0.500, ...
     0.500, ...
     2.500, ...
    -2.500, ...
    -0.500, ...
     0.500, ...
     2.500, ...
    -2.500, ...
    -0.500, ...
     0.500, ...
     2.500, ...
     0.825, ...
     1.100, ...
     1.650, ...
     3.300, ...
     0.825, ...
     1.100, ...
     1.650, ...
     3.300, ...
     0.825, ...
     1.100, ...
     1.650,...
     3.300 ];
  b_vec = [ ...
     3.3, ...
     1.1, ...
     1.1, ...
     3.3, ...
     3.3, ...
     1.1, ...
     1.1, ...
     3.3, ...
     3.3, ...
     1.1, ...
     1.1, ...
     3.3, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7, ...
     6.7 ];
  fx_vec = [ ...
     0.81879926689265186854, ...
     0.88283984828032972070, ...
     1.1245023764952626690, ...
     1.2101049301639599598, ...
     0.12723045536781567174, ...
     0.12326016871544045107, ...
     2.3297954665128293051, ...
     3.3890020264468009733, ...
    -0.18819510282516768874, ...
    -1.0764203806547022727, ...
     5.7521824680907968433, ...
     9.9998567403304086593, ...
     1.0317208964319891384, ...
     1.0424867029249952040, ...
     1.0643112000949092012, ...
     1.1321844369742336326, ...
     1.2328402688568452181, ...
     1.3200654482027340732, ...
     1.5104811522310825217, ...
     2.2307520785940524365, ...
     1.5197286298183137741, ...
     1.7364938170250847619, ...
     2.2492330307668135926, ...
     4.6377737119178965298 ];
  x_vec = [ ...
     0.25, ...
     0.25, ...
     0.25, ...
     0.25, ...
     1.55, ...
     1.55, ...
     1.55, ...
     1.55, ...
     2.85, ...
     2.85, ...
     2.85, ...
     2.85, ...
     0.25, ...
     0.25, ...
     0.25, ...
     0.25, ...
     1.55, ...
     1.55, ...
     1.55, ...
     1.55, ...
     2.85, ...
     2.85, ...
     2.85, ...
     2.85 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    a = 0.0;
    b = 0.0;
    x = 0.0;
    fx = 0.0;
  else
    a = a_vec(n_data);
    b = b_vec(n_data);
    x = x_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function plotMultipleAmps_SingleCondition(view)
%
% plotMultipleAmps_SingleCondition(view)
%
% Bar plot of the amplitudes for each scan, averaging across
% all pixels (in all slices) in a selection of ROIs. All y-axes are made the same. The bar heights
% and a coarse SEM can be obtained from get(gca,'UserData').
%
% gmb  5/25/98
% bw   2/19/99  Added seY field to the UserData field.
%	    seY is an estimate of the variability in the
%      amplitudes.  It is the SEM of the in the complex
%      (amp*exp(-i*ph)) representation.  The values are
%      computed in vectorMean.m
% fwc   11/07/02 plots data relative to current view
%       added plotting of multiple ROIs
%       ROI selection copied from plotMultipleTSeries.m
% Plots the mean amplitudes for each ROI in the current scan
% Based on plotMultipleAmps ARW 072803

mrGlobals;

% Select ROIs
nROIs=size(view.ROIs,2);
roiList=cell(1,nROIs);
for r=1:nROIs
    roiList{r}=view.ROIs(r).name;
end
selectedROIs = find(buttondlg('ROIs to Plot',roiList));
nROIs=length(selectedROIs);
if (nROIs==0)
    error('No ROIs selected');
end

% Plot it
selectGraphWin
clf
fontSize = 8;
headerStr = ['Mean Amplitudes'];
set(gcf,'Name',headerStr);

minylim=0;
maxylim=0;
nrows=0;
ncols=0;

nscans = numScans(view);
ROIamps=zeros(nscans,nROIs);
ROIseZ=zeros(nscans,nROIs);
ROImeanPhs=zeros(nscans,nROIs);

for r=1:nROIs

    n=selectedROIs(r);
    view = selectROI(view,n); % is there another way?
    [meanAmps,meanPhs,seZ] = vectorMeans(view);
    ROIamps(:,r)=meanAmps(:);
    ROIseZ(:,r)=seZ(:);
    ROImeanPhs(:,r)=meanPhs(:);
    %xstr{r}=[view.ROIs(selectedROIs(r)).name];

    roiName{r}=view.ROIs(selectedROIs(r)).name;
    fprintf(['\nROI #%d :',roiName{r}],r);
    xstr{r}=roiName{r};
end

% Now do the plotting

% Only plotting the current scan
   r=getCurScan(view);

   subplot(1,1,1);

    %plot the bar graph
    h=mybar(ROIamps(r,:)',ROIseZ(r,:)',xstr);
    xlabel('ROI','FontSize',fontSize);
    ylabel('Mean Amplitude','FontSize',fontSize);
    set(gca,'FontSize',ceil(fontSize*1.2));
    conditionName{r}=dataTYPES(view.curDataType).scanParams(r).annotation;
    fprintf(['\nCondition #%d :',conditionName{r}],r);
    title(conditionName{r});

%Save the data in gca('UserData')
data.y =ROIamps(r,:);

data.seY = ROIseZ(r,:); % this should probably be adapted

set(gca,'UserData',data);

return;


function [ DP_cost, DP_time, av_cost, av_time ] = DP_cut_k( cutCostsFile, cut_num, search_k)
%DP_CUT Summary of this function goes here
%   Detailed explanation goes here

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

    % get the initial averaging-cut

    cut_length = length(cut_costs) / cut_num;

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

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

    tic;
    best_values = [];
    best_cuts = [];
    search_domain = floor(cut_length / search_k);

    for i = 1:length(initial_cuts)

        domain = cut_costs(initial_cuts(i,:)-search_domain:initial_cuts(i,:)+search_domain,:);

        [min_value, min_cut] = min(domain);

        best_values(i,:) = min_value;

        best_cuts(i,:) = initial_cuts(i,:) + (min_cut - search_domain - 1);

    end

    DP_cost = sum(best_values);
    DP_time = toc;

    tic
    % get average cost sum
    av_sum = 0;
    for i = 1:length(initial_cuts)
       av_sum = av_sum + cut_costs(initial_cuts(i));
    end
    av_cost = av_sum;
    av_time = toc;

end


function LPproblem = liftModel(model, BIG, printLevel,fileName,directory)
% Lifts a COBRA model with badly-scaled stoichiometric and
% coupling constraints of the form:
% :math:`max c*v`  subject to: :math:`Sv = 0, x, Cv <= 0`
% Converts it into a COBRA LPproblem structure, which can be used with
% solveCobraLP. Fluxes for the reactions should stay the same i.e.
% sol.full(1:nRxns) should yield an optimal flux vector.
%
% USAGE:
%
%    LPproblem = liftModel(model, BIG, printLevel,fileName,directory)
%
% INPUTS:
%    model:     COBRA LPproblem Structure containing the original LP to be solved. The format of
%                   this struct is described in the documentation for `solveCobraLP.m`
%
% OPTIONAL INPUTS:
%    BIG:           A parameter the controls the largest entries that appear in the
%                   reformulated problem (default = 1000).
%    printLevel:    printLevel = 1 enables printing of problem statistics (default);
%                   printLevel = 0 silent
%    fileName:      name of th file to load
%    directory:     file directory (if `model` is empty, you can load it using `fileName` and `directory`)
%
%
% OUTPUTS:
%    LPproblem:         COBRA Structure contain the reformulated LP to be solved.
%
% .. Authors:
%       - Michael Saunders, saunders@stanford.edu
%       - Yuekai Sun, yuekai@stanford.edu, Systems Optimization Lab (SOL), Stanford University
%       - Ronan Fleming   (updated interface to take COBRA model structure)
%       - Thomas Pfau - Updated information, that the lifted problem is
%                       converted to a COBRA LP structure

if ~exist('BIG','var')
    BIG=1000;
end
if ~exist('printLevel','var')
    printLevel=1;
end
if exist('fileName','var') && exist('directory','var') && isempty(model)
    model = loadIdentifiedModel(fileName,directory);
end

%save original model
LPproblem = buildLPproblemFromModel(model);

% Assume constraint matrix is S if no A provided.
if ~isfield(LPproblem,'A')
    if isfield(LPproblem,'S')
        LPproblem.A = LPproblem.S;
    end
end

% Assume constraint S*v = b if csense not provided
if ~isfield(LPproblem,'csense')
    % If csense is not declared in the model, assume that all
    % constraints are equalities.
    LPproblem.csense(:,1) = 'E';
end

% Assume constraint S*v = 0 if b not provided
if ~isfield(LPproblem,'b')
    warning('LP problem has no defined b in S*v=b. b should be defined, for now we assume b=0')
    LPproblem.b=zeros(size(LPproblem.A,1),1);
end

% Assume max c'v s.t. S v = b if osense not provided
if ~isfield(LPproblem,'osense')
    LPproblem.osense = -1;
end

%call the LP reformulate script by Michael and Yuekai
LPproblem = reformulate(LPproblem, BIG, printLevel);

if exist('fileName','var') && exist('directory','var')
    save([directory filesep 'L_' fileName '.mat'],'LPproblem');
end


function [els_sun,els_ode,els_spm] = mci_compare_forward (model)
% Compare integration methods
% FORMAT [els_sun,els_ode,els_spm] = mci_compare_forward (model)
%
% model     'phase', 'nmm-r2p2'
%
% Run integration 9 times - compare speed and accuracy
%__________________________________________________________________________
% Copyright (C) 2015 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: mci_compare_forward.m 6548 2015-09-11 12:39:47Z will $

[P,M,U,Y] = mci_compare_setup (model);

M.reltol=1e-2;
M.abstol=1e-4;

M = spm_mci_minit (M);
if isstruct(M.pC)
    pC=full(diag(spm_vec(M.pC)));
else
    pC = M.pC;
end

UI.u=U';
UI.dt=M.T/M.N;

Nsamp=9;
for s=1:Nsamp,
    % New point
    P=spm_normrnd(M.vpE,pC,1);

    % Parameters in reduced space
    Pr = M.V'*(P-M.vpE);

    M.int='sundials';
    tic;
    y = spm_mci_fwd (P,M,U);
    ysun(:,s)=y(:,1);
    els_sun(s)=toc;

    M.int='ode15';
    tic;
    y = spm_mci_fwd (P,M,U);
    yode(:,s)=y(:,1);
    els_ode(s)=toc;

    % DCM for fMRI uses spm_int
    % DCM for ERP uses spm_int_L
    tic;
    y=spm_int_L(P,M,UI);
    yspm(:,s)=y(:,1);
    els_spm(s)=toc;

end

hs=figure;
set(hs,'Name','First Time Series');
lw=2;
rN=ceil(sqrt(Nsamp));
for s=1:Nsamp,
    subplot(rN,rN,s);
    plot(ysun(:,s),'k');
    hold on
    grid on
    plot(yode(:,s),'b');
    plot(yspm(:,s),'r');
end
legend('Sundials','ODE15','spm-int-L');

hs=figure;
set(hs,'Name','Integration Speed');
k=1; lw=2;
plot(els_ode,els_sun,'kx','MarkerSize',10);
hold on
mo=min(els_ode);
ma=max(els_ode);
plot([mo ma],[mo ma],'k','LineWidth',lw);
set(gca,'FontSize',18);
grid on
xlabel('ODE15');
ylabel('Sundials');

hs=figure;
set(hs,'Name','Integration Speed');
k=1; lw=2;
plot(els_spm,els_sun,'kx','MarkerSize',10);
hold on
mo=min(els_spm);
ma=max(els_spm);
plot([mo ma],[mo ma],'k','LineWidth',lw);
set(gca,'FontSize',18);
grid on
xlabel('spm-int-L');
ylabel('Sundials');


function x = NormL1_project(x,weights,tau)

if isreal(x)
   x = oneProjector(x,weights,tau);
else
   xa  = abs(x);
   idx = xa < eps;
   xc  = oneProjector(xa,weights,tau);
   xc  = xc ./ xa; xc(idx) = 0;
   x   = x .* xc;
end


function check = f_check ( m, n )

%*****************************************************************************80
%
%% F_CHECK checks the parameters of the F PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 August 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the parameters of the PDF.
%    0 < M
%    0 < N
%
%    Output, logical CHECK, is TRUE if the parameters are legal.
%
  if ( m ~= round ( m ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  M is not an integer.\n' );
    check = 0;
  end

  if ( m <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  M <= 0.\n' );
    check = 0;
  end

  if ( n ~= round ( n ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  N is not an integer.\n' );
    check = 0;
  end

  if ( n <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'F_CHECK - Fatal error!\n' );
    fprintf ( 1, '  N <= 0.\n' );
    check = 0;
  end

  check = 1;

  return
end


% =======================================================================
%     Improved adaptive complex diffusion despeckling filter (NCDF)
% =======================================================================
% DESCRIPTION: Demo for despeckling filter
%
% _________________________________________________________________________
% REFERENCES:
% [1] Rui Bernardes, Cristina Maduro, Pedro Serranho, Adérito Araújo,
%     Sílvia Barbeiro, and José Cunha-Vaz,
%     "Improved adaptive complex diffusion despeckling filter,"
%     Opt. Express 18, 24048-24059 (2010).
%
% Work developed under the research project supported by Fundação para a
% Ciência e a Tecnologia (FCT): PTDC/SAU-BEB/103151/2008 and program
% COMPETE (FCOMP-01-0124-FEDER-010930)
%
% http://www.opticsinfobase.org/oe/abstract.cfm?URI=oe-18-23-24048
% http://www.aibili.pt
%
%
% DEPENDENCIES:
% needs the matlab image processing toolbox
% _________________________________________________________________________
%
%
%
% AIBILI - Association for Innovation and Biomedical Research on Light and
% Image

% Demo
clear all;
close all;

% diffusion time in seconds
TMAX         = .80;

% display parameters
minIntensity = 0;
maxIntensity = 220;
iRoi         = [250  750     250    500];

% read an image
Img_noisy    = imread('img.jpg');

% Apply filter to reduce the speckle noise
[Img_filtered, nIter, dTT] = twodncdf(Img_noisy, TMAX);

% DISPLAY results
figure(1),
imagesc(Img_noisy),    title('original image');
caxis([minIntensity maxIntensity]), axis off, colormap(gray)
rect=[iRoi(3) iRoi(1) iRoi(4)-iRoi(3) iRoi(2)-iRoi(1)];
rectangle('Position',rect,'EdgeColor','y','LineStyle','--','Linewidth',2)

figure(2),
imagesc(Img_noisy),    title('original image');
caxis([minIntensity maxIntensity]), axis off, colormap(hsv)
axis(iRoi([3 4 1 2]))

figure(3),
imagesc(Img_filtered), title('filtered image');
caxis([minIntensity maxIntensity]), axis off, colormap(hsv)
axis(iRoi([3 4 1 2]))


% Clear and close everything
clear all;
close all;

% Add paths needed
addpath('functions/matGeom/polygons2d/')
addpath('functions/matGeom/geom3d/')
addpath('functions/lips/')

%% CONFIG INFORMATION IS ALL IN HERE

pathplanes = '../input/floorplan_spencer_small.txt';
pathpath = '../input/path_spencer_small_01.txt';

planeheight = 3; %meters

imurate = 200; %hz
lidarrate = 0.5; %hz

totalruntime = 240; %seconds

%% ROBOT TRAJECTORY LOADING AND PROCESSING
% Load our data from file
data = csvread(pathpath,1,0);
%pathtime = linspace(0,totalruntime,size(path_IMUinG,1));
pathtime = totalruntime*data(:,1)/max(data(:,1));
path_IMUinG = data(:,2:end);

% NOTE: Convert from feet poses to meters
path_IMUinG(:,1:3) = 0.3048.*path_IMUinG(:,1:3);

% Convert from the degrees to radians
path_IMUinG(:,4:6) = pi/180.*path_IMUinG(:,4:6);

% Create a spline for each dimension x,y,z and roll,pitch,yaw
% This will be used to evaluate the derivative to give us accelerations
% Can find the value at any point using "ppval"
pp_IMUinG = {};
for k=1:size(path_IMUinG,2)
    pp_IMUinG{end+1} = spline(pathtime,[0; path_IMUinG(:,k); 0]);
    %pp_IMUinG{end+1} = spline(pathtime,path_IMUinG(:,k));
    %pp_IMUinG{end+1} = csapi(pathtime,path_IMUinG(:,k));
    %pp_IMUinG{end+1} = pchip(pathtime,path_IMUinG(:,k));
    %pp_IMUinG{end+1} = csape(pathtime,path_IMUinG(:,k))
end

% Our time for the states is a combo of our different sensors
% Here we will combine the two rates, so everything is in "sync"
timeimu = linspace(0,totalruntime,imurate*totalruntime);
timelidar = linspace(0,totalruntime,lidarrate*totalruntime);

%% PLANES AND ENVIROMENT DATA LOADING AND PROCESSING
% Load our data from file (convert from feet to meters)
planes2d_PinG = 0.3048.*load_2dplanedata(pathplanes);

% Convert to polygons with a height of 8ft
planes3d_PinG = planes2dtopolygons3d(planes2d_PinG, planeheight);

%% VISULIZATION AND PLOTTING OF DATA FOR ANALYSIS
% Plot the polyons (uses geom3d function)
fh = figure(2);

fontsize = 20;
set(gcf,'PaperPositionMode','auto');
set(gcf,'defaultuicontrolfontsize',fontsize);
set(gcf,'defaultuicontrolfontname','Bitstream Charter');
set(gcf,'DefaultAxesFontSize',fontsize);
set(gcf,'DefaultAxesFontName','Bitstream Charter');
set(gcf,'DefaultTextFontSize',fontsize);
set(gcf,'DefaultTextFontname','Bitstream Charter');

clf(fh)
for ii=1:size(planes3d_PinG,2)
	drawPolygon3d(planes3d_PinG{ii}(:,1),planes3d_PinG{ii}(:,2),planes3d_PinG{ii}(:,3),'b');
    hold on;
end

% Plot path points and their IDs
for ii=1:size(path_IMUinG,1)
    drawPoint3d(path_IMUinG(ii,1),path_IMUinG(ii,2),path_IMUinG(ii,3),'or');
    drawCoordinates3d([path_IMUinG(ii,1),path_IMUinG(ii,2),path_IMUinG(ii,3)], [path_IMUinG(ii,4),path_IMUinG(ii,5),path_IMUinG(ii,6)], 1.5)
    hold on;
end
%text(path_IMUinG(:,1),path_IMUinG(:,2),path_IMUinG(:,3),[repmat('  ',size(path_IMUinG,1),1), num2str((1:size(path_IMUinG,1))')])
hold on;

% Plot the spline (use IMU rate so it is smooth)
plot3(ppval(pp_IMUinG{1},timeimu),ppval(pp_IMUinG{2},timeimu),ppval(pp_IMUinG{3},timeimu),'r');
hold on;

% Plot lidar poses over the spline
for time=timelidar
    %drawPoint3d(ppval(pp_IMUinG{1},time),ppval(pp_IMUinG{2},time),ppval(pp_IMUinG{3},time),'og');
    hold on;
    drawCoordinates3d([ppval(pp_IMUinG{1},time),ppval(pp_IMUinG{2},time),ppval(pp_IMUinG{3},time)],...
        [ppval(pp_IMUinG{4},time),ppval(pp_IMUinG{5},time),ppval(pp_IMUinG{6},time)], 0.75)
    hold on;
end

% Draw the global axis
drawCoordinates3d([0,0,0],[0,0,0],2)

% Do the labels and the such...
axis equal
xlabel('x-axis (m)')
ylabel('y-axis (m)')
zlabel('z-axis (m)')
view([0 90])

%% PLOT SAVE TO FILE FOR PAPER FIGURES
save_to_file = 0;
if save_to_file
    set(gcf,'Position',[0 0 1200 400])
    view([-25 25]);
    set(get(gca,'ylabel'),'rotation',-45)
    print(fh,'-dpng','-r500','trajectory_3d.png')
    view([0 90]);
    set(get(gca,'ylabel'),'rotation',90)
    print(fh,'-dpng','-r500','trajectory_2d_top.png')
    view([0 0]);
    print(fh,'-dpng','-r500','trajectory_2d_side.png')
end


function Y = scale(X,S,dims)
%SCALE Scale along specified dimensions of tensor.
%
%   Y = SCALE(X,S,DIMS) scales the tensor X along the dimension(s)
%   specified in DIMS using the scaling data in S. If DIMS contains
%   only one dimension, then S can be a column vector. Otherwise, S
%   should be a tensor.
%
%   Examples
%   X = tenones([3,4,5]);
%   S = 10 * [1:5]'; Y = scale(X,S,3)
%   S = tensor(10 * [1:5]',5); Y = scale(X,S,3)
%   S = tensor(1:12,[3 4]); Y = scale(X,S,[1 2])
%   S = tensor(1:12,[3 4]); Y = scale(X,S,-3)
%   S = tensor(1:60,[3 4 5]); Y = scale(X,S,1:3)
%
%   See also TENSOR, TENSOR/COLLAPSE.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

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

dims = tt_dimscheck(dims,ndims(X));
remdims = setdiff(1:ndims(X),dims);

% Convert to a matrix so that each column of A can be scaled by a
% vectorized version of S.
A = double(tenmat(X,dims,remdims));

switch(class(S))
    case {'tensor'}
        if ~isequal(size(S), X.size(dims))
            error 'Size mismatch';
        end
        % Vectorize S.
        S = double(tenmat(S,1:ndims(S),[]));
    case {'double'}
        if size(S,1) ~= X.size(dims)
            error 'Size mismatch';
        end
    otherwise
        error('Invalid scaling factor');
end

[m,n] = size(A);

% If the size of S is pretty small, we can convert it to a diagonal matrix
% and multiply by A. Otherwise, we scale A column-by-column.
if (m <= n)
    B = diag(S) * A;
else
    B = zeros(size(A));
    for j = 1:n
        B(:,j) = S .* A(:,j);
    end
end

% Convert the matrix B back into a tensor and return.
Y = tensor(tenmat(B,dims,remdims,X.size));


%{
Run this file to compile the mex files if you need to
(or if the binaries work for your computer, then you can skip this)
This file also contains two simple test problems

Stephen Becker, Feb 14, 2012  srbecker@alumni.caltech.edu
Updated         May  3, 2012  adding the f2c version for Windows userss
Updated         Jun 24, 2012  adding a lot of trouble-shooting support mainly for 64-bit linux
MAJOR Update    Feb 21, 2015  stephen.becker@colorado.edu
    completely redid mex file and lbfgs library
    (converted LBFGSB fortran code to C). Many changes. Most of the Matlab
    interface remains the same, but mex interface has changed.
    Should not affect end-user. Compilation is SO MUCH EASIER than it
    was with Fortran
    Most of the C code is the same as the Fortran, but a few fcn signatures
    different, and chanted "task" from str to int, and modified print functions
    No longer able to print to file

See also lbfgsb.m and lbfgsb_wrapper.c
%}

%% -- Compile --

% run mex -setup if you haven't already...

SRC_DIR = fullfile('..','src');
% Note: make sure to run be in the directory containing this file
% when you run this, otherwise above path is wrong
INCLUDES = SRC_DIR;
SRC = {'lbfgsb.c','linesearch.c','subalgorithms.c','print.c',...
    'linpack.c','miniCBLAS.c','timer.c'};
for i = 1:length(SRC)
    SRC{i} = fullfile(SRC_DIR,SRC{i});
end

if ispc
    % do not specify -lm flag
    mex('lbfgsb_wrapper.c','-largeArrayDims','-UDEBUG',...
        ['-I',SRC_DIR], SRC{:} );
else
    mex('lbfgsb_wrapper.c','-largeArrayDims','-lm','-UDEBUG',...
        ['-I',SRC_DIR], SRC{:} );
end

%% test the new function
disp('=== lbfgsb "driver1" test problem (Rosenbrock, 25 dimensions) === ');
% Here's the test problem included with lbfgsb called 'driver1'
% (It's a version of the Rosenbrock test function)

n   = 25;

l   = ones(n,1); u = l;
odd = 1:2:n;
even= 2:2:n;
l(odd) = 1.0;
u(odd) = 1.0e2;
l(even)= -1.0e2;
u(even)=  1.0e2;

opts    = struct( 'x0', 3*ones(n,1) );
opts.printEvery     = 2; % controls how often we print output from .m file wrapper
opts.m  = 5;
%opts.maxIts = 10;
opts.errFcn = @(x) norm(x-1); % for now just an arbitrary fcn
% opts.verbose = -1; % default is -1, i.e., no output from mex

[x,f,info] = lbfgsb( @driver1, l, u, opts );

% The true objective value is 0.
if abs(f) < 1e-8
    disp('Success!');
    semilogy( abs(info.err(:,1)-f),'o-' );
    xlabel('iteration');
    ylabel('error in objective function');
else
    disp('Something didn''t work right :-(  ');
end

% the structure info.err contains the objective function (1st column)
%   and norm(gradient,Inf) (2nd column)

%% another test function, the 2D Rosenbrock function
disp('=== Rosenbrock test function, 2D === ');
n = 2;

fxy = @(x,y) 100*( y-x.^2).^2  +  (1-x ).^2 ;
f   = @(x)   fxy( x(1,:), x(2,:) );
gxy = @(x,y) [100*(4*x.^3-4*x.*y)+2*x-2; 100*(2*y-2*x.^2)];
g   = @(x)   gxy( x(1,:), x(2,:) );

% There are no constraints
l   = -inf(n,1);
u   = inf(n,1);

opts    = struct( 'x0', [-1.9;2] );
opts.printEvery     = 1;
opts.m  = 5;

% Here's an example of using an error function. For Rosenbrock,
%   we know the true solution, so we can measure the error at every
%   iteration:
trueSoln = [1;1];
% "errFcn" will be printed to the screen
opts.errFcn     = @(x) norm(x-trueSoln)/max(norm(trueSoln),1);
% "outputFcn" will save values in the "info" output
opts.outputFcn  = opts.errFcn;

% Ask for very high accuracy
opts.pgtol      = 1e-10;
opts.factr      = 1e3;

% The {f,g} is another way to call it
[x,f,info] = lbfgsb( {f,g} , l, u, opts );

if abs(f) < 1e-8
    disp('Success!');
% since we included opts.outputFcn, the info.err now has 3 columns.
%   The first 2 columns are the same as before; the 3rd column
%   is the output of our outputFcn
semilogy( info.err(:,3)-f,'o-' ); xlabel('iteration'); ylabel('relative error in iterate function');
else
    disp('Something didn''t work right :-(  ');
end


% rbm_get_visible
% Copyright (C) 2011 KyungHyun Cho, Tapani Raiko, Alexander Ilin
%
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [x] = rbm_get_visible(h0, R)

n_visible = size(R.W, 1);
n_hidden = size(R.W, 2);

if R.data.binary == 1
    x = sigmoid(bsxfun(@plus,binornd(1, h0) * R.W', R.vbias'));
else
    x = bsxfun(@plus, h0 * R.W', R.vbias');
end
end


function L = log_lik_complete(bnet, cases, clamped)
% LOG_LIK_COMPLETE Compute sum_m sum_i log P(x(i,m)| x(pi_i,m), theta_i) for a completely observed data set
% L = log_lik_complete(bnet, cases, clamped)
%
% If there is a missing data, you must use an inference engine.
% cases(i,m) is the value assigned to node i in case m.
% (If there are vector-valued nodes, cases should be a cell array.)
% clamped(i,m) = 1 if node i was set by intervention in case m (default: clamped = zeros)
% Clamped nodes contribute a factor of 1.0 to the likelihood.

if iscell(cases), usecell = 1; else usecell = 0; end

n = length(bnet.dag);
ncases = size(cases, 2);
if n ~= size(cases, 1)
  error('data should be of size nnodes * ncases');
end

if nargin < 3, clamped = zeros(n,ncases); end

L = 0;
for i=1:n
  ps = parents(bnet.dag, i);
  e = bnet.equiv_class(i);
  u = find(clamped(i,:)==0);
  L = L + log_prob_node(bnet.CPD{e}, cases(i,u), cases(ps,u));
end


function u=glotros(d,t,p)
%GLOTROS  Rosenberg glottal model U=(D,T,P)
% d is derivative of flow waveform
% t is in fractions of a cycle
% p has parameters
%	p(1)=closure time
%	p(2)=+ve/-ve slope ratio

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: glotros.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin < 2
  tt=(0:99)'/100;
else
  tt=mod(t,1);
end
u=zeros(size(tt));
de=[0.6 0.5]';
if nargin < 3
  p=de;
elseif length(p)<2
  p=[p(:); de(length(p)+1:2)];
end
pp=p(1)/(1+p(2));
ta=tt<pp;
tb=tt<p(1) & ~ta;
wa=pi/pp;
wb=0.5*pi/(p(1)-pp);
fb=wb*pp;
if d==0
  u(ta)=0.5*(1-cos(wa*tt(ta)));
  u(tb)=cos(wb*tt(tb)-fb);
elseif d==1
  u(ta)=0.5*wa*sin(wa*tt(ta));
  u(tb)=-wb*sin(wb*tt(tb)-fb);
elseif d==2
  u(ta)=0.5*wa^2.*cos(wa*tt(ta));
  u(tb)=-wb^2*cos(wb*tt(tb)-fb);
else
  error('Derivative must be 0,1 or 2');
end


function train_timit_demo(context_win, hidden_units, num_layers, isdropout, isRNN, iscleanonly,...
    circular_step , isinputL1, MFCCorlogMelorSpectrum, framerate, pos_neg_r, outputnonlinear, opt, act, train_mode, const, const2, isGPU)
% Copyright (c) 2014-present University of Illinois at Urbana-Champaign
% All rights reserved.
%
% Developed by:     Po-Sen Huang, Paris Smaragdis
%                   Department of Electrical and Computer Engineering
%                   Department of Computer Science
%
%%
rand('state',0)
randn('state',0)
%%
%% setup paths for code. assumed this script runs in its own directory
baseDir= '../../';
codeDir = [baseDir,'codes', filesep];
minFuncDir = [baseDir, 'tools', filesep, 'minFunc_2012', filesep];

saveDir = [codeDir,filesep,'timit',...
           filesep,'discrim_joint_offset_all_results'];

addpath([baseDir, filesep,'tools', filesep,'labrosa']);
addpath([baseDir, filesep,'tools', filesep,'bss_eval']);

addpath(baseDir);
addpath(genpath(minFuncDir));
addpath([baseDir,filesep,'codes',filesep,'timit', filesep,'Data_with_dev']);
addpath(codeDir);
addpath([codeDir,'timit']);

CFGPath=[baseDir,'tools',filesep,'htk_features', filesep];
addpath(CFGPath);

%% setup network architecture
eI = [];
% 0- mfcc, 1- logmel, 2- spectrum
eI.MFCCorlogMelorSpectrum=MFCCorlogMelorSpectrum;
eI.CFGPath=CFGPath;
eI.seqLen = [1 10 25 50 100];
% eI.seqLen = [1];

eI.framerate=framerate;
if eI.framerate==64
    eI.winsize = 1024;    eI.nFFT = 1024;    eI.hop =eI.winsize/2;    eI.scf=1;%scf = 2/3;
else %32
    eI.winsize = 512;    eI.nFFT = 512;    eI.hop = eI.winsize/2;    eI.scf=1;%scf = 2/3;
end
winsize=eI.winsize; nFFT= eI.nFFT; hop= eI.hop; scf=eI.scf;
windows=sin(0:pi/winsize:pi-pi/winsize);

% single target or multiple targets
eI.cleanonly=iscleanonly;

% context window size of the input.
eI.num_contextwin = context_win;

% dimension of each input frame
if eI.MFCCorlogMelorSpectrum==0 %0 for mfcc 1 for logmel
    eI.featDim =39;
elseif eI.MFCCorlogMelorSpectrum==1 %0 for mfcc 1 for logmel
    eI.featDim =123;
else
    eI.featDim = (eI.nFFT/2+1);
end

eI.dropout = isdropout;

% weight tying in hidden layers
% if you want tied weights, must have odd number of *hidden* layers
eI.tieWeights = 0;

eI.const=const; eI.const2=const2;

hidden_units_set=[];

for il=1:num_layers
    hidden_units_set=[hidden_units_set, hidden_units];
end
% 2 hidden layers and output layer
if eI.cleanonly==1,
    eI.layerSizes = [hidden_units_set  nFFT/2+1 ];
else
    eI.layerSizes = [hidden_units_set  (nFFT/2+1)*2];
end
% highest hidden layer is temporal
eI.temporalLayer =isRNN;
% dim of network input at each timestep (final size after window & whiten)
eI.inputDim = eI.featDim * eI.num_contextwin;
% length of input sequence chunks.
% eI.seqLen = [1 10 25 50 100];
% activation function
switch act
    case 0
        eI.activationFn = 'logistic';
    case 1
        eI.activationFn = 'tanh';
    case 2
        eI.activationFn = 'RELU';
end

% temporal initialization type
eI.temporalInit = 'rand';
% weight norm penaly
eI.lambda = 0;
% file containing whitening matrices for outputs

eI.outputL1=0;

eI.inputL1=isinputL1;

eI.r=pos_neg_r;

eI.isdiscrim=2;

if opt==0,
    eI.opt='softlinear';
elseif opt==1,
    eI.opt='softabs';
elseif opt==2,
    eI.opt='softquad';
elseif opt==3,
    eI.opt='softabs_const';
elseif opt==4,
    eI.opt='softabs_kl_const';
end

eI.train_mode= train_mode;
eI.outputnonlinear=outputnonlinear;

%% setup weight caching
if isRNN,
    modelname=['model_RNN',num2str(isRNN)];
else
    modelname='model_DNN';
end
modelname=[modelname,'_win',num2str(context_win),'_h',num2str(hidden_units),'_l',num2str(num_layers)];
if iscleanonly, modelname=[modelname,'_cleanonly']; end
if isdropout,  modelname=[modelname,'_dropout'];    end
modelname=[modelname,['_r', num2str(eI.r)]];

modelname=[modelname,['_', num2str(eI.framerate),'ms']];
% modelname=[modelname,'_off',num2str(offset_step), '_snr', num2str(SNR_step)];
modelname=[modelname, '_', num2str(circular_step)];
eI.circular_step = circular_step;

modelname=[modelname,'_',eI.opt];
if outputnonlinear==0, modelname=[modelname,'_linearout']; end

modelname=[modelname, '_', eI.activationFn];

if eI.inputL1, modelname=[modelname, '_L',num2str(eI.inputL1)]; end

if eI.MFCCorlogMelorSpectrum==0
    modelname=[modelname,'_mfcc'];
elseif  eI.MFCCorlogMelorSpectrum==1
    modelname=[modelname,'_logmel'];
elseif  eI.MFCCorlogMelorSpectrum==2
    modelname=[modelname,'_spectrum'];
else
    modelname=[modelname,'_logpowspect'];
end

modelname= [modelname, '_trn', num2str(eI.train_mode)];

modelname=[modelname,'_c',num2str(const), '_c',num2str(const2)];

eI.modelname=modelname;
disp(modelname);

eI.saveDir = [saveDir, filesep, modelname, filesep];
if ~exist(eI.saveDir,'dir'), mkdir(eI.saveDir); end

%% initialize weights
[stack_i, W_t_i] = initialize_weights(eI);
[theta] = rnn_stack2params(stack_i, eI, W_t_i);

%% Directory of features
eI.featInBase =baseDir;

%% load data
eI.useCache = 0;

%% setup minFunc
options.Diagnostics = 'on';
options.Display = 'iter';
options.MaxIter = 800;
options.MaxFunEvals = 2500;
options.Corr = 50;
options.DerivativeCheck = 'off';
options.outputFcn = @save_callback_timit_general;

%% compute feature
SNRs=0;

[train1, fs, nbits]=wavread('female_train.wav');
[train2, fs, nbits]=wavread('male_train.wav');

maxLength=max([length(train1), length(train2)]);
train1(end+1:maxLength)=eps;
train2(end+1:maxLength)=eps;

train1=train1./sqrt(sum(train1.^2));
train2=train2./sqrt(sum(train2.^2));

eI.fs=fs;
%%
% chunk
[data_cell, targets_cell, mixture_spectrum]=formulate_data(train1, train2, eI, eI.train_mode); %0 -- chunk, 2--no chunk

  global SDR;
  SDR.deviter=0;   SDR.devmax=0;   SDR.testmax=0;

  eI.writewav=0;

if isGPU==1
  [theta,val]=minFunc(@drdae_discrim_joint_kl_obj_gpu, theta, options, eI, data_cell, targets_cell, mixture_spectrum, false, false);
else
  [theta,val]=minFunc(@drdae_discrim_joint_kl_obj, theta, options, eI, data_cell, targets_cell, mixture_spectrum, false, false);
end

  fprintf('%s\tdevmaxiter:\t%d\tdevSDR:\t%.3f\ttestSDR:\t%.3f\n',modelname, SDR.deviter, SDR.devmax, SDR.testmax);

return;

%% unit test

% context window size
context_win = 1;
% hidden units
hidden_units = 16;
num_layers = 1;
isdropout = 0;
% RNN temporal connection
isRNN = 2;
% One output source or two
iscleanonly = 0;
% Circular shift step
circular_step = 10000;
% normalize input as L1 norm = 1
isinputL1 = 0;
% 0: MFCC, 1: logmel, 2: spectra
MFCCorlogMelorSpectrum = 2;
% feature frame rate
framerate = 64;
% discriminative training gamma parameter
pos_neg_r = 0.05;
% Last layer - linear or nonlinear
outputnonlinear = 0;
% soft mask obj
softabs = 1;
% 0: logistic, 1: tanh, 2: RELU
act = 2;
% constant for avoiding numerical problems
const = 1e-10;
% constant for avoiding numerical problems
const2 = 0.001;
% 0: not using GPU, 1: using GPU
isGPU = 0;

train_mode = 0;
% 0:'softlinear',1:'softabs', 2:'softquad', 3:'softabs_const',
% 4:'softabs_kl_const'
opt = 1;

train_timit_demo(context_win, hidden_units, num_layers, isdropout, isRNN, iscleanonly,...
    circular_step , isinputL1, MFCCorlogMelorSpectrum, framerate, pos_neg_r, ...
    outputnonlinear, opt, act, train_mode, const, const2, isGPU)


if 0
    rxnForms = {' -> A','A -> B','B -> C', 'B -> D','D -> C','C ->'};
    rxnNames = {'Ain','AB','BC','BD','DC', 'Cout'};
    model = createModel(rxnNames, rxnNames,rxnForms);
    model.lb(3) = 2; %BD
    model.lb(4) = 2;
    model.ub(6) = 2;
else
    rxnForms = {' -> A','A -> B','A -> D','B -> C', 'B -> D','D -> C','C ->'};
    rxnNames = {'Ain','AB','AD','BC','BD','DC', 'Cout'};
    model = createModel(rxnNames, rxnNames,rxnForms);
    model.lb(1) = 5;
    %model.ub(6) = 2;
    model.ub(6) = 2.1;
    model.ub(4) = 1;
end

disp('-')
[nMet,nRxn]=size(model.S);

if exist('param','var')
clear param
end

if 0
    %weighting on zero norm of fluxes
    param.gamma0   = 0*1e-6;
    param.gamma1   = 1e-6;

    % weighting on relaxation of reaction bounds
    param.alpha0   = 10;
    param.alpha1   = 1;

    %weighting on relaxation of steady state constraints S*v = b
    param.lambda0   = 0;
    param.lambda1   = 0;
end

param.printLevel=2;

%stopping criterion
param.epsilon = 1e-6;
%capped l0 parameter
param.theta0   = 0.5;

sol = relaxedFBA(model,param);

model = findSExRxnInd(model);

return
plotRelaxedFBA(sol, model)


%
% [Sin,SNin] = Sin_vsh_ctf_fast(r_sphere,R,EX,EY,EZ,Lin)
%
% Calculate the internal SSS basis Sin for a CTF system
% using vector spherical harmonics
%
function [Sin,SNin] = Sin_vsh_ctf_fast(r_sphere,R,EX,EY,EZ,Lin)

% Copyright (c) 2016, Elekta Oy
% ---------------------------------------
%
% Redistribution and use of the Software in source and binary forms, with or without
% modification, are permitted for non-commercial use.
%
% The Software is provided "as is" without warranties of any kind, either express or
% implied including, without limitation, warranties that the Software is free of defects,
% merchantable, fit for a particular purpose. Developer/user agrees to bear the entire risk
% in connection with its use and distribution of any and all parts of the Software under this license.
%

mu0 = 1.25664e-6; % Permeability of vacuum
%
% For numerical surface integration:
%
%baseline = 50e-3;
dx = 4.5e-3;
dy = 4.5e-3;
dz1 = 0;
dz2 = 50e-3;
D = [dx dy dz1; dx -dy dz1; -dx dy dz1; -dx -dy dz1; dx dy dz2; dx -dy dz2; -dx dy dz2; -dx -dy dz2]';
for j = 1:8
   if j <= 4
      weights(j) = 1/(4*1);
   else
      weights(j) = -1/(4*1);
   end
end
weights = weights';

for ch = 1:size(R,2)
   count = 1;
   R(:,ch) = R(:,ch) - r_sphere;
   Sin(ch,:) = -mu0*vsh_response(R(:,ch),EX(:,ch),EY(:,ch),EZ(:,ch),D,weights,Lin);
end
for j = 1:size(Sin,2)
   SNin(:,j) = Sin(:,j)/norm(Sin(:,j));
end

function Sin_elements = vsh_response(r,ex,ey,ez,D,weights,Lin)

for j = 1:length(weights)
    r_this = r + D(1,j)*ex + D(2,j)*ey;
    rn(j) = norm(r_this);
    theta(j) = acos(r_this(3)/rn(j));
    phi(j) = atan2(r_this(2),r_this(1));
    sint(j) = sin(theta(j));
    sinp(j) = sin(phi(j));
    cost(j) = cos(theta(j));
    cosp(j) = cos(phi(j));
    for l = 1:Lin
       p0{l}(:,j) = legendre(l,cos(theta(j)));
       rnv(j,l) = rn(j)^(l+2);
   end
end
Sin_elements = [];
for l = 1:Lin
  for m = -l:l
    for j = 1:length(weights)
      %vs = vsh_modified_in_fast(theta(j),phi(j),l,m,p0{l}(:,j))'/rn(j)^(l+2);
      vs = vsh_modified_in_fast(theta(j),phi(j),l,m,p0{l}(:,j))'/rnv(j,l);
      V(1,j) = vs(1)*sint(j)*cosp(j) + vs(2)*cost(j)*cosp(j) - vs(3)*sinp(j);
      V(2,j) = vs(1)*sint(j)*sinp(j) + vs(2)*cost(j)*sinp(j) + vs(3)*cosp(j);
      V(3,j) = vs(1)*cost(j) - vs(2)*sint(j);
    end
    Sin_elements = [Sin_elements dot(V*weights,ez)]; % Cartesian coordinates
                                                     %Sin_element = Sin_element/sqrt((l+1)*(2*l+1));  % Back to orthonormal presentation
  end
end
%Sin_element = dot(V*weights,ez); % Cartesian coordinates
%Sin_element = Sin_element/sqrt((l+1)*(2*l+1));  % Back to orthonormal presentation


function Constraint = sdd(X)

if issymmetric(X)
    Constraint = [];
    n = size(X,1);
    M = 0;
    for ii = 1:n
        for jj = [1:1:ii-1 ii+1:1:n]
           Mij = sdpvar(2);
           Constraint = Constraint + sdp2socp(Mij);
            M = M + sparse([ii jj ii jj],[ii ii jj jj],Mij(:),n,n);
        end
    end
    Constraint = Constraint + [M == X];
else
    error('sdd requires a symmetric argument.');
end

function F = sdp2socp(M)
F=rcone(M(1,2),.5*M(1,1),M(2,2));


function [ Z, H, dnorm ] = deep_seminmf ( X, layers, varargin )

% Process optional arguments
pnames = { ...
    'z0' 'h0' 'bUpdateH' 'bUpdateLastH' 'maxiter' 'TolFun', ...
    'verbose', 'bUpdateZ', 'cache', 'gnd' ...
};

X = bsxfun(@rdivide,X,sqrt(sum(X.^2,1)));

num_of_layers = numel(layers);

Z = cell(1, num_of_layers);
H = cell(1, num_of_layers);

dflts  = {0, 0, 1, 1, 500, 1e-5, 1, 1, 1, 0};

[z0, h0, bUpdateH, bUpdateLastH, maxiter, tolfun, verbose, bUpdateZ, cache, gnd] = ...
        internal.stats.parseArgs(pnames,dflts,varargin{:});

if  ~iscell(h0)
    for i_layer = 1:length(layers)
        if i_layer == 1
            % For the first layer we go linear from X to Z*H, so we use id
            V = X;
        else
            V = H{i_layer-1};
        end

        if verbose
            display(sprintf('Initialising Layer #%d with k=%d with size(V)=%s...', i_layer, layers(i_layer), mat2str(size(V))));
        end
        if ~iscell(z0)
            % For the later layers we use nonlinearities as we go from
            % g(H_{k-1}) to Z*H_k
            [Z{i_layer}, H{i_layer}, ~] = ...
                 seminmf(V, ...
                     layers(i_layer), ...
                     'maxiter', maxiter, ...
                     'bUpdateH', true, 'bUpdateZ', bUpdateZ, 'verbose', verbose, 'save', cache, 'fast', 1);
        else
            display('Using existing Z');
            [Z{i_layer}, H{i_layer}, ~] = ...
                 seminmf(V, ...
                     layers(i_layer), ...
                     'maxiter', 1, ...
                     'bUpdateH', true, 'bUpdateZ', 0, 'z0', z0{i_layer}, 'verbose', verbose, 'save', cache, 'fast', 1);
        end
    end

else
    Z=z0;
    H=h0;

    if verbose
        display('Skipping initialization, using provided init matrices...');
    end
end

dnorm0 = cost_function(X, Z, H);
dnorm = dnorm0 + 1;

if verbose
    display(sprintf('#%d error: %f', 0, dnorm0));
end

%% Error Propagation
if verbose
    display('Finetuning...');
end
H_err = cell(1, num_of_layers);

for iter = 1:maxiter
    H_err{numel(layers)} = H{numel(layers)};
    for i_layer = numel(layers)-1:-1:1
        H_err{i_layer} = Z{i_layer+1} * H_err{i_layer+1};
    end

    for i = 1:numel(layers)
        if bUpdateZ
            try
                if i == 1
                    Z{i} = X  * pinv(H_err{1});
                else
                    Z{i} = pinv(D') * X * pinv(H_err{i});
                end
            catch
                display(sprintf('Convergance error %f. min Z{i}: %f. max %f', norm(Z{i}, 'fro'), min(min(Z{i})), max(max(Z{i}))));
            end
        end

        if i == 1
            D = Z{1}';
        else
            D = Z{i}' * D;
        end

        if bUpdateH && (i < numel(layers) || (i == numel(layers) && bUpdateLastH))
            A = D * X;
            Ap = (abs(A)+A)./2;
            An = (abs(A)-A)./2;

            B = D * D';

            Bp = (abs(B)+B)./2;
            Bn = (abs(B)-B)./2;

            H{i} = H{i} .* sqrt((Ap + Bn * H{i}) ./ max(An + Bp * H{i}, 1e-10));
        end
    end

    assert(i == numel(layers));

    dnorm = cost_function(X, Z, H);

    if verbose
        display(sprintf('#%d error: %f', iter, dnorm));
    end

%     assert(dnorm <= dnorm0 + 0.01, ...
%         sprintf('Rec. error increasing! From %f to %f. (%d)', ...
%         dnorm0, dnorm, iter) ...
%     );

    if verbose && length(gnd) > 1
       if mod(iter, 50) == 0
          ac = evalResults(H{numel(H)}, gnd);
          fprintf(1, 'Clustering accuracy is %.2f\n', ac);
       end
    end
%
%     if dnorm0-dnorm <= tolfun*max(1,dnorm0)
%         if verbose
%             display( ...
%                 sprintf('Stopped at %d: dnorm: %f, dnorm0: %f', ...
%                     iter, dnorm, dnorm0 ...
%                 ) ...
%             );
%         end
%         break;
%     end

    dnorm0 = dnorm;
end
end

function error = cost_function(X, Z, H)
    error = norm(X - reconstruction(Z, H), 'fro');
end

function [ out ] = reconstruction( Z, H )

    out = H{numel(H)};

    for k = numel(H) : -1 : 1;
        out =  Z{k} * out;
    end

end


% Reference Parameters

% Finish Time and Pen Time Table
[time_finish, time_table_pen] = cal_time_data(cp_ptp, ts1, time_wait);
max_pen_idx = length(time_table_pen) - 1;

% Position Reference
cp_ptp = cell2mat(cp_ptp);
x_ref = cp_ptp(1, :);
y_ref = cp_ptp(2, :);

% Angle Reference
[theta1_ref, theta2_ref] = xy2theta(x_ref, y_ref, l1, l2);

% Theta and Omega Limitation Check
thetas = [theta1_ref; theta2_ref];
thetas_max = [theta1_max; theta2_max];
omega1m_max = (100 - pwm1_offset) / pwm1_gain;
omega2m_max = (100 - pwm2_offset) / pwm2_gain;
omegas_max = [omega1m_max / g1; omega2m_max / g2];
chk_limit(thetas, thetas_max, omegas_max, ts1)

clear cp_ptp
clear thetas thetas_max omegas_max omega1m_max omega2m_max


function kern = nddisimKernParamInit(kern)

% NDDISIMKERNPARAMINIT NDDISIM kernel parameter initialisation.
% The driven input single input motif (DISIM) kernel is specifically designed for
% working with gene networks where there is assumed to be a single
% transcription factor controlling several genes. This transcription
% factor, in turn, is driven by its own gene's RNA. The model takes the
% following form: each gene is
% related to the transcription factor through the following
% differential equation,
%
% dx(t)/dt = B + C f(t-delta) - D x(t),
%
% where D is a decay term, C is a response term, delta is a time delay
% and B is an initial level. Then if f(t) is assumed to be the result of
% a further differential equation,
%
% df(t)/dt = Sx'(t) - D' x'(t)
%
% where x'(t) is assumed to come from a Gaussian process with an RBF
% covariance function f(t) is a Gaussian process with a covariance function
% provided by the single input motif kernel (SIM) and x(t) is a Gaussian
% process with covariance function provided by this kernel, the DISIM kernel.
%
% The kernel is designed to interoperate with the multiple output
% block kernel so that f(t) can be inferred given several different
% instantiations of x(t) (associated with different genes).
%
% The parameters (B, C, delta, S, D and D') are constrained positive.
%
% FORMAT
% DESC initialises the single input motif
%  kernel structure with some default parameters.
% ARG kern : the kernel structure which requires initialisation.
% RETURN kern : the kernel structure with the default parameters placed in.
%
% SEEALSO : kernCreate, kernParamInit, simKernCompute
%
% COPYRIGHT : Neil D. Lawrence, 2006
%
% COPYRIGHT : Antti Honkela, 2007, 2009
%
% COPYRIGHT: Jaakko Peltonen, 2011

% KERN

if kern.inputDimension > 1
  error('NDDISIM kernel is only valid for one-dimensional input.')
end

if isfield(kern, 'options') && isfield(kern.options, 'gaussianInitial') && ...
      kern.options.gaussianInitial,
  kern.gaussianInitial = 1;
  kern.initialVariance = 1;
else
  kern.gaussianInitial = 0;
end

kern.inverseWidth = 1;
kern.di_variance = 1;
kern.decay = 1;
kern.variance = 1;
kern.delay = 1e-7;

if kern.gaussianInitial,
  kern.nParams = 6;
else
  kern.nParams = 5;
end

if isfield(kern, 'options') && isfield(kern.options, 'paramTransform'),
  paramTransform = kern.options.paramTransform;
else
  paramTransform = 'sigmoidab';
end

switch paramTransform,
 case 'sigmoidab',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = 'sigmoidab';
    kern.transforms(k).transformsettings = [0 1e6];
  end;
 case 'bounded',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = optimiDefaultConstraint('bounded');
    kern.transforms(k).transformsettings = [0 1e6];
  end;
 case 'identity',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = 'identity';
    kern.transforms(k).transformsettings = [0 1e6];
  end;
 case 'positive',
  for k=1:kern.nParams,
    kern.transforms(k).index = k;
    kern.transforms(k).type = optimiDefaultConstraint('positive');
  end;
 case 'none',
 otherwise,
  error('Unknown paramTransform');
end

kern.isStationary = false;


function [latb,lonb] = bufferm2(varargin) %lat,lon,dist,direction,npts,outputformat)
%BUFFERM2 Computes buffer zone around a polygon
%
% [latb,lonb] = bufferm2(lat,lon,dist,direction)
% [latb,lonb] = bufferm2(lat,lon,dist,direction,npts)
% [latb,lonb] = bufferm2(lat,lon,dist,direction,npts,outputformat)
% [xb,  yb]   = bufferm2('xy',x,y,dist,direction,npts,outputformat)
%
% This function was originally designed as a replacement for the Mapping
% Toolbox function bufferm, which calculates a buffer zone around a
% polygon. The original bufferm function had some serious bugs that could
% result in incorrect buffer results and/or errors, and was also very slow.
% As of R2006b, those bugs have been fixed.  However, this version still
% maintains a few advantages over the original:
%
%   - Can be applied to polygons in either geographical space (as in
%   bufferm) or in cartesian coordinates.
%
%   - Better treatment of polygon holes.  The original function simply
%   filled in all holes; this version trims or pads holes according to the
%   buffer width given.
%
% Input and output format is identical to bufferm unless the 'xy' option is
% specified, so it can be used interchangeably.
%
% Input variables:
%
%   lat:            Latitude values defining the polygon to be buffered.
%                   This can be either a NaN-delimited vector, or a cell
%                   array containing individual polygonal contours (each of
%                   which is a vector). External contours should be listed
%                   in a clockwise direction, and internal contours (holes)
%                   in a counterclockwise direction.
%
%   lon:            Longitude values defining the polygon to be buffered.
%                   Same format as lat.
%
%   dist:           Width of buffer, in degrees of arc along the surface
%                   (unless 'xy' is used, in which case units correspond to
%                   x-y coordinates)
%
%   direction:      'in' or 'out'
%
%   npts:           Number of points used to contruct the circles around
%                   each polygon vertex.  If omitted, default is 13.
%
%   outputformat:   'vector' (NaN-delimited vectors), 'cutvector'
%                   (NaN-clipped vectors with cuts connecting holes to the
%                   exterior of the polygon), or 'cell' (cell arrays in
%                   which each element of the cell array is a separate
%                   polygon), defining format of output.  If omitted,
%                   default is 'vector'.
%
%   'xy':           If first input is 'xy', then data will be assumed to
%                   lie on a cartesian plane rather than on a sphere.  Use
%                   x and y coordinates as first two inputs rather than lat
%                   and lon.  Units of x, y, and distance should be the
%                   same.
%
% Output variables:
%
%   latb:           Latitude values for buffer polygon
%
%   lonb:           Longitude values for buffer polygon
%
% Example:
%
%   load conus
%   tol = 0.1; % Tolerance for simplifying polygon outlines
%   [reducedlat, reducedlon] = reducem(gtlakelat, gtlakelon, tol);
%   dist = 1;  % Buffer distance in degrees
%   [latb, lonb] = bufferm2(reducedlat, reducedlon, dist, 'out');
%   figure('Renderer','painters')
%   usamap({'MN','NY'})
%   geoshow(latb, lonb, 'DisplayType', 'polygon', 'FaceColor', 'yellow')
%   geoshow(gtlakelat, gtlakelon,...
%                       'DisplayType', 'polygon', 'FaceColor', 'blue')
%   geoshow(uslat, uslon)
%   geoshow(statelat, statelon)
%
% See also:
%
%   bufferm, polybool

% Copyright 2010 Kelly Kearney

%---------------------------
% Check input
%---------------------------

error(nargchk(3,7,nargin));

% Determine if geographic or cartesian

if ischar(varargin{1}) && strcmp(varargin{1}, 'xy')
    geo = false;
    param = varargin(2:end);
else
    geo = true;
    param = varargin;
end

% Set defaults if not provided as input

nparam = length(param);

if geo
    [lat, lon, dist] = deal(param{1:3});
else
    [lon, lat, dist] = deal(param{1:3}); % lon = x, lat = y for mental clarity, will switch back at end
end

if nparam < 4
    direction = 'out';
else
    direction = param{4};
end

if nparam < 5
    npts = 13;
else
    npts = param{5};
end

if nparam < 6
    outputformat = 'vector';
else
    outputformat = param{6};
end

% Check format and dimensions of input

if ~ismember(direction, {'in', 'out'})
    error('Direction must be either ''in'' or ''out''.');
end

if ~ismember(outputformat, {'vector', 'cutvector', 'cell'})
    error('Unrecognized output format flag.');
end

if ~isnumeric(dist) || numel(dist) > 1
    error('Distance must be a scalar.')
end

if ~isnumeric(npts) || numel(npts) > 1
    error('Number of points must be a scalar.')
end

if iscell(lat)
    for il = 1:numel(lat)
        if ~isvector(lat{il}) | ~isvector(lon{il}) | ~isequal(length(lat{il}), length(lon{il}))
            error('Lat (or x) and lon (or y) must be vectors or cells of vectors with identical dimensions');
        end
        lat{il} = lat{il}(:);
        lon{il} = lon{il}(:);
    end
else
    if ~isvector(lat) || ~isvector(lon) || ~isequal(length(lat), length(lon))
        error('Lat (or x) and lon (or y) must be vectors or cells of vectors with identical dimensions');
    end
    lat = lat(:);
    lon = lon(:);
end

%---------------------------
% Split polygon(s) into
% separate faces
%---------------------------

if iscell(lat)
    [lat, lon] = polyjoin(lat, lon);  % In case multiple faces in one cell.
end

[latcells, loncells] = polysplit(lat, lon);

%---------------------------
% Create buffer shapes
%---------------------------

plotflag = 0;

if plotflag

    Plt.x = lon;
    Plt.y = lat;

end

latcrall = cell(0);
loncrall = cell(0);

for ipoly = 1:length(latcells)

    % Circles around each vertex

    if geo
        [latc, lonc] = calccircgeo(latcells{ipoly}, loncells{ipoly}, dist, npts);
    else
        [lonc, latc] = calccirccart(loncells{ipoly}, latcells{ipoly}, dist, npts);
    end

    % Rectangles around each edge

    if geo
        [latr, lonr] = calcrecgeo(latcells{ipoly}, loncells{ipoly}, dist);
    else
        [lonr, latr] = calcreccart(loncells{ipoly}, latcells{ipoly}, dist);
    end

    % Union of circles and rectangles

    if plotflag
        Plt.rectx = lonr;
        Plt.recty = latr;
        Plt.circx = lonc;
        Plt.circy = latc;
    end

    [latc, lonc] = multipolyunion(latc, lonc);
    [latr, lonr] = multipolyunion(latr, lonr);

    if plotflag
        Plt.rectcombox = lonr;
        Plt.rectcomboy = latr;
        Plt.circcombox = lonc;
        Plt.circcomboy = latc;
    end

    [loncr, latcr] = polybool('+', lonr, latr, lonc, latc);

    % Union of new circle/rectangle combo with that from other faces

    [loncrall, latcrall] = polybool('+', loncrall, latcrall, loncr, latcr);

    % Plotting (for debugging only)

    if plotflag

        Plt.allx = loncrall;
        Plt.ally = latcrall;

        if ipoly == 1
            figure;
            plot(Plt.x, Plt.y, 'k', 'linewidth', 2);
            hold on
        end

        plot(cat(2, Plt.rectx{:}), cat(2, Plt.recty{:}), 'b');
        plot(cat(2, Plt.circx{:}), cat(2, Plt.circy{:}), 'r');
        plot(Plt.allx{1}, Plt.ally{1}, 'g', 'linewidth', 2);

    end

end

%---------------------------
% Calculate union/difference
%---------------------------

switch direction
    case 'out'
        [lonb, latb] = polybool('+', loncells, latcells, loncrall, latcrall);
    case 'in'
        [lonb, latb] = polybool('-', loncells, latcells, loncrall, latcrall);
end

if plotflag
    [Plt.yfinal, Plt.xfinal] = polyjoin(latb, lonb);
    plot(Plt.xfinal, Plt.yfinal, 'linestyle', '--', 'color', [0 .5 0], 'linewidth', 2);
end

%---------------------------
% Reformat output
%---------------------------

if ~geo
    y = latb; % Switch, since cartesion uses opposite order
    x = lonb;
    latb = x;
    lonb = y;
end

switch outputformat
    case 'vector'
        [latb, lonb] = polyjoin(latb, lonb);
    case 'cutvector'
        [latb, lonb] = polycut(latb, lonb);
    case 'cell'
end

%**************************************************************************

function [latc, lonc] = calccircgeo(lat, lon, radius, npts)
% lat and lon: n x 1 vectors
% radius: scalar

radius = ones(length(lat),1) * radius;
[latc, lonc] = scircle1(lat, lon, radius, [], [], [], npts);
latc = num2cell(latc, 1);
lonc = num2cell(lonc, 1);

function [latr, lonr] = calcrecgeo(lat, lon, halfwidth)
% lat and lon: n x 1 vectors
% halfwidth: scalar

range = halfwidth * ones(length(lat)-1, 1);

az = azimuth(lat(1:end-1), lon(1:end-1), lat(2:end), lon(2:end));

[latbl1,lonbl1] = reckon(lat(1:end-1), lon(1:end-1), range, az-90);
[latbr1,lonbr1] = reckon(lat(1:end-1), lon(1:end-1), range, az+90);
[latbl2,lonbl2] = reckon(lat(2:end),   lon(2:end),   range, az-90);
[latbr2,lonbr2] = reckon(lat(2:end),   lon(2:end),   range, az+90);

latr = [latbl1 latbl2 latbr2 latbr1 latbl1]';
lonr = [lonbl1 lonbl2 lonbr2 lonbr1 lonbl1]';
latr = num2cell(latr, 1);
lonr = num2cell(lonr, 1);

function [latu, lonu] = multipolyunion(lat, lon)
% lat and lon are n x 1 cell arrays of vectors

latu = lat{1};
lonu = lon{1};

for ip = 2:length(lat)
    [lonu, latu] = polybool('+', lonu, latu, lon{ip}, lat{ip});
end
[latu, lonu] = polysplit(latu, lonu);

function [xc, yc] = calccirccart(x, y, radius, npts)

ang = linspace(0, 2*pi, npts+1);
ang = ang(end-1:-1:1);
xc = bsxfun(@plus, x, radius * cos(ang));
yc = bsxfun(@plus, y, radius * sin(ang));
xc = num2cell(xc', 1);
yc = num2cell(yc', 1);

% if ~ispolycw(x,y)
%     [xc,yc] = poly2ccw(xc,yc);
% end

function [xrec, yrec] = calcreccart(x, y, halfwidth)

dx = diff(x);
dy = diff(y);

is1 = dx >= 0 & dy >= 0;
is2 = dx < 0 & dy >= 0;
is3 = dx < 0 & dy < 0;
is4 = dx >= 0 & dy < 0;

ish1 = dy == 0 & dx > 0;
ish2 = dy == 0 & dx < 0;

theta = zeros(5,1);
theta(is1 | is3) = atan(dy(is1 | is3)./dx(is1 | is3));
theta(is2 | is4) = -atan(dy(is2 | is4)./dx(is2 | is4));

[xl,xr,yl,yr] = deal(zeros(size(dx)));

xl(is1) = -halfwidth * sin(theta(is1));
xr(is1) =  halfwidth * sin(theta(is1));
yl(is1) =  halfwidth * cos(theta(is1));
yr(is1) = -halfwidth * cos(theta(is1));

xl(is2) = -halfwidth * sin(theta(is2));
xr(is2) =  halfwidth * sin(theta(is2));
yl(is2) = -halfwidth * cos(theta(is2));
yr(is2) =  halfwidth * cos(theta(is2));

xl(is3) =  halfwidth * sin(theta(is3));
xr(is3) = -halfwidth * sin(theta(is3));
yl(is3) = -halfwidth * cos(theta(is3));
yr(is3) =  halfwidth * cos(theta(is3));

xl(is4) =  halfwidth * sin(theta(is4));
xr(is4) = -halfwidth * sin(theta(is4));
yl(is4) =  halfwidth * cos(theta(is4));
yr(is4) = -halfwidth * cos(theta(is4));

xrec = [xl+x(1:end-1) xl+x(2:end) xr+x(2:end) xr+x(1:end-1) xl+x(1:end-1)];
yrec = [yl+y(1:end-1) yl+y(2:end) yr+y(2:end) yr+y(1:end-1) yl+y(1:end-1)];

xrec = num2cell(xrec, 2);
yrec = num2cell(yrec, 2);


function [nv,ev,uv] = AltVelCalc(msg)

% Copyright 2010, The MathWorks, Inc.

%AltVelCalc
% msg = '8DABDEEC99153E09802C013F4BDF';
% msg = '8DA66A1399104CAA80A406129D1D';
% msg = '8DAA8A83991502A7000411EE1091';
% msg = '8D40067899050CA680050D26CFB1';
% msg = '8D4CA74E9914C0AC80040F8D6965';
% msg = '8D80043C9904E4A7A0070D597008';
% msg = '8DABC6519904AFAC000513E3A869';
% msg = '8D4B187E9944CDAAA8040F67994E';
% msg = '8DA6CC41990488AE00070049848E';
% msg = '8DA9BF4B99948BB0A0040E5828F0';
% msg = '8D40067899050CA680050D26CFB1';
% msg = '8DA3CC3790C380976B152295D11F';  % Bad results
% msg = '8DA3CC37994483ACB004003CB62A';

a = hex2dec(msg');
b = dec2bin(a);
bin = reshape(b',1,length(msg)*4);

aircraftID = msg(3:8);

ewDir = bin(46);
if ewDir == '0'
    EW = 'East';
    ed = 1;
else
    EW = 'West';
    ed = -1;
end
ewVel = bin2dec(bin(47:56))-1;

nsDir = bin(57);
if nsDir == '0'
    NS = 'North';
    nd = 1;
else
    NS = 'South';
    nd = -1;
end
nsVel = bin2dec(bin(58:67))-1;

udDir = bin(69);
if udDir == '0'
    UD = 'Up';
    ud = 1;
else
    UD = 'Down';
    ud = -1;
end
udVel = (bin2dec(bin(70:78))-1)*64;

speed = sqrt(ewVel^2+nsVel^2);   % Speed in knots

disp(sprintf('Aircraft ID %s is traveling at %f knots\nDirection %s at %f knots, direction %s at %f knots ', aircraftID, speed, EW, ewVel, NS, nsVel));
disp(sprintf('Aircraft ID %s is going %s at %f feet/min\n', aircraftID, UD, udVel));

nv = nd*nsVel;
ev = ed*ewVel;
uv = ud*udVel;


function m=v_ccwarpf(f,n,s)
%V_CCWARPF  Warp cepstral coefficients M=(F,N,S)
% f(1) is the original sample freq, f(2) is the new sample freq
% n(1) is the original number of coefficients, n(2) is the new number
% s is a string: s(1),s(2) =l for linear, m for mel frequency, use capitals if c0 included

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_ccwarpf.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if nargin<3
   s='ll';
end
if length(f)<2
   f(2)=1;
end
if length(n)<2
   n(2)=n(1);
end
z=s<'a';
s=s+32*z;
if all(s=='l')
   k=1:n(2)-z(2);
   ff=((1:n(1)).'-z(1))*f(2)/f(1);
   fa=2*sin(ff*pi).*ff/pi;
   fb=ff.^2;
   ka=1-2*rem(k,2);
   kb=k.^2;
   r1=ones(n(1),1);
   c1=ones(1,n(2)-z(2));
   a=fa(:,c1).*ka(r1,:);
   b=fb(:,c1)-kb(r1,:);
   f0=find(fix(ff)==ff);
   if length(f0)
      a(f0,:)=ff(f0,c1)==k(ones(length(f0),1),:);
      b(f0,:)=1;
   end
   m=a./b;
   if z(2)
      m=[[1; 0.5*fa(2:n(1))./fb(2:n(1))] m];
   end
end


function y = meanfilter(x,r,dir)

if ~exist('dir','var'),
  if size(x,1) == 1 && size(x,2) > size(x,1),
    dir = 2;
  else
    dir = 1;
  end
end
sz = size(x);
notdir = true(1,length(sz)); notdir(dir) = false;
notdiridx = find(notdir);
x = permute(x,[dir,notdiridx]);
m = prod(sz(notdir));
x = reshape(x,[sz(dir),m]);

off1 = floor(r/2);
off2 = r - off1 - 1;
y = imfilter(x,ones(r,1),'same',0);
y(1:off1,:) = y(1:off1)./repmat(2*(1:off1),[m,1]);
y(end-off2+1:end,:) = y(end-off2+1:end,:) ./ repmat(2*(off2:-1:1),[m,1]);
y(off1+1:end-off2,:) = y(off1+1:end-off2,:) / r;

y = reshape(y,[sz(dir),sz(notdir)]);
y = permute(y,[2:dir,1,dir+1:length(sz)]);


function weighted_rmse = mohsst5_rmse_sparsepart(Yh, data)

Itest = load('ind20test.mat');
Itest = Itest.Itest;
if nargin < 2 || isempty(data)
  data = mohsst5_loaddata;
end

% Test set
Ytest = data.observations;
Ytest(~Itest) = nan;
missing = sum(isnan(Ytest(:))) / numel(Ytest);

% Take only data from EARLY parts
rows = size(data.observations,1);
rminds = data.time > datenum([1900 1 1 0 0 0]); % remove by time period
rminds = rowsum(~isnan(data.observations)) > 0.3 * rows; % remove by sparsity
rminds = data.time > datenum([9999 1 1 0 0 0]); % full data
Ytest(:,rminds) = nan;
missing = sum(isnan(Ytest(:))) / numel(Ytest);
testsize = sum(~isnan(Ytest(:)));

% $$$ % Take only data from sparse parts
% $$$ rows = size(data.observations,1);
% $$$ rminds = rowsum(~isnan(data.observations)) > 0.7 * rows;
% $$$ Ytest(:,rminds) = nan;
% $$$ missing = sum(isnan(Ytest(:))) / numel(Ytest)
% $$$ testsize = sum(~isnan(Ytest(:)))

weights = sqrt(cosd(data.coordinates(2,:)));
weights = weights(:);
Weights = bsxfun(@times, ~isnan(Ytest), weights(:)); % mask with missing values
Yhw = bsxfun(@times, Yh, weights(:));
Ytestw = bsxfun(@times, Ytest, weights(:));
err = Yhw(:) - Ytestw(:);
err(isnan(err)) = []; % remove missing values
weighted_rmse = sqrt(err'*err / sum(Weights(:).^2));


function [GAmp,GTime]=GyTrapezoid(p)

tStart=p.tStart; % start time ms
tEnd=p.tEnd;     % end time ms
tRamp=p.tRamp;   % ramp duration time ms
sRamp=p.sRamp;   % ramp steps
GyAmp=p.GyAmp;   % Gy amplitude
Duplicates=max(1,p.Duplicates);
DupSpacing=max(0,p.DupSpacing);

[GAmp,GTime]=StdTrap(tStart-tRamp, ...
                     tEnd+tRamp,   ...
                     tStart,               ...
                     tEnd,                 ...
                     GyAmp,max(2,sRamp),2,max(2,sRamp));

[GTime,m,n]=unique(GTime);
GAmp=GAmp(m);

% Create Duplicates
if Duplicates~=1 & DupSpacing ~=0
    GAmp=repmat(GAmp,[1 Duplicates]);
    TimeOffset = repmat(0:DupSpacing:(Duplicates-1)*DupSpacing,[length(GTime) 1]);
    GTime=repmat(GTime,[1 Duplicates]) + (TimeOffset(:))';
end

end


function [cc] = mi32cc(mi3)
% Convert volume from cubic miles to cubic centimeters.
% Chad Greene 2012
cc = mi3*4168181825400000 ;


%DEMO_SMOOTHPOLYGON  One-line description here, please.
%
%   output = demo_smoothPolygon(input)
%
%   Example
%   demo_smoothPolygon
%
%   See also
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% INRAE - BIA Research Unit - BIBS Platform (Nantes)
% Created: 2022-02-18,    using Matlab 9.10.0.1739362 (R2021a) Update 5
% Copyright 2022 INRAE.

% read data
poly = load('leaf_poly.txt');

% display polygon
figure; axis equal; axis([0 600 0 450]); hold on;
drawPolygon(poly, 'lineWidth', 3, 'color', 'k');

% smooth strongly
polyM11 = smoothPolygon(poly, 11);
drawPolygon(polyM11, 'color', 'g', 'linewidth', 2);
polyM31 = smoothPolygon(poly, 31);
drawPolygon(polyM31, 'color', 'r', 'linewidth', 2);
axis([0 200 150 300]);

% print(gcf, 'leafPoly_smoothM31.png', '-dpng');

legend({'Original polygon', 'Smoothed polygon (M=11)', 'Smoothed polygon (M=31)'}, 'Location', 'NorthEast');
print(gcf, 'leafPoly_smoothM31_annot.png', '-dpng');


function writeGraph(nodes, edges, fileName)
%WRITEGRAPH Write a graph to an ascii file
%
%   writeGraph(NODES, EDGES, FILENAME)
%
%   Example
%     % create a basic graph and save it to a file
%     nodes = [10 10;20 10;10 20;20 20;27 15];
%     edges = [1 2;1 3;2 4;2 5;3 4;4 5];
%     writeGraph(nodes, edges, 'simpleGraph.txt');
%
%   See also
%     readGraph
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2014-01-20,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2014 INRA - Cepia Software Platform.

%% File creation and init

% extract "sizes" of the graph
nNodes = size(nodes, 1);
nEdges = size(edges, 1);
nDims  = size(nodes, 2);

% file opening
f = fopen(fileName, 'wt');
if f == -1
    error(['could not open file for writing: ' fileName]);
end

% write header
fprintf(f, '# graph\n');

%% write nodes info

fprintf(f, '# nodes\n');
fprintf(f, '%d %d\n', nNodes, nDims);
format = ['%g' repmat(' %g', 1, nDims-1) '\n'];
for iNode = 1:nNodes
    fprintf(f, format, nodes(iNode, :));
end

%% write edges info

fprintf(f, '# edges\n');
fprintf(f, '%d\n', nEdges);

format = '%d %d\n';
for iEdge = 1:nEdges
    fprintf(f, format, edges(iEdge, :));
end

% close file
fclose(f);


function [ y, m, d ] = day_borrow_eg_civil ( y, m, d )

%*****************************************************************************80
%
%% DAY_BORROW_EG_CIVIL borrows days from months in an Egyptian Civil date.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input/output, integer Y, M, D, a year, month, and day
%    representing a date.  On input, D might be negative.  On output,
%    M should have decreased by one month, and D gone up by the
%    number of days in the month we "cashed in".  Y may be affected
%    if the input value of M was 1.
%
  while ( d <= 0 )

    m = m - 1;

    [ y, m ] = month_borrow_eg_civil ( y, m );

    days = month_length_eg_civil ( y, m );

    d = d + days;

  end

  return
end


clear all; close all; clc

addpath('apm')

s = 'http://byu.apmonitor.com';
c = 'mpc';

% Connect to Arduino
tclab;

% Run time in minutes
run_time = 5.0;

% Number of cycles (1 cycle per second)
loops = round(60*run_time);

% Temperature (degC)
T1 = ones(1,loops) * T1C(); % measured T
Tsp1 = ones(1,loops) * 30;
T2 = ones(1,loops) * T2C(); % measured T
Tsp2 = ones(1,loops) * 23;
time = zeros(1,loops);

% Changes in set point
Tsp1(50:200) = 40;
Tsp1(201:300) = 35;

% milli-volts input
Q1 = zeros(1,loops);
Q2 = zeros(1,loops);

% model predictive control initialization
mpc_init(s,c);

start_time = clock;
prev_time = start_time;

% dynamic plot (note: subplots needs to be declared here first)
figure(1)
subplot(3,1,1)
hold on, grid on
anexp1 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
ansp1 = animatedline('LineStyle','--','Color', 'r','LineWidth', 2);
ylabel('Temperature \circC')
legend('T_1 Measured', 'T_1 Set Point', ...
    'Location', 'northwest')
subplot(3,1,2)
hold on, grid on
anexp2 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
ansp2 = animatedline('LineStyle','--','Color', 'r','LineWidth', 2);
ylabel('Temperature \circC')
legend('T_2 Measured', 'T_2 Set Point', ...
    'Location', 'northwest')
subplot(3,1,3)
hold on, grid on
anQ1 = animatedline('LineStyle','-', 'Color', 'k', 'LineWidth', 2);
anQ2 = animatedline('LineStyle','--', 'Color', 'b', 'LineWidth', 2);
ylabel('Power Level Q (%)')
legend('Q_1', 'Q_2', 'Location', 'northwest')
xlabel('Time (sec)')

for ii = 1:loops
    % Pause Sleep time
    pause_max = 1.0;
    pause_time = pause_max - etime(clock,prev_time);
    if pause_time >= 0.0
        pause(pause_time - 0.01)
    else
        pause(0.01)
    end

    % Record time and change in time
    t = clock;
    dt = etime(t,prev_time);
    if ii>=2
        time(ii) = time(ii-1) + dt;
    end
    prev_time = t;

    % read and record from temperature controller
    T1(ii) = T1C();
    T2(ii) = T2C();

    % model predictive control
    Q1(ii) = mpc2(T1(ii),Tsp1(ii));

    % adjust power level
    h1(Q1(ii));
    h2(Q2(ii));

    % plot
    addpoints(anexp1,time(ii),T1(ii))
    addpoints(ansp1,time(ii),Tsp1(ii))
    addpoints(anexp2,time(ii),T2(ii))
    addpoints(ansp2,time(ii),Tsp2(ii))
    addpoints(anQ1,time(ii),Q1(ii))
    addpoints(anQ2,time(ii),Q2(ii))
    drawnow

    if ii==10
        apm_web(s,c);
    end
end

h1(0);
h2(0);
disp('Heaters off')
% turn off heater but keep LED on if T > 50
if (T1C() || T2C()) > 50
    led(1)
    disp(['Warning, heater temperature 1 =', num2str(T1C())])
    disp(['Warning, heater temperature 2 =', num2str(T2C())])
else
    led(0)
end

% save txt file with data
data = [time',Q1',Q2',T1',T2',Tsp1',Tsp2'];
csvwrite('data.txt',data);


function [bar] = dynpcm22bar(dynpcm2)
% Convert pressure from dynes per square centimeter to bar
% Chad Greene 2012
bar = dynpcm2*0.00000100000;