########################
##
##  Example 2
##
########################

import time

from tc import Traffic_Controller

print("Intializing TC")

myTC = Traffic_Controller()
myTC.Settings_Enable_Logging()
myTC.TC_Init()
Num_Vehicles = myTC.Monitoring_Get_Num_Vehicles()

print("Generation is: " + str(myTC.TC_Generation()))
print("Number of vehicles: " + str(Num_Vehicles))

### Environment

print("Creating environment")

myTC.Environment_Clean()

print("Max pos: " + str(myTC.Environment_Get_Max_Pos()))
print("Max roads: " + str(myTC.Environment_Get_Max_Roads()))

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

myTC.Environment_Build()
print("Environment built")

###

print("Defining current positions")
myTC.Vehicle_Set_Current_Pos(1, 2)
myTC.Vehicle_Set_Current_Pos(2, 6)
myTC.Vehicle_Set_Current_Pos(3, 8)

print("Pos for vehicle 1 is: " + str(myTC.Vehicle_Get_Current_Pos(1)))
print("Pos for vehicle 2 is: " + str(myTC.Vehicle_Get_Current_Pos(2)))
print("Pos for vehicle 3 is: " + str(myTC.Vehicle_Get_Current_Pos(3)))

print("Defining goal positions")

myTC.Vehicle_Set_Goal_Pos(1, 4)
myTC.Vehicle_Set_Goal_Pos(2, 8)
myTC.Vehicle_Set_Goal_Pos(3, 1)

print("Goal for vehicle 1 is 4")
print("Goal for vehicle 2 is 8")
print("Goal for vehicle 3 is 1")

### 

print("Start_Mission (the solution is firstly computed)")
succ = myTC.TC_Start_Mission()
print("Solution " + str(succ))

CPU = float(myTC.Monitoring_Get_CPU_Time()) * 1000
print("CPU (ms): " + str(CPU))

Num_Steps = int(myTC.TC_Get_Num_Steps())
print("Number of steps: " + str(Num_Steps))

# Flags and counters

Flag_Obstacle = False
Step_Counter = 0

for s in range(0, 8):

    # Obstacle appears =P, only once =)
    if ((Step_Counter == 1) and (Flag_Obstacle == False)):

        print("")
        print("Obstacle in road 4")
        myTC.Obstacles_Clean()
        myTC.Obstacles_Add_Obstacle(1, 4)

        Result = myTC.Obstacles_Confirm()
        print("Solution: " + str(Result))

        Num_Steps = int(myTC.TC_Get_Num_Steps())
        print("Number of steps: " + str(Num_Steps))

        CPU = float(myTC.Monitoring_Get_CPU_Time()) * 1000
        print("CPU (ms): " + str(CPU))

        Flag_Obstacle = True

    print("Next positions are:")
    print("Next pos for vehicle 1 is: " + str(myTC.Vehicle_Get_Next_Pos(1)))
    print("Next pos for vehicle 2 is: " + str(myTC.Vehicle_Get_Next_Pos(2)))
    print("Next pos for vehicle 3 is: " + str(myTC.Vehicle_Get_Next_Pos(3)))

    print(" ")    
    print("Move forward")
    Flag_Goal =  myTC.Vehicle_Moved_Forward()
    print("Goal: " + str(Flag_Goal))

    pos_v1 = myTC.Vehicle_Get_Current_Pos(1)
    pos_v2 = myTC.Vehicle_Get_Current_Pos(2)
    pos_v3 = myTC.Vehicle_Get_Current_Pos(3)
    
    print("Pos for vehicle 1 is: " + str(pos_v1))
    print("Pos for vehicle 2 is: " + str(pos_v2))
    print("Pos for vehicle 3 is: " + str(pos_v3))
    
    # Increase step counter
    Step_Counter += 1

print("")
print("Done, end of mission");

myTC.TC_End_Mission()

#
# EOF
#