"""
In diesem Skript werden die Plots generiert. Das Skript arbeitet unabhängig vom Hauptskript layout.py, sodass die Plots
ohne GUI angezeigt werden können
"""

import sys
import os
import math

from PyQt5 import QtWidgets
from Classes import Verschneidung, Werkzeug
import json
import numpy as np
import pandas as pd

from PyQt5.QtWidgets import (
    QApplication,
    QHBoxLayout,
    QVBoxLayout,
    QLabel,
    QMainWindow,
    QWidget,
    QPushButton,
)


from plot_tabmenu import secondtabdemo
from matplotlib_settings import MplCanvas
from matplotlib_settings import TwoDim_MplCanvas
import sys
import matplotlib
matplotlib.use('Qt5Agg')
from Classes import Verschneidung, Werkzeug
import json
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg, NavigationToolbar2QT as NavigationToolbar
from matplotlib.figure import Figure
import matplotlib
import matplotlib.pyplot as plt
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg, NavigationToolbar2QT as NavigationToolbar
from stl import mesh
from mpl_toolkits import mplot3d

SETTINGS_PATH = './settings.json'
SETTINGS_TOOL = './exampleConfigFiles/tool.json'
SETTINGS_MACHINE = './exampleConfigFiles/machine.json'
SETTINGS_GEOMETRY = './exampleConfigFiles/geometry_setup.json'
SETTING_PROCESSING = './exampleConfigFiles/processing_setup.json'
matplotlib.use('Qt5Agg')

with open(SETTINGS_PATH, 'r') as f:
    settings = json.load(f)

with open(SETTINGS_TOOL, 'r') as f:
    tool = json.load(f)

with open(SETTINGS_MACHINE, 'r') as f:
    machine = json.load(f)

with open(SETTINGS_GEOMETRY, 'r') as f:
    geometry = json.load(f)

with open(SETTING_PROCESSING, 'r') as f:
    processing = json.load(f)





def plot_kantenvektoren():
    with open('settings.json', 'r') as f:
        settings = json.load(f)
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)

    fig = Figure(figsize=(5, 4), dpi=100)
    # 2D Plot:
    # .axes = fig.add_subplot(111)

    # 3D Plot:

    #f1 = plt.figure()
    axes = plt.figure().add_subplot(projection='3d')
    axes.scatter(verschneidung.x, verschneidung.y, verschneidung.z, facecolor='gray')

    axes.quiver(verschneidung.x, verschneidung.y, verschneidung.z, verschneidung.kantenvektor_hb_x,
                        verschneidung.kantenvektor_hb_y, verschneidung.kantenvektor_hb_z)

    axes.quiver(verschneidung.x, verschneidung.y, verschneidung.z, verschneidung.kantenvektor_qb_x,
                        verschneidung.kantenvektor_qb_y, verschneidung.kantenvektor_qb_z)


    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in
                                                        'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden




def plot_verschneidung():
    with open('settings.json', 'r') as f:
        settings = json.load(f)
    # 2D Plot:
    # .axes = fig.add_subplot(111)

    # 3D Plot:
    #f2 = plt.figure()
    axes = plt.figure().add_subplot(projection='3d')

    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes.plot3D(verschneidung.x + [verschneidung.x[0]], verschneidung.y + [verschneidung.y[0]],
                  verschneidung.z + [verschneidung.z[0]], 'gray')
    axes.scatter(verschneidung.x, verschneidung.y, verschneidung.z, facecolor='gray')
    axes.scatter(verschneidung.x[0], verschneidung.y[0], verschneidung.z[0], facecolor='green')
    axes.scatter(verschneidung.x[-1], verschneidung.y[-1], verschneidung.z[-1], facecolor='red')
        # ax.set_box_aspect(aspect=(1, 1, 1))

    xqb, yqb, zqb = Verschneidung.data_for_cylinder_along_z(0.0, 0.0, verschneidung.d_qb / 2.0, -30.0, 30.0)
    zhb, yhb, xhb = Verschneidung.data_for_cylinder_along_z(0.0, 0.0, verschneidung.d_hb / 2.0, -30.0, 30.0)
    axes.plot_surface(xqb, yqb, zqb, color='grey', alpha=0.3)
    axes.plot_surface(xhb, yhb, zhb, color='grey', alpha=0.3)

    axes.set_xlabel('x in mm')
    axes.set_ylabel('y in mm')
    axes.set_zlabel('z in mm')

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in
                                                            'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden

def plot_kantenwinkel():
    with open('settings.json', 'r') as f:
            settings = json.load(f)

    axes = plt.figure().add_subplot(111)
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes.stem(verschneidung.kantenwinkel)                            #Striche von x-Achse bis zu y-Wert
    x = np.linspace(0, 1, verschneidung.n_points)
    axes.plot(x, verschneidung.kantenwinkel)

    axes.set_xlabel('Positionsnummer')
    axes.set_ylabel('Kantenwinkel in °')


def plot_drehwinkel():
    with open('settings.json', 'r') as f:
        settings = json.load(f)

    axes = plt.figure().add_subplot(111)
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    numbers_schnittpkt_mitte=[]
    for number in verschneidung.schnittpunkt_mitte_phi:
        number*=180/math.pi
        numbers_schnittpkt_mitte.append(number)
    axes.stem(numbers_schnittpkt_mitte)
    x = np.linspace(0, 1, verschneidung.n_points)
    axes.plot(x, numbers_schnittpkt_mitte)

def plot_patches():
    with open('settings.json', 'r') as f:
        settings = json.load(f)

    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes = plt.figure().add_subplot(projection='3d')
    for i in range(0, verschneidung.n_points):
        j = i + 1
        if j >= verschneidung.n_points:                     #sonst Fehler wenn Kante v. Pkt. 90 auf Kante v. Pkt. 0 liegt
            j = 0
        X = [verschneidung.x[i], verschneidung.x[i] + verschneidung.kantenvektor_qb_x[i],
            verschneidung.x[j] + verschneidung.kantenvektor_qb_x[j], verschneidung.x[j]]
        Y = [verschneidung.y[i], verschneidung.y[i] + verschneidung.kantenvektor_qb_y[i],
            verschneidung.y[j] + verschneidung.kantenvektor_qb_y[j], verschneidung.y[j]]
        Z = [verschneidung.z[i], verschneidung.z[i] + verschneidung.kantenvektor_qb_z[i],
            verschneidung.z[j] + verschneidung.kantenvektor_qb_z[j], verschneidung.z[j]]
        verts = [list(zip(X, Y, Z))]

        X = [verschneidung.x[i], verschneidung.x[i] + verschneidung.kantenvektor_hb_x[i],
             verschneidung.x[j] + verschneidung.kantenvektor_hb_x[j], verschneidung.x[j]]
        Y = [verschneidung.y[i], verschneidung.y[i] + verschneidung.kantenvektor_hb_y[i],
             verschneidung.y[j] + verschneidung.kantenvektor_hb_y[j], verschneidung.y[j]]
        Z = [verschneidung.z[i], verschneidung.z[i] + verschneidung.kantenvektor_hb_z[i],
             verschneidung.z[j] + verschneidung.kantenvektor_hb_z[j], verschneidung.z[j]]
        verts2 = [list(zip(X, Y, Z))]

        axes.add_collection3d(mplot3d.art3d.Poly3DCollection(verts))
        axes.add_collection3d(mplot3d.art3d.Poly3DCollection(verts2))

        #Default Zoom:
        axes.set_ylim(-7, 7)
        axes.set_xlim(-7, 7)
        axes.set_zlim(5, 10)

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in 'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden



def plot_fase():
    with open('settings.json', 'r') as f:
        settings = json.load(f)
    axes = plt.figure().add_subplot(projection='3d')
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes.scatter(verschneidung.x, verschneidung.y, verschneidung.z, facecolor='gray')

    #axes.scatter(verschneidung.schnittpunkt_hb_x, verschneidung.schnittpunkt_hb_y, verschneidung.schnittpunkt_hb_z, facecolor='gred')
    #axes.scatter(verschneidung.schnittpunkt_qb_x, verschneidung.schnittpunkt_qb_y, verschneidung.schnittpunkt_qb_z, facecolor='green')
    axes.quiver(verschneidung.x, verschneidung.y, verschneidung.z, verschneidung.kantenvektor_hb_x,
                        verschneidung.kantenvektor_hb_y, verschneidung.kantenvektor_hb_z)
    axes.quiver(verschneidung.x, verschneidung.y, verschneidung.z, verschneidung.kantenvektor_qb_x,
                        verschneidung.kantenvektor_qb_y, verschneidung.kantenvektor_qb_z)

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in 'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden


def plot_TCP_Verschneidung():
    with open('settings.json', 'r') as f:
        settings = json.load(f)
    axes = plt.figure().add_subplot(projection='3d')
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes.scatter(verschneidung.x, verschneidung.y, verschneidung.z, facecolor='gray')
    axes.scatter(verschneidung.tcp_x, verschneidung.tcp_y, verschneidung.tcp_z, facecolor='cyan', edgecolor='green')

    your_mesh = mesh.Mesh.from_file('STL/combined.stl')
    axes.add_collection3d(mplot3d.art3d.Poly3DCollection(your_mesh.vectors))

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in 'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden



def plot_TCP_Verschneidung_transformed():
    with open('settings.json', 'r') as f:
        settings = json.load(f)
    axes = plt.figure().add_subplot(projection='3d')
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes.scatter(verschneidung.translated_x, verschneidung.translated_y, verschneidung.translated_z, facecolor='gray')
    axes.scatter(verschneidung.tcp_translated_x, verschneidung.tcp_translated_y, verschneidung.tcp_translated_z, facecolor='gray')

    axes.scatter(verschneidung.translated_x[0], verschneidung.translated_y[0], verschneidung.translated_z[0], facecolor='green', edgecolor='green')
    axes.scatter(verschneidung.translated_x[-1], verschneidung.translated_y[-1], verschneidung.translated_z[-1], facecolor='red', edgecolor='red')

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in 'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden

    axes.set_xlabel('x in mm')
    axes.set_ylabel('y in mm')
    axes.set_zlabel('z in mm')

def plot_werkzeug_combined():
    axes = plt.figure().add_subplot(projection='3d')
    your_mesh = mesh.Mesh.from_file('STL/combined.stl')         #BUG: pfad wird nicht gefunden
    axes.add_collection3d(mplot3d.art3d.Poly3DCollection(your_mesh.vectors))

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in
                                                'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden


# def plot_werkzeug_combined():
#     axes = plt.figure().add_subplot(projection='3d')
#     your_mesh = mesh.Mesh.from_file('STL/combined_real.stl')
#     axes.add_collection3d(mplot3d.art3d.Poly3DCollection(your_mesh.vectors))



def plot_schaft_vektoren():
    with open('settings.json', 'r') as f:
        settings = json.load(f)

    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    axes = plt.figure().add_subplot(projection='3d')
    your_mesh = mesh.Mesh.from_file('STL/combined_real.stl')
    collection = mplot3d.art3d.Poly3DCollection(your_mesh.vectors, linewidths=1, alpha=0.5)
    face_color = [0, 0, 1]
    collection.set_facecolor(face_color)
    axes.add_collection3d(collection)

    normals = pd.read_csv('STL/normals_combined_real.txt')
    normalized_normals = np.zeros((normals.shape))

    #Default Zoom
    axes.set_ylim(-15,15)
    axes.set_xlim(-15, 15)
    axes.set_zlim(0, 15)


    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in
                                                'xyz')])

def plot_kollision_schaft_horizontal():

    #read function schon am Anfang von plotErstellung.py implementiert

    with open(SETTINGS_PATH, 'r') as f:
        settings = json.load(f)

    with open(SETTINGS_TOOL, 'r') as g:
        tool = json.load(g)

    with open(SETTINGS_GEOMETRY, 'r') as h:
        geometry = json.load(h)

    axes = plt.figure().add_subplot(projection='3d')

    d_schaft = tool["d_schaft"]
    r_schaft = d_schaft*0.5

    d_qb = geometry["Verschneidung"]["d_qb"]
    r_qb = d_qb*0.5

    abstand_sicherheit = 0.001

    x_pkte_außerhalb_tol = []
    y_pkte_außerhalb_tol = []
    z_pkte_außerhalb_tol = []

    x_pkte_innerhalb_tol = []
    y_pkte_innerhalb_tol = []
    z_pkte_innerhalb_tol = []

    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)

    r_tcp = np.hypot(verschneidung.tcp_x, verschneidung.tcp_y)
    phi = np.arctan2(verschneidung.tcp_y, verschneidung.tcp_x) * 180 / np.pi

    # transformation kartes. in polar-Koord.:
    #for-schleife
    for i in range(verschneidung.n_points):



        #Kollision
        if r_tcp[i] > (r_qb-r_schaft-abstand_sicherheit):
            x_pkte_außerhalb_tol.append(verschneidung.tcp_x[i])
            y_pkte_außerhalb_tol.append(verschneidung.tcp_y[i])
            z_pkte_außerhalb_tol.append(verschneidung.tcp_z[i])

        elif r_tcp[i] < (r_qb-r_schaft-abstand_sicherheit):
            x_pkte_innerhalb_tol.append(verschneidung.tcp_x[i])
            y_pkte_innerhalb_tol.append(verschneidung.tcp_y[i])
            z_pkte_innerhalb_tol.append(verschneidung.tcp_z[i])

    axes.set_zlim(-3, -7)
    axes.set_xlim(-0.6, 0.6)
    axes.set_ylim(-0.6, 0.6)

    print("Anz. Punkte außerhalb Toleranz:", len(x_pkte_außerhalb_tol))
    print("x Punkte außerhalb Toleranz:", x_pkte_außerhalb_tol)
    print("tcp x", verschneidung.tcp_x)

    axes.scatter(x_pkte_außerhalb_tol, y_pkte_außerhalb_tol, z_pkte_außerhalb_tol, facecolor='red', edgecolor='red')
    axes.scatter(x_pkte_innerhalb_tol, y_pkte_innerhalb_tol, z_pkte_innerhalb_tol, facecolor='cyan', edgecolor='green')

    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in 'xyz')])


def plot_kollision_schaft_horizontal_2dim():
    #read function schon am Anfang von plotErstellung.py implementiert

    with open(SETTINGS_PATH, 'r') as f:
        settings = json.load(f)

    with open(SETTINGS_TOOL, 'r') as g:
        tool = json.load(g)

    with open(SETTINGS_GEOMETRY, 'r') as h:
        geometry = json.load(h)

    axes = plt.figure().add_subplot(projection='3d')

    d_schaft = tool["d_schaft"]
    r_schaft = d_schaft*0.5

    d_qb = geometry["Verschneidung"]["d_qb"]
    r_qb = d_qb*0.5

    abstand_sicherheit = 0.3

    x_pkte_außerhalb_tol = []
    y_pkte_außerhalb_tol = []
    z_pkte_außerhalb_tol = []

    x_pkte_innerhalb_tol = []
    y_pkte_innerhalb_tol = []
    z_pkte_innerhalb_tol = []

    print("d_qb", d_qb)
    print(r_schaft)
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)

    r_tcp = np.hypot(verschneidung.tcp_x, verschneidung.tcp_y)
    phi = np.arctan2(verschneidung.tcp_y, verschneidung.tcp_x) #* 180 / np.pi

    r_tcp_schaft = [0] * len(r_tcp)

    for i in range(len(r_tcp)):
        r_tcp_schaft[i] = r_tcp + (d_schaft / 2)

    r_qb = [r_qb] * 100

    t = np.arange(0, 100, 1)
    theta = 2 * np.pi * t * 1/100

    fig, ax = plt.subplots(subplot_kw={'projection': 'polar'})
    ax.plot(theta, r_qb, label='Querschnitt Bohrung')
    ax.fill(theta, r_qb, facecolor='lime', alpha=1)
    ax.plot(phi, r_tcp_schaft, label='Hüllkurve Werkzeugschaft')
    ax.fill(phi, r_tcp_schaft, facecolor='steelblue', alpha=1)
    l3 = ax.plot(phi, r_tcp, color="tab:orange", ls="--", lw=1, label="TCP-Bahn")
    ax.set_rmax(1)
    ax.set_rticks(np.arange(0.5, (d_qb/2) + 1, 0.5))  # Less radial ticks
    ax.set_rlabel_position(-22.5)  # Move radial labels away from plotted line
    angle = np.deg2rad(225)
    #ax.legend(loc="lower left",
              #bbox_to_anchor=(.5 + np.cos(angle) / 2, .5 + np.sin(angle) / 2))
    #ax.legend([l1, l2, l3], ['Querschnitt Bohrung', 'Hüllkurve Werkzeugschaft', "TCP-Bahn"])
    ax.grid(True)

    ax.set_title("Kollisionswarnung Schaft", va='bottom')

def plot_verschneidung_mit_werkzeug():
    with open('settings.json', 'r') as f:
        settings = json.load(f)
    axes = plt.figure().add_subplot(projection='3d')
    verschneidung = Verschneidung.Verschneidung(tool, machine, geometry, processing)
    #axes.plot3D(verschneidung.x + [verschneidung.x[0]], verschneidung.y + [verschneidung.y[0]],
                        #verschneidung.z + [verschneidung.z[0]], 'gray')

    your_mesh = mesh.Mesh.from_file('STL/Sweep/combined_real_rotated003.stl')
    collection = mplot3d.art3d.Poly3DCollection(your_mesh.vectors, linewidths=1, alpha=0.5)
    face_color = [0, 0, 1]
    collection.set_facecolor(face_color)
    #collection.set_edgecolor(face_color)       wenn edgecolor aktiviert, dann keine Transparenz mehr vorhanden
    axes.add_collection3d(collection)

    axes.scatter(verschneidung.x, verschneidung.y, verschneidung.z, facecolor='gray', edgecolor='gray')
    axes.scatter(verschneidung.x[0], verschneidung.y[0], verschneidung.z[0], facecolor='green')
    axes.scatter(verschneidung.x[-1], verschneidung.y[-1], verschneidung.z[-1], facecolor='red')

    axes.quiver(verschneidung.x, verschneidung.y, verschneidung.z, verschneidung.kantenvektor_hb_x,
                        verschneidung.kantenvektor_hb_y, verschneidung.kantenvektor_hb_z, facecolor='gray', edgecolor='gray')
    axes.quiver(verschneidung.x, verschneidung.y, verschneidung.z, verschneidung.kantenvektor_qb_x,
                        verschneidung.kantenvektor_qb_y, verschneidung.kantenvektor_qb_z, facecolor='gray', edgecolor='gray')
    #Tipp: andere Punkte und danach Anfangs und Endpunkte plotten

    axes.set_xlabel('x in mm')
    axes.set_ylabel('y in mm')
    axes.set_zlabel('z in mm')




    src = ('./STL/Sweep')
    os.chdir(src)

    for dirpath, dirnames, filenames in os.walk(src):
        # print(filenames)
        for file in filenames:
            file_num = (os.path.splitext(file)[0][-3:])   #Nummern separat ausgeben: 000, ..., 089
            #print(file_num)
        for i in range(verschneidung.n_points):
            i_num = f'0{i}'
            #print(i_num)

            if str(i_num) == str(file_num):
                print(file)
                #  axes.add_collection3d(mplot3d.art3d.Poly3DCollection(mesh.Mesh.from_file(file).vectors))



    axes.set_box_aspect(aspect=(1, 1, 1))
    axes.set_box_aspect([ub - lb for lb, ub in (getattr(axes, f'get_{a}lim')() for a in
                                                        'xyz')])  # notwendig, damit axis gleich skaliert u. Plots nicht verzerrt werden


#plot_kantenvektoren()
# plot_fase()
#plot_verschneidung()
#plot_drehwinkel()
#plot_kantenwinkel()
#plot_patches()
plot_schaft_vektoren()
#plot_TCP_Verschneidung()
#plot_TCP_Verschneidung_transformed()
#plot_verschneidung_mit_werkzeug()
#plot_werkzeug_combined()
plot_kollision_schaft_horizontal()
plot_kollision_schaft_horizontal_2dim()

plt.show()