# ==============================================================================
# -- import packages -----------------------------------------------------------
# ==============================================================================
#import necessary other packages which has been built in the Anaconda

import glob
import os
import sys

sys.path.append('D:\Carla\CARLA_0.9.5\PythonAPI\carla\dist\carla-0.9.5-py3.7-win-amd64.egg');
sys.path.append('D:\Carla\CARLA_0.9.5');

#search for the file path of 'carla'
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

#import carla and other packages
import carla
import random
import time
import pygame
from math import *
from matplotlib.pyplot import *
from numpy import *

# ==============================================================================
# -- Function and class --------------------------------------------------------
# ==============================================================================
#Define reset function
def reset() :
    #Variable Globalization
    global vehicle
    global transform
    #Delete vehicle
    print('Ready to restart')
    vehicle.destroy()
    #Duration
    time.sleep(1)
    #Regenerate the actor
    vehicle = world.spawn_actor(bp, transform)
    time.sleep(2)

# ==============================================================================
# -- Initialization ------------------------------------------------------------
# ==============================================================================
location_x = 18.1668
location_y = 9
location_z = 0.5
rotation_pitch = 0
rotation_yaw = 0
rotation_roll = 0

dt = 0.1
#list which stores speed information
vel_red = []

# ==============================================================================
# -- Environment settings ------------------------------------------------------
# ==============================================================================
###Connection module
#local port 2000
client = carla.Client('localhost', 2000)
#Connection timeout
client.set_timeout(20.0)        
#Get map

world = client.get_world()

world = client.load_world('Town02')


###Environment setting module
#Get the world blue Library
blueprint_library = world.get_blueprint_library()
print(blueprint_library) #View all models

#Select model
bp = blueprint_library.find('vehicle.ford.mustang')
#or bp = random.choice(blueprint_library.filter('vehicle'))

#set color
color = random.choice(bp.get_attribute('color').recommended_values)
bp.set_attribute('color', color)

#set initial position
transform = Transform(Location(x=location_x, y=location_y, z=location_z), Rotation(pitch=rotation_pitch,yaw=rotation_yaw,roll=rotation_roll))

#Generate actor in world
vehicle = world.spawn_actor(bp, transform)

# ==============================================================================
# -- main function -------------------------------------------------------------
# ==============================================================================

#Get speed information
print('velocity_x',vehicle.get_velocity().x)
print('velocity_y',vehicle.get_velocity().y)
print('velocity_z',vehicle.get_velocity().z)

#Get location information
print('position_x',vehicle.get_transform().location.x)
print('position_y',vehicle.get_transform().location.y)
print('position_z',vehicle.get_transform().location.z)

#Get angle information
print('vehicle_pitch',vehicle.get_transform().rotation.pitch)
print('vehicle_yaw',vehicle.get_transform().rotation.yaw)
print('vehicle_roll',vehicle.get_transform().rotation.roll)

#Define control variables
control = VehicleControl(throttle=0,steer=0,brake =0)

for i in range (150) : 
    
    vel_red.append(vehicle.get_velocity().x)
    
    #Set control variables
    control.throttle = 0.5  #range[0,1]
    control.brake = 0       #range[0,1]
    control.steer = 0       #range[-1,1]
    
    #Application of control variable
    vehicle.apply_control(control)
    
    #Control time domain settings
    time.sleep(dt)

#Call reset function
reset()

###Painting 
a = len(vel_red)

fig = figure(figsize=(20,10))

x1 = linspace(0,a-1,a)
y1 = linspace(0,a-1,a)
for q in range (a) :
    x1[q] = x1[q] * 0.1
    y1[q] = vel_red[q]

plot(x1,y1,label="vel",color="blue",linewidth=2)

xlabel("time (s)")
ylabel("velocity (m/s)")
title("Simulation results")
legend()
show()