TIME_STEP = 16;
global Xdd4 Xdd2 L4 L2 E4_0 E4_2 D U42hat U2

r1=0.02;
r2=0.12;
L4=-3;
step=0;
TS=TIME_STEP*0.001;
D=TIME_STEP*0.001;

E4_2=[0;0;0;0;0;0];
E4_0=[0;0;0;0;0;0];
U4=zeros(4,1);
U2=zeros(4,1);
U42hat=zeros(4,1);
n=10/D;
F4=zeros(n,1);
XD4=zeros(n,2);
Ee4=zeros(n,2);
Uu4=zeros(n,2);
X4_0=zeros(12,1);

Xdd4=[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)];
      
      
emitter4 = wb_robot_get_device('emitter4');
receiver4 = wb_robot_get_device('receiver4');
GPSDevice4=wb_robot_get_device('gps4');
InertialUnit4=wb_robot_get_device('inertial unit4');

wb_gps_enable(GPSDevice4,TIME_STEP);
wb_inertial_unit_enable(InertialUnit4,TIME_STEP);
wb_receiver_enable(receiver4,TIME_STEP);


RData=[0;0;0];

while wb_robot_step(TIME_STEP) ~= -1

 t=wb_robot_get_time;
 pos4=wb_gps_get_values(GPSDevice4);
 Rotation4=wb_inertial_unit_get_roll_pitch_yaw(InertialUnit4);
 
 P4=[pos4(3);pos4(1)];
 teta4=Rotation4(3)+pi;
 
 if t<=25
  Pd4=[0.1*t;1.5*sin(0.15*t)-0.4/sqrt(2)];
  Pd4Dot=[0.1;0.225*cos(0.15*t)];
 else
  Pd4=[0.1*t;1.5*sin(3.75)-0.4/sqrt(2)];
  Pd4Dot=[0.1;0];
 end
 
 e4=P4-Pd4;
 E4_0=[E4_0(3:6);e4];
 SData=[4;e4];
 wb_emitter_set_channel(emitter4,3);
 wb_emitter_send(emitter4,SData);
 
 while (wb_receiver_get_queue_length(receiver4) > 0)
  pointer = wb_receiver_get_data(receiver4);
  setdatatype(pointer,'doublePtr',1,3);
  RData=get(pointer,'value');
  wb_receiver_next_packet(receiver4);    
 end
 
 [X4,fval4]=fmincon(@Agent4,X4_0,[],[],[],[],[],[],@Con4);
 
 K42=[X4(1) X4(2);X4(3) X4(4)]/D;
 u4=L4*e4+K42*E4_2(1:2);
 
 Speed4=22*[cos(teta4) sin(teta4);-50*sin(teta4) 50*cos(teta4)]*u4;
 v4=Speed4(1);
 w4=Speed4(2);
 RightSeed4=(v4+r2*w4)/r1;
 leftSpeed4=(v4-r2*w4)/r1;
 
 wb_differential_wheels_set_speed(leftSpeed4,RightSeed4);
 
 E4_2=[E4_2(3:6);RData(2);RData(3)];
 U42hat=[U42hat(3:4);X4(5:6)];
 U4=[U4(3:4);u4];
 F4(step+1)=fval4;
 Ee4(step+1,:)=e4';
 Uu4(step+1,:)=u4';
 XD4(step+1,:)=Pd4';
 X4_0=X4;  
 
 if ((step+5)*TS)<=25 
  Xdd2=[Xdd2(3:8);(step+5)*0.1*TS+0.4/sqrt(2);1.5*sin(0.15*(step+5)*TS)];
  Xdd4=[Xdd4(3:8);(step+5)*0.1*TS;1.5*sin(0.15*(step+5)*TS)-0.4/sqrt(2)];
 else
  Xdd2=[Xdd2(3:8);(step+5)*0.1*TS+0.4/sqrt(2);1.5*sin(3.75)]; 
  Xdd4=[Xdd4(3:8);(step+5)*0.1*TS;1.5*sin(3.75)-0.4/sqrt(2)]; 
 end
 
 step=step+1;
 
 drawnow;

end

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