import FreeCAD
import numpy as np
import math
import time
import Part


TeachPointFold = """
;FOLD LIN P4  Vel= 0.2 m/s CPDAT1 Tool[1] Base[0];%{PE}%R 5.4.27,%MKUKATPBASIS,%CMOVE,%VLIN,%P 1:LIN, 2:P4, 3:, 5:0.2, 7:CPDAT1
$BWDSTART = FALSE
LDAT_ACT=LCPDAT1
FDAT_ACT=FP4
BAS(#CP_PARAMS,0.2)
LIN XP4
;ENDFOLD
"""

TeachPointDat = """
DECL E6POS XP4={X -25.1844196,Y 1122.42603,Z 1158.07996,A -14.3267002,B 0.537901878,C 179.028305,S 6,T 59,E1 0.0,E2 0.0,E3 0.0,E4 0.0,E5 0.0,E6 0.0}
DECL FDAT FP4={TOOL_NO 1,BASE_NO 0,IPO_FRAME #BASE,POINT2[] " "}
DECL LDAT LCPDAT1={VEL 2.0,ACC 100.0,APO_DIST 100.0,APO_FAC 50.0,ORI_TYP #VAR}
"""

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




class Kuka_Prog:
    def __init__(self):
        self.pose_list = []
        self.base = (0,0,0)

    def append_pose(self, pose):
        self.pose_list.append(pose)

    def append_poses(self, poses):
        if not len(self.pose_list):
            self.pose_list.extend(poses)
        # poses are sorted
        # but maybe we need to reverse
        #get the point distance from first and last pose
        last = self.pose_list[-1]
        nfirst = poses[0]
        nlast = poses[-1]
        last = FreeCAD.Base.Vector(last.X, last.Y, last.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:
            poses.reverse()
        self.pose_list.extend(poses)

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

    def save_prog(self, filename):
        srcfile = open(filename+'.src','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(";------------- 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,6) ;Initialicing the defaults for Vel and so on \n\n")
        srcfile.write("BAS (#BASE,2) ;Initialicing the defaults for Vel and so on \n\n")
        srcfile.write("PTP {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(";------------- main part ------------\n")
        srcfile.write("startp=$POS_ACT\n")
        #V = w.Velocity / 1000.0 # from mm/s to m/s
        V = 0.05
        srcfile.write("$VEL.CP = %f ; m/s ; m/s \n"%V)
        for pose in self.pose_list:
            srcfile.write("LIN startp:{} C_VEL; GENERATED\n".format(pose.to_string()))
        # end of subroutine
        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):
        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) + "}"

    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, edge, n):
    poses = []
    points = edge.discretize(n)
    lines = []
    for point in points:
        uv = face.Surface.parameter(point)
        normal = face.normalAt(uv[0], uv[1])
        line=Part.makeLine(point, point+3*normal)
        lines.append(line)
        # create pose
        pose = Kuka_Pose.from_point_and_normal(point, normal)
        poses.append(pose)
    return poses