function OUT=cuadrilatero(r1,r2,r3,r4,teta1,teta4)
% OUT=cuadrilatero(r1,r2,r3,r4,teta1,teta4)
% r1+r2+r3+r4=0 tetar2?,tetar3?
% r1=2;r2=4.5;r3=6.5;r4=6;teta1=45*pi/180;teta4=pi;
% OUT=[ca1;dca1;ddca1;ca2;dca2;ddca2];

% PROBLEMA DE POSICION
% resolvemos r4+r1=G => Caso 1 (G, tetaG)
out1=Raven([r1 r4 0 teta1 teta4 0 1]);

% resolvemos r2+r3+G=0 => r2+r3=-G => Caso 4 [2 soluciones]
% (teta2(1),teta3(1)); (teta2(2), teta3(2))
out2=Raven([r2 r3 -out1(3) 0 0 out1(end) 4]);



% Construcción del vector de datos para representación del cuadrilátero 
% ca=[r1 r2 r3 r4;teta1 teta2 teta3 teta4]
% Solución 1
ca1=[r1 r2 r3 r4;teta1 out2(1,4) out2(1,5) teta4]
% Solución 2
ca2=[r1 r2 r3 r4;teta1 out2(2,4) out2(2,5) teta4]


% Obtención de las coordenadas de las articulaciones del Cuadrilátero
% Articulado
xa1=cumsum([0 ca1(1,:).*cos(ca1(2,:))]);
ya1=cumsum([0 ca1(1,:).*sin(ca1(2,:))]);
xa2=cumsum([0 ca2(1,:).*cos(ca2(2,:))]);
ya2=cumsum([0 ca2(1,:).*sin(ca2(2,:))]);

% Representación de ambas soluciones
plot(xa1(1:end-1),ya1(1:end-1))
hold on
plot(xa1([1 end-1]),ya1([1 end-1]),'r*')
plot(xa1([1 end-1]),ya1([1 end-1]),'o')
plot(xa2(2:4),ya2(2:4),'r')
axis equal


% PROBLEMA DE VELOCIDAD
% Derivamos la ecuación de cierre, teniendo en cuenta que los módulos de
% todos los vectores son constantes y que el ángulo teta4 también es
% constante
% De este modo obtenemos una ecuación en forma compleja con la forma
% j*r2*dteta2*exp(j*teta2)+j*r3*dteta3*exp(j*teta3)=-j*r1*dteta1*exp(j*teta1)
% Se trata de una ecuación vectorial que podemos resolver considerando el
% CASO 2 [¿r2*dteta2? ¿r3*dteta3? r1*dteta1 teta2 teta3 teta1]

dteta1=10; %rad/seg constante
out3=Raven([0 0 -ca1(1,1)*dteta1 ca1(2,2) ca1(2,3) ca1(2,1) 2]);

dteta2=out3(1)/ca1(1,2)
dteta3=out3(2)/ca1(1,3)
dca1=[0 0 0 0 ;dteta1 dteta2 dteta3 0]

out3=Raven([0 0 -ca2(1,1)*dteta1 ca2(2,2) ca2(2,3) ca2(2,1) 2]);

dteta2=out3(1)/ca2(1,2)
dteta3=out3(2)/ca2(1,3)
dca2=[0 0 0 0 ;dteta1 dteta2 dteta3 0]


% PROBLEMA DE ACELERACION
% Derivando la ecuación de velocidad teniendo en cuenta que dteta1 es
% constante y reorganizando la ecuación
% r2*ddteta2*exp(j*teta2)+r3*ddteta3*exp(j*teta3)=-j*(r1*dteta1^2*exp(j*teta1)+
% r2*dteta2^2*exp(j*teta2)+r3*dteta3^2*exp(j*teta3))
% Los términos a la derecha pueden sumarse proporcionando un vector de
% módulo y dirección conocidas con lo que las incógnitas serán ddteta2 y
% ddteta3

% Ensamblado 1
% Suma de vectores conocidos CASO1
%r2*dteta2^2*exp(j*teta2)+r3*dteta3^2*exp(j*teta3)=C*exp(j*tetaC)
out4=Raven([ca1(1,2)*(dca1(2,2)^2) ca1(1,3)*(dca1(2,3)^2) 0 ca1(2,2) ca1(2,3) 0 1]);
%C*exp(j*tetaC)+r1*dteta1^2*exp(j*teta1)=D*exp(j*tetaD)
out5=Raven([out4(3) ca1(1,1)*(dca1(2,1))^2 0 out4(6) ca1(2,1) 0 1]);

%También podríamos hacer
%r1*dteta1^2*exp(j*teta1)+r2*dteta2^2*exp(j*teta2)=G*exp(j*tetaG)
%out4=Raven([ca1(1,1)*(dca1(2,1)^2) ca1(1,2)*(dca1(2,2)^2) 0 ca1(2,1) ca1(2,2) 0 1]);
%G*exp(j*tetaG)+r3*dteta3^2*exp(j*teta3)=D*exp(j*tetaD)
%out5=Raven([out4(3) ca1(1,3)*(dca1(2,3))^2 0 out4(6) ca1(2,3) 0 1]);

% r2*ddteta2*exp(j*teta2)+r3*ddteta3*exp(j*teta3)=-j*D*exp(j*tetaD)
% teniendo en cuenta que -j*D*exp(j*tetaD)=-D*exp(j*(tetaD+pi/2))
% r2*ddteta2*exp(j*teta2)+r3*ddteta3*exp(j*teta3)=-D*exp(j*(tetaD+pi/2))
% CASO2
out6=Raven([0 0 -out5(3) ca1(2,2) ca1(2,3) out5(6)+pi/2 2]);
ddteta1=0;
ddteta2=out6(1)/ca1(1,2)
ddteta3=out6(2)/ca1(1,3)
ddca1=[0 0 0 0 ;ddteta1 ddteta2 ddteta3 0]

% Ensamblado 2
out4=Raven([ca2(1,2)*(dca2(2,2)^2) ca2(1,3)*(dca2(2,3)^2) 0 ca2(2,2) ca2(2,3) 0 1]);
out5=Raven([out4(3) ca2(1,1)*(dca2(2,1))^2 0 out4(6) ca2(2,1) 0 1]);
out6=Raven([0 0 -out5(3) ca2(2,2) ca2(2,3) out5(6)+pi/2 2]);

ddteta1=0;
ddteta2=out6(1)/ca2(1,2)
ddteta3=out6(2)/ca2(1,3)
ddca2=[0 0 0 0 ;ddteta1 ddteta2 ddteta3 0]


OUT=[ca1;dca1;ddca1;ca2;dca2;ddca2];