% Síntese para geração de função
% Exemplo 1.1 do Mabie

clear all
close all
clc
% -------------------------------------------------------------
% fução y=x^1.5
% 1 < x < 4
% -------------------------------------------------------------
d = 1;

fis = 30*pi/180;
psis = 90*pi/180;

deltafi = 90*pi/180;
deltapsi = 90*pi/180;

xs=1;
xf=4;

NP = 20;
% -------------------------------------------------------------
vetq=zeros(NP,1);
vetA=zeros(NP,1);
vetB=zeros(NP,1);
vetdetjaco=zeros(NP,1);
% -------------------------------------------------------------
ys=xs^(1.5);
yf=xf^(1.5);
deltax=xf-xs;
deltay=yf-ys;
fif=fis+deltafi;
psif=psis+deltapsi;
qmin = fis;
qmax = fif;
% -------------------------------------------------------------
%% a) extremos
x=[xs; 
  (xf+xs)/2;
  xf];
% -------------------------------------------------------------
%% b) Chebyshev
% a=(xf+xs)/2;
% h=(xf-xs)/2;
% x=[a-h*cos(pi/6);
%     a;
%     a-h*cos(5*pi/6)];
% -------------------------------------------------------------
y=x.^(1.5);
% -------------------------------------------------------------
fi=deltafi/deltax*(x-xs)+fis;
psi=deltapsi/deltay*(y-ys)+psis;
% -------------------------------------------------------------
w1=cos(fi(1))-cos(fi(2));
w2=cos(fi(1))-cos(fi(3));
w3=cos(psi(1))-cos(psi(2));
w4=cos(psi(1))-cos(psi(3));
w5=cos(fi(1)-psi(1))-cos(fi(2)-psi(2));
w6=cos(fi(1)-psi(1))-cos(fi(3)-psi(3));
R1=(w3*w6-w4*w5)/(w2*w3-w1*w4);
R2=(w1*w6-w2*w5)/(w2*w3-w1*w4);
R3=cos(fi(1)-psi(1))+R2*cos(psi(1))-R1*cos(fi(1));
% -------------------------------------------------------------
a=d/R2;
c=d/R1;
b=sqrt(a^2+c^2+d^2-2*a*c*R3);
% -------------------------------------------------------------
%% Resultados da síntese
% -------------------------------------------------------------
fprintf('  fis = %6.2f     fif = %6.2f     deltafi = %6.2f\n',...
    fis*180/pi,fif*180/pi,deltafi*180/pi)
fprintf(' psis = %6.2f    psif = %6.2f    deltapsi = %6.2f\n\n',...
    psis*180/pi,psif*180/pi,deltapsi*180/pi)
fprintf(' fi1 = %6.2f    fi2 = %6.2f    fi3 = %6.2f\n',...
    fi(1)*180/pi,fi(2)*180/pi,fi(3)*180/pi)
fprintf('psi1 = %6.2f   psi2 = %6.2f   psi3 = %6.2f\n\n',...
    psi(1)*180/pi,psi(2)*180/pi,psi(3)*180/pi)
fprintf('a = %6.2f   b = %6.2f   c = %6.2f   d = %6.2f\n',a,b,c,d)
% -------------------------------------------------------------
%% Análise de Posição
%-------------------------------------------------------------
vetq=linspace(qmin,qmax,NP);
ZERO=10^(-6);     % tolerancia para o metodo de Newton Raphson
% valores iniciais das variaveis secundarias
A = 45*pi/180;   
B = 90*pi/180;
s=[A; B];
%funçao f cuja norma e maior que ZERO
f=[1;1];          
for i = 1:NP
    q = vetq(i);
    f=[1;1];
    while norm(f) > ZERO
        norm(f);
        A = s(1);
        B = s(2);
        f = [ -a*cos(q)+b*cos(A)+c*cos(B)-d;
              a*sin(q)+b*sin(A)-c*sin(B)];
        jaco=[-b*sin(A) -c*sin(B);
               b*cos(A) -c*cos(B)];
        s=s-inv(jaco)*f;
    end
    % distribuição do resultado nas variáveis secundárias
    vetA(i)=s(1);     % armazenamento no vetor
    vetB(i)=s(2);     % armazenamento no vetor
    vetdetjaco(i)=det(jaco);
end
%% posições limite e de precisão
vetqprec=[fis; fi; fif];
% valores iniciais das variaveis secundarias
A = 45*pi/180;   
B = 90*pi/180;
s=[A; B];
%funçao f cuja norma e maior que ZERO
f=[1;1]; 
for i = 1:size(vetqprec,1)
    q = vetqprec(i);
    f=[1;1];
    while norm(f) > ZERO
        norm(f);
        A = s(1);
        B = s(2);
        f = [ -a*cos(q)+b*cos(A)+c*cos(B)-d;
              a*sin(q)+b*sin(A)-c*sin(B)];
        jaco=[-b*sin(A) -c*sin(B);
               b*cos(A) -c*cos(B)];
        s=s-inv(jaco)*f;
    end
    % distribuição do resultado nas variáveis secundárias
    vetAprec(i)=s(1);     % armazenamento no vetor
    vetBprec(i)=s(2);     % armazenamento no vetor
end

%-------------------------------------------------------------
%% Gráficos da análise geral
%-------------------------------------------------------------
figure('Name','Análise Geral')    % título na moldura superior
% primeiro gráfico
subplot(1,2,1)                    % gráfico da esquerda (posição 1)
plot(vetq*180/pi,vetA*180/pi,...
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('A(q) (^o)')               % rótulo do eixo y
title('Posição do elo intermediário')    % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
% segundo gráfico
subplot(1,2,2)                    % gráfico da direita (posição 2)
plot(vetq*180/pi,vetB*180/pi,...
    'r',...                       % cor da curva
    'LineWidth',2)                % espessura da linha
grid on                           % insere linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('B(q) (^o)')               % rótulo do eixo y
title('Posição do elo de saída')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
%% Gráfico do determinante da matriz jacobiana
%-------------------------------------------------------------
figure('Name','Determinante da Matriz Jacobiana')% título na moldura
plot(vetq*180/pi,vetdetjaco,...
    'LineWidth',2)                % espessura da linha
grid on                           % linhas de grade
xlabel('q (^o)')                  % rótulo do eixo x
ylabel('Det J(q)')                % rótulo do eixo y
title('Determinante da Matriz Jacobiana')        % título do gráfico
xlim([qmin*180/pi qmax*180/pi])
%-------------------------------------------------------------
%% Gráfico do resultado
%-------------------------------------------------------------
x_mec=((vetq-fis)/deltafi*(deltax)+xs)';
y_mec=(vetB-psis)/deltapsi*(deltay)+ys;
y_calc=x_mec.^(1.5);

figure('Name','Comparativo')% título na moldura
plot(x_mec,y_mec,'r','LineWidth',2)        
grid on 
hold on
plot(x_mec,y_calc,'b','LineWidth',2)   
xlabel('x')
ylabel('y')
legend('mecanismo','função')
title('Comparativo')
figure('Name','Erro')% título na moldura
plot(x_mec,y_mec-y_calc,'LineWidth',2)        
grid on 
xlabel('x')
ylabel('erro')
title('Erro')
%-------------------------------------------------------------
%% Posições do mecanismo
%-------------------------------------------------------------
vetAprec=vetAprec';
Pprecs=[                                    0                                     0;
                          -a*cos(vetqprec(1))                    a*sin(vetqprec(1));
       -a*cos(vetqprec(1))+b*cos(vetAprec(1)) a*sin(vetqprec(1))+b*sin(vetAprec(1));
                                            d                                     0];
                                        
Pprec1=[                                  0                                 0;
                       -a*cos(vetqprec(2))                a*sin(vetqprec(2));
       -a*cos(vetqprec(2))+b*cos(vetAprec(2)) a*sin(vetqprec(2))+b*sin(vetAprec(2));
                                         d                                 0];

Pprec2=[                                  0                                 0;
                       -a*cos(vetqprec(3))                a*sin(vetqprec(3));
       -a*cos(vetqprec(3))+b*cos(vetAprec(3)) a*sin(vetqprec(3))+b*sin(vetAprec(3));
                                         d                                 0];

Pprec3=[                                  0                                 0;
                       -a*cos(vetqprec(4))                a*sin(vetqprec(4));
       -a*cos(vetqprec(4))+b*cos(vetAprec(4)) a*sin(vetqprec(4))+b*sin(vetAprec(4));
                                         d                                 0];
                                     
Pprecf=[                                  0                                 0;
                       -a*cos(vetqprec(5))                a*sin(vetqprec(5));
       -a*cos(vetqprec(5))+b*cos(vetAprec(5)) a*sin(vetqprec(5))+b*sin(vetAprec(5));
                                         d                                 0];
                                     
figure('Name','Posições de Precisão')

plot(Pprecs(:,1),Pprecs(:,2),'k','LineWidth',1)        
grid on 
axis equal
hold on
plot(Pprec1(:,1),Pprec1(:,2),'r','LineWidth',2)
hold on
plot(Pprec2(:,1),Pprec2(:,2),'g','LineWidth',2)
hold on
plot(Pprec3(:,1),Pprec3(:,2),'b','LineWidth',2)
hold on
plot(Pprecf(:,1),Pprecf(:,2),'k','LineWidth',1)
%-------------------------------------------------------------
% FIM
%-------------------------------------------------------------

