import math
import numpy as np


class Geometry:
  def __init__(self, delta=0.5):
    self._delta = delta

  def almost_equal(self, v1, v2):
    delta = math.sqrt(pow((v1[0] - v2[0]), 2) + pow((v1[1] - v2[1]), 2) + pow((v1[2] - v2[2]), 2))
    return delta <= self._delta

  def is_almost_same_surface(self, s1, s2):
    # delta is grads an need to be converted into radians
    delta = np.rad2deg(self._delta)
    difference = (s1.inclination - s2.inclination) % math.pi
    if abs(difference) > delta:
      return False
    # s1 and s2 are at least almost parallel surfaces
    p1 = s1.polygon.get_parametric()
    selected_coefficient = 0
    for coefficient in p1:
      if coefficient != 0:
        break;
      selected_coefficient += 1

    # calculate distance point to plane
    # select surface1 value for the point (X,Y,Z) where two of the values are 0

    s1_coefficient = -p1[3] / p1[selected_coefficient]
    p2 = s2.polygon.get_parametric()
    n2 = s2.normal
    parametric = abs(p2[2] * s1_coefficient + p2[3])
    distance = parametric / math.sqrt(pow(n2[0], 2) + pow(n2[1], 2) + pow(n2[2], 2))
    return distance <= self._delta

  @staticmethod
  def to_points_matrix(points, remove_last=False):
    rows = points.size // 3
    points = points.reshape(rows, 3)
    if remove_last:
      points = np.delete(points, rows - 1, 0)
    return points