parallel1()

function [j_vars] = parallel1()
clear all
clc
global l lengths radiusx radiusy ell_angle
global l1a l2a l3a l3b
syms th1 th2 th3 th4 th5 th6 th7 th8 th9 th10 th11 th12 real
syms l1 l2 l3  l4 l5 l6 l7 l8 l9 l10 l11 l12 real
% thetas = [th1 th2 th3;th4 th5 th6;th7 th8 th9;th10 th11 th12];
% lengths1 = [l1 l2 l3;l4 l5 l6;l7 l8 l9;l10 l11 l12];
clc
d2r=pi/180;
radiusx=1; radiusy=3; ell_angle=30;
x=radiusx*cos(ell_angle*d2r);%-radiusy*sin(ell_angle*d2r);
y=radiusx*sin(ell_angle*d2r);%+radiusy*sin(ell_angle*d2r);
phi=30; side_angle=60; l=2/cos(pi/4);
bxs=4; bys=4;
% bases=[bxs bys; -bxs bys; -bxs -bys; bxs -bys];
bases=[bxs 0; 0 bys; -bxs 0; 0 -bys];
Sim_Angle=3*pi/2;
l1a=4; l2a=4; l3a=l*cos(side_angle/2*d2r); l3b=l*sin(side_angle/2*d2r);
lengths=[l1a l2a l3a; l1a l2a l3b; l1a l2a l3a; l1a l2a l3b];
j_vars=zeros(2,12);
options=odeset;
% [A B]=newtry();
feasible=1;
ends=myfunc(x,y,phi,side_angle);
for i=1:4
    j_vars(1:2,i*3-2:i*3-1)=myrevkin1(bases(i,:),ends(i,:),lengths(i,1:2));
    if(j_vars(1,i*2-1)==10)
        j_vars(1,i*3)=10;
        j_vars(2,i*3)=10;
        feasible=0;
    else
        j_vars(1,i*3)=(i+1)*pi/2+phi*d2r;
%         if(i==1)
%             j_vars(1,i*3)=pi+phi*d2r;
%         elseif(i==2)
%             j_vars(1,i*3)=3*pi/2+phi*d2r;
%         elseif(i==3)
%             j_vars(1,i*3)=0+phi*d2r;
%         else
%             j_vars(1,i*3)=pi/2+phi*d2r;
%         end
        j_vars(2,i*3)=j_vars(1,i*3);
    end
end
if(feasible~=1)
    disp('no feasible soln in parallel');
    pause
end
[T Y]=ode45(@Mydiff,0:0.05:Sim_Angle,j_vars(1,:),options);
span=length(T);
x=zeros(span,4);
y=zeros(span,4);
for i=1:length(T)
  for i2=1:4
      x(i,i2)=bases(i2,1)+lengths(i2,1)*cos(Y(i,i2*3-2))+lengths(i2,2)*cos(Y(i,i2*3-1)+Y(i,i2*3-2))+lengths(i2,3)*cos(Y(i,i2*3));%+Y(i,2)+Y(i,1));
      y(i,i2)=bases(i2,2)+lengths(i2,1)*sin(Y(i,i2*3-2))+lengths(i2,2)*sin(Y(i,i2*3-1)+Y(i,i2*3-2))+lengths(i2,3)*sin(Y(i,i2*3));%+Y(i,2)+Y(i,1));
  end    
%     er1=bases(2,1)+lengths(2,1)*cos(Y(i,4))+lengths(2,2)*cos(Y(i,5)+Y(i,4))+lengths(2,3)*cos(Y(i,6))-x;%+Y(i,5)+Y(i,4));
%     er2=bases(2,2)+lengths(2,1)*sin(Y(i,4))+lengths(2,2)*sin(Y(i,5)+Y(i,4))+lengths(2,3)*sin(Y(i,6))-y;%+Y(i,5)+Y(i,4));
%     er3=bases(3,1)+lengths(3,1)*cos(Y(i,7))+lengths(3,2)*cos(Y(i,8)+Y(i,7))+lengths(3,3)*cos(Y(i,9))-x;%+Y(i,8)+Y(i,7));
%     er4=bases(3,2)+lengths(3,1)*sin(Y(i,7))+lengths(3,2)*sin(Y(i,8)+Y(i,7))+lengths(3,3)*sin(Y(i,9))-y;%+Y(i,8)+Y(i,7));
%     er5=bases(4,1)+lengths(4,1)*cos(Y(i,10))+lengths(4,2)*cos(Y(i,11)+Y(i,10))+lengths(4,3)*cos(Y(i,12))-x;%+Y(i,11)+Y(i,10));
%     er6=bases(4,2)+lengths(4,1)*sin(Y(i,10))+lengths(4,2)*sin(Y(i,11)+Y(i,10))+lengths(4,3)*sin(Y(i,12))-y;%+Y(i,11)+Y(i,10));
  ends=myfunc(x(i,1), y(i,1), phi, side_angle);
  clf('reset');
  hold on;
  plot(x(1:i,2),y(1:i,2),'color','k','linewidth',2);
  plot(bases(:,1),bases(:,2),'or');
  parallelplot1(Y(i,:),bases,ends,lengths);
  hold off;
%   pause(0.2);
end
end