"""
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 logging
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 hub.city_model_structure.attributes.plane import Plane
from hub.city_model_structure.attributes.point import Point
import hub.helpers.constants as cte


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.
    n = len(self.vertices)
    angle_sum = 0
    for i in range(0, n):
      vector_0 = self.vertices[i]
      vector_1 = self.vertices[(i+1) % n]
      # 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) < cte.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
    """
    if self._points_list is None:
      s = self.coordinates
      self._points_list = np.reshape(s, len(s) * 3)
    return self._points_list

  @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 = np.zeros(3)
        ac = np.zeros(3)
        for i in range(0, 3):
          ab[i] = triangle.coordinates[1][i] - triangle.coordinates[0][i]
          ac[i] = triangle.coordinates[2][i] - triangle.coordinates[0][i]
        self._area += np.linalg.norm(np.cross(ab, ac)) / 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):
    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:

      vertices_2d, 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:
      logging.error(f'Not able to triangulate polygon\n')
      _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]:
    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

  @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 i in range(0, len(triangles)):
      for edge in triangles[i].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 i in range(len(vertices)):
      # ensure not duplicated vertex
      power = 0
      vertex2 = vertices[i]
      for dimension in range(0, 3):
        power += math.pow(vertex2[dimension] - point[dimension], 2)
      distance = math.sqrt(power)
      if i not in face and distance == 0:
        return i
    return -1