from __future__ import print_function


# ==============================================================================
# -- find carla module ---------------------------------------------------------
# ==============================================================================


import glob
import os
import sys

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


# ==============================================================================
# -- imports -------------------------------------------------------------------
# ==============================================================================


import carla

from carla import ColorConverter as cc

import argparse
import time
import collections
import datetime
import logging
import math
import random
import re
import weakref
import queue
import numpy
from transforms3d.euler import euler2mat, quat2euler, euler2quat


class Sensor:
    def __init__(self, name, parent, relative_spawn_pose, carla_actor):
        self.name = name
        self.parent = parent
        self.relative_spawn_pose = relative_spawn_pose
        self.carla_actor = carla_actor
        self.queue = queue.Queue()
        self.carla_actor.listen(self.sensor_callback)

    def sensor_callback(self, carla_sensor_data):
        self.queue.put(carla_sensor_data)

    def _update_synchronous_sensor(self, frame, timestamp):
        # Drain queue to the latest datum
        carla_sensor_data = None
        try:
            carla_sensor_data = self.queue.get(timeout=1.0)
        except queue.Empty:
            print("No data after 1 second wait! " + self.name + " frame: " + str(frame))

        return


def game_loop(args):
    original_settings = None
    sensor_list = []
    actor_list = []
    try:
        client = carla.Client(args.host, args.port)
        client.set_timeout(10.0)
        sim_world = client.get_world()
        original_settings = sim_world.get_settings()
        settings = sim_world.get_settings()
        settings.synchronous_mode = True
        settings.no_rendering_mode = True
        settings.fixed_delta_seconds = 0.05
        sim_world.apply_settings(settings)

        spawn_points = sim_world.get_map().get_spawn_points()
        spawn_point = spawn_points[0]

        position = carla.Vector3D(spawn_point.location.x, spawn_point.location.y, spawn_point.location.z + 1.0)
        bpLeon = sim_world.get_blueprint_library().filter("vehicle.seat.leon")[0]
        leonActor = sim_world.try_spawn_actor(bpLeon, carla.Transform(carla.Location(position.x, position.y, position.z), carla.Rotation(pitch=0.0, yaw=90.0, roll=0.0)))
        actor_list.append(leonActor)

        gnss_bp = sim_world.get_blueprint_library().find('sensor.other.gnss')
        gnss_bp.set_attribute('sensor_tick', '0.05')
        spawn_point = carla.Transform(carla.Location(x=0.0, y=0.0, z=2.0))
        gnss_actor = sim_world.spawn_actor(gnss_bp, spawn_point, attach_to=leonActor)
        gnss_sensor = Sensor("gnss01", parent=leonActor, relative_spawn_pose=spawn_point, carla_actor=gnss_actor)
        actor_list.append(gnss_actor)
        sensor_list.append(gnss_sensor)

        camera_bp = sim_world.get_blueprint_library().find('sensor.camera.rgb')
        camera_bp.set_attribute('image_size_x', '1920')
        camera_bp.set_attribute('image_size_y', '1080')
        camera_bp.set_attribute('fov', '90')       
        spawn_point = carla.Transform(carla.Location(x=-4.0, y=0.0, z=2.0))  # hood-mounted
        cam_actor_1 = sim_world.spawn_actor(camera_bp, spawn_point, attach_to=leonActor)
        cam_sensor_1 = Sensor("cam_test_1", parent=leonActor, relative_spawn_pose=spawn_point, carla_actor=cam_actor_1)
        actor_list.append(cam_actor_1)
        sensor_list.append(cam_sensor_1)

        camera_bp = sim_world.get_blueprint_library().find('sensor.camera.rgb')
        camera_bp.set_attribute('image_size_x', '1920')
        camera_bp.set_attribute('image_size_y', '1080')
        camera_bp.set_attribute('fov', '90')
        spawn_point = carla.Transform(carla.Location(x=-4.0, y=0.0, z=2.0))  # hood-mounted
        cam_actor_2 = sim_world.spawn_actor(camera_bp, spawn_point, attach_to=leonActor)
        cam_sensor_2 = Sensor("cam_test_2", parent=leonActor, relative_spawn_pose=spawn_point, carla_actor=cam_actor_2)
        actor_list.append(cam_actor_2)
        sensor_list.append(cam_sensor_2)

        prev_time = time.perf_counter()
        while True:
            frame = sim_world.tick()
            snapshot = sim_world.get_snapshot()
            timestamp = snapshot.timestamp.elapsed_seconds

            for sensor in sensor_list:
                sensor._update_synchronous_sensor(frame, timestamp)

            now = time.perf_counter()
            elapsed_ms = (now - prev_time) * 1000.0
            print(f"Frame: {frame}, wall-clock time: {now}, elapsed time: {elapsed_ms:.3f} ms")
            prev_time = now

    except KeyboardInterrupt:
        print("KeyboardInterrupt received, cleaning up...")

        if original_settings:
            print("Apply original_settings")
            sim_world.apply_settings(original_settings) 

        for actor in actor_list:
            if actor != None:
                actor.destroy()

def main():
    parser = argparse.ArgumentParser(
        description='CARLA Manual Control Client')
    parser.add_argument(
        '-v', '--verbose',
        action='store_true',
        dest='debug',
        help='print debug information')
    parser.add_argument(
        '--host',
        metavar='H',
        default='127.0.0.1',
        help='IP of the host server (default: 127.0.0.1)')
    parser.add_argument(
        '-p', '--port',
        metavar='P',
        default=2000,
        type=int,
        help='TCP port to listen to (default: 2000)')


    args = parser.parse_args()

    log_level = logging.DEBUG if args.debug else logging.INFO
    logging.basicConfig(format='%(levelname)s: %(message)s', level=log_level)

    logging.info('listening to server %s:%s', args.host, args.port)

    #print(__doc__)

    try:
        game_loop(args)

    except KeyboardInterrupt:
        print('\nCancelled by user. Bye!')


if __name__ == '__main__':
    main()