# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .py
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Python 3 (ipykernel)
#     language: python
#     name: python3
# ---

# # Model Evaluation

# +
import os
import gym
import gym_donkeycar
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import cv2 as cv
import time
import pickle
import birds_eye_vector_space
import random
from pandas import Series, DataFrame
from collections import deque

#from keras.layers import Dense
#from tensorflow.keras.optimizers import Adam
#from keras.initializers import normal, identity
#from keras.models import model_from_json
#from keras.models import Sequential
#from keras.layers.core import Dense, Dropout, Activation, Flatten
#from keras.layers.convolutional import Convolution2D, MaxPooling2D

import tensorflow as tf
from tensorflow import keras
from tensorflow.keras import layers
from tensorflow.python.client import device_lib
#from keras import backend as K
#import tensorflow.keras.backend as K
# -

len(tf.config.list_physical_devices('GPU'))
device_lib.list_local_devices()

# Supress scientific notation like: e*+03
np.set_printoptions(suppress=True)

# +
ANCHOR_POINTS = 8
ROAD_ROI = np.array([(120,90),(200,90),(0,200),(320,200)],dtype='float32')
WARPED_IMAGE_SHAPE = np.array([[10,320],[0,0],[200,0],[200,310]],np.int32)            # NEW IMAGE Shape after Warping !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
WARPED_IMAGE_HEIGHT = 320
WARPED_IMAGE_WIDTH = 200
NUMBER_OF_DEPTH_LAYERS = 50

VECTOR_SPACE_IMAGE_ROWS = 50
VECTOR_SPACE_IMAGE_COLUMNS = 100
VECTOR_SPACE_IMAGE_CHANNELS = 4 # 4*(50,100) stacked frames 


# -

# function to save the plots
def save_plot(data, name, episode):
    plt.figure(figsize=(8,5), frameon=True)
    #plt.plot([episode for episode in range(len(data))], data)
    plt.plot([ep for ep in range(episode)], data)
    plt.xlabel('Episodes')
    plt.ylabel(name)
    figplot = '%s-episode-%s.png' % (name,episode)
    #figplot = '{}-episode-{}.png'.format(name,episode)
    #location = '{}/saves/torch/{}'.format(os.getcwd(),figplot)
    #location = '%s/collected_data/plots/%s' % (os.getcwd(),figplot)
    location = 'C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\collected_data\\plots\\%s' % figplot 
    plt.savefig(location, transparent=False)
    plt.close()


def save_data_as_dataframe(episode_number, reward, loss, measured_time, max_cte, average_cte, last_lap_time):
    episodes = [i for i in range(1, episode_number+1)]
    collected_data = {'episode': episodes, 'reward': reward, 'loss':loss, 'time': measured_time, 'max_cte': max_cte, 'average_cte': average_cte, 'lap_time': last_lap_time}
    df_data = DataFrame.from_dict(collected_data).set_index('episode')
    
    df_name = 'data-episode-%s.pkl' % episode
    #location = '%s/collected_data/raw_data/%s' % (os.getcwd(),df_name)
    location = 'C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\collected_data\\raw_data\\%s' % df_name
    df_data.to_pickle(location)


vector_space = birds_eye_vector_space.Vector_space(ANCHOR_POINTS, ROAD_ROI, WARPED_IMAGE_SHAPE, WARPED_IMAGE_HEIGHT, WARPED_IMAGE_WIDTH, NUMBER_OF_DEPTH_LAYERS)

os.getcwd()


class DQNAgent:
    
    def __init__(self, state_size, action_size):
        self.t = 0
        self.max_Q = 0
        self.train = True
        # Set to True to train on images with segmented lane lines
        self.lane_detection = False
        
        # Huber loss
        self.huber_loss = tf.keras.losses.Huber(reduction=tf.keras.losses.Reduction.SUM)
        
        # Get size of state and action
        self.state_size = state_size
        self.action_size = action_size
        
        # CNN Input
        #self.input_shape = (VECTOR_SPACE_IMAGE_CHANNELS, VECTOR_SPACE_IMAGE_ROWS, VECTOR_SPACE_IMAGE_COLUMNS, 4)   # == 4 * (50, 100, 1); keras: (1,50,100,4)
        self.input_shape = (1, VECTOR_SPACE_IMAGE_ROWS, VECTOR_SPACE_IMAGE_COLUMNS, VECTOR_SPACE_IMAGE_CHANNELS)   # == 4 * (50, 100, 1); keras: (1,50,100,4)
        
        # These are hyper parameters for the DQN
        self.discount_factor = 0.99
        self.learning_rate = 1e-4
        if (self.train):
            self.epsilon = 1.0
            self.initial_epsilon = 1.0
        else:
            self.epsilon = 1e-6
            self.initial_epsilon = 1e-6
        self.epsilon_min = 0.02
        self.batch_size = 512
        self.train_start = 100
        self.explore = 10000
        
        # Create replay memory using deque
        self.memory = deque(maxlen=10000)
        
        # Create main model and target model                # Double DQN !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        self.model = self.build_model()
        self.target_model = self.build_model()

        # Copy the model to target model
        # --> initialize the target model so that the parameters of model & target model to be same
        self.update_target_model()
        
    def build_model_old(self):
        print('delte this')
        #model = Sequential()
        #model.add(Convolution2D(32, 8, 8, subsample=(4, 4), border_mode='same',input_shape=(VECTOR_SPACE_IMAGE_ROWS,VECTOR_SPACE_IMAGE_COLUMNS,VECTOR_SPACE_IMAGE_CHANNELS)))  #80*80*4
        #model.add(Activation('relu'))
        #model.add(Convolution2D(64, 4, 4, subsample=(2, 2), border_mode='same'))
        #model.add(Activation('relu'))
        #model.add(Convolution2D(64, 3, 3, subsample=(1, 1), border_mode='same'))
        #model.add(Activation('relu'))
        #model.add(Flatten())
        #model.add(Dense(512))
        #model.add(Activation('relu'))

        ## 15 categorical bins for Steering angles
        #model.add(Dense(15, activation="linear")) 

        #adam = Adam(lr=self.learning_rate)
        #model.compile(loss='mse',optimizer=adam)

        #return model

    def build_model(self):
        model = keras.Sequential([
            layers.Conv2D(filters=32, strides=(4, 4),kernel_size=8, padding='same', activation='relu', input_shape=(self.input_shape[1:])),
            layers.Conv2D(filters=64, strides=(2, 2),kernel_size=4, padding='same', activation='relu'),
            layers.Conv2D(filters=64, strides=(1, 1),kernel_size=3, padding='same', activation='relu'),
            layers.Flatten(),
            layers.Dense(units=512,activation='relu'),
            layers.Dense(units=512,activation='relu'),
            layers.Dense(units=15,activation='linear'),
        ])
        optimizer = tf.keras.optimizers.Adam(learning_rate=self.learning_rate)
        model.compile(loss='mse', optimizer=optimizer)
        return model
    
    def calculate_loss(self, y_true, y_pred):    
        y_pred = tf.convert_to_tensor_v2(y_pred)
        y_true = tf.cast(y_true, y_pred.dtype)
        return tf.reduce_mean(math_ops.square(y_pred - y_true), axis=-1)
    
    def update_target_model(self):
        self.target_model.set_weights(self.model.get_weights())

    # Get action from model using epsilon-greedy policy
    def get_action(self, s_t):
        #if np.random.rand() <= self.epsilon:
        #    #print("Return Random Value")
        #    #return random.randrange(self.action_size)
        #    return np.random.uniform(-1,1)
        #else:
        #    #print("Return Max Q Prediction")
        #    q_value = self.model.predict(s_t)
        #    # Convert q array to steering value
        #    return linear_unbin(q_value[0])
        q_value = self.model.predict(s_t)
        # Convert q array to steering value
        return linear_unbin(q_value[0])

    def replay_memory(self, state, action, reward, next_state, done):
        self.memory.append((state, action, reward, next_state, done))
        #if self.epsilon > self.epsilon_min:
        #    #self.epsilon *= self.epsilon_decay
        #    self.epsilon -= (self.initial_epsilon - self.epsilon_min) / self.explore

    def train_replay(self):
        if len(self.memory) < self.train_start:
            return
        
        # Epsilion decay over time/amount of trainings
        if self.epsilon > self.epsilon_min:
            #self.epsilon *= self.epsilon_decay
            #self.epsilon -= (self.initial_epsilon - self.epsilon_min) / self.explore
            self.epsilon -= 0.00025
            
        batch_size = min(self.batch_size, len(self.memory))
        minibatch = random.sample(self.memory, batch_size)

        state_t, action_t, reward_t, state_t1, terminal = zip(*minibatch)
        #print('Starting training\nstate_t: %s\naction_t: %s\nreward_t: %s\nstate_1: %s\nterminal: %s' % (len(state_t), len(action_t), len(reward_t), len(state_t1), len(terminal)))
        state_t = np.concatenate(state_t)
        state_t1 = np.concatenate(state_t1)
        #print('Concat:\nstate: %s\nstate_t1: %s' % (state_t, state_t1))
        targets = self.model.predict(state_t)
        self.max_Q = np.max(targets[0])
        target_val = self.model.predict(state_t1)
        target_val_ = self.target_model.predict(state_t1)
        for i in range(batch_size):
            if terminal[i]:
                targets[i][action_t[i]] = reward_t[i]
            else:
                a = np.argmax(target_val[i])
                targets[i][action_t[i]] = reward_t[i] + self.discount_factor * (target_val_[i][a])

        
        # train on the current batch and return a dictionary with the loss and so on
        #metrics = self.model.train_on_batch(x=state_t, y=targets, reset_metrics=True, return_dict=True)           
        metrics = self.model.train_on_batch(x=state_t, y=targets, reset_metrics=False, return_dict=True) #reset Metrix different
        return metrics['loss']
        
    def load_model(self, name):
        self.model.load_weights(name)

    # Save the model which is under training
    def save_model(self, name):
        self.model.save_weights(name)


def linear_bin(a):
    """
    Convert a value to a categorical array.

    Parameters
    ----------
    a : int or float
        A value between -1 and 1

    Returns
    -------
    list of int
        A list of length 15 with one item set to 1, which represents the linear value, and all other items set to 0.
    """
    a = a + 1
    b = round(a / (2 / 14))
    arr = np.zeros(15)
    arr[int(b)] = 1
    return arr


def linear_unbin(arr):
    """
    Convert a categorical array to value.

    See Also
    --------
    linear_bin
    """
    if not len(arr) == 15:
        raise ValueError('Illegal array length, must be 15')
    b = np.argmax(arr)
    a = b * (2 / 14) - 1
    return a


# # Testing the model

# +
#%% SET UP ENVIRONMENT
# Normal one 
#os.environ['DONKEY_SIM_PATH'] = "/home/zamy/masterthesis/DonkeySimLinux/donkey_sim.x86_64"
os.environ['DONKEY_SIM_PATH'] = "C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\test\\DonkeySimWin\\donkey_sim.exe"
os.environ['DONKEY_SIM_PORT'] = str(9091)
os.environ['DONKEY_SIM_HEADLESS'] = str(0) # "1" is headless

CAMERA_CONF = {'cam_config':{'img_w': '320',
                        'img_h': '200',
                        'img_d': '1',   # 3 for colored Tensor image
                        'img_enc': 'PNG', 
                        'fov': '90', 
                        'fish_eye_x': '0.0', 
                        'fish_eye_y': '0.0', 
                        'offset_x': '0.0', 
                        'offset_y': '0.0', 
                        'offset_z': '0.0', 
                        'rot_x': '0'}}
# Other tracks
#env = gym.make("donkey-generated-track-v0",conf=CAMERA_CONF)

# TRACKS TRACKS - TKarmer Tracks
#env = gym.make("donkey-generated-track-v0",conf=CAMERA_CONF) #,conf=config
# Random track, but the reward is kinda not working, IF THE REWARD WORKS IT IS KINDA RANDOM !!, this maybe debends on the environment

env = gym.make("donkey-generated-roads-v0", conf=CAMERA_CONF) #,conf=config
#env = gym.make("donkey-generated-track-v0", conf=CAMERA_CONF)

# Mini Monaco Track
#env = gym.make("donkey-minimonaco-track-v0",conf=CAMERA_CONF) #,conf=config
#env = gym.make("donkey-generated-track-v0") #,conf=config
#env = gym.wrappers.ResizeObservation(env,(200,320))

## DELETE LATER
#env.frameskip = 1
#gym.wrappers.max

# Create DQN Model
# Get size of state and action from environment
state_size = (VECTOR_SPACE_IMAGE_ROWS, VECTOR_SPACE_IMAGE_COLUMNS, VECTOR_SPACE_IMAGE_CHANNELS)
action_size = 15 # Steering and Throttle
throttle = 0.15 # Set the throttle as a constant value
agent = DQNAgent(state_size, action_size)

# loading the latest model
#agent.load_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\models\\model_episode_5000.h5')
#agent.load_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\models\\model_episode_10000.h5')
#agent.load_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\models\\model_episode_1400.h5')
#agent.load_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\models\\model_episode_5000.h5')
agent.load_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\1505model\\model_episode_4400.h5')

#first model that kinda works
#agent.load_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\old_model\\model_episode_5000.h5')

# Arrays for data collection / exploratory data analysis
average_cte = []
average_speed = []
measured_time = []
average_rewards = []
average_loss = []
collected_max_cte = []
collected_lap_time = []

# when a model and the data should be saved
save_state = 200

for episode in range(1,5):
    # Resetting the environment and preprocessing the first image
    observation = env.reset()
    # Graykonvertion of the observation image
    observation = cv.cvtColor(observation, cv.COLOR_BGR2GRAY)
    observation = vector_space.image_preprocessing(observation)
    # frame stacking (4 times)
    obv_stack = np.stack((observation, observation, observation, observation), axis= 2)
    # reshaping for keras
    obv_stack = obv_stack.reshape(1, obv_stack.shape[0], obv_stack.shape[1], obv_stack.shape[2])
    
    #action = np.array([0,0.10]) # drive straight with small speed
    #action = np.array([1,0.1]) # drive straight with small speed
    
    # summed up values for data collection
    # cumulative values for data collection
    total_cte = float(0)
    total_speed = float(0)
    total_reward = float(0)
    total_time = time.time()
    total_loss = float(0)
    max_cte = int(0)
    lap_time = int(0)
        
    # counting the amount of frames per episode
    frames = int(0)
    
    # boolean that describes if the env is done with this episode
    done = False
    while not done:
        # incrementing the amount of frames per episode
        frames += 1
        
        # Making a prediction for the current state
        # and getting the information form the next step
        steering = agent.get_action(obv_stack)
        action = [steering, throttle]
        next_observation, reward, done, info = env.step(action)
        # Graykonvertion of the observation image
        next_observation = cv.cvtColor(next_observation, cv.COLOR_BGR2GRAY)
        
        # printing current 'CTE'
        #print('CTE: %s' % info['cte'])
        
        # preprocessing the new observation
        next_observation_show = vector_space.image_preprocessing(next_observation)
        # reshaping for keras
        #next_observation = next_observation_show.reshape(1, next_observation_show.shape[0], next_observation_show.shape[1], 1)
        # appending to the observation stack
        #obv_stack = np.append(next_observation, obv_stack[:, :, :, :3], axis=3)
        
        obv_stack_t1 = np.stack([next_observation_show, obv_stack[0,:,:,0],obv_stack[0,:,:,1],obv_stack[0,:,:,2]], axis=2)
        obv_stack_t1 = obv_stack_t1.reshape(1, obv_stack_t1.shape[0], obv_stack_t1.shape[1], obv_stack_t1.shape[2])
                
        # saving the sample <s, a, r, s'> to the replay memory
        #agent.replay_memory(obv_stack, np.argmax(linear_bin(steering)), reward, obv_stack_t1, done)
        
        current_cte = abs(info['cte'])
        # punish and reward for a low or high 'CTE' with the logarithmic function
        if current_cte < .25:
            reward += abs(np.log(.10)) * 20
        elif current_cte >= .25 and current_cte < .5:
            reward += current_cte * 10
        elif current_cte >= .5 and current_cte < 1: 
            reward += abs(np.log(current_cte))
        elif current_cte >= 1 and current_cte < 2: 
            reward -= abs(current_cte) * 10
        elif current_cte >= 2 and current_cte < 3: 
            reward -= abs(current_cte) * 20
        elif current_cte >= 3 and current_cte < 4: 
            reward -= abs(current_cte) * 30
        elif current_cte >=4:
            reward -= abs(current_cte) * 40
                    
        # if the car leaves the track punish it
        if done:
            reward -= 250
        
        # training the DDQN, if training is enabled
        #if agent.train:
        #    loss = agent.train_replay()
        #    if loss != None:
        #        total_loss += loss
        #        #print('loss: %s ' % (total_loss / frames))
        
        # overwriting the stack and incrementing the time/frame counter
        obv_stack = obv_stack_t1
        agent.t += 1       
        
        # adding up the collected data
        current_cte = info['cte'] 
        total_cte += current_cte
        total_speed += info['speed']
        total_reward += reward
        # update if there is any change in incrementation 
        if max_cte < current_cte:
            max_cte = current_cte
        if lap_time < info['last_lap_time']:
            lap_time = info['last_lap_time']
            
        cv.imshow('vec img', next_observation_show)

        # Graykonvertion of the observation image
        #observation = cv.cvtColor(observation, cv.COLOR_BGR2GRAY)
        
        # Image preprocessing
        #vec_image = vector_space.image_preprocessing(observation)
        
        #print('REWARD: %s' % reward)
        
        #cv.imshow('vec img', vec_image)
        
        #break       # TODO: DELTE LATER
        if cv.waitKey(25) & 0xFF == ord('q'):
            cv.destroyAllWindows()
            break
    
    # Training after each episode
    #if agent.train:    
    #    total_loss = agent.train_replay()
    #    #if loss != None:
    #    #    total_loss = loss
    
    
    # AFTER this Episode & the environment returns True for the done variable
    # updating the target DDQN
    #agent.update_target_model()
    
    # calculating the average loss in this episode
    #episode_loss = total_loss.copy() #/ frames
    episode_cte = total_cte / frames
    episode_reward = total_reward / frames
    
    # updating the time parameter
    total_time = abs(total_time - time.time())
    
    # appending the collected data
    average_cte.append(episode_cte)
    average_speed.append(total_speed / frames)
    measured_time.append(total_time)
    average_rewards.append(episode_reward)
    average_loss.append(total_loss)
    collected_max_cte.append(max_cte)
    collected_lap_time.append(lap_time) 
    
    # Testing of saving the plot
    #if episode % 3 == 0 and episode != 0:
    #    save_plot(colleted_rewards, 'Reward', episode)
    #    cv.destroyAllWindows()
    #    break
    
    # Print episode information    
    print('EPISODE: %s | TIME: %s s | REWARD: %s | FRAMES: %s | QMAX: %s | EPSILON: %s | CTE: %s | LOSS: %s' % (episode, total_time, episode_reward, frames, str(agent.max_Q), agent.epsilon, episode_cte, total_loss))    
    
    if episode % save_state == 0 and episode != 0:
        print('saving after %s episodes' % save_state)
        # saving the model
        agent.save_model('C:\\Users\\studwilksa2535\\Desktop\\DonkeyCarAI\\models\\model_episode_%s.h5' % episode)        
        
        # Saving the data as plots
        save_plot(average_rewards, 'Reward', episode)
        save_plot(average_cte, 'Average cte', episode)
        save_plot(collected_max_cte, 'Max cte', episode)
        save_plot(measured_time, 'Time', episode)
        save_plot(average_loss, 'Loss', episode)
        save_plot(collected_lap_time, 'Lap Time', episode)
        
        # Saving the collected data
        save_data_as_dataframe(episode, average_rewards, average_loss, measured_time, collected_max_cte, average_cte, collected_lap_time)
    
    # closing all cv windows
    cv.destroyAllWindows()

# closing all cv windows
cv.destroyAllWindows()
# Close the enviroment
env.close()

StarcoderdataJupyterScriptsDedupFiltered