%%%%%%%%%%%%%%%%%%
% Example Matlab
%%%%%%%%%%%%%%%%%%

% Clean command window
clc
clear all

% Create object/variable
pyTC = py.tc.Traffic_Controller;

% Print generation
generation = pyTC.TC_Generation;
disp("Generation is:")
disp(string(generation))

pyTC.Settings_Enable_Logging;

% Initialize
pyTC.TC_Init

% Get number of defined vehicles
NumVehicles = pyTC.Monitoring_Get_Num_Vehicles;

disp("Create environment")

pyTC.Environment_Clean;

disp("Maximum number of positions")
disp(string(pyTC.Environment_Get_Max_Pos))

disp("Maximum number of roads")
disp(string(pyTC.Environment_Get_Max_Roads))

pyTC.Environment_Add_Two_Way_Road_One_Lane('0', '1')
pyTC.Environment_Add_Two_Way_Road_One_Lane('0', '3')
pyTC.Environment_Add_Two_Way_Road_One_Lane('1', '4')
pyTC.Environment_Add_Two_Way_Road_One_Lane('2', '3')
pyTC.Environment_Add_Two_Way_Road_One_Lane('3', '4')
pyTC.Environment_Add_Two_Way_Road_One_Lane('3', '6')
pyTC.Environment_Add_Two_Way_Road_One_Lane('4', '7')
pyTC.Environment_Add_Two_Way_Road_One_Lane('5', '6')
pyTC.Environment_Add_Two_Way_Road_One_Lane('6', '7')
pyTC.Environment_Add_Two_Way_Road_One_Lane('6', '8')
pyTC.Environment_Add_Two_Way_Road_One_Lane('7', '9')
pyTC.Environment_Add_Two_Way_Road_One_Lane('8', '9')

pyTC.Environment_Build()
disp("Environment built")

% Define current and goal positions

disp("Defining current positions")
pyTC.Vehicle_Set_Current_Pos('1', '2')
pyTC.Vehicle_Set_Current_Pos('2', '3')
pyTC.Vehicle_Set_Current_Pos('3', '4')

disp("Pos for vehicle 1 is: " + string(pyTC.Vehicle_Get_Current_Pos('1')))
disp("Pos for vehicle 2 is: " + string(pyTC.Vehicle_Get_Current_Pos('2')))
disp("Pos for vehicle 3 is: " + string(pyTC.Vehicle_Get_Current_Pos('3')))

disp("Defining goal positions")

pyTC.Vehicle_Set_Goal_Pos('1', '8')
disp("Goal for vehicle 1 is 8")
pyTC.Vehicle_Set_Goal_Pos('2', '2')
disp("Goal for vehicle 2 is 2")
pyTC.Vehicle_Set_Goal_Pos('3', '6')
disp("Goal for vehicle 3 is 6")

% Start mission

succ = pyTC.TC_Start_Mission;
disp(string(succ))

disp("CPU time is (ms): " + string(pyTC.Monitoring_Get_CPU_Time()))

Num_Steps = str2num(string(pyTC.TC_Get_Num_Steps()));
disp("Number of steps: " + Num_Steps)

% Follow paths
for i = 1:Num_Steps

    disp(' ')
    disp("Next positions are:")
    disp("Next pos for vehicle 1 is: " + string(pyTC.Vehicle_Get_Next_Pos('1')))
    disp("Next pos for vehicle 2 is: " + string(pyTC.Vehicle_Get_Next_Pos('2')))
    disp("Next pos for vehicle 3 is: " + string(pyTC.Vehicle_Get_Next_Pos('3')))

    disp("Move forward")
    disp("Goal: " + string(pyTC.Vehicle_Moved_Forward()))

    disp(' ')

end

% End mission
pyTC.TC_End_Mission()

% EOF