package imat.tc;

import java.io.IOException;

/**
 *
 * @author Imanol
 */

public class Example_5 {
   
    public static void main(String[] args) throws IOException, InterruptedException {
                
        System.out.println("Example 5" + "\n");
        
        // Create TC instance
        TC myTC = new TC();
    
        // Initialize
        myTC.TC_Init();
        myTC.Settings_Enable_Logging();
        
        // Get generation
        System.out.println("Generation");
        System.out.println(myTC.TC_Generation());

        // Get build version
        System.out.println("Build Version");
        System.out.println(myTC.TC_Build_Version() + "\n");     
        
        // Get num vehicles
        int Num_Vehicles = myTC.Monitoring_Get_Num_Vehicles();
        System.out.println("Number of vehicles: " + Num_Vehicles);
        
        // Create Environment
        System.out.println("\nCreating environment");

        myTC.Environment_Clean();

        System.out.println("Max pos: " + myTC.Environment_Get_Max_Pos());
        System.out.println("Max roads: " + 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();
        
        System.out.println("Environment built");

        // Define current and goal positions
        System.out.println("\nDefining current positions");
        myTC.Vehicle_Set_Current_Pos(1, 1);
        myTC.Vehicle_Set_Current_Pos(2, 2);
        myTC.Vehicle_Set_Current_Pos(3, 8);

        System.out.println("Pos for vehicle 1 is: " + myTC.Vehicle_Get_Current_Pos(1));
        System.out.println("Pos for vehicle 2 is: " + myTC.Vehicle_Get_Current_Pos(2));
        System.out.println("Pos for vehicle 3 is: " + myTC.Vehicle_Get_Current_Pos(3));

        System.out.println("Defining goal positions");

        myTC.Vehicle_Set_Goal_Pos(1, 9);
        myTC.Vehicle_Set_Goal_Pos(2, 8);
        myTC.Vehicle_Set_Goal_Pos(3, 1);

        System.out.println("Goal for vehicle 1 is 9");
        System.out.println("Goal for vehicle 2 is 8");
        System.out.println("Goal for vehicle 3 is 1");

        // Start mission
        System.out.println("\n\nStart_Mission (the solution is firstly computed)");
        boolean succ = myTC.TC_Start_Mission();
        System.out.println("Solution: " + succ);

        System.out.println("CPU time is (s): " + myTC.Monitoring_Get_CPU_Time());

        int Num_Steps = myTC.TC_Get_Num_Steps();
        System.out.println("Number of steps: " + Num_Steps);        
        
        boolean obstacles_trigger = false;
        
        // Move to goal
        for (int s = 0; s < Num_Steps; s++){
            
            // Obstacle detected at step 2
            if ((s==1)&& (obstacles_trigger == false)){
                
                System.out.println("\n Obstacles detected !!!");
                
                myTC.Obstacles_Clean();
                myTC.Obstacles_Add_Obstacle(3, 6);
                
                System.out.println("Num obstacles: "+myTC.Obstacles_Get_Num());
                
                succ = myTC.Obstacles_Confirm();
                                
                System.out.println("Solution " + succ);
                System.out.println("CPU time is (s): " + myTC.Monitoring_Get_CPU_Time());
                
                Num_Steps = myTC.TC_Get_Num_Steps();
                System.out.println("New number of steps: " + Num_Steps+"\n");  
                
                // Reset step counter and toggle obstacles trigger
                s = 0;
                obstacles_trigger = true;
                
            }
            
            System.out.println("\nNext positions are:");
            System.out.println("Next pos for vehicle 1 is: " + myTC.Vehicle_Get_Next_Pos(1));
            System.out.println("Next pos for vehicle 2 is: " + myTC.Vehicle_Get_Next_Pos(2));
            System.out.println("Next pos for vehicle 3 is: " + myTC.Vehicle_Get_Next_Pos(3));

            System.out.println(" ");    
            System.out.println("Move forward");
            System.out.println("Goal: " + myTC.Vehicle_Moved_Forward());

            int pos_v1 = myTC.Vehicle_Get_Current_Pos(1);
            int pos_v2 = myTC.Vehicle_Get_Current_Pos(2);
            int pos_v3 = myTC.Vehicle_Get_Current_Pos(3);
    
            System.out.println("Pos for vehicle 1 is: " + pos_v1);
            System.out.println("Pos for vehicle 2 is: " + pos_v2);
            System.out.println("Pos for vehicle 3 is: " + pos_v3);
            
            System.out.println("Goal reached ¿? " + myTC.Vehicle_Goal_Reached());
            
        }
        
        // Done, end mission
        myTC.TC_End_Mission();
                
    }
    
}