# ==============================================================================
# -- Find CARLA module ---------------------------------------------------------
# ==============================================================================

import glob
import os
import sys
import queue

try:
    sys.path.append(
        glob.glob(
            '../carla/dist/carla-*%d.%d-%s.egg'
            % (
                sys.version_info.major,
                sys.version_info.minor,
                'win-amd64' if os.name == 'nt' else 'linux-x86_64',
            )
        )[0]
    )
except IndexError:
    pass

# ==============================================================================
# -- Add PythonAPI for release mode --------------------------------------------
# ==============================================================================
try:
    sys.path.append(
        os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla'
    )
    sys.path.append("../examples/")
except IndexError:
    pass

from manual_control import HUD, KeyboardControl
from manual_control import CameraManager, get_actor_display_name, find_weather_presets
from manual_control import CollisionSensor, LaneInvasionSensor, GnssSensor, IMUSensor
import numpy as np
import carla
import random

# import logging
import time
import cv2
import pygame
import argparse

# ==============================================================================
# -- set Global Variebels ---------------------------------------------------------
# ==============================================================================

# logging.basicConfig(format='%(levelname)s: %(message)s', level=logging.INFO)
random.seed(123456)


# client.load_world('Town07')
# 设置等待的最大时间
# client.set_timeout(30.0)
# 加载地图
# client.load_world("Town05")

# 设置演员列表
actors_list = []

# 设置是否保存相机图片
listen_flag = True

# 设置采集的图片的参数
IM_WIDTH = 1920
IM_HEIGHT = 1080

# 设置显示窗口大小
windos_width = 1280
windos_height = 720

# ==============================================================================
# --define  class and function ---------------------------------------------------------
# ==============================================================================


class World(object):
    def __init__(self, carla_world, hud, args, player):
        self.world = carla_world
        self.sync = args.sync
        self.actor_role_name = args.rolename
        try:
            self.map = self.world.get_map()
        except RuntimeError as error:
            print('RuntimeError: {}'.format(error))
            print('  The server could not send the OpenDRIVE (.xodr) file:')
            print(
                '  Make sure it exists, has the same name of your town, and is correct.'
            )
            sys.exit(1)
        self.hud = hud
        self.player = player
        self.collision_sensor = None
        self.lane_invasion_sensor = None
        self.gnss_sensor = None
        self.imu_sensor = None
        self.radar_sensor = None
        self.camera_manager = None
        self._weather_presets = find_weather_presets()
        self._weather_index = 0
        self._actor_filter = args.filter
        self._actor_generation = args.generation
        self._gamma = args.gamma
        self.restart()
        self.world.on_tick(hud.on_world_tick)
        self.recording_enabled = False
        self.recording_start = 0
        self.constant_velocity_enabled = False
        self.show_vehicle_telemetry = False
        self.current_map_layer = 0
        self.map_layer_names = [
            carla.MapLayer.NONE,
            carla.MapLayer.Buildings,
            carla.MapLayer.Decals,
            carla.MapLayer.Foliage,
            carla.MapLayer.Ground,
            carla.MapLayer.ParkedVehicles,
            carla.MapLayer.Particles,
            carla.MapLayer.Props,
            carla.MapLayer.StreetLights,
            carla.MapLayer.Walls,
            carla.MapLayer.All,
        ]

    def restart(self):
        self.player_max_speed = 1.589
        self.player_max_speed_fast = 3.713
        # Keep same camera config if the camera manager exists.
        cam_index = self.camera_manager.index if self.camera_manager is not None else 0
        cam_pos_index = (
            self.camera_manager.transform_index
            if self.camera_manager is not None
            else 0
        )

        self.collision_sensor = CollisionSensor(self.player, self.hud)
        self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud)
        self.gnss_sensor = GnssSensor(self.player)
        self.imu_sensor = IMUSensor(self.player)
        self.camera_manager = CameraManager(self.player, self.hud, self._gamma)
        self.camera_manager.transform_index = cam_pos_index
        self.camera_manager.set_sensor(cam_index, notify=False)
        actor_type = get_actor_display_name(self.player)
        self.hud.notification(actor_type)

        if self.sync:
            self.world.tick()
        else:
            self.world.wait_for_tick()

    def next_weather(self, reverse=False):
        self._weather_index += -1 if reverse else 1
        self._weather_index %= len(self._weather_presets)
        preset = self._weather_presets[self._weather_index]
        self.hud.notification('Weather: %s' % preset[1])
        self.player.get_world().set_weather(preset[0])

    def next_map_layer(self, reverse=False):
        self.current_map_layer += -1 if reverse else 1
        self.current_map_layer %= len(self.map_layer_names)
        selected = self.map_layer_names[self.current_map_layer]
        self.hud.notification('LayerMap selected: %s' % selected)

    def load_map_layer(self, unload=False):
        selected = self.map_layer_names[self.current_map_layer]
        if unload:
            self.hud.notification('Unloading map layer: %s' % selected)
            self.world.unload_map_layer(selected)
        else:
            self.hud.notification('Loading map layer: %s' % selected)
            self.world.load_map_layer(selected)

    def toggle_radar(self):
        if self.radar_sensor is None:
            self.radar_sensor = RadarSensor(self.player)
        elif self.radar_sensor.sensor is not None:
            self.radar_sensor.sensor.destroy()
            self.radar_sensor = None

    def modify_vehicle_physics(self, actor):
        # If actor is not a vehicle, we cannot use the physics control
        try:
            physics_control = actor.get_physics_control()
            physics_control.use_sweep_wheel_collision = True
            actor.apply_physics_control(physics_control)
        except Exception:
            pass

    def tick(self, clock):
        self.hud.tick(self, clock)

    def render(self, display):
        self.camera_manager.render(display)
        self.hud.render(display)

    def destroy_sensors(self):
        self.camera_manager.sensor.destroy()
        self.camera_manager.sensor = None
        self.camera_manager.index = None

    def destroy(self):
        if self.radar_sensor is not None:
            self.toggle_radar()
        sensors = [
            self.camera_manager.sensor,
            self.collision_sensor.sensor,
            self.lane_invasion_sensor.sensor,
            self.gnss_sensor.sensor,
            self.imu_sensor.sensor,
        ]
        for sensor in sensors:
            if sensor is not None:
                sensor.stop()
                sensor.destroy()
        if self.player is not None:
            self.player.destroy()


def process_img(image, sensor_queue):
    print("get a img!")
    image.save_to_disk('out/%06d.png' % image.frame)
    i = np.array(image.raw_data)  # convert to an array
    i2 = i.reshape(
        (IM_HEIGHT, IM_WIDTH, 4)
    )  # was flattened, so we're going to shape it.
    i3 = i2[
        :, :, :3
    ]  # remove the alpha (basically, remove the 4th index  of every pixel. Converting RGBA to RGB)
    sensor_queue.put((image.frame, "camera"))
    return i3 / 255.0  # normalize


# ==============================================================================
# -- define main funtion ---------------------------------------------------------
# ==============================================================================


def main():

    argparser = argparse.ArgumentParser(description='CARLA Manual Control Client')
    argparser.add_argument(
        '-v',
        '--verbose',
        action='store_true',
        dest='debug',
        help='print debug information',
    )
    argparser.add_argument(
        '--host',
        metavar='H',
        default='127.0.0.1',
        help='IP of the host server (default: 127.0.0.1)',
    )
    argparser.add_argument(
        '-p',
        '--port',
        metavar='P',
        default=2000,
        type=int,
        help='TCP port to listen to (default: 2000)',
    )
    argparser.add_argument(
        '-a', '--autopilot', action='store_true', help='enable autopilot'
    )
    argparser.add_argument(
        '--res',
        metavar='WIDTHxHEIGHT',
        default='1280x720',
        help='window resolution (default: 1280x720)',
    )
    argparser.add_argument(
        '--filter',
        metavar='PATTERN',
        default='vehicle.*',
        help='actor filter (default: "vehicle.*")',
    )
    argparser.add_argument(
        '--generation',
        metavar='G',
        default='2',
        help='restrict to certain actor generation (values: "1","2","All" - default: "2")',
    )
    argparser.add_argument(
        '--rolename',
        metavar='NAME',
        default='hero',
        help='actor role name (default: "hero")',
    )
    argparser.add_argument(
        '--gamma',
        default=2.2,
        type=float,
        help='Gamma correction of the camera (default: 2.2)',
    )
    argparser.add_argument(
        '--sync',
        action='store_true',
        help='Activate synchronous mode execution',
        default=True,
    )
    args = argparser.parse_args()
    args.width, args.height = [int(x) for x in args.res.split('x')]
    # ==============================================================================
    # -- create client and world_sim ---------------------------------------------------------
    # ==============================================================================

    # Connect to the client and retrieve the world object
    client = carla.Client('127.0.0.1', 2000)
    # 获取世界对象
    world_sim = client.get_world()
    try:
        # ==============================================================================
        # --set world parameters---------------------------------------------------------
        # ==============================================================================

        # 设置同步模式,carla服务器默认运行在异步模式,由于我们需要在脚本中运行一个AI,所以需要使用同步模式.
        settings = world_sim.get_settings()
        if not settings.synchronous_mode:
            settings.synchronous_mode = True
            # 仿真步长设置为0.05s
            settings.fixed_delta_seconds = 0.05
        # 设置正常运行渲染,交通流复杂时可以关闭渲染
        settings.no_rendering_mode = False
        # 将设置应用于世界
        world_sim.apply_settings(settings)

        # ==============================================================================
        # --create a car actor---------------------------------------------------------
        # ==============================================================================

        # 创建一辆录制视频流的车

        # 车辆蓝图
        vehicles_blueprints = world_sim.get_blueprint_library().filter('*vehicle*')
        ego_blueprint = random.choice(vehicles_blueprints)

        # 车辆坐标
        spawn_points = world_sim.get_map().get_spawn_points()
        ego_spawn_point = random.choice(spawn_points)

        # 生成车辆
        ego_vehicleActor = world_sim.spawn_actor(ego_blueprint, ego_spawn_point)
        print(f"ID: {ego_vehicleActor.id}  ,create ego vehicle sussesed!")
        actors_list.append(ego_vehicleActor)

        # ==============================================================================
        # --create a Image sensor  actor---------------------------------------------------------
        # ==============================================================================

        # 下面要添加传感器,这里以添加RGB相机为例:
        # Create a transform to place the camera on top of the vehicle
        camera_init_trans = carla.Transform(carla.Location(z=3))

        # We create the camera through a blueprint that defines its properties
        camera_bp = world_sim.get_blueprint_library().find('sensor.camera.rgb')

        # get the blueprint for this sensor
        # change the dimensions of the image
        camera_bp.set_attribute('image_size_x', f'{IM_WIDTH}')  # 图像宽度
        camera_bp.set_attribute('image_size_y', f'{IM_HEIGHT}')  # 图像高度
        camera_bp.set_attribute('fov', '110')  # 水平视长角 （度）
        camera_bp.set_attribute('sensor_tick', '1.0')  # 消息间隔时间 （s）
        camera_bp.set_attribute('shutter_speed', '2000')

        # We spawn the camera and attach it to our ego vehicle
        camera_actor = world_sim.spawn_actor(
            camera_bp, camera_init_trans, attach_to=ego_vehicleActor
        )
        actors_list.append(camera_actor)
        sensor_queue = queue.Queue()
        # # 有了RGB相机,下面我们需要对数据进行订阅和存储
        # # Start camera with PyGame callback
        if listen_flag:
            camera_actor.listen(lambda image: process_img(image, sensor_queue))
        # ==============================================================================
        # --create a pygame display  windows---------------------------------------------------------
        # ==============================================================================

        # 这里使用pygame模块
        pygame.init()
        pygame.font.init()
        # 使用pygame创建显示窗口
        display = pygame.display.set_mode(
            (windos_width, windos_height), pygame.HWSURFACE | pygame.DOUBLEBUF
        )
        display.fill((0, 0, 0))
        pygame.display.flip()

        # ==============================================================================
        # --create a infomation windos and controler---------------------------------------------------------
        # ==============================================================================

        # 创建车辆信息打印窗口
        hud = HUD(windos_width, windos_height)
        # 创建世界变量控制器，用于控制天气、地图等信息(禁用了车辆信息更换)
        world = World(world_sim, hud, args, ego_vehicleActor)
        # 创建键盘控制器，用于操控车辆
        controller = KeyboardControl(world, args.autopilot)

        # 真实世界更新
        if args.sync:
            world_sim.tick()
        else:
            world_sim.wait_for_tick()

        clock = pygame.time.Clock()

        while True:
            if args.sync:
                world_sim.tick()
            data = sensor_queue.get(block=True)

            clock.tick_busy_loop(60)
            if controller.parse_events(client, world, clock, args.sync):
                break
            world.tick(clock)
            world.render(display)
            pygame.display.flip()

    except KeyboardInterrupt:
        pass

    finally:
        # 摧毁已经创建的actors
        # camera_actor.destroy()
        print('\ndestroying %d actors' % len(actors_list))
        client.apply_batch([carla.command.DestroyActor(x) for x in actors_list])

        if world is not None:
            world.destroy()
        pygame.quit()

        print('\nCancelled by user. Bye!')
        settings = world_sim.get_settings()
        settings.synchronous_mode = False
        settings.fixed_delta_seconds = None
        world_sim.apply_settings(settings)
        print("finally processed!")


if __name__ == '__main__':
    main()