function [sys,x0,str,ts] = three_1(t,x,u,flag)
switch flag,
  case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
  case 1,
    sys=mdlDerivatives(t,x,u);
  case 2,
    sys=mdlUpdate(t,x,u);
 case 3,
    sys=mdlOutputs(t,x,u);
 case 4,
    sys=mdlGetTimeOfNextVarHit(t,x,u);
 case 9,
    sys=mdlTerminate(t,x,u);
 otherwise
    error(['Unhandled flag = ',num2str(flag)]);

end
%=============================================================================
%
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;

sizes.NumContStates  = 6;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 6;
sizes.NumInputs      =3;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);
x0  = [0;0;0;0;0;0];
str = [];
ts  = [0 0];
function sys=mdlDerivatives(t,x,u)
m1=2.0;
m2=1.0;
m3=1.0;
r1=0.15;
r2=0.15;
r3=0.15;
l1=0.3;
l2=0.3;
l3=0.3;
I1=0.2;
I2=0.2;
I3=0.2;

T1=u(1);
T2=u(2);
T3=u(3);


a1=m1*r1^2+m2*l1^2+m3*l1^2+I1;
a2=m2*r2^2+m3*l2^2+I2;
a3=m3*r3^2+I3;
b1=m2*l1*r2;
b2=m3*l1*l2;
b3=m3*l2*r3;
b4=m3*l1*r3;
m11=a1+a2+a3+2*(b1+b2)*cos(x(3))+2*b3*cos(x(5))+2*b4*cos(x(3)+x(5));
m12=a2+a3+(b1+b2)*cos(x(3))+2*b3*cos(x(5))+b4*cos(x(3)+x(5));
m21=a2+a3+(b1+b2)*cos(x(3))+2*b3*cos(x(5))+b4*cos(x(3)+x(5));
m13=a3+b3*cos(x(5))+b4*cos(x(3)+x(5));
m31=a3+b3*cos(x(5))+b4*cos(x(3)+x(5));
m22=a2+a3+2*b3*cos(x(5));
m23=a3+b3*cos(x(5));
m32=a3+b3*cos(x(5));
m33=a3;


n111=0;
n212=0;
n221=0;
n222=0;
n313=0;
n331=0;
n333=0;
n323=0;
n332=0;
n112=-b1*sin(u(4))-b2*sin(u(3))-b4*sin(u(3)+u(5));
n121=-b1*sin(u(3))-b2*sin(u(3))-b4*sin(u(3)+u(5));
n122=-b1*sin(u(3))-b2*sin(u(3))-b4*sin(u(3)+u(5));
n211=b1*sin(u(3))+b2*sin(u(3))+b4*sin(u(3)+u(5));
n113=-b3*sin(u(5))-b4*sin(u(3)+u(5));
n131=-b3*sin(u(5))-b4*sin(u(3)+u(5));
n123=-b3*sin(u(5))-b4*sin(u(3)+u(5));
n132=-b3*sin(u(5))-b4*sin(u(3)+u(5));
n133=-b3*sin(u(5))-b4*sin(u(3)+u(5));
n311=b3*sin(u(5))+b4*sin(u(3)+u(5));
n213=-b3*sin(u(5));
n231=-b3*sin(u(5));
n223=-b3*sin(u(5));
n232=-b3*sin(u(5));
n233=-b3*sin(u(5));
n312=b3*sin(u(5));
n321=b3*sin(u(5));
n322=b3*sin(u(5));

k=[x(2) x(4) x(6)]';

b11=[n111 n112 n113]*k;
b12=[n121 n122 n123]*k;
b13=[n131 n132 n133]*k;
b21=[n211 n212 n213]*k;
b22=[n221 n222 n223]*k;
b23=[n231 n232 n233]*k;
b31=[n311 n312 n313]*k;
b32=[n321 n322 n323]*k;
b33=[n331 n332 n333]*k;

M=[m11 m12 m13;m21 m22 m23;m31 m32 m33];
B=[b11 b12 b13;b21 b22 b23;b31 b32 b33];
T=[T1 T2 T3]';
W=[; ;];

sys(1)=x(2);
sys(3)=x(4);
sys(5)=x(6);
W=-inv(M)*B*[x(2);x(4);x(6)]+inv(M)*(T);
sys(2)=W(1);
sys(4)=W(2);
sys(6)=W(3);
function sys=mdlUpdate(t,x,u)

sys = [];
function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);
sys(5)=x(5);
sys(6)=x(6);

function sys=mdlGetTimeOfNextVarHit(t,x,u)

sampleTime = [];    %  Example, set the next hit to be one second later.
function sys=mdlTerminate(t,x,u)

sys = [];