import math
import numpy as np
from stl import mesh, Mode
import stl.base
from os import path


STL_PATH = './STL/'


def get_faces(n_points):
    faces = np.zeros((2 * n_points, 3))
    for i in range(n_points):
        faces[2 * i][0] = i
        faces[2 * i][1] = (i + 1) % n_points
        faces[2 * i][2] = i + n_points

        faces[2 * i + 1][0] = (i + 1) % n_points
        faces[2 * i + 1][1] = (i + 1) % n_points + n_points
        faces[2 * i + 1][2] = i + n_points
    return faces


def combined_stl(files, name):
    save_path = path.join(STL_PATH, name)
    meshes = []
    for file in files:
        meshes.append(mesh.Mesh.from_file(file))
    combined = mesh.Mesh(np.concatenate([m.data for m in meshes]))
    combined.save(save_path)
    # Save normals
    name_normals = "normals_%s" % name.replace('.stl', '.txt')
    save_path = path.join(STL_PATH, name_normals)
    np.savetxt(save_path, combined.normals, delimiter=',')
    # combined.save(save_path)


def create_stl(vertices, faces, name):
    save_path = path.join(STL_PATH, name)
    my_mesh = mesh.Mesh(np.zeros(faces.shape[0], dtype=mesh.Mesh.dtype))
    for i, f in enumerate(faces):
        for j in range(3):
            my_mesh.vectors[i][j] = vertices[int(f[j]), :]

    # Write the mesh to file "cube.stl"
    # my_mesh.normals
    my_mesh.save(save_path)


class Werkzeug:
    def __init__(self, tool, n_points):
        self.toolname = tool['toolname']
        self.d_kopf = tool['d_kopf']
        self.l_kopf = tool['l_kopf']
        self.l_kopf_gesamt = tool['l_kopf_gesamt']
        self.d_schaft = tool['d_schaft']
        self.halslaenge = tool['l_werkzeug']
        self.l_werkzeug_gesamt = tool['l_werkzeug_gesamt']
        self.l_schaft = tool['l_schaft']
        self.d_schaft2 = tool['d_schaft2']
        self.spitzenwinkel = tool['spitzenwinkel']
        self.schneidenwinkel = tool['schneidenwinkel']
        self.freiwinkel = tool['freiwinkel']
        self.n_points = n_points
        self.winkeloffset = tool["winkeloffset"]

        self.h_spitze = self.__get_spitze()
        self.h_kopf = self.__get_kopf()
        self.h_schneide = self.__get_schneide()

        # Add h_Schneide to Halslaenge
        self.l_werkzeug = self.h_schneide + self.halslaenge

        self.wz_rad_distance = ((self.d_kopf / 2.0) + (self.d_schaft / 2.0)) / 2.0
        self.wz_ax_distance = (self.h_kopf + self.h_schneide) / 2.0
        self.__get_schaft()
        self.__get_deckel()

        files = ['./STL/spitze.stl', './STL/kopf.stl', './STL/schneide.stl', './STL/schaft.stl', './STL/deckel.stl']
        combined_stl(files, 'combined.stl')
        files = ['./STL/spitze.stl', './STL/kopf_real.stl', './STL/schneide_real.stl', './STL/schaft_real.stl', './STL/deckel.stl']
        combined_stl(files, 'combined_real.stl')

    def __get_spitze(self):
        vertices = np.zeros((self.n_points + 1, 3))
        faces = np.zeros((self.n_points, 3))

        # Calculate vertices
        winkelschritt = 360.0 / self.n_points
        r_kopf = self.d_kopf / 2.0
        halber_spitzenwinkel = self.spitzenwinkel / 2.0
        h = r_kopf / math.tan(halber_spitzenwinkel * math.pi / 180)
        for i in range(self.n_points):
            vertices[i + 1][0] = math.cos(i * winkelschritt * math.pi / 180) * r_kopf
            vertices[i + 1][1] = math.sin(i * winkelschritt * math.pi / 180) * r_kopf
            vertices[i + 1][2] = h

        # Create Faces
        for i in range(self.n_points):
            faces[i][0] = 0
            faces[i][1] = i + 2
            faces[i][2] = i + 1
        # Last
        faces[-1][0] = 0
        faces[-1][1] = 1
        faces[-1][2] = self.n_points

        # Create Stl
        create_stl(vertices, faces, 'spitze.stl')
        return h

    def __get_kopf(self):
        vertices = np.zeros((2*self.n_points, 3))
        vertices_real = np.zeros((2 * self.n_points, 3))
        faces = get_faces(self.n_points)

        h_kopf = self.h_spitze + self.l_kopf
        winkelschritt = 360.0 / self.n_points
        r_kopf = self.d_kopf / 2.0

        for i in range(self.n_points):
            # Calculate Values
            x = math.cos(i * winkelschritt * math.pi / 180) * r_kopf
            y = math.sin(i * winkelschritt * math.pi / 180) * r_kopf

            vertices[i][0] = x
            vertices[i][1] = y
            vertices[i][2] = self.h_spitze

            vertices[self.n_points + i][0] = x
            vertices[self.n_points + i][1] = y
            vertices[self.n_points + i][2] = h_kopf

            # Real verticies
            vertices_real[i][0] = x
            vertices_real[i][1] = y
            vertices_real[i][2] = self.h_spitze

            vertices_real[self.n_points + i][0] = x
            vertices_real[self.n_points + i][1] = y
            # Abfall aufgrund Freiwinkel
            if winkelschritt * i <= 90:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_kopf * (winkelschritt * i * math.pi / 180)
            elif winkelschritt * i >= 180 and winkelschritt * i <= 270:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_kopf * ((winkelschritt * i - 180) * math.pi / 180)
            else:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_kopf * (90 * math.pi / 180)
            vertices_real[self.n_points + i][2] = h_kopf - dz

        # Create Stl
        create_stl(vertices, faces, 'kopf.stl')
        create_stl(vertices_real, faces, 'kopf_real.stl')
        return h_kopf

    def __get_schneide(self):
        vertices = np.zeros((2 * self.n_points, 3))
        vertices_real = np.zeros((2 * self.n_points, 3))
        faces = get_faces(self.n_points)

        r_kopf = self.d_kopf / 2.0
        r_schaft = self.d_schaft / 2.0
        delta_r = r_kopf - r_schaft
        x = delta_r / math.tan(self.schneidenwinkel * math.pi / 180)
        h_schneide = self.h_kopf + x

        winkelschritt = 360.0 / self.n_points

        for i in range(self.n_points):
            # Calculate Values
            vertices[i][0] = math.cos(i * winkelschritt * math.pi / 180) * r_kopf
            vertices[i][1] = math.sin(i * winkelschritt * math.pi / 180) * r_kopf
            vertices[i][2] = self.h_kopf

            # Real verticies
            vertices_real[i][0] = math.cos(i * winkelschritt * math.pi / 180) * r_kopf
            vertices_real[i][1] = math.sin(i * winkelschritt * math.pi / 180) * r_kopf
            # Abfall aufgrund Freiwinkel
            if winkelschritt * i <= 90:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_kopf * (winkelschritt * i * math.pi / 180)
            elif winkelschritt * i >= 180 and winkelschritt * i <= 270:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_kopf * ((winkelschritt * i - 180) * math.pi / 180)
            else:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_kopf * (90 * math.pi / 180)
            vertices_real[i][2] = self.h_kopf - dz

            vertices[self.n_points + i][0] = math.cos(i * winkelschritt * math.pi / 180) * r_schaft
            vertices[self.n_points + i][1] = math.sin(i * winkelschritt * math.pi / 180) * r_schaft
            vertices[self.n_points + i][2] = h_schneide

            # Real verticies
            vertices_real[self.n_points + i][0] = math.cos(i * winkelschritt * math.pi / 180) * r_schaft
            vertices_real[self.n_points + i][1] = math.sin(i * winkelschritt * math.pi / 180) * r_schaft
            # Abfall aufgrund Freiwinkel
            if winkelschritt * i <= 90:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_schaft * (winkelschritt * i * math.pi / 180)
            elif winkelschritt * i >= 180 and winkelschritt * i <= 270:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_schaft * ((winkelschritt * i - 180) * math.pi / 180)
            else:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_schaft * (90 * math.pi / 180)
            vertices_real[self.n_points + i][2] = h_schneide - dz

        # Create Stl
        create_stl(vertices, faces, 'schneide.stl')
        create_stl(vertices_real, faces, 'schneide_real.stl')
        return h_schneide

    def __get_schaft(self):
        vertices = np.zeros((2*self.n_points, 3))
        vertices_real = np.zeros((2 * self.n_points, 3))
        faces = get_faces(self.n_points)

        winkelschritt = 360.0 / self.n_points
        r_schaft = self.d_schaft / 2.0

        for i in range(self.n_points):
            # Calculate Values
            x = math.cos(i * winkelschritt * math.pi / 180) * r_schaft
            y = math.sin(i * winkelschritt * math.pi / 180) * r_schaft

            vertices[i][0] = x
            vertices[i][1] = y
            vertices[i][2] = self.h_schneide

            vertices[self.n_points + i][0] = x
            vertices[self.n_points + i][1] = y
            vertices[self.n_points + i][2] = self.l_werkzeug

            # Real verticies
            vertices_real[i][0] = x
            vertices_real[i][1] = y

            # Abfall aufgrund Freiwinkel
            if winkelschritt * i <= 90:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_schaft * (winkelschritt * i * math.pi / 180)
            elif winkelschritt * i >= 180 and winkelschritt * i <= 270:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_schaft * ((winkelschritt * i - 180) * math.pi / 180)
            else:
                dz = math.tan(self.freiwinkel * math.pi / 180) * r_schaft * (90 * math.pi / 180)
            vertices_real[i][2] = self.h_schneide - dz

            vertices_real[self.n_points + i][0] = x
            vertices_real[self.n_points + i][1] = y
            vertices_real[self.n_points + i][2] = self.l_werkzeug

        # Create Stl
        create_stl(vertices, faces, 'schaft.stl')
        create_stl(vertices_real, faces, 'schaft_real.stl')

    def __get_deckel(self):
        vertices = np.zeros((self.n_points + 1, 3))
        faces = np.zeros((self.n_points, 3))

        # Calculate vertices
        winkelschritt = 360.0 / self.n_points
        r_schaft = self.d_schaft / 2.0

        vertices[0][0] = 0.0
        vertices[0][1] = 0.0
        vertices[0][2] = self.l_werkzeug

        for i in range(self.n_points):
            vertices[i + 1][0] = math.cos(i * winkelschritt * math.pi / 180) * r_schaft
            vertices[i + 1][1] = math.sin(i * winkelschritt * math.pi / 180) * r_schaft
            vertices[i + 1][2] = self.l_werkzeug

        # Create Faces
        for i in range(self.n_points):
            faces[i][0] = 0
            faces[i][1] = i + 1
            faces[i][2] = i + 2
        # Last
        faces[-1][0] = 0
        faces[-1][1] = self.n_points
        faces[-1][2] = 1
        create_stl(vertices, faces, 'deckel.stl')

    def rotate_translate_tool(self, x, y, z, phi, n_points):
        file = path.join(STL_PATH, 'combined_real.stl')
        for i in range(n_points):
            tool = mesh.Mesh.from_file(file)
            tool.rotate([0.0, 0.0, -1.0], phi[i])
            tool.x += x[i]
            tool.y += y[i]
            tool.z += z[i]

            save_path = path.join(STL_PATH, 'Sweep/combined_real_rotated%04d.stl' % i)
            tool.save(save_path)