# -*- coding: utf-8 -*-
# ##### BEGIN GPL LICENSE BLOCK #####
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software Foundation,
#  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#
# ##### END GPL LICENSE BLOCK #####

"""
    geometry_utils.py
    3d geometry calculations
"""

from mathutils import Vector
from mathutils import geometry


# 3D Geometry
class G3:

    @classmethod
    def distanceP2P(cls, p1, p2):
        return (p1 - p2).length

    @classmethod
    def closestP2L(cls, p, l1, l2):
        vA = p - l1
        vL = l2 - l1
        vL.normalize()
        return vL * (vL.dot(vA)) + l1

    @classmethod
    def closestP2E(cls, p, e1, e2):
        q = G3.closestP2L(p, e1, e2)
        de = G3.distanceP2P(e1, e2)
        d1 = G3.distanceP2P(q, e1)
        d2 = G3.distanceP2P(q, e2)
        if d1 > de and d1 > d2:
            q = e2
        if d2 > de and d2 > d1:
            q = e1
        return q

    @classmethod
    def heightP2S(cls, p, sO, sN):
        return (p - sO).dot(sN) / sN.dot(sN)

    @classmethod
    def closestP2S(cls, p, sO, sN):
        k = - G3.heightP2S(p, sO, sN)
        q = p + sN * k
        return q

    @classmethod
    def closestP2F(cls, p, fv, sN):
        q = G3.closestP2S(p, fv[0], sN)
        # pi = MeshEditor.addVertex(p)
        # qi = MeshEditor.addVertex(q)
        # MeshEditor.addEdge(pi, qi)
        # print([d0, d1, d2])

        if len(fv) == 3:
            h = G3.closestP2L(fv[0], fv[1], fv[2])
            d = (fv[0] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[1], fv[2])
            h = G3.closestP2L(fv[1], fv[2], fv[0])
            d = (fv[1] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[2], fv[0])
            h = G3.closestP2L(fv[2], fv[0], fv[1])
            d = (fv[2] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[0], fv[1])
            return q
        if len(fv) == 4:
            h = G3.closestP2L(fv[0], fv[1], fv[2])
            d = (fv[0] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[1], fv[2])
            h = G3.closestP2L(fv[1], fv[2], fv[3])
            d = (fv[1] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[2], fv[3])
            h = G3.closestP2L(fv[2], fv[3], fv[0])
            d = (fv[2] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[3], fv[0])
            h = G3.closestP2L(fv[3], fv[0], fv[1])
            d = (fv[3] - h).dot(q - h)
            if d <= 0:
                return G3.closestP2E(q, fv[0], fv[1])
            return q

    @classmethod
    def medianTriangle(cls, vv):
        m0 = (vv[1] + vv[2]) / 2
        m1 = (vv[0] + vv[2]) / 2
        m2 = (vv[0] + vv[1]) / 2
        return [m0, m1, m2]

    @classmethod
    def orthoCenter(cls, fv):
        try:
            h0 = G3.closestP2L(fv[0], fv[1], fv[2])
            h1 = G3.closestP2L(fv[1], fv[0], fv[2])
            # h2 = G3.closestP2L(fm[2], fm[0], fm[1])
            return geometry.intersect_line_line(fv[0], h0, fv[1], h1)[0]
        except(RuntimeError, TypeError):
            return None

    # Poor mans approach of finding center of circle
    @classmethod
    def circumCenter(cls, fv):
        fm = G3.medianTriangle(fv)
        return G3.orthoCenter(fm)

    @classmethod
    def ThreePnormal(cls, fv):
        n = (fv[1] - fv[0]).cross(fv[2] - fv[0])
        n.normalize()
        return n

    @classmethod
    def closestP2CylinderAxis(cls, p, fv):
        n = G3.ThreePnormal(fv)
        c = G3.circumCenter(fv)
        if c is None:
            return None
        return G3.closestP2L(p, c, c + n)

    # Poor mans approach of finding center of sphere
    @classmethod
    def centerOfSphere(cls, fv):
        try:
            if len(fv) == 3:
                return G3.circumCenter(fv)  # Equator
            if len(fv) == 4:
                fv3 = [fv[0], fv[1], fv[2]]
                c1 = G3.circumCenter(fv)
                n1 = G3.ThreePnormal(fv)
                fv3 = [fv[1], fv[2], fv[3]]
                c2 = G3.circumCenter(fv3)
                n2 = G3.ThreePnormal(fv3)
                d1 = c1 + n1
                d2 = c2 + n2
                return geometry.intersect_line_line(c1, d1, c2, d2)[0]
        except(RuntimeError, TypeError):
            return None

    @classmethod
    def closestP2Sphere(cls, p, fv):
        # print ("G3.closestP2Sphere")
        try:
            c = G3.centerOfSphere(fv)
            if c is None:
                return None
            pc = p - c
            if pc.length == 0:
                pc = pc + Vector((1, 0, 0))
            else:
                pc.normalize()
            return c + (pc * G3.distanceP2P(c, fv[0]))
        except(RuntimeError, TypeError):
            return None

    @classmethod
    def closestP2Cylinder(cls, p, fv):
        # print ("G3.closestP2Sphere")
        c = G3.closestP2CylinderAxis(p, fv)
        if c is None:
            return None
        r = (fv[0] - G3.centerOfSphere(fv)).length
        pc = p - c
        if pc.length == 0:
            pc = pc + Vector((1, 0, 0))
        else:
            pc.normalize()
        return c + (pc * r)

    """
    @classmethod
    def closestP2Sphere4(cls, p, fv4):
        # print ("G3.closestP2Sphere")
        fv = [fv4[0],fv4[1],fv4[2]]
        c1 = G3.circumCenter(fv)
        n1 = G3.ThreePnormal(fv)
        fv = [fv4[1],fv4[2],fv4[3]]
        c2 = G3.circumCenter(fv)
        n2 = G3.ThreePnormal(fv)
        d1 = c1+n1
        d2 = c2+n2
        c = geometry.intersect_line_line (c1, d1, c2, d2)[0]
        pc = p-c
        if pc.length == 0:
            pc = pc + Vector((1,0,0))
        else:
            pc.normalize()
        return c + (pc * G3.distanceP2P(c, fv[0]))
    """
