function create_parking_simulink_model()
% CREATE_PARKING_SIMULINK_MODEL - 生成倒车入库Simulink模型
% 兼容版本：MATLAB R2016b
% 功能：创建与CarSim联合仿真的倒车入库控制系统模型
%
% 作者：LiHongwei
% 日期：2026-05-29

    model_name = 'parking_reverse_control';

    % 检查模型是否已加载
    if bdIsLoaded(model_name)
        close_system(model_name, 0);
    end

    % 创建新模型
    new_system(model_name);
    open_system(model_name);

    % 设置仿真参数
    set_param(model_name, 'StopTime', '60');
    set_param(model_name, 'Solver', 'ode4');
    set_param(model_name, 'FixedStep', '0.01');
    set_param(model_name, 'SimulationMode', 'Normal');

    fprintf('正在创建倒车入库Simulink模型: %s\n', model_name);

    %% ==================== 添加CarSim S-Function模块 ====================
    % CarSim通过S-Function与Simulink通信
    add_block('simulink/User-Defined Functions/S-Function', ...
        [model_name '/CarSim_Vehicle'], ...
        'Position', [100, 150, 250, 250], ...
        'FunctionName', 'vs_connect_sf2', ...
        'Parameters', '');

    %% ==================== 添加输入信号处理模块 ====================

    % 车辆状态输入（从CarSim）
    add_block('simulink/Sources/In1', ...
        [model_name '/Vehicle_State_In'], ...
        'Position', [50, 100, 80, 114]);

    % 参考路径输入
    add_block('simulink/Sources/In1', ...
        [model_name '/Reference_Path_In'], ...
        'Position', [50, 200, 80, 214]);

    %% ==================== 添加路径规划模块 ====================

    % 路径规划子系统
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model_name '/Path_Planner'], ...
        'Position', [350, 80, 500, 170]);

    % 配置路径规划子系统内部
    configure_path_planner(model_name);

    %% ==================== 添加控制器模块 ====================

    % 横向控制器（方向盘转角）
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model_name '/Lateral_Controller'], ...
        'Position', [350, 220, 500, 310]);

    % 纵向控制器（制动/油门）
    add_block('simulink/Ports & Subsystems/Subsystem', ...
        [model_name '/Longitudinal_Controller'], ...
        'Position', [350, 360, 500, 450]);

    % 配置控制器子系统
    configure_controllers(model_name);

    %% ==================== 添加制动控制模块 ====================

    % 制动压力计算
    add_block('simulink/Math Operations/Gain', ...
        [model_name '/Brake_Gain'], ...
        'Position', [600, 360, 650, 400], ...
        'Gain', '1000');  % 转换为kPa

    % 制动限幅
    add_block('simulink/Discontinuities/Saturation', ...
        [model_name '/Brake_Limit'], ...
        'Position', [700, 360, 750, 400], ...
        'UpperLimit', '15000', ...  % 最大制动压力15MPa
        'LowerLimit', '0');

    %% ==================== 添加转向控制模块 ====================

    % 转向角度限幅
    add_block('simulink/Discontinuities/Saturation', ...
        [model_name '/Steering_Limit'], ...
        'Position', [600, 250, 650, 290], ...
        'UpperLimit', '540', ...  % 最大转向角度540度
        'LowerLimit', '-540');

    %% ==================== 添加状态监测模块 ====================

    % 车辆位置显示
    add_block('simulink/Sinks/Scope', ...
        [model_name '/Vehicle_Position_Scope'], ...
        'Position', [800, 100, 850, 140], ...
        'NumInputPorts', '2');

    % 制动压力显示
    add_block('simulink/Sinks/Scope', ...
        [model_name '/Brake_Pressure_Scope'], ...
        'Position', [800, 350, 850, 390]);

    % 转向角度显示
    add_block('simulink/Sinks/Scope', ...
        [model_name '/Steering_Angle_Scope'], ...
        'Position', [800, 240, 850, 280]);

    %% ==================== 添加数据记录模块 ====================

    % 记录仿真数据到工作空间
    add_block('simulink/Sinks/To Workspace', ...
        [model_name '/Log_Vehicle_State'], ...
        'Position', [800, 450, 850, 480], ...
        'VariableName', 'vehicle_state_log', ...
        'SaveFormat', 'Timeseries');

    add_block('simulink/Sinks/To Workspace', ...
        [model_name '/Log_Brake_Cmd'], ...
        'Position', [800, 500, 850, 530], ...
        'VariableName', 'brake_cmd_log', ...
        'SaveFormat', 'Timeseries');

    %% ==================== 连接信号线 ====================

    % 连接CarSim输出到路径规划器
    add_line(model_name, 'Vehicle_State_In/1', 'Path_Planner/1', 'autorouting', 'on');

    % 连接参考路径到路径规划器
    add_line(model_name, 'Reference_Path_In/1', 'Path_Planner/2', 'autorouting', 'on');

    % 连接路径规划器到控制器
    add_line(model_name, 'Path_Planner/1', 'Lateral_Controller/1', 'autorouting', 'on');
    add_line(model_name, 'Path_Planner/1', 'Longitudinal_Controller/1', 'autorouting', 'on');

    % 连接控制器到执行器
    add_line(model_name, 'Lateral_Controller/1', 'Steering_Limit/1', 'autorouting', 'on');
    add_line(model_name, 'Longitudinal_Controller/1', 'Brake_Gain/1', 'autorouting', 'on');
    add_line(model_name, 'Brake_Gain/1', 'Brake_Limit/1', 'autorouting', 'on');

    % 连接到CarSim输入
    add_line(model_name, 'Steering_Limit/1', 'CarSim_Vehicle/1', 'autorouting', 'on');
    add_line(model_name, 'Brake_Limit/1', 'CarSim_Vehicle/2', 'autorouting', 'on');

    % 连接显示和记录
    add_line(model_name, 'Vehicle_State_In/1', 'Vehicle_Position_Scope/1', 'autorouting', 'on');
    add_line(model_name, 'Path_Planner/1', 'Vehicle_Position_Scope/2', 'autorouting', 'on');
    add_line(model_name, 'Brake_Limit/1', 'Brake_Pressure_Scope/1', 'autorouting', 'on');
    add_line(model_name, 'Steering_Limit/1', 'Steering_Angle_Scope/1', 'autorouting', 'on');
    add_line(model_name, 'Vehicle_State_In/1', 'Log_Vehicle_State/1', 'autorouting', 'on');
    add_line(model_name, 'Brake_Limit/1', 'Log_Brake_Cmd/1', 'autorouting', 'on');

    %% ==================== 添加注释 ====================

    add_block('simulink/Commonly Used Blocks/DocBlock', ...
        [model_name '/Model_Info'], ...
        'Position', [100, 50, 300, 70], ...
        'DocumentType', 'Text', ...
        'Document', '倒车入库CarSim/Simulink联合仿真模型 - LiHongwei 2026');

    % 保存模型
    save_system(model_name);
    fprintf('模型创建完成: %s\n', model_name);

end

function configure_path_planner(model_name)
% 配置路径规划子系统内部结构

    planner_path = [model_name '/Path_Planner'];

    % 添加MATLAB Function块实现路径规划算法
    add_block('simulink/User-Defined Functions/MATLAB Function', ...
        [planner_path '/Path_Planning_Algo'], ...
        'Position', [100, 80, 250, 120]);

    % 设置MATLAB Function代码
    % 注意：R2016b使用旧版MATLAB Function语法
    set_param([planner_path '/Path_Planning_Algo'], ...
        'Script', get_path_planning_code());

    % 添加输出端口
    add_block('simulink/Sinks/Out1', ...
        [planner_path '/Path_Cmd_Out'], ...
        'Position', [350, 95, 380, 109]);

    % 连接
    add_line(planner_path, 'In1/1', 'Path_Planning_Algo/1', 'autorouting', 'on');
    add_line(planner_path, 'In2/1', 'Path_Planning_Algo/2', 'autorouting', 'on');
    add_line(planner_path, 'Path_Planning_Algo/1', 'Path_Cmd_Out/1', 'autorouting', 'on');

end

function configure_controllers(model_name)
% 配置横向和纵向控制器子系统

    % ============ 横向控制器 ============
    lat_path = [model_name '/Lateral_Controller'];

    % 添加PID控制器
    add_block('simulink/Continuous/PID Controller', ...
        [lat_path '/Steering_PID'], ...
        'Position', [150, 80, 250, 120], ...
        'P', '2.5', ...
        'I', '0.1', ...
        'D', '0.5', ...
        'UpperSaturationLimit', '540', ...
        'LowerSaturationLimit', '-540');

    % 输入输出端口
    add_block('simulink/Sources/In1', [lat_path '/Ref_Path_In'], ...
        'Position', [50, 95, 80, 109]);
    add_block('simulink/Sinks/Out1', [lat_path '/Steering_Cmd_Out'], ...
        'Position', [350, 95, 380, 109]);

    % 连接
    add_line(lat_path, 'Ref_Path_In/1', 'Steering_PID/1', 'autorouting', 'on');
    add_line(lat_path, 'Steering_PID/1', 'Steering_Cmd_Out/1', 'autorouting', 'on');

    % ============ 纵向控制器 ============
    lon_path = [model_name '/Longitudinal_Controller'];

    % 添加制动控制器
    add_block('simulink/Continuous/PID Controller', ...
        [lon_path '/Brake_PID'], ...
        'Position', [150, 80, 250, 120], ...
        'P', '1.8', ...
        'I', '0.05', ...
        'D', '0.3', ...
        'UpperSaturationLimit', '1', ...
        'LowerSaturationLimit', '-1');

    % 输入输出端口
    add_block('simulink/Sources/In1', [lon_path '/Speed_Ref_In'], ...
        'Position', [50, 95, 80, 109]);
    add_block('simulink/Sinks/Out1', [lon_path '/Brake_Cmd_Out'], ...
        'Position', [350, 95, 380, 109]);

    % 连接
    add_line(lon_path, 'Speed_Ref_In/1', 'Brake_PID/1', 'autorouting', 'on');
    add_line(lon_path, 'Brake_PID/1', 'Brake_Cmd_Out/1', 'autorouting', 'on');

end

function code = get_path_planning_code()
% 返回路径规划MATLAB Function的代码

    code = sprintf([...
        'function [path_cmd] = fcn(vehicle_state, ref_path)\n' ...
        '%%#codegen\n' ...
        '%% 倒车入库路径规划算法\n' ...
        '%% 输入:\n' ...
        '%%   vehicle_state - [x, y, yaw, v, steer_angle]\n' ...
        '%%   ref_path - [x_ref, y_ref, yaw_ref]\n' ...
        '%% 输出:\n' ...
        '%%   path_cmd - [target_x, target_y, target_yaw, target_speed]\n' ...
        '\n' ...
        '%% 提取车辆状态\n' ...
        'x_veh = vehicle_state(1);\n' ...
        'y_veh = vehicle_state(2);\n' ...
        'yaw_veh = vehicle_state(3);\n' ...
        'v_veh = vehicle_state(4);\n' ...
        '\n' ...
        '%% 提取参考路径\n' ...
        'x_ref = ref_path(1);\n' ...
        'y_ref = ref_path(2);\n' ...
        'yaw_ref = ref_path(3);\n' ...
        '\n' ...
        '%% 计算横向偏差\n' ...
        'dx = x_ref - x_veh;\n' ...
        'dy = y_ref - y_veh;\n' ...
        'lateral_error = dy * cos(yaw_veh) - dx * sin(yaw_veh);\n' ...
        '\n' ...
        '%% 计算航向偏差\n' ...
        'heading_error = yaw_ref - yaw_veh;\n' ...
        '%% 归一化到[-pi, pi]\n' ...
        'heading_error = mod(heading_error + pi, 2*pi) - pi;\n' ...
        '\n' ...
        '%% 计算目标速度（低速倒车）\n' ...
        'target_speed = -2.0;  %% 倒车速度2 km/h\n' ...
        '\n' ...
        '%% 输出路径命令\n' ...
        'path_cmd = [x_ref, y_ref, yaw_ref, target_speed];\n' ...
        'end\n'
    ]);

end
