%
% Symbolic code for WFSM
%
syms lss l0 l1 m0 np th lrr lf mr ld lq lsr msf msd msq ...
    m3to2 cv mdq m3dq lssdq lsrdq lrsdq psidq psid psiq psih ...
    edq om idq id iq ih ir ifc iD iQ tm real
%
% Original model
%
a=2*pi/3;
lss=[l0+l1*cos(2*np*th) m0+l1*cos(2*np*th-a) m0+l1*cos(2*np*th+a);
     m0+l1*cos(2*np*th-a) l0+l1*cos(2*np*th+a) m0+l1*cos(2*np*th) ;
     m0+l1*cos(2*np*th+a) m0+l1*cos(2*np*th) l0+l1*cos(2*np*th-a)]; 
lrr=[lf mr 0;mr ld 0;0 0 lq];
lsr=[msf*cos(np*th) msd*cos(np*th) msq*cos(np*th+pi/2); ...
    msf*cos(np*th-a) msd*cos(np*th-a) msq*cos(np*th-a+pi/2);
    msf*cos(np*th+a) msd*cos(np*th+a) msq*cos(np*th+a+pi/2)];
%
% DQ model
%
m3to2=cv*[1 -1/2 -1/2;0 sqrt(3)/2 -sqrt(3)/2;1/sqrt(2) ...
    1/sqrt(2) 1/sqrt(2)];
mdq=[cos(np*th) sin(np*th) 0;-sin(np*th) cos(np*th) 0;0 0 1];
m3dq=simplify(mdq*m3to2)
lssdq=simplify(m3dq*lss*inv(m3dq))    
lsrdq=simplify(m3dq*lsr)
lrsdq=simplify(lsr'*inv(m3dq))
psidq=[psid;psiq;psih];
edq=simplify(om*diff(m3dq,th)*inv(m3dq)*psidq)  
idq=[id;iq;ih];ir=[ifc;iD;iQ];
tm=(1/2)*idq'*inv(m3dq)'*diff(lss,th)*inv(m3dq)*idq;
tm=simplify(tm+idq'*inv(m3dq)'*diff(lsr,th)*ir)