"""
Polygon module
SPDX - License - Identifier: LGPL - 3.0 - or -later
Copyright © 2022 Concordia CERC group
Project Coder Pilar Monsalvete Alvarez de Uribarri pilar.monsalvete@concordia.ca
"""

from __future__ import annotations
import math
import sys
from typing import List
import numpy as np
from trimesh import Trimesh
import trimesh.intersections
import trimesh.creation
import trimesh.geometry
from shapely.geometry.polygon import Polygon as shapley_polygon
from geometry.point import Point
from geometry.plane import Plane

EPSILON = 0.0000001


class Polygon:
    """
    Polygon class
    """

    def __init__(self, coordinates):
        self._area = None
        self._points = None
        self._points_list = None
        self._normal = None
        self._inverse = None
        self._edges = None
        self._coordinates = coordinates
        self._triangles = None
        self._vertices = None
        self._faces = None
        self._plane = None

    @property
    def points(self) -> List[Point]:
        """
        Get the points belonging to the polygon [[x, y, z],...]
        :return: [Point]
        """
        if self._points is None:
            self._points = []
            for coordinate in self.coordinates:
                self._points.append(Point(coordinate))
        return self._points

    @property
    def plane(self) -> Plane:
        """
        Get the polygon plane
        :return: Plane
        """
        if self._plane is None:
            self._plane = Plane(normal=self.normal, origin=self.points[0])
        return self._plane

    @property
    def coordinates(self) -> List[np.ndarray]:
        """
        Get the points in the shape of its coordinates belonging to the polygon [[x, y, z],...]
        :return: [np.ndarray]
        """
        return self._coordinates

    def contains_point(self, point):
        """
        Determines if the given point is contained by the current polygon
        :return: boolean
        """
        # fixme: This method doesn't seems to work.
        vertices_length = len(self.vertices)
        angle_sum = 0
        for i in range(0, vertices_length):
            vector_0 = self.vertices[i]
            vector_1 = self.vertices[(i + 1) % vertices_length]
            # set to origin
            vector_0[0] = vector_0[0] - point.coordinates[0]
            vector_0[1] = vector_0[1] - point.coordinates[1]
            vector_0[2] = vector_0[2] - point.coordinates[2]
            vector_1[0] = vector_1[0] - point.coordinates[0]
            vector_1[1] = vector_1[1] - point.coordinates[1]
            vector_1[2] = vector_1[2] - point.coordinates[2]
            module = np.linalg.norm(vector_0) * np.linalg.norm(vector_1)
            scalar_product = np.dot(vector_0, vector_1)
            angle = np.pi / 2
            if module != 0:
                angle = abs(np.arcsin(scalar_product / module))
            angle_sum += angle
        return abs(angle_sum - math.pi * 2) < EPSILON

    def contains_polygon(self, polygon):
        """
        Determines if the given polygon is contained by the current polygon
        :return: boolean
        """

        for point in polygon.points:
            if not self.contains_point(point):
                return False
        return True

    @property
    def points_list(self) -> np.ndarray:
        """
        Get the solid surface point coordinates list [x, y, z, x, y, z,...]
        :return: np.ndarray
        """
        try:
            if self._points_list is None:
                coordinates = self.coordinates
                self._points_list = np.reshape(coordinates, len(coordinates) * 3)
            return self._points_list
        except ValueError as error:
            sys.stderr.write(f'Wrong coordinates: {error}')
            return None

    @property
    def edges(self) -> List[List[Point]]:
        """
        Get polygon edges list
        :return: [[Point]]
        """
        if self._edges is None:
            self._edges = []
            for i in range(0, len(self.points) - 1):
                point_1 = self.points[i]
                point_2 = self.points[i + 1]
                self._edges.append([point_1, point_2])
            self._edges.append([self.points[len(self.points) - 1], self.points[0]])
        return self._edges

    @property
    def area(self):
        """
        Get surface area in square meters
        :return: float
        """
        if self._area is None:
            self._area = 0
            for triangle in self.triangles:
                ab_value = np.zeros(3)
                ac_value = np.zeros(3)
                for i in range(0, 3):
                    ab_value[i] = triangle.coordinates[1][i] - triangle.coordinates[0][i]
                    ac_value[i] = triangle.coordinates[2][i] - triangle.coordinates[0][i]
                self._area += np.linalg.norm(np.cross(ab_value, ac_value)) / 2
        return self._area

    @area.setter
    def area(self, value):
        self._area = value

    @property
    def normal(self) -> np.ndarray:
        """
        Get surface normal vector
        :return: np.ndarray
        """
        if self._normal is None:
            points = self.coordinates
            # todo: IF THE FIRST ONE IS 0, START WITH THE NEXT
            point_origin = points[len(points) - 2]
            vector_1 = points[len(points) - 1] - point_origin
            vector_2 = points[0] - point_origin
            vector_3 = points[1] - point_origin
            cross_product = np.cross(vector_1, vector_2)
            if np.linalg.norm(cross_product) != 0:
                cross_product = cross_product / np.linalg.norm(cross_product)
                alpha = self._angle_between_vectors(vector_1, vector_2)
            else:
                cross_product = [0, 0, 0]
                alpha = 0
            if len(points) == 3:
                return cross_product
            if np.linalg.norm(cross_product) == 0:
                return cross_product
            alpha += self._angle(vector_2, vector_3, cross_product)
            for i in range(0, len(points) - 4):
                vector_1 = points[i + 1] - point_origin
                vector_2 = points[i + 2] - point_origin
                alpha += self._angle(vector_1, vector_2, cross_product)
            vector_1 = points[len(points) - 1] - point_origin
            vector_2 = points[0] - point_origin
            if alpha < 0:
                cross_product = np.cross(vector_2, vector_1)
            else:
                cross_product = np.cross(vector_1, vector_2)
            self._normal = cross_product / np.linalg.norm(cross_product)
        return self._normal

    @staticmethod
    def _angle(vector_1, vector_2, cross_product):
        """
        alpha angle in radians
        :param vector_1: [float]
        :param vector_2: [float]
        :param cross_product: [float]
        :return: float
        """
        accepted_normal_difference = 0.01
        cross_product_next = np.cross(vector_1, vector_2)
        if np.linalg.norm(cross_product_next) != 0:
            cross_product_next = cross_product_next / np.linalg.norm(cross_product_next)
            alpha = Polygon._angle_between_vectors(vector_1, vector_2)
        else:
            cross_product_next = [0, 0, 0]
            alpha = 0
        delta_normals = 0
        for j in range(0, 3):
            delta_normals += cross_product[j] - cross_product_next[j]
        if np.abs(delta_normals) < accepted_normal_difference:
            return alpha
        return -alpha

    @staticmethod
    def triangle_mesh(vertices, normal):
        """
        builds triangle mesh
        :param vertices: the vertices of the triangle
        :param normal: normal to the plane
        :return: trimesh
        """
        min_x = 1e16
        min_y = 1e16
        min_z = 1e16
        for vertex in vertices:
            if vertex[0] < min_x:
                min_x = vertex[0]
            if vertex[1] < min_y:
                min_y = vertex[1]
            if vertex[2] < min_z:
                min_z = vertex[2]
        new_vertices = []
        for vertex in vertices:
            vertex = [vertex[0] - min_x, vertex[1] - min_y, vertex[2] - min_z]
            new_vertices.append(vertex)

        transformation_matrix = trimesh.geometry.plane_transform(origin=new_vertices[0], normal=normal)
        coordinates = []
        for vertex in vertices:
            transformed_vertex = [vertex[0] - min_x, vertex[1] - min_y, vertex[2] - min_z, 1]
            transformed_vertex = np.dot(transformation_matrix, transformed_vertex)
            coordinate = [transformed_vertex[0], transformed_vertex[1]]
            coordinates.append(coordinate)

        polygon = shapley_polygon(coordinates)

        try:
            _, faces = trimesh.creation.triangulate_polygon(polygon, engine='triangle')
            mesh = Trimesh(vertices=vertices, faces=faces)

            # check orientation
            normal_sum = 0
            for i in range(0, 3):
                normal_sum += normal[i] + mesh.face_normals[0][i]

            if abs(normal_sum) <= 1E-10:
                new_faces = []
                for face in faces:
                    new_face = []
                    for i in range(0, len(face)):
                        new_face.append(face[len(face) - i - 1])
                    new_faces.append(new_face)
                mesh = Trimesh(vertices=vertices, faces=new_faces)

            return mesh

        except ValueError:
            sys.stderr.write('Not able to triangulate polygon')
            _vertices = [[0, 0, 0], [0, 0, 1], [0, 1, 0]]
            _faces = [[0, 1, 2]]
            return Trimesh(vertices=_vertices, faces=_faces)

    @property
    def triangles(self) -> List[Polygon]:
        """
        triangles property
        """
        try:
            if self._triangles is None:
                self._triangles = []
                _mesh = self.triangle_mesh(self.coordinates, self.normal)
                for face in _mesh.faces:
                    points = []
                    for vertex in face:
                        points.append(self.coordinates[vertex])
                    polygon = Polygon(points)
                    self._triangles.append(polygon)
            return self._triangles
        except ModuleNotFoundError as error:
            print(f'Issues with trimesh module imports: {error}')
            return None

    @staticmethod
    def _angle_between_vectors(vec_1, vec_2):
        """
        angle between vectors in radians
        :param vec_1: vector
        :param vec_2: vector
        :return: float
        """
        if np.linalg.norm(vec_1) == 0 or np.linalg.norm(vec_2) == 0:
            sys.stderr.write("Warning: impossible to calculate angle between planes' normal. Return 0\n")
            return 0
        cosine = np.dot(vec_1, vec_2) / np.linalg.norm(vec_1) / np.linalg.norm(vec_2)
        if cosine > 1 and cosine - 1 < 1e-5:
            cosine = 1
        elif cosine < -1 and cosine + 1 > -1e-5:
            cosine = -1
        alpha = math.acos(cosine)
        return alpha

    @property
    def inverse(self):
        """
        Get the polygon coordinates in reversed order
        :return: [np.ndarray]
        """
        if self._inverse is None:
            self._inverse = self.coordinates[::-1]
        return self._inverse

    def divide(self, plane):
        """
        Divides the polygon in two by a plane
        :param plane: plane that intersects with self to divide it in two parts (Plane)
        :return: Polygon, Polygon, [Point]
        """
        tri_polygons = Trimesh(vertices=self.vertices, faces=self.faces)
        intersection = trimesh.intersections.mesh_plane(tri_polygons,
                                                        plane.normal, plane.origin.coordinates)
        polys_1 = trimesh.intersections.slice_mesh_plane(tri_polygons,
                                                         plane.opposite_normal, plane.origin.coordinates)
        polys_2 = trimesh.intersections.slice_mesh_plane(tri_polygons,
                                                         plane.normal, plane.origin.coordinates)
        triangles_1 = []
        for triangle in polys_1.triangles:
            triangles_1.append(Polygon(triangle))
        polygon_1 = self._reshape(triangles_1)
        triangles_2 = []
        for triangle in polys_2.triangles:
            triangles_2.append(Polygon(triangle))
        polygon_2 = self._reshape(triangles_2)
        return polygon_1, polygon_2, intersection

    def _reshape(self, triangles) -> Polygon:
        edges_list = []
        for _, triangle in enumerate(triangles):
            for edge in triangle.edges:
                if not self._edge_in_edges_list(edge, edges_list):
                    edges_list.append(edge)
                else:
                    edges_list = self._remove_from_list(edge, edges_list)
        points = self._order_points(edges_list)
        return Polygon(points)

    @staticmethod
    def _edge_in_edges_list(edge, edges_list):
        for edge_element in edges_list:
            if (edge_element[0].distance_to_point(edge[0]) == 0 and
                edge_element[1].distance_to_point(edge[1]) == 0) or \
                (edge_element[1].distance_to_point(edge[0]) == 0 and
                 edge_element[0].distance_to_point(edge[1]) == 0):
                return True
        return False

    @staticmethod
    def _order_points(edges_list):
        # todo: not sure that this method works for any case -> RECHECK
        points = edges_list[0]
        for _ in range(0, len(points)):
            for i in range(1, len(edges_list)):
                point_1 = edges_list[i][0]
                point_2 = points[len(points) - 1]
                if point_1.distance_to_point(point_2) == 0:
                    points.append(edges_list[i][1])
        points.remove(points[len(points) - 1])
        array_points = []
        for point in points:
            array_points.append(point.coordinates)
        return np.array(array_points)

    @staticmethod
    def _remove_from_list(edge, edges_list):
        new_list = []
        for edge_element in edges_list:
            if not ((edge_element[0].distance_to_point(edge[0]) == 0 and edge_element[1]
                .distance_to_point(edge[1]) == 0) or
                    (edge_element[1].distance_to_point(edge[0]) == 0 and edge_element[0]
                        .distance_to_point(edge[1]) == 0)):
                new_list.append(edge_element)
        return new_list

    @property
    def vertices(self) -> np.ndarray:
        """
        Get polygon vertices
        :return: np.ndarray(int)
        """
        if self._vertices is None:
            vertices, self._vertices = [], []
            _ = [vertices.extend(s.coordinates) for s in self.triangles]
            for vertex_1 in vertices:
                found = False
                for vertex_2 in self._vertices:
                    found = False
                    power = 0
                    for dimension in range(0, 3):
                        power += math.pow(vertex_2[dimension] - vertex_1[dimension], 2)
                    distance = math.sqrt(power)
                    if distance == 0:
                        found = True
                        break
                if not found:
                    self._vertices.append(vertex_1)
            self._vertices = np.asarray(self._vertices)
        return self._vertices

    @property
    def faces(self) -> List[List[int]]:
        """
        Get polygon triangular faces
        :return: [face]
        """
        if self._faces is None:
            self._faces = []
            for polygon in self.triangles:
                face = []
                points = polygon.coordinates
                if len(points) != 3:
                    sub_polygons = polygon.triangles
                    # TODO: I modified this! To be checked @Guille
                    if len(sub_polygons) >= 1:
                        for sub_polygon in sub_polygons:
                            face = []
                            points = sub_polygon.coordinates
                            for point in points:
                                face.append(self._position_of(point, face))
                            self._faces.append(face)
                else:
                    for point in points:
                        face.append(self._position_of(point, face))
                    self._faces.append(face)
        return self._faces

    def _position_of(self, point, face):
        """
        position of a specific point in the list of points that define a face
        :return: int
        """
        vertices = self.vertices
        for count, vertex2 in enumerate(vertices):
            # ensure not duplicated vertex
            power = 0
            for dimension in range(0, 3):
                power += math.pow(vertex2[dimension] - point[dimension], 2)
            distance = math.sqrt(power)
            if count not in face and distance == 0:
                return count
        return -1
