import DeviceManagement.Syringe_Movement_Coordinator as syr_mover 
from xbox360controller import controller
import XboxController_interface as xbox 
from time import sleep
from os.path import expanduser
from DeviceManagement.PositionSupport import *
from PBMSupport import *
import threading
from RunDBRecorder import *

#ToDo
#fix issue going from Painting mode to Swap
#
home = expanduser('~')
savedir = home + '/SIEData/'
axisDelay = False

class IOOrchestrator:
    def __init__(self,dbname='test.db'):
        #self.dbpath = savedir+dbname
        self.dbname = dbname
        self.dbrecord = RunDBRecorder(dbname)
        self.xbox = xbox.Xbox_Interface()
        self.delay = 0.01
        self.servoMultiplier = 6
        #we use this flag to prevent sending excessive jog
        #cancel commands when the joystick goes to neutral
        self.cncJoystickNeutral = True
        self.current_pos = SystemPosition()
        self.cnc_joystick_pos = CNCPosition()
        #self.current_pos = SystemPosition(0,0,0,0,0,0,0,0)
        #self.cnc_joystick_pos = CNCPosition(0,0,0,0)
        self.current_table_pos = TableMotorPosition(0,0,0)
        self.mc = syr_mover.Syringe_Movement_Coordinator(
            'cnc',
            'rs',
            #'ra',08/03/21
            #'tab',ToBeAdded for tableMotors
            'syr',
            isEmulating=False,isPainting=False)
        self.xbox.button_press_event += self.HandleButtonPress
        self.xbox.button_release_event += self.HandleButtonRelease
        self.xbox.axis_moved_event += self.HandleAxisMove
        if self.mc.cnc_ma:
            self.mc.cnc_ma.sent_command_event += self.RecordCNCCommand
        #if self.mc.ra_ma: 08/03/21
        #    self.mc.ra_ma.sent_command_event += self.RecordRACommand
        if self.mc.syr_ma:
            self.mc.syr_ma.sent_command_event += self.RecordSYRCommand
        self.ActionsDict = {}    

    def FetchRuns(self):
        print('FetchRuns()')
        runs = self.dbrecord.FetchRuns()
        #at some point we'll be more sophisticated than CLI?

    def ReRunRun(self, runId):
        print('ReRunRun()')
        self.mc.HandleCommand('Paint')
        rrcomms = self.dbrecord.FetchRunData(runId=runId)
        st = time.perf_counter()
        i=0
        while i < len(rrcomms):
            t = time.perf_counter()
            et = t - st
            row = rrcomms[i]
            if row[0] < et:
                print('et: ', str(et))
                print('row[0]: ', str(row[0]))
                if row[1]:
                    print('row[1]: ', str(row[1]))
                    self.mc.cnc_ma.SendCommand(row[1])
                if row[2]:
                    print('row[2]: ', str(row[2]))
                    #ToBeImplemented: change this to
                    #TableMotors controller
                    #self.mc.ra_ma.SendCommand(row[2])
                if row[3]:
                    print('row[3]: ', str(row[3]))
                    self.mc.syr_ma.SendCommand(row[3])
                i += 1
        self.mc.isPainting = False

    def EnterPaintingMode(self):
        print('EnterPaintingMode()')      
        self.mc.HandleCommand('Paint')  

    def StartRecordingXbox(self):
        print('RecordXbox()')      
        self.dbrecord.StartRun()

    def StopRecordingXbox(self):
        if self.dbrecord.isRecording:
            self.dbrecord.StopRun()
        else:
            print('already stopped recording, geeeeze')

    def RecordCNCCommand(self, com):
        if self.dbrecord.isRecording:
            self.dbrecord.AddCommandData(cnc=com)

    def RecordRACommand(self, com):
        if self.dbrecord.isRecording:
            self.dbrecord.AddCommandData(ra=com)

    def RecordSYRCommand(self, com):
        if self.dbrecord.isRecording:
            self.dbrecord.AddCommandData(syr=com)

    #we want to filter the controller input to avoid overloading
    #the MarlinCNC and RoboArm MCUs
    def AxisDelay(self):
        global axisDelay
        axisDelay = True
        sleep(self.delay)
        axisDelay = False

    def StartDelay(self):
        th = threading.Thread(target=self.AxisDelay)
        th.start()

    def HandleAxisMove(self, axis):
        global axisDelay
        #block axis input when not painting
        if self.mc.isPainting:
            if axisDelay:
                #print('Waiting on AxisDelay')
                pass
            else:
                #CNC AXIS
                if(axis.name == 'axis_r'):
                    if(abs(axis.x) > 0.1):
                        self.cnc_joystick_pos.X = MakeDec(axis.x)   
                        print('cnc_joystick_pos.X: '+str(self.cnc_joystick_pos.X))   
                        self.cncJoystickNeutral = False       
                    else:
                        self.cnc_joystick_pos.X = 0
                    if(abs(axis.y) > 0.1):
                        self.cnc_joystick_pos.Y = MakeDec(axis.y)
                        print('cnc_joystick_pos.Y: '+str(self.cnc_joystick_pos.Y))
                        self.cncJoystickNeutral = False
                    else:
                        self.cnc_joystick_pos.Y = 0
                    #stop jogging and reset flag when neutral state detected
                    if(abs(axis.x) <0.1 and abs(axis.y) <0.1 
                    and not self.cncJoystickNeutral):
                        print('CNC joystick returned to neutral')
                        self.cncJoystickNeutral = True
                        self.mc.cnc_ma.CancelJog()
                    self.current_pos.CNC = self.cnc_joystick_pos
                    self.mc.RelativeCNCPosition(self.cnc_joystick_pos)
                    #reset positions after sending to avoid  
                    #changing the position with other axis input
                    self.cnc_joystick_pos.X = 0
                    self.cnc_joystick_pos.Y = 0
                    print('axis.x: ' + str(axis.x))
                    print('axis.y: ' + str(axis.y))
                    print('cncJoystickNeutral: ' + str(self.cncJoystickNeutral))
                    self.StartDelay()
                #SERVO AXIS
                #ToBeImplemented for TableMotors 08/03/21
                '''
                if(axis.name == 'axis_l'):
                    if(axis.x > 0.1):
                        self.current_table_pos.M4 = 1*self.servoMultiplier
                    elif(axis.x < -0.1):
                        self.current_table_pos.M4 = -1*self.servoMultiplier
                    else:
                        self.current_table_pos.M4 = 0
                    if(axis.y > 0.1):
                        self.current_table_pos.M5 = 1*self.servoMultiplier
                    elif(axis.y < -0.1):
                        self.current_table_pos.M5 = -1*self.servoMultiplier
                    else:
                        self.current_table_pos.M5 = 0
                    self.current_pos.Servo = self.current_table_pos
                    self.mc.RelativeServoPosition(self.current_table_pos)
                    #reset positions after sending to avoid  
                    #changing the position with other axis input
                    self.current_table_pos.X = 0
                    self.current_table_pos.Y = 0
                    self.StartDelay()
                    '''
        
    def HandleButtonPress(self, button):
        #was doing it this way to pass params but 
        #no longer necessary to do that with 1 pump?
        msg = None
        if(button.name == 'button_trigger_r'):
            print('Swapping to next color')
            self.mc.SwapHigherSyringe()
        elif(button.name == 'button_trigger_l'):
            print('Swapping to previous color Not Implemented!')
            #self.mc.SwapLowerSyringe()
        elif(button.name == 'button_b'):
            #ToDo: make this swapToPrevSyringe
            self.mc.UnloadSyringe()
        elif(button.name == 'button_thumb_l'):
            #Go to center of substrate
            self.mc.HandleCommand('PaintingCenter')
        elif(button.name == 'button_thumb_r'):
            #Go to corner closest to CNC home
            self.mc.HandleCommand('PaintingHome')
        elif(button.name == 'button_start'):
            #msg = 'Paint'
            self.mc.BeginPainting()
        elif self.mc.isPainting:
            if(button.name == 'button_a'):
                #Start dispensing the loaded syringe
                #ToDo: remove multipump functionality
                msg = 'Run,0'
            elif(button.name == 'button_x'):
                #Begin recording commands to DB
                self.StartRecordingXbox()            
            elif(button.name == 'button_y'):
                #Stop recording, reset positions + controllers
                self.StopRecordingXbox()
                self.mc.cnc_ma.ser.readlines()
                self.mc.cnc_ma.ser.flushInput()
                self.mc.HandleCommand('PaintingHome')
                self.mc.isPainting = False                
            
            
        if msg:
            print('msg: %s' % msg)
            self.mc.HandleCommand(msg)

    def HandleButtonRelease(self, button):
        msg = None
        if(button.name == 'button_a'):
            #Stop dispensing with the active syringe
            msg = 'Stop,0'    
            #self.mc.syr_ma.Stop()    
        if msg:
            print('msg: %s' % msg)
            self.mc.HandleCommand(msg)               

    def HandleTerminalInput(self, var):
        if(var in self.ActionsDict.keys()):
            self.ActionsDict[var]()
        else:
            print('unrecognized action')

if __name__ == '__main__':  
    orc = IOOrchestrator()
    #orc.StartRecordingXbox()
    while True:
        orc.FetchRuns()
        var = input('Please enter a command: ')
        print('Entered: ' + var)
        orc.ReRunRun(var)
'''
     ;)
        while True:
            x = self.xbox.get_pos().X
            y = self.xbox.get_pos().Y
            if(not self.mc.isBusy):
                if(abs(x) > 0.1 or abs(y) > 0.1):
                    print('current_pos X: ' + str(x))
                    print('current_pos Y: ' + str(y))
                    self.mc.RelativePosition(self.xbox.current_pos)
            if(self.xbox.get_msg()):
                self.mc.HandleCommand(self.xbox.msg)'''
import DeviceManagement.Syringe_Movement_Coordinator as syr_mover 
from xbox360controller import controller
import XboxController_interface as xbox 
from time import sleep
from os.path import expanduser
from DeviceManagement.PositionSupport import *
from PBMSupport import *
import threading
from RunDBRecorder import *

#ToDo
#fix issue going from Painting mode to Swap
#
home = expanduser('~')
savedir = home + '/SIEData/'
axisDelay = False

class IOOrchestrator:
    def __init__(self,dbname='test.db'):
        #self.dbpath = savedir+dbname
        self.dbname = dbname
        self.dbrecord = RunDBRecorder(dbname)
        self.xbox = xbox.Xbox_Interface()
        self.delay = 0.01
        self.servoMultiplier = 6
        #we use this flag to prevent sending excessive jog
        #cancel commands when the joystick goes to neutral
        self.cncJoystickNeutral = True
        self.current_pos = SystemPosition()
        self.cnc_joystick_pos = CNCPosition()
        #self.current_pos = SystemPosition(0,0,0,0,0,0,0,0)
        #self.cnc_joystick_pos = CNCPosition(0,0,0,0)
        self.current_table_pos = TableMotorPosition(0,0,0)
        self.mc = syr_mover.Syringe_Movement_Coordinator(
            'cnc',
            'rs',
            #'ra',08/03/21
            #'tab',ToBeAdded for tableMotors
            'syr',
            isEmulating=False,isPainting=False)
        self.xbox.button_press_event += self.HandleButtonPress
        self.xbox.button_release_event += self.HandleButtonRelease
        self.xbox.axis_moved_event += self.HandleAxisMove
        if self.mc.cnc_ma:
            self.mc.cnc_ma.sent_command_event += self.RecordCNCCommand
        #if self.mc.ra_ma: 08/03/21
        #    self.mc.ra_ma.sent_command_event += self.RecordRACommand
        if self.mc.syr_ma:
            self.mc.syr_ma.sent_command_event += self.RecordSYRCommand
        self.ActionsDict = {}    

    def FetchRuns(self):
        print('FetchRuns()')
        runs = self.dbrecord.FetchRuns()
        #at some point we'll be more sophisticated than CLI?

    def ReRunRun(self, runId):
        print('ReRunRun()')
        self.mc.HandleCommand('Paint')
        rrcomms = self.dbrecord.FetchRunData(runId=runId)
        st = time.perf_counter()
        i=0
        while i < len(rrcomms):
            t = time.perf_counter()
            et = t - st
            row = rrcomms[i]
            if row[0] < et:
                print('et: ', str(et))
                print('row[0]: ', str(row[0]))
                if row[1]:
                    print('row[1]: ', str(row[1]))
                    self.mc.cnc_ma.SendCommand(row[1])
                if row[2]:
                    print('row[2]: ', str(row[2]))
                    #ToBeImplemented: change this to
                    #TableMotors controller
                    #self.mc.ra_ma.SendCommand(row[2])
                if row[3]:
                    print('row[3]: ', str(row[3]))
                    self.mc.syr_ma.SendCommand(row[3])
                i += 1
        self.mc.isPainting = False

    def EnterPaintingMode(self):
        print('EnterPaintingMode()')      
        self.mc.HandleCommand('Paint')  

    def StartRecordingXbox(self):
        print('RecordXbox()')      
        self.dbrecord.StartRun()

    def StopRecordingXbox(self):
        if self.dbrecord.isRecording:
            self.dbrecord.StopRun()
        else:
            print('already stopped recording, geeeeze')

    def RecordCNCCommand(self, com):
        if self.dbrecord.isRecording:
            self.dbrecord.AddCommandData(cnc=com)

    def RecordRACommand(self, com):
        if self.dbrecord.isRecording:
            self.dbrecord.AddCommandData(ra=com)

    def RecordSYRCommand(self, com):
        if self.dbrecord.isRecording:
            self.dbrecord.AddCommandData(syr=com)

    #we want to filter the controller input to avoid overloading
    #the MarlinCNC and RoboArm MCUs
    def AxisDelay(self):
        global axisDelay
        axisDelay = True
        sleep(self.delay)
        axisDelay = False

    def StartDelay(self):
        th = threading.Thread(target=self.AxisDelay)
        th.start()

    def HandleAxisMove(self, axis):
        global axisDelay
        #block axis input when not painting
        if self.mc.isPainting:
            if axisDelay:
                #print('Waiting on AxisDelay')
                pass
            else:
                #CNC AXIS
                if(axis.name == 'axis_r'):
                    if(abs(axis.x) > 0.1):
                        self.cnc_joystick_pos.X = MakeDec(axis.x)   
                        print('cnc_joystick_pos.X: '+str(self.cnc_joystick_pos.X))   
                        self.cncJoystickNeutral = False       
                    else:
                        self.cnc_joystick_pos.X = 0
                    if(abs(axis.y) > 0.1):
                        self.cnc_joystick_pos.Y = MakeDec(axis.y)
                        print('cnc_joystick_pos.Y: '+str(self.cnc_joystick_pos.Y))
                        self.cncJoystickNeutral = False
                    else:
                        self.cnc_joystick_pos.Y = 0
                    #stop jogging and reset flag when neutral state detected
                    if(abs(axis.x) <0.1 and abs(axis.y) <0.1 
                    and not self.cncJoystickNeutral):
                        print('CNC joystick returned to neutral')
                        self.cncJoystickNeutral = True
                        self.mc.cnc_ma.CancelJog()
                    self.current_pos.CNC = self.cnc_joystick_pos
                    self.mc.RelativeCNCPosition(self.cnc_joystick_pos)
                    #reset positions after sending to avoid  
                    #changing the position with other axis input
                    self.cnc_joystick_pos.X = 0
                    self.cnc_joystick_pos.Y = 0
                    print('axis.x: ' + str(axis.x))
                    print('axis.y: ' + str(axis.y))
                    print('cncJoystickNeutral: ' + str(self.cncJoystickNeutral))
                    self.StartDelay()
                #SERVO AXIS
                #ToBeImplemented for TableMotors 08/03/21
                '''
                if(axis.name == 'axis_l'):
                    if(axis.x > 0.1):
                        self.current_table_pos.M4 = 1*self.servoMultiplier
                    elif(axis.x < -0.1):
                        self.current_table_pos.M4 = -1*self.servoMultiplier
                    else:
                        self.current_table_pos.M4 = 0
                    if(axis.y > 0.1):
                        self.current_table_pos.M5 = 1*self.servoMultiplier
                    elif(axis.y < -0.1):
                        self.current_table_pos.M5 = -1*self.servoMultiplier
                    else:
                        self.current_table_pos.M5 = 0
                    self.current_pos.Servo = self.current_table_pos
                    self.mc.RelativeServoPosition(self.current_table_pos)
                    #reset positions after sending to avoid  
                    #changing the position with other axis input
                    self.current_table_pos.X = 0
                    self.current_table_pos.Y = 0
                    self.StartDelay()
                    '''
        
    def HandleButtonPress(self, button):
        #was doing it this way to pass params but 
        #no longer necessary to do that with 1 pump?
        msg = None
        if(button.name == 'button_trigger_r'):
            print('Swapping to next color')
            self.mc.SwapHigherSyringe()
        elif(button.name == 'button_trigger_l'):
            print('Swapping to previous color Not Implemented!')
            #self.mc.SwapLowerSyringe()
        elif(button.name == 'button_b'):
            #ToDo: make this swapToPrevSyringe
            self.mc.UnloadSyringe()
        elif(button.name == 'button_thumb_l'):
            #Go to center of substrate
            self.mc.HandleCommand('PaintingCenter')
        elif(button.name == 'button_thumb_r'):
            #Go to corner closest to CNC home
            self.mc.HandleCommand('PaintingHome')
        elif(button.name == 'button_start'):
            #msg = 'Paint'
            self.mc.BeginPainting()
        elif self.mc.isPainting:
            if(button.name == 'button_a'):
                #Start dispensing the loaded syringe
                #ToDo: remove multipump functionality
                msg = 'Run,0'
            elif(button.name == 'button_x'):
                #Begin recording commands to DB
                self.StartRecordingXbox()            
            elif(button.name == 'button_y'):
                #Stop recording, reset positions + controllers
                self.StopRecordingXbox()
                self.mc.cnc_ma.ser.readlines()
                self.mc.cnc_ma.ser.flushInput()
                self.mc.HandleCommand('PaintingHome')
                self.mc.isPainting = False                
            
            
        if msg:
            print('msg: %s' % msg)
            self.mc.HandleCommand(msg)

    def HandleButtonRelease(self, button):
        msg = None
        if(button.name == 'button_a'):
            #Stop dispensing with the active syringe
            msg = 'Stop,0'    
            #self.mc.syr_ma.Stop()    
        if msg:
            print('msg: %s' % msg)
            self.mc.HandleCommand(msg)               

    def HandleTerminalInput(self, var):
        if(var in self.ActionsDict.keys()):
            self.ActionsDict[var]()
        else:
            print('unrecognized action')

if __name__ == '__main__':  
    orc = IOOrchestrator()
    #orc.StartRecordingXbox()
    while True:
        orc.FetchRuns()
        var = input('Please enter a command: ')
        print('Entered: ' + var)
        orc.ReRunRun(var)
'''
     ;)
        while True:
            x = self.xbox.get_pos().X
            y = self.xbox.get_pos().Y
            if(not self.mc.isBusy):
                if(abs(x) > 0.1 or abs(y) > 0.1):
                    print('current_pos X: ' + str(x))
                    print('current_pos Y: ' + str(y))
                    self.mc.RelativePosition(self.xbox.current_pos)
            if(self.xbox.get_msg()):
                self.mc.HandleCommand(self.xbox.msg)'''
#ToDo #fix issue going from Painting mode to Swap # #self.dbpath = savedir+dbname #we use this flag to prevent sending excessive jog #cancel commands when the joystick goes to neutral #self.current_pos = SystemPosition(0,0,0,0,0,0,0,0) #self.cnc_joystick_pos = CNCPosition(0,0,0,0) #'ra',08/03/21 #'tab',ToBeAdded for tableMotors #if self.mc.ra_ma: 08/03/21 #    self.mc.ra_ma.sent_command_event += self.RecordRACommand #at some point we'll be more sophisticated than CLI? #ToBeImplemented: change this to #TableMotors controller #self.mc.ra_ma.SendCommand(row[2]) #we want to filter the controller input to avoid overloading #the MarlinCNC and RoboArm MCUs #block axis input when not painting #print('Waiting on AxisDelay') #CNC AXIS #stop jogging and reset flag when neutral state detected #reset positions after sending to avoid #changing the position with other axis input #SERVO AXIS #ToBeImplemented for TableMotors 08/03/21 if(axis.name == 'axis_l'):                     if(axis.x > 0.1):                         self.current_table_pos.M4 = 1*self.servoMultiplier                     elif(axis.x < -0.1):                         self.current_table_pos.M4 = -1*self.servoMultiplier                     else:                         self.current_table_pos.M4 = 0                     if(axis.y > 0.1):                         self.current_table_pos.M5 = 1*self.servoMultiplier                     elif(axis.y < -0.1):                         self.current_table_pos.M5 = -1*self.servoMultiplier                     else:                         self.current_table_pos.M5 = 0                     self.current_pos.Servo = self.current_table_pos                     self.mc.RelativeServoPosition(self.current_table_pos)                     #reset positions after sending to avoid                       #changing the position with other axis input                     self.current_table_pos.X = 0                     self.current_table_pos.Y = 0                     self.StartDelay() #was doing it this way to pass params but #no longer necessary to do that with 1 pump? #self.mc.SwapLowerSyringe() #ToDo: make this swapToPrevSyringe #Go to center of substrate #Go to corner closest to CNC home #msg = 'Paint' #Start dispensing the loaded syringe #ToDo: remove multipump functionality #Begin recording commands to DB #Stop recording, reset positions + controllers #Stop dispensing with the active syringe #self.mc.syr_ma.Stop() #orc.StartRecordingXbox() ;)         while True:             x = self.xbox.get_pos().X             y = self.xbox.get_pos().Y             if(not self.mc.isBusy):                 if(abs(x) > 0.1 or abs(y) > 0.1):                     print('current_pos X: ' + str(x))                     print('current_pos Y: ' + str(y))                     self.mc.RelativePosition(self.xbox.current_pos)             if(self.xbox.get_msg()):                 self.mc.HandleCommand(self.xbox.msg)