text: #! /usr/bin/env python3
  """
  Differential Drive Robot: Simulator
  Iain Brookshaw
  21 September 2019
  """
  from diff_sim.odometry import DiffDriveOdometry as Odo
  
  import matplotlib.pyplot as plt
  import matplotlib.animation as animation
  
  from typing import Tuple, Dict, List
  import numpy as np
  import time
  
  # ----------------------------------------------------------------------------------------------------------------------
  # Robot Chassis Constants
  # When run as an executable, these may be set by the args
  
  _max_history: int = 1000
  _robot_wheelbase: float = 0.03       # meters
  _robot_wheel_radius: float = 0.01    # meters
  _robot_wheel_v_max: float = 0.2      # meters / second
  _robot_max_motor_power: float = 2.5  # watts
  _wheel_v_noise: float = 0.5          # +/- n% error in velocity calc
  _robot_wheel_max_omega: float = _robot_wheel_v_max/_robot_wheel_radius  # rad/sec
  
  # Aesthetics
  _bg_color = "White"  # "#0d0d0f"
  _text_color = "Black"  # "#fce5c7"
  _robot_color = "DodgerBlue"
  _ground_truth_color = "DodgerBlue"
  _odo_path_color = "Coral"
  #
  _minimum_max_x = 0.1
  _minimum_min_x = -0.1
  _minimum_max_y = 0.1
  _minimum_min_y = -0.1
  
  
  # ----------------------------------------------------------------------------------------------------------------------
  
  
  class SimulationAnimation:
      """
      This is the 2d animated plot that simulated the movement and odometry of
      a differential drive robot. This class contains the odometry system, the
      the ground-truth calculation, noise generation, user interface callbacks
      and the plotting mechanics
      """
  
      def __init__(self, start_pose: Tuple[float, float, float], wheelbase: float, delta_t: float) -> None:
          """
          :param start_pose: the (x,y,theta) original pose of the robot (m, m, rad)
          :param wheelbase:  the distance between the robot's two wheels (m)
          :param delta_t:    the simulation time-step (s)
          """
          self._odo: Odo = Odo(start_pose, wheelbase)  # the noisy computation
          self._gt: Odo = Odo(start_pose, wheelbase)   # the ground truth
          self._delta_t: float = delta_t
  
          # robot speed from user (power to motors)
          self._total_motor_power = 0.5   # watts
          self._delta_motor_power = 0.01  # watts
          self._motor_prop = 0.5
          self._delta_prop = 0.01
  
          # the plot-able paths for ground truth and estimated
          self.ground_truth_path_x: List = [start_pose[0]]
          self.ground_truth_path_y: List = [start_pose[1]]
          self.ground_truth_path_theta: List = [start_pose[2]]
          #
          self.odo_path_x: List = [start_pose[0]]
          self.odo_path_y: List = [start_pose[1]]
          self.odo_path_theta: List = [start_pose[2]]
  
          # Plot data and turn off ALL keybindings
          # the init will re-establish this
          self.fig: plt.Figure = plt.figure(
              figsize=(12, 12),
              facecolor=_bg_color
  
          )
          self.fig.canvas.mpl_disconnect(
              self.fig.canvas.manager.key_press_handler_id)
  
          self._display_text = None
          self._plot_lines = None
          self._ax = None
  
      # --------------------------------------------------------------------------
      # Animation Methods
  
      def connect_up_callbacks(self) -> None:
          """
          hooks up the keystroke callbacks to self.fig
          """
          self.fig.canvas.mpl_connect(
              'key_press_event',
              self._keystroke_handler_cb
          )
  
      def update_animation(self, dunno) -> List:  # TODO: WHAT IS THE dunno arg ??
          """
          This method updates the simulation state (computing new odometry and gt)
          and updates the plot
          """
          # compute and update the ground truth
          l_v, r_v = self._get_wheel_velocities()
          true_pose = self._gt.update_pose(l_v, r_v, self._delta_t)
          self._update_true_pose(true_pose)
  
          # compute and update the noisy odometry pose
          l_nv, r_nv = self._add_noise_to_velocities(l_v, r_v)
          odom_pose = self._odo.update_pose(l_nv, r_nv, self._delta_t)
          self._update_odom_pose(odom_pose)
  
          # compute axes limits
          self._re_scale_axes()
  
          # update and return the plots
          if self._plot_lines:
              self._plot_lines[0].set_data(
                  self.ground_truth_path_x,
                  self.ground_truth_path_y
              )
              self._plot_lines[1].set_data(
                  self.odo_path_x,
                  self.odo_path_y
              )
  
          # update heading arrows
          gt_heading_vector = self._compute_heading_vector(
              true_pose, true_pose[2])
          odo_heading_vector = self._compute_heading_vector(
              odom_pose, odom_pose[2])
  
          # self._plot_lines[2].set_data(
          #     true_pose[0], true_pose[1], gt_heading_vector[0], gt_heading_vector[1])
          # self._plot_lines[3].set_data(
          #     odom_pose[0], odom_pose[1], odo_heading_vector[0], odo_heading_vector[1])
  
          # print summary data:
          self._display_text.set_text(
              """
  pose       = [{0:.2f}, {1:.2f}] mm, {2:.2f} degrees
  wheel_v    = [{3:.2f}, {4:.2f}] m/s, 
  motor_p    = {5:.2f} watts
  pose error = {6:.2f} mm by, Heading Error = {7:.2f} degrees
  """.format(
                  true_pose[0]*1000, true_pose[1]*1000, np.degrees(true_pose[2]),
                  l_v, r_v,
                  self._total_motor_power,
                  1000.0*self._compute_pose_error_abs(odom_pose, true_pose),
                  np.degrees(true_pose[2] - odom_pose[2])
              ))
          self._display_text.set_color(_text_color)
          self._display_text.set_backgroundcolor(_bg_color)
  
          return self._plot_lines
  
      def init_animation(self) -> List:
  
          self._ax = plt.axes(
              xlim=(-0.5, 0.5),  # todo: make args or global parameters
              ylim=(-0.5, 0.5))  # todo: make args or global parameters
          #
          self._ax.set_facecolor(_bg_color)
          self._ax.xaxis.label.set_color(_text_color)
          self._ax.yaxis.label.set_color(_text_color)
          self._ax.spines['bottom'].set_color(_text_color)
          self._ax.spines['left'].set_color(_text_color)
          self._ax.spines['right'].set_color(_text_color)
          self._ax.spines['top'].set_color(_text_color)
          self._ax.tick_params(axis='x', colors=_text_color)
          self._ax.tick_params(axis='y', colors=_text_color)
  
          self._plot_lines = [
              self._ax.plot([], [], lw=1, ls='-', c=_ground_truth_color,
                            label="Ground Truth Pose")[0],
              self._ax.plot([], [], lw=2, ls=':',
                            color=_odo_path_color,
                            markersize=5,
                            label="Odometer Estimate Pose")[0]
          ]
  
          # TODO: show direction arrow
          # self._true_dir_arrow = plt.Arrow(
          #     0, 0, 0, 0, label="Ground Truth Heading")
          # self._odo_dir_arrow = plt.Arrow(
          #     0, 0, 0, 0, label="Odometer Estimate Heading")
  
          # self._plot_lines.append(self._true_dir_arrow)
          # self._plot_lines.append(self._odo_dir_arrow)
  
          self.connect_up_callbacks()
          self._display_text = self._ax.text(0, 0, "",
                                             horizontalalignment='left',
                                             backgroundcolor=_bg_color,
                                             color=_text_color,
                                             linespacing=1.2,
                                             fontfamily='monospace',
                                             fontsize=12)
  
          plt.title("Differential Drive Robot: Odometry Simulation",
                    fontsize=20, color=_text_color)
  
          plt.xlabel("Global X (meters)", fontsize=15)
          plt.ylabel("Global Y (meters)", fontsize=15)
          leg = self._ax.legend(loc='upper right',
                                fontsize=15,
                                fancybox=False,
                                numpoints=1)
          leg.get_frame().set_facecolor(_bg_color)
  
          for text in leg.get_texts():
              text.set_color(_text_color)
  
          plt.grid()
          self._ax.set_aspect('equal', 'box')
  
          return self._plot_lines
  
      # --------------------------------------------------------------------------
      # User robot control keyboard callbacks
  
      def _keystroke_handler_cb(self, event) -> None:
  
          if event.key in ("up", "w"):
              self._total_motor_power += self._delta_motor_power
              if self._total_motor_power > _robot_max_motor_power:
                  self._total_motor_power = _robot_max_motor_power
  
          elif event.key in ("left", "d"):
              self._motor_prop -= self._delta_prop
              if self._motor_prop < -1:
                  self._motor_prop = -1.0
  
          elif event.key in ("right", "a"):
              self._motor_prop += self._delta_prop
              if self._motor_prop > 1:
                  self._motor_prop = 1.0
  
          elif event.key in ("down", "s"):
              self._total_motor_power -= self._delta_motor_power
              if self._total_motor_power < -_robot_max_motor_power:
                  self._total_motor_power = -_robot_max_motor_power
  
      # --------------------------------------------------------------------------
      # Private Methods
  
      def _get_wheel_velocities(self) -> Tuple[float, float]:
          """
          compute the current wheel velocities from the user settings of motor power
          """
          l_motor_power = self._total_motor_power * (self._motor_prop - 1)
          r_motor_power = self._total_motor_power * (self._motor_prop + 1)
  
          vl = (_robot_wheel_max_omega / _robot_max_motor_power) * \
              l_motor_power * _robot_wheel_radius
          vr = (_robot_wheel_max_omega / _robot_max_motor_power) * \
              r_motor_power * _robot_wheel_radius
  
          return (vl, vr)
  
      def _add_noise_to_velocities(self, vl: float, vr: float) -> Tuple[float, float]:
          """
          generate a bit of noise between +/- noise percent
          """
          ln = 1 + np.random.uniform(low=-_wheel_v_noise, high=_wheel_v_noise)
          rn = 1 + np.random.uniform(low=-_wheel_v_noise, high=_wheel_v_noise)
  
          return (vl*ln, vr*rn)
  
      def _update_true_pose(self, true_pose: Tuple[float, float, float]) -> None:
          if _max_history < len(self.ground_truth_path_x):
              self.ground_truth_path_x = self.ground_truth_path_x[: -2]
              self.ground_truth_path_y = self.ground_truth_path_y[: -2]
              self.ground_truth_path_theta = self.ground_truth_path_theta[: -2]
  
          self.ground_truth_path_x.append(true_pose[0])
          self.ground_truth_path_y.append(true_pose[1])
          self.ground_truth_path_theta.append(true_pose[2])
  
      def _update_odom_pose(self, odo_pose: Tuple[float, float, float]) -> None:
          if _max_history < len(self.odo_path_x):
              self.odo_path_x = self.odo_path_x[: -2]
              self.odo_path_y = self.odo_path_y[: -2]
              self.odo_path_theta = self.odo_path_theta[: -2]
  
          self.odo_path_x.append(odo_pose[0])
          self.odo_path_y.append(odo_pose[1])
          self.odo_path_theta.append(odo_pose[2])
  
      def _compute_pose_error_abs(self, odo_pose, true_pose) -> float:
          dx = odo_pose[0] - true_pose[0]
          dy = odo_pose[1] - true_pose[1]
          return float(np.sqrt(dx**2 + dy**2))
  
      def _compute_heading_vector(self, pose, theta):
          return (
              pose[0] + 0.25 * np.cos(theta),
              pose[1] + 0.25 * np.sin(theta)
          )
  
      def _get_axes_limits(self, arr):
          arr_min = 1e6
          arr_max = -1e6
          for entry in arr:
              if arr_min > entry:
                  arr_min = entry
              if arr_max < entry:
                  arr_max = entry
  
          return (1.1*arr_min, 1.1*arr_max)
  
      def _re_scale_axes(self):
          gt_min_x, gt_max_x = self._get_axes_limits(self.ground_truth_path_x)
          gt_min_y, gt_max_y = self._get_axes_limits(self.ground_truth_path_y)
          #
          od_min_x, od_max_x = self._get_axes_limits(self.odo_path_x)
          od_min_y, od_max_y = self._get_axes_limits(self.odo_path_y)
  
          min_x = gt_min_x if gt_min_x < od_min_x else od_min_x
          min_y = gt_min_y if gt_min_y < od_min_y else od_min_y
          max_x = gt_max_x if gt_max_x < od_max_x else od_max_x
          max_y = gt_max_y if gt_max_y < od_max_y else od_max_y
  
          if max_x < _minimum_max_x:
              max_x = _minimum_max_x
  
          if min_x > _minimum_min_x:
              min_x = _minimum_min_x
  
          if max_y < _minimum_max_y:
              max_y = _minimum_max_y
  
          if min_y > _minimum_min_y:
              min_y = _minimum_min_y
  
          self._ax.set_xlim(min_x, max_x)
          self._ax.set_ylim(min_y, max_y)
          self._display_text.set_position((.95*min_x, 0.95*min_y))
  
          # ----------------------------------------------------------------------------------------------------------------------
  if __name__ == "__main__":
  
      # default values. TODO: make args
      start_pose = (0, 0, 0)
      wheelbase = 0.10
      delta_t = 0.01
  
      sim = SimulationAnimation(start_pose, wheelbase, delta_t)
  
      anim = animation.FuncAnimation(
          sim.fig,
          sim.update_animation,
          init_func=sim.init_animation
      )
  
      plt.show()
  