"""
.. _example_system_of_frames:

Frame defined with respect to another frame (Person in a train)
===============================================================

In this example, we will see how to define a frame with respect to another frame.

Lets consider a person walking in a train. 
The person is represented by a frame :math:`F` and the train is represented by a frame :math:`E`.
It is possible to represent the position of the person in the train by defining the frame :math:`F` in the frame :math:`E` coordinates.

"""

# %%
# Define the frames
# ---------------------------------------------------
#
# The frame :math:`F` is defined with respect to the frame :math:`E` by setting the parent of the frame :math:`F` to the frame :math:`E`.
#

from py3dframe import Frame, Rotation
import numpy as np

rotation = Rotation.from_euler('xyz', [0, 0, 0], degrees=True)
translation = np.array([1, 2, 3]).reshape(3, 1)

frame_E = Frame.from_rotation(translation=translation, rotation=rotation, convention=0)

rotation = Rotation.from_euler('xyz', [0, 0, 0], degrees=True)
translation = np.array([0, 0, 0]).reshape(3, 1)

frame_F = Frame.from_rotation(translation=translation, rotation=rotation, convention=0, parent=frame_E)

# %% 
# 
# When the frame :math:`E` (train) moves, the frame :math:`F` (person) moves with it.
# However the position of the person in the train does not change, since it is defined with respect to the train.
#
# - ``origin`` is the position of the frame in the local coordinates of the parent frame.
# - ``global_origin`` is the position of the frame in the global coordinates.

print(f"Before train moves :")
print(f"Global train position: {frame_E.global_origin}")
print(f"Global person position: {frame_F.global_origin}")

frame_E.origin = np.array([4, 5, 6]).reshape(3, 1)

print(f"After train moves: ")
print(f"Global train position: {frame_E.global_origin}")
print(f"Global person position: {frame_F.global_origin}")

frame_F.origin = np.array([1, 2, 3]).reshape(3, 1)

print(f"After person moves in the train: ")
print(f"Global train position: {frame_E.global_origin}")
print(f"Global person position: {frame_F.global_origin}")


# %% 
# Perform the transformation between the frames
# ------------------------------------------------
# 
# The transformation from the frame :math:`E` to the frame :math:`F` can be perform 
# using the class :class:`py3dframe.FrameTransform` as follows.
#
# This object will follow the movement of the train and the person, 
# and will always give the correct transformation from the train to the person.

from py3dframe import FrameTransform

transform = FrameTransform(input_frame=frame_E, output_frame=frame_F)

print("Transformation from train to person (before person moves): ")
print(transform.translation)
print(transform.rotation.as_euler('xyz', degrees=True))

frame_F.euler_angles = [0, 0, 90]

print("Transformation from train to person (after person moves): ")
print(transform.translation)
print(transform.rotation.as_euler('xyz', degrees=True))


# %%
#
# This object can be used to transform points or vector from one frame to another.
# The points and vectors can be transformed in both directions, 
# from the train to the person and from the person to the train.
#
# The points and vectors must be :math:`(3, N)` numpy arrays, where :math:`N` is the number of points or vectors to transform.
#

point_E = np.array([1, 2, 3]).reshape(3, 1)
point_F = transform.transform(point=point_E) # In convention 0 : pE = R * pF + T
point_E_bis = transform.inverse_transform(point=point_F)
assert np.allclose(point_E, point_E_bis)

print(f"Point in train coordinates: {point_E.flatten()}")
print(f"Point in person coordinates: {point_F.flatten()}")

vector_E = np.array([1, 2, 3]).reshape(3, 1)
vector_F = transform.transform(vector=vector_E) # In convention 0 : vE = R * vF
vector_E_bis = transform.inverse_transform(vector=vector_F)
assert np.allclose(vector_E, vector_E_bis)

print(f"Vector in train coordinates: {vector_E.flatten()}")
print(f"Vector in person coordinates: {vector_F.flatten()}")