import os
import csv
import cv2
import matplotlib.pyplot as plt
import random
import pprint

import numpy as np
from numpy import expand_dims

%tensorflow_version 1.x
import tensorflow as tf
tf.logging.set_verbosity(tf.logging.ERROR)

from keras import backend as K
from keras.models import Model, Sequential
from keras.models import load_model
from keras.layers import Dense, GlobalAveragePooling2D, MaxPooling2D, Lambda, Cropping2D
from keras.layers.convolutional import Convolution2D
from keras.layers.core import Flatten, Dense, Dropout, SpatialDropout2D
from keras.optimizers import Adam
from keras.callbacks import ModelCheckpoint, TensorBoard
from keras.callbacks import EarlyStopping, ReduceLROnPlateau
from keras.preprocessing.image import ImageDataGenerator
from keras.preprocessing.image import load_img
from keras.preprocessing.image import img_to_array 
   
from google.colab.patches import cv2_imshow
  
import sklearn
from sklearn.model_selection import train_test_split
import pandas as pd

print("Tensorflow Version:",tf.__version__)
print("Tensorflow Keras Version:",tf.keras.__version__)
print("Eager mode: ", tf.executing_eagerly())


"""
Explanation: <a href="https://colab.research.google.com/github/wilselby/diy_driverless_car_ROS/blob/ml-model/rover_ml/colab/RC_Car_End_to_End_Image_Regression_with_CNNs_(Lidar).ipynb" target="_parent"><img src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
Development of an End-to-End ML Model for Navigating an RC car with a Lidar
<table class="tfo-notebook-buttons" align="left">
  <td>
    <a target="_blank" href="https://colab.research.google.com/github/wilselby/diy_driverless_car_ROS/blob/ml-model/RC_Car_End_to_End_Image_Regression_with_CNNs_(RGB_camera).ipynb">
    <img src="https://www.tensorflow.org/images/colab_logo_32px.png" />
    Run in Google Colab</a>
  </td>
  <td>
    <a target="_blank" href="https://github.com/wilselby/diy_driverless_car_ROS/blob/ml-model/rover_ml/colab/RC_Car_End_to_End_Image_Regression_with_CNNs_(RGB_camera).ipynb">
    <img src="https://www.tensorflow.org/images/GitHub-Mark-32px.png" />
    View source on GitHub</a>
  </td>
</table>

Environment Setup
Import Dependencies
End of explanation
"""


device_name = tf.test.gpu_device_name()

if device_name != '/device:GPU:0':
  # Raise SystemError('GPU device not found')
  print('GPU device not found')
else:
  print('Found GPU at: {}'.format(device_name))
  
  # GPU count and name
  !nvidia-smi -L

"""
Explanation: Confirm TensorFlow can see the GPU
Simply select "GPU" in the Accelerator drop-down in Notebook Settings (either through the Edit menu or the command palette at cmd/ctrl-shift-P).
End of explanation
"""


# Download the dataset
!curl -O https://selbystorage.s3-us-west-2.amazonaws.com/research/office_3/office_3.tar.gz

data_set = 'office_3'
tar_file = data_set + '.tar.gz'

# Unzip the .tgz file
# -x for extract
# -v for verbose 
# -z for gnuzip
# -f for file (should come at last just before file name)
# -C to extract the zipped contents to a different directory
!tar -xvzf $tar_file

"""
Explanation: Load the Dataset
Download and Extract the Dataset
End of explanation
"""


# Define path to csv file
csv_path = data_set + '/interpolated.csv'

# Load the CSV file into a pandas dataframe
df = pd.read_csv(csv_path, sep=",")

# Print the dimensions
print("Dataset Dimensions:")
print(df.shape)

# Print the first 5 lines of the dataframe for review
print("\nDataset Summary:")
df.head(5)
  

"""
Explanation: Parse the CSV File
End of explanation
"""


# Remove 'index' and 'frame_id' columns 
df.drop(['index','frame_id'],axis=1,inplace=True)

# Verify new dataframe dimensions
print("Dataset Dimensions:")
print(df.shape)

# Print the first 5 lines of the new dataframe for review
print("\nDataset Summary:")
df.head(5)

"""
Explanation: Clean and Pre-process the Dataset
Remove Unneccessary Columns
End of explanation
"""


# Detect Missing Values
print("Any Missing Values?: {}".format(df.isnull().values.any()))

# Total Sum
print("\nTotal Number of Missing Values: {}".format(df.isnull().sum().sum()))

# Sum Per Column
print("\nTotal Number of Missing Values per Column:")
print(df.isnull().sum())

"""
Explanation: Detect Missing Data
End of explanation
"""


# Determine if any throttle values are zeroes
print("Any 0 throttle values?: {}".format(df['speed'].eq(0).any()))

# Determine number of 0 throttle values:
print("\nNumber of 0 throttle values: {}".format(df['speed'].eq(0).sum()))

# Remove rows with 0 throttle values
if df['speed'].eq(0).any():
  df = df.query('speed != 0')
  
  # Reset the index
  df.reset_index(inplace=True,drop=True)
  
# Verify new dataframe dimensions
print("\nNew Dataset Dimensions:")
print(df.shape)
df.head(5)

"""
Explanation: Remove Zero Throttle Values
End of explanation
"""


# Steering Command Statistics
print("\nSteering Command Statistics:")
print(df['angle'].describe())

print("\nThrottle Command Statistics:")
# Throttle Command Statistics
print(df['speed'].describe())

"""
Explanation: View Label Statistics
End of explanation
"""


#@title Select the number of histogram bins

num_bins = 25 #@param {type:"slider", min:5, max:50, step:1}

hist, bins = np.histogram(df['angle'], num_bins)
center = (bins[:-1]+ bins[1:]) * 0.5
plt.bar(center, hist, width=0.05)
#plt.plot((np.min(df['angle']), np.max(df['angle'])), (samples_per_bin, samples_per_bin))

# Normalize the histogram (150-300 for RBG)
#@title Normalize the Histogram { run: "auto" }
hist = False #@param {type:"boolean"}

remove_list = []
samples_per_bin = 150

if hist:
  for j in range(num_bins):
    list_ = []
    for i in range(len(df['angle'])):
      if df.loc[i,'angle'] >= bins[j] and df.loc[i,'angle'] <= bins[j+1]:
        list_.append(i)
    random.shuffle(list_)
    list_ = list_[samples_per_bin:]
    remove_list.extend(list_)

  print('removed:', len(remove_list))
  df.drop(df.index[remove_list], inplace=True)
  df.reset_index(inplace=True)
  df.drop(['index'],axis=1,inplace=True)
  print('remaining:', len(df))
  
  hist, _ = np.histogram(df['angle'], (num_bins))
  plt.bar(center, hist, width=0.05)
  plt.plot((np.min(df['angle']), np.max(df['angle'])), (samples_per_bin, samples_per_bin))

"""
Explanation: View Histogram of Steering Commands
End of explanation
"""


# View a Single Image 
index = random.randint(0,df.shape[0]-1)

img_name = data_set + '/' + df.loc[index,'filename']
angle = df.loc[index,'angle']

center_image = cv2.imread(img_name)
center_image_mod = cv2.resize(center_image, (320,180))
center_image_mod = cv2.cvtColor(center_image_mod,cv2.COLOR_RGB2BGR)

# Crop the image
height_min = 100 
height_max = 170
width_min = 120
width_max = 200
bottom_crop = center_image_mod.shape[0] - height_max
right_crop = center_image_mod.shape[1] - width_max

crop_img = center_image_mod[height_min:height_max, width_min:width_max]

plt.subplot(2,1,1)
plt.imshow(center_image_mod)
plt.grid(False)
plt.xlabel('angle: {:.2}'.format(angle))
plt.show() 

plt.subplot(2,1,2)
plt.imshow(crop_img)
plt.grid(False)
plt.xlabel('angle: {:.2}'.format(angle))
plt.show() 

"""
Explanation: View a Sample Image
End of explanation
"""


# Number of Images to Display
num_images = 4

# Display the images
i = 0
for i in range (i,num_images):
    index = random.randint(0,df.shape[0]-1)
    image_path = df.loc[index,'filename']
    angle = df.loc[index,'angle']
    img_name = data_set + '/' + image_path
    image = cv2.imread(img_name)
    image = cv2.resize(image, (320,180))
    image = cv2.cvtColor(image,cv2.COLOR_RGB2BGR)
    plt.subplot(num_images/2,num_images/2,i+1)
    plt.xticks([])
    plt.yticks([])
    plt.grid(False)
    plt.imshow(image, cmap=plt.cm.binary)
    plt.xlabel('angle: {:.3}'.format(angle))
    i += 1

"""
Explanation: View Multiple Images
End of explanation
"""


# Create image data augmentation generator and choose augmentation types
datagen = ImageDataGenerator(
                             #rotation_range=20,
                             zoom_range=0.15,
                             #width_shift_range=0.1,
                             #height_shift_range=0.2,
                             #shear_range=10,
                             brightness_range=[0.5,1.0],
                          	 #horizontal_flip=True,
                             #vertical_flip=True,
                             #channel_shift_range=100.0,
                             fill_mode="reflect")

"""
Explanation: Split the Dataset
Define an ImageDataGenerator to Augment Images
End of explanation
"""


# load the image
index = random.randint(0,df.shape[0]-1)

img_name = data_set + '/' + df.loc[index,'filename']
original_image = cv2.imread(img_name)
original_image = cv2.cvtColor(original_image,cv2.COLOR_RGB2BGR)
original_image = cv2.resize(original_image, (320,180))
label = df.loc[index,'angle']

# convert to numpy array
data = img_to_array(original_image)

# expand dimension to one sample
test = expand_dims(data, 0)

# prepare iterator
it = datagen.flow(test, batch_size=1)

# generate batch of images
batch = it.next()

# convert to unsigned integers for viewing
image_aug = batch[0].astype('uint8')

print("Augmenting a Single Image: \n")

plt.subplot(2,1,1)
plt.imshow(original_image)
plt.grid(False)
plt.xlabel('angle: {:.2}'.format(label))
plt.show() 

plt.subplot(2,1,2)
plt.imshow(image_aug)
plt.grid(False)
plt.xlabel('angle: {:.2}'.format(label))
plt.show() 

print("Multiple Augmentations: \n")
# generate samples and plot
for i in range(0,num_images):
	# define subplot
	plt.subplot(num_images/2,num_images/2,i+1)
	# generate batch of images
	batch = it.next()
	# convert to unsigned integers for viewing
	image = batch[0].astype('uint8')
	# plot raw pixel data
	plt.imshow(image)
# show the figure
plt.show()


"""
Explanation: View Image Augmentation Examples
End of explanation
"""


def generator(samples, batch_size=32, aug=0):
    num_samples = len(samples)

    while 1:  # Loop forever so the generator never terminates
        for offset in range(0, num_samples, batch_size):
            batch_samples = samples[offset:offset + batch_size]

            #print(batch_samples)
            images = []
            angles = []
            for batch_sample in batch_samples:
                if batch_sample[5] != "filename":
                    name = data_set + '/' + batch_sample[3]
                    center_image = cv2.imread(name)
                    center_image = cv2.cvtColor(center_image,cv2.COLOR_RGB2BGR)
                    center_image = cv2.resize(
                        center_image,
                        (320, 180))  #resize from 720x1280 to 180x320
                    angle = float(batch_sample[4])
                    if not aug:
                      images.append(center_image)
                      angles.append(angle)
                    else:
                        data = img_to_array(center_image)
                        sample = expand_dims(data, 0)
                        it = datagen.flow(sample, batch_size=1)
                        batch = it.next()
                        image_aug = batch[0].astype('uint8')
                        if random.random() < .5:
                          image_aug = np.fliplr(image_aug)
                          angle = -1 * angle
                        images.append(image_aug)
                        angles.append(angle)

            X_train = np.array(images)
            y_train = np.array(angles)

            yield sklearn.utils.shuffle(X_train, y_train)

"""
Explanation: Define a Data Generator
End of explanation
"""


samples = []

samples = df.values.tolist()

sklearn.utils.shuffle(samples)
train_samples, validation_samples = train_test_split(samples, test_size=0.2)

print("Number of traing samples: ", len(train_samples))
print("Number of validation samples: ", len(validation_samples))

"""
Explanation: Split the Dataset
End of explanation
"""


batch_size_value = 32
img_aug = 1

train_generator = generator(train_samples, batch_size=batch_size_value, aug=img_aug)
validation_generator = generator(
    validation_samples, batch_size=batch_size_value, aug=0)

"""
Explanation: Define Training and Validation Data Generators
End of explanation
"""


# Initialize the model
model = Sequential()

# trim image to only see section with road
# (top_crop, bottom_crop), (left_crop, right_crop)
model.add(Cropping2D(cropping=((height_min,bottom_crop), (width_min,right_crop)), input_shape=(180,320,3)))

# Preprocess incoming data, centered around zero with small standard deviation
model.add(Lambda(lambda x: (x / 255.0) - 0.5))

# Nvidia model
model.add(Convolution2D(24, (5, 5), activation="relu", name="conv_1", strides=(2, 2)))
model.add(Convolution2D(36, (5, 5), activation="relu", name="conv_2", strides=(2, 2)))
model.add(Convolution2D(48, (5, 5), activation="relu", name="conv_3", strides=(2, 2)))
model.add(SpatialDropout2D(.5, dim_ordering='default'))

model.add(Convolution2D(64, (3, 3), activation="relu", name="conv_4", strides=(1, 1)))
model.add(Convolution2D(64, (3, 3), activation="relu", name="conv_5", strides=(1, 1)))

model.add(Flatten())

model.add(Dense(1164))
model.add(Dropout(.5))
model.add(Dense(100, activation='relu'))
model.add(Dropout(.5))
model.add(Dense(50, activation='relu'))
model.add(Dropout(.5))
model.add(Dense(10, activation='relu'))
model.add(Dropout(.5))
model.add(Dense(1))

model.compile(loss='mse', optimizer=Adam(lr=0.001), metrics=['mse','mae','mape','cosine'])

# Print model sumamry
model.summary()

"""
Explanation: Compile and Train the Model
Build the Model
End of explanation
"""


# checkpoint
model_path = './model'

!if [ -d $model_path ]; then echo 'Directory Exists'; else mkdir $model_path; fi

filepath = model_path + "/weights-improvement-{epoch:02d}-{val_loss:.2f}.hdf5"
checkpoint = ModelCheckpoint(filepath, monitor='val_loss', verbose=1, save_best_only=True, mode='auto', period=1)

"""
Explanation: Setup Checkpoints
End of explanation
"""


# The patience parameter is the amount of epochs to check for improvement
early_stop = EarlyStopping(monitor='val_loss', patience=10)

"""
Explanation: Setup Early Stopping to Prevent Overfitting
End of explanation
"""


reduce_lr = ReduceLROnPlateau(monitor='val_loss', factor=0.2,
                              patience=5, min_lr=0.001)

"""
Explanation: Reduce Learning Rate When a Metric has Stopped Improving
End of explanation
"""


# Clear any logs from previous runs
!rm -rf ./Graph/ 

# Launch Tensorboard
!pip install -U tensorboardcolab

from tensorboardcolab import *

tbc = TensorBoardColab()

# Configure the Tensorboard Callback
tbCallBack = TensorBoard(log_dir='./Graph', 
                        histogram_freq=1,
                        write_graph=True,
                        write_grads=True,
                        write_images=True,
                        batch_size=batch_size_value,
                        update_freq='epoch')


"""
Explanation: Setup Tensorboard
End of explanation
"""


load = True #@param {type:"boolean"}

if load:
  # Returns a compiled model identical to the previous one
  !curl -O https://selbystorage.s3-us-west-2.amazonaws.com/research/office_3/model_intensity.h5
  !mv model_intensity.h5 model/
  model_path_full = model_path + '/' + 'model_intensity.h5'
  model = load_model(model_path_full)
  print("Loaded previous model: {} \n".format(model_path_full))
else:
  print("No previous model loaded \n")

"""
Explanation: Load Existing Model
End of explanation
"""


# Define step sizes
STEP_SIZE_TRAIN = len(train_samples) / batch_size_value
STEP_SIZE_VALID = len(validation_samples) / batch_size_value

# Define number of epochs
n_epoch = 50

# Define callbacks
# callbacks_list = [TensorBoardColabCallback(tbc)]
# callbacks_list = [TensorBoardColabCallback(tbc), early_stop]
# callbacks_list = [TensorBoardColabCallback(tbc), early_stop, checkpoint]
callbacks_list = [TensorBoardColabCallback(tbc), early_stop, checkpoint, reduce_lr]

# Fit the model
history_object = model.fit_generator(
    generator=train_generator,
    steps_per_epoch=STEP_SIZE_TRAIN,
    validation_data=validation_generator,
    validation_steps=STEP_SIZE_VALID,
    callbacks=callbacks_list,
    use_multiprocessing=True,
    epochs=n_epoch)


"""
Explanation: Train the Model
End of explanation
"""


# Save model
model_path_full = model_path + '/'

model.save(model_path_full + 'model.h5')
with open(model_path_full + 'model.json', 'w') as output_json:
    output_json.write(model.to_json())

"""
Explanation: Save the Model
End of explanation
"""


# Plot the training and validation loss for each epoch
print('Generating loss chart...')
plt.plot(history_object.history['loss'])
plt.plot(history_object.history['val_loss'])
plt.title('model mean squared error loss')
plt.ylabel('mean squared error loss')
plt.xlabel('epoch')
plt.legend(['training set', 'validation set'], loc='upper right')
plt.savefig(model_path + '/model.png')

# Done
print('Done.')

"""
Explanation: Evaluate the Model
Plot the Training Results
End of explanation
"""


scores = model.evaluate_generator(validation_generator, STEP_SIZE_VALID, use_multiprocessing=True)

metrics_names = model.metrics_names

for i in range(len(model.metrics_names)):
  print("Metric: {} - {}".format(metrics_names[i],scores[i]))


"""
Explanation: Print Performance Metrics
End of explanation
"""


# Define image loading function
def load_images(dataframe):
  
  # initialize images array
  images = []
  
  for i in dataframe.index.values:
    name = data_set + '/' + dataframe.loc[i,'filename']
    center_image = cv2.imread(name)
    center_image = cv2.resize(center_image, (320,180))
    images.append(center_image)
    
  return np.array(images)
  
# Load images 
test_size = 200
df_test = df.sample(frac=1).reset_index(drop=True)
df_test = df_test.head(test_size)

test_images = load_images(df_test)

batch_size = 32
preds = model.predict(test_images, batch_size=batch_size, verbose=1)

#print("Preds: {} \n".format(preds))

testY = df_test.iloc[:,4].values

#print("Labels: {} \n".format(testY))

df_testY = pd.Series(testY)
df_preds = pd.Series(preds.flatten)

# Replace 0 angle values
if df_testY.eq(0).any():
  df_testY.replace(0, 0.0001,inplace=True)

# Calculate the difference
diff = preds.flatten() - df_testY
percentDiff = (diff / testY) * 100
absPercentDiff = np.abs(percentDiff)

# compute the mean and standard deviation of the absolute percentage
# difference
mean = np.mean(absPercentDiff)
std = np.std(absPercentDiff)
print("[INFO] mean: {:.2f}%, std: {:.2f}%".format(mean, std))

# Compute the mean and standard deviation of the difference
print(diff.describe())

# Plot a histogram of the prediction errors
num_bins = 25
hist, bins = np.histogram(diff, num_bins)
center = (bins[:-1]+ bins[1:]) * 0.5
plt.bar(center, hist, width=0.05)
plt.title('Historgram of Predicted Error')
plt.xlabel('Steering Angle')
plt.ylabel('Number of predictions')
plt.xlim(-2.0, 2.0)
plt.plot(np.min(diff), np.max(diff))

# Plot a Scatter Plot of the Error
plt.scatter(testY, preds)
plt.xlabel('True Values ')
plt.ylabel('Predictions ')
plt.axis('equal')
plt.axis('square')
plt.xlim([-1.75,1.75])
plt.ylim([-1.75,1.75])
plt.plot([-1.75, 1.75], [-1.75, 1.75], color='k', linestyle='-', linewidth=.1)

"""
Explanation: Compute Prediction Statistics
End of explanation
"""


# Plot the image with the actual and predicted steering angle
index = random.randint(0,df_test.shape[0]-1)
img_name = data_set + '/' + df_test.loc[index,'filename']
center_image = cv2.imread(img_name)
center_image = cv2.cvtColor(center_image,cv2.COLOR_RGB2BGR)
center_image_mod = cv2.resize(center_image, (320,180)) 
plt.imshow(center_image_mod)
plt.grid(False)
plt.xlabel('Actual: {:.2f} Predicted: {:.2f}'.format(df_test.loc[index,'angle'],float(preds[index])))
plt.show() 


"""
Explanation: Plot a Prediction
End of explanation
"""


model.summary()

"""
Explanation: Visualize the Network
Show the Model Summary
End of explanation
"""


# Creating a mapping of layer name ot layer details 
# We will create a dictionary layers_info which maps a layer name to its charcteristics
layers_info = {}
for i in model.layers:
    layers_info[i.name] = i.get_config()

# Here the layer_weights dictionary will map every layer_name to its corresponding weights
layer_weights = {}
for i in model.layers:
    layer_weights[i.name] = i.get_weights()

pprint.pprint(layers_info['conv_5'])

"""
Explanation: Access Individual Layers
End of explanation
"""


# Visualize the first filter of each convolution layer
layers = model.layers
layer_ids = [2,3,4,6,7]

#plot the filters
fig,ax = plt.subplots(nrows=1,ncols=5)
for i in range(5):
    ax[i].imshow(layers[layer_ids[i]].get_weights()[0][:,:,:,0][:,:,0],cmap='gray')
    ax[i].set_title('Conv'+str(i+1))
    ax[i].set_xticks([])
    ax[i].set_yticks([])

"""
Explanation: Visualize the Filters
End of explanation
"""


!pip install -I scipy==1.2.*
!pip install git+https://github.com/raghakot/keras-vis.git -U

# import specific functions from keras-vis package
from vis.utils import utils
from vis.visualization import visualize_saliency, visualize_cam, overlay

# View a Single Image 
index = random.randint(0,df.shape[0]-1)
img_name = data_set + '/' + df.loc[index,'filename']

sample_image = cv2.imread(img_name)
sample_image = cv2.cvtColor(sample_image,cv2.COLOR_RGB2BGR)
sample_image_mod = cv2.resize(sample_image, (320,180))
plt.imshow(sample_image_mod)
 
layer_idx = utils.find_layer_idx(model, 'conv_5')

grads = visualize_saliency(model, 
                           layer_idx, 
                           filter_indices=None, 
                           seed_input=sample_image_mod,
                           grad_modifier='absolute',
                           backprop_modifier='guided')

plt.imshow(grads, alpha = 0.6)

"""
Explanation: Visualize the Saliency Map
End of explanation
"""


