import FreeCAD
import math
import time
import Part
import copy
import os
import os.path
from jinja2 import Environment, FileSystemLoader
import io


header_src = """&ACCESS RVP
&REL 1
&PARAM TEMPLATE = C:\KRC\Roboter\Template\ExpertVorgabe
&PARAM EDITMASK = *
"""

z_up_pose = """{X 0.0, Y 0.0, Z 30.0, A 0.0, B 0.0, C 0.0, E1 0.0, E2 0.0}"""

class Kuka_Layer:
    def __init__(self):
        self.contours = []
        self.hatchlines = []

class Kuka_Prog:
    def __init__(self):
        # each layer is a tuple of Contours[] and Hatchlines[]
        self.layers = []
        self.current_layer = -1
        # self.contour_path_list = []
        # self.hatchlines_list = []
        self.baseorigin = (0, 0, 0)
        self.tool = 6
        self.base = 6
        self.vproc = 0.023
        self.vmax = 0.15
        self.laser_power = 0.4
        self.laser_out = 3
        self.laser_pilot_out = 4  # default is pilot laser
        self.use_laser_out = self.laser_pilot_out
        self.inert_gas_out = 7 # gas
        self.powder_out = 8   # first powder pot
        self.simulation = True
        self.label = "REPLACEME"

    def set_baseorigin(self, vec):
        self.baseorigin = (vec.x, vec.y, vec.z)

    def set_tool(self, tool):
        self.tool = tool

    def set_base(self, base):
        self.base = base

    def set_velocity(self, vproc, vmax):
        self.vproc = vproc
        self.vmax = vmax

    def set_laser_power(self, power):
        self.laser_power = power

    def set_powder_out(self, powder):
        self.powder_out = powder

    def set_inert_gas_out(self, inert_gas):
        self.inert_gas_out = inert_gas

    def set_laser_out(self, laser_output):
        self.laser_out = laser_output

    def set_laser_pilot_out(self, laser_pilot_out):
        self.laser_pilot_out = laser_pilot_out

    def set_simulation(self, sim):
        self.simulation = sim
        if not self.simulation:
            self.use_laser_out = self.laser_out
        else:
            self.use_laser_out = self.laser_pilot_out

    def set_label(self, label):
        self.label = label

    def create_layer(self):
        self.layers.append(Kuka_Layer())
        self.current_layer += 1
        return self.current_layer

    def append_contour(self, poses, segmenttype='LIN'):
        layer = self.layers[self.current_layer]
        layer.contours.append((poses, segmenttype))

    def append_hatchline(self, line, segmenttype='LIN'):
        layer = self.layers[self.current_layer]
        if not len(layer.hatchlines):
            layer.hatchlines.append((line, segmenttype))
            return
        # poses are sorted
        # but maybe we need to reverse
        # get the point distance from first and last pose
        last, _ = layer.hatchlines[-1]
        nfirst = line[0]
        nlast = line[-1]
        last = FreeCAD.Base.Vector(last[1].X, last[1].Y, last[1].Z)
        nlast = FreeCAD.Base.Vector(nlast.X, nlast.Y, nlast.Z)
        nfirst = FreeCAD.Base.Vector(nfirst.X, nfirst.Y, nfirst.Z)
        dnl = last.distanceToPoint(nlast)
        dnf = last.distanceToPoint(nfirst)
        if dnl < dnf:
            line.reverse()
        layer.hatchlines.append((line, segmenttype))

    def draw_wire(self, obj):
        path = Part.makePolygon([FreeCAD.Base.Vector(p.X, p.Y, p.Z) for p in self.pose_list ])
        #s = Part.show(path)
        obj.addObject(s)
        s.ViewObject.LineColor=(1.0,0.5,0.0)
        s.ViewObject.LineWidth=(2.5)

    def get_vectors(self):
        return [FreeCAD.Base.Vector(p.X, p.Y, p.Z) for p in poses for poses in self.contour_path_list ]

    def save_with_template(self, article, path, templatename):
        if article[0].isdigit():
            article = "_"+article
        if self.simulation:
            filename_src = "{}_sim.src".format(article)
            filename_dat = "{}_sim.dat".format(article)
        else:
            filename_src = "{}.src".format(article)
            filename_dat = "{}.dat".format(article)

        user_dir = FreeCAD.getUserAppDataDir()
        template_dir = os.path.join(user_dir, "Mod", "fc_lasercladding_wb", "freecad", "LaserCladdingWorkbench", "templates")
        environment = Environment(loader=FileSystemLoader(template_dir))
        template_src = environment.get_template(templatename+".src")

        with io.StringIO("") as f:
            for layer_idx, layer in enumerate(self.layers):
                f.write(";- =============================\n")
                f.write(";- Layer {}\n".format(layer_idx))
                f.write(";- Contourpaths\n")
                for contour_idx, (poses, seg_type) in enumerate(layer.contours):
                    f.write(";- Contourpath {}\n".format(contour_idx))
                    # start laser code
                    f.write(";- Move to pose up from contour start\n")
                    f.write("$VEL.CP = TRAVELSPEED ; m/s (vmax)\n")
                    f.write("LIN_REL {Z 90.0} C_VEL; relative 90mm up\n")
                    f.write("LIN refpose:{}:{} C_VEL; move to start point but save z distance\n".format(poses[0].translate_with(self.baseorigin).to_string(), z_up_pose))
                    if seg_type == 'LIN':
                        f.write("LIN refpose:{} C_VEL; GENERATED\n".format(poses[0].translate_with(self.baseorigin).to_string()))
                        f.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=TRUE ; Turn on Laser at point\n" % self.use_laser_out)
                        f.write("$VEL.CP = WELDSPEED ; m/s ; m/s (vproc)\n")
                        for pose in poses[1:]:
                            f.write("LIN refpose:{} C_VEL; GENERATED\n".format(pose.translate_with(self.baseorigin).to_string()))

                    if seg_type == 'SPLINE':
                        f.write("SPLINE\n")
                        for pose in poses:
                            f.write("  SPL refpose:{} ; GENERATED\n".format(pose.translate_with(self.baseorigin).to_string()))
                        f.write("ENDSPLINE\n")

                    f.write(";- Turn off Laser\n")
                    f.write("$OUT[%d] = FALSE\n" % self.use_laser_out)
                    # end of subroutine

                f.write(";- =============================\n")
                if len(layer.hatchlines):
                    print("Number Hatchlines: ", len(layer.hatchlines))
                    f.write(";- Hatchlines\n")
                    f.write("$VEL.CP = TRAVELSPEED ; m/s (vmax)\n")
                    f.write("LIN_REL {Z 90.0} C_VEL; just move up \n")
                    # to first point on part
                    (first_hatch_line, _) = layer.hatchlines[0]
                    segment = first_hatch_line[0]
                    f.write("LIN refpose:{}:{} C_VEL; move to first hatch point but with z_up\n".format(segment.translate_with(self.baseorigin).to_string(), z_up_pose))

                    for (line, seg_type) in layer.hatchlines:
                        # a line has many segments
                        # start laser at first segment
                        # stop with last
                        f.write(";- Hatchline\n")
                        f.write(";- First Point in line \n")
                        segment = line[0]
                        f.write("$VEL.CP = TRAVELSPEED ; m/s (vproc)\n")
                        f.write("LIN refpose:{} C_VEL; GENERATED\n".format(segment.translate_with(self.baseorigin).to_string()))
                        f.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=True; Turn on Laser at point  \n" % self.use_laser_out)
                        f.write("$VEL.CP = WELDSPEED ; m/s (vproc)\n")
                        # each segment
                        for segment in line[1:-1]:
                            f.write("LIN refpose:{} C_VEL; GENERATED\n".format(segment.translate_with(self.baseorigin).to_string()))
                        segment = line[-1]
                        f.write(";- Last Point in line \n")
                        f.write("LIN refpose:{} C_VEL; GENERATED\n".format(segment.translate_with(self.baseorigin).to_string()))
                        f.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=FALSE; Turn off Laser at point\n" % self.use_laser_out)
                        # if z height difference between this and next point, we should move up, else it's faster to just go on.
                        #f.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax)
                        #f.write("LIN refpose:{}:{} C_VEL; move up from last hatch point \n".format(segment.translate_with(self.baseorigin).to_string(), z_up_pose))
                        f.write(";- End line \n")


                f.write(";- ========= END LAYER =========\n")
                f.write(";- =============================\n")
                # end of subroutine

            content = template_src.render(
                simulation=self.simulation,
                artikel=article,
                tool=self.tool,
                base=self.base,
                powder_out=self.powder_out,
                inert_gas_out=self.inert_gas_out,
                laser_out=self.use_laser_out,
                laserpower=self.laser_power,
                vproc=self.vproc,
                vmax=self.vmax,
                paths=f.getvalue(),
                label=self.label
            )

            # dat template
            template_dat = environment.get_template(templatename+".dat")
            datcontent = template_dat.render(
                simulation=self.simulation,
                artikel=article
            )

        ### Save the rendere template to output file
        with open(filename_src, mode="w", encoding="utf-8") as message:
            message.write(content)
            print(f"... wrote {filename_src}")

        with open(filename_dat, mode="w", encoding="utf-8") as message:
            message.write(datcontent)
            print(f"... wrote {filename_dat}")



    def save_prog(self, article, path):
        if self.simulation:
            filename = "kvt_{}_sim.src".format(article)
        else:
            filename = "kvt_{}.src".format(article)

        srcfile = open(os.path.join(path, filename), 'w')
        srcfile.write(header_src)
        # subroutine definition
        srcfile.write("DEF "+filename+"( )\n\n")
        srcfile.write(";- Kuka src file, generated by KVT\n")
        srcfile.write(";- " + time.asctime() + "\n\n")
        # defining world and base
        srcfile.write("E6POS startp\n")
        srcfile.write("REAL WELDSPEED\n")
        srcfile.write("REAL TRAVELSPEED\n")
        srcfile.write("REAL LASERPOWER\n")
        srcfile.write("E6POS point1\n")
        # srcfile.write("DECL E6AXIS xp1={A1 -1.9, A2 -105.76, A3 79.97, A4 178.83, A5 -20.3, A6 -4.37, E1 -90, E2 0}\n")
        srcfile.write(";------------- definitions ------------\n")
        srcfile.write("EXT BAS (BAS_COMMAND :IN,REAL :IN ) ;set base to World\n")
        srcfile.write("BAS (#INITMOV,0 ) ;Initialicing the defaults for Vel and so on \n\n")
        srcfile.write("BAS (#TOOL,%d) ;Initialicing the defaults for Vel and so on \n\n" % self.tool)
        srcfile.write("BAS (#BASE,%d) ;Initialicing the defaults for Vel and so on \n\n" % self.base)
        srcfile.write("PTP {A1 -33.31, A2 -104.71, A3 114.60, A4 282.66, A5 -39.21, A6 -104.87, E1 -90, E2 1.0}\n")
        srcfile.write("\n;------------- main part ------------\n")
        #V = w.Velocity / 1000.0 # from mm/s to m/s
        CDIS = 2.3
        CVEL = 95.0
        srcfile.write(";- Process Parameters (change here)\n")
        srcfile.write("TRAVELSPEED = %f ; m/s\n" % self.vmax)
        srcfile.write("WELDSPEED = %f ; m/s\n" % self.vproc)
        srcfile.write("LASERPOWER = %f ;  Set laser power\n" % self.laser_power)

        #srcfile.write("; hier teach punkt eingeben\n;FOLD PTP P1 Vel=25 % PDAT1 Tool[2]:LASER Base[2]:Laser;%{PE}%R 8.2.24,%MKUKATPBASIS,%CMOVE,%VPTP,%P 1:PTP, 2:P1, 3:, 5:25, 7:PDAT1\n$BWDSTART=FALSE\nPDAT_ACT=PPDAT1\nFDAT_ACT=FP1\nBAS(#PTP_PARAMS,25)\nPTP XP1\n;ENDFOLD\n")



        srcfile.write("startp=$POS_ACT\n")
        srcfile.write(";- Ab hier nicht mehr aendern!\n")
        srcfile.write(";- Movement parameters\n")
        srcfile.write("$VEL.CP = TRAVELSPEED\n")
        srcfile.write("$APO.CDIS = %f ; mm \n" % CDIS)
        srcfile.write("$APO.CVEL = %f ; percent \n" % CVEL)

        srcfile.write(";- Input/Output settings\n")
        srcfile.write("$ANOUT[1] = LASERPOWER ;  Set laser power\n")
        srcfile.write("$OUT[%d] = FALSE ;  Set Laser off\n" % self.use_laser_out)  # don't know but must be on
        if not self.simulation:
            srcfile.write("$OUT[2] = TRUE ;  Set Laser activation on\n")  # don't know but must be on
            srcfile.write("$OUT[%d] = TRUE ; Set powder on \n" % self.powder_out)
            srcfile.write("$OUT[%d] = TRUE ; Set inert gas on \n" % self.inert_gas_out)
        else:
            srcfile.write("$OUT[%d] = FALSE ;  set powder off \n" % self.powder_out)
            srcfile.write("$OUT[%d] = FALSE ;  set inert gas off\n" % self.inert_gas_out)

        srcfile.write(";- Starting point\n")
        srcfile.write("point1 = {X -110.0, Y 0.0, Z 0.0, A 0.0000, B 0.0000, C 0.0000, E1 0.0000, E2 0.0000}\n")
        srcfile.write("point1.S = startp.S\n")
        srcfile.write("point1.T = startp.T\n")
        srcfile.write("LIN point1 C_VEL; GENERATED\n")
        srcfile.write("WAIT SEC 7.0\n")
        for layer_idx, layer in enumerate(self.layers):
            srcfile.write(";- =============================\n")
            srcfile.write(";- Layer {}\n".format(layer_idx))
            srcfile.write(";- Contourpaths\n")
            for contour_idx, (poses, seg_type) in enumerate(layer.contours):
                srcfile.write(";- Contourpath {}\n".format(contour_idx))
                # start laser code
                srcfile.write(";- Move to pose up from contour start\n")
                srcfile.write("$VEL.CP = %f ; m/s ; m/s (vmax)\n" % self.vmax)
                srcfile.write("LIN_REL {Z 90.0} C_VEL; relative 90mm up\n")
                srcfile.write("LIN {}:{} C_VEL; move to start point but save z distance\n".format(poses[0].translate_with(self.baseorigin).to_string(), z_up_pose))
                if seg_type == 'LIN':
                    srcfile.write("LIN {} C_VEL; GENERATED\n".format(poses[0].translate_with(self.baseorigin).to_string()))
                    srcfile.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=True ; Turn on Laser at point\n" % self.use_laser_out)
                    srcfile.write("$VEL.CP = %f ; m/s ; m/s (vproc)\n" % self.vproc)
                    for pose in poses[1:]:
                        srcfile.write("LIN {} C_VEL; GENERATED\n".format(pose.translate_with(self.baseorigin).to_string()))

                if seg_type == 'SPLINE':
                    srcfile.write("SPLINE\n")
                    for pose in poses:
                        srcfile.write("  SPL {} ; GENERATED\n".format(pose.translate_with(self.baseorigin).to_string()))
                    srcfile.write("ENDSPLINE\n")

                srcfile.write(";- Turn off Laser\n")
                srcfile.write("$OUT[%d] = FALSE\n" % self.use_laser_out)
                # end of subroutine

            srcfile.write(";- =============================\n")
            srcfile.write(";- Hatchlines\n")
            srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax)
            srcfile.write("LIN_REL {Z 90.0} C_VEL; just move up \n")
            for (line, seg_type) in layer.hatchlines:
                # a line has many segments
                # start laser at first segment
                # stop with last
                segment = line[0]
                srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax)
                srcfile.write("LIN {}:{} C_VEL; move to first hatch point but with z_up\n".format(segment.translate_with(self.baseorigin).to_string(), z_up_pose))
                # One Hatchline
                srcfile.write(";- Hatchline\n")
                srcfile.write("LIN {} C_VEL; GENERATED\n".format(segment.translate_with(self.baseorigin).to_string()))
                srcfile.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=True; Turn on Laser at point  \n" % self.use_laser_out)
                # each segment
                for segment in line[1:-1]:
                    srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vproc)
                    srcfile.write("LIN {} C_VEL; GENERATED\n".format(segment.translate_with(self.baseorigin).to_string()))
                segment = line[-1]
                srcfile.write("LIN {} C_VEL; GENERATED\n".format(segment.translate_with(self.baseorigin).to_string()))
                srcfile.write("TRIGGER WHEN DISTANCE=0 DELAY=0 DO $OUT[%d]=FALSE; Turn off Laser at point\n" % self.use_laser_out)

            srcfile.write(";- ========= END LAYER =========\n")
            srcfile.write(";- =============================\n")
        # end of subroutine
        srcfile.write("$OUT[%d] = FALSE; laser off\n" % self.use_laser_out)
        srcfile.write("$OUT[%d] = FALSE; powder off\n" % self.powder_out)
        srcfile.write("$OUT[%d] = FALSE; inert gas off\n" % self.inert_gas_out)
        srcfile.write("$VEL.CP = %f ; m/s ; m/s \n" % self.vmax)
        srcfile.write(";- Move to HOME position\n")
        srcfile.write("PTP {A1 -33.31, A2 -104.71, A3 114.60, A4 282.66, A5 -39.21, A6 -104.87, E1 -90, E2 1.0}\n")
        srcfile.write("\n;------------- end ------------\n")
        srcfile.write("END \n\n")
        srcfile.close()

class Kuka_Pose:
    def __init__(self):
        self.X = 0.0
        self.Y = 0.0
        self.Z = 0.0
        self.A = 0.0
        self.B = 0.0
        self.C = 0.0

        self.S = 0
        self.T = 0

        self.E1 = 0.0
        self.E2 = 0.0

    def set_from_point_and_normal(self, point, normal):
        self.X = point.x
        self.Y = point.y
        self.Z = point.z

        r = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,0,1), normal)
        ABC_in_deg = r.toEulerAngles('ZYX')
        self.A = math.radians(ABC_in_deg[0])
        self.B = math.radians(ABC_in_deg[1])
        self.C = math.radians(ABC_in_deg[2])
        #print("Rotation:", self.A, self.B, self.C)

    def from_point_and_normal(point, normal):
        pose = Kuka_Pose()
        pose.X = point.x
        pose.Y = point.y
        pose.Z = point.z

        r = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,0,1), normal)
        ABC_in_deg = r.toEulerAngles('ZYX')
        pose.A = math.radians(ABC_in_deg[0])
        pose.B = math.radians(ABC_in_deg[1])
        pose.C = math.radians(ABC_in_deg[2])
        #print("Rotation:", self.A, self.B, self.C)
        return pose

    def to_string(self, rot=False):
        if rot:
            pose_string="X {:.3f}, Y {:.3f}, Z {:.3f}, A {:.4f}, B {:.4f}, C {:.4f}, E1 {:.4f}, E2 {:.4f}"
            return "{" + pose_string.format(self.X, self.Y, self.Z, self.A, self.B, self.C, self.E1, self.E2) + "}"
        else:
            pose_string="X {:.3f}, Y {:.3f}, Z {:.3f}, A {:.4f}, B {:.4f}, C {:.4f}, E1 {:.4f}, E2 {:.4f}"
            return "{" + pose_string.format(self.X, self.Y, self.Z, 0,0,0,0,0) + "}"


    def translate_with(self, vector):
        pose = copy.copy(self)
        pose.X = pose.X - vector[0]
        pose.Y = pose.Y - vector[1]
        pose.Z = pose.Z - vector[2]
        return pose


    def draw_pose(self):
        #line=Part.makeLine(point, point+3*normal)
        #lines.append(line)
        # from euler to some line
        # create upfacing vector then rotate around each axis?
        up = FreeCAD.Base.Vector(0,0,1)
        rotx = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(1,0,0), math.degrees(self.C))
        roty = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,1,0), math.degrees(self.B))
        rotz = FreeCAD.Base.Rotation(FreeCAD.Base.Vector(0,0,1), math.degrees(self.A))

        rot = rotz.multiply(roty.multiply(rotx))
        up_rotated = rot.multVec(up)

        basepoint = FreeCAD.Base.Vector(self.X, self.Y, self.Z)
        line = Part.makeLine(basepoint, basepoint+5*up_rotated)
        #line.Placement.Rotation = rot
        s = Part.show(line)
        s.ViewObject.LineColor=(1.0,0.0,0.0)



def get_list_of_poses(face, edges):
    poses = []
    for edge in edges:
        p0 = edge.Vertexes[0].Point
        p1 = edge.Vertexes[1].Point

        for p in [p0, p1]:
            uv = face.Surface.parameter(p)
            normal = face.normalAt(uv[0], uv[1])
            pose = Kuka_Pose.from_point_and_normal(p, normal)
            poses.append(pose)
    return poses