#!/usr/bin/env python

SoftwareInfo = \
    { 'Function' : 'PP FATCATTRAINER',
      'Design'   : 'CreativePeach c 2013',
      'Version'  : '1.0',
    }  

from cpphlib import *
import getopt
import sys
import cPickle
import random
        
class FATCATTRAINER:

    def __init__(self, CAEIF, IOIF, verbose):
        self.verbose = verbose
        self.DGIAddr = -1
        self.DGOAddr = -1
        self.SCCAddr = -1
        self.CAEIF = CAEIF
        self.IOIF = IOIF
        self.IOCG = PPIOCARDGADGETS(self.CAEIF, self.IOIF)
        self.BM = PPBITMANIPULATOR()
        self.DIOH = PPDIGINOUTHANDLER(self.CAEIF, self.IOIF)
        try:
            self.LightPointPositions = cPickle.load(open('./laserpositions.dat','r'))
        except:    
            self.LightPointPositions = {}
        
        if not(self.hardwarecheck()):
            if self.verbose:
                print "Error: Hardware is not compatible!"
                print 
            return
        if self.verbose:
            print "Hardware is OK"    
        self.run()    
         
    def hardwarecheck(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.IOCG.DiscoverDict.get(addr) == "6 channel RC SERVO CONTROLLER":
                self.SCCAddr = addr    
        if self.DGOAddr == -1 or self.DGIAddr == -1 or self.SCCAddr==-1:            
            return False
        else:
            return True           

    def moveservo(self, Direction, Position):
        if "Azimuth" in Direction:
            self.IOIF.send("a%Xb0d%0.2X\r\n"%(self.SCCAddr,Position))
        if "Elevation" in Direction:
            self.IOIF.send("a%Xb1d%0.2X\r\n"%(self.SCCAddr,Position))        
            
    def storepositions(self):
        cPickle.dump(self.LightPointPositions, open('./laserpositions.dat','w'))
            
    def run(self):
        if self.verbose:
            print
            print "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,">",500)
        if self.verbose:
            print "Switch On pollings on used cards"
            
        self.CAEIF.commandstatemachine("POLLON:%X\r\n"%self.DGIAddr,">",500)
        self.CAEIF.commandstatemachine("ENABLEPOLL\r\n",">",500)
        
        if "ANSWER WRONG" in self.CAEIF.commandstatemachine("GETSCA0:%X\r\n"%self.SCCAddr,":SCA0:0x2A",2500):
            self.CAEIF.commandstatemachine("SETSCA0:%X=2A\r\n"%self.SCCAddr,">",2500)
            if self.verbose:
                print "SCA0 value is wrong, set the good one"
        else:
            if self.verbose:
                print "SCA0 value is OK"
        if "ANSWER WRONG" in self.CAEIF.commandstatemachine("GETSCB0:%X\r\n"%self.SCCAddr,":SCB0:0xA8",2500):
            self.CAEIF.commandstatemachine("SETSCB0:%X=A8\r\n"%self.SCCAddr,">",2500)
            if self.verbose:
                print "SCB0 value is wrong, set the good one"
        else:
            if self.verbose:
                print "SCB0 value is OK"
        if "ANSWER WRONG" in self.CAEIF.commandstatemachine("GETSCA1:%X\r\n"%self.SCCAddr,":SCA1:0x3F",2500):
            self.CAEIF.commandstatemachine("SETSCA1:%X=3F\r\n"%self.SCCAddr,">",2500)
            if self.verbose:
                print "SCA1 value is wrong, set the good one"
        else:
            if self.verbose:
                print "SCA1 value is OK"
        if "ANSWER WRONG" in self.CAEIF.commandstatemachine("GETSCB1:%X\r\n"%self.SCCAddr,":SCB1:0x62",2500):
            self.CAEIF.commandstatemachine("SETSCB1:%X=62\r\n"%self.SCCAddr,">",2500)
            if self.verbose:
                print "SCB1 value is wrong, set the good one"
        else:
            if self.verbose:
                print "SCB1 value is OK"  
        
        self.DIOH.registeroutchannel("LASER", self.DGOAddr, 0, 0)
        self.DIOH.registeroutchannel("FASTLED", self.DGOAddr, 0, 1)
        self.DIOH.registeroutchannel("SLOWLED", self.DGOAddr, 1, 1)
        self.DIOH.registeroutchannel("PRGRUNLED", self.DGOAddr, 1, 7)
        self.DIOH.registeroutchannel("PRGSTOPLED", self.DGOAddr, 0, 7)
        self.DIOH.registerinchannel("EXIT", self.DGIAddr, 1, 7)
        self.DIOH.registerinchannel("MOVAZILEFT", self.DGIAddr, 0, 0)
        self.DIOH.registerinchannel("MOVAZIRIGHT", self.DGIAddr, 0, 1)
        self.DIOH.registerinchannel("MOVELEUP", self.DGIAddr, 0, 2)
        self.DIOH.registerinchannel("MOVELEDWN", self.DGIAddr, 0, 3)
        self.DIOH.registerinchannel("LASERSWBTN", self.DGIAddr, 1, 4)
        self.DIOH.registerinchannel("FASTBTN", self.DGIAddr, 0, 4)
        self.DIOH.registerinchannel("MEMORIZEBTN", self.DGIAddr, 0, 5)
        self.DIOH.registerinchannel("EREASEBTN", self.DGIAddr, 0, 6)
        self.DIOH.registerinchannel("CLEARBTN", self.DGIAddr, 0, 7)
        self.DIOH.registerinchannel("PRGRUNBTN", self.DGIAddr, 1, 5)
        self.DIOH.registerinchannel("PRGSTOPBTN", self.DGIAddr, 1, 6)
        self.DIOH.registerinoneventaliases(["PUSH"])
        self.DIOH.registerinoffeventaliases(["RELEASE"])
        self.DIOH.switchoffall()
        self.DIOH.switch("SLOWLED", "ON", True)
        self.DIOH.switch("PRGSTOPLED", "ON", True)
        self.moveservo("Azimuth", 128)
        self.moveservo("Elevation", 128)
        TCRGENERATOR = PPPASSIVETIMER(0.02, True)
        if self.verbose:
            print "Working loop started."
            print "Press Block1 Button7 to exit from working loop."
        ElPos = 128
        AzPos = 128
        AzStep = 1
        ElStep = 1
        TCR = 0
        NEXTTCR = 30
        ReadPointCnt = 0
        LaserState = False
        FastState = False
        RunProgram = False
        while(1):
            self.DIOH.chekinchannelschanges("MASTER")
            if self.DIOH.inputevent("EXIT","PUSH"):
                break
            if self.DIOH.inputevent("LASERSWBTN", "PUSH"):
                for i in range(1):
                    if not LaserState:
                        self.DIOH.switch("LASER", "ON")
                        LaserState = True
                        if self.verbose:
                            print "LASER ON"
                        break
                    if LaserState:
                        self.DIOH.switch("LASER", "OFF")
                        LaserState = False
                        if self.verbose:
                            print "LASER OFF"
                        break
                    
            if self.DIOH.inputevent("FASTBTN", "PUSH"):
                for i in range(1):
                    if not FastState:
                        self.DIOH.switch("FASTLED", "ON")
                        self.DIOH.switch("SLOWLED", "OFF")
                        FastState = True
                        ElStep = 10
                        AzStep = 10
                        if self.verbose:
                            print "FASTMOVE ON"
                        break
                    if FastState:
                        self.DIOH.switch("FASTLED", "OFF")
                        self.DIOH.switch("SLOWLED", "ON")
                        FastState = False
                        ElStep = 1
                        AzStep = 1
                        if self.verbose:
                            print "FASTMOVE OFF"
                        break
            if self.DIOH.inputevent("MOVAZIRIGHT", "PUSH"):    
                AzPos = AzPos + AzStep
                self.moveservo("Azimuth",AzPos)
            if self.DIOH.inputevent("MOVAZILEFT", "PUSH"):    
                AzPos = AzPos - AzStep
                self.moveservo("Azimuth",AzPos)
            if self.DIOH.inputevent("MOVELEDWN", "PUSH"):    
                ElPos = ElPos + ElStep
                self.moveservo("Elevation",ElPos)
            if self.DIOH.inputevent("MOVELEUP", "PUSH"):    
                ElPos = ElPos - ElStep
                self.moveservo("Elevation",ElPos)    
            
            if self.DIOH.inputevent("MEMORIZEBTN", "PUSH"): 
                MemoNumber = self.BM.mask(self.DIOH.getinputblockdata(self.DGIAddr, 1), "0F", "int")
                PointPos = {}
                PointPos["Azimuth"] = AzPos
                PointPos["Elevation"] = ElPos
                LightPositionsAtMemo = self.LightPointPositions.get(MemoNumber)
                if not LightPositionsAtMemo:
                    LightPositionsAtMemo = []
                LightPositionsAtMemo.append(PointPos)
                self.LightPointPositions[MemoNumber] = LightPositionsAtMemo
                if self.verbose:
                    print "Point added to memory %d"%MemoNumber
                    print LightPositionsAtMemo
                self.storepositions()
            if self.DIOH.inputevent("EREASEBTN", "PUSH"):
                MemoNumber = self.BM.mask(self.DIOH.getinputblockdata(self.DGIAddr, 1), "0F", "int")
                del self.LightPointPositions[MemoNumber]
                if self.verbose:
                    print "Memo %d ereased"%MemoNumber
                self.storepositions()
            if self.DIOH.inputevent("CLEARBTN", "PUSH"):
                MemoNumber = self.BM.mask(self.DIOH.getinputblockdata(self.DGIAddr, 1), "0F", "int")
                LP =self.LightPointPositions.get(MemoNumber)
                LP.pop()
                self.LightPointPositions[MemoNumber] = LP
                if self.verbose:
                    print "Memo %d last point cleared"%MemoNumber
                self.storepositions()
        
            if self.DIOH.inputevent("PRGRUNBTN", "PUSH"):
                self.DIOH.switch("PRGRUNLED", "ON")
                self.DIOH.switch("PRGSTOPLED", "OFF")
                RunProgram = True
                MemoNumber = self.BM.mask(self.DIOH.getinputblockdata(self.DGIAddr, 1), "0F", "int")
                LightPositionsAtMemo = self.LightPointPositions.get(MemoNumber)
                if self.verbose:
                    print "PROGRAM is RUNNING"
            if self.DIOH.inputevent("PRGSTOPBTN", "PUSH"):
                self.DIOH.switch("PRGRUNLED", "OFF")
                self.DIOH.switch("PRGSTOPLED", "ON")
                self.DIOH.switch("LASER", "OFF")
                RunProgram = False
                if self.verbose:
                    print "PROGRAM STOPPED"
        
            if TCRGENERATOR.isexpired():
                TCR = TCR + 1
                if TCR >= NEXTTCR:
                    if RunProgram:
                        self.DIOH.switch("LASER", "ON", True)
                        if ReadPointCnt == len(LightPositionsAtMemo) - 1:
                           ReadPointCnt = 0
                           random.shuffle(LightPositionsAtMemo)
                        MovePosition = LightPositionsAtMemo[ReadPointCnt]
                        if self.verbose:
                            print MovePosition
                        for position in MovePosition:
                            self.moveservo(position,MovePosition.get(position))
                        SelectSPMove = random.randrange(0,4)
                        SPMoveCounter = random.randrange(3,12)
                        if SelectSPMove == 0:
                            time.sleep(float(SPMoveCounter)/2)
                        if SelectSPMove == 1:
                            for i in range(SPMoveCounter):
                               self.moveservo("Azimuth",MovePosition.get("Azimuth")+3)
                               time.sleep(0.2)
                               self.moveservo("Azimuth",MovePosition.get("Azimuth")-3)
                               time.sleep(0.2)
                        if SelectSPMove == 2:
                            for i in range(SPMoveCounter):
                               self.moveservo("Elevation",MovePosition.get("Elevation")+3)
                               time.sleep(0.2)
                               self.moveservo("Elevation",MovePosition.get("Elevation")-3)
                               time.sleep(0.2)
                        if SelectSPMove == 3:
                            for i in range(SPMoveCounter):
                               self.moveservo("Elevation",MovePosition.get("Elevation")+3)
                               self.moveservo("Azimuth",MovePosition.get("Azimuth")-3)
                               time.sleep(0.1)
                               self.moveservo("Elevation",MovePosition.get("Elevation")-3)
                               self.moveservo("Azimuth",MovePosition.get("Azimuth")+3)
                               time.sleep(0.1)       
                        ReadPointCnt = ReadPointCnt + 1
                        if random.randrange(0,2):
                            self.DIOH.switch("LASER", "OFF", True)
                        NEXTTCR = random.randrange(25,100)
                        TCR = 0    
                TCRGENERATOR.reset()    

            self.DIOH.sendoutputs()        
                  
        if self.verbose:
            print "Switch off LASER and control LEDs"                  
        self.DIOH.switchoffall(True)
        if self.verbose:
            print "Set servo positions to middle"
        self.moveservo("Azimuth", 128)
        self.moveservo("Elevation", 128)
        if self.verbose:
            print "Disable global poll"               
        self.CAEIF.commandstatemachine("DISABLEPOLL\r\n",">",500)
        if self.verbose:
            print "Switch Off pollings on used cards"
        self.CAEIF.commandstatemachine("POLLOFF:%X\r\n"%self.DGIAddr,">",500) 
        if self.verbose:
            print "Exit from working loop" 
            print 

if __name__ == '__main__': 
    """Main program"""
    Options, Arguments = getopt.getopt(sys.argv[1:], "v")
    verbose = False
    for option in Options:            
        if option[0] == "-v":    
            verbose = True
    PPDriver = PPDRIVER();
    PPDriver.registerCAEinterface("localhost", 8000)
    PPDriver.registerIOinterface("localhost", 12000)
    PPDriver.connectinterfaces()
    CAEIF, IOIF, MONIF = PPDriver.getinterfaces()
    FCT = FATCATTRAINER(CAEIF, IOIF, verbose)   
    PPDriver.disconnectinterfaces()
    sys.exit(0)
    

    
