# load extern modules
import gym
import csv
import time
import numpy as np
from stable_baselines.bench import Monitor
from supervisors.utils import distance_from_obstacles
from env.utils import obs_lidar_pseudo
import threading
from supervisors.cbf import initialize_gp_dynamics, predict_successor_state_gp, initialize_svm_prediction
from supervisors.cbf import initialize_svm_prediction_gp
from operator import itemgetter
from skopt.space import Space
from skopt.sampler import Grid


# from qpsolvers import solve_qp


class Supervisor(Monitor):
    """
    A monitor wrapper for Gym environments to save collisions events

    Parameters:
        env (gym.Env): The environment
        filename (Optional[str]): the location to save a log file, can be None for no log
    """

    def __init__(self,
                 env: gym.Env,
                 filename: str,
                 safety_distance: float or None,
                 visibility: float,
                 safe_info: bool,
                 safe_states: bool,
                 supervisor: str,
                 coordinates_static_obstacles: np.array,
                 lidar_num_bins: int,
                 which_static: int,
                 record_svm_gp: bool,
                 cbf_quadratic: bool,
                 cbf_learn_online: bool,
                 rewards: np.array or None,
                 num_prior_data_cbf: str or None,
                 search_space: str or None,
                 scale_reward: bool):
        super(Supervisor, self).__init__(env=env, filename=filename)
        self.safety_distance = safety_distance
        self.visibility = visibility
        self.supervisor = supervisor
        self.lidar_num_bins = lidar_num_bins
        self.Supervised = 0
        self.Intervention = 0
        self.SafelyDone = 0
        self.scale_reward = scale_reward
        self.record_svm_gp = record_svm_gp
        self.Crashed = 0
        self.last_teacher_crash = 0
        self.timeOut = 0
        self.which_static = which_static
        self.safe_states = safe_states
        self.total_distance = 0
        if supervisor == 'cbf-gp-logical' or supervisor == 'cbf-gp-svm':
            if num_prior_data_cbf is None:
                num_prior_data_cbf = '2k'
            self.gp = initialize_gp_dynamics('2k')
        if supervisor == 'cbf-gp-svm':
            if num_prior_data_cbf is None:
                num_prior_data_cbf = 100000
            self.svm = initialize_svm_prediction_gp(num_prior_data=num_prior_data_cbf)
            self.unsafe_threshold = 0.5
            search_space = 'grid'
        if supervisor == 'cbf-svm':
            if num_prior_data_cbf is None:
                num_prior_data_cbf = 2000
            self.svm = initialize_svm_prediction(num_prior_data=num_prior_data_cbf)
            self.unsafe_threshold = 0.6
        if supervisor == 'cbf-gp-logical':
            self.unsafe_threshold = 0.85
            search_space = 'random'
        self.cbf_quadratic = cbf_quadratic
        if search_space == 'grid':
            space = Space([(-1., 1.), (-1., 1.)])
            grid = Grid(border="include", use_full_layout=False)
            action_manipulated = grid.generate(space.dimensions, 160)
            action_manipulated = np.array(action_manipulated)
            action_manipulated2 = \
                np.append(action_manipulated[(action_manipulated[:, 0] < -0.3) * (action_manipulated[:, 1] < -0.3), :],
                          action_manipulated[(action_manipulated[:, 0] > 0.3) * (action_manipulated[:, 1] > 0.3), :],
                          axis=0)
            action_manipulated2 = \
                np.append(action_manipulated2,
                          action_manipulated[(action_manipulated[:, 0] > 0.3) * (action_manipulated[:, 1] < -0.3), :],
                          axis=0)
            action_manipulated2 = \
                np.append(action_manipulated2,
                          action_manipulated[(action_manipulated[:, 0] < -0.3) * (action_manipulated[:, 1] > 0.3), :],
                          axis=0)
            self.action_manipulated = action_manipulated2
        if search_space == 'random':
            self.action_manipulated = np.array([[-0.1, 0],
                                               [0.1, 0],
                                               [0, 0.1],
                                               [0, -0.1],
                                               [-0.25, 0],
                                               [0.25, 0],
                                               [0, 0.25],
                                               [0, -0.25],
                                               [-0.1, 0.1],
                                               [0.1, 0.1],
                                               [-0.1, -0.1],
                                               [0.1, -0.1],
                                               [-0.25, 0.25],
                                               [0.25, 0.25],
                                               [-0.25, -0.25],
                                               [0.25, -0.25], ###############
                                               [0.1, 0.05],
                                               [0.05, 0.1],
                                               [0.05, -0.1],
                                               [-0.25, 0.1],
                                               [0.25, 0.8],
                                               [0.6, 0.25],
                                               [0.3, -0.25],
                                               [-0.1, 0.7],
                                               [0.9, 0.1],
                                               [-0.1, -1],
                                               [1, -0.1],
                                               [-0.2, 0.75],
                                               [0.5, 0.5],
                                               [-0.5, -0.5],
                                               [0.75, 0],
                                               [0.15, 0.05],
                                               [0.6, 0.1],
                                               [0.4, -0.1],
                                               [-0.25, 0.15],
                                               [0.25, 0.9],
                                               [-0.35, 0.25],
                                               [0.5, -0.25],
                                               [-0.19, 0.19],
                                               [1, 1],
                                               [-1, -1],
                                               [0, 1],
                                               [-1, 0],
                                               [0.2, 0.75],
                                               [-0.8, 0],
                                               [0, -0.58]])
        self.cbf_learn_online = cbf_learn_online
        self.TotalCollisions = []
        self.filename = filename
        self.observation_storage = []
        self.safe_info = safe_info
        self.csv_writer_lock = threading.Lock()
        self.coordinates_static_obstacles = coordinates_static_obstacles
        self.rewards = rewards
        # reward shaping
        if self.rewards is not None:
            self.reward_reached_target = self.rewards[0]
            self.reward_distance = self.rewards[1]
            self.reward_crashed = self.rewards[2]
            if self.supervisor == "logical":
                self.reward_logical = self.rewards[3]
            if self.supervisor == "cbf-gp-logical" or self.supervisor == "cbf-svm" or self.supervisor == "cbf-gp-svm":
                self.reward_cbf = self.rewards[3]
        # default reward settings
        if self.rewards is None:
            self.reward_reached_target = 150
            self.reward_distance = 1.5
            self.reward_crashed = 150
            self.reward_cbf = 0.5
            self.reward_logical = 100
        # add extra static obstacles as boundary of the garden
        self.extra_obstacles = np.array([[-2.5, -.15],
                                         [-2.5, -.35],
                                         [-2.5, -.55],
                                         [-2.5, -.75],
                                         [-2.5, -.95],
                                         [-2.5, -1.15],
                                         [-2.5, -1.35],
                                         [-2.5, -1.55],
                                         [-2.5, -1.75],
                                         [-2.5, -1.95],
                                         [-2.5, -2.15],
                                         [-2.5, -2.35],
                                         [-2.5, -2.55],
                                         [-2.5, -2.75],
                                         [-2.5, -2.95],
                                         [-2.5, -3.15],
                                         [-2.5, -3.35],
                                         [2.55, -.25],
                                         [2.55, -.35],
                                         [2.55, -.55],
                                         [2.55, -.75],
                                         [2.55, -.95],
                                         [2.55, -1.15],
                                         [2.55, -1.35],
                                         [2.55, -1.55],
                                         [2.55, -1.75],
                                         [2.55, -1.95],
                                         [2.55, -2.15],
                                         [2.55, -2.35],
                                         [2.55, -2.55],
                                         [2.55, -2.75],
                                         [2.55, -2.95],
                                         [2.55, -3.15],
                                         [2.55, -3.35],
                                         [-2.50, -0.15],
                                         [-2.30, -0.15],
                                         [-2.10, -0.15],
                                         [-1.90, -0.15],
                                         [-1.70, -0.15],
                                         [-1.50, -0.15],
                                         [-1.30, -0.15],
                                         [-1.10, -0.15],
                                         [-.90, -0.15],
                                         [-.70, -0.15],
                                         [-.5, -0.15],
                                         [-.3, -0.15],
                                         [-.1, -0.15],
                                         [0.7, -0.15],
                                         [0.9, -0.15],
                                         [1.1, -0.15],
                                         [1.3, -0.15],
                                         [1.5, -0.15],
                                         [1.7, -0.15],
                                         [1.9, -0.15],
                                         [2.1, -0.15],
                                         [2.3, -0.15],
                                         [2.5, -0.15],
                                         [-2.40, -3.4],
                                         [-2.20, -3.4],
                                         [-2.00, -3.4],
                                         [-1.90, -3.4],
                                         [-1.70, -3.4],
                                         [-1.50, -3.4],
                                         [-1.30, -3.4],
                                         [-1.10, -3.4],
                                         [-.90, -3.4],
                                         [-.70, -3.4],
                                         [-.50, -3.4],
                                         [-.3, -3.4],
                                         [-.1, -3.4],
                                         [0.1, -3.4],
                                         [0.3, -3.4],
                                         [0.5, -3.4],
                                         [0.7, -3.4],
                                         [0.9, -3.4],
                                         [1.1, -3.4],
                                         [1.3, -3.4],
                                         [1.5, -3.4],
                                         [1.7, -3.4],
                                         [1.9, -3.4],
                                         [2.1, -3.4],
                                         [2.3, -3.4],
                                         [2.5, -3.4],
                                         ])
        # add extra obstacles as tress in the middle of the garden
        self.extra_obstacles = np.concatenate((self.extra_obstacles, self.coordinates_static_obstacles))

    def step(self, action):
        """
        Get information for the next RL step

        Parameters:
            action (float, float): Action vector (speed)

        Returns:
            [float]: Observation vector
            float: reward value
            observation, reward, done, info

        """
        reward = 0
        # check if env needs reset
        if self.needs_reset:
            raise RuntimeError("Tried to step environment that needs reset")
        # pass proposed action to the CBF (if CBF is active) and return the manipulated or the proposed action
        if self.supervisor == 'cbf-gp-logical' or self.supervisor == 'cbf-svm' or self.supervisor == 'cbf-gp-svm':
            index = int(len(self.observation_storage) - 1)
            old_observation = self.observation_storage[index]
            if self.cbf_learn_online is True:
                if len(self.observation_storage) > 2:
                    state_x = self.observation_storage[index - 1]
                    state_x = state_x[0:self.lidar_num_bins]
                    state_x = np.asarray(state_x)
                    action_x = self.observation_storage[index]
                    action_x = action_x[28:30]
                    action_x = np.asarray(action_x)
                    state_action_x = np.append(state_x, action_x)
                    state_action_x = state_action_x.reshape(1, -1)
                    state_y = self.observation_storage[index]
                    state_y = state_y[26]
                    if state_y > 0:
                        state_y = int(1)
                    else:
                        state_y = int(-1)
                    state_y = np.asarray(state_y)
                    state_y = state_y.reshape(1, -1)
                    self.svm.fit(state_action_x, state_y)
            change_proposed_action = False
            if self.supervisor == 'cbf-gp-svm' or self.supervisor == 'cbf-gp-logical':
                successor_state_worst = predict_successor_state_gp(self.gp,
                                                                   observation=old_observation[0:self.lidar_num_bins],
                                                                   action=action)
                if self.supervisor == 'cbf-gp-svm':
                    successor_state_worst = np.sort(successor_state_worst)
                    successor_state_worst = successor_state_worst.reshape(1, -1)
                    probability = self.svm.predict_proba(successor_state_worst)
                    unsafe_probability = probability[0, 1]
                    # print(unsafe_probability)
                    if unsafe_probability > self.unsafe_threshold:
                        change_proposed_action = True
                if self.supervisor == 'cbf-gp-logical':
                    change_proposed_action = max(successor_state_worst[0]) > self.unsafe_threshold
            if self.supervisor == 'cbf-svm':
                # hier nur svm predicten
                state_action = np.concatenate((old_observation[0:self.lidar_num_bins], action), axis=0)
                state_action = state_action.reshape(1, -1)
                probability = self.svm.predict_proba(state_action)
                unsafe = probability[0, 1]
                if unsafe > self.unsafe_threshold:
                    change_proposed_action = True
            if change_proposed_action:
                if self.supervisor == 'cbf-gp-logical' or self.supervisor == 'cbf-gp-svm':
                    successor_state_worst_manipulated = []
                    manipulated = predict_successor_state_gp(self.gp,
                                                             observation=old_observation[0:self.lidar_num_bins],
                                                             action=self.action_manipulated)
                    if self.supervisor == 'cbf-gp-logical':
                        successor_state_worst_manipulated = np.amax(manipulated, axis=1)
                    if self.supervisor == 'cbf-gp-svm':
                        manipulated = np.sort(manipulated, axis=1)
                        probability = self.svm.predict_proba(manipulated)
                        successor_state_worst_manipulated = probability[:, 1]
                    if not self.cbf_quadratic:
                        index = np.argmin(successor_state_worst_manipulated)
                        distance_org = np.sqrt((self.action_manipulated[index, 0] - action[0]) ** 2 +
                                               (self.action_manipulated[index, 1] - action[1]) ** 2)
                        self.total_distance += distance_org
                        action = self.action_manipulated[index]
                        if self.scale_reward:
                            reward -= self.reward_cbf * (distance_org / 2.8)
                            self.total_distance += distance_org
                    if self.cbf_quadratic:
                        if int(sum(successor_state_worst_manipulated < self.unsafe_threshold)) > 0.1:
                            safety_actions = self.action_manipulated[successor_state_worst_manipulated < self.unsafe_threshold]
                        else:
                            safety_actions = self.action_manipulated
                        distance_org = np.sqrt((safety_actions[:, 0] - action[0]) ** 2 +
                                               (safety_actions[:, 1] - action[1]) ** 2)
                        index = min(enumerate(distance_org), key=itemgetter(1))[0]
                        action = safety_actions[index]
                        if self.scale_reward:
                            reward -= self.reward_cbf * (distance_org[index] / 2.8)
                            self.total_distance += distance_org[index]


                    #if self.supervisor == 'cbf-svm':
                    #    successor_state_unsafe_prob_manipulated = []
                    #    for j in range(0, len(self.action_manipulated)):
                    #        state_action = np.concatenate((old_observation[0:self.lidar_num_bins],
                    #                                       self.action_manipulated[j]), axis=0)
                    #        state_action = state_action.reshape(1, -1)
                    #        probability = self.svm.predict_proba(state_action)
                    #        unsafe = probability[0, 1]
                    #        successor_state_unsafe_prob_manipulated.append(unsafe)
                    #    index = np.argmin(successor_state_unsafe_prob_manipulated)
                    #    action = self.action_manipulated[index]
                self.last_teacher_crash = 1
                self.Intervention += 1
                self.Supervised = 1
                if not self.scale_reward:
                    reward -= self.reward_cbf

        # step env
        observation, reward_unity, done, info = self.env.step(action)
        # manipulate reward with distance to target. maximum distance is ~5.
        reward -= (observation[0] / 5) * self.reward_distance
        # save org ibm obs
        org_observation = observation
        # check if safely done
        if done:
            self.SafelyDone += 1
            reward += self.reward_reached_target
        # compute distances to obstacles
        distance = distance_from_obstacles(self, observation)
        # check if drone crashed
        if not done:
            if np.min(distance) <= .2:
                self.Crashed += 1
                reward -= self.reward_crashed
                done = True
        # check if logical supervisor is active
        if not done:
            if self.supervisor == 'logical':
                if not self.record_svm_gp:
                    if np.min(distance) <= (self.safety_distance + np.random.rand(1) / 25):
                        self.Supervised += 1
                        self.Intervention += 1
                        done = True
                        reward -= self.reward_logical
                # the following is used when recording data
                if self.record_svm_gp:
                    if np.min(distance) <= 0.29 + np.random.rand(1) / 25:
                        self.Supervised += 1
                        self.Intervention += 1
                        done = True
                        reward -= self.reward_logical
                    else:
                        if np.min(distance) <= 0.35 + np.random.rand(1) / 25:
                            self.Supervised += 1
        # append reward
        self.rewards.append(reward)
        # and didnt end safely
        if len(self.rewards) == 120:
            if not done:
                done = True
                self.timeOut += 1
        # transform observation to lidar like observation
        observation = obs_lidar_pseudo(self, observation, lidar_num_bins=self.lidar_num_bins, lidar_max_dist=None,
                                       lidar_alias=True, lidar_exp_gain=1.0, distance=distance)
        # append supervisor and save current observation to the storage
        # first self.lidar_num_bins entries of the observation storage correspond to the lidar
        # the next entry is the supervised indicator
        # the next entry is the done indicator
        # the last 23 entries correspond to the original oracle observation by IBM
        observation_storage = np.append(observation, self.Supervised)
        observation_storage = np.append(observation_storage, self.which_static)
        observation_storage = np.append(observation_storage, self.Intervention)
        observation_storage = np.append(observation_storage, self.Crashed)
        observation_storage = np.append(observation_storage, done)
        observation_storage = np.append(observation_storage, action)
        observation_storage = np.append(observation_storage, org_observation)
        self.observation_storage.append(observation_storage)

        # append the observations of time step t-1 and t-2 to the observation of time step t
        # when resetting the env the observations of time step t-1 and t-2 are np.zeros()
        # in time step 1 the observation of time step t-2 are np.zeros()
        if len(self.rewards) == 1:
            observation = np.concatenate((observation, self.observation_storage[0][0:(self.lidar_num_bins + 8)]),
                                         axis=0)
            observation_dummy = np.zeros(self.lidar_num_bins + 8)
            observation = np.concatenate((observation, observation_dummy), axis=0)

        if len(self.rewards) > 1:
            observation = np.concatenate((observation, self.observation_storage[len(self.rewards) - 2
                                                                                ][0:(self.lidar_num_bins + 8)]), axis=0)
            observation = np.concatenate((observation, self.observation_storage[len(self.rewards) - 1
                                                                                ][0:(self.lidar_num_bins + 8)]), axis=0)

        if done:
            self.needs_reset = True
            ep_rew = sum(self.rewards)
            eplen = len(self.rewards)
            if self.Crashed is True and self.last_teacher_crash == 1:
                teacher_failed = 1
            else:
                teacher_failed = 0
            ep_info = [round(ep_rew, 6), eplen, self.Crashed, self.Supervised, self.Intervention, self.SafelyDone,
                       self.timeOut, round(time.time(), 6), teacher_failed, self.total_distance]
            self.episode_rewards.append(ep_rew)
            self.episode_lengths.append(eplen)
            self.episode_times.append(time.time() - self.t_start)
            info['episode'] = ep_info
            if self.safe_states:  # and self.Crashed > 0:####################################################
                with open(self.filename + 'states.csv', 'a', newline='') as f:
                    writer = csv.writer(f)
                    with self.csv_writer_lock:
                        writer.writerows(self.observation_storage)
            if self.safe_info:
                with open(self.filename + 'monitor.csv', 'a', newline='') as f:
                    writer = csv.writer(f)
                    with self.csv_writer_lock:
                        writer.writerow(ep_info)
            self.TotalCollisions.append(self.Supervised)
            self.Supervised = 0
            self.SafelyDone = 0
            self.Intervention = 0
            self.Crashed = 0
            self.total_distance = 0
            self.timeOut = 0
            self.rewards = []
            self.observation_storage = []
        if not done:
            if self.supervisor == 'logical' and self.record_svm_gp:
                self.Supervised = 0
            self.last_teacher_crash = 0
        self.total_steps += 1
        return observation, reward, done, info

    def reset(self, **kwargs):
        observation = self.env.reset(**kwargs)
        org_observation = observation
        distance = distance_from_obstacles(self, observation)
        while np.min(distance) < .35:
            observation = self.env.reset(**kwargs)
            org_observation = observation
            distance = distance_from_obstacles(self, observation)
        if np.min(distance) < .35:
            self.needs_reset = True
        if np.min(distance) >= .35:
            self.needs_reset = False
        self.rewards = []
        observation = obs_lidar_pseudo(self, observation, lidar_num_bins=self.lidar_num_bins, lidar_max_dist=None,
                                       lidar_alias=True, lidar_exp_gain=1.0, distance=distance)
        # append supervisor and save current observation to the storage
        supervised = False
        observation_storage = np.append(observation, supervised)
        observation_storage = np.append(observation_storage, self.which_static)
        intervention = np.array([0])
        observation_storage = np.append(observation_storage, intervention)
        crash_dummy = np.array([0])
        observation_storage = np.append(observation_storage, crash_dummy)
        done = False
        observation_storage = np.append(observation_storage, done)
        action_dummy = np.array([0, 0])
        observation_storage = np.append(observation_storage, action_dummy)
        observation_storage = np.append(observation_storage, org_observation)
        self.observation_storage.append(observation_storage)
        observation_dummy = np.zeros(self.lidar_num_bins + 8)
        observation = np.concatenate((observation, observation_dummy), axis=0)
        observation = np.concatenate((observation, observation_dummy), axis=0)
        return observation

# load extern modules
import gym
import csv
import time
import numpy as np
from stable_baselines.bench import Monitor
from supervisors.utils import distance_from_obstacles
from env.utils import obs_lidar_pseudo
import threading
from supervisors.cbf import initialize_gp_dynamics, predict_successor_state_gp, initialize_svm_prediction
from supervisors.cbf import initialize_svm_prediction_gp
from operator import itemgetter
from skopt.space import Space
from skopt.sampler import Grid


# from qpsolvers import solve_qp


class Supervisor(Monitor):
    """
    A monitor wrapper for Gym environments to save collisions events

    Parameters:
        env (gym.Env): The environment
        filename (Optional[str]): the location to save a log file, can be None for no log
    """

    def __init__(self,
                 env: gym.Env,
                 filename: str,
                 safety_distance: float or None,
                 visibility: float,
                 safe_info: bool,
                 safe_states: bool,
                 supervisor: str,
                 coordinates_static_obstacles: np.array,
                 lidar_num_bins: int,
                 which_static: int,
                 record_svm_gp: bool,
                 cbf_quadratic: bool,
                 cbf_learn_online: bool,
                 rewards: np.array or None,
                 num_prior_data_cbf: str or None,
                 search_space: str or None,
                 scale_reward: bool):
        super(Supervisor, self).__init__(env=env, filename=filename)
        self.safety_distance = safety_distance
        self.visibility = visibility
        self.supervisor = supervisor
        self.lidar_num_bins = lidar_num_bins
        self.Supervised = 0
        self.Intervention = 0
        self.SafelyDone = 0
        self.scale_reward = scale_reward
        self.record_svm_gp = record_svm_gp
        self.Crashed = 0
        self.last_teacher_crash = 0
        self.timeOut = 0
        self.which_static = which_static
        self.safe_states = safe_states
        self.total_distance = 0
        if supervisor == 'cbf-gp-logical' or supervisor == 'cbf-gp-svm':
            if num_prior_data_cbf is None:
                num_prior_data_cbf = '2k'
            self.gp = initialize_gp_dynamics('2k')
        if supervisor == 'cbf-gp-svm':
            if num_prior_data_cbf is None:
                num_prior_data_cbf = 100000
            self.svm = initialize_svm_prediction_gp(num_prior_data=num_prior_data_cbf)
            self.unsafe_threshold = 0.5
            search_space = 'grid'
        if supervisor == 'cbf-svm':
            if num_prior_data_cbf is None:
                num_prior_data_cbf = 2000
            self.svm = initialize_svm_prediction(num_prior_data=num_prior_data_cbf)
            self.unsafe_threshold = 0.6
        if supervisor == 'cbf-gp-logical':
            self.unsafe_threshold = 0.85
            search_space = 'random'
        self.cbf_quadratic = cbf_quadratic
        if search_space == 'grid':
            space = Space([(-1., 1.), (-1., 1.)])
            grid = Grid(border="include", use_full_layout=False)
            action_manipulated = grid.generate(space.dimensions, 160)
            action_manipulated = np.array(action_manipulated)
            action_manipulated2 = \
                np.append(action_manipulated[(action_manipulated[:, 0] < -0.3) * (action_manipulated[:, 1] < -0.3), :],
                          action_manipulated[(action_manipulated[:, 0] > 0.3) * (action_manipulated[:, 1] > 0.3), :],
                          axis=0)
            action_manipulated2 = \
                np.append(action_manipulated2,
                          action_manipulated[(action_manipulated[:, 0] > 0.3) * (action_manipulated[:, 1] < -0.3), :],
                          axis=0)
            action_manipulated2 = \
                np.append(action_manipulated2,
                          action_manipulated[(action_manipulated[:, 0] < -0.3) * (action_manipulated[:, 1] > 0.3), :],
                          axis=0)
            self.action_manipulated = action_manipulated2
        if search_space == 'random':
            self.action_manipulated = np.array([[-0.1, 0],
                                               [0.1, 0],
                                               [0, 0.1],
                                               [0, -0.1],
                                               [-0.25, 0],
                                               [0.25, 0],
                                               [0, 0.25],
                                               [0, -0.25],
                                               [-0.1, 0.1],
                                               [0.1, 0.1],
                                               [-0.1, -0.1],
                                               [0.1, -0.1],
                                               [-0.25, 0.25],
                                               [0.25, 0.25],
                                               [-0.25, -0.25],
                                               [0.25, -0.25], ###############
                                               [0.1, 0.05],
                                               [0.05, 0.1],
                                               [0.05, -0.1],
                                               [-0.25, 0.1],
                                               [0.25, 0.8],
                                               [0.6, 0.25],
                                               [0.3, -0.25],
                                               [-0.1, 0.7],
                                               [0.9, 0.1],
                                               [-0.1, -1],
                                               [1, -0.1],
                                               [-0.2, 0.75],
                                               [0.5, 0.5],
                                               [-0.5, -0.5],
                                               [0.75, 0],
                                               [0.15, 0.05],
                                               [0.6, 0.1],
                                               [0.4, -0.1],
                                               [-0.25, 0.15],
                                               [0.25, 0.9],
                                               [-0.35, 0.25],
                                               [0.5, -0.25],
                                               [-0.19, 0.19],
                                               [1, 1],
                                               [-1, -1],
                                               [0, 1],
                                               [-1, 0],
                                               [0.2, 0.75],
                                               [-0.8, 0],
                                               [0, -0.58]])
        self.cbf_learn_online = cbf_learn_online
        self.TotalCollisions = []
        self.filename = filename
        self.observation_storage = []
        self.safe_info = safe_info
        self.csv_writer_lock = threading.Lock()
        self.coordinates_static_obstacles = coordinates_static_obstacles
        self.rewards = rewards
        # reward shaping
        if self.rewards is not None:
            self.reward_reached_target = self.rewards[0]
            self.reward_distance = self.rewards[1]
            self.reward_crashed = self.rewards[2]
            if self.supervisor == "logical":
                self.reward_logical = self.rewards[3]
            if self.supervisor == "cbf-gp-logical" or self.supervisor == "cbf-svm" or self.supervisor == "cbf-gp-svm":
                self.reward_cbf = self.rewards[3]
        # default reward settings
        if self.rewards is None:
            self.reward_reached_target = 150
            self.reward_distance = 1.5
            self.reward_crashed = 150
            self.reward_cbf = 0.5
            self.reward_logical = 100
        # add extra static obstacles as boundary of the garden
        self.extra_obstacles = np.array([[-2.5, -.15],
                                         [-2.5, -.35],
                                         [-2.5, -.55],
                                         [-2.5, -.75],
                                         [-2.5, -.95],
                                         [-2.5, -1.15],
                                         [-2.5, -1.35],
                                         [-2.5, -1.55],
                                         [-2.5, -1.75],
                                         [-2.5, -1.95],
                                         [-2.5, -2.15],
                                         [-2.5, -2.35],
                                         [-2.5, -2.55],
                                         [-2.5, -2.75],
                                         [-2.5, -2.95],
                                         [-2.5, -3.15],
                                         [-2.5, -3.35],
                                         [2.55, -.25],
                                         [2.55, -.35],
                                         [2.55, -.55],
                                         [2.55, -.75],
                                         [2.55, -.95],
                                         [2.55, -1.15],
                                         [2.55, -1.35],
                                         [2.55, -1.55],
                                         [2.55, -1.75],
                                         [2.55, -1.95],
                                         [2.55, -2.15],
                                         [2.55, -2.35],
                                         [2.55, -2.55],
                                         [2.55, -2.75],
                                         [2.55, -2.95],
                                         [2.55, -3.15],
                                         [2.55, -3.35],
                                         [-2.50, -0.15],
                                         [-2.30, -0.15],
                                         [-2.10, -0.15],
                                         [-1.90, -0.15],
                                         [-1.70, -0.15],
                                         [-1.50, -0.15],
                                         [-1.30, -0.15],
                                         [-1.10, -0.15],
                                         [-.90, -0.15],
                                         [-.70, -0.15],
                                         [-.5, -0.15],
                                         [-.3, -0.15],
                                         [-.1, -0.15],
                                         [0.7, -0.15],
                                         [0.9, -0.15],
                                         [1.1, -0.15],
                                         [1.3, -0.15],
                                         [1.5, -0.15],
                                         [1.7, -0.15],
                                         [1.9, -0.15],
                                         [2.1, -0.15],
                                         [2.3, -0.15],
                                         [2.5, -0.15],
                                         [-2.40, -3.4],
                                         [-2.20, -3.4],
                                         [-2.00, -3.4],
                                         [-1.90, -3.4],
                                         [-1.70, -3.4],
                                         [-1.50, -3.4],
                                         [-1.30, -3.4],
                                         [-1.10, -3.4],
                                         [-.90, -3.4],
                                         [-.70, -3.4],
                                         [-.50, -3.4],
                                         [-.3, -3.4],
                                         [-.1, -3.4],
                                         [0.1, -3.4],
                                         [0.3, -3.4],
                                         [0.5, -3.4],
                                         [0.7, -3.4],
                                         [0.9, -3.4],
                                         [1.1, -3.4],
                                         [1.3, -3.4],
                                         [1.5, -3.4],
                                         [1.7, -3.4],
                                         [1.9, -3.4],
                                         [2.1, -3.4],
                                         [2.3, -3.4],
                                         [2.5, -3.4],
                                         ])
        # add extra obstacles as tress in the middle of the garden
        self.extra_obstacles = np.concatenate((self.extra_obstacles, self.coordinates_static_obstacles))

    def step(self, action):
        """
        Get information for the next RL step

        Parameters:
            action (float, float): Action vector (speed)

        Returns:
            [float]: Observation vector
            float: reward value
            observation, reward, done, info

        """
        reward = 0
        # check if env needs reset
        if self.needs_reset:
            raise RuntimeError("Tried to step environment that needs reset")
        # pass proposed action to the CBF (if CBF is active) and return the manipulated or the proposed action
        if self.supervisor == 'cbf-gp-logical' or self.supervisor == 'cbf-svm' or self.supervisor == 'cbf-gp-svm':
            index = int(len(self.observation_storage) - 1)
            old_observation = self.observation_storage[index]
            if self.cbf_learn_online is True:
                if len(self.observation_storage) > 2:
                    state_x = self.observation_storage[index - 1]
                    state_x = state_x[0:self.lidar_num_bins]
                    state_x = np.asarray(state_x)
                    action_x = self.observation_storage[index]
                    action_x = action_x[28:30]
                    action_x = np.asarray(action_x)
                    state_action_x = np.append(state_x, action_x)
                    state_action_x = state_action_x.reshape(1, -1)
                    state_y = self.observation_storage[index]
                    state_y = state_y[26]
                    if state_y > 0:
                        state_y = int(1)
                    else:
                        state_y = int(-1)
                    state_y = np.asarray(state_y)
                    state_y = state_y.reshape(1, -1)
                    self.svm.fit(state_action_x, state_y)
            change_proposed_action = False
            if self.supervisor == 'cbf-gp-svm' or self.supervisor == 'cbf-gp-logical':
                successor_state_worst = predict_successor_state_gp(self.gp,
                                                                   observation=old_observation[0:self.lidar_num_bins],
                                                                   action=action)
                if self.supervisor == 'cbf-gp-svm':
                    successor_state_worst = np.sort(successor_state_worst)
                    successor_state_worst = successor_state_worst.reshape(1, -1)
                    probability = self.svm.predict_proba(successor_state_worst)
                    unsafe_probability = probability[0, 1]
                    # print(unsafe_probability)
                    if unsafe_probability > self.unsafe_threshold:
                        change_proposed_action = True
                if self.supervisor == 'cbf-gp-logical':
                    change_proposed_action = max(successor_state_worst[0]) > self.unsafe_threshold
            if self.supervisor == 'cbf-svm':
                # hier nur svm predicten
                state_action = np.concatenate((old_observation[0:self.lidar_num_bins], action), axis=0)
                state_action = state_action.reshape(1, -1)
                probability = self.svm.predict_proba(state_action)
                unsafe = probability[0, 1]
                if unsafe > self.unsafe_threshold:
                    change_proposed_action = True
            if change_proposed_action:
                if self.supervisor == 'cbf-gp-logical' or self.supervisor == 'cbf-gp-svm':
                    successor_state_worst_manipulated = []
                    manipulated = predict_successor_state_gp(self.gp,
                                                             observation=old_observation[0:self.lidar_num_bins],
                                                             action=self.action_manipulated)
                    if self.supervisor == 'cbf-gp-logical':
                        successor_state_worst_manipulated = np.amax(manipulated, axis=1)
                    if self.supervisor == 'cbf-gp-svm':
                        manipulated = np.sort(manipulated, axis=1)
                        probability = self.svm.predict_proba(manipulated)
                        successor_state_worst_manipulated = probability[:, 1]
                    if not self.cbf_quadratic:
                        index = np.argmin(successor_state_worst_manipulated)
                        distance_org = np.sqrt((self.action_manipulated[index, 0] - action[0]) ** 2 +
                                               (self.action_manipulated[index, 1] - action[1]) ** 2)
                        self.total_distance += distance_org
                        action = self.action_manipulated[index]
                        if self.scale_reward:
                            reward -= self.reward_cbf * (distance_org / 2.8)
                            self.total_distance += distance_org
                    if self.cbf_quadratic:
                        if int(sum(successor_state_worst_manipulated < self.unsafe_threshold)) > 0.1:
                            safety_actions = self.action_manipulated[successor_state_worst_manipulated < self.unsafe_threshold]
                        else:
                            safety_actions = self.action_manipulated
                        distance_org = np.sqrt((safety_actions[:, 0] - action[0]) ** 2 +
                                               (safety_actions[:, 1] - action[1]) ** 2)
                        index = min(enumerate(distance_org), key=itemgetter(1))[0]
                        action = safety_actions[index]
                        if self.scale_reward:
                            reward -= self.reward_cbf * (distance_org[index] / 2.8)
                            self.total_distance += distance_org[index]


                    #if self.supervisor == 'cbf-svm':
                    #    successor_state_unsafe_prob_manipulated = []
                    #    for j in range(0, len(self.action_manipulated)):
                    #        state_action = np.concatenate((old_observation[0:self.lidar_num_bins],
                    #                                       self.action_manipulated[j]), axis=0)
                    #        state_action = state_action.reshape(1, -1)
                    #        probability = self.svm.predict_proba(state_action)
                    #        unsafe = probability[0, 1]
                    #        successor_state_unsafe_prob_manipulated.append(unsafe)
                    #    index = np.argmin(successor_state_unsafe_prob_manipulated)
                    #    action = self.action_manipulated[index]
                self.last_teacher_crash = 1
                self.Intervention += 1
                self.Supervised = 1
                if not self.scale_reward:
                    reward -= self.reward_cbf

        # step env
        observation, reward_unity, done, info = self.env.step(action)
        # manipulate reward with distance to target. maximum distance is ~5.
        reward -= (observation[0] / 5) * self.reward_distance
        # save org ibm obs
        org_observation = observation
        # check if safely done
        if done:
            self.SafelyDone += 1
            reward += self.reward_reached_target
        # compute distances to obstacles
        distance = distance_from_obstacles(self, observation)
        # check if drone crashed
        if not done:
            if np.min(distance) <= .2:
                self.Crashed += 1
                reward -= self.reward_crashed
                done = True
        # check if logical supervisor is active
        if not done:
            if self.supervisor == 'logical':
                if not self.record_svm_gp:
                    if np.min(distance) <= (self.safety_distance + np.random.rand(1) / 25):
                        self.Supervised += 1
                        self.Intervention += 1
                        done = True
                        reward -= self.reward_logical
                # the following is used when recording data
                if self.record_svm_gp:
                    if np.min(distance) <= 0.29 + np.random.rand(1) / 25:
                        self.Supervised += 1
                        self.Intervention += 1
                        done = True
                        reward -= self.reward_logical
                    else:
                        if np.min(distance) <= 0.35 + np.random.rand(1) / 25:
                            self.Supervised += 1
        # append reward
        self.rewards.append(reward)
        # and didnt end safely
        if len(self.rewards) == 120:
            if not done:
                done = True
                self.timeOut += 1
        # transform observation to lidar like observation
        observation = obs_lidar_pseudo(self, observation, lidar_num_bins=self.lidar_num_bins, lidar_max_dist=None,
                                       lidar_alias=True, lidar_exp_gain=1.0, distance=distance)
        # append supervisor and save current observation to the storage
        # first self.lidar_num_bins entries of the observation storage correspond to the lidar
        # the next entry is the supervised indicator
        # the next entry is the done indicator
        # the last 23 entries correspond to the original oracle observation by IBM
        observation_storage = np.append(observation, self.Supervised)
        observation_storage = np.append(observation_storage, self.which_static)
        observation_storage = np.append(observation_storage, self.Intervention)
        observation_storage = np.append(observation_storage, self.Crashed)
        observation_storage = np.append(observation_storage, done)
        observation_storage = np.append(observation_storage, action)
        observation_storage = np.append(observation_storage, org_observation)
        self.observation_storage.append(observation_storage)

        # append the observations of time step t-1 and t-2 to the observation of time step t
        # when resetting the env the observations of time step t-1 and t-2 are np.zeros()
        # in time step 1 the observation of time step t-2 are np.zeros()
        if len(self.rewards) == 1:
            observation = np.concatenate((observation, self.observation_storage[0][0:(self.lidar_num_bins + 8)]),
                                         axis=0)
            observation_dummy = np.zeros(self.lidar_num_bins + 8)
            observation = np.concatenate((observation, observation_dummy), axis=0)

        if len(self.rewards) > 1:
            observation = np.concatenate((observation, self.observation_storage[len(self.rewards) - 2
                                                                                ][0:(self.lidar_num_bins + 8)]), axis=0)
            observation = np.concatenate((observation, self.observation_storage[len(self.rewards) - 1
                                                                                ][0:(self.lidar_num_bins + 8)]), axis=0)

        if done:
            self.needs_reset = True
            ep_rew = sum(self.rewards)
            eplen = len(self.rewards)
            if self.Crashed is True and self.last_teacher_crash == 1:
                teacher_failed = 1
            else:
                teacher_failed = 0
            ep_info = [round(ep_rew, 6), eplen, self.Crashed, self.Supervised, self.Intervention, self.SafelyDone,
                       self.timeOut, round(time.time(), 6), teacher_failed, self.total_distance]
            self.episode_rewards.append(ep_rew)
            self.episode_lengths.append(eplen)
            self.episode_times.append(time.time() - self.t_start)
            info['episode'] = ep_info
            if self.safe_states:  # and self.Crashed > 0:####################################################
                with open(self.filename + 'states.csv', 'a', newline='') as f:
                    writer = csv.writer(f)
                    with self.csv_writer_lock:
                        writer.writerows(self.observation_storage)
            if self.safe_info:
                with open(self.filename + 'monitor.csv', 'a', newline='') as f:
                    writer = csv.writer(f)
                    with self.csv_writer_lock:
                        writer.writerow(ep_info)
            self.TotalCollisions.append(self.Supervised)
            self.Supervised = 0
            self.SafelyDone = 0
            self.Intervention = 0
            self.Crashed = 0
            self.total_distance = 0
            self.timeOut = 0
            self.rewards = []
            self.observation_storage = []
        if not done:
            if self.supervisor == 'logical' and self.record_svm_gp:
                self.Supervised = 0
            self.last_teacher_crash = 0
        self.total_steps += 1
        return observation, reward, done, info

    def reset(self, **kwargs):
        observation = self.env.reset(**kwargs)
        org_observation = observation
        distance = distance_from_obstacles(self, observation)
        while np.min(distance) < .35:
            observation = self.env.reset(**kwargs)
            org_observation = observation
            distance = distance_from_obstacles(self, observation)
        if np.min(distance) < .35:
            self.needs_reset = True
        if np.min(distance) >= .35:
            self.needs_reset = False
        self.rewards = []
        observation = obs_lidar_pseudo(self, observation, lidar_num_bins=self.lidar_num_bins, lidar_max_dist=None,
                                       lidar_alias=True, lidar_exp_gain=1.0, distance=distance)
        # append supervisor and save current observation to the storage
        supervised = False
        observation_storage = np.append(observation, supervised)
        observation_storage = np.append(observation_storage, self.which_static)
        intervention = np.array([0])
        observation_storage = np.append(observation_storage, intervention)
        crash_dummy = np.array([0])
        observation_storage = np.append(observation_storage, crash_dummy)
        done = False
        observation_storage = np.append(observation_storage, done)
        action_dummy = np.array([0, 0])
        observation_storage = np.append(observation_storage, action_dummy)
        observation_storage = np.append(observation_storage, org_observation)
        self.observation_storage.append(observation_storage)
        observation_dummy = np.zeros(self.lidar_num_bins + 8)
        observation = np.concatenate((observation, observation_dummy), axis=0)
        observation = np.concatenate((observation, observation_dummy), axis=0)
        return observation

# load extern modules # from qpsolvers import solve_qp A monitor wrapper for Gym environments to save collisions events      Parameters:         env (gym.Env): The environment         filename (Optional[str]): the location to save a log file, can be None for no log ############### # reward shaping # default reward settings # add extra static obstacles as boundary of the garden # add extra obstacles as tress in the middle of the garden Get information for the next RL step          Parameters:             action (float, float): Action vector (speed)          Returns:             [float]: Observation vector             float: reward value             observation, reward, done, info # check if env needs reset # pass proposed action to the CBF (if CBF is active) and return the manipulated or the proposed action # print(unsafe_probability) # hier nur svm predicten #if self.supervisor == 'cbf-svm': #    successor_state_unsafe_prob_manipulated = [] #    for j in range(0, len(self.action_manipulated)): #        state_action = np.concatenate((old_observation[0:self.lidar_num_bins], #                                       self.action_manipulated[j]), axis=0) #        state_action = state_action.reshape(1, -1) #        probability = self.svm.predict_proba(state_action) #        unsafe = probability[0, 1] #        successor_state_unsafe_prob_manipulated.append(unsafe) #    index = np.argmin(successor_state_unsafe_prob_manipulated) #    action = self.action_manipulated[index] # step env # manipulate reward with distance to target. maximum distance is ~5. # save org ibm obs # check if safely done # compute distances to obstacles # check if drone crashed # check if logical supervisor is active # the following is used when recording data # append reward # and didnt end safely # transform observation to lidar like observation # append supervisor and save current observation to the storage # first self.lidar_num_bins entries of the observation storage correspond to the lidar # the next entry is the supervised indicator # the next entry is the done indicator # the last 23 entries correspond to the original oracle observation by IBM # append the observations of time step t-1 and t-2 to the observation of time step t # when resetting the env the observations of time step t-1 and t-2 are np.zeros() # in time step 1 the observation of time step t-2 are np.zeros() # and self.Crashed > 0:#################################################### # append supervisor and save current observation to the storage