"""
This file implements the functionalities of a Mors Gym environment using pybullet physical engine.
"""
import copy
import numpy as np
import os
import inspect
currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
parentdir = os.path.dirname(os.path.dirname(currentdir))
os.sys.path.insert(0, parentdir)

import time
import random
import gym
from gym import spaces
from gym.utils import seeding
import pybullet
from pybullet_utils import bullet_client as bc
import pybullet_data
from pkg_resources import parse_version
from mors_sim.motor_simple import MotorSimple
from mors_sim.motor_accurate import MotorAccurate
from mors_sim.world_creator import WorldCreator
from mors_sim.lidar import Lidar
from mors_sim.camera import Camera

import math

INIT_POSITION = [0, 0, .76]
INIT_ORIENTATION = [0, 0, 0, 1]
LEG_POSITION = ["R1", "L1", "R2", "L2"]
MOTOR_NAMES = [
    "abad_joint_R1", "hip_joint_R1", "knee_joint_R1",
    "abad_joint_L1", "hip_joint_L1", "knee_joint_L1",
    "abad_joint_R2", "hip_joint_R2", "knee_joint_R2",
    "abad_joint_L2", "hip_joint_L2", "knee_joint_L2",
]
LEG_LINK_ID = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15]
MOTOR_LINK_ID = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11] # shoulders: 0, 3, 6, 9
SHOULDER_MOTOR_ID = [0, 3, 6, 9]
FOOT_LINK_ID = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15] # it is needed for friction setting
BASE_LINK_ID = -1

NUM_SUBSTEPS = 5
NUM_MOTORS = 12
MOTOR_ANGLE_OBSERVATION_INDEX = 0
MOTOR_VELOCITY_OBSERVATION_INDEX = MOTOR_ANGLE_OBSERVATION_INDEX + NUM_MOTORS
MOTOR_TORQUE_OBSERVATION_INDEX = MOTOR_VELOCITY_OBSERVATION_INDEX + NUM_MOTORS
BASE_ORIENTATION_OBSERVATION_INDEX = MOTOR_TORQUE_OBSERVATION_INDEX + NUM_MOTORS
ACTION_EPS = 0.01
OBSERVATION_EPS = 0.01
RENDER_HEIGHT = 720
RENDER_WIDTH = 960
K_SCALE = 2

MOTOR_ANGLE_MAX = 2.62
MOTOR_VELOCITY_MAX = 12
MOTOR_TORQUE_MAX = 110
BASE_ORIENTATION_MAX = 1.57

# LIDAR_JOINT = 27

class MorsMiniBulletEnv(gym.Env):
  """
  The gym environment for Mors.

  It simulates the locomotion of a mors, a quadruped robot. The state space
  include base orientation, the angles, velocities and torques for all the motors and the action
  space is the desired motor angle for each motor. The reward function is based
  on how fast robot can move without twitches and side deviations.

  """
  metadata = {"render.modes": ["human", "rgb_array"], "video.frames_per_second": 50}

  def __init__(self,
              urdf_root="./urdf",
              sim_freq=100,
              world="empty",
              hard_reset=False,
              render=True,
              on_rack=False,
              self_collision_enabled=False,
              debug_mode = False,
              floating_camera = False,
              step_enabled = True,

              accurate_motor_model_enabled=False,
              simple_motor_model_enabled=False,
              torque_control_enabled=False,
              motor_kp=30.0,
              motor_kd=0.1,
              motor_velocity_limit=np.inf,

              max_timesteps = np.inf,
              action_repeat=1,
              rew_scale = 1,
              distance_limit=float("inf"),
              observation_noise_stdev=0.0,
              normalization=True,

              camera_enabled=False,
              lidar_enabled=False,
              ext_disturbance_enabled=False,

              ):

    """Initialize the mors gym environment.

    Args:
      urdf_root: The path to the urdf data folder relative to parent folder.
      sim_freq: The value that used to simulate environment.
      hard_reset: Whether to wipe the simulation and load everything when reset
        is called. If set to false, reset just place the mors back to start
        position and set its pose to initial configuration.
      render: Whether to render the simulation.
      on_rack: Whether to place the mors on rack. This is only used to debug
        walking gaits. In this mode, the mors's base is hanged midair so
        that its walking gait is clearer to visualize.
      self_collision_enabled: Whether to enable self collision in the sim.
      max_timesteps: Time limit during which the simulation is going on.
      debug_mode: Whether to show side panel with debug parameters or not.
      floating_camera: If True, camera follows the robot.
      step_enabled: If True, it is needed to send motor commands as arguments in
        step() function. If False, it is neede to use set_motor_commands() function.
      accurate_motor_model_enabled: Whether to use the accurate BLDC motor model.
      simple_motor_model_enabled: Whether to use the simple version of BLDC motor model.
      torque_control_enabled: Whether to use the torque control, if set to
        False, pose control will be used.
      motor_kp: proportional gain for the accurate and simple motor models.
      motor_kd: derivative gain for the accurate and simple motor models.
      motor_velocity_limit: The velocity limit of each motor.

      action_repeat: The number of simulation steps before actions are applied.
      rew_scale: How much to gain reward. Sometimes it helps to learn a model.
      distance_limit: The maximum distance to terminate the episode.
      observation_noise_stdev: The standard deviation of observation noise.
      normalization: Whether to normalize observations or not.

          """
    self._time_step = 1/sim_freq
    self._num_bullet_solver_iterations = sim_freq*NUM_SUBSTEPS
    self._action_repeat = action_repeat
    self._urdf_root = urdf_root
    self._self_collision_enabled = self_collision_enabled
    self._motor_velocity_limit = motor_velocity_limit
    self._is_render = render
    self._world_name = world
    self._step_enabled = step_enabled
    self._floating_camera = floating_camera
    self._distance_limit = distance_limit
    self._observation_noise_stdev = observation_noise_stdev
    self._accurate_motor_model_enabled = accurate_motor_model_enabled
    self._simple_motor_model_enabled = simple_motor_model_enabled
    self._motor_kp = motor_kp
    self._motor_kd = motor_kd
    self._torque_control_enabled = torque_control_enabled
    self._on_rack = on_rack
    self._max_timesteps = max_timesteps
    self._rew_scale = rew_scale
    self._hard_reset = True
    self._debug_mode = debug_mode
    self._normalization = normalization
    self._ext_disturbance_enabled = ext_disturbance_enabled
    self._lidar_enabled = lidar_enabled
    self._camera_enabled = camera_enabled
    self.force = 136

    self._lateral_friction = 1.0
    self._spinning_friction = 0.0063

    self._ext_force_magn = 1200
    self._ext_force_duration = 6
    self._ext_force_interval = 2000
    self._force_dir = 1

    self._env_step_counter = 0
    self._action_bound = 1

    self._cam_dist = 1.0
    self._cam_yaw = 0
    self._cam_pitch = -30
    self._last_frame_time = 0.0

    self.num_legs = 4
    self.num_motors = int(self.num_legs * 3)
    self._motor_direction = [1]*12
    self._observed_motor_torques = np.zeros(self.num_motors)
    self._applied_motor_torques = np.zeros(self.num_motors)
    self._max_force = 3.5

    self.motor_angles = np.array([0]*12)
    self.motor_velocities = np.array([0]*12)
    self.motor_torques = np.array([0]*12)
    self.base_orientation = np.array([0]*3)

    # reward terms
    self.k_vel_x = 48
    self.k_body = 2
    self.k_vel_y = 3
    self.k_smooth = 0.2
    self.k_torq = 0.0003

    # motor models need smaller time step for stability.
    if self._simple_motor_model_enabled or self._accurate_motor_model_enabled:
      self._time_step /= NUM_SUBSTEPS
      self._num_bullet_solver_iterations /= NUM_SUBSTEPS
      self._action_repeat *= NUM_SUBSTEPS

    # create motor model objects
    if self._accurate_motor_model_enabled:
      self._accurate_motor_model = MotorAccurate(NUM_MOTORS)
      if self._torque_control_enabled:
        self._accurate_motor_model.set_kp(0)
        self._accurate_motor_model.set_kd(self._motor_kd)
      else:
        self._accurate_motor_model.set_kp(self._motor_kp)
        self._accurate_motor_model.set_kd(self._motor_kd)
    elif self._simple_motor_model_enabled:
      self._simple_motor_model = MotorSimple(NUM_MOTORS)
      if self._torque_control_enabled:
        self._simple_motor_model.set_kp(0)
        self._simple_motor_model.set_kd(self._motor_kd)
      else:
        self._simple_motor_model.set_kp(self._motor_kp)
        self._simple_motor_model.set_kd(self._motor_kd)

    if self._is_render:
      self._pybullet_client = bc.BulletClient(connection_mode=pybullet.GUI)
    else:
      self._pybullet_client = bc.BulletClient()
    self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())

    self.action_dim = NUM_MOTORS
    action_high = np.array([self._action_bound] * self.action_dim)
    self.action_space = spaces.Box(-np.float32(action_high), np.float32(action_high), dtype=np.float32)
    self.action = np.array([0]*self.action_dim, dtype=np.float32)
    self.action_prev = np.array([0]*self.action_dim, dtype=np.float32)
    self.action_space = spaces.Box(low=np.float32(-1), high=np.float32(1), shape=(self.action_dim,), dtype=np.float32)

    self.obs_dim = 39
    self.obs = np.array([0]*self.obs_dim, dtype=np.float32)
    observation_high = (self.get_observation_upper_bound())
    observation_low = (self.get_observation_lower_bound())
    self.observation_space = spaces.Box(np.float32(observation_low), np.float32(observation_high), dtype=np.float32)

    self.world_creator = WorldCreator(self._pybullet_client, spinning_friction=self._spinning_friction, lateral_friction=self._lateral_friction)

    self.set_friction()
    self.seed()
    self.reset()

    if self._lidar_enabled == True:
      self.lidar =  Lidar(pybullet_client=self._pybullet_client,
                          robot_id=self.quadruped,
                          joint_id=18,
                          frequency=20,
                          base_frequency=sim_freq,
                          render=True,

                          angle_min=np.radians(130),
                          angle_max=-np.radians(130),
                          point_num=360,
                          range_min=0.25,
                          range_max=8.0,
                          )

    if self._camera_enabled == True:
      self.camera = Camera(pybullet_client=self._pybullet_client,
                           robot_id=self.quadruped,
                           joint_id=17,
                           frequency=20,
                           base_frequency=sim_freq)

    self._hard_reset = hard_reset # this needs to be after reset

    self.viewer = None

    self.fallen = 0

    # debug parameters
    self.view = True
    if self._debug_mode == True and self._is_render == True:
      self.reset_id = self._pybullet_client.addUserDebugParameter("Reset",1,0,1)
      self.view_id = self._pybullet_client.addUserDebugParameter("View",1,0,1)
      self.reset_clicked_prev = self._pybullet_client.readUserDebugParameter(self.reset_id)
      self.reset_clicked = self._pybullet_client.readUserDebugParameter(self.reset_id)
      self.view_clicked_prev = self._pybullet_client.readUserDebugParameter(self.view_id)
      self.view_clicked = self._pybullet_client.readUserDebugParameter(self.view_id)

      self.distance_id = self._pybullet_client.addUserDebugParameter("Distance",0.3, 2, 1.2)
      self.yaw_id = self._pybullet_client.addUserDebugParameter("Yaw",-180, 180, 47)
      self.pitch_id = self._pybullet_client.addUserDebugParameter("Pitch",-180, 180, -20)

      self.sl_id = self._pybullet_client.addUserDebugParameter("Step Length", 0, 0.2, 0)
      self.sh_id = self._pybullet_client.addUserDebugParameter("Step Height", 0, 0.1, 0)
      self.omega_id = self._pybullet_client.addUserDebugParameter("Step Frequency", 0, 4, 2.9)
      self.psi_id = self._pybullet_client.addUserDebugParameter("Sidewalk Angle", -3.14, 3.14, 0)
      self.rot_dir_id = self._pybullet_client.addUserDebugParameter("Turning Direction", -1.0, 1.0, 0.0)
      self.rot_r_id = self._pybullet_client.addUserDebugParameter("Turning Radius", 0, 10, 5)

      self.body_tr_x_id = self._pybullet_client.addUserDebugParameter("Body Translation X", -0.0003, 0.0003, 0)
      self.body_tr_y_id = self._pybullet_client.addUserDebugParameter("Body Translation Y", -0.0003, 0.0003, 0)
      self.body_tr_z_id = self._pybullet_client.addUserDebugParameter("Body Translation Z", -0.0003, 0.0003, 0)

      self.body_rot_x_id = self._pybullet_client.addUserDebugParameter("Body Rotation X", -0.5, 0.5, 0)
      self.body_rot_y_id = self._pybullet_client.addUserDebugParameter("Body Rotation Y", -0.4, 0.4, 0)
      self.body_rot_z_id = self._pybullet_client.addUserDebugParameter("Body Rotation Z", -0.4, 0.4, 0)

    self.first = True

  def reset(self):
    """
    Resets all Mors parameters.
    If hard_reset is True, it also resets simulation.
    """
    if self._hard_reset:
      self._pybullet_client.resetSimulation()
      self._pybullet_client.setPhysicsEngineParameter(
          numSolverIterations=int(self._num_bullet_solver_iterations))
      self._pybullet_client.setTimeStep(self._time_step)

      # plane = self._pybullet_client.loadURDF("%s/plane.urdf" % pybullet_data.getDataPath())
      # self._pybullet_client.changeVisualShape(plane, -1, rgbaColor=[1, 1, 1, 0.9])
      # self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_PLANAR_REFLECTION, 0)
      self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_SHADOWS, 0)
      self._pybullet_client.setGravity(0, 0, -9.81)
      # self.set_world(self._world_name)
      self.world_creator.create_world(self._world_name, lateralFriction=self._lateral_friction, spinningFriction=self._spinning_friction)
      self._reset_robot(reload_urdf=True)
    else:
      self._reset_robot(reload_urdf=False)

    self._env_step_counter = 0
    # self._last_base_position = np.array([0, 0, 0])
    # self._objectives = []
    self._pybullet_client.resetDebugVisualizerCamera(self._cam_dist, self._cam_yaw,
                                                     self._cam_pitch, [0, 0, 0])

    self.obs = self._noisy_observation()
    self.obs = self.norm_obs(self.obs)
    return self.obs

  def seed(self, seed=None):
    self.np_random, seed = seeding.np_random(seed)
    return [seed]

  def step(self, action):
    """Step forward the simulation, given the action.

    Args:
      action: A list of desired motor angles for eight motors.

    Returns:
      observations: The angles, velocities and torques of all motors.
      reward: The reward for the current state-action pair.
      done: Whether the episode has ended.
      info: A dictionary that stores diagnostic information.

    Raises:
      ValueError: The action dimension is not the same as the number of motors.
      ValueError: The magnitude of actions is out of bounds.
    """
    # debug parameters

    if self._is_render:
      if self._debug_mode == True:
        # reset button
        self.reset_clicked = self._pybullet_client.readUserDebugParameter(self.reset_id)
        if self.reset_clicked != self.reset_clicked_prev:
            self._reset_robot(reload_urdf=False)
        self.reset_clicked_prev = self.reset_clicked
        # view button
        self.view_clicked = self._pybullet_client.readUserDebugParameter(self.view_id)
        if self.view_clicked != self.view_clicked_prev:
            self.view = not self.view
        self.view_clicked_prev = self.view_clicked
        # camera position
        distance = self._pybullet_client.readUserDebugParameter(self.distance_id)
        pitch = self._pybullet_client.readUserDebugParameter(self.pitch_id)
        yaw = self._pybullet_client.readUserDebugParameter(self.yaw_id)
      else:
        distance = 0.5
        yaw = -10
        pitch = -20
      # Sleep, otherwise the computation takes less time than real time,
      # which will make the visualization like a fast-forward video.
      time_spent = time.time() - self._last_frame_time
      self._last_frame_time = time.time()
      time_to_sleep = self._action_repeat * self._time_step - time_spent
      if time_to_sleep > 0:
        time.sleep(time_to_sleep)
      base_pos, _ = self.get_base_position_and_orientation()
      if self.view == True and self._floating_camera == False:
        self._pybullet_client.resetDebugVisualizerCamera(distance, yaw, pitch, base_pos)

    if self._ext_disturbance_enabled:
      self._apply_force()

    if self._lidar_enabled:
      self.lidar.update(self._env_step_counter)
    # if self._camera_enabled == True:
    #   self.camera.update(self._env_step_counter)

    self.action = action
    for _ in range(self._action_repeat):
      self._apply_action(self.action)
      self._pybullet_client.stepSimulation()

    self._env_step_counter += 1
    reward = self._rew_scale*self._reward()
    done = self._termination()
    self.obs = self._noisy_observation()
    if self._normalization == True:
      self.obs = self.norm_obs(self.obs)
    self.action_prev[:] = self.action[:]
    return self.obs, reward, done, {}

  def render(self, mode="rgb_array", close=False):
    if mode != "rgb_array":
      return np.array([])
    base_pos, _ = self.get_base_position_and_orientation()
    view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
        cameraTargetPosition=base_pos,
        distance=self._cam_dist,
        yaw=self._cam_yaw,
        pitch=self._cam_pitch,
        roll=0,
        upAxisIndex=2)
    proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(fov=60,
                                                                   aspect=float(RENDER_WIDTH) /
                                                                   RENDER_HEIGHT,
                                                                   nearVal=0.1,
                                                                   farVal=100.0)
    (_, _, px, _,
     _) = self._pybullet_client.getCameraImage(width=RENDER_WIDTH,
                                               height=RENDER_HEIGHT,
                                               viewMatrix=view_matrix,
                                               projectionMatrix=proj_matrix,
                                               renderer=pybullet.ER_BULLET_HARDWARE_OPENGL)
    rgb_array = np.array(px)
    rgb_array = rgb_array[:, :, :3]
    return rgb_array

  def close(self):
    """
    Close the environment and disconnect physical engine.
    It is recommended to use this function at the end of your program.
    """
    super().close()
    self._pybullet_client.disconnect()

  def is_fallen(self):
    """Decide whether the mors has fallen.

    If the up directions between the base and the world is larger (the dot
    product is smaller than 0.85) or the base is very low on the ground
    (the height is smaller than 0.13 meter), the mors is considered fallen.

    Returns:
      Boolean value that indicates whether the mors has fallen.
    """
    orientation = self.get_base_orientation()
    rot_mat = self._pybullet_client.getMatrixFromQuaternion(orientation)
    local_up = rot_mat[6:]
    pos, _ = self.get_base_position_and_orientation()
    return (np.dot(np.asarray([0, 0, 1]), np.asarray(local_up)) < 0.5 or pos[2] < 0.05)

  def _termination(self):
    position, _ = self.get_base_position_and_orientation()
    distance = np.sqrt(position[0]**2 + position[1]**2)
    if self.is_fallen():
      self.fallen = 1
    else:
      self.fallen = 0
    if self._env_step_counter >= self._max_timesteps:
      self.timeout = 1
    else:
      self.timeout = 0
    return bool(self.timeout or self.fallen or (distance > self._distance_limit))

  def _reward(self):
    """
    Calculate reward function
    Returns:
      reward value during current timestep
    """
    orient = np.abs(self.get_base_orientation())
    ang_vel = self.get_base_ang_vel()
    lin_vel = self.get_base_lin_vel()
    d_theta_ref_offset = self.action[0:12] - self.action_prev[0:12]

    self.vel_x_reward = lin_vel[0]**2
    self.body_reward = np.exp(-5*np.abs(ang_vel[0])) + np.exp(-5*np.abs(ang_vel[1])) + np.exp(-40*orient[0]) + np.exp(-40*orient[1])
    self.vel_y_reward = np.exp(-(lin_vel[1])**2/0.005)
    self.smooth_reward = np.exp(-np.linalg.norm(self.action[0:12], 1)) + np.exp(-np.linalg.norm(d_theta_ref_offset, 1))
    self.torque_penalty = np.linalg.norm(self.get_motor_torques()[:], 1)**2

    reward = (self.k_vel_x*self.vel_x_reward
              + self.k_vel_y *self.vel_y_reward
              + self.k_body*self.body_reward
              + self.k_smooth*self.smooth_reward
              - self.k_torq*self.torque_penalty)
    return reward

  def set_reward_terms(self, k_vel_x=48, k_vel_y=3, k_body=2, k_smooth=0.2, k_torq=0.0003):
    self.k_vel_x = k_vel_x
    self.k_vel_y = k_vel_y
    self.k_body = k_body
    self.k_smooth = k_smooth
    self.k_torq = k_torq

  def get_reward_parts(self):
    return [self.k_vel_x*self.vel_x_reward , self.k_vel_y *self.vel_y_reward,
            self.k_body*self.body_reward, self.k_smooth*self.smooth_reward,
            -self.k_torq*self.torque_penalty]

  def _noisy_observation(self):
    observation = self._get_observation()
    obs = np.array(observation, dtype=np.float32)
    if self._observation_noise_stdev > 0:
      obs += np.random.normal(scale=self._observation_noise_stdev, size=obs.shape)
    return obs

  def _get_observation(self):
    """Get the observations of mors.

    It includes the angles, velocities, torques and the orientation of the base.

    Returns:
      The observation list
      observation[0:11] are motor angles
      observation[12:23] are motor velocities
      observation[24:35] are motor torques
      observation[35:38] is the orientation of the base, in Euler form.
    """
    observation = []
    self.motor_angles = self.get_motor_angles().tolist()
    self.motor_velocities = self.get_motor_velocities().tolist()
    self.motor_torques = self.get_motor_torques().tolist()
    self.base_orientation = list(self.get_base_orientation_euler())
    observation.extend(self.motor_angles)
    observation.extend(self.motor_velocities)
    observation.extend(self.motor_torques)
    observation.extend(self.base_orientation)
    return observation

  def norm_obs(self, obs):
    norm_obs = np.array([0]*self.obs_dim, dtype=np.float32)
    for i in range(0,12):
        norm_obs[i] = (obs[i]+MOTOR_ANGLE_MAX)/(MOTOR_ANGLE_MAX+MOTOR_ANGLE_MAX)
    for i in range(12,24):
        norm_obs[i] = np.clip((obs[i]+MOTOR_VELOCITY_MAX)/(MOTOR_VELOCITY_MAX+MOTOR_VELOCITY_MAX), 0, 1)
    for i in range(24, 36):
        norm_obs[i] = np.clip((obs[i]+MOTOR_TORQUE_MAX)/(MOTOR_TORQUE_MAX+MOTOR_TORQUE_MAX), 0, 1)
    norm_obs[36] = (obs[36]+BASE_ORIENTATION_MAX)/(BASE_ORIENTATION_MAX+BASE_ORIENTATION_MAX)
    norm_obs[37] = (obs[37]+BASE_ORIENTATION_MAX)/(BASE_ORIENTATION_MAX+BASE_ORIENTATION_MAX)
    norm_obs[38] = (obs[38]+BASE_ORIENTATION_MAX)/(BASE_ORIENTATION_MAX+BASE_ORIENTATION_MAX)

    return norm_obs

  def _denorm_obs(self, norm_obs):
    obs = np.array([0]*self.obs_dim, dtype=np.float32)
    for i in range(0,12):
        obs[i] = 2*norm_obs[i]*MOTOR_ANGLE_MAX - MOTOR_ANGLE_MAX
    for i in range(12,24):
        obs[i] = 2*norm_obs[i]*MOTOR_VELOCITY_MAX - MOTOR_VELOCITY_MAX
    for i in range(24, 36):
        obs[i] = 2*norm_obs[i]*MOTOR_TORQUE_MAX - MOTOR_TORQUE_MAX
    obs[36] = 2*BASE_ORIENTATION_MAX*norm_obs[36] - BASE_ORIENTATION_MAX
    obs[37] = 2*BASE_ORIENTATION_MAX*norm_obs[37] - BASE_ORIENTATION_MAX
    obs[38] = 2*BASE_ORIENTATION_MAX*norm_obs[38] - BASE_ORIENTATION_MAX

    return obs

  def set_kpkd(self, kp : float, kd : float):
    self._motor_kp = kp
    self._motor_kd = kd

  def set_motor_commands(self, angles, vels, torques):
    self.ref_angles = angles
    self.ref_vels = vels
    self.ref_torques = torques

  def _apply_action(self, motor_commands):
    """Set the desired motor commands to the motors of the mors.

    The desired motor commands are clipped based on the maximum allowed velocity.
    If the pd_control_enabled is True, a torque is calculated according to
    the difference between current and desired joint angle, as well as the joint
    velocity. This torque is exerted to the motor. For more information about
    PD control, please refer to: https://en.wikipedia.org/wiki/PID_controller.

    Args:
      motor_commands: The twelve desired motor angles or torques depending on torque_control_enabled variable.
    """
    current_motor_angle = self.get_motor_angles()
    if self._motor_velocity_limit < np.inf:

      motor_commands_max = (current_motor_angle + self._time_step * self._motor_velocity_limit)
      motor_commands_min = (current_motor_angle - self._time_step * self._motor_velocity_limit)
      motor_commands = np.clip(motor_commands, motor_commands_min, motor_commands_max)

    motor_commands_with_direction = np.multiply(motor_commands, self._motor_direction)
    self.motor_angles = current_motor_angle[:].tolist()
    self.motor_velocities = self.get_motor_velocities().tolist()

    if self._accurate_motor_model_enabled:
      self._accurate_motor_model.set_sensor_data(self.motor_angles, self.motor_velocities)

      if self._step_enabled == True:
        if self._torque_control_enabled:
          self._accurate_motor_model.set_ref_torque(motor_commands_with_direction)
          self._accurate_motor_model.set_kd(self._motor_kd)
        else:
          self._accurate_motor_model.set_ref_angle(motor_commands_with_direction)
          self._accurate_motor_model.set_kp(self._motor_kp)
          self._accurate_motor_model.set_kd(self._motor_kd)

      else:
        self._accurate_motor_model.set_ref_angle(self.ref_angles)
        self._accurate_motor_model.set_ref_vel(self.ref_vels)
        self._accurate_motor_model.set_ref_torque(self.ref_torques)
        self._accurate_motor_model.set_kp(self._motor_kp)
        self._accurate_motor_model.set_kd(self._motor_kd)

      self.tau = self._accurate_motor_model.step()

      for motor_id, tau in zip(self._motor_id_list, self.tau):
        self._SetMotorTorqueById(motor_id, float(tau))

    elif self._simple_motor_model_enabled:
        self._simple_motor_model.set_sensor_data(self.motor_angles, self.motor_velocities)

        if self._step_enabled == True:
          if self._torque_control_enabled:
            self._simple_motor_model.set_ref_torque(motor_commands_with_direction)
            self._simple_motor_model.set_kd(self._motor_kd)
          else:
            self._simple_motor_model.set_ref_angle(motor_commands_with_direction)
            self._simple_motor_model.set_kp(self._motor_kp)
            self._simple_motor_model.set_kd(self._motor_kd)

        else:
          self._simple_motor_model.set_ref_angle(self.ref_angles)
          self._simple_motor_model.set_ref_vel(self.ref_vels)
          self._simple_motor_model.set_ref_torque(self.ref_torques)
          self._simple_motor_model.set_kp(self._motor_kp)
          self._simple_motor_model.set_kd(self._motor_kd)

        self.tau = self._simple_motor_model.step()

        for motor_id, tau in zip(self._motor_id_list, self.tau):
          self._SetMotorTorqueById(motor_id, float(tau))
    else:

      if self._torque_control_enabled == False:
        if self._step_enabled == True:
          for motor_id, motor_command_with_direction in zip(self._motor_id_list, motor_commands_with_direction):
            self._SetMotorAngleById(motor_id, motor_command_with_direction)
        else:
          for motor_id, motor_command_with_direction in zip(self._motor_id_list, self.ref_angles):
            self._SetMotorAngleById(motor_id, motor_command_with_direction)

      else:
        if self._step_enabled == True:
          for motor_id, motor_command_with_direction in zip(self._motor_id_list, motor_commands_with_direction):
            self._SetMotorTorqueById(motor_id, float(motor_command_with_direction))
        else:
          for motor_id, motor_command_with_direction in zip(self._motor_id_list, self.ref_torques):
            self._SetMotorTorqueById(motor_id, float(motor_command_with_direction))

  def _RecordMassInfoFromURDF(self):
    self._base_mass_urdf = self._pybullet_client.getDynamicsInfo(self.quadruped, BASE_LINK_ID)[0]
    self._leg_masses_urdf = []
    self._leg_masses_urdf.append(
        self._pybullet_client.getDynamicsInfo(self.quadruped, LEG_LINK_ID[0])[0])
    self._leg_masses_urdf.append(
        self._pybullet_client.getDynamicsInfo(self.quadruped, MOTOR_LINK_ID[0])[0])

  def _BuildJointNameToIdDict(self):
    num_joints = self._pybullet_client.getNumJoints(self.quadruped)
    self._joint_name_to_id = {}
    for i in range(num_joints):
      joint_info = self._pybullet_client.getJointInfo(self.quadruped, i)
      self._joint_name_to_id[joint_info[1].decode("UTF-8")] = joint_info[0]

  def _BuildMotorIdList(self):
    self._motor_id_list = [self._joint_name_to_id[motor_name] for motor_name in MOTOR_NAMES]

  def _SetMotorTorqueById(self, motor_id, torque : float):
    self._pybullet_client.setJointMotorControl2(bodyIndex=self.quadruped,
                                                jointIndex=motor_id,
                                                controlMode=pybullet.TORQUE_CONTROL,
                                                force=torque)

  def _SetMotorVelocityById(self, motor_id, desired_velocity):
    self._pybullet_client.setJointMotorControl2(bodyIndex=self.quadruped,
                                                jointIndex=motor_id,
                                                controlMode=self._pybullet_client.VELOCITY_CONTROL,
                                                targetVelocity=desired_velocity,
                                                #positionGain=self._kp,
                                                #velocityGain=self._kd,
                                                maxVelocity=self._motor_velocity_limit,
                                                force=self.force
                                                )

  def _SetMotorAngleById(self, motor_id, desired_angle):
    self._pybullet_client.setJointMotorControl2(bodyIndex=self.quadruped,
                                                jointIndex=motor_id,
                                                controlMode=self._pybullet_client.POSITION_CONTROL,
                                                targetPosition=desired_angle,
                                                #positionGain=self._kp,
                                                #velocityGain=self._kd,
                                                maxVelocity=self._motor_velocity_limit,
                                                force=self.force#self._max_force
                                                )

  def _SetMotorAngleByName(self, motor_name, desired_angle):
    self._SetMotorAngleById(self._joint_name_to_id[motor_name], desired_angle)

  def _ResetPose(self):
    """Reset the pose of the mors.
      constraint: Whether to add a constraint at the joints of two feet.
    """
    for i in range(self.num_legs):
      self._ResetPoseForLeg(i)

    for j in range(16):
            self._pybullet_client.setJointMotorControl2(self.quadruped, j, pybullet.VELOCITY_CONTROL, targetVelocity=0, force=0)

  def _ResetPoseForLeg(self, leg_id):
    """Reset the initial pose for the leg.

    Args:
      leg_id: It should be 0, 1, 2, or 3, which represents the leg at
        front_left, back_left, front_right and back_right.
    """
    knee_angle = 0
    shoulder_angle = 0
    hip_angle = 0

    leg_position = LEG_POSITION[leg_id]
    self._pybullet_client.resetJointState(self.quadruped,
                                          self._joint_name_to_id["abad_joint_" + leg_position],
                                          self._motor_direction[2 * leg_id] * shoulder_angle,
                                          targetVelocity=0)
    self._pybullet_client.resetJointState(self.quadruped,
                                          self._joint_name_to_id["hip_joint_" + leg_position],
                                          self._motor_direction[2 * leg_id] * knee_angle,
                                          targetVelocity=0)
    self._pybullet_client.resetJointState(self.quadruped,
                                          self._joint_name_to_id["knee_joint_" + leg_position],
                                          self._motor_direction[2 * leg_id + 1] * hip_angle,
                                          targetVelocity=0)

  def _reset_robot(self, reload_urdf=True):
    self._urdf_root = self._urdf_root.replace('.', '')
    addr = parentdir + self._urdf_root + "/mors.urdf"
    print(addr)
    if reload_urdf:
      if self._self_collision_enabled:
        self.quadruped = self._pybullet_client.loadURDF(
            addr,
            INIT_POSITION,
            flags=(pybullet.URDF_USE_SELF_COLLISION_INCLUDE_PARENT | pybullet.URDF_USE_SELF_COLLISION))
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 0, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 1, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 2, 1)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 0, 1, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 1, 2, 0)

        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 4, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 5, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 6, 1)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 4, 5, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 5, 6, 1)

        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 8, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 9, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 10, 1)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 8, 9, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 9, 10, 0)

        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 12, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 13, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, -1, 14, 1)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 12, 13, 0)
        self._pybullet_client.setCollisionFilterPair(self.quadruped, self.quadruped, 13, 14, 0)
      else:
        self.quadruped = self._pybullet_client.loadURDF(
            addr, INIT_POSITION)
      if self._on_rack:
        self._pybullet_client.createConstraint(self.quadruped, BASE_LINK_ID, -1, -1,
                                               self._pybullet_client.JOINT_FIXED, [0, 0, 0],
                                               [0, 0, 0], INIT_POSITION)
      self._BuildJointNameToIdDict()
      self._BuildMotorIdList()
      self._RecordMassInfoFromURDF()
      self._ResetPose()

      for joint_number in range(self._pybullet_client.getNumJoints(self.quadruped)):
        self._pybullet_client.enableJointForceTorqueSensor(self.quadruped, joint_number)

    else:
      self._pybullet_client.resetBasePositionAndOrientation(self.quadruped, INIT_POSITION,
                                                            INIT_ORIENTATION)
      self._pybullet_client.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0])
      self._ResetPose()

    self._motor_enabled_list = [True] * self.num_motors

  def get_action_dimension(self):
    """Get the length of the action list.

    Returns:
      The length of the action list.
    """
    return self.action_dim

  def get_observation_upper_bound(self):
    """Get the upper bound of the observation.

    Returns:
    The upper bound of an observation. See GetObservation() for the details
        of each element of an observation.
    """
    upper_bound = np.array([0.0] * self.get_observation_dimension())
    upper_bound[:] = 1.0  # Quaternion of base orientation.
    return upper_bound

  def get_observation_lower_bound(self):
    """Get the lower bound of the observation."""
    lower_bound = np.array([0.0] * self.get_observation_dimension())
    lower_bound[:] = 0.0  # Quaternion of base orientation.
    return lower_bound

  def get_observation_dimension(self):
    """Get the length of the observation list.

    Returns:
    The length of the observation list.
    """
    return self.obs.size

  #--------------------------
  # get sensor data
  #--------------------------
  def get_base_orientation(self):
    """Get the orientation of mors's base, represented as quaternion.

    Returns:
      The orientation of mors's base.
    """
    _, orientation = (self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
    return orientation

  def get_base_orientation_euler(self):
    """Get the orientation of mors's base, represented as Euler angles.

    Returns:
      The orientation of mors's base.
    """
    _, orientation = (self._pybullet_client.getBasePositionAndOrientation(self.quadruped))
    euler_orient = self._pybullet_client.getEulerFromQuaternion(orientation)
    return euler_orient

  def get_base_position_and_orientation(self):
    position, quat = self._pybullet_client.getBasePositionAndOrientation(self.quadruped)
    return position, quat

  def get_base_lin_vel(self):
    vel = self._pybullet_client.getBaseVelocity(self.quadruped)
    return vel[0]

  def get_base_ang_vel(self):
    vel = self._pybullet_client.getBaseVelocity(self.quadruped)
    return vel[1]

  def get_motor_states(self):
    joint_number_range = range(self._pybullet_client.getNumJoints(self.quadruped))
    joint_states = self._pybullet_client.getJointStates(self.quadruped, joint_number_range)
    joint_infos = [self._pybullet_client.getJointInfo(self.quadruped, i) for i in joint_number_range]
    joint_states, joint_name = \
        zip(*[(j, i[1]) for j, i in zip(joint_states, joint_infos) if i[2] != self._pybullet_client.JOINT_FIXED])
    joint_positions = [state[0] for state in joint_states]
    joint_velocities = [state[1] for state in joint_states]
    joint_reaction_forces = [state[2] for state in joint_states]
    joint_torques = [0]*12
    for i in range(12):
      joint_torques[i] = joint_reaction_forces[i][5]
    # print(f"{[ '%.3f' % elem for elem in joint_torques ]}")
    # print(joint_name)
    return joint_positions, joint_velocities, joint_torques, joint_name

  def get_motor_angles(self):
    """Get the eight motor angles at the current moment.

    Returns:
      Motor angles.
    """
    motor_angles = [
        self._pybullet_client.getJointState(self.quadruped, motor_id)[0]
        for motor_id in self._motor_id_list
    ]
    motor_angles = np.multiply(motor_angles, self._motor_direction)
    return motor_angles

  def get_motor_velocities(self):
    """Get the velocity of all motors.

    Returns:
      Velocities of all motors.
    """
    motor_velocities = [
        self._pybullet_client.getJointState(self.quadruped, motor_id)[1]
        for motor_id in self._motor_id_list
    ]
    motor_velocities = np.multiply(motor_velocities, self._motor_direction)
    return motor_velocities

  def get_motor_torques(self):
    """Get the amount of torques the motors are exerting.

    Returns:
      Motor torques of all twelve motors.
    """
    motor_torques = [
        self._pybullet_client.getJointState(self.quadruped, motor_id)[3]
        for motor_id in self._motor_id_list
    ]
    motor_torques = np.multiply(motor_torques, self._motor_direction)
    return motor_torques

  def GetCOMPos(self):
    base_pos, base_orientation = self._pybullet_client.getBasePositionAndOrientation(self.quadruped)
    com_pos_loc = [-0.002185, -0.000018, 0.000286]
    com_orient_loc = [0.0, 0.0, 0.0, 1.0]

    com_pos, com_orient = self._pybullet_client.multiplyTransforms(com_pos_loc, com_orient_loc, base_pos, base_orientation)
    return com_pos

  def GetEFPos(self):
    r1_pos, r1_or,_,_,_,_  = self._pybullet_client.getLinkState(self.quadruped, 4)
    l1_pos, l1_or,_,_,_,_  = self._pybullet_client.getLinkState(self.quadruped, 12)
    r2_pos, r2_or,_,_,_,_  = self._pybullet_client.getLinkState(self.quadruped, 8)
    l2_pos, l2_or,_,_,_,_  = self._pybullet_client.getLinkState(self.quadruped, 16)

    ef = [0]*12
    for i in range(3):
      ef[i] = r1_pos[i]
      ef[i+3] = l1_pos[i]
      ef[i+6] = r2_pos[i]
      ef[i+9] = l2_pos[i]

    return ef

  def get_contact_flags(self):
    self.contact_points = self._pybullet_client.getContactPoints(self.quadruped)
    contact_flags = [False]*4

    for contact_point in self.contact_points:
      if contact_point[3] == 3:
          contact_flags[1] = True
      if contact_point[3] == 7:
          contact_flags[0] = True
      if contact_point[3] == 11:
          contact_flags[3] = True
      if contact_point[3] == 15:
          contact_flags[2] = True
    return self.contact_points, contact_flags

  # ----------------------
  # Physical Parameters
  # ----------------------
  def get_base_mass_from_urdf(self):
    """Get the mass of the base from the URDF file."""
    return self._base_mass_urdf

  def get_leg_masses_from_urdf(self):
    """Get the mass of the legs from the URDF file."""
    return self._leg_masses_urdf

  def set_base_mass(self, base_mass):
    self._pybullet_client.changeDynamics(self.quadruped, BASE_LINK_ID, mass=base_mass)

  def set_leg_masses(self, shoulder, hip, knee):
    for i in range(4):
      self._pybullet_client.changeDynamics(self.quadruped, i*4, mass=shoulder)
      self._pybullet_client.changeDynamics(self.quadruped, i*4+1, mass=hip)
      self._pybullet_client.changeDynamics(self.quadruped, i*4+2, mass=knee)

  def set_foot_friction(self, foot_friction):
    """Set the lateral friction of the feet.

    Args:
      foot_friction: The lateral friction coefficient of the foot. This value is
        shared by all four feet.
    """
    # for link_id in FOOT_LINK_ID:
      # self._pybullet_client.changeDynamics(self.quadruped, link_id, lateralFriction=foot_friction)
    self._pybullet_client.changeDynamics(self.quadruped, 3, lateralFriction=self._lateral_friction, spinningFriction=self._spinning_friction)
    self._pybullet_client.changeDynamics(self.quadruped, 7, lateralFriction=self._lateral_friction, spinningFriction=self._spinning_friction)
    self._pybullet_client.changeDynamics(self.quadruped, 11, lateralFriction=self._lateral_friction, spinningFriction=self._spinning_friction)
    self._pybullet_client.changeDynamics(self.quadruped, 15, lateralFriction=self._lateral_friction, spinningFriction=self._spinning_friction)

  def set_friction(self, lateral_friction=1.0, spinning_friction=0.0063):
    self._lateral_friction = lateral_friction
    self._spinning_friction = spinning_friction

  def set_joint_forces(self, force):
    self.force = force

  def set_joint_max_velocity(self, velocity):
    self._motor_velocity_limit = velocity

  def set_control_mode(self, mode):
    self._control_mode = mode
    if mode == pybullet.TORQUE_CONTROL:
      if self.first == True:
        self.first = False
        for j in range(16):
              self._pybullet_client.setJointMotorControl2(self.quadruped, j, pybullet.VELOCITY_CONTROL, force=0)
    else:
      self.first = True

  # ----------------------
  # Terrain Parameters
  # ----------------------
  def set_world(self, world):
    self._world_name = world

  # def set_reset_type(self, type : bool):
  #   self._hard_reset = type

  # ----------------------
  # External Forces
  # ----------------------
  def set_ext_forces_params(self, magn, duration, interval): #duration and interval in timesteps
    self._ext_force_magn = magn
    self._ext_force_duration = duration
    self._ext_force_interval = interval

  def _apply_force(self):
    if 100 < (self._env_step_counter%self._ext_force_interval) <= (100+self._ext_force_duration):
        if (self._env_step_counter%self._ext_force_interval) < 102:
            self._force_dir = -self._force_dir
        force = [0, self._force_dir*self._ext_force_magn, 0]
        print(force)
        self._pybullet_client.applyExternalForce(self.quadruped, -1, force, [0,0,0], pybullet.LINK_FRAME)
        com_pos = self.GetCOMPos()
        self._pybullet_client.addUserDebugLine([com_pos[0], com_pos[1], com_pos[2]],
                                                    [com_pos[0]+force[0]/1000, com_pos[1]+force[1]/1000, com_pos[2]+force[2]/1000],
                                                    [1, 0, 0], 3, 0.4)


from behave import given, when, then, register_type
import parse
from src.Rover import Rover
from src.Plateau import Plateau

@parse.with_pattern(r"\d+")
def parse_number(text):
    return int(text)

register_type(Number=parse_number)

@given(
    "the rover is at position ({initial_x:Number}, {initial_y:Number}) and facing '{direction}'"
)
def step_impl(context, initial_x: int, initial_y: int, direction: str):
    plateau = Plateau(4, 4)
    context.rover = Rover(initial_x, initial_y, direction, plateau)

@when("the rover moves forward one step")
def step_impl(context):
    context.rover.navigate("M")

@then("the rover is at position ({final_x:Number}, {final_y:Number})")
def step_impl(context, final_x: int, final_y: int):
    assert context.rover.position_x == final_x
    assert context.rover.position_y == final_y


import logging

from .exceptions import DuplicateReferenceException, UnknownReferenceException

logger = logging.getLogger(__name__)

# Some references are not found in the dump file
MAGIC_REFERENCES = ("sound",)

def convert_value(value):
    # Convert values from postgresql format to python values
    if value == "true":
        return True
    elif value == "false":
        return False
    else:
        return value

class ImportSet:
    def __init__(self, raw_data, data_definition):
        self._data = raw_data
        self._data_definition = data_definition
        self._resources = {}
        for resource_type in data_definition.keys():
            self._resources[resource_type] = []
        self._referenced_resources = {}

        self._build_resources()
        self._build_references()

    def _build_resources(self):
        logger.info("building all resources")
        for type_, content in self._data.items():
            if not content:
                continue

            headers = content[0]
            for row in content[1:]:
                resource = {k: convert_value(v) for k, v in zip(headers, row)}
                resource["type_"] = type_
                if type_ not in self._resources:
                    self._resources[type_] = []
                logger.debug("resource built : %s", resource)
                self._resources[type_].append(resource)
        logger.debug("resource building done")

    def _build_references(self):
        logger.info("building references")
        for resources in self._resources.values():
            if not resources:
                continue  # There's no resources for this type

            headers = list(resources[0].keys())
            if "ref" not in headers:
                continue  # The resources in this list have no "ref" column

            for resource in resources:
                reference = resource["ref"]
                if not reference:
                    continue
                logger.debug("reference built : %s", reference)
                if reference in self._referenced_resources:
                    raise DuplicateReferenceException(reference)

                self._referenced_resources[reference] = resource
        logger.debug("reference building done")

    def check_references(self):
        logger.info("checking references")
        unknown_references = set()

        for type_, resources in self._resources.items():
            if not resources:
                continue

            references = self._data_definition.get(type_, {}).get("references", [])
            for reference in references:
                for resource in resources:
                    lookedup_reference = resource.get(reference)
                    if not lookedup_reference:
                        continue
                    if lookedup_reference in MAGIC_REFERENCES:
                        continue
                    if lookedup_reference not in self._referenced_resources:
                        unknown_references.add(lookedup_reference)

        logger.debug("reference checking done")

        if unknown_references:
            raise UnknownReferenceException(list(unknown_references))

    def list(self, resource_type):
        return self._resources.get(resource_type, [])

    def get_resource(self, reference):
        return self._referenced_resources.get(reference)


import matplotlib.pyplot as plt
import numpy as np
import matplotlib

matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
plt.rcParams["font.size"] = 16

runtimes=[]
lengths=[]
makespans = []
for i in range(10):
    with open("../build/logs/"+"29-29-40-"+str(i)+"-100.txt", "r") as res_file:
        runtime = []
        length = []
        makespan = []
        for _ in range(40):
            r, l, m = map(int, res_file.readline().split())
            runtime.append(r)
            length.append(l)
            makespan.append(m)
        runtimes.append(runtime)
        lengths.append(length)
        makespans.append(makespan)

runtimes = np.array(runtimes)/1000.
lengths = np.array(lengths)
makespans = np.array(makespans)

fig, ax = plt.subplots(figsize = (5, 4))

from matplotlib.ticker import ScalarFormatter
#plt.legend()
plt.xlim(1.9, 41)
#plt.ylim(16,43)
ax.set_xscale('log')
ax.set_yscale('log')
ax.yaxis.set_major_formatter(ScalarFormatter())
ax.yaxis.set_minor_formatter(ScalarFormatter())
#ax.set_aspect('equal')
plt.ylabel("Makespan")
plt.xlabel("Number of agents")
plt.errorbar(range(2,41),makespans[:,1:].mean(axis=0),makespans[:,1:].std(axis=0))
fig.subplots_adjust(left=0.16, right=0.98, bottom=0.16, top=0.95)
plt.savefig("./makespan.pdf")
plt.show()


import matplotlib.pyplot as plt

#displays error graph
def errorgraph(data):
    error = data.errors
    x = list(range(0, len(data.errors)))
    plt.scatter(x, error, alpha=0.8, edgecolors='none', s=5)
    plt.title('Graph of error for ' + data.kind + " " + str(data.params[len(data.params) - 2]))
    plt.axis([0, 1000, 0, max(error) + max(error)/10])
    plt.show()


"""
Wrap the transaction created in the previous task in a function that will take the first and
last name as a kwarg. Based on the given name or surname, the function will decide which query
to use to search for a director and then delete him/her. The search query should be in the text
form ().
"""

# https://docs.python.org/3/library/argparse.html
# allows us to define input arguments and help
import argparse

from sqlalchemy.orm import sessionmaker
from sqlalchemy import text
from cinematic import Movies, Directors, engine

# define the argparser, with description
ARG_PARSER = argparse.ArgumentParser(
    description="Deletes directors from db"
)

# create 2 arguments name and surname
ARG_PARSER.add_argument('-n', '--name', dest='name', help='handles directors name')
ARG_PARSER.add_argument('-s', '--surname', dest='surname', help='handles directors surname')
# parse all arguments from input
ARGS = ARG_PARSER.parse_args()

def delete_director(**kwargs):
    """ Deletes director """
    if kwargs.get('name') is not None:  # 'name' in kwargs:
        query = text('SELECT * FROM directors WHERE name = :name')
    elif kwargs.get('surname') is not None:
        query = text('SELECT * FROM directors WHERE surname = :surname')
    else:
        print('No valid arguments parsed')
        return

    # important, use autocommit=True
    session = sessionmaker(bind=engine, autocommit=True)()

    with session.begin():
        # select first found director, remember value is tuple with one elem: tuple(elem,)
        director_id = session\
                    .query(Directors.director_id)\
                    .from_statement(query)\
                    .params(kwargs)\
                    .first()

        try:
            # return deleted movies amount
            print('movies:', session
                  .query(Movies)
                  .filter(Movies.director_id == director_id[0])
                  .delete())

            # return deleted director amount
            print('directors', session
                  .query(Directors)
                  .filter(Directors.director_id == director_id[0])
                  .delete())
        # if no director found, catch exception, return human readable message
        except TypeError:
            print(f'No director found for {kwargs}')

# call function with two arguments
delete_director(name=ARGS.name, surname=ARGS.surname)


from typing import List, Optional

from sqlalchemy.orm import Query

from app.lib import db
from app.models import Student
from app.schemas import StudentCreate

def create(student: StudentCreate) -> Student:
    session = db.get_session()
    db_student = Student(**student.dict())
    session.add(db_student)
    session.commit()
    session.refresh(db_student)
    return db_student

def get(sort: str, name: Optional[str]) -> List[Student]:
    session = db.get_session()
    query = session.query(Student)
    query = _filter(query, name)
    return _sort(query, sort)

def get_one(id: int) -> Optional[Student]:
    session = db.get_session()
    return session.query(Student).filter(Student.id == id).first()

def update(id: int, student: StudentCreate) -> Student:
    session = db.get_session()
    db_student = session.query(Student).filter(Student.id == id).first()
    db_student.name = student.name
    session.commit()
    session.refresh(db_student)
    return db_student

def delete(id: int) -> None:
    session = db.get_session()
    db_student = session.query(Student).filter(Student.id == id).first()
    session.delete(db_student)
    session.commit()

def _filter(query: Query, name: Optional[str]) -> Query:
    if name:
        query = query.filter(Student.name == name)
    return query

def _sort(query: Query, sort: str) -> List[Student]:
    order_by = sort_params[sort]
    return query.order_by(*(order_by,)).all()

sort_params = {
    "name:asc": Student.name.asc(),
    "name:desc": Student.name.desc(),
}


'''
Created on Feb 5, 2016

@author: yumilceh
'''
import matplotlib.pyplot as plt
import numpy as np
from numpy import random as np_rnd

if __name__ == '__main__':

    ## Adding the projects folder to the path##
    import sys,random
    sys.path.append("../../")

    ## Adding libraries##
    from exploration.systems.trash.Parabola_Test import ParabolicRegion as System
    from exploration.algorithm.AlgorithmRandom import Algorithm_Random as Algorithm
    from exploration.algorithm.AlgorithmRandom import MODELS
    from exploration.models.GMM_SM import GMM_SM
    from exploration.models.GMM_SS import GMM_SS
    from exploration.data.PlotTools import initializeFigure

    ## Simulation Parameters ##
    n_initialization=50
    n_experiments=10000

    random_seed=1234

    k_sm = 30
    sm_step=1000
    alpha_sm=0.05

    k_ss = 6
    ss_step=100
    alpha_ss=0.05

    ## To guarantee reproductible experiments##
    random.seed(random_seed)
    np_rnd.seed(random_seed)

    ## Creating Agent ##
    system=System()
    system.params.sigma_noise = 0.0000000001

    ## Creating models ##
    models=MODELS()

    models.f_sm = GMM_SM(system,
                         k_sm,
                         sm_step=sm_step,
                         alpha=alpha_sm)
    models.f_ss = GMM_SS(system,
                         k_ss,
                         ss_step=ss_step,
                         alpha=alpha_ss)

    ## Creating Simulation object, running simulation and plotting experiments##
    file_prefix='Sinus_GMM_'
    simulation1=Algorithm(system,
                          models,
                          file_prefix=file_prefix,
                          n_experiments = n_experiments
                          )

    simulation1.runNonProprioceptiveAlgorithm()

    initialization_data_sm_ss=simulation1.data.initialization_data_sm_ss
    initialization_data_im=simulation1.data.initialization_data_im
    simulation_data=simulation1.data.simulation_data

    s_dims = 2
    data_as_matrix = simulation_data.sensor_data.data.as_matrix()
    somatodata_as_matrix =simulation_data.somato_data.data.as_matrix()

    fig1,ax1=initializeFigure();
    fig1.show()
    #===========================================================================
    # fig1.show()
    #===========================================================================
    fig1.canvas.draw()
    plt.scatter(data_as_matrix[:, 0], data_as_matrix[:, 1], 0.8, color='k')
    fig1.canvas.draw()
    plt.hold(True)
    ax1.autoscale_view()

    data_as_matrix = data_as_matrix[np.where(somatodata_as_matrix[:,0] == 0)[:], :][0]
    plt.scatter(data_as_matrix[:, 0], data_as_matrix[:, 1], 0.8, color='r')
    fig1.canvas.draw()

    #===========================================================================
    # fig1,ax1=simulation_data.plot_2D(fig1,ax1,'sensor', 0, 'sensor', 1,"or")
    # fig1, ax1 = simulation1.models.f_sm.model.plot_gmm_projection(fig1,ax1,2, 3)
    #===========================================================================

    min_distance = 0.2
    change = True

    while change: #No optimal
        change = False
        for i in range(data_as_matrix.shape[0]):
            if change == True:
                break
            for j in range(i+1,data_as_matrix.shape[0]):
                distance_ij = np.linalg.norm(data_as_matrix[i,:]-data_as_matrix[j,:])
                if distance_ij < min_distance:
                    data_as_matrix = np.delete(data_as_matrix, j, 0)
                    change = True
                    break

    plt.plot(data_as_matrix[:, 0], data_as_matrix[:, 1], "or")
    fig1.canvas.draw()

    import pandas as pd
    from DataManager.SimulationData import SimulationData
    validation_data_set = SimulationData(simulation1.agent)
    validation_data_set.sensor_data.data = pd.DataFrame(data_as_matrix)
    validation_data_set.save_data('parabola_dataset_2.h5')

    try:
        str_opt = raw_input("Press [enter] to continue or press 'Y' to keep plots...   ")
        if str_opt == 'Y':
            plt.show()
    except SyntaxError:
        plt.show()
        pass


import os
import sys
sys.path.insert(0, os.path.abspath('../'))

import torch
import pandas as pd
import seaborn as sns
import matplotlib.pyplot as plt

#
sns.set_context('paper')
dataset = 'pubmed'
ptb_rate = 0.05
select_range = [0]

for select_index in select_range:

    target_records = torch.load(f"{dataset}-budgets.pth")
    budget_dict = target_records['budget']
    budget_collect = dict()
    for n, b in budget_dict.items():
        if b not in budget_collect:
            budget_collect[b] = list()
        budget_collect[b].append(n)

budget_dict = dict()

for k, v in budget_collect.items():
    budget_dict[k] = len(v)

keys = sorted(budget_dict.keys())
values = [budget_dict[key] for key in keys]
print(keys)
print(values)

data = pd.DataFrame({f'{dataset}': keys, 'y': values})

plt.figure(figsize=(12, 8))
# sns.set(font_scale=10.0)
sns.set(style="white")
# Create the distribution plot
p = sns.histplot(
    data=data,
    bins=range(21),
    x=f'{dataset}', weights='y',
    color='#4c72b0',
    alpha=0.5,
    edgecolor='k')
p.set_ylabel("The number of nodes", fontsize=30)
p.set_xlabel(f"Targeted attack budgets", fontsize=30)

plt.xlim((0, 21))
# Set x-axis labels as dictionary keys
plt.xticks(range(21))

plt.savefig(f"../pictures/budget_plot_{dataset}.pdf", format='pdf', dpi=1200)
plt.show()


import re
from functools import reduce
from datetime import datetime as dt

from mpygit import mpygit, gitutil

from django.test import TestCase, Client

from mfgd_app.models import UserProfile, CanAccess, Repository

class ChainTestCase(TestCase):
    REGEX_CHAIN_ENTS = (
        r"""
\s*"""
        r""""""
            r"""<a href="/linear/info/(?P<big_hash>[A-z0-9]*)/?">"""
                r"""(?P<short_hash>[A-z0-9]*)"""
            r""""""
        r"""	"""
        r"""\s*(?P<commit_msg>.*)	"""
        r"""\s*(?P<author>.*)	"""
        r"""\s*"""
            r"""\s*<time datetime="[0-9-]*">(?P<date>[0-9-]*)</time>"""
        r"""\s*	"""
        r"""\s*
"""
    )

    def setUp(self):
        Repository.objects.create(name="simple", path="tests/repo/simple", isPublic=True)
        Repository.objects.create(name="linear", path="tests/repo/linear", isPublic=True)
        Repository.objects.create(name="n_merge", path="tests/repo/n_merge", isPublic=True)

        self.client = Client()

    def tearDown(self):
        Repository.objects.all().delete()

    def _get_chain(self, repo_name):
        db_repo = Repository.objects.get(name=repo_name)
        repo = mpygit.Repository(db_repo.path)

        response = self.client.get(f"/{repo_name}/chain/", follow=True)
        self.assertEquals(response.status_code, 200)
        return response.content.decode()

    def test_displays_commits(self):
        content = self._get_chain("linear")
        repo = mpygit.Repository(Repository.objects.get(name="linear").path)
        repo_walker = gitutil.walk(repo, repo.HEAD, 5)
        for fs_ent, match in zip(repo_walker, re.finditer(self.REGEX_CHAIN_ENTS, content)):
            oid, short_oid, msg, committer, timestamp = match.groups()
            self.assertEqual(fs_ent.oid, oid)
            self.assertEqual(fs_ent.short_oid, short_oid)
            self.assertEqual(fs_ent.message, msg)
            self.assertEqual(fs_ent.committer.name, committer)

            fs_timestamp = dt.utcfromtimestamp(fs_ent.committer.timestamp).strftime("%Y-%m-%d")
            self.assertEqual(fs_timestamp, timestamp)

    def test_correct_ordering(self):
        content = self._get_chain("linear")
        # you can't stop me
        is_sorted, _ = reduce(
            lambda acc, v: (acc[0] and acc[1] >= v, v),
            map(lambda x: x.group(5), re.finditer(self.REGEX_CHAIN_ENTS, content)),
            (True, "9999-99-99"))
        self.assertTrue(is_sorted)

    def test_n_way_merge_flattened(self):
        content = self._get_chain("n_merge")
        order = (5, 6, 4, 3, 2, 1)
        for n, match in zip(order, re.finditer(self.REGEX_CHAIN_ENTS, content)):
            self.assertEqual(f"commit #{n}", match.group(3))

    def test_entries_link_to_info(self):
        # this is ensured by displays_commits because it must match the regex which
        # then checks to see whether the long oid in the anchor tag is associated with
        # the short oid
        pass

    def test_empty_chain(self):
        db_repo = Repository.objects.get(name="simple")
        repo = mpygit.Repository(db_repo.path)

        response = self.client.get("/simple/chain/", follow=True)
        self.assertEquals(response.status_code, 200)
        content = response.content.decode()
        self.assertFalse(re.search(self.REGEX_CHAIN_ENTS, content))


import cv2

class FramesWithInfo:
    def __init__(self, frames, output_shape=None):
        self.frames = frames
        if output_shape is None:
            self.output_shape = frames[0].shape[:-1][::-1]
        else: self.output_shape = output_shape
        self.end = len(frames)
        self.read_head = 0

    def __next__(self):
        if self.read_head < self.end:
            frame = self.frames[self.read_head]
            self.read_head+=1
            return frame

        else:
            raise StopIteration

    def __iter__(self):
        return self

class AdvancedFrameReader():
    def __init__(self, video_name, read_every, rescale_factor, init_time_min, init_time_s):

        self.cap = cv2.VideoCapture(video_name)

        self.original_width = self.cap.get(cv2.CAP_PROP_FRAME_WIDTH)
        self.original_height = self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT)

        if rescale_factor != 1:
            self.set_rescale_factor(rescale_factor)
        else:
            self.original_shape_mode = True

        self.init_rescale_factor = rescale_factor

        self.frame_skip = read_every -  1
        self.fps = self.cap.get(cv2.CAP_PROP_FPS)/read_every
        print('Reading at {} fps.'.format(self.fps))

        self.set_time_position(init_time_min, init_time_s)
        self.init_frame = self.cap.get(cv2.CAP_PROP_POS_FRAMES)
        self.total_num_frames = self.cap.get(cv2.CAP_PROP_FRAME_COUNT)

    def post_process(self, ret, frame):
        if ret:
            if self.original_shape_mode:
                return ret, frame
            else:
                return ret, cv2.resize(frame, self.new_shape)
        else:
            return ret, []

    def skip(self):
        if not self.nb_frames_read:
            return
        else:
            for _ in range(self.frame_skip):
                self.cap.read()

    def read_frame(self):

        self.skip()
        ret, frame = self.cap.read()
        self.nb_frames_read += 1
        return self.post_process(ret, frame)

    def set_time_position(self, time_min, time_s):
        time = 60 * time_min  + time_s
        self.cap.set(cv2.CAP_PROP_POS_MSEC, 1000 * time)
        self.init_frame = self.cap.get(cv2.CAP_PROP_POS_FRAMES)
        print('Reading from {}min{}sec'.format(time_min,time_s))
        self.nb_frames_read = 0

    def reset_init_frame(self):
        self.cap.set(cv2.CAP_PROP_POS_FRAMES,int(self.init_frame))
        self.nb_frames_read = 0

    def set_init_frame(self, init_frame):
        self.cap.set(cv2.CAP_PROP_POS_FRAMES, int(init_frame))
        self.init_frame = self.cap.get(cv2.CAP_PROP_POS_FRAMES)
        self.nb_frames_read = 0

    def set_rescale_factor(self, rescale_factor):
        width = int(self.original_width/rescale_factor)
        height = int(self.original_height/rescale_factor)
        self.new_shape  = (width, height)
        self.original_shape_mode = False
        print('Reading in {}x{}'.format(width, height))

    def set_original_shape_mode(self, mode):
        self.original_shape_mode = mode

    def reset_init_rescale_factor(self):
        self.set_rescale_factor(self.init_rescale_factor)

class IterableFrameReader:

    def __init__(self, video_filename, skip_frames=0, output_shape=None):
        self.video = cv2.VideoCapture(video_filename)
        self.input_shape = (self.video.get(cv2.CAP_PROP_FRAME_WIDTH), self.video.get(cv2.CAP_PROP_FRAME_HEIGHT))
        self.skip_frames = skip_frames
        if output_shape is None:
            w, h = self.input_shape
            new_h = (h | 31) + 1
            new_w = (w | 31) + 1
            self.output_shape = (new_w, new_h)
        else:
            self.output_shape = output_shape
        self.fps = self.video.get(cv2.CAP_PROP_FPS) / (self.skip_frames+1)

    def __next__(self):
        ret, frame = self.video.read()
        self._skip_frames()
        if ret:
            return cv2.resize(frame, self.output_shape)
        raise StopIteration

    def __iter__(self):
        return self

    def init(self):
        self.video.set(cv2.CAP_PROP_POS_FRAMES, 0)

    def _skip_frames(self):
        for _ in range(self.skip_frames):
            self.video.read()

class SimpleVideoReader:
    def __init__(self, video_filename, skip_frames=0):
        self.skip_frames = skip_frames
        self.video = cv2.VideoCapture(video_filename)
        self.shape = (int(self.video.get(cv2.CAP_PROP_FRAME_WIDTH)), int(self.video.get(cv2.CAP_PROP_FRAME_HEIGHT)))
        self.fps = self.video.get(cv2.CAP_PROP_FPS) / (skip_frames+1)
        self.frame_nb = 0
        self.num_frames = self.video.get(cv2.CAP_PROP_FRAME_COUNT)

    def read(self):
        ret, frame = self.video.read()
        self.frame_nb+=1
        self._skip_frames()
        return ret, frame, self.frame_nb-1

    def set_frame(self, frame_nb_to_set):
        self.video.set(cv2.CAP_PROP_POS_FRAMES, frame_nb_to_set)
        self.frame_nb = frame_nb_to_set

    def _skip_frames(self):
        for _ in range(self.skip_frames):
            self.video.read()


from django.http import Http404, HttpResponseRedirect, HttpResponse
from django_mako_plus.controller import view_function
import homepage.models as hmod
from django_mako_plus.controller.router import get_renderer
from django import forms
from django.contrib.auth.decorators import permission_required

templater = get_renderer('homepage')

@view_function
@permission_required('homepage.view_product', login_url='/homepage/login/')
def process_request(request):
    params = {}

    params["products"] = hmod.Product.objects.all().order_by('product_specification__category', 'product_specification__name')

    return templater.render_to_response(request, 'product.html', params)

#########################################################################
### Edit a product

@view_function
# @permission_required('homepage.change_product', login_url='/homepage/login/')
def edit(request):
    params = {}
    product_id_str = request.urlparams[1]

    if request.urlparams[0] == 'new':
        # print("It is a new product")
        form = ProductEditForm()
    else:
        # print("I'm editing an existing product")
        try:
            product = hmod.Product.objects.get(id=product_id_str)
        except hmod.Product.DoesNotExist:
            return HttpResponseRedirect('/homepage/product/')

        form = ProductEditForm(initial={
            'name': product.product_specification.name,
            'description': product.product_specification.description,
            'category': product.product_specification.category,
            'price': product.product_specification.price,
            'qty_on_hand': product.qty_on_hand,
            # 'producer': product.producer,
        })

    if request.method == 'POST':
        form = ProductEditForm(request.POST)  # POST is a dictionary of all the data sent with the form

        if form.is_valid():
            name = form.cleaned_data['name']
            category_str = form.cleaned_data['category']
            desc = form.cleaned_data['description']
            price = form.cleaned_data['price']
            qty_on_hand = form.cleaned_data['qty_on_hand']
            # producer_str = form.cleaned_data['producer']

            try:
                category = hmod.Category.objects.get(name=category_str)
            except hmod.Category.DoesNotExist:
                raise forms.ValidationError("Category doesn't exist")

            if request.urlparams[0] == 'new':
                ps = hmod.ProductSpecification()
                ps.name = name
                ps.category = category
                ps.description = desc
                ps.price = price
                ps.save()

                product = hmod.Product()
                product.qty_on_hand = qty_on_hand
                product.product_specification = ps
                product.save()
            else:
                try:
                    product = hmod.Product.objects.get(id=product_id_str)
                except hmod.Product.DoesNotExist:
                    return HttpResponseRedirect('/homepage/product/')

                product.product_specification.name = name
                product.product_specification.category = category
                product.product_specification.description = desc
                product.product_specification.price = price
                product.product_specification.save()

                product.qty_on_hand = qty_on_hand
                product.save()

            # try:
            #     producer = hmod.Organization.objects.get(given_name=form.cleaned_data['producer'])
            # except hmod.Organization.DoesNotExist:
            #     raise forms.ValidationError("Producer doesn't exist")
            # product.producer = producer
            # product.save()
            return HttpResponseRedirect('/homepage/product/')

    params['form'] = form

    return templater.render_to_response(request, 'product.edit.html', params)

class ProductEditForm(forms.Form):
    name = forms.CharField(label="Name*", max_length=100)
    description = forms.CharField(max_length=100, required=False)
    category = forms.ModelChoiceField(label="Category*", queryset=hmod.Category.objects, empty_label=None)
    price = forms.DecimalField(label="Price*",max_digits=10, decimal_places=2)
    qty_on_hand = forms.IntegerField(label="Quantity On Hand*")
    # producer = forms.ModelChoiceField(queryset=hmod.Organization.objects)
    # empty_label in a ModelChoiceField will not let you continue on past the clean if that field is required

    # def clean_producer(self):
    #     try:
    #         producer = hmod.Organization.objects.get(given_name=self.cleaned_data['producer'])
    #     except hmod.Organization.DoesNotExist:
    #         raise forms.ValidationError('The producer you entered does not exist.')
    #
    #     if producer.city == 'temp':
    #         raise forms.ValidationError('You must enter a producer for this product.')
    #     return self.cleaned_data['producer']

# @view_function
# # @permission_required('homepage.add_product', login_url='/homepage/login/')
# def create(request):
#     product = hmod.Product()
#     product.name = ''
#     product.category = ''
#     product.current_price = 0
#
#     try:
#         product.producer = hmod.Organization.objects.get(city='temp')
#     except hmod.Product.DoesNotExist:
#         raise forms.ValidationError("The temporary producer doesn't exist.")
#
#     product.save()
#
#     return HttpResponseRedirect('/homepage/product.edit/{}/new/'.format(product.id))

@view_function
# @permission_required('homepage.add_product', login_url='/homepage/login/')
def delete(request):
    # delete a product before product specification

    try:
        product = hmod.Product.objects.get(id=request.urlparams[0])
    except hmod.Product.DoesNotExist:
        return HttpResponseRedirect('/homepage/product/')

    ps = product.product_specification

    product.delete()

    if hmod.Product.objects.filter(product_specification=ps).count() != 0:
        ps.delete()

    return HttpResponseRedirect('/homepage/product/')


import cv2
import numpy as np

#Inicializar los parametros del detector de arucos
parametros = cv2.aruco.DetectorParameters_create()

#Cargamos el diccionario de nuestro aruco
diccionario = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_100)

#Lectura de la camara#
cap = cv2.VideoCapture(0)
cap.set(3,1280)
cap.set(4,720)

while True:
	ret, frame = cap.read()
	gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)
	#Detectamos los marcadores en la imagen
	esquinas,ids,candidatos_malos = cv2.aruco.detectMarkers(gray,diccionario,parameters=parametros)

	if np.all(ids!=None):
		aruco = cv2.aruco.drawDetectedMarkers(frame,esquinas)

		#Extraemos los puntos de las esquinas en coordenadas separadas
		c1 = (esquinas[0][0][0][0], esquinas[0][0][0][1])
		c2 = (esquinas[0][0][1][0], esquinas[0][0][1][1])
		c3 = (esquinas[0][0][2][0], esquinas[0][0][2][1])
		c4 = (esquinas[0][0][3][0], esquinas[0][0][3][1])

		copy = frame
		#Leemos la imagen que vamos a sobreponer
		imagen = cv2.imread("imagen.jpg")
		#Extraemos el tamaño de la imagen
		tamaño = imagen.shape
		#Organizamos las coordenadas del aruco en una matriz
		puntos_aruco = np.array([c1,c2,c3,c4])

		#Organizamos las coordenadas de la imagen en otra matriz
		puntos_imagen = np.array([
			[0,0],
			[tamaño[1] - 1, 0],
			[tamaño[1] - 1, tamaño[0] - 1],
			[0, tamaño[0] - 1]
			], dtype = float)

		#Realizamos la superposicion de la imagen (homografia)
		h,estado = cv2.findHomography(puntos_imagen, puntos_aruco)

		#Realizamos la transformacion de perspectiva
		perspectiva = cv2.warpPerspective(imagen, h, (copy.shape[1], copy.shape[0]))
		cv2.fillConvexPoly(copy, puntos_aruco.astype(int), 0, 16)
		copy = copy + perspectiva
		cv2.imshow("Realidad Virtual", copy)

	else:
		cv2.imshow("Realidad Virtual", frame)

	k = cv2.waitKey(1)
	if k == 27:
		break

cap.release()
cv2.destroyAllWindows()


import json
import requests
import datetime
#
import cherrypy

#html request
import requests
from cherrypy.lib import static
import os
from jinja2 import Environment, FileSystemLoader
from figo import FigoSession
import json

session = FigoSession("ASHWLIkouP2O6_bgA2wWReRhletgWKHYjLqDaqb0LFfamim9RjexTo22ujRIP_cjLiRiSyQXyt2kM1eXU2XLFZQ0Hro15HikJQT_eNeT_9XQ")

localDir = os.path.dirname(__file__)
absDir = os.path.join(os.getcwd(), localDir)

env = Environment(loader=FileSystemLoader('html'))

percentage=-0.01

class Landing_Page(object):
    @cherrypy.expose
    def index(self):
        tmpl = env.get_template('index.html')
        return tmpl.render(name="Apple")

    @cherrypy.expose
    def portfolio(self):
        tmpl = env.get_template('portfolio.html')
        return tmpl.render()

    @cherrypy.expose
    def transactionHistory(self):
        headers = {'Accept': 'application/json'}
        # Rest call towards the "John Snow" mock account on DB
        url = "https://007investment.table.core.windows.net:443/Transaction?sv=2016-05-31&si=Transaction-15EEC51E092&tn=transaction&sig=G%2BAfHj8oMMxKdTwj93q4KiR7tmnIPJdKkjwyHYdggpM%3D"
        response = requests.get(url, headers=headers)
        trans_j = response.json()
        all_transactions= []

        #Make a list of all transactions with the fields below ('Timestamp' etc.)
        for value in trans_j['value']:
                price_paid_long =(float(value['Price'])*float(value['Amount']))
                price_paid ="{0:.2f}".format(price_paid_long)
                time_stamp=datetime.datetime.strptime(value['Timestamp'][0:16], "%Y-%m-%dT%H:%M")
                if value['CreatedDate'][19] is 'Z':
                    time_stamp_purch= datetime.datetime.strptime(value['CreatedDate'][0:16], "%Y-%m-%dT%H:%M").strftime("%d.%m.%Y %H:%M")
                else:
                    time_stamp_purch= datetime.datetime.strptime(value['CreatedDate'], "%m/%d/%Y %I:%M:%S %p").strftime("%d.%m.%Y %H:%M")
                amount ="{0:.2f}".format(float(value['Amount']))
                single_transaction = {'Timestamp': time_stamp ,'TimestampPurch':time_stamp_purch, 'StockId': value['StockId'].split(':')[1], 'StockPrice': value['Price'], 'Amount': amount, 'MoneySpent': price_paid}
                all_transactions.append(single_transaction)

        all_transactions = sorted(all_transactions, key=lambda k: k['Timestamp'], reverse=True)
        tmpl = env.get_template('transactionHistory.html')
        return tmpl.render(seq=all_transactions)

    @cherrypy.expose
    def newTransaction(self):

        companyName=''
        stockName=''
        stockPrice=0
        stockAmount=0
        stockPercentage=0
        transAmount=0
        booking_date=''

        # Print out a list of accounts including its balance
        #for account in session.accounts:
        #    print(account.name)
        #    print(account.balance)

        headers = {'Accept': 'application/json'}
        url = "https://007investment.table.core.windows.net:443/Stock?sv=2016-05-31&si=Stock-15EEC52A495&tn=stock&sig=PbqLEW4Mi0hoNBw5nflEDszu36wzJ%2Bw592%2Bih%2BIfMI0%3D"
        response = requests.get(url, headers=headers)
        print(response.text)
        stocks = response.json()

        # Print out the list of all transactions on a specific account
        for transaction in session.get_account("A1.1").transactions:

            i = -1
            for stock in stocks['value']:
                for name in stock['CompanyName'].split('\n'):
                    i = i+1
                    #print(name.split(' ')[0] + "   "+transaction.name.split(' ')[0])
                    if name.split(' ')[0] == transaction.name.split(' ')[0]:
                        companyName = name.split(' ')[0]
                        stockName = name
                        stockPrice=stocks['value'][i]['Price']
                        stockId = stocks['value'][i]['Exchange']
                        stockAmount=(transaction.amount*percentage)
                        stockPercentage=((transaction.amount*percentage)/stocks['value'][i]['Price'])
                        transAmount=-1*transaction.amount
                        booking_date=transaction.booking_date
                        print(str(transaction.amount)+" "+str(i))

        print(companyName+" / "+stockName+" / "+str(stockPrice)+" / "+str(stockPercentage))
        #Post Request
        url = "https://007investment.table.core.windows.net:443/Transaction?sv=2016-05-31&si=Transaction-15EEC51E092&tn=transaction&sig=G%2BAfHj8oMMxKdTwj93q4KiR7tmnIPJdKkjwyHYdggpM%3D"
        response = requests.get(url, headers=headers)
        trans_j = response.json()
        allRow =[]
        for value in trans_j['value']:
            allRow.append(int(value['RowKey']))
            print(value['RowKey'])
        rowKey= str(1+allRow.pop())
        time= (datetime.timedelta(hours=2)+datetime.datetime.utcnow())
        headers = {'Content-Type': 'application/json', 'Accept': 'application/json'}
        payload = {
            "PartitionKey": "10-2017",
            "RowKey": rowKey,
            "UserId": "1",
            "StockId": stockId,
            "Price": stockPrice,
            "Status": "confirmed",
            "Amount": stockPercentage,
            "CreatedDate": time.strftime("%m/%d/%Y %I:%M:%S %p")
                }

        r = requests.post("https://007investment.table.core.windows.net:443/Transaction?sv=2016-05-31&si=Transaction-15EEC51E092&tn=transaction&sig=G%2BAfHj8oMMxKdTwj93q4KiR7tmnIPJdKkjwyHYdggpM%3D", data=json.dumps(payload), headers=headers)
        #End Post Request
        tmpl = env.get_template('newTransaction.html')
        return tmpl.render(companyName=companyName,stockName=stockName,stockPrice=stockPrice,stockPercentage=round(stockPercentage,2),transAmount=transAmount,stockAmount=stockAmount,booking_date=time.strftime("%d.%m.%Y %H:%M"))

    @cherrypy.expose
    def myLink(self):
        headers = {'Content-Type': 'application/json', 'Accept': 'application/json'}

        payload = {
            "PartitionKey": "10-2017",
            "RowKey": "48",
            "UserId": "1",
            "StockId": "1",
            "Price": 10.6,
            "Status": "pending",
            "Amount": 100
                }

        r = requests.post("https://007investment.table.core.windows.net:443/Transaction?sv=2016-05-31&si=Transaction-15EEC51E092&tn=transaction&sig=G%2BAfHj8oMMxKdTwj93q4KiR7tmnIPJdKkjwyHYdggpM%3D", data=json.dumps(payload), headers=headers)
        status = r.status_code
        header = r.headers
        text = r.text
        tmpl = env.get_template('post_success.html')
        return tmpl.render(status=status, header=header, text=text)

config = {
    'global': {
        'server.socket_host': '0.0.0.0',
        'server.socket_port': int(os.environ.get('PORT', 5000)),
    },
    '/assets': {
         'tools.staticdir.root': os.path.dirname(os.path.abspath(__file__)),
         'tools.staticdir.on': True,
         'tools.staticdir.dir': 'assets',
    }
}

cherrypy.quickstart(Landing_Page(), '/', config=config)

##json read


import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.font_manager as fm
import numpy as np
import itertools
import json
import re
import pickle

from sklearn.decomposition import LatentDirichletAllocation
import pandas as pd

from data_loader import DataLoader

from collections import Counter
from wordcloud import WordCloud

from sklearn.feature_extraction.text import TfidfVectorizer
import matplotlib.pyplot as plt

from bareunpy import Tagger

import networkx as nx
import os
from matplotlib import rc
from tqdm import tqdm

rc('font', family='AppleGothic')
plt.rcParams['font.family'] = 'AppleGothic' # 사용할 한글 폰트명 입력
plt.rcParams['axes.unicode_minus'] = False # 마이너스 폰트 설정

class FrequencyAnalysis:
    def __init__(self):
        pass

    def cloud_visulize(self, noun_list, filename, white = True):
        if white == True:
            wordcloud = WordCloud(width=2000, height=1000, font_path = '/Library/Fonts/AppleGothic.ttf', collocations=True, background_color='white')
        else:
            wordcloud = WordCloud(width=2000, height=1000, font_path = '/Library/Fonts/AppleGothic.ttf', collocations=True)
        wordcloud.generate_from_frequencies(dict(noun_list))
        wordcloud.to_file(filename)
        wordcloud = None

    def cloud_visualize_idf(self, noun_list, filename):
        from sklearn.feature_extraction.text import TfidfVectorizer

    def wordcloud_Rank(self, token, N = 30, white = True, outdir = 'out', filename=''):
        if not os.path.exists(outdir + '/Wordcloud'):
            os.makedirs(outdir + '/Wordcloud')
        filename_png = outdir + f'/Wordcloud/{filename}.png'
        filename_csv = outdir + f'/Wordcloud/{filename}.csv'

        flatten_tokens = list(itertools.chain(*token))
        token_count = Counter(flatten_tokens)
        Top_token_count = token_count.most_common(N)

        word_count = pd.DataFrame(Top_token_count)
        word_count.columns = ['Word', 'Count']
        word_count.to_csv(filename_csv, index = False, encoding='euc-kr')

        self.cloud_visulize(Top_token_count, filename_png, white)

        return word_count

    def Top_Keyword_Edgelist(self, Token, Edge_list, N, outdir = '', node_size = 50, edge_size = 10, filename=''):
        if not os.path.exists(outdir + '/Graph'):
            os.makedirs(outdir + '/Graph')
        filename = outdir + f'/Graph/{filename}'
        flatten_tokens = list(itertools.chain(*Token))
        All_words = Counter(flatten_tokens)
        Top_words = All_words.most_common(N)
        Top_words = [word for (word, count) in Top_words]
        Top_Edge_list = list((x, y, v) for (x,y,v) in Edge_list if (x in Top_words and  y in Top_words))
        G_top = nx.Graph()

        for A,B, weight in Top_Edge_list: G_top.add_edge(A, B, weight=weight, distance = 1/weight)
        width_top = list(nx.get_edge_attributes(G_top, 'weight').values())
        degree = [All_words[node] * node_size for node in list(G_top.nodes)]
        edges_top = G_top.edges()
        weights = [1 for u,v in edges_top]

        with open(f'{filename}.json', 'w', encoding='utf-8') as f:
            json.dump(G_top.adj._atlas, f, ensure_ascii=False, indent=2)
        plt.figure(figsize=(20,20))
        pos = nx.spring_layout(G_top, k = 0.5)
        nx.draw(G_top, pos, width=weights, node_color= 'yellow', node_size = degree, edge_color= 'green', edge_cmap=plt.cm.Reds)
        nx.draw_networkx_labels(G_top, pos, font_size=32, font_family='AppleGothic', font_color='black')
        plt.savefig(f'{filename}.jpg')

    def make_Co_Keyword(self, Text):
        Co_Keyword = list(itertools.combinations(Text,2))
        Co_Keyword = [(x,y) for (x,y) in Co_Keyword if x != y]
        return Co_Keyword

    def make_Edgelist(self, Token):
        Co_Keyword = [self.make_Co_Keyword(text) for text in Token]
        Co_Keyword = list(itertools.chain(*Co_Keyword))
        Edge_list = list((x, y, v) for (x,y), v in Counter(Co_Keyword).items())
        return Edge_list

    def preprocess_text(self, lines):
        """
        불용어를 제거하기 위해 토큰화한다.
        :param lines: 문장 리스트
        :return: 토큰화된 문장 리스트
        """
        count_dict = {}
        tagger = Tagger('2AVZBLY-D7QUNYQ-XNCRIOA-NVP3TLA', 'localhost', port=5656)
        tokenized_lines = []
        for line in tqdm(lines):
            tagged = tagger.tags([line])
            nouns = []
            for p in tagged.pos():
                if len(p[0]) > 1:
                    if p[1] == 'NNG' or p[1] == 'NNP':
                        nouns.append(p[0])
            if len(nouns) != 0:
                for n in nouns:
                    if n in count_dict:
                        count_dict[n] += 1
                    else:
                        count_dict[n] = 1
                tokenized_lines.append(nouns)

        return tokenized_lines

def main(files):
    for file in files:
        if file.endswith('.txt'):
            texts = []
            basename = os.path.basename(file).split('.txt')[0]
            with open(file, 'r', encoding='utf-8') as f:
                for line in f:
                    texts.append(line.strip())
            join_text = "".join(texts)
            cleaned_text = re.sub(r"●|•|ㅇ|■", "", join_text)
            texts =  [t + '.' for t in cleaned_text.split(". ")]
            # 빈도 분석 객체 생성
            fa = FrequencyAnalysis()

            # 형태소 분석 명사 추출
            Tokens = fa.preprocess_text(texts)

            # 워드클라우드 생성
            word_count = fa.wordcloud_Rank(Tokens, 50, white = True, outdir = 'out', filename=basename)
            print(word_count.head(10))

            # 키워드 네트워크
            Edge_list = fa.make_Edgelist(Tokens)
            fa.Top_Keyword_Edgelist(Tokens, Edge_list, 50, outdir = 'out', node_size = 50, edge_size = 6, filename=basename)

def find_most_frequent_elements(strings):
    frequency = {}
    max_frequency = 0

    # Count the frequency of each element
    for string in strings:
        if string in frequency:
            frequency[string] += 1
        else:
            frequency[string] = 1

        # Update max_frequency if necessary
        if frequency[string] > max_frequency:
            max_frequency = frequency[string]

    most_frequent_elements = []

    # Find elements with maximum frequency
    for string, count in frequency.items():
        if count == max_frequency:
            most_frequent_elements.append(string)

    return most_frequent_elements

# Example usage
strings = ["apple", "banana", "apple", "orange", "banana", "apple"]
most_frequent = find_most_frequent_elements(strings)
print("Most frequent elements:", most_frequent)

if __name__ == '__main__':
    # 데이터 로딩
    dl = DataLoader()
    files = dl.get_file_list('data')

    # 분석
    main(files)

    # 문서와 라벨을 생성한다.
    documents = []
    if os.path.isfile('data/pkl/idf.pkl'):
        with open('data/pkl/idf.pkl', 'rb') as f:
           documents = pickle.load(f)
    else:
        for file in files:
            texts = []
            basename = os.path.basename(file).split('.txt')[0]
            with open(file, 'r', encoding='utf-8') as f:
                for line in f:
                    texts.append(line.strip())
            join_text = "".join(texts)
            cleaned_text = re.sub(r"●|•|ㅇ|■", "", join_text)
            texts =  [t + '.' for t in cleaned_text.split(". ")]
            # 빈도 분석 객체 생성
            fa = FrequencyAnalysis()

            # 형태소 분석 명사 추출
            temp_lines = []
            Tokens = fa.preprocess_text(texts)
            for t in Tokens:
                temp_lines.append(' '.join(t))
            documents.append((" ".join(temp_lines), basename))
        with open('data/pkl/idf.pkl', 'wb') as f:
            pickle.dump(documents, f, pickle.HIGHEST_PROTOCOL)
    # TF-IDF 벡터를 생성
    vectorizer = TfidfVectorizer()
    tfidf_matrix = vectorizer.fit_transform([doc[0] for doc in documents])
    outdir = 'out/tf-idf'
    # 각 문서의 중요한 단어를 추출
    for i, doc in enumerate(documents):
        if not os.path.exists(outdir):
            os.makedirs(outdir)
        filename = outdir + f'/{doc[1]}'
        tfidf_scores = dict(zip(vectorizer.get_feature_names_out(), tfidf_matrix[i].toarray().flatten()))
        sorted_scores = sorted(tfidf_scores.items(), key=lambda item: item[1], reverse=True)
        top_scores = sorted_scores[:20]  # 상위 n개 단어를 선택
        plot_title = f"대선별 선거 공약집에서 각 단어의 중요도({doc[1]})"
        plt.figure(figsize=(10, 5))
        plt.bar(*zip(*top_scores))
        plt.title(plot_title, y=1.05)
        plt.xlabel('상위 20개 단어', labelpad=10)
        plt.ylabel('TF-IDF 점수', labelpad=10)
        plt.savefig(f'{filename}.jpg')

    # # LDA 모델 생성
    # lda = LatentDirichletAllocation(n_components=10, random_state=0)
    # lda_doc = [d[0].split(' ') for d in documents]
    # # tf-idf 벡터에 대해 LDA 학습
    # from gensim import corpora
    # dictionary = corpora.Dictionary(lda_doc)
    # corpus = [dictionary.doc2bow(text) for text in lda_doc]
    # # print(corpus[1]) # 수행된 결과에서 두번째 뉴스 출력. 첫번째 문서의 인덱스는 0
    # import gensim
    # NUM_TOPICS = 10 # 20개의 토픽, k=20
    # ldamodel = gensim.models.ldamodel.LdaModel(corpus, num_topics = NUM_TOPICS, id2word=dictionary, passes=15)
    # topics = ldamodel.print_topics(num_words=4)
    # # for topic in topics:
    # #     print(topic)
    # # lda.fit(tfidf_matrix)
    # # lda 시각화

    # import pyLDAvis.gensim_models

    # pyLDAvis.enable_notebook()
    # vis = pyLDAvis.gensim_models.prepare(ldamodel, corpus, dictionary)
    # pyLDAvis.display(vis)
    # for i, topic_list in enumerate(ldamodel[corpus]):
    #     if i==5:
    #         break
    #     print(i,'번째 문서의 topic 비율은',topic_list)

    # def make_topictable_per_doc(ldamodel, corpus):
    #     topic_table = pd.DataFrame()

    #     # 몇 번째 문서인지를 의미하는 문서 번호와 해당 문서의 토픽 비중을 한 줄씩 꺼내온다.
    #     for i, topic_list in enumerate(ldamodel[corpus]):
    #         doc = topic_list[0] if ldamodel.per_word_topics else topic_list
    #         doc = sorted(doc, key=lambda x: (x[1]), reverse=True)
    #         # 각 문서에 대해서 비중이 높은 토픽순으로 토픽을 정렬한다.
    #         # EX) 정렬 전 0번 문서 : (2번 토픽, 48.5%), (8번 토픽, 25%), (10번 토픽, 5%), (12번 토픽, 21.5%),
    #         # Ex) 정렬 후 0번 문서 : (2번 토픽, 48.5%), (8번 토픽, 25%), (12번 토픽, 21.5%), (10번 토픽, 5%)
    #         # 48 > 25 > 21 > 5 순으로 정렬이 된 것.

    #         # 모든 문서에 대해서 각각 아래를 수행
    #         for j, (topic_num, prop_topic) in enumerate(doc): #  몇 번 토픽인지와 비중을 나눠서 저장한다.
    #             if j == 0:  # 정렬을 한 상태이므로 가장 앞에 있는 것이 가장 비중이 높은 토픽
    #                 topic_table = topic_table._append(pd.Series([int(topic_num), round(prop_topic,4), topic_list]), ignore_index=True)
    #                 # 가장 비중이 높은 토픽과, 가장 비중이 높은 토픽의 비중과, 전체 토픽의 비중을 저장한다.
    #             else:
    #                 break
    #     return(topic_table)
    # topictable = make_topictable_per_doc(ldamodel, corpus)
    # topictable = topictable.reset_index() # 문서 번호을 의미하는 열(column)로 사용하기 위해서 인덱스 열을 하나 더 만든다.
    # topictable.columns = ['문서 번호', '가장 비중이 높은 토픽', '가장 높은 토픽의 비중', '각 토픽의 비중']
    # topictable[:10]
    # # 각 문서에서의 주제 비율 추출
    # topic_proportions = lda.transform(tfidf_matrix)

    # # 주제 비율을 데이터프레임으로 변환
    # df_topic_proportions = pd.DataFrame(topic_proportions)

    # n_top_words = 10 # 각 주제에서 상위 10개 단어를 확인
    # feature_names = vectorizer.get_feature_names_out()

    # topic_labels = []
    # for topic_idx, topic in enumerate(lda.components_):
    #     top_features_ind = topic.argsort()[:-n_top_words - 1:-1]
    #     top_features = [feature_names[i] for i in top_features_ind]
    #     topic_labels.append(", ".join(top_features)) # 주제 라벨 생성
    # topic_labels = find_most_frequent_elements(topic_labels)
    # topic_labels = topic_labels[0].split(', ')
    # outdir = 'out/lda'
    # if not os.path.exists(outdir):
    #     os.makedirs(outdir)
    # filename = outdir + f'/{doc[1]}'

    # # 데이터프레임에서 각 문서의 주제 비율을 파이 차트로 그리기
    # for i in range(len(df_topic_proportions)):
    #     plt.figure(i)
    #     plt.pie(df_topic_proportions.iloc[i], labels=topic_labels)
    #     plt.title(f'Document {i+1} Topic Proportions')
    #     plt.savefig(f'{filename}.jpg')


import mpl_toolkits.mplot3d.axes3d as axes3d
import numpy as np
import matplotlib.pyplot as plt
import h5py as h5

points = h5.File('od_master.h5')['data']['neutron_xyz']

Height = 172 #cm
Radius = 64 # cm

fig = plt.figure(figsize = (10,8))
ax = fig.add_subplot(1, 1, 1, projection='3d')

u = np.linspace(0, np.pi*2, 60)
t = np.linspace(0, Radius, 60)
z = np.linspace(-Height/2, Height/2, 2)

#Defining the parametric space for making the top and bottom circles
U, T = np.meshgrid(u, t)

#For top and bottom
X1 = T*np.cos(U)
Y1 = T*np.sin(U)
ZTop = Height/2*np.ones(X1.shape)
ZBottom = -Height/2*np.ones(X1.shape)

#Defining the parametric space for making the cylindrical column
z = np.linspace(-Height/2, Height/2, 2)
u, z = np.meshgrid(u, z)

x = Radius * np.cos(u)
y = Radius * np.sin(u)

#Column
ax.plot_surface(x, y, z, color='b', alpha = 0.3)
#Top & Bottom
ax.plot_surface(X1, Y1, ZTop, alpha=0.3, color='cyan', rstride=6, cstride=12)
ax.plot_surface(X1, Y1, ZBottom, alpha=0.7, color='green', rstride=6, cstride=12)
for point in points:
    ax.scatter(point[0], point[1], point[2], color = 'b')
plt.show()