% close all
clear
clc

%build code
buildit

%image data
if 1
    [fn,pn] = uigetfile('/home/robocup/svn/trunk/data/testimages_Hefei2015/*.png');
else
    pn = '/home/robocup/svn/trunk/data/testimages_Hefei2015';
    fn = 'turtle4_1237568491.png';
%     fn = 'turtle4_1237569712.png';
end
turtle = fn(1:7);
A = imread(fullfile(pn,fn));
Y = A(:,:,1);
% Y = imread('pruts.png');

%pose
pose = [-3.5055 -.40435 0.039741];

%mirror calibration data
fid = fopen(['/home/robocup/svn/trunk/src/Turtle2/Vision/Calibration_data_' turtle '/mc.dat'],'rb');
mc = fread(fid,9,'double');
fclose(fid);

% load inputs
% pose = input1;
% Y = input2;
% mc = input3;


%run
T = 100;
tic
sim('findobstacles_sim')
toc/T

%% show results
B = ycbcr2rgb(A);
figure,imshow(A(:,:,1))
C = B;
C = setcolor(C,[128 128 128],bitand(debugimage,1));
C = setcolor(C,[255 255 255],imdilate(bitand(debugimage,32),strel('diamond',2)));
 C = setcolor(C,[255 0 0],imdilate(bitand(debugimage,8),strel('diamond',1)));
 C = setcolor(C,[255 0 0],imdilate(bitand(debugimage,16),strel('diamond',1)));
 C = setcolor(C,[255 0 0],imdilate(bitand(debugimage,64),strel('diamond',6)));
figure
imshow(C)

figure
plot(obstacles(((1:nobstacles)*3)-2),obstacles(((1:nobstacles)*3)-1),'*')
axis image
axis([ -7 7 -10 10])