function [y1, y2] = att_ctrl(gamma,u,quad)
%#codegen
%State Measurements


xE  = u(1);  yE = u(2);  zE = u(3);
phi = u(4); the = u(5); psi = u(6);
vx  = u(7);  vy = u(8);  vz = u(9);
p   = u(10);  q = u(11);  r = u(12);

%Commands
the_d = u(13); the_d_dot = u(14); the_d_ddot = u(15);
phi_d = u(16); phi_d_dot = u(17); phi_d_ddot = u(18);
z_d   = u(19);   z_d_dot = u(20);   z_d_ddot = u(21);
psi_d = u(22); psi_d_dot = u(23); psi_d_ddot = u(24);

% If there is a fault configured we will do different adaptive laws
% has_fault=fault(1,2);

%LOE
theta_hat=[0;0;0;0];
theta_hat(1) = u(25);
theta_hat(2) = u(26);
theta_hat(3) = u(27);
theta_hat(4) = u(28);

% This is used for I4-Lambda
theta_I = diag([1-theta_hat(1) 1-theta_hat(2) 1-theta_hat(3) 1-theta_hat(4)]);

% The q1/alpha function derivitives
q1_dot=[u(29); u(30); u(31); u(32)];

% Lambda matrices
lam_1=diag([1;0;0;0]);
lam_2=diag([0;1;0;0]);
lam_3=diag([0;0;1;0]);
lam_4=diag([0;0;0;1]);


Cd = quad.Cd;
m  = quad.M; g = quad.g;
Jx = quad.Ixx; Jy = quad.Iyy; Jz = quad.Izz;
Jbarx = (Jy-Jz)/Jx; Jbary = (Jz-Jx)/Jy; Jbarz = (Jx-Jy)/Jz;
d = quad.d; b = quad.b; k = quad.k;
sq2=sqrt(2);
Map = [b     b     b    b;
       d*b/sq2  -d*b/sq2  -d*b/sq2  d*b/sq2;
       d*b/sq2   d*b/sq2  -d*b/sq2 -d*b/sq2;
       k    -k     k   -k ];

% define g1
g1 = [ 1 0        0                 0 ;
       0 1 sin(phi)*tan(the) cos(phi)*tan(the);
       0 0 cos(phi) -sin(phi);
       0 0 sin(phi)*sec(the) cos(phi)*sec(the)];

% define f2
f2 = [ g-(Cd/m)*vz;
       Jbarx*q*r;
       Jbary*p*r;
       Jbarz*p*q];

% define g2
g2 = [ -(cos(phi)*cos(the))/m  0  0   0;
          0  1/Jx   0     0;
          0   0   1/Jy    0;
          0   0    0    1/Jz];

% set the variables for the backstepping controller here
x1 = [zE; phi; the; psi];
% these are the desired positions/angles
x1_d = [z_d; phi_d; the_d; psi_d];
% derivitives
x1_d_dot = [z_d_dot; phi_d_dot; the_d_dot; psi_d_dot];


% control variables we want to drive to 0
x2 = [vz; p; q; r];

z1 = x1-x1_d;
%q1
k1=diag([1 1 1 1]);
k2=diag([1 1 1 1]);
c1=diag([1 1 1 1]);
c2=diag([1 1 1 1]);
c3=diag([10 10 10 10]);

%q1=inv(k1*g1)*(-k1*z1-k2*(x1-x1_d)+k1*x1_d_dot);
q1 = (c1*g1)\(c1*x1_d_dot - c2*z1);

z2 = x2 - q1;

q1_dot = [0;0;0;0];

Omega_bar = (g2*Map*theta_I)\(q1_dot - f2 - (c1*g1)'*z1-c3*z2);

w_squared = Omega_bar;

% adaptive laws for each rotor
THETA_h_dot = [0;0;0;0];
% if has_fault>0
    THETA_h_dot(1) = -gamma*(z2'*g2*Map*lam_1*Omega_bar);
    THETA_h_dot(2) = -gamma*(z2'*g2*Map*lam_2*Omega_bar);
    THETA_h_dot(3) = -gamma*(z2'*g2*Map*lam_3*Omega_bar);
    THETA_h_dot(4) = -gamma*(z2'*g2*Map*lam_4*Omega_bar);
% end

w1 =  sqrt(abs(w_squared(1)));
w2 = -sqrt(abs(w_squared(2)));
w3 =  sqrt(abs(w_squared(3)));
w4 = -sqrt(abs(w_squared(4)));
w = [w1;w2;w3;w4];

y1 = [w;theta_hat];
y2 = [THETA_h_dot;q1];