function [Te,Wr,X] = aci(Wr0,X0,U,p)
% This function simulates the dynamic response of Induction machine
% in the stationary reference frame
% Inputs:  
%         Wr0 = Rotor electrically angular velocity (rad/sec)
%         X0  = [psi_rq; psi_rd; i_sq; i_sd] 
%         U   = [v_sq; v_sd] = stator phase voltages  (volt)
%         p = [T; Rs; Rr; Ls; Lr; Lm; np; J; B; Tl; Wb; Ib; Vb; Lb; Tb]
%              1  2   3   4    5   6   7  8  9  10  11  12  13  14  15
% Outputs:
%         Te = Electromagnetic torque (N.m)
%         Wr = Rotor electrically angular velocity (rad/sec)
%         X  = [psi_rq; psi_rd; i_sq; i_sd] 
% where 
%       psi_rq = Stationary q-axis rotor flux linkage (volt.sec/rad)
%       psi_rd = Stationary d-axis rotor flux linkage (volt.sec/rad)
%       i_sq = Stationary q-axis stator current (amp)
%       i_sd = Stationary d-axis stator current (amp)
%       v_sq = Stationary q-axis stator voltage (volt)
%       v_sd = Stationary d-axis stator voltage (volt)
%       T = Sampling period (sec)
%       Rs = Stator resistance (ohm)
%       Rr = Rotor resistance referred to stator (ohm)
%       Ls = Stator self inductance (H)
%       Lr = Rotor self inductance referred to stator (H)
%       Lm = Magnetizing inductance (H)
%       np = Pole pairs
%       J = Moment of inertia of rotor mass (kg.m^2)
%       B = Damping coefficient (N.m.sec/rad) - always ignored
%       Tl = Load torque (N.m)
%       Wb = Base electrically angular velocity (rad/sec)
%       Ib = Base phase current (amp)
%       Vb = Base phase voltage (volt)
%       Lb = Base flux linkage (volt.sec/rad)
%       Tb = Base torque (N.m)

% Defining constants based on machine parameters
sig = 1-p(6)^2/(p(4)*p(5));               % sig = 1-Lm^2/(Ls*Lr)
gam = (p(6)^2*p(3)+p(5)^2*p(2))/(sig*p(4)*p(5)^2); % gam = (Lm^2*Rr+Lr^2*Rs)/(sig*Ls*Lr^2) 
                                          % (depending on Rr)
alp = p(3)/p(5);                          % alp = Rr/Lr (depending on Rr)
Tr = p(5)/p(3);                           % Tr = Lr/Rr, Rotor time constant; depending on Rr
bet = p(6)/(sig*p(4)*p(5));               % bet = Lm/(sig*Ls*Lr)

% Constants
K1 = p(1)*alp;
K2 = p(1)*p(11);
K3 = p(1)*alp*p(6)*p(12)/p(14);
K4 = p(1)*alp*bet*p(14)/p(12);
K5 = p(1)*bet*p(14)*p(11)/p(12);
K6 = p(1)*gam;
K7 = p(1)*(1/(sig*p(4)))*(p(13)/p(12));
K8 = 1.5*p(7)*(p(6)/p(5))*(p(14)*p(12)/p(15));
K9 = p(1)*p(9)/p(8);
K10 = p(1)*p(7)*(1/p(8))*(p(15)/p(11));

% Rotor flux/Stator current calculation
%%%%%% Trapezoidal integration method with damping, alpha = 0.01 %%%%%%%
alpha = 0.01;  % The damping factor for the trapezoidal integration method

% Name the variables
psi_r_beta = X0(1);
psi_r_alfa = X0(2);
ibeta = X0(3);
ialfa = X0(4);
ubeta = U(1);
ualfa = U(2);
wr = Wr0;
load_torque = p(10);

% Predictor
psi_r_beta_p = psi_r_beta - K1*psi_r_beta + K2*wr*psi_r_alfa + K3*ibeta;  
psi_r_alfa_p = psi_r_alfa - K1*psi_r_alfa - K2*wr*psi_r_beta + K3*ialfa;
ibeta_p = ibeta + K4*psi_r_beta - K5*wr*psi_r_alfa - K6*ibeta + K7*ubeta;  
ialfa_p = ialfa + K4*psi_r_alfa + K5*wr*psi_r_beta - K6*ialfa + K7*ualfa;

% Corrector
dpsi_r_beta_p = - K1*psi_r_beta_p + K2*wr*psi_r_alfa_p + K3*ibeta_p;  
dpsi_r_alfa_p = - K1*psi_r_alfa_p - K2*wr*psi_r_beta_p + K3*ialfa_p;
dibeta_p = K4*psi_r_beta_p - K5*wr*psi_r_alfa_p - K6*ibeta_p + K7*ubeta;  
dialfa_p = K4*psi_r_alfa_p + K5*wr*psi_r_beta_p - K6*ialfa_p + K7*ualfa;

%dpsi_r_beta = - K1*psi_r_beta + K2*wr*psi_r_alfa + K3*ibeta;  
%dpsi_r_alfa = - K1*psi_r_alfa - K2*wr*psi_r_beta + K3*ialfa;
%dibeta = K4*psi_r_beta - K5*wr*psi_r_alfa - K6*ibeta + K7*ubeta;  
%dialfa = K4*psi_r_alfa + K5*wr*psi_r_beta - K6*ialfa + K7*ualfa;

dpsi_r_beta = psi_r_beta_p - psi_r_beta;  
dpsi_r_alfa = psi_r_alfa_p - psi_r_alfa;  
dibeta = ibeta_p - ibeta;  
dialfa = ialfa_p - ialfa; 

psi_r_beta = psi_r_beta + 0.5*((1+alpha)*dpsi_r_beta_p + (1-alpha)*dpsi_r_beta);
psi_r_alfa = psi_r_alfa + 0.5*((1+alpha)*dpsi_r_alfa_p + (1-alpha)*dpsi_r_alfa);
 
ibeta = ibeta + 0.5*((1+alpha)*dibeta_p + (1-alpha)*dibeta);
ialfa = ialfa + 0.5*((1+alpha)*dialfa_p + (1-alpha)*dialfa);
  
% Electromagnetic torque calculation
torque = K8*(psi_r_alfa*ibeta - psi_r_beta*ialfa);
   
% Rotor speed calculation */    
wr_p = wr - K9*wr + K10*(torque - load_torque);

dwr_p = - K9*wr_p + K10*(torque - load_torque);
%dwr = - K9*wr + K10*(torque - load_torque);
dwr = wr_p - wr;

wr = wr + 0.5*((1+alpha)*dwr_p + (1-alpha)*dwr);

% Return the results
X(1) = psi_r_beta;
X(2) = psi_r_alfa;
X(3) = ibeta;
X(4) = ialfa;
Wr = wr;
Te = torque;