#!/usr/bin/env python
# -*- coding: utf-8 -*-

import scapy.all as scapy
import scapy.contrib.modbus as mb

def decode(pkt):
    prt=0
    addr,value = "",""
    bigL = [ ]
    global miniL
    if "ModbusADU" in pkt:
        grPkt=pkt["ModbusADU"].show(dump=True)
        lPkt=grPkt.splitlines()
        for i in lPkt:
            if "###" in i:
                prt=0
                typResp=""
            if "Coil" in i and "Read" in i:
                prt=1
                miniL[0] = "c"
            if "Coil" in i and "Write" in i:
                prt=1
                miniL[0]="cw"
            elif "Register" in i:
                prt=2
                miniL[0] = "r"
                miniL[3]=1
            elif "startAddr" in i or "outputAddr" in i or "registerAddr" in i:
                miniL[1] = i[i.find("=")+2:]
            elif "coilStatus" in i or "registerVal=" in i or ("outputsValue" in i and miniL[0]=="r"):
                value = i[i.find("[")+1:i.rfind("]")]
                value = value.split(", ")
                miniL[2] = value
            elif "outputValue" in i or "registerValue" in i:
                miniL[2] = i[i.find("=")+2:]
                miniL[3]=1
            elif "quantity" in i and miniL[0]=="c":
                miniL[3]=i[i.find("=")+2:]
            elif "quantityOutput" in i and miniL[0]=="cw":
                miniL[3]=i[i.find("=")+2:]
            elif "outputsValue" in i and miniL[0]=="cw":
                valInter=i[i.find("=")+2:]
                valInter2=valInter[1:-1]
                miniL[2]=valInter2
    if miniL.count(0)==0:
        if(type(miniL[2])==list):
            if miniL[0]=="r":
                for i in range(len(miniL[2])):
                    if("0x" in miniL[2][i]):
                        miniL[2][i]=int(miniL[2][i],16)
                    if("0x" in str(miniL[1])):
                        miniL[1]=int(miniL[1],16)
                    bigL.append([miniL[0],miniL[1]+i,miniL[2][i]])
            if miniL[0]=="c":
                nbOct=int(miniL[3],16)/8
                if nbOct*10%10!=0:
                    nbOct=int(nbOct)+1
                if nbOct==len(miniL[2]):
                    for i in range(int(nbOct)):
                        byte=8*[0]
                        convert=bin(int(miniL[2][i]))
                        for j in range(len(convert)-2):
                            byte[j]=convert[-(j+1)]
                        for j in range(8):
                            bigL.append(["c",int(str(miniL[1]),16)+i*8+j,byte[j]])
        else:
            if "0x" in str(miniL[1]):
                miniL[1]=int(miniL[1],16)
            if "0x" in str(miniL[2]):
                miniL[2]=int(miniL[2],16)
            if "0x" in str(miniL[3]):
                miniL[3]=int(miniL[3],16)
            if miniL[0]=="cw" and (miniL[2]>=1 or miniL[3]>1):
                if miniL[3]>1:
                    valeurs=bin(miniL[2])[::-1][:-2]
                    while len(valeurs)<miniL[3]:
                        valeurs+='0'
                    nouvList=[]
                    for i in range(miniL[3]):
                        nouvList.append(["c",miniL[1]+i,valeurs[i]])
                        i+=1
                    miniL=nouvList
                else:
                    if str(miniL[2])==str(65280):
                        miniL[2]=1
            if miniL[0]=="cw" and miniL[3]==1:
                miniL=miniL[:3]
                miniL[0]="c"
            if miniL[0]=="r" and miniL[3]==1:
                miniL=miniL[:3]
            bigL.append(miniL)
        
        if bigL!=[]:
            print(bigL)


        miniL = [0,0,0,0]
        bigL=[]
miniL = [0,0,0,0]
scapy.sniff(iface="lo", prn=decode)