%%demo 1 - odometry localization with simple collision avoidance
function project_2004_kiks(port,baud,time)
% port = serial port to communicate with (port<0 ==> simulated robot, port>=0 ==> real robot
% baud = baud rate
% time = time to run behaviour
if nargin<3 time=30; end; %% simulation time
if nargin<2 baud=9600; end;
if nargin<1 port=-1; end;

%%% ====== DEFINE ROBOT ARENA ======
arena_width = 600;  
ARENA = zeros(arena_width,arena_width);
%ARENA(100:300,100:180)=1;
%ARENA(400:480,100:300)=1;
kiks_arena(ARENA);
%%% ================================

ref = kiks_kopen([port,baud,1]);
kSetEncoders(ref,0,0);
enc_1=kGetEncoders(ref);
reflex = 0;

%% ----- Inital speed settings -----
omega = 0;
robot_speed = [0 0 omega]; %%[vl vr omega]
%% max speed (robot's left and right wheel) 
robot_speed_max = [10 10];

plot_area_x = arena_width/2; %dimensions of plot area defined by kiks 
plot_area_y = arena_width/2;
ratio_x = plot_area_x/arena_width;
ratio_y = plot_area_y/arena_width;


%% ----- initial state (after kiks reset) ------
stat = kiks_siminfo_robotpos;
%% initial state-estimate error (variance) 
state_var = 0;

x0 = stat(1); %arena_width/2;
y0 = stat(2); %arena_width/2;
th0 = -stat(3); %80*pi/180;
state = [x0 y0 th0];
target = [100 200];
robot_width = 52; %%Khepera robot actual diameter: ~53 mm (**CONSIDERING 1mm GEOMETRIC ERROR**)
%plot(ratio_x*target(1),ratio_y*target(2), 'r*');
%% ----------------------------------------------


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Proximity sensors numbering for Khepera robots
%%
%      3  4
%    2      5
%   1        6
%    
%      8  7
%%
%%  Read with function "readings = kProxility(ref);"
%%  Values range from <10 for no detected obstacle, to 
%%                    ~300 for visible obstacle, to
%%                    ~1000 for obstacle in close proximity
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

time_1 = 0;
t0=clock;
loops=0;
while (kiks_ktime(port)<time)
   loops=loops+1
   lights = kAmbient(ref);
   
   %% ==== GET PROXIMITY SENSORS (IR DETECTORS) READINGS ===
   reflex = kProximity(ref);
   
   %% ==== CALCULATE ROBOT SPEED ('REACTIVE' COLLISION AVOIDANCE) =====
   weightsL = [robot_speed_max(1) 2 4 6 6 4 2 4 4];
   weightsR = [robot_speed_max(2) -2 -4 -6 -6 -4 -2 4 4];
   speed = calcSpd(weightsL,weightsR,reflex)/2;
   
   %% ==== SET ROBOT SPEED ====
   vl = speed(1);  %cm/sec
   vr = speed(2);  %cm/sec
   kSetSpeed( ref, vl, vr );
   
   %% ==== GET ENCODERS READINGS (WHEELS MOTION)  ====
   %sp = kGetSpeed(ref);
   enc_2 = kGetEncoders(ref);
   denc = (enc_2 - enc_1);
   enc_1 = enc_2;
   ddl = denc(1)/12; %%12 pulses per millimeter of path --> ddl: distance travelled by left wheel in mm
   ddr = denc(2)/12; %%12 pulses per millimeter of path --> ddr: distance travelled by right wheel in mm
   
   time_2 = kiks_ktime(port); 
   dt = time_2 - time_1;  %% ==== SAMPLING TIME dt ====
   time_1 = time_2;
   
   %% ===== ODOMETRY --> COMPUTE CURRENT ROBOT SPEED (FROM ENCODERS READINGS) ====
   %if (vl ~= vr)
      vl = ddl/dt; %in mm/sec
      vr = ddr/dt;
   %else
    % vl = 10*vl;
    % vr = 10*vr;
   %end
   omega = (vl-vr)/robot_width; %rad/sec (robot_width is defined in mm)
   d_theta = omega*dt;
   
   %% ==== STATE PREDICTION - MOTION MODEL =====
   if (omega ~= 0)
      ICC_R = 0.5*robot_width*(vl+vr)/(vl-vr);  %%distance of ICC from robot center
      ICC_dx = ICC_R*sin(state(3));
      ICC_dy = - ICC_R*cos(state(3));
      ICC_x = state(1) - ICC_dx;
      ICC_y = state(2) - ICC_dy; 
	   x_new = ICC_dx*cos(d_theta) - ICC_dy*sin(d_theta) + ICC_x;
   	y_new = ICC_dx*sin(d_theta) + ICC_dy*cos(d_theta) + ICC_y;
   else
   	dx = (vl+vr)*0.5*cos(state(3))*dt; %*10
   	dy = (vl+vr)*0.5*sin(state(3))*dt; %*10
   	x_new =  state(1) + dx;
   	y_new =  state(2) + dy;
   end
   %% new robot state prediction state
   state(1) = x_new;
   state(2) = y_new;
   state(3) = state(3) + d_theta;
   %% current robot speed
   robot_speed = [0.1*vl 0.1*vr omega]; %%cm/sec
   %% ==========================================      
   
	%% === PLOT ESTIMATED ROBOT POSITION ===   
   plot(ratio_x*state(1), ratio_y*state(2), 'bo');
   
   %% === PLOT ACTUAL ROBOT POSITION ===   
	stat = kiks_siminfo_robotpos
	plot(ratio_x*stat(1), ratio_y*stat(2), 'g+')
   
   %% === CALL FUNCTION FOR OBTAINING NEW UPDATED ROBOT POSITION ESTIMATE ===
   %%out = pos_estimate(state, state_var, arena_width, reflex);
   %%% *** ADD HERE: NEW UPDATED ROBOT STATE ***
   
end;

kSetSpeed(ref,0,0);
kiks_kclose(ref);



%%% ===== FUNCTION FOR CALCULATING ROBOT MOTION FOR REACTIVE COLLISION AVOIDANCE ===
function out1 = calcSpd(weightsL, weightsR, reflex)

mL = weightsL(1);
mR = weightsR(1);
for i=2:9
   mL = weightsL(i)*(1/400)*reflex(i-1)+mL;
   mR = weightsR(i)*(1/400)*reflex(i-1)+mR;
end
if sum(reflex(1:4)) > sum(reflex(5:8)) 
   out1 = [round(mL) round(mR)];
else
   out1 = [round(mR) round(mL)];
end;



%%% *** ADD HERE: FUNCTION FOR UPDATING ROBOT POSITION ESTIMATE (USING EXTERNAL GEOMETRIC BEACONS) *** ...
%function out2 = pos_estimate(state, state_error, arena_width, reflex) 
	%% INPUT: current robot state estimate, variance, arena info, and proximity sensor readings
   %% out2 --> output vector containing info about which robot's state coordinate is updated, new updated estimates, 
   %%				and new state variance
%% ********************************

