#Initial setup of the platforms
sqrt3 = math.sqrt(3)
coef = sqrt3/6
coef2 = sqrt3/3
coef3 = sqrt3/2
pi = 3.141592653

#0.567,0.076,0.524,1.244
#Size of the equlateral triangle
Sb = 0.567
Sp = 0.076
l = 1.244
L = 0.524

Wb = coef * Sb
Ub = coef2 * Sb

Wp = coef * Sp
Up = coef2 * Sp


def calcAngle(e,f,g):
    s = e*e + f*f - g*g
    if( s < 0 ):
        return None #point does not exist
    else:
        sq = math.sqrt(e*e + f*f - g*g)
        t1 = (-f + sq )/ (g-e)
        t2 = (-f - sq )/ (g-e)
        #is the smaller angle the better solution
        o1 = math.degrees(2* math.atan(t1))
        o2 = math.degrees(2* math.atan(t2))

        if(o1 < 90 or o1 > -90):
            return o2
        else:
            return o1



def inverse(x,y,z):
    a = Wb - Up
    b = Sp*0.5 - coef3 * Wb
    c = Wp - 0.5*Wb


    e1 = 2*L*(y+a)
    f1 = 2*z*L
    g1 = x*x + y*y + z*z + a*a + L*L + 2*y*a - l*l

    e2 = -L * ( math.sqrt(3) * (x + b) + y + c)
    g2 = x*x + y*y + z*z + b*b + c*c + L*L + 2*(x*b + y*c) - l*l

    e3 = L*(sqrt3*(x-b) - y - c )
    g3 = x*x + y*y + z*z + b*b + c*c + L*L + 2*(-x*b + y*c) - l*l

    return [
        calcAngle(e1,f1,g1),
        calcAngle(e2,f1,g2),
        calcAngle(e3,f1,g3)
    ]


import numpy as np
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from scipy import stats
import matplotlib.axes

n = 30

X = np.linspace(-2, 2, num=n)
Y = np.linspace(-2, 2, num=n)
Z = np.linspace(-2, 0, num=n)
X, Y, Z = np.meshgrid(X, Y, Z)

colors = {True: "blue", False: "white"}
opacity = {True: 50, False: 0}
opacity2 = {True: 1, False: 0}

def possibleAngles(t1,t2,t3):
    return (t1 < 90 and t1 > -90 ) and (t2 < 90 and t2 > -90 ) and (t3 < 90 and t3 > -90 )

def opacityShite(x,y,z):
    angles = inverse(x,y,z)
    isReal = not any(e is None for e in [angles[0],angles[1],angles[2]]) and possibleAngles(angles[0],angles[1],angles[2])
    return opacity[isReal]

def densityShite(x,y,z):
    angles = inverse(x,y,z)
    isReal = not any(e is None for e in [angles[0],angles[1],angles[2]]) and possibleAngles(angles[0],angles[1],angles[2])
    return opacity2[isReal]

vop = np.vectorize(opacityShite)
dop = np.vectorize(densityShite)

fig = plt.figure(figsize=(50,50))


ax1 = plt.subplot2grid((1, 3), (0,1), projection='3d',aspect=1)

ax1.scatter(X,Y,Z,s=vop(X,Y,Z),marker=".")
ax1.view_init(elev=15., azim=20)
ax1.set_xlabel('X Label')
ax1.set_ylabel('Y Label')
ax1.set_zlabel('Z Label')


# sizes = dop(X,Y,Z)
# density = np.sum(sizes, axis=0)
# X.shape
# X1 = np.linspace(-2, 2, num=50)
# Y1 = np.linspace(-2, 2, num=50)
# X1,Y1 = np.meshgrid(X1,Y1)
# ax2.contourf(X1,Y1,density,zdir='x',stride=0.1,cmap='jet')
# ax2.contour3D(X1,Y1,density,zdir='y',stride=0.1,cmap='jet')
# ax2.plot_wireframe(X1,Y1,density)