TIME_STEP = 16;
global Xdd1 Xdd2 L1 L2 E2_0 E2_1 D U21hat U1

r1=0.02;
r2=0.12;
L2=-6;
step=0;
TS=TIME_STEP*0.001;
D=TIME_STEP*0.001;

E2_1=[0;0;0;0;0;0];
E2_0=[0;0;0;0;0;0];
U2=zeros(4,1);
U1=zeros(4,1);
U21hat=zeros(4,1);
n=10/D;
F2=zeros(n,1);
XD2=zeros(n,2);
Ee2=zeros(n,2);
Uu2=zeros(n,2);
X2_0=zeros(12,1);

Xdd1=[0.1*TS;1.5*sin(0.15*TS)+0.4/sqrt(2);
      0.2*TS;1.5*sin(0.15*2*TS)+0.4/sqrt(2);
      0.3*TS;1.5*sin(0.15*3*TS)+0.4/sqrt(2);
      0.4*TS;1.5*sin(0.15*4*TS)+0.4/sqrt(2);];
      
Xdd2=[0.1*TS+0.4/sqrt(2);1.5*sin(0.15*TS);
      0.2*TS+0.4/sqrt(2);1.5*sin(0.15*2*TS);
      0.3*TS+0.4/sqrt(2);1.5*sin(0.15*3*TS);
      0.4*TS+0.4/sqrt(2);1.5*sin(0.15*4*TS)];

emitter2 = wb_robot_get_device('emitter2');
receiver2 = wb_robot_get_device('receiver2');
GPSDevice2=wb_robot_get_device('gps2');
InertialUnit2=wb_robot_get_device('inertial unit2');

wb_gps_enable(GPSDevice2,TIME_STEP);
wb_inertial_unit_enable(InertialUnit2,TIME_STEP);
wb_receiver_enable(receiver2,TIME_STEP);


RData=[0;0;0];

while wb_robot_step(TIME_STEP) ~= -1

 t=wb_robot_get_time;
 pos2=wb_gps_get_values(GPSDevice2);
 Rotation2=wb_inertial_unit_get_roll_pitch_yaw(InertialUnit2);
 
 P2=[pos2(3);pos2(1)];
 teta2=Rotation2(3)+pi;
 
 if t<=25
  Pd2=[0.1*t+0.4/sqrt(2);1.5*sin(0.15*t)];
  Pd1Dot=[0.1;0.225*cos(0.15*t)];
 else 
  Pd2=[0.1*t+0.4/sqrt(2);1.5*sin(3.75)];
  Pd1Dot=[0.1;0];
 end
 
 
 e2=P2-Pd2;
 E2_0=[E2_0(3:6);e2];
 
 SData=[2;e2];
 wb_emitter_set_channel(emitter2,1);
 wb_emitter_send(emitter2,SData);
 wb_emitter_set_channel(emitter2,3);
 wb_emitter_send(emitter2,SData);
 wb_emitter_set_channel(emitter2,4);
 wb_emitter_send(emitter2,SData);
 
 while (wb_receiver_get_queue_length(receiver2) > 0)
  pointer = wb_receiver_get_data(receiver2);
  setdatatype(pointer,'doublePtr',1,3);
  RData=get(pointer,'value');
  wb_receiver_next_packet(receiver2);    
 end
 
 [X2,fval2]=fmincon(@Agent2,X2_0,[],[],[],[],[],[],@Con2);
 
 K21=[X2(1) X2(2);X2(3) X2(4)]/D;
 u2=L2*e2+K21*E2_1(1:2);
 
 Speed2=62*[cos(teta2) sin(teta2);-50*sin(teta2) 50*cos(teta2)]*u2;
 v2=Speed2(1);
 w2=Speed2(2);
 RightSeed2=(v2+r2*w2)/r1;
 leftSpeed2=(v2-r2*w2)/r1;
 
 wb_differential_wheels_set_speed(leftSpeed2,RightSeed2);
 
 E2_1=[E2_1(3:6);RData(2);RData(3)];
 U21hat=[U21hat(3:4);X2(5:6)];
 U2=[U2(3:4);u2];
 F2(step+1)=fval2;
 Ee2(step+1,:)=e2';
 Uu2(step+1,:)=u2';
 XD2(step+1,:)=Pd2';
 X2_0=X2;  
 
 if ((step+5)*TS)<=25 
  Xdd1=[Xdd1(3:8);(step+5)*0.1*TS;1.5*sin(0.15*(step+5)*TS)+0.4/sqrt(2)];
  Xdd2=[Xdd2(3:8);(step+5)*0.1*TS+0.4/sqrt(2);1.5*sin(0.15*(step+5)*TS)];
 else
  Xdd1=[Xdd1(3:8);(step+5)*0.1*TS;1.5*sin(3.75)+0.4/sqrt(2)];
  Xdd2=[Xdd2(3:8);(step+5)*0.1*TS+0.4/sqrt(2);1.5*sin(3.75)]; 
 end
 
 step=step+1;
 
 drawnow;

end

% cleanup code goes here: write data to files, etc.
