#!/usr/bin/env python3
"""
Generate custom OD_custom.c and OD_custom.h based on the original example OD files.
Adds manufacturer-specific objects 0x2000-0x2003 (4 x uint32) with PDO mapping.
"""
import shutil, os, re

base = "/root/workspace/canopen_lab/CANopenLinux"
src = f"{base}/CANopenNode/example"
dst = base

# ─── Step 1: Copy originals ───
shutil.copy2(f"{src}/OD.h", f"{dst}/OD_custom.h")
shutil.copy2(f"{src}/OD.c", f"{dst}/OD_custom.c")

# ─── Step 2: Modify OD_custom.h ───
with open(f"{dst}/OD_custom.h") as f:
    h = f.read()

# Add members to OD_PERSIST_COMM_t after x1019 line
h = h.replace(
    'uint8_t x1019_synchronousCounterOverflowValue;',
    'uint8_t x1019_synchronousCounterOverflowValue;\n'
    '    uint32_t x2000_MfrData1;\n'
    '    uint32_t x2001_MfrData2;\n'
    '    uint32_t x2002_MfrData3;\n'
    '    uint32_t x2003_MfrData4;'
)

# Add OD_ENTRY macros
h = h.replace(
    '#define OD_ENTRY_H1A03_TPDOMappingParameter &OD->list[32]',
    '#define OD_ENTRY_H1A03_TPDOMappingParameter &OD->list[32]\n'
    '#define OD_ENTRY_H2000_MfrData1 &OD->list[33]\n'
    '#define OD_ENTRY_H2001_MfrData2 &OD->list[34]\n'
    '#define OD_ENTRY_H2002_MfrData3 &OD->list[35]\n'
    '#define OD_ENTRY_H2003_MfrData4 &OD->list[36]'
)

with open(f"{dst}/OD_custom.h", "w") as f:
    f.write(h)
print("[OK] OD_custom.h")

# ─── Step 3: Modify OD_custom.c ───
with open(f"{dst}/OD_custom.c") as f:
    c = f.read()

# Fix include to use OD_custom.h
c = c.replace('#include "OD.h"', '#include "OD_custom.h"')

# Add initialization values after x1019
c = c.replace(
    '.x1019_synchronousCounterOverflowValue = 0x00,',
    '.x1019_synchronousCounterOverflowValue = 0x00,\n'
    '    .x2000_MfrData1 = 0x00000000,\n'
    '    .x2001_MfrData2 = 0x00000000,\n'
    '    .x2002_MfrData3 = 0x00000000,\n'
    '    .x2003_MfrData4 = 0x00000000,'
)

# Add members to ODObjs_t struct
c = c.replace(
    'OD_obj_var_t o_1019_synchronousCounterOverflowValue;',
    'OD_obj_var_t o_1019_synchronousCounterOverflowValue;\n'
    '    OD_obj_var_t o_2000_MfrData1;\n'
    '    OD_obj_var_t o_2001_MfrData2;\n'
    '    OD_obj_var_t o_2002_MfrData3;\n'
    '    OD_obj_var_t o_2003_MfrData4;'
)

# Add initialization in ODObjs
# Find the pattern: closing of o_1019 and start of o_1200, insert between them
init_block = '''    },
    .o_2000_MfrData1 = {
        .dataOrig = &OD_PERSIST_COMM.x2000_MfrData1,
        .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB,
        .dataLength = 4
    },
    .o_2001_MfrData2 = {
        .dataOrig = &OD_PERSIST_COMM.x2001_MfrData2,
        .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB,
        .dataLength = 4
    },
    .o_2002_MfrData3 = {
        .dataOrig = &OD_PERSIST_COMM.x2002_MfrData3,
        .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB,
        .dataLength = 4
    },
    .o_2003_MfrData4 = {
        .dataOrig = &OD_PERSIST_COMM.x2003_MfrData4,
        .attribute = ODA_SDO_RW | ODA_TRPDO | ODA_MB,
        .dataLength = 4
    },

    .o_1200_SDOServerParameter = {'''

# Find the closing of o_1019 init and the start of o_1200
# The pattern is: .o_1019_synchronousCounterOverflowValue = { ... }, then newlines, then .o_1200
# We need to find where .o_1200_SDOServerParameter = { begins and replace that specific occurrence
# with our insert + the original .o_1200

# First, find the exact line with .o_1200_SDOServerParameter in the ODObjs context
lines = c.split('\n')
target_line_idx = None
in_odobjs = False
for i, line in enumerate(lines):
    if 'static CO_PROGMEM ODObjs_t ODObjs = {' in line:
        in_odobjs = True
    if in_odobjs and '.o_1200_SDOServerParameter = {' in line:
        target_line_idx = i
        break
    # Check for closing of struct
    if in_odobjs and line.strip() == '};' and any('o_1200' in l for l in lines[max(0,i-5):i]):
        break

if target_line_idx is not None:
    # Replace only this specific occurrence
    # We can't just do string replace because .o_1200 might appear elsewhere
  
    # Get the line number within the ODObjs init block  
    old_line = lines[target_line_idx]
    # Only replace if it's the right context (after o_1019)
    context = '\n'.join(lines[target_line_idx-15:target_line_idx])
    if 'o_1019_synchronousCounterOverflowValue' in context:
        # This is the right o_1200
        lines[target_line_idx] = init_block
        c = '\n'.join(lines)
        print(f"[OK] ODObjs init at line {target_line_idx+1}")
    else:
        print(f"[WARN] Found .o_1200 but not after o_1019 context context")
        print(f"  Line {target_line_idx+1}: {old_line.strip()[:80]}")
else:
    print("[ERR] Could not find .o_1200_SDOServerParameter in ODObjs context!")

# Add entries to ODList (before the terminator)
c = c.replace(
    '{0x1A03, 0x09, ODT_REC, &ODObjs.o_1A03_TPDOMappingParameter, NULL},\n'
    '    {0x0000, 0x00, 0, NULL, NULL}',
    '{0x1A03, 0x09, ODT_REC, &ODObjs.o_1A03_TPDOMappingParameter, NULL},\n'
    '    {0x2000, 0x01, ODT_VAR, &ODObjs.o_2000_MfrData1, NULL},\n'
    '    {0x2001, 0x01, ODT_VAR, &ODObjs.o_2001_MfrData2, NULL},\n'
    '    {0x2002, 0x01, ODT_VAR, &ODObjs.o_2002_MfrData3, NULL},\n'
    '    {0x2003, 0x01, ODT_VAR, &ODObjs.o_2003_MfrData4, NULL},\n'
    '    {0x0000, 0x00, 0, NULL, NULL}'
)

with open(f"{dst}/OD_custom.c", "w") as f:
    f.write(c)

print("[OK] OD_custom.c")
print("[DONE] Custom OD files generated")