#!/usr/bin/env python3

"""
Reads the calibration results from a json file and computes the evaluation metrics
"""

# -------------------------------------------------------------------------------
# --- IMPORTS
# -------------------------------------------------------------------------------

# Standard imports
from math import sin
from random import random
from tf.broadcaster import TransformBroadcaster
from geometry_msgs.msg import Point
from visualization_msgs.msg import *
from interactive_markers.menu_handler import *
from interactive_markers.interactive_marker_server import *
import copy
import json
import os
import argparse
import sys
from collections import OrderedDict

import numpy as np
import cv2
from scipy.spatial import distance
from colorama import Fore, Style
from prettytable import PrettyTable
from copy import deepcopy

# ROS imports
import rospy
from view_controller_msgs.msg import CameraPlacement

# Atom imports
from atom_core.utilities import atomError

# -------------------------------------------------------------------------------
# --- MAIN
# -------------------------------------------------------------------------------

#!/usr/bin/env python

server = None
menu_handler = MenuHandler()
br = None
counter = 0


def frameCallback(msg):
    global counter, br
    time = rospy.Time.now()
    br.sendTransform((0, 0, sin(counter/140.0)*2.0), (0, 0, 0, 1.0), time, "base_link", "moving_frame")
    counter += 1


def processFeedback(feedback):
    s = "Feedback from marker '" + feedback.marker_name
    s += "' / control '" + feedback.control_name + "'"

    mp = ""
    if feedback.mouse_point_valid:
        mp = " at " + str(feedback.mouse_point.x)
        mp += ", " + str(feedback.mouse_point.y)
        mp += ", " + str(feedback.mouse_point.z)
        mp += " in frame " + feedback.header.frame_id

    if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
        rospy.loginfo(s + ": button click" + mp + ".")
    elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
        rospy.loginfo(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
    elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
        rospy.loginfo(s + ": pose changed")
# TODO
#          << "\nposition = "
#          << feedback.pose.position.x
#          << ", " << feedback.pose.position.y
#          << ", " << feedback.pose.position.z
#          << "\norientation = "
#          << feedback.pose.orientation.w
#          << ", " << feedback.pose.orientation.x
#          << ", " << feedback.pose.orientation.y
#          << ", " << feedback.pose.orientation.z
#          << "\nframe: " << feedback.header.frame_id
#          << " time: " << feedback.header.stamp.sec << "sec, "
#          << feedback.header.stamp.nsec << " nsec" )
    elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
        rospy.loginfo(s + ": mouse down" + mp + ".")
    elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
        rospy.loginfo(s + ": mouse up" + mp + ".")
    server.applyChanges()


def alignMarker(feedback):
    pose = feedback.pose

    pose.position.x = round(pose.position.x-0.5)+0.5
    pose.position.y = round(pose.position.y-0.5)+0.5

    rospy.loginfo(feedback.marker_name + ": aligning position = " + str(feedback.pose.position.x) + "," + str(feedback.pose.position.y) + "," + str(feedback.pose.position.z) + " to " +
                  str(pose.position.x) + "," + str(pose.position.y) + "," + str(pose.position.z))

    server.setPose(feedback.marker_name, pose)
    server.applyChanges()


def rand(min_, max_):
    return min_ + random()*(max_-min_)


def makeBox(msg):
    marker = Marker()

    marker.type = Marker.CUBE
    marker.scale.x = msg.scale * 0.45
    marker.scale.y = msg.scale * 0.45
    marker.scale.z = msg.scale * 0.45
    marker.color.r = 0.5
    marker.color.g = 0.5
    marker.color.b = 0.5
    marker.color.a = 1.0

    return marker


def makeBoxControl(msg):
    control = InteractiveMarkerControl()
    control.always_visible = True
    control.markers.append(makeBox(msg))
    msg.controls.append(control)
    return control


def saveMarker(int_marker):
    server.insert(int_marker, processFeedback)


#####################################################################
# Marker Creation

def make6DofMarker(fixed, interaction_mode, position, show_6dof=False):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "world"
    int_marker.pose.position = position
    int_marker.scale = 0.5

    int_marker.name = "simple_6dof"
    int_marker.description = "Simple 6-DOF Control"

    # insert a box
    makeBoxControl(int_marker)
    int_marker.controls[0].interaction_mode = interaction_mode

    if fixed:
        int_marker.name += "_fixed"
        int_marker.description += "\n(fixed orientation)"

    if interaction_mode != InteractiveMarkerControl.NONE:
        control_modes_dict = {
            InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
            InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
            InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"}
        int_marker.name += "_" + control_modes_dict[interaction_mode]
        int_marker.description = "3D Control"
        if show_6dof:
            int_marker.description += " + 6-DOF controls"
        int_marker.description += "\n" + control_modes_dict[interaction_mode]

    if show_6dof:
        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "rotate_x"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 1
        control.orientation.y = 0
        control.orientation.z = 0
        control.name = "move_x"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "rotate_z"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 1
        control.orientation.z = 0
        control.name = "move_z"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "rotate_y"
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

        control = InteractiveMarkerControl()
        control.orientation.w = 1
        control.orientation.x = 0
        control.orientation.y = 0
        control.orientation.z = 1
        control.name = "move_y"
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        if fixed:
            control.orientation_mode = InteractiveMarkerControl.FIXED
        int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)
    menu_handler.apply(server, int_marker.name)


def makeRandomDofMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base_link"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "6dof_random_axes"
    int_marker.description = "6-DOF\n(Arbitrary Axes)"

    makeBoxControl(int_marker)

    control = InteractiveMarkerControl()

    for i in range(3):
        control.orientation.w = rand(-1, 1)
        control.orientation.x = rand(-1, 1)
        control.orientation.y = rand(-1, 1)
        control.orientation.z = rand(-1, 1)
        control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
        int_marker.controls.append(copy.deepcopy(control))
        control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
        int_marker.controls.append(copy.deepcopy(control))

    server.insert(int_marker, processFeedback)


def makeViewFacingMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base_link"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "view_facing"
    int_marker.description = "View Facing 6-DOF"

    # make a control that rotates around the view axis
    control = InteractiveMarkerControl()
    control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    control.orientation.w = 1
    control.name = "rotate"
    int_marker.controls.append(control)

    # create a box in the center which should not be view facing,
    # but move in the camera plane.
    control = InteractiveMarkerControl()
    control.orientation_mode = InteractiveMarkerControl.VIEW_FACING
    control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
    control.independent_marker_orientation = True
    control.name = "move"
    control.markers.append(makeBox(int_marker))
    control.always_visible = True
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)


def makeQuadrocopterMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base_link"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "quadrocopter"
    int_marker.description = "Quadrocopter"

    makeBoxControl(int_marker)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
    int_marker.controls.append(copy.deepcopy(control))
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)


def makeChessPieceMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base_link"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "chess_piece"
    int_marker.description = "Chess Piece\n(2D Move + Alignment)"

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
    int_marker.controls.append(copy.deepcopy(control))

    # make a box which also moves in the plane
    control.markers.append(makeBox(int_marker))
    control.always_visible = True
    int_marker.controls.append(control)

    # we want to use our special callback function
    server.insert(int_marker, processFeedback)

    # set different callback for POSE_UPDATE feedback
    server.setCallback(int_marker.name, alignMarker, InteractiveMarkerFeedback.POSE_UPDATE)


def makePanTiltMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base_link"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "pan_tilt"
    int_marker.description = "Pan / Tilt"

    makeBoxControl(int_marker)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    control.orientation_mode = InteractiveMarkerControl.FIXED
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 0
    control.orientation.z = 1
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    control.orientation_mode = InteractiveMarkerControl.INHERIT
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)


def makeMenuMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "base_link"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "context_menu"
    int_marker.description = "Context Menu\n(Right Click)"

    # make one control using default visuals
    control = InteractiveMarkerControl()
    control.interaction_mode = InteractiveMarkerControl.MENU
    control.description = "Options"
    control.name = "menu_only_control"
    int_marker.controls.append(copy.deepcopy(control))

    # make one control showing a box
    marker = makeBox(int_marker)
    control.markers.append(marker)
    control.always_visible = True
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)
    menu_handler.apply(server, int_marker.name)


def makeMovingMarker(position):
    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "moving_frame"
    int_marker.pose.position = position
    int_marker.scale = 1

    int_marker.name = "moving"
    int_marker.description = "Marker Attached to a\nMoving Frame"

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 1
    control.orientation.y = 0
    control.orientation.z = 0
    control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
    int_marker.controls.append(copy.deepcopy(control))

    control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
    control.always_visible = True
    control.markers.append(makeBox(int_marker))
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)


if __name__ == "__main__":
    ap = argparse.ArgumentParser()
    ap.add_argument("-train_json", "--train_json_file", help="Json file containing input training dataset.", type=str,
                    required=True)

#     rospy.init_node("basic_controls")
#     br = TransformBroadcaster()
#
#     # create a timer to update the published transforms
#     rospy.Timer(rospy.Duration(0.01), frameCallback)
#
#     server = InteractiveMarkerServer("basic_controls")
#
#     menu_handler.insert("First Entry", callback=processFeedback)
#     menu_handler.insert("Second Entry", callback=processFeedback)
#     sub_menu_handle = menu_handler.insert("Submenu")
#     menu_handler.insert("First Entry", parent=sub_menu_handle, callback=processFeedback)
#     menu_handler.insert("Second Entry", parent=sub_menu_handle, callback=processFeedback)
#
#     position = Point(-3, 3, 0)
#     make6DofMarker(False, InteractiveMarkerControl.NONE, position, True)
#     position = Point(0, 3, 0)
#     make6DofMarker(True, InteractiveMarkerControl.NONE, position, True)
#     position = Point(3, 3, 0)
#     makeRandomDofMarker(position)
#     position = Point(-3, 0, 0)
#     make6DofMarker(False, InteractiveMarkerControl.ROTATE_3D, position, False)
    # position = Point(3, 0, 0)
#     make6DofMarker(False, InteractiveMarkerControl.MOVE_3D, position, False)
#     position = Point(-3, -3, 0)
#     makeViewFacingMarker(position)
#     position = Point(0, -3, 0)
#     makeQuadrocopterMarker(position)
#     position = Point(3, -3, 0)
#     makeChessPieceMarker(position)
#     position = Point(-3, -6, 0)
#     makePanTiltMarker(position)
#     position = Point(0, -6, 0)
#     makeMovingMarker(position)
#     position = Point(3, -6, 0)
#     makeMenuMarker(position)
#
#     server.applyChanges()
#
#     rospy.spin()

    # ROS initialization
    publisher = rospy.Publisher('/rviz/camera_placement', CameraPlacement, queue_size=1)
    rospy.init_node('talker', anonymous=True)

    server = InteractiveMarkerServer("basic_controls")

    menu_handler.insert("Circular Motion", callback=processFeedback)

    position = Point(0, 0, 0)

    # --------------------------------------
    # make marker
    # --------------------------------------

    int_marker = InteractiveMarker()
    int_marker.header.frame_id = "world"
    int_marker.pose.position = position
    int_marker.scale = 0.5

    int_marker.name = "simple_6dof"
    int_marker.description = "Simple 6-DOF Control"

    # insert a box
    control = InteractiveMarkerControl()
    control.always_visible = True

    marker = Marker()
    marker.type = Marker.SPHERE
    marker.scale.x = 0.45
    marker.scale.y = 0.45
    marker.scale.z = 0.45
    marker.color.r = 0.0
    marker.color.g = 0.0
    marker.color.b = 0.5
    marker.color.a = 0.3

    control.markers.append(marker)
    # msg.controls.append(control)
    int_marker.controls.append(control)

    int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_3D

    # int_marker.name += "_fixed"
    # int_marker.description += "\n(fixed orientation)"

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 1
    control.orientation.y = 0
    control.orientation.z = 0
    control.name = "move_x"
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 1
    control.orientation.z = 0
    control.name = "move_z"
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    int_marker.controls.append(control)

    control = InteractiveMarkerControl()
    control.orientation.w = 1
    control.orientation.x = 0
    control.orientation.y = 0
    control.orientation.z = 1
    control.name = "move_y"
    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
    int_marker.controls.append(control)

    server.insert(int_marker, processFeedback)
    menu_handler.apply(server, int_marker.name)

    # end of make marker
    # make6DofMarker(False, InteractiveMarkerControl.MOVE_ROTATE_3D, position, True)
    # position = Point(3, -6, 0)
    # makeMenuMarker(position)

    server.applyChanges()

    rospy.spin()

#     topic = '/rviz/current_camera_placement'
#     print('Waiting for first message on topic ' + Fore.BLUE + topic +
#           Style.RESET_ALL + '\nPlease move the rviz viewpoint ...')
#     try:
#         ccp = rospy.wait_for_message(topic, CameraPlacement,  timeout=10)
#     except:
#         atomError('Could not receive message on topic ' + topic)
#
#     print('Received current camera placement')
#
#     cp = deepcopy(ccp)
    rate = rospy.Rate(10)
    alpha = 0
    while not rospy.is_shutdown():

        alpha += 0.01
        # cp.eye.point.x =
        # cp.eye.point.x += 0.1

        # publisher.publish(cp)
        rate.sleep()