## @package NeoModbusServer
#  Neobotix Modbus server module                     
#
#  This module is used to execute robot scripts in a seperate thread.
#
#  @author ovb
#
# for questions and comments:
# support@neobotix.de, subject: "ModbusServer"
#
# -----------------------------------------------------------------------------------
#
#		Copyright (c) 2009 Neobotix (www.neobotix.de)
#
#      This software is allowed to be used and modified only in association with a Neobotix
#      robot platform. It is allowed to include the software into applications and
#      to distribute it with a Neobotix robot platform. 
#      
#      This software is provided WITHOUT ANY WARRANTY; without even the
#		   implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
#		   PURPOSE. See the Neobotix License (Version 1.0) for more details.
#
#      You should have received a copy of the Neobotix License
#      along with this software; if not, write to the 
#      Gesellschaft fuer Produktionssysteme, Neobotix, Nobelstrasse 12, 70569 Stuttgart, Germany
#
# -----------------------------------------------------------------------------------

import socket
import string
import array
import struct
import threading
import time
import types
import logging

#Get Logger
logger = logging.getLogger("NeoPyClientLog")

## Modbus server.
# sets input registers which are requested by the client with MODBUS_READ_INPUT_REGISTERS
# read output registers which are set by the client with MODBUS_WRITE_MULTIPLE_REGISTERS
class NeoModbusServer(threading.Thread): 

    # modbus function codes
    MODBUS_READ_COILS = 1
    MODBUS_READ_DISCRETE_INPUTS = 2
    MODBUS_READ_HOLDING_REGISTERS = 3
    MODBUS_READ_INPUT_REGISTERS = 4
    MODBUS_WRITE_SINGLE_COIL = 5
    MODBUS_WRITE_SINGLE_REGISTER = 6
    MODBUS_WRITE_MULTIPLE_COILS = 15
    MODBUS_WRITE_MULTIPLE_REGISTERS = 16
    
    ## Class constructor function
    #  @param self The object pointer
    #  @param numDiscreteInputs Number of bit inputs
    #  @param numInputRegisters Number of 16Bit input registers
    #  @param numCoils Number of bit outputs
    #  @param numHoldingRegisters Number of 16bit output registers
    def __init__(self, numDiscreteInputs, numInputRegisters, numCoils, numHoldingRegisters):
        self.discreteInputs = numDiscreteInputs*[False]
        self.inputRegisters = numInputRegisters*[0]
        
        self.coils = numCoils*[False]
        self.holdingRegisters = numHoldingRegisters*[0]
        
        threading.Thread.__init__(self)
        threading.Thread.start(self)
    
    #value must be boolean
#    def setDiscreteInput(self, address, value):
#        if(address<len(self.discreteInputs) and type(value)==types.BooleanType):
#            self.discreteInputs[address] = value
#            return True
#        else: return False
    
    ## Write an input register
    #  @param self The object pointer
    #  @param address Address to write to
    #  @param value The value to write as INT
    #  @return True if address valid and value valid else False
    def setInputRegister(self, address, value):
        if(address<len(self.inputRegisters) and type(value)==types.IntType):
            self.inputRegisters[address] = value
            return True
        else: return False


    ## Read an input register-Only for Debugging
    #  @param self The object pointer
    #  @param address Address to read from
    #  @return Register Value if address valid else False
    #
    #  Returns the value of an input register. Only for debugging!
    def getInputRegister(self, address):
        if(address<len(self.inputRegisters)):
            return self.inputRegisters[address]
        else: return False


#    def getCoil(self, address):
#        if(address<len(self.coils)):
#            return self.coils[address]
#        else: return 0


    ## Get register value
    #  @param self The object pointer
    #  @param address The address to read from
    #  @return the register value
    def getHoldingRegister(self, address):
        if(address<len(self.holdingRegisters)):
            return self.holdingRegisters[address]
        else: return 0


    ## Set register value - Only for Debugging
    #  @param self The object pointer
    #  @param address The address to write to
    #  @param value The value to write
    #  @return True if address and value are valid, else False
    #
    #  Set a holding register. Will be normally be done from the python client
    #  Only for Debugging!
    def setHoldingRegister(self, address, value):
        if(address<len(self.holdingRegisters) and type(value)==types.IntType):
            self.holdingRegisters[address] = value
            return True
        else: return 0


    ## reads a modbus message and sends answer
    #  @param self The object pointer
    #  @param sockData The raw socket stream data
    def parseModbusData(self, sockData):
        try :
            # read header
            sHeader = sockData.recv(6)
    
            if not sHeader: return False
            header = array.array('B', sHeader)
            transactionID = (header[0] << 8) | header[1]
            protocol = (header[2] << 8) | header[3]
            numBytes = (header[4] << 8) | header[5]
            
            # read data
            sData = sockData.recv(numBytes)
            msg = array.array('B', sData)
            unitID = msg[0]
            function = msg[1]
            
            if function == self.MODBUS_READ_COILS:
                startAddress = (msg[2] << 8) | msg[3]
                numOutputs = (msg[4] << 8) | msg[5]
                logger.debug("Read "+str(numOutputs)+" coils ("+str(startAddress)+"-"+str(startAddress+numOutputs-1)+")  (not implemented)") 
                
                startData = startAddress / 8        #array index to the byte containing the first coil
                startOffset = startAddress % 8      #offset to the first coil inside the first byte
                
                numData = startOffset+numOutputs / 8
                if numOutputs % 8 != 0:
                    numData += 1
        
                numBytesReply = numData + 3
                sHeaderReply = struct.pack('9B', header[0], header[1], 0, 0, 0, numBytesReply, msg[0], msg[1], numData)
         
                sDataReply = ""
                for i in range (0, numData):
                    sDataReply += struct.pack('B', self.holdingRegisters[startData+i])
                
                sockData.send(sHeaderReply+sDataReply) 
                    
            elif function == self.MODBUS_READ_DISCRETE_INPUTS:
                startAddress = (msg[2] << 8) | msg[3]
                numInputs = (msg[4] << 8) | msg[5]
                logger.info("read input discrete (not implemented)")
                
                numData = numInputs / 8
                if numInputs % 8 != 0:
                    numData += 1
        
                numBytesReply = numData + 3
                sHeaderReply = struct.pack('9B', header[0], header[1], 0, 0, 0, numBytesReply, msg[0], msg[1], numData)
                sockData.send(sHeaderReply)
         
                for i in range (0, numRegisters):
                    sDataReply = struct.pack('2B', 0, 0)
                    sockData.send(sDataReply)            
                
            elif function == self.MODBUS_READ_HOLDING_REGISTERS:
                startAddress = (msg[2] << 8) | msg[3]
                numRegisters = (msg[4] << 8) | msg[5]
                logger.debug("Read Holding Registers ("+str(numRegisters)+" registers from "+str(startAddress)+" to "+str(startAddress+numRegisters-1)+")")
                       
                numData = 2 * numRegisters
                numBytesReply = numData + 3
                sHeaderReply = struct.pack('9B', header[0], header[1], 0, 0, 0, numBytesReply, msg[0], msg[1], numData)
                sDataReply = ""
                for i in range (0, numRegisters):
                    registerValue = self.holdingRegisters[startAddress+i]
                    sDataReply += struct.pack('2B', registerValue >> 8 & 0xFF, registerValue & 0xFF)
                sockData.send(sHeaderReply+sDataReply)
                
            elif function == self.MODBUS_READ_INPUT_REGISTERS:
                startAddress = (msg[2] << 8) | msg[3]
                numRegisters = (msg[4] << 8) | msg[5]
                logger.debug("Read Input Registers ("+str(numRegisters)+" registers from "+str(startAddress)+" to "+str(startAddress+numRegisters-1)+")")
                       
                numData = 2 * numRegisters
                numBytesReply = numData + 3
                sHeaderReply = struct.pack('9B', header[0], header[1], 0, 0, 0, numBytesReply, msg[0], msg[1], numData)
                sDataReply = ""
                for i in range (0, numRegisters):
                    registerValue = self.inputRegisters[startAddress+i]
                    sDataReply += struct.pack('2B', registerValue >> 8 & 0xFF, registerValue & 0xFF)
                sockData.send(sHeaderReply+sDataReply)
                    
            elif function == self.MODBUS_WRITE_SINGLE_COIL:
                outputAddress = (msg[2] << 8) | msg[3]
                outputValue = (msg[4] << 8) | msg[5]
                logger.info("write single coil (not implemented)")
        
            elif function == self.MODBUS_WRITE_SINGLE_REGISTER:
                registerAddress = (msg[2] << 8) | msg[3]
                registerValue = (msg[4] << 8) | msg[5]
                logger.debug("write single register ("+str(registerAddress)+"="+str(registerValue)+")")
                self.holdingRegisters[registerAddress] = registerValue
    
            elif function == self.MODBUS_WRITE_MULTIPLE_COILS:
                outputAddress = (msg[2] << 8) | msg[3]
                outputValue = (msg[4] << 8) | msg[5]
                logger.info("write multiple coils (not implemented)")
        
            elif function == self.MODBUS_WRITE_MULTIPLE_REGISTERS:
                registerAddress = (msg[2] << 8) | msg[3]
                numRegister = (msg[4] << 8) | msg[5]
                logger.debug("write Multiple Registers ("+str(numRegister)+")")
                
                numBytes = msg[6]
                
                for i in range(0, numBytes/2):
                    registerValue = (msg[7+i*2] << 8) | msg[8+i*2]
                    self.holdingRegisters[registerAddress+i] = registerValue
                    logger.debug("Register "+str(registerAddress+i)+"="+str(registerValue))
                
                sReply = struct.pack('12B', header[0], header[1], 0, 0, 0, 6, msg[0], msg[1], msg[2], msg[3], msg[4], msg[5])
                sockData.send(sReply)
                
            else:
                logger.error("Unknown Function Code") 
            return True
        except Exception, e:
            logger.error(" Exception: " + e)
            return False
          
    ## Server thread run function 
    #  @param self Object pointer
    #
    def run(self):
        
        logger.info("Started")
        
        mySocket = socket.socket (socket.AF_INET, socket.SOCK_STREAM)
        HOST = socket.gethostname()
        PORT = 11000         #502
        mySocket.bind(('', PORT))
        mySocket.listen(1)

	while (True):
            logger.info("waiting for client")
            conn, addr = mySocket.accept()
            logger.info("Client connected from " + str(addr))
            
            while conn:
                if(self.parseModbusData(conn) == False): break
            
            
            logger.info("Client disconnected")
            conn.close()            

## Main test function  
#
if __name__ == "__main__":
    #this is for testing the modbus slave alone
    #New Modbus Slave device : 16 bit inputs, 10 input regs, 16 coils, 10 holding regs
    modbusServer = NeoModbusServer(16, 10, 16, 10)
    
    i = 0 
    while True:
        #modbusServer.setInputRegister(0, i)
        #i += 1
        time.sleep(2)