%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Spiros Ioannou 2008
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% convert video to frames:
% C:\HOME\2-Projects\matlab\gps2dtmf>\winap\ffmpeg -i ilisia2fotis.divx.pcm.avi  -vframes 1000 -ss 00:01:00  -f image2 ilisia2fotis%05d.jpeg
%
%TODO:
%multivideoroutes:
% 1. find where routes (videos) intersect
%     points in each route that are closer than thresh. (distance())
% 2. in points of intersection user selects route (once)
%    but doesn't reselect if routes continue to intersect (same road with >1 movies)
%
%continuous driving operation
% 1. select a starting point and cruising speed
% 2. offer available paths & select (routes that intersect the starting point)
% 3. display frame closest to current pos
% 4. calculate next pos depending on speed and elapsed time from previous frame display
% 5. goto 3
%%%%%%%%%%%%%%%%%%%%%5
[allFrames,utctime,date,lat,lon,sats,hdg,speed,alt]=textread('data1.txt','%s%s%s%s%s%s%s%s%s','delimiter','@');

%distance of origin and destination (there are about 60 nautical miles in a degree of arc length)
%distance(dlat(1),dlon(1),dlat(end),dlon(end))*60; %in nm
%current frame ->. matrix
for i=1:length(allFrames) frames(i)=str2num(allFrames{i}(:)');end
frames=frames';

if (length(unique(frames))-length(frames)) ~= 0
	disp('repeated frame numbers (why?), quiting');
	return
	pause
end	

lat=str2num(cell2mat(lat));lat=lat(:,1);
lon=str2num(cell2mat(lon));lon=lon(:,1);

%convert to decimal
dlat=fix(lat/100)+(lat-fix(lat/100)*100)/60;
dlon=fix(lon/100)+(lon-fix(lon/100)*100)/60;

%find invalid readings
invalidlat=find(abs([dlat-nlfilter(dlat,[4 1],@median)])>.1);
invalidlon=find(abs([dlon-nlfilter(dlon,[4 1],@median)])>.1);
invalid=unique([invalidlat invalidlon]);

%remove invalid readings
dlon(invalid)=[];
dlat(invalid)=[];

%interpolate more points between data measurements
%interpolates frames. add frame numbers from i to i+1 in there (evenly)
%TODO: +++ raise interpolation density with measured speed , with upper limit: frames(i+1)-frames(i)  (number of frames)
%           i.e. raise interpscale when vehicle is running faster (and measurements are more scarce).
% (why not interpollating all frames? dhladh interpscale=frames(i+1)-frames(i) ? (estw apo ena speed kai panw) opote meta den thes kai matlab.)
% measurement time distance: either from utctime, or from  frame(i+1)-frame(i)/25 (25fps). =~23 seconds
% and this is about 1 measurement every 575 frames. 
%For 30km/h ara 23/3600*30000=319meters for every measurement w/ 30km/h speed.
%and (1/25)/3600*30000 = 0.33 meters per frame

dlat_interp=[];
dlon_interp=[];
frames_interp=[];
interpscale=15; 
for i=1:length(dlat)-1
	%produce interpscale-1 intermediate waypoints between points i and i+1
	[a b]=gcwaypts(dlat(i),dlon(i),dlat(i+1),dlon(i+1),interpscale);
	dlat_interp=[dlat_interp;a(1:end-1)]; %last element of a is dlat(i+1), and will be included in the next loop
	dlon_interp=[dlon_interp;b(1:end-1)];
	%select intermediate frames:
	framestep=fix((frames(i+1)-frames(i))/interpscale);
	f=frames(i):framestep:frames(i+1);
	frames_interp=[frames_interp;f(1:end-1)'];
end

worldmap([37 39],[22 25])
plotm(dlat_interp,dlon_interp,'r.')

%[l,g]=gcwaypts(dlat(1),dlon(1),dlat(30),dlon(30))
%[lattrk,lontrk] = track(dlat,dlon,[0 0],'degrees',3)
[courses,dists] = legs(dlat_interp,dlon_interp);
dists=nm2km(dists)*1000; %convert nautical miles to meters. go metric.

%x=aviread('ilisia2fotis.divx.pcm.avi',15505);imagesc(x.cdata)

%find closest record for frame 'frameno'
x=sortrows([abs(frames-frameno)  [1:length(frames)]'],1);
closest=x(1,2);

% DEG2KM Converts distances from degrees to kilometers

%find closest frame for position latlon
xlat=38;xlon=23.8;
x=sortrows([sqrt((dlat-xlat).^2+(dlon-xlon).^2) [1:length(dlat)]'],1);
closest=x(1,2);
%distance from shown image to clicked in km:
km=deg2km(distance(dlat(16),dlon(16),xlat,xlon,'degrees'));


% 4. calculate next pos depending on speed and elapsed time from previous frame display
currpos=5; %[dlon_interp(currpos) dlat_interp(currpos)]
travelled_acc=0;
subplot(1,2,1);worldmap([37.90 38.15],[23.6 24]);plotm(dlat_interp,dlon_interp,'r.')
for i=1:50000
	curspeed=4.1; %m/s %estw speed 15km/h = 15000/3600=4.1 m/s
	cnow=clock; %current time
	%display frame here
	%x=aviread('ilisia2fotis.divx.pcm.avi',15505);imagesc(x.cdata)
	
	x=aviread('ilisia2fotis.avi',frames_interp(currpos));
	subplot(1,2,2);imshow(x.cdata);
	pause (.2)
	%
	cnow2=clock;
	elapsed=cnow2(6)-cnow(6);
	if elapsed<0 continue; end;%sometimes clock is inacurrate
	
	travelled=curspeed*elapsed+travelled_acc; %meters+accumulated from previous loop
	distssum=cumsum(dists(currpos:end)); %sum up legs
	x=distssum-travelled;
	distssumpos=find(x.*(x>0)); % minimum positive value of distssum-travelled (value of distssum closest to travelled)
	if length(distssumpos)==0 disp('end of road');break;end
	newpos=distssumpos(1)+currpos-1; % pos in dists vector
	%newdlat=dlat(newpos); newdlon=dlon(newpos);
	%accumulate travelled distance in case we are moving too slow to display another frame
	if currpos==newpos travelled_acc=travelled;else travelled_acc=0;end
	currpos=newpos;
	[i currpos frames_interp(currpos) elapsed travelled]
	subplot(1,2,1);	plotm(dlat_interp(currpos),dlon_interp(currpos),'b*')
end

