function DEM_coupled_oscillators
% Dual estimation of the Lorenz system: Cross-validation of Laplace schemes
%__________________________________________________________________________
% This routine illustrates the inversion of a loosely coupled oscillator
% model using generalised filtering. In this example, three regions are
% coupled in terms of their amplitude and phase in a hierarchical fashion.
% Data are generated under a particular set of parameters. The timeseries
% are then transformed using a Hilbert transform into the corresponding
% analytic signal. This then constitutes the data feature for subsequent
% inversion using generalised filtering; here, in four generalised
% coordinates of motion. By assuming fairly precise priors on the amplitude
% of random fluctuations one can recover the parameters and use the
% posterior density for subsequent Bayesian model comparison. In this
% example, we used Bayesian model reduction to assess the evidence for
% models with and without amplitude or phase coupling.
%
% The parameters and orders of this example have been optimised to provide
% proof of principle this sort of  model can be inverted using generalised
% filtering.  The sensitivity to these parameters and orders can be
% assessed numerically by editing the code.
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: DEM_coupled_oscillators.m 7679 2019-10-24 15:54:07Z spm $

% specify states and parameters
%==========================================================================
N     = 128;                             % number of time points
n     = 3;                               % number of sources (oscillators)
Hz    = 8;                               % characteristic frequency (Hz)
dt    = 1/64;                            % sampling interval (sec)

% model states (where hidden states comprise phase differences)
%--------------------------------------------------------------------------
x.r   = zeros(n,1);                      % amplitude
x.p   = zeros(n,1);                      % phase  differences
x.w   = zeros(1,1);                      % phase (common to sources)

% model parameters
%--------------------------------------------------------------------------
A     = spm_speye(n,n,-1);               % form of adjacency matrix
P.L   = diag(ones(n,1));                 % lead field (measurement mapping)
P.Ap  = A/16 - A'/32 - eye(n,n)/16;      % amplitude coupling
P.Ar  = A/16 + A'/4;                     % phase coupling
P.C   = sparse(1,1,1,n,1)/8;             % exogenous input to first source
P.r   = 1/8;                             % weak amplitude
P.w   = 2*pi*Hz*dt;                      % intrinsic frequency

% observation function (to generate timeseries)
%--------------------------------------------------------------------------
g = @(x,v,P) P.L*((x.r).*cos(x.p + x.w));

% equations of motion (simplified coupled oscillator model)
%--------------------------------------------------------------------------
f = @(x,v,P) [P.Ap*(x.r - P.r) + P.C*v;
              sum(P.Ar.*sin(bsxfun(@minus,x.p,x.p')),2) - P.C*v; ...
              P.w];

% causes or exogenous input (a Gaussian function of peristimulus time)
%--------------------------------------------------------------------------
U = exp(-((1:N) - N/2).^2/((N/8)^2));    % exogenous input
T = (1:N)*dt;                            % sample times (seconds)

% parameters for generalised filtering (see spm_LAP)
%--------------------------------------------------------------------------
E.n     = 4;                             % embedding dimension
E.d     = 1;                             % data embedding
E.nN    = 8;                             % number of iterations
E.s     = 1/2;                           % smoothness of fluctuations

% first level state space model
%--------------------------------------------------------------------------
M(1).E  = E;                             % filtering parameters
M(1).x  = x;                             % initial states
M(1).f  = f;                             % equations of motion
M(1).g  = g;                             % observation mapping
M(1).pE = P;                             % model parameters
M(1).V  = exp(12);                       % precision of observation noise
M(1).W  = exp(12);                       % precision of state noise

% second level - causes or exogenous forcing term
%--------------------------------------------------------------------------
M(2).v  = 0;                             % initial causes
M(2).V  = exp(16);                       % precision of exogenous causes

% create data with known parameters (P)
%==========================================================================
DEM = spm_DEM_generate(M,U,P);

% transform analytic signal to create a new data feature
%==========================================================================

% analytic signal (via Hilbert transform)
%--------------------------------------------------------------------------
Y  = spm_hilbert(full(DEM.Y)');          % analytic signal
Yr = abs(Y)';                            % amplitude
Yp = unwrap(angle(Y));                   % phase
Yp = Yp' - ones(n,1)*(1:N)*P.w;          % phase difference
Y  = [Yr; Yp];                           % analytic data feature

% show synthetic data,latent states and exogenous input
%--------------------------------------------------------------------------
spm_figure('GetWin','synthetic data');
spm_DEM_qU(DEM.pU);

subplot(4,2,2), plot(T,DEM.pU.x{1}(1:n,:)')
title('hidden amplitude','FontSize',16)
xlabel('time (seconds)'), spm_axis tight, box off

subplot(4,2,4), plot(T,DEM.pU.x{1}((1:n) + n,:)')
title('phase difference','FontSize',16)
xlabel('time (seconds)'), spm_axis tight, box off

subplot(4,2,6), plot(T,Yr)
title('response amplitude','FontSize',16)
xlabel('time (seconds)'), spm_axis tight, box off

subplot(4,2,8), plot(T,Yp)
title('unwrapped phase','FontSize',16)
xlabel('time (seconds)'), spm_axis tight, box off
drawnow

% Now try to recover model parameters from data features
%==========================================================================

% change observation function (g) to generate analytic signal
%--------------------------------------------------------------------------
g = @(x,v,P) [P.L*x.r; x.p];

% initialization of priors over parameters
%--------------------------------------------------------------------------
pE       = P;                            % prior parameters
pC       = spm_zeros(P);                 % prior variance

pE.Ar    = zeros(n,n);                   % set prior phase and amplitude
pE.Ap    = -speye(n,n)/16;               % coupling parameters to 0
pC.Ar    = (P.Ar ~= 0);                  % and set the prior variance to 1
pC.Ap    = (P.Ap ~= 0);

Vr       = ones(1,n)*8;                  % log precision of sampling noise
Vp       = ones(1,n)*8;                  % and state noise

% place new observation function and priors in generative model
%--------------------------------------------------------------------------
DEM.M(1).g  = g;
DEM.M(1).pE = pE;
DEM.M(1).pC = pC;
DEM.M(1).V  = exp([Vr Vp]);
DEM.M(1).W  = exp([Vr Vp 32]);           % use precise beliefs about time

% data and known input; removing initial time points to suppress artefacts
%--------------------------------------------------------------------------
DEM.Y = Y(:,8:end);
DEM.U = U(:,8:end);

% Inversion using generalised filtering
%==========================================================================
LAP   = spm_DEM(DEM);

% Show parameters
%--------------------------------------------------------------------------
spm_figure('GetWin','Parameters'); clf; spm_DEM_qP(LAP.qP,LAP.pP)
subplot(2,1,1),legend('mean','90% CI','Location','North'), legend(gca,'boxoff')
title('Estimated and true (black) parameters','FontSize',16)

% use Bayesian model reduction to test different hypotheses
%==========================================================================
model{1} = 'no coupling';
model{2} = 'no amplitude coupling';
model{3} = 'no phase coupling';
model{4} = 'Full model';

% apply precise shrinkage priors to off-diagonal coupling elements
%--------------------------------------------------------------------------
PC{1} = pC; PC{1}.Ar = diag(diag(pC.Ar)); PC{1}.Ap = diag(diag(pC.Ap));
PC{2} = pC; PC{2}.Ap = diag(diag(pC.Ap));
PC{3} = pC; PC{3}.Ar = diag(diag(pC.Ar));
PC{4} = pC;

%  evaluate the evidence for these new models or prior constraints
%--------------------------------------------------------------------------
qE    = LAP.qP.P{1};
qC    = LAP.qP.C;
pE    = LAP.M(1).pE;
pC    = LAP.M(1).pC;
for m = 1:numel(PC)
    rC     = diag(spm_vec(PC{m}));
    F(m,1) = spm_log_evidence(qE,qC,pE,pC,pE,rC);
end

% report marginal log likelihood or evidence
%--------------------------------------------------------------------------
F = F - min(F);

spm_figure('GetWin','Model Comparison');clf;
subplot(2,2,1), bar(F,'c')
title('Log evidence','FontSize',16)
xlabel(model), axis square, box off

subplot(2,2,2), bar(spm_softmax(F(:)),'c')
title('Probability','FontSize',16)
xlabel(model), axis square, box off


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


function [sys,x0,str,ts]=ADRC_Two_order(t,x,u,flag,r0,h,B01,B02,B03,D,b0,c,r1,h1)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2
        sys=mdlUpdate(x,u,r0,h,B01,B02,B03,b0,D);
    case 3
        sys=mdlOutputs(x,c,r1,h1,b0);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=5;
    sizes.NumOutputs=1;
    sizes.NumInputs=3;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdate(x,u,r0,h,B01,B02,B03,b0,D)
    e1=x(1)-u(1);
    fh=fhan(e1,x(2),r0,h);
    sys(1)=x(1)+h*x(2);
    sys(2)=x(2)+h*fh;
    e2=x(3)-u(2);
    sys(3)=x(3)+h*(x(4)-B01*e2);
    sys(4)=x(4)+h*(x(5)-B02*fal(e2,0.5,D)+b0*u(3));
    sys(5)=x(5)+h*(-B03*fal(e2,0.25,D));

function sys=mdlOutputs(x,c,r1,h1,b0)
    e3=x(1)-x(3);
    e4=x(2)-x(4);
    sys=-fhan(e3,c*e4,r1,h1)-x(5)/b0;
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;

 function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end

function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function imsegs = APPgetSpStats(imsegs)
% imsegs = APPgetSpStats(imsegs)
% Gets basic information about the superpixels
%
% Copyright(C) Derek Hoiem, Carnegie Mellon University, 2005
% Current Version: 1.0  09/30/2005

for ii = 1:length(imsegs)

	nseg = imsegs(ii).nseg;
	segimage = double( imsegs(ii).segimage );
%     segimage = imsegs(ii).segimage;

    imh = size(segimage, 1);

	adjmat = eye([nseg nseg]);

    % get adjacency
    dx = segimage ~= segimage(:,[2:end end]);
    dy = segimage ~= segimage([2:end end], :);

    ind1 = find(dy);
    ind2 = ind1 + 1;
    s1 = segimage(ind1);
    s2 = segimage(ind2);
%     adjmat(s1 + nseg*(s2-1)) = 1;
%     adjmat(s2 + nseg*(s1-1)) = 1;
    adjmat(sub2ind([nseg, nseg], s1, s2)) = 1;
    adjmat(sub2ind([nseg, nseg], s2, s1)) = 1;

    ind3 = find(dx);
    ind4 = ind3 + imh;
    s3 = segimage(ind3);
    s4 = segimage(ind4);
%     adjmat(s3 + nseg*(s4-1)) = 1;
%     adjmat(s4 + nseg*(s3-1)) = 1;
    adjmat(sub2ind([nseg, nseg], s3, s4)) = 1;
    adjmat(sub2ind([nseg, nseg], s4, s3)) = 1;

%   slower code
% 	[height, width] = size(segimage);
%
% 	for y = 1:height-1
%         for x = 1:width-1
%             s1 = segimage(y, x);
%             s2 = segimage(y+1, x);
%             s3 = segimage(y, x+1);
%             if s1 > 0
%                 npixels(s1) = npixels(s1) + 1;
%                 if s2 > 0
%                     adjmat(s1, s2) = 1;
%                     adjmat(s2, s1) = 1;
%                 end
%                 if s3 > 0
%                     adjmat(s1, s3) = 1;
%                     adjmat(s3, s1) = 1;
%                 end
%             end
%         end
% 	end
%
% 	x = width;
% 	for y = 1:height
%         s1 = segimage(y, x);
%         if s1 > 0
%             npixels(s1) = npixels(s1) + 1;
%         end
% 	end
%
% 	y = height;
% 	for x = 1:width-1
%         s1 = segimage(y, x);
%         if s1 > 0
%             npixels(s1) = npixels(s1) + 1;
%         end
% 	end

    stats = regionprops(segimage, 'Area');
    imsegs(ii).npixels = vertcat(stats(:).Area);
	imsegs(ii).adjmat = logical(adjmat);

end


%WAVE_SIGNIF  Significance testing for the 1D Wavelet transform WAVELET
%
%   [SIGNIF,FFT_THEOR] = ...
%      wave_signif(Y,DT,SCALE,SIGTEST,LAG1,SIGLVL,DOF,MOTHER,PARAM)
%
% INPUTS:
%
%    Y = the time series, or, the VARIANCE of the time series.
%        (If this is a single number, it is assumed to be the variance...)
%    DT = amount of time between each Y value, i.e. the sampling time.
%    SCALE = the vector of scale indices, from previous call to WAVELET.
%
%
% OUTPUTS:
%
%    SIGNIF = significance levels as a function of SCALE
%    FFT_THEOR = output theoretical red-noise spectrum as fn of PERIOD
%
%
% OPTIONAL INPUTS:
% *** Note *** setting any of the following to -1 will cause the default
%               value to be used.
%
%    SIGTEST = 0, 1, or 2.    If omitted, then assume 0.
%
%         If 0 (the default), then just do a regular chi-square test,
%             i.e. Eqn (18) from Torrence & Compo.
%         If 1, then do a "time-average" test, i.e. Eqn (23).
%             In this case, DOF should be set to NA, the number
%             of local wavelet spectra that were averaged together.
%             For the Global Wavelet Spectrum, this would be NA=N,
%             where N is the number of points in your time series.
%         If 2, then do a "scale-average" test, i.e. Eqns (25)-(28).
%             In this case, DOF should be set to a
%             two-element vector [S1,S2], which gives the scale
%             range that was averaged together.
%             e.g. if one scale-averaged scales between 2 and 8,
%             then DOF=[2,8].
%
%    LAG1 = LAG 1 Autocorrelation, used for SIGNIF levels. Default is 0.0
%
%    SIGLVL = significance level to use. Default is 0.95
%
%    DOF = degrees-of-freedom for signif test.
%         IF SIGTEST=0, then (automatically) DOF = 2 (or 1 for MOTHER='DOG')
%         IF SIGTEST=1, then DOF = NA, the number of times averaged together.
%         IF SIGTEST=2, then DOF = [S1,S2], the range of scales averaged.
%
%       Note: IF SIGTEST=1, then DOF can be a vector (same length as SCALEs),
%            in which case NA is assumed to vary with SCALE.
%            This allows one to average different numbers of times
%            together at different scales, or to take into account
%            things like the Cone of Influence.
%            See discussion following Eqn (23) in Torrence & Compo.
%
%
%----------------------------------------------------------------------------
%   Copyright (C) 1995-1998, Christopher Torrence and Gilbert P. Compo
%   University of Colorado, Program in Atmospheric and Oceanic Sciences.
%   This software may be used, copied, or redistributed as long as it is not
%   sold and this copyright notice is reproduced on each copy made.  This
%   routine is provided as is without any express or implied warranties
%   whatsoever.
%----------------------------------------------------------------------------
function [signif,fft_theor] = ...
	wave_signif(Y,dt,scale1,sigtest,lag1,siglvl,dof,mother,param);

if (nargin < 9), param = -1;, end
if (nargin < 8), mother = -1;, end
if (nargin < 7), dof = -1;, end
if (nargin < 6), siglvl = -1;, end
if (nargin < 5), lag1 = -1;, end
if (nargin < 4), sigtest = -1;, end
if (nargin < 3)
	error('Must input a vector Y, sampling time DT, and SCALE vector')
end

n1 = length(Y);
J1 = length(scale1) - 1;
scale(1:J1+1) = scale1;
s0 = min(scale);
dj = log(scale(2)/scale(1))/log(2.);

if (n1 == 1)
	variance = Y;
else
	variance = std(Y)^2;
end

if (sigtest == -1), sigtest = 0;, end
if (lag1 == -1), lag1 = 0.0;, end
if (siglvl == -1), siglvl = 0.95;, end
if (mother == -1), mother = 'MORLET';, end

mother = upper(mother);

% get the appropriate parameters [see Table(2)]
if (strcmp(mother,'MORLET'))  %----------------------------------  Morlet
	if (param == -1), param = 6.;, end
	k0 = param;
	fourier_factor = (4*pi)/(k0 + sqrt(2 + k0^2)); % Scale-->Fourier [Sec.3h]
	empir = [2.,-1,-1,-1];
	if (k0 == 6), empir(2:4)=[0.776,2.32,0.60];, end
elseif (strcmp(mother,'PAUL'))  %--------------------------------  Paul
	if (param == -1), param = 4.;, end
	m = param;
	fourier_factor = 4*pi/(2*m+1);
	empir = [2.,-1,-1,-1];
	if (m == 4), empir(2:4)=[1.132,1.17,1.5];, end
elseif (strcmp(mother,'DOG'))  %---------------------------------  DOG
	if (param == -1), param = 2.;, end
	m = param;
	fourier_factor = 2*pi*sqrt(2./(2*m+1));
	empir = [1.,-1,-1,-1];
	if (m == 2), empir(2:4) = [3.541,1.43,1.4];, end
	if (m == 6), empir(2:4) = [1.966,1.37,0.97];, end
else
	error('Mother must be one of MORLET,PAUL,DOG')
end

period = scale.*fourier_factor;
dofmin = empir(1);     % Degrees of freedom with no smoothing
Cdelta = empir(2);     % reconstruction factor
gamma_fac = empir(3);  % time-decorrelation factor
dj0 = empir(4);        % scale-decorrelation factor

freq = dt ./ period;   % normalized frequency
fft_theor = (1-lag1^2) ./ (1-2*lag1*cos(freq*2*pi)+lag1^2);  % [Eqn(16)]
fft_theor = variance*fft_theor;  % include time-series variance
signif = fft_theor;
if (dof == -1), dof = dofmin;, end

if (sigtest == 0)    % no smoothing, DOF=dofmin [Sec.4]
	dof = dofmin;
	chisquare = chisquare_inv(siglvl,dof)/dof;
	signif = fft_theor*chisquare ;  % [Eqn(18)]
elseif (sigtest == 1)  % time-averaged significance
	if (length(dof) == 1), dof=zeros(1,J1+1)+dof;, end
	truncate = find(dof < 1);
	dof(truncate) = ones(size(truncate));
	dof = dofmin*sqrt(1 + (dof*dt/gamma_fac ./ scale).^2 );   % [Eqn(23)]
	truncate = find(dof < dofmin);
	dof(truncate) = dofmin*ones(size(truncate));   % minimum DOF is dofmin
	for a1 = 1:J1+1
		chisquare = chisquare_inv(siglvl,dof(a1))/dof(a1);
		signif(a1) = fft_theor(a1)*chisquare;
	end
elseif (sigtest == 2)  % time-averaged significance
	if (length(dof) ~= 2)
		error('DOF must be set to [S1,S2], the range of scale-averages')
	end
	if (Cdelta == -1)
		error(['Cdelta & dj0 not defined for ',mother, ...
			' with param = ',num2str(param)])
	end
	s1 = dof(1);
	s2 = dof(2);
	avg = find((scale >= s1) & (scale <= s2));  % scales between S1 & S2
	navg = length(avg);
	if (navg == 0)
		error(['No valid scales between ',num2str(s1),' and ',num2str(s2)])
	end
	Savg = 1./sum(1 ./ scale(avg));       % [Eqn(25)]
	Smid = exp((log(s1)+log(s2))/2.);     % power-of-two midpoint
	dof = (dofmin*navg*Savg/Smid)*sqrt(1 + (navg*dj/dj0)^2);  % [Eqn(28)]
	fft_theor = Savg*sum(fft_theor(avg) ./ scale(avg));  % [Eqn(27)]
	chisquare = chisquare_inv(siglvl,dof)/dof;
	signif = (dj*dt/Cdelta/Savg)*fft_theor*chisquare;    % [Eqn(26)]
else
	error('sigtest must be either 0, 1, or 2')
end

return


% This program is an implementation of the on-line
% Co-Active Neuro-Fuzzy Inference System (CANFIS) algorithm.
% The structure of the network is determoned by the user.
% The premise (nonlinear) parameters are estimated by Gradient Descent (GD)
% through error backpropagation.
% The consequent (linear) parameters are estimated by Recursive LSE.

clc;
clear;
close all;
colordef black;

NumOfEpochs   = 10;
NumOfSamples = 400;
NumInVars         = 6;
NumOutVars      = 3;
NumInTerms      = 2;
lamda                 = 1;
rmse                   = zeros(NumOutVars,NumOfEpochs);
NumRules          = NumInTerms^NumInVars;
Ita1                      = .0015;
Ita2                      = .0025;
alpha1                = 0.00;
alpha2                = 0.00;
P                         = 1e8*eye((NumInVars+1)*NumRules);

% INPUT-OUTPUT training pair construction.
% We will try to teach the ANFIS network the chaotic

load lorenz_data.mat;
Process  = lorenz_dat; % a (3 X 8056) Lorenz Attractor Solution [X Y Z]' Points Data Matrix.

% Target Output is formulated as a matrix whose lines represent different
% output variables and its columns different time instants.

e         = zeros(NumOutVars,NumOfSamples);
TrgtOut   = zeros(NumOutVars,NumOfSamples);
TrgtOutCh = zeros(NumOutVars,NumOfSamples);

ThetaL4  = zeros((NumInVars+1)*NumRules,NumOutVars);  % Consequent Parameters in Layer 4.
dThetaL4 = zeros((NumInVars+1)*NumRules,NumOutVars);

In1           = zeros(NumInVars,NumOfSamples);
In11         = zeros(NumInVars,NumOfSamples);
Out5        = zeros(NumOutVars,NumOfSamples);
Theta21  = zeros(NumInVars,NumInTerms);
Theta32  = zeros(NumRules,NumRules);
mean1    = zeros(NumInVars,NumInTerms);
sigma1   = zeros(NumInVars,NumInTerms);
dmean1  = zeros(NumInVars,NumInTerms);
dsigma1 = zeros(NumInVars,NumInTerms);
db1          = zeros(NumInVars,NumInTerms);

% Each row corresponds to a different Input Linguistic Variable Xi.
% Each i-j entry corresponds to a different mean (center)
% of the bell-shaped function of the j-th Term of the i-th Input Linguistic
% Variable Xi.

Process0 = Process(:,1:NumOfSamples);
Process1 = Process(:,2:NumOfSamples+1);
Process2 = Process(:,3:NumOfSamples+2);

In1 = [Process1; Process0;];
%In1 = Process1;
TrgtOut = Process2;

Process0ch = Process(:,5*NumOfSamples+1:6*NumOfSamples);
Process1ch = Process(:,5*NumOfSamples+2:6*NumOfSamples+1);
Process2ch = Process(:,5*NumOfSamples+3:6*NumOfSamples+2);

%In11 = [Process1ch; Process0ch;];
In11 = Process1ch;
TrgtOutCh = Process2ch;

for i=1:NumInVars
	 mean1(i,:) = linspace(min(In1(i,:)), max(In1(i,:)),NumInTerms);
	 sigma1(i,:) = ((mean1(i,2)-mean1(i,1))/2)*ones(1,NumInTerms);
end
b1     = 2*ones(NumInVars,NumInTerms);

% plotgenbell(mean1,sigma1,b1,[-60,80]);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                			Beginning of the Main "for" loop of the ANFIS  program.        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for ne=1:NumOfEpochs

for n=1:NumOfSamples
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%        								 							     				                                              %
%   			                  	NETWORK FUNCTIONALITY SECTION						      %
%      																					                                              %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% LAYER 1 - INPUT TERM NODES
In2 = In1(:,n)*ones(1,NumInTerms);
Out1 = 1./(1 + (abs((In2-mean1)./sigma1)).^(2*b1));

% LAYER 2 - PRODUCT NODES
precond = comb(Out1);
Out2 = prod(precond,2);
S_2 = sum(Out2);

% LAYER 3 - NORMALIZATION NODES
 Out3 = Out2/S_2;

% LAYERS 4 - 5: CONSEQUENT NODES - SUMMING NODE
Aux1 = [In1(:,n); 1]*Out3';

a = reshape(Aux1,(NumInVars+1)*NumRules,1);  % New Input Training Data shaped as a column vector.

Out5(:,n) = ThetaL4'*a;

e(:,n) = TrgtOut(:,n)-Out5(:,n);

%%%%%%%%%%% END OF NETWORK FUNCTIONALITY SECTION %%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%			 					PARAMETER LEARNING SECTION	                    			 %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% BACKWARD PASS. Error Backpropagation

% We may adjust the Consequent Parameters by LMS.
 for m=1:NumOutVars
     deltaThetaL4 = a*e(m,n);
     dThetaL4(:,m) = Ita1*deltaThetaL4 + alpha1*dThetaL4(:,m);           % Classic LMS.
 end

% LAYER 4
for m=1:NumOutVars
        ThetaL4_mat = reshape(ThetaL4(:,m),NumInVars+1,NumRules);
        f(m,:) = [In1(:,n)' 1]*ThetaL4_mat*e(m,n);
end

% LAYER 3
e3 = sum(f,1);
denom = S_2*S_2;

% LAYER 2
ThetaE32 = zeros(NumRules,NumRules);
if denom~=0
    for k1=1:NumRules
         for k2=1:NumRules
              if k1==k2
                 ThetaE32(k1,k2) = ((S_2-Out2(k2))/denom)*e3(k2);
              else
                 ThetaE32(k1,k2) = -(Out2(k2)/denom)*e3(k2);
              end
         end
    end

else
    continue;
end

e2 = sum(ThetaE32,2);

% LAYER 1
Q = zeros(NumInVars,NumInTerms,NumRules);
for i=1:NumInVars
     for j=1:NumInTerms
          for k=1:NumRules
      	       if Out1(i,j)== precond(k,i) && Out1(i,j)~=0
      			  Q(i,j,k) = (Out2(k)/Out1(i,j))*e2(k);
               end
          end
      end
 end

ThetaE21 = sum(Q,3);

% LAYER 1 PARAMETER ADJUSTMENT BY GRADIENT DESCENT.
deltamean1   = -ThetaE21.*(2*b1./(In2-mean1)).*Out1.*(1-Out1);
deltab1          = -ThetaE21.*(-2).*log(abs((In2-mean1)./sigma1)).*Out1.*(1-Out1);
deltasigma1  = -ThetaE21.*(2*b1./sigma1).*Out1.*(1-Out1);

% dmean1 = -Ita2*deltamean1 + alpha2*dmean1;
% mean1 = mean1 + dmean1;
%
% dsigma1 = -Ita2*deltasigma1 + alpha2*dsigma1;
% sigma1 = sigma1 + dsigma1;
%
% db1 = -Ita2*deltab1 + alpha2*db1;
% b1 = b1 + db1;

% Now update the Layer 4 linear parameters.
ThetaL4_old = ThetaL4;  % Keep here the old values of ThetaL4.
% Fixing of Consequent Parameters by LMS.
ThetaL4  = ThetaL4 + dThetaL4;

% Fixing of Consequent Parameters by Recursive LSE.
% P = (1./lamda).*(P - P*a*a'*P./(lamda+a'*P*a));
% ThetaL4 = ThetaL4 + P*a*e(:,n)';

%%%%%%%%%%%% END OF PARAMETER LEARNING PROCESS %%%%%%%%%%%%

end %Of for i=1:NumOfSamples loop.

for m=1:NumOutVars
    rmse(m,ne) = norm(e(m,:))/sqrt(NumOfSamples);
end

rmse(:,ne)

end   %Of for i=1:NumOfEpochs loop.

figure
subplot(3,1,1),plot(rmse(1,:),'*r');
title('RMSE for X-coordinate');
grid on;

subplot(3,1,2),plot(rmse(2,:),'*g');
title('RMSE for Y-coordinate');
grid on;

subplot(3,1,3),plot(rmse(3,:),'*b');
title('RMSE for Z-coordinate');
grid on;

X = 1:NumOfSamples;
figure;
subplot(3,2,1)
plot(X,TrgtOut(1,:),'r',X,Out5(1,:),'m');
title('Training Target X-coordinate (Red) VS ANFIS X Output (Magenta)')
grid on;

subplot(3,2,2)
plot(X,e(1,:),'r')
title('Training Error(n) for X-coordinate')
grid on;

subplot(3,2,3)
plot(X,TrgtOut(2,:),'b',X,Out5(2,:),'c');
title('Training Target Y-coordinate (Blue) VS ANFIS Y Output (Cyan)')
grid on;

subplot(3,2,4)
plot(X,e(2,:),'b')
title('Training Error(n) for Y-coordinate')
grid on;

subplot(3,2,5)
plot(X,TrgtOut(3,:),'g',X,Out5(3,:),'y');
title('Training Target Z-coordinate (Green) VS ANFIS Z Output (Yellow)')
grid on;

subplot(3,2,6)
plot(X,e(3,:),'g')
title('Training Error(n) for Z-coordinate')
grid on

% plotgenbell(mean1,sigma1,b1,[-60,80]);

%% %%% %%%%%%% Gradient Checking Section %%%%%%%%%%%%%
ThetaJ_mean = zeros(NumInVars,NumInTerms);
ThetaJ_sigma = zeros(NumInVars,NumInTerms);
ThetaJ_b         = zeros(NumInVars,NumInTerms);
epsilon             = 1e-4;

for i=1:NumInVars
    for j=1:NumInTerms

        mean1_up = mean1;
        mean1_do = mean1;

        mean1_up(i,j) = mean1(i,j) + epsilon;
        mean1_do(i,j) = mean1(i,j) - epsilon;

        J_up_mean = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1_up,sigma1,b1,ThetaL4_old)).^2);
        J_do_mean = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1_do,sigma1,b1,ThetaL4_old)).^2);

        ThetaJ_mean(i,j) = (J_up_mean - J_do_mean)/2/epsilon;

        sigma1_up = sigma1;
        sigma1_do = sigma1;

        sigma1_up(i,j) = sigma1(i,j) + epsilon;
        sigma1_do(i,j) = sigma1(i,j) - epsilon;

        J_up_sigma = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1_up,b1,ThetaL4_old)).^2);
        J_do_sigma = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1_do,b1,ThetaL4_old)).^2);

        ThetaJ_sigma(i,j) = (J_up_sigma - J_do_sigma)/2/epsilon;

        b1_up = b1;
        b1_do = b1;

        b1_up(i,j) = b1(i,j) + epsilon;
        b1_do(i,j) = b1(i,j) - epsilon;

        J_up_b = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1,b1_up,ThetaL4_old)).^2);
        J_do_b = sum(1/2*(TrgtOut(:,n) - canfis_grid_forward(In1(:,n),mean1,sigma1,b1_do,ThetaL4_old)).^2);

        ThetaJ_b(i,j) = (J_up_b - J_do_b)/2/epsilon;

    end
end

rel_diff_mean  = deltamean1(:) - ThetaJ_mean(:);
rel_diff_sigma = deltasigma1(:) - ThetaJ_sigma(:);
rel_diff_b         = deltab1(:) - ThetaJ_b(:);

[rel_diff_mean rel_diff_sigma rel_diff_b]

[deltamean1(:)   ThetaJ_mean(:) ]

[deltasigma1(:)  ThetaJ_sigma(:)]

[deltab1(:)           ThetaJ_b(:)        ]