from pathlib import Path

import numpy
from matplotlib import pyplot

from transformations import centroid, calculate_rotation_matrix, convert_rotation_matrix_to_angles

expected_points_file_path = Path('expected_points.txt')
actual_points_file_path = Path('actual_points.txt')

plot_3d = pyplot.figure().add_subplot(projection='3d')


def read_points_from_file(points_file_path: Path) -> numpy.ndarray:
    with points_file_path.open('r') as points_file:
        return numpy.array([[float(coordinate) for coordinate in point_line.strip().split(' ')]
                            for point_line in points_file])


if __name__ == '__main__':
    expected_points = read_points_from_file(expected_points_file_path)
    actual_points = read_points_from_file(actual_points_file_path)

    rotation_matrix = calculate_rotation_matrix(actual_points, expected_points)
    angles = convert_rotation_matrix_to_angles(rotation_matrix)

    actual_points_rotated = numpy.dot(actual_points, rotation_matrix)

    offset = centroid(actual_points_rotated) - centroid(expected_points)
    actual_points_converted = actual_points_rotated - offset

    print(f"{angles = }")
    print(f"{offset = }")
    plot_3d.plot(actual_points_converted[:, 0], actual_points_converted[:, 1], actual_points_converted[:, 2], 'o-', markersize=12, linewidth=3)
    plot_3d.plot(expected_points[:, 0], expected_points[:, 1], expected_points[:, 2], 'o-', markersize=12, linewidth=3)
    plot_3d.grid(True)
    plot_3d.tick_params(labelsize=15)

    pyplot.show()