clear -all
close all
%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1; 5 3 1; 5 1 1;6 4 1; 6 2 1; 5 6 1];
conRobot = [1 2 1;
            1 4 1;
            1 5 1;
            1 6 1;
            2 3 1;
            2 4 1;
            3 4 1;
            4 5 1;
            5 6 1;];
%% conRobot = [2 1 1; 3 2 1; 3 1 1;3 4 1; 2 4 1;];

%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1; -1; -1; 0; -1; ]; % xy xy xy
corRobot = [2; 2.5;
            2.7; 2.6;
            3; 2.3;
            2.5; 1.5;
            1.8; 1.5;
            1.5; 2;
           ]; % xy xy xy
%% corRobot = [0; 0; 1; 0; 2; 2; 1; 1;]; % xy xy xy
d = 3;
%% figure(1)
[R,K] = rigidityMatrixFnc(conRobot);
vref = [-5; 5];
B = [1 0;0 1; 0 0;0 0; 0 0;0 0; 0 0; 0 0; 0 0; 0 0; 0 0; 0 0;];
kp = 2;
ki = 3;

sInit = [corRobot; zeros(size(R(corRobot,K),1),1)];
tspan = linspace(1,30);
dydt = @(t, y) systm_robot(t, y,d, R,K, kp, ki, B, vref);

[t,y] = ode45(dydt, tspan, sInit);
%% figure(2)
str_tmp = "plot(";
for i = 1:length(corRobot)-1
  str_tmp = strcat(str_tmp,sprintf("y'(%i,:),",i));
endfor
str_tmp = strcat(str_tmp,sprintf("y'(%i,:));",i+1));
eval(str_tmp);
hold on
str_tmp = " plot_rb = @(t) plot( [";
for i = 1:2:length(corRobot)
  str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
endfor
str_tmp = strcat( str_tmp, sprintf("], ["));
for i = 2:2:length(corRobot)
  str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
endfor
str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
eval(str_tmp)

xrb = 1:2:length(corRobot);
yrb = 2:2:length(corRobot);
t = 1;
plot_rb(t);
for i = 1:length(conRobot)
  plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)],
       [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" )
endfor
t = 20;
plot_rb(t);
for i = 1:length(conRobot)
  plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)],
       [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" )
endfor

t = 40;
plot_rb(t);
for i = 1:length(conRobot)
  plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)],
       [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" )
endfor

t = 80;
plot_rb(t);
for i = 1:length(conRobot)
  plot([y'(xrb(conRobot(i, 1)),t), y'(xrb(conRobot(i, 2)),t)],
       [y'(yrb(conRobot(i,1)),t), y'(yrb(conRobot(i,2)),t)], "r" )
endfor


%% plot_rb(20);
%% plot_rb(40);
%% plot_rb(60);
%% plot_rb(80);
%% plot_rb(100);


