parallel1()

% Main function for traversing the ellipse
function [j_vars] = parallel1()
clear all
clc
%%
global l lengths radiusx radiusy ell_angle phidot
global l1a l2a l3a l3b
d2r=pi/180;
%% Ellipse Parameters
radiusx=1; radiusy=2.5; ell_angle=30*d2r;
x=radiusx*cos(ell_angle);%-radiusy*sin(ell_angle);
y=radiusx*sin(ell_angle);%+radiusy*sin(ell_angle);
Sim_Angle=3*pi/2; % Angle to be traversed along rotated ellipse
%% Platform Parameters
phi=30*d2r; % initial rotation of phi
phidot=pi/10; % rate of rotation in rad/s
side_angle=60*d2r; % angle between adjascent platform sides(deg)
l=2/cos(pi/4); % side length of rhombus platform
%% Base Parameters
bxs=4; bys=4;
bases=[bxs bys; -bxs bys; -bxs -bys; bxs -bys]; % alternate base locations
% bases=[bxs 0; 0 bys; -bxs 0; 0 -bys]; % alternate base locations

l1a=4; l2a=4; l3a=l*cos(side_angle/2); l3b=l*sin(side_angle/2);
lengths=[l1a l2a l3a; l1a l2a l3b; l1a l2a l3a; l1a l2a l3b];
j_vars=zeros(2,12);
options=odeset;
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;
        j_vars(2,i*3)=j_vars(1,i*3);
    end
end
if(feasible~=1)
    disp('no feasible soln in parallel');
    pause
end
tspan=0:0.05:Sim_Angle;
% [T Y]=ode45(@Mydiff,tspan,j_vars(1,:),options);
Y=ode4(@Mydiff,0:0.05:Sim_Angle,j_vars(1,:)); T=tspan;
span=length(T);
x=zeros(span,4);
y=zeros(span,4);
[platformplot serplot]=parallelplot1init(Y(1,:),bases,lengths);
plot(bases(:,1),bases(:,2),'or');
curveplot=plot(x(1:i,2),y(1:i,2),'color','k','linewidth',2,'EraseMode','xor');
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    
  set(curveplot,'Xdata',x(1:i,2),'Ydata',y(1:i,2));
  parallelplot1(Y(i,:),bases,lengths, platformplot, serplot);
end
end