#!/usr/bin/env python

SoftwareInfo = \
    { 'Function' : '4COIL stepper motor test',
      'Design'   : 'CreativePeach c 2013',
      'Version'  : '1.0',
    }  

from cpphlib import *


class STEPPER4COILS:

    def __init__(self):
        self.stepData = {
            1 : "1",
            2 : "9",
            3 : "8",
            4 : "A",
            5 : "2",
            6 : "6",
            7 : "4",
            8 : "5",            
        }    
        self.delay = PPPASSIVETIMER()
        self.steps = 0
        self.stepposition = 1
        self.dir = "cw"
        self.sequences = Queue.Queue()
        self.actualsequence = {"state":"executed", "steps":200, "dir":"cw", "delay":0.01, "refmode":False}
        self.refmode = False
        
    def movingsequence(self, sequences):
        for seq in sequences:
            if not("state" in seq.keys() and "refmode" in seq.keys()):
                seq["state"] = "wait"
                seq["refmode"] = False
            self.sequences.put_nowait(seq)
        
    def setsteps(self, steps, dir, delay):
        self.steps = steps
        self.dir = dir
        self.delay.set(delay)
    
    def debug(self):
        print self.steps
        print self.refmode
        print
    
    def gotorefpoint(self, mode):
        if mode == "normal":
            seq = {"state":"wait", "steps":200, "dir":"cw", "delay":0.02, "refmode":True}
            self.sequences.put_nowait(seq)
        if mode == "precise":
            seq = {"state":"wait", "steps":200, "dir":"cw", "delay":0.02, "refmode":True}
            self.sequences.put_nowait(seq)
            seq = {"state":"wait", "steps":10, "dir":"ccw", "delay":0.02, "refmode":False}
            self.sequences.put_nowait(seq)
            seq = {"state":"wait", "steps":20, "dir":"cw", "delay":0.1, "refmode":True}
            self.sequences.put_nowait(seq)
    
    def move(self, outstate, refstate):
        if self.actualsequence.get("state") == "executed" and not(self.sequences.empty()):
            self.actualsequence = self.sequences.get_nowait()
            if "sleep" in self.actualsequence.keys():
                time.sleep(self.actualsequence.get("sleep"))
                self.actualsequence["state"] = "executed"
            else:    
                self.actualsequence["state"] = "executenow"
                self.refmode = self.actualsequence.get("refmode")
                self.setsteps(self.actualsequence.get("steps"), self.actualsequence.get("dir"), self.actualsequence.get("delay"))            
            
        if self.steps == 0:
            self.actualsequence["state"] = "executed"   
            
        if refstate and self.refmode:
           self.steps = 0
           self.refmode = False

        if self.delay.isexpired():    
            if self.steps > 0:
                if self.dir == "ccw":
                    self.stepposition +=1
                    if self.stepposition > 8:
                        self.stepposition = 1
                if self.dir == "cw":
                    self.stepposition -=1
                    if self.stepposition < 1:
                        self.stepposition = 8                               
                self.steps -= 1   
                outstate = self.stepData.get(self.stepposition) + outstate[1:]  
            self.delay.reset()
        return outstate
               
                   
class STEPPERTEST:
    
    def __init__(self, CAEIF, IOIF):
        self.CAEIF = CAEIF
        self.IOIF = IOIF
        self.IOCG = PPIOCARDGADGETS(self.CAEIF, self.CAEIF)
        self.BM = PPBITMANIPULATOR()   
        self.MOTOR = STEPPER4COILS()      
        self.DGOAddr = -1
        self.LCDAddr = -1
        self.DGIAddr = -1
  
    def runtest(self):
        """Identify used cards address by discover dict"""
        for addr in range(16):
            if self.IOCG.DiscoverDict.get(addr) == "16 channel DIGITAL INPUT":
                self.DGIAddr = addr
            if self.IOCG.DiscoverDict.get(addr) == "16 channel DIGITAL OUTPUT":
                self.DGOAddr = addr
        if self.DGOAddr == -1 or self.DGIAddr == -1:
            print "Identify unsuccessfull!"
            return        
        """switch off pollings by discover dict"""        
        for addr in range(16):
            if self.IOCG.DiscoverDict.get(addr) != "EMPTY":
                if self.IOCG.CardIOTypes.get(self.IOCG.DiscoverDict.get(addr)) == "INP":
                    self.CAEIF.commandstatemachine("POLLOFF:%X\r\n"%addr,"CIA: POLLOFF OK at address %X\r\n>\r\n"%addr,500)                      
        """Switch On pollings on used cards"""
        self.CAEIF.commandstatemachine("POLLON:%X\r\n"%self.DGIAddr,"CIA: POLLON OK at address %X\r\n>\r\n"%addr,500)
        print " "
        print "Hardware OK."
        print 
        print "Control handles:"
        print " Program selector on digital input card's BCD0:"
        print " - 0 go to forward 10 steps and backward 5 steps slowly"
        print " - 1 go to refpoint"
        print " - 2 go to refpoint precisely"
        print " - 3 go to 120 degs forward relatively"
        print " - 4 go to refpoint, wait 3 secs, after go to 270 degs forward fastly,"
        print "     wait 2 secs, and go backward 30 steps slowly"
        print " Push BLUE button on input matching unit to program start"
        print " Push RED button on input matching unit to motor induce down"
        print " Push YELLOW button on input matching unit to litle debug that prints"
        print " how many steps back and how the state of refmode"
        print 
        print 
        print "Outputs:"
        print " Programselector position on BLOCK1 LEDS on output card"
        print
        print "Exit from test:"
        print " Push GREEN button on input matching unit"
        print " "
        self.CAEIF.commandstatemachine("ENABLEPOLL\r\n",">",500)
        self.IOIF.send("a%Xb0dFF\r\n"%self.DGOAddr)
        self.IOIF.send("a%Xb1dFF\r\n"%self.DGOAddr) 
        ProgramSelector = -1        
        self.IOIF.send("a%Xb0dFF\r\n"%self.DGOAddr)
        OutState = "00"
        prevOutState = "00"
        selectedsequence = [    
                               {"steps":10, "dir":"cw", "delay":1.0},
                               {"steps":5, "dir":"ccw", "delay":1.0},
                           ]    
        Refpoint = False
        waitbuttonrelease = False   
        while(1):   
            if self.IOIF.getinputflag():
                data = self.IOIF.getinputdata()
                if "a%Xb1"%self.DGIAddr in data:
                    
                    if data[5:7] == "00":
                        selectedsequence = [    
                                               {"steps":10, "dir":"cw", "delay":1.0},
                                               {"steps":5, "dir":"ccw", "delay":1.0},    
                                           ]
                    if data[5:7] == "01":
                        selectedsequence = [    
                                               {"state":"wait", "steps":200, "dir":"cw", "delay":0.02, "refmode":True}   
                                           ]                           
                    if data[5:7] == "02":
                        selectedsequence = [    
                                               {"state":"wait", "steps":200, "dir":"cw", "delay":0.02, "refmode":True},
                                               {"state":"wait", "steps":10, "dir":"ccw", "delay":0.02, "refmode":False},
                                               {"state":"wait", "steps":20, "dir":"cw", "delay":0.1, "refmode":True}
                                           ]    
                    if data[5:7] == "03":
                        selectedsequence = [
                                               {"steps":32, "dir":"cw", "delay":0.01}
                                           ]   
                    if data[5:7] == "04":
                        selectedsequence = [    
                                               {"state":"wait", "steps":200, "dir":"cw", "delay":0.02, "refmode":True},
                                               {"state":"wait", "steps":10, "dir":"ccw", "delay":0.02, "refmode":False},
                                               {"state":"wait", "steps":20, "dir":"cw", "delay":0.1, "refmode":True},
                                               {"steps":72, "dir":"cw", "delay":0.01},
                                               {"sleep":2.0},
                                               {"steps":30, "dir":"ccw", "delay":0.5}, 
                                           ]                                        
                    self.IOIF.send("a%Xb1d%s\r\n"%(self.DGOAddr, self.BM.inverse(data[5:7])))                                                
                if "a%Xb0"%self.DGIAddr in data:
                    if  waitbuttonrelease and self.BM.getbitstate(data[5:7],0) == "0":                         
                        self.MOTOR.movingsequence(selectedsequence)
                        waitbuttonrelease = False
                    if self.BM.getbitstate(data[5:7],0) == "1" and not(waitbuttonrelease):
                        waitbuttonrelease = True 
                    if self.BM.getbitstate(data[5:7],1) == "1":    
                        self.IOIF.send("a%Xb0dFF\r\n"%self.DGOAddr) 
                    Refpoint = False
                    if self.BM.getbitstate(data[5:7],7) == "1": 
                        Refpoint = True
                    if self.BM.getbitstate(data[5:7],2) == "1":    
                        self.MOTOR.debug()
                    if self.BM.getbitstate(data[5:7],3) == "1":
                        break                                         
            OutState = self.MOTOR.move(OutState, Refpoint)
            if prevOutState != OutState:
                self.IOIF.send("a%Xb0d%s\r\n"%(self.DGOAddr, self.BM.inverse(OutState)))
                prevOutState = OutState            
        self.IOIF.send("a%Xb0dFF\r\n"%self.DGOAddr)
        self.IOIF.send("a%Xb1dFF\r\n"%self.DGOAddr)                                   
        self.CAEIF.commandstatemachine("DISABLEPOLL\r\n",">",500)
        """Switch Off pollings on used cards"""
        self.CAEIF.commandstatemachine("POLLOFF:%X\r\n"%self.DGIAddr,"CIA: POLLOFF OK at address %X\r\n>\r\n"%addr,500)


if __name__ == '__main__': 
    """Main program"""   
    PPDriver = PPDRIVER();
    PPDriver.registerCAEinterface("localhost", 8000)
    PPDriver.registerIOinterface("localhost", 12000)
    PPDriver.connectinterfaces()
    CAEIF, IOIF, MONIF = PPDriver.getinterfaces()
    ST = STEPPERTEST(CAEIF, IOIF)
    ST.runtest()
    PPDriver.disconnectinterfaces()
    sys.exit(0)
    