clc; clear; close all;
try 
    load('analytics_reduction.mat')
catch
    syms x y  th1 th2 dx dy dbeta dth1 dth2 ddx ddy ddbeta ddth1 ddth2 ... % beta
    L m mx my mt I k c real
    beta = sym('beta','real');
    % All bodies are equal
    L0 = L;    L1 = L;    L2 = L;
    m0 = m;    m1 = m;    m2 = m;
    I0 = I;    I1 = I;    I2 = I;
    th1_0 = 0;
    delta_mi = 1;
    
    M_i = delta_mi*diag([mx , my , mt]);                 %added mass matrix
    M0 = diag([m0 m0 I0]);  M1 = diag([m1 m1 I1]);  M2 = diag([m2 m2 I2]); %mass matrix

    % velocity:
    % velocity In link center of mass coordinate system
    V0_w = [dx  , dy , dbeta].';
    V1_w = [dx+L0*sin(beta)*dbeta+L1*sin(beta+th1)*(dbeta+dth1) , dy-L0*cos(beta)*dbeta-L1*cos(beta+th1)*(dbeta+dth1) , dbeta+dth1].';
    V2_w = [dx-L0*sin(beta)*dbeta-L2*sin(beta+th2)*(dbeta+dth2) , dy+L0*cos(beta)*dbeta+L2*cos(beta+th2)*(dbeta+dth2) , dbeta+dth2].';

    Rotation_mat = @(angle) [cos(angle) sin(angle) 0 ; -sin(angle) cos(angle) 0 ; 0 0 1]; %Rotation matrix
    % velocity In link fixed coordinate system
    V0 = Rotation_mat(beta)*V0_w;
    V1 = Rotation_mat(beta+th1)*V1_w;
    V2 = Rotation_mat(beta+th2)*V2_w;

    U = 0.5*k*(th1-th1_0)^2; %Potential energy

    R = 0.5*c*(dth1)^2; %dissipation energy

    T_tot = (0.5*(V0.'*(M_i+M0)*V0 + V1.'*(M_i+M1)*V1 + V2.'*(M_i+M2)*V2)); %kinetic energy

    Lagrangian = T_tot - U;
    X   = {x dx y dy beta dbeta th1 dth1 th2 dth2};
    DDX = {ddx ddy ddbeta ddth1 ddth2};

    [H,B] = MY_EulerLagrange_H(Lagrangian,R,X,DDX); 

%% ODE reduction to be dependent only by (th1,th2,t)
    R_rot = [cos(beta) -sin(beta) 0 0 0
             sin(beta) cos(beta) 0 0 0
             0         0         1 0 0
             0         0         0 1 0
             0         0         0 0 1];
    dR_rot = [-dbeta*sin(beta) -dbeta*cos(beta) 0 0 0
               dbeta*cos(beta) -dbeta*sin(beta) 0 0 0
               0         0         0 0 0
               0         0         0 0 0
               0         0         0 0 0];
%% body system is removing the dependence on beta  
    V_b = [V0;dth1;dth2];
    H_0 = simplify(R_rot'*H*R_rot);
    B_0 = simplify(R_rot'*(H*dR_rot*V_b+B));

%% switching from dx dy to vx vy - bidy system velocity
    syms vx vy real
    ss = solve([vx==V0(1) , vy==V0(2)],[dx,dy]);
    B_0 = simplify(subs(B_0,[dx,dy],[ss.dx,ss.dy]));

    Bb = B_0(1:3);
    Bs = B_0(4:5);
    Hbb = H_0(1:3,1:3);
    Hbs = H_0(1:3,4:5);
    Hss = H_0(4:5,4:5);
    
%% Zero momentum conservation eq is removing the dependence on vx vy dbeta
    invHbb = inv(Hbb);
    Vb_ZMC = simplify(-invHbb*Hbs*[dth1 dth2]'); %Zero momentum conservation 
    Bb = subs(Bb,[vx,vy,dbeta],Vb_ZMC');
    dVb_temp = Hbs*[ddth1;ddth2] + Bb;
    dVb = -invHbb*dVb_temp;
    % dVb = (-inv(Hbb)*(Hbs*[ddth1;ddth2]+subs(Bb,[vx,vy,dbeta],ZMC')))

    Bs = subs(Bs,[vx,vy,dbeta],Vb_ZMC');
    eq4 = Hbs'*dVb+Hss*[ddth1;ddth2]+Bs;

    [cxy, txy] = coeffs(eq4(1),[ddth1 ddth2]);
    H_t = cxy(1:2);
    B_t = cxy(3);
    [cxy, txy] = coeffs(eq4(2),[ddth1 ddth2]);
    H_t = [H_t;cxy(1:2)];
    B_t = [B_t ; cxy(3)];
    %%
    save('analytics_reduction.mat','H_t','B_t','th1','th2','dth1','dth2','ddth1','ddth2','k','c')%,'m11','m12')
end

%% Perturbation and approximate solution
%%% 
if ~isfile('f_ddth1_ana.m')||~isfile('f_dth1_ana.m')||~isfile('f_th1_ana.m')||~isfile('f_a_th1_ana.m')||~isfile('f_b_th1_ana.m')
    M_11 = simplify(subs(H_t(1,1),[th1,th2,dth1,dth2,ddth1,ddth2],[0,0,0,0,0,0]));
    M_12 = simplify(subs(H_t(1,2),[th1,th2,dth1,dth2,ddth1,ddth2],[0,0,0,0,0,0]));
    matlabFunction(M_11,'File','M_11_in0');
    matlabFunction(M_12,'File','M_12_in0');

    %%% Steady periodic solution of ODE
    syms A B m11 m12 w t epsilon real
    odesol = A*sin(w*t)+B*cos(w*t);
    fs = odesol;
    dfs = diff(odesol,t);
    ddfs = diff(odesol,t,2);
    ode = m11*ddfs+c*dfs+k*fs-m12*w^2*cos(w*t);
    %%% ode steady periodic solution
    [csc, tsc] = coeffs(ode,[sin(w*t) cos(w*t)]);
    eq4 = csc;
    [solA, solB] = solve(eq4,[A,B]);
    f1_th1 = subs(odesol,[A,B],[solA, solB]); %% th1 = ep*f1_th1
    df1_th1 = simplify(diff(f1_th1,t));           %% dth1  = ep*df1_th1
    ddf1_th1 = simplify(diff(f1_th1,t,2));

    th1_ana = epsilon*subs(f1_th1,[m11,m12],[M_11,M_12]);
    dth1_ana = epsilon*subs(df1_th1,[m11,m12],[M_11,M_12]);
    ddth1_ana = epsilon*subs(ddf1_th1,[m11,m12],[M_11,M_12]);

    matlabFunction(th1_ana,'File','f_th1_ana');
    matlabFunction(dth1_ana,'File','f_dth1_ana');
    matlabFunction(ddth1_ana,'File','f_ddth1_ana');
    
    %%% epsilon*(a_th*sin(w*t)+b_th*cos(w*t)
    [csc, tsc] = coeffs(f1_th1,[sin(w*t) cos(w*t)]);
    a_th1_ana = subs(csc(1),[m11,m12],[M_11,M_12]);
    b_th1_ana = subs(csc(2),[m11,m12],[M_11,M_12]);
    matlabFunction(a_th1_ana,'File','f_a_th1_ana');
    matlabFunction(b_th1_ana,'File','f_b_th1_ana');
end


%% Variationla equation
ode = H_t(1,1)*ddth1 + H_t(1,2)*ddth2 + B_t(1);

[csc, tsc] = coeffs(ode,[ddth1 dth1 ddth2 dth2]);

coeff_ddth1 = csc(1);
coeff_dth1dth1 = csc(2);
coeff_dth1dth2 = csc(3);
coeff_dth1 = csc(4);
coeff_ddth2 = csc(5);
coeff_dth2dth2 = csc(6);
coeff_th1 = csc(7);

m11_tilde = coeff_ddth1;
m12_tilde = coeff_ddth2;
b11_tilde = coeff_dth1dth1;
b12_tilde = coeff_dth1dth2;
b22_tilde = coeff_dth2dth2;

syms th1_bar dth1_bar ddth1_bar del ddel dddel real

b2_second_order = simplify(subs(m11_tilde,[th1,th2],[0,0]) + subs(diff(m11_tilde,th1),[th1,th2],[0,0])*th1_bar + subs(diff(m11_tilde,th2),[th1,th2],[0,0])*th2 + 0.5*subs(diff(m11_tilde,th1,2),[th1,th2],[0,0])*th1_bar^2 + 0.5*subs(diff(m11_tilde,th2,2),[th1,th2],[0,0])*th2^2 +subs(diff(diff(m11_tilde,th1),th2),[th1,th2],[0,0])*th1_bar*th2);
b1_second_order = simplify(2*((subs(b11_tilde,[th1,th2],[0,0])) + subs(diff(b11_tilde,th1),[th1,th2],[0,0])*th1_bar + subs(diff(b11_tilde,th2),[th1,th2],[0,0])*th2)*dth1_bar + (subs(b12_tilde,[th1,th2],[0,0]) + subs(diff(b12_tilde,th1),[th1,th2],[0,0])*th1_bar + subs(diff(b12_tilde,th2),[th1,th2],[0,0])*th2)*dth2 + c);
b0_second_order = simplify((subs(diff(m11_tilde,th1),[th1,th2],[0,0]) + subs(diff(m11_tilde,th1,2),[th1,th2],[0,0])*th1_bar + subs(diff(diff(m11_tilde,th1),th2),[th1,th2],[0,0])*th2)*ddth1_bar + (subs(diff(m12_tilde,th1),[th1,th2],[0,0]) + subs(diff(m12_tilde,th1,2),[th1,th2],[0,0])*th1_bar + subs(diff(diff(m12_tilde,th1),th2),[th1,th2],[0,0])*th2)*ddth2 + subs(diff(b11_tilde,th1),[th1,th2],[0,0])*dth1_bar^2 + subs(diff(b12_tilde,th1),[th1,th2],[0,0])*dth1_bar*dth2 + subs(diff(b22_tilde,th1),[th1,th2],[0,0])*dth2^2 + k);

% matlabFunction(b2_second_order,'File','f_variational_b2_second_order','Var',[th1_bar, dth1_bar , ddth1_bar , th2 , dth2 , ddth2 , I, L , c , k, m, mt, mx, my]);
% matlabFunction(b1_second_order,'File','f_variational_b1_second_order','Var',[th1_bar, dth1_bar , ddth1_bar , th2 , dth2 , ddth2 , I, L , c , k, m, mt, mx, my]);
% matlabFunction(b0_second_order,'File','f_variational_b0_second_order','Var',[th1_bar, dth1_bar , ddth1_bar , th2 , dth2 , ddth2 , I, L , c , k, m, mt, mx, my]);
%% Hill equation
%In the paper, we use the notation \tilde{b}_0, \tilde{b}_1, and
%\tilde{b_2}, instead of b0, b1, and b2, respectively.
b2 = simplify(b2_second_order,'Steps', 100);
b1 = simplify(b1_second_order,'Steps', 100);
b0 = simplify(b0_second_order,'Steps', 100);

[N_b2, D_b2] = numden(b2);
[Ncb2, Ntb2] = coeffs(N_b2,[th1_bar th2]);
[Ncb2_simp] = simplify_vec(Ncb2);
D_b2=simplify(D_b2);

[N_b1, D_b1] = numden(b1);
[Ncb1, Ntb1] = coeffs(N_b1,[th1_bar th2 dth1_bar dth2]);
[Ncb1_simp] = simplify_vec(Ncb1);
D_b1=simplify(D_b1);

[N_b0, D_b0] = numden(b0);
[Ncb0, Ntb0] = coeffs(N_b0,[th1_bar th2 dth1_bar dth2 ddth1_bar ddth2]);
[Ncb0_simp] = simplify_vec(Ncb0);
D_b0=simplify(D_b0);

charNtb2 = char(Ntb2); charNtb2([1:8, end-1:end]) = [];
charNtb1 = char(Ntb1); charNtb1([1:8, end-1:end]) = [];
charNtb0 = char(Ntb0); charNtb0([1:8, end-1:end]) = [];

comm = ["b2*dddel + b1*ddel + b0*del = 0"  " alpha_i*"  charNtb2 " beta_i*"  charNtb1  " gamma_i*" charNtb0];
matlabFunction(Ncb2_simp./D_b2,'File','f_hill_b2_alpha_i','Comments',comm);
matlabFunction(Ncb1_simp./D_b1,'File','f_hill_b1_beta_i','Comments',comm);
matlabFunction(Ncb0_simp./D_b0,'File','f_hill_b0_gamma_i','Comments',comm);

%% Hill's Determinante
syms th1_bar dth1_bar ddth1_bar th2 dth2 ddth2 ...
alpha_1 alpha_2 alpha_3 alpha_4 ... 
beta_1 beta_2 beta_3 beta_4 beta_5 ...
gamma_1 gamma_2 gamma_3 gamma_4 gamma_5 gamma_6 gamma_7 gamma_8...
a_th b_th w epsilon t real

b2 = alpha_1*th1_bar^2 + alpha_2*th2*th1_bar + alpha_3*th2^2 + alpha_4;
b1 = beta_1*dth1_bar*th1_bar + beta_2*dth2*th1_bar + beta_3*dth1_bar*th2 + beta_4*dth2*th2 + beta_5;
b0 = gamma_1*ddth1_bar*th1_bar + gamma_2*ddth2*th1_bar + gamma_3*ddth1_bar*th2 + gamma_4*ddth2*th2 + gamma_5*dth1_bar^2 + gamma_6*dth2*dth1_bar + gamma_7*dth2^2 + gamma_8;

b2_subs = subs(b2,[th1_bar dth1_bar ddth1_bar th2 dth2 ddth2] , [epsilon*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*w*(a_th*cos(w*t)-b_th*sin(w*t)) , -epsilon*w^2*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*cos(w*t) , -epsilon*w*sin(w*t) , -epsilon*w^2*cos(w*t)]);
b1_subs = subs(b1,[th1_bar dth1_bar ddth1_bar th2 dth2 ddth2] , [epsilon*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*w*(a_th*cos(w*t)-b_th*sin(w*t)) , -epsilon*w^2*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*cos(w*t) , -epsilon*w*sin(w*t) , -epsilon*w^2*cos(w*t)]);
b0_subs = subs(b0,[th1_bar dth1_bar ddth1_bar th2 dth2 ddth2] , [epsilon*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*w*(a_th*cos(w*t)-b_th*sin(w*t)) , -epsilon*w^2*(a_th*sin(w*t)+b_th*cos(w*t)) , epsilon*cos(w*t) , -epsilon*w*sin(w*t) , -epsilon*w^2*cos(w*t)]);
%%
b2_simplify = sin2_cossin_cos2_to_sincos2(b2_subs);
b1_simplify = sin2_cossin_cos2_to_sincos2(b1_subs);
b0_simplify = sin2_cossin_cos2_to_sincos2(b0_subs);

%% order 2
syms M0 M2 N2 real
del_bar2 = M0+M2*cos(2*w*t)+N2*sin(2*w*t);
ddel_bar2 = 2*w*(-M2*sin(2*w*t)+N2*cos(2*w*t));
dddel_bar2 = -4*w^2*(M2*cos(2*w*t)+N2*sin(2*w*t));

eq2 = b2_simplify*dddel_bar2 + b1_simplify*ddel_bar2 + b0_simplify*del_bar2;
% [csc, tsc] = coeffs(eq,[sin(2*w*t),cos(2*w*t)]);
eq_simp2 = sin2_cossin_cos2_to_sincos4(eq2);
[csc, tsc] = coeffs(eq_simp2,[sin(2*w*t),cos(2*w*t) sin(4*w*t) cos(4*w*t)]);
[csc1, tsc1] = coeffs(csc(1)+(M0+M2+N2),[M0 M2 N2]);
[csc2, tsc2] = coeffs(csc(2)+(M0+M2+N2),[M0 M2 N2]);
[csc3, tsc3] = coeffs(csc(3)+(M0+M2+N2),[M0 M2 N2]);
[csc4, tsc4] = coeffs(csc(4)+(M0+M2+N2),[M0 M2 N2]);
[csc5, tsc5] = coeffs(csc(5)+(M0+M2+N2),[M0 M2 N2]);

Hill_H3x3 = simplify([csc5(1)-1 , csc5(2)-1 csc5(3)-1
    csc1(1)-1 , csc1(2)-1 csc1(3)-1
    csc2(1)-1 , csc2(2)-1 csc2(3)-1]);

det_Hill_H3x3 = simplify(det(Hill_H3x3));
det_Hill_H3x3_f = matlabFunction(det_Hill_H3x3,'File','f_det_Hill_H_dellbarorder_2');

%%

syms m11 m12 real
% form_a_th = -(c*w^3*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 4*L^4*m^2 + 8*L^4*m*my + 4*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(k^2 + c^2*w^2 + (4*w^4*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2)^2)/(9*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)^2) - (4*k*w^2*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)))*(3*I + 3*mt + 8*L^2*m + 8*L^2*my));
form_a_th = (c*m12*w^3)/(c^2*w^2+k^2-2*k*m11*w^2+m11^2*w^4);
% form_b_th = -(w^2*(k - (2*w^2*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)))*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 4*L^4*m^2 + 8*L^4*m*my + 4*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(k^2 + c^2*w^2 + (4*w^4*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2)^2)/(9*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)^2) - (4*k*w^2*(3*I^2 + 9*I*L^2*m + 9*I*L^2*my + 6*I*mt + 2*L^4*m^2 + 4*L^4*m*my + 2*L^4*my^2 + 9*L^2*m*mt + 9*L^2*mt*my + 3*mt^2))/(3*(3*I + 3*mt + 8*L^2*m + 8*L^2*my)))*(3*I + 3*mt + 8*L^2*m + 8*L^2*my));
form_b_th = (m12*w^2*(k-m11*w^2))/(c^2*w^2+k^2-2*k*m11*w^2+m11^2*w^4);
Hill_H3X3_a_b_th = subs(Hill_H3x3,[a_th,b_th],[form_a_th,form_b_th]);

%%
syms a_th b_th real
det_Hill_H3X3_a_b_th = subs(det_Hill_H3x3,[a_th,b_th],[form_a_th,form_b_th]);


[N , D] = numden(det_Hill_H3X3_a_b_th);
[Nc_det_Hill_H3X3_a_b_th, Nt_det_Hill_H3X3_a_b_th] = coeffs(N,epsilon);
[Dcwe, Dtwe] = coeffs(D,epsilon);

matlabFunction(Nc_det_Hill_H3X3_a_b_th,'File','f_eq_zero_det_Hill_H3X3_N_1.m','Comments','output1*epsilon^6 + output2*epsilon^4 + output3*epsilon^2 + output4*1 = 0')
%%
% dimentions and propertis
w = 20*pi; % [rad/sec]

spring_k=0.002;%1; % [N*m/rad]        spring coefficient 
Damping_c=1.9747*sqrt(spring_k); % [N*m*sec/rad] Critical Damping coefficient
link_a = 0.1524; %[m]      link length = 2a
link_b = 0.0762; %[m]      link width = 2b
f_ro = 1000;  %[kg/m^3] liquid density
link_hight = 0.0254; %[m] link hight
link_m = f_ro*pi*link_a*link_b*link_hight; % link 0 mass
link_mx = link_hight*pi*f_ro*link_b^2;             % added mass in direction x;
link_my = link_hight*pi*f_ro*link_a^2;             % added mass in direction y
link_mt = link_hight*(1/8)*pi*f_ro*(link_a^2-link_b^2)^2; % added rotational mass
link_I = (link_m*(link_a^2+link_b^2))/4; % link 0 inertia
link_L = 0.198; %[m] Distance from center of link to axis

m11 = M_11_in0(link_I,link_L,link_m,link_mt,link_my);
m12 = M_12_in0(link_I,link_L,link_m,link_mt,link_my);
a_th = f_a_th1_ana(link_I,link_L,Damping_c,spring_k,link_m,link_mt,link_my,w);
b_th = f_b_th1_ana(link_I,link_L,Damping_c,spring_k,link_m,link_mt,link_my,w);
alpha_i = f_hill_b2_alpha_i(link_I,link_L,link_m,link_mt,link_mx,link_my);
alpha_1 = alpha_i(1); alpha_2 = alpha_i(2); alpha_3 = alpha_i(3); alpha_4 = alpha_i(4);
%Note that in the paper we expressed beta_i, for i=1, 2, 3, and 4 in terms of alpha_j with the
%corresponding j and beta_5=c.
beta_i = f_hill_b1_beta_i(link_I,link_L,Damping_c,link_m,link_mt,link_mx,link_my);
beta_1 = beta_i(1); beta_2 = beta_i(2); beta_3 = beta_i(3); beta_4 = beta_i(4); beta_5 = beta_i(5);
gamma_i = f_hill_b0_gamma_i(link_I,link_L,spring_k,link_m,link_mt,link_mx,link_my);

%The gamma_i's that appear here were denoted by mu_j in the Appendix of the paper (more
%specifically: mu_1=gamma_1=gamma_5, mu_2=gamma_2=gamma_10,
%mu_3=gamma_3=gamma_6=2*gamma_11, mu_4=gamma_4, mu_5=gamma_7, mu_6=gamma_8,
%mu_7=gamma_9=0.5*gamma_12, mu_8=gamma_13, mu_9=gamma_14,
%mu_10=gamma_15=-2*gamma_16.
gamma_1 = gamma_i(1); gamma_2 = gamma_i(2); gamma_3 = gamma_i(3); gamma_4 = gamma_i(4); gamma_5 = gamma_i(5); gamma_6 = gamma_i(6); gamma_7 = gamma_i(7); gamma_8 = gamma_i(8);

Si = f_eq_zero_det_Hill_H3X3_N_1(alpha_1,alpha_2,alpha_3,alpha_4,beta_1,beta_2,beta_3,beta_4,beta_5,Damping_c,gamma_1,gamma_2,gamma_3,gamma_4,gamma_5,gamma_6,gamma_7,gamma_8,spring_k,m11,m12,w);

zero_det_Hill_H3X3_eq = Nt_det_Hill_H3X3_a_b_th.*Si
%%
function [exp_out] = sin2_cossin_cos2_to_sincos2(exp)
%%% a*sin(w*t)^2 + b*cos(w*t)*sin(w*t) + c*cos(w*t)^2 + d
%%% to e*sin(2*w*t) + f*cos(2*w*t) + g
    syms w t real
    [csc, tsc] = coeffs(exp,[sin(w*t),cos(w*t)],'All');
    a = csc(1,3);
    b = csc(2,2);
    c = csc(3,1);
    d = csc(3,3);
    e = 0.5*b;
    f = 0.5*(c-a);
    g = 0.5*(a+c+2*d);
    exp_out = e*sin(2*w*t) + f*cos(2*w*t) + g;
end

function [exp_out] = sin2_cossin_cos2_to_sincos4(exp)
%%%  a*sin(2*t*w)^2 + b*cos(2*t*w)*sin(2*t*w) + c*sin(2*t*w) + d*cos(2*t*w)^2 + e*cos(2*t*w) + f
%%% to g*sin(4*w*t) + h*cos(4*w*t) + i*sin(2*w*t) + j*cos(2*w*t) + k
    syms w t real
    [csc, tsc] = coeffs(exp,[sin(2*w*t),cos(2*w*t)],'All');
    a = csc(1,3);
    b = csc(2,2);
    c = csc(2,3);
    d = csc(3,1);
    e = csc(3,2);
    f = csc(3,3);
    
    g = 0.5*b;
    h = 0.5*(d-a);
    i = c;
    j = e;
    k = 0.5*(a+d+2*f);
    exp_out = g*sin(4*w*t) + h*cos(4*w*t) + i*sin(2*w*t) + j*cos(2*w*t) + k;
end

function [H,B] = MY_EulerLagrange_H(L,R,X,DDX)
    % Euler - Lagrange
    syms t
    % Reshape coordinates/velocities state vector to help with substitution
    X2n     = reshape(X,2,[]);
    X3n     = [X2n;DDX];
    numcoor = size(X2n,2);
    qt = arrayfun(@(ii) symfun(['q' int2str(ii) '(t)'],t),1:numcoor,'UniformOutput',false);
    Xt2n = [qt; arrayfun(@(ii) diff(qt{1,ii},t),1:numcoor,'UniformOutput',false)];
    Xt3n = [Xt2n;arrayfun(@(ii) diff(qt{1,ii},t,t),1:numcoor,'UniformOutput',false)];

    R  = sym(R);  % take care of numeric zero

    DE = [];

    for ii = 1:numcoor
        % Differentiate w.r.t. coordinate/velocity
        L_q        = diff(L,X2n{1,ii});
        L_qdot     = diff(L,X2n{2,ii});
        R_qdot     = diff(R,X2n{2,ii});
        % Substitute coordinates/velocities by corresponding symfuns
        L_qt(t)    = subs(L_q,   X2n,Xt2n); %#ok<*AGROW>
        L_qtdot(t) = subs(L_qdot,X2n,Xt2n);
        R_qtdot(t) = subs(R_qdot,X2n,Xt2n);
        % Differentiate "velocity" term w.r.t. time
        L_qdotdt   = diff(L_qtdot,t);
        % Collect all Lagrange terms
        DE         = [DE; (L_qdotdt - L_qt + R_qtdot)];
    end % of for
    % Return to original coordinates
    DE             = subs(DE,Xt3n,X3n);

    DE = formula(DE);
    %%%% DE is the eq system of the problem [DE]=[0] is the homogenos eq
    % finding all the coeffs of DDX and building H 
    for ii = 1:numcoor
        [c, t] = coeffs(DE(ii),[X,DDX]);
        for jj = 1:numcoor
            [row,col]=find(t == DDX(jj));
            if isempty(row)
            H(ii,jj) = 0;
            else
            H(ii,jj) = c(row,col);
            end
        end
    end
    H=simplify(H);
    B = simplify(DE-H*DDX'); % B is what's left
end

function [simp_vec] = simplify_vec(vec)
    for i = 1:length(vec)
       simp_vec(i) =  simplify(vec(i),'Steps', 5000);
%        simp_vec(i) =  simplify(simp_vec(i),'Steps', 100);
    end
end

function [str_n]=latex2(symbol)
% example
% CCC = latex2(L);
% clipboard('copy',CCC);
%
%
    str = latex(symbol);
    str = strrep(str, '\mathrm{mx}', 'm_{x}');
    str = strrep(str, '\mathrm{my}', 'm_{y}');
    str = strrep(str, '\mathrm{link_mt}', 'm_{t}');
    % str = strrep(str, 'beta', '\beta');
    str = strrep(str, '\mathrm{th}_{1}', '\theta_{1}');
    str = strrep(str, '\mathrm{th}_{2}', '\theta_{2}');

    str = strrep(str, '\mathrm{dbeta}', '\dot{\beta}');
    str = strrep(str, '\mathrm{dy}', '\dot{y}');
    str = strrep(str, '\mathrm{dx}', '\dot{x}');

    str = strrep(str, '\mathrm{dth}_{1}', '\dot{\theta}_{1}');
    str = strrep(str, '\mathrm{dth}_{2}', '\dot{\theta}_{2}');
    str = strrep(str, '\mathrm{ddth}_{1}', '\ddot{\theta}_{1}');
    str = strrep(str, '\mathrm{ddth}_{2}', '\ddot{\theta}_{2}');
    
    str = strrep(str, '\mathrm{th}_{\bar{1}}', '\overline{\theta_{1}}');
    str = strrep(str, '\mathrm{dth}_{\bar{1}}', '\dot{\overline{\theta}}_{1}');
    str = strrep(str, '\mathrm{ddth}_{\bar{1}}', '\ddot{\overline{\theta}}_{1}');
    str = strrep(str, '\mathrm{del}', '\delta');
    str = strrep(str, '\mathrm{ddel}', '\dot{\delta}');
    str = strrep(str, '\mathrm{dddel}', '\ddot{\delta}');

    str = strrep(str, 'f_{1,\mathrm{th}}', '{f}_{\theta}^{(1)}');
    str = strrep(str, '\mathrm{df}_{1,\mathrm{th}}', '\dot{f}_{\theta}^{(1)}');
    str = strrep(str, '\mathrm{ddf}_{1,\mathrm{th}}', '\ddot{f}_{\theta}^{(1)}');
    
    str = strrep(str, 'f_{2,\mathrm{th}}', '{f}_{\theta}^{(2)}');
    str = strrep(str, '\mathrm{df}_{2,\mathrm{th}}', '\dot{f}_{\theta}^{(2)}');
    str = strrep(str, '\mathrm{ddf}_{2,\mathrm{th}}', '\ddot{f}_{\theta}^{(2)}');
    
    str = strrep(str, 'b_{\mathrm{th}}', 'b_{\theta}');
    str = strrep(str, 'a_{\mathrm{th}}', 'a_{\theta}');

    str = strrep(str, 'w', '\omega');

    str = strrep(str, '\delta _{\mathrm{mi}}', '\delta');
    str = strrep(str, 'mathrm{ep}', '\varepsilon');
    str = strrep(str, '\epsilon', '\varepsilon');

    str = erase(str, '\,');
    str = erase(str, '\left');
    str = erase(str, '\right');
 
    
     str_n=str;
end