#!/usr/bin/env python
# Copyright (c) 2017 Michel Trahan
#------------------------------------------------------------------------------------
#------------------------------------------------------------------------------------
#------------------------------------------------------------------------------------
#   MOVES                          needs access to the xmlBiesse and linuxcnc objects
#------------------------------------------------------------------------------------
#------------------------------------------------------------------------------------
#------------------------------------------------------------------------------------
# 
#   This section interacts with G-Code using Python
#  
#------------------------------------------------------------------------------------
import time

class Moves(object): 

    def __init__(self, x, c):    # ou x, i
        self.x = x
        self.cnc = c
        self.command = self.cnc.command()            # self.i = i        self.i.execute()
        self.stat = self.cnc.stat()
        self.feedSpeed = self.x.getFeedSpeed()
        
    #----------------------------------------------------------
    #----------------------------------------------------------
    def ok_for_mdi(self):
        self.stat.poll()
        return not self.stat.estop and self.stat.enabled and self.stat.homed and (self.stat.interp_state == self.cnc.INTERP_IDLE)

    def set_mdi(self):
        self.command.mode(self.cnc.MODE_MDI)
        self.command.wait_complete() # wait until mode switch executed
        print("DONE Waiting for completion of setting the mode to MDI")
    
    #----------------------------------------------------------
    #----------------------------------------------------------
    def MoveToFrontOfPocket(self, id):
        # fetch the front position of said pocket in the xml file
        # go to that position in machine coordinates
        posX, posY, posZ = self.x.getPocketPos(id, "Front")
        #execute("G1 G53 F%d X%f Y%f Z0.0" % (self.feedSpeed, posX, posY), lineno())
        
        # this code BOB and comment out all of the if ok for mdi ...
        #execute("G1 G53 F%d Z%f" % (self.feedSpeed, posZ), lineno())
        
        #self.command.mdi("G1 G53 F%d X%f Y%f Z0.0" % (self.feedSpeed, posX, posY))     #,lineno())
        #self.command.mdi("G1 G53 F%d Z%f" % (self.feedSpeed, posZ))                    #,lineno())

        if self.ok_for_mdi():
            print("ok for mdi")
            self.set_mdi()
            print("Completed, send the MDI command  F%d X%f Y%f" % (self.feedSpeed, posX, posY))
            #self.command.mdi("G1 G53 F%d X%f Y%f Z0.0" % (self.feedSpeed, posX, posY))  
#            self.command.mdi("G1 F%d X%f Y%f" % (self.feedSpeed, posX, posY))  
#            time.sleep(self.x.getDelay("moveToFrontDelay"))
            self.command.mdi("G0 G53 F%d X%f Y%f" % (self.feedSpeed, posX, posY))
            self.command.wait_complete()            
 
        print("after the if ok for mdi")
        
    #----------------------------------------------------------
    #----------------------------------------------------------
    def MoveToTopOfPocket(self, id):
        # fetch the top position of said pocket in the xml file
        # go to that position in machine coordinates
        posX, posY, posZ = self.x.getPocketPos(id, "Top")
        #self.execute("G1 G53 F%d X%f Y%f Z0.0" % (self.feedSpeed, posX, posY),lineno())
        #self.execute("G1 G53 F%d Z%f" % (self.feedSpeed, posZ),lineno())
        #self.command.mdi("G1 G53 F%d X%f Y%f Z0.0" % (self.feedSpeed, posX, posY))     #,lineno())
        #self.command.mdi("G1 G53 F%d Z%f" % (self.feedSpeed, posZ))                    #,lineno())

        self.command.mdi("G0 G53 F%d X%f Y%f Z0.0" % (self.feedSpeed, posX, posY))     #,lineno())
        self.command.wait_complete()            
        #time.sleep(self.x.getDelay("moveToTopDelay"))
        
        self.command.mdi("G1 G53 F%d Z%f" % (self.feedSpeed, posZ))                    #,lineno())
        self.command.wait_complete()            
        #time.sleep(self.x.getDelay("moveToTopDelay"))
        
        
    #----------------------------------------------------------
    #----------------------------------------------------------
    def MoveIntoOrOutOfPocket(self, id, direction): # either "INTO" or "OUT"
        # this requires a length ... found in the XML using getMoveDistance() (NOT)
        # this could be calculated ... or do we simply move the the FRONT pos ...
        # going to the FRONT position is ok to move OUT, but not for move INTO
        # going INTO, simply move to Top Y position, and Front X and Z
        posX, posY, posZ = self.x.getPocketPos(id, "Front")
        if direction == "INTO": # moving into is moving to the Top X position
            posX2, posY2, posZ2 = self.x.getPocketPos(id, "Top")
            #self.execute("G1 G53 F%d X%f " % (self.feedSpeed, posX2),lineno())
            #self.command.mdi("G1 G53 F%d X%f " % (self.feedSpeed, posX2))              #,lineno())
            print("Time to move into pocket, Bob, we could have 2 speeds ...")
            self.command.mdi("G1 G53 F%d Z%f " % (self.feedSpeed, posZ2))              #,lineno())
            #self.command.wait_complete()            
            time.sleep(self.x.getDelay("moveIntoDelay"))
            
            print("Move into X2")
            self.command.mdi("G1 G53 F%d X%f " % (self.feedSpeed, posX2))              #,lineno())
            #self.command.wait_complete()            
            time.sleep(self.x.getDelay("moveIntoDelay") * 2)
           
        else: # moving out is simply going to Front position
            #self.execute("G1 G53 F%d X%f " % (self.feedSpeed, posX),lineno())
            #self.execute("G1 G53 F%d Z0.0 " % self.feedSpeed,lineno())
            #self.command.mdi("G1 G53 F%d X%f " % (self.feedSpeed, posX))               #,lineno())
            #self.command.mdi("G1 G53 F%d Z0.0 " % self.feedSpeed)                      #,lineno())
            print("Time to move out of pocket, Bob, we could have 2 speeds ...")
            self.command.mdi("G1 G53 F%d X%f " % (self.feedSpeed, posX))
            #self.command.wait_complete()            
            time.sleep(self.x.getDelay("moveOutDelay"))
            print("Move to Z0")
            self.command.mdi("G1 G53 Z0.0")
            #self.command.wait_complete()            
            time.sleep(self.x.getDelay("moveOutDelay") / 2)
            print("Out of pocket, we can carry on")               #,lineno())
            #self.command.mdi("G1 G53 F%d Z0.0 " % self.feedSpeed)                      #,lineno())