#!/usr/bin/python3

# Siehe auch https://en.wikipedia.org/wiki/HP-GL
# und https://paulbourke.net/dataformats/hpgl/

import sys
import math

Scale = 0.025  # KiCad hpgl Auflösung, anpassen falls nötig
DistTol = 0.04 # Toleranz um Rundungsfehler zwischen zusammengehängten Linien abzufangen, anpassen falls nötig


MirrorX = False
MirrorY = False
Incremental = False
Zero = [0.0, 0.0]
CurPos = (0.0, 0.0)
InputFile = ""
OutputFile = ""
PenDown = False
PenIsDown = True

Init = []

if len(sys.argv) > 1:
    for arg in sys.argv[1:]:
        if arg == "-inc":
            Incremental = True
        elif arg == "-mx":
            MirrorX = True
        elif arg == "-my":
            MirrorY = True
        elif arg[:4] == "-zx=":
            Zero[0] = float(arg[4:])
        elif arg[:4] == "-zy=":
            Zero[1] = float(arg[4:])
        elif arg[:3] == "-i=":
            Init.append(arg[3:])
        elif arg[:1] == "-":
            print("Unbekannte Option:" + arg)
            InputFile = ""
            break
        else:
            if InputFile == "":
                InputFile = arg
            elif OutputFile == "":
                OutputFile = arg
            else:
                print("Unbekannte Option:" + arg)
                InputFile = ""
                break

if InputFile == "":
    print ("Konvertiert ein HPGL Plotfille von KiCat Layout in ein CNC File.")
    print ("Aufruf: plt2cnc [-inc] [-mx] [-my] [-zx=nn.nn] [-zy=nn.nn] [-i=init] Input.plt [Output.cnc], wobei:")
    print ("-inc = Inkremental Werte, also XI/YI anstelle X/Y")
    print ("-mx/y = Mirror X/Y")
    print ("-zx/y = Zero X/Y")
    print ("-i = Init cnc, mehrfach möglich z.B -i=V12 -i='F 200' -i=\"P X0 Y0\"")
    print ("Input.plt = Eingabedatei, Output.cnc = Ausgabedatei oder Console falls keine angegeben")
    exit()
# Datei lesen und verarbeiten

# Speichert nur die zu zeichnenden Linien und Radien um sie später zu optimieren
LineStack = []
PosFrom = (0.0, 0.0)
PosTo = (0.0, 0.0)
def StoLn(NPos, Rad, Pen):
    global LineStack, PosFrom, PosTo
    if Pen == False:
        PosFrom = NPos
    else:
        PosTo = NPos
        LineStack.append((PosFrom, PosTo, -Rad))
        PosFrom = PosTo

# Schreibt cnc Kommandos. NPos(x,y) Werte sind Absolut, Rad ist Radius
def WriteCNC(NPos, Rad, Pen):
    global CurPos, PenIsDown, MirrorX, MirrorY, Incremental, DistTol

    if PenIsDown != Pen:
        if Pen == True: FileOut.write("Z F\n")
        else: FileOut.write("Z P\n")
        PenIsDown = Pen

    nx = NPos[0]
    ny = NPos[1]
    if Incremental == True:
        nx -= CurPos[0]
        ny -= CurPos[1]
    if MirrorX == True: nx = -nx
    if MirrorY == True: ny = -ny
    if MirrorX == MirrorY: Rad = -Rad
    if Incremental == True:
        if abs(nx) > DistTol:
            FileOut.write(str("XI{:7.3f}".format(nx)))
            if abs(ny) > DistTol: FileOut.write(" ")
        if abs(ny) > DistTol:
            FileOut.write(str("YI{:7.3f}".format(ny)))
    else:
        FileOut.write(str("X{:7.3f}".format(nx)) + str(" Y{:7.3f}".format(ny)))
    if abs(Rad) > DistTol: FileOut.write(str(" R{:7.3f}".format(Rad)))
    FileOut.write("\n")
    CurPos = NPos

def GetCmdVal(Cmd):
    global Zero, Scale
    Val = Cmd[3:].split(",")
    if   Cmd[:3] == "CI ": return( (float(Val[0]) * Scale) ) # Kreis Radius
    elif Cmd[:3] == "PA ": return( (float(Val[0]) * Scale) - Zero[0], (float(Val[1]) * Scale) - Zero[1] ) # X-Y Pos
    elif Cmd[:3] == "AA ": return( (float(Val[0]) * Scale) - Zero[0], (float(Val[1]) * Scale) - Zero[1], (float(Val[2])) ) # X, Y, Winkel, Step(unused)


FileIn = open(InputFile)
if OutputFile != "":
    FileOut = open(OutputFile, 'w')
else:
    FileOut = sys.stdout

for init in Init:
    FileOut.write(init + "\n")

# Wir lesen die Plot Datei und speichern die Linien und Radien. Radien von mehr als 180 Grad sowie Kreise werden als 2 Radien gespeichert.
for InputLine in FileIn:
    Line = InputLine.split(";")
    for Cmd in Line:
        # FileOut.write(Cmd + ":")
        if Cmd == "PU": PenDown = False
        elif Cmd == "PD": PenDown = True
        elif Cmd[:3] == "CI ": # Circle
            Radius = GetCmdVal(Cmd)
            # if Remarks == True: FileOut.write(str(" ; Kreis R={:7.3f}\n".format(Radius)))
            NewPos = (NewPos[0] - Radius, NewPos[1]) # Ist im Zentrum, fahre den Radius an den Kreis Anfang
            StoLn(NewPos, 0.0, PenDown)
            CurPos = NewPos
            PenDown = True
            NewPos = (NewPos[0] + 2.0 * Radius, NewPos[1]) # Halbkreis auf die andere Seite des Zentrums
            StoLn(NewPos, -Radius, PenDown)
            CurPos = NewPos
            NewPos = (NewPos[0] - 2.0 * Radius, NewPos[1]) # Halbkreis auf die andere Seite des Zentrums
            StoLn(NewPos, -Radius, PenDown)
            CurPos = NewPos
            PenDown = False

        elif Cmd[:3] == "AA ": # Absolute Arc
            # Siehe Kreiswinkel.ods
            NPos = GetCmdVal(Cmd)
            # Der Radius ist die Distanz, also der Pythagoras zwischen Zentrum und Anfang (Kante B im Dreieck)
            Radius = ( ((NPos[0] - CurPos[0]) ** 2.0) + ((NPos[1] - CurPos[1]) ** 2.0)) ** 0.5
            Ang = float(NPos[2]) # Winkel
            if Ang < 0.0: Ang = 360.0 + Ang # Negativ
            # if Remarks == True: FileOut.write(str("; Radius Zentrum X={:7.3f}".format(NPos[0])) + str(" Y={:7.3f}".format(NPos[1])) + str(" Radius={:7.3f}".format(Radius)) + str(" Winkel={:7.3f}\n".format(Ang)))
            # Dreieck Berechnung, Kreismitte ist Alpha, Kreisende ist Beta, Gamma = 90°, Arcsin is der Zentrumswinkel des Startpunktes
            TriaA = CurPos[0] - NPos[0]
            TriaC = Radius
            Alpha = math.asin(TriaA / TriaC) * 180.0 / math.pi
            if CurPos[1] - NPos[1] < 0.0: Alpha = 180.0 - Alpha # Y gespiegelt
            AngE = Alpha
            if Ang >= 180.0: Steps = 2       # wir benötigen einen Zwischenschritt da max. 180° aufs Mal möglich sind
            else: Steps = 1
            nr = Radius
            if MirrorX == MirrorY: nr = -nr
            for stp in range(1, Steps + 1):
                AngE -= Ang / float(Steps) # Winkel gegen Uhrzeigersinn
                EX = (math.sin(AngE * math.pi / 180.0) * Radius) + NPos[0]
                EY = (math.cos(AngE * math.pi / 180.0) * Radius) + NPos[1]
                NewPos = (EX, EY)
                StoLn(NewPos, Radius, PenDown)
                CurPos = NewPos
            PenDown = False

        elif Cmd[:3] == "PA ": # Pen Absolute
            NewPos = GetCmdVal(Cmd)
            # if Remarks == True: FileOut.write(str("; Linie von X={:7.3f}".format(CurPos[0])) + str(" Y={:7.3f}".format(CurPos[1])) + str("Nach X={:7.3f}".format(NewPos[0])) + str(" Y={:7.3f}\n".format(NewPos[1])) )
            StoLn(NewPos, 0.0, PenDown)
            CurPos = NewPos


def Distance(p1, p2): #Distanz zwischen 2 X,Y Punkten (Pythagoras)
    # sqrt( (P1x-P2x)^2 + (P1y-P2y)^2 )
    return((((p1[0] - p2[0]) ** 2) + ((p1[1] - p2[1]) ** 2)) ** 0.5)

def SwapLine(Line):  # Vertausche Anfang und Ende der Linie bzw. des Radius
    return(Line[1], Line[0], -Line[2]) # Anfang und Ende vertauschen und Vorzeichen vom Radius ändern


# https://docs.python.org/3/tutorial/datastructures.html#
# Linien Sortieren, Optimieren und aneinanderhängen
#

# Wir setzen alle Linienanfänge auf den minimalen X Linienanfang. Falls das Ende tiefer liegt, vertauschen wir Anfang und Ende
LineStackNew = [] # Die neue optimierte Liste
# Ordne Linien Anfang und Ende nach minimalem X und vertausche Anfang und Ende wenn nötig
for LineNext in LineStack:
    if LineNext[0][0] > LineNext[1][0]: LineNext = SwapLine(LineNext)
    LineStackNew.append(LineNext)

LineStack = LineStackNew

# Nun beginnen wir mit der ersten Linie und suchen den Anschluss am Anfang oder Ende bis kein Anschluss mehr vorhanden ist.
# Danach beginnen wir eine neue Linie bis alle Linien bearbeitet sind.

# Sortieren nach X aufsteigend
LineStack.sort()

LineStackNew = [] # Die neue optimierte Liste
NoConn = True   # Neue Linie

while len(LineStack) > 0:
    if NoConn == True:  # ist noch leer, nimm das erste Element
        LineNext = LineStack[0]
        LineStackNew.append(LineNext) # An die neue Liste anhängen
        LineStack.remove(LineNext) # von der Alten entfernen
        NoConn = False
    Startover = True
    while Startover == True: # Nach dem Entfernen einer Linie beginnen wir von Vorne da eine passende Linie davor liegen könnte
        Startover = False
        for LineNext in LineStack: # Prüfe ob es im Stack einen Anschluss an LineStackNew gibt
            LineFirst = LineStackNew[0]
            LineLast = LineStackNew[len(LineStackNew) -1]
            if Distance(LineLast[1], LineNext[0]) < DistTol: #Ende Letzte Linie zu Anfang Nächste Linie < DistTol
                LineStack.remove(LineNext) # von der Alten entfernen
                LineStackNew.append(LineNext) # Am Ende der Liste anhängen
                Startover = True
                break
            if Distance(LineLast[1], LineNext[1]) < DistTol: #Ende Letzte Linie zu Ende Nächste Linie < DistTol
                LineStack.remove(LineNext) # von der Alten entfernen
                LineStackNew.append(SwapLine(LineNext)) # Anfang und Ende vertauschen und am Ende der Liste anhängen
                Startover = True
                break
            if Distance(LineFirst[0], LineNext[1]) < DistTol: #Anfang Erste Linie zu Ende Nächste Linie < DistTol
                LineStack.remove(LineNext) # von der Alten entfernen
                LineStackNew.insert(0, LineNext) # Am Anfang der Liste anhängen
                Startover = True
                break
            if Distance(LineFirst[0], LineNext[0]) < DistTol: #Anfang Erste Linie zu Anfang Nächste Linie < DistTol
                LineStack.remove(LineNext) # von der Alten entfernen
                LineStackNew.insert(0, (SwapLine(LineNext))) # Anfang und Ende vertauschen und am Anfang der Liste anhängen
                Startover = True
                break

    NoConn = True   # Neue Linie


# Nun erzeugen wir die CNC Datei

CurPos = [0.0, 0.0]

for Line in LineStackNew:
    FromPos = Line[0]
    ToPos = Line[1]
    Radius = Line[2]
    if Distance(CurPos, FromPos) > DistTol:
        NewPos = FromPos
        WriteCNC(NewPos, 0.0, False)

    NewPos = ToPos
    WriteCNC(NewPos, -Radius, True)

FileOut.write("Z P\n")

FileIn.close()
if OutputFile != False:
    FileOut.close()

FileOut = sys.stdout