import os
_filePath = os.path.dirname(os.path.abspath(__file__))

import FreeCAD
import Path
import Part
import PathSimulator
import math
from FreeCAD import Vector, Base
from PathScripts.PathGeom import PathGeom
if FreeCAD.GuiUp:
    import FreeCADGui
    from PySide import QtGui, QtCore

#compiled with pyrcc4 -py3 Resources\CAM_Sim.qrc -o CAM_Sim_rc.py
       
class CAMSimTaskUi:
    def __init__(self, parent):
        # this will create a Qt widget from our ui file
        self.form = FreeCADGui.PySideUic.loadUi(":/panels/TaskPathSimulator.ui")
        self.parent = parent

    def accept(self):
        self.parent.accept()
        FreeCADGui.Control.closeDialog()

    def reject(self):
        self.parent.cancel()
        FreeCADGui.Control.closeDialog()

#for cmd in obj.Path.Commands:
#	if cmd.Name[0] == 'G':
#		e1 = 
#	print cmd.Name

def TSError(msg):	
    QtGui.QMessageBox.information(None,"Path Simulation",msg)

class PathSimulation:
    def __init__(self):
        self.debug = False
        self.timer = QtCore.QTimer()
        QtCore.QObject.connect(self.timer, QtCore.SIGNAL("timeout()"), self.PerformCut)
        self.stdrot = FreeCAD.Rotation(Vector(0,0,1),0)
        self.iprogress = 0
        self.numCommands = 0;

    def Connect(self, but, sig):
        QtCore.QObject.connect(but, QtCore.SIGNAL("clicked()"), sig)

    def UpdateProgress(self):
        if self.numCommands > 0:
            self.taskForm.form.progressBar.setValue(self.iprogress * 100 / self.numCommands)

    def Activate(self):
        self.taskForm = CAMSimTaskUi(self)
        form = self.taskForm.form
        self.Connect(form.toolButtonStop, self.SimStop)
        self.Connect(form.toolButtonPlay, self.SimPlay)
        self.Connect(form.toolButtonPause, self.SimPause)
        self.Connect(form.toolButtonStep, self.SimStep)
        self.Connect(form.toolButtonFF, self.SimFF)
        form.comboJobs.currentIndexChanged.connect(self.onJobChange)
        jobList = FreeCAD.ActiveDocument.findObjects("Path::FeaturePython", "Job.*")
        form.comboJobs.clear()
        self.jobs = []            
        for j in jobList:
            self.jobs.append(j)
            form.comboJobs.addItem(j.ViewObject.Icon, j.Label)
        FreeCADGui.Control.showDialog(self.taskForm)
        self.disableAnim = False
        self.isVoxel = True
        self.voxSim = PathSimulator.PathSim()
        self.SimulateMill()

    def SetupSimulation(self):
        form = self.taskForm.form       
        self.activeOps = []
        self.numCommands = 0
        for i in range(form.listOperations.count()):
            if form.listOperations.item(i).checkState() == QtCore.Qt.CheckState.Checked:
                self.activeOps.append(self.operations[i])
                self.numCommands += len(self.operations[i].Path.Commands)
        if len(self.activeOps) == 0:
            return 0
        self.stock = self.job.Stock.Shape
        if (self.isVoxel):
            self.voxSim.BeginSimulation(self.stock,0.5)
            self.cutMaterial.Mesh = self.voxSim.GetResultMesh()
        else:
            self.cutMaterial.Shape = self.stock
        self.busy = False
        self.SetupOperation(0)
        self.iprogress = 0
        self.UpdateProgress()

    def SetupOperation(self, itool):
        self.operation = self.activeOps[itool]
        self.tool = self.operation.ToolController.Tool
        toolProf = self.CreateToolProfile(self.tool, Vector(0,1,0), Vector(0,0,0), self.tool.Diameter / 2.0)
        self.cutTool.Shape = Part.makeSolid(toolProf.revolve(Vector(0,0,0), Vector(0,0,1)))
        self.cutTool.ViewObject.show()
        self.voxSim.SetCurrentTool(self.tool)
        self.icmd = 0
        self.curpos = FreeCAD.Placement(self.initialPos, self.stdrot)
        #self.cutTool.Placement = FreeCAD.Placement(self.curpos, self.stdrot)
        self.cutTool.Placement = self.curpos
       

    def SimulateMill(self):
        self.job = self.jobs[self.taskForm.form.comboJobs.currentIndex()]
        self.busy = False
        #self.timer.start(100)
        self.ioperation = 0
        self.height = 10
        self.skipStep = False
        self.initialPos = Vector(0, 0, self.job.Stock.Shape.BoundBox.ZMax)
        # Add cut tool
        self.cutTool = FreeCAD.ActiveDocument.addObject("Part::FeaturePython","CutTool")
        self.cutTool.ViewObject.Proxy = 0
        self.cutTool.ViewObject.hide()

        # Add cut material
        if self.isVoxel:
            self.cutMaterial = FreeCAD.ActiveDocument.addObject("Mesh::FeaturePython","CutMaterial")
        else:
            self.cutMaterial = FreeCAD.ActiveDocument.addObject("Part::FeaturePython","CutMaterial")
            self.cutMaterial.Shape = self.job.Stock.Shape
        self.cutMaterial.ViewObject.Proxy = 0
        self.cutMaterial.ViewObject.show()

        # Add cut path solid for debug
        if self.debug:
            self.cutSolid = FreeCAD.ActiveDocument.addObject("Part::FeaturePython","CutDebug")
            self.cutSolid.ViewObject.Proxy = 0
            self.cutSolid.ViewObject.hide()
        
        self.SetupSimulation()
        self.resetSimulation = False
        FreeCAD.ActiveDocument.recompute()
        
        #self.dialog.show()

    def SkipStep(self):
        self.skipStep = True
        self.PerformCut()


    def PerformCutBoolean(self):
        if self.resetSimulation:
            self.resetSimulation = False
            self.SetupSimulation()

        if self.busy:
            return
        self.busy = True

        cmd = self.operation.Path.Commands[self.icmd]
        #for cmd in job.Path.Commands:
        pathSolid = None
        if cmd.Name in ['G0']:
            self.curpos = self.RapidMove(cmd, self.curpos)
        if cmd.Name in ['G1', 'G2', 'G3']:
            if self.skipStep:
                self.curpos = self.RapidMove(cmd, self.curpos)
            else:
                (pathSolid, self.curpos) = self.GetPathSolid(self.tool, cmd, self.curpos)
        self.skipStep = False
        if pathSolid is not None:
            if self.debug:
                self.cutSolid.Shape = pathSolid
            newStock = self.stock.cut([pathSolid], 1e-3)
            try:
                if newStock.isValid():
                    self.stock = newStock.removeSplitter()
            except:
                if self.debug:
                    print "invalid cut at cmd #" + str(self.icmd)
        if not self.disableAnim:
            self.cutTool.Placement = FreeCAD.Placement(self.curpos, self.stdrot)
        self.icmd += 1
        self.iprogress += 1
        self.UpdateProgress()
        if self.icmd >= len(self.operation.Path.Commands):
            #self.cutMaterial.Shape = self.stock.removeSplitter()
            self.ioperation += 1
            if self.ioperation >= len(self.activeOps):
                self.EndSimulation()
                return
            else:
                self.SetupOperation(self.ioperation)
        if not self.disableAnim:
            self.cutMaterial.Shape = self.stock
        self.busy = False
    
    def PerformCutVoxel(self):
        if self.resetSimulation:
            self.resetSimulation = False
            self.SetupSimulation()

        if self.busy:
            return
        self.busy = True

        cmd = self.operation.Path.Commands[self.icmd]
        #for cmd in job.Path.Commands:
        if cmd.Name in ['G0', 'G1', 'G2', 'G3']:
            self.curpos = self.voxSim.ApplyCommand(self.curpos, cmd)
            if not self.disableAnim:
                self.cutTool.Placement = self.curpos #FreeCAD.Placement(self.curpos, self.stdrot)
                self.cutMaterial.Mesh = self.voxSim.GetResultMesh()
        self.icmd += 1
        self.iprogress += 1
        self.UpdateProgress()
        if self.icmd >= len(self.operation.Path.Commands):
            #self.cutMaterial.Shape = self.stock.removeSplitter()
            self.ioperation += 1
            if self.ioperation >= len(self.activeOps):
                self.EndSimulation()
                return
            else:
                self.SetupOperation(self.ioperation)
        self.busy = False

    def PerformCut(self):
        if (self.isVoxel):
            self.PerformCutVoxel()
        else:
            self.PerformCutBoolean()
        
    def RapidMove(self, cmd, curpos):
        path = PathGeom.edgeForCmd(cmd, curpos) # hack to overcome occ bug
        if path is None:
            return curpos
        return path.valueAt(path.LastParameter)

    def GetPathSolidOld(self, tool, cmd, curpos):
        e1 = PathGeom.edgeForCmd(cmd, curpos)
        #curpos = e1.valueAt(e1.LastParameter)
        n1 = e1.tangentAt(0)
        n1[2] = 0.0
        try:
            n1.normalize()
        except:
            return (None, e1.valueAt(e1.LastParameter))
        height = self.height
        rad = tool.Diameter / 2.0 - 0.001 * curpos[2] # hack to overcome occ bug
        if type(e1.Curve) is Part.Circle and e1.Curve.Radius <= rad: # hack to overcome occ bug
            rad = e1.Curve.Radius - 0.001
            #return (None, e1.valueAt(e1.LastParameter))
        xf = n1[0] * rad
        yf = n1[1] * rad
        xp = curpos[0]
        yp = curpos[1]
        zp = curpos[2]
        v1 = Vector(yf + xp, -xf + yp, zp)
        v2 = Vector(yf + xp, -xf + yp, zp + height)
        v3 = Vector(-yf + xp, xf + yp, zp + height)
        v4 = Vector(-yf + xp, xf + yp, zp)
        vc1 = Vector(xf + xp, yf + yp, zp)
        vc2 = Vector(xf + xp, yf + yp, zp + height)
        l1 = Part.makeLine(v1, v2)
        l2 = Part.makeLine(v2, v3)
        #l2 = Part.Edge(Part.Arc(v2, vc2, v3))
        l3 = Part.makeLine(v3, v4)
        l4 = Part.makeLine(v4, v1)
        #l4 = Part.Edge(Part.Arc(v4, vc1, v1))
        w1 = Part.Wire([l1, l2, l3, l4])
        w2 = Part.Wire(e1)
        try:
            ex1 = w2.makePipeShell([w1],True, True)
        except:
            #Part.show(w1)
            #Part.show(w2)
            return (None, e1.valueAt(e1.LastParameter))
        cyl1 = Part.makeCylinder(rad, height, curpos)
        curpos = e1.valueAt(e1.LastParameter)
        cyl2 = Part.makeCylinder(rad, height, curpos)
        ex1s = Part.Solid(ex1)
        f1 = ex1s.fuse([cyl1,cyl2]).removeSplitter()
        return (f1, curpos)
    
    # get a solid representation of a tool going along path
    def GetPathSolid(self, tool, cmd, pos):
        toolPath = PathGeom.edgeForCmd(cmd, pos)
        #curpos = e1.valueAt(e1.LastParameter)
        startDir = toolPath.tangentAt(0)
        startDir[2] = 0.0
        endPos = toolPath.valueAt(toolPath.LastParameter)
        endDir = toolPath.tangentAt(toolPath.LastParameter)
        try:
            startDir.normalize()
            endDir.normalize()
        except:
            return (None, endPos)
        height = self.height

        # hack to overcome occ bugs
        rad = tool.Diameter / 2.0 - 0.001 * pos[2]
        #rad = rad + 0.001 * self.icmd
        if type(toolPath.Curve) is Part.Circle and toolPath.Curve.Radius <= rad:
            rad = toolPath.Curve.Radius - 0.01 * (pos[2] + 1)
            return (None, endPos)

        # create the path shell
        toolProf = self.CreateToolProfile(tool, startDir, pos, rad)
        rotmat = Base.Matrix()
        rotmat.move(pos.negative())
        rotmat.rotateZ(math.pi)
        rotmat.move(pos)
        mirroredProf = toolProf.transformGeometry(rotmat)
        fullProf = Part.Wire([toolProf, mirroredProf])
        pathWire = Part.Wire(toolPath)
        try:
            pathShell = pathWire.makePipeShell([fullProf],False, True)
        except:
            if self.debug:
                Part.show(pathWire)
                Part.show(fullProf)
            return (None, endPos)

        # create the start cup
        startCup = toolProf.revolve(pos, Vector(0,0,1), -180)

        #create the end cup
        endProf = self.CreateToolProfile(tool, endDir, endPos, rad)
        endCup = endProf.revolve(endPos, Vector(0,0,1), 180)

        fullShell = Part.makeShell(startCup.Faces + pathShell.Faces + endCup.Faces)
        return (Part.makeSolid(fullShell).removeSplitter(), endPos)

    # create radial profile of the tool (90 degrees to the direction of the path)
    def CreateToolProfile(self, tool, dir, pos, rad):
        type = tool.ToolType
        #rad = tool.Diameter / 2.0 - 0.001 * pos[2] # hack to overcome occ bug
        xf = dir[0] * rad
        yf = dir[1] * rad
        xp = pos[0]
        yp = pos[1]
        zp = pos[2]
        h = tool.CuttingEdgeHeight
        # common to all tools
        vTR = Vector(xp + yf, yp - xf, zp + h)
        vTC = Vector(xp, yp, zp + h)
        vBC = Vector(xp, yp, zp)
        lT = Part.makeLine(vTR, vTC)
        res = None
        if type == "ChamferMill":
            ang = 90 - tool.CuttingEdgeAngle / 2.0
            if ang > 80:
                ang = 80
            if ang < 0:
                ang = 0
            h1 = math.tan(ang * math.pi / 180) * rad
            if h1 > (h - 0.1):
                h1 = h - 0.1
            vBR = Vector(xp + yf, yp - xf, zp + h1)
            lR = Part.makeLine(vBR, vTR)
            lB = Part.makeLine(vBC, vBR)
            res = Part.Wire([lB, lR, lT])

        elif type == "BallEndMill":
            h1 = rad
            if h1 >= h:
                h1 = h - 0.1
            vBR = Vector(xp + yf, yp - xf, zp + h1)
            r2 = h1 / 2.0
            h2 = rad - math.sqrt(rad * rad - r2 * r2)
            vBCR = Vector(xp + yf / 2.0, yp - xf / 2.0, zp + h2)
            cB = Part.Edge(Part.Arc(vBC, vBCR, vBR))
            lR = Part.makeLine(vBR, vTR)
            res = Part.Wire([cB, lR, lT])

        else: # default: assume type == "EndMill"
            vBR = Vector(xp + yf, yp - xf, zp)
            lR = Part.makeLine(vBR, vTR)
            lB = Part.makeLine(vBC, vBR)
            res = Part.Wire([lB, lR, lT])

        return res  

    def onJobChange(self):
        form = self.taskForm.form
        j = self.jobs[form.comboJobs.currentIndex()]
        form.listOperations.clear()
        self.operations = []
        for op in j.Operations.OutList:
            listItem = QtGui.QListWidgetItem(op.ViewObject.Icon, op.Label) 
            listItem.setFlags(listItem.flags() | QtCore.Qt.ItemIsUserCheckable)
            listItem.setCheckState(QtCore.Qt.CheckState.Checked)
            self.operations.append(op)
            form.listOperations.addItem(listItem)

    def GuiBusy(self, isBusy):
        form = self.taskForm.form
        #form.toolButtonStop.setEnabled()
        form.toolButtonPlay.setEnabled(not isBusy)
        form.toolButtonPause.setEnabled(isBusy)
        form.toolButtonStep.setEnabled(not isBusy)
        form.toolButtonFF.setEnabled(not isBusy)

    def EndSimulation(self):
        self.UpdateProgress()
        self.timer.stop()
        self.GuiBusy(False)
        self.ViewShape()
        self.resetSimulation = True

    def SimStop(self):
        self.cutTool.ViewObject.hide()
        self.iprogress = 0
        self.EndSimulation()

    def SimFF(self):
        self.GuiBusy(True)
        self.timer.start(10)
        self.disableAnim = True

    def SimStep(self):
        self.disableAnim = False
        self.PerformCut()

    def SimPlay(self):
        self.disableAnim = False
        self.GuiBusy(True)
        self.timer.start(20)

    def ViewShape(self):
        if self.isVoxel:
            self.cutMaterial.Mesh = self.voxSim.GetResultMesh()
        else:
            self.cutMaterial.Shape = self.stock

    def SimPause(self):
        if self.disableAnim:
            self.ViewShape()
        self.GuiBusy(False)
        self.timer.stop()

    def RemoveTool(self):
        if self.cutTool is None:
            return
        FreeCAD.ActiveDocument.removeObject(self.cutTool.Name)
        self.cutTool = None

    def RemoveMaterial(self):
        if self.cutMaterial is None:
            return
        FreeCAD.ActiveDocument.removeObject(self.cutMaterial.Name)
        self.cutMaterial = None
        

    def accept(self):
        self.EndSimulation()
        self.RemoveTool()
        
    def cancel(self):
        self.EndSimulation()
        self.RemoveTool()
        self.RemoveMaterial()
        

class CommandPathSimulate:

    def GetResources(self):
        return {'Pixmap': 'Path-Simulator',
                'MenuText': QtCore.QT_TRANSLATE_NOOP("Path_Simulator", "CAM Simulator"),
                'Accel': "P, M",
                'ToolTip': QtCore.QT_TRANSLATE_NOOP("Path_Simulator", "Simulate Path G-Code on stock")}

    def IsActive(self):
        if FreeCAD.ActiveDocument is not None:
            for o in FreeCAD.ActiveDocument.Objects:
                if o.Name[:3] == "Job":
                        return True
        return False

    def Activated(self):
        pathSimulation.Activate()

pathSimulation = PathSimulation()
if FreeCAD.GuiUp:
    # register the FreeCAD command
    FreeCADGui.addCommand('Path_Simulator',CommandPathSimulate())
    FreeCAD.Console.PrintLog("Loading PathSimulator Gui... done\n")