"""
Omron PLC FINS Protocol - Read Data Memory (DM) Words
======================================================
Reads 3 consecutive DM words from an Omron PLC using FINS over UDP.

FINS Frame Structure:
  Header (10 bytes) + Command (2 bytes) + Address (4 bytes) + Count (2 bytes)

Usage:
  python omron_fins_reader.py
  python omron_fins_reader.py --host 192.168.1.10 --dm 100 --count 3
"""

import socket
import struct
import argparse
import time


# ---------------------------------------------------------------------------
# FINS Constants
# ---------------------------------------------------------------------------
FINS_PORT          = 9600          # Default Omron FINS UDP port
DEFAULT_TIMEOUT    = 3.0           # Socket timeout in seconds

# Memory area codes
MEM_DM_WORD        = 0x82          # DM area, word access
MEM_DM_BIT         = 0x02          # DM area, bit access  (not used here)
MEM_CIO_WORD       = 0xB0          # CIO/Work area, word access

# FINS command codes
CMD_MEMORY_READ    = (0x01, 0x01)  # MEMORY AREA READ
CMD_MEMORY_WRITE   = (0x01, 0x02)  # MEMORY AREA WRITE


# ---------------------------------------------------------------------------
# FINS Frame Builder
# ---------------------------------------------------------------------------
def build_fins_read_frame(
    dst_net: int,
    dst_node: int,
    dst_unit: int,
    src_net: int,
    src_node: int,
    src_unit: int,
    sid: int,
    mem_area: int,
    start_address: int,
    start_bit: int,
    word_count: int,
) -> bytes:
    """
    Build a FINS UDP read request frame.

    Frame layout (18 bytes total for a read command):
      [0]     ICF  – Information Control Field  (0x80 = command, no response needed flag clear)
      [1]     RSV  – Reserved                  (0x00)
      [2]     GCT  – Gateway Count             (0x02)
      [3]     DNA  – Destination Network
      [4]     DA1  – Destination Node
      [5]     DA2  – Destination Unit
      [6]     SNA  – Source Network
      [7]     SA1  – Source Node
      [8]     SA2  – Source Unit
      [9]     SID  – Service ID (counter)
      [10:12] MRC/SRC – Command code
      [12]    Memory area code
      [13:15] Start address (word, big-endian)
      [15]    Start bit
      [16:18] Word count (big-endian)
    """
    header = struct.pack(
        "BBBBBBBBBB",
        0x80,       # ICF: command frame
        0x00,       # RSV
        0x02,       # GCT: gateway count (max hops)
        dst_net,    # DNA
        dst_node,   # DA1
        dst_unit,   # DA2
        src_net,    # SNA
        src_node,   # SA1
        src_unit,   # SA2
        sid & 0xFF, # SID
    )
    command = struct.pack("BB", *CMD_MEMORY_READ)
    address = struct.pack(
        "BBHB",
        mem_area,
        (start_address >> 8) & 0xFF,
        start_address & 0xFF,
        start_bit,
    )
    # Re-pack properly: area(1) + addr_high(1) + addr_low(1) + bit(1)
    address = bytes([
        mem_area,
        (start_address >> 8) & 0xFF,
        start_address & 0xFF,
        start_bit,
    ])
    count = struct.pack(">H", word_count)
    return header + command + address + count


# ---------------------------------------------------------------------------
# FINS Response Parser
# ---------------------------------------------------------------------------
def parse_fins_response(data: bytes, expected_words: int) -> list[int]:
    """
    Parse a FINS MEMORY AREA READ response.

    Response layout:
      [0:10]  FINS header (same structure as request)
      [10:12] Command code echo (01 01)
      [12:14] End code (00 00 = success)
      [14:]   Data: word_count × 2 bytes, big-endian unsigned shorts
    """
    if len(data) < 14:
        raise ValueError(f"Response too short: {len(data)} bytes")

    # End code
    end_code = struct.unpack_from(">H", data, 12)[0]
    if end_code != 0x0000:
        main_code = (end_code >> 8) & 0xFF
        sub_code  = end_code & 0xFF
        raise RuntimeError(
            f"FINS error – main: 0x{main_code:02X}, sub: 0x{sub_code:02X}  "
            f"({fins_error_description(main_code, sub_code)})"
        )

    payload = data[14:]
    required_bytes = expected_words * 2
    if len(payload) < required_bytes:
        raise ValueError(
            f"Payload too short: got {len(payload)} bytes, "
            f"expected {required_bytes} for {expected_words} words"
        )

    words = []
    for i in range(expected_words):
        word = struct.unpack_from(">H", payload, i * 2)[0]
        words.append(word)
    return words


# ---------------------------------------------------------------------------
# Error code lookup (subset of common codes)
# ---------------------------------------------------------------------------
def fins_error_description(main: int, sub: int) -> str:
    errors = {
        (0x01, 0x01): "Local node not in network",
        (0x02, 0x01): "Destination node not in network",
        (0x03, 0x01): "Communications controller error",
        (0x04, 0x01): "Command not supported",
        (0x05, 0x01): "Routing table error",
        (0x10, 0x01): "Command too long",
        (0x10, 0x02): "Command too short",
        (0x11, 0x01): "Area type error (wrong memory area)",
        (0x11, 0x02): "Access size error",
        (0x11, 0x03): "Address range error",
        (0x20, 0x02): "Protected – cannot write",
        (0x21, 0x01): "CPU unit in wrong mode",
        (0x22, 0x01): "Service already executing",
        (0x23, 0x05): "Aborted",
    }
    return errors.get((main, sub), "Unknown error")


# ---------------------------------------------------------------------------
# Main reader class
# ---------------------------------------------------------------------------
class OmronFINSReader:
    def __init__(
        self,
        plc_host: str,
        plc_port: int = FINS_PORT,
        plc_node: int = 0,        # DA1: 0 = auto-detect by IP
        src_node: int = 1,        # SA1: this PC's node number (1–254)
        timeout: float = DEFAULT_TIMEOUT,
    ):
        self.plc_host = plc_host
        self.plc_port = plc_port
        self.plc_node = plc_node
        self.src_node = src_node
        self.timeout  = timeout
        self._sid     = 0
        self._sock    = None

    def connect(self):
        """Open UDP socket."""
        self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self._sock.settimeout(self.timeout)
        print(f"[FINS] UDP socket ready → {self.plc_host}:{self.plc_port}")

    def disconnect(self):
        if self._sock:
            self._sock.close()
            self._sock = None
            print("[FINS] Socket closed.")

    def _next_sid(self) -> int:
        self._sid = (self._sid + 1) & 0xFF
        return self._sid

    def read_dm_words(self, start_address: int, count: int) -> list[int]:
        """
        Read `count` words starting at DM[start_address].
        Returns a list of unsigned 16-bit integers.
        """
        if self._sock is None:
            raise RuntimeError("Not connected. Call connect() first.")

        # Resolve node from last octet of IP if not set explicitly
        plc_node = self.plc_node or int(self.plc_host.split(".")[-1])

        frame = build_fins_read_frame(
            dst_net=0,
            dst_node=plc_node,
            dst_unit=0x00,        # 0x00 = CPU unit
            src_net=0,
            src_node=self.src_node,
            src_unit=0x00,
            sid=self._next_sid(),
            mem_area=MEM_DM_WORD,
            start_address=start_address,
            start_bit=0,
            word_count=count,
        )

        print(f"[FINS] TX ({len(frame)} bytes): {frame.hex(' ').upper()}")
        self._sock.sendto(frame, (self.plc_host, self.plc_port))

        response, addr = self._sock.recvfrom(2048)
        print(f"[FINS] RX ({len(response)} bytes) from {addr}: {response.hex(' ').upper()}")

        return parse_fins_response(response, count)


# ---------------------------------------------------------------------------
# Entry point
# ---------------------------------------------------------------------------
def main():
    parser = argparse.ArgumentParser(
        description="Read DM words from an Omron PLC via FINS/UDP"
    )
    parser.add_argument("--host",    default="192.168.1.10", help="PLC IP address")
    parser.add_argument("--port",    default=FINS_PORT, type=int, help="FINS UDP port (default 9600)")
    parser.add_argument("--dm",      default=100, type=int, help="DM start address (default 100)")
    parser.add_argument("--count",   default=3,   type=int, help="Number of words to read (default 3)")
    parser.add_argument("--src-node",default=1,   type=int, help="Source node number (default 1)")
    parser.add_argument("--plc-node",default=0,   type=int, help="PLC node (0 = auto from IP)")
    parser.add_argument("--repeat",  default=1,   type=int, help="Poll N times (0 = forever)")
    parser.add_argument("--interval",default=1.0, type=float, help="Seconds between polls")
    args = parser.parse_args()

    reader = OmronFINSReader(
        plc_host=args.host,
        plc_port=args.port,
        plc_node=args.plc_node,
        src_node=args.src_node,
    )
    reader.connect()

    iteration = 0
    try:
        while True:
            iteration += 1
            print(f"\n─── Poll #{iteration} ───────────────────────────────")
            try:
                words = reader.read_dm_words(args.dm, args.count)
                print(f"\n  DM area read  (start=D{args.dm:05d}, count={args.count})")
                print(f"  {'Address':<12} {'Dec':>6}  {'Hex':>6}  {'Bin':>18}")
                print(f"  {'-'*46}")
                for i, w in enumerate(words):
                    addr = args.dm + i
                    print(f"  D{addr:05d}      {w:>6}  {w:#06x}  {w:016b}")
            except RuntimeError as e:
                print(f"  [ERROR] {e}")
            except socket.timeout:
                print("  [TIMEOUT] No response from PLC.")

            if args.repeat != 0 and iteration >= args.repeat:
                break
            time.sleep(args.interval)

    except KeyboardInterrupt:
        print("\n[INFO] Interrupted by user.")
    finally:
        reader.disconnect()


if __name__ == "__main__":
    main()