clear;
close all;
%% Generating Trajectory in Cartesian Space
% phi = linspace(0,2*pi,50);
% x = 80*cos(phi);
% y = 80*sin(phi);
% z = -100*ones(1,length(x));
% x = [0,40,-40];
% y = [0,40,-40];
% z = [-150,-150,-150];
halfL = 40;
d = 40;
L = -halfL : d : halfL;
X = zeros(length(L));
Y = zeros(length(L));
for i = 1:length(L)
     X(i,:) = L(:);
     Y(:,i) =sort(L(:),'descend')';
end
Z = -200*ones(length(L));
index = 1:length(L);
col = zeros(1,1);
row = zeros(1,1);
for j = 1:length(L)
     if mod(j,2) == 1
         col(length(L)*(j-1)+1:length(L)*j) = index;
     else
         col(length(L)*(j-1)+1:length(L)*j) = sort(index,'descend');
     end
    row(length(L)*(j-1)+1:length(L)*j) = index(j);
end
for k = 1:length(col)
    x(1,k) = X(row(k),col(k));
    y(1,k) = Y(row(k),col(k));
    z(1,k) = Z(row(k),col(k));
end
x = [0,x];
y = [0,y];
z = [-150,z];
%% Parameters
d = 7.97;
% sB = d*(11/2/sqrt(3)+12)*sqrt(3)*2;
% sB = d*(9*sqrt(3)+9);
sB = d*15;
sP = d*(9/2);
% L = d*9;
L = d*10;
% l = d*21;
l = d*19;
%% Inverse Kinematics
wB = sB*sqrt(3)/6;
uB = sB*sqrt(3)/3;
wP = sP*sqrt(3)/6;
uP = sP*sqrt(3)/3;

a = wB-uP;
b = sP/2-wB*sqrt(3)/2;
c = wP-wB/2;
theta1 = zeros(1,2);
theta2 = zeros(1,2);
theta3 = zeros(1,2);
for i = 1:length(x)
    A(1,1) = (x(i)^2 + y(i)^2 + z(i)^2 + a^2 + L^2 + 2*a*y(i))-...
        2*(a+y(i))*L - l^2;
    A(1,2) = 2*2*z(i)*L;
    A(1,3) = (x(i)^2 + y(i)^2 + z(i)^2 + a^2 + L^2 + 2*a*y(i))+...
        2*(a+y(i))*L - l^2;
    t1(1,:) = roots(A(1,:));
    theta1(i,:) = atan(t1(1,:))*2*180/pi;
    A(2,1) = (x(i)^2 + y(i)^2 + z(i)^2 + b^2 + c^2 + L^2 + 2*x(i)*b + 2*y(i)*c)+...
        (sqrt(3)*(x(i)+b) + y(i) + c)*L - l^2;
    A(2,2) = 2*2*z(i)*L;
    A(2,3) = (x(i)^2 + y(i)^2 + z(i)^2 + b^2 + c^2 + L^2 + 2*x(i)*b + 2*y(i)*c)-...
        (sqrt(3)*(x(i)+b) + y(i) + c)*L - l^2;
    t2(1,:) = roots(A(2,:));
    theta2(i,:) = atan(t2(1,:))*2*180/pi;
    A(3,1) = (x(i)^2 + y(i)^2 + z(i)^2 + b^2 + c^2 + L^2 - 2*x(i)*b + 2*y(i)*c)-...
        (sqrt(3)*(x(i)-b) - y(i) - c)*L - l^2;
    A(3,2) = 2*2*z(i)*L;
    A(3,3) = (x(i)^2 + y(i)^2 + z(i)^2 + b^2 + c^2 + L^2 - 2*x(i)*b + 2*y(i)*c)+...
        (sqrt(3)*(x(i)-b) - y(i) - c)*L - l^2;
    t3(1,:) = roots(A(3,:));
    theta3(i,:) = atan(t3(1,:))*2*180/pi;
end
gear = 6;
% in radius
th1_r = theta1(:,2)*pi/180;
th2_r = theta2(:,2)*pi/180;
th3_r = theta3(:,2)*pi/180;
% in degree
the1 = (theta1(:,2)+23)*gear;
the2 = (theta2(:,2)+23)*gear;
the3 = (theta3(:,2)+23)*gear;
%plot(1:length(phi),theta1,1:length(phi),theta2,1:length(phi),theta3)
th1 = diff([0;the1]);
th2 = diff([0;the2]);
th3 = diff([0;the3]);
r_ini = (abs(th1(2)/th1(1))+abs(th2(2)/th2(1))+abs(th3(2)/th3(1)))/3;
r = 0.8;
p1(1) = th1(1)*r_ini*r;       
p2(1) = -th2(1)*r_ini*r;
p3(1) = -th3(1)*r_ini*r;
p1(2:length(th1)) = th1(2:length(th1))*r;
p2(2:length(th2)) = -th2(2:length(th2))*r;
p3(2:length(th3)) = -th3(2:length(th3))*r;



%{
%% Vector
BB(:,1) = [0; -wB; 0];
BB(:,2) = [wB*sqrt(3)/2; wB/2; 0];
BB(:,3) = [-wB*sqrt(3)/2; wB/2; 0];

Bb(:,1) = [sB/2; -wB; 0];
Bb(:,2) = [0; uB; 0];
Bb(:,3) = [-sB/2; -wB; 0];

PP(:,1) = [0; -uP; 0];
PP(:,2) = [sP/2; wP; 0];
PP(:,3) = [-sP/2; wP; 0];

A = zeros(3,3,1);
P = zeros(3,3,1);
for i = 1:length(th1)
    B_Pp(:,1) = [x(i); y(i); z(i)];
    B_Pp(:,2) = [x(i); y(i); z(i)];
    B_Pp(:,3) = [x(i); y(i); z(i)];
    BL(:,1) = [0; -L*cos(th1_r(i)); -L*sin(th1_r(i))];
    BL(:,2) = [sqrt(3)/2*L*cos(th2_r(i)); L/2*cos(th2_r(i)); -L*sin(th2_r(i))];
    BL(:,3) = [-sqrt(3)/2*L*cos(th3_r(i)); L/2*cos(th3_r(i)); -L*sin(th3_r(i))];
    Bl = B_Pp + PP - BB - BL;
    A(:,:,i) = BB+BL;
    P(:,:,i) = BB+BL+Bl;
end
%% Plotting the theta data
figure(1)
subplot(3,1,1)
plot(1:length(th1_r),th1_r(:,1),1:length(th2_r),th2_r(:,1),1:length(th3_r),th3_r(:,1))
title('\theta')
subplot(3,1,2)
plot(1:length(th1_r)-1,diff(th1_r(:,1)),1:length(th2_r)-1,diff(th2_r(:,1)),1:length(th3_r)-1,diff(th3_r(:,1)))
title('d\theta/dt')
subplot(3,1,3)
plot(1:length(th1_r)-2,diff(diff(th1_r(:,1))),1:length(th2_r)-2,diff(diff(th2_r(:,1))),1:length(th3_r)-2,diff(diff(th3_r(:,1))))
title('d^2\theta/dt^2')
%legend('\theta_1','\theta_2','\theta_3','location','southoutside','orientation','horizontal')
hold off

%% Plotting the Delta Robot
figure(2)
hold on
plot3([Bb(1,1) Bb(1,2) Bb(1,3) Bb(1,1)],...
    [Bb(2,1) Bb(2,2) Bb(2,3) Bb(2,1)],...
    [Bb(3,1) Bb(3,2) Bb(3,3) Bb(3,1)]);
h1(1) = plot3([BB(1,1) A(1,1,1)],[BB(2,1) A(2,1,1)],[BB(3,1) A(3,1,1)],'r');
h1(2) = plot3([BB(1,2) A(1,2,1)],[BB(2,2) A(2,2,1)],[BB(3,2) A(3,2,1)],'r');
h1(3) = plot3([BB(1,3) A(1,3,1)],[BB(2,3) A(2,3,1)],[BB(3,3) A(3,3,1)],'r');

h2(1) = plot3([A(1,1,1) P(1,1,1)],[A(2,1,1) P(2,1,1)],[A(3,1,1) P(3,1,1)],'g');
h2(2) = plot3([A(1,2,1) P(1,2,1)],[A(2,2,1) P(2,2,1)],[A(3,2,1) P(3,2,1)],'g');
h2(3) = plot3([A(1,3,1) P(1,3,1)],[A(2,3,1) P(2,3,1)],[A(3,3,1) P(3,3,1)],'g');

h3 = plot3([P(1,1,1) P(1,2,1) P(1,3,1) P(1,1,1)],...
    [P(2,1,1) P(2,2,1) P(2,3,1) P(2,1,1)],...
    [P(3,1,1) P(3,2,1) P(3,3,1) P(3,1,1)],'k');
h4 = plot3(x(1,1),y(1,1),z(1,1),'kx');
axis equal
axis([-sB sB -sB sB -(l+L) (l+L)/2])
grid on
view(3);

title('Delta Robot Test')
text(BB(1,1),BB(2,1),BB(3,1),'j1')
text(BB(1,2),BB(2,2),BB(3,2),'j2')
text(BB(1,3),BB(2,3),BB(3,3),'j3')
%{
title({['(\theta_1,\theta_2,\theta_3) = (' ...
    num2str(round(th0(1)*180/pi,2)) '^o, '...
    num2str(round(th0(2)*180/pi,2)) '^o, '...
    num2str(round(th0(3)*180/pi,2)) '^o)'],...
    ['(x,y,z) = (' ...
    num2str(round(X(1),2)) ', ' ...
    num2str(round(X(2),2)) ', ' ...
    num2str(round(X(3),2)) ')']})
%}
%% Animation
%{
%video
filename=('DeltaRobotTest');
writerObj=VideoWriter(filename);
writerObj.FrameRate=30;
open(writerObj);
%}
for i = 1:length(x)
    set(h1(1),'xdata',[BB(1,1) A(1,1,i)],'ydata',[BB(2,1) A(2,1,i)],'zdata',[BB(3,1) A(3,1,i)]);
    set(h1(2),'xdata',[BB(1,2) A(1,2,i)],'ydata',[BB(2,2) A(2,2,i)],'zdata',[BB(3,2) A(3,2,i)]);
    set(h1(3),'xdata',[BB(1,3) A(1,3,i)],'ydata',[BB(2,3) A(2,3,i)],'zdata',[BB(3,3) A(3,3,i)]);
    
    set(h2(1),'xdata',[A(1,1,i) P(1,1,i)],'ydata',[A(2,1,i) P(2,1,i)],'zdata',[A(3,1,i) P(3,1,i)]);
    set(h2(2),'xdata',[A(1,2,i) P(1,2,i)],'ydata',[A(2,2,i) P(2,2,i)],'zdata',[A(3,2,i) P(3,2,i)]);
    set(h2(3),'xdata',[A(1,3,i) P(1,3,i)],'ydata',[A(2,3,i) P(2,3,i)],'zdata',[A(3,3,i) P(3,3,i)]);
    
    set(h3,'xdata',[P(1,1,i) P(1,2,i) P(1,3,i) P(1,1,i)],...
        'ydata',[P(2,1,i) P(2,2,i) P(2,3,i) P(2,1,i)],...
        'zdata',[P(3,1,i) P(3,2,i) P(3,3,i) P(3,1,i)]);
    
    set(h4,'xdata',x(1,i),'ydata',y(1,i),'zdata',z(1,i));
    plot3(x(1,i),y(1,i),z(1,i),'k.','markersize',1)
    drawnow
    pause(0.5);
    %{
    %video
    videoframe=getframe(gcf);
    writeVideo(writerObj,videoframe);
    %}
end
%{
%video
close(writerObj);
fprintf('video finished\n');
%}
%}