from stl import mesh
from mpl_toolkits import mplot3d
from matplotlib import pyplot
#Python 3.7

from numpy import linalg as LA
import math
import numpy as np
import timeit

def Gefährdungserkennung():
    figure = pyplot.figure()
    axes = mplot3d.Axes3D(figure)
    start_time = timeit.default_timer()
    # Lade die STL files:
    torso = mesh.Mesh.from_file('45_grad_blech_5mm.stl') # THIS GETS AN Errno2 Error
       

    # Linienplot, nur 2 der 3 Linien des Dreiecks in blau:
    axes.add_collection3d(mplot3d.art3d.Line3DCollection(torso.vectors))

    # Flächenplot in hellgrau:
    axes.add_collection3d(mplot3d.art3d.Poly3DCollection(torso.vectors, facecolors='gray', alpha = 0.2))

    # Von der Matrix zu einem Vektor:
    scale = torso.points.flatten() #Macht die Matrix zu einem Vektor
    axes.auto_scale_xyz(scale, scale, scale) #Skaliert die Achsen im Plot

    # Die farbige Eckpunkte der unterschiedlichen Dreiecke:
    #axes.scatter3D(torso.x[2,:], torso.y[2,:], torso.z[2,:], color='red', s=100)
    #axes.scatter3D(torso.x[4,:], torso.y[4,:], torso.z[4,:], color='green', s=100)
    #axes.scatter3D(torso.x[6,:], torso.y[6,:], torso.z[6,:], color='purple', s=100)
    #axes.scatter3D(torso.x[5,:], torso.y[5,:], torso.z[5,:], color='orange', s=100)
    #axes.scatter3D(torso.x[1,:], torso.y[1,:], torso.z[1,:], color='black', s=100)
    #axes.scatter3D(torso.x[0,:], torso.y[0,:], torso.z[0,:], color='yellow', s=100)

    # Namen der Achsen:
    axes.set_xlabel('x')
    axes.set_ylabel('y')
    axes.set_zlabel('z')

    # Zeig den Plot in der Console an:
    #pyplot.show()

    # Zähle die Anzahl der Dreiecke = cT:
    cT = np.size(torso.attr)

    # Lege eine Vergleichsliste an:
    vergleichsListe = []

    # Vergleiche alle Eckpunkt des ersten Dreiecks mit jeden Punkt von jeden anderen Dreieck:
    for l in range(0,cT): #Iteriere durch alle Dreiecke 0 bis 5
        for k in range(0,3): #Iteriere von Punkt 1 bis Punkt 3 des ersten Dreiecks
            for i in range(l+1,cT): #Iteriere durch die Zeilen, bei Zeile 1 angefangen
                for j in range(0,3): #Iteriere die x,y,z-Werte der Punkte
                    if torso.points[l,3*k] == torso.points[i,3*j] and torso.points[l,3*k+1] == torso.points[i,3*j+1] and torso.points[l,3*k+2] == torso.points[i,3*j+2]:
                        winkel = np.dot(torso.normals[l,:],torso.normals[i,:])/(LA.norm(torso.normals[l,:])*LA.norm(torso.normals[i,:]))
                        winkel = math.acos(round(winkel,6))
                        winkel = np.rad2deg(winkel)
                        if winkel !=0:
                            winkel = 180 - winkel
                            if winkel < 90:
                                print("Dreieck " + str(l) + " und Dreieck " + str(i) + " besitzen einen gleichen Punkt.")
                                print("Winkel: " +str(winkel))
                                vergleichsListe.append([l,i,torso.points[l,3*k],torso.points[l,3*k+1],torso.points[l,3*k+2],winkel])
            
    # um auf die Liste zuzugreifen:
    #vergleichsListe[i][j]
    vergleichsArray = np.array(vergleichsListe)

    # benötigte Liste für Dreiecke mit zwei gleichen Punkten:
    removeZeilenliste = []
    # Kanten mithilfe zweier Punkte detektieren:
    kantenListe = []
    cK = np.size(vergleichsArray,0) #zähle Zeilenanzahl des vergleichsArrays = cK
    for l in range(0,cK-1): #Zeile
        for k in range(l+1,cK): #zweiter Wert der Zeile
            if vergleichsArray[l,0] == vergleichsArray[k,0] and vergleichsArray[l,1] == vergleichsArray[k,1]: #Identische Dreiecksbezeichnungen finden
                kantenListe.append([vergleichsArray[l,0],vergleichsArray[k,1],vergleichsArray[l,2],vergleichsArray[l,3],vergleichsArray[l,4],vergleichsArray[k,2],vergleichsArray[k,3],vergleichsArray[k,4],vergleichsArray[k,5]])
                vx = [kantenListe[-1][2],kantenListe[-1][5]] #x-Wert
                vy = [kantenListe[-1][3],kantenListe[-1][6]] #y-Wert
                vz = [kantenListe[-1][4],kantenListe[-1][7]] #z-Wert
                axes.plot3D(vx,vy,vz, 'red', linewidth = 3) #gefährliche Kanten plotten
                removeZeilenliste.append(l) 
                removeZeilenliste.append(k)
                axes.scatter3D(vx,vy,vz, color='red', s=100) #gefährliche Eckpunkte der Kanten plotten
    kantenArray = np.array(kantenListe) #Liste als Array abspeichern

    cR = np.size(removeZeilenliste) #zähle Zeilenanzahl der removeZeilenliste = cR
    removeZeilenliste.sort(reverse=True) #Ordne die Dreiecksbezeichnung absteigend
    punktArray = vergleichsArray #Einführung des punktArray
    for l in range(0,cR): #lösche alle doppelten Dreiecksbezeichungen und deren Werte aus dem punktArray
        punktArray = np.delete(punktArray,(removeZeilenliste[l]), axis = 0)
        
    if np.size(punktArray) !=0: #falls die Größe des punktArray's nicht leer ist, plotte Eckpunkte
        axes.scatter3D(punktArray[:,2],punktArray[:,3],punktArray[:,4], color='red', s=100)
    pyplot.show()
    print('Rechenzeit',timeit.default_timer()- start_time, 'in Sekunden')