clc;    % Clear Command Window
cla;    % Clear current axes

%% 각각의 링크 길이
A = 136.709; B = 189.786; C = 170.442; D = 204.155;
E = 626.063; F = 438.039; G = 211.934; H = 390.793;
I = 849.686; J = 360.727; K = 584.382; L = 441.093;
M = 298.84; N = 69.671; O = 269.715; P = 78.874;
Q= 345; S=115.443; T=187.486;
w=25; h=50;
TH1 = 47.7260;
TH1 = TH1 * pi/180; %[Deg. -> rad]

for(TH2_ = 25 : 1: 130) % 회전 범위
    cla;
    %% 위치해석1 (4절링크1)
    TH2_ = TH2_ * pi/180; %[Deg. -> rad]

    K1 = D/A;
    K2 = D/C;
    K3 = (A^2 - B^2 + C^2 + D^2)/(2*A*C);
    K4 = D/B;
    K5 = (C^2 - D^2 - A^2 - B^2)/(2*A*B);

    L1 = cos(TH2_) - K1 - K2*cos(TH2_) + K3;
    L2 = -2*sin(TH2_);
    L3 = K1 - (K2 +1)*cos(TH2_) + K3;
    L4 = cos(TH2_) - K1 + K4*cos(TH2_) + K5;
    L5 = -2*sin(TH2_);
    L6 = K1 + (K4-1)*cos(TH2_) + K5;

    TH3_ = 2*atan( ( -L5 - sqrt(L5^2 - 4*L4*L6) )/(2*L4) );
    TH4_ = 2*atan( ( -L2 - sqrt(L2^2 - 4*L1*L3) )/(2*L1) );

    TH2 = TH1+TH2_;
    TH3 = TH1+TH3_;
    TH4 = TH1+TH4_;
    %% 위치해석2 (4절링크2)
    TH5 = 24.1790;
    TH5 = TH5 * pi/180; %[Deg. -> rad]
    % TH5_ = 0;
    Angle_C_G = 18.2900; % C와 G 사이 각도
    Angle_C_G = Angle_C_G * pi/180; %[Deg. -> rad]
    
    TH8=TH4-Angle_C_G;
    TH8_=TH8-TH5;
    
    K6 = -H/G;
    K7 = -H/F;
    K8 = (E^2-F^2-G^2-H^2)/(2*F*G);
    K9 = -H/E;
    K10 = (E^2-F^2 +G^2+H^2)/(2*E*G);
    
    L7 = cos(TH8_)-K6-K7*cos(TH8_)+K8;
    L8 = -2*sin(TH8_);
    L9 = K6 - (K7 +1)*cos(TH8_)+K8;
    L10 = cos(TH8_) - K6 -K9*cos(TH8_) + K10;
    L11 = -2*sin(TH8_);
    L12 = K6 - (K9+1)*cos(TH8_) + K10;
    
    TH7_ = 2*atan( ( -L8 + sqrt(L8^2 - 4*L7*L9) )/(2*L7) );
    TH6_ = 2*atan( ( -L11 + sqrt(L11^2 - 4*L10*L12) )/(2*L10) );
    
    TH6 = TH6_ + TH5;
    TH7 = TH7_ + TH5;

    %% 위치해석3 (5절링크)
    
    Angle_E_I = 2.64; % E와 I 사이 각도
    Angle_E_I = Angle_E_I * pi/180; %[Deg. -> rad]
    
    TH9 = TH6 + Angle_E_I;
    TH9_ = TH9 - TH5;
    TH4_ = TH4 - TH5; % TH4_를 재정의
    
    K11 = K^2-I^2-J^2-L^2-H^2 + 2*I*L*(cos(TH9_)*cos(TH4_)+sin(TH9_)*sin(TH4_))...
        + 2*I*H*cos(TH9_) - 2*L*H*cos(TH4_);
    K12 = 2*I*J;
    K13 = -2*J*L;
    K14 = -2*J*H;
    K15 = J^2-I^2-L^2-K^2-H^2 + 2*I*L*(cos(TH9_)*cos(TH4_) + sin(TH9_)*sin(TH4_))...
        + 2*I*H*cos(TH9_) - 2*L*H*(cos(TH4_));
    K16 = -2*I*K;
    K17 = 2*K*L;
    K18 = 2*K*H;
    
    L13 = K11+K12*cos(TH9_)+K13*cos(TH4_)+K14;
    L14 = -2*K12*sin(TH9_) - 2*K13*sin(TH4_);
    L15 = K11 - K12*cos(TH9_) - K13*cos(TH4_) - K14;
    L16 = K15 + K16*cos(TH9_) + K17*cos(TH4_) + K18;
    L17 = -2*K16*sin(TH9_) -2*K17*sin(TH4_);
    L18 = K15 - K16*cos(TH9_) -K17*cos(TH4_) - K18;
    
    TH10_ = 2*atan( ( -L14 - sqrt(L14^2 - 4*L13*L15) )/(2*L13) );
    TH11_ = 2*atan( ( -L17 + sqrt(L17^2 - 4*L16*L18) )/(2*L16) );
    
    TH10 = TH10_ + TH5;
    TH11 = TH11_ + TH5;
    
    %% 위치해석4 (4절링크3)
    Angle_K_O = 43.821; % P와 O 사이 각도
    Angle_K_O = Angle_K_O * pi/180; %[Deg. -> rad]
    TH14 = TH11 + Angle_K_O;
    TH14_ = TH14 - TH10;
    
    K19 = -P/O;
    K20 = -P/N;
    K21 = ( -M^2 + N^2 + O^2 + P^2 ) / ( 2*N*O);
    K22 = -P/M;
    K23 = ( M^2 - N^2 + O^2 + P^2 ) / ( 2*M*O);
    
    L19 = cos(TH14_) - K19 - K20*cos(TH14_) + K21;
    L20 = -2*sin(TH14_);
    L21 = K19 - (K20 +1)*cos(TH14_) + K21;
    L22 = cos(TH14_) - K19 - K22*cos(TH14_) +K23;
    L23 = -2*sin(TH14_);
    L24 = K19 - (K22+1)*cos(TH14_)+K23;
    
    TH13_ = 2*atan( ( -L20 + sqrt(L20^2 - 4*L19*L21) )/(2*L19));
    TH12_ = 2*atan( ( -L23 - sqrt(L23^2 - 4*L22*L24) )/(2*L22));
    
    TH13 = TH13_ + TH10;
    TH12 = TH12_ + TH10;
    %% 위치해석5 (슬라이드 크랭크)
    Angle_R_S = 157.759; % R과 S사이 각도
    Angle_R_S = Angle_R_S * pi/180; %[Deg. -> rad]
    
    TH16 = Angle_R_S - pi + TH14;
    TH14_ = TH14 + pi - TH13;
    TH15_ = TH15 + pi - TH13;
    TH16_ = TH16 + pi - TH13;
    
    M1 = 2*S*(cos(TH16_)*cos(TH14_)+sin(TH16_)*sin(TH14_))+2*T*cos(TH16_);
    M2 = T^2 +S^2 -Q^2 + 2*T*S*cos(TH16_);
    
    R = ( -M1 + sqrt(M1^2 - 4*M2) ) / 2;
    
    %% Coordinate of the points
    x1 = 0;                y1 = 0;
    x2 = x1 + A*cos(TH2);  y2 = y1 + A*sin(TH2);
    x3 = x2 + B*cos(TH3);  y3 = y2 + B*sin(TH3);
    x4 = x1 + D*cos(TH1);  y4 = y1 + D*sin(TH1);

    x5 = x1 - 220;         y5 = y1 - 10;
    x6 = x5 + E*cos(TH6);  y6 = y5 + E*sin(TH6);
    x8 = x5 + H*cos(TH5);  y8 = y5 + H*sin(TH5);
    x7 = x8 + G*cos(TH8);  y7 = y8 + G*sin(TH8);
    
    x9 = x1 - 220;         y9 = y1 - 10;
    x10 = x9 + I*cos(TH9);  y10 = y9 + I*sin(TH9);
    x11 = x10 + J*cos(TH10);  y11 = y10 + J*sin(TH10);
    x13 = x9 + H*cos(TH5); y13 = y9 + H*sin(TH5);
    x12 = x13 + L*cos(TH4); y12 = y13 + L*sin(TH4);
    
    x14 = x10 + (J-P)*cos(TH10); y14 = y10 + (J-P)*sin(TH10);
    x15 = x14 + M*cos(TH12); y15 = y14 + M*sin(TH12);
    x16 = x15 + N*cos(TH13); y16 = y15 + N*sin(TH13);
    x17 = x11; y17 = y11; 
    
    x18= x16; y18=y16;
    x19 = x18 + S*cos(TH14); y19= y18+S*sin(TH14);
    x20 = x19 + R*cos(TH16)- L26 ; y20 = y19 +R*sin(TH16);
    x21 = x18 + T*cos(TH13); y21 = y18 + T*sin(TH13);
 
    
    %% Link by two points
    L1x = [x1, x4];  L1y = [y1, y4];
    L2x = [x1, x2];  L2y = [y1, y2];
    L3x = [x2, x3];  L3y = [y2, y3];
    L4x = [x3, x4];  L4y = [y3, y4];
    
    L5x = [x5, x8];  L5y = [y5, y8];
    L6x = [x5, x6];  L6y = [y5, y6];
    L7x = [x6, x7];  L7y = [y6, y7];
    L8x = [x7, x8];  L8y = [y7, y8];
    
    L9x = [x9, x13];  L9y = [y9, y13];
    L10x = [x9, x10];  L10y = [y9, y10];
    L11x = [x10, x11];  L11y = [y10, y11];
    L12x = [x11, x12];  L12y = [y11, y12];
    L13x = [x12, x13];  L13y = [y12, y13];
    
    L14x = [x14, x17];  L14y = [y14, y17];
    L15x = [x14, x15];  L15y = [y14, y15];
    L16x = [x15, x16];  L16y = [y15, y16];
    L17x = [x16, x17];  L17y = [y16, y17];
    
    L19x = [x18, x19];  L19y = [y18, y19];
    L20x = [x19, x20];  L20y = [y19, y20];
    L21x = [x20, x21];  L21y = [y20, y21];
    L22x = [x18, x21];  L22y = [y18, y21];

    rectangle('position', [x20 - w/2, y20 - h/2, w, h])
    %% Drawing of links
    line(L1x, L1y, 'LineWidth',4, 'Color',[0 153 68]/255); 
    line(L2x, L2y, 'LineWidth',4, 'Color',[0 153 68]/255);
    line(L3x, L3y, 'LineWidth',4, 'Color',[0 153 68]/255);
    line(L4x, L4y, 'LineWidth',4, 'Color',[0 153 68]/255);
    
    line(L5x, L5y, 'LineWidth',4, 'Color',[255 0 255]/255); 
    line(L6x, L6y, 'LineWidth',4, 'Color',[255 0 255]/255);
    line(L7x, L7y, 'LineWidth',4, 'Color',[255 0 255]/255);
    line(L8x, L8y, 'LineWidth',4, 'Color',[255 0 255]/255);

    line(L9x, L9y, 'LineWidth',4, 'Color',[0 160 233]/255); 
    line(L10x, L10y, 'LineWidth',4, 'Color',[0 160 233]/255);
    line(L11x, L11y, 'LineWidth',4, 'Color',[0 160 233]/255);
    line(L12x, L12y, 'LineWidth',4, 'Color',[0 160 233]/255);
    line(L13x, L13y, 'LineWidth',4, 'Color',[0 160 233]/255);

    line(L14x, L14y, 'LineWidth',4, 'Color',[255 241 0]/255);
    line(L15x, L15y, 'LineWidth',4, 'Color',[255 241 0]/255);
    line(L16x, L16y, 'LineWidth',4, 'Color',[255 241 0]/255);
    line(L17x, L17y, 'LineWidth',4, 'Color',[255 241 0]/255);
   
    line(L19x, L19y, 'LineWidth',4, 'Color',[255 0 0]/255);
    line(L20x, L20y, 'LineWidth',4, 'Color',[255 0 0]/255);
    line(L21x, L21y, 'LineWidth',4, 'Color',[255 0 0]/255);
    line(L22x, L22y, 'LineWidth',4, 'Color',[255 0 0]/255);
    
    pause(0.05);
end
