# import functions to read xml file and visualize commonroad objects
from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.visualization.mp_renderer import MPRenderer

# generate path of the file to be opened
file_path = "ZAM_Tutorial-1_1_T-1.xml"

# read in the scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader(file_path).open()

# plot the scenario
rnd = MPRenderer(figsize=(25, 10))
scenario.draw(rnd)
rnd.render(show=True)

import os
from simulation.simulations import simulate_without_ego, simulate_with_solution, simulate_with_planner

path = os.path.abspath("")
# specify required arguments
name_scenario = 'DEU_Frankfurt-34_11_I-1'
folder_scenarios = os.path.join(path, 'interactive_scenarios')
path_scenario = os.path.join(folder_scenarios, name_scenario)
path_video = os.path.join(path, '../outputs/videos')
# run simulation. alternatively, use functions simulate_with_solution() and simulate_with_planner()
scenario_without_ego, pps = simulate_without_ego(interactive_scenario_path=path_scenario,
			output_folder_path=path_video,
			create_video=True)

from commonroad.common.file_reader import CommonRoadFileReader
from commonroad.common.solution import CommonRoadSolutionReader
from commonroad_dc.feasibility.solution_checker import obstacle_collision,
     boundary_collision, solution_feasible
from commonroad_dc.costs.evaluation import CostFunctionEvaluator

# load scenario and planning problem set
scenario, planning_problem_set = CommonRoadFileReader('Your_Scenario.xml').open()
# load solution (see commonroad-io Tutorial 2 for creating solutions)
solution = CommonRoadSolutionReader('Your_Solution.xml')

# check obstacle collisions
collision_result = obstacle_collision(scenario, planning_problem_set, solution)

# check road compliance
boundary_result = boundary_collision(scenario, planning_problem_set, solution)

# check kinematic feasibility
feasibility_result = solution_feasible(solution, scenario.dt, planning_problem_set)

# evaluate solution w.r.t. a cost function
ce = CostFunctionEvaluator.init_from_solution(solution)
cost_result = ce.evaluate_solution(scenario, planning_problem_set, solution)

# commonrad
from commonroad.common.file_reader import CommonRoadFileReader

# Own Code base
import commonroad_route_planner.fast_api.fast_api as fast_api

def main(path_to_xml: str, save_imgs: bool = False, save_path: str = ""):
    # Load the CommonRoad Scenario Object
    scenario, planning_problem_set = CommonRoadFileReader(
        f"{path_to_xml}"
    ).open()

    # retrieve the first planning problem in the problem set
    planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]


    # get reference path
    reference_path = fast_api.generate_reference_path_from_lanelet_network_and_planning_problem(
               lanelet_network=scenario.lanelet_network,
               planning_problem=planning_problem
     )



if __name__ == "__main__":
    path_to_xml: str = "absolute/path/to/cr/xml.xml"
    save_path: str = "absolute/path/to/saving/dir"
    main(save_imgs=True, path_to_xml=path_to_xml)

from commonroad.common.file_reader import CommonRoadFileReader
from commonroad_route_planner.route_planner import RoutePlanner
from commonroad_route_planner.route import Route
from commonroad_velocity_planner.velocity_planner_interface import IVelocityPlanner
from commonroad_velocity_planner.configuration.configuration_builder import ConfigurationBuilder
from commonroad_velocity_planner.velocity_planning_problem import VppBuilder

# cr-io
scenario, planning_problem_set = CommonRoadFileReader("path/to/commonroad/xml").open()
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]

# route planner
route: Route = RoutePlanner(
    lanelet_network=scenario.lanelet_network, planning_problem=planning_problem
).plan_routes().retrieve_shortest_route()

# Velocity Planner
global_trajectory = IVelocityPlanner().plan_velocity(
    route=route,
    planner_config=ConfigurationBuilder().get_predefined_configuration(),
    velocity_planning_problem=VppBuilder().build_vpp(
        route=route, 
        planning_problem=planning_problem, 
        default_goal_velocity=planning_problem.initial_state.velocity
    )
)

from commonroad.common.file_reader import CommonRoadFileReader
from commonroad_raceline_planner.raceline import RaceLine
from commonroad_raceline_planner.raceline_fast_api import generate_ftm_minimum_curvature_raceline_from_cr

scenario, planning_problem_set = CommonRoadFileReader("PATH/TO/YOUR/SCENARIO").open()
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]

raceline: RaceLine = generate_ftm_minimum_curvature_raceline_from_cr(
                    cr_scenario=scenario,
                    planning_problem=planning_problem,
                    ini_path="PATH/TO/.ini",
                    ggv_file="PATH/TO/ggv",
                    ax_max_machines_file="PATH/TO/ENGINECONSTRAINTS",
                    show_plot=True
                )

from commonroad_rp.reactive_planner import ReactivePlanner
from commonroad_rp.utility.config import ReactivePlannerConfiguration
from commonroad_route_planner.route_planner import RoutePlanner

# scenario filename
filename = "ZAM_Over-1_1.xml"

# Build planner configuration object (includes loading scenario)
config = ReactivePlannerConfiguration.load(f"tutorial_config_{filename[:-4]}.yaml", filename)
config.update()

# run route planner to compute route
route_planner = RoutePlanner(config.scenario, config.planning_problem)
route = route_planner.plan_routes().retrieve_first_route()

# initialize reactive planner
planner = ReactivePlanner(config)

# set reference path and desired velocity (here we just keep the current speed)
planner.set_reference_path(route.reference_path)
planner.set_desired_velocity(current_speed=planner.x_0.velocity)

# call plan function
optimal_result = planner.plan()

from commonroad.common.file_reader import CommonRoadFileReader
from maneuver_automaton.maneuver_automaton import ManeuverAutomaton
from motion_planner.motion_planner import MotionPlanner

scenario = '../../scenarios/tutorial/ZAM_Tutorial_Urban-3_2.xml'
motionprimitives = 'V_9.0_9.0_Vstep_0_SA_-0.2_0.2_SAstep_0.4_T_0.5_Model_BMW320i.xml'

# load scenario and planning problem set, retrieve the first planning problem
scenario, planning_problem_set = CommonRoadFileReader(scenario).open()
planning_problem = list(planning_problem_set.planning_problem_dict.values())[0]

# create maneuver automaton
automaton = ManeuverAutomaton.generate_automaton(motionprimitives)

# create and execute planner
planner = MotionPlanner.AStarSearch(scenario=scenario,
									planning_problem=planning_problem,
									automaton=automaton)
path_solution, _, _ = planner.execute_search()
for primitive in path_solution:
	for state in primitive:
		print(state)

from commonroad_reach.data_structure.configuration_builder import ConfigurationBuilder
from commonroad_reach.data_structure.reach.reach_interface import ReachableSetInterface
from commonroad_reach.utility import visualization as util_visual


# build configuration
# make sure that the "scenarios" directory in your current directory contains the ARG_Carcarana-1_1_T-1 scenario
config = ConfigurationBuilder().build_configuration("ARG_Carcarana-1_1_T-1")
config.update()

# compute reachable sets using reachability interface
reach_interface = ReachableSetInterface(config)
reach_interface.compute_reachable_sets()

# plot computation results
util_visual.plot_scenario_with_reachable_sets(reach_interface)

import gym
import commonroad_rl.gym_commonroad

# kwargs overwrites configs defined in commonroad_rl/gym_commonroad/configs.yaml
env = gym.make("commonroad-v1",
		# if installed via pip add the paths to the data folders and uncomment the following two lines 
		# meta_scenario_path=meta_scenario_path, #path to meta scenario specification 
		# train_reset_config_path= training_data_path, #path to training pickels
               action_configs={"action_type": "continuous"},
               goal_configs={"observe_distance_goal_long": True, "observe_distance_goal_lat": True},
               surrounding_configs={"observe_lane_circ_surrounding": False,
               		     "fast_distance_calculation": False,
                                    "observe_lidar_circle_surrounding": True,
                                    "lidar_circle_num_beams": 20},
               reward_type="sparse_reward",
               reward_configs={"sparse_reward":{"reward_goal_reached": 50.,
                                      "reward_collision": -100.,
                                      "reward_off_road": -50.,
      					"reward_time_out": -10.,
					"reward_friction_violation": 0.}})

observation = env.reset()
for _ in range(500):
    # env.render() # rendered images with be saved under ./img
    action = env.action_space.sample() # your agent here (this takes random actions)
    observation, reward, done, info = env.step(action)

    if done:
        observation = env.reset()
env.close()

from commonroad.common.file_reader import CommonRoadFileReader
from carlacr.carla_interface import CarlaInterface
from carlacr.helper.config import CarlaParams, CustomVis

# Specify CommonRoad scenario
scenario, pps = CommonRoadFileReader(cr_scenario_path := "scenarios/DEU_Test-1_1_T-2.xml").open()

# Configure simulation and scenario settings
param = CarlaParams()
param.map = cr_scenario_path
param.vis_type = CustomVis.EGO

# Execute simulation for replaying CommonRoad scenario
CarlaInterface(param).replay(scenario, ego_id=6)

from commonroad_crime.data_structure.configuration import CriMeConfiguration
from commonroad_crime.data_structure.crime_interface import CriMeInterface
from commonroad_crime.measure import (TTC, TTCStar, TTB, TTS, TTK, TTR, THW, WTTC, BTN, PF, HW,
                                      ALongReq, ALatReq, STN, P_MC, LongJ, LatJ, DeltaV)

# ==== specify scenario ID
scenario_id = "DEU_Gar-1_1_T-1"

# ==== build configuration
config = CriMeConfiguration.load(f"../config_files/{scenario_id}.yaml", scenario_id)
# or use the default one with: config = CriMeConfiguration()
config.update()
config.print_configuration_summary()

# ==== compute the criticality using CriMe interface
crime_interface = CriMeInterface(config)
crime_interface.evaluate_scenario([HW, THW, TTC, WTTC, TTCStar, TTS, TTK, TTB, TTR, ALongReq,
                                   ALatReq, LongJ, LatJ, DeltaV, BTN, STN, P_MC, PF], )  # and more ...

# ==== visualize the results for debugging and showcasing
crime_interface.visualize()

simulation = SumoSimulation(
    "data/osm_crawled/DEU_Munich_1-100.xml"
)
traffic_extractor = TrafficExtractor(
    simulation=simulation,
    options=TrafficExtractorOptions(
        edge_drawer=VoronoiEdgeDrawer(dist_threshold=30.0),
        #feature_computers=...
    )
)

with simulation:
    for time_step, scenario in simulation(to_time_step=200):
        data = traffic_extractor.extract(TrafficExtractionParams(
            index=time_step,
        ))
        print(data)
        print(data.vehicle.velocity)

>> CommonRoadData(scenario_id=ZAM_TMP-0_0, t=0, dt=0.04)
>>  - VirtualAttributesNodeStorage('vehicle')
>>        x: Tensor (torch.Size([1, 19]), torch.float32)
>>                  velocity: idx (0, 2)
>>                  acceleration: idx (2, 4)
>>                  ...
>>        ...
>>  - VirtualAttributesEdgeStorage(('vehicle', 'to', 'vehicle'))
>>       edge_index: Tensor (torch.Size([2, 0]), torch.int64)
>>       edge_attr: Tensor (torch.Size([0, 10]), torch.float32)
>>                  same_lanelet: idx (0, 1) 
>>                  distance: idx (1, 2)
>>                  ...
>>       ...
>>  - VirtualAttributesNodeStorage('lanelet')
>>  ...

>> tensor([[6.21., 0.], [5.12, 0.], [7.45, 0.], ...])

from osc_cr_converter.converter.osc2cr import Osc2CrConverter
from osc_cr_converter.utility.configuration import ConverterParams
from osc_cr_converter.utility.visualization import visualize_scenario

# scenario path and name
openscenario = 'Path_to_Your_OpenSCENARIO.xosc'

# build configuration
config = ConverterParams()

# convert OpenSCENARIO to CommonRoad
converter = Osc2CrConverter(config)
scenario = converter.run_conversion(openscenario)

# visualize the converted CommonRoad scenario
visualize_scenario(scenario, config)