%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This file specifies the F in y'=F(t,y) for the stable indirect adaptive controller
function dzdt=sindwingrockc(t,z)

% Define variables to be global from associated program
global a1 a2 a3 a4 a5 b tau beta0 beta1 thetam
global pbeta R palpha
global c
global sigma
global k1 k0 gamma
global Walpha Wbeta
global Am
global etaalpha etabeta

% Define the various subvectors of the overall closed-loop system state vector

x=z(1:3);
ym=z(4:7);

thetaalpha=z(length(x)+length(ym)+1:length(x)+length(ym)+palpha);
thetabeta=z(length(x)+length(ym)+palpha+1);

% Controller

% Define the phi vector for the alpha approximator

% Next, use only x(1) an x(2) as inputs to the premise

for i=1:R
	mu(i,1)=exp(-0.5*((x(1)-c(1,i))/sigma)^2)*exp(-0.5*((x(2)-c(2,i))/sigma)^2);
end
	
denominator=sum(mu(i,1)); 

xi=(1/denominator)*mu(:,1);

% To use all of the state of the plant to the consequents:

phi=[ xi', x(1)*xi', x(2)*xi', x(3)*xi']';

% Next, specify the estimates of alpha and beta

alphahat=thetaalpha'*phi;
betahat=thetabeta;

% Next, compute the nu signal

yddot=a1*x(1)+a2*x(2)+a3*(x(2)^3)+a4*(x(1)^2)*x(2)+a5*x(1)*(x(2)^2)+b*x(3);

es=(ym(3)-yddot) + k1*(ym(2)-x(2)) + k0*(ym(1)-x(1));

%es=(ym(3)-x(4)) + k1*(ym(2)-x(2)) + k0*(ym(1)-x(1));

esbar=k1*(ym(3)-yddot) + k0*(ym(2)-x(2));

nu=ym(4)+gamma*es+esbar;

% Certainty equivalence controller

uce=(1/betahat)*(-alphahat+nu);  

% Sliding mode term

usi=(1/beta0)*(Walpha+Wbeta*abs(uce))*sgn(es);

% Form the control input to the plant

u=uce+usi;

%u=0; % Use to see how the wing will rock (oscillate) if you use no controller

% Plant calculations

dxdt=x; % Just to force it to be a column

dxdt(1)=x(2);
dxdt(2)=a1*x(1)+a2*x(2)+a3*(x(2)^3)+a4*(x(1)^2)*x(2)+a5*x(1)*(x(2)^2)+b*x(3);
dxdt(3)=-(1/tau)*x(3)+(1/tau)*u;

% Reference model calculations

%dymdt=ym;  % Just to force it to be a column

dymdt=Am*ym;

% Parameter update for adaptation

% Set adaptation gains

% Next, do parameter updates with projection.  Here, we place
% no restrictions on the parameters thetaalpha.  We do, however restrict all
% the parameters of thetabeta to >= beta0 and <= beta1

dthetaalphadt=-etaalpha*phi*es;

thetabetaud=-etabeta*es*uce; % The update normally used when projection is not used

if (thetabeta>=beta1 | thetabeta<=beta0) & (thetabetaud*(thetabeta-thetam)>=0)
	dthetabetadt=0; % Don't update since will not put in valid range
else
	dthetabetadt=-etabeta*es*uce;
end

% Normally this last line would be: dthetabetadt=-etabeta*phi*es*uce;
% but here we remove the phi since we are only using a constant for the 
% approximator

% Form the output vector

dzdt=[dxdt; dymdt; dthetaalphadt; dthetabetadt];

%-----------------
function value=sgn(x)

if x>0, value=1; end
if x<0, value=-1; end


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