import numpy as np

from coordinate import Coordinate


class Cartesian2D(Coordinate):

    def __init__(self, x, y):
        self.x = x
        self.y = y

    def __str__(self):
        return f'Cartesian2D({self.x}, {self.y})'

    def distance_to(self, new_coord):
        assert isinstance(new_coord, Cartesian2D), 'new_coord must also be a Cartesian2D'
        squared = (new_coord.x - self.x) ** 2 + (new_coord.y - self.y) ** 2
        return np.sqrt(squared)

    def toroidal_position(self, grid_size):
        x_out = self.x % grid_size
        y_out = self.y % grid_size
        out_coord = Cartesian2D(x_out, y_out)
        return out_coord

    def angle_to(self, new_coord):
        assert isinstance(new_coord, Cartesian2D), 'new_coord must also be a Cartesian2D'
        dx = new_coord.x - self.x
        dy = new_coord.y - self.y
        theta = np.arctan2(dy, dx)
        return theta

    def get_unit_vector(self, new_coord):
        assert isinstance(new_coord, Cartesian2D), 'new_coord must also be a Cartesian2D'
        dx = new_coord.x - self.x
        dy = new_coord.y - self.y
        norm = np.sqrt(dx ** 2 + dy ** 2)
        return dx / norm, dy / norm


def main():
    # coord1 = Cartesian2D(1,1)
    # print(coord1)
    # coord2 = Cartesian2D(1,0)

    # print(coord1.angle_to(coord2))
    # print(coord1.get_unit_vector(coord2))

    coord3 = Cartesian2D(11,-3)
    print(coord3.toroidal_position(5))

if __name__ == '__main__':
    main()