clear -all

graphics_toolkit('gnuplot')
omni = omni = omnidirectional(0.94, 0.96, 0.01, 2.2, 1.5,0.099,0.1,1.5,0.025,30,0.035,19,0.00011,1.69,0.0059,0.0059,0.9,9.1,6,'./');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang kene
conRobot = [1 2 1;
            3 2 1;
            3 1 1;
           ];
length_d = .5;
dRobot = [ 1;
           1;
           1;
];
corRobot = [1.5; 1.7;0;
            2; 2.5;0;
            2.5; 2.8;0;
           ]; % xy xy xy
robStateLen = size(omni.origin.A,1);
B = [
     1 0; 0 1;
     0 0; 0 0;
     0 0; 0 0;
    ];
kp = 150;
ki = 3;
% fvref = fncSpeedRef('ysin',50,2,robStateLen);
% fvref = fncSpeedRef('xsin',50,2,robStateLen);
% fvref = fncSpeedRef('cw',-300,1,robStateLen);
% fvref = fncSpeedRef('s',50,50,robStateLen);
fvref = fncSpeedRef('s',0,0,robStateLen);
h = .01;
tspan = 1:h:10;
sys = formationControl2ndBuilder(conRobot,robStateLen,kp,kp,ki,ki,length_d,1);
% sys = formationControl2ndModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);
% sys = formationControl1stModelBuilder(conRobot,omni.origin,kp,kp,ki,ki,length_d,1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% config nang nduwur

[R,K,d] = rigidityMatrixFnc(conRobot,robStateLen);
_zero = zeros(size(R(corRobot,K),1),1);
sInit = [corRobot; _zero;];
sAnsInit =[_zero; _zero;];
s2ndInit = [corRobot;     %x1
           zeros(length(corRobot),1); %x2
           zeros(length(corRobot),1); %xi1
            _zero;]; %xi2
          %  _zero; zeros(length(corRobot),1);]; %x2*
s1stInit = [corRobot; zeros(size(corRobot)); zeros(size(dRobot))];

% state_init = sInit;
state_init = s2ndInit;
% state_init = s1stInit;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start solving
printf("Mulai memecakan masalah \n\n");
startExe = tic;

% continue
% dydt = @(t, y) sys.ss(y,fvref(t));
% [t,y] = ode45(dydt, tspan, state_init);

% discrate taylor methode
dydt = @(t, y) sys.dss(y, h, fvref(t))
t(1,:) = tspan(1);
y(1,:) = dydt(t(1), state_init); 
Dd(1,:) = [norm((K*y(1,1:9)')(1:3)) norm((K*y(1,1:9)')(4:6)) norm((K*y(1,1:9)')(7:9))]';
for i = 2:length(tspan)
  t(i,:) = tspan(i);
  y(i,:) = dydt(t(i),y(i-1,:)');
  Dd(i,:) = [norm((K*y(i,1:9)')(1:3)) norm((K*y(i,1:9)')(4:6)) norm((K*y(i,1:9)')(7:9))]';
endfor

endExe = toc(startExe);
printf("Membutuhkan waktu %i menit, %i detik \n untuk memecahkan masalah mu  \n\n",
       floor(endExe/60), rem(endExe,60))
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% End Solving
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% start plot it

close all

figure(1)

                                % plot trayektori dari setiap robot
str_tmp = "plot(";


xi = 1:robStateLen:length(corRobot)-1
yi = 2:robStateLen:length(corRobot)

for i = 1:sys.n-1
  str_tmp = strcat(str_tmp,sprintf("y'(%i,:),y'(%i,:),",xi(i),yi(i)));
endfor
str_tmp = strcat(str_tmp,sprintf("y'(%i,:),y'(%i,:));",xi(i+1),yi(i+1)));
eval(str_tmp);

hold on
                                % membuat fungsi plot robot di waktu
                                % tertentu
str_tmp = "@(t) plot( [";
for i = 1:robStateLen:length(corRobot)
  str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
endfor
str_tmp = strcat( str_tmp, sprintf("], ["));
for i = 2:robStateLen:length(corRobot)
  str_tmp = strcat( str_tmp, sprintf("y'(%i,t), ",i));
endfor
str_tmp = strcat( str_tmp, sprintf("], \"^\");"));
plot_rb = eval(str_tmp);

xrb = 1:robStateLen:length(corRobot);
yrb = 2:robStateLen:length(corRobot);

                                % fungsi untuk plot coneksi di waktu
                                % tertentu
function plot_con (pltRb, yOut, conIn,xm,ym, time)
  pltRb(time);
  for i = 1:length(conIn)
    plot([yOut(xm(conIn(i, 1)),time), yOut(xm(conIn(i, 2)),time)],
         [yOut(ym(conIn(i,1)),time), yOut(ym(conIn(i,2)),time)], "r" )
  endfor
endfunction

plot_con(plot_rb, y', conRobot, xrb, yrb, 1);
% plot_con(plot_rb, y', conRobot, xrb, yrb, round(length(tspan)/2));
plot_con(plot_rb, y', conRobot, xrb, yrb, length(tspan));


str_tmp = "legend(";
for i = 1:numNodes(edgeL2adjL(conRobot))-1;
  str_tmp =strcat(str_tmp,sprintf("\"R%i \", ",i));
endfor
str_tmp =strcat(str_tmp,sprintf("\"R%i \" )",++i));
eval(str_tmp)
title("Motion dari Robot");

figure(2)
plot(t,Dd(:,1),t,Dd(:,2),t,Dd(:,3))
title("distance error")
legend()

csvwrite("DataOutMotion.csv", [t y])
% save DataOutMotion.data y
% save DataErrorEdge.data yans