#!/usr/bin/env python

SoftwareInfo = \
    { 'Function' : 'Mars buggy controller',
      'Design'   : 'CreativePeach c 2013',
      'Version'  : '1.0',
    }  

from cpphlib import *
import getopt
import sys
import termios, tty
import binascii
        
class MARSBUGGYCONTROLLER:

    def __init__(self, CAEIF, IOIF):
        """Initializing class of the control program"""
        
        self.CAEIF = CAEIF
        self.IOIF = IOIF
        self.IOCG = PPIOCARDGADGETS(self.CAEIF, self.CAEIF)
        self.BM = PPBITMANIPULATOR()
        self.TRAVELTIMER = PPPASSIVETIMER()
        self.AINAddr = -1
        self.IRAddr = -1
        self.DINAddr = -1
        self.LCDAddr = -1
        if not(self.hardwarecheck()):
            sys.exit(1)
        self.AIH = PPAINHANDLER(self.CAEIF, self.IOIF)
        self.AIH.registerinchannel("MOTOR",self.AINAddr,4)
        self.AIH.registerinchannel("WHEEL",self.AINAddr,0)
        self.DIOH = PPDIGINOUTHANDLER(self.CAEIF, self.IOIF)
        self.DIOH.registerinchannel("SEND",self.DINAddr,0,7)
        self.DIOH.registerinchannel("EARTHMODE",self.DINAddr,0,0)
        self.DIOH.registerinchannel("MARSMODE",self.DINAddr,0,1)
        self.DIOH.registerinchannel("EXIT",self.DINAddr,0,4)
        self.DIOH.registerinchannel("SHUTDOWN",self.DINAddr,0,5)
        self.DIOH.registerinoneventaliases(["PUSH"])
        self.LCDH = PPLCDHANDLER(self.CAEIF, self.IOIF)
        self.LCDH.setlcdcontrolleraddress(self.LCDAddr)
        self.ExitCode = 0
        self.run()    

 
    def hardwarecheck(self):
        """Identifying used cards, and their addresses based on discover dict"""
        
        LCDCards = self.IOCG.searchcards("ALPHANUMERIC LCD CONTROLLER")
        if LCDCards:
           self.LCDAddr = LCDCards[0]
        DICards = self.IOCG.searchcards("16 channel DIGITAL INPUT")
        if DICards:
           self.DINAddr = DICards[0]
        AICards = self.IOCG.searchcards("6 channel ANALOGUE INPUT")
        if AICards:
           self.AINAddr = AICards[0] 
        RCCards = self.IOCG.searchcards("IR REMOTE CONTROLLER")
        if RCCards:
           self.IRAddr = RCCards[0] 
        if self.AINAddr == -1 or self.IRAddr == -1 or self.DINAddr == -1 or self.LCDAddr == -1:            
            return False
        else:
            return True           

    def CRCCalc(self,Data):
        s=binascii.unhexlify(Data)
        b=[ord(x) for x in s]
        CRC="%0.2X"%(sum(b) % 256)    
        return "%0.2X%s%s"%(1+(len(Data)/2),Data,CRC)

    def getkey(self):
        """Non blocking keyboard reader for console input or mixed input applications"""
        file = sys.stdin.fileno()
        mode = termios.tcgetattr(file)
        try:
            ch = None
            inFile = None
            tty.setraw(file, termios.TCSANOW)
            inFile, unused1, unused2 = select.select([sys.stdin],[],[],0.001)
            if inFile:
                ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(file, termios.TCSANOW, mode)
        return ch
    

    def run(self):
        """The core of controller class"""
        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)
        self.CAEIF.commandstatemachine("POLLON:%X\r\n"%self.AINAddr,">",500)
        self.CAEIF.commandstatemachine("POLLON:%X\r\n"%self.DINAddr,">",500)
        self.CAEIF.commandstatemachine("ENABLEPOLL\r\n",">",500)    
        
        ToNextInputHandle = None
        ChangeData = False
        LastMotorData = "80"
        LastWheelData = "80"
        MarsMotorCommand = ""
        MarsWheelCommand = ""
        LastBCD0State = ""
        LastBCD1State = ""
        Mode = "NONE"
        MyTime = 215
        TimeFast = MyTime - 4
        TimeSlow = 4
        TimeMode = "START"
        RunTravelTimer = False
        MarsWheelData = {
            "4": {"DISP" : "0  ", "SERVO" : "80", "COMP" : "00"},
            "3": {"DISP" : "10L", "SERVO" : "99", "COMP" : "00"},
            "2": {"DISP" : "25L", "SERVO" : "B5", "COMP" : "01"},
            "1": {"DISP" : "30L", "SERVO" : "D0", "COMP" : "01"},
            "0": {"DISP" : "45L", "SERVO" : "F0", "COMP" : "02"},
            "5": {"DISP" : "10R", "SERVO" : "67", "COMP" : "00"},
            "6": {"DISP" : "25R", "SERVO" : "4B", "COMP" : "01"},
            "7": {"DISP" : "30R", "SERVO" : "30", "COMP" : "01"},
            "8": {"DISP" : "45R", "SERVO" : "00", "COMP" : "02"},
          
            "9": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},    
            "A": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},
            "B": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},
            "C": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},
            "D": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},
            "E": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},
            "F": {"DISP" : "---", "SERVO" : "80", "COMP" : "00"},    
                
        }
        MarsDriveData = {
            "1": {"DISP" : "15cm F", "MOTOR" : "8B", "TIME" : 0.7},
            "2": {"DISP" : "30cm F", "MOTOR" : "8B", "TIME" : 2},
            "3": {"DISP" : "60cm F", "MOTOR" : "8B", "TIME" : 4},
            "4": {"DISP" : "99cm F", "MOTOR" : "8B", "TIME" : 6},
            "0": {"DISP" : " 0cm  ", "MOTOR" : "80", "TIME" : 0},
            "F": {"DISP" : "15cm B", "MOTOR" : "75", "TIME" : 0.7},
            "E": {"DISP" : "30cm B", "MOTOR" : "75", "TIME" : 2},
            "D": {"DISP" : "60cm B", "MOTOR" : "75", "TIME" : 4},
            "C": {"DISP" : "99cm B", "MOTOR" : "75", "TIME" : 6},    

            "5": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},    
            "6": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},
            "7": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},  
            "8": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},
            "9": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},
            "A": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},
            "B": {"DISP" : "------", "MOTOR" : "80", "TIME" : 0},      
        }
        MWD = MarsWheelData.get("4")
        MDD = MarsDriveData.get("0")                
           
        self.LCDH.clear()
        self.LCDH.printat(0,0,"     MARS BUGGY     ")
        self.LCDH.printat(0,1,"     CONTROLLER     ")
        self.LCDH.printat(0,3,"    SELECT  MODE    ")
        
        #self.LCDH.printat(0,1,"     MARS BUGGY     ")
        #self.LCDH.printat(0,2,"     CONTROLLER     ")
        #Mode = "EARTH" 
        # 201,213 uncomment ,3 lines this before delete, 4th to 6th this before uncomment
        
        while(1):
            ch = self.getkey()
            if ch == "q":
                break
            ToNextInputHandle = self.AIH.chekinchannelschanges("MASTER")  
            ChangeData = False
            if self.AIH.inputevent("MOTOR", "CHANGE"):
                ChangeData = True
                LastMotorData = self.AIH.getvalue("MOTOR")
            if self.AIH.inputevent("WHEEL", "CHANGE"):
                ChangeData = True
                LastWheelData = self.AIH.getvalue("WHEEL")
            if "CHECK" in Mode:
                if int(LastMotorData,16) < 121 or int(LastMotorData,16) > 135:
                    self.LCDH.clear()
                    self.LCDH.printat(0,0," PLEASE TURN  MOTOR ")  
                    self.LCDH.printat(0,1," DRIVE  POTMETER  TO ")
                    self.LCDH.printat(0,2,"  MIDDLE POSITION!  ")
                Mode = "POTWAIT"
            if "POTWAIT" in Mode:
                if int(LastMotorData,16) >= 121 and int(LastMotorData,16) <= 135:
                    self.LCDH.clear()    
                    self.LCDH.printat(0,0,"  MODE: EARTH TEST  ")
                    self.LCDH.printat(0,1,"Wheel:00   Drw:000% ")
                    Mode = "EARTH"        
            if ChangeData and "EARTH" in Mode:
                if int(LastWheelData,16) == 128:
                    Dir = " "
                    Value = 0
                if int(LastWheelData,16) < 128:
                    Dir = "R"
                    Value = 45-round(int(LastWheelData,16)*45.0/128.0)
                if int(LastWheelData,16) > 128:
                    Dir = "L"
                    Value = round((-128+int(LastWheelData,16)*45.0)/128.0)-45
                if Value >= 0:
                    #pass
                    self.LCDH.printat(0,1,"Wheel:%0.2d%s"%(Value,Dir))
                if int(LastMotorData,16) == 128:
                    Dir = " "
                    Value = 0
                if int(LastMotorData,16) < 128:
                    Dir = "B"
                    Value = 100-round(int(LastMotorData,16)*100.0/128.0)
                if int(LastMotorData,16) > 128:
                    Dir = "F"
                    Value = round((-128+int(LastMotorData,16)*100.0)/128.0)-100
                if Value >= 0:
                    #pass
                    self.LCDH.printat(11,1,"Drw:%0.3d"%Value+"%"+"%s"%Dir)
                snt = "%s%s"%(LastWheelData,LastMotorData)
                self.IOIF.send("a%XsHS%s\r\n"%(self.IRAddr, self.CRCCalc(snt)))
            
            ToNextInputHandle = self.DIOH.chekinchannelschanges(ToNextInputHandle)
            if self.DIOH.inputevent("SEND", "PUSH") and Mode == "MARS" and not RunTravelTimer:
                self.LCDH.printat(0,2,"Command sent to Mars")
                time.sleep(2)
                self.LCDH.printat(0,2," And cannot recall! ")
                time.sleep(2)
                self.LCDH.printat(0,2,"     Lets pray!     ")
                time.sleep(2)
                self.LCDH.printat(0,2," Remaining time is: ")
                self.TRAVELTIMER.set(1)
                RunTravelTimer = True
                
            if self.DIOH.inputevent("EARTHMODE", "PUSH"):
                self.CAEIF.commandstatemachine("GETLASTINPST:%X=4\r\n"%self.AINAddr,">",500)
                Mode = "CHECK"
                time.sleep(0.2)
            if self.DIOH.inputevent("MARSMODE", "PUSH"):
                self.LCDH.clear()
                self.LCDH.printat(0,0,"  MODE: MARS LIVE   ")
                self.CAEIF.commandstatemachine("GETLASTINPST:%X=1\r\n"%self.DINAddr,">",500)
                Mode = "MARS"
            if self.DIOH.inputevent("EXIT", "PUSH"):
                self.LCDH.clear()
                self.LCDH.printat(2,1," MARS BUGGY IS")
                self.LCDH.printat(2,2,"OUT OF CONTROL!!")
                break
            if self.DIOH.inputevent("SHUTDOWN", "PUSH"):
                self.LCDH.clear()
                self.LCDH.printat(2,0," MARS BUGGY IS")
                self.LCDH.printat(2,1,"OUT OF CONTROL!!")
                self.LCDH.printat(2,2," RPI SHUT DOWN ")
                self.LCDH.printat(2,3,"  IN PROGRESS")
                self.ExitCode = 16
                break
            
            if self.TRAVELTIMER.isexpired():
                MyTime = MyTime - 1
                if MyTime < TimeFast and MyTime > TimeSlow  and TimeMode == "START":
                    self.TRAVELTIMER.set(0.01)
                    TimeMode = "FAST"
                if MyTime < TimeSlow and TimeMode == "FAST":   
                    self.TRAVELTIMER.set(1)
                    TimeMode = "SLOW"
                self.LCDH.printat(7,3,time.strftime("%M:%S",  time.localtime(MyTime)))
                if MyTime != 0:
                    self.TRAVELTIMER.reset()
                else:    
                    MyTime = 215
                    TimeMode = "START"
                    self.LCDH.printat(0,2,"Action is  operating")
                    self.LCDH.printat(0,3,"      on  MARS      ")
                    snt = "%s%s"%(MWD.get("SERVO"),"80")
                    for i in range(5):
                        self.IOIF.send("a%XsHS%s\r\n"%(self.IRAddr, self.CRCCalc(snt)))
                        time.sleep(0.1)
                    time.sleep(1)
                    if self.BM.mask(MDD.get("MOTOR"),255,'int') >= 128:
                        MD=self.BM.mask(self.BM.mask(MDD.get("MOTOR"),255,'int') + self.BM.mask(MWD.get("COMP"),255,'int'),255)
                    else:   
                        MD=self.BM.mask(self.BM.mask(MDD.get("MOTOR"),255,'int') - self.BM.mask(MWD.get("COMP"),255,'int'),255)
                    snt = "%s%s"%(MWD.get("SERVO"),MD) 
                    for i in range(5):
                        self.IOIF.send("a%XsHS%s\r\n"%(self.IRAddr, self.CRCCalc(snt)))
                        time.sleep(0.1)    
                    time.sleep(MDD.get("TIME"))
                    snt = "%s%s"%(MWD.get("SERVO"),"80")
                    for i in range(5):
                        self.IOIF.send("a%XsHS%s\r\n"%(self.IRAddr, self.CRCCalc(snt)))
                        time.sleep(0.1)
                    self.LCDH.printat(0,2,"                    ")    
                    self.LCDH.printat(0,3,"                    ")
                    RunTravelTimer = False
                    
            if ToNextInputHandle:
                if "a%Xb1"%self.DINAddr in ToNextInputHandle:
                    LastBCD1State = ToNextInputHandle[5:6]
                    LastBCD0State = ToNextInputHandle[6:7]
                    if Mode == "MARS":
                        MWD = MarsWheelData.get(LastBCD1State)
                        if MWD:
                            self.LCDH.printat(0,1,"Wheel:%s"%MWD.get("DISP"))
                        MDD = MarsDriveData.get(LastBCD0State)
                        if MDD:
                            self.LCDH.printat(11,1,"Go:%s"%MDD.get("DISP"))    
            
        snt = "8080"
        self.IOIF.send("a%XsHS%s\r\n"%(self.IRAddr, self.CRCCalc(snt)))     
        self.CAEIF.commandstatemachine("DISABLEPOLL\r\n",">",500)
        self.CAEIF.commandstatemachine("POLLOFF:%X\r\n"%self.AINAddr,">",500) 
        self.CAEIF.commandstatemachine("POLLOFF:%X\r\n"%self.DINAddr,">",500) 

def usage():
    sys.stdout.write("""\r\nMars buggy controller %s %s \r\n\

USAGE: ./mbrc.py [options]  

    options:
    --------
    -h  : print this help and exit
              
    Receiver settings:
    MSR1 = 0x07   SCA0 = 0x82   SCB0 = 0x40    MODE = 0x0F 
    
    CONTROLS:
    ---------
    In Earth test mode:
    Wheel        - Potmeter on ANALOGUE INPUT CARD's channel 0
    Motor drive  - Potmeter on ANALOGUE INPUT CARD's channel 4
    
    In Mars live mode:
    Wheel        - BCD1 on DIGITAL INPUT CARD
    Motor drive  - BCD0 on DIGITAL INPUT CARD
    Send Command - Button 7 in Block 0 on DIGITAL INPUT CARD
    Earth mode   - Button 0 in Block 0 on DIGITAL INPUT CARD
    Mars mode    - Button 1 in Block 0 on DIGITAL INPUT CARD
    EXIT program - Button 4 in Block 0 on DIGITAL INPUT CARD
                   or press key q on console
    Shutdown RPI - Button 5 in Block 0 on DIGITAL INPUT CARD
                   
    AUTOMATIC START AND SHUTDOWN:
    -----------------------------
    mbprun.sh, mbpshutdown.sh scripcts are needed. Use /etc/inittab,
    write the following 3 lines into it, and use 'init q' command.
    
    #Mars buggy control autostart and keep alive, computer shutdown after exit
    ASP1:2345:respawn:'YOUR_PATH'/mbrc/mbprun.sh
    ASP2:2345:respawn:/root/mbpshutdown.sh

    
"""%(SoftwareInfo.get('Version'), SoftwareInfo.get('Design')))


if __name__ == '__main__': 
    """Main program"""
    Options, Arguments = getopt.getopt(sys.argv[1:], "h")
    for option in Options:            
        if option[0] == "-h":
            usage()
            sys.exit(0)
    
    PPDriver = PPDRIVER();
    PPDriver.registerCAEinterface("localhost", 8000) 
    PPDriver.registerIOinterface("localhost", 12000)
    PPDriver.connectinterfaces()
    CAEIF, IOIF, MONIF = PPDriver.getinterfaces()
    MbController = MARSBUGGYCONTROLLER(CAEIF, IOIF)
    PPDriver.disconnectinterfaces()
    sys.exit(MbController.ExitCode)
    
