function [ stack, W_t ] = initialize_weights( eI )
%INITIALIZE_WEIGHTS Random weight structures for a network architecture
%   eI describes an RNN via the fields layerSizes, inputDim and
%   temporalLayer
%
%   This uses Xavier's weight initialization tricks for better backprop
%   See: X. Glorot, Y. Bengio. Understanding the difficulty of training
%        deep feedforward neural networks. AISTATS 2010.

%% initialize hidden layers
stack = cell(1, numel(eI.layerSizes));
for l = 1 : numel(eI.layerSizes)
    if l > 1
        prevSize = eI.layerSizes(l-1);
    else
        prevSize = eI.inputDim;
    end;
    curSize = eI.layerSizes(l);
    % Xaxier's scaling factor
    s = sqrt(6) / sqrt(prevSize + curSize);
    % Ilya suggests smaller scaling for recurrent layer
    if l == eI.temporalLayer
        s = sqrt(6) / sqrt(prevSize + 2*curSize);
    end;
    stack{l}.W = rand(curSize, prevSize)*2*s - s;
    stack{l}.b = zeros(curSize, 1);
end
%% weight tying
% default weight tying to false
if ~isfield(eI, 'tieWeights')
    eI.tieWeights = 0;
end;
% overwrite decoder layers for tied weights
if eI.tieWeights
    decList = [(numel(eI.layerSizes)/2)+1 : numel(eI.layerSizes)-1];
    for l = 1:numel(decList)
        lDec = decList(l);
        lEnc = decList(1) - l;
        assert( norm(size(stack{lEnc}.W') - size(stack{lDec}.W)) == 0, ...
            'Layersizes dont match for tied weights');
        stack{lDec}.W = stack{lEnc}.W';
    end;
end;
%% initialize temporal weights if they should exist
W_t = [];
if eI.temporalLayer
    % assuems temporal init type set
    if strcmpi(eI.temporalInit, 'zero')
        W_t = zeros(eI.layerSizes(eI.temporalLayer));
    elseif strcmpi(eI.temporalInit, 'rand')
        % Ilya's modification to Xavier's update rule
        s = sqrt(6) / sqrt(3*eI.layerSizes(eI.temporalLayer));
        W_t = rand(eI.layerSizes(eI.temporalLayer))*2*s - s;
    elseif strcmpi(eI.temporalInit, 'eye')
        W_t = eye(eI.layerSizes(eI.temporalLayer));
    else
        error('unrecognized temporal initialization: %s', eI.temporalInit);
    end;
end;
%% init short circuit connections
% default short circuits to false
if ~isfield(eI, 'shortCircuit')
    eI.shortCircuit = 0;
end;
if eI.shortCircuit
    %padSize = (eI.winSize-1) / 2;
    %stack{end}.W_ss = [zeros(eI.featDim, padSize*eI.featDim), eye(eI.featDim),...
    %    zeros(eI.featDim, padSize*eI.featDim)];
    % use random init since input might contain noise estimate
    s = sqrt(6) / sqrt(eI.inputDim + eI.layerSizes(end));
    stack{end}.W_ss = rand(eI.inputDim, eI.layerSizes(end))*2*s - s;
end;


function op = smooth_logLLogistic(y)
% SMOOTH_LOGLLOGISTIC Log-likelihood function of a logistic: sum_i( y_i mu_i - log( 1+exp(mu_i) ) )
%   OP = SMOOTH_LOGLLOGISTIC( Y )
%   returns a function that computes the log-likelihood function
%   in a standard logistic regression model with independent entries. There
%   are two classes y_i = 0 and y_i = 1 with
%
%   prob(y_i = 1) = exp(mu_i)/(1 + exp(mu_i)
%
%   so that the log-likelihood is given by
%
%       log-likelihood(mu) = sum_i ( y_i mu_i - log(1+ exp(mu_i)) )
%
%   where mu is the parameter of the distribution (this is unknown,
%   so it is the variable), and Y is a vector of observations.

error(nargchk(1,1,nargin));
op = tfocs_smooth( @smooth_logLlogistic_impl);

function [ v, g ] = smooth_logLlogistic_impl( mu )

  if length(mu) == 1,
      mu = mu * ones(size(y));
  elseif size(mu) ~= size(y),
      error('Parameters and data must be of the same size'),
  end

  aux = 1 + exp(-abs(mu));
  v = tfocs_dot(y-1,mu.*(mu > 0)) ...
         + tfocs_dot(y,mu.*(mu < 0)) ...
              - tfocs_dot(ones(size(y)), log(aux));
  if nargout > 1,
      g = y - ((mu > 0) + (mu <= 0).*exp(mu))./aux;
  end
end

end

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function [y,coef,window,Cx,Ff] = lanczosfilter(x,varargin)
%LANCZOSFILTER   Filters a time series via Lanczos method (cosine filter).
%   [Y,coef,window,Cx,Ff] = LANCZOSFILTER(X,dT,Cf,M,pass) Filters the time
%   series via the Lanczos filter in the frequency space (FFT), where
%
%   INPUTS:
%      X    - Time series
%      dT   - Sampling interval       (default: 1)
%      Cf   - Cut-off frequency       (default: half Nyquist)
%      M    - Number of coefficients  (default: 100)
%      pass - Low or high-pass filter (default: 'low')
%
%   OUTPUTS:
%      Y      - Filtered time series
%      coef   - Coefficients of the time window (cosine)
%      window - Frequency window (aprox. ones for Ff lower(greater) than Fc
%               if low(high)-pass filter and ceros otherwise)
%      Cx     - Complex Fourier Transform of X for Ff frequencies
%      Ff     - Fourier frequencies, from 0 to the Nyquist frequency.
%
%   The program removes from the time series the frequencies greater than
%   the cut off frequency if "pass" is 'low', i.e., low-pass filter .
%   Otherwise, if pass is 'high', frequencies from zero to Cf are removed,
%   i.e., a high-pass filter. Units of the cut-off frequency, [Cf], must be
%   [dT]^{-1}.
%
%   In consequence, when used as a low-pass the time series is smoothed
%   like a cosine filter in time space with M coefficients where greater is
%   better (see the reference).
%
%   If any option is empty, defaults are used.
%
%   Note: NaN's elements are replaced by the mean of the time series and
%         ignored. If you have a better idea, just let me know.
%
%   Reference:
%   Emery, W. J. and R. E. Thomson. "Data Analysis Methods in Physical
%     Oceanography". Elsevier, 2d ed., 2004. On pages 533-539.
%
%   Example:
%      dT = 30; % min
%      N = 7*24*60/dT; t = (0:N-1)*dT; % data for 7 days
%      pnoise = 0.30;
%      T1 = 12.4*60; T2 = 24*60; T3 = 15*24*60; Tc = 10*60; % min
%      xn = 5 + 3*cos(2*pi*t/T1) + 2*cos(2*pi*t/T2) + 1*cos(2*pi*t/T3);
%      xn = xn + pnoise*max(xn-mean(xn))*(0.5 - rand(size(xn)));
%      [xs,c,h,Cx,f] = lanczosfilter(xn,dT,1/Tc,[],'low');
%      subplot(211), plot(t,xn,t,xs), legend('noisy','smooth'), axis tight
%      subplot(212), plot(f,h,f,abs(Cx)/max(abs(Cx)),...
%         [1 1]/Tc,[min(h) max(h)],'-.',...
%         [1/T1 1/T2 1/T3],([1/T1 1/T2 1/T3]<=1/Tc),'o'), axis tight
%
%   See also FILTER, FFT, IFFT

%   Written by
%   Lic. on Physics Carlos Adrián Vargas Aguilera
%   Physical Oceanography MS candidate
%   UNIVERSIDAD DE GUADALAJARA
%   Mexico, 2004
%
%   nubeobscura@hotmail.com

% Check arguments:
if nargin<1 || nargin>5
 error('Lanczosfilter:ArgumentNumber','Incorrect number of arguments.')
elseif ~isvector(x) || ~isreal(x)
 error('Lanczosfilter:ArgumentType','Incorrect time series.')
end
if nargin<2 || isempty(varargin{1})
  dT = 1;
elseif ~(numel(varargin{1})==1) || ~isreal(varargin{1})
 error('Lanczosfilter:ArgumentType','Incorrect time interval.')
else
 dT = varargin{1};
end
Nf = 1/(2*dT); % Nyquist frequency
if nargin<3 || isempty(varargin{2})
 Cf = Nf/2;
elseif ~(numel(varargin{2})==1) || ~isreal(varargin{2}) || varargin{2}<=0 || varargin{2}>Nf
 error('Lanczosfilter:ArgumentType','Incorrect cut-off frequency.')
else
 Cf = varargin{2};
end
if nargin<4 || isempty(varargin{3})
 M = 100;
elseif ~(numel(varargin{3})==1) || ~isreal(varargin{3}) || (varargin{3}==round(varargin{3}))
 error('Lanczosfilter:ArgumentType','Incorrect Number of coeffients.')
else
 M = varargin{3};
end
if nargin<5 || isempty(varargin{4})
 LoH = 'l';
elseif ~ischar(varargin{4}) || isempty(strfind('lh',lower(varargin{4}(1))))
 error('Lanczosfilter:ArgumentType','Incorrect filter pass type.')
else
 LoH = varargin{4};
end
if strcmpi(LoH(1),'h')
 LoH = 2;
else
 LoH = 1;
end

% Normalize the cut off frequency with the Nyquist frequency:
Cf = Cf/Nf;

% Lanczos cosine coeficients:
coef = lanczos_filter_coef(Cf,M); coef = coef(:,LoH);

% Filter in frequency space:
[window,Ff] = spectral_window(coef,length(x)); Ff = Ff*Nf;

% Replace NaN's with the mean (ideas?):
inan = isnan(x);
xmean = mean(x(~inan));
x(inan) = xmean;

% Filtering:
[y,Cx] = spectral_filtering(x,window);

function coef = lanczos_filter_coef(Cf,M)
% Positive coeficients of Lanczos [low high]-pass.
hkcs = lowpass_cosine_filter_coef(Cf,M);
sigma = [1 sin(pi*(1:M)/M)./(pi*(1:M)/M)];
hkB = hkcs.*sigma;
hkA = -hkB; hkA(1) = hkA(1)+1;
coef = [hkB(:) hkA(:)];

function coef = lowpass_cosine_filter_coef(Cf,M)
% Positive coeficients of cosine filter low-pass.
coef = Cf*[1 sin(pi*(1:M)*Cf)./(pi*(1:M)*Cf)];

function [window,Ff] = spectral_window(coef,N)
% Window of cosine filter in frequency space.
Ff = 0:2/N:1; window = zeros(length(Ff),1);
for i = 1:length(Ff)
 window(i) = coef(1) + 2*sum(coef(2:end).*cos((1:length(coef)-1)'*pi*Ff(i)));
end

function [y,Cx] = spectral_filtering(x,window)
% Filtering in frequency space is multiplication, (convolution in time
% space).
Nx  = length(x);
Cx  = fft(x(:)); Cx = Cx(1:floor(Nx/2)+1);
CxH = Cx.*window(:);
CxH(length(CxH)+1:Nx) = conj(CxH(Nx-length(CxH)+1:-1:2));
y = real(ifft(CxH));

% Carlos Adrián Vargas Aguilera. nubeobscura@hotmail.com


function [rhsu] = HeatLDGRHS1D(u,time)

% function [rhsu] = HeatLDGRHS1D(u,time,a,ax)
% Purpose  : Evaluate RHS flux in 1D heat equation using an LDG flux

Globals1D;

% Define field differences at faces
du = zeros(Nfp*Nfaces,K); du(:) = (1.0+nx(:)).*(u(vmapM)-u(vmapP))/2.0;

% impose boundary condition -- Dirichlet BC's
%uin  = -u(vmapI); du(mapI) = (1.0+nx(mapI)).*(u(vmapI)- uin)/2.0;
%uout = -u(vmapO); du(mapO) = (1.0+nx(mapO)).*(u(vmapO)-uout)/2.0;
uin  = 0.1; du(mapI) = (1.0+nx(mapI)).*(u(vmapI)- uin)/2.0;
uout = 0.1; du(mapO) = (1.0+nx(mapO)).*(u(vmapO)-uout)/2.0;

% Compute q
q = rx.*(Dr*u) - LIFT*(Fscale.*(nx.*du));
dq = zeros(Nfp*Nfaces,K); dq(:) = (1.0-nx(:)).*(q(vmapM)-q(vmapP))/2.0;

% impose boundary condition -- Neumann BC's
%qin  = q(vmapI); dq(mapI) = (1.0-nx(mapI)).*(q(vmapI)- qin)/2.0;
%qout = q(vmapO); dq(mapO) = (1.0-nx(mapO)).*(q(vmapO)-qout)/2.0;
dq(mapI)=0;
dq(mapO)=0;

% compute right hand sides of the semi-discrete PDE
rhsu = rx.*(Dr*q) - LIFT*(Fscale.*(nx.*dq));
return


function alpha = meshDihedralAngles(vertices, edges, faces)
%MESHDIHEDRALANGLES Dihedral at edges of a polyhedal mesh.
%
%   ALPHA = meshDihedralAngles(V, E, F)
%   where V, E and F represent vertices, edges and faces of a mesh,
%   computes the dihedral angle between the two adjacent faces of each edge
%   in the mesh. ALPHA is a column array with as many rows as the number of
%   edges. The i-th element of ALPHA corresponds to the i-th edge.
%
%   Note: the function assumes that the faces are correctly oriented. The
%   face vertices should be indexed counter-clockwise when considering the
%   supporting plane of the face, with the outer normal oriented outwards
%   of the mesh.
%
%   Example
%   [v, e, f] = createCube;
%   rad2deg(meshDihedralAngles(v, e, f))
%   ans =
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%       90
%
%   See also
%   meshes3d, polyhedronMeanBreadth, trimeshMeanBreadth, dihedralAngle, meshEdgeFaces
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-10-04,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% compute normal of each face
normals = meshFaceNormals(vertices, faces);

% indices of faces adjacent to each edge
edgeFaces = meshEdgeFaces(vertices, edges, faces);

% allocate memory for resulting angles
Ne = size(edges, 1);
alpha = zeros(Ne, 1);

% iterate over edges
for i = 1:Ne
    % indices of adjacent faces
    indFace1 = edgeFaces(i, 1);
    indFace2 = edgeFaces(i, 2);

    % normal vector of adjacent faces
    normal1 = normals(indFace1, :);
    normal2 = normals(indFace2, :);

    % compute dihedral angle of two vectors
    alpha(i) = vectorAngle3d(normal1, normal2);
end


function lattice_rule_test085 ( )

%*****************************************************************************80
%
%% LATTICE_RULE_TEST085 tests LATTICE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 November 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ian Sloan, Stephen Joe,
%    Lattice Methods for Multiple Integration,
%    Oxford, 1994, page 18.
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LATTICE_RULE_TEST085\n' );
  fprintf ( 1, '  LATTICE is a lattice rule for periodic functions.\n' );
  fprintf ( 1, '  However, we apply it to a nonperiodic function\n' );
  fprintf ( 1, '  just to see how it does.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The spatial dimension DIM_NUM = %d\n', dim_num );

  a(1:dim_num) = 0.0;
  b(1:dim_num) = 1.0;

  exact = e_01_2d ( dim_num, a, b );

  z(1:dim_num) = [ 1, 2 ];
  i4vec_print ( dim_num, z, '  The lattice generator vector:' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '         K         M    EXACT       ESTIMATE    ERROR\n' );
  fprintf ( 1, '\n' );

  for k = 3 : 18

    m = fibonacci ( k );

    quad = lattice ( dim_num, m, z, @f_01_2d );

    error = abs ( exact - quad );

    fprintf ( 1, '  %8d  %8d  %10.6f  %10.6f  %10.6e\n', k, m, exact, quad, error );

  end

  return
end


function [THD_U,THD_I,K_U,K_I,MaxHarm_I,MaxIHarm_I,MaxHFHarm_I,MaxHarm_U,MaxIHarm_U,MaxHFHarm_U,Order_Harm,Order_IHarm,Order_HFHarm] = IEC_Harmonics_final(f,Vn,Inom,U1,U2,U3,I1,I2,I3,U1_time,U2_time,U3_time,I1_time,I2_time,I3_time,figures)
% This function will calculate the Harmonics spectrum values according to
% the IEC 61000-4-7 Norm which spesify the need for 10 min Voltage and
% current measured signals of the system under study and calulate the
% Harmonics mean values every 10 Periods of the measured signal (In this case 50 Hz signal)
% This Fucntion inputs are:
%           1- The nominal values of the measured signal: Frequency,
%           Voltage, and the current. For example f = 50%Hz
%                                                 Vn = 400%V (Phase to Phase Voltage of the grid)
%                                                 Inom = 475%A (The nominal
%                                                 current of the system under measurement)
%           2- The measured Voltages and current signals with there time
%           measurement signals (U1,U2,U3,I1,I2,I3,U1_time,U2_time,U3_time,I1_time,I2_time,I3_time)
%           3- figures is an input of 1 or 0 in order to activate showing the figures of the different  harmonics spectrums figures calculated in this function.
%PS: This function assume that you have the same sampling rate of the
%different measured signals and that all measured signals has the same
%length.
Fn = f;
Un = Vn;
In = Inom;
Delta_Time_Orginal = I1_time(2)-I1_time(1);
F_Sample = abs(1/Delta_Time_Orginal);
%------------------------------------------------------------------------------------------------------
%---------------------- The Harmonics Spectrum of the complet input signal ----------------------------
%------------------------------------------------------------------------------------------------------
Tp = length(I3_time);
T_S = abs(Tp*Delta_Time_Orginal);
[nyquist_number_I1,Bezugsfrequenz_I1,Signal_amp_I1] = Fast_Harmonics (I1,F_Sample,T_S);
[nyquist_number_I2,Bezugsfrequenz_I2,Signal_amp_I2] = Fast_Harmonics (I2,F_Sample,T_S);
[nyquist_number_I3,Bezugsfrequenz_I3,Signal_amp_I3] = Fast_Harmonics (I3,F_Sample,T_S);
[nyquist_number_U1,Bezugsfrequenz_U1,Signal_amp_U1] = Fast_Harmonics (U1,F_Sample,T_S);
[nyquist_number_U2,Bezugsfrequenz_U2,Signal_amp_U2] = Fast_Harmonics (U2,F_Sample,T_S);
[nyquist_number_U3,Bezugsfrequenz_U3,Signal_amp_U3] = Fast_Harmonics (U3,F_Sample,T_S);
if figures
    figure;
    subplot(2,1,1);
    plot([0:nyquist_number_I1]*Bezugsfrequenz_I1,Signal_amp_I1);ylabel('amplitude Currents [A]');title('Harmonic spectrom');hold on
    plot([0:nyquist_number_I2]*Bezugsfrequenz_I2,Signal_amp_I2,'-.r');hold on
    plot([0:nyquist_number_I3]*Bezugsfrequenz_I3,Signal_amp_I3,'--k');hold off
    subplot(2,1,2);
    plot([0:nyquist_number_U1]*Bezugsfrequenz_U1,Signal_amp_U1);xlabel('frequency [Hz]'),ylabel('amplitude Voltages [V]');hold on
    plot([0:nyquist_number_U2]*Bezugsfrequenz_U2,Signal_amp_U2,'-.r');hold on
    plot([0:nyquist_number_U3]*Bezugsfrequenz_U3,Signal_amp_U3,'.k','MarkerSize',0.1);hold off
end
%**********************************************************************
%---------------------- Creating a grouping matrix --------------------
%**********************************************************************
Sampling_Time = Delta_Time_Orginal;
T_10_period = 10/Fn;
F_Sample_Orginal = 1/Sampling_Time;
if abs(F_Sample_Orginal) < 20000
    F_Sample_Complete_Data = 20000;
    [p, q] = rat(abs(Delta_Time_Orginal/(1/F_Sample_Complete_Data)));
    I1_Resampled = resample(I1,p,q);
    I2_Resampled = resample(I2,p,q);
    I3_Resampled = resample(I3,p,q);
    U1_Resampled = resample(U1,p,q);
    U2_Resampled = resample(U2,p,q);
    U3_Resampled = resample(U3,p,q);
    t_P = I1_time(1):((q/p)/F_Sample_Orginal):(I1_time(end)+((q/p)/F_Sample_Orginal));
    tt = length(t_P);
    if length(t_P) == length(I1_Resampled)
        I1_Resampled_time = t_P;
        I2_Resampled_time = t_P;
        I3_Resampled_time = t_P;
        U1_Resampled_time  = t_P;
        U2_Resampled_time  = t_P;
        U3_Resampled_time  = t_P;
    else if length(t_P) < length(I1_Resampled)
            for kk = 1:abs(length(I1_Resampled)-length(t_P))
                t_P(tt+kk) = ((q/p)/F_Sample_Orginal)+t_P(tt+kk-1);
            end
            I1_Resampled_time = t_P;
            I2_Resampled_time = t_P;
            I3_Resampled_time = t_P;
            U1_Resampled_time  = t_P;
            U2_Resampled_time  = t_P;
            U3_Resampled_time  = t_P;
        elseif length(t_P) > length(I1_Resampled)
            t_P = t_P(1:length(I1_Resample));
            I1_Resampled_time = t_P;
            I2_Resampled_time = t_P;
            I3_Resampled_time = t_P;
            U1_Resampled_time  = t_P;
            U2_Resampled_time  = t_P;
            U3_Resampled_time  = t_P;
        end
    end
    Delta_Time_Complete_Data = 1/F_Sample_Complete_Data;
    Sampling_Time_New = Delta_Time_Complete_Data;
    Sampling_frq_New = (1/Sampling_Time_New)/1000;
    Sample_New = T_10_period*Sampling_frq_New*1000;
    ii= 1;
    Points = 2^ii;
    while Points < Sample_New
        Points = 2^ii;
        ii = ii+1;
    end
else if abs(F_Sample_Orginal) > 20000
        F_Sample_Complete_Data = 20000;
        [p, q] = rat(abs(Delta_Time_Orginal/(1/F_Sample_Complete_Data)));
        I1_Resampled = resample(I1,p,q);
        I2_Resampled = resample(I2,p,q);
        I3_Resampled = resample(I3,p,q);
        U1_Resampled = resample(U1,p,q);
        U2_Resampled = resample(U2,p,q);
        U3_Resampled = resample(U3,p,q);
        t_Q = I1_time(1):((q/p)/F_Sample):I1_time(end);
        I1_Resampled_time = t_Q;
        I2_Resampled_time = t_Q;
        I3_Resampled_time = t_Q;
        U1_Resampled_time  = t_Q;
        U2_Resampled_time  = t_Q;
        U3_Resampled_time  = t_Q;
        Delta_Time_Complete_Data = 1/F_Sample_Complete_Data;
        %Sampling_Time_New = Delta_Time_Complete_Data;
        Sampling_frq_New = (1/Sampling_Time_Complete_Data)/1000;
        Sample_New = T_10_period*Sampling_frq_New*1000;
        ii= 1;
        Points = 2^ii;
        while Points < Sample_New
            Points = 2^ii;
            ii = ii+1;
        end
    else
        I1_Resampled = I1;
        I2_Resampled = I2;
        I3_Resampled = I3;
        U1_Resampled = U1;
        U2_Resampled = U2;
        U3_Resampled = U3;
        I1_Resampled_time = I1_time;
        I2_Resampled_time = I2_time;
        I3_Resampled_time = I3_time;
        U1_Resampled_time  = U1_time;
        U2_Resampled_time  = U2_time;
        U3_Resampled_time  = U3_time;
        F_Sample_Complete_Data = 20000;
        Delta_Time_Complete_Data = I1_Resampled_time(2)-I1_Resampled_time(1);
        Sampling_frq = 1/Delta_Time_Complete_Data;
        T_10_period = 10/Fn;
        Sample = T_10_period*Sampling_frq;
        ii= 1;
        Points = 2^ii;
        while Points < Sample
            Points = 2^ii;
            ii = ii+1;
        end
    end
end
%----------------------------------------------------------------------
%--------------- Zeros Results Array Definition -----------------------
%----------------------------------------------------------------------
%--------------- Try --------------------
LOWPASS_CUTOFF = 100;% Filter cutoff frequency is 100 Hz
LOWPASS_ORDER = 4;% Filter order is 4
[b_bw, a_bw] = butter(LOWPASS_ORDER, LOWPASS_CUTOFF / (F_Sample_Complete_Data / 2), 'low');
Filter = filter(b_bw, a_bw, U1_Resampled);
%--------------- End --------------------
null_L1 = find(diff(Filter < 0));
null_L1_x = Zeros_finding(Filter,I1_Resampled_time');
Tp = length(U1_Resampled_time);
len = length(null_L1_x);
%len_1_period = (1/Fn)/Delta_Time_Complete_Data;
%len_10_period = (10*(1/Fn))/Delta_Time_Complete_Data;
Results_U_H1 = zeros(50,length(0:20:len-30));
Results_U_H2 = zeros(50,length(0:20:len-30));
Results_U_H3 = zeros(50,length(0:20:len-30));
Results_U_IH1 = zeros(39,length(0:20:len-30));
Results_U_IH2 = zeros(39,length(0:20:len-30));
Results_U_IH3 = zeros(39,length(0:20:len-30));
Results_U_HF1 = zeros(35,length(0:20:len-30));
Results_U_HF2 = zeros(35,length(0:20:len-30));
Results_U_HF3 = zeros(35,length(0:20:len-30));
Results_P = zeros(length(0:20:len-30));
Results_Q = zeros(length(0:20:len-30));
%----------------------------------------------------------------------
%--------------------------- End --------------------------------------
%----------------------------------------------------------------------
%**********************************************************************
%----------------------------------END---------------------------------
%**********************************************************************
%^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
%**********************************************************************
%--------------------------- Gruoping ---------------------------------
%**********************************************************************
j = 0;
jj = 1;
while j < len-30 % t < t2+0.1
    data_I1 = I1_Resampled(null_L1(j+3):null_L1(j+23)+1);
    data_I2 = I2_Resampled(null_L1(j+3):null_L1(j+23)+1);
    data_I3 = I3_Resampled(null_L1(j+3):null_L1(j+23)+1);
    data_U1 = U1_Resampled(null_L1(j+3):null_L1(j+23)+1);
    data_U2 = U2_Resampled(null_L1(j+3):null_L1(j+23)+1);
    data_U3 = U3_Resampled(null_L1(j+3):null_L1(j+23)+1);
    data_I1_time = I1_Resampled_time(null_L1(j+3):null_L1(j+23)+1);
    %------------------- Cut End --------------------------------------
    %------------------------------------------------------------------
    %--------- Data Resampling with the new Delta_10_Periods ----------
    %------------------------------------------------------------------
    delta = (null_L1_x(j+23)-null_L1_x(j+3))/Points;
    %Rampe = null_L1_x(j+3):delta:(((Points-1)*delta)+null_L1_x(j+3));
    iii=2;
    Rampe = zeros();
    Rampe(1) = null_L1_x(j+3);
    while iii <= Points
        Rampe(iii) = delta+Rampe(iii-1);
        iii = iii+1;
    end
    F_Sample_10_Periods = 1/delta;
    if abs(F_Sample_10_Periods) < F_Sample_Complete_Data
        F_Sample_New = F_Sample_10_Periods;
        [p, q] = rat(abs(Delta_Time/(1/F_Sample_New)));
        I1_Resampled_10_Period = resample(data_I1,p,q);
        I2_Resampled_10_Period = resample(data_I2,p,q);
        I3_Resampled_10_Period = resample(data_I3,p,q);
        U1_Resampled_10_Period = resample(data_U1,p,q);
        U2_Resampled_10_Period = resample(data_U2,p,q);
        U3_Resampled_10_Period = resample(data_U3,p,q);
        t_P = data_I1_time(1):(1/F_Sample_New):(data_I1_time(end)+(1/F_Sample_New));
        I1_Resampled_time_10_Period = t_P;
        I2_Resampled_time_10_Period = t_P;
        I3_Resampled_time_10_Period = t_P;
        U1_Resampled_time_10_Period  = t_P;
        U2_Resampled_time_10_Period  = t_P;
        U3_Resampled_time_10_Period  = t_P;
    else if abs(F_Sample_10_Periods) > F_Sample_Complete_Data
            F_Sample_New = F_Sample_10_Periods;
            [p, q] = rat(abs(Delta_Time_Complete_Data/(1/F_Sample_New)));
            I1_Resampled_10_Period = resample(data_I1,p,q);
            I2_Resampled_10_Period = resample(data_I2,p,q);
            I3_Resampled_10_Period = resample(data_I3,p,q);
            U1_Resampled_10_Period = resample(data_U1,p,q);
            U2_Resampled_10_Period = resample(data_U2,p,q);
            U3_Resampled_10_Period = resample(data_U3,p,q);
            for kk=1:length(I1_Resampled_10_Period)-1
                I1_Resample_10_Period_Final(kk) =  ((I1_Resampled_10_Period(kk)+I1_Resampled_10_Period(kk+1))/2);
                I2_Resample_10_Period_Final(kk) =  ((I2_Resampled_10_Period(kk)+I2_Resampled_10_Period(kk+1))/2);
                I3_Resample_10_Period_Final(kk) =  ((I3_Resampled_10_Period(kk)+I3_Resampled_10_Period(kk+1))/2);
                U1_Resample_10_Period_Final(kk) =  ((U1_Resampled_10_Period(kk)+U1_Resampled_10_Period(kk+1))/2);
                U2_Resample_10_Period_Final(kk) =  ((U2_Resampled_10_Period(kk)+U2_Resampled_10_Period(kk+1))/2);
                U3_Resample_10_Period_Final(kk) =  ((U3_Resampled_10_Period(kk)+U3_Resampled_10_Period(kk+1))/2);
            end
            if length(I1_Resample_10_Period_Final) > length(Rampe)
                I1_Resample_10_Period_Final = I1_Resample_10_Period_Final(1:length(Rampe));
                I2_Resample_10_Period_Final = I2_Resample_10_Period_Final(1:length(Rampe));
                I3_Resample_10_Period_Final = I3_Resample_10_Period_Final(1:length(Rampe));
            else if length(I1_Resample_10_Period_Final) < length(Rampe)
                    error('Please check the Resampled data something wrong there :(')
                end
            end
            t_Q = Rampe(1):((q/p)/F_Sample_Complete_Data):Rampe(end);
            if length(t_Q) == length(I1_Resampled_10_Period)
                I1_Resampled_time_10_Period = t_Q;
                I2_Resampled_time_10_Period = t_Q;
                I3_Resampled_time_10_Period = t_Q;
                U1_Resampled_time_10_Period  = t_Q;
                U2_Resampled_time_10_Period  = t_Q;
                U3_Resampled_time_10_Period  = t_Q;
            else if length(t_Q) > length(I1_Resampled_10_Period)
                    I1_Resampled_time_10_Period = t_Q(1:length(I1_Resampled_10_Period));
                    I2_Resampled_time_10_Period = t_Q(1:length(I2_Resampled_10_Period));
                    I3_Resampled_time_10_Period = t_Q(1:length(I3_Resampled_10_Period));
                    U1_Resampled_time_10_Period = t_Q(1:length(U1_Resampled_10_Period));
                    U2_Resampled_time_10_Period = t_Q(1:length(U2_Resampled_10_Period));
                    U3_Resampled_time_10_Period = t_Q(1:length(U3_Resampled_10_Period));
                else if length(t_Q) < length(I1_Resampled_10_Period)
                        I1_Resampled_time_10_Period = [t_Q t_Q(end)+((q/p)/F_Sample_Complete_Data)];
                        I2_Resampled_time_10_Period = [t_Q t_Q(end)+((q/p)/F_Sample_Complete_Data)];
                        I3_Resampled_time_10_Period = [t_Q t_Q(end)+((q/p)/F_Sample_Complete_Data)];
                        U1_Resampled_time_10_Period = [t_Q t_Q(end)+((q/p)/F_Sample_Complete_Data)];
                        U2_Resampled_time_10_Period = [t_Q t_Q(end)+((q/p)/F_Sample_Complete_Data)];
                        U3_Resampled_time_10_Period = [t_Q t_Q(end)+((q/p)/F_Sample_Complete_Data)];
                    end
                end
            end
        else
            error('Please check the Delta Value of the 10 Period signal')
        end
    end
    %------------------------------------------------------------------
    %---------------- Data Re-sampling End ----------------------------
    %------------------------------------------------------------------
    %----------------Spectrum Calculation------------------------------
    %------------------------------------------------------------------
    Tp_10_Period_Resampled = length(I1_Resampled_time_10_Period);
    Delta_Time_10_Period_Resampled = I1_Resampled_time_10_Period(2)-I1_Resampled_time_10_Period(3);
    F_Sample_10_Period_Resampled = abs(1/Delta_Time_10_Period_Resampled);
    T_S_10_Period_Resampled = abs(Tp*Delta_Time_10_Period_Resampled);
    %----------------------------------------------------------------------
    [nyquist_number_I1_Resampled_10_Period,Bezugsfrequenz_I1_Resampled_10_Period,Signal_amp_I1_Resampled_10_Period] = Fast_Harmonics_1 (I1_Resample_10_Period_Final,F_Sample_10_Period_Resampled,T_S_10_Period_Resampled);
    [nyquist_number_I2_Resampled_10_Period,Bezugsfrequenz_I2_Resampled_10_Period,Signal_amp_I2_Resampled_10_Period] = Fast_Harmonics_1 (I2_Resample_10_Period_Final,F_Sample_10_Period_Resampled,T_S_10_Period_Resampled);
    [nyquist_number_I3_Resampled_10_Period,Bezugsfrequenz_I3_Resampled_10_Period,Signal_amp_I3_Resampled_10_Period] = Fast_Harmonics_1 (I3_Resample_10_Period_Final,F_Sample_10_Period_Resampled,T_S_10_Period_Resampled);
    [nyquist_number_U1_Resampled_10_Period,Bezugsfrequenz_U1_Resampled_10_Period,Signal_amp_U1_Resampled_10_Period] = Fast_Harmonics_1 (U1_Resample_10_Period_Final,F_Sample_10_Period_Resampled,T_S_10_Period_Resampled);
    [nyquist_number_U2_Resampled_10_Period,Bezugsfrequenz_U2_Resampled_10_Period,Signal_amp_U2_Resampled_10_Period] = Fast_Harmonics_1 (U2_Resample_10_Period_Final,F_Sample_10_Period_Resampled,T_S_10_Period_Resampled);
    [nyquist_number_U3_Resampled_10_Period,Bezugsfrequenz_U3_Resampled_10_Period,Signal_amp_U3_Resampled_10_Period] = Fast_Harmonics_1 (U3_Resample_10_Period_Final,F_Sample_10_Period_Resampled,T_S_10_Period_Resampled);
    Figure_Show = 0;
    if Figure_Show == 1
        figure;
        subplot(2,1,1);
        plot([0:nyquist_number_data_I1]*Bezugsfrequenz_data_I1,Signal_amp_data_I1);ylabel('amplitude Currents [A]');title('Harmonic spectrom');hold on
        plot([0:nyquist_number_data_I2]*Bezugsfrequenz_data_I2,Signal_amp_data_I2,'-.r');hold on
        plot([0:nyquist_number_data_I3]*Bezugsfrequenz_data_I3,Signal_amp_data_I3,'--k');hold off
        subplot(2,1,2);
        plot([0:nyquist_number_data_U1]*Bezugsfrequenz_data_U1,Signal_amp_data_U1);xlabel('frequency [Hz]'),ylabel('amplitude Voltages [A]');hold on
        plot([0:nyquist_number_data_U2]*Bezugsfrequenz_data_U2,Signal_amp_data_U2,'-.r');hold on
        plot([0:nyquist_number_data_U3]*Bezugsfrequenz_data_U3,Signal_amp_data_U3,'.k','MarkerSize',0.1);hold off
    end
    % Grouping U1
    data_specM_x_H = [0:nyquist_number_U1_Resampled_10_Period]*Bezugsfrequenz_U1_Resampled_10_Period;
    deta_specM_x_V = data_specM_x_H';
    data_specM_Y_V = Signal_amp_U1_Resampled_10_Period';
    data_specM = data_specM_Y_V;
    i = 11;
    ii = 1;
    while i < 502
        harm_calc = sqrt(power(data_specM(i-1),2)+power(data_specM(i),2)+power(data_specM(i+1),2));
        iTxt = (i-1)/10;
        Results_U_H1(ii,jj) = harm_calc;
        if i < 392
            iharm_calc = sqrt(power(data_specM(i+2),2)+power(data_specM(i+3),2)+power(data_specM(i+4),2)+power(data_specM(i+5),2)+power(data_specM(i+6),2)+power(data_specM(i+7),2)+power(data_specM(i+8),2));
            Results_U_IH1(ii,jj) = iharm_calc;
        end
        if i <= 360
            j1 = (i-1)*4+360+1;
            j_end = j1+40;
            hfharm_calc = 0;
            while j1 <= j_end
                hfharm_calc = hfharm_calc+power(data_specM(j1),2);
                j1 =j1+1;
            end
            hfharm_calc = sqrt(hfharm_calc);
            j1 = 200*(i-1)/10+1900;
            Results_U_HF1(ii,jj) = hfharm_calc;
        end
        i = i+10;
        ii = ii+1;
    end
    % Grouping U2
    data_specM_x_H = [0:nyquist_number_U2_Resampled_10_Period]*Bezugsfrequenz_U2_Resampled_10_Period;
    deta_specM_x_V = data_specM_x_H';
    data_specM_Y_V = Signal_amp_U2_Resampled_10_Period';
    data_specM = data_specM_Y_V;
    i = 11;
    ii = 1;
    while i < 502
        harm_calc = sqrt(power(data_specM(i-1),2)+power(data_specM(i),2)+power(data_specM(i+1),2));
        iTxt = (i-1)/10;
        Results_U_H2(ii,jj) = harm_calc;
        if i < 392
            iharm_calc = sqrt(power(data_specM(i+2),2)+power(data_specM(i+3),2)+power(data_specM(i+4),2)+power(data_specM(i+5),2)+power(data_specM(i+6),2)+power(data_specM(i+7),2)+power(data_specM(i+8),2));
            Results_U_IH2(ii,jj) = iharm_calc;
        end
        if i <= 360
            j1 = (i-1)*4+360+1;
            j_end = j1+40;
            hfharm_calc = 0;
            while j1 <= j_end
                hfharm_calc = hfharm_calc+power(data_specM(j1),2);
                j1 =j1+1;
            end
            hfharm_calc = sqrt(hfharm_calc);
            j1 = 200*(i-1)/10+1900;
            Results_U_HF2(ii,jj) = hfharm_calc;
        end
        i = i+10;
        ii = ii+1;
    end
    % Grouping U3
    data_specM_x_H = [0:nyquist_number_U3_Resampled_10_Period]*Bezugsfrequenz_U3_Resampled_10_Period;
    deta_specM_x_V = data_specM_x_H';
    data_specM_Y_V = Signal_amp_U3_Resampled_10_Period';
    data_specM = data_specM_Y_V;
    i = 11;
    ii = 1;
    while i < 502
        harm_calc = sqrt(power(data_specM(i-1),2)+power(data_specM(i),2)+power(data_specM(i+1),2));
        iTxt = (i-1)/10;
        Results_U_H3(ii,jj) = harm_calc;
        if i < 392
            iharm_calc = sqrt(power(data_specM(i+2),2)+power(data_specM(i+3),2)+power(data_specM(i+4),2)+power(data_specM(i+5),2)+power(data_specM(i+6),2)+power(data_specM(i+7),2)+power(data_specM(i+8),2));
            Results_U_IH3(ii,jj) = iharm_calc;
        end
        if i <= 360
            j1 = (i-1)*4+360+1;
            j_end = j1+40;
            hfharm_calc = 0;
            while j1 <= j_end
                hfharm_calc = hfharm_calc+power(data_specM(j1),2);
                j1 =j1+1;
            end
            hfharm_calc = sqrt(hfharm_calc);
            j1 = 200*(i-1)/10+1900;
            Results_U_HF3(ii,jj) = hfharm_calc;
        end
        i = i+10;
        ii = ii+1;
    end
    % Grouping I1
    data_specM_x_H = [0:nyquist_number_I1_Resampled_10_Period]*Bezugsfrequenz_I1_Resampled_10_Period;
    %deta_specM_x_V = data_specM_x_H';
    data_specM_Y_V = Signal_amp_I1_Resampled_10_Period';
    data_specM = data_specM_Y_V;
    i = 11;
    ii = 1;
    while i < 502
        harm_calc = sqrt(power(data_specM(i-1),2)+power(data_specM(i),2)+power(data_specM(i+1),2));
        iTxt = (i-1)/10;
        Results_I_H1(ii,jj) = harm_calc;
        if i < 392
            iharm_calc = sqrt(power(data_specM(i+2),2)+power(data_specM(i+3),2)+power(data_specM(i+4),2)+power(data_specM(i+5),2)+power(data_specM(i+6),2)+power(data_specM(i+7),2)+power(data_specM(i+8),2));
            Results_I_IH1(ii,jj) = iharm_calc;
        end
        if i <= 360
            j1 = (i-1)*4+360+1;
            j_end = j1+40;
            hfharm_calc = 0;
            while j1 <= j_end
                hfharm_calc = hfharm_calc+power(data_specM(j1),2);
                j1 =j1+1;
            end
            hfharm_calc = sqrt(hfharm_calc);
            j1 = 200*(i-1)/10+1900;
            Results_I_HF1(ii,jj) = hfharm_calc;
        end
        i = i+10;
        ii = ii+1;
    end
    % Grouping I2
    data_specM_x_H = [0:nyquist_number_I2_Resampled_10_Period]*Bezugsfrequenz_I2_Resampled_10_Period;
    %deta_specM_x_V = data_specM_x_H';
    data_specM_Y_V = Signal_amp_I2_Resampled_10_Period';
    data_specM = data_specM_Y_V;
    i = 11;
    ii = 1;
    while i < 502
        harm_calc = sqrt(power(data_specM(i-1),2)+power(data_specM(i),2)+power(data_specM(i+1),2));
        iTxt = (i-1)/10;
        Results_I_H2(ii,jj) = harm_calc;
        if i < 392
            iharm_calc = sqrt(power(data_specM(i+2),2)+power(data_specM(i+3),2)+power(data_specM(i+4),2)+power(data_specM(i+5),2)+power(data_specM(i+6),2)+power(data_specM(i+7),2)+power(data_specM(i+8),2));
            Results_I_IH2(ii,jj) = iharm_calc;
        end
        if i <= 360
            j1 = (i-1)*4+360+1;
            j_end = j1+40;
            hfharm_calc = 0;
            while j1 <= j_end
                hfharm_calc = hfharm_calc+power(data_specM(j1),2);
                j1 =j1+1;
            end
            hfharm_calc = sqrt(hfharm_calc);
            j1 = 200*(i-1)/10+1900;
            Results_I_HF2(ii,jj) = hfharm_calc;
        end
        i = i+10;
        ii = ii+1;
    end
    % Grouping I3
    data_specM_x_H = [0:nyquist_number_I3_Resampled_10_Period]*Bezugsfrequenz_I3_Resampled_10_Period;
    %deta_specM_x_V = data_specM_x_H';
    data_specM_Y_V = Signal_amp_I3_Resampled_10_Period';
    data_specM = data_specM_Y_V;
    i = 11;
    ii = 1;
    while i < 502
        harm_calc = sqrt(power(data_specM(i-1),2)+power(data_specM(i),2)+power(data_specM(i+1),2));
        iTxt = (i-1)/10;
        Results_I_H3(ii,jj) = harm_calc;
        if i < 392
            iharm_calc = sqrt(power(data_specM(i+2),2)+power(data_specM(i+3),2)+power(data_specM(i+4),2)+power(data_specM(i+5),2)+power(data_specM(i+6),2)+power(data_specM(i+7),2)+power(data_specM(i+8),2));
            Results_I_IH3(ii,jj) = iharm_calc;
        end
        if i <= 360
            j1 = (i-1)*4+360+1;
            j_end = j1+40;
            hfharm_calc = 0;
            while j1 <= j_end
                hfharm_calc = hfharm_calc+power(data_specM(j1),2);
                j1 =j1+1;
            end
            hfharm_calc = sqrt(hfharm_calc);
            j1 = 200*(i-1)/10+1900;
            Results_I_HF3(ii,jj) = hfharm_calc;
        end
        i = i+10;
        ii = ii+1;
    end
    if 0 == 1
        v_alpha = sqrt(2)/3*(data_U1-0.5*data_U2-0.5*data_U3);
        v_beta = sqrt(2)/3*(sqrt(3)/2*data_U2-sqrt(3)/2*data_U3);
        I_alpha = sqrt(2)/3*(data_I1-0.5*data_I2-0.5*data_I3);
        I_beta = sqrt(2)/3*(sqrt(3)/2*data_I2-sqrt(3)/2*data_I3);
        P = 3*(v_alpha*I_alpha+v_beta*I_beta);
        Q = 3*(-v_alpha*I_beta+v_beta*I_alpha);
        Results_P(jj) = P;
        Results_Q(jj) = Q;
    end
    j = j+20; % Calculation every 10 Periods
    jj = jj+1;
end
%**********************************************************************
%----------------------------------END---------------------------------
%**********************************************************************
%-------------------------- Normalization -----------------------------
%**********************************************************************
Results_U_H1_pu = Results_U_H1/sqrt(2)/Un;
Results_U_H2_pu = Results_U_H2/sqrt(2)/Un;
Results_U_H3_pu = Results_U_H3/sqrt(2)/Un;
Results_I_H1_pu = Results_I_H1/sqrt(2)/In;
Results_I_H2_pu = Results_I_H2/sqrt(2)/In;
Results_I_H3_pu = Results_I_H3/sqrt(2)/In;
%----------------------------------------------------------------------
Results_U_IH1_pu = Results_U_IH1/sqrt(2)/Un;
Results_U_IH2_pu = Results_U_IH2/sqrt(2)/Un;
Results_U_IH3_pu = Results_U_IH3/sqrt(2)/Un;
Results_I_IH1_pu = Results_I_IH1/sqrt(2)/In;
Results_I_IH2_pu = Results_I_IH2/sqrt(2)/In;
Results_I_IH3_pu = Results_I_IH3/sqrt(2)/In;
%----------------------------------------------------------------------
Results_U_HF1_pu = Results_U_HF1/sqrt(2)/Un;
Results_U_HF2_pu = Results_U_HF2/sqrt(2)/Un;
Results_U_HF3_pu = Results_U_HF3/sqrt(2)/Un;
Results_I_HF1_pu = Results_I_HF1/sqrt(2)/In;
Results_I_HF2_pu = Results_I_HF2/sqrt(2)/In;
Results_I_HF3_pu = Results_I_HF3/sqrt(2)/In;
%**********************************************************************
%------------------------ End -----------------------------------------
%**********************************************************************
% ------------------- Mean and Max Value Calculation ------------------
% *********************************************************************
[~, colum] = size(Results_I_H1_pu);
if colum > 1
MeanHarm_U1 = mean(Results_U_H1_pu');
MeanHarm_U2 = mean(Results_U_H2_pu');
MeanHarm_U3 = mean(Results_U_H3_pu');
MeanHarm_I1 = mean(Results_I_H1_pu');
MeanHarm_I2 = mean(Results_I_H2_pu');
MeanHarm_I3 = mean(Results_I_H3_pu');
%----------------------------------------------------------------------
MeanIHarm_U1 = mean(Results_U_IH1_pu');
MeanIHarm_U2 = mean(Results_U_IH2_pu');
MeanIHarm_U3 = mean(Results_U_IH3_pu');
MeanIHarm_I1 = mean(Results_I_IH1_pu');
MeanIHarm_I2 = mean(Results_I_IH2_pu');
MeanIHarm_I3 = mean(Results_I_IH3_pu');
%----------------------------------------------------------------------
MeanHFHarm_U1 = mean(Results_U_HF1_pu');
MeanHFHarm_U2 = mean(Results_U_HF2_pu');
MeanHFHarm_U3 = mean(Results_U_HF3_pu');
MeanHFHarm_I1 = mean(Results_I_HF1_pu');
MeanHFHarm_I2 = mean(Results_I_HF2_pu');
MeanHFHarm_I3 = mean(Results_I_HF3_pu');
%----------------------------------------------------------------------
MaxHarm_U = max(max(MeanHarm_U1,MeanHarm_U2),MeanHarm_U3);
MaxHarm_I = max(max(MeanHarm_I1,MeanHarm_I2),MeanHarm_I3);
MaxIHarm_U = max(max(MeanIHarm_U1,MeanIHarm_U2),MeanIHarm_U3);
MaxIHarm_I = max(max(MeanIHarm_I1,MeanIHarm_I2),MeanIHarm_I3);
MaxHFHarm_U = max(max(MeanHFHarm_U1,MeanHFHarm_U2),MeanHFHarm_U3);
MaxHFHarm_I = max(max(MeanHFHarm_I1,MeanHFHarm_I2),MeanHFHarm_I3);
else
MeanHarm_I1 = Results_I_H1_pu;
MeanHarm_I2 = Results_I_H2_pu;
MeanHarm_I3 = Results_I_H3_pu;
MeanHarm_U1 = Results_U_H1_pu;
MeanHarm_U2 = Results_U_H2_pu;
MeanHarm_U3 = Results_U_H3_pu;
%----------------------------------------------------------------------
MeanIHarm_U1 = Results_U_IH1_pu;
MeanIHarm_U2 = Results_U_IH2_pu;
MeanIHarm_U3 = Results_U_IH3_pu;
MeanIHarm_I1 = Results_I_IH1_pu;
MeanIHarm_I2 = Results_I_IH2_pu;
MeanIHarm_I3 = Results_I_IH3_pu;
%----------------------------------------------------------------------
MeanHFHarm_U1 = Results_U_HF1_pu;
MeanHFHarm_U2 = Results_U_HF2_pu;
MeanHFHarm_U3 = Results_U_HF3_pu;
MeanHFHarm_I1 = Results_I_HF1_pu;
MeanHFHarm_I2 = Results_I_HF2_pu;
MeanHFHarm_I3 = Results_I_HF3_pu;
%----------------------------------------------------------------------
MaxHarm_U = max(max(MeanHarm_U1,MeanHarm_U2),MeanHarm_U3);
MaxHarm_I = max(max(MeanHarm_I1,MeanHarm_I2),MeanHarm_I3);
MaxIHarm_U = max(max(MeanIHarm_U1,MeanIHarm_U2),MeanIHarm_U3);
MaxIHarm_I = max(max(MeanIHarm_I1,MeanIHarm_I2),MeanIHarm_I3);
MaxHFHarm_U = max(max(MeanHFHarm_U1,MeanHFHarm_U2),MeanHFHarm_U3);
MaxHFHarm_I = max(max(MeanHFHarm_I1,MeanHFHarm_I2),MeanHFHarm_I3);
end
%**********************************************************************
%------------------------------- End ----------------------------------
%**********************************************************************
%--------------- Order (x-axie) Calculation and Ploting ---------------
%**********************************************************************
Order_Harm = 1:1:50;
Order_IHarm = 1:1:39;
%Order_HFHarm = 1:1:35;
%Order_HFHarm = 2100:1:2134;
Order_HFHarm = 2100:200:8900;
if figures
    figure;
    subplot(2,3,1)
    bar(Order_Harm,MaxHarm_I);xlabel('Order Number');ylabel('Current Hamonics Levels I/In in pu');title('Current Harmonics spectrom')
    subplot(2,3,2)
    bar(Order_IHarm,MaxIHarm_I);xlabel('Order Number');ylabel('Current Hamonics Levels I/In in pu');title('Current interharmonics spectrom')
    subplot(2,3,3)
    bar(Order_HFHarm,MaxHFHarm_I);xlabel('Frequency in Hz');ylabel('Current Hamonics Levels I/In in pu');title('Current Higher Frequencies spectrom')
    subplot(2,3,4)
    bar(Order_Harm,MaxHarm_U);xlabel('Order Number');ylabel('Voltage Hamonics Levels U/Un in pu');title('Voltage Harmonics spectrom')
    subplot(2,3,5)
    bar(Order_IHarm,MaxIHarm_U);xlabel('Order Number');ylabel('Voltage Hamonics Levels U/Un in pu');title('Voltage interharmonics spectrom')
    subplot(2,3,6)
    bar(Order_HFHarm,MaxHFHarm_U);xlabel('Frequency in Hz');ylabel('Voltage Hamonics Levels U/Un in pu');title('Voltage Higher Frequencies spectrom')
end
%**********************************************************************
%------------------------------- End ----------------------------------
%**********************************************************************
%-------------------------- K, THD Factor  ----------------------------
%**********************************************************************
THD_U = sqrt(sum((MaxHarm_U(2:end).^2)'))/(MaxHarm_U(1));
THD_I = sqrt(sum((MaxHarm_I(2:end).^2)'))/(MaxHarm_I(1));
K_U = sqrt(sum((MaxHarm_U(2:end).^2)')/(MaxHarm_U(1)+sum((MaxHarm_U(2:end).^2)')));
K_I = sqrt(sum((MaxHarm_I(2:end).^2)')/(MaxHarm_I(1)+sum((MaxHarm_I(2:end).^2)')));
%**********************************************************************
%------------------------------- End ----------------------------------
%**********************************************************************
%------------------------ Saving Results ------------------------------
%**********************************************************************
save('Harmonics','THD_U','THD_I','K_U','K_I','MaxHarm_I','MaxHarm_U','MaxIHarm_I','MaxIHarm_U','MaxHFHarm_I','MaxHFHarm_U','MaxHarm_I','Order_IHarm','Order_Harm','Order_HFHarm')
%**********************************************************************
%------------------------------- End ----------------------------------
%**********************************************************************
end


function [p2, R2] = SE3exp(j, dt)
global uLINK
% see Murray, Li, Sastry p.42

norm_w = norm(uLINK(j).w);
if norm_w < eps
    p2 = uLINK(j).p + dt * uLINK(j).vo;
    R2 = uLINK(j).R;
else
    th = norm_w*dt;
    wn = uLINK(j).w/norm_w;		% normarized vector
    vo = uLINK(j).vo/norm_w;

    w_wedge = [0 -wn(3) wn(2);wn(3) 0 -wn(1);-wn(2) wn(1) 0];
    drot = w_wedge * sin(th) + w_wedge^2 * (1-cos(th));
    rot  = eye(3) + drot;

    p2 = rot * uLINK(j).p -drot*cross(wn, vo) + wn * wn' * vo * th;
    R2 = rot * uLINK(j).R;
end


% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz

%GEOMETRICAL PARAMETERS

l0 = 30;
h0 = 6.5;

h1 = 10;
h2 = 19;

lm = 30;
hm = 23;

lb = l0 - 8.5;

%Minimum radius of the camshaft
v = hm - h2;

%Y coordinate variation between min and max positions of the camshaft
dv = h2 - h1;

%BREATHING CYCLE PARAMETERS

%Duration of the inhale cycle / duration of the whole cycle
lambda1 = 0.50;
lambda2 = 0.08;

%Soft transition between inhale and exhale cycles
dpsi21 = 0.12;
dpsi12 = 0.15;

%Adjust parameters of the inhale curve
ga1 = 4;
gb1 = .85;
ff1 = 50;

%Adjust parametes of the exhale curve
ga2 = 3.5;
gb2 = .70;
ff2 = 55;

%GENERATION OF THE CURVES AND THE CAMSHAFT

%Time increment
dt = 0.01;

%time coordinate during the whole cycle
theta = 0:dt:2*pi;

%Generation of the soft transition between inhale and exhale curves
psi1 = [];
psi2 = [];

for i = 1:numel(theta);

    if theta(i) < (lambda2-dpsi21/2)*2*pi;
        psi1(i) = 0;

    elseif theta(i) < (lambda2+dpsi21/2)*2*pi;
        psi1(i) = 0.5 + 0.5*cos((theta(i)-(lambda2+dpsi21/2)*2*pi)/(2*dpsi21));

    elseif theta(i) < (lambda1-dpsi12/2)*2*pi;
        psi1(i) = 1;

    elseif theta(i) < (lambda1+dpsi12/2)*2*pi;
        psi1(i) = 0.5 - 0.5*cos((theta(i)-(lambda1+dpsi12/2)*2*pi)/(2*dpsi12));

    else
        psi1(i) = 0;

    end;

    psi2(i) = 1 - psi1(i);

end;

%Generation of the complete breathing cycle rho(theta)

rho = [];
rho1 = [];
rho2 = [];
rho2next = [];

rhomin = 1000;
rhomax = 0;

for i = 1:numel(theta);

    %Inhale curve
    rho1(i) = ff1*gampdf(theta(i), ga1, gb1);

    %Exhale curve
    rho2(i) = ff2*gampdf(theta(i), ga2, gb2);
    rho2next(i) = ff2*gampdf(theta(i)+2*pi, ga2, gb2);

    if theta(i) < lambda1*pi
        rho(i) = psi1(i)*rho1(i) + psi2(i)*rho2next(i);

    else
        rho(i) = psi1(i)*rho1(i) + psi2(i)*rho2(i);

    end;

    %Capturing min and max in order to generate the normalized curve
    if rho(i) > rhomax
        rhomax = rho(i);
    end;

    if rho(i) < rhomin
        rhomin = rho(i);
    end;

end;

%Generation of a normalised curve and camshaft

rhonorm = [];
rhocam = [];

for i = 1:numel(theta);

    rhonorm(i) = (rho(i)-rhomin)/(rhomax-rhomin);

    rhocam(i) = v + rhonorm(i)*dv;

end;

%Generation of the first derivate of the camshaft geometry to analize and
%validate the design

drho = [];
drhonorm = [];
drhocam = [];

a = v;
b = 1/dt;

for i = 1 : numel(rho)-1;

    drho(i) = b*(rho(i+1)-rho(i));
    drhonorm(i) = b*(rhonorm(i+1)-rhonorm(i));
    drhocam(i) = b*(rhocam(i+1)-rhocam(i)) + a;

end;

drho(numel(rho)) = b*(rho(1)-rho(numel(rho)));
drhonorm(numel(rhonorm)) = b*(rhonorm(1)-rhonorm(numel(rhonorm)));
drhocam(numel(rhocam)) = b*(rhocam(1)-rhocam(numel(rhocam))) + a;

%X and Y coordinates during two cycles (for 2-cycle camshaft plot)

theta2 = [theta/2, (2*pi+theta)/2];

rhocam2 = [rhocam, rhocam];
drhocam2 = [drho, drho];

%X and Y coordinates during three cycles (for 3-cycle camshaft plot)

theta3 = [theta/3, (2*pi+theta)/3, (4*pi+theta)/3];

rhocam3 = [rhocam, rhocam, rhocam];
drhocam3 = [drho, drho, drho];

%GENERATION OF THE PLOTS

%Trying to print it out in real size (FAIL)
%set(gcf,'PaperUnits','centimeters');
%set(gcf,'PaperSize',[42 29.7]);

%Plot of the breathing cycle interpolation by curves

fcycle = figure('Name', 'Breath Cycle curves', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
plot(theta, psi1, '-k')
plot(theta, psi2, '-k')
plot(theta, rho1, '-g')
plot(theta, rho2, '-g')
plot(theta, rho2next, '-g')
%plot(theta, drho, '-r')
plot(theta, rho, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of the normalized breathing cycle and first derivate

fnorm = figure('Name', 'Normalized Breath Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
plot(theta, drhonorm, '-r')
plot(theta, rhonorm, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 1-cycle camshaft

fcam1 = figure('Name', '1-Cycle Camshaft', 'Units', 'centimeters', 'NumberTitle', 'off');

%hold on
%polar(theta, drhocam, '-r')
polar(theta, rhocam, '-b')
%hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 2-cycle camshaft

fcam2 = figure('Name', '2-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

%hold on;
%polar(theta, drhocam2, '-r')
polar(theta2, rhocam2, '-b')
%hold off;

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 3-cycle camshaft

fcam3 = figure('Name', '3-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

%hold on;
%polar(theta,drhocam3)
polar(theta3, rhocam3)
%hold off;

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz


function value = r8_exprel ( x )

%*****************************************************************************80
%
%% R8_EXPREL evaluates the exponential relative error term of an R8 argument.
%
%  Discussion:
%
%    The relative error term is ( exp ( x ) - 1 ) / x.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the exponential relative error term
%    at X.
%
  persistent nterms
  persistent xbnd

  if ( isempty ( nterms ) )
    alneps = log ( r8_mach ( 3 ) );
    xn = 3.72 - 0.3 * alneps;
    xln = log ( ( xn + 1.0 ) / 1.36 );
    nterms = r8_aint ( xn - ( xn * xln + alneps ) / ( xln + 1.36 ) + 1.5 );
    xbnd = r8_mach ( 3 );
  end

  absx = abs ( x );

  if ( absx < xbnd )
    value = 1.0;
  elseif ( absx <= 0.5 )
    value = 0.0;
    for i = 1 : nterms
      value = 1.0 + value * x / ( nterms + 2 - i );
    end
  else
    value = ( exp ( x ) - 1.0 ) / x;
  end

  return
end


function [xroot, froot] = brent (f, x1, x2, rtol)

% solve for a single real root of a nonlinear equation

% Brent's method

% input

%  f    = objective function coded as y = f(x)
%  x1   = lower bound of search interval
%  x2   = upper bound of search interval
%  rtol = algorithm convergence criterion

% output

%  xroot  = real root of f(x) = 0
%  froot  = function value at f(x) = 0

% Orbital Mechanics with MATLAB

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

global iter;

% machine epsilon

eps = 2.23e-16;

e = 0;

a = x1;
b = x2;

fa = feval(f, a);

fb = feval(f, b);

fc = fb;

for iter = 1:1:50

    if (fb * fc > 0)
       c = a;
       fc = fa;
       d = b - a;
       e = d;
    end

    if (abs(fc) < abs(fb))
       a = b;
       b = c;
       c = a;
       fa = fb;
       fb = fc;
       fc = fa;
    end

    tol1 = 2 * eps * abs(b) + 0.5 * rtol;

    xm = 0.5 * (c - b);

    if (abs(xm) <= tol1 || fb == 0)
       break;
    end

    if (abs(e) >= tol1 && abs(fa) > abs(fb))
       s = fb / fa;

       if (a == c)
          p = 2 * xm * s;
          q = 1 - s;
       else
          q = fa / fc;
          r = fb / fc;
          p = s * (2 * xm * q * (q - r) - (b - a) * (r - 1));
          q = (q - 1) * (r - 1) * (s - 1);
       end

       if (p > 0)
          q = -q;
       end

       p = abs(p);

       min = abs(e * q);

       tmp = 3 * xm * q - abs(tol1 * q);

       if (min < tmp)
          min = tmp;
       end

       if (2 * p < min)
          e = d;
          d = p / q;
       else
          d = xm;
          e = d;
       end
    else
       d = xm;
       e = d;
    end

    a = b;
    fa = fb;

    if (abs(d) > tol1)
       b = b + d;
    else
       b = b + sign(xm) * tol1;
    end

    fb = feval(f, b);

end

xroot = b;
froot = fb;


%SerialLink.JACOB0 Jacobian in world coordinates
%
% J0 = R.jacob0(Q, OPTIONS) is the Jacobian matrix (6xN) for the robot in
% pose Q (1xN), and N is the number of robot joints.  The manipulator
% Jacobian matrix maps joint velocity to end-effector spatial velocity V =
% J0*QD expressed in the world-coordinate frame.
%
% Options::
% 'rpy'     Compute analytical Jacobian with rotation rate in terms of
%           roll-pitch-yaw angles
% 'eul'     Compute analytical Jacobian with rotation rates in terms of
%           Euler angles
% 'trans'   Return translational submatrix of Jacobian
% 'rot'     Return rotational submatrix of Jacobian
%
% Note::
% - The Jacobian is computed in the end-effector frame and transformed to
%   the world frame.
% - The default Jacobian returned is often referred to as the geometric
%   Jacobian, as opposed to the analytical Jacobian.
%
% See also SerialLink.jacobn, jsingu, deltatr, tr2delta, jsingu.

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function J0 = jacob0(robot, q, varargin)

    opt.rpy = false;
    opt.eul = false;
    opt.trans = false;
    opt.rot = false;

    opt = tb_optparse(opt, varargin);

	%
	%   dX_tn = Jn dq
	%
	Jn = jacobn(robot, q);	% Jacobian from joint to wrist space

	%
	%  convert to Jacobian in base coordinates
	%
	Tn = fkine(robot, q);	% end-effector transformation
	R = t2r(Tn);
	J0 = [R zeros(3,3); zeros(3,3) R] * Jn;

    if opt.rpy
        rpy = tr2rpy( fkine(robot, q) );
        B = rpy2jac(rpy);
        if rcond(B) < eps
            error('Representational singularity');
        end
        J0 = blkdiag( eye(3,3), inv(B) ) * J0;
    elseif opt.eul
        eul = tr2eul( fkine(robot, q) );
        B = eul2jac(eul);
        if rcond(B) < eps
            error('Representational singularity');
        end
        J0 = blkdiag( eye(3,3), inv(B) ) * J0;
    end

    if opt.trans
        J0 = J0(1:3,:);
    elseif opt.rot
        J0 = J0(4:6,:);
    end


function pols = klegeypols ( x, y, n )

%*****************************************************************************80
%
%% KLEGEYPOLS evaluates scaled Legendre polynomials.
%
%  Discussion:
%
%    This routine evaluate a sequence of scaled Legendre polynomials
%    P_n(x/y) y^n, with the parameter y in [0,1].
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    13 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, real X, the evaluation point.
%
%    Input, real Y, the parameter.
%
%    Input, integer N, the highest degree to be evaluated.
%
%    Output, real POLS(N+1), the polynomial values.
%
  pols = zeros(n+1,1);

  pkp1 = 1.0;
  pols(1) = pkp1;
  if ( n == 0 )
    return
  end

  pk = pkp1;
  pkp1 = x;
  pols(2) = pkp1;
  if ( n == 1 )
    return
  end

  for k = 1 : n - 1
    pkm1 = pk;
    pk = pkp1;
    pkp1 = ( ( 2.0 * k + 1.0 ) * x * pk ...
      - k * pkm1 * y * y ) / ( k + 1.0 );
    pols(k+2) = pkp1;
  end

  return
end


function meanRot=meanRotation(param1,w)
%%MEANROTATION Perform a weighted or unweighted average of a set of
%              rotation matrices or unit quaternions (which represent
%              directions). The weights are scalar and the cost function
%              finds the rotation matrix REst to minimize the sum of
%              w(i)*norm(REst-R{i},'fro'), where R{i} is the ith rotation
%              matrix to be averaged (either given explicitly as a
%              rotation matrix or given as a quaternion).
%
%INPUTS: param1 A 3X3XN hypermatrix of N rotation matrices that are to be
%               averaged or a 4XN set of unit-quaternions that are to be
%               averaged. The function automatically detects whether
%               rotation matrices or quaternions were passed. The
%               handedness of the quaternions does not matter.
%             w An optional set of N weights for averaging the rotations.
%               If omitted, all of the rotation matrices/ unit quaternions
%               in param1 are equally weighted.
%
%OUTPUTS: meanRot The weighted mean 3X3 rotation matrix or 4X1 rotation
%                 quaternion, depending on the data type passed in param1.
%
%The algorithm of [1] for performing a weighted average of quaternions or
%rotation matrices is used. Note that the paper contains an additional
%technique for averaging quaternions with non-scalar weights, which is not
%implemented here.
%
%A quaternion can represent an orientation (rotation) in space as a 4X1
%unit vector. However, one cannot simply take the weighted average of a
%set of quaternions to get an average quaternion (that is then normalized)
%to represent the average rotation, because the quaternion representation
%is not unique. Thus, while q and -q represent the same rotation, they
%affect such an average differently, which is bad. This is why the more
%complicated algorithms of the above paper are used.
%
%REFERENCES:
%[1] F. L. Markley, Y. Cheng, J. L. Crassidis, and Y. Oshman, "Averaging
%    quaternions," Journal of Guidance, Control, and Dynamics, vol. 30,
%    no. 4, pp. 1193-1196, Jul. - Aug. 2007.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(size(param1,1)==3)
    usingQuaternions=false;
    RotMats=param1;

    numVals=size(param1,3);
else
    usingQuaternions=true;
    q=param1;

    numVals=size(param1,2);
end

if(nargin<2)
   w=ones(numVals,1);
end

%If quaternions are being averaged rather than rotation matrices.
if(usingQuaternions)
    %Build M as in Equation 2 of [1].
    M=zeros(4,4);
    for curQuat=1:numVals
        M=M+w(curQuat)*(q(:,curQuat)*q(:,curQuat)');
    end

    %The solution to the cost function in Equation 13 is the eigenvector
    %corresponding to the largest eigenvalue in M.
    [V,D]=eig(M);
    %Get the indices of the sorted eigenvalues (and thus of the
    %corresponding eigenvectors).
    [~,idx]=sort(diag(D),'descend');
    %The rotation quaternion is the eigenvector associated with the
    %largest eigenvalue.
    meanRot=V(:,idx(1));%A quaternion
else%If rotation matrices are being averaged rather than quaternions.
    %Build B as in Equation 5 in [1].
    B=zeros(3,3);
    for curRot=1:numVals
       B=B+w(curRot)*RotMats(:,:,curRot);
    end

    %This solves the equivalent formulation of Wahba's problem for
    %Equation 4.
    meanRot=findTransParam(B,eye(3));%A rotation matrix.
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.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LMS Algorithm
% Author : Arsal Butt
% Contact information :
% School of Electrical & Electronic Engineering,
% University of Bradford,
% Bradford, BD7 1DP,
% United Kingdom .
% email : a.butt7@bradford.ac.uk
% Date    : 03-8-2010
% Version : 1.0.2
% Reference : Statistical and Adaptive Signal Processing By Dimitris G. Manolakis, Vinay K. Ingle & Stephen M. Kogon.
% Note : The author doesn't take any responsibility for any harm caused by
% the use of this code;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Use N iterations of an adapting filter system to identify an unknown
% Mth-order FIR filter.
function lmsalgorithm = lms()
sysorder = input('Enter the System Order? ')
N = input('Enter the number of Itterations? ')
if ((sysorder >= 2) && (N >= 2))
    x  = randn(N,1);     % Input to the filter
b  = fir1(sysorder-1,0.5);  % FIR system to be identified
n  = 0.1*randn(N,1); % Uncorrelated noise signal
d  = filter(b,1,x)+n;  % Desired signal = output of H + Uncorrelated noise signal
w = zeros (sysorder, 1) ; % Initially filter weights are zero
for n = sysorder : N
	u = x(n:-1:n-sysorder+1) ;
    y(n)= w' * u; % output of the adaptive filter
    e(n) = d(n) - y(n) ; % error signal = desired signal - adaptive filter output
    mu=0.008;
    w = w + mu * u * e(n) ; % filter weights update
end
hold on
plot(d,'g')
plot(y,'r');
semilogy((abs(e)),'m') ;
title('System output') ;
xlabel('Samples')
ylabel('True and estimated output')
legend('Desired','Output','Error')
axis([0 N -2 2.5])
figure
plot(b', 'k+')
hold on
plot(w, 'r*')
legend('Actual weights','Estimated weights')
title('Comparison of the actual weights and the estimated weights') ;
else ('System Order and numbers of Itterations should be greater than 1')
end

end


function value = triangulation_area ( node_num, node_xy, element_order, ...
  element_num, element_node )

%*****************************************************************************80
%
%% TRIANGULATION_AREA computes the area of a triangulation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(NODE_NUM,2), the coordinates of the nodes.
%
%    Input, integer ELEMENT_ORDER, the order of the triangles.
%
%    Input, integer ELEMENT_NUM, the number of triangles.
%
%    Input, integer ELEMENT_NODE(ELEMENT_NUM,ELEMENT_ORDER),
%    the nodes making up each triangle.
%
%    Output, real VALUE, the area.
%
  value = 0.0;

  for element = 1 : element_num

    element_xy(1:3,1:2) = node_xy(element_node(element,1:3),1:2);

    element_area = 0.5 * abs ( ...
        element_xy(1,1) * ( element_xy(2,2) - element_xy(3,2) ) ...
      + element_xy(2,1) * ( element_xy(3,2) - element_xy(1,2) ) ...
      + element_xy(3,1) * ( element_xy(1,2) - element_xy(2,2) ) );

    value = value + element_area;

  end

  return
end


internet    = load('data/internet.dat')';
y           = internet(2, 2:end);
fprintf(1, '\n');

P       = 5;
Q       = 5;

%% Model selection for complete series %%
fprintf(1, 'AIC for ARMA(p, q) models on complete data:\n');
fprintf(1, '    '); for q = 0:Q, fprintf(1, '%-12d', q); end; fprintf(1, '\n');
logL    = zeros(P+1, Q+1);
AIC     = zeros(P+1, Q+1);
arma    = cell(P+1, Q+1);
for p = 0 : P
    fprintf(1, '%-4d', p);
    for q = 0 : Q
        model                                   = ssm_arma(p, q);
        [arma{p+1, q+1} logL(p+1, q+1) output]  = estimate(y, model, [repmat(0.3, 1, model.w-1) 10], [], 'fmin', 'bfgs', 'disp', 'off');
        AIC(p+1, q+1)                           = output.AIC;
        fprintf(1, '%-12g', AIC(p+1, q+1));
    end
    fprintf(1, '\n');
end
[m i] = min(AIC(:)); temp = AIC(i); AIC(i) = Inf;
fprintf(1, 'ARMA(%d, %d) found to be the best model, ', mod(i-1, P+1), floor((i-1)/(P+1)));
[m j] = min(AIC(:)); AIC(i) = temp;
fprintf(1, 'ARMA(%d, %d) is the second best.\n\n', mod(j-1, P+1), floor((j-1)/(P+1)));

%% Model selection for data with missing values %%
ymis        = y; ymis([6 16 26 36 46 56 66 72 73 74 75 76 86 96]) = NaN;
fprintf(1, 'AIC for ARMA(p, q) models on data w/ missing observations:\n');
fprintf(1, '    '); for q = 0:Q, fprintf(1, '%-12d', q); end; fprintf(1, '\n');
logLmis     = zeros(P+1, Q+1);
AICmis      = zeros(P+1, Q+1);
armamis     = cell(P+1, Q+1);
for p = 0 : P
    fprintf(1, '%-4d', p);
    for q = 0 : Q
        model                                           = ssm_arma(p, q);
        [armamis{p+1, q+1} logLmis(p+1, q+1) output]    = estimate(ymis, model, [repmat(-0.1, 1, model.w-1) 1], [], 'fmin', 'bfgs', 'disp', 'off');
        AICmis(p+1, q+1)                                = output.AIC;
        fprintf(1, '%-12g', AICmis(p+1, q+1));
    end
    fprintf(1, '\n');
end
[m i] = min(AICmis(:)); temp = AICmis(i); AICmis(i) = Inf;
fprintf(1, 'ARMA(%d, %d) found to be the best model, ', mod(i-1, P+1), floor((i-1)/(P+1)));
[m j] = min(AICmis(:)); AICmis(i) = temp;
fprintf(1, 'ARMA(%d, %d) is the second best.\n\n', mod(j-1, P+1), floor((j-1)/(P+1)));

%% Forecast with ARMA(1, 1) on the complete data %%
[armafore logL] = estimate(y, ssm_arma(1, 1), 0.1);
yf              = signal(kalman([y repmat(NaN, 1, 20)], armafore), armafore);
figure('Name', 'Internet series forecast');
plot(yf, 'DisplayName', 'forecast'), hold all, scatter(1:length(y), y, 10, 'r', 's', 'filled', 'DisplayName', 'data'), hold off, ylim([-15 15]), legend('show');
if ispc, set(gcf, 'WindowStyle', 'docked'); end

%% Forecast with ARMA(1, 1) on the data w/ missing values %%
[armafore logLmis]  = estimate(ymis, ssm_arma(1, 1), 0.1);
ymisf               = signal(kalman([ymis repmat(NaN, 1, 20)], armafore), armafore);
figure('Name', 'Internet series in-sample one-step and out-of-sample forecasts');
plot(ymisf), hold all, scatter(1:length(ymis), ymis, 10, 'r', 's', 'filled'), hold off, ylim([-15 15]);
if ispc, set(gcf, 'WindowStyle', 'docked'); end


function [jac,invjac,phi,dphidx,dphidy] = deriv(s,t,xl,yl)
%deriv  evaluates derivatives of bilinear shape functions
%   [jac,invjac,phi,dphidx,dphidy] = deriv(s,t,xl,yl);
%   input
%          s         reference element x coordinate
%          t         reference element y coordinate
%          xl        physical element x vertex coordinates
%          yl        physical element y vertex coordinates
%   output
%          jac       elementwise jacobian (evaluated at (s,t))
%          invjac    elementwise inverse of jacobian
%          phi       elementwise shape functions
%          dphidx    x derivatives of phi
%          dphidy    y derivatives of phi
%
%   IFISS function: DJS; 4 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
      nel=length(xl(:,1));
      zero_v = zeros(nel,1);
      one_v = ones(nel,1);
%
% evaluate shape functions
     [phi_e,dphids,dphidt] = shape(s,t);
%
      dxds = zero_v;
      dxdt = zero_v;
      dyds = zero_v;
      dydt = zero_v;
	   jac = zero_v;
    invjac = zero_v;
%
      for ivtx = 1:4
         dxds(:) = dxds(:) + xl(:,ivtx) .* one_v*dphids(ivtx);
         dxdt(:) = dxdt(:) + xl(:,ivtx) .* one_v*dphidt(ivtx);
         dyds(:) = dyds(:) + yl(:,ivtx) .* one_v*dphids(ivtx);
         dydt(:) = dydt(:) + yl(:,ivtx) .* one_v*dphidt(ivtx);
      end
%
      jac(:) = dxds(:).*dydt(:) - dxdt(:).*dyds(:);
%
% check element Jacobian
      if any(jac < 1e-9)
      fprintf('Bad element warning ...\n')
         if any(jac <= 0.0)
         error('singular Jacobian ... Aborted ...')
         end
      end
      invjac(:) = one_v ./ jac(:);
%
      for ivtx = 1:4
         phi(:,ivtx) = phi_e(ivtx)*one_v;
		 dphidx(:,ivtx) = ( dphids(:,ivtx).*dydt(:) - dphidt(:,ivtx).*dyds(:));
         dphidy(:,ivtx) = (-dphids(:,ivtx).*dxdt(:) + dphidt(:,ivtx).*dxds(:));
      end
      return


function dq = DQconj(dv,WHICHCONJ)

% DQCONJ   Dual quaternion conjugate
%
%    DQ = DQCONJ(DV) returns the dual quaternion conjugate of the dual
%      quaternion DV. DV (resp. DQ) is a 8-vector representing a dual
%      quaternion or an array 8*N (column i represents dual quaternion i)
%      where N is the number of dual quaternions. The default type of
%      conjugate is 'point' (see below). DQ has the same size as DV.
%
%    DQ = DQCONJ(DV,WHICHCONJ) returns the type of dual quaternion conjugate
%       corresponding to WHICHCONJ (DV = Q0+eps*Q1):
%         - 'point': mixed conjugate: dV* = Q0*-eps*Q1* (used for point
%         transformations).
%         - 'line': quaternion conjugate: dV* = Q0*+eps*Q1* (used for line
%         transformations).
%         - 'other': dual number conjugate: dV* = Q0-eps*Q1
%
% See also QCONJ, DQMULT

if nargin < 2
    WHICHCONJ = 'point'; % default is 'mixed' conjugate' (point transformation)
end

sdv = size(dv);
if sdv == [1 8], dv = dv'; sdv = size(dv); end

% wrong size
if sdv(1) ~= 8
    error('DualQuaternion:DQconj:wrongsize',...
        '%d rows in array dv. It should be 8.',sdv(1));
end

n = sdv(2);
dq = zeros(8,n);
switch WHICHCONJ
    case 'point' % % case of point transformation
        dq(1:4,:) = Qconj(dv(1:4,:));
        dq(5:8,:) = -Qconj(dv(5:8,:));
    case 'line' % case of line transformation
        dq(1:4,:) = Qconj(dv(1:4,:));
        dq(5:8,:) = Qconj(dv(5:8,:));
    case 'other' % dual number conjugate
        dq(1:4,:) = dv(1:4,:);
        dq(5:8,:) = -dv(5:8,:);
    otherwise
        error('DualQuaternion:DQconj:wrongsize',...
            'WHICHCONJ must be ''point'', ''line'' or ''other'' ');
end


function z = compute_zeros(x,y)

% compute_zeros - compute the location of zeros
%
% z = compute_zeros(x,y)
%
%   find the roots z of f(z)=0 where f is sampled at y=f(x)
%
%   Copyright (c) 2007 Gabriel Peyre

I = find( y(1:end-1).*y(2:end) < 0);

d = y(I+1)-y(I);
J = find(d==0); d(J) = 1;
z = ( x(I).*y(I+1)-x(I+1).*y(I) )./d;
z(J) = (x(I(J))+x(I(J)+1))/2;


% Fig. 8.13   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clear all;
%close all;
clf

w=logspace(-1,2,100);
l=length(w);

% s-plane
num=5;
den=[1 5];
[magC,phaseC]=bode(num,den,w);
subplot(2,1,1),
loglog(w,magC,'-')
axis([.1  100  .01 1]);
ylabel('Magnitude')
%xlabel('Frequency (rad/sec)')
title('Fig. 8.13  (a) 100 rad/sec  T=1/15 sec')
nicegrid
hold on

%              T = 1/15 sec
T = 1/15;

%  MPZ

num=.143*[1 1];
den=[1 -.715];
[mag1,phase]=dbode(num,den,T,w);

%  MMPZ

num=.285;
den=[1 -.715];
[mag2,phase]=dbode(num,den,T,w);

% Tustins

num=.143*[1 1];
den=[1 -.713];
[mag3,phase]=dbode(num,den,T,w);

loglog(w,mag1,'--',w,mag2,'--',w,mag3,'-.')
hold off

%              T = 1/3 sec

subplot(2,1,2),loglog(w,magC,'-')
ylabel('Magnitude')
xlabel('Frequency (rad/sec)')
title('(b) 20 rad/sec  T=1/3 sec')
nicegrid
hold on

T = 1/3;

%  MPZ

num=.405*[1 1];
den=[1 -.189];
wp=w;
wp=wp(wp < 24);
[mag1,phase]=dbode(num,den,T,wp);

%  MMPZ

num=.811;
den=[1 -.189];
[mag2,phase]=dbode(num,den,T,wp);

% Tustins

num=.454*[1 1];
den=[1 -.0914];
[mag3,phase]=dbode(num,den,T,wp);

loglog(wp,mag1,'--',wp,mag2,'--',wp,mag3,'-.')

hold off


function x = beta_binomial_cdf_inv ( cdf, a, b, c )

%*****************************************************************************80
%
%% BETA_BINOMIAL_CDF_INV inverts the Beta Binomial CDF.
%
%  Discussion:
%
%    A simple discrete approach is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 October 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%
%    Input, real A, B, parameters of the PDF.
%    0.0D+00 < A,
%    0.0D+00 < B.
%
%    Input, integer C, a parameter of the PDF.
%    0 <= C.
%
%    Output, integer X, the smallest X whose cumulative density function
%    is greater than or equal to CDF.
%
  if ( cdf <= 0.0 )

    x = 0;

  else

    cum = 0.0;

    for y = 0 : c

      pdf = beta ( a + y, b + c - y ) / ( ( c + 1 ) ...
        * beta ( y + 1, c - y + 1 ) * beta ( a, b ) );

      cum = cum + pdf;

      if ( cdf <= cum )
        x = y;
        return
      end

    end

    x = c;

  end

  return
end


%DEMO_REGRESSION_MEANF  Regression problem demonstration for GP model with a
%                       mean function
%
%  Description
%    The regression problem consist of a data with one input
%    variable and one output variable with Gaussian noise. The
%    problem is modelled with Full GP model with gaussian
%    likelihood and a specified mean function. The mean function m
%    is a weighted sum of some basis functions h, where
%
%                   m=h'*Beta
%
%    and we have set a gaussian prior for the weights Beta
%
%                   Beta ~ N(b,B)
%
%    Inference is done according to Rasmussen and Williams (2006)
%    p. 27-29.
%
%    In this demonstration the data is from a function:
%
%         y = 2 + x + x^2 + 4*cos(x)*sin(x) + epsilon
%
%    and we define the base functions to be:
%
%       h1(x)=x^2, h2(x)=x, h3(x)=2
%
%    References:
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.

% Copyright (c) 2010 Tuomas Nikoskinen

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

% Create the data
 x=[-2:0.6:2]';
 res=4*cos(x).*sin(x)+0.4*randn(size(sin(x)));
 y= 2 + x + x.^2 + res;
%---------------

gpcf = gpcf_sexp('lengthScale', [0.5], 'magnSigma2', .5);
lik = lik_gaussian('sigma2', 0.4^2);

% Initialize base functions for GP's mean function.
gpmf1 = gpmf_constant('prior_mean',.3,'prior_cov',1);
gpmf2 = gpmf_linear('prior_mean',.3,'prior_cov',1);
gpmf3 = gpmf_squared('prior_mean',.3,'prior_cov',1);

% Initialize gp structure
gp = gp_set('lik', lik, 'cf', gpcf, 'meanf', {gpmf1,gpmf2,gpmf3});

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'DerivativeCheck','on');
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Predictions
xt=[-3:0.1:3]';
[Eft, Varft] = gp_pred(gp, x, y, xt);

% PLOT THE DATA

figure
m=plot(xt,Eft,'k','lineWidth',2);
hold on
plot(xt,Eft+2*sqrt(Varft),'k--')
hold on
m95=plot(xt,Eft-2*sqrt(Varft),'k--');
hold on
hav=plot(x,y, 'ro','markerSize',6,'MarkerFaceColor','r');
hold on
h=plot(xt,2+xt+xt.^2+4*cos(xt).*sin(xt),'b--','lineWidth',2);
hold on
mmmean=plot(xt,2+xt+xt.^2,'r--','lineWidth',1);
legend([m m95 h mmmean hav],'prediction','95%','f(x)','mean function','observations');
xlabel('input x')
ylabel('output y')
title('GP regression with a mean function')


function z = EulerToQuaternion(phi, theta, psi)
%
%
sinPhi   = sin(phi/2);    cosPhi   = cos(phi/2);
sinTheta = sin(theta/2);  cosTheta = cos(theta/2);
sinPsi   = sin(psi/2);    cosPsi   = cos(psi/2);

z = [ cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi;
      sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi;
      cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi;
      cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi;
    ];


function cdf = frechet_cdf ( x, alpha )

%*****************************************************************************80
%
%% FRECHET_CDF evaluates the Frechet CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the parameter.
%    It is required that 0.0 < ALPHA.
%
%    Input, real X, the argument of the CDF.
%
%    Output, real CDF, the value of the CDF.
%
  if ( alpha <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'FRECHET_CDF - Fatal error!\n' );
    fprintf ( 1, '  ALPHA <= 0.0.\n' );
    error ( 'FRECHET_CDF - Fatal error!' );
  end

  if ( x <= 0.0 )
    cdf = 0.0;
  else
    cdf = exp ( - 1.0 / x^alpha );
  end

  return
end


function [P,exitCode]=RiccatiPostNoClutter(H,F,R,Q,PD,RelTol,AbsTol,maxIter)
%%RICCATIPOSTNOCLUTTER Solve the posterior Riccati equation that has been
%                      modified for a probability of detection less
%                      than or equal to 1. This provides the average
%                      asymptotic covariance of a linear Kalman filter with
%                      linear measurements just after a (possible with
%                      probability PD) measurement update.
%
%INPUTS: H The zDim X xDim measurement matrix such that H*x+w is the
%          measurement, where x is the state and w is zero-mean Gaussian
%          noise with covariance matrix R.
%        F The xDim X xDim state transition matrix The state at discrete-
%          time k+1 is modeled as F times the state at time k plus zero-
%          mean Gaussian process noise with covariance matrix Q.
%        R The zDim X zDim measurement covariance matrix.
%        Q The xDim X xDim process noise covariance matrix. This can not be
%          a singular matrix.
%       PD The optional detection probability of the target at each scan.
%          If omitted, PD is assumed to be one.
%   RelTol The maximum relative error tolerance allowed, a positive scalar.
%          This tolerance applies to all elements in the matrix and is only
%          needed if PD<1. If omitted or an empty matrix is passed, the
%          default value of 1e-13 is used.
%   AbsTol The absolute error tolerance allowed, a positive scalar. This
%          tolerance applies to all elements in the matrix and is only
%          needed if PD<1. If omitted or an empty matrix is passed, the
%          default value of 1e-10 is used.
%  maxIter An optional integer specifying the maximum number of iterations.
%          If omitted or an empty matrix is passed, the default is 5000.
%
%OUTPUTS: P The average asymptotic error covariance matrix of the linear
%           Kalman filter with a probability of detection of PD after a
%           (possible) measurement update.
%  exitCode This is zero if convergence is achieved and 1 if it is not
%           achieved.
%
%The notion of a Riccati equation with a detection probability less than
%one is discussed in [1]. However, they do not explicitly write out the
%posterior form. The posterior recursion goes from P_{k|k} to P_{k+1|k+1}.
%This stands in contrast to the prior Riccati equation, which expresses the
%transition from P_{k|k-1} to P_{k+1|k}.
%
%The posterior Riccati equation is
%P=F*P*F'+Q-PD*(F*P*F'*H'+Q*H')*inv(H*F*P*F'*H'+H*Q*H'+R)*(F*P*F'*H'+Q*H')'
%and is derived by substituting the prediction step into the measurement
%update step of the linear Kalman filter with additive process noise and
%additive measurement noise.
%
%EXAMPLE:
% T=1;
% F=FPolyKal(T,6,1);
% q0=1;
% Q=QPolyKal(T,6,1,q0);
% R=diag([10;10;10]);
% H=[1,0,0,0,0,0;
%    0,1,0,0,0,0;
%    0,0,1,0,0,0];
% PD=0.5;
% P=RiccatiPostNoClutter(H,F,R,Q,PD)
%
%REFERENCES:
%[1] Y. Boers and H. Driessen, "Modified Riccati equation and its
%    application to target tracking," IEE Proceedings Radar, Sonar and
%    Navigation, vol. 153, no. 1, pp. 7-12, Feb. 2006.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(PD))
   PD=1;
end

if(nargin<6||isempty(AbsTol))
    AbsTol=1e-13;
end

if(nargin<7||isempty(RelTol))
    RelTol=1e-10;
end

if(nargin<8||isempty(maxIter))
   maxIter=5000;
end

%Get an initial estimate by solving the Riccati equation with PD=1.
AD=F';
BD=F'*H';
SD=Q*H';
RD=H*Q*H'+R;
QD=Q;
PPrev=RiccatiSolveD(AD,BD,QD,RD,SD);
exitCode=0;
if(PD~=1)
    %Now, iterate the Riccati equation with PD<1 until convergence of the
    %Frobenis norm or until a maximum number of iterations has occurred.
    curIter=0;
    while(curIter<maxIter)
        P=F*PPrev*F'+Q-PD*(F*PPrev*F'*H'+Q*H')*inv(H*F*PPrev*F'*H'+H*Q*H'+R)*(F*PPrev*F'*H'+Q*H')';
        %Ensure symmetry is preserved.
        P=(P+P')/2;

        diffMag=abs(P-PPrev);
        if(all((diffMag(:)<=RelTol*abs(P(:)))|(diffMag(:)<=AbsTol)))
            return;
        end

        PPrev=P;
        curIter=curIter+1;
    end
    exitCode=1;%Convergence not achieved.
else
    P=PPrev;
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 a = sylvester_kac_inverse ( n )

%*****************************************************************************80
%
%% SYLVESTER_KAC_INVERSE returns the inverse of the SYLVESTER_KAC matrix.
%
%  Example:
%
%    N = 6:
%
%      0     1/5    0  -2/15  0   8/15
%      1      0     0    0    0    0
%      0      0     0   1/3   0  -4/3
%    -4/3     0    1/3   0    0    0
%      0      0     0    0    0    1
%     8/15    0   -2/15  0   1/5   0
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  if ( mod ( n, 2 ) == 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'SYLVESTER_KAC_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  The matrix is singular for odd N.\n' );
    error ( 'SYLVESTER_KAC_INVERSE - Fatal error!' );
  end

  a = zeros ( n, n );

  for i = 1 : n

    if ( mod ( i, 2 ) == 1 )

      for j = i : 2 : n-1

        if ( j == i )
          p1 = 1.0 / ( n - j );
          p2 = 1.0 /       j;
        else
          p1 = - p1 * (     j - 1 ) / ( n - j );
          p2 = - p2 * ( n - j + 1 ) /       j;
        end

        a(i,j+1) = p1;
        a(j+1,i) = p2;

      end

    end

  end

  return
end


function [rotmat,cornerpoints,volume,surface,edgelength] = minboundbox(x,y,z,metric,level)
% minboundbox: Compute the minimal bounding box of points in 3d
% usage: [rotmat,cornerpoints,volume,surface,edgelength] = minboundbox(x,y,z,metric,level)
%
% arguments: (input)
%  x,y,z - vectors of points, describing points in 3d as
%        (x,y,z) pairs. x, y and z must be the same lengths.
%
%  metric - (OPTIONAL) - single letter character flag which
%        denotes the use of minimal volume, surface or sum of edges as the
%        metric to be minimized. metric may be either 'v', 's' or 'e',
%        capitalization is ignored.
%
%        DEFAULT: 'v'    ('volume')
%
%  level - (OPTIONAL) - either 1, 2, 3 or 4. This denotes the level of
%       reliability of the resulting minimal bounding box.
%       '1' denotes the search for a bounding box, where one side of the
%           box coincides with a face of the convex hull. (fast)
%       '2' like '1', but also incorporates a search through each pair of edges
%           which form a plane which coincides with one side of the box (slow)
%       '3' like '1', but also incorporates a search through each edge which is
%           parallel to an edge of the box (reasonably fast)
%       '4' like '1', '2' and '3' together. (slowest) (Never needed that.)
%
%       It depends on the application, what should be chosen here.
%       See the example at the end for the effects of this parameter.
%
%        DEFAULT: '3'
%
% arguments: (output)
%  rotmat - (3x3) rotation matrix for mapping of the pointcloud into a
%                 box which is axis-parallel (use inv(rotmat) for inverse
%                 mapping).
%
%  cornerpoints - (8x3) the cornerpoints of the bounding box.
%
%  volume - (scalar) volume of the minimal box itself.
%
%  surface - (scalar) surface of the minimal box as found.
%
%  edgelength - (scalar) sum of the edgelengths of the minimal box as found.
%
% Thanks to John d'Errico for providing the solution to the 2d version of
% this problem via minboundrect from the FEX. I took a huge amount of his
% syntax, organisation and comments for this submission. (Hope he wont sue
% me, though.)
%
% Thanks to Roger Stafford and John d'Errico for helpful discussions and
% suggestions.
%
% The algorithm is still not proven to produce the smallest possible
% enclosing box. But i never found any counterexamples for the 'best'
% algorithm with level 4, whilst the second example below shows, there are
% differences between the levels. I would be happy to have a proof that
% level 3 suffices for each case.
%
% Example usage:
%
% (1)
%      x = rand(100,1);
%      y = rand(100,1);
%      z = rand(100,1);
%      [rotmat,cornerpoints,volume,surface] = minboundbox(x,y,z);
%      plot3(x,y,z,'b.');hold on;plotminbox(cornerpoints,'r');
%
% (2)
%      x=[1,0,0.1,1];y=[0,1,0.1,1];z=[0,0,0.9,1];
%      [nerd,cornerpoints1,nerd,nerd] = minboundbox(x,y,z,'v',1);
%      [nerd,cornerpoints2,nerd,nerd] = minboundbox(x,y,z,'v',2);
%      [nerd,cornerpoints3,nerd,nerd] = minboundbox(x,y,z,'v',3);
%      plot3(x,y,z,'bo','LineWidth',5);hold on;
%      plotminbox(cornerpoints1,'b');hold on;
%      plotminbox(cornerpoints2,'m');hold on;
%      plotminbox(cornerpoints3,'r');axis equal;grid on;hold off;
%
%
% See also: minboundcircle, minboundtri, minboundsphere, minboundrect
%
%
% Author: Johannes Korsawe
% E-mail: johannes.korsawe@volkswagen.de
% Release: 1.0
% Release date: 09/01/2008

% default for metric
if (nargin<4) || isempty(metric)
  metric = 'v';
elseif ~ischar(metric)
  error 'metric must be a character flag if it is supplied.'
else
  % check for 'v', 's' or 'e'
  metric = lower(metric(:)');
  ind = strmatch(metric,{'volume','surface','edges'});
  if isempty(ind)
    error 'metric does not match either ''volume'', ''surface'' or ''edges''.'
  end
  % just keep the first letter.
  metric = metric(1);
end

% default for level
if (nargin<5 || isempty(level)),
    level=4;
elseif ~isnumeric(level) || (level~=1 && level~=2 && level~=3 && level~=4),
        error 'metric does not match either ''1'', ''2'', ''3'' or ''3''.'
end

% preprocess data
x = x(:);
y = y(:);
z = z(:);

% not many error checks to worry about
n1 = length(x);n2 = length(y);n3 = length(z);
if n1 ~= n2 || n1 ~= n3 || n2 ~= n3,
  error 'x, y and z must be the same sizes'
end

% start out with the convex hull of the points to
% reduce the problem dramatically. Note that any
% points in the interior of the convex hull are
% never needed, so we drop them.

try
 K = convhulln([x,y,z],{'Qt'});  % 'Pp' will silence the warnings

  % exclude those points inside the hull as not relevant
  % also sorts the points into their convex hull as a
  % closed polygon

 Ki = unique(K(:));
 [tf,loc] = ismember(K(:),Ki);K = reshape(loc,size(K));
 x = x(Ki);
 y = y(Ki);
 z = z(Ki);
 n1 =length(x);

catch
    error 'The number and/or distribution of given points does not allow the construction of a convex hull.'
end

% now we must find the bounding box of those points that remain

% get the angle of each face of the hull polygon.
% calculate local coordinate system
% x,y and z values of faces' cornerpoints
fx = [x(K(:,1)),x(K(:,2)),x(K(:,3))];
fy = [y(K(:,1)),y(K(:,2)),y(K(:,3))];
fz = [z(K(:,1)),z(K(:,2)),z(K(:,3))];
% faces' edges
v1 = [fx(:,2)-fx(:,1),fy(:,2)-fy(:,1),fz(:,2)-fz(:,1)];
v1 = v1./(repmat(sqrt(sum(v1.^2,2)),1,3));
v2 = [fx(:,3)-fx(:,1),fy(:,3)-fy(:,1),fz(:,3)-fz(:,1)];
v2 = v2-repmat(dot(v1,v2,2),1,3).*v1;
v2 = v2./(repmat(sqrt(sum(v2.^2,2)),1,3));
% faces' normals
nv = cross(v1,v2,2);
% faces' orientations measured in Euler angles
[alpha,beta,gamma]=euler123(v1,v2,nv);

nang = size(alpha,1);
d = inf;        % this will be the minimal value
rotmat=[];minmax=[];
xyz = [x,y,z];
if level==1 || level==3,    % check each face of the hull
    for i = 1:nang,
      % check current orientation
      % receive minimal value, rotation matrix and dimensions of the enclosing box
      [d, rotmat, minmax] = checkbox(alpha(i),beta(i),gamma(i),xyz,metric,d,rotmat,minmax);
    end
end

if level>1,
    e = edgelist(K);ne = size(e,1); % get a sorted list of edges
end

if level==2 || level==4,    % check each set of two edges of the convex hull (which obviously includes case '1' above)
    for i = 1:ne,   % go through edge list
        va = xyz(e(i,1),:)-xyz(e(i,2),:);
        va = va/norm(va);
        for j = i+1:ne, % go through remaining edge list
            vb = xyz(e(j,1),:)-xyz(e(j,2),:);
            vb = vb - dot(va,vb)*va; % orthogonaylize second edge wrt first
            nv = cross(va,vb);  % normal to plane formed by the two edges
            if sum(abs(nv))>0,
                vb = vb/norm(vb);nv = nv/norm(nv);
                [alp,bet,gam]=euler123(va,vb,nv);
                [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
            end
        end
    end
end

if level==3 || level==4,    % check edge as parallel to one edge of the bounding box
    for i = 1:ne, % go through edge list
        % calculate rhs with edge as one of the axes
        va = xyz(e(i,1),:)-xyz(e(i,2),:);
        vb = [va(2),-va(1),0];if sum(abs(vb))==0,vb = [va(3),0,-va(1)];end
        va = va/norm(va);vb = vb/norm(vb);
        nv = cross(va,vb);
        % check all combinations of possible rhs
        [alp,bet,gam]=euler123(va,vb,nv);
        [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
        [alp,bet,gam]=euler123(vb,nv,va);
        [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
        [alp,bet,gam]=euler123(nv,va,vb);
        [d, rotmat, minmax] = checkbox(alp,bet,gam,xyz,metric,d,rotmat,minmax);
    end
end

% get the output values
h = [minmax(1,1),minmax(1,2),minmax(1,3);    % xmin,ymin,zmin
     minmax(2,1),minmax(1,2),minmax(1,3);    % xmax,ymin,zmin
     minmax(2,1),minmax(2,2),minmax(1,3);    % xmax,ymax,zmin
     minmax(1,1),minmax(2,2),minmax(1,3);    % xmin,ymax,zmin
     minmax(1,1),minmax(1,2),minmax(2,3);    % xmin,ymin,zmax
     minmax(2,1),minmax(1,2),minmax(2,3);    % xmax,ymin,zmax
     minmax(2,1),minmax(2,2),minmax(2,3);    % xmax,ymax,zmax
     minmax(1,1),minmax(2,2),minmax(2,3);    % xmin,ymax,zmax
     ];

cornerpoints = h*inv(rotmat); % minimal boxes' cornerpoints
h = minmax(2,:)-minmax(1,:);
volume = h(1)*h(2)*h(3);
surface = 2*(h(1)*h(2)+h(2)*h(3)+h(3)*h(1));
edgelength = 4*sum(h);

% all done

end % mainline end

function [alpha,beta,gamma]=euler123(v1,v2,nv)
% calculate Euler angles for the x, y', z'' Euler sequence
% see also
% http://en.wikipedia.org/wiki/Euler_angles
% http://de.wikipedia.org/wiki/Eulersche_Winkel

% --> beta
beta = asin(sign(nv(:,1)).*min(1,abs(nv(:,1))));
% --> alpha
alpha=0*beta;
i1 = find(nv(:,1)==1);i2 = setdiff(1:size(nv,1),i1);
if ~isempty(i1),
    alpha(i1) = asin(sign(v2(i1,3)).*min(1,abs(v2(i1,3))));
end
if ~isempty(i2),
    alpha(i2) = acos(sign(nv(i2,3)./cos(beta(i2))).*min(1,abs(nv(i2,3)./cos(beta(i2)))));
    i3 = find(sign(nv(i2,2)) ~= sign(-sin(alpha(i2)).*cos(beta(i2))));
    alpha(i2(i3)) = -alpha(i2(i3));
end
% --> gamma
gamma = 0*alpha;
if ~isempty(i2),
    singamma = -v2(i2,1)./cos(beta(i2));
    i21 = find(v1(i2,1)>=0);i22=setdiff(1:length(i2),i21);
    gamma(i2(i21)) = asin(sign(singamma(i21)).*min(1,abs(singamma(i21))));
    gamma(i2(i22)) = -pi-asin(sign(singamma(i22)).*min(1,abs(singamma(i22))));
end

end % function euler123

function [d, rotmat, minmax] = checkbox(alpha,beta,gamma,xyz,metric,d,rotmat,minmax)

% will need a 3x3 rotation matrix through (Euler X, Y', Z'')-angles alpha, beta and gamma
Rmat = @(alpha,beta,gamma) [cos(beta)*cos(gamma) -cos(beta)*sin(gamma) sin(beta)
                            sin(alpha)*sin(beta)*cos(gamma)+cos(alpha)*sin(gamma) -sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma) -sin(alpha)*cos(beta)
                            -cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma) cos(alpha)*sin(beta)*sin(gamma)+sin(alpha)*cos(gamma) cos(alpha)*cos(beta)];

rot = Rmat(alpha,beta,gamma);
xyz_i = xyz*rot;                      % now the actual face is in the x-y plane

x_i = xyz_i(:,1);y_i = xyz_i(:,2);          % .. so take only the x and y values
rot2 = minrect(x_i,y_i,metric);             % find the optimal rotation around z-axis
rot = rot*[[rot2,[0;0]];[0,0,1]];           % combine that with the formar rotation
xyz_i = xyz*rot;                            % now again the xyz_i values, but in optimal axisparallel shape

xyzmin = min(xyz_i,[],1);
xyzmax = max(xyz_i,[],1);
h = xyzmax-xyzmin;

if metric == 'v',       % smallest volume
    d_i = h(1)*h(2)*h(3);
elseif metric == 's',   % smallest surface
    d_i = h(1)*h(2)+h(2)*h(3)+h(3)*h(1);
else,                   % smallest sum of edges
    d_i = sum(h);
end

if d_i < d,
    d = d_i;
    rotmat = rot;
    minmax = [xyzmin;xyzmax];
end

end % function checkbox

function rot = minrect(x,y,metric)
% see comments from minboundrect of John d'Errico
% i am only interested in the additional rotation matrix around [0,0,1]
edges = convhull(x,y,{'Qt'});
x = x(edges);y = y(edges);
ind = 1:length(x)-1;
Rmat = @(theta) [cos(theta) sin(theta);-sin(theta) cos(theta)];
edgeangles = atan2(y(ind+1) - y(ind),x(ind+1) - x(ind));edgeangles = unique(mod(edgeangles,pi/2));
nang = length(edgeangles);
area = inf;perimeter = inf;met = inf;xy = [x,y];
for i = 1:nang
  rot_i = Rmat(-edgeangles(i));xyr = xy*rot_i;
  xymin = min(xyr,[],1);xymax = max(xyr,[],1);
  A_i = prod(xymax - xymin);P_i = 2*sum(xymax-xymin);
  if metric=='v', M_i = A_i;else, M_i = P_i;end
  if M_i<met, rot = rot_i;met = M_i;end
end

end % function minrect

function e = edgelist(K)
% generate an edgelist from the convex body K
e = [K(:,1),K(:,2)];e = [e;K(:,2),K(:,3)];e = [e;K(:,1),K(:,3)];
e = [min(e,[],2),max(e,[],2)];ne = size(e,1);
e = [e,e(:,2)+ne*e(:,1)];
[nerd,I] = sort(e(:,3));
e = e(I(1:2:end),:);

end % edgelist