import sys

# Read the file
with open('qt_gui_demo.cpp', 'r', encoding='utf-8') as f:
    lines = f.readlines()

# Find and replace the initial state section (around line 448-451)
new_lines = []
skip_count = 0
for i, line in enumerate(lines):
    if skip_count > 0:
        skip_count -= 1
        continue

    if i >= 447 and 'Set up tracker' in line:
        # Add the original comment
        new_lines.append(line)
        # Add the next line (setReferencePath)
        new_lines.append(lines[i+1])
        # Add blank line
        new_lines.append('\n')
        # Add the new initial state code
        new_lines.append('        // 修复: 从路径起点获取初始状态，确保完美匹配\n')
        new_lines.append('        const auto& path_points = path.getPathPoints();\n')
        new_lines.append('        AGVModel::State initial_state;\n')
        new_lines.append('        if (!path_points.empty()) {\n')
        new_lines.append('            const PathPoint& start = path_points[0];\n')
        new_lines.append('            initial_state = AGVModel::State(start.x, start.y, start.theta);\n')
        new_lines.append('        } else {\n')
        new_lines.append('            initial_state = AGVModel::State(0.0, 0.0, 0.0);\n')
        new_lines.append('        }\n')
        new_lines.append('        tracker_->setInitialState(initial_state);\n')
        skip_count = 3  # Skip the next 3 lines (setReferencePath, old initial_state, setInitialState)
    else:
        new_lines.append(line)

# Write back
with open('qt_gui_demo.cpp', 'w', encoding='utf-8') as f:
    f.writelines(new_lines)

print("Initial state fix applied successfully!")