% motion planning example


% simple 2D holonomic

clear; clc; close all;

x_init = [0.2; 0.2];
x_goal = [0.8; 0.9];
DELTA = 0.05;
rrt.states = x_init;
rrt.parents= [0];

figure; hold on; axis([0 1 0 1]); plot(x_init(1), x_init(2), 'xr', 'markersize', 20); plot(x_goal(1), x_goal(2), 'xg', 'markersize', 20); axis equal;
K=500;
for k=1:K
	if(rand() < 0.01)
		x_rand = x_goal;
	else
		x_rand = rand(2,1);
	end
	x_diff = rrt.states - repmat(x_rand, 1, k);
	x_diff_abs = sum(x_diff.^2,1);
	[min_val, min_idx] = min(x_diff_abs);
	x_near = rrt.states(:, min_idx);
	if(norm(x_rand-x_near,2) < DELTA)
		delta = norm(x_rand - x_near,2);
	else 
		delta = DELTA;
	end
	x_new = x_near + delta * (x_rand - x_near)/(norm(x_rand-x_near,2));
	rrt.states = [ rrt.states x_new ];
	rrt.parents = [rrt.parents min_idx];
	plot(rrt.states(1, [min_idx k+1]), rrt.states(2, [min_idx k+1]), 'b');
	if( norm(x_new - x_goal,2) <= .01 )
		% found path to close enough to goal, let's plot it:
		current_idx = k+1;
		parent_idx = rrt.parents(end);
		while(parent_idx ~= 0 )
			plot(rrt.states(1, [current_idx parent_idx]), rrt.states(2, [current_idx parent_idx]), 'r');
			current_idx = parent_idx;
			parent_idx = rrt.parents(parent_idx);
		end
	end
end



clear; clc; close all;

profile off; profile on;

x_init = [ 0; pi/2; 0; 0];
x_goal = [0; pi; 0; 0];
dt = 0.2;

rrt.states = x_init;
rrt.parents= [0];
rrt.controls_to_reach_state = [0];
us = -10:0.1:10;

figure; hold on;  plot(x_init(1), x_init(2), 'xr', 'markersize', 20); plot(x_goal(1), x_goal(2), 'xg', 'markersize', 20); 
K=10000;
for k=1:K
	if(mod(k-1,50)==0)
		k
	end
	if(rand() < 0.05)
		x_rand = x_goal;
	else
		x_rand = (1 -2*rand(4,1)).*[10 ; 2*pi ; 5 ; 2];
	end
	x_diff = rrt.states - repmat(x_rand, 1, k);
	x_diff_abs = sum(x_diff.^2,1);
	[min_val, min_idx] = min(x_diff_abs);
	x_near = rrt.states(:, min_idx);
	% try out all controls we consider in this search (more generally could
	% try to solve 2-point boundary value problem here, which would take
	% longer yet would often give a better extension, there is a trade-off
	% here)
	closest_point = [];
	closest_point_distance = inf;
	for u_idx = 1:length(us)
		u = us(u_idx);
		x_next = sim_cartpole(x_near, u, dt);
		if(norm(x_next - x_rand,2) < closest_point_distance)
			closest_point_distance = norm(x_next - x_rand,2);
			closest_point = x_next;
			closest_point_u = u;
		end
	end
	
	x_new = closest_point;
	rrt.states = [ rrt.states x_new ];
	rrt.parents = [rrt.parents min_idx];
	rrt.controls_to_reach_state = [rrt.controls_to_reach_state u];
	plot(rrt.states(1, [min_idx k+1]), rrt.states(2, [min_idx k+1]), 'b');

	if( abs(x_new(2)-x_goal(2))<= 0.5 && abs(x_new(3)-x_goal(3)) <= 0.5 && abs(x_new(4)-x_goal(4)) <= 0.5 && abs(x_new(1) - x_goal(1)) <= 5 )
		% found path to close enough to goal, let's plot it:
		current_idx = k+1;
		parent_idx = rrt.parents(end);
		while(parent_idx ~= 0 )
			plot(rrt.states(1, [current_idx parent_idx]), rrt.states(2, [current_idx parent_idx]), 'r');
			current_idx = parent_idx;
			parent_idx = rrt.parents(parent_idx);
		end
	end

end

profile report;

