from pathlib import Path

import numpy
from matplotlib import pyplot

from transformations import centroid, kabsch_algorithm

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)

    expected_points -= centroid(expected_points)
    actual_points -= centroid(actual_points)

    actual_points = numpy.dot(actual_points, kabsch_algorithm(actual_points, expected_points))

    plot_3d.plot(actual_points[:, 0], actual_points[:, 1], actual_points[:, 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()