TIME_STEP = 16;
global Xdd1 Xdd2 L1 L2 D U12hat E1_0 E1_2 U2

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

E1_2=[0;0;0;0;0;0];
E1_0=[0;0;0;0;0;0];
U2=zeros(4,1);
U1=zeros(4,1);
U12hat=zeros(4,1);
n=10/D;
F1=zeros(n,1);
XD1=zeros(n,2);
Ee1=zeros(n,2);
Uu1=zeros(n,2);
X1_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)];

emitter1 = wb_robot_get_device('emitter1');
receiver1 = wb_robot_get_device('receiver1');
GPSDevice1=wb_robot_get_device('gps1');
InertialUnit1=wb_robot_get_device('inertial unit1');

wb_gps_enable(GPSDevice1,TIME_STEP);
wb_inertial_unit_enable(InertialUnit1,TIME_STEP);
wb_receiver_enable(receiver1,TIME_STEP);



RData=[0;0;0];

while wb_robot_step(TIME_STEP) ~= -1

 t=wb_robot_get_time;
 pos1=wb_gps_get_values(GPSDevice1);
 Rotation1=wb_inertial_unit_get_roll_pitch_yaw(InertialUnit1);
 
 P1=[pos1(3);pos1(1)];
 teta1=Rotation1(3)+pi;
 
 if(t<=25)
  Pd1=[0.1*t;1.5*sin(0.15*t)+0.4/sqrt(2)];
  Pd1Dot=[0.1;0.225*cos(0.15*t)];
 else 
  Pd1=[0.1*t;1.5*sin(3.75)+0.4/sqrt(2)];
  Pd1Dot=[0.1;0];
 end
  
 
 e1=P1-Pd1;
 
 E1_0=[E1_0(3:6);e1];
 
 SData=[1;e1];
 wb_emitter_set_channel(emitter1,2);
 wb_emitter_send(emitter1,SData);
 wb_emitter_set_channel(emitter1,3);
 wb_emitter_send(emitter1,SData);
 
 while (wb_receiver_get_queue_length(receiver1) > 0)
  pointer = wb_receiver_get_data(receiver1);
  setdatatype(pointer,'doublePtr',1,3);
  RData=get(pointer,'value');
  wb_receiver_next_packet(receiver1);    
 end
 
 [X1,fval1]=fmincon(@Agent1,X1_0,[],[],[],[],[],[],@Con1);
 
 K12=[X1(1) X1(2);X1(3) X1(4)]/D;
 u1=L1*e1+K12*E1_2(1:2);
 
 Speed1=50*[cos(teta1) sin(teta1);-50*sin(teta1) 50*cos(teta1)]*u1;
 v1=Speed1(1);
 w1=Speed1(2);
 RightSeed1=(v1+r2*w1)/r1;
 leftSpeed1=(v1-r2*w1)/r1;
 
 wb_differential_wheels_set_speed(leftSpeed1,RightSeed1);
 
 E1_2=[E1_2(3:6);RData(2);RData(3)];
 U12hat=[U12hat(3:4);X1(5:6)];
 U1=[U1(3:4);u1];
 F1(step+1)=fval1;
 Ee1(step+1,:)=e1';
 Uu1(step+1,:)=u1';
 XD1(step+1,:)=Pd1';
 X1_0=X1; 
 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.
