"""
.. module: elements
:synopsis: Obsahuje základní geometrické útvary v CGA algebře.

"""

from clifford.cga import CGA, Round, Translation
from clifford.tools.g3c import *
from math import cos, pi, sin
from numpy import argmin, dot, empty, linalg, sqrt as npSqrt

_spaceDimensions = 3 # Pracujeme ve 3D prostoru

# Inicializace CGA parametrů
g3, blades = Cl(3)
cga = CGA(g3)  # make cga from existing ga
locals().update(cga.blades)   # put ga's blades in local namespace


class Point:

    # create CGA point from 3D
    # @params : p = [x, y, z]
    @staticmethod
    def from3D(p):
        return (
            (p[0] * e1) + (p[1] * e2) + (p[2] * e3)
            + 0.5 * (p[0]**2  + p[1]**2  + p[2] **2 ) * einf
            + eo
        )


    # create 3D point from CGA
    # @params : s = Point in CGA
    @staticmethod
    def to3D(S):
        return ([ S[e1], S[e2], S[e3] ])


class Plane:

    # construct plane from 3 points in 3D space
    # @params : p, q, r = [x, y, z]
    @staticmethod
    def fromPoints(p, q, r):
        plane = (Point.from3D(p) ^ Point.from3D(q) ^ Point.from3D(r) ^ einf).dual()
        return ( plane )


    # construct plane from 3 points[index] from 3D Pointclound
    # @params : pointcloud = Pointcloud
    #           p, q, r = int
    @staticmethod
    def fromPointcloud(pointcloud, p, q, r):
        return (
            Plane.fromPoints(
                pointcloud.points[p],
                pointcloud.points[q],
                pointcloud.points[r]
            )
        )


    # construct sphere from 5 element vector
    # @params : vector = [a,b,c,d,e]
    @staticmethod
    def fromVector(vector):
        return (
            (vector[0] ^ e1) + (vector[1] ^ e2) + (vector[2] ^ e3) + (vector[3] ^ e4) + (vector[4] ^ e5)
        )


    @staticmethod
    def fit(plane, pointcloud, width):

        pointsCnt = int(np.asarray(pointcloud.points).size / 3 -1)
        #   prazdny list pro ulozeni prislusnych bodu
        indexlist = []
        #   vytvoreni listu s prislusnymi body
        for i in range(pointsCnt):
            if Plane.isInliner3D(plane, width, pointcloud.points[i]):
                indexlist.append(i)
        #   Metoda nejmensich ctvercu
        #   w[i,k]=p[i,k]         pro k = {1,2,3}
        #   w[i,k]=-1             pro k = 4
        #   w[i,k]=-1/2p[i]^2     pro k = 5
        shape = (len(indexlist), 5)
        P = empty(shape)
        print(pointcloud.points[indexlist[1]])
        print(pointcloud.points[indexlist[2]])
        print(pointcloud.points[indexlist[3]])
        print(pointcloud.points[indexlist[4]])

        for i in range(len(indexlist)):
            P[i, 0] = pointcloud.points[indexlist[i]][0]
            P[i, 1] = pointcloud.points[indexlist[i]][1]
            P[i, 2] = pointcloud.points[indexlist[i]][2]
            P[i, 3] = -1
            P[i, 4] = -0.5 * (P[i, 0] ** 2 + P[i, 1] ** 2 + P[i, 2] ** 2)

        #   vytvoreni symetricke matice B
        B = dot(P.transpose(), P)

        #   nalezeni nejmensiho vlastniho cisla a prislusneho vektoru
        eigenvalue, eigenvector = linalg.eigh(B)

        minValueIndice = argmin(eigenvalue)
        solution = np.empty(shape = (5,1))
        for i in range(5):
            solution[i] = eigenvector.transpose()[minValueIndice, i]
        solution[3] = eigenvector.transpose()[minValueIndice, 4]
        solution[4] = eigenvector.transpose()[minValueIndice, 3]
        sphere = Plane.fromVector(solution)
        sphere1=sphere[0]

        #   prislusny vektor odpovida reseni LMS
        return(sphere1)


    @staticmethod
    def isInliner3D(plane, width, point):
        dist = plane.normal() | Point.from3D(point)
        if abs(dist) < width:
            return True
        else:
            return False

    @staticmethod
    def isBoxImpossible(pointcloud, x, y, z, maxbox):
        p = pointcloud.points[x]
        q = pointcloud.points[y]
        r = pointcloud.points[z]
        dist1 = np.sqrt((p[0]-q[0])**2+(p[1]-q[1])**2+(p[1]-q[1])**2)
        dist2 = np.sqrt((p[0]-r[0])**2+(p[1]-r[1])**2+(p[1]-r[1])**2)
        dist3 = np.sqrt((r[0]-q[0])**2+(r[1]-q[1])**2+(r[1]-q[1])**2)
        if dist1>maxbox or dist2 >maxbox or dist3>maxbox:
            return True



class Circle:

    # construct circle from 3 points in 3D space
    # @params : p, q, r = [x, y, z]
    @staticmethod
    def fromPoints(p, q, r):
        return (
            (Point.from3D(p) ^ Point.from3D(q) ^ Point.from3D(r))
        )


    # construct circle from 3 points[index] from 3D Pointclound
    # @params : pointcloud = Pointcloud
    #           p, q, r = int
    @staticmethod
    def fromPointcloud(pointcloud, p, q, r):
        return (
            Circle.fromPoints(
                pointcloud.points[p],
                pointcloud.points[q],
                pointcloud.points[r]
            )
        )

    # returns N points from the circle defined by center and radius
    # @params : n = int
    #         : center = [x, y]
    #         : r = int
    @staticmethod
    def getPoints(n, center, r):
        return ([
            [
                center[0] + (cos(2 * pi / n * x) * r),
                center[1] + (sin(2 * pi / n * x) * r)
            ] for x in range(0, n + 1)
        ])

    @staticmethod
    def radius(circle):
        return (
            np.sqrt(
                (circle[0] - Circle.center(circle)[0]) ** 2 +
                (circle[1] - Circle.center(circle)[1]) ** 2 +
                (circle[2] - Circle.center(circle)[2]) ** 2
            )
        )

    @staticmethod
    def center(circle):
        circle_center = down(circle*einf*circle)
        center = [circle_center[e1], circle_center[e2], circle_center[e3]]
        return center

    @staticmethod
    def isInliner3D(circle, width, point):
        center = Circle.center(circle)
        circle_radius = Circle.radius(circle)
        dist = np.sqrt(
            (center[e1] - point[0]) ** 2
            + (center[e2] - point[1]) ** 2
            + (center[e3] - point[2]) ** 2
        )
        if (dist > circle_radius * (1 - width) and
            dist < circle_radius * (1 + width)):
            return True
        else :
            return False


class Sphere:

    # construct an unit sphere
    @staticmethod
    def unit():
        return (
            (up(e1) ^ up(e2) ^ up(e3) ^ up(-1 * e1))
            .dual()
        )


    # construct sphere from center and radius
    # @params : center = [x,y,z]
    #           radius = float
    @staticmethod
    def fromCenter(center, radius):
        return (
            ((Point.from3D(center) - 0.5 * (radius ** 2) * einf))
        )


    # construct sphere from 4 points in 3D space
    # @params : p, q, r, s = [x, y, z]
    @staticmethod
    def fromPoints(p, q, r, s):
        return (
            (Point.from3D(p) ^ Point.from3D(q) ^ Point.from3D(r) ^ Point.from3D(s))
        )


    # construct sphere from 4 points[index] from 3D Pointclound
    # @params : pointcloud = Pointcloud
    #           p, q, r, s = int
    @staticmethod
    def fromPointcloud(pointcloud, p, q, r, s):
        return (
            Sphere.fromPoints(
                pointcloud.points[p],
                pointcloud.points[q],
                pointcloud.points[r],
                pointcloud.points[s]
            )
        )


    # construct sphere from 5 element vector
    # @params : vector = [a,b,c,d,e]
    @staticmethod
    def fromVector(vector):
        return (
            (vector[0] ^ e1) + (vector[1] ^ e2) + (vector[2] ^ e3) + (vector[3] ^ e4) + (vector[4] ^ e5)
        )

    @staticmethod
    def find_max_list(list):
        list_len = [len(i) for i in list]
        return (np.argmax(list_len))

    # recalculate the sphere to fit better points in pointcloud
    # @params : sphere = Sphere in CGA
    #         : pointcloud = Pointcloud
    #         : width = float
    @staticmethod
    def fit(pointcloud, indexlist):

        print("Number of inliners:", len(indexlist))
        #   prazdny list pro ulozeni prislusnych bodu
        #   vytvoreni listu s prislusnymi body
        #for i in range(pointsCnt):
        #    if Sphere.isInliner3D(sphere, width, pointcloud.points[i]):
        #        indexlist.append(i)
        #   Metoda nejmensich ctvercu
        #   w[i,k]=p[i,k]         pro k = {1,2,3}
        #   w[i,k]=-1             pro k = 4
        #   w[i,k]=-1/2p[i]^2     pro k = 5
        shape = (len(indexlist), 5)
        P = empty(shape)
        for i in range(len(indexlist)):
            P[i, 0] = pointcloud.points[indexlist[i]][0]
            P[i, 1] = pointcloud.points[indexlist[i]][1]
            P[i, 2] = pointcloud.points[indexlist[i]][2]
            P[i, 3] = -1
            P[i, 4] = -0.5 * (P[i, 0] ** 2 + P[i, 1] ** 2 + P[i, 2] ** 2)


        #   vytvoreni symetricke matice B
        B = dot(P.transpose(), P)

        #   nalezeni nejmensiho vlastniho cisla a prislusneho vektoru
        eigenvalue, eigenvector = linalg.eigh(B)

        minValueIndice = argmin(eigenvalue)
        solution = np.empty(shape = (5,1))
        for i in range(5):
            solution[i] = eigenvector.transpose()[minValueIndice, i] / eigenvector.transpose()[minValueIndice, 4]
        solution[3] = eigenvector.transpose()[minValueIndice, 4] / eigenvector.transpose()[minValueIndice, 4]
        solution[4] = eigenvector.transpose()[minValueIndice, 3] / eigenvector.transpose()[minValueIndice, 4]

        sphere = Sphere.fromVector(solution)
        sphere1 = sphere[0]

        #   prislusny vektor odpovida reseni LMS
        return(sphere1)


    # calculate the radius of sphere
    # @params : sphere = Sphere in CGA
    @staticmethod
    def radius(sphere):
            sphere_radius = get_radius_from_sphere(sphere)
            return sphere_radius



    @staticmethod
    def radius3(sphere):
        return (
            np.sqrt(
                (
                    1 / ((sphere[e4]) ** 2)
                    * ((sphere[e1]) ** 2 + (sphere[e2]) ** 2 + (sphere[e3]) ** 2)
                )
                - 2 * (sphere[e5]) / (sphere[e4])
            )
        )


    @staticmethod
    def center(sphere):
            center = down(get_center_from_sphere(sphere))
            return (center)

    @staticmethod
    def center2(sphere):
            P = sphere[e1] * e1 + sphere[e2] * e2 + sphere[e3] * e3
            return (P)


    # Checks whether point belongs to the sphere line of width
    # @params : sphere = Sphere in CGA
    #         : width = float
    #         : point = Point in 3D
    @staticmethod
    def isInliner3D(sphere, width, point):
        center = Sphere.center(sphere)
        sphere_radius = Sphere.radius(sphere)
        dist = np.sqrt(
            (center[e1] - point[0]) ** 2
            + (center[e2] - point[1]) ** 2
            + (center[e3] - point[2]) ** 2
        )
        if (
            (dist > (sphere_radius - width))
            and (dist < (sphere_radius + width))
        ):
            return True
        else:
            return False

    @staticmethod
    def isInliner3Dfit(sphere, width, point):
        center = Sphere.center2(sphere)
        sphere_radius = Sphere.radius3(sphere)
        dist = np.sqrt(
            (center[e1] - point[0]) ** 2
            + (center[e2] - point[1]) ** 2
            + (center[e3] - point[2]) ** 2
        )
        if (
            (dist > (sphere_radius - width))
            and (dist < (sphere_radius + width))
        ):
            return True
        else:
            return False

    @staticmethod
    def distance(sphere, sphere2):
            center = Sphere.center(sphere)
            center2 = Sphere.center(sphere2)
            distance = np.sqrt((center[0]-center2[0]) ** 2 + (center[1]-center2[1]) ** 2 + (center[2]-center2[2]) ** 2)
            return (distance)
