import math
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
import numpy as np
import json
from time import strftime
from os import path
from Classes import Werkzeug
from PyQt5.QtWidgets import *

from datetime import datetime

from utils.globalSettings import (
    GUI_SETTINGS,
    LANGUAGE_SETTINGS,
    PROGRAMM_VERSION
)

def cart2pol(x, y):
    rho = np.sqrt(x**2 + y**2)
    phi = np.arctan2(y, x)
    return rho, phi


def get_rotation_matrix(phi, theta, psi):
    """
    :param phi: um X-Achse "Rollen"
    :param theta: um Y-Achse "Nicken"
    :param psi: Um z-Achse "Gieren"
    :return: Rotation Matrix
    """
    Rot_Trans_X_Phi = np.array([[1, 0, 0]   ,
                                [0, np.cos(np.deg2rad(phi)), np.sin(np.deg2rad(phi))],
                                [0, -np.sin(np.deg2rad(phi)), np.cos(np.deg2rad(phi))]])
    Rot_Trans_Y_Theta = np.array([[np.cos(np.deg2rad(theta)), 0, -np.sin(np.deg2rad(theta))],
                                  [0, 1, 0],
                                  [np.sin(np.deg2rad(theta)), 0, np.cos(np.deg2rad(theta))]])
    Rot_Trans_Z_Psi = np.array([[np.cos(np.deg2rad(psi)), np.sin(np.deg2rad(psi)), 0],
                                [-np.sin(np.deg2rad(psi)), np.cos(np.deg2rad(psi)), 0],
                                [0, 0, 1]])

    RotMatrix = np.matmul(Rot_Trans_Y_Theta, Rot_Trans_Z_Psi)
    return np.matmul(Rot_Trans_X_Phi, RotMatrix)


def rotate_translate_points(transformation, x, y, z, angles, n_points):
    phi = transformation["Rotation"]["phi"]
    theta = transformation["Rotation"]["theta"]
    psi = transformation["Rotation"]["psi"]
    rot_matrix = get_rotation_matrix(phi, theta, psi)

    transformed_x = [0] * n_points
    transformed_y = [0] * n_points
    transformed_z = [0] * n_points
    transformed_angles = [0] * n_points

    for i in range(n_points):
        point = np.array([x[i], y[i], z[i]])
        x_, y_, z_ = np.matmul(rot_matrix, point)

        transformed_x[i] = x_ + transformation["Translation"]["x"]
        transformed_y[i] = y_ + transformation["Translation"]["y"]
        transformed_z[i] = z_ + transformation["Translation"]["z"]

        transformed_angles[i] = angles[i] + (psi * math.pi / 180)

    return transformed_x, transformed_y, transformed_z, transformed_angles


def data_for_cylinder_along_z(center_x, center_y, radius, start_z, end_z):
    z = np.linspace(start_z, end_z, 50)
    theta = np.linspace(0, 2*np.pi, 50)
    theta_grid, z_grid = np.meshgrid(theta, z)
    x_grid = radius*np.cos(theta_grid) + center_x
    y_grid = radius*np.sin(theta_grid) + center_y
    return x_grid, y_grid, z_grid


def rotate_cylinder(x, y, z, phi, theta, psi):

    #nach ata_for_cylinder_along_z sind die Koord. in der Dimension: 50x50, Dimension muss geändert werden -> alle Einträge in eine Zeile schreiben:
    points = np.array([x.ravel(), y.ravel(), z.ravel()])

    Rot_Trans_X_Phi = np.array([[1, 0, 0]   ,
                                [0, np.cos(np.deg2rad(phi)), np.sin(np.deg2rad(phi))],
                                [0, -np.sin(np.deg2rad(phi)), np.cos(np.deg2rad(phi))]])
    Rot_Trans_Y_Theta = np.array([[np.cos(np.deg2rad(theta)), 0, -np.sin(np.deg2rad(theta))],
                                  [0, 1, 0],
                                  [np.sin(np.deg2rad(theta)), 0, np.cos(np.deg2rad(theta))]])
    Rot_Trans_Z_Psi = np.array([[np.cos(np.deg2rad(psi)), np.sin(np.deg2rad(psi)), 0],
                                [-np.sin(np.deg2rad(psi)), np.cos(np.deg2rad(psi)), 0],
                                [0, 0, 1]])

    RotMatrix = np.matmul(Rot_Trans_Y_Theta, Rot_Trans_Z_Psi)

    rotated_points = np.dot(RotMatrix, points[:, :])

    x_rotated = rotated_points[0, :].reshape(x.shape)
    y_rotated = rotated_points[1, :].reshape(y.shape)
    z_rotated = rotated_points[2, :].reshape(z.shape)

    return x_rotated, y_rotated, z_rotated

def verschneidungspunkte(r_hb, r_qb, av, n_points):
    # Aktuell nur postiver Achsversatz zulässig
    winkelschritt = 360.0 / n_points
    winkelschritt = 2 * math.pi / n_points
    x = [0] * n_points
    y = [0] * n_points
    z = [0] * n_points
    omega_rad = [0] * n_points
    omega_deg = [0] * n_points
    for i in range(n_points):
        omega_rad[i] = i * winkelschritt
        omega_deg[i] = (i * winkelschritt) / math.pi * 180.0
        h_squared = (r_hb ** 2 - (r_qb * math.cos(omega_rad[i]) + av) ** 2)
        # Für Platte
        # h_squared = r_hb ** 2
        z[i] = math.sqrt(h_squared)
        x[i] = r_qb * math.sin(omega_rad[i])
        y[i] = r_qb * math.cos(omega_rad[i]) + av
    return x, y, z, omega_rad, omega_deg

def verschneidungspunkte_eben(r_qb, z_, n_points):
    # Aktuell nur postiver Achsversatz zulässig
    winkelschritt = 360.0 / n_points
    winkelschritt = 2 * math.pi / n_points
    x = [0] * n_points
    y = [0] * n_points
    z = [0] * n_points
    omega_rad = [0] * n_points
    omega_deg = [0] * n_points
    for i in range(n_points):
        omega_rad[i] = i * winkelschritt
        omega_deg[i] = (i * winkelschritt) / math.pi * 180.0
        # Für Platte
        # h_squared = r_hb ** 2
        z[i] = z_
        x[i] = r_qb * math.sin(omega_rad[i])
        y[i] = r_qb * math.cos(omega_rad[i])
    return x, y, z, omega_rad, omega_deg


def angle_check(angles, n_points):
    return_angles = [0] * n_points
    for i in range(n_points):
        angle = angles[i]
        # Check Upper Bound
        while angle > 360:
            angle -= 360
        # Check Lower Bound
        while angle <= 0:
            angle += 360
        return_angles[i] = angle
    return return_angles



class Verschneidung:
    def __init__(self, tool, machine, geometry, processing):
        print('Init Verschneidung Start')
        self.Auslegung = geometry['Allgemein']['Auslegung']
        self.ebener_Austritt = geometry['Allgemein']['ebener_Austritt']
        self.d_hb = geometry['Verschneidung']['d_hb']
        self.r_hb = self.d_hb / 2.0
        self.d_qb = geometry['Verschneidung']['d_qb']
        self.r_qb = self.d_qb / 2.0
        if self.ebener_Austritt:
            self.av = 0
        else:
            self.av = geometry['Verschneidung']['av']
        self.fb_soll = geometry['Verschneidung']['fb_soll']
        self.n_points = int(processing['n_points'])

        # Setup Flag that tracks if warning currently exist
        self.flag_warnings = False

        if not self.Auslegung:
            self.werkzeug = Werkzeug.Werkzeug(tool, self.n_points)

        self.transformation = geometry["Transformation"]
        self.transf_transl_x = geometry["Transformation"]["Translation"]["x"]
        self.transf_transl_y = geometry["Transformation"]["Translation"]["y"]
        self.transf_transl_z = geometry["Transformation"]["Translation"]["z"]

        self.phi = geometry["Transformation"]["Rotation"]["phi"]
        self.theta = geometry["Transformation"]["Rotation"]["theta"]
        self.psi = geometry["Transformation"]["Rotation"]["psi"]

        if not self.Auslegung:
            self.setupBohren = processing["Bohren"]
            self.setupEntgraten = processing["Entgraten"]
            self.machine = machine

        self.mitte_qb_x = 0.0
        self.mitte_qb_y = self.av
        self.mitte_qb_z = 0.0

        self.bearbeitungsrichtung = processing['Bearbeitungsrichtung']

        # Verschneidung und Kantenvektoren
        print('Verschneidungspunkte start')
        values = self.__punkte_verschneidung()
        self.x = values["Verschneidungspunkte"]["x"]
        self.y = values["Verschneidungspunkte"]["y"]
        self.z = values["Verschneidungspunkte"]["z"]
        self.omega_rad = values["Verschneidungspunkte"]["omega"]["rad"]
        self.omega_deg = values["Verschneidungspunkte"]["omega"]["deg"]

        self.kantenvektor_hb_x = values["KantenvektorHB"]["x"]
        self.kantenvektor_hb_y = values["KantenvektorHB"]["y"]
        self.kantenvektor_hb_z = values["KantenvektorHB"]["z"]

        self.kantenvektor_qb_x = values["KantenvektorQB"]["x"]
        self.kantenvektor_qb_y = values["KantenvektorQB"]["y"]
        self.kantenvektor_qb_z = values["KantenvektorQB"]["z"]

        print('Verschneidungspunkte Ende')

        self.kantenwinkel = self.__calculate_kantenwinkel()

        values = self.__calculate_steigungswinkel()
        self.steigungswinkel_num = values["numerisch"]
        self.steigungswinkel_ana = values["analytisch"]
        self.normalenvektor_x = values["normelenvektor"]["x"]
        self.normalenvektor_y = values["normelenvektor"]["y"]
        self.normalenvektor_z = values["normelenvektor"]["z"]

        # Fase bestimmen
        if not self.Auslegung:
            values = self.__calculate_fase()
            self.orthogonaler_Eingriff = values["orthogonalerEingriff"]

            self.schnittpunkt_hb_x = values["SchittpunktHQ"]["x"]
            self.schnittpunkt_hb_y = values["SchittpunktHQ"]["y"]
            self.schnittpunkt_hb_z = values["SchittpunktHQ"]["z"]

            self.schnittpunkt_qb_x = values["SchittpunktQB"]["x"]
            self.schnittpunkt_qb_y = values["SchittpunktQB"]["y"]
            self.schnittpunkt_qb_z = values["SchittpunktQB"]["z"]

            self.angebrachte_fase = values["Fasenbreite"]

            self.schnittpunkt_mitte_x = values["SchnittpunktMitte"]["x"]
            self.schnittpunkt_mitte_y = values["SchnittpunktMitte"]["y"]
            self.schnittpunkt_mitte_z = values["SchnittpunktMitte"]["z"]

            self.schnittpunkt_mitte_r, self.schnittpunkt_mitte_phi = self.__fase_zylinder_koordinaten()

            self.tcp_x, self.tcp_y, self.tcp_z, self.tcp_phi = self.__calculate_tcps()

            self.werkzeug.rotate_translate_tool(self.tcp_x, self.tcp_y, self.tcp_z, self.tcp_phi, self.n_points)
            self.tcp_translated_x, self.tcp_translated_y, self.tcp_translated_z, self.tcp_translated_phi = self.__rotate_translate_tcp()
            self.translated_x, self.translated_y, self.translated_z, _ = self.__rotate_translate_verschneidung()

            # Spezielle Bahnpunkte
            Sicherheitsebene_z1 = geometry["Sicherheitsebenen"]["Sicherheitsebene1"] + geometry["Sicherheitsebenen"]["Sicherheitsebene2"] + geometry["Sicherheitsebenen"]["Bohrtiefe"]
            self.sicherheitsebene1_x = self.mitte_qb_x
            self.sicherheitsebene1_y = self.mitte_qb_y
            self.sicherheitsebene1_z = Sicherheitsebene_z1

            Sicherheitsebene_z2 = geometry["Sicherheitsebenen"]["Sicherheitsebene2"] + geometry["Sicherheitsebenen"]["Bohrtiefe"] # Used for tool design
            self.sicherheitsebene2_x = self.mitte_qb_x
            self.sicherheitsebene2_y = self.mitte_qb_y
            self.sicherheitsebene2_z = Sicherheitsebene_z2
        self.bohrtiefe = geometry["Sicherheitsebenen"]["Bohrtiefe"]

        if not self.Auslegung:
            self.bohrtiefe_x = self.mitte_qb_x
            self.bohrtiefe_y = self.mitte_qb_y
            self.bohrtiefe_z = min(self.tcp_z)
            self.bohrtiefe_phi = self.tcp_phi[0]
        print('Init Verschneidung Done')

    def checkCollision(self, mainWindow, showWindowFlag=True):
        print("CheckCollisions")
        if not self.Auslegung:
            d_schaft = self.werkzeug.d_schaft
            r_schaft = d_schaft * 0.5

            d_qb = self.d_qb
            r_qb = d_qb * 0.5

            r_tcp = np.hypot(self.tcp_x, self.tcp_y)

            # Achtung: abstand_sicherheit wurde hier manuell festgelegt und wird noch in settings ergänzt

            abstand_sicherheit = GUI_SETTINGS["safteyTolerance"]

            Kollision = False

            # Kollision
            for i in range(self.n_points):

                if r_tcp[i] > ((r_qb + self.av) - r_schaft - abstand_sicherheit):
                    Kollision = True

            if Kollision:
                QMessageBox.warning(mainWindow, LANGUAGE_SETTINGS.languagePack["plotErstellung"]["CollisionWarningTitle"],
                                    LANGUAGE_SETTINGS.languagePack["plotErstellung"]["CollisionWarning"],
                                    buttons=QMessageBox.Ok)
                print("Kollision")
            self.checkWarnings(mainWindow, showWindowFlag)
        else:
            self.checkAusgelgungsWarnings(mainWindow, showWindowFlag)

    def checkWarnings(self, mainWindow, showWindowFlag=True):
        self.flag_warnings = False
        warnings = []

        # Get maximum
        max_num = max(self.steigungswinkel_num)
        max_ana = max(self.steigungswinkel_ana)
        max_total = max(max_num, max_ana)

        if not self.ebener_Austritt:
            schneidenwinkel = self.werkzeug.schneidenwinkel
            if schneidenwinkel == 0:
                schneidenwinkel = 90 / 180 * math.pi
            fb_x = self.fb_soll * np.sin(schneidenwinkel)
            bohrtiefe = self.bohrtiefe * -1
            schnittpunkt_hb_qb = bohrtiefe + np.sqrt((self.d_hb / 2.0) ** 2 - (self.d_qb / 2.0 + self.av + fb_x) ** 2)
            minnumum_halslaenge = 1.5 * self.d_qb - schnittpunkt_hb_qb
        else:
            minnumum_halslaenge = 1.5 * self.d_qb + self.bohrtiefe

        x_rechts = self.av + 0.5 * self.d_qb + self.fb_soll
        max_kopflaenge = 2 * np.sqrt((self.d_hb / 2) ** 2 - x_rechts ** 2)

        if max_total > self.werkzeug.freiwinkel:
            warnings.append("Freiwinkel")

        max_allowed_angle = 180.0 - self.werkzeug.schneidenwinkel
        if max(self.kantenwinkel) > max_allowed_angle:
            warnings.append("Kantenwinkel")

        if self.d_qb != self.werkzeug.d_kopf:
            warnings.append("Kopfdurchmesser")

        if self.werkzeug.halslaenge < minnumum_halslaenge:
            warnings.append("Halslänge")

        max_d_hals = self.d_qb - 2 * math.sin(self.werkzeug.schneidenwinkel) * (self.fb_soll + 0.1)
        if self.werkzeug.d_schaft > max_d_hals:
            warnings.append("Halsdurchmesser")

        if self.werkzeug.l_kopf < self.d_qb:
            warnings.append("Kopflaenge")

        if not self.ebener_Austritt:
            laenge = self.werkzeug.l_kopf + (self.werkzeug.d_kopf / 2.0) / np.tan((self.werkzeug.spitzenwinkel/2)*np.pi/180)
            if laenge > max_kopflaenge:
                warnings.append("Kopflaenge2")

            if self.bohrtiefe < self.d_hb / 2.0:
                warnings.append("Bohrtiefe")

        valid, missingParameters = self.checkMachineValid()

        if not valid:
            warnings.append("Maschine")
            print(missingParameters)


        if len(warnings) > 0:
            self.flag_warnings = True
            warning_text = [LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["Warnings"]["Maintext"], "\n"]
            for warning in warnings:
                warning_text.extend(["   -", LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["Warnings"][warning], "\n"])

            text = "".join(warning_text[:-1])
            if showWindowFlag:
                QMessageBox.warning(mainWindow, LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["WarningTitle"],
                                    text,
                                    buttons=QMessageBox.Ok)
            print(text)
        else:
            if showWindowFlag:
                QMessageBox.information(mainWindow, LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["InfoTitle"],
                                        LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["InfoTextSuccess"],
                                        buttons=QMessageBox.Ok)

    def checkMachineValid(self):
        valid = True
        missingParameters = []

        # List of keys that need to be checked
        needToBeFilled = ["Name", "Steuerung", "WKS", "MCodeWerkzeugwechsel", "MCodeEinschaltenSpindel",
                          "MCodeEinschaltenSpindelAlt", "MCodeAusschaltenSpindel"]

        # Iterate over the required keys
        for key in needToBeFilled:
            # Check if the key exists in the dictionary and is not an empty string
            if key not in self.machine:
                # Key not in Dict
                valid = False
                missingParameters.append(key)
                continue

            if not self.machine[key]:
                # Empty String
                valid = False
                missingParameters.append(key)
                continue

            if self.machine[key] == '---':
                # Detault null value
                valid = False
                missingParameters.append(key)
                continue

        return valid, missingParameters

    def checkAusgelgungsWarnings(self, mainWindow, showWindowFlag=True):
        self.flag_warnings = False
        warnings = []

        x_rechts = self.av + 0.5 * self.d_qb + self.fb_soll
        if x_rechts < (self.d_hb / 2):
            max_kopflaenge = 2 * np.sqrt((self.d_hb / 2) ** 2 - x_rechts ** 2)
        else:
            max_kopflaenge = 0

        if max_kopflaenge < 0.5 * self.d_qb:
            warnings.append("Kopflaenge0.5D")
        elif max_kopflaenge < 1 * self.d_qb:
            warnings.append("Kopflaenge1D")

        if len(warnings) > 0:
            self.flag_warnings = True
            warning_text = [LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["Warnings"]["Maintext"], "\n"]
            for warning in warnings:
                warning_text.extend(["   -", LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["Warnings"]["Auslegung"][warning], "\n"])

            text = "".join(warning_text[:-1])
            if showWindowFlag:
                QMessageBox.warning(mainWindow, LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["WarningTitle"],
                                    text,
                                    buttons=QMessageBox.Ok)
            print(text)
        else:
            if showWindowFlag:
                QMessageBox.information(mainWindow, LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["InfoTitle"],
                                        LANGUAGE_SETTINGS.languagePack["setupTabmenu"]["InfoTextSuccess"],
                                        buttons=QMessageBox.Ok)

    def __punkte_verschneidung(self):
        r_hb = self.d_hb/2.0
        r_qb = self.d_qb/2.0
        delta_r = 0.01

        if self.ebener_Austritt == True:
            x, y, z, omega_rad, omega_deg = verschneidungspunkte_eben(r_qb, 0.0, self.n_points)
            # Für Berechnung des Kantenvektors in Richtung der Hauptbohrung r_qb leicht vergrößern
            x_, y_, z_, _, _ = verschneidungspunkte_eben(r_qb + delta_r, 0.0, self.n_points)
        else:
            x, y, z, omega_rad, omega_deg = verschneidungspunkte(r_hb, r_qb, self.av, self.n_points)
            # Für Berechnung des Kantenvektors in Richtung der Hauptbohrung r_qb leicht vergrößern
            x_, y_, z_, _, _ = verschneidungspunkte(r_hb, r_qb + delta_r, self.av, self.n_points)

        kantenvektor_hb_x = [0] * self.n_points
        kantenvektor_hb_y = [0] * self.n_points
        kantenvektor_hb_z = [0] * self.n_points

        for i in range(self.n_points):
            kantenvektor_hb_x[i] = x_[i] - x[i]
            kantenvektor_hb_y[i] = y_[i] - y[i]
            kantenvektor_hb_z[i] = z_[i] - z[i]
            # Normieren
            length = math.sqrt(kantenvektor_hb_x[i]**2+kantenvektor_hb_y[i]**2+kantenvektor_hb_z[i]**2)
            kantenvektor_hb_x[i] *= 1 / length
            kantenvektor_hb_y[i] *= 1 / length
            kantenvektor_hb_z[i] *= 1 / length

        # Für Berechnung des Kantenvektors in Richtung der Querbohrung r_hb leicht vergrößern
        if self.ebener_Austritt == True:
            x_, y_, z_, _, _ = verschneidungspunkte_eben(r_qb, 0.0 + delta_r, self.n_points)
        else:
            x_, y_, z_, _, _ = verschneidungspunkte(r_hb + delta_r, r_qb, self.av, self.n_points)

        kantenvektor_qb_x = [0] * self.n_points
        kantenvektor_qb_y = [0] * self.n_points
        kantenvektor_qb_z = [0] * self.n_points
        for i in range(self.n_points):
            kantenvektor_qb_x[i] = x_[i] - x[i]
            kantenvektor_qb_y[i] = y_[i] - y[i]
            kantenvektor_qb_z[i] = z_[i] - z[i]
            # Normieren
            length = math.sqrt(kantenvektor_qb_x[i] ** 2 + kantenvektor_qb_y[i] ** 2 + kantenvektor_qb_z[i] ** 2)
            kantenvektor_qb_x[i] *= 1 / length
            kantenvektor_qb_y[i] *= 1 / length
            kantenvektor_qb_z[i] *= 1 / length

        # Punkte im oder gegen Uhrzeigersinn
        if self.bearbeitungsrichtung != 'Uhrzeigersinn':
            x.reverse()
            y.reverse()
            z.reverse()
            omega_rad.reverse()
            omega_deg.reverse()

            kantenvektor_hb_x.reverse()
            kantenvektor_hb_y.reverse()
            kantenvektor_hb_z.reverse()

            kantenvektor_qb_x.reverse()
            kantenvektor_qb_y.reverse()
            kantenvektor_qb_z.reverse()

        # Return values
        values = {
            "Verschneidungspunkte":
                {
                    "x": x,
                    "y": y,
                    "z": z,
                    "omega":
                        {
                            "rad": omega_rad,
                            "deg": omega_deg
                        }
                },
            "KantenvektorHB":
                {
                    "x": kantenvektor_hb_x,
                    "y": kantenvektor_hb_y,
                    "z": kantenvektor_hb_z
                },
            "KantenvektorQB":
                {
                    "x": kantenvektor_qb_x,
                    "y": kantenvektor_qb_y,
                    "z": kantenvektor_qb_z
                }
        }
        return values

    def __calculate_kantenwinkel(self):
        # dot = lambda X, Y: sum(map(lambda x, y: x * y, X, Y))
        betrag = lambda X: math.sqrt(sum(map(lambda x: x**2, X)))
        kantenwinkel = [0] * self.n_points
        for i in range(self.n_points):
            vektor1 = [self.kantenvektor_qb_x[i], self.kantenvektor_qb_y[i], self.kantenvektor_qb_z[i]]
            vektor2 = [self.kantenvektor_hb_x[i], self.kantenvektor_hb_y[i], self.kantenvektor_hb_z[i]]
            kantenwinkel[i] = (math.acos(np.dot(vektor1, vektor2) / (betrag(vektor1)*betrag(vektor2))) * 180) / math.pi
        return kantenwinkel

    def __calculate_steigungswinkel(self):
        betrag = lambda X: math.sqrt(sum(map(lambda x: x ** 2, X)))
        numerisch = [0] * self.n_points
        analytisch = [0] * self.n_points
        vector_x = [0] * self.n_points
        vector_y = [0] * self.n_points
        vector_z = [0] * self.n_points
        normalenvektor = [0, 0, 1]

        for i in range(self.n_points):
            if i <= self.n_points-2:
                vektor1 = [self.x[i+1], self.y[i+1], self.z[i+1]]
                vektor2 = [self.x[i], self.y[i], self.z[i]]
            else:
                vektor1 = [self.x[0], self.y[0], self.z[0]]
                vektor2 = [self.x[i], self.y[i], self.z[i]]

            verbindungsvektor = [vektor1[0] - vektor2[0], vektor1[1] - vektor2[1], vektor1[2] - vektor2[2]]
            numerisch[i] = (math.asin((verbindungsvektor[0] * normalenvektor[0] +
                                       verbindungsvektor[1] * normalenvektor[1] +
                                       verbindungsvektor[2] * normalenvektor[2]) / (
                                                    betrag(verbindungsvektor) * betrag(
                                                normalenvektor))) * 180) / math.pi

            if not self.ebener_Austritt:
                # GL 5-8 bis 5-11 Adrian Dis
                x_strich = self.r_qb * math.cos(self.omega_rad[i])
                y_strich = -(self.r_qb * math.sin(self.omega_rad[i]))
                zaehler = self.r_qb * math.sin(self.omega_rad[i]) * (self.r_qb * math.cos(self.omega_rad[i]) + self.av)
                nenner = math.sqrt(self.r_hb ** 2 - (self.r_qb * math.cos(self.omega_rad[i]) + self.av) ** 2)
                z_strich = zaehler / nenner

                ableitung = [x_strich, y_strich, z_strich]
                vector_x[i] = x_strich
                vector_y[i] = y_strich
                vector_z[i] = z_strich

                analytisch[i] = (math.asin(z_strich / (betrag(ableitung)))) * 180 / math.pi
            else:
                x_strich = self.r_qb * math.cos(self.omega_rad[i])
                y_strich = -(self.r_qb * math.sin(self.omega_rad[i]))
                z_strich = 0
                vector_x[i] = x_strich
                vector_y[i] = y_strich
                vector_z[i] = z_strich
                analytisch[i] = 0.0

        if self.bearbeitungsrichtung != 'Uhrzeigersinn':
            analytisch = [x * -1 for x in analytisch]
            vector_x = [x * -1 for x in vector_x]
            vector_y = [x * -1 for x in vector_y]
            vector_z = [x * -1 for x in vector_z]

        values = {
            "numerisch": numerisch,
            "analytisch": analytisch,
            "normelenvektor": {
                "x": vector_x,
                "y": vector_y,
                "z": vector_z
            }
        }

        return values

    def __calculate_fase(self):
        orthogonaler_Eingriff = self.fb_soll * math.cos((90 - self.werkzeug.schneidenwinkel) * math.pi / 180)

        schnittpunkt_hb_x = [0] * self.n_points
        schnittpunkt_hb_y = [0] * self.n_points
        schnittpunkt_hb_z = [0] * self.n_points

        schnittpunkt_qb_x = [0] * self.n_points
        schnittpunkt_qb_y = [0] * self.n_points
        schnittpunkt_qb_z = [0] * self.n_points

        fasenbreite = [0] * self.n_points

        schnittpunkt_mitte_x = [0] * self.n_points
        schnittpunkt_mitte_y = [0] * self.n_points
        schnittpunkt_mitte_z = [0] * self.n_points

        for i in range(self.n_points):
            gegenkantenwinkel = 180 - self.kantenwinkel[i]
            hypothenuse = orthogonaler_Eingriff / math.sin(gegenkantenwinkel * math.pi / 180)

            schnittpunkt_hb_x[i] = self.x[i] + hypothenuse * self.kantenvektor_hb_x[i]
            schnittpunkt_hb_y[i] = self.y[i] + hypothenuse * self.kantenvektor_hb_y[i]
            schnittpunkt_hb_z[i] = self.z[i] + hypothenuse * self.kantenvektor_hb_z[i]

            h_luft = orthogonaler_Eingriff / math.tan(gegenkantenwinkel * math.pi / 180)
            h_komplett = self.fb_soll * math.sin((90 - self.werkzeug.schneidenwinkel) * math.pi / 180)
            h_material = h_komplett - h_luft

            schnittpunkt_qb_x[i] = self.x[i] + h_material * self.kantenvektor_qb_x[i]
            schnittpunkt_qb_y[i] = self.y[i] + h_material * self.kantenvektor_qb_y[i]
            schnittpunkt_qb_z[i] = self.z[i] + h_material * self.kantenvektor_qb_z[i]

            # Fasenbreite
            dx = schnittpunkt_hb_x[i] - schnittpunkt_qb_x[i]
            dy = schnittpunkt_hb_y[i] - schnittpunkt_qb_y[i]
            dz = schnittpunkt_hb_z[i] - schnittpunkt_qb_z[i]
            fasenbreite[i] = math.sqrt(dx**2 + dy**2 + dz**2)

            # Fasenmitte
            schnittpunkt_mitte_x[i] = (schnittpunkt_hb_x[i] + schnittpunkt_qb_x[i]) / 2.0
            schnittpunkt_mitte_y[i] = (schnittpunkt_hb_y[i] + schnittpunkt_qb_y[i]) / 2.0
            schnittpunkt_mitte_z[i] = (schnittpunkt_hb_z[i] + schnittpunkt_qb_z[i]) / 2.0

        values = {
            "orthogonalerEingriff": orthogonaler_Eingriff,
            "SchittpunktHQ":
                {
                    "x": schnittpunkt_hb_x,
                    "y": schnittpunkt_hb_y,
                    "z": schnittpunkt_hb_z
                },
            "SchittpunktQB":
                {
                    "x": schnittpunkt_qb_x,
                    "y": schnittpunkt_qb_y,
                    "z": schnittpunkt_qb_z
                },
            "Fasenbreite": fasenbreite,
            "SchnittpunktMitte":
                {
                    "x": schnittpunkt_mitte_x,
                    "y": schnittpunkt_mitte_y,
                    "z": schnittpunkt_mitte_z
                }
        }
        return values

    def __fase_zylinder_koordinaten(self):
        r = [0] * self.n_points
        phi = [0] * self.n_points

        for i in range(self.n_points):
            r[i] = math.sqrt(self.schnittpunkt_mitte_x[i]**2 + self.schnittpunkt_mitte_y[i]**2)
            phi[i] = np.arctan2(self.schnittpunkt_mitte_y[i], self.schnittpunkt_mitte_x[i])

        return r, phi

    def __calculate_tcps(self):
        tcp_x = [0] * self.n_points
        tcp_y = [0] * self.n_points
        tcp_z = [0] * self.n_points
        tcp_phi = [0] * self.n_points

        # Debug
        ax_distance = self.werkzeug.wz_ax_distance

        for i in range(self.n_points):
            # Radialer Abstand und Winkel zur Querbohrung
            r, phi = cart2pol(self.schnittpunkt_mitte_x[i] - self.mitte_qb_x,
                              self.schnittpunkt_mitte_y[i] - self.mitte_qb_y)
            distance = r - self.werkzeug.wz_rad_distance
            tcp_x[i] = distance * math.cos(phi) + self.mitte_qb_x
            tcp_y[i] = distance * math.sin(phi) + self.mitte_qb_y
            tcp_z[i] = self.schnittpunkt_mitte_z[i] - ax_distance
            tcp_phi[i] = phi

        return tcp_x, tcp_y, tcp_z, tcp_phi

    def __rotate_translate_tcp(self):
        return rotate_translate_points(self.transformation, self.tcp_x, self.tcp_y, self.tcp_z, self.tcp_phi,
                                       self.n_points)

    def __rotate_translate_verschneidung(self):
        # Keine Winkeltransformation
        return rotate_translate_points(self.transformation, self.x, self.y, self.z, [0] * self.n_points, self.n_points)

    def plot_verschneidung(self):
        fig = plt.figure()
        ax = plt.axes(projection='3d')
        ax.plot3D(self.x+[self.x[0]], self.y+[self.y[0]], self.z+[self.z[0]], 'gray')
        ax.scatter(self.x, self.y, self.z, facecolor='gray')
        ax.scatter(self.x[0], self.y[0], self.z[0], facecolor='green')
        ax.scatter(self.x[-1], self.y[-1], self.z[-1], facecolor='red')
        #ax.set_box_aspect(aspect=(1, 1, 1))

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

        plt.show()

    def plot_kantenvektoren(self):
        fig = plt.figure()
        ax = plt.axes(projection='3d')
        ax.scatter(self.x, self.y, self.z, facecolor='gray')
        ax.quiver(self.x, self.y, self.z, self.kantenvektor_hb_x, self.kantenvektor_hb_y, self.kantenvektor_hb_z)
        ax.quiver(self.x, self.y, self.z, self.kantenvektor_qb_x, self.kantenvektor_qb_y, self.kantenvektor_qb_z)
        ax.set_box_aspect(aspect=(1, 1, 1))
        plt.show()

    def plot_kantenwinkel(self):
        fig = plt.figure()
        plt.stem(self.kantenwinkel)
        plt.xlim(0, self.n_points)
        axes = plt.gca()
        y_min, y_max = axes.get_ylim()
        plt.ylim(0, y_max)
        plt.show()

    def output_data(self):
        result_dict = {
            "d_hb": self.d_hb,
            "d_qb": self.d_qb,
            "av": self.av,
            "n_points": self.n_points,
            "fb_soll": self.fb_soll,
            "Verschneidung":
                {
                    "x": self.x,
                    "y": self.y,
                    "z": self.z,
                    "omega":
                        {
                            "rad": self.omega_rad,
                            "deg": self.omega_deg
                        }
                },
            "KantenvektorHB":
                {
                    "x": self.kantenvektor_hb_x,
                    "y": self.kantenvektor_hb_y,
                    "z": self.kantenvektor_hb_z
                },
            "KantenvektorQB":
                {
                    "x": self.kantenvektor_qb_x,
                    "y": self.kantenvektor_qb_y,
                    "z": self.kantenvektor_qb_z
                },
            "Kantenwinkel": self.kantenwinkel,
            "Steigungswinkel":
                {
                    "numerisch": self.steigungswinkel_num,
                    "analytisch": self.steigungswinkel_ana
                },
            "orthogonaler_Eingriff": self.orthogonaler_Eingriff,
            "SchnittpunktHB":
                {
                    "x": self.schnittpunkt_hb_x,
                    "y": self.schnittpunkt_hb_y,
                    "z": self.schnittpunkt_hb_z
                },
            "SchnittpunktQB":
                {
                    "x": self.schnittpunkt_qb_x,
                    "y": self.schnittpunkt_qb_y,
                    "z": self.schnittpunkt_qb_z
                },
            "SchnittpunktMitte":
                {
                    "kartesisch":
                        {
                            "x": self.schnittpunkt_mitte_x,
                            "y": self.schnittpunkt_mitte_y,
                            "z": self.schnittpunkt_mitte_z
                        },
                    "Zylinder":
                        {
                            "r": self.schnittpunkt_mitte_r,
                            "phi": self.schnittpunkt_mitte_phi,
                            "z": self.schnittpunkt_mitte_z
                        }
                },
            "Fasenbreite": self.angebrachte_fase,
            "TCP":
                {
                    "x": self.tcp_x,
                    "y": self.tcp_y,
                    "z": self.tcp_z,
                    "phi": self.tcp_phi
                },
            "Transformed":
                {
                    "TCP":
                        {
                            "x": self.tcp_translated_x,
                            "y": self.tcp_translated_y,
                            "z": self.tcp_translated_z,
                            "phi": self.tcp_translated_phi
                        },
                    "Verschneidung":
                        {
                            "x": self.translated_x,
                            "y": self.translated_y,
                            "z": self.translated_z
                        }
                }
        }

        with open("results.json", "w") as f:
            json.dump(result_dict, f, indent=4)

    def __output_g_code_sinumerik_lagegeregelter_spindelbetrieb(self):
        lines = []
        lines.extend(self.__g_code_header())
        lines.extend(self.__g_code_bohren())
        lines.extend(self.__g_code_entgraten())
        lines.append('M30 ;Programmende')

        n_lines = len(lines)
        returnString = ""
        padding = math.floor(math.log10(n_lines)) + 4
        for i, line in enumerate(lines):
            satznummer = 'N%s' % ((i + 1) * 10)
            satznummer = satznummer.ljust(padding)
            returnString = returnString + satznummer + line + "\n"

        return returnString

    def __output_g_code_sinumerik_spindel_positionieren(self):
        lines = []
        lines.extend(self.__g_code_header())
        lines.extend(self.__g_code_bohren())
        lines.extend(self.__g_code_entgraten_sinumerik_spos())
        lines.append('M30 ;Programmende')

        n_lines = len(lines)
        returnString = ""
        padding = math.floor(math.log10(n_lines)) + 4
        for i, line in enumerate(lines):
            satznummer = 'N%s' % ((i + 1) * 10)
            satznummer = satznummer.ljust(padding)
            returnString = returnString + satznummer + line + "\n"

        return returnString

    def output_g_code(self):
        defaultFlag = True
        if self.machine["Steuerung"] == LANGUAGE_SETTINGS.languagePack["configManager"]["Steuerungstyp"]["Sinumerik"]["LagegeregelterSpindelbetrieb"]:
            returnString = self.__output_g_code_sinumerik_lagegeregelter_spindelbetrieb()
            print("Used Control:", LANGUAGE_SETTINGS.languagePack["configManager"]["Steuerungstyp"]["Sinumerik"]["LagegeregelterSpindelbetrieb"])
            defaultFlag = False
        if self.machine["Steuerung"] == LANGUAGE_SETTINGS.languagePack["configManager"]["Steuerungstyp"]["Sinumerik"]["SpindelnPositionieren"]:
            returnString = self.__output_g_code_sinumerik_spindel_positionieren()
            print("Used Control:", LANGUAGE_SETTINGS.languagePack["configManager"]["Steuerungstyp"]["Sinumerik"]["SpindelnPositionieren"])
            defaultFlag = False

        if defaultFlag:
            print('No Control Option found. Return default Option: Sinumerik lagegeregelter Spindelbetrieb')
            returnString = self.__output_g_code_sinumerik_lagegeregelter_spindelbetrieb()

        return returnString


    def __g_code_header(self):
        # get current date
        now = datetime.now()
        formatted_date_time = now.strftime("%d.%m.%Y %H:%M")

        lines = []
        lines.append(';%s - %s' % (self.machine["Name"],
                                   self.machine["Name"]))
        lines.append(';Verschneidungseigenschaften:')
        if self.ebener_Austritt:
            lines.append(';Ebener Austritt')
        else:
            lines.append(";Durchmesser Hauptbohrung: %s" % self.d_hb)
        lines.append(";Durchmesser Querohrung:   %s" % self.d_qb)
        lines.append(";Achsversatz:              %s" % self.av)
        lines.append(";Anzahl Punkte:            %s" % self.n_points)
        lines.append(";Soll Fasenbreite:         %s" % self.fb_soll)
        lines.append(";Programmversion:          %s" % PROGRAMM_VERSION)
        lines.append(";Erstellungsdatum:         %s" % formatted_date_time)
        lines.append('')
        lines.append(';Manuelle Anapssungen durch Bedienenden der Werkzeugmaschine')
        lines.append('def real z_offset=0.0      ;Werkzeug beim Entgraeten hoeher [mm]')
        lines.append('def real winkel_offset=0.0 ;Korrektur fuer Winkelposition der Entgratschneide [Grad]')
        lines.append('')
        lines.append(';Winkeloffset pruefen:')
        lines.append('IF (winkel_offset > 360)')
        lines.append('   MSG("Winkel Offset muss kleiner als 360 Grad sein")')
        lines.append('   M30')
        lines.append('ENDIF')
        lines.append('IF (-360 > winkel_offset)')
        lines.append('   MSG("Winkel Offset muss groesser als -360 Grad sein")')
        lines.append('   M30')
        lines.append('ENDIF')
        lines.append('IF (0 > winkel_offset)')
        lines.append('   winkel_offset = winkel_offset + 360')
        lines.append('ENDIF')
        lines.append('')
        lines.append('G90 ;Absolute Maßangabe')
        lines.append('G17 ;X/Y Ebenenauswahl')
        lines.append('%s ;Aktives Koordinatensystem' % self.machine["WKS"])
        lines.append('')
        lines.append('T="%s" ;Verwendetes Werkzeug' % self.werkzeug.toolname)
        lines.append('%s ;Werkzeugwechsel' % self.machine["MCodeWerkzeugwechsel"])
        lines.append('')
        lines.append('%s ;Drehachsen klemmen' % self.machine["MCodeDrehachsenKlemmen"])
        lines.append('')
        return lines

    def __g_code_bohren(self):
        # Process parameters
        vc = self.setupBohren["Schnittgeschwindigkeit_vc"]
        f = self.setupBohren["Vorschub_f"]
        d = self.d_qb / 1000.0
        n = vc / (d * math.pi)

        # Positionen
        x_ = [self.sicherheitsebene1_x, self.sicherheitsebene2_x, self.bohrtiefe_x]
        y_ = [self.sicherheitsebene1_y, self.sicherheitsebene2_y, self.bohrtiefe_y]
        z_ = [self.sicherheitsebene1_z, self.sicherheitsebene2_z, self.bohrtiefe_z]
        phi_ = [0, 0, self.bohrtiefe_phi]

        x, y, z, _ = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 3)

        lines = []
        lines.append(';Bohren')
        lines.append('G00 Z%.3f ;Sicherheitsebene 1' % z[0])
        lines.append('G00 X%.3f Y%.3f Z%.3f ;Sicherheitsebene 1' % (x[0], y[0], z[0]))
        lines.append('G00 X%.3f Y%.3f Z%.3f ;Sicherheitsebene 2' % (x[1], y[1], z[1]))
        lines.append('')
        lines.append('G95 ;Umdrehungsvorschub [mm/Umdrehung]')
        lines.append('%s ;Maschinenhalt zur Kontrolle' % self.machine["MCodeKontrollMaschinenhalt"])
        lines.append('')
        lines.append('%s ; KSS an' % self.machine["MCodeEinschaltenKSS"])
        lines.append('S%d F%.3f %s ;Spindel an' % (n, f, self.machine["MCodeEinschaltenSpindel"]))
        lines.append('G4 F0.3 ;Verweilzeit für Druckaufbau')
        lines.append('G01 X%.3f Y%.3f Z%.3f ;Bohrtiefe' % (x[2], y[2], z[2]))
        lines.append('G4 F0.3 ;Verweilzeit Freischneiden')
        lines.append('')
        lines.append('%s ;Spindel aus' % self.machine["MCodeAusschaltenSpindel"])
        lines.append('%s ;Maschinenhalt zur Kontrolle' % self.machine["MCodeKontrollMaschinenhalt"])
        lines.append('')
        return lines

    def __g_code_entgraten_sinumerik_spos(self):
        # Positionen
        x_ = [self.sicherheitsebene1_x, self.bohrtiefe_x]
        y_ = [self.sicherheitsebene1_y, self.bohrtiefe_y]
        z_ = [self.sicherheitsebene1_z, self.bohrtiefe_z]
        phi_ = [0, self.bohrtiefe_phi]

        winkel_offset = self.werkzeug.winkeloffset

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 2)
        phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in phi]
        phi = angle_check(phi, 2)

        lines = []
        lines.append(';Entgraten')
        lines.append('G94 ;Linearvorschub')
        # Entfernt da es nicht zum richten Ergebnis führt
        # lines.append('%s ;Bahnsteuerbetrieb' % self.prozessparameter["Maschine"]["Hermle C32 U"]["Entgraten"]["Bahnsteuerbetrieb"])
        # lines.append('M70 ;Spindel in Lageregelung')
        lines.append('F=%.3f ;Vorschub' % self.setupEntgraten["Vorschub"])
        lines.append('')

        # Werkzeug in Stellung bringen
        n = int(self.setupEntgraten["Einfahren"])
        phi=[round(self.tcp_translated_phi[-n - 1] * 180.0 / math.pi) + winkel_offset]
        lines.append('G01 X%.3f Y%.3f Z%.3f ;Werkzeug in Stellung bringen' % (x[1], y[1], z[1]))
        lines.append('G00 SPOS=((%d + winkel_offset) MOD 360) ;Werkzeug in Stellung bringen' % (360.0 - phi[0]))
        lines.append('%s ;Maschinenhalt zur Kontrolle' % self.machine["MCodeKontrollMaschinenhalt"])

        # Einfahren
        x_ = [self.bohrtiefe_x]
        y_ = [self.bohrtiefe_y]
        z_ = [self.bohrtiefe_z]
        phi_ = [self.bohrtiefe_phi]

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 1)

        n = int(self.setupEntgraten["Einfahren"])

        lines.append('')
        lines.append(';Einfahren')
        for i in reversed(range(n)):

            alpha = (n - (i + 1)) / (n - 1)
            delta_x = self.tcp_translated_x[-i - 1] - x[0]
            delta_y = self.tcp_translated_y[-i - 1] - y[0]
            delta_z = self.tcp_translated_z[-i - 1] - z[0]

            tcp_x = x[0] + alpha * delta_x
            tcp_y = y[0] + alpha * delta_y
            tcp_z = z[0] + alpha * delta_z
            tcp_phi = [self.tcp_translated_phi[-i-1]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset SPOS=((%d + winkel_offset) MOD 360)' % (tcp_x, tcp_y, tcp_z, 360.0 - tcp_phi))

        # Punkte
        lines.append('')
        lines.append(';Punkte')
        for i in range(self.n_points):
            tcp_x = self.tcp_translated_x[i]
            tcp_y = self.tcp_translated_y[i]
            tcp_z = self.tcp_translated_z[i]
            tcp_phi = [self.tcp_translated_phi[i]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset SPOS=((%d + winkel_offset) MOD 360)' % (tcp_x, tcp_y, tcp_z, 360.0-tcp_phi))

        # Overlap
        x_ = [self.bohrtiefe_x, self.sicherheitsebene1_x]
        y_ = [self.bohrtiefe_y, self.sicherheitsebene1_y]
        z_ = [self.bohrtiefe_z, self.sicherheitsebene1_z]
        phi_ = [self.bohrtiefe_phi, 0, 0]

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 2)

        lines.append('')
        lines.append(';Overlap')
        for i in range(int(self.setupEntgraten["Overlap"])):
            tcp_x = self.tcp_translated_x[i]
            tcp_y = self.tcp_translated_y[i]
            tcp_z = self.tcp_translated_z[i]
            tcp_phi = [self.tcp_translated_phi[i]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset SPOS=((%d + winkel_offset) MOD 360)' % (tcp_x, tcp_y, tcp_z, 360.0-tcp_phi))

        lines.append('')
        lines.append(';Werkzeug rausfahren')
        lines.append('G01 X%.3f Y%.3f Z%.3f' % (x[0], y[0], z[0]))

        # Eventuelle M-Befehle ausschalten
        lines.append('%s ;KSS aus' % self.machine["MCodeAusschaltenKSS"])
        lines.append('%s ;Klemmung Drehachsen loesen' % self.machine["MCodeDrehachsenLoesen"])

        # Werkzeug beim Herausfahren drehen
        lines.append('S%d %s ;Spindel an' % (500, self.machine["MCodeEinschaltenSpindelAlt"]))
        lines.append('G00 X%.3f Y%.3f Z%.3f ;Sicherheitsebene 1' % (x[1], y[1], z[1]))
        return lines

    def __g_code_entgraten(self):
        # Positionen
        x_ = [self.sicherheitsebene1_x, self.bohrtiefe_x]
        y_ = [self.sicherheitsebene1_y, self.bohrtiefe_y]
        z_ = [self.sicherheitsebene1_z, self.bohrtiefe_z]
        phi_ = [0, self.bohrtiefe_phi]

        winkel_offset = self.werkzeug.winkeloffset

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 2)
        phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in phi]
        phi = angle_check(phi, 2)

        lines = []
        lines.append(';Entgraten')
        lines.append('G94 ;Linearvorschub')
        # Entfernt da es nicht zum richten Ergebnis führt
        # lines.append('%s ;Bahnsteuerbetrieb' % self.prozessparameter["Maschine"]["Hermle C32 U"]["Entgraten"]["Bahnsteuerbetrieb"])
        # lines.append('M70 ;Spindel in Lageregelung')
        lines.append('SPCON(1) ;Spindel in Lageregelung')
        lines.append('F=%.3f ;Vorschub' % self.setupEntgraten["Vorschub"])
        lines.append('')

        # Werkzeug in Stellung bringen
        n = int(self.setupEntgraten["Einfahren"])
        phi=[round(self.tcp_translated_phi[-n - 1] * 180.0 / math.pi) + winkel_offset]
        lines.append('G01 X%.3f Y%.3f Z%.3f ;Werkzeug in Stellung bringen' % (x[1], y[1], z[1]))
        lines.append('G00 SP=DC((%d + winkel_offset) MOD 360) ;Werkzeug in Stellung bringen' % (360.0 - phi[0]))
        lines.append('%s ;Maschinenhalt zur Kontrolle' % self.machine["MCodeKontrollMaschinenhalt"])

        # Einfahren
        x_ = [self.bohrtiefe_x]
        y_ = [self.bohrtiefe_y]
        z_ = [self.bohrtiefe_z]
        phi_ = [self.bohrtiefe_phi]

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 1)

        n = int(self.setupEntgraten["Einfahren"])

        lines.append('')
        lines.append(';Einfahren')
        for i in reversed(range(n)):

            alpha = (n - (i + 1)) / (n - 1)
            delta_x = self.tcp_translated_x[-i - 1] - x[0]
            delta_y = self.tcp_translated_y[-i - 1] - y[0]
            delta_z = self.tcp_translated_z[-i - 1] - z[0]

            tcp_x = x[0] + alpha * delta_x
            tcp_y = y[0] + alpha * delta_y
            tcp_z = z[0] + alpha * delta_z
            tcp_phi = [self.tcp_translated_phi[-i-1]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset SP=DC((%d + winkel_offset) MOD 360)' % (tcp_x, tcp_y, tcp_z, 360.0 - tcp_phi))

        # Punkte
        lines.append('')
        lines.append(';Punkte')
        for i in range(self.n_points):
            tcp_x = self.tcp_translated_x[i]
            tcp_y = self.tcp_translated_y[i]
            tcp_z = self.tcp_translated_z[i]
            tcp_phi = [self.tcp_translated_phi[i]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset SP=DC((%d + winkel_offset) MOD 360)' % (tcp_x, tcp_y, tcp_z, 360.0-tcp_phi))

        # Overlap
        x_ = [self.bohrtiefe_x, self.sicherheitsebene1_x]
        y_ = [self.bohrtiefe_y, self.sicherheitsebene1_y]
        z_ = [self.bohrtiefe_z, self.sicherheitsebene1_z]
        phi_ = [self.bohrtiefe_phi, 0, 0]

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 2)

        lines.append('')
        lines.append(';Overlap')
        for i in range(int(self.setupEntgraten["Overlap"])):
            tcp_x = self.tcp_translated_x[i]
            tcp_y = self.tcp_translated_y[i]
            tcp_z = self.tcp_translated_z[i]
            tcp_phi = [self.tcp_translated_phi[i]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset SP=DC((%d + winkel_offset) MOD 360)' % (tcp_x, tcp_y, tcp_z, 360.0-tcp_phi))

        lines.append('')
        lines.append(';Werkzeug rausfahren')
        lines.append('G01 X%.3f Y%.3f Z%.3f' % (x[0], y[0], z[0]))
        lines.append('SPCOF ;Spindel in Drehzahlsteuerbetrieb')

        # Eventuelle M-Befehle ausschalten
        lines.append('%s ;KSS aus' % self.machine["MCodeAusschaltenKSS"])
        lines.append('%s ;Klemmung Drehachsen loesen' % self.machine["MCodeDrehachsenLoesen"])

        # Werkzeug beim Herausfahren drehen
        lines.append('S%d %s ;Spindel an' % (500, self.machine["MCodeEinschaltenSpindelAlt"]))
        lines.append('G00 X%.3f Y%.3f Z%.3f ;Sicherheitsebene 1' % (x[1], y[1], z[1]))
        return lines

        #To-Do nicht an M Codes angepasst
    def __g_code_entgraten_fraesend(self):
        # Positionen
        x_ = [self.sicherheitsebene1_x, self.bohrtiefe_x]
        y_ = [self.sicherheitsebene1_y, self.bohrtiefe_y]
        z_ = [self.sicherheitsebene1_z, self.bohrtiefe_z]
        phi_ = [0, self.bohrtiefe_phi]

        winkel_offset = self.werkzeug.winkeloffset

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 2)
        phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in phi]
        phi = angle_check(phi, 2)

        lines = []
        lines.append(';Entgraten')
        lines.append('G94 ;Linearvorschub')
        # Debug fixed values
        lines.append('S%d F%.3f M3 M51;Spindel an' % (4000, 260.0))
        lines.append('')
        lines.append('G01 X%.3f Y%.3f Z%.3f ;Werkzeug in Stellung bringen' % (x[1], y[1], z[1]))
        lines.append('M1 ;Maschinenhalt zur Kontrolle')

        # Einfahren
        x_ = [self.bohrtiefe_x]
        y_ = [self.bohrtiefe_y]
        z_ = [self.bohrtiefe_z]
        phi_ = [self.bohrtiefe_phi]

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 1)

        n = self.setupEntgraten["Einfahren"]

        lines.append('')
        lines.append(';Einfahren')
        for i in reversed(range(n)):

            alpha = (n - (i + 1)) / (n - 1)
            delta_x = self.tcp_translated_x[-i - 1] - x[0]
            delta_y = self.tcp_translated_y[-i - 1] - y[0]
            delta_z = self.tcp_translated_z[-i - 1] - z[0]

            tcp_x = x[0] + alpha * delta_x
            tcp_y = y[0] + alpha * delta_y
            tcp_z = z[0] + alpha * delta_z
            tcp_phi = [self.tcp_translated_phi[-i-1]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset' % (tcp_x, tcp_y, tcp_z))

        # Punkte
        lines.append('')
        lines.append(';Punkte')
        for i in range(self.n_points):
            tcp_x = self.tcp_translated_x[i]
            tcp_y = self.tcp_translated_y[i]
            tcp_z = self.tcp_translated_z[i]
            tcp_phi = [self.tcp_translated_phi[i]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset' % (tcp_x, tcp_y, tcp_z))

        # Overlap
        x_ = [self.bohrtiefe_x, self.sicherheitsebene1_x]
        y_ = [self.bohrtiefe_y, self.sicherheitsebene1_y]
        z_ = [self.bohrtiefe_z, self.sicherheitsebene1_z]
        phi_ = [self.bohrtiefe_phi, 0, 0]

        x, y, z, phi = rotate_translate_points(self.transformation, x_, y_, z_, phi_, 2)

        lines.append('')
        lines.append(';Overlap')
        for i in range(self.setupEntgraten["Overlap"]):
            tcp_x = self.tcp_translated_x[i]
            tcp_y = self.tcp_translated_y[i]
            tcp_z = self.tcp_translated_z[i]
            tcp_phi = [self.tcp_translated_phi[i]]
            tcp_phi[:] = [round(x * 180.0 / math.pi + winkel_offset) for x in tcp_phi]
            tcp_phi = angle_check(tcp_phi, 1)[0]

            lines.append('G01 X%.3f Y%.3f Z=%.3f+z_offset' % (tcp_x, tcp_y, tcp_z))

        lines.append('')
        lines.append(';Werkzeug rausfahren')
        lines.append('G01 X%.3f Y%.3f Z%.3f' % (x[0], y[0], z[0]))
        lines.append('F=500')
        lines.append('G01 X%.3f Y%.3f Z%.3f ;Sicherheitsebene 1' % (x[1], y[1], z[1]))
        lines.append('M52 ;KSS aus')
        lines.append('M05')
        return lines

    def __str__(self):
        text = "Durchmesser Hauptbohrung: %s \n" \
               "Durchmesser Querbohrung:  %s \n" \
               "Achsversatz:              %s \n" \
               "Anzahl Punkte:            %s \n" \
               "Soll Fasenbreite:         %s" % (self.d_hb, self.d_qb, self.av, self.n_points, self.fb_soll)
        return text
