# Code modified from https://github.com/seungeunrho/minimalRL/blob/master/a3c.py
import gym
import torch
import torch.nn as nn
import torch.nn.functional as F
import torch.optim as optim
from torch.distributions import Categorical
import torch.multiprocessing as mp
import time, sys
import gym_multi_car_racing
from gym_multi_car_racing import MultiCarRacing
import numpy as np
sys.path.insert(0, '../../benchmarking')
from model_tester import TesterAgent


# Hyperparameters
n_train_processes = 6
learning_rate = 0.0001
beta = 0.01
update_interval = 5
gamma = 0.99
max_train_ep = 700
max_test_ep = 800
num_frames = 5
max_temp = 5.0
SCALE = 10.0  # Track scale
PLAYFIELD = 2000 / SCALE  # Game over boundary
ACTIONS = [
        (-1, 1, 0.2), (0, 1, 0.2), (1, 1, 0.2),  # Action Space Structure
        (-1, 1, 0), (0, 1, 0), (1, 1, 0),  # (Steering Wheel, Gas, Break)
        (-1, 0, 0.2), (0, 0, 0.2), (1, 0, 0.2),  # Range        -1~1       0~1   0~1
        (-1, 0, 0), (0, 0, 0), (1, 0, 0)
        ]
VELOCITY_REWARD_PROPORTIONAL = 0.0
VELOCITY_REWARD_LOW = -10.0
ANGLE_DIFF_REWARD = 0.0
ON_GRASS_REWARD = -1.0
BACKWARD_REWARD = 0.0

def to_grayscale(img):
    return np.dot(img, [0.299, 0.587, 0.144])


def zero_center(img):
    return (img - 127.0) / 256.0


def crop(img, bottom=12, left=6, right=6):
    height, width = img.shape
    return img[0: height - bottom, left: width - right]


class ActorCritic(nn.Module):
    def __init__(self, num_of_inputs, num_of_actions):
        super().__init__()

        self.conv1 = nn.Conv2d(num_of_inputs, 16, 8, stride=4)
        self.conv2 = nn.Conv2d(16, 32, 3, stride=2)
        self.linear1 = nn.Linear(32*9*9, 256)
        self.policy = nn.Linear(256, num_of_actions)
        self.value = nn.Linear(256, 1)
        self.num_of_actions = num_of_actions

    def forward(self, x, t=1.0):
        conv1_out = F.relu(self.conv1(x))
        conv2_out = F.relu(self.conv2(conv1_out))

        flattened = torch.flatten(conv2_out, start_dim=1)  # N x 9*9*32
        linear1_out = self.linear1(flattened)

        policy_output = self.policy(linear1_out)
        value_output = self.value(linear1_out).squeeze(-1)

        probs = F.softmax(policy_output / t, dim=-1)
        log_probs = F.log_softmax(policy_output, dim=-1)
        return probs.float(), log_probs.float(), value_output.float()


def preprocess_state(s, car_id):
    s = s[car_id, ...]
    s = to_grayscale(s)
    s = zero_center(s)
    s = crop(s)
    return torch.from_numpy(s.astype(np.float32)).unsqueeze(0).unsqueeze(0)  # N, C, H, W tensor


def add_frame(s_new, s_stack):
    s_stack = torch.roll(s_stack, shifts=-1, dims=1)
    s_stack[:, -1, :, :] = s_new
    return s_stack

def action_space_setup(env):
    env.cont_action_space = ACTIONS
    env.action_space = gym.spaces.Discrete(len(env.cont_action_space))

def get_reward(env, action):
    step_reward = np.zeros(env.num_agents)
    step_reward += env.track_reward
    for car_id, car in enumerate(env.cars):  # First step without action, called from reset()

        velocity = abs(env.all_feats[car_id, 47])
        step_reward[car_id] += (VELOCITY_REWARD_LOW if velocity < 2.0 else velocity * VELOCITY_REWARD_PROPORTIONAL)  # normalize the velocity later
        step_reward[car_id] += abs(env.all_feats[car_id, 3]) * ANGLE_DIFF_REWARD  # normalize angle diff later

        step_reward[car_id] += float(env.all_feats[car_id, 4]) * ON_GRASS_REWARD  # driving on grass
        step_reward[car_id] += float(env.all_feats[car_id, 5]) * BACKWARD_REWARD  # driving backward
    return step_reward

def episode_end(env, car_id):
    done = False
    x, y = env.cars[car_id].hull.position
    if (abs(x) > PLAYFIELD or abs(y) > PLAYFIELD) or (env.driving_backward[car_id]):
        done = True
    return done

def train(global_model, rank):
    car_id = 0  # Change this for multi-agent caseß
    local_model = ActorCritic(num_frames, global_model.num_of_actions)
    local_model.load_state_dict(global_model.state_dict())

    optimizer = optim.Adam(global_model.parameters(), lr=learning_rate)

    env = gym.make("MultiCarRacing-v0",
                   num_agents=1,
                   direction='CCW',
                   use_random_direction=True,
                   backwards_flag=True,
                   h_ratio=0.25,
                   use_ego_color=False,
                   setup_action_space_func=action_space_setup,
                   get_reward_func=get_reward,
                   episode_end_func=episode_end,
                   observation_type='frames')

    for n_epi in range(max_train_ep):
        done = False

        # Initialize state with repeated first frame
        s = env.reset()
        s = preprocess_state(s, car_id)
        s = s.repeat(1, num_frames, 1, 1)
        temperature = np.exp(-(n_epi - max_train_ep) / max_train_ep * np.log(max_temp))
        #print('Temperature: ', temperature)
        while not done:
            s_lst, a_lst, r_lst = [], [], []
            for t in range(update_interval):
                prob = local_model(s, t=temperature)[0]
                m = Categorical(prob)
                a = m.sample().item()
                action_vec = np.array(ACTIONS[a])
                s_prime, r, done, _ = env.step(action_vec)
                r = r[0]
                r /= 100.
                s_prime = preprocess_state(s_prime, car_id)

                s_lst.append(s)
                a_lst.append([a])
                r_lst.append(r.astype(np.float32))

                s = add_frame(s_prime, s)
                if done:
                    break

            s_final = s
            R = 0.0 if done else local_model(s_final, t=temperature)[2].item()
            td_target_lst = []
            for reward in r_lst[::-1]:
                R = gamma * R + reward
                td_target_lst.append(R)
            td_target_lst.reverse()

            s_batch, a_batch, td_target = torch.cat(s_lst, dim=0), torch.tensor(a_lst).squeeze(-1), \
                torch.tensor(td_target_lst).squeeze(-1).float()
            advantage = td_target - local_model(s_batch, t=temperature)[2]

            pi = local_model(s_batch, t=temperature)[0]
            pi_a = pi.gather(1, a_batch.unsqueeze(-1)).squeeze(-1)
            entropy_loss = -(pi * torch.log(pi + 1e-20)).sum(dim=1).mean().float()
            policy_loss = (-torch.log(pi_a) * advantage.detach()).mean().float()
            value_loss = F.smooth_l1_loss(local_model(s_batch, t=temperature)[2], td_target.detach()).float()
            loss = 1.0 * policy_loss + \
                0.5 * value_loss + \
                -beta * entropy_loss
            #print('Probs:{}, E:{}'.format(pi.detach(), entropy_loss.detach()))

            optimizer.zero_grad()
            loss.backward()
            for global_param, local_param in zip(global_model.parameters(), local_model.parameters()):
                global_param._grad = local_param.grad
            optimizer.step()
            local_model.load_state_dict(global_model.state_dict())
        print('Rank {}, episode {}, loss {}'.format(rank, n_epi, loss.mean().detach()))
    env.close()
    print("Training process {} reached maximum episode.".format(rank))


def test(global_model):
    env = gym.make("MultiCarRacing-v0",
                   num_agents=1,
                   direction='CCW',
                   use_random_direction=True,
                   backwards_flag=True,
                   h_ratio=0.25,
                   use_ego_color=False,
                   setup_action_space_func=action_space_setup,
                   get_reward_func=get_reward,
                   episode_end_func=episode_end,
                   observation_type='frames',
                   )
    score = 0.0
    print_interval = 10
    car_id = 0
    best_score = -100000000.0
    for n_epi in range(max_test_ep):
        done = False
        s = env.reset()
        s = preprocess_state(s, car_id)
        s = s.repeat(1, num_frames, 1, 1)
        while not done:
            prob = global_model(s)[0]
            a = Categorical(prob).sample().item()
            action_vec = np.array(ACTIONS[a])
            s_prime, r, done, info = env.step(action_vec)
            r = r[0]
            s_prime = preprocess_state(s_prime, car_id)
            s = add_frame(s_prime, s)
            score += r
            env.render()
        print('Final score: ', info['total_score'])

        if n_epi % print_interval == 0 and n_epi != 0:
            avg_score = score / print_interval
            print("# of episode :{}, avg score : {}".format(n_epi, avg_score))
            if avg_score > best_score:
                best_score = avg_score
                print('Saving best model.')
                torch.save(global_model.state_dict(), '../a3c_agent.pth')
            score = 0.0
            time.sleep(1)
    env.close()


class A3CTesterAgent(TesterAgent):
    def __init__(self,
                 model_path='../saved_models/A3C/a3c_12actions_T_5_ent_0_01.pth',
                 car_id=0,
                 num_frames=5,
                 actions=ACTIONS,
                 **kwargs
                 ):
        super().__init__(**kwargs)
        self.car_id = car_id
        self.frame_buffer = None
        self.num_frames = num_frames
        self.actions = actions
        self.agent = self._load_model(model_path)

    def _load_model(self, model_path):
        agent = ActorCritic(self.num_frames, len(self.actions))
        agent.load_state_dict(torch.load(model_path))
        agent.eval()
        return agent

    def _update_frame_buffer(self, new_frame):
        if self.frame_buffer is None:
            self.frame_buffer = new_frame.repeat(1, self.num_frames, 1, 1)
        else:
            self.frame_buffer = add_frame(new_frame, self.frame_buffer)

    def state_to_action(self, s):
        """
        This function should take the most recent state and return the
        action vector in the exact same form it is needed for the environment.
        If you are using frame buffer see example in _update_frame_buffer
        how to take care of that.
        """
        s = preprocess_state(s, self.car_id)
        self._update_frame_buffer(s)
        prob = self.agent(self.frame_buffer)[0]
        a = Categorical(prob).sample().item()
        action_vec = np.array(self.actions[a])
        return action_vec

    @staticmethod
    def setup_action_space(env):
        """
        This should be the same action space setup function that you used for training.
        Make sure that the actions set here are the same as the ones used to train the model.
        """
        env.cont_action_space = ACTIONS
        env.action_space = gym.spaces.Discrete(len(env.cont_action_space))

    @staticmethod
    def get_observation_type():
        """
        Simply return 'frames' or 'features'
        """
        return 'frames'



if __name__ == '__main__':
    global_model = ActorCritic(num_frames, len(ACTIONS))
    global_model.share_memory()

    processes = []
    for rank in range(n_train_processes + 1):  # + 1 for test process
        if rank == 0:
            p = mp.Process(target=test, args=(global_model,))
        else:
            p = mp.Process(target=train, args=(global_model, rank,))
        p.start()
        processes.append(p)
    for p in processes:
        p.join()

# Code modified from https://github.com/seungeunrho/minimalRL/blob/master/a3c.py
import gym
import torch
import torch.nn as nn
import torch.nn.functional as F
import torch.optim as optim
from torch.distributions import Categorical
import torch.multiprocessing as mp
import time, sys
import gym_multi_car_racing
from gym_multi_car_racing import MultiCarRacing
import numpy as np
sys.path.insert(0, '../../benchmarking')
from model_tester import TesterAgent


# Hyperparameters
n_train_processes = 6
learning_rate = 0.0001
beta = 0.01
update_interval = 5
gamma = 0.99
max_train_ep = 700
max_test_ep = 800
num_frames = 5
max_temp = 5.0
SCALE = 10.0  # Track scale
PLAYFIELD = 2000 / SCALE  # Game over boundary
ACTIONS = [
        (-1, 1, 0.2), (0, 1, 0.2), (1, 1, 0.2),  # Action Space Structure
        (-1, 1, 0), (0, 1, 0), (1, 1, 0),  # (Steering Wheel, Gas, Break)
        (-1, 0, 0.2), (0, 0, 0.2), (1, 0, 0.2),  # Range        -1~1       0~1   0~1
        (-1, 0, 0), (0, 0, 0), (1, 0, 0)
        ]
VELOCITY_REWARD_PROPORTIONAL = 0.0
VELOCITY_REWARD_LOW = -10.0
ANGLE_DIFF_REWARD = 0.0
ON_GRASS_REWARD = -1.0
BACKWARD_REWARD = 0.0

def to_grayscale(img):
    return np.dot(img, [0.299, 0.587, 0.144])


def zero_center(img):
    return (img - 127.0) / 256.0


def crop(img, bottom=12, left=6, right=6):
    height, width = img.shape
    return img[0: height - bottom, left: width - right]


class ActorCritic(nn.Module):
    def __init__(self, num_of_inputs, num_of_actions):
        super().__init__()

        self.conv1 = nn.Conv2d(num_of_inputs, 16, 8, stride=4)
        self.conv2 = nn.Conv2d(16, 32, 3, stride=2)
        self.linear1 = nn.Linear(32*9*9, 256)
        self.policy = nn.Linear(256, num_of_actions)
        self.value = nn.Linear(256, 1)
        self.num_of_actions = num_of_actions

    def forward(self, x, t=1.0):
        conv1_out = F.relu(self.conv1(x))
        conv2_out = F.relu(self.conv2(conv1_out))

        flattened = torch.flatten(conv2_out, start_dim=1)  # N x 9*9*32
        linear1_out = self.linear1(flattened)

        policy_output = self.policy(linear1_out)
        value_output = self.value(linear1_out).squeeze(-1)

        probs = F.softmax(policy_output / t, dim=-1)
        log_probs = F.log_softmax(policy_output, dim=-1)
        return probs.float(), log_probs.float(), value_output.float()


def preprocess_state(s, car_id):
    s = s[car_id, ...]
    s = to_grayscale(s)
    s = zero_center(s)
    s = crop(s)
    return torch.from_numpy(s.astype(np.float32)).unsqueeze(0).unsqueeze(0)  # N, C, H, W tensor


def add_frame(s_new, s_stack):
    s_stack = torch.roll(s_stack, shifts=-1, dims=1)
    s_stack[:, -1, :, :] = s_new
    return s_stack

def action_space_setup(env):
    env.cont_action_space = ACTIONS
    env.action_space = gym.spaces.Discrete(len(env.cont_action_space))

def get_reward(env, action):
    step_reward = np.zeros(env.num_agents)
    step_reward += env.track_reward
    for car_id, car in enumerate(env.cars):  # First step without action, called from reset()

        velocity = abs(env.all_feats[car_id, 47])
        step_reward[car_id] += (VELOCITY_REWARD_LOW if velocity < 2.0 else velocity * VELOCITY_REWARD_PROPORTIONAL)  # normalize the velocity later
        step_reward[car_id] += abs(env.all_feats[car_id, 3]) * ANGLE_DIFF_REWARD  # normalize angle diff later

        step_reward[car_id] += float(env.all_feats[car_id, 4]) * ON_GRASS_REWARD  # driving on grass
        step_reward[car_id] += float(env.all_feats[car_id, 5]) * BACKWARD_REWARD  # driving backward
    return step_reward

def episode_end(env, car_id):
    done = False
    x, y = env.cars[car_id].hull.position
    if (abs(x) > PLAYFIELD or abs(y) > PLAYFIELD) or (env.driving_backward[car_id]):
        done = True
    return done

def train(global_model, rank):
    car_id = 0  # Change this for multi-agent caseß
    local_model = ActorCritic(num_frames, global_model.num_of_actions)
    local_model.load_state_dict(global_model.state_dict())

    optimizer = optim.Adam(global_model.parameters(), lr=learning_rate)

    env = gym.make("MultiCarRacing-v0",
                   num_agents=1,
                   direction='CCW',
                   use_random_direction=True,
                   backwards_flag=True,
                   h_ratio=0.25,
                   use_ego_color=False,
                   setup_action_space_func=action_space_setup,
                   get_reward_func=get_reward,
                   episode_end_func=episode_end,
                   observation_type='frames')

    for n_epi in range(max_train_ep):
        done = False

        # Initialize state with repeated first frame
        s = env.reset()
        s = preprocess_state(s, car_id)
        s = s.repeat(1, num_frames, 1, 1)
        temperature = np.exp(-(n_epi - max_train_ep) / max_train_ep * np.log(max_temp))
        #print('Temperature: ', temperature)
        while not done:
            s_lst, a_lst, r_lst = [], [], []
            for t in range(update_interval):
                prob = local_model(s, t=temperature)[0]
                m = Categorical(prob)
                a = m.sample().item()
                action_vec = np.array(ACTIONS[a])
                s_prime, r, done, _ = env.step(action_vec)
                r = r[0]
                r /= 100.
                s_prime = preprocess_state(s_prime, car_id)

                s_lst.append(s)
                a_lst.append([a])
                r_lst.append(r.astype(np.float32))

                s = add_frame(s_prime, s)
                if done:
                    break

            s_final = s
            R = 0.0 if done else local_model(s_final, t=temperature)[2].item()
            td_target_lst = []
            for reward in r_lst[::-1]:
                R = gamma * R + reward
                td_target_lst.append(R)
            td_target_lst.reverse()

            s_batch, a_batch, td_target = torch.cat(s_lst, dim=0), torch.tensor(a_lst).squeeze(-1), \
                torch.tensor(td_target_lst).squeeze(-1).float()
            advantage = td_target - local_model(s_batch, t=temperature)[2]

            pi = local_model(s_batch, t=temperature)[0]
            pi_a = pi.gather(1, a_batch.unsqueeze(-1)).squeeze(-1)
            entropy_loss = -(pi * torch.log(pi + 1e-20)).sum(dim=1).mean().float()
            policy_loss = (-torch.log(pi_a) * advantage.detach()).mean().float()
            value_loss = F.smooth_l1_loss(local_model(s_batch, t=temperature)[2], td_target.detach()).float()
            loss = 1.0 * policy_loss + \
                0.5 * value_loss + \
                -beta * entropy_loss
            #print('Probs:{}, E:{}'.format(pi.detach(), entropy_loss.detach()))

            optimizer.zero_grad()
            loss.backward()
            for global_param, local_param in zip(global_model.parameters(), local_model.parameters()):
                global_param._grad = local_param.grad
            optimizer.step()
            local_model.load_state_dict(global_model.state_dict())
        print('Rank {}, episode {}, loss {}'.format(rank, n_epi, loss.mean().detach()))
    env.close()
    print("Training process {} reached maximum episode.".format(rank))


def test(global_model):
    env = gym.make("MultiCarRacing-v0",
                   num_agents=1,
                   direction='CCW',
                   use_random_direction=True,
                   backwards_flag=True,
                   h_ratio=0.25,
                   use_ego_color=False,
                   setup_action_space_func=action_space_setup,
                   get_reward_func=get_reward,
                   episode_end_func=episode_end,
                   observation_type='frames',
                   )
    score = 0.0
    print_interval = 10
    car_id = 0
    best_score = -100000000.0
    for n_epi in range(max_test_ep):
        done = False
        s = env.reset()
        s = preprocess_state(s, car_id)
        s = s.repeat(1, num_frames, 1, 1)
        while not done:
            prob = global_model(s)[0]
            a = Categorical(prob).sample().item()
            action_vec = np.array(ACTIONS[a])
            s_prime, r, done, info = env.step(action_vec)
            r = r[0]
            s_prime = preprocess_state(s_prime, car_id)
            s = add_frame(s_prime, s)
            score += r
            env.render()
        print('Final score: ', info['total_score'])

        if n_epi % print_interval == 0 and n_epi != 0:
            avg_score = score / print_interval
            print("# of episode :{}, avg score : {}".format(n_epi, avg_score))
            if avg_score > best_score:
                best_score = avg_score
                print('Saving best model.')
                torch.save(global_model.state_dict(), '../a3c_agent.pth')
            score = 0.0
            time.sleep(1)
    env.close()


class A3CTesterAgent(TesterAgent):
    def __init__(self,
                 model_path='../saved_models/A3C/a3c_12actions_T_5_ent_0_01.pth',
                 car_id=0,
                 num_frames=5,
                 actions=ACTIONS,
                 **kwargs
                 ):
        super().__init__(**kwargs)
        self.car_id = car_id
        self.frame_buffer = None
        self.num_frames = num_frames
        self.actions = actions
        self.agent = self._load_model(model_path)

    def _load_model(self, model_path):
        agent = ActorCritic(self.num_frames, len(self.actions))
        agent.load_state_dict(torch.load(model_path))
        agent.eval()
        return agent

    def _update_frame_buffer(self, new_frame):
        if self.frame_buffer is None:
            self.frame_buffer = new_frame.repeat(1, self.num_frames, 1, 1)
        else:
            self.frame_buffer = add_frame(new_frame, self.frame_buffer)

    def state_to_action(self, s):
        """
        This function should take the most recent state and return the
        action vector in the exact same form it is needed for the environment.
        If you are using frame buffer see example in _update_frame_buffer
        how to take care of that.
        """
        s = preprocess_state(s, self.car_id)
        self._update_frame_buffer(s)
        prob = self.agent(self.frame_buffer)[0]
        a = Categorical(prob).sample().item()
        action_vec = np.array(self.actions[a])
        return action_vec

    @staticmethod
    def setup_action_space(env):
        """
        This should be the same action space setup function that you used for training.
        Make sure that the actions set here are the same as the ones used to train the model.
        """
        env.cont_action_space = ACTIONS
        env.action_space = gym.spaces.Discrete(len(env.cont_action_space))

    @staticmethod
    def get_observation_type():
        """
        Simply return 'frames' or 'features'
        """
        return 'frames'



if __name__ == '__main__':
    global_model = ActorCritic(num_frames, len(ACTIONS))
    global_model.share_memory()

    processes = []
    for rank in range(n_train_processes + 1):  # + 1 for test process
        if rank == 0:
            p = mp.Process(target=test, args=(global_model,))
        else:
            p = mp.Process(target=train, args=(global_model, rank,))
        p.start()
        processes.append(p)
    for p in processes:
        p.join()

# Code modified from https://github.com/seungeunrho/minimalRL/blob/master/a3c.py # Hyperparameters # Track scale # Game over boundary # Action Space Structure # (Steering Wheel, Gas, Break) # Range        -1~1       0~1   0~1 # N x 9*9*32 # N, C, H, W tensor # First step without action, called from reset() # normalize the velocity later # normalize angle diff later # driving on grass # driving backward # Change this for multi-agent caseß # Initialize state with repeated first frame #print('Temperature: ', temperature) #print('Probs:{}, E:{}'.format(pi.detach(), entropy_loss.detach())) This function should take the most recent state and return the         action vector in the exact same form it is needed for the environment.         If you are using frame buffer see example in _update_frame_buffer         how to take care of that. This should be the same action space setup function that you used for training.         Make sure that the actions set here are the same as the ones used to train the model. Simply return 'frames' or 'features' # + 1 for test process