#!/usr/bin/env python
# -*- encoding: utf-8 -*-
"""
This node computes the distance between two meshes using the Frobenius norm.

**Input(s):**

  * `event_in`: The desired event for the node:

      `e_start`: starts the component.

      `e_stop`: stops the component.

    - *type:* `std_msgs/String`

  * `mesh_reference`: Mesh to measure the distance to the target mesh.

    - *type:* `udom_modeling_msgs/Mesh`

  * `mesh_target`: Mesh to measure the distance to the reference mesh.

    - *type:* `udom_modeling_msgs/Mesh`


**Output(s):**

  * `distance`: The distance between the two meshes.
    - *type:* `std_msgs/Float32`

  * `event_out`: The current event of the node.

      `e_running`: when the component is running.

      `e_stopped`: when the component is stopped.

    - *type:* `std_msgs/String`

**Parameter(s):**

  * `loop_rate`: Node cycle rate (in Hz).

  * `weight_procrustes`: Weight for the Procrustes distance.

  * `weight_frobenius`: Weight for the Frobenius distance.

"""

import numpy as np
import rospy
from dynamic_reconfigure.server import Server
import std_msgs.msg
import udom_modeling_msgs.msg
import udom_geometric_transformation.mesh_measurer_utils as utils
from udom_geometric_transformation.cfg import MeshDisparityConfig as MeshDisparity


class MeshMeasurerNode(object):
    """
    Subscribes to two udom_modeling_msgs.msg.Mesh topics, computes their distance
    and publishes it as a std_msgs/Float32 message.

    """
    def __init__(self):
        """
        Instantiates a mesh measurer node.

        :return: Distance between two meshes.
        :rtype: MeshMeasurerNode

        """
        # Params
        self.event = None
        self.mesh_reference = None
        self.mesh_target = None
        self.distance = std_msgs.msg.Float32()

        # Weight for the Procrustes distance.
        self.weight_procrustes = rospy.get_param('~weight_procrustes', 10.0)

        # Weight for the Frobenius distance.
        self.weight_frobenius = rospy.get_param('~weight_frobenius', 10.0)

        # Node cycle rate (in Hz).
        self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 100))
        # Publishers
        self.event_out = rospy.Publisher("~event_out", std_msgs.msg.String, queue_size=10)
        self.distance_publisher = rospy.Publisher(
            "~distance", std_msgs.msg.Float32, queue_size=1, tcp_nodelay=True)

        # Subscribers
        rospy.Subscriber("~event_in", std_msgs.msg.String, self.event_in_cb)
        rospy.Subscriber(
            '~mesh_reference', udom_modeling_msgs.msg.Mesh, self.mesh_reference_cb)
        rospy.Subscriber('~mesh_target', udom_modeling_msgs.msg.Mesh, self.mesh_target_cb)

        # Dynamic Server Reconfiguration
        dynamic_reconfig_srv = Server(MeshDisparity, self.dynamic_reconfig_cb)

    def dynamic_reconfig_cb(self, config, level):
        """
        Reconfigures the weights for the disparity measure.

        """
        self.weight_procrustes = config.weight_procrustes
        self.weight_frobenius = config.weight_frobenius
        return config

    def event_in_cb(self, msg):
        """
        Obtains the event for the node (e.g. start, stop).

        :param msg: Event message for the node.
        :type msg: std_msgs.msg.String

        """
        self.event = msg.data

    def mesh_reference_cb(self, msg):
        """
        Obtains the reference mesh.

        :param msg: Reference mesh
        :type msg: udom_modeling_msgs.msg.Mesh

        """
        self.mesh_reference = msg

    def mesh_target_cb(self, msg):
        """
        Obtains the target mesh.

        :param msg: Target mesh
        :type msg: udom_modeling_msgs.msg.Mesh

        """
        self.mesh_target = msg

    def start(self):
        """
        Starts the node.

        """
        rospy.loginfo("Ready to start...")
        state = 'INIT'

        while not rospy.is_shutdown():

            if state == 'INIT':
                state = self.init_state()
            elif state == 'IDLE':
                state = self.idle_state()
            elif state == 'RUNNING':
                state = self.running_state()

            rospy.logdebug("State: {0}".format(state))
            self.loop_rate.sleep()

    def init_state(self):
        """
        Executes the INIT state of the state machine.

        :return: The updated state.
        :rtype: str

        """
        if self.event == 'e_start':
            return 'IDLE'
        else:
            return 'INIT'

    def idle_state(self):
        """
        Executes the IDLE state of the state machine.

        :return: The updated state.
        :rtype: str

        """
        if self.event == 'e_stop':
            self.event_out.publish('e_stopped')
            self.reset_component_data()
            return 'INIT'
        elif (self.mesh_reference is not None) and (self.mesh_target is not None):
            return 'RUNNING'
        else:
            return 'IDLE'

    def running_state(self):
        """
        Executes the RUNNING state of the state machine.

        :return: The updated state.
        :rtype: str

        """
        if self.event == 'e_stop':
            self.event_out.publish('e_stopped')
            self.reset_component_data()
            return 'INIT'
        else:
            self.distance.data = self.mesh_distance()
            self.event_out.publish('e_running')
            self.distance_publisher.publish(self.distance)
            self.reset_component_data()
            return 'IDLE'

    def mesh_distance(self):
        """
        Computes the distance between the reference and target meshes using the
        Frobenius norm.

        :return: The distance between the meshes.
        :rtype: Float32

        """
        x = utils.mesh_to_points(self.mesh_reference)
        y = utils.mesh_to_points(self.mesh_target)

        return np.linalg.norm(x - y)

    def reset_component_data(self):
        """
        Clears the data of the component.

        """
        self.mesh_reference = None
        self.mesh_target = None
        self.event = None


def main():
    rospy.init_node("mesh_measurer_node", anonymous=True)
    mesh_measurer_node = MeshMeasurerNode()
    mesh_measurer_node.start()