import math
import numpy as np

from egse.bits import set_bit
from egse.coordinates.reference_frame import ReferenceFrame
from egse.hexapod.symetrie import logger
from egse.hexapod.symetrie.pmac import decode_Q36


class HexapodSimulator:
    """
    HexapodSimulator simulates the Symétrie Hexapod. The class is heavily based on the
    ReferenceFrames in the `egse.coordinates` package.

    The simulator implements the same methods as the HexapodController class which acts on the
    real hardware controller in either simulation mode or with a real Hexapod PUNA connected.

    Therefore, the HexapodSimulator can be used instead of the Hexapod class in test harnesses
    and when the hardware is not available.

    This class simulates all the movements and status of the Hexapod.
    """

    def __init__(self):
        identity = np.identity(4)

        # Rotation around static axis, and around x, y and z in that order
        self.rot_config = "sxyz"

        # Configure the Master Reference Frame
        self.cs_master = ReferenceFrame.create_master()

        # Configure the Machine Coordinate System, i.e. cs_mec [ref:cs_master]
        self.cs_machine = ReferenceFrame(
            transformation=identity,
            ref=self.cs_master,
            name="Machine[Master]",
            rot_config=self.rot_config,
        )

        # Configure the Platform Coordinate System, i.e. cs_platform [ref:cs_machine]
        # default after homing: PLATFORM = MACHINE

        self.cs_platform = ReferenceFrame(
            transformation=identity,
            ref=self.cs_machine,
            name="Platform[Machine]",
            rot_config=self.rot_config,
        )

        # Configure the User Coordinate System, i.e. cs_user [ref:cs_machine]
        self.cs_user = ReferenceFrame(
            transformation=identity,
            ref=self.cs_machine,
            name="User[Machine]",
            rot_config=self.rot_config,
        )

        # Configure the Object Coordinate System, i.e. cs_object [ref:cs_platform]
        self.cs_object = ReferenceFrame(
            transformation=identity,
            ref=self.cs_platform,
            name="Object[Platform]",
            rot_config=self.rot_config,
        )

        # We use a CS called cs_object_in_user, i.e. Object as defined in the User CS,
        # and we define this
        # from the transformation user -> object.

        tf_user_to_object = self.cs_user.get_active_transformation_to(self.cs_object)
        self.cs_object_in_user = ReferenceFrame(
            tf_user_to_object, rot_config=self.rot_config, ref=self.cs_user, name="Object[User]"
        )

        # Define the invariant links within the system, i.e. some systems are bound with an
        # invariant transformation
        # matrix and those links shall be preserved throughout the movement within the system.

        # We link this cs_object_in_user to cs_object with the identity transformation,
        # which connects them together

        self.cs_object_in_user.add_link(self.cs_object, transformation=identity)

        # The User Coordinate System is linked to the Machine Coordinate System

        self.cs_machine.add_link(self.cs_user, transformation=self.cs_user.transformation)

        # The Object Coordinate System is linked to the Platform Coordinate System

        self.cs_platform.add_link(self.cs_object, transformation=self.cs_object.transformation)

        # Keep a record if the homing() command has been executed.

        self.homing_done = False
        self.control_loop = False
        self._virtual_homing = False
        self._virtual_homing_position = None

        # Just keep the speed settings, no used in movement currently

        self._speed = [1.0, 1.0, 0.01, 0.001, 4.0, 2.0]

        # Print out some debugging information

        logger.debug(f"Linked to cs_object_in_user  {[i.name for i in self.cs_object_in_user.linked_to]}")
        logger.debug(f"Linked to cs_object          {[i.name for i in self.cs_object.linked_to]}")
        logger.debug(f"Linked to cs_platform        {[i.name for i in self.cs_platform.linked_to]}")
        logger.debug(f"Linked to cs_machine         {[i.name for i in self.cs_machine.linked_to or {}]}")

    def is_simulator(self):
        return True

    def connect(self):
        pass

    def reconnect(self):
        pass

    def disconnect(self):
        # TODO:
        #   Should I keep state in this class to check if it has been disconnected?
        #
        # TODO:
        #   What happens when I re-connect to this Simulator? Shall it be in Homing position or
        #   do I have to keep state via a persistency mechanism?
        pass

    def is_connected(self):
        return True

    def reset(self, wait=True, verbose=False):
        # TODO:
        #   Find out what exactly a reset() should be doing. Does it bring back the Hexapod
        #   in it's original state, loosing all definitions of coordinate systems? Or does it
        #   do a clearError() and a homing()?
        pass

    def homing(self):
        self.goto_zero_position()
        self.homing_done = True
        self._virtual_homing = False
        self._virtual_homing_position = None
        return 0

    def is_homing_done(self):
        return self.homing_done

    def set_virtual_homing(self, tx, ty, tz, rx, ry, rz):
        self._virtual_homing_position = [tx, ty, tz, rx, ry, rz]
        self._virtual_homing = True
        return 0

    def stop(self):
        pass

    def clear_error(self):
        return 0

    def activate_control_loop(self):
        self.control_loop = True
        return self.control_loop

    def deactivate_control_loop(self):
        self.control_loop = False
        return self.control_loop

    def configure_coordinates_systems(self, tx_u, ty_u, tz_u, rx_u, ry_u, rz_u, tx_o, ty_o, tz_o, rx_o, ry_o, rz_o):
        identity = np.identity(4)

        # Redefine the User Coordinate System

        translation = np.array([tx_u, ty_u, tz_u])
        rotation = np.array([rx_u, ry_u, rz_u])
        degrees = True

        # Remove the old links between user and machine CS, and between Object in User and Object CS

        self.cs_machine.remove_link(self.cs_user)
        self.cs_object_in_user.remove_link(self.cs_object)

        # Redefine the User Coordinate System

        self.cs_user = ReferenceFrame.from_translation_rotation(
            translation,
            rotation,
            rotation_config=self.rot_config,
            reference_frame=self.cs_machine,
            name="User[Machine]",
            degrees=degrees,
        )

        # Redefine the Object in User Coordinate System

        tf_user_to_object = self.cs_user.get_active_transformation_to(self.cs_object)
        self.cs_object_in_user = ReferenceFrame(
            tf_user_to_object, rot_config=self.rot_config, ref=self.cs_user, name="Object[User]"
        )

        # Define the invariant links within the system, i.e. some systems are bound with an
        # invariant transformation
        # matrix and those links shall be preserved throughout the movement within the system.

        # User and Machine CS are invariant, reset the transformation. User in Object is
        # identical to Object

        self.cs_machine.add_link(self.cs_user, transformation=self.cs_user.transformation)
        self.cs_object_in_user.add_link(self.cs_object, transformation=identity)

        # Redefine the Object Coordinates System

        translation = np.array([tx_o, ty_o, tz_o])
        rotation = np.array([rx_o, ry_o, rz_o])
        degrees = True

        # Remove the old links between user and machine CS, and between Object in User and Object CS

        self.cs_platform.remove_link(self.cs_object)
        self.cs_object_in_user.remove_link(self.cs_object)

        self.cs_object = ReferenceFrame.from_translation_rotation(
            translation,
            rotation,
            rotation_config=self.rot_config,
            reference_frame=self.cs_platform,
            name="Object[Platform]",
            degrees=degrees,
        )

        # Redefine the Object in User Coordinate System

        tf_user_to_object = self.cs_user.get_active_transformation_to(self.cs_object)
        self.cs_object_in_user = ReferenceFrame(
            tf_user_to_object, rot_config=self.rot_config, ref=self.cs_user, name="Object[User]"
        )

        # Object CS and Platform CS are invariant, reset the transformation. User in Object is
        # identical to Object

        self.cs_platform.add_link(self.cs_object, transformation=self.cs_object.transformation)
        self.cs_object_in_user.add_link(self.cs_object, transformation=identity)

        return 0

    def get_coordinates_systems(self):
        degrees = True

        t_user, r_user = self.cs_user.get_translation_rotation_vectors(degrees=degrees)
        t_object, r_object = self.cs_object.get_translation_rotation_vectors(degrees=degrees)

        return list(np.concatenate((t_user, r_user, t_object, r_object)))

    def move_absolute(self, tx, ty, tz, rx, ry, rz):
        # FIXME:
        #  to really simulate the behavior of the Hexapod, this method should implement limit
        #  checking and other condition or error checking, e.g. argument matching, etc.

        logger.debug(f"moveAbsolute with {tx}, {ty}, {tz}, {rx}, {ry}, {rz}")

        translation = np.array([tx, ty, tz])
        rotation = np.array([rx, ry, rz])

        # We set a new transformation for cs_object_in_user which will update our model,
        # because cs_object and cs_object_in_user are linked.

        self.cs_object_in_user.set_translation_rotation(
            translation,
            rotation,
            rotation_config=self.rot_config,
            active=True,
            degrees=True,
            preserve_links=True,
        )

        return 0

    def move_relative_object(self, tx, ty, tz, rx, ry, rz):
        tr_rel = np.array([tx, ty, tz])
        rot_rel = np.array([rx, ry, rz])

        self.cs_object.apply_translation_rotation(
            tr_rel,
            rot_rel,
            rotation_config=self.rot_config,
            active=True,
            degrees=True,
            preserve_links=True,
        )

    def move_relative_user(self, tx, ty, tz, rx, ry, rz):
        # The Symétrie Hexapod definition of moveRelativeUser
        #
        # - Translation and rotations are expressed in USER reference frame
        #
        # - Actually,
        #     - the translations happen parallel to the USER reference frame
        #     - the rotations are applied after the translations, on the OBJ ReferenceFrame
        #
        # To achieve this,
        #
        #     * OBUSR is "de-rotated", to become parallel to USR
        #     * the requested translation is applied
        #     * OBUSR is "re-rotated" to its original orientation
        #     * the requested rotation is applied

        # Derotation of cs_object --> cs_user

        derotation = self.cs_object.get_active_transformation_to(self.cs_user)
        derotation[:3, 3] = [0, 0, 0]

        # Reverse rotation

        rerotation = derotation.T

        # Requested translation matrix

        translation = np.identity(4)
        translation[:3, 3] = [tx, ty, tz]

        # Requested rotation matrix

        import egse.coordinates.transform3d_addon as t3add

        rotation = t3add.translation_rotation_to_transformation(
            [0, 0, 0], [rx, ry, rz], rotation_config=self.rot_config
        )

        transformation = derotation @ translation @ rerotation @ rotation

        # Adapt our model

        self.cs_object.apply_transformation(transformation)

    def check_absolute_movement(self, tx, ty, tz, rx, ry, rz):
        rc = 0
        rc_dict = {}
        if not -30.0 <= tx <= 30.0:
            rc += 1
            rc_dict.update({1: "Tx should be in range ±30.0 mm"})
        if not -30.0 <= ty <= 30.0:
            rc += 1
            rc_dict.update({2: "Ty should be in range ±30.0 mm"})
        if not -20.0 <= tz <= 20.0:
            rc += 1
            rc_dict.update({3: "Tz should be in range ±20.0 mm"})
        if not -11.0 <= rx <= 11.0:
            rc += 1
            rc_dict.update({4: "Rx should be in range ±11.0 mm"})
        if not -11.0 <= ry <= 11.0:
            rc += 1
            rc_dict.update({5: "Ry should be in range ±11.0 mm"})
        if not -20.0 <= rz <= 20.0:
            rc += 1
            rc_dict.update({6: "Rz should be in range ±20.0 mm"})
        return rc, rc_dict

    def check_relative_object_movement(self, tx, ty, tz, rx, ry, rz):
        return 0, {}

    def check_relative_user_movement(self, tx, ty, tz, rx, ry, rz):
        return 0, {}

    def get_user_positions(self):
        t, r = self.cs_user.get_active_translation_rotation_vectors_to(self.cs_object_in_user)

        pos = list(np.concatenate((t, r)))

        return pos

    def get_machine_positions(self):
        t, r = self.cs_platform.get_translation_rotation_vectors()
        t, r = self.cs_machine.get_active_translation_rotation_vectors_to(self.cs_platform)

        pos = list(np.concatenate((t, r)))

        return pos

    def get_actuator_length(self):
        alen = [math.nan for _ in range(6)]

        return alen

    def get_general_state(self):
        state = 0
        state = set_bit(state, 1)  # System Initialized
        state = set_bit(state, 2)  # In Position
        if self.homing_done:
            state = set_bit(state, 4)
        if self.control_loop:
            state = set_bit(state, 3)
        if self._virtual_homing:
            state = set_bit(state, 18)
        return state, decode_Q36(state)

    def goto_specific_position(self, pos):
        return 0

    def goto_retracted_position(self):
        translation = np.array([0, 0, -20])
        rotation = np.array([0, 0, 0])

        self.cs_platform.set_translation_rotation(
            translation,
            rotation,
            rotation_config=self.rot_config,
            active=True,
            degrees=True,
            preserve_links=True,
        )

        return 0

    def goto_zero_position(self):
        # We set a new transformation for cs_platform which will update our model.
        # See issue #58: updating the cs_platform is currently not a good idea because that will
        # not properly update the chain/path upwards, i.e. the chain/path is followed in the
        # direction of the references that are defined, not in the other direction.
        #
        translation = np.array([0, 0, 0])
        rotation = np.array([0, 0, 0])

        self.cs_platform.set_translation_rotation(
            translation,
            rotation,
            rotation_config=self.rot_config,
            active=True,
            degrees=True,
            preserve_links=True,
        )

        # As a work around for the bug in issue #58, we determine the transformation from
        # cs_object as it is
        # invariantly linked with cs_platform. Updating cs_object_in_user with that
        # transformation will
        # properly update all the reference frames.

        # tr_abs, rot_abs = self.cs_object.getTranslationRotationVectors()
        #
        # self.cs_object_in_user.setTranslationRotation(
        #     tr_abs,
        #     rot_abs,
        #     rot_config=self.rot_config,
        #     active=True,
        #     degrees=True,
        #     preserveLinks=True,
        # )

        return 0

    def is_in_position(self):
        return True

    def jog(self, axis: int, inc: float) -> int:
        pass

    def get_debug_info(self):
        pass

    def set_speed(self, vt, vr):
        self._speed[0] = vt
        self._speed[1] = vr

    def get_speed(self):
        return tuple(self._speed)

    def get_actuator_state(self):
        return [
            (
                {
                    0: "In position",
                    1: "Control loop on servo motors active",
                    2: "Homing done",
                    4: 'Input "Positive limit switch"',
                    5: 'Input "Negative limit switch"',
                    6: "Brake control output",
                },
                [1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            ),
            (
                {
                    0: "In position",
                    1: "Control loop on servo motors active",
                    2: "Homing done",
                    4: 'Input "Positive limit switch"',
                    5: 'Input "Negative limit switch"',
                    6: "Brake control output",
                },
                [1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            ),
            (
                {
                    0: "In position",
                    1: "Control loop on servo motors active",
                    2: "Homing done",
                    4: 'Input "Positive limit switch"',
                    5: 'Input "Negative limit switch"',
                    6: "Brake control output",
                },
                [1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            ),
            (
                {
                    0: "In position",
                    1: "Control loop on servo motors active",
                    2: "Homing done",
                    4: 'Input "Positive limit switch"',
                    5: 'Input "Negative limit switch"',
                    6: "Brake control output",
                },
                [1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            ),
            (
                {
                    0: "In position",
                    1: "Control loop on servo motors active",
                    2: "Homing done",
                    4: 'Input "Positive limit switch"',
                    5: 'Input "Negative limit switch"',
                    6: "Brake control output",
                },
                [1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            ),
            (
                {
                    0: "In position",
                    1: "Control loop on servo motors active",
                    2: "Homing done",
                    4: 'Input "Positive limit switch"',
                    5: 'Input "Negative limit switch"',
                    6: "Brake control output",
                },
                [1, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
            ),
        ]

    def perform_maintenance(self, axis):
        pass

    def info(self):
        msg = "Info about the PunaSimulator:\n"
        msg += "\n"
        msg += "This Hexapod PUNA Simulator works with several reference frames:\n"
        msg += "  * The machine reference frame\n"
        msg += "  * The platform reference frame\n"
        msg += "  * The object reference frame\n"
        msg += "  * The user reference frame\n\n"
        msg += "Any movement commands result in a transformation of the appropriate coordinate systems."

        logger.info(msg)

        return msg
