function [LMTRad,LMT1,LMT2]=TT2LMT(TT1,TT2,rObsITRS,deltaT,xpyp)
%%TT2LMT Convert from terrestrial time to local mean solar time (LMT). This
%        is a measure of the time at a place on the Earth given the mean
%        location of the Sun, not a high-precision ephemeris.
%
%INPUTS: Jul1,Jul2 Two parts of a Julian date given in TT. The
%                  units of the date are days. The full date is the sum of
%                  both terms. The date is broken into two parts to
%                  provide more bits of precision. It does not matter how
%                  the date is split.
%         rObsITRS The 3X1 location of the observer in the International
%                  Terrestrial Reference System (ITRS). Only the direction
%                  matters, not the magnitude.
%           deltaT An optional parameter specifying the offset between TT
%                  and UT1 in seconds. If this parameter is omitted or if
%                  an empty matrix is passed, then the value of the
%                  function getEOP will be used.
%             xpyp xpyp=[xp;yp] are the polar motion coordinates in radians
%                  including the effects of tides and librations. If this
%                  parameter is omitted or if an empty matrix is passed,
%                  the value from the function getEOP will be used.
%
%OUTPUTS: LMTRad The local apparent solar time in radians. 0 radians is
%                solar midnight. pi radians is solar noon. The mapping is
%                is 2*pi radians for 24 hours. Thus, LATRad*(24/(2*pi))
%                gives the time of day in hours.
%      LMT1,LMT2 Two parts of the local mean solar time in Julian days. The
%                date is split so that LMT1 is the integer part and LMT2 is
%                the fractional part. Like UT1, a zero fractional part of
%                the day corresponds to noon, not midnight.
%
%The local mean solar time is UT1 plus the East longitude of the observer
%in radians in the Terrestrial Intermediate Reference System (TIRS). The
%East longitude is added using the convention that 360 degrees equals 24
%hours of 60 minutes and 60 seconds. For LMT in radians, an additional 12
%hours (pi radians) is added to adhere to the convention that 0 hours/ 0
%radians is midnight, not noon.
%
%The use of UT1 as a definition of Greenwhich mean solar time is given in
%[1]. Since UT1 is defined as a rotation in the TIRS, it only makes sense
%that a local mean solar time is defined by using the longitude offset in
%the TIRS.
%
%REFERENCES:
%[1] D. D. McCarthy, "Evolution of timescales from astronomy to physical
%    metrology," Metrologia, vol. 48, no. 4, pp. S132-S144, Aug. 2011.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%Get any Earth orientation parameters that were not provided.
if(nargin<5||isempty(deltaT)||isempty(xpyp))
    [UTC1,UTC2]=TT2UTC(TT1,TT2);
    [xpypNew,~,~,deltaTTUT1]=getEOP(UTC1,UTC2);

    if(nargin<4||isempty(deltaT))
       deltaT=deltaTTUT1;
    end

    if(nargin<5||isempty(xpyp))
        xpyp=xpypNew;
    end
end

[UT11,UT12]=TT2UT1(TT1,TT2,deltaT);
rObsTIRS=ITRS2TIRS(rObsITRS,TT1,TT2,xpyp);
rObsSphere=Cart2Sphere(rObsTIRS);

%2*pi radians per day.
deltaUT1=rObsSphere(2)/(2*pi);

%Add preserving precision.
if(UT11<UT12)
    LMT2=UT11+deltaUT1;
    LMT1=UT12;
else
    LMT2=UT12+deltaUT1;
    LMT1=UT11;
end

%Put the fractional part into LMT2 and the integer part into LMT1.
frac1=LMT1-fix(LMT1);
frac2=LMT2-fix(LMT2);
sumVal=frac1+frac2;
sumInt=fix(sumVal);
sumFrac=sumVal-sumInt;
LMT1=fix(LMT2)+fix(LMT1)+sumInt;
LMT2=sumFrac;

%The local mean solar time in radians. This is equivalent to
%wrapRange(2*pi*(LMT1+LMT2)+pi,-pi,pi); However, since LMT1 is always
%integers, it can just be omitted.
LMTRad=wrapRange(2*pi*LMT2+pi,-pi,pi);

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [V,t,Err] = evoked(data,Fs,win,width,plt,err)
% Function to calculate the evoked response given continuous data in the
% form time x channels
% Usage [V,t,Err] = evoked(data,Fs,win,width,plt,err)
%
% Inputs
%   Note that all times can be in arbitrary units. But the units have to be
%   consistent. So, if win is in secs, width is in secs and Fs has to be Hz.
%   If win is in samples, so is width and Fs=1.
%
%    data(times, channels/trials or a single vector)      (required)
%    Fs  sampling frequency            (required)
%    win   subsection of data to be used. Default all available data
%    width (s) of smoothing kernel. Default 50 samples
%    plt plot 'n' for no plot, otherwise plot color. Default blue colored lines.
%    err = 0/1. Default 1=calculate bootstrap errorbars.
%
% Outputs
%    V = evoked potential
%    t = times of evaluation
%    Err = bootstrap statdard deviation

if nargin < 2;error('Data, sampling frequency required');end
data=change_row_to_column(data);
N=size(data,1);
data=data';
if nargin <3; win = [0 (N-1)/Fs];end
if nargin <4; width = 50/Fs;end
if nargin <5; plt = 'b';end
if nargin <6;err = 1;end
T=win;
if isempty(T); T = [0 (N-1)/Fs];end
if isempty(width); width = 50/Fs;end
if isempty(plt); plt = 'b';end
if isempty(err);err = 1;end

t = min(T):1/Fs:max(T);
if nargin >= 5
  indx = find(t>T(1) & t<T(2));
  t = t(indx);
  data = data(:,indx);
end

if width > (t(length(t))-t(1))/2
  disp('Width is too large for data segment: should be in seconds')
  disp('Turn off smoothing')
  width = 0;
end

s = t(2)-t(1);
N = fix(width/s);
NT = length(data(:,1));

if NT > 1
    mdata = mean(data);
else
    mdata = data;
end
if N > 4
  smdata = locsmooth(mdata,N,fix(N/2));
else
  smdata = mdata;
end

% if errorbars requested then do a bootstrap over trials...

Err = 0;
if NT < 4;
  disp('Too few trials: no errorbars calculated')
  err = 0;
end

if err ~= 0 && NT > 1
  Nboot = 10;
  bevk = 0;
  sevk = 0;
  for b=1:Nboot
    indx = floor(NT*rand(1,NT)) + 1;
    evktmp = mean(data(indx,:));
    if N > 4
      evktmp = locsmooth(evktmp,N,fix(N/2));
    end
    bevk = bevk + evktmp;
    sevk = sevk + evktmp.^2;
  end
  stdevk = sqrt((sevk/Nboot - bevk.^2/Nboot^2));
  Err = stdevk;
end

V = smdata;
if plt ~= 'n'
  plot(t,smdata,plt)
  hold on
  mn = mean(smdata);
  ax = get(gca,'xlim');
  line(ax,mn*[1 1],'color','k')
  if err
    line(ax,(mn+2*mean(stdevk))*[1 1],'color','r')
    line(ax,(mn-2*mean(stdevk))*[1 1],'color','r')
    hold off
  end
end


function test_failed=test_pbspline

Lr=[15,16,18,20];
ar=[ 3, 4, 6, 5];
or=[1, 1.5, 2,3];

%btypes={'ed','xd','stard','ec','xc','starc'};
btypes={'ed','xd','stard'};
centtypes={'wp','hp'};

test_failed=0;

disp(' ===============  TEST_PBSPLINE ============');

for ii=1:length(Lr)
  L=Lr(ii);
  a=ar(ii);
  N=L/a;

  for jj=1:length(or)
    order=or(jj);

    for kk=1:numel(btypes)
      btype=btypes{kk};

      for ll=1:2
        centstring=centtypes{ll};

        [g,nlen]=pbspline(L,order,a,btype,centstring);

        A=zeros(L,1);

        for n=0:N-1
          A=A+circshift(g,n*a);
        end;

        res=max(abs(A-1/sqrt(a)));
        [test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=sprintf('PBSPLINE PU   %2s %s L:%3i a:%3i o:%3.5g %0.5g %s', ...
                  btype,centstring,L,a,order,res,fail);
        disp(s);

        gcutextend=middlepad(middlepad(g,nlen,centstring),L,centstring);

        res=norm(g-gcutextend);

        [test_failed,fail]=ltfatdiditfail(res,test_failed);
        s=sprintf('PBSPLINE NLEN %2s %s L:%3i a:%3i o:%3.5g %0.5g %s', ...
                  btype,centstring,L,a,order,res,fail);
        disp(s);

      end;

    end;
  end;

end;


function r = logdet(M)
% Compute log(det(A)) without the usual numerical inaccuracies.

% Copyright (C) Christian Kothe, SCCN, 2011, christian@sccn.ucsd.edu
%
% 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307
% USA

r = 2 * sum(log(diag(chol(M))), 1);


function [ y ] = lp_thresh_real( x,threshold,p )
% generalized softthresholding (GST) based on the paper by Zuo et al.
%
% (c) Marc Fischer, Thomas Kuestner
% ---------------------------------------------------------------------

threshold_nonconvex = (2*threshold*(1-p))^(1/(2-p)) + threshold*p*(2*threshold*(1-p))^((p-1)/(2-p));
y = zeros(size(x));
i0 = find(abs(x)>threshold_nonconvex);

if p < 1
    J = 3;
else
    J = 1;
end;
% if length(i0) > 1
    x0 = x(i0);
    x_temp = abs(x0);
    for j = 1:J
        x_temp = abs(x0) - threshold*p*(x_temp.^(p-1));
    end;
    y(i0) = sign(x0).*x_temp;
% end


function [A] = spm_matrix(P, order)
% returns an affine transformation matrix
% FORMAT [A] = spm_matrix(P, order)
% P(1)  - x translation
% P(2)  - y translation
% P(3)  - z translation
% P(4)  - x rotation about - {pitch} (radians)
% P(5)  - y rotation about - {roll}  (radians)
% P(6)  - z rotation about - {yaw}   (radians)
% P(7)  - x scaling
% P(8)  - y scaling
% P(9)  - z scaling
% P(10) - x affine
% P(11) - y affine
% P(12) - z affine
%
% order (optional) application order of transformations.
%
% A     - affine transformation matrix
%___________________________________________________________________________
%
% spm_matrix returns a matrix defining an orthogonal linear (translation,
% rotation, scaling or affine) transformation given a vector of
% parameters (P).  By default, the transformations are applied in the
% following order (i.e., the opposite to which they are specified):
%
% 1) shear
% 2) scale (zoom)
% 3) rotation - yaw, roll & pitch
% 4) translation
%
% This order can be changed by calling spm_matrix with a string as a
% second argument. This string may contain any valid MATLAB expression
% that returns a 4x4 matrix after evaluation. The special characters 'S',
% 'Z', 'R', 'T' can be used to reference the transformations 1)-4)
% above. The default order is 'T*R*Z*S', as described above.
%
% SPM uses a PRE-multiplication format i.e. Y = A*X where X and Y are 4 x n
% matrices of n coordinates.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_matrix.m 1149 2008-02-14 14:29:04Z volkmar $

% pad P with 'null' parameters
%---------------------------------------------------------------------------
q  = [0 0 0 0 0 0 1 1 1 0 0 0];
P  = [P q((length(P) + 1):12)];

% default multiplication order if not specified
%---------------------------------------------------------------------------
if nargin < 2
    order = 'T*R*Z*S';
end;

T  =   [1   0   0   P(1);
        0   1   0   P(2);
        0   0   1   P(3);
        0   0   0   1];

R1  =  [1    0      0          0;
        0    cos(P(4))  sin(P(4))  0;
        0   -sin(P(4))  cos(P(4))  0;
        0    0      0          1];

R2  =  [cos(P(5))  0    sin(P(5))  0;
        0          1    0      0;
       -sin(P(5))  0    cos(P(5))  0;
        0          0    0          1];

R3  =  [cos(P(6))   sin(P(6))   0  0;
       -sin(P(6))   cos(P(6))   0  0;
        0           0           1  0;
        0           0       0  1];

R   = R1*R2*R3;

Z   =  [P(7)    0       0       0;
        0       P(8)    0       0;
        0       0       P(9)    0;
        0       0       0       1];

S   =  [1       P(10)   P(11)   0;
        0       1   P(12)   0;
        0       0       1   0;
        0       0       0       1];

A = eval(sprintf('%s;', order));
if ~isnumeric(A) || ndims(A) ~= 2 || any(size(A) ~= 4)
    error('Order expression ''%s'' did not return a valid 4x4 matrix.', ...
          order);
end;


function imshow_numeric ( A )

%*****************************************************************************80
%
%% IMSHOW_NUMERIC displays a numeric 2D array as a grayscale image.
%
%  Discussion:
%
%    A numeric 2D array, typically of type "double", cannot be displayed by
%    the Image Processing Toolbox function imshow.
%
%    This is because the data is of the wrong type (signed real numbers
%    rather than unsigned integers) and generally also in an unsuitable
%    range.  Typical image data might range from 0 to 255, for instance.
%
%    This function makes a quick and simple conversion of numeric data to
%    a format of the appropriate type and range for display by imshow,
%    and displays the data as a grayscale image.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A(M,N), the numeric array to be displayed.
%
  a_min = min ( min ( A ) );
  a_max = max ( max ( A ) );

  if ( a_min == a_max )
    A = 127;
  else
    A = floor ( 255 * ( A - a_min ) / ( a_max - a_min ) );
  end

  A = uint8 ( A );

  imshow ( A );

  return
end


function [X,proc]=genMarkedPointProc(lambda,dt,ts,tf,X0,mark,eventDist,timeDist,staycell)
%%GENMARKEDPOINTPROC A function to simulate a marked point process with
%                    rate parameter lambda, initial value X0, and given
%                    time parameters. Using the defaults for the last 3
%                    parameters will generate a compound Poisson process
%                    provided the mark distribution is properly defined
%                    (see note below). The null mark is assumed to be zero.
%                    See [1] for more details on general marked point
%                    processes.
%
%INPUTS: lambda The unit rate parameter for the default Poisson
%               distribution. This may be arbitrarily defined if eventDist
%               is given.
%            dt Time step for event draws.
%            ts Start time for process.
%            tf End time for process.
%            X0 The initial value for the process. Defaults to 0 if
%               omitted.
%          mark An optional function handle f(x,t,j,proc) to be used for
%               sampling mark values. This must accept three scalar inputs
%               and a structure input:
%               x The process value at the beginning of a dt time step.
%               t The elapsed time at the beginning of the dt time step,
%                 starting from ts.
%               j The index corresponding to the ordinal number of the
%                 jump since the beginning of the current time step.
%               proc A structure containing information about the process as
%                 currently constructed. See output information below for
%                 more details.
%               If mark is not given or is empty, it defaults to the unity
%               function for all potential values (jumps will have mark 1
%               regardless of the variables).
%     eventDist An optional function handle for the distribution of events.
%               This must output a scalar value in the interval [0,inf). If
%               not given, it defaults to a Poisson distribution with rate
%               parameter lambda*dt. This function handle must take as
%               input:
%               x The process value at the beginning of a dt time step.
%               t The elapsed time at the beginning of the dt time step,
%                 starting from ts.
%               i The index corresponding to the ordinal number of the
%                 time step since the beginning of the process at time
%                 ts.
%               proc A structure containing information about the process as
%                 currently constructed. See output information below for
%                 more details.
%     timeDist An optional function handle for the distribution of
%              times during a step. This must output a vector of values in
%              the interval (0,1). If not given, it defaults to a uniform
%              random distribution on (0,1). This function handle must take
%              as input:
%              x The process value at the beginning of a dt time step.
%              t The elapsed time at the beginning of the dt time step,
%                 starting from ts.
%              i The index corresponding to the ordinal number of the
%                 time step since the beginning of the process at time
%                 ts.
%              proc A structure containing information about the process as
%                 currently constructed. See output information below for
%                 more details.
%     staycell A logical value. If this is equivalent to true, then X is
%              returned as a cell array. Otherwise, X is returned as a
%              vector. Defaults to false.
%
%OUTPUTS: X Defaults to a 1Xnsteps+1 vector of the process values after
%           each step. X(1) = X0. If the staycell variable evaluates to
%           true, then X is returned as a 1Xnsteps+1 cell array.
%      proc A structure containing information which can be used to
%           reconstruct the exact process X. Members are:
%           nEvents A 1Xnsteps+1 vector of the number of jumps which
%                   occured during each step prior to the index. So
%                   nEvents(1) = 0 and nEvents(i) will be the number of
%                   jumps which occured during the (i-1) time step
%                   corresponding to the change X{i}-X{i-1}.
%               mks A 1Xnsteps cell array where each cell contains a row
%                   vector with the mark values generated for a step.
%              times A 1X((tf-ts)/dt) cell array with each cell
%                   containing a row vector containing the jump times for
%                   the process at the corresponding time step.
%
%Note: If the mark distribution is defined such that all marks are
%independently and identically distributed and the events are Poisson
%distributed, then the generated process will be a compound Poisson
%process. For more on compound Poisson processes, see pages 10-12 of [2].
%
%WARNING: No check is performed to ensure that eventDist and timeDist
%output reasonable values. This function could technically work with marks
%which are not numbers, provided some addition operation is defined for
%them. However, read  the documentation above to ensure your inputs will
%generate a proper process as intended.
%
%EXAMPLE 1: Generating several realizations of a time dependent marked
%           Poisson process and comparing it with the expected path.
% %Set path parameters
% ts = 40; % Start time
% tf = 50; % End time
% dt = 1; % Time step
% numpaths = 100; % Number of simulated paths
% t = ts:dt:tf;
% lambda1 = 5; % Poisson rate parameter for unit time
% lambda2 = 6; % Exponential rate parameter
% X0 = 100; % Initial value for the process
%
% close all
% figure
% hold on
%
% pathends = zeros([1,numpaths]);
% %Generate paths
% for i = 1:numpaths
%     [cp,proc] = genMarkedPointProc(lambda1,dt,ts,tf,X0,@(x,t,j,proc)(tf-t)*ExponentialD.rand(1,lambda2));
%     plot(t,cp)
%     pathends(i) = cp(end);
% end
% avgend = mean(pathends);
% stdend = std(pathends);
%
% %Compute the expected path
% expected = X0+[0,cumsum(lambda1*dt*(tf-(t(1:end-1)+t(2:end))/2)/lambda2)];
% expectedstd = sqrt(cumsum(lambda1*dt*((tf-(t(1:end-1)+t(2:end))/2)).^2*2/(lambda2)^2));
% plot(t,expected,'--k','LineWidth',5)
% xlabel('time (t)')
%
% %Compare simulated path statistics to the expected value
% fprintf("The average path value at t = %0.0f is : %0.5f\n",tf,avgend)
% fprintf("The path standard deviation at t = %0.0f is : %0.5f\n",tf,stdend)
% fprintf("The expected path value at t = %0.0f is : %0.5f\n",tf,expected(end))
% fprintf("The expected standard deviation at t = %0.0f is : %0.5f\n",tf,expectedstd(end))
%
%REFERENCES:
%[1] Stover, Christopher. "Marked Point Process." From MathWorld--A
%    Wolfram Web Resource, created by Eric W. Weisstein.
%    http://mathworld.wolfram.com/MarkedPointProcess.html
%[2] Platen, Eckhard, and Nicola Bruti-Liberati. Numerical solution of
%    stochastic differential equations with jumps in finance. Vol. 64.
%    Springer Science & Business Media, 2010.
%
%July 2019 Codie T. Lewis, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(~exist('X0','var')||isempty(X0))
    X0 = 0;
end
if(~exist('mark','var')||isempty(mark))
    mark = @(x,t,j,proc) 1;
end
if(~exist('eventDist','var')||isempty(eventDist))
    eventDist = @(x,t,i,proc) PoissonD.rand(1,lambda*dt);
end
if(~exist('timeDist','var')||isempty(timeDist))
    timeDist = @(x,t,j,proc) rand([1,proc.nEvents(j)]);
end
if(~exist('staycell','var'))
    staycell = false;
elseif(~islogical(staycell))
    error('The value in staycell is not a logical value.')
end

nsteps = floor((tf-ts)/dt);
X = cell(1,nsteps+1);
X{1} = X0;

proc.nEvents = zeros([1,length(X)]);
proc.mks = cell([1,length(X)]);
proc.times = cell([1,length(X)]);

for i = 1:nsteps
    t = (i-1)*dt+ts;
    proc.nEvents(i) = eventDist(X{i},t,i,proc);
    stepmks = cell([1,proc.nEvents(i)]);
    steptimes = sort(timeDist(X{i},t,i,proc)*dt+t);
    proc.times{i} = steptimes;

    stepIntensity = 0;
    for j = 1:proc.nEvents(i)
        stepmks{j} = mark(X{i},steptimes(j),j,proc);
        proc.mks{i}{j} = stepmks{j};
        stepIntensity = stepIntensity+stepmks{j};
    end
    X{i+1} = X{i}+stepIntensity;
end

if(~staycell)
    X = cell2mat(X);
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [date, doy, dow] = gps2date(gps_week, gps_sow)

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

%--- * --. --- --. .--. ... * ---------------------------------------------
%               ___ ___ ___
%     __ _ ___ / __| _ | __|
%    / _` / _ \ (_ |  _|__ \
%    \__, \___/\___|_| |___/
%    |___/                    v 1.0RC1
%
%--------------------------------------------------------------------------
%  Copyright (C) 2021 Geomatics Research & Development srl (GReD)
%  Written by:
%  Contributors:     ...
%  A list of all the historical goGPS contributors is in CREDITS.nfo
%--------------------------------------------------------------------------
%
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%--------------------------------------------------------------------------
% 01100111 01101111 01000111 01010000 01010011
%--------------------------------------------------------------------------

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

gps_dow = fix(gps_sow/86400);                             %day of week
date = datevec(gps_start_datenum + 7*gps_week + gps_dow); %calendar date up to days
gps_sod = gps_sow - gps_dow*86400;                        %seconds of day
date(:,4) = floor(gps_sod/3600);                          %hours
date(:,5) = floor(gps_sod/60 - date(:,4)*60);             %minutes
date(:,6) = gps_sod - date(:,4)*3600 - date(:,5)*60;      %seconds

%day of year (DOY)
if (nargout > 1)
    doy = date2doy(datenum(date));
    doy = floor(doy);
end

%day of week
if (nargout > 2)
    dow = gps_dow;
end


function seed = get_seed ( )

%*****************************************************************************80
%
%% GET_SEED returns a random seed for the random number generator.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 November 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, integer SEED, a random seed value.
%
  time_array = clock;

  hour = time_array(4);
  minute = time_array(5);
  second = time_array(6);

  seed = second + 60 * ( minute + 60 * hour );
%
%  We want values in [1,43200], not [0,43199].
%
  seed = seed + 1;
%
%  Remap SEED from [1,43200] to [1,I_MAX].
%
  i4_huge = 2147483647;
  seed = i4_huge * ( seed  / ( 60.0 * 60.0 * 24.0 ) );

  seed = floor ( seed );
%
%  Never use a seed of 0.
%
  if ( seed == 0 )
    seed = 1;
  end

  return
end


function [yMeas] = stereoCamProject(p_fc_c, calibParams)
% triangulate Triangulates 3D point from stereo camera measurement

c_u = calibParams.c_u;
c_v = calibParams.c_v;
f_u = calibParams.f_u;
f_v = calibParams.f_v;
b = calibParams.b;

x = p_fc_c(1);
y = p_fc_c(2);
z = p_fc_c(3);

yMeas = (1/z)*[f_u*x; f_v*y; f_u*(x-b); f_v*y] + [c_u;c_v;c_u;c_v];
end


function gaussian = gaussianFunction
s = 0.1;
gaussian = @(txi,psi) 1/sqrt(2*pi*s^2)*exp(-(txi - psi).^2/(2*s^2));
end


function binomial_test ( )

%*****************************************************************************80
%
%% BINOMIAL_TEST tests BINOMIAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BINOMIAL_TEST:\n' );
  fprintf ( 1, '  A demonstration of the binomial method\n' );
  fprintf ( 1, '  for option valuation.\n' );

  s0 = 2.0;
  e = 1.0;
  r = 0.05;
  sigma = 0.25;
  t1 = 3.0;
  m = 256;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The asset price at time 0, S0    = %f\n', s0 );
  fprintf ( 1, '  The exercise price         E     = %f\n', e );
  fprintf ( 1, '  The interest rate          R     = %f\n', r );
  fprintf ( 1, '  The asset volatility       SIGMA = %f\n', sigma );
  fprintf ( 1, '  The expiry date            T1    = %f\n', t1 );
  fprintf ( 1, '  The number of intervals    M     = %d\n', m );

  c = binomial ( s0, e, r, sigma, t1, m );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The option value is %f\n', c );

  return
end


function y = sfb3D(lo, hi, sf1, sf2, sf3)

% 3D Synthesis Filter Bank
%
% USAGE:
%   y = sfb3D(lo, hi, sf1, sf2, sf3);
% INPUT:
%   lo, hi - lowpass subbands
%   sfi - synthesis filters for dimension i
% OUPUT:
%   y - output array
% See afb3D
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

if nargin < 4
   sf2 = sf1;
   sf3 = sf1;
end

LLL = lo;
LLH = hi{1};
LHL = hi{2};
LHH = hi{3};
HLL = hi{4};
HLH = hi{5};
HHL = hi{6};
HHH = hi{7};

% filter along dimension 3
LL = sfb3D_A(LLL, LLH, sf3, 3);
LH = sfb3D_A(LHL, LHH, sf3, 3);
HL = sfb3D_A(HLL, HLH, sf3, 3);
HH = sfb3D_A(HHL, HHH, sf3, 3);

% filter along dimension 3
L = sfb3D_A(LL, LH, sf2, 2);
H = sfb3D_A(HL, HH, sf2, 2);

% filter along dimension 1
y = sfb3D_A(L, H, sf1, 1);

% LOCAL FUNCTION

function y = sfb3D_A(lo, hi, sf, d)

% 3D Synthesis Filter Bank
% (along single dimension only)
%
% y = sfb3D_A(lo, hi, sf, d);
% sf - synthesis filters
% d  - dimension of filtering
% see afb2D_A

lpf = sf(:, 1);     % lowpass filter
hpf = sf(:, 2);     % highpass filter

% permute dimensions of lo and hi so that dimension d is first.
p = mod(d-1+[0:2], 3) + 1;
lo = permute(lo, p);
hi = permute(hi, p);

[N1, N2, N3] = size(lo);
N = 2*N1;
L = length(sf);
y = zeros(N+L-2, N2, N3);

for k = 1:N3
   y(:, :, k) = upfirdn(lo(:, :, k), lpf, 2, 1) + upfirdn(hi(:, :, k), hpf, 2, 1);
end
y(1:L-2, :, :) = y(1:L-2, :, :) + y(N+[1:L-2], :, :);
y = y(1:N, :, :);
y = cshift3D(y, 1-L/2, 1);

% permute dimensions of y (inverse permutation)
y = ipermute(y, p);


close all; clear all; clc;

%  image = imread('Resources/images/3096.jpg');
 image = imread('Resources/images/28075.jpg');
%   image = imread('Resources/images/113016.jpg');

 %image = imread('Resources/images/3096.jpg');
%     image = rgb2gray(image);

% neighborhoodType = 4; % 4-point connectivity
neighborhoodType = 4; % 8-point connectivty

tic;
[ segmentedImage, binaryImage, regionMatrix ] = RegionGrowingSegmentation(image, neighborhoodType);
display('Segmentation time:');
toc;


function D = compute_distance_graph(W, point_list)

% compute the distance between each point on a graph
%
%   D = compute_distance_graph(W, point_list);
%
%   Uses either 'perform_dijkstra_fast' or 'perform_dijkstra' mex code
%   (depending on which is available).
%
%   D(i,j) is the geodesic graph distance between vertex point_list(i) and
%   vertex j.
%
%   Copyright (c) 2006 Gabriel Peyr?

n = size(W,1); % number of points in the graph
if nargin<2
    point_list = 1:n;
end

%% use the fastest code available
if exist('perform_dijkstra_fast')==3
	D = perform_dijkstra_fast(W, point_list);
    return;
end

%% use slow mex code
W = full(W);

D = zeros(length(point_list),n);
hh = waitbar(0,['Computing distances.']);
for i=1:length(point_list)
    waitbar( i/length(point_list) ,hh);
    warning off;
    [d,S] = perform_dijkstra(W, point_list(i));
    warning on;
    D(i,:) = d(:)';
end
close(hh);

% symmetrize
if length(point_list)==n
    D = (D+D')/2;
end


function asksys(bin,f)
disp('========================================');
disp(' HAM DIEU CHE DICH BIEN: ASK');
disp(' VI DU: asksys([0 1 0 1 1 0 1 1 0],3)');
disp('Written by Nguyen Hoang Minh DHCNTPHCM. he..he..');
disp('========================================');

bin=[0 1 0 1 1 0 1 1 1 0];f=4;k=1000;

t=0:2*pi/(k-1):2*pi;

L = length(bin);sig=cos(f*t);
bit1=ones(1,k);bit0=zeros(1,k);
mbit=[];mcw=[];
%==============================================
for n=1:L;
    if bin(n)==0;
       cw=sig;bit=bit0;
    else
       cw=2*sig;bit=bit1;
    end
   mbit=[mbit bit];
   mcw=[mcw cw];
end
ask=mcw;

%============================================

s=length(mcw);mrec=[];

for m=1:k:s
    vm=abs(mcw(m));
   if vm > 1.5
      rec=bit1;
   elseif vm < 1.2
      rec=bit0;
    end
    mrec=[mrec rec];
end
deask=mrec;
subplot(3,1,1);plot(mbit,'r','linewidth',2);axis([0  k*L -0.5 1.5]);grid on;title('Data in');
subplot(3,1,2);plot(ask,'m','linewidth',1.5);axis([0  k*L -2.5 2.5]);grid on;title('ASK modulation');
subplot(3,1,3);plot(deask,'g','linewidth',1.5);axis([0  k*L -0.5 1.5]);grid on;title('ASK demodulation,Data out');


function N = tangentspacefactory(M, x)
% Returns a manifold structure representing the tangent space to M at x.
%
% N = tangentspacefactory(M, x)
%
% N defines a (linear) manifold that is the tangent space to M at x. Points
% are represented as tangent vectors to M at x. Tangent vectors are also
% represented as tangent vectors to M at x.
%
% This is chiefly useful to solve optimization problems involving tangent
% vectors to M at x, which notably comes up when solving linear systems
% involving, for example, the Hessian of the cost on M at x (think of the
% Newton equations.) The Riemannian (actually, Euclidean) structure on N is
% that of the tangent space to M, that is, the inner product is inherited.
%
% See also: preconhessiansolve

% This file is part of Manopt: www.manopt.org.
% Original author: Nicolas Boumal, April 9, 2015.
% Contributors:
% Change log:
%
%   Jan. 25, 2017 (NB):
%       Following a comment by Jesus Briales on the Manopt forum, the
%       functions N.egrad2rgrad, N.ehess2rhess and N.tangent now include a
%       projection (they were formerly identities.)
%
%   Feb. 2, 2017 (NB):
%       Following a comment by Jesus Briales on the Manopt forum, the
%       function N.proj now calls M.proj(x, .) instead of M.proj(y, .).
%       Furthermore, N.ehess2rhess was corrected in the same way.
%
%   Dec. 14, 2019 (NB):
%       Fixed N.tangent so that it should now work with factories that
%       have a non-identity tangent2ambient, e.g., fixedrankembeddedfactory
%       and rotationsfactory.

    % N is the manifold we build. y will be a point on N, thus also a
    % tangent vector to M at x. This is a typical Euclidean space, hence it
    % will be easy to describe in terms of the tools available for M.
    N = struct();

    % u, u1 and u2 will be tangent vectors to N at y. The tangent space to
    % N at y is the tangent space to M at x, thus u, u1 and u2 are also
    % tangent vectors to M at x.

    if isfield(M, 'name')
        N.name  = @() ['Tangent space to ' M.name()];
    end
    N.dim   = @() M.dim();
    N.inner = @(y, u1, u2) M.inner(x, u1, u2);
    N.norm  = @(y, u) M.norm(x, u);
    N.proj  = @(y, u) M.proj(x, u);
    N.typicaldist = @() sqrt(N.dim());
    if isfield(M, 'tangent2ambient')
        N.tangent = @(y, u) M.proj(x, M.tangent2ambient(x, u));
    else
        N.tangent = N.proj;
    end

    N.egrad2rgrad = N.proj;
    N.ehess2rhess = @(y, eg, eh, d) M.proj(x, eh);
    N.exp = @exponential;
    N.retr = @exponential;
    N.log = @(y1, y2) M.lincomb(x, 1, y2, -1, y1);
    N.pairmean = @(y1, y2) M.lincomb(x, 0.5, y1, 0.5, y2);
    N.rand = @() M.randvec(x);
    N.randvec = @(y) M.randvec(x);
    N.zerovec = M.zerovec;
    N.lincomb = M.lincomb;
    N.transp = @(y1, y2, u) u;
    N.hash = @(y) ['z' hashmd5(M.vec(x, y))];

    % In a Euclidean space, the exponential is merely the sum: y + tu.
    function yy = exponential(y, u, t)
        if nargin == 2
            t = 1;
        end
        yy = M.lincomb(x, 1, y, t, u);
    end

end


classdef stats

methods(Static)

    function [frequencies, labels] = relative_frequencies(data)
        % Returns the frequencies of values of a discrete random variable in the dataset
        n = length(data);
        % Reshape data to a row vector
        data = spx.vector.reshape_as_row_vec(data);
        % Identify unique labels in the data set and their frequencies
        [labels, ~, label_map] = unique(data);
        % Lets get the number of unique labels
        nl = numel(labels);
        % Let's construct an n x nl sparse matrix in which
        % each column identifies c-th label
        % each row identifies the r-th data point
        % each row contains only one 1 and rest of the entries are 0.
        % The column in which 1 is stored identifies the label of the data entry
        % We store this information as the sparse matrix.
        % We already know that the matrix will have n non-zero entries
        non_zero_max = n;
        rows = 1:n;
        columns = label_map;
        index_to_label_matrix = sparse(rows, columns,  1,    n, nl,    non_zero_max);
        % Take mean over each column to obtain the frequencies
        frequencies = full(mean(index_to_label_matrix,1));
    end

    function [ statistic ] = compute_statistic_per_vector(...
        receivedSequence, N, statisticFunc)
        %COMPUTE_STATISTIC_PER_VECTOR Computes a statistic for each vector
        %   Detailed explanation goes here

        % Number of samples
        L = length(receivedSequence);
        % Number of vectors
        M = round(L / N);
        % Each row now represents one vector
        receivedSequence = reshape(receivedSequence, N, M)';
        statistic = zeros(M, 1);
        for i=1:M
            data = receivedSequence(i, :);
            statistic(i) = statisticFunc(data);
        end
    end

    function result = format_descriptive_statistics(x)
        mu = mean(x);
        med = median(x);
        rng = range(x);
        r = iqr(x);
        sigma = std(x);
        [m1, min_idx] = min(x);
        [m2, max_idx] = max(x);
        m3 = mad(x);
        result = sprintf('Min: %.2f (%d), Max: %.2f (%d), Mean: %.2f, Median: %.2f, Range : %.2f, IQR: %.2f, Deviation: %.2f, MAD: %.2f', m1, min_idx, m2, max_idx, mu, med, rng, r, sigma, m3);
    end

    function y = rand_subset(n, k)
        % Returns y has a column vector of k values sampled uniformly
        % at random without replacement from the integers 1:n
        rp = randperm(n);
        y = rp(1:k);
        y = y(:);
    end

end

end


function e1_values_test ( )

%*****************************************************************************80
%
%% E1_VALUES_TEST demonstrates the use of E1_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'E1_VALUES_TEST:\n' );
  fprintf ( 1, '  E1_VALUES stores values of\n' );
  fprintf ( 1, '  the exponential integral function E1(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      X          E1(X)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = e1_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', x, fx );

  end

  return
end


function rgbcube(vx, vy, vz)
%RGBCUBE Displays an RGB cube on the MATLAB desktop.
%   RGBCUBE(VX, VY, VZ) displays an RGB color cube, viewed from point
%   (VX, VY, VZ).  With no input arguments, RGBCUBE uses (10, 10, 4)
%   as the default viewing coordinates.  To view individual color
%   planes, use the following viewing coordinates, where the first
%   color in the sequence is the closest to the viewing axis, and the
%   other colors are as seen from that axis, proceeding to the right
%   right (or above), and then moving clockwise.
%
%      -------------------------------------------------
%           COLOR PLANE                  ( vx,  vy,  vz)
%      -------------------------------------------------
%       Blue-Magenta-White-Cyan          (  0,   0,  10)
%       Red-Yellow-White-Magenta         ( 10,   0,   0)
%       Green-Cyan-White-Yellow          (  0,  10,   0)
%       Black-Red-Magenta-Blue           (  0, -10,   0)
%       Black-Blue-Cyan-Green            (-10,   0,   0)
%       Black-Red-Yellow-Green           (  0,   0, -10)
%

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.5 $  $Date: 2003/10/13 00:52:14 $

% Set up parameters for function patch.
vertices_matrix = [0 0 0;0 0 1;0 1 0;0 1 1;1 0 0;1 0 1;1 1 0;1 1 1];
faces_matrix = [1 5 6 2;1 3 7 5;1 2 4 3;2 4 8 6;3 7 8 4;5 6 8 7];
colors = vertices_matrix;
% The order of the cube vertices was selected to be the same as
% the  order of the (R,G,B) colors (e.g., (0,0,0) corresponds to
% black, (1,1,1) corresponds to white, and so on.)

% Generate RGB cube using function patch.
patch('Vertices', vertices_matrix, 'Faces', faces_matrix, ...
      'FaceVertexCData', colors, 'FaceColor', 'interp', ...
      'EdgeAlpha', 0)

% Set up viewing point.
if nargin == 0
   vx = 10; vy = 10; vz = 4;
elseif nargin ~= 3
   error('Wrong number of inputs.')
end
axis off
view([vx, vy, vz])
axis square


function [  ] = gsp_plot_sgram( G,A,param )
%GSP_PLOT_SGRAM Plot graph spectrogram
%   Usage:  gsp_plot_sgram( G,A );
%           gsp_plot_sgram( G,A,param );
%
%   Input parameters:
%         G     : Graph
%         A     : Graph windowed Fourrier transform
%         param : Structure of optional parameter
%   Output parameters:
%         none
%
%   *param* is a structure of optional parameter with
%
%   * *param.colorbar*: Use the colorbar (default 1)
%
%   Example:::
%
%           N = 15;
%           G = gsp_ring(2*N);
%           G = gsp_compute_fourier_basis(G);
%           x = [0:N,(N-1):-1:1]';
%           s = 3;
%           g = exp(-(x-1).^2/s^2);
%           f = gsp_localize(G,g,N);
%           c = gsp_gwft(G,f,g);
%           gsp_plot_sgram(G,c);
%
%   See also: gsp_plot_signal, gsp_plot_graph, gsp_plot_signal_spectral
%

% Author: Nathanael Perraudin
% Date  : 09.12.2013
% testing: test_plotting

% Optional parameter handling
if nargin<3
    param=struct;
end

if ~isfield(param, 'colorbar'), param.colorbar = 1; end;

imagesc(1:size(A,2), 0:size(A,1)-1,abs( A));

% Hack to overpass a matlab bug with latex interpretex
latex = get(gca,'DefaultTextInterpreter');
set(gca,'DefaultTextInterpreter','Tex');

xlabel('Nodes');
ylabel('Freqencies');

set(gca,'DefaultTextInterpreter',latex);

if param.colorbar
    colorbar
end

end


clear;
clc;
close all;
format

%% 参考: https://x-io.co.uk/oscillatory-motion-tracking-with-x-imu/

% 利用速度，位置高通滤波来获得短时位移曲线

%load filter_vel.mat
% load example_ins4.mat

data = ch_data_import('data_20210609_163153.csv');
acc = data.imu.acc*9.8;
gyr = deg2rad(data.imu.gyr);
%% 读取数据
acc = acc';
gyr = gyr';

q= [1 0 0 0]';

Fs = 100;
dt = 1 / Fs;
N = length(acc);

fprintf("共%d数据，用时:%.3fs\n", N, N/Fs);

% 惯导解算, 初始化
p = zeros(3, 1);
v = zeros(3, 1);
linVel = zeros(N,3);

for i=1:N
    [p , v , q] = ch_nav_equ_local_tan(p, v, q, acc(i,:)', gyr(i,:)', 1 / Fs, [0, 0, -9.8]');

    % 获得惯性系下加速度并扣除重力
    linAcc(i,:) = ch_qmulv(q, acc(i,:));
    linAcc(i,3) = linAcc(i,3) -9.8;

%   linVel(i,:) = v;
end

% 速度积分

for i = 2:N
    linVel(i,:) = linVel(i-1,:) + linAcc(i,:) * dt;
end

order = 1;
filtCutOff = 0.01;
[b, a] = butter(order, (2*filtCutOff)/(1/dt), 'high');
%linVelHP = filtfilt(b, a, linVel);
linVelHP = filter(b, a, linVel);

% 位置积分
linPos = zeros(size(linVelHP));
for i = 2:length(linVelHP)
    linPos(i,:) = linPos(i-1,:) + linVelHP(i,:) * dt;
end

order = 1;
filtCutOff = 0.5;
[b, a] = butter(order, (2*filtCutOff)/(1/dt), 'high');
%linPosHP = filtfilt(b, a, linPos);
linPosHP = filter(b, a, linPos);

%% Plot
figure('NumberTitle', 'off', 'Name', '速度');
subplot(2,1,1);
plot(linVel);
title('速度');
legend('X', 'Y', 'Z');
subplot(2,1,2);
plot(linVelHP);
title('HP后速度');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', '位置');
subplot(2,1,1);
plot(linPos);
title('位置');
legend('X', 'Y', 'Z');
subplot(2,1,2);
plot(linPosHP);
title('HP后位置');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', '原始数据');
subplot(2,1,1);
plot(acc);
legend("X", "Y", "Z");
title("加速度");
subplot(2,1,2);
plot(gyr);
title("陀螺");
legend("X", "Y", "Z");

figure('NumberTitle', 'off', 'Name', '3D位置');
plot3(linPosHP(1,1), linPosHP(1,2), linPosHP(1,3), '-ks');
hold on;
plot3(linPosHP(:,1), linPosHP(:,2), linPosHP(:,3), '.b');
axis equal
xlabel('X(m)');  ylabel('Y(m)');   zlabel('Z(m)');
title('3D位置');
legend('起始', '3D');

linPosHP(end,:)

% SamplePlotFreq = 4;
%
% SixDOFanimation(linPosHP, R, ...
%                 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'Off', ...
%                 'Position', [9 39 400 400], ...
%                 'AxisLength', 0.1, 'ShowArrowHead', false, ...
%                 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)', 'ShowLegend', false, 'Title', 'Unfiltered',...
%                 'CreateAVI', false, 'AVIfileNameEnum', false, 'AVIfps', ((1/dt) / SamplePlotFreq));


function S = mixgetcovariance(mix,inds,centers)

if ~exist('inds','var'),
  inds = 1:mix.nin;
end;
if ~exist('centers','var'),
  centers = 1:mix.ncentres;
end;

inds = inds(:)';
ninds = length(inds);
centers = centers(:)';
ncenters = length(centers);
S = zeros(ninds,ninds,ncenters);

switch mix.covar_type,

 case 'spherical',
  for i = 1:ncenters,
    j = centers(i);
    S(:,:,i) = eye(ninds)*mix.covars(j);
  end;

 case 'diag',
  for i = 1:ncenters,
    j = centers(i);
    S(:,:,i) = diag(mix.covars(j,:));
  end;

 case {'full','sparse'},
  S = mix.covars(inds,inds,centers);

 case 'ppca',
  for i = 1:ncenters,
    j = centers(i);
    covars = mix.covars(j) * eye(mix.nin) + ...
	     mix.U(:, :, j)*(diag(mix.lambda(j, :))-diag(mix.covars(j)))* ...
	     (mix.U(:, :, j)');
    S(:,:,i) = covars(inds,inds);
  end;

 case 'zero',
  S(:) = 0;

 otherwise,
  error(['Unknown covar_type ',mix.covar_type]);
end;


function qr = rightQuaternionProductMatrix(q)
% q 4x1 w,x,y,z
% p * q = rightQuaternionProductMatrix(q) * p
% eq 18 in Sola Quaternion kinematics for the error-state Kalman filter
w = q(1);
x = q(2);
y = q(3);
z = q(4);
qr = [w, -x, -y, -z;
    x, w, z, -y;
    y, -z, w, x;
    z, y, -x, w];
end


function r=rsi2(x,N)
L = length(x);
dx = diff([0;x]);
up=dx;
down=abs(dx);
% up and down moves
I=dx<=0;
up(I) = 0;
down(~I)=0;
% calculate exponential moving averages
m1 = ema(up,N); m2 = ema(down,N);
warning off
r = 100*m1./(m1+m2);
%r(isnan(r))=50;
I2=~((up+down)>0);
r(I2)=50;
warning on


function [lat2,lon2,derr,aerr]=vdistinv(lat1,lon1,dist,azim)
% VDISTINV - Invert VDIST function using numerical inversion
%
% Usage:
%
% [lat2,lon2] = vdistinv(lat1,lon1,dist,azim)
% [lat2,lon2,derr,aerr] = vdistinv(lat1,lon1,dist,azim)
%
% Variables:
%
% lat1, lon1 = coordinates of intial point in degrees
% dist       = geodesic distance in meters
% azim       = geodesic azimuth in degrees clockwise from north
% lat2,lon2  = destination coordinates in degrees
% derr       = optional output: error between input distance and the
%              calculated length of the path to the computed endpoint
%              (as computed by VDIST)
% aerr       = optional output: error between the provided azimuth and the
%              calculated azimuth (as computed by VDIST)
%
% Notes: (1) This "quick and dirty" approach was written in response to
%            a user request. The use of a numerical optimization to invert
%            VDIST is a relatively slow and crude approach. (It would be
%            better to implement an algorithm written specifically for that
%            purpose by Vincenty or others.)
%        (2) The distance between essentially antipodal points on an
%            ellipsoid is very sensitive to small deviations in azimuth,
%            so such points should be avoided. (A warning is given.)
%        (3) For other cases, precision is set to about one part in 10^12
%        (3) Tested but no warranty; use at your own risk.
%        (4) Written by Michael Kleder, April 2006
%
% Example:
% >> [dist,azim]=vdist(10,20,30,40)
% dist =    3035728.95690893
% azim =    40.3196402221127
% >> [lat2,lon2]=vdistinv(10,20,dist,azim)
% lat2 =    29.9999999999981
% lon2 =    39.9999999999979
% >>

% initial guess for path endpoint is computed using spherical earth trig:
t1=lat1*0.0174532925199433; % degrees to radians
n1=lon1*0.0174532925199433; % degrees to radians
a=azim*0.0174532925199433; % degrees to radians
d=dist*1.56961230576048e-007; % meters to radians
lat2 = asin(sin(t1)*cos(d)+cos(t1)*sin(d)*cos(a));
lon2 = n1+atan2(sin(d)*sin(a),cos(t1)*cos(d)-sin(t1)*sin(d)*cos(a));
X=[lat2;lon2]*57.2957795130823; % radians to dgrees
% other parameters (start point, arc length, and azimuth) are fixed:
params=[lat1;lon1;dist;azim];
% optimization control settings:
opt=optimset('MaxFunEvals',5000,'TolFun',1e-12);
% solve for accurate end point:
X=fminsearch(@tryone,X,opt,params);
% recover coordinates of endpoint:
lat2=X(1);
lon2=X(2);
if nargout > 2 % if error data is requested
    % compute distance and azimuth from startpoint to endpoint
    [d,a] = vdist(lat1,lon1,lat2,lon2);
    % error in distance:
    derr=abs(dist-d);
    % error in azimuth:
    azim = mod(azim,360);
    a = mod(a,360);
    aerr = abs(azim-a);
    aerr = min(aerr,abs(360-aerr));
end
return
function err=tryone(X,params)
lat2=X(1);
lon2=X(2);
lat1=params(1);
lon1=params(2);
dist=params(3);
azim=params(4);
% crude catch for out-of-bounds attempts:
if lat1<-90 || lat1 > 90 || lat2 < -90 || lat2 > 90
    err = 1e6;
    return
end
[trydist,tryazim]=vdist(lat1,lon1,lat2,lon2);
% compute overall error. First convert distance to approximate arc length
% in degrees so as to weight the terms reasonably closely. (Both move to
% zero in the optimization, but reasonably balancing the units can help.)
err = sqrt((9e-6*(dist-trydist))^2 + (azim-tryazim)^2);
return
function varargout = vdist(lat1,lon1,lat2,lon2)
% VDIST - Using the WGS-84 Earth ellipsoid, compute the distance between
%         two points within a few millimeters of accuracy, compute forward
%         azimuth, and compute backward azimuth, all using a vectorized
%         version of Vincenty's algorithm.
%
% s = vdist(lat1,lon1,lat2,lon2)
% [s,a12] = vdist(lat1,lon1,lat2,lon2)
% [s,a12,a21] = vdist(lat1,lon1,lat2,lon2)
%
% s = distance in meters (inputs may be scalars, vectors, or matrices)
% a12 = azimuth in degrees from first point to second point (forward)
% a21 = azimuth in degrees from second point to first point (backward)
%       (Azimuths are in degrees clockwise from north.)
% lat1 = GEODETIC latitude of first point (degrees)
% lon1 = longitude of first point (degrees)
% lat2, lon2 = second point (degrees)
%
%  Original algorithm source:
%  T. Vincenty, "Direct and Inverse Solutions of Geodesics on the Ellipsoid
%  with Application of Nested Equations", Survey Review, vol. 23, no. 176,
%  April 1975, pp 88-93.
%  Available at: http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
%
% Notes: (1) lat1,lon1,lat2,lon2 can be any (identical) size/shape. Outputs
%            will have the same size and shape.
%        (2) Error correcting code, convergence failure traps, antipodal
%            corrections, polar error corrections, WGS84 ellipsoid
%            parameters, testing, and comments: Michael Kleder, 2004.
%        (3) Azimuth implementation (including quadrant abiguity
%            resolution) and code vectorization, Michael Kleder, Sep 2005.
%        (4) Vectorization is convergence sensitive; that is, quantities
%            which have already converged to within tolerance are not
%            recomputed during subsequent iterations (while other
%            quantities are still converging).
%        (5) Vincenty describes his distance algorithm as precise to within
%            0.01 millimeters, subject to the ellipsoidal model.
%        (6) For distance calculations, essentially antipodal points are
%            treated as exactly antipodal, potentially reducing accuracy
%            slightly.
%        (7) Distance failures for points exactly at the poles are
%            eliminated by moving the points by 0.6 millimeters.
%        (8) The Vincenty distance algorithm was transcribed verbatim by
%            Peter Cederholm, August 12, 2003. It was modified and
%            translated to English by Michael Kleder.
%            Mr. Cederholm's website is http://www.plan.aau.dk/~pce/
%        (9) Distances agree with the Mapping Toolbox, version 2.2 (R14SP3)
%            with a max relative difference of about 5e-9, except when the
%            two points are nearly antipodal, and except when one point is
%            near the equator and the two longitudes are nearly 180 degrees
%            apart. This function (vdist) is more accurate in such cases.
%            For example, note this difference (as of this writing):
%            >>vdist(0.2,305,15,125)
%            18322827.0131551
%            >>distance(0.2,305,15,125,[6378137 0.08181919])
%            0
%       (10) Azimuths FROM the north pole (either forward starting at the
%            north pole or backward when ending at the north pole) are set
%            to 180 degrees by convention. Azimuths FROM the south pole are
%            set to 0 degrees by convention.
%       (11) Azimuths agree with the Mapping Toolbox, version 2.2 (R14SP3)
%            to within about a hundred-thousandth of a degree, except when
%            traversing to or from a pole, where the convention for this
%            function is described in (10), and except in the cases noted
%            above in (9).
%       (12) No warranties; use at your own risk.

% reshape inputs
keepsize = size(lat1);
lat1=lat1(:);
lon1=lon1(:);
lat2=lat2(:);
lon2=lon2(:);
% Input check:
if any(abs(lat1)>90 | abs(lat2)>90)
    error('Input latitudes must be between -90 and 90 degrees, inclusive.')
end
% Supply WGS84 earth ellipsoid axis lengths in meters:
a = 6378137; % definitionally
b = 6356752.31424518; % computed from WGS84 earth flattening coefficient
% preserve true input latitudes:
lat1tr = lat1;
lat2tr = lat2;
% convert inputs in degrees to radians:
lat1 = lat1 * 0.0174532925199433;
lon1 = lon1 * 0.0174532925199433;
lat2 = lat2 * 0.0174532925199433;
lon2 = lon2 * 0.0174532925199433;
% correct for errors at exact poles by adjusting 0.6 millimeters:
kidx = abs(pi/2-abs(lat1)) < 1e-10;
if any(kidx);
    lat1(kidx) = sign(lat1(kidx))*(pi/2-(1e-10));
end
kidx = abs(pi/2-abs(lat2)) < 1e-10;
if any(kidx)
    lat2(kidx) = sign(lat2(kidx))*(pi/2-(1e-10));
end
f = (a-b)/a;
U1 = atan((1-f)*tan(lat1));
U2 = atan((1-f)*tan(lat2));
lon1 = mod(lon1,2*pi);
lon2 = mod(lon2,2*pi);
L = abs(lon2-lon1);
kidx = L > pi;
if any(kidx)
    L(kidx) = 2*pi - L(kidx);
end
lambda = L;
lambdaold = 0*lat1;
itercount = 0;
notdone = logical(1+0*lat1);
alpha = 0*lat1;
sigma = 0*lat1;
sinsigma=nan*lat1;
cossigma=nan*lat1;
cos2sigmam = 0*lat1;
C = 0*lat1;
warninggiven = false;
while any(notdone)  % force at least one execution
    %disp(['lambda(21752) = ' num2str(lambda(21752),20)]);
    itercount = itercount+1;
    if itercount > 50
        if ~warninggiven
            warning('VDIST:antipodal',['Essentially antipodal points ' ...
                'encountered. Precision may be reduced.']);
        end
        lambda(notdone) = pi;
        break
    end
    lambdaold(notdone) = lambda(notdone);
    sinsigma(notdone) = sqrt((cos(U2(notdone)).*sin(lambda(notdone)))...
        .^2+(cos(U1(notdone)).*sin(U2(notdone))-sin(U1(notdone)).*...
        cos(U2(notdone)).*cos(lambda(notdone))).^2);
    cossigma(notdone) = sin(U1(notdone)).*sin(U2(notdone))+...
        cos(U1(notdone)).*cos(U2(notdone)).*cos(lambda(notdone));
    % eliminate rare imaginary portions at limit of numerical precision:
    sinsigma(notdone)=real(sinsigma(notdone));
    cossigma(notdone)=real(cossigma(notdone));
    sigma(notdone) = atan2(sinsigma(notdone),cossigma(notdone));
    alpha(notdone) = asin(cos(U1(notdone)).*cos(U2(notdone)).*...
        sin(lambda(notdone))./sin(sigma(notdone)));
    cos2sigmam(notdone) = cos(sigma(notdone))-2*sin(U1(notdone)).*...
        sin(U2(notdone))./cos(alpha(notdone)).^2;
    C(notdone) = f/16*cos(alpha(notdone)).^2.*(4+f*(4-3*...
        cos(alpha(notdone)).^2));
    lambda(notdone) = L(notdone)+(1-C(notdone)).*f.*sin(alpha(notdone))...
        .*(sigma(notdone)+C(notdone).*sin(sigma(notdone)).*...
        (cos2sigmam(notdone)+C(notdone).*cos(sigma(notdone)).*...
        (-1+2.*cos2sigmam(notdone).^2)));
    %disp(['then, lambda(21752) = ' num2str(lambda(21752),20)]);
    % correct for convergence failure in the case of essentially antipodal
    % points
    if any(lambda(notdone) > pi)
        warning('VDIST:antipodal',['Essentially antipodal points ' ...
            'encountered. Precision may be reduced.']);
        warninggiven = true;
        lambdaold(lambda>pi) = pi;
        lambda(lambda>pi) = pi;
    end
    notdone = abs(lambda-lambdaold) > 1e-12;
end
u2 = cos(alpha).^2.*(a^2-b^2)/b^2;
A = 1+u2./16384.*(4096+u2.*(-768+u2.*(320-175.*u2)));
B = u2./1024.*(256+u2.*(-128+u2.*(74-47.*u2)));
deltasigma = B.*sin(sigma).*(cos2sigmam+B./4.*(cos(sigma).*(-1+2.*...
    cos2sigmam.^2)-B./6.*cos2sigmam.*(-3+4.*sin(sigma).^2).*(-3+4*...
    cos2sigmam.^2)));
varargout{1} = reshape(b.*A.*(sigma-deltasigma),keepsize);
if nargout > 1
    % From point #1 to point #2
    % correct sign of lambda for azimuth calcs:
    lambda = abs(lambda);
    kidx=sign(sin(lon2-lon1)) .* sign(sin(lambda)) < 0;
    lambda(kidx) = -lambda(kidx);
    numer = cos(U2).*sin(lambda);
    denom = cos(U1).*sin(U2)-sin(U1).*cos(U2).*cos(lambda);
    a12 = atan2(numer,denom);
    kidx = a12<0;
    a12(kidx)=a12(kidx)+2*pi;
    % from poles:
    a12(lat1tr <= -90) = 0;
    a12(lat1tr >= 90 ) = pi;
    varargout{2} = reshape(a12 * 57.2957795130823,keepsize); % to degrees
end
if nargout > 2
    a21=NaN*lat1; %#ok this variable won't be computed if not needed
    % From point #2 to point #1
    % correct sign of lambda for azimuth calcs:
    lambda = abs(lambda);
    kidx=sign(sin(lon1-lon2)) .* sign(sin(lambda)) < 0;
    lambda(kidx)=-lambda(kidx);
    numer = cos(U1).*sin(lambda);
    denom = sin(U1).*cos(U2)-cos(U1).*sin(U2).*cos(lambda);
    a21 = atan2(numer,denom);
    kidx=a21<0;
    a21(kidx)= a21(kidx)+2*pi;
    % backwards from poles:
    a21(lat2tr >= 90) = pi;
    a21(lat2tr <= -90) = 0;
    varargout{3} = reshape(a21 * 57.2957795130823,keepsize); % to degrees
end
return


%########################################################################
%
%	- PPGI Toolbox -
%   A MATLAB toolbox for Photoplethysmography Imaging (PPGI)
%
% Author   : Christian S. Pilz
% Company  : The Nature of Space of Time
% Date     : 07.05.2019
%
% Contact  : cpi@partofthestars.com
% Web Page : www.partofthestars.com
%
% Version  : Alpha RA 1.0
%
%########################################################################
%
%	ground_truth_stats.m:
%
% Description:
%
%   A static MATLAB class to compute ground truth statistics
%

classdef ground_truth_stats

   methods(Static)
       function [pearson, rmse, snr, snr_std, bpm] = get(ppg,waveform, fs)
           y=ppg;
           L=length(y);
           NFFT = 2^nextpow2(L);
           f = fs/2*linspace(0,1,NFFT/2+1);

           [spec_ppg]=spectrogram(y,256,240,f,fs,'yaxis');

           [peak_values, peak_frequencies] = max(abs(spec_ppg));
           bpm_ppg=f(peak_frequencies)*60;

           y=waveform;
           L=length(y);
           NFFT = 2^nextpow2(L);
           f = fs/2*linspace(0,1,NFFT/2+1);

           [spec,freqs,tmp, pxx]=spectrogram(y,256,240,f,fs,'yaxis');

           [peak_values, peak_frequencies] = max(abs(spec));
           bpm=f(peak_frequencies)*60;

           num_elements=size(bpm_ppg,2);
           if num_elements>size(bpm,2)
               num_elements=size(bpm,2);
           end

           [pearson]=corr(bpm_ppg(1,1:num_elements)',bpm(1,1:num_elements)');
           [rmse]=sum(sqrt((bpm_ppg(1,1:num_elements)-bpm(1,1:num_elements)).^2))/num_elements;

           %snr
           hr_f=bpm_ppg/60;
           for i=1:num_elements
               GTMask1 = (freqs >= hr_f(1,i)-0.1)&(freqs <= hr_f(1,i)+0.1);
               GTMask2 = (freqs >= hr_f(1,i)*2-0.2)&(freqs <= hr_f(1,i)*2+0.2);
               %SPower = sum(abs(spec(GTMask1|GTMask2,i)));

               %SPower = sum(abs(spec(GTMask1,i)));
               signal_power = sum(pxx(GTMask1,i));
               FMask2 = (freqs >= 0.5)&(freqs <= 4);
               %total_power = sum(abs(spec(FMask2,i)));
               total_power = sum(pxx(FMask2,i));
               SNR(i) = pow2db(signal_power/(total_power-signal_power));
           end

           [snr] = mean(SNR);
           [snr_std] =std(SNR);
       end
   end
end


function [Xt,Yt] = elec_3d_2d(X,Y,Z,Zrad)

% elec_3d_2d - Project Cartesian 3D coordinates to a 2D plane.
%
% [Xt,Yt] = elec_3d_2d(X,Y,Z,Zrad)
%
% Project 3D electrode positions onto a 2D plane, using
% gnomonic projection, which draws a line from a point
% halfway between equator and south pole, through electrode,
% to a plane tangential to north pole.
%
% Given:  Set of 3D Cartesian coordinates (X,Y,Z with Z > 0)
%         X,Y midpoint at 0
%         Zrad is radius in Z
%
% See also elec_2d_3d.m
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:54 $

% Licence:  GNU GPL, no implied or express warranties
% History:  10/1999, Chris Harvey
%           07/2001, Darren.Weber_at_radiology.ucsf.edu
%                    - using matrix algebra rather than indexed looping
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rad = Zrad / 2;

t = (Z + rad) * ( 1/rad );

Xt = X .* t;
Yt = Y .* t;

return


function R = gqr3 (A)
%GQR3 QR factorization, based on Givens rotations
%
% Example:
%   R = gqr3 (A)
% See also: testall

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

[m n] = size (A) ;

% parent = cs_etree (sparse (A), 'col') ;

for i = 2:m
    % i
    for k = 1:min(i-1,n)
    % k
        % Givens rotation to zero out A(i,k) using A(k,k)
        G = givens2 (A(k,k), A(i,k)) ;
        A ([k i],k:n) = G * A ([k i],k:n) ;
        A (i,k) = 0 ;
        % fprintf ('A(21,25)=%g\n', A(21,25)) ;
        % if (A(21,25) ~= 0)
            % pause
        % end
    end
end
R = A ;


function [v1, v2] = lambertBattinVector(r1, r2, dt, numRevs, gmu)
%lambertBattin Summary of this function goes here
%   Detailed explanation goes here
    if(size(r1,1) ~= 3)
        error('r1 not of length 3');
    end

    if(size(r2,1) ~= 3)
        error('r2 not of length 3');
    end

    r1Mag = sqrt(sum(abs(r1).^2,1));
    r2Mag = sqrt(sum(abs(r2).^2,1));

    tm = zeros(size(dt));
    if(any(dt(dt<0)))
        B = dt<0;
        tm(B) = -1.0;
    end
    if(any(dt(dt>0)))
        B = dt>0;
        tm(B) = 1.0;
    end
    if(any(dt(dt==0)))
        error('dt = 0; lambert cannot compute');
    end
    dt = abs(dt);

    cosDeltaTA = dot(r1,r2,1)./(r1Mag.*r2Mag);
    sinDeltaTA = tm .* sqrt(1 - cosDeltaTA.^2);
    deltaTA = atan2(sinDeltaTA,cosDeltaTA);
    deltaTA = AngleZero2Pi(deltaTA);

    c = sqrt(r1Mag.^2 + r2Mag.^2 - 2.*r1Mag.*r2Mag.*cosDeltaTA);
    s = (r1Mag + r2Mag + c)./2;
    epsilon = (r2Mag - r1Mag)./r1Mag;

    intemed1 = r2Mag./r1Mag;
    TanSqr2w = (epsilon.^2/4) ./ (sqrt(intemed1) + intemed1.*(2 + sqrt(intemed1)));

    sinSqrDeltaTAOver4 = (sin(deltaTA/4)).^2;
    cosSqrDeltaTAOver4 = (cos(deltaTA/4)).^2;
    rop = sqrt(r1Mag.*r2Mag) .* (cosSqrDeltaTAOver4 + TanSqr2w);

    l = zeros(size(tm));
    if(any(tm(tm==1.0)))
        bool = tm==1.0;
        l(bool) = (sinSqrDeltaTAOver4(bool) + TanSqr2w(bool))./(sinSqrDeltaTAOver4(bool) + TanSqr2w(bool) + cos(deltaTA(bool)/2));
    end
    if(any(tm(tm==-1.0)))
        bool = tm==-1.0;
        l(bool) = (cosSqrDeltaTAOver4(bool) + TanSqr2w(bool) - cos(deltaTA(bool)/2)) ./ (cosSqrDeltaTAOver4(bool) + TanSqr2w(bool));
    end

    m = (gmu .* dt.^2)./(8*rop.^3);

    x = l;
    x_change = 1;
    loops=0;
    while(any(x_change > 1E-6) && loops <= 30)
            ksi = computeKsi(x, 20);

            h1 = ((l+x).^2.*(1 + 3.*x + ksi)) ./ ((1+2.*x+l).*(4.*x + ksi.*(3+x)));
            h2 = (m.*(x - l + ksi)) ./ ((1+2.*x+l).*(4.*x + ksi.*(3+x)));

            B=27*h2./(4*(1+h1).^3);
            U=B./(2*(sqrt(1+B)+1));
            K_U=Kay(U);
            y=(1+h1)./3.*(2+sqrt(1+B)./(1+2*U.*K_U.^2));

            x_new = sqrt(((1-l)/2).^2 + m./(y.^2)) - (1+l)/2;
            x_change=abs(x-x_new);
            x = x_new;
            loops=loops+1;
    end
    a = (gmu .* dt.^2)./(16*(rop.^2).*x.*(y.^2));

    sinBetaEOver2 = zeros(size(a));
    betaE = zeros(size(a));
    amin = zeros(size(a));
    tmin = zeros(size(a));
    alphaE = zeros(size(a));
    deltaE = zeros(size(a));
    alphaH = zeros(size(a));
    betaH = zeros(size(a));
    deltaH = zeros(size(a));
    f = zeros(size(a));
    g = zeros(size(a));
    g_dot = zeros(size(a));
    if(any(a(a > 0.0)))
        bool = a > 0.0;

        sinBetaEOver2(bool) = sqrt((s(bool)-c(bool))./(2*a(bool)));
        betaE(bool) = real(2*asin(sinBetaEOver2(bool))); %real needed to prevent complex numbers from appearing

        if(any(deltaTA(bool) > pi))
            bool2 = bool & deltaTA > pi;
            betaE(bool2) = -betaE(bool2);
        end

        amin(bool) = s(bool)./2;
        tmin(bool) = sqrt(amin(bool).^3./gmu(bool)) .* (pi - betaE(bool) + sin(betaE(bool)));

        alphaE(bool) = real(2*asin(sqrt(s(bool)./(2*a(bool))))); %real needed to prevent complex numbers from appearing

        if(any(dt > tmin))
            bool2 = bool & dt > tmin;
            alphaE(bool2) = 2*pi - alphaE(bool2);
        end

        deltaE(bool) = alphaE(bool) - betaE(bool);

        f(bool)     = real(1 - (a(bool)./r1Mag(bool)) .* (1 - cos(deltaE(bool)))); %real() necessary to prevent complex doubles from forming
        g(bool)     = real(dt(bool) - sqrt(a(bool).^3./gmu(bool)) .* (deltaE(bool) - sin(deltaE(bool)))); %real() necessary to prevent complex doubles from forming
        g_dot(bool) = real(1 - (a(bool)./r2Mag(bool)) .* (1- cos(deltaE(bool))));  %real() necessary to prevent complex doubles from forming
    end
    if(any(a(a < 0.0)))
        bool = a < 0.0;

        alphaH(bool) = 2*asinh(sqrt(s(bool)./(-2*a(bool))));
        betaH(bool)  = 2*asinh(sqrt((s(bool)-c(bool))./(-2*a(bool))));
        deltaH(bool) = alphaH(bool)-betaH(bool);

        f(bool)     = real(1 - (a(bool)./r1Mag(bool)) .* (1 - cosh(deltaH(bool)))); %real() necessary to prevent complex doubles from forming
        g(bool)     = real(dt(bool) - sqrt(-(a(bool).^3)./gmu(bool)) .* (sinh(deltaH(bool)) - deltaH(bool))); %real() necessary to prevent complex doubles from forming
        g_dot(bool) = real(1 - (a(bool)./r2Mag(bool)) .* (1 - cosh(deltaH(bool)))); %real() necessary to prevent complex doubles from forming
    end
    if(any(a(a == 0.0)))
        error('a = 0.0');
    end

    v1 = bsxfun(@rdivide,r2 - bsxfun(@times,r1,f),g);
    v2 = bsxfun(@rdivide,bsxfun(@times,r2,g_dot) - r1,g);
end

function ksi = computeKsi(x, numLevels)
    eta = x./(sqrt(1+x) + 1).^2;
    num = 8.*(sqrt(1+x) + 1);
    denom = 1;

    if(numLevels>0)
        denom = 3 + 1./(eta + computeKsi(eta, numLevels-1));
    end
    ksi = num ./ denom;
%     disp(ksi);
end

function [K_U]=Kay(U)
    % setup the C variable
    c(1)=4/27;   %@n=0
    c(2)=8/27; %@n=1
    c(3)=208/891; %n=2
    c(4)=340/1287; %@n=3
    c(5)=700/2907;
    c(6)=928/3591;
    c(7)=296/1215;
    c(8)=1804/7047;
    c(9)=2548/10395;
    c(10)=2968/11655;
    c(11)=3904/15867;
    c(12)=884/3483;
    c(13)=5548/22491;
    c(14)=6160/24327;
    c(15)=7480/30267;
    c(16)=8188/32391;
    c(17)=1940/7839;
    c(18)=10504/41607;
    c(19)=12208/49275;
    c(20)=13108/51975;

    % sum up all the intermediate variables
    Z=1+c(20).*U;
    Z=1+c(19).*U./Z;
    Z=1+c(18).*U./Z;
    Z=1+c(17).*U./Z;
    Z=1+c(16).*U./Z;
    Z=1+c(15).*U./Z;
    Z=1+c(14).*U./Z;
    Z=1+c(13).*U./Z;
    Z=1+c(12).*U./Z;
    Z=1+c(11).*U./Z;
    Z=1+c(10).*U./Z;
    Z=1+c(9).*U./Z;
    Z=1+c(8).*U./Z;
    Z=1+c(7).*U./Z;
    Z=1+c(6).*U./Z;
    Z=1+c(5).*U./Z;
    Z=1+c(4).*U./Z;
    Z=1+c(3).*U./Z;
    Z=1+c(2).*U./Z;
    Z=1+c(1).*U./Z;

    K_U=1./(3*Z);
end


function b = dsisl ( a, lda, n, kpvt, b )

%*****************************************************************************80
%
%% DSISL solves a real symmetric system factored by DSIFA.
%
%  Discussion:
%
%    To compute inverse(A) * C where C is a matrix with P columns
%
%      call dsifa ( a, lda, n, kpvt, info )
%
%      if ( info == 0 ) then
%        do j = 1, p
%          call dsisl ( a, lda, n, kpvt, c(1,j) )
%        end do
%      end if
%
%    A division by zero may occur if the inverse is requested
%    and DSICO has set RCOND == 0.0D+00 or DSIFA has set INFO /= 0.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Dongarra, Moler, Bunch and Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%    ISBN 0-89871-172-X
%
%  Parameters:
%
%    Input, real A(LDA,N), the output from DSIFA.
%
%    Input, integer LDA, the leading dimension of the array A.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer KPVT(N), the pivot vector from DSIFA.
%
%    Input, real B(N), the right hand side.
%
%    Output, real B(N), the solution.
%

%
%  Loop backward applying the transformations and D inverse to B.
%
  k = n;

  while ( 0 < k )

    if ( 0 <= kpvt(k) )
%
%  1 x 1 pivot block.
%
      if ( k ~= 1 )

        kp = kpvt(k);
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end
%
%  Apply the transformation.
%
        b(1:k-1) = daxpy ( k-1, b(k), a(1:k-1,k), 1, b(1:k-1), 1 );

      end
%
%  Apply D inverse.
%
      b(k) = b(k) / a(k,k);
      k = k - 1;

    else
%
%  2 x 2 pivot block.
%
      if ( k ~= 2 )

        kp = abs ( kpvt(k) );
%
%  Interchange.
%
        if ( kp ~= k-1 )
          temp = b(k-1);
          b(k-1) = b(kp);
          b(kp) = temp;
        end
%
%  Apply the transformation.
%
        b(1:k-2) = daxpy ( k-2, b(k), a(1:k-2,k), 1, b(1:k-2), 1 );
        b(1:k-2) = daxpy ( k-2, b(k-1), a(1:k-2,k-1), 1, b(1:k-2), 1 );

      end
%
%  Apply D inverse.
%
      ak = a(k,k) / a(k-1,k);
      akm1 = a(k-1,k-1) / a(k-1,k);
      bk = b(k) / a(k-1,k);
      bkm1 = b(k-1) / a(k-1,k);
      denom = ak * akm1 - 1.0;
      b(k) = ( akm1 * bk - bkm1 ) / denom;
      b(k-1) = ( ak * bkm1 - bk ) / denom;
      k = k - 2;

    end

  end
%
%  Loop forward applying the transformations.
%
  k = 1;

  while ( k <= n )

    if ( 0 <= kpvt(k) )
%
%  1 x 1 pivot block.
%
      if ( k ~= 1 )
%
%  Apply the transformation.
%
        b(k) = b(k) + b(1:k-1)' * a(1:k-1,k);
        kp = kpvt(k);
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end

      end

      k = k + 1;

    else
%
%  2 x 2 pivot block.
%
      if ( k ~= 1 )
%
%  Apply the transformation.
%
        b(k) = b(k) + b(1:k-1) * a(1:k-1,k);
        b(k+1) = b(k+1) + b(1:k-1) * a(1:k-1,k+1);
        kp = abs ( kpvt(k) );
%
%  Interchange.
%
        if ( kp ~= k )
          temp = b(k);
          b(k) = b(kp);
          b(kp) = temp;
        end

      end

      k = k + 2;

    end

  end

  return
end


function h = fun2hist(fun,S)
%FUN2HIST Generates a histogram from a given digital function.
%   H = FUN2HIST(FUN,S) generates histogram H from a 1-D input
%   function FUN (a vector). The number of bins in H is equal to the
%   number of elements of FUN. If only FUN is provided in the input,
%   then H is normalized so that the sum of its components equals 1.
%   If S (a scalar) is provided, H is unnormalized, in the sense
%   that the sum of its components is equal to S. This is useful,
%   for example, if it is required that the sum of the components of
%   the histogram equal the number of pixels in an image. In this
%   case, the value of S would be S=M*N, where M and N are the
%   number of rows and columns in the image, respectively.
%
%   When S is specified, the elements of H are converted to
%   integers, so there is likely to be small roundoff errors between
%   the shapes of FUN and H. The reason for converting to integers
%   is that the sum of the components of an unnormalized histogram
%   has to equal the number of pixels (an integer) in the
%   corresponding image, as noted above.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Preliminaries
% Set defaults.
mode = 'u';
if nargin == 1
   mode = 'n';
end
% Number of bins (intensity levels in the histogram).
L = numel(fun);
% Initialize histogram.
h(1:L) = 0;
% Get index of the elements of fun that are not 0. These will be the
% populated bins in H.
idx = find(fun ~= 0);
IDXL = numel(idx);

% Generate the histogram from input function fun.
switch mode
   case 'n'
      h = fun/sum(fun);
   case 'u'
      % Convert to integers.
      h = round(S*(fun/sum(fun)));
      % Most likely, sum(H) will not equal S because of rounding.
      % If sum(H) is less than S, distribute the difference
      % between S and sum(H) equally (i.e., by increments of 1)
      % among elements of the populated histogram bins, starting
      % from the left and proceesing to the right. If sum(H) > S,
      % then take away 1 pixel from the populated bins.
      D = sum(h) - S;
      if D < 0
         count = abs(D);
         % Loop through the histogram, adding elements to the
         % populated bins until all D elements have been added.
         while count
            K = IDXL;
            if count < IDXL
               K = count;
            end
            for I = 1:K
               % Add counts to the populated bins only.
               h(idx(I)) = h(idx(I)) + 1;
               count = count - 1;
            end
         end
      elseif D > 0
         count = D;
         % Loop through the histogram, subtracting elements until
         % all D elements have been subtracted.
         while count
            K = IDXL;
            if count < IDXL
               K = count;
            end
            for I = 1:K
               % Subtract counts from the populated bins only.
               % But make sure they don't go negative.
               h(idx(I)) = h(idx(I)) - 1;
               if h(idx(I)) < 0
                  % Restore the amount subtracted.
                  h(idx(I)) = h(idx(I)) - 1;
                  % And reduce count so that the count will be
                  % subtracted elsewhere.
                  count = count + 1; % As if nothing had happened.
               end
            count = count - 1;
            end
         end
      end
end


function [ftlb] = eV2ftlb(eV)
% Convert energy or work from electron volts to foot-pounds.
% Chad A. Greene 2012
ftlb = eV*1.181705375e-19;


function inside = p13_inside ( m, n, point )

%*****************************************************************************80
%
%% P13_INSIDE reports if a point is inside the region in problem 13.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, logical INSIDE(N), is TRUE if the point is in the region.
%
  inside(1:n) =                                           ...
       ( 45.0 <= point(1,1:n) & point(1,1:n) <= 55.0 &    ...
         30.0 <= point(2,1:n) & point(2,1:n) <= 90.0    ) ...
    |                                                     ...
       ( 900.0 <=                                         ...
         ( point(1,1:n) - 50.0 ).^2                       ...
       + ( point(2,1:n) -  0.0 ).^2 &                     ...
         ( point(1,1:n) - 50.0 ).^2                       ...
       + ( point(2,1:n) -  0.0 ).^2 <= 1600.0 );

  return
end


% test_trifacet_signed_area.m

% Author: Ramon Casero <rcasero@gmail.com>
% Copyright © 2013 University of Oxford
% Version: 0.1.0
%
% University of Oxford means the Chancellor, Masters and Scholars of
% the University of Oxford, having an administrative office at
% Wellington Square, Oxford OX1 2JD, UK.
%
% This file is part of Gerardus.
%
% 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. The offer of this
% program under the terms of the License is subject to the License
% being interpreted in accordance with English Law and subject to any
% action against the University of Oxford being under the jurisdiction
% of the English Courts.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see
% <http://www.gnu.org/licenses/>.

% Triangle with positive area
x = [
    0 0
    1 0
    0 1
    ];

tri = [1 2 3];

a = trifacet_signed_area(tri, x)

% Triangle with negative area
tri = [1 3 2];

a = trifacet_signed_area(tri, x)


function enlarged_tt = add_non_essential_dims(small_tt, new_n, old_vars_pos)
    % enlarged_tt = add_non_essential_dims(small_tt, new_n, old_vars_pos)
    % creates enlarged version of tt_tensor, expanding it by ones.
    % Mode sizes of the enlarged_tt are equal to new_n.
    % First dimension of the small_tt is old_vars_pos(1) dimension of the enlarged_tt.
    % This procedure doesn't increase the maximal TT-rank.
    %
    % Input:
    % 	small_tt     -- P-dimensional tensor in the TT-format.
    % 	new_n        -- Q by 1 vector with enlarged_tt mode sizes (Q > P).
    % 	old_vars_pos -- P by 1 vector in ascending order. Consists of positions
    % 							of the small_tt dimensions in the enlarged_tt tensor.
    %
    % Output:
    % 	enlarged_tt  -- Q-dimensional tensor in the TT format.
    % 			enlarged_tt(i1, ..., iQ)
    % 			i1 take values from {1, ..., new_n(1)}
    % 			...
    % 			iQ take values from {1, ..., new_n(Q)}
    %
    % Example:
    %       m = rand(3, 4);
    %       small_tt = tt_tensor(m); % small_tt(i2, i4)
    %       enlarged_tt = add_non_essential_dims(small_tt, [2, 3, 6, 4, 4], [2, 4]);
    %       % enlarged_tt(i1, i2, i3, i4, i5, i6) == small_tt(i2, i4) for any i1, i3, i5, i6
    %

    if (~issorted(old_vars_pos))
        error('Dimensions (third argument) must be in ascending order.');
    end

    if ~isvector(new_n) | ~isvector(old_vars_pos) | small_tt.d ~= length(old_vars_pos)
    	error('Wrong usage, see help tt_tensor.add_non_essential_dims.');
    end

    d = length(new_n);
    enlarged_tt = tt_tensor;
    enlarged_tt.d = d;
    enlarged_tt.r = zeros(d + 1, 1);
    enlarged_tt.r(1) = 1;
    enlarged_tt.n = new_n(:);
    enlarged_tt.core = [];
    enlarged_tt.ps = zeros(d + 1, 1);
    enlarged_tt.over = 0;
    curr_var_i = 0;
    vars_length = length(old_vars_pos);
    before_first_var = true;
    after_last_var = false;
    for dimension_i = 1:d
        if curr_var_i == vars_length
            after_last_var = true;
        end

        if curr_var_i < vars_length && old_vars_pos(curr_var_i + 1) == dimension_i
            curr_var_i = curr_var_i + 1;
            before_first_var = false;
            if new_n(dimension_i) ~= small_tt.n(curr_var_i)
                error('Dimensions must agree!');
            end
        end

        if before_first_var || after_last_var
            % Dimension_i is before old_vars_pos(1) or after old_vars_pos(end).
            curr_core = ones(new_n(dimension_i), 1);
            curr_rank = 1;
        elseif old_vars_pos(curr_var_i) == dimension_i
            % It's real dimension.
            curr_core = core(small_tt, curr_var_i);
            curr_rank = small_tt.r(curr_var_i);
        else
            % Non-essential dimension between two real ones.
            curr_rank = small_tt.r(curr_var_i + 1);
            curr_core = core_eye(curr_rank, new_n(dimension_i));
        end
        enlarged_tt.r(dimension_i) = curr_rank;
        enlarged_tt.ps(dimension_i) = length(enlarged_tt.core) + 1;
        enlarged_tt.core = [enlarged_tt.core; curr_core(:)];
    end
    enlarged_tt.r(end) = 1;
    enlarged_tt.ps(end) = length(enlarged_tt.core) + 1;
end

function core = core_eye(r, n)
    % Return 3-dimensional array filled with eye matrices,
    % which corresponds to non-essential dimension core.
    core = zeros(r, n, r);
    for i = 1:n
        core(:, i, :) = eye(r);
    end
end


function [I,J] = stam_order(F,A)
  % STAM_ORDER For a regular patch of 13 faces and 12 vertices of a mesh (V,F)
  % around a "center" facet f determined by all its corners having valence 6,
  % determine the reordering of vertices, so that the reordered mesh
  % (V(I,:),J(F)) will fit Jos Stam's ordering in "Evaluation of Loop
  % Subdivision Surfaces". The facet F(f,:) is mapped to [4 7 8] in Stam's
  % patch.
  %
  % [I,J] = stam_order(F)
  % [I,J] = stam_order(F,A)
  %
  % Input:
  %   F  13 by 3 list of triangle indices into some V
  %   A  max(F) by max(F) adjacency matrix {[]}
  % Output:
  %   I  12 long list of indices such that V(I,:) are the reorderd vertices.
  %   J  12 long list of indices such that J(F) are the reordered faces,
  %     {J=full(sparse(I,1,1:12))}
  %
  % Example:
  %   % Canonical regular patch
  %   WV = [-1 0;0 -1;-1 1;0 0;1 -1;-1 2;0 1;1 0;2 -1;0 2;1 1;2 0];
  %   F = [ ...
  %     1 3 4;4 2 1;4 5 2; ...
  %     6 7 3;7 4 3;4 7 8;8 5 4;8 9 5; ...
  %     6 10 7;10 11 7;7 11 8;11 12 8;12 9 8];
  %   % Scramble patch vertex order and face order
  %   R = randperm(size(WV,1));
  %   G = R(F(randperm(end),:));
  %   U = full(sparse([R R],repmat([1 2],size(WV,1),1),WV));
  %   % Uncover order
  %   [I,J] = stam_order(G);
  %   % plot before and after
  %   subplot(1,2,1);
  %   tsurf(G,U,'VertexIndices',1);
  %   set(gca,'YDir','reverse');
  %   subplot(1,2,2);
  %   tsurf(J(G),U(I,:),'VertexIndices',1);
  %   set(gca,'YDir','reverse');
  %

  % There should be 13 face in a regular patch
  assert(size(F,1)==13,'F should have 13 faces to be a regular patch');

  if nargin<2 || isempty(A)
    A = adjacency_matrix(F);
  end
  assert(nnz(any(A))  == 12,'F should reference 12 vertices');
  % valences
  C6 = sum(A,2) == 6;
  % Locate the center face:
  f = find(all(C6(F),2),1);
  if isempty(f)
    error('F is not a regular patch');
  end
  % Stams 4,7,8 vertices
  I = zeros(12,1);
  I(4) = F(f,1);
  I(7) = F(f,2);
  I(8) = F(f,3);
  A(:,F(f,:)) = 0;
  I(5)  = find(A(I(4),:) & A(I(8),:));
  I(3)  = find(A(I(7),:) & A(I(4),:));
  I(11) = find(A(I(8),:) & A(I(7),:));
  A(:,I(I~=0)) = 0;
  I(12) = find(A(I(11),:) & A(I(8),:));
  I( 9) = find(A(I(12),:) & A(I(8),:));
  I( 2) = find(A(I( 5),:) & A(I(4),:));
  I( 1) = find(A(I( 2),:) & A(I(4),:));
  I( 6) = find(A(I( 3),:) & A(I(7),:));
  I(10) = find(A(I( 6),:) & A(I(7),:));
  J = full(sparse(I,1,1:12));
end


% gener_mcardat

%%Jouet 1
N=5;
dag=zeros(N);
dag(1,2)=1;
dag(2,[3 4])=1;
dag(4,5)=1;
node_sizes= [2 3 4 5 2]; ns = node_sizes;
bnet = mk_bnet(dag, node_sizes);
bnet.CPD{1} = tabular_CPD(bnet, 1, [0.2 0.8]);
bnet.CPD{2} = tabular_CPD(bnet, 2, [0.1 0.6 0.3 0.9 0.4 0.7]);
bnet.CPD{3} = tabular_CPD(bnet, 3, [0.3 0.5 0.7 0.2   0.2 0.1 0.05 0.5   0.5 0.4 0.25 0.3]);
bnet.CPD{4} = tabular_CPD(bnet, 4, [0.35 0.05 0.65 0.15 0.25  0.15 0.15 0.10 0.25 0.45  0.5 0.8 0.25 0.6 0.3]);
bnet.CPD{5} = tabular_CPD(bnet, 5, [0.25 0.15  0.15 0.25  0.10 0.15  0.2 0.05  0.3 0.4]);
bnet_orig=bnet

base_proba=0.2;

%%%%%%%% MCAR

bnet_miss = gener_MCAR_net(bnet_orig, base_proba);

carre=zeros(1,3*N);
names={'X1','X2','X3','X4','X5','R1','R2','R3','R4','R5','M1','M2','M3','M4','M5'};
xx=[.1,.3,.5,.7,.9,.05,.25,.45,.65,.85,.15,.35,.55,.75,.95];
yy=[.85,.70,.90,.75,.80,.65,.40,.60,.45,.50,.2,.2,.2,.2,.2];
figure;draw_graph(bnet_miss.dag,names,carre,xx,yy)

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

OK = ~notok

%%%%%%%% MAR

bnet_miss = gener_MAR_net(bnet_orig, base_proba);

carre=zeros(1,3*N);
names={'X1','X2','X3','X4','X5','R1','R2','R3','R4','R5','M1','M2','M3','M4','M5'};
xx=[.1,.3,.5,.7,.9,.05,.25,.45,.65,.85,.15,.35,.55,.75,.95];
yy=[.85,.70,.90,.75,.80,.65,.40,.60,.45,.50,.2,.2,.2,.2,.2];
figure;draw_graph(bnet_miss.dag,names,carre,xx,yy)

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

OK = ~notok


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nonRegressionTest.m                           |
%|    #    |   VERSION    : 0.55                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 05.09.2019                                    |
%| ( === ) |   SYNOPSIS   : run all non regression test                   |
%|  `---'  |                                                              |
%+========================================================================+

clear all
close all
clc

% Gypsilab path
run('../addpathGypsilab.m')

% Mesh management
cd('meshManagement/')
run('nrtMshBitree.m')
run('nrtMshClean.m');
run('nrtMshCube.m');
run('nrtMshOctree.m')
run('nrtMshRead.m');
run('nrtMshRefine.m');
run('nrtMshSegment.m');
run('nrtMshSquare.m');
run('nrtMshTransfo.m');
run('nrtMshWrite.m');

% Domain quadrature
cd('../domainQuadrature/')
run('nrtDom1D.m');
run('nrtDomEdge.m');
run('nrtDomHalfSquare.m');
run('nrtDomIntSing2D.m');
run('nrtDomIntSing3D.m');
run('nrtDomND.m');
run('nrtDomTetra.m');
run('nrtDomTrace.m');
run('nrtDomTriangle.m');

% Finite element
cd('../finiteElement/')
run('nrtFemAllWithShuffle.m');
run('nrtFemContinuity1.m');
run('nrtFemContinuity2.m');
run('nrtFemConvergence2D.m');
run('nrtFemDirichletCube.m');
run('nrtFemDirichletDisk.m');
run('nrtFemDirichletSquare.m');
run('nrtFemDirichletString.m');
run('nrtFemElasticite2D')
run('nrtFemJunction.m');
run('nrtFemLaplace.m');
run('nrtFemOperators.m');
run('nrtFemRwgNed.m');
run('nrtFemWave1D.m');
run('nrtFemWave2D.m');

% Hierarchical matrices
cd('../hierarchicalMatrix')
run('nrtHmxAlgebra.m');
run('nrtHmxBEMConvergence.m');
run('nrtHmxBuilder.m');
run('nrtHmxBuilderFem.m');
run('nrtHmxCompareLU.m');
run('nrtHmxCompressorPartial.m');
run('nrtHmxCompressorsBox.m');
run('nrtHmxCompressorSingular');
run('nrtHmxCompressorTotal.m');
run('nrtHmxCriticalDimension.m');
run('nrtHmxCriticalForm.m');
run('nrtHmxLowrank.m');
run('nrtHmxNonSquare.m');

% Fast & Free memory Method
cd('../fastFreeMemory')
run('nrtFfmAlgebra.m');
run('nrtFfmBuilder.m');
run('nrtFfmBuilderFem.m');
run('nrtFfmHelmholtzBWdir.m');
run('nrtFfmHelmholtzBWneu.m');
run('nrtFfmMaxwellCFIEpec.m');

% Block matrix
cd('../blockMatrix')
run('nrtBmmAlgebra.m');
run('nrtBmmStokesConv.m');

% Scattering 2D
cd('../scattering2d')
run('nrtLaplace2dSDrad.m')
run('nrtHelmholtz2dSDrad.m')
run('nrtHmxHelmholtz2dS.m');
run('nrtHmxHelmholtz2dD.m');
run('nrtHmxHelmholtz2dDt.m');
run('nrtHmxHelmholtz2dH.m');
run('nrtHmxHelmholtz2dBWdir.m');
run('nrtHmxHelmholtz2dBWneu.m');

% Scattering 3D
cd('../scattering3d')
run('nrtHelmholtzCalderon.m')
run('nrtHelmholtzSDrad.m');
run('nrtHmxHelmholtzS.m');
run('nrtHmxHelmholtzD.m');
run('nrtHmxHelmholtzDxy.m');
run('nrtHmxHelmholtzDt.m');
run('nrtHmxHelmholtzH.m');
run('nrtHmxHelmholtzBWdir.m');
run('nrtHmxHelmholtzBWneu.m');
run('nrtHmxMaxwellT.m');
run('nrtHmxMaxwellNxK.m');
run('nrtHmxMaxwellCFIE.m');

% Fem-Bem dielectrique
cd('../femBemDielectrique')
run('nrtHmxFemBemEFIE.m');
run('nrtHmxFemBemEFIEhalf.m');
run('nrtHmxFemBemCFIE.m');
run('nrtHmxFemBemCFIEhalf.m');

% Inverse problem
cd('../inverseProblem')
run('nrtIpbHelmholtz.m');
run('nrtIpbHelmholtz0.m');

% Ray-tracing
cd('../rayTracing');
run('nrtRayCube.m');
run('nrtRayFabryPerot.m');
run('nrtRayLabyrinthe.m');
run('nrtRaySource.m');
run('nrtRaySphere.m');
run('nrtRayTheatre');

% Stokes
cd('../stokes');
run('nrtHmxStkConvergence.m');
run('nrtStkConvergence.m');
run('nrtStkRadiation.m');
run('translatingSphere/nrtStkTranslatingSphere.m');

% Vibro-acoustic
cd('../vibroAcoustic');
run('nrtVibroSlab2d.m');
run('nrtHmxVibroSlab2d.m');

% Operators
cd('../operators');
run('nrtOprValidation');

% End
disp('~~> Non Regresion Test are done. Michto gypsilab !');


%% Test of different Fourier boundaries detection strategies

clear all

%% User setup
% Choose the signal you want to analyze
% (sig1,sig2,sig3,sig4=ECG,sig5=seismic,lena,textures)
signal = 'sig4';

% Choose the wanted preprocessing (none,plaw,poly,morpho)
params.preproc = 'none';
params.degree=5; % degree for the polynomial interpolation

% Choose the wanted detection method (locmax,locmaxmin,ftc)
params.method = 'locmaxmin';
params.N = 6; % maximum number of band for the locmaxmin method
params.completion = 0;

% Perform the detection on the log spectrum instead the spectrum
params.log=0;

%% Load signals
switch lower(signal)
    case 'sig1'
        load('sig1.mat');
        t=0:1/length(f):1-1/length(f);
    case 'sig2'
        load('sig2.mat');
        t=0:1/length(f):1-1/length(f);
    case 'sig3'
        load('sig3.mat');
        t=0:1/length(f):1-1/length(f);
    case 'sig4'
        load('sig4.mat');
        t=0:length(f)-1;
    case 'sig5'
        load('seismic.mat');
        f=f(10000:20000); %sub portion of the signal used in the paper
        t=0:length(f)-1;
    case 'lena'
        load lena
        l=round(size(f,2)/2);
        imR=[f(:,(l-1:-1:1)) f f(:,(end:-1:end-l+1))];
        fftim=fft(imR');
        ff=abs(sum(abs(fftim),2)/size(fftim,2));
    case 'textures'
        load('texture.mat');
        l=round(size(f,2)/2);
        imR=[f(:,(l-1:-1:1)) f f(:,(end:-1:end-l+1))];
        fftim=fft(imR');
        ff=abs(sum(abs(fftim),2)/size(fftim,2));
end

if (~strcmp(signal,'lena')) && (~strcmp(signal,'textures'))
    % We extend the signal by miroring to deal with the boundaries
    l=round(length(f)/2);
    f=[f(l-1:-1:1);f;f(end:-1:end-l+1)];

    % We compute the Fourier transform of f
    ff=abs(fft(f));
end

%% Perform the detection and plot the detected boundaries
boundaries = EWT_Boundaries_Detect(ff,params);
boundaries = boundaries*pi/round(length(ff)/2);
Show_EWT_Boundaries(abs(fft(f)),boundaries,10);


clear all; close all; clc

addpath('apm')

s = 'http://byu.apmonitor.com';
b = 'mhe';

% Connect to Arduino
tclab;

% Run time in minutes
run_time = 5.0;

% Number of cycles (1 cycle per 2 seconds)
loops = round(30*run_time);

% Temperature (degC)
T1 = ones(1,loops) * T1C(); % measured T
T1mhe = ones(1,loops) * T1C(); % measured T
Kp = ones(1,loops) * 0.3598;
tau = ones(1,loops) * 47.73;
TC_ss = ones(1,loops) * 23;
time = zeros(1,loops);

% milli-volts input
Q1 = zeros(1,loops);
Q2 = zeros(1,loops);
Q1(6:end) = 100.0;
Q1(50:end) = 20.0;
Q1(100:end) = 80.0;
Q1(150:end) = 10.0;
Q1(200:end) = 95.0;
Q1(250:end) = 0.0;

% time
tm = zeros(1,loops);

% moving horizon estimation
mhe_init();

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);
anpred1 = animatedline('LineStyle','--','Color', 'r','LineWidth', 2);
ylabel('Temperature \circC')
legend('T_1 Measured', 'T_1 Predicted', ...
    'Location', 'northwest')
title('Temperature Estimation')
subplot(3,1,2)
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')
subplot(3,1,3)
hold on, grid on
anK = animatedline('LineStyle','-', 'Color', 'r', 'LineWidth', 2);
antau = animatedline('LineStyle','--', 'Color', 'b', 'LineWidth', 2);
ylabel('Parameters')
legend('K x 100', 'tau', 'Location', 'northwest')
xlabel('Time (sec)')

for ii = 1:loops
    % adjust power level
    h1(Q1(ii));
    h2(Q2(ii));

    % Pause Sleep time
    pause_max = 2.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();

    % non-linear energy balance
    jj = ii+1;
    params = mhe(T1(ii),Q1(ii));
    Kp(jj) = params(1);
    tau(jj) = params(2);
    T1mhe(jj) = params(4);

    % plot
    addpoints(anexp1,time(ii),T1(ii))
    addpoints(anpred1,time(ii),T1mhe(ii))
    addpoints(anQ1,time(ii),Q1(ii))
    addpoints(anQ2,time(ii),Q2(ii))
    addpoints(anK,time(ii),100*Kp(ii))
    addpoints(antau,time(ii),tau(ii))
    drawnow

    if ii==10
        apm_web(s,b);
    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'];
csvwrite('data.txt',data);


function G = gsp_jtv_graph(G,T,fs,param)
% GSP_JTV_GRAPH Add time information to the graph structure
%   Usage:  G = gsp_jtv_graph(G);
%           G = gsp_jtv_graph(G,T);
%           G = gsp_jtv_graph(G,T,fs);
%           G = gsp_jtv_graph(G,T,fs,param);
%
%   Input parameters:
%         G          : Graph structure
%         T          : Time length
%         fs         : Sampling frequency (default 1)
%         param      : Structure of optional parameters
%
%   Output parameters:
%         G          : Time-Vertex Graph structure
%
%   This function adds the time domain information to the structure G.
%   The fields stored inside G.jtv are:
%   - T         : Time length
%   - fs        : Sampling frequency
%   - NFFT      : Number of frequency point (default [])
%   - Transform : Time Fourier basis: 'dft' or 'dct'
%   - Extension : Signal will be zero padded in time to take into account negative lag
%   - Lag       : Lag axis
%   - omega     : Frequency axis
%   - DiffT     : Time Gradient
%   - LT        : Time Laplacian
%
%   Additional parameters
%   ---------------------
%
%   * *param.transform*  : Fourier basis: 'dft' or 'dct'. (default 'dft')
%   * *param.approx*     : Stencil approx for the gradient: 'forward' or 'backward'. (default 'forward')
%   * *param.extension*  : Signal will be zero padded in time when needed. (default '0')
%   * *param.NFFT*       : Number of frequency point. (default [])
%

% Author :  Francesco Grassi
% Date   : September 2016

if nargin<4
    param = struct;
end

if ~isstruct(G)
    error('G is not a valid graph');
end

if nargin<2 || ~isnumeric(T) || T<1
    error('Time length T must be a numeric value greater than 0');
end

if nargin<3 || isempty(fs)
    fs=1;
end

if ~isnumeric(fs) || fs<0
    error('Sampling frequency fs must be a numeric value greater than 0');
end

if ~isfield(param,'NFFT'),      param.NFFT = []; end
if ~isfield(param,'transform'), param.transform = 'dft'; end
if ~isfield(param,'approx'),    param.approx = 'forward'; end
if ~isfield(param,'extension'), param.extension = 0; end

%% JTV PARAMETERS
G.jtv = struct;
G.jtv.T = T;
G.jtv.fs = fs;
G.jtv.NFFT = param.NFFT;
G.jtv.transform = param.transform;
G.jtv.extension = param.extension;
G.jtv.omega = gsp_jtv_fa(G);

if G.jtv.extension
    G.jtv.lag = 2*G.jtv.T-1;
else
    G.jtv.lag = T;
end

%% DIFFERENTIAL OPERATORS
z = ones(T,1);

% G.jtv.LT = spdiags([-z 2*z -z], [-1 0 1], T, T);

G.jtv.DiffT = spdiags([-z z], [0 1], T, T);

switch param.transform
    case 'dft'
        G.jtv.DiffT(T,1) = 1;
    case 'dct'
        G.jtv.DiffT(T,T-1) = 1;
    otherwise
        error('Unknown transform');
end

G.jtv.LT = G.jtv.DiffT'*G.jtv.DiffT;

if strcmpi(param.approx,'backward')
    G.jtv.DiffT = -G.jtv.DiffT.';
end

% toeplitz([-1 zeros(T-2,1) 1],[-1 1 zeros(T-2,1)]); %periodic forward
% toeplitz([1 -1 zeros(T-2,1)],[1 zeros(T-2,1) -1]); %periodic backward (= -transpose(periodic forward))

end