clear all;
display_active = false;

%% INIT TRACK
disp('Startar bilbanan. Avsluta med q.')
hf=figure('position', [0 0 eps eps], 'menubar', 'none');

initialize_counters(1)
initialize_counters(2)

config_IOs

start_race(1)
start_race(2)

%% INIT
global log_debug;
log_debug = true;
global log_verbose;
log_verbose = false;
% INIT DISPLAY
if display_active
    
    addpath display/ClientServerApp/Release
    cd display/ClientServerApp/Release
    !startServer
    cd ../../..
end

display = struct;
display.data = [clear_display()];
display.out = 0;
display.shm = 0;
display.shm_interp = struct;
display.shm_interp.ack = 0;
display.shm_interp.start_code = '';
display.shm_interp.data = [];
display.last_send = tic;
display.last_request = tic;
display.send_interval = 0.5;

load('bilbana\files\Bana1.mat')
load('bilbana\files\Bana2.mat')

car1 = struct;
car1.num = 1;
car1.running = false;
car1.automatic = true;
car1.stopping = false;
car1.stopped = false;
car1.segment = 1;
car1.lap = 0;
car1.lap_times = [];
car1.seg_times = [];
car1.seg_constant_list = []; % TODO
car1.position = 0;
car1.pos_at  = [0.0 2.53 3.05 4.73 7.68 8.98 10.93 14.69 17.57 19.60];
car1.seg_len = [2.53 0.53 1.68 2.92 1.2 2.01 3.83 2.89 1.99];
car1.map = Bana1;
car1.approximation = [];
car1.miss_probability = 0.0;
car1.lap_constants = [1,1,1,1,1,1,1,1,1]; % TODO
car1.constant = 0.1;
car1.stop = false;

car2 = struct;
car2.num = 2;
car2.running = false;
car2.automatic = true;
car2.stopping = false;
car2.stopped = false;
car2.segment = 1;
car2.lap = 0;
car2.lap_times = [];
car2.seg_times = [];
car2.position = 0;
car2.pos_at  = [0.0 2.53 3.05 4.92 7.62 9.02 10.72 14.68 17.76 19.95];
car2.seg_len = [2.53 0.52 1.87 2.70 1.40 1.70 4.03 3.08 2.19];
car2.map = Bana2;
car2.miss_probability = 0.1;
car2.seg_constant_list = []; % TODO
car2.lap_constants = [1,1,1,1,1,1,1,1,1]; % TODO
car2.seg_constant = 1;
car2.constant = 0.1;
car2.stop = false;

boot1 = struct;
boot1.status = false;
boot1.time = 0;

boot2 = struct;
boot2.status = false;
boot2.time = 0;

t = 0;
highToc = 0;

%% ASK ACTIVE CARS
disp('J = Ja (automatiskt), M = Ja (manuellt), N = Nej');

car1.response = input('Vill du köra bil 1? [N] ', 's');
if car1.response == 'J'
	car1.running = true;
	car1.automatic = true;
    boot1.status = true;
    boot1.time = tic;
elseif car1.response == 'M'
	car1.running = true;
	car1.automatic = false;
else
	car1.running = false;
end


car2.response = input('Vill du köra bil 2? [N] ', 's');
if car2.response == 'J'
	car2.running = true;
	car2.automatic = true;
    boot2.status = true;
    boot2.time = tic;
elseif car2.response == 'M'
	car2.running = true;
	car2.automatic = false;
else
	car2.running = false;
end
%{
ref_time = input('Vilken referenstid ska användas? [13] ', 's');
ref_time = str2double(ref_time);
if isnan(ref_time)
	ref_time = 13;
elseif not(isreal(ref_time))
	ref_time = 13;
end
%}
ref_time = 13
%% MAIN LOOP
while 1
	readTime = tic;
	%% PRE-LOOP
	if strcmp(get(hf,'currentcharacter'),'q')
		close(hf)
		break
	elseif strcmp(get(hf, 'currentcharacter'), 's')
		car1.stopping = true;
		car2.stopping = true;
	end

	figure(hf)
	drawnow
    
    %% CORE OF LOOP
    [car1, car1.stop, display.data] = do_car(car1, t, display.data, boot1);
    [car2, car2.stop, display.data] = do_car(car2, t, display.data, boot2);
    %% BOOTSTRAP
    if boot1.status
        [car1, boot1] = do_boot(car1, boot1);
    end
    if boot2.status
        [car2, boot2] = do_boot(car2, boot2);
    end

	if car1.stop == true
		disp('stopped by car 1');
		break;
	end
	if car2.stop == true
		disp('stopped by car 2');
		break;
	end

	if (~car2.running && car1.stopped) || (~car1.running && car2.stopped) || (car1.stopped && car2.stopped)
		break;
	end

	%% END OF LOOP
	while 1                     %Whileloop med paus som k�rs till pausen �verskridit 0.07 sekunder
		% DISPLAY
		display.send_delay = tic;
		if toc(display.last_send) > display.send_interval && display_active
			% queue control signal
			if car1.running && car1.automatic
				% display.data = [display.data, put_text(20, 16 + (16 * 1), 'L', num2str(car1.u))];
			end
			if car2.running && car2.automatic
				% display.data = [display.data, put_text(20, 16 + (16 * 2), 'L', num2str(car2.u))];
			end

			% send all queued data
			if ~isempty(display.data)
				[display.out] = matlabclient(1, get_smallpackage(display.data));
				display.data = [];
			end
			display.last_send = tic;

			% read internal mem from last send
			[display.out, display.shm] = matlabclient(2);
			[display.shm_interp.ack, display.shm_interp.start_code, display.shm_interp.data] = get_response(display.shm);

			% request internal mem
			% matlabclient(1, hex2dec(['12'; '01'; '53'; '66']));
		end
		% disp(strjoin({'display took additional ', num2str(toc(display.send_delay))}));
		% ACTUAL END OF LOOP
		t = toc(readTime);

		if t > 0.07
			if t > highToc
				highToc = t;     %Om det nya v�rdet p� pausen �r h�gre �n den tidigare h�gsta s� sparas det som den h�gsta
			end
			if t > 0.1
				% beep;
			end
			break;
		end
		pause(0.001);
	end
end

%% END OF PROGRAM
disp(highToc);
disp(car1);
disp(car2);

terminate(1);
terminate(2)
if display_active
    matlabclient(3);
end
%% DISPLAY GRAPHS

if car1.running == true
	graphs(car1.lap_times, ref_time, car1.seg_times, 1);
end


if car2.running == true
	graphs(car2.lap_times, ref_time, car2.seg_times, 2);
end

%% SAVE VARIABLES FROM CAR STRUCT
dateStr = datestr(now, 'yyyy-mm-dd');
timeStr = datestr(now, 'HH.MM');

if car1.lap > 2
    filenameMat1 = strjoin({'bilbana1_', dateStr, 'T', timeStr, '.mat'}, '');
    save(filenameMat1, '-struct', 'car1');
end
if car2.lap > 2
    filenameMat2 = strjoin({'bilbana2_', dateStr, 'T', timeStr, '.mat'}, '');
    save(filenameMat2, '-struct', 'car2');
end