from evo.tools import log
log.configure_logging()

from evo.tools import plot
from evo.tools.plot import PlotMode
from evo.core.metrics import PoseRelation, Unit
from evo.tools.settings import SETTINGS

# temporarily override some package settings
SETTINGS.plot_figsize = [6, 6]
SETTINGS.plot_split = True
SETTINGS.plot_usetex = False

# magic plot configuration
import matplotlib.pyplot as plt
%matplotlib inline
%matplotlib notebook

# interactive widgets configuration
import ipywidgets

check_opts_ape = {"align": False, "correct_scale": False, "show_plot": True}
check_boxes_ape=[ipywidgets.Checkbox(description=desc, value=val) for desc, val in check_opts_ape.items()]
check_opts_rpe = {"align": False, "correct_scale": False, "all_pairs": False, "show_plot": True}
check_boxes_rpe=[ipywidgets.Checkbox(description=desc, value=val) for desc, val in check_opts_rpe.items()]
delta_input = ipywidgets.FloatText(value=1.0, description='delta', disabled=False, color='black')
delta_unit_selector=ipywidgets.Dropdown(
    options={u.value: u for u in Unit if u is not Unit.seconds},
    value=Unit.frames, description='delta_unit'
)
plotmode_selector=ipywidgets.Dropdown(
    options={p.value: p for p in PlotMode},
    value=PlotMode.xy, description='plot_mode'
)
pose_relation_selector=ipywidgets.Dropdown(
    options={p.value: p for p in PoseRelation},
    value=PoseRelation.translation_part, description='pose_relation'
)

"""
Explanation: Interactive Demo for Metrics

command line executables: see README.md
algorithm documentation: metrics.py API & Algorithm Documentation

...some modules and settings for this demo:
End of explanation
"""


from evo.tools import file_interface
from evo.core import sync

"""
Explanation: Load trajectories
End of explanation
"""


traj_ref = file_interface.read_kitti_poses_file("../test/data/KITTI_00_gt.txt")
traj_est = file_interface.read_kitti_poses_file("../test/data/KITTI_00_ORB.txt")

"""
Explanation: Load KITTI files with entries of the first three rows of $\mathrm{SE}(3)$ matrices per line (no timestamps):
End of explanation
"""


from rosbags.rosbag1 import Reader as Rosbag1Reader
with Rosbag1Reader("../test/data/ROS_example.bag") as reader:
    traj_ref = file_interface.read_bag_trajectory(reader, "groundtruth")
    traj_est = file_interface.read_bag_trajectory(reader, "ORB-SLAM")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)

"""
Explanation: ...or load a ROS bagfile with geometry_msgs/PoseStamped, geometry_msgs/TransformStamped, geometry_msgs/PoseWithCovarianceStamped or nav_msgs/Odometry topics:
End of explanation
"""


traj_ref = file_interface.read_tum_trajectory_file("../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file("../test/data/fr2_desk_ORB_kf_mono.txt")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)

print(traj_ref)
print(traj_est)

"""
Explanation: ... or load TUM files with 3D position and orientation quaternion per line ($x$ $y$ $z$ $q_x$ $q_y$ $q_z$ $q_w$):
End of explanation
"""


import evo.main_ape as main_ape
import evo.common_ape_rpe as common

count = 0
results = []

def callback_ape(pose_relation, align, correct_scale, plot_mode, show_plot):
    global results, count
    est_name="APE Test #{}".format(count)
    
    result = main_ape.ape(traj_ref, traj_est, est_name=est_name,
                          pose_relation=pose_relation, align=align, correct_scale=correct_scale)
    count += 1
    results.append(result)
    
    if show_plot:
        fig = plt.figure()
        ax = plot.prepare_axis(fig, plot_mode)
        plot.traj(ax, plot_mode, traj_ref, style="--", alpha=0.5)
        plot.traj_colormap(
            ax, result.trajectories[est_name], result.np_arrays["error_array"], plot_mode,
            min_map=result.stats["min"], max_map=result.stats["max"])
    
_ = ipywidgets.interact_manual(callback_ape, pose_relation=pose_relation_selector, plot_mode=plotmode_selector,
                               **{c.description: c.value for c in check_boxes_ape})

"""
Explanation: APE
Algorithm and API explanation: see here
Interactive APE Demo
Run the code below, configure the parameters in the GUI and press the update button.
(uses the trajectories loaded above)
End of explanation
"""


import evo.main_rpe as main_rpe

count = 0
results = []

def callback_rpe(pose_relation, delta, delta_unit, all_pairs, align, correct_scale, plot_mode, show_plot):
    global results, count
    est_name="RPE Test #{}".format(count)
    result = main_rpe.rpe(traj_ref, traj_est, est_name=est_name,
                          pose_relation=pose_relation, delta=delta, delta_unit=delta_unit, 
                          all_pairs=all_pairs, align=align, correct_scale=correct_scale, 
                          support_loop=True)
    count += 1
    results.append(result)
    
    if show_plot:
        fig = plt.figure()
        ax = plot.prepare_axis(fig, plot_mode)
        plot.traj(ax, plot_mode, traj_ref, style="--", alpha=0.5)
        plot.traj_colormap(
            ax, result.trajectories[est_name], result.np_arrays["error_array"], plot_mode,
            min_map=result.stats["min"], max_map=result.stats["max"])

_ = ipywidgets.interact_manual(callback_rpe, pose_relation=pose_relation_selector, plot_mode=plotmode_selector, 
                               delta=delta_input, delta_unit=delta_unit_selector, 
                               **{c.description: c.value for c in check_boxes_rpe})

"""
Explanation: RPE
Algorithm and API explanation: see here
Interactive RPE Demo
Run the code below, configure the parameters in the GUI and press the update button.
(uses the trajectories loaded above, alignment only useful for visualization here)
End of explanation
"""


import pandas as pd
from evo.tools import pandas_bridge

df = pd.DataFrame()
for result in results:
    df = pd.concat((df, pandas_bridge.result_to_df(result)), axis="columns")
df

df.loc["stats"]

"""
Explanation: Do stuff with the result objects:
End of explanation
"""


