import os
import json
import warnings
import time
import random
import numpy as np
from gym_unity.envs import UnityEnv
from stable_baselines.common.vec_env import SubprocVecEnv
from supervisors.supervisor import Supervisor


def create_vec_env(visibility=2.5, safe_info=True, safe_states=True, supervisor=None, safety_distance_logical=.35,
                   repl=1, worker_start=1000, lidar_num_bins=15, num_envs=20, cbf_quadratic=False,
                   cbf_learn_online=True, rewards=None, log_name=None, num_prior_data_cbf=None, search_space='random',
                   scale_reward=False):
    n_forks = num_envs
    warnings.filterwarnings("ignore")
    env_list = []
    for i in range(0, n_forks):
        def create_env(number=i):
            time.sleep(number)
            # set seed - unity engine sets a seed by default
            seed = int(number + (repl + 1) * n_forks + 1)
            random.seed(seed)
            np.random.seed(seed)
            #movement = [['circular', 'linear', 'circular'],
            #            ['linear', 'circular', 'linear'],
            #            ['circular', 'circular', 'circular'],
            #            ['circular', 'circular', 'linear'],
            #            ['circular', 'circular', 'linear'],
             #           ['linear', 'linear', 'linear'],
             #           ['linear', 'linear', 'circular'],
            #            ['linear', 'circular', 'linear']]

            movement = [['chase', 'linear', 'linear'],
                        ['linear', 'linear', 'chase'],
                        ['linear', 'chase', 'linear'],
                        ['chase', 'chase', 'linear'],
                        ['linear', 'chase', 'chase'],
                        ['linear', 'linear', 'linear'],
                        ['chase', 'linear', 'chase'],
                        ['linear', 'linear', 'linear']]

            #coordinate_static = [np.array([[-.13, -1], [1.2, -1.4], [-.9, -0.25], [-1, 3.0], [-1, -2.20], [0.6, -0.4],
            #                               [-1, -1.80], [-1, -2.0], [-1, -1.75], [-1, -1.55], [1.6, -3], [1.4, -1.23],
            #                               [-1, -1.35], [-1, -1.15], [-1, -0.95], [-1, -0.75], [-1, -0.55], [-1, -0.35],
            #                               [0.94, -0.5], [0.3, -2.2], [1.9, -0.4], [0.8, -0.99], [1.34, -2.2]
            #                               ]),
            #                     np.array([[1.75, -0.75], [-1.25, -1.25], [.7, -1.4], [-1.3, -0.32], [-.3, -1.55],
            #                               [-1, -2.2],
            #                               [-1.3, -0.75], [-.2, -0.86], [.3, -1.9], [1.3, -1.22], [-2.2, -1], [2.2, -1],
            #                               [-1.7, -0.5], [2.2, -2.7], [0.9, -1.9], [2.3, -2.9], [1.7, -2.2]
            #                               ]),
            #                     np.array([[.25, -.95], [.05, -1.25], [-.7, -1], [.55, -1.75], [.9, -1.25], [1.7, -1],
            #                               [1, -1.75], [.2, -2.25], [.7, 0.2], [1.6, -1.5], [1.4, -.5], [-1.1, -.5],
            #                               [2, -.95], [-1.05, -1.25], [-1.7, -1.7], [-1.25, -.95], [-1.05, -2.25],
            #                               [-1.7, -1.2], [-1.25, .95], [-2.05, 1.25], [-2.7, -1.5], [1.25, 2.95],
            #                               [1.05, 1.25]
            #                               ]),
            #                     np.array([[.55, -2.15], [-.55, -1.25], [1.7, -.71], [1.45, -1.15], [-.95, -1.25],
            #                               [.7, -1.9], [2.05, -1.55], [-1.25, -1.25], [.7, -2.1], [-1.45, -1.15],
            #                               [-2.25, -.25], [.7, -1.7], [-2.05, -1.15], [-.25, -.75], [.7, -.9],
            #                               [-0.1, -1.15], [1.25, 1.25], [1.7, -.51], [2.3, -2.9], [1.7, -2.2],
            #                               [2.2, -2.7]
            #                               ])]

            coordinate_static = [np.array([[1.2, -1.4], [-.9, -0.25], [-1, 3.0], [-1, -2.20],
                                           [-1, -1.80], [-1, -2.0], [-1, -1.75], [-1, -1.55], [1.4, -1.23],
                                           [-1, -1.35], [-1, -0.55],
                                           [0.3, -2.2], [1.9, -0.4], [0.8, -0.99], [1.34, -2.2]
                                           ]),
                                 np.array([[1.75, -0.75], [-1.25, -1.25], [.7, -1.4],
                                           [-1, -2.2],
                                           [-1.3, -0.75], [.3, -1.9], [1.3, -1.22], [-2.2, -1], [2.2, -1],
                                           [-1.7, -0.5], [2.2, -2.7], [2.3, -2.9], [1.7, -2.2]
                                           ]),
                                 np.array([[-.7, -1], [.55, -1.75], [.9, -1.25], [1.7, -1],
                                           [1, -1.75], [1.4, -.5], [-1.1, -.5],
                                           [2, -.95], [-1.05, -1.25], [-1.25, -.95], [-2.7, -1.5]
                                           ]),
                                 np.array([[.55, -2.15], [1.7, -.71], [1.45, -1.15], [-.95, -1.25],
                                           [.7, -2.1], [-1.45, -1.15],
                                           [-2.05, -1.15], [-.25, -.75], [.7, -.9],
                                           [2.3, -2.9], [1.7, -2.2],
                                           [2.2, -2.7]
                                           ])]

            coordinate_dynamic = [[{'x': '.25', 'y': '0.46', 'z': '-0.75'},
                                   {'x': '0.25', 'y': '0.46', 'z': '-1.25'}, ##
                                   {'x': '0.7', 'y': '0.46', 'z': '-1.4'}],
                                  [{'x': '-0.13', 'y': '0.46', 'z': '-1'},
                                   {'x': '1.2', 'y': '0.46', 'z': '-1.4'},
                                   {'x': '-0.9', 'y': '0.46', 'z': '-0.45'}],
                                  [{'x': '.45', 'y': '0.46', 'z': '-0.75'},
                                   {'x': '0.25', 'y': '0.46', 'z': '-1.25'},
                                   {'x': '-0.35', 'y': '0.46', 'z': '-.35'}],
                                  [{'x': '.25', 'y': '0.46', 'z': '-0.95'},
                                   {'x': '.05', 'y': '0.46', 'z': '-1.25'},
                                   {'x': '-.7', 'y': '0.46', 'z': '-1'}]]
            env_config = open("env_config.json", "r")
            json_object = json.load(env_config)
            env_config.close()
            n_socket = number
            if number > 14:
                number = number - 15
            if number > 7:
                index = number % 8
            else:
                index = number
            if index > 7:
                index = 0
            json_object['movement'] = movement[index]
            if number > 3:
                index = number % 4
            else:
                index = number
            if index > 3:
                index = 0
            coordinate_static = coordinate_static[index]
            json_object['animatedPositions'] = coordinate_dynamic[index]
            env_config = open("env_config.json", "w")
            json.dump(json_object, env_config)
            env_config.close()
            worker = worker_start + repl * n_forks + 1 + n_socket
            file_name = 'drone-delivery-0.1'
            env_name = "env/drone-delivery-0.1/" + file_name
            if log_name is None:
                log_dir = "logs/drone_ppo_" + supervisor + "_" + str(safety_distance_logical) + "_" + str(visibility) +\
                          "/drone_ppo_" + supervisor + "_" + str(safety_distance_logical) + "_" + str(visibility) + \
                          "_" + str(repl) + "/"
            else:
                log_dir = "./logs/" + log_name + "/" + str(repl) + "/"
            os.makedirs(log_dir, exist_ok=True)
            env = UnityEnv(env_name,
                           worker_id=worker,
                           use_visual=False,
                           uint8_visual=False,
                           allow_multiple_visual_obs=False,
                           no_graphics=True)
            env.observation_space.shape = (((lidar_num_bins + 8) * 3),)
            env = Supervisor(env, log_dir, safety_distance=safety_distance_logical, visibility=visibility,
                             safe_info=safe_info, safe_states=safe_states, supervisor=supervisor,
                             coordinates_static_obstacles=coordinate_static, lidar_num_bins=lidar_num_bins,
                             which_static=index, record_svm_gp=False, cbf_quadratic=cbf_quadratic,
                             cbf_learn_online=cbf_learn_online, rewards=rewards, num_prior_data_cbf=num_prior_data_cbf,
                             search_space=search_space, scale_reward=scale_reward)
            return env
        env_list.append(create_env)
    env = SubprocVecEnv(env_list, start_method="fork")
    time.sleep(num_envs + 3)
    return env

import os
import json
import warnings
import time
import random
import numpy as np
from gym_unity.envs import UnityEnv
from stable_baselines.common.vec_env import SubprocVecEnv
from supervisors.supervisor import Supervisor


def create_vec_env(visibility=2.5, safe_info=True, safe_states=True, supervisor=None, safety_distance_logical=.35,
                   repl=1, worker_start=1000, lidar_num_bins=15, num_envs=20, cbf_quadratic=False,
                   cbf_learn_online=True, rewards=None, log_name=None, num_prior_data_cbf=None, search_space='random',
                   scale_reward=False):
    n_forks = num_envs
    warnings.filterwarnings("ignore")
    env_list = []
    for i in range(0, n_forks):
        def create_env(number=i):
            time.sleep(number)
            # set seed - unity engine sets a seed by default
            seed = int(number + (repl + 1) * n_forks + 1)
            random.seed(seed)
            np.random.seed(seed)
            #movement = [['circular', 'linear', 'circular'],
            #            ['linear', 'circular', 'linear'],
            #            ['circular', 'circular', 'circular'],
            #            ['circular', 'circular', 'linear'],
            #            ['circular', 'circular', 'linear'],
             #           ['linear', 'linear', 'linear'],
             #           ['linear', 'linear', 'circular'],
            #            ['linear', 'circular', 'linear']]

            movement = [['chase', 'linear', 'linear'],
                        ['linear', 'linear', 'chase'],
                        ['linear', 'chase', 'linear'],
                        ['chase', 'chase', 'linear'],
                        ['linear', 'chase', 'chase'],
                        ['linear', 'linear', 'linear'],
                        ['chase', 'linear', 'chase'],
                        ['linear', 'linear', 'linear']]

            #coordinate_static = [np.array([[-.13, -1], [1.2, -1.4], [-.9, -0.25], [-1, 3.0], [-1, -2.20], [0.6, -0.4],
            #                               [-1, -1.80], [-1, -2.0], [-1, -1.75], [-1, -1.55], [1.6, -3], [1.4, -1.23],
            #                               [-1, -1.35], [-1, -1.15], [-1, -0.95], [-1, -0.75], [-1, -0.55], [-1, -0.35],
            #                               [0.94, -0.5], [0.3, -2.2], [1.9, -0.4], [0.8, -0.99], [1.34, -2.2]
            #                               ]),
            #                     np.array([[1.75, -0.75], [-1.25, -1.25], [.7, -1.4], [-1.3, -0.32], [-.3, -1.55],
            #                               [-1, -2.2],
            #                               [-1.3, -0.75], [-.2, -0.86], [.3, -1.9], [1.3, -1.22], [-2.2, -1], [2.2, -1],
            #                               [-1.7, -0.5], [2.2, -2.7], [0.9, -1.9], [2.3, -2.9], [1.7, -2.2]
            #                               ]),
            #                     np.array([[.25, -.95], [.05, -1.25], [-.7, -1], [.55, -1.75], [.9, -1.25], [1.7, -1],
            #                               [1, -1.75], [.2, -2.25], [.7, 0.2], [1.6, -1.5], [1.4, -.5], [-1.1, -.5],
            #                               [2, -.95], [-1.05, -1.25], [-1.7, -1.7], [-1.25, -.95], [-1.05, -2.25],
            #                               [-1.7, -1.2], [-1.25, .95], [-2.05, 1.25], [-2.7, -1.5], [1.25, 2.95],
            #                               [1.05, 1.25]
            #                               ]),
            #                     np.array([[.55, -2.15], [-.55, -1.25], [1.7, -.71], [1.45, -1.15], [-.95, -1.25],
            #                               [.7, -1.9], [2.05, -1.55], [-1.25, -1.25], [.7, -2.1], [-1.45, -1.15],
            #                               [-2.25, -.25], [.7, -1.7], [-2.05, -1.15], [-.25, -.75], [.7, -.9],
            #                               [-0.1, -1.15], [1.25, 1.25], [1.7, -.51], [2.3, -2.9], [1.7, -2.2],
            #                               [2.2, -2.7]
            #                               ])]

            coordinate_static = [np.array([[1.2, -1.4], [-.9, -0.25], [-1, 3.0], [-1, -2.20],
                                           [-1, -1.80], [-1, -2.0], [-1, -1.75], [-1, -1.55], [1.4, -1.23],
                                           [-1, -1.35], [-1, -0.55],
                                           [0.3, -2.2], [1.9, -0.4], [0.8, -0.99], [1.34, -2.2]
                                           ]),
                                 np.array([[1.75, -0.75], [-1.25, -1.25], [.7, -1.4],
                                           [-1, -2.2],
                                           [-1.3, -0.75], [.3, -1.9], [1.3, -1.22], [-2.2, -1], [2.2, -1],
                                           [-1.7, -0.5], [2.2, -2.7], [2.3, -2.9], [1.7, -2.2]
                                           ]),
                                 np.array([[-.7, -1], [.55, -1.75], [.9, -1.25], [1.7, -1],
                                           [1, -1.75], [1.4, -.5], [-1.1, -.5],
                                           [2, -.95], [-1.05, -1.25], [-1.25, -.95], [-2.7, -1.5]
                                           ]),
                                 np.array([[.55, -2.15], [1.7, -.71], [1.45, -1.15], [-.95, -1.25],
                                           [.7, -2.1], [-1.45, -1.15],
                                           [-2.05, -1.15], [-.25, -.75], [.7, -.9],
                                           [2.3, -2.9], [1.7, -2.2],
                                           [2.2, -2.7]
                                           ])]

            coordinate_dynamic = [[{'x': '.25', 'y': '0.46', 'z': '-0.75'},
                                   {'x': '0.25', 'y': '0.46', 'z': '-1.25'}, ##
                                   {'x': '0.7', 'y': '0.46', 'z': '-1.4'}],
                                  [{'x': '-0.13', 'y': '0.46', 'z': '-1'},
                                   {'x': '1.2', 'y': '0.46', 'z': '-1.4'},
                                   {'x': '-0.9', 'y': '0.46', 'z': '-0.45'}],
                                  [{'x': '.45', 'y': '0.46', 'z': '-0.75'},
                                   {'x': '0.25', 'y': '0.46', 'z': '-1.25'},
                                   {'x': '-0.35', 'y': '0.46', 'z': '-.35'}],
                                  [{'x': '.25', 'y': '0.46', 'z': '-0.95'},
                                   {'x': '.05', 'y': '0.46', 'z': '-1.25'},
                                   {'x': '-.7', 'y': '0.46', 'z': '-1'}]]
            env_config = open("env_config.json", "r")
            json_object = json.load(env_config)
            env_config.close()
            n_socket = number
            if number > 14:
                number = number - 15
            if number > 7:
                index = number % 8
            else:
                index = number
            if index > 7:
                index = 0
            json_object['movement'] = movement[index]
            if number > 3:
                index = number % 4
            else:
                index = number
            if index > 3:
                index = 0
            coordinate_static = coordinate_static[index]
            json_object['animatedPositions'] = coordinate_dynamic[index]
            env_config = open("env_config.json", "w")
            json.dump(json_object, env_config)
            env_config.close()
            worker = worker_start + repl * n_forks + 1 + n_socket
            file_name = 'drone-delivery-0.1'
            env_name = "env/drone-delivery-0.1/" + file_name
            if log_name is None:
                log_dir = "logs/drone_ppo_" + supervisor + "_" + str(safety_distance_logical) + "_" + str(visibility) +\
                          "/drone_ppo_" + supervisor + "_" + str(safety_distance_logical) + "_" + str(visibility) + \
                          "_" + str(repl) + "/"
            else:
                log_dir = "./logs/" + log_name + "/" + str(repl) + "/"
            os.makedirs(log_dir, exist_ok=True)
            env = UnityEnv(env_name,
                           worker_id=worker,
                           use_visual=False,
                           uint8_visual=False,
                           allow_multiple_visual_obs=False,
                           no_graphics=True)
            env.observation_space.shape = (((lidar_num_bins + 8) * 3),)
            env = Supervisor(env, log_dir, safety_distance=safety_distance_logical, visibility=visibility,
                             safe_info=safe_info, safe_states=safe_states, supervisor=supervisor,
                             coordinates_static_obstacles=coordinate_static, lidar_num_bins=lidar_num_bins,
                             which_static=index, record_svm_gp=False, cbf_quadratic=cbf_quadratic,
                             cbf_learn_online=cbf_learn_online, rewards=rewards, num_prior_data_cbf=num_prior_data_cbf,
                             search_space=search_space, scale_reward=scale_reward)
            return env
        env_list.append(create_env)
    env = SubprocVecEnv(env_list, start_method="fork")
    time.sleep(num_envs + 3)
    return env

# set seed - unity engine sets a seed by default #movement = [['circular', 'linear', 'circular'], #            ['linear', 'circular', 'linear'], #            ['circular', 'circular', 'circular'], #            ['circular', 'circular', 'linear'], #            ['circular', 'circular', 'linear'], #           ['linear', 'linear', 'linear'], #           ['linear', 'linear', 'circular'], #            ['linear', 'circular', 'linear']] #coordinate_static = [np.array([[-.13, -1], [1.2, -1.4], [-.9, -0.25], [-1, 3.0], [-1, -2.20], [0.6, -0.4], #                               [-1, -1.80], [-1, -2.0], [-1, -1.75], [-1, -1.55], [1.6, -3], [1.4, -1.23], #                               [-1, -1.35], [-1, -1.15], [-1, -0.95], [-1, -0.75], [-1, -0.55], [-1, -0.35], #                               [0.94, -0.5], [0.3, -2.2], [1.9, -0.4], [0.8, -0.99], [1.34, -2.2] #                               ]), #                     np.array([[1.75, -0.75], [-1.25, -1.25], [.7, -1.4], [-1.3, -0.32], [-.3, -1.55], #                               [-1, -2.2], #                               [-1.3, -0.75], [-.2, -0.86], [.3, -1.9], [1.3, -1.22], [-2.2, -1], [2.2, -1], #                               [-1.7, -0.5], [2.2, -2.7], [0.9, -1.9], [2.3, -2.9], [1.7, -2.2] #                               ]), #                     np.array([[.25, -.95], [.05, -1.25], [-.7, -1], [.55, -1.75], [.9, -1.25], [1.7, -1], #                               [1, -1.75], [.2, -2.25], [.7, 0.2], [1.6, -1.5], [1.4, -.5], [-1.1, -.5], #                               [2, -.95], [-1.05, -1.25], [-1.7, -1.7], [-1.25, -.95], [-1.05, -2.25], #                               [-1.7, -1.2], [-1.25, .95], [-2.05, 1.25], [-2.7, -1.5], [1.25, 2.95], #                               [1.05, 1.25] #                               ]), #                     np.array([[.55, -2.15], [-.55, -1.25], [1.7, -.71], [1.45, -1.15], [-.95, -1.25], #                               [.7, -1.9], [2.05, -1.55], [-1.25, -1.25], [.7, -2.1], [-1.45, -1.15], #                               [-2.25, -.25], [.7, -1.7], [-2.05, -1.15], [-.25, -.75], [.7, -.9], #                               [-0.1, -1.15], [1.25, 1.25], [1.7, -.51], [2.3, -2.9], [1.7, -2.2], #                               [2.2, -2.7] #                               ])] ##